(57)【要約】  複合センサ（１０）は、互いに独立した３軸の回りの角速度を検出する角速度センサ（３）と、この３軸の方向の加速度を検出する第１の加速度センサ（１）と、第１の加速度センサ（１）と離間した位置に配置され、少なくとも１軸方向の加速度を検出する第２の加速度センサ（２）と、第１の加速度センサ（１）および第２の加速度センサ（２）により検出される加速度に基づいて、角速度センサ（３）により検出される角速度を補正する演算部（４）とを備える。
2020158485.tif 000002
【特許請求の範囲】
【請求項１】
互いに独立した３軸の回りの角速度を検出する角速度センサと、
前記３軸の方向の加速度を検出する第１の加速度センサと、
前記第１の加速度センサと離間した位置に配置され、少なくとも１軸方向の加速度を検出する第２の加速度センサと、
前記第１の加速度センサおよび前記第２の加速度センサにより検出される加速度に基づいて、前記角速度センサにより検出される角速度を補正する演算部と
を備える複合センサ。
【請求項２】
前記第２の加速度センサは、前記第１の加速度センサに対して、前記３軸のうちの特定の１軸方向のみに離間させないように配置される請求項１に記載の複合センサ。
【請求項３】
前記第２の加速度センサは、前記第１の加速度センサに対する前記第２の加速度センサの配置をベクトルh = [ hx 0 0 ]Tとしたとき、前記特定の１軸とベクトルhの両方に直交した方向の加速度を検出する請求項２に記載の複合センサ。
【請求項４】
前記演算部は、前記第１の加速度センサおよび前記第２の加速度センサにより検出される加速度に基づいて、微分を用いることなく被測定物の角加速度を求め、求めた角加速度を利用することで、前記角速度センサにより検出される角速度を補正する請求項１から３のいずれか１項に記載の複合センサ。
【請求項５】
前記演算部は、前記第１の加速度センサに対する前記第２の加速度センサの配置をベクトルh = [ hx 0 0 ]Tとしたとき、式(21)により被測定物のz軸回りの角加速度を求める請求項４に記載の複合センサ。
【数２１】
2020158485.tif 000096
u2 = a1-a2
a1:前記第１の加速度センサにより検出される加速度ベクトル
a2:前記第２の加速度センサにより検出される加速度ベクトル
【請求項６】
前記演算部は、前記角速度センサにより検出される角速度に対して大きさδ1の不感帯を設定し、かつ前記第１の加速度センサおよび前記第２の加速度センサにより検出される加速度に基づいて求めた角加速度に対して大きさδ2の不感帯を設定する請求項４または５に記載の複合センサ。
【請求項７】
角速度センサが、互いに独立した３軸の回りの角速度を検出する角速度検出ステップと、
第１の加速度センサが、前記３軸の方向の加速度を検出する第１の加速度検出ステップと、
前記第１の加速度センサと離間した位置に配置された第２の加速度センサが、少なくとも１軸方向の加速度を検出する第２の加速度検出ステップと、
演算部が、前記第１の加速度検出ステップおよび前記第２の加速度検出ステップで検出される加速度に基づいて、前記角速度検出ステップで検出される角速度を補正する演算ステップと
を備える角速度補正方法。
【請求項８】
互いに独立した２軸の回りの角速度を検出する角速度センサと、
前記２軸方向の各々と垂直な方向となる２軸方向の加速度を検出する第１の加速度センサと、
前記角速度センサの第１の検出軸方向と前記第１の加速度センサの第１の検出軸方向に垂直な方向に離間し、かつ前記角速度センサの第２の検出軸方向と前記第１の加速度センサの第２の検出軸方向に垂直な方向に離間した位置に配置され、前記第１の加速度センサが検出する２軸で構成される平面内に存在し、かつ２軸とは一致しない軸方向の加速度を検出する第２の加速度センサと、
前記第１の加速度センサおよび前記第２の加速度センサにより検出される加速度に基づいて、前記角速度センサにより検出される角速度を補正する演算部と
を備える複合センサ。
【請求項９】
１軸の回りの角速度を検出する角速度センサと、
前記１軸方向と垂直な方向となる１軸方向の加速度を検出する第１の加速度センサと、
前記角速度センサの検出軸方向と前記第１の加速度センサの検出軸方向に垂直な方向に離間した位置に配置され、前記第１の加速度センサの検出軸と同一方向の軸方向の加速度を検出する第２の加速度センサと、
前記第１の加速度センサおよび前記第２の加速度センサにより検出される加速度に基づいて、前記角速度センサにより検出される角速度を補正する演算部と
を備える複合センサ。
【請求項１０】
角速度センサが、互いに独立した２軸の回りの角速度を検出する角速度検出ステップと、
第１の加速度センサが、前記２軸方向の各々と垂直な方向となる２軸方向の加速度を検出する第１の加速度検出ステップと、
第２の加速度センサが、前記角速度センサの第１の検出軸方向と前記第１の加速度センサの第１の検出軸方向に垂直な方向に離間し、かつ前記角速度センサの第２の検出軸方向と前記第１の加速度センサの第２の検出軸方向に垂直な方向に離間した位置に配置され、前記第１の加速度センサが検出する２軸で構成される平面内に存在し、かつ２軸とは一致しない軸方向の加速度を検出する第２の加速度検出ステップと、
演算部が、前記第１の加速度検出ステップおよび前記第２の加速度検出ステップで検出される加速度に基づいて、前記角速度検出ステップで検出される角速度を補正する演算ステップと
を備える角速度補正方法。
【請求項１１】
角速度センサが、１軸の回りの角速度を検出する角速度検出ステップと、
第１の加速度センサが、前記１軸方向と垂直な方向となる１軸方向の加速度を検出する第１の加速度検出ステップと、
第２の加速度センサが、前記角速度センサの検出軸方向と前記第１の加速度センサの検出軸方向に垂直な方向に離間した位置に配置され、前記第１の加速度センサの検出軸と同一方向の軸方向の加速度を検出する第２の加速度検出ステップと、
演算部が、前記第１の加速度検出ステップおよび前記第２の加速度検出ステップで検出される加速度に基づいて、前記角速度検出ステップで検出される角速度を補正する演算ステップと
を備える角速度補正方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本開示は、複合センサおよび角速度補正方法に関する。
【背景技術】
【０００２】
従来より、互いに独立する３つの軸の回りの角速度を検出できるようにジャイロセンサ（角速度センサ）を剛体に搭載することで、静止基準座標系における剛体の情報（剛体の姿勢や回転など）を推定することが提案されている。一般的には、ジャイロセンサは、互いに直交する３つの軸（例えば、ヨー軸、ピッチ軸、ロール軸）の回りの角速度を検出するものである。そして、ジャイロセンサによって別個独立して検出された３つの軸の回りの角速度の情報から、剛体のヨー角、ロール角、ピッチ角の情報や剛体の所定の軸に対する回転の情報などを得ている。このように、ジャイロセンサは、剛体に固定された直交座標系（回転座標系）の各軸の回りの角速度を別個独立して検出するものである。【０００３】
また、従来、複数の加速度センサを用いて角速度を得る技術もある。例えば、特許文献１には、自動車がヨー運動をしたとき、２つの加速度センサの出力の差分からヨー角加速度を求め、これを積分してヨー角速度を求めることが記載されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平６−１１５１４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、角速度センサのみを用いた場合、微分誤差や不感帯の影響を受けてしまうので、正確に角速度を得ることができない。また、特許文献１のように、単に複数の加速度センサのみを用いた場合、重力の影響を排除できない。具体的には、特許文献１に記載される技術によれば、坂道を自動車が登る際にピッチ軸回りに車体の傾斜が変化すると、角速度の出力信号が変動する。【０００６】
本開示は、上記従来の課題を解決するものであって、高精度に角速度を得ることのできる複合センサおよび角速度補正方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本開示にかかる複合センサは、角速度センサと、第１の加速度センサと、第２の加速度センサと、演算部とを備える。前記角速度センサは、互いに独立した３軸の回りの角速度を検出する。前記第１の加速度センサは、前記３軸の方向の加速度を検出する。前記第２の加速度センサは、前記第１の加速度センサと離間した位置に配置され、少なくとも１軸方向の加速度を検出する。前記演算部は、前記第１の加速度センサおよび前記第２の加速度センサにより検出される加速度に基づいて、前記角速度センサにより検出される角速度を補正する。【０００８】
本開示にかかる角速度補正方法は、角速度検出ステップと、第１の加速度検出ステップと、第２の加速度検出ステップと、演算ステップとを備える。前記角速度検出ステップでは、角速度センサが、互いに独立した３軸の回りの角速度を検出する。前記第１の加速度検出ステップでは、第１の加速度センサが、前記３軸の方向の加速度を検出する。前記第２の加速度検出ステップでは、前記第１の加速度センサと離間した位置に配置された第２の加速度センサが、少なくとも１軸方向の加速度を検出する。前記演算ステップでは、演算部が、前記第１の加速度検出ステップおよび前記第２の加速度検出ステップで検出される加速度に基づいて、前記角速度検出ステップで検出される角速度を補正する。【０００９】
本開示にかかる複合センサは、角速度センサと、第１の加速度センサと、第２の加速度センサと、演算部とを備える。前記角速度センサは、互いに独立した２軸の回りの角速度を検出する。前記第１の加速度センサは、前記２軸方向の各々と垂直な方向となる２軸方向の加速度を検出する。前記第２の加速度センサは、前記角速度センサの第１の検出軸方向と前記第１の加速度センサの第１の検出軸方向に垂直な方向に離間し、かつ前記角速度センサの第２の検出軸方向と前記第１の加速度センサの第２の検出軸方向に垂直な方向に離間した位置に配置され、前記第１の加速度センサが検出する２軸で構成される平面内に存在し、かつ２軸とは一致しない軸方向の加速度を検出する。前記演算部は、前記第１の加速度センサおよび前記第２の加速度センサにより検出される加速度に基づいて、前記角速度センサにより検出される角速度を補正する。【００１０】
本開示にかかる複合センサは、角速度センサと、第１の加速度センサと、第２の加速度センサと、演算部とを備える。前記角速度センサは、１軸の回りの角速度を検出する。前記第１の加速度センサは、前記１軸方向と垂直な方向となる１軸方向の加速度を検出する。前記第２の加速度センサは、前記角速度センサの検出軸方向と前記第１の加速度センサの検出軸方向に垂直な方向に離間した位置に配置され、前記第１の加速度センサの検出軸と同一方向の軸方向の加速度を検出する。前記演算部は、前記第１の加速度センサおよび前記第２の加速度センサにより検出される加速度に基づいて、前記角速度センサにより検出される角速度を補正する。【００１１】
本開示にかかる角速度補正方法は、角速度検出ステップと、第１の加速度検出ステップと、第２の加速度検出ステップと、演算ステップとを備える。前記角速度検出ステップでは、角速度センサが、互いに独立した２軸の回りの角速度を検出する。前記第１の加速度検出ステップでは、第１の加速度センサが、前記２軸方向の各々と垂直な方向となる２軸方向の加速度を検出する。前記第２の加速度検出ステップでは、第２の加速度センサが、前記角速度センサの第１の検出軸方向と前記第１の加速度センサの第１の検出軸方向に垂直な方向に離間し、かつ前記角速度センサの第２の検出軸方向と前記第１の加速度センサの第２の検出軸方向に垂直な方向に離間した位置に配置され、前記第１の加速度センサが検出する２軸で構成される平面内に存在し、かつ２軸とは一致しない軸方向の加速度を検出する。前記演算ステップでは、演算部が、前記第１の加速度検出ステップおよび前記第２の加速度検出ステップで検出される加速度に基づいて、前記角速度検出ステップで検出される角速度を補正する。【００１２】
本開示にかかる角速度補正方法は、角速度検出ステップと、第１の加速度検出ステップと、第２の加速度検出ステップと、演算ステップとを備える。前記角速度検出ステップでは、角速度センサが、１軸の回りの角速度を検出する。前記第１の加速度検出ステップでは、第１の加速度センサが、前記１軸方向と垂直な方向となる１軸方向の加速度を検出する。前記第２の加速度検出ステップでは、第２の加速度センサが、前記角速度センサの検出軸方向と前記第１の加速度センサの検出軸方向に垂直な方向に離間した位置に配置され、前記第１の加速度センサの検出軸と同一方向の軸方向の加速度を検出する。前記演算ステップでは、演算部が、前記第１の加速度検出ステップおよび前記第２の加速度検出ステップで検出される加速度に基づいて、前記角速度検出ステップで検出される角速度を補正する。
【発明の効果】
【００１３】
本開示によれば、高精度に角速度を得ることのできる複合センサおよび角速度補正方法を提供することが可能となる。
【図面の簡単な説明】
【００１４】
【図１】第一の実施形態にかかる複合センサの機能ブロック図である。
【図２】第一の実施形態にかかる複合センサが備える第１の加速度センサ、第２の加速度センサ、ジャイロセンサの配置例を示す構成図であり、（ａ）は平面図、（ｂ）は側面図である。
【図３】一般的な複合センサにおける不感帯設定法の説明図である。
【図４】第一の実施形態にかかる複合センサにおける不感帯設定法の説明図である。
【図５】図２に静止基準座標系を追加した構成図である。
【図６】第一の実施形態にかかる複合センサの動作を示すフローチャートである。
【図７】第一の実施形態にかかる複合センサの第２の加速度センサの配置の説明図である。
【図８】第二の実施形態にかかる複合センサが備える第１の加速度センサ、第２の加速度センサ、ジャイロセンサの配置例を示す構成図であり、（ａ）は平面図、（ｂ）は側面図である。
【図９】図８に静止基準座標系を追加した構成図である。
【図１０】第二の実施形態にかかる複合センサの動作を示すフローチャートである。
【図１１】第三の実施形態にかかる複合センサが備える第１の加速度センサ、第２の加速度センサ、ジャイロセンサの配置例を示す構成図であり、（ａ）は平面図、（ｂ）は側面図である。
【図１２】図１１に静止基準座標系を追加した構成図である。
【図１３】第三の実施形態にかかる複合センサの動作を示すフローチャートである。
【発明を実施するための形態】
【００１５】
以下、図面を参照しながら、本実施形態にかかる複合センサおよび角速度補正方法を説明する。なお、図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。
【００１６】
≪複合センサ≫
図１は、第一の実施形態にかかる複合センサ１０の機能ブロック図である。この複合センサ１０は、二つの加速度センサと一つのジャイロセンサを併用した複合センサであって、図１に示すように、第１の加速度センサ１と、第２の加速度センサ２と、角速度センサ３と、演算部４とを備える。以下の説明では、第１の加速度センサ１と、第２の加速度センサ２と、角速度センサ３とを一括して「センサ部Ｓ」という場合がある。
【００１７】
演算部４は、センサ部Ｓの出力に基づいて各種の演算を行うマイコンなどであって、角加速度算出部４Ａ、角速度補正部４Ｂ、不感帯処理部４Ｃ、姿勢角推定部４Ｄ、姿勢角補正部４Ｅなどを備える。角加速度算出部４Ａは、第１の加速度センサ１および第２の加速度センサ２により検出される加速度に基づいて、被測定物の角加速度を算出する。角速度補正部４Ｂは、角加速度算出部４Ａにより算出された角加速度に基づいて、角速度センサ３により検出される角速度を補正する。不感帯処理部４Ｃは、角速度補正部４Ｂにより補正された角速度に対して、角加速度算出部４Ａにより算出された角加速度を考慮した不感帯処理を施す。姿勢角推定部４Ｄは、不感帯処理部４Ｃにより不感帯処理が施された角速度に基づいて、被測定物の姿勢を推定する。姿勢角補正部４Ｅは、第１の加速度センサ１および第２の加速度センサ２により検出される加速度に基づいて、姿勢角推定部４Ｄで用いられる姿勢角を補正する。
【００１８】
以上のように、第一の実施形態にかかる複合センサ１０では、第１の加速度センサ１および第２の加速度センサ２の出力信号に基づいて角速度センサ３の出力信号を正確に補正するようにしている。このような複合センサ１０は、航空機、車両のような移動体の姿勢推定やナビゲーションシステムなど、様々な分野に適用することができる。例えば、自動車に適用した場合は、坂道を自動車が登る際にピッチ軸回りに車体が傾斜しても、高精度に角速度を得て横滑りや横転を防止することが期待できる。
【００１９】
また、第一の実施形態にかかる複合センサ１０は、３軸角速度センサ、３軸加速度センサ、１軸加速度センサの合計７軸で構成可能である。そのため、一般的な複合センサ（３軸ジャイロセンサと３軸加速度センサ）に１軸加速度センサを追加するだけでよく、センサ部Ｓの小型化が期待できる。一般に、角速度センサという場合は１軸の角速度センサを意味し、加速度センサという場合は１軸の加速度センサを意味するが、以下の説明では、軸数を特に区別することなく、単に「角速度センサ」や「加速度センサ」と記載する場合がある。
【００２０】
なお、図１では、角速度補正部４Ｂの後段に不感帯処理部４Ｃを設けた場合を例示しているが、角速度補正部４Ｂの前段に不感帯処理部４Ｃを設けてもよい。もちろん、この場合の不感帯処理部４Ｃも、角加速度算出部４Ａにより算出された角加速度を考慮した不感帯処理を施す。
【００２１】
また、ここでは図示していないが、アナログ信号をデジタル信号に変換するＡ／Ｄ変換回路や、各種データを記憶する記憶部などを備えている点は、一般的なセンサと同様である。
【００２２】
また、第１の加速度センサ１、第２の加速度センサ２、角速度センサ３、演算部４は、一つのチップに集積されてもよいし、複数のチップに設けられてもよい。複数のチップは一つの装置に集約されていてもよいし、複数の装置に備えられていてもよい。
【００２３】
≪姿勢推定技術≫
以下、第一の実施形態にかかる複合センサ１０を具体的に説明する。以下では、二つの加速度センサと一つのジャイロセンサを併用した姿勢推定技術について説明する。
【００２４】
１  はじめに
陸上を運動する移動ロボットや海洋ロボット、飛翔ロボットなどの制御において、現在の姿勢を高精度かつ遅延なく推定する技術は重要である。
【００２５】
既に高精度な姿勢推定が実現できているものの一例としては、飛行機やロケットが挙げられる。それらは高精度に角速度情報を取得できる光ファイバジャイロセンサやリングレーザジャイロセンサ（参考文献１）を用いることで精度の高い姿勢推定が行われているが、これらの光学式ジャイロセンサは高価であり、かつ小型化が困難であるため、容易に利用できるものではない。その一方で、近年ではＭＥＭＳ技術の発展により慣性センサは小型化低価格化が進んでいるが、光学式のものと比べて検出精度が劣っている点が課題である。
【００２６】
１．１  関連技術
慣性センサから得られた情報を用いて姿勢（オイラー角あるいはクォータニオン）を推定するまでの過程を次の四つの段階に分けて考えることにする。
(i)使用する慣性センサおよびその配置方法を決定する。
(ii)各センサの出力情報にキャリブレーションあるいはフィルタリング(カルマンフィルタ、相補フィルタなど)を施すことで、ノイズなどの影響を抑制する。
(iii)センサの出力情報に対して座標変換と積分を行うことで、静止基準座標系から見たときの姿勢を算出する。
(iv)徐々に増大するドリフト誤差を抑制するための措置（地磁気センサとの併用など）を施す。
【００２７】
もちろん、過去に報告されている全ての技術を上記四つの段階に分類できるわけではないが、このような分類を行うことで第一の実施形態および従来技術における位置付けの把握が容易になる。
【００２８】
上記の段階(i)に関する技術としては、複数の加速度計のみを用いて角加速度を算出する方法が参考文献２や参考文献３に開示されている。これらの方法は、ある特定の加速度計の配置方法についてのみ議論されているものであり、コリオリの加速度による影響も無視されている。また、複数の加速度センサから得られる角加速度と、その角速度の関係を非線形状態空間モデルで表現する方法が提案されている（参考文献４）。
【００２９】
段階(ii)に関する技術では、参考文献３の方法とカルマンフィルタを併用することで、より高精度な角加速度が得られることをシミュレーション実験により確認されている（参考文献５）。また、複数の加速度計を円周上に配置することで、センサ出力に含まる誤差の解析と較正の効率化について議論されている（参考文献６）。また、センサ内部の温度変動により生じるオフセット誤差の抑制方法が提案されている（参考文献７）。その他に、センサの周波数特性をモデル化し、それぞれのセンサ出力のうち周波数特性の観点から信頼性が高い信号を相補的に足し合わせる相補フィルタも提案されている（参考文献８，９，１０）。
【００３０】
段階(iii),(iv)に関する技術としては、磁気センサを併用してロール・ピッチ・ヨー角を推定する方法（参考文献１１〜１６）や、クォータニオンを推定する方法（参考文献１７）、局所的な磁場外乱に対策を行った方法（参考文献１８）などが提案されている。
【００３１】
上記のように、ヨー角のドリフト誤差を抑制するための対処法として、磁気センサを併用する方法が数多く提案されている。しかし、磁気センサの周辺に磁界を乱す要因が存在する場合は逆効果となってしまう。大抵の移動ロボットでは、永久磁石と電磁石により駆動する電気モータが複数設置されており、磁気センサから得られる情報の信頼性は低くなることが予想される。そのため、移動ロボットにおいて高精度に姿勢推定を行うためには、精度の高い角速度情報を取得することが重要となる。特に、ヨー角におけるドリフト誤差は重力加速度の方向を用いて修正することができないため、ヨー角の角速度はより高精度に取得できることが望ましい。
【００３２】
ジャイロ・加速度センサを用いて姿勢推定を行う場合、３軸ジャイロセンサと３軸加速度センサが一つずつ用いられるのが一般的である。それに対して第一の実施形態では、二つの３軸加速度センサと一つの３軸ジャイロセンサを併用した姿勢推定技術を提案する。
【００３３】
２  二つの３軸加速度センサと３軸ジャイロセンサを用いた角速度および角加速度の推定
図２は、第一の実施形態にかかる複合センサ１０が備える二つの３軸加速度センサ１，２と３軸ジャイロセンサ３の配置例を示す図であり、（ａ）は平面図、（ｂ）は側面図である。加速度センサ１、加速度センサ２、ジャイロセンサ３は、それぞれ、図１でいう第１の加速度センサ１、第２の加速度センサ２、角速度センサ３に相当するため、同じ符号を用いて説明する。
【００３４】
第一の実施形態では、図２に示すように、二つの３軸加速度センサ１，２と３軸ジャイロセンサ３を剛体Ｂに固定したとき、そのセンサ出力の理論値をベクトル解析により算出する。
【００３５】
２．１  ベクトル解析による理論値算出
二つの加速度センサ１，２を図２のように平行移動した位置に配置したとき、加速度センサ１および加速度センサ２から得られる加速度ベクトルa1,a2をそれぞれ
【００３６】
【数１】
2020158485.tif 000003
【００３７】
【数２】
2020158485.tif 000004
【００３８】
とする。また、加速度センサ１から加速度センサ２を見たときの位置ベクトルhを
【００３９】
【数３】
2020158485.tif 000005
【００４０】
とし、回転中心Ｏから加速度センサ１および加速度センサ２を見たときの位置ベクトルr1,r2をそれぞれ
【００４１】
【数４】
2020158485.tif 000006
【００４２】
【数５】
2020158485.tif 000007
【００４３】
とする。回転中心Ｏから剛体Ｂを見たときの角速度ベクトル（ジャイロセンサ３から得られる角速度ベクトル）ωを
【００４４】
【数６】
2020158485.tif 000008
【００４５】
とし、剛体Ｂ（センサ座標系Σxyz）から見たときの剛体Ｂに働く重力加速度ベクトルgを
【００４６】
【数７】
2020158485.tif 000009
【００４７】
とする。このとき、加速度センサ１，２から得られる加速度ベクトルa1,a2は
【００４８】
【数８】
2020158485.tif 000010
【００４９】
【数９】
2020158485.tif 000011
【００５０】
となる（以下、時間微分をドットではなくd/dtで表す場合がある）。上式のd2r1/dt2,d2r2/dt2は並進加速度、dω/dt×r1,dω/dt×r2は接線加速度、2ω×dr1/dt,2ω×dr2/dtはコリオリの加速度、ω×(ω×r1),ω×(ω×r2)は遠心加速度を表している。
【００５１】
式(8)、式(9)の差をとると、
【００５２】
【数１０】
2020158485.tif 000012
【００５３】
となる。上式のΩはベクトルωの外積行列であり、次のように表される。
【００５４】
【数１１】
2020158485.tif 000013
【００５５】
行列Ωは交代行列(ΩT = -Ω)であり、その固有値はすべて純虚数または0（非正則）となる。
【００５６】
さらに、
【００５７】
【数１２】
2020158485.tif 000014
【００５８】
【数１３】
2020158485.tif 000015
【００５９】
とすると、式(10)は、
【００６０】
【数１４】
2020158485.tif 000016
【００６１】
と表記できる。
【００６２】
２．２  加速度センサの配置をh = [ hx 0 0 ]T としたとき
加速度センサ１に対する加速度センサ２の配置を
【００６３】
【数１５】
2020158485.tif 000017
【００６４】
とする。このとき、
【００６５】
【数１６】
2020158485.tif 000018
【００６６】
【数１７】
2020158485.tif 000019
【００６７】
であり、これらを式(14)に代入することにより、
【００６８】
【数１８】
2020158485.tif 000020
【００６９】
【数１９】
2020158485.tif 000021
【００７０】
【数２０】
2020158485.tif 000022
【００７１】
となる。式(19)より
【００７２】
【数２１】
2020158485.tif 000023
【００７３】
となる。上式を用いて得られるdωz/dtは、ジャイロセンサ３のz軸方向の出力ωzを微分して得たものではない。そのため、式(21)により得られたdωz/dtとジャイロセンサ３の出力から得られたωzをカルマンフィルタにかけることで、被測定物のヨー角の角速度を高精度に取得できることが期待される。
【００７４】
また、u2=a2y-a1yであるから、ヨー角の角加速度dωz/dtを算出するためには、加速度センサ１，２のy軸方向（ヨー軸とベクトルhの両方に直交した方向）の精度が重要であることがわかる。
【００７５】
３  最小二乗法による個体差補正
前述した理論では、観測ノイズやセンサの特性による誤差の影響が考慮されていなかった。しかし、実際に加速度センサ１，２から検出される加速度ベクトルsa1,sa2には誤差が含まれる。加速度センサ１および加速度センサ２に含まれる誤差をそれぞれ
【００７６】
【数２２】
2020158485.tif 000024
【００７７】
【数２３】
2020158485.tif 000025
【００７８】
とすると、
【００７９】
【数２４】
2020158485.tif 000026
【００８０】
【数２５】
2020158485.tif 000027
【００８１】
となる。a1,a2は加速度センサ１，２から得られる理論上の加速度ベクトルであり、sa1,sa2は実際に加速度センサ１，２から出力される誤差を含んだ加速度ベクトルである。
【００８２】
ω=0,dω/dt=0のとき、式(24)、式(25)の差をとると、
【００８３】
【数２６】
2020158485.tif 000028
【００８４】
となり、Δa1-Δa2は加速度センサ１と加速度センサ２の個体差と解釈することもできる。そこで、加速度センサ２からの出力sa2にある適当な射影変換行列Qをかけて個体差を補正することを考える。
【００８５】
ω=0,dω/dt=0のとき、二つの加速度センサ１，２から時刻tにおいて得られる加速度情報をsa1(t),sa2(t)とする。このとき、
【００８６】
【数２７】
2020158485.tif 000029
【００８７】
を満たすような行列QとベクトルΔα(t)が存在する。Q=I（単位行列）のときは、Δα(t)=Δa1(t)-Δa2(t)となり、式(27)は式(26)と一致する。ΔαTΔαを最小にするような行列Qを求め、加速度センサ２から得られる加速度情報をQsa2(t)として扱うことで、個体差を補正することができる。
【００８８】
二つの加速度センサ１，２を同じ姿勢で静止させているとき（同じ重力加速度を受けているとき）、時刻t1…tnに取得した加速度情報を用いて行列A,Bを次のように定める。
【００８９】
【数２８】
2020158485.tif 000030
【００９０】
【数２９】
2020158485.tif 000031
【００９１】
このとき、行列BTBが正則であれば、ΔαTΔαを最小にするような行列Qは
【００９２】
【数３０】
2020158485.tif 000032
【００９３】
となる。
【００９４】
４  加速度センサの精度とセンサ間距離の関係性
前述したセンサ系の小型化を行う場合、二つの加速度センサ１，２間の距離||h||は、より小さいことが望ましい。理論上は||h||を限りなく小さくすることが可能だが、実際にはノイズなどの影響により||h||の短縮化には限界がある。そこで第一の実施形態では、加速度センサ１，２に含まれる観測誤差Δa1,Δa2とセンサ間距離||h||の関係性について説明する。加速度センサ１，２の出力に含まれる誤差の差を
【００９５】
【数３１】
2020158485.tif 000033
【００９６】
とすると、加速度センサ１，２の出力から得られる加速度ベクトルsa1,sa2の差は、
【００９７】
【数３２】
2020158485.tif 000034
【００９８】
となる。このとき、誤差Δuを考慮した形で式(21)を表すと
【００９９】
【数３３】
2020158485.tif 000035
【０１００】
となる。上式より、hxを大きくすると誤差Δu2による影響が小さくなり、逆にhxを小さくすると誤差による影響が大きくなる。よって、hxを小さくすることと、誤差による影響を抑制することはトレードオフの関係になることが明らかとなった。
【０１０１】
５  角加速度を併用した不感帯の設定法
角速度ωに対して、大きさδの不感帯を設けた場合、角速度の大きさがδ以下の領域においては正しく角速度を検出できない（図３）。しかし、図３に示すように、角速度の大きさがδ以下の領域においても傾きは大きく出ているので、角加速度は大きな値を示すことになる。そのため、角速度と角加速度の両方を併用した不感帯の設定法を用いることで、図３に示される点線部分が検出され、上記の問題を解決できる。図４にその擬似コードを示す。
【０１０２】
図４に示すように、第一の実施形態では、|ω|<δ1かつ|dω/dt|<δ2の条件を満たす場合はω=0とし、それ以外の場合は何もしない。このような不感帯設定法は、小刻みに静止と運動を繰り返す場合や低角速度時に対して、特にその効果を期待できる。
【０１０３】
用途によっては、低角速度時に取りこぼしなく角速度情報を取得することは重要となる。例えば、二輪駆動移動ロボットを直進させたいとする。このとき、左右の駆動輪の個体差により、機体は徐々に旋回してしまう。この問題を慣性センサを用いた姿勢推定技術を用いて解決しようとした場合、低角速度を取得する必要がある。
【０１０４】
６  姿勢角への変換方法
また、図５は図２へ静止基準座標系ΣXYZを追加したものであり、静止基準座標系ΣXYZから剛体Ｂを見たときの姿勢（ロール、ピッチ、ヨー角）を表す。この静止基準座標系に対し、剛体Ｂ上の座標系は運動座標系ということができる。静止基準座標系ΣXYZから剛体Ｂを見たときの姿勢（ロール、ピッチ、ヨー角）を表すベクトルを
【０１０５】
【数３４】
2020158485.tif 000036
【０１０６】
とする。加速度センサ１，２が重力加速度のみを検知している場合には、
【０１０７】
【数３５】
2020158485.tif 000037
【０１０８】
【数３６】
2020158485.tif 000038
【０１０９】
が成り立つ。すなわち、ロール角θRとピッチ角θPを加速度センサ１，２の出力のみから求めることができる。また、加速度センサ１，２が重力加速度のみを検知している場合、次式が成り立つ。
【０１１０】
【数３７】
2020158485.tif 000039
【０１１１】
ただし、逆は成り立つとは限らない。すなわち、式(37)を満たしていても、加速度センサ１，２が重力加速度のみを検知しているとは限らない。例えば、センサ系が重力方向に向かって2gの加速度で落下している場合が挙げられる。しかし、そのような現象が起こることは稀であることから、実用上は式(37)を用いて重力加速度のみを検知しているかどうかを判定しても問題ない場合が多い。また、cosθP≠0のとき
【０１１２】
【数３８】
2020158485.tif 000040
【０１１３】
となる（参考文献１９，２０）。上式の導出法については、参考文献２１に示されている。式(38)により得られた姿勢角の微分値を積分することで、姿勢角を求めることができる。第一の実施形態ではジャイロセンサ３からの出力を姿勢角の微分値に変換する方法を示したが、ジャイロセンサ３からの出力をクォータニオンの微分値に変換し、現在の姿勢を表すクォータニオンを求める方法もある。
【０１１４】
７  動作
図６は、第一の実施形態にかかる複合センサ１０の動作を示すフローチャートである。以下、図６を参照しながら上述した方法を用いて姿勢角を求める動作について説明する。
【０１１５】
まず、ジャイロセンサ３により角速度ベクトルωが検出され、加速度センサ１により加速度ベクトルa1が検出され、加速度センサ２により加速度ベクトルa2が検出される（ステップＳ１，Ｓ２，Ｓ３）。ジャイロセンサ３の出力、加速度センサ１の出力、加速度センサ２の出力は、後段の演算部４に入力されるようになっている。
【０１１６】
次いで、演算部４は、ジャイロセンサ３の出力、加速度センサ１の出力、加速度センサ２の出力に基づいて、式(21)を用いてヨー角の角加速度dωz/dtを算出する（ステップＳ４）。そして、式(21)により得られたdωz/dtとジャイロセンサ３の出力から得られたωzをカルマンフィルタにかけることで、ジャイロセンサ３の出力（角速度）を補正する（ステップＳ５）。ここではカルマンフィルタを例示しているが、角速度を補正するアルゴリズムは限定されるものではない。
【０１１７】
また、演算部４は、角加速度を考慮した不感帯処理を施す（ステップＳ６）。具体的には、|ω|<δ1かつ|dω/dt|<δ2の条件を満たす場合はω=0とし、それ以外の場合は何もしない。
【０１１８】
更に、演算部４は、式(38)により得られた姿勢角の微分値を積分することで、姿勢角（ロール角、ピッチ角、ヨー角）を求める（ステップＳ７→Ｓ８）。
【０１１９】
一方、演算部４は、加速度センサ１の出力、加速度センサ２の出力に基づいて、静止判定を行う（ステップＳ９）。具体的には、被測定物が静止しているときは、式(35)(36)によりロール、ピッチ角を算出し、ステップＳ７で用いるロール、ピッチ角を補正する（ステップＳ１０→Ｓ１１）。
【０１２０】
８  まとめ
上述した姿勢推定技術の特徴をまとめると、以下のようになる。
（１）最低で、３軸ジャイロセンサ、３軸加速度センサ、１軸加速度センサの合計７軸を用いることで適用可能である。
（２）加速度センサを一つ追加するためには、式(21)を導出する必要がある。
（３）加速度センサを一つ多く用いることで、被測定物の角加速度を、微分を用いることなく求めることが可能である。一般に、微分によって得られる情報はノイズなどの影響によって瞬間的に大きな誤差が生じることが知られている。
（４）求めた角加速度を利用することで、ジャイロセンサから得られる角速度に補正（カルマンフィルタ）を施すことが可能となり、より高精度に被測定物の角速度を求められることが期待される。
（５）角加速度と併用した不感帯の適用により、低角速度時における角速度情報の取りこぼしを防げることが期待される。
【０１２１】
以上のように、第一の実施形態にかかる複合センサ１０は、角速度センサ３と、第１の加速度センサ１と、第２の加速度センサ２と、演算部４とを備える。角速度センサ３は、互いに独立した３軸の回りの角速度を検出する。第１の加速度センサ１は、この３軸の方向の加速度を検出する。第２の加速度センサ２は、第１の加速度センサ１と離間した位置に配置され、少なくとも１軸方向の加速度を検出する。演算部４は、第１の加速度センサ１および第２の加速度センサ２により検出される加速度に基づいて、角速度センサ３により検出される角速度を補正する。これにより、第１の加速度センサ１および第２の加速度センサ２の出力信号に基づいて角速度センサ３の出力信号が補正されるため、高精度に角速度を得ることのできる複合センサ１０を提供することが可能となる。
【０１２２】
ここで、第２の加速度センサ２は、第１の加速度センサ１に対して、３軸のうちの特定の１軸方向のみに離間させないように配置されるのが望ましい。この配置条件を満たせば、第２の加速度センサ２に１軸加速度センサを用いた場合でも、第１の加速度センサ１および第２の加速度センサ２の出力信号に基づいて角速度センサ３の出力信号を補正することが可能である。
【０１２３】
また、第２の加速度センサ２は、第１の加速度センサ１に対する第２の加速度センサ２の配置をベクトルh = [ hx 0 0 ]Tとしたとき、特定の１軸とベクトルhの両方に直交した方向の加速度を検出するのが望ましい。例えば、特定の１軸（z軸）回りの角速度を求めたい場合は、特定の１軸（z軸）とベクトルhの両方に直交した方向（y軸方向）を精度よく検出すれば、高精度に角速度センサ３の出力信号を補正することが可能である。
【０１２４】
また、演算部４は、第１の加速度センサ１および第２の加速度センサ２により検出される加速度に基づいて、微分を用いることなく被測定物の角加速度を求め、求めた角加速度を利用することで、角速度センサ３により検出される角速度を補正するのが望ましい。微分を用いることなく被測定物の角加速度を求めれば、ノイズなどの影響を受けにくい効果がある。
【０１２５】
また、演算部４は、第１の加速度センサ１に対する第２の加速度センサ２の配置をベクトルh = [ hx 0 0 ]Tとしたとき、式(21)により被測定物のz軸回りの角加速度を求めるのが望ましい。ベクトルh = [ hx 0 0 ]Tとしたときは、センサ部Ｓの配置が簡単になり、また式(21)のような簡単な演算により被測定物のz軸回りの角加速度を求めることが可能である。
【０１２６】
また、演算部４は、角速度センサ３により検出される角速度に対して大きさδ1の不感帯を設定し、かつ第１の加速度センサ１および第２の加速度センサ２により検出される加速度に基づいて求めた角加速度に対して大きさδ2の不感帯を設定するのが望ましい。このような不感帯設定法は、小刻みに静止と運動を繰り返す場合や低角速度時に対して、特にその効果を期待できる。
【０１２７】
また、第一の実施形態にかかる角速度補正方法は、角速度検出ステップと、第１の加速度検出ステップと、第２の加速度検出ステップと、演算ステップとを備える。角速度検出ステップでは、角速度センサ３が、互いに独立した３軸の回りの角速度を検出する。第１の加速度検出ステップでは、第１の加速度センサ１が、この３軸の方向の加速度を検出する。第２の加速度検出ステップでは、第１の加速度センサ１と離間した位置に配置された第２の加速度センサ２が、少なくとも１軸方向の加速度を検出する。演算ステップでは、演算部４が、第１の加速度検出ステップおよび第２の加速度検出ステップで検出される加速度に基づいて、角速度検出ステップで検出される角速度を補正する。これにより、第１の加速度センサ１および第２の加速度センサ２の出力信号に基づいて角速度センサ３の出力信号が補正されるため、高精度に角速度を得ることのできる角速度補正方法を提供することが可能となる。
【０１２８】
９  参考文献
以下、参考文献について記載する。
［参考文献１］大野有孝: 航空宇宙用姿勢検出センサ（ジャイロ）の技術動向, 精密工学会誌, vol.75, no.1,pp.159-160, 2009.
［参考文献２］Peter G. Martin, Gregory W. Hall, Jeff R. Crandall, and Walter D. Pilkey: Measuring the Acceleration of a Rigid Body, Shock and Vibration, vol.5, no.4, pp.211-224, 1998.
［参考文献３］A. J. Padgaonkar, K. W. Krieger and A. I. King: Measurement of Angular Acceleration of a Rigid Body Using Linear Accelerometers, ASME Journal of Applied Mechanics, vol.42, no.3, pp.552-556, 1975.
［参考文献４］Patrick Schopp, Hagen Graf, Michael Maurer, Michailas Romanovas, Lasse Klingbeil, and Yiannos Manoli: Observing Relative Motion With Three Accelerometer Triads, IEEE Transactions on Instrumentation and Measurement, vol.63, no.12, pp.3137-3151, 2014.
［参考文献５］太田憲，小林一敏: 加速度計を用いたスポーツにおける角速度・角加速度計測, 計測自動制御学会論文集, vol.30, no.12, pp.1442-1448, 1994.
［参考文献６］三村宣治, 小野寺良二, 小松原亮: 複数加速度計を使用した６軸加速度センサシステムの誤差解析と較正の効率化について, 日本機械学会論文集(C 編), vol.74, no.739, pp.134-140, 2008.
［参考文献７］藤田孔明，中原充也，佐藤祐之，寺田篤人: ロボット用高精度モーションセンシングユニット, Panasonic Technical Journal, vol.63, no.2, pp.30-34, 2017.
［参考文献８］杉原知道，舛屋賢，山本元司: 三次元高精度姿勢推定のための慣性センサの線形・非線形特性分離に基づいた相補フィルタ, 日本ロボット学会誌, vol.31, no.3, pp.251-262, 2013.
［参考文献９］A. El Hadri and A. Benallegue: Attitude estimation with gyros-bias compensation using low-cost sensors, Proceeding of the 48th Conference on Decision and Control, pp.8077-8082, 2009.
［参考文献１０］A. J. Baerveldt and R. Klang: A low-cost and low-weight attitude estimation system for an autonomous helicopter, Intelligent Engineering System, pp.391-395, 1997.
［参考文献１１］Jurman D, Jankovec M, Kamnik R, Topic M: Calibration and data fusion solution for the miniature attitude and heading reference system, Sensors and Actuators A, vol.138, no.2, pp.411-420, 2007.
［参考文献１２］Foxlin E: Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter, IEEE Proceedings of VRAIS, pp.185-194, 1996.
［参考文献１３］Vahanay J, Aldon M J, Fournier A: Mobile robot attitude estimation by fusion of inertial data, Proceedings of the IEEE International Conference on Robotics and Automation, pp.277-282, 1993.
［参考文献１４］Ying-Chih Lai, Shau-Shiun Jan and Fei-Bin Hsiao: Development of a Low-Cost Attitude and Heading Reference System Using a Three-Axis Rotating Platform, sensors, vol.10, no.4, pp.2472-2491, 2010.
［参考文献１５］Tae Suk Yoo, Sung Kyung Hong, Hyok Min Yoon and Sungsu Park: Gain-Scheduled Complementary Filter Design for a MEMS Based Attitude and Heading Reference System, sensors, vol.11, no.4,pp.3816-3830, 2011.
［参考文献１６］廣瀬圭, 土岐仁, 近藤亜希子: 慣性センサ・地磁気センサを用いたスポーツにおける姿勢計測に関する研究, スポーツ産業学研究, vol.22, no.2,pp.255-262, 2012.
［参考文献１７］Sabatini A. M.: Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing, IEEE Transactions on Biomedical Engineering, vol.53, no.7, pp.1346-1356, 2006.
［参考文献１８］Roetenberg D, Luinge H J, Baten C T, Veltink P H: Compensation of Magnetic Disturbances Improves Inertial and Magnetic Sensing of Human Body Segment Orientation, IEEE transaction on Neural Systems and Rehabilitation Engineering, vol.13, no.3, pp.395-405, 2005.
［参考文献１９］廣瀬圭，近藤亜希子: 人間工学のための計測手法, 日本人間工学会, vol.50, no.4, pp.182-190, 2014.
［参考文献２０］Cooke J. M., Zyda M. J., Pratt D. R., McGhee R. B.: Flight simulation dynamic modeling using quaternions, NPSNET, vol.1, no.4, pp.404-420, 1994.
［参考文献２１］長谷川律雄: 回転表現のキネマティックス方程式の一般的な導出方法, 計測自動制御学会論文集, vol.40, no.11, pp.1160-1162, 2004.
【０１２９】
１０  第１の加速度センサに対する第２の加速度センサの配置
複合センサを用いて剛体の回転を考える場合には、剛体に固定された直交座標系を基準座標系とした上で各軸の回りの回転を別個独立して考えればよい。そこで、以下では、剛体に固定された互いに直交する３つの軸をそれぞれx軸、y軸、z軸とし、それぞれの軸の回りの回転を考えることによって、２つの加速度センサから角加速度を求める方法について説明する。
【０１３０】
１０．１  前提条件
まず、前提条件として、直交座標系の１つの軸の回りの回転の基本的な性質について説明する。以下では、主にz軸の回りの回転を用い、１つの軸の回りの回転の基本的な性質を説明する。
【０１３１】
図７に示すように、空間上の点Ｒは、一般的に、原点等の基準点から見たときのベクトルr=(rx,ry,rz)で表すことができる。ここで、ベクトルrとz軸とがなす角をθ、z軸に沿って見た状態におけるベクトルr（ベクトルrのxy平面（z軸に直交する平面）への射影ベクトル）とx軸とがなす角をφとすると、式(39)のように表すことができる。
【０１３２】
【数３９】
2020158485.tif 000041
【０１３３】
したがって、剛体Ｂの回転中心Ｏから加速度センサ１を見たときの位置ベクトルをr1=(r1x,r1y,r1z)、ベクトルr1とz軸（求めたい角加速度成分に対応する軸）とがなす角をθ1、z軸に沿って見た状態におけるベクトルr1（ベクトルr1のxy平面(z軸に直交する平面)への射影ベクトル）とx軸とがなす角をφ1とした場合、式(40)のように表すことができる。
【０１３４】
【数４０】
2020158485.tif 000042
【０１３５】
また、剛体Ｂの回転中心Ｏから加速度センサ２を見たときの位置ベクトルをr2=(r2x,r2y,r2z)、ベクトルr2とz軸（求めたい角加速度成分に対応する軸）とがなす角をθ2、z軸に沿って見た状態におけるベクトルr2（ベクトルr2のxy平面（z軸に直交する平面）への射影ベクトル）とx軸とがなす角をφ2とした場合、式(41)のように表すことができる。
【０１３６】
【数４１】
2020158485.tif 000043
【０１３７】
また、加速度センサ１から加速度センサ２を見たときの位置ベクトルをh=(hx,hy,hz)とすると、式(42)、すなわち式(43)のように表すことができる。
【０１３８】
【数４２】
2020158485.tif 000044
【０１３９】
【数４３】
2020158485.tif 000045
【０１４０】
そして、ベクトルhとz軸（求めたい角加速度成分に対応する軸）とがなす角をθ3、z軸に沿って見た状態におけるベクトルh（ベクトルhのxy平面（z軸に直交する平面）への射影ベクトル）とx軸とがなす角をφ3とすると、式(44)のように表すことができる。
【０１４１】
【数４４】
2020158485.tif 000046
【０１４２】
ここで、剛体Ｂを所定の位置（h=(hx,hy,hz)となる状態）からz軸回りに角度φだけ回転させると、ベクトルr1=(r1x,r1y,r1z)およびベクトルr2=(r2x,r2y,r2z)は、それぞれ、式(45)のベクトルr1',r2'に移動する。
【０１４３】
【数４５】
2020158485.tif 000047
【０１４４】
したがって、剛体Ｂをz軸回りに角度φだけ回転させると、加速度センサ１の位置が(r1x,r1y,r1z)から(r1xcosφ-r1ysinφ,r1xsinφ+r1ycosφ,r1z)へと移動し、加速度センサ２の位置が(r2x,r2y,r2z)から(r2xcosφ-r2ysinφ,r2xsinφ+r2ycosφ,r2z)へと移動することになる。このとき、r2'-r1'は、式(46)のようになる。
【０１４５】
【数４６】
2020158485.tif 000048
【０１４６】
ここで、r2x-r1x=hxであり、r2y-r1y=hyであり、r2z-r1z=hzである。また、r2'-r1'は、剛体Ｂをz軸回りに角度φだけ回転させた状態における加速度センサ１から加速度センサ２を見たときの位置ベクトルである。そこで、h'=r2'-r1'とすると、式(47)のようになる。
【０１４７】
【数４７】
2020158485.tif 000049
【０１４８】
したがって、剛体Ｂをz軸回りに角度φだけ回転させると、位置ベクトルh=(hx,hy,hz)は、位置ベクトルh'=(hxcosφ-hysinφ,hxsinφ+hycosφ,hz)へと移動することになる。ここで、上述したように、sinφ,cosφは式(48)の通りであるため、z軸に沿って見た状態におけるベクトルhとx軸とがなす角φは、hz成分を用いずに、hx成分およびhy成分のみで表すことができる。
【０１４９】
【数４８】
2020158485.tif 000050
【０１５０】
このように、剛体Ｂをz軸回りに角度φだけ回転させると、位置ベクトルh=(hx,hy,hz)は、位置ベクトルh'=(hxcosφ-hysinφ,hxsinφ+hycosφ,hz)へと移動することから、剛体Ｂをz軸回りに回転させた場合には、２つの加速度センサ１，２のx軸方向の差であるhx成分およびy軸方向の差であるhy成分は変化するが、z軸方向の差であるhz成分は変化しないことが分かる。すなわち、剛体Ｂをz軸回りに回転させた場合における角度φは、z軸方向の差であるhz成分に依存しない値であることが分かる。
【０１５１】
そして、剛体Ｂをz軸回りに回転させると、角度φは時間によって変化する。そこで、z軸回りの角速度をωz、時刻t=0における角度φをφ=0とすると、時刻tにおける角度φはφ=ωztとなるため、位置ベクトルhは、式(49)のようになる。
【０１５２】
【数４９】
2020158485.tif 000051
【０１５３】
このωztは、hz成分に依存しない角度φであり、z軸回りの角速度ωzは角度φの１階時間微分であるから、z軸方向の差であるhz成分は、剛体Ｂをz軸回りに回転させた場合における角度φの変化（時間変化）にも影響を与えない成分であることが分かる。そして、z軸回りの角加速度dωz/dtは、z軸回りの角速度ωzの１階時間微分であり、角度φの２階時間微分である。したがって、z軸方向の差であるhz成分は、剛体Ｂをz軸回りに回転させた場合における角速度ωzの変化（角加速度dωz/dt）にも影響を与えない成分であることが分かる。このことから、z軸回りの角加速度dωz/dtは、２つの加速度センサ１，２（第１の加速度センサ１および第２の加速度センサ２）のz軸方向の差であるhz成分には依存しない値であり、z軸回りの角加速度dωz/dtもhz成分を用いずに表すことができる。
【０１５４】
なお、y軸の場合も同様に、y軸回りの角加速度dωy/dtは、２つの加速度センサ１，２のy軸方向の差であるhy成分には依存しない値であることが分かる。また、x軸の場合も同様に、x軸回りの角加速度dωx/dtは、２つの加速度センサ１，２のx軸方向の差であるhx成分には依存しない値であることが分かる。
【０１５５】
以上より、剛体Ｂに固定された直交座標系の各軸の回りの回転を考える場合には、回転軸方向の成分を考慮しなくても差し支えないことが分かる。
【０１５６】
１０．２  ２つの加速度センサを用いた角加速度の求め方
次に、上記１０．１で説明した前提条件のもと、２つの加速度センサ１，２を用いた角加速度の求め方について説明する。
【０１５７】
まず、上述したように、剛体Ｂの回転中心Ｏから加速度センサ１を見たときの位置ベクトルは、式(50)のように表すことができる。また、剛体Ｂの回転中心Ｏから加速度センサ２を見たときの位置ベクトルは、式(51)のように表すことができる。さらに、加速度センサ１から加速度センサ２を見たときの位置ベクトルは、式(52)のように表すことができる。
【０１５８】
【数５０】
2020158485.tif 000052
【０１５９】
【数５１】
2020158485.tif 000053
【０１６０】
【数５２】
2020158485.tif 000054
【０１６１】
また、加速度センサ１から得られる加速度ベクトルを式(53)とし、加速度センサ２から得られる加速度ベクトルを式(54)とする。そして、加速度ベクトルa2と加速度ベクトルa1との差を式(55)とする。
【０１６２】
【数５３】
2020158485.tif 000055
【０１６３】
【数５４】
2020158485.tif 000056
【０１６４】
【数５５】
2020158485.tif 000057
【０１６５】
具体的には、式(56)、すなわち式(57)とする。
【０１６６】
【数５６】
2020158485.tif 000058
【０１６７】
【数５７】
2020158485.tif 000059
【０１６８】
また、ジャイロセンサ３から得られた角速度ベクトルを式(58)とし、剛体Ｂから見たときの剛体Ｂに働く重力加速度ベクトルを式(59)とする。このとき、各加速度センサ１，２から得られる加速度ベクトル（加速度ベクトルa1および加速度ベクトルa2）は、式(8)(9)のようになり、式(10)〜式(14)のようになる。
【０１６９】
【数５８】
2020158485.tif 000060
【０１７０】
【数５９】
2020158485.tif 000061
【０１７１】
ここで、式(12)より、式(60)のようになる。
【０１７２】
【数６０】
2020158485.tif 000062
【０１７３】
したがって、式(61)のようになる。
【０１７４】
【数６１】
2020158485.tif 000063
【０１７５】
また、式(62)のようになる。
【０１７６】
【数６２】
2020158485.tif 000064
【０１７７】
ここで、式(63)とすると、式(64)のようになる。
【０１７８】
【数６３】
2020158485.tif 000065
【０１７９】
【数６４】
2020158485.tif 000066
【０１８０】
なお、式(65)であるから、式(66)のようになる。
【０１８１】
【数６５】
2020158485.tif 000067
【０１８２】
【数６６】
2020158485.tif 000068
【０１８３】
また、ベクトルhの外積行列は式(67)となるので、角加速度ベクトルdω/dtを式(68)とすると、式(69)のようになる。
【０１８４】
【数６７】
2020158485.tif 000069
【０１８５】
【数６８】
2020158485.tif 000070
【０１８６】
【数６９】
2020158485.tif 000071
【０１８７】
したがって、式(70)は式(71)、すなわち式(72)と表すことができる。
【０１８８】
【数７０】
2020158485.tif 000072
【０１８９】
【数７１】
2020158485.tif 000073
【０１９０】
【数７２】
2020158485.tif 000074
【０１９１】
したがって、式(73)のようになる。
【０１９２】
【数７３】
2020158485.tif 000075
【０１９３】
これを成分ごとに分けると、式(74)のようになる。
【０１９４】
【数７４】
2020158485.tif 000076
【０１９５】
１０．３  z軸回りの角加速度dωz/dtの求め方
z軸回りの角加速度dωz/dtは、式(74)中の１〜３を用いて下記のように考えることで求めることができる。
【０１９６】
まず、上述したように、２つの加速度センサ１，２のz軸方向の差であるhz成分は、z軸回りの角加速度dωz/dtに寄与しない成分である。したがって、h=(hx,hy,hz)とした場合におけるz軸回りの角加速度dωz/dtは、h=(hx,hy,0)とした場合におけるz軸回りの角加速度dωz/dtを求めることで得ることができる。そこで、h=(hx,hy,0)の場合におけるz軸回りの角加速度dωz/dtを求めることにする。具体的には、式(74)中の１〜３のhzに0を代入する。こうすれば、式(74)中の１〜３は、それぞれ、式(75)のようになる。
【０１９７】
【数７５】
2020158485.tif 000077
【０１９８】
そして、２´×hx-１´×hyを計算して、ωz2を消去すると、式(76)のようになる。
【０１９９】
【数７６】
2020158485.tif 000078
【０２００】
したがって、式(77)のようになる。
【０２０１】
【数７７】
2020158485.tif 000079
【０２０２】
以上より、２つの加速度センサ１，２をxyz空間上で、h=(hx,hy,hz)となるように離間配置した場合におけるz軸回りの角加速度dωz/dtは、式(78)のように表すことができる。
【０２０３】
【数７８】
2020158485.tif 000080
【０２０４】
ここで、h=(0,0,hz)とした場合、h=(0,0,hz)を式(74)中の１〜３に代入すると、式(79)となってしまうため、２つの加速度センサ１，２を用いてz軸回りの角加速度dωz/dtが求められないことが分かる。
【０２０５】
【数７９】
2020158485.tif 000081
【０２０６】
なお、h=(0,0,hz)の場合には、式(78)の分母(hx2+hy2)が0になってしまうので、このことからも、h=(0,0,hz)の場合には、２つの加速度センサ１，２を用いてz軸回りの角加速度dωz/dtを求めることができないということが分かる。このように、式(78)より、加速度センサ２を加速度センサ１に対してz方向のみに離間させた場合、２つの加速度センサ１，２では、z軸回りの角加速度dωz/dtが求められないことが分かる。また、式(78)から、２つの加速度センサ１，２を、xy平面上で離間配置した場合（h=(hx,hy,0)の場合）も含め、xyz空間上で離間配置した場合（h=(hx,hy,hz)の場合）には、z軸回りの角加速度dωz/dtは、ジャイロセンサ３から得られたx軸回りの角速度ωxおよびy軸回りの角速度ωyと、２つの加速度センサ１，２から得られたx方向成分の加速度およびy方向成分の加速度を用いて求められることが分かる。
【０２０７】
また、h=(hx,0,0)の場合、hy=0,hz=0となるため、式(80)、すなわち式(81)となる。
【０２０８】
【数８０】
2020158485.tif 000082
【０２０９】
【数８１】
2020158485.tif 000083
【０２１０】
この式から、２つの加速度センサ１，２をx方向に離間配置した場合（h=(hx,0,0)の場合）には、z軸回りの角加速度dωz/dtは、y方向成分の加速度を用いて求められることが分かる。なお、この式は、h=(hx,0,0)を式(74)中の１〜３に代入することで求めることができる。
【０２１１】
また、h=(0,hy,0)の場合、hx=0,hz=0となるため、式(82)、すなわち式(83)となる。
【０２１２】
【数８２】
2020158485.tif 000084
【０２１３】
【数８３】
2020158485.tif 000085
【０２１４】
この式から、２つの加速度センサ１，２をy方向に離間配置した場合（h=(0,hy,0)の場合）には、z軸回りの角加速度dωz/dtは、x方向成分の加速度を用いて求められることが分かる。なお、この式は、h=(0,hy,0)を式(74)中の１〜３に代入することで求めることができる。
【０２１５】
以上より、２つの加速度センサ１，２を用いてz軸回りの角加速度dωz/dtを求めるためには、加速度センサ２を加速度センサ１に対してz方向のみに離間させないようにする必要があることが分かる。すなわち、加速度センサ１から見たときの位置ベクトルhが、加速度センサ１を通りz軸方向に延在する直線と一致しないように、加速度センサ２を配置する必要があることが分かる。言い換えると、加速度センサ１から見たときの位置ベクトルhが、加速度センサ１を通りz軸方向に延在する直線と交差するように加速度センサ２を配置する必要があることが分かる。
【０２１６】
さらに、加速度センサ２を加速度センサ１に対して上記のように配置した状態で、加速度センサ２が、z軸に直交するとともに、ベクトルhのxy平面（z軸に直交する平面）への射影ベクトルに直交する方向の加速度を検出できるようにする必要があることが分かる。このことから、加速度センサ２を加速度センサ１に対して、x方向のみにもy方向のみにも離間させていない場合には、加速度センサ２がx方向成分の加速度およびy方向成分の加速度を検出できるようにする必要があることが分かる。
【０２１７】
なお、通常は、加速度センサ２がx方向成分の加速度およびy方向成分の加速度を検出できるようにする必要があることは、上記の各式から分かることである。
【０２１８】
ここで、x方向成分の加速度およびy方向成分の加速度を検出できるようにするためには、加速度センサ２が３軸以上の加速度を検出できるものであることが望ましい。このように、３軸以上の加速度を検出できる加速度センサ２を用いれば、加速度センサ２をどのように配置しても、検出した加速度からx方向成分の加速度およびy方向成分の加速度を求めることができる。
【０２１９】
なお、加速度センサ２が２軸の加速度を検出できるものであっても、加速度センサ２がx方向成分の加速度およびy方向成分の加速度を検出できるように配置されていれば、z軸回りの角加速度dωz/dtを求めることができる。ただし、加速度センサ２の２つの軸の検出方向がともにxz平面に沿っている場合、または、yz平面に沿っている場合には、加速度センサ２によってx方向成分の加速度およびy方向成分の加速度を検出することができなくなってしまう。
【０２２０】
また、加速度センサ２が１軸のみ検出する場合であっても、検出した加速度をx方向成分の加速度およびy方向成分の加速度に分解できるように配置すれば、z軸回りの角加速度dωz/dtを求めることができる。ただし、加速度センサ２の検出軸方向がz軸に沿っている場合には、加速度センサ２によってx方向成分の加速度およびy方向成分の加速度を検出することができなくなってしまう。また、加速度センサ２の検出軸方向が、z軸に沿っていない場合でも、xz平面に沿っている場合、または、yz平面に沿っている場合には、後述する条件を満たさない限り、加速度センサ２によってx方向成分の加速度およびy方向成分の加速度を検出することができなくなってしまう。
【０２２１】
このように、通常では、加速度センサ２は、x方向成分の加速度およびy方向成分の加速度の両方を検出できるように配置させる必要がある。
【０２２２】
ただし、加速度センサ２を加速度センサ１に対してx方向のみに離間させた場合には、y方向成分の加速度のみを検出するだけでz軸回りの角加速度dωz/dtを求めることができる。すなわち、加速度センサ２を加速度センサ１に対してx方向のみに離間させた場合には、加速度センサ２の検出軸方向が、yz平面に沿っていたとしても、z軸に対して交差していれば、z軸回りの角加速度dωz/dtを求めることができる。このとき、検出精度の向上の観点からは、加速度センサ２の検出軸方向をy軸方向に沿わせるようにするのが好ましい。
【０２２３】
また、加速度センサ２を加速度センサ１に対してy方向のみに離間させた場合には、x方向成分の加速度のみを検出するだけでz軸回りの角加速度dωz/dtを求めることができる。すなわち、加速度センサ２を加速度センサ１に対してy方向のみに離間させた場合には、加速度センサ２の検出軸方向が、xz平面に沿っていたとしても、z軸に対して交差していれば、z軸回りの角加速度dωz/dtを求めることができる。このとき、検出精度の向上の観点からは、加速度センサ２の検出軸方向をx軸方向に沿わせるようにするのが好ましい。
【０２２４】
このように、加速度センサ２を加速度センサ１に対して１軸方向のみ（x方向のみまたはy方向のみ）に離間させた場合、１軸のみ検出する加速度センサ２を用い、加速度センサ２にて検出される加速度の軸方向をy方向またはx方向に一致させた状態でも、z軸回りの角加速度dωz/dtを求めることができる。
【０２２５】
１０．４  y軸回りの角加速度dωy/dtの求め方
同様に、h=(hx,hy,hz)とした場合におけるy軸回りの角加速度dωy/dtは、h=(hx,0,hz)とした場合におけるy軸回りの角加速度dωy/dtを求めることで得ることができる。
【０２２６】
具体的には、y軸回りの角加速度dωy/dtは、式(84)のようになる。この式は、h=(hx,0,hz)を式(74)中の１〜３に代入することで求めることができる。
【０２２７】
【数８４】
2020158485.tif 000086
【０２２８】
なお、h=(0,hy,0)の場合には、式(84)の分母(hz2+hx2)が0になってしまうため、h=(0,hy,0)の場合には、２つの加速度センサ１，２を用いてy軸回りの角加速度dωy/dtを求めることができないことが分かる。すなわち、加速度センサ２を加速度センサ１に対してy方向のみに離間させた場合、y軸回りの角加速度dωy/dtが求められないことが、式(84)より分かる。また、式(84)から、２つの加速度センサ１，２を、xz平面上で離間配置した場合（h=(hx,0,hz)の場合）も含め、xyz空間上で離間配置した場合（h=(hx,hy,hz)の場合）には、y軸回りの角加速度dωy/dtは、ジャイロセンサ３から得られたx軸回りの角速度ωxおよびz軸回りの角速度ωzと、２つの加速度センサ１，２から得られたx方向成分の加速度およびz方向成分の加速度を用いて求められることが分かる。
【０２２９】
また、h=(hx,0,0)の場合、hy=0,hz=0となるため、式(85)、すなわち式(86)となる。
【０２３０】
【数８５】
2020158485.tif 000087
【０２３１】
【数８６】
2020158485.tif 000088
【０２３２】
この式から、２つの加速度センサ１，２をx方向に離間配置した場合（h=(hx,0,0)の場合）には、y軸回りの角加速度dωy/dtは、z方向成分の加速度を用いて求められることが分かる。なお、この式は、h=(hx,0,0)を式(74)中の式１〜３に代入することで求めることができる。
【０２３３】
また、h=(0,0,hz)の場合、hx=0,hy=0となるため、式(87)、すなわち式(88)となる。
【０２３４】
【数８７】
2020158485.tif 000089
【０２３５】
【数８８】
2020158485.tif 000090
【０２３６】
この式から、２つの加速度センサ１，２をz方向に離間配置した場合（h=(0,0, hz)の場合）には、y軸回りの角加速度dωy/dtは、x方向成分の加速度を用いて求められることが分かる。なお、この式は、h=(0,0,hz)を式(74)中の式１〜３に代入することで求めることができる。
【０２３７】
以上より、２つの加速度センサ１，２を用いてy軸回りの角加速度dωy/dtを求めるためには、加速度センサ２を加速度センサ１に対してy方向のみに離間させないようにする必要があることが分かる。すなわち、加速度センサ１から見たときの位置ベクトルhが、加速度センサ１を通りy軸方向に延在する直線と一致しないように、加速度センサ２を配置する必要があることが分かる。言い換えると、加速度センサ１から見たときの位置ベクトルhが、加速度センサ１を通りy軸方向に延在する直線と交差するように加速度センサ２を配置する必要があることが分かる。
【０２３８】
さらに、加速度センサ２を加速度センサ１に対して上記のように配置した状態で、加速度センサ２が、y軸に直交するとともに、ベクトルhのxz平面（y軸に直交する平面）への射影ベクトルに直交する方向の加速度を検出できるようにする必要があることが分かる。このことから、加速度センサ２を加速度センサ１に対して、x方向のみにもz方向のみにも離間させていない場合には、加速度センサ２がx方向成分の加速度およびz方向成分の加速度を検出できるようにする必要があることが分かる。
【０２３９】
なお、通常は、加速度センサ２がx方向成分の加速度およびz方向成分の加速度を検出できるようにする必要があることは、上記の各式から分かることである。
【０２４０】
ここで、x方向成分の加速度およびz方向成分の加速度を検出できるようにするためには、加速度センサ２が３軸以上の加速度を検出できるものであることが望ましい。このように、３軸以上の加速度を検出できる加速度センサ２を用いれば、加速度センサ２をどのように配置しても、検出した加速度からx方向成分の加速度およびz方向成分の加速度を求めることができる。
【０２４１】
なお、加速度センサ２が２軸の加速度を検出できるものであっても、加速度センサ２がx方向成分の加速度およびz方向成分の加速度を検出できるように配置されていれば、y軸回りの角加速度dωy/dtを求めることができる。ただし、加速度センサ２の２つの軸の検出方向がともにxy平面に沿っている場合、または、yz平面に沿っている場合には、加速度センサ２によってx方向成分の加速度およびz方向成分の加速度を検出することができなくなってしまう。
【０２４２】
また、加速度センサ２が１軸のみ検出する場合であっても、検出した加速度をx方向成分の加速度およびz方向成分の加速度に分解できるように配置すれば、y軸回りの角加速度dωy/dtを求めることができる。ただし、加速度センサ２の検出軸方向がy軸に沿っている場合には、加速度センサ２によってx方向成分の加速度およびz方向成分の加速度を検出することができなくなってしまう。また、加速度センサ２の検出軸方向が、y軸に沿っていない場合でも、xy平面に沿っている場合、または、yz平面に沿っている場合には、後述する条件を満たさない限り、加速度センサ２によってx方向成分の加速度およびz方向成分の加速度を検出することができなくなってしまう。
【０２４３】
このように、通常では、加速度センサ２は、x方向成分の加速度およびz方向成分の加速度の両方を検出できるように配置させる必要がある。
【０２４４】
ただし、加速度センサ２を加速度センサ１に対してx方向のみに離間させた場合には、z方向成分の加速度のみを検出するだけでy軸回りの角加速度dωy/dtを求めることができる。すなわち、加速度センサ２を加速度センサ１に対してx方向のみに離間させた場合には、加速度センサ２の検出軸方向が、yz平面に沿っていたとしても、y軸に対して交差していれば、y軸回りの角加速度dωy/dtを求めることができる。このとき、検出精度の向上の観点からは、加速度センサ２の検出軸方向をz軸方向に沿わせるようにするのが好ましい。
【０２４５】
また、加速度センサ２を加速度センサ１に対してz方向のみに離間させた場合には、x方向成分の加速度のみを検出するだけでy軸回りの角加速度dωy/dtを求めることができる。すなわち、加速度センサ２を加速度センサ１に対してz方向のみに離間させた場合には、加速度センサ２の検出軸方向が、xy平面に沿っていたとしても、y軸に対して交差していれば、y軸回りの角加速度dωy/dtを求めることができる。このとき、検出精度の向上の観点からは、加速度センサ２の検出軸方向をx軸方向に沿わせるようにするのが好ましい。
【０２４６】
このように、加速度センサ２を加速度センサ１に対して１軸方向のみ（x方向のみまたはz方向のみ）に離間させた場合、１軸のみ検出する加速度センサ２を用い、加速度センサ２にて検出される加速度の軸方向をz方向またはx方向に一致させた状態でも、y軸回りの角加速度dωy/dtを求めることができる。
【０２４７】
１０．５  x軸回りの角加速度dωx/dtの求め方
同様に、h=(hx,hy,hz)とした場合におけるx軸回りの角加速度dωx/dtは、h=(0,hy,hz)とした場合におけるx軸回りの角加速度dωx/dtを求めることで得ることができる。
【０２４８】
具体的には、x軸回りの角加速度dωx/dtは、式(89)のようになる。この式は、h=(0,hy,hz)を式(74)中の１〜３に代入することで求めることができる。
【０２４９】
【数８９】
2020158485.tif 000091
【０２５０】
なお、h=(hx,0,0)の場合には、式(89)の分母(hy2+hz2)が0になってしまうため、h=(hx,0,0)の場合には、２つの加速度センサ１，２を用いてx軸回りの角加速度dωx/dtを求めることができないことが分かる。すなわち、加速度センサ２を加速度センサ１に対してx方向のみに離間させた場合、x軸回りの角加速度dωx/dtが求められないことが、式(89)より分かる。また、式(89)から、２つの加速度センサ１，２を、yz平面上で離間配置した場合（h=(0,hy,hz)の場合）も含め、xyz空間上で離間配置した場合（h=(hx,hy,hz)の場合）には、x軸回りの角加速度dωx/dtは、ジャイロセンサ３から得られたy軸回りの角速度ωyおよびz軸回りの角速度ωzと、２つの加速度センサ１，２から得られたy方向成分の加速度およびz方向成分の加速度を用いて求められることが分かる。
【０２５１】
また、h=(0,hy,0)の場合、hx=0,hz=0となるため、式(90)、すなわち式(91)となる。
【０２５２】
【数９０】
2020158485.tif 000092
【０２５３】
【数９１】
2020158485.tif 000093
【０２５４】
この式から、２つの加速度センサ１，２をy方向に離間配置した場合（h=(0,hy,0)の場合）には、x軸回りの角加速度dωx/dtは、z方向成分の加速度を用いて求められることが分かる。なお、この式は、h=(0,hy,0)を式(74)中の１〜３に代入することで求めることができる。
【０２５５】
また、h=(0,0,hz)の場合、hx=0,hy=0となるため、式(92)、すなわち式(93)となる。
【０２５６】
【数９２】
2020158485.tif 000094
【０２５７】
【数９３】
2020158485.tif 000095
【０２５８】
この式から、２つの加速度センサ１，２をz方向に離間配置した場合（h=(0,0,hz)の場合）には、x軸回りの角加速度dωx/dtは、y方向成分の加速度を用いて求められることが分かる。なお、この式は、h=(0,0,hz)を式(74)中の１〜３に代入することで求めることができる。
【０２５９】
以上より、２つの加速度センサ１，２を用いてx軸回りの角加速度dωx/dtを求めるためには、加速度センサ２を加速度センサ１に対してx方向のみに離間させないようにする必要があることが分かる。すなわち、加速度センサ１から見たときの位置ベクトルhが、加速度センサ１を通りx軸方向に延在する直線と一致しないように、加速度センサ２を配置する必要があることが分かる。言い換えると、加速度センサ１から見たときの位置ベクトルhが、加速度センサ１を通りx軸方向に延在する直線と交差するように加速度センサ２を配置する必要があることが分かる。
【０２６０】
さらに、加速度センサ２を加速度センサ１に対して上記のように配置した状態で、加速度センサ２が、x軸に直交するとともに、ベクトルhのyz平面（x軸に直交する平面）への射影ベクトルに直交する方向の加速度を検出できるようにする必要があることが分かる。このことから、加速度センサ２を加速度センサ１に対して、y方向のみにもz方向のみにも離間させていない場合には、加速度センサ２がy方向成分の加速度およびz方向成分の加速度を検出できるようにする必要があることが分かる。
【０２６１】
なお、通常は、加速度センサ２がy方向成分の加速度およびz方向成分の加速度を検出できるようにする必要があることは、上記の各式から分かることである。
【０２６２】
ここで、y方向成分の加速度およびz方向成分の加速度を検出できるようにするためには、加速度センサ２が３軸以上の加速度を検出できるものであることが望ましい。このように、３軸以上の加速度を検出できる加速度センサ２を用いれば、加速度センサ２をどのように配置しても、検出した加速度からy方向成分の加速度およびz方向成分の加速度を求めることができる。
【０２６３】
なお、加速度センサ２が２軸の加速度を検出できるものであっても、加速度センサ２がy方向成分の加速度およびz方向成分の加速度を検出できるように配置されていれば、x軸回りの角加速度dωx/dtを求めることができる。ただし、加速度センサ２の２つの軸の検出方向がともにxy平面に沿っている場合、または、xz平面に沿っている場合には、加速度センサ２によってy方向成分の加速度およびz方向成分の加速度を検出することができなくなってしまう。
【０２６４】
また、加速度センサ２が１軸のみ検出する場合であっても、検出した加速度をy方向成分の加速度およびz方向成分の加速度に分解できるように配置すれば、x軸回りの角加速度dωx/dtを求めることができる。ただし、加速度センサ２の検出軸方向がx軸に沿っている場合には、加速度センサ２によってy方向成分の加速度およびz方向成分の加速度を検出することができなくなってしまう。また、加速度センサ２の検出軸方向が、x軸に沿っていない場合でも、xy平面に沿っている場合、または、xz平面に沿っている場合には、後述する条件を満たさない限り、加速度センサ２によってy方向成分の加速度およびz方向成分の加速度を検出することができなくなってしまう。
【０２６５】
このように、通常では、加速度センサ２は、y方向成分の加速度およびz方向成分の加速度の両方を検出できるように配置させる必要がある。
【０２６６】
ただし、加速度センサ２を加速度センサ１に対してy方向のみに離間させた場合には、z方向成分の加速度のみを検出するだけでx軸回りの角加速度dωx/dtを求めることができる。すなわち、加速度センサ２を加速度センサ１に対してy方向のみに離間させた場合には、加速度センサ２の検出軸方向が、xz平面に沿っていたとしても、x軸に対して交差していれば、x軸回りの角加速度dωx/dtを求めることができる。このとき、検出精度の向上の観点からは、加速度センサ２の検出軸方向をz軸方向に沿わせるようにするのが好ましい。
【０２６７】
また、加速度センサ２を加速度センサ１に対してz方向のみに離間させた場合には、y方向成分の加速度のみを検出するだけでx軸回りの角加速度dωx/dtを求めることができる。すなわち、加速度センサ２を加速度センサ１に対してz方向のみに離間させた場合には、加速度センサ２の検出軸方向が、xy平面に沿っていたとしても、x軸に対して交差していれば、x軸回りの角加速度dωx/dtを求めることができる。このとき、検出精度の向上の観点からは、加速度センサ２の検出軸方向をy軸方向に沿わせるようにするのが好ましい。
【０２６８】
このように、加速度センサ２を加速度センサ１に対して１軸方向のみ（y方向のみまたはz方向のみ）に離間させた場合、１軸のみ検出する加速度センサ２を用い、加速度センサ２にて検出される加速度の軸方向をz方向またはy方向に一致させた状態でも、x軸回りの角加速度dωx/dtを求めることができる。
【０２６９】
１１  第二の実施形態、第三の実施形態
既に説明した通り、図７に示すように、空間上の点Ｒは、一般的に、原点等の基準点から見たときのベクトルr=(rx,ry,rz)で表すことができる。このとき、z軸回りの角加速度dωz/dtは、２つの加速度センサ１，２（第１の加速度センサ１および第２の加速度センサ２）のz軸方向の差であるhz成分には依存しない値であり、z軸回りの角加速度dωz/dtもhz成分を用いずに表すことができる。なお、y軸の場合も同様に、y軸回りの角加速度dωy/dtは、２つの加速度センサ１，２のy軸方向の差であるhy成分には依存しない値であり、また、x軸の場合も同様に、x軸回りの角加速度dωx/dtは、２つの加速度センサ１，２のx軸方向の差であるhx成分には依存しない値であるため、剛体Ｂに固定された直交座標系の各軸の回りの回転を考える場合には、回転軸方向の成分を考慮しなくても差し支えないことが分かる。
【０２７０】
第一の実施形態は3自由度の回転運動、すなわちロール、ピッチ、ヨー軸回りの回転運動に対応しているが、例えばロール軸すなわちｘ軸回りの回転運動を除く2自由度の回転運動の場合、ピッチ、ヨー軸回りの回転運動のみを検出すれば良いため、複合センサ１０は、ｙ軸及びｚ軸検出用2軸角速度センサ、ｘ軸及びｚ軸検出用2軸加速度センサ、x軸もしくはｚ軸検出用1軸加速度センサの合計5軸で構成可能である。以下、図面を参照しながら、この第二の実施形態にかかる複合センサ１０および角速度補正方法を説明する。なお、図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。
【０２７１】
図８は、第二の実施形態にかかる複合センサ１０が備える2軸加速度センサ１，1軸加速度センサ２と2軸ジャイロセンサ３の配置例を示す図であり、（ａ）は平面図、（ｂ）は側面図である。加速度センサ１、加速度センサ２、ジャイロセンサ３は、それぞれ、図１でいう第１の加速度センサ１、第２の加速度センサ２、角速度センサ３に相当するため、同じ符号を用いて説明する。
【０２７２】
第二の実施形態では、図８に示すように、2軸加速度センサ１，1軸加速度センサ２と2軸ジャイロセンサ３を剛体Ｂに固定したとき、そのセンサ出力の理論値をベクトル解析により算出する。
【０２７３】
また、図９は図８へ静止基準座標系ΣXYZを追加したものであり、静止基準座標系ΣXYZから剛体Ｂを見たときの姿勢（ピッチ、ヨー角）を表す。  図１０は、第二の実施形態にかかる複合センサ１０の動作を示すフローチャートである。以下、図１０を参照しながら上述した方法を用いて姿勢角を求める動作について説明する。なお、第一の実施形態と同一又は類似の部分には同一又は類似のステップ番号を付している。
【０２７４】
まず、ジャイロセンサ３により角速度ベクトルωが検出され、加速度センサ１により加速度ベクトルa1が検出され、加速度センサ２により加速度ベクトルa2が検出される（ステップＳ１，Ｓ２，Ｓ３）。ジャイロセンサ３の出力、加速度センサ１の出力、加速度センサ２の出力は、後段の演算部４に入力されるようになっている。
【０２７５】
次いで、演算部４は、ジャイロセンサ３の出力、加速度センサ１の出力、加速度センサ２の出力に基づいて、式(21)を用いてヨー角の角加速度dωz/dtを算出する（ステップＳ４）。そして、式(21)により得られたdωz/dtとジャイロセンサ３の出力から得られたωzをカルマンフィルタにかけることで、ジャイロセンサ３の出力（角速度）を補正する（ステップＳ５）。ここではカルマンフィルタを例示しているが、角速度を補正するアルゴリズムは限定されるものではない。
【０２７６】
また、演算部４は、角加速度を考慮した不感帯処理を施す（ステップＳ６）。具体的には、|ω|<δ1かつ|dω/dt|<δ2の条件を満たす場合はω=0とし、それ以外の場合は何もしない。
【０２７７】
更に、演算部４は、式(38)により得られた姿勢角の微分値を積分することで、姿勢角（ピッチ角、ヨー角）を求める（ステップＳ７→Ｓ８）。
【０２７８】
一方、演算部４は、加速度センサ１の出力、加速度センサ２の出力に基づいて、静止判定を行う（ステップＳ９）。具体的には、被測定物が静止しているときは、式(35)(36)によりピッチ角を算出し、ステップＳ７で用いるピッチ角を補正する（ステップＳ１０→Ｓ１１）。  更に、ロール及びピッチ軸すなわちx軸及びｙ軸回りの回転運動を除く1自由度の回転運動の場合、ヨー軸回りの回転運動のみを検出すれば良いため、複合センサ１０は、ｚ軸検出用1軸角速度センサ、ｘもしくはｙ軸検出用1軸加速度センサ、ｘ軸もしくはｙ軸検出用1軸加速度センサの合計3軸で構成可能である。以下、図面を参照しながら、この第三の実施形態にかかる複合センサ１０および角速度補正方法を説明する。なお、図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。
【０２７９】
図１１は、第三の実施形態にかかる複合センサ１０が備える二つの１軸加速度センサ１，２と１軸ジャイロセンサ３の配置例を示す図であり、（ａ）は平面図、（ｂ）は側面図である。加速度センサ１、加速度センサ２、ジャイロセンサ３は、それぞれ、図１でいう第１の加速度センサ１、第２の加速度センサ２、角速度センサ３に相当するため、同じ符号を用いて説明する。
【０２８０】
第三の実施形態では、図１１に示すように、二つの１軸加速度センサ１，２と１軸ジャイロセンサ３を剛体Ｂに固定したとき、そのセンサ出力の理論値をベクトル解析により算出する。
【０２８１】
また、図１２は図１１へ静止基準座標系ΣXYZを追加したものであり、静止基準座標系ΣXYZから剛体Ｂを見たときの姿勢（ヨー角）を表す。
【０２８２】
図１３は、第三の実施形態にかかる複合センサ１０の動作を示すフローチャートである。以下、図１３を参照しながら上述した方法を用いて姿勢角を求める動作について説明する。なお、第一の実施形態と同一又は類似の部分には同一又は類似のステップ番号を付している。
【０２８３】
まず、ジャイロセンサ３により角速度ベクトルωが検出され、加速度センサ１により加速度ベクトルa1が検出され、加速度センサ２により加速度ベクトルa2が検出される（ステップＳ１，Ｓ２，Ｓ３）。ジャイロセンサ３の出力、加速度センサ１の出力、加速度センサ２の出力は、後段の演算部４に入力されるようになっている。
【０２８４】
次いで、演算部４は、ジャイロセンサ３の出力、加速度センサ１の出力、加速度センサ２の出力に基づいて、式(21)を用いてヨー角の角加速度dωz/dtを算出する（ステップＳ４）。そして、式(21)により得られたdωz/dtとジャイロセンサ３の出力から得られたωzをカルマンフィルタにかけることで、ジャイロセンサ３の出力（角速度）を補正する（ステップＳ５）。ここではカルマンフィルタを例示しているが、角速度を補正するアルゴリズムは限定されるものではない。
【０２８５】
また、演算部４は、角加速度を考慮した不感帯処理を施す（ステップＳ６）。具体的には、|ω|<δ1かつ|dω/dt|<δ2の条件を満たす場合はω=0とし、それ以外の場合は何もしない。
【０２８６】
更に、演算部４は、式(38)により得られた姿勢角の微分値を積分することで、姿勢角（ヨー角）を求める（ステップＳ７→Ｓ８）。
【０２８７】
以上のように、第二の実施形態にかかる複合センサ１０は、角速度センサ３と、第１の加速度センサ１と、第２の加速度センサ２と、演算部４とを備える。角速度センサ３は、互いに独立した２軸の回りの角速度を検出する。第１の加速度センサ１は、２軸方向の各々と垂直な方向となる２軸方向の加速度を検出する。第２の加速度センサ２は、角速度センサ３の第１の検出軸方向と第１の加速度センサ１の第１の検出軸方向に垂直な方向に離間し、かつ角速度センサ３の第２の検出軸方向と第１の加速度センサ１の第２の検出軸方向に垂直な方向に離間した位置に配置され、第１の加速度センサ１が検出する２軸で構成される平面内に存在し、かつ２軸とは一致しない軸方向の加速度を検出する。演算部４は、第１の加速度センサ１および第２の加速度センサ２により検出される加速度に基づいて、角速度センサ３により検出される角速度を補正する。これにより、第１の加速度センサ１および第２の加速度センサ２の出力信号に基づいて角速度センサ３の出力信号が補正されるため、高精度に角速度を得ることのできる複合センサ１０を提供することが可能となる。
【０２８８】
また、第三の実施形態にかかる複合センサ１０は、角速度センサ３と、第１の加速度センサ１と、第２の加速度センサ２と、演算部４とを備える。角速度センサ３は、１軸の回りの角速度を検出する。第１の加速度センサ１は、１軸方向と垂直な方向となる１軸方向の加速度を検出する。第２の加速度センサ２は、角速度センサ３の検出軸方向と第１の加速度センサ１の検出軸方向に垂直な方向に離間した位置に配置され、第１の加速度センサ１の検出軸と同一方向の軸方向の加速度を検出する。演算部４は、第１の加速度センサ１および第２の加速度センサ２により検出される加速度に基づいて、角速度センサ３により検出される角速度を補正する。これにより、第１の加速度センサ１および第２の加速度センサ２の出力信号に基づいて角速度センサ３の出力信号が補正されるため、高精度に角速度を得ることのできる複合センサ１０を提供することが可能となる。
【０２８９】
また、第二の実施形態にかかる角速度補正方法は、角速度検出ステップと、第１の加速度検出ステップと、第２の加速度検出ステップと、演算ステップとを備える。角速度検出ステップでは、角速度センサ３が、互いに独立した２軸の回りの角速度を検出する。第１の加速度検出ステップでは、第１の加速度センサ１が、２軸方向の各々と垂直な方向となる２軸方向の加速度を検出する。第２の加速度検出ステップでは、第２の加速度センサ２が、角速度センサ３の第１の検出軸方向と第１の加速度センサ１の第１の検出軸方向に垂直な方向に離間し、かつ角速度センサ３の第２の検出軸方向と第１の加速度センサ１の第２の検出軸方向に垂直な方向に離間した位置に配置され、第１の加速度センサ１が検出する２軸で構成される平面内に存在し、かつ２軸とは一致しない軸方向の加速度を検出する。演算ステップでは、演算部４が、第１の加速度検出ステップおよび第２の加速度検出ステップで検出される加速度に基づいて、角速度検出ステップで検出される角速度を補正する。これにより、第１の加速度センサ１および第２の加速度センサ２の出力信号に基づいて角速度センサ３の出力信号が補正されるため、高精度に角速度を得ることのできる角速度補正方法を提供することが可能となる。
【０２９０】
また、第三の実施形態にかかる角速度補正方法は、角速度検出ステップと、第１の加速度検出ステップと、第２の加速度検出ステップと、演算ステップとを備える。角速度検出ステップでは、角速度センサ３が、１軸の回りの角速度を検出する。第１の加速度検出ステップでは、第１の加速度センサ１が、１軸方向と垂直な方向となる１軸方向の加速度を検出する。第２の加速度検出ステップでは、第２の加速度センサ２が、角速度センサ３の検出軸方向と第１の加速度センサ１の検出軸方向に垂直な方向に離間した位置に配置され、第１の加速度センサ１の検出軸と同一方向の軸方向の加速度を検出する。演算ステップでは、演算部４が、第１の加速度検出ステップおよび第２の加速度検出ステップで検出される加速度に基づいて、角速度検出ステップで検出される角速度を補正する。これにより、第１の加速度センサ１および第２の加速度センサ２の出力信号に基づいて角速度センサ３の出力信号が補正されるため、高精度に角速度を得ることのできる角速度補正方法を提供することが可能となる。
【０２９１】
≪その他の実施形態≫
以上、本開示の好適な実施形態について例示して説明したが、上記実施形態には限定されず、種々の変形が可能である。例えば、センサ部Ｓや演算部４の細部のスペック（形状、大きさ、レイアウト等）は適宜変更することが可能である。
【０２９２】
本出願は、２０１９年１月２８日に出願された日本国特許出願第２０１９−０１２２５９号に基づく優先権を主張しており、これらの出願の全内容が参照により本願明細書に組み込まれる。
【産業上の利用可能性】
【０２９３】
本開示によれば、高精度に角速度を得ることのできる複合センサおよび角速度補正方法を提供することができる。
【図１】
2020158485.tif 000097
【図２】
2020158485.tif 000098
【図３】
2020158485.tif 000099
【図４】
2020158485.tif 000100
【図５】
2020158485.tif 000101
【図６】
2020158485.tif 000102
【図７】
2020158485.tif 000103
【図８】
2020158485.tif 000104
【図９】
2020158485.tif 000105
【図１０】
2020158485.tif 000106
【図１１】
2020158485.tif 000107
【図１２】
2020158485.tif 000108
【図１３】
2020158485.tif 000109
【手続補正書】
【提出日】令和3年7月13日(2021.7.13)
【手続補正１】
【補正対象書類名】明細書
【補正対象項目名】０１２８
【補正方法】変更
【補正の内容】
【０１２８】
９  参考文献
以下、参考文献について記載する。
［参考文献１］大野有孝: 航空宇宙用姿勢検出センサ（ジャイロ）の技術動向, 精密工学会誌, vol.75, no.1,pp.159-160, 2009.
［参考文献２］Peter G. Martin, Gregory W. Hall, Jeff R. Crandall, and Walter D. Pilkey: Measuring the Acceleration of a Rigid Body, Shock and Vibration, vol.5, no.4, pp.211-224, 1998.
［参考文献３］A. J. Padgaonkar, K. W. Krieger and A. I. King: Measurement of Angular Acceleration of a Rigid Body Using Linear Accelerometers, ASME Journal of Applied Mechanics, vol.42, no.3, pp.552-556, 1975.
［参考文献４］Patrick Schopp, Hagen Graf, Michael Maurer, Michailas Romanovas, Lasse Klingbeil, and Yiannos Manoli: Observing Relative Motion With Three Accelerometer Triads, IEEE Transactions on Instrumentation and Measurement, vol.63, no.12, pp.3137-3151, 2014.
［参考文献５］太田憲，小林一敏: 加速度計を用いたスポーツにおける角速度・角加速度計測, 計測自動制御学会論文集, vol.30, no.12, pp.1442-1448, 1994.
［参考文献６］三村宣治, 小野寺良二, 小松原亮: 複数加速度計を使用した６軸加速度センサシステムの誤差解析と較正の効率化について, 日本機械学会論文集(C 編), vol.74, no.739, pp.134-140, 2008.
［参考文献７］藤田孔明，中原充也，佐藤祐之，寺尾篤人: ロボット用高精度モーションセンシングユニット, Panasonic Technical Journal, vol.63, no.2, pp.30-34, 2017.
［参考文献８］杉原知道，舛屋賢，山本元司: 三次元高精度姿勢推定のための慣性センサの線形・非線形特性分離に基づいた相補フィルタ, 日本ロボット学会誌, vol.31, no.3, pp.251-262, 2013.
［参考文献９］A. El Hadri and A. Benallegue: Attitude estimation with gyros-bias compensation using low-cost sensors, Proceeding of the 48th Conference on Decision and Control, pp.8077-8082, 2009.
［参考文献１０］A. J. Baerveldt and R. Klang: A low-cost and low-weight attitude estimation system for an autonomous helicopter, Intelligent Engineering System, pp.391-395, 1997.
［参考文献１１］Jurman D, Jankovec M, Kamnik R, Topic M: Calibration and data fusion solution for the miniature attitude and heading reference system, Sensors and Actuators A, vol.138, no.2, pp.411-420, 2007.
［参考文献１２］Foxlin E: Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter, IEEE Proceedings of VRAIS, pp.185-194, 1996.
［参考文献１３］Vahanay J, Aldon M J, Fournier A: Mobile robot attitude estimation by fusion of inertial data, Proceedings of the IEEE International Conference on Robotics and Automation, pp.277-282, 1993.
［参考文献１４］Ying-Chih Lai, Shau-Shiun Jan and Fei-Bin Hsiao: Development of a Low-Cost Attitude and Heading Reference System Using a Three-Axis Rotating Platform, sensors, vol.10, no.4, pp.2472-2491, 2010.
［参考文献１５］Tae Suk Yoo, Sung Kyung Hong, Hyok Min Yoon and Sungsu Park: Gain-Scheduled Complementary Filter Design for a MEMS Based Attitude and Heading Reference System, sensors, vol.11, no.4,pp.3816-3830, 2011.
［参考文献１６］廣瀬圭, 土岐仁, 近藤亜希子: 慣性センサ・地磁気センサを用いたスポーツにおける姿勢計測に関する研究, スポーツ産業学研究, vol.22, no.2,pp.255-262, 2012.
［参考文献１７］Sabatini A. M.: Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing, IEEE Transactions on Biomedical Engineering, vol.53, no.7, pp.1346-1356, 2006.
［参考文献１８］Roetenberg D, Luinge H J, Baten C T, Veltink P H: Compensation of Magnetic Disturbances Improves Inertial and Magnetic Sensing of Human Body Segment Orientation, IEEE transaction on Neural Systems and Rehabilitation Engineering, vol.13, no.3, pp.395-405, 2005.
［参考文献１９］廣瀬圭，近藤亜希子: 人間工学のための計測手法, 日本人間工学会, vol.50, no.4, pp.182-190, 2014.
［参考文献２０］Cooke J. M., Zyda M. J., Pratt D. R., McGhee R. B.: Flight simulation dynamic modeling using quaternions, NPSNET, vol.1, no.4, pp.404-420, 1994.
［参考文献２１］長谷川律雄: 回転表現のキネマティックス方程式の一般的な導出方法, 計測自動制御学会論文集, vol.40, no.11, pp.1160-1162, 2004.
【国際調査報告】
2020158485.tif 000110
2020158485.tif 000111
2020158485.tif 000112
2020158485.tif 000113