特許 7595550 姿勢角導出装置及び姿勢角センサ

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-11-28

(45)【発行日】2024-12-06

(54)【発明の名称】姿勢角導出装置及び姿勢角センサ

(51)【国際特許分類】

   G01C 1/00 20060101AFI20241129BHJP

   G01C 21/18 20060101ALI20241129BHJP

   G01C 19/00 20130101ALI20241129BHJP

【ＦＩ】

G01C1/00 Z

G01C21/18

G01C19/00 Z

【請求項の数】 11

(21)【出願番号】P 2021181568

(22)【出願日】2021-11-08

(65)【公開番号】P2023069581

(43)【公開日】2023-05-18

【審査請求日】2024-03-01

(73)【特許権者】

【識別番号】000003078

【氏名又は名称】株式会社東芝

(74)【代理人】

【識別番号】110004026

【氏名又は名称】弁理士法人ｉＸ

(72)【発明者】

【氏名】小野 大騎

(72)【発明者】

【氏名】丸藤 竜之介

(72)【発明者】

【氏名】冨澤 泰

【審査官】國田 正久

(56)【参考文献】

【文献】特開２０１１－１８５６６９（ＪＰ，Ａ）

【文献】特開平１１－３３９４４（ＪＰ，Ａ）

【文献】特表２０１７－５１３１３０（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｃ １／００

１９／００－１９／７２

Ｇ０１Ｃ ２１／１８

(57)【特許請求の範囲】

【請求項1】

第１座標系に関する回転角を取得可能な取得部であって、前記回転角は、対象物に設けられた角度センサから得られる、前記取得部と、
前記対象物の第２座標系に関する姿勢角と、前記回転角と、を記憶可能な記憶部と、
前記回転角及び前記姿勢角を取得して、前記回転角から導出される前記回転角の時間変化に基づいて、前記姿勢角を更新して、前記更新された姿勢角を出力可能な処理部と、
を備えた姿勢角導出装置。

【請求項2】

前記角度センサは、積分型ジャイロスコープを含む、請求項１に記載の姿勢角導出装置。

【請求項3】

前記回転角は、第１軸に関する第１回転角と、第２軸に関する第２回転角と、第３軸に関する第３回転角と、を含み、
前記第２軸は、前記第１軸と交差し、
前記第３軸は、前記第１軸及び前記第２軸を含む平面と交差する、請求項１または２に記載の姿勢角導出装置。

【請求項4】

前記第２軸は、前記第１軸と直交し、
前記第３軸は、前記第１軸及び前記第２軸と直交する、請求項３に記載の姿勢角導出装置。

【請求項5】

前記回転角は、第１時刻ｔにおける前記第１回転角Ａ_ｘ（ｔ）、前記第１時刻ｔにおける前記第２回転角Ａ_ｙ（ｔ）、及び、前記第１時刻ｔにおける前記第３回転角Ａ_ｚ（ｔ）と、を含み、
前記処理部は、前記回転角の前記時間変化として、第１時間変化ｄＡ_ｘ（ｔ）、第２時間変化ｄＡ_ｙ（ｔ）及び、第３時間変化ｄＡ_ｚ（ｔ）を導出し、
前記第１時間変化ｄＡ_ｘ（ｔ）、前記第２時間変化ｄＡ_ｙ（ｔ）、前記第３時間変化ｄＡ_ｚ（ｔ）、前記第１回転角Ａ_ｘ（ｔ）、前記第２回転角Ａ_ｙ（ｔ）、前記第３回転角Ａ_ｚ（ｔ）、前記第１時刻ｔの前の第２時刻（ｔ－１）における前記第１回転角Ａ_{ｘ（ｔ－１）}、前記第２時刻（ｔ－１）における前記第２回転角Ａ_{ｙ（ｔ－１）}、及び、前記第２時刻（ｔ－１）における前記第３回転角Ａｚ_{（ｔ－１）}は、

【数1】

を満たす、請求項４に記載の姿勢角導出装置。

【請求項6】

前記姿勢角は、第１姿勢軸に関する第１姿勢角と、第２姿勢軸に関する第２姿勢角と、第３姿勢軸に関する第３姿勢角と、を含み、
前記第２姿勢軸は、前記第１姿勢軸と交差し、
前記第３姿勢軸は、前記第１姿勢軸及び前記第２姿勢軸を含む平面と交差する、請求項５に記載の姿勢角導出装置。

【請求項7】

前記第２姿勢軸は、前記第１姿勢軸と直交し、
前記第３姿勢軸は、前記第１姿勢軸及び前記第２姿勢軸と直交する、請求項６に記載の姿勢角導出装置。

【請求項8】

前記姿勢角は、前記第２時刻（ｔ－１）における前記第１姿勢角φ_{（ｔ－１）}、前記第２時刻（ｔ－１）における前記第２姿勢角θ_{（ｔ－１）}、及び、前記第２時刻（ｔ－１）における前記第３姿勢角ψ_{（ｔ－１）}を含み、
前記処理部は、前記姿勢角の変化として、第１姿勢角変化ｄφ_（ｔ）、第２姿勢角変化ｄθ_（ｔ）、及び、第３姿勢角変化ｄψ_（ｔ）を導出し、
前記第１姿勢角変化ｄφ_（ｔ）、前記第２姿勢角変化ｄθ_（ｔ）、及び、前記第３姿勢角変化ｄψ_（ｔ）は、

【数2】

を満たす、請求項７に記載の姿勢角導出装置。

【請求項9】

前記処理部は、前記更新された前記姿勢角として、前記第１時刻ｔにおける前記第１姿勢角φ_（ｔ）、前記第１時刻ｔにおける前記第２姿勢角θ_（ｔ）、及び、前記第１時刻ｔにおける前記第３姿勢角ψ_（ｔ）を導出し、
前記第１時刻ｔにおける前記第１姿勢角φ_（ｔ）、前記第１時刻ｔにおける前記第２姿勢角θ_（ｔ）、及び、前記第１時刻ｔにおける前記第３姿勢角ψ_（ｔ）は、

【数3】

を満たす、請求項８記載の姿勢角導出装置。

【請求項10】

前記姿勢角は、第１クォータニオン値と、第２クォータニオン値と、第３クォータニオン値と、第４クォータニオン値と、を含む、請求項５に記載の姿勢角導出装置。

【請求項11】

請求項１～１０のいずれか１つに記載の姿勢角導出装置と、
前記角度センサと、
を備えた姿勢角センサ。

【発明の詳細な説明】

【技術分野】

【0001】

本発明の実施形態は、姿勢角導出装置及び姿勢角センサに関する。

【背景技術】

【0002】

ジャイロセンサなどのセンサがある。センサにより、対象物の姿勢（姿勢角）が検出できる。高精度の姿勢角の検出が望まれる。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１６－２００５１２号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

本発明の実施形態は、高精度の姿勢角の検出が可能な姿勢角導出装置及び姿勢角センサを提供する。

【課題を解決するための手段】

【0005】

本発明の実施形態によれば、姿勢角導出装置は、取得部、記憶部及び処理部を含む。前記取得部は、第１座標系に関する回転角を取得可能である。前記回転角は、対象物に設けられた角度センサから得られる。前記記憶部は、前記対象物の第２座標系に関する姿勢角と、前記回転角と、を記憶可能である。前記処理部は、前記回転角及び前記姿勢角を取得して、前記回転角から導出される前記回転角の時間変化に基づいて、前記姿勢角を更新して、前記更新された姿勢角を出力可能である。

【図面の簡単な説明】

【0006】

【図1】図１は、第１実施形態に係る姿勢角導出装置を例示する模式図である。

【図2】図２は、第１実施形態に係る姿勢角導出装置を例示する模式図である。

【図3】図３は、第１実施形態に係る姿勢角導出装置を例示する模式図である。

【図4】図４は、第１実施形態に係る姿勢角導出装置を例示する模式図である。

【図5】図５（ａ）及び図５（ｂ）は、実施形態に係るセンサを例示する模式図である。

【図6】図６は、第３実施形態に係る電子装置を例示する模式図である。

【図7】図７（ａ）～図７（ｈ）は、電子装置の応用を例示する模式図である。

【発明を実施するための形態】

【0007】

以下に、本発明の各実施の形態について図面を参照しつつ説明する。
図面は模式的または概念的なものであり、各部分の厚さと幅との関係、部分間の大きさの比率などは、必ずしも現実のものと同一とは限らない。同じ部分を表す場合であっても、図面により互いの寸法や比率が異なって表される場合もある。
本願明細書と各図において、既出の図に関して前述したものと同様の要素には同一の符号を付して詳細な説明は適宜省略する。

【0008】

（第１実施形態）
図１は、第１実施形態に係る姿勢角導出装置を例示する模式図である。
図１に示すように実施形態に係る姿勢角導出装置５０は、取得部５８、記憶部５５及び処理部５０Ｐを含む。

【0009】

取得部５８は、角度センサ１１０から得られる角度（回転角ＤＡ１）を取得可能である。取得部５８は、例えば、インターフェース回路（Ｉ／Ｆ）である。

【0010】

後述するように、角度センサ１１０は、対象物に設けられる。角度センサ１１０は、例えば、積分型ジャイロスコープ（例えば、ＲＩＧ：Rate Integrating Gyroscope）を含む。角度センサ１１０は、ＲＩＧで良い。

【0011】

角度センサ１１０において、回転角ＤＡ１が直接検出されることが可能である。参考例のジャイロスコープにおいて、角速度が検出される。検出された角速度を積分することで角度が検出できる。

【0012】

実施形態においては、積分型ジャイロスコープが角度センサ１１０に適用される。これにより、例えば、積分における累積誤差が生じない。角度センサ１１０は、例えば、３軸の角度センサである。

【0013】

角度センサ１１０から得られる角度（回転角ＤＡ１）は、第１座標系に関する。第１座標系は、例えば、角度センサ１１０が設けられる位置が基準となる座標系である。第１座標系は、例えば、センサ座標系またはボディ座標系である。

【0014】

回転角ＤＡ１は、例えば、第１軸に関する第１回転角Ａ_ｘと、第２軸に関する第２回転角Ａ_ｙと、第３軸に関する第３回転角Ａ_ｚと、を含んで良い。第２軸は、第１軸と交差する。第３軸は、第１軸及び第２軸を含む平面と交差する。例えば、第１軸はＸ軸である。第２軸はＹ軸である。第３軸はＺ軸である。例えば、第２軸は、第１軸と直交する。第３軸は、第１軸及び第２軸と直交する。

【0015】

第１回転角Ａ_ｘは、第１軸の周りの回転の角度に対応する。第２回転角Ａ_ｙは、第２軸の周りの回転の角度に対応する。第３回転角Ａ_ｚは、第３軸の周りの回転の角度に対応する。

【0016】

１つの時刻を第１時刻ｔとする。回転角ＤＡ１は、第１時刻ｔにおける第１回転角Ａ_ｘ（ｔ）、第１時刻ｔにおける第２回転角Ａ_ｙ（ｔ）、及び、第１時刻ｔにおける第３回転角Ａ_ｚ（ｔ）と、を含む。第１時刻ｔの前の１つの時刻を第２時刻（ｔ－１）とする。第２時刻（ｔ－１）における第１回転角Ａ_ｘを第１回転角Ａ_{ｘ（ｔ－１）}とする。第２時刻（ｔ－１）における第２回転角Ａ_ｙを第２回転角Ａ_{ｙ（ｔ－１）}とする。第２時刻（ｔ－１）における第３回転角Ａ_ｚを第３回転角Ａｚ_{（ｔ－１）}とする。回転角ＤＡ１は、第１回転角Ａ_{ｘ（ｔ－１）}、第２回転角Ａ_{ｙ（ｔ－１）}、第３回転角Ａｚ_{（ｔ－１）}を含んで良い。これらの回転角ＤＡ１が、角度センサ１１０から取得部５８に供給される。

【0017】

記憶部５５は、例えば、取得部５８から供給された回転各ＤＡ１を記憶可能である。記憶部５５は、角度センサ１１０が設けられる対象物の姿勢角ＤＡ２を記憶可能である。記憶部５５は、メモリ（またはストレージ）である。記憶部５５は、バッファメモリで良い。記憶部５５は、回転角ＤＡ１を記憶可能である。

【0018】

姿勢角ＤＡ２は、対象物の第２座標系に関する姿勢角である。第２座標系は、第１座標系と異なって良い。１つの例において、第２座標系は、例えば、絶対座標系である。第２座標系は、例えば、重力加速度の向きを基準とした座標系で良い。第２座標系は、例えば、空間座標系または基準座標系で良い。第２座標系は、global coordinateで良い。

【0019】

姿勢角ＤＡ２は、例えば、３次元の姿勢角である。姿勢角ＤＡ２は、例えば、第１姿勢軸に関する第１姿勢角φと、第２姿勢軸に関する第２姿勢角θと、第３姿勢軸に関する第３姿勢角ψと、を含む。第２姿勢軸は、第１姿勢軸と交差する。第３姿勢軸は、第１姿勢軸及び第２姿勢軸を含む平面と交差する。例えば、第２姿勢軸は、第１姿勢軸と直交する。第３姿勢軸は、第１姿勢軸及び第２姿勢軸と直交する。第１～第３姿勢軸は、第１～第３軸と異なって良い。

【0020】

第１姿勢角φは、第１姿勢軸の周りの回転の角度に対応する。第２姿勢角θは、第２姿勢軸の周りの回転の角度に対応する。第３姿勢角ψは、第３姿勢軸の周りの回転の角度に対応する。

【0021】

記憶部５５は、第２時刻（ｔ－１）における姿勢角ＤＡ２を記憶して良い。姿勢角ＤＡ２は、第２時刻（ｔ－１）における第１姿勢角φ_{（ｔ－１）}、第２時刻（ｔ－１）における第２姿勢角θ_{（ｔ－１）}、及び、第２時刻（ｔ－１）における第３姿勢角ψ_{（ｔ－１）}を含む。これらの姿勢角ＤＡ２が記憶部５５から処理部５０Ｐに供給されて良い。

【0022】

処理部５０Ｐは、回転角ＤＡ１及び姿勢角ＤＡ２を取得する。例えば、処理部５０Ｐは、取得部５８から記憶部５５に供給された回転角ＤＡ１を、記憶部５５から取得する。例えば、処理部５０Ｐは、記憶部５５から姿勢角ＤＡ２を取得する。処理部５０Ｐは、回転角ＤＡ１から導出される回転角ＤＡ１の時間変化に基づいて、姿勢角ＤＡ２を更新して、更新された姿勢角ＤＡ３を出力可能である。

【0023】

実施形態においては、角度センサ１１０から得られる角度（回転角ＤＡ１）を用いて、姿勢角ＤＡ３が導出される。上記の参考例のように、角速度に基づいて姿勢角が導出されない。これにより、角速度を積分する際の誤差が生じない。実施形態においては、高い精度で姿勢角を導出（検出）できる。例えば、姿勢角を高速で導出できる。

【0024】

既に説明したように、第１回転角Ａ_ｘ（ｔ）、第２回転角Ａ_ｙ（ｔ）、第３回転角Ａ_ｚ（ｔ）、第１回転角Ａ_{ｘ（ｔ－１）}、第２回転角Ａ_{ｙ（ｔ－１）}、及び、第３回転角Ａｚ_{（ｔ－１）}は、例えば、記憶部５５に記憶されて良い。

【0025】

処理部５０Ｐは、回転角ＤＡ１の時間変化として、第１時間変化ｄＡ_ｘ（ｔ）、第２時間変化ｄＡ_ｙ（ｔ）、及び、第３時間変化ｄＡ_ｚ（ｔ）を導出する。

【0026】

第１時間変化ｄＡ_ｘ（ｔ）、第２時間変化ｄＡ_ｙ（ｔ）、第３時間変化ｄＡ_ｚ（ｔ）、第１回転角Ａ_ｘ（ｔ）、第２回転角Ａ_ｙ（ｔ）、第３回転角Ａ_ｚ（ｔ）、第１回転角Ａ_{ｘ（ｔ－１）}、第２回転角Ａ_{ｙ（ｔ－１）}、及び、第３回転角Ａｚ_{（ｔ－１）}は、以下の第１式を満たす。

【数1】

【0027】

処理部５０Ｐは、このような時間変化を導出する。

【0028】

処理部５０Ｐは、導出された時間変化と、記憶部５５から取得した第２時刻（ｔ－１）の姿勢角ＤＡ２を更新する。処理部５０Ｐは、例えば、第２時刻（ｔ－１）における第１姿勢角φ_{（ｔ－１）}と、関数ｆ１（ｄＡ_ｘ（ｔ），ｄＡ_ｙ（ｔ），ｄＡ_ｚ（ｔ））と、の和から、第１時刻ｔの第１姿勢角φ_（ｔ）を導出する。処理部５０Ｐは、例えば、第２時刻（ｔ－１）における第２姿勢角θ_{（ｔ－１）}と、関数ｆ２（ｄＡ_ｘ（ｔ），ｄＡ_ｙ（ｔ），ｄＡ_ｚ（ｔ））と、の和から、第１時刻ｔの第２姿勢角θ_（ｔ）を導出する。処理部５０Ｐは、例えば、第２時刻（ｔ－１）における第３姿勢角ψ_{（ｔ－１）}と、関数ｆ３（ｄＡ_ｘ（ｔ），ｄＡ_ｙ（ｔ），ｄＡ_ｚ（ｔ））と、の和から、第１時刻ｔの第３姿勢角ψ_（ｔ）を導出する。

【0029】

このように、実施形態においては、角度センサ１１０から得られる角度（回転角ＤＡ１）から回転角ＤＡ１の時間変化が導出される。導出された回転角ＤＡ１の時間変化に基づいて、姿勢角ＤＡ２が更新され、更新された姿勢角ＤＡ３が導出される。姿勢角ＤＡ３は、例えば、第１姿勢角φ_（ｔ）、第３姿勢角θ_（ｔ）及び第３姿勢角ψ_（ｔ）を含む。

【0030】

以下、処理部５０Ｐのいくつかの例について説明する。
図２は、第１実施形態に係る姿勢角導出装置を例示する模式図である。
図２に示すように実施形態に係る姿勢角導出装置５０Ａにおいて、処理部５０Ｐは、第１演算部分５１を含んで良い。第１演算部分５１は、回転角ＤＡ１の時間変化を導出可能である。第１演算部分５１においては、上記の第１式の演算が行われる。

【0031】

処理部５０Ｐは、第２演算部分５２をさらに含んで良い。第１演算部分５１で導出された時間変化が、第２演算部分５２に供給される。第２演算部分５２は、第１演算部分５１が導出した時間変化と、記憶部５５から取得した姿勢角ＤＡ２と、に基づいて、姿勢角の変化を導出可能である。

【0032】

姿勢角の変化は、例えば、第１姿勢角変化ｄφ_（ｔ）、第２姿勢角変化ｄθ_（ｔ）、及び、第３姿勢角変化ｄψ_（ｔ）を含む。第１姿勢角変化ｄφ_（ｔ）、第２姿勢角変化ｄθ_（ｔ）、及び、第３姿勢角変化ｄψ_（ｔ）は、以下の第２式を満たす。

【数2】

【0033】

処理部５０Ｐは、第３演算部分５３をさらに含んで良い。第２演算部分５２で導出された姿勢角の変化が、第３演算部分５３に供給される。第３演算部分５３は、第２演算部分５２が導出した姿勢角の変化と、記憶部５５から取得された姿勢角ＤＡ２と、を加算して、更新された姿勢角ＤＡ３を導出可能である。

【0034】

例えば、第３演算部分５３では、以下の第３式の演算が行われる。

【数3】

【0035】

このように、処理部５０Ｐ（例えば、第３演算部分５３）は、更新された姿勢角ＤＡ３として、第１時刻ｔにおける第１姿勢角φ_（ｔ）、第１時刻ｔにおける第２姿勢角θ_（ｔ）、及び、第１時刻ｔにおける第３姿勢角ψ_（ｔ）を導出可能である。第１姿勢角φ_（ｔ）、第２姿勢角θ_（ｔ）、及び、第３姿勢角ψ_（ｔ）は、上記の第３式を満たす。第３式の演算の結果が、導出されるべき姿勢角ＤＡ３に対応する。

【0036】

実施形態において、処理部５０Ｐ（例えば第３演算部分５３）は、更新された姿勢角ＤＡ３を記憶部５５に供給しても良い。記憶部５５は、更新された姿勢角ＤＡ３を記憶可能である。上記の処理が繰り返して実施されても良い。更新された姿勢角ＤＡ３を用いて後の任意の時刻の姿勢角が導出されても良い。

【0037】

実施形態において、上記のように、オイラー角が用いられて良い。実施形態において、以下のように、姿勢角として、クォータニオンが適用されても良い。

【0038】

例えば、姿勢角ＤＡ２は、第１クォータニオン値αと、第２クォータニオン値βと、第３クォータニオン値γと、第４クォータニオン値δと、で表されることができる。例えば、姿勢角ＤＡ２は、第１クォータニオン値αと、第２クォータニオン値βと、第３クォータニオン値γと、第４クォータニオン値δと、を含む。

【0039】

図３は、第１実施形態に係る姿勢角導出装置を例示する模式図である。
図３に示すように実施形態に係る姿勢角導出装置５０Ｂにおいて、処理部５０Ｐは、第１～第３演算部分５１～５３を含む。第１演算部分５１では、上記と同様の演算が行われる。

【0040】

姿勢角ＤＡ２は、第２時刻（ｔ－１）における第１クォータニオン値α_{（ｔ－１）}、第２時刻（ｔ－１）における第２クォータニオン値β_{（ｔ－１）}、第２時刻（ｔ－１）における第３クォータニオン値γ_{（ｔ－１）}、及び、第２時刻（ｔ－１）における第４クォータニオン値δ_{（ｔ－１）}を含む。これらのクォータニオン値が処理部５０Ｐ（第２演算部分５２及び第３演算部分５３）に供給される。

【0041】

処理部５０Ｐ（第２演算部分５２）は、姿勢角ＤＡ２の変化として、第１クォータニオン値変化ｄα_（ｔ）、第２クォータニオン値変化ｄβ_（ｔ）、第３クォータニオン値変化ｄγ_（ｔ）、及び、第４クォータニオン値変化ｄδ_（ｔ）を導出する。

【0042】

第１クォータニオン値変化ｄα_（ｔ）、第２クォータニオン値変化ｄβ_（ｔ）、第３クォータニオン値変化ｄγ_（ｔ）、及び、第４クォータニオン値変化ｄδ_（ｔ）は、以下の第４式を満たす。

【数4】

【0043】

例えば、第２演算部分５２は、回転角の時間変化（第１時間変化ｄＡ_ｘ（ｔ）、第２時間変化ｄＡ_ｙ（ｔ）、及び、第３時間変化ｄＡ_ｚ（ｔ））と、記憶部５５から取得される姿勢角ＤＡ２と、を用いて、上記の第４式の演算を行う。

【0044】

処理部５０Ｐ（第３演算部分５３）は、更新された姿勢角ＤＡ３として、第１時刻ｔにおける第１クォータニオン値α_（ｔ）、第１時刻ｔにおける第２クォータニオン値β_（ｔ）、第１時刻ｔにおける第３クォータニオン値γ_（ｔ）、及び、第１時刻ｔにおける第４クォータニオン値δ_（ｔ）を導出する。

【0045】

第１時刻ｔにおける第１クォータニオン値α_（ｔ）、第１時刻ｔにおける第２クォータニオン値β_（ｔ）、第１時刻ｔにおける第３クォータニオン値γ_（ｔ）、及び、第１時刻ｔにおける第４クォータニオン値δ_（ｔ）は、以下の第５式を満たす。

【数5】

【0046】

第３演算部分５３において、上記の第５式の演算が行われ、更新された姿勢角ＤＡ３として第１クォータニオン値α_（ｔ）、第２クォータニオン値β_（ｔ）、第３クォータニオン値γ_（ｔ）、及び、第４クォータニオン値δ_（ｔ）が導出される。

【0047】

以下に説明するように、クォータニオン値がオイラー角に変換されても良い。

【0048】

図４は、第１実施形態に係る姿勢角導出装置を例示する模式図である。
図４に示すように実施形態に係る姿勢角導出装置５０Ｃにおいて、処理部５０Ｐは、第４演算部分５４を含む。これを除く姿勢角導出装置５０Ｃの構成は、姿勢角導出装置５０Ｂと同様で良い。

【0049】

姿勢角導出装置５０Ｃにおいて、処理部５０Ｐ（第４演算部分５４）は、第１時刻ｔにおける第１クォータニオン値α_（ｔ）、第１時刻ｔにおける第２クォータニオン値β_（ｔ）、第１時刻ｔにおける第３クォータニオン値γ_（ｔ）、及び、第１時刻ｔにおける第４クォータニオン値δ_（ｔ）に基づいて、更新された姿勢角ＤＡ３として、第１時刻ｔにおける第１姿勢角φ_（ｔ）、第１時刻ｔにおける第２姿勢角θ_（ｔ）、及び、第１時刻ｔにおける第３姿勢角ψ_（ｔ）を導出する。

【0050】

第１時刻ｔにおける第１姿勢角φ_（ｔ）、第１時刻ｔにおける第２姿勢角θ_（ｔ）、及び、第１時刻ｔにおける第３姿勢角ψ_（ｔ）は、以下の第６式を満たす。

【数6】

【0051】

例えば、第４演算部分５４により、上記の第６式の演算が行われる。これにより、クォータニオン値がオイラー角に変換される。

【0052】

実施形態において、処理部５０Ｐとして、例えば、デジタル演算装置が適用されて良い。実施形態において、処理部５０Ｐの少なくとも一部は、アナログ回路を含んでも良い。実施形態において、処理部５０Ｐの構成は、任意である。

【0053】

実施形態において、更新された姿勢角ＤＡ３は、取得部５８（例えばインターフェース）を介して出力されて良い。取得部５８は、出力部として機能しても良い。

【0054】

（第２実施形態）
第２実施形態に係る姿勢角センサ２１０（図１～図４参照）は、実施形態に係る姿勢角導出装置（姿勢角導出装置５０～５０Ｃなど）と、角度センサ１１０と、を含む。実施形態に係る姿勢角センサ２１０によれば、高精度の姿勢角の検出が可能な姿勢角センサが提供できる。

【0055】

以下、角度センサ１１０の例について説明する。
図５（ａ）及び図５（ｂ）は、実施形態に係るセンサを例示する模式図である。
図５（ａ）は、平面図である。図５（ｂ）は、図５（ａ）のＡ１－Ａ２線断面図である。図５（ａ）及び図５（ｂ）に示すように、実施形態に係る角度センサ１１０は、基体６０と、構造体６０Ａと、制御装置７０と、を含む。角度センサ１１０は、対象物９０に設けられる。例えば、角度センサ１１０は、対象物９０に固定される。

【0056】

基体６０は、第１面６０Ｆを含む。構造体６０Ａは、例えば、ＲＩＧである。

【0057】

構造体６０Ａは、可動部材１０を含む。可動部材１０は、振動可能である。可動部材１０の振動は、第１成分及び第２成分を含む。第１成分は、第１方向Ｄ１に沿う成分である。第２成分は、第２方向Ｄ２に沿う成分である。第１方向Ｄ１は、第１面６０Ｆに沿う。第２方向Ｄ２は、第１方向Ｄ１と交差し、第１面６０Ｆに沿う。

【0058】

第１面６０ＦをＸ１－Ｙ１平面とする。Ｘ１－Ｙ１平面内の１つの方向をＸ１軸方向とする。Ｘ１－Ｙ１平面に沿い、Ｘ軸１方向に対して垂直な方向をＹ１軸方向とする。Ｘ１軸方向及びＹ１軸方向に対して垂直な方向をＺ１軸方向とする。第１方向Ｄ１は、例えば、Ｘ１軸方向である。第２方向Ｄ２は、例えば、Ｙ１軸方向である。

【0059】

制御装置７０は、第１成分及び第２成分に基づいて得られた可動部材１０の回転角Ａｖ１を出力することが可能である。

【0060】

例えば、制御装置７０は、可動部材１０の振動の第１成分の振幅及び第２成分の振幅を検出する。これらの振幅の比が、回転角Ａｖ１に対応する。

【0061】

図５（ａ）及び図５（ｂ）に示すように、構造体６０Ａは、例えば、固定部１０Ｆと、接続部１０Ｓと、を含む。固定部１０Ｆは、基体６０に固定される。接続部１０Ｓは、固定部１０Ｆに支持される。接続部１０Ｓは、可動部材１０と接続される。この例では、Ｘ１－Ｙ１平面において、可動部材１０は、固定部１０Ｆの周りに設けられる。可動部材１０は、環状である。複数の接続部１０Ｓにより、可動部材１０が支持される。可動部材１０と基体６０との間に間隙ｇ１が設けられる。接続部１０Ｓは、例えば、折れ曲がり形状を有する。接続部１０Ｓは、例えば、ミアンダ形状を有しても良い。接続部１０Ｓは、例えば、ばね構造体である。接続部１０Ｓは、変形可能である。

【0062】

可動部材１０及び接続部１０Ｓは、例えば、導電性である。

【0063】

図５（ａ）及び図５（ｂ）に示すように、例えば、構造体６０Ａは、第１対向電極部材２０Ｍを含む。この例では、複数の第１対向電極部材２０Ｍが設けられる。これらの複数の第１対向電極部材２０Ｍにより可動部材１０の振動が制御されて良い。第１対向電極部材２０Ｍに印加される電圧（例えば交流成分を含む電圧）により、可動部材１０の振動が制御されて良い。電圧は、例えば、第１対向電極部材２０Ｍと、可動部材１０と、の間に印加される。

【0064】

振動している可動部材１０が外力などにより回転すると、振動状態が変化する。振動状態の変化は、例えば、コリオリ力の作用によると考えられる。例えば、可動部材１０は、バネ機構（例えば接続部１０Ｓ）により振動する。第１方向Ｄ１に振動している可動部材１０に、回転の角速度Ωによるコリオリ力が作用する。これにより、可動部材１０に第２方向Ｄ２に沿う振動の成分が生じる。制御装置７０は、第２方向Ｄ２に沿う振動の振幅を検出する。一方、第２方向Ｄ２に振動している可動部材１０に、回転の角速度Ωによるコリオリ力が作用する。これにより、可動部材１０に第１方向Ｄ１に沿う振動の成分が生じる。制御装置７０は、第１方向Ｄ１に沿う振動の振幅を検出する。例えば、第１方向Ｄ１の第１成分の振幅を「Ａ１」とし、第２方向Ｄ２の第２成分の振幅を「Ａ２」としたとき、回転角は、例えば、ｔａｎ^－１（－Ａ２／Ａ１）に対応する。

【0065】

このような角度センサ１１０により、回転角（例えば回転角Ａｖ１）が得られる。異なる複数の軸に関しての回転角を求めることで、図１などに例示した回転角ＤＡ１が得られる。

【0066】

実施形態においては、例えば、角速度を用いるのではなく、回転角（角度）がそのまま姿勢角導出装置に入力される。これにより、姿勢角を導出可能である。例えば、角速度を用いる参考例においては、積分誤差によるドリフトまたは高周波ノイズが発生し易い。実施形態においては、ドリフトまたは高周波ノイズが抑制され、姿勢角を高精度で検出可能である。実施形態においては、姿勢角の導出に時間情報が不要である。これにより、非定期サンプリングにおいても、姿勢角を高精度で検出可能である。

【0067】

(第３実施形態）
第３実施形態は、電子装置に係る。電子装置は、例えば、対象物９０の少なくとも一部で良い。
図６は、第３実施形態に係る電子装置を例示する模式図である。
図６に示すように、実施形態に係る電子装置３１０は、実施形態に係るセンサと、回路制御部１７０と、を含む。図６の例では、センサとして、姿勢角センサ２１０（及び角度センサ１１０）が描かれている。回路制御部１７０は、センサから得られる信号Ｓ１に基づいて回路１８０を制御可能である。回路１８０は、例えば駆動装置１８５の制御回路などである。実施形態によれば、高精度の検出結果に基づいて、駆動装置１８５を制御するための回路１８０などを高精度で制御できる。

【0068】

図７（ａ）～図７（ｈ）は、電子装置の応用を例示する模式図である。
図７（ａ）に示すように、電子装置３１０は、ロボットの少なくとも一部でも良い。図７（ｂ）に示すように、電子装置３１０は、製造工場などに設けられる工作ロボットの少なくとも一部でも良い。図７（ｃ）に示すように、電子装置３１０は、工場内などの自動搬送車の少なくとも一部でも良い。図７（ｄ）に示すように、電子装置３１０は、ドローン（無人航空機）の少なくとも一部でも良い。図７（ｅ）に示すように、電子装置３１０は、飛行機の少なくとも一部でも良い。図７（ｆ）に示すように、電子装置３１０は、船舶の少なくとも一部でも良い。図７（ｇ）に示すように、電子装置３１０は、潜水艦の少なくとも一部でも良い。図７（ｈ）に示すように、電子装置３１０は、自動車の少なくとも一部でも良い。電子装置３１０は、例えば、ロボット及び移動体の少なくともいずれかを含んでも良い。

【0069】

実施形態は、以下の構成（例えば技術案）を含んで良い。
（構成１）
第１座標系に関する回転角を取得可能な取得部であって、前記回転角は、対象物に設けられた角度センサから得られる、前記取得部と、
前記対象物の第２座標系に関する姿勢角と、前記回転角と、を記憶可能な記憶部と、
前記回転角及び前記姿勢角を取得して、前記回転角から導出される前記回転角の時間変化に基づいて、前記姿勢角を更新して、前記更新された姿勢角を出力可能な処理部と、
を備えた姿勢角導出装置。

【0070】

（構成２）
前記角度センサは、積分型ジャイロスコープを含む、構成１に記載の姿勢角導出装置。

【0071】

（構成３）
前記角度センサは、振動可能な可動部材を含み、前記可動部材の前記振動は、第１方向に沿う第１成分と、前記第１方向と交差する第２方向に沿う第２成分と、を含み、
前記角度センサは、前記第１成分及び前記第２成分に基づいて得られた前記可動部材の回転角を前記回転角として出力可能である、構成１または２に記載の姿勢角導出装置。

【0072】

（構成４）
前記処理部は、第１演算部分を含み、
前記第１演算部分は、前記回転角の前記時間変化を導出可能である、構成１～３のいずれか１つに記載の姿勢角導出装置。

【0073】

（構成５）
前記処理部は、第２演算部分をさらに含み、
前記第２演算部分は、前記第１演算部分が導出した前記時間変化と、前記記憶部から取得した前記姿勢角と、に基づいて、前記姿勢角の変化を導出可能である、構成４に記載の姿勢角導出装置。

【0074】

（構成６）
前記処理部は、第３演算部分をさらに含み、
前記第３演算部分は、前記第２演算部分が導出した前記姿勢角の変化と、前記記憶部から取得された前記姿勢角と、を加算して前記更新された前記姿勢角を導出可能である、構成５に記載の姿勢角導出装置。

【0075】

（構成７）
前記処理部は、前記更新された前記姿勢角を前記記憶部に供給し、
前記記憶部は、前記更新された前記姿勢角を記憶可能である、構成１～６のいずれか１つに記載の姿勢角導出装置。

【0076】

（構成８）
前記回転角は、第１軸に関する第１回転角と、第２軸に関する第２回転角と、第３軸に関する第３回転角と、を含み、
前記第２軸は、前記第１軸と交差し、
前記第３軸は、前記第１軸及び前記第２軸を含む平面と交差する、構成１～７のいずれか１つに記載の姿勢角導出装置。

【0077】

（構成９）
前記第２軸は、前記第１軸と直交し、
前記第３軸は、前記第１軸及び前記第２軸と直交する、構成８に記載の姿勢角導出装置。

【0078】

（構成１０）
前記回転角は、第１時刻ｔにおける前記第１回転角Ａ_ｘ（ｔ）、前記第１時刻ｔにおける前記第２回転角Ａ_ｙ（ｔ）、及び、前記第１時刻ｔにおける前記第３回転角Ａ_ｚ（ｔ）と、を含み、
前記処理部は、前記回転角の前記時間変化として、第１時間変化ｄＡ_ｘ（ｔ）、第２時間変化ｄＡ_ｙ（ｔ）及び、第３時間変化ｄＡ_ｚ（ｔ）を導出し、
前記第１時間変化ｄＡ_ｘ（ｔ）、前記第２時間変化ｄＡ_ｙ（ｔ）、前記第３時間変化ｄＡ_ｚ（ｔ）、前記第１回転角Ａ_ｘ（ｔ）、前記第２回転角Ａ_ｙ（ｔ）、前記第３回転角Ａ_ｚ（ｔ）、前記第１時刻ｔの前の第２時刻（ｔ－１）における前記第１回転角Ａ_{ｘ（ｔ－１）}、前記第２時刻（ｔ－１）における前記第２回転角Ａ_{ｙ（ｔ－１）}、及び、前記第２時刻（ｔ－１）における前記第３回転角Ａｚ_{（ｔ－１）}は、

【数1】

を満たす、構成９に記載の姿勢角導出装置。

【0079】

（構成１１）
前記姿勢角は、第１姿勢軸に関する第１姿勢角と、第２姿勢軸に関する第２姿勢角と、第３姿勢軸に関する第３姿勢角と、を含み、
前記第２姿勢軸は、前記第１姿勢軸と交差し、
前記第３姿勢軸は、前記第１姿勢軸及び前記第２姿勢軸を含む平面と交差する、構成１０に記載の姿勢角導出装置。

【0080】

（構成１２）
前記第２姿勢軸は、前記第１姿勢軸と直交し、
前記第３姿勢軸は、前記第１姿勢軸及び前記第２姿勢軸と直交する、構成１１に記載の姿勢角導出装置。

【0081】

（構成１３）
前記姿勢角は、前記第２時刻（ｔ－１）における前記第１姿勢角φ_{（ｔ－１）}、前記第２時刻（ｔ－１）における前記第２姿勢角θ_{（ｔ－１）}、及び、前記第２時刻（ｔ－１）における前記第３姿勢角ψ_{（ｔ－１）}を含み、
前記処理部は、前記姿勢角の変化として、第１姿勢角変化ｄφ_（ｔ）、第２姿勢角変化ｄθ_（ｔ）、及び、第３姿勢角変化ｄψ_（ｔ）を導出し、
前記第１姿勢角変化ｄφ_（ｔ）、前記第２姿勢角変化ｄθ_（ｔ）、及び、前記第３姿勢角変化ｄψ_（ｔ）は、

【数2】

を満たす、構成１２に記載の姿勢角導出装置。

【0082】

（構成１４）
前記処理部は、前記更新された前記姿勢角として、前記第１時刻ｔにおける前記第１姿勢角φ_（ｔ）、前記第１時刻ｔにおける前記第２姿勢角θ_（ｔ）、及び、前記第１時刻ｔにおける前記第３姿勢角ψ_（ｔ）を導出し、
前記第１時刻ｔにおける前記第１姿勢角φ_（ｔ）、前記第１時刻ｔにおける前記第２姿勢角θ_（ｔ）、及び、前記第１時刻ｔにおける前記第３姿勢角ψ_（ｔ）は、

【数3】

を満たす、構成１３記載の姿勢角導出装置。

【0083】

（構成１５）
前記姿勢角は、第１クォータニオン値と、第２クォータニオン値と、第３クォータニオン値と、第４クォータニオン値と、を含む、構成１０に記載の姿勢角導出装置。

【0084】

（構成１６）
前記姿勢角は、前記第２時刻（ｔ－１）における前記第１クォータニオン値α_{（ｔ－１）}、前記第２時刻（ｔ－１）における前記第２クォータニオン値β_{（ｔ－１）}、前記第２時刻（ｔ－１）における前記第３クォータニオン値γ_{（ｔ－１）}、及び、前記第２時刻（ｔ－１）における前記第４クォータニオン値δ_{（ｔ－１）}を含み、
前記処理部は、前記姿勢角の変化として、第１クォータニオン値変化ｄα_（ｔ）、第２クォータニオン値変化ｄβ_（ｔ）、第３クォータニオン値変化ｄγ_（ｔ）、及び、第４クォータニオン値変化ｄδ_（ｔ）を導出し、
前記第１クォータニオン値変化ｄα_（ｔ）、前記第２クォータニオン値変化ｄβ_（ｔ）、前記第３クォータニオン値変化ｄγ_（ｔ）、及び、前記第４クォータニオン値変化ｄδ_（ｔ）は、

【数4】

を満たす、構成１５に記載の姿勢角導出装置。

【0085】

（構成１７）
前記処理部は、前記更新された前記姿勢角として、前記第１時刻ｔにおける前記第１クォータニオン値α_（ｔ）、前記第１時刻ｔにおける前記第２クォータニオン値β_（ｔ）、前記第１時刻ｔにおける前記第３クォータニオン値γ_（ｔ）、及び、前記第１時刻ｔにおける前記第４クォータニオン値δ_（ｔ）を導出し、
前記第１時刻ｔにおける前記第１クォータニオン値α_（ｔ）、前記第１時刻ｔにおける前記第２クォータニオン値β_（ｔ）、前記第１時刻ｔにおける前記第３クォータニオン値γ_（ｔ）、及び、前記第１時刻ｔにおける前記第４クォータニオン値δ_（ｔ）は、

【数5】

を満たす、構成１６に記載の姿勢角制御装置。

【0086】

（構成１８）
前記処理部は、前記第１時刻ｔにおける前記第１クォータニオン値α_（ｔ）、前記第１時刻ｔにおける前記第２クォータニオン値β_（ｔ）、前記第１時刻ｔにおける前記第３クォータニオン値γ_（ｔ）、及び、前記第１時刻ｔにおける前記第４クォータニオン値δ_（ｔ）に基づいて、前記更新された前記姿勢角として、前記第１時刻ｔにおける前記第１姿勢角φ_（ｔ）、前記第１時刻ｔにおける前記第２姿勢角θ_（ｔ）、及び、前記第１時刻ｔにおける前記第３姿勢角ψ_（ｔ）を導出し、
前記第１時刻ｔにおける前記第１姿勢角φ_（ｔ）、前記第１時刻ｔにおける前記第２姿勢角θ_（ｔ）、及び、前記第１時刻ｔにおける前記第３姿勢角ψ_（ｔ）は、

【数6】

を満たす、構成１７に記載の姿勢角導出装置。

【0087】

（構成１９）
構成１～１８のいずれか１つに記載の姿勢角導出装置と、
前記角度センサと、
を備えた姿勢角センサ。

【0088】

実施形態によれば、高精度の姿勢角の検出が可能な姿勢角導出装置及び姿勢角センサが提供できる。

【0089】

本願明細書において、「垂直」及び「平行」は、厳密な垂直及び厳密な平行だけではなく、例えば製造工程におけるばらつきなどを含むものであり、実質的に垂直及び実質的に平行であれば良い。

【0090】

以上、具体例を参照しつつ、本発明の実施の形態について説明した。しかし、本発明は、これらの具体例に限定されるものではない。例えば、姿勢角導出装置及び姿勢角センサに含まれる各要素の具体的な構成に関しては、当業者が公知の範囲から適宜選択することにより本発明を同様に実施し、同様の効果を得ることができる限り、本発明の範囲に包含される。

【0091】

また、各具体例のいずれか２つ以上の要素を技術的に可能な範囲で組み合わせたものも、本発明の要旨を包含する限り本発明の範囲に含まれる。

【0092】

その他、本発明の実施の形態として上述した姿勢角導出装置及び姿勢角センサを基にして、当業者が適宜設計変更して実施し得る全ての姿勢角導出装置及び姿勢角センサも、本発明の要旨を包含する限り、本発明の範囲に属する。

【0093】

その他、本発明の思想の範疇において、当業者であれば、各種の変更例及び修正例に想到し得るものであり、それら変更例及び修正例についても本発明の範囲に属するものと了解される。

【0094】

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

【符号の説明】

【0095】

１０…可動部材、 １０Ｆ…固定部、 １０Ｓ…接続部、 ２０Ｍ…対向電極部材、 ５０、５０Ａ～５０Ｃ…姿勢角導出装置、 ５０Ｐ…処理部、 ５１～５４…第１～第４演算部分、 ５５…記憶部、 ５８…取得部、 ６０…基体、 ６０Ａ…構造体、 ６０Ｆ…第１面、 ７０…制御装置、 ９０…対象物、 α、β、γ、δ…クォータニオン値、 θ、φ、ψ…姿勢角、 １１０…角度センサ、 １７０…回路制御部、 １８０…回路、 １８５…駆動装置、 ２１０…姿勢角センサ、 ３１０…電子装置、 Ａｖ１…回転角、 Ａ_ｘ、Ａ_ｙ、Ａ_ｚ…第１～第３回転角、 Ｄ１、Ｄ２…第１、第２方向、 ＤＡ１…回転角、 ＤＡ２、ＤＡ３…姿勢角、 Ｓ１…信号、 ｄα、ｄβ、ｄγ、ｄδ…クォータニオン値変化、 ｄθ、ｄφ、ｄψ…姿勢角変化、 ｄＡ_ｘ、ｄＡ_ｙ、ｄＡ_ｚ…時間変化、 ｇ１…間隙、 ｔ…第１時刻

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】