JP5061264B1 - 小型姿勢センサ - Google Patents

Abstract

【課題】 ジャイロバイアス誤差を導入したアルゴリズムを用いて、加速度環境下における高精度の姿勢・方位推定を実現する小型姿勢センサを提供すること。
【解決手段】 移動物体の独立な３軸上の角速度を計測する角速度センサ１０１、加速度センサ１０２、磁気センサ１０３と、角速度センサ１０１、加速度センサ１０２、及び磁気センサ１０３の計測値を基にクォータニオンの推定値を算出する演算処理部１０４とを備え、演算処理部１０４において、角速度データについて、バイアス誤差、スケールファクタ誤差、ミスアラインメント誤差、ノイズの要素を含む角速度センサ誤差とクォータニオンレベルの誤差を演算により求め、これらの値を用いて得られた拡張カルマンフィルタを用いて現在の姿勢を表すクォータニオン推定し、出力する小型姿勢センサを提供する。
【選択図】図１

Description

本発明は姿勢制御を要する移動物体に搭載され、ジャイロバイアス誤差を導入したアルゴリズムを用いて、加速度環境下における高精度の姿勢・方位推定を実現する小型姿勢センサに関する。

小型無人ヘリコプタなどの姿勢制御を要する移動物体には、その三次元姿勢角を計測するため姿勢センサを備えている。一般的に姿勢センサは移動物体の独立な３軸上の角速度を計測する角速度センサ、及び角速度センサにより得られる角速度の計測値を時間的に積分することにより、移動物体の三次元姿勢角を取得する処理を行う演算処理部を備えている。三次元姿勢角はロール・ピッチ・ヨー角等を用いてオイラー角により表現されるものがあり、姿勢センサにより計測される三次元姿勢角を基に移動物体の姿勢制御が行われる。このような移動物体の姿勢制御技術の従来例としては、例えば特許文献１に記載されたものがある。また、本発明の基礎となる技術として特許文献２に記載された発明がある。

特開平９−５１０４号公報 特開２００７−１８３１３８号公報

以下、従来の移動物体の姿勢制御技術について説明する。本発明の前提となるセンサとして特許文献２に示されたセンサを作製し、これをＭＡＲＧセンサという。このＭＡＲＧセンサの仕様を表１に示す。

表１に示されるような比較的低い精度ではあるが、非常に軽量で小型無人ヘリコプタにも充分搭載可能なハードウェアに対して提案アルゴリズムを実装し、検証を行う。このような小型軽量のセンサが実現されれば、重量の上では小型無人ヘリコプタに限らず一般的な航空機や移動ロボット等への応用も期待できる。以下、この説明で用いる座標系及びクォータニオンについて説明を行う。

１）座標系及びクォータニオン
先ず、この説明で用いる座標系および各座標系上で表されるベクトルの表記方法についての定義を行う。ここで用いる座標系を図１９に示す。ここで、図中のr-frame はReference frame （参照フレーム）といい、地上の任意の点を原点として、磁北をＸr軸、重力方向をＺr軸、ＸrＺr平面の垂直方向をＹr軸にとった座標系である。また、b-frame はBody frame （機体フレーム）といい、機体の重心を原点として、機体前方をＸb軸、機体下方をＺb軸、ＸbＺb平面の垂直方向をＹb軸にとった座標系である。ここで、三次元空間中の任意の幾何ベクトルをｒとしたとき、ｒを各座標系上の代数ベクトルとして表したものをそれぞれｒ_r，ｒ_bと定義する。また、r-frameに対する b-frameの姿勢を機体姿勢として定義する。

次に、この説明で用いる機体姿勢の表現方法として用いられるクォータニオンの概要を説明する。クォータニオンとは、複素数（通常、ａ＋ｂｉのように虚数部分が一次元で表現される）を三次元に拡張した数であり、次式で示すようにクォータニオンｑは１つの実数部と３つの虚数部から構成されている。
ｑ＝ｑ_o ＋ｑ₁i＋ｑ₂j＋ｑ₃k ・・・（１）
ただしここで、式中のｉ，ｊ，ｋは虚数単位であり、それぞれ次式のような関係を満たしている。
ｉ²＝ｊ²＝ｋ²＝−１ ・・・（２）
ｉｊ＝ｋ， ｊｋ＝ｉ， ｋｉ＝ｊ ・・・（３）

クォータニオンは四元数、又はオイラーパラメータと呼ばれることもあり（例えば文献、田島洋、マルチボディダイナミクス−３次元運動方程式の立て方、東京電機大学出版局、２００６年 を参照）、工学的には物体の回転姿勢の表現方法として用いられることが多い。回転姿勢を表現するクォータニオンは上記文献のSimple Rotationを用いて定義することができる。いま、オイラーの定理によれば、図１０中のb-frame がr-frameに対してどのような回転姿勢をとっていた場合でも、回転軸を表す単位ベクトルとその軸周りの回転角度を用いてその回転姿勢を表現することが可能である。上記ベクトルと回転角度の組を用いた回転姿勢の表現法をSimple Rotationと呼ぶ。ただしここで、ベクトルは式４のように成分で表すことができ、そのノルムは（５）式に示すように１となる。
Ｖ＝［ｌ ｍ ｎ］^T
・・・（４）
ここで、Ｖ：ベクトル
（ｌ²＋ｍ²＋ｎ²）^1/2＝１
・・・（５）

以上のようなSimple Rotationを用いてr-frameに対する b-frameの回転姿勢を表現するクォータニオンは次式のように定義される。
ここで、クォータニオンのノルムは式（５）より常に１となる。

クォータニオンはｑ＝［ｑo ｑ₁ ｑ₂ ｑ₃］^Tというように虚数単位を省いたベクトル表記で表されることも多く、この表記法によってクォータニオンｐ，ｑの和、差、積は次式のように定義される。
ｑ＋ｐ＝［ｑo＋ｐo ｑ₁＋ｐ₁ ｑ₂＋ｐ₂ ｑ₃＋ｐ₃］^T
・・・（７）
ｑ−ｐ＝［ｑo−ｐo ｑ₁−ｐ₁ ｑ₂−ｐ₂ ｑ₃−ｐ₃］^T
・・・（８）
・・・（９）

また、ｑに共役なクォータニオンｑ^*は次式のように定義される。
ｑ^*＝［ｑo −ｑ₁ −ｑ₂ −ｑ₃］^T
・・・（１０）
いま、r-frame上の代数ベクトルｒ_rからb-frame上の代数ベクトルｒ_bへの座標変換は次式で表すことができる。
・・・（１１）
ただしここで、式中の（ｒ_b）_qおよび（ｒ_r）_qは三次元ベクトルをクォータニオン表記したものであり、それぞれ次式のように定義される。
・・・（１２）

［ＭＡＲＧセンサ］
ここでは、先行技術において開発されたＭＡＲＧセンサのハードウェアの諸元と従来の機体姿勢推定アルゴリズムの概要およびその問題点について説明する。ＭＡＲＧセンサの諸元を表２に示す。

このセンサは、３軸の加速度センサ、ジャイロセンサ、磁気センサの出力を用いて現在の姿勢を表すクォータニオンを推定するアルゴリズムを実装した姿勢センサの一種である。

従来のＭＡＲＧセンサに実装されていた姿勢推定アルゴリズムの概要について述べると、先ず、r-frame上での重力ベクトルをg_r ＝［０ ０ g］^T，地磁気ベクトルをｍ_r ＝［ｍ_n ０ ｍ_d］^T とする。ただし、gは重力加速度、ｍ_nは水平地磁気、ｍ_dは垂直地磁気をそれぞれ表している。ここで、現在の姿勢を表すクォータニオンの推定値からb-frame上での重力ベクトルおよび地磁気ベクトル推定値を求めると次式となる。

続いて、機体に搭載された加速度センサおよび磁気センサの出力をそれぞれａ_measure＝［ａ_x ａ_y ａ_z］^T、ｍ_measure＝［ｍ_x ｍ_y ｍ_z］^Tとすると、これらと上述したb-frame上での重力ベクトルおよび地磁気ベクトル推定値との誤差ベクトルは次式となる。

いま、各センサの誤差がないと仮定すれば、最も確からしいクォータニオン推定値は式（１５）において、
とした同次連立方程式を解くことで得られることが分かる。しかし、式（１５）は４個の未知数に対して式が６個ある過剰条件の連立方程式であるため、一般には解くことができない。よって、最小二乗法を用いて誤差の二乗和を最小とするような近似解を求める。Ｇａｕｓｓ−Ｎｅｗｔｏｎ反復法を用いれば、あるステップにおいて誤差の二乗和を最小に近づけるような修正ベクトルは次式となる。
ただしここでＸは以下に示すようなヤコビアンである。

一方、現在の姿勢を示す真のクォータニオンの時間微分と機体角速度ω_bとは以下の関係式であることが知られている。
これにより、クォータニオンの推定値の時間微分に関しても同様に次式のような関係が成り立つことが分かる。

式（１６）と式（２０）を次のように組み合わせることによって、クォータニオン推定値を得るというのが従来のアルゴリズムである。ただしここで、ｋはスカラゲインである。

以上のアルゴリズムを信号線で表すと図２０のようになる。このアルゴリズムは単純なフィルタアルゴリズムであり、角速度から計算されたクォータニオンの時間微分を加速度、地磁気のデータを用いて補正し、それを積分した後に正規化して単位クォータニオンの推定値を求めるというものである。基本的に、低周波数の姿勢変動は加速度、地磁気のデータから推定する一方、高周波数の姿勢変動はジャイロを用いて推定するという構成となっている。しかし、このアルゴリズムには以下のような問題点がある。

式（１５）において、ａｍには重力をｂ−ｆｒａｍｅ上に座標変換した成分だけではなく、センサが動くことで発生する動的加速度の成分も含まれている。このため、動的加速度が印加されることで姿勢データが大きな誤差を持ってしまうのである。実際にセンサを水平に保った状態で大きな加速度を与えた際の姿勢データを図２１に示す。ただし、図２１に示されているのは推定されたクォータニオンを変換して得られたオイラー角である。図２１より、加速度を加えることで最大で２０［ｄｅｇ］程度の姿勢誤差が生じていることが分かる。このような動的加速度による誤差は移動体に搭載するためのセンサとしては致命的であり、なんとしても誤差を低減化する必要がある。

本発明はこのような従来技術の不具合に鑑みてなされたもので、その目的は、拡張カルマンフィルタのアルゴリズムを用いて、加速度環境下、とりわけ低周波加速度外乱の下における高精度の姿勢・方位推定を実現する小型姿勢センサを提供することである。

本発明は上記目的を達成するために、移動物体の独立な３軸上の角速度を計測する角速度センサ、加速度センサ及び磁気センサと、前記角速度センサ、前記加速度センサ、及び前記磁気センサの計測値を基にクォータニオンの推定値を算出する演算処理部と、を備え、前記演算処理部は、予め推定されたクォータニオン推定値を用いて座標変換行列を算出し、当該座標変換行列を用いて地上固定座標系成分の地磁気ベクトル及び重力加速度ベクトルを変換し、現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを推定する座標変換部と、前記磁気センサ及び前記加速度センサから現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを取得する磁気・加速度センサデータ取得部と、前記座標変換部で推定した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルと、前記磁気・加速度センサデータ取得部で取得した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルとの誤差を算出するベクトル誤差算出部と、前記角速度センサから現ステップのセンサ座標系成分の角速度ベクトルを取得する角速度センサデータ取得部と、前記取得した角速度データについて、バイアス誤差、スケールファクタ誤差、ミスアラインメント誤差、ノイズの要素を含む角速度センサ誤差を演算により求める角速度データ誤差算出部と、前ステップのクォータニオンと現ステップのクォータニオンの時間変化を求めるクォータニオン算出部と前記座標変換部、前記磁気・加速度センサデータ取得部、前記ベクトル誤差算出部、前記角速度データ誤差算出部、及び前記クォータニオン算出部を基に現在の姿勢を表すクォータニオンおよび角速度誤差を推定する拡張カルマンフィルタを構成し、前記拡張カルマンフィルタによるクォータニオン推定値を現在の姿勢として出力する小型姿勢センサを提供する。

本発明によれば、角速度データについて、バイアス誤差、スケールファクタ誤差、ミスアラインメント誤差、ノイズの要素を含む角速度センサ誤差を演算により求めてこの誤差を補正することにより、加速度環境下における高精度の姿勢・方位推定を実現する小型姿勢センサを実現できる。

また、本発明では、移動体に一定値の低周波数の加速度外乱が加えられたときでも、正常に低周波数の加速度による外乱が推定され、低周波加速度環境下における高精度の姿勢・方位推定を実現する小型姿勢センサを実現できる。

本発明の第１の実施の形態で用いる小型姿勢センサに備えられる各構成をブロックで示した図 上記第１の実施の形態で用いる小型姿勢センサの演算処理部で行う処理をブロックで示した図 上記第１の実施の形態で用いる小型姿勢センサの演算処理部で行う処理をフローで示した図 上記第１の実施の形態において、小型姿勢センサを移動物体に搭載し、動的加速度による誤差が少なくなるように観測ノイズの共分散行列の値を決定したときのシミュレーション結果を示す図 上記第１の実施の形態において、小型姿勢センサを移動物体に搭載し、姿勢の追従を充分に速くなるように観測ノイズの共分散行列の値を決定したときのシミュレーション結果を示す図 上記第１の実施の形態において、小型姿勢センサを移動物体に搭載し、動的加速度による誤差を少なく、且つ姿勢の追従を充分に速くできるように観測ノイズの共分散行列の値を決定したときのシミュレーション結果を示す図 上記第１の実施の形態において、移動物体に搭載した小型姿勢センサに、線形誤差推定カルマンフィルタ（ＫＦ）を用いた場合と、本発明の拡張カルマンフィルタ（ＥＫＦ）用いた場合におけるロール角についてのシミュレーション結果を比較して示す図 上記第１の実施の形態において、移動物体に搭載した小型姿勢センサに、線形誤差推定カルマンフィルタ（ＫＦ）を用いた場合と、本発明の拡張カルマンフィルタ（ＥＫＦ）用いた場合におけるピッチ角についてのシミュレーション結果を比較して示す図 上記第１の実施の形態において、移動物体に搭載した小型姿勢センサに、線形誤差推定カルマンフィルタ（ＫＦ）を用いた場合と、本発明の拡張カルマンフィルタ（ＥＫＦ）用いた場合におけるヨー角についてのシミュレーション結果を比較して示す図 上記第１の実施の形態において、従来のアルゴリズムおよび本発明の拡張カルマンフィルタアルゴリズムをマイクロコンピュータ上に実装し、小型無人ヘリコプタの運動の際に発生する角速度を大きく上回るような角速度環境下で、ロール角についてリアルタイムで姿勢・方位推定を行った結果を示す図 上記第１の実施の形態において、従来のアルゴリズムおよび本発明の拡張カルマンフィルタアルゴリズムをマイクロコンピュータ上に実装し、小型無人ヘリコプタの運動の際に発生する角速度を大きく上回るような角速度環境下で、ピッチ角についてリアルタイムで姿勢・方位推定を行った結果を示す図 上記第１の実施の形態において、従来のアルゴリズムおよび本発明の拡張カルマンフィルタアルゴリズムをマイクロコンピュータ上に実装し、小型無人ヘリコプタの運動の際に発生する角速度を大きく上回るような角速度環境下で、ヨー角についてリアルタイムで姿勢・方位推定を行った結果を示す図 上記第１の実施の形態において、従来のアルゴリズムおよび本発明の拡張カルマンフィルタアルゴリズムをマイクロコンピュータ上に実装し、小型無人ヘリコプタの飛行中に加わる加速度と比較して充分大きな加速度を印加し、ピッチ角についてリアルタイムで姿勢・方位推定を行った結果を示す図 本発明の第２の実施の形態において、移動体に一定値の低周波数の加速度外乱を加えたときの姿勢推定シミュレーションに用いた各軸の加速度データを示す図 上記第２の実施の形態の姿勢推定シミュレーションにおける各アルゴリズムの姿勢推定結果を示す図 上記第２の実施の形態の姿勢推定シミュレーションにおける加速度外乱推定値と加速度外乱の真値との比較を示す図 上記第２の実施の形態において、センサを車に搭載して一定値の低周波数の加速度外乱を加えた実験での各アルゴリズムの姿勢計測結果を示す図 上記第２の実施の形態の実験において、本発明のアルゴリズムにおける加速度外乱推定値と加速度外乱の真値との比較を示す図 本発明の前提となるクォータニオンの概要を説明するための三次元座標を示した図 従来技術において用いられた小型姿勢センサの演算処理部で処理を行う際に用いる数式やデータの流れを示した図 従来技術における小型姿勢センサを移動物体に搭載し、クォータニオンにより表現する三次元姿勢角を計測した実験データを示した図

以下、本発明を実施するための形態について説明する。本発明においても、上記特許文献２に開示された装置と同様の構成を有するセンサ装置回路が用いられる。ただし、本発明のセンサ装置回路と従来のセンサ装置回路においては演算処理の内容が異なる。

（第１の実施の形態）
図１は本発明の第１の実施の形態に係る小型姿勢センサに備えられる各ハードウェア構成を示すブロック図である。小型姿勢センサ１００は、移動物体の独立な３軸上の角速度を計測する角速度センサ１０１、移動物体の独立な３軸上の加速度を計測する加速度センサ１０２、移動物体の独立な３軸上の地磁気を計測する磁気センサ１０３、及び角速度センサ１０１、加速度センサ１０２、及び磁気センサ１０３の計測値を基にクォータニオンを算出する演算処理部１０４を備えている。この第１の実施の形態の小型姿勢センサ１００は１入力１出力の線形システムに適用し、定常カルマンフィルタを構成するとともに、演算項目の中にジャイロセンサ（角速度センサ）の誤差を導入することにより、「カルマンゲイン」や「クォータニオン」の算出をより精密にすることができ、現実的な実装を可能にし、さらに、小型化及び実時間処理を可能とする。

図２は演算処理部１０４で行う処理を機能ブロックで示したものである。演算処理部１０４は、拡張カルマンフィルタ２００、磁気・加速度センサデータ取得部２０２、角速度センサデータ取得部２０５、及び角速度データ誤差算出部２０８を備えている。また、拡張カルマンフィルタ２００は、座標変換部２０１、観測更新演算部２０３、時間更新演算部２０７を備えている。また、図３は演算処理部１０４で行う処理をフローで示したものである。

本発明の基本的な考えとして、動的加速度が姿勢センサシステムの観測ノイズに相当しているということに着目すると、ノイズを含む観測信号よりシステムの真の状態を推定することに適したカルマンフィルタを用いて姿勢推定アルゴリズムを構成することで誤差を低減化できると考えられる。

［姿勢・方位推定アルゴリズム］
プロセスモデルの構築
カルマンフィルタを構成するためには、システムのプロセスモデルが必要となる。以下では、本発明で対象とするシステムの離散時間プロセスモデルを導出する。先ず状態方程式を求める。式（１９）の右辺において、機体角速度ω_bはジャイロセンサから得ることができるが、ジャイロセンサや加速度センサといった慣性センサには様々な要因による誤差があることが知られている。より精度の高い推定を行うためにはこのような誤差も推定することが望ましいため、ジャイロセンサ誤差を状態量として導入する。いま、ジャイロセンサより得られた角速度をω_measure ＝［ω_x ω_y ω_z］^T、ジャイロセンサの誤差をΔω_b＝［δω_x δω_y δω_z］^Tとすると、これらとω_bとは以下のような関係となる。
ω_measure ＝ω_b＋Δω_b
・・・（２２）
この式を式（１９）に代入することで次式が得られる。

一般的に、ジャイロセンサ誤差の要因としてはバイアス誤差、スケールファクタ誤差、ミスアラインメント誤差、ノイズ等が考えられ、これを式で表すと次式のようになる。

上式において、右辺第１項目がバイアス誤差、第２項目がスケールファクタ誤差、第３項目がミスアラインメント誤差、第４項目がノイズをそれぞれ表している。この中で、スケールファクタ誤差およびミスアラインメント誤差に関しては予め計測試験を行ったデータを基に補正することが可能であるため、リアルタイムに推定する必要があるのはバイアス誤差とノイズのみとなる。また、ノイズをリアルタイムに推定するのは不可能であるため、ここでは
として、バイアス誤差の推定のみを行う。
ジャイロセンサのバイアス誤差は経験的に次のようなダイナミクスを持つことが知られている。

ただしここでｗ = ［w_x w_y w_z］^Tはホワイトノイズである。また、β_x，β_y,β_zはパラメータであり、これらの値およびホワイトノイズｗの分散は、静置試験のデータおよび式（２５）の出力データのアラン分散［１３］が一致するように調整し、決定した。各パラメータの値を示すと表３のようになる。

以上より、
を状態量としたときの状態方程式は次式となる。
ただし、ここで各行列は次式のようになっている。

式（２６）を離散化すると式（３０）となる。ただし、ｘ_tはｔステップ目の状態量であり、Δｔはサンプリングタイムである。ここではサンプリング周波数は５０［Ｈｚ］であるため、Δｔ＝０．０２［ｓ］となっている。
ここで、改めて
とおくと、離散時間の状態方程式は次式となる。

続いて、観測方程式を求める。機体に印加される動加速度を含む加速度誤差Δａ＝［δａ_x δａ_y δａ_z ］^Tおよび地磁気誤差Δｍ＝［δｍ_x δｍ_y δｍ_z］^Tを考えると、加速度センサ出力および磁気センサ出力は次式のように表すことができる。
ここで、
とすると、ｙ_tを出力とする観測方程式は次式となる。
ただし、ここでｈ_t（ｘ_t）は次式のようになっている。

［拡張カルマンフィルタアルゴリズム］
先に示した式（３１）、式（３４）のような離散時間のプロセスモデルを得ることができた。しかし、両式はともに非線形方程式となっているため、線形カルマンフィルタアルゴリズムをそのまま適用することができない。このような場合、式（３１）、式（３４）を真値に対する推定誤差に関して線形化し、推定誤差を改めて状態量に選び、線形カルマンフィルタを適用して誤差推定カルマンフィルタを構成する方法と、非線形システムに線形カルマンフィルタを適用するための近似手法である拡張カルマンフィルタを構成するという２つの方法があるが、ここでは後者を選択した。以下では、拡張カルマンフィルタアルゴリズムについて説明を行う。いま、式（３１）、式（３４）のシステムに関して、ｔステップ目におけるｘ_tの濾波推定値及び予備推定値を次のように表記したとき、
行列Ｆ_tおよびＨ_tを次式のように定義する。

以上のように定義された行列を用いた拡張カルマンフィルタアルゴリズムは以下に示す式で与えられる。

ただしここで、式中のＫ_tはカルマンゲイン、Ｐ_t/tおよびＰ_t/t-1は推定誤差の共分散行列、Ｑ_tはシステムノイズの共分散行列、Ｒ_tは観測ノイズの共分散行列をそれぞれ表している。上記の式（３７）〜（４１）のアルゴリズムは、得られた観測値を用いて推定値を濾波する部分と、次ステップの推定値を予測する部分の２つから構成されており、それぞれ式（３７）〜（３９）が前者に当たり、式（４０）〜（４１）が後者に当たる。また、前者を観測更新、後者を時間更新と呼ぶこともある。以上のように与えられた式を順に計算することで、ｔステップ目におけるｘ_tの最も確からしい濾波推定値
を得ることができる。

以下、図２及び図３を用いて、演算処理部１０４で行う処理について説明する。ステップ３０１において、座標変換部２０１は初期姿勢における磁気、加速度について、クォータニオンで表された式（１１）を用いて上述のr-frameから b-frameへの座標変換を行う。クォータニオンの各要素は、Ｓｉｍｐｌｅ Ｒｏｔａｔｉｏｎと呼ばれている姿勢表現方法を用いて式（６）で定義されている。式（６）は３つの虚数と１つの実数からなる４次のベクトル表現を用いた予め設定されるクォータニオンの初期値である式（１）及び式（１０）を用いて表される。初期姿勢における磁気は、地磁気ベクトルの地上固定座標系（r-frame）成分であり、一方、初期姿勢における加速度は、重力加速度ベクトルの地上固定座標系（ｒ-frame）成分である。

次に、ステップ３０２では、予め設定されるクォータニオンの初期値を基に現ステップのセンサ座標系の磁気、加速度を推定することができ、これらを観測更新演算部２０３に入力する。

ステップ３０３では、磁気・加速度センサデータ取得部２０２は、磁気センサ１０３及び加速度センサ１０２から現ステップの磁気センサ出力（ベクトル）ａ_measure＝［ａ_x ａ_y ａ_z］^T、加速度センサ出力（ベクトル）ｍ_measure＝［ｍ_x ｍ_y ｍ_z］^Tを取得する。取得する現ステップの磁気センサ出力、加速度センサ出力は、センサ座標系成分のデータであり、これらを観測更新演算部２０３に入力する。

ステップ３０４では、観測更新演算部２０３は、ステップ３０２で座標変換部２０１が算出した推定される現ステップの地磁気ベクトル推定値及び重力加速度ベクトル推定値と、ステップ３０３で磁気・加速度センサデータ取得部２０２から取得した現ステップの地磁気ベクトル及び重力加速度ベクトルとの誤差を求め、さらに式（３４）を使って式（３７）〜式（３９）の演算を行って推定値を濾波する。

上述のステップ３０１〜ステップ３０４の処理とは別に、或いは並列的に、角速度の検出及びその誤差の算出が行われる。ステップ３０５において、角速度センサデータ取得部２０５は、角速度センサ１０１から現ステップの角速度を取得する。取得する現ステップの角速度は、センサ座標系成分のデータであり、これを角速度データ誤差算出部２０８に入力して角速度データの誤差を算出する。

ステップ３０６では、角速度データ誤差算出部２０８は角速度センサ１０１の誤差であるバイアス誤差、スケールファクタ誤差、ミスアライメント誤差を考慮して角速度データの誤差求める。この角速度データの誤差算出に当たっては、上述の式（２２）〜式（３１）の演算を行い、式（２６）の状態方程式、およびこれをさらに変形させた式（３１）の離散時間の状態方程式を求める。

先のステップ３０４における演算結果とステップ３０６における角速度データの誤差算出結果とは加算されて時間更新演算部２０７へ入力される。時間更新演算部２０７では、式（２６）を使って式（４０）及び式（４１）の演算を行い推定値を濾波する。以上の演算処理の結果クォータニオン補正もなされ、次のステップにおけるｘ_tの最も確からしい濾波推定値を得ることができる。

最後に、ステップ３０８において、上記のようにして得られた算出された次ステップの濾波推定値は、座標変換部２０１に入力される。すなわち、座標変換部２０１では、算出された次ステップの濾波推定値を用いて次ステップの座標変換行列を算出し、その結果は観測更新演算部２０３に入力されてクォータニオン推定値として小型姿勢センサの最終出力となる一方、さらに次のステップにおける観測更新演算処理、さらには時間更新演算処理に用いる。上記ステップ３０１乃至ステップ３０８の処理が、小型姿勢センサの演算処理部１０４で行う一ステップの処理であり、これを繰り返し行う。

［シミュレーション１］
本発明の主目的である動的加速度環境下における姿勢誤差の低減化を実現するためには、観測ノイズの共分散行列Ｒ_tの決め方が重要になる。ここでは、Ｒ_tの値をシミュレーションにより決定する。先ず動的加速度による誤差が少なくなるようにＲ_tの値を決定した。このときの値をＲ₁とする。図４にＲ₁によるシミュレーション結果を示す。図４（ａ）が姿勢を変動させたとき、図４（ｂ）がセンサを水平に保った状態で、図１９のb-frame上のＸｂ軸方向、すなわちピッチ角度に大きな誤差が生じやすい方向に正弦波状の動的加速度を印加したときのシミュレーション結果である。また、それぞれ破線が従来のアルゴリズムによる結果、実線が拡張カルマンフィルタによる結果、点線がその際の真の姿勢値である。図４より、Ｒ₁を用いると動的加速度による誤差は低減化するが、姿勢の追従が極端に遅くなってしまうことが分かる。一方、姿勢の追従を充分に速くなるようなＲ_tの値をＲ₂として、Ｒ₂による結果を図５に示す。図５よりＲ₂を用いると姿勢の追従はよくなるが、動的加速度誤差が低減化できないことが分かる。そこで、上述のＲ₁，Ｒ₂を用いてＲ_tを次式のような関数とした。ただし、Ｋ_Rは適当なゲインである。また、Ｔに関してはシミュレーションを繰り返すことによって適当な値を決定した。この事例では、Ｔの値は１１［ｓ］となっている。

このときのシミュレーション結果を図６に示す。以上のようなＲ_tを用いることで、姿勢の追従が速く、動的加速度による誤差は低減化できるような結果を得ることができた。続いて図７〜図９に線形誤差推定カルマンフィルタ（ＫＦ）と本発明において用いた拡張カルマンフィルタ（ＥＫＦ）の比較を示す。これらのうち、図７は、移動物体に搭載した小型姿勢センサに、線形誤差推定カルマンフィルタ（ＫＦ）を用いた場合と、本発明の拡張カルマンフィルタ（ＥＫＦ）用いた場合におけるロール角についてのシミュレーション結果を比較して示す図である。図７中において、点線で示された線はＫＦによるシミュレーション結果であり、実線で示された線はＥＫＦによるシミュレーション結果である。両者を比較すると、ＫＦの結果はＥＫＦと比べて振動的になってしまっていることが分かる。これは、式（３４）の観測方程式に含まれる座標変換の非線形性をＫＦでは充分に考慮できていないためであると考えられる。

図８は、移動物体に搭載した小型姿勢センサに、線形誤差推定カルマンフィルタ（ＫＦ）を用いた場合と、本発明の拡張カルマンフィルタ（ＥＫＦ）用いた場合におけるピッチ角についてのシミュレーション結果を比較して示す図である。図８中において、点線で示された線はＫＦによるシミュレーション結果であり、実線で示された線はＥＫＦによるシミュレーション結果である。両者を比較すると、図７の場合と同様に、ＫＦの結果はＥＫＦと比べて振動的になってしまっていることが分かる。

図９は、移動物体に搭載した小型姿勢センサに、線形誤差推定カルマンフィルタ（ＫＦ）を用いた場合と、本発明の拡張カルマンフィルタ（ＥＫＦ）用いた場合におけるヨー角についてのシミュレーション結果を比較して示す図である。図９中において、点線で示された線はＫＦによるシミュレーション結果であり、実線で示された線はＥＫＦによるシミュレーション結果である。両者を比較すると、図７及び図８の場合と同様に、ＫＦの結果はＥＫＦと比べて振動的になってしまっていることが分かる。

［実験１］
従来のアルゴリズムおよび本発明の拡張カルマンフィルタアルゴリズムをマイクロコンピュータ上に実装し、リアルタイムで姿勢・方位推定を行った結果を図１０〜図１３に示す。比較対象として、Ｃｒｏｓｓｂｏｗ社製の高精度姿勢センサＡＨＲＳ４００を選択した。このＡＨＲＳ４００は移動体に搭載することを目的とした姿勢センサであり、動的加速度を印加しても姿勢誤差が生じにくいという特徴を持っている。しかし、その重量は７００（ｇ）であり、ＭＡＲＧセンサの１０倍以上となっている。本来であればＡＨＲＳ４００とＭＡＲＧセンサをともに小型無人ヘリコプタ上のマウントに搭載し、飛行しつつ姿勢データの比較を行うことが望ましいが、ＡＨＲＳ４００の重量の関係上それは難しいため、この実験では両方のセンサを同一マウント上に搭載した状態で、そのマウントを地上で動かすという試験方法をとった。ちなみに、比較を行いやすくするために、得られたクォータニオン推定値をオイラー角に変換した後にプロットしている。図１０は、従来のアルゴリズムおよび本発明の拡張カルマンフィルタアルゴリズムをマイクロコンピュータ上に実装し、小型無人ヘリコプタの運動の際に発生する角速度を大きく上回るような角速度環境下で、ロール角についてリアルタイムで姿勢・方位推定を行った結果を示す図である。この図において、実線は拡張カルマンフィルタアルゴリズムによる方位推定結果を示し、細かい点線は従来のアルゴリズムによる方位推定結果を示す。また、粗い目の鎖線は上記高精度姿勢センサＡＨＲＳ４００による方位推定結果を示す。これらのグラフ図から、本発明の拡張カルマンフィルタアルゴリズムは高精度の姿勢・方位推定結果が得られることが分かる。

図１１は、従来のアルゴリズムおよび本発明の拡張カルマンフィルタアルゴリズムをマイクロコンピュータ上に実装し、小型無人ヘリコプタの運動の際に発生する角速度を大きく上回るような角速度環境下で、ピッチ角についてリアルタイムで姿勢・方位推定を行った結果を示す図である。グラフ図の線の内訳については図１０と同じである。また図１１からも、本発明の拡張カルマンフィルタアルゴリズムは高精度の姿勢・方位推定結果が得られることが分かる。図１２は、従来のアルゴリズムおよび本発明の拡張カルマンフィルタアルゴリズムをマイクロコンピュータ上に実装し、小型無人ヘリコプタの運動の際に発生する角速度を大きく上回るような角速度環境下で、ヨー角についてリアルタイムで姿勢・方位推定を行った結果を示す図である。グラフ図の線の内訳については図１０、図１１と同じである。また図１２からも、本発明の拡張カルマンフィルタアルゴリズムは高精度の姿勢・方位推定結果が得られることが分かる。図１３は、従来のアルゴリズムおよび本発明の拡張カルマンフィルタアルゴリズムをマイクロコンピュータ上に実装し、センサを水平に保った状態で、シミュレーションと同様に、小型無人ヘリコプタの飛行中に加わる加速度と比較して充分大きく、図１９のb-frame上Ｘｂ軸方向に対して正弦波状の動的加速度を印加し、ピッチ角についてリアルタイムで姿勢・方位推定を行った結果を示す図である。グラフ図の線の内訳については図１０−図１２と同じである。また図１３からも、本発明の拡張カルマンフィルタアルゴリズムは高精度の姿勢・方位推定結果が得られることが分かる。

このときの実験では、図１０〜図１２に関しては小型無人ヘリコプタの運動の際に発生する角速度を大きく上回るように最大２５０［ｄｅｇ／ｓ］の角速度環境下において試験を行っており、図１３に関しては、小型無人ヘリコプタの飛行中に加わる加速度と比較して充分大きな加速度を印加しているが、これは、小型姿勢センサを小型無人ヘリコプタに搭載することを想定したからである。表４に小型無人ヘリコプタの各飛行状態およびこの試験における加速度のＲＭＳ値を示している。

それぞれ、「Ｈｏｖｅｒ」がホバリング飛行中、「Ｃｉｒｃｕｌａｒ ｆｌｉｇｈｔ」が比較的大きな加速度が印加される旋回飛行中、「Ｅｘｐｅｒｉｍｅｎｔ」が図１３に示す試験中の値である。図１０〜図１２より、概ねＡＨＲＳ４００とＭＡＲＧセンサのデータは一致しており、正確な推定が行われているということが分かる。しかし、図１２の６〜１６秒付近においてＡＨＲＳ４００と比較して提案手法および従来手法のデータが誤差を持っていることが分かる。これは磁気外乱の影響であると考えられ、小型無人ヘリコプタの飛行する屋外では特に大きな問題ではないが、今後、このような磁気外乱の影響も低減化するように改良する必要があると考えられる。また、ＡＨＲＳ４００のデータが一定値となっている箇所がいくつかあるが、これは、ＡＨＲＳ４００は角速度が２００［ｄｅｇ／ｓ］以下でしか姿勢の推定を行えず、２００［ｄｅｇ／ｓ］以上の角速度を与えると姿勢の推定が停止してしまうためである。これに対して、ＭＡＲＧセンサでは２００［ｄｅｇ／ｓ］以上の角速度を与えた際にも姿勢の推定を行うことができている。一方、動的加速度を印加しても姿勢誤差を充分に低減化できており、仕様を充分に満たしていることが分かる。

以上説明したように、本発明によれば、先行技術において開発されたＭＡＲＧセンサの問題点である動的加速度環境下での姿勢誤差を低減化するために、慣性センサのバイアス推定を含む拡張カルマンフィルタを姿勢推定アルゴリズムとして適用し、動的加速度環境下での精度の高い姿勢・方位推定を実現した。

（実施の形態２）
本発明の第２の実施の形態は、加速度環境下における高精度の姿勢・方位推定を実現するに当って、低周波数の加速度環境下での姿勢推定誤差の低減化を図るものである。

［加速度信号のモデル化と拡張カルマンフィルタへの組込］
第１の実施の形態の方法においては、加速度環境下における高精度の姿勢・方位推定を或る程度は実現している。しかしながら、一定値に近い低周波数の加速度外乱が印加されると姿勢推定値がドリフトしてしまう。そこで、低周波数の加速度外乱をシステムの状態の一部として推定することで姿勢誤差を低減化する方法を以下に提案する。加速度外乱を推定するためのもう一つの手法として、カルマンフィルタの状態量に加速度外乱を含めることで、姿勢と加速度外乱を同時に推定する手法を提案する。移動や回転運動に伴って発生する加速度外乱は加速度センサのサンプリング周期に比べて十分に低周波であることから、サンプリング間における加速度外乱のダイナミクスを下記の式で示す。

まず低周波数の加速度外乱を
ａ_dr＝［ａ_bx ａ_by ａ_bz］^T
とし、外乱のダイナミクスを次式のように表す。
式（４４）より、
を状態量としたときの状態方程式は次の式（４５）のように置き換えられる。

ただし、ここで各行列は次式のようになっている。

式（４５）を離散化すると下記の式（４９）のようになる。ただし、ｘ_t はt ステップ目の状態量であり、Δt はサンプリングタイムである。
ここで、改めて
とおくと、離散時間の状態方程式は次式となる。

続いて、観測方程式を求める。式（４５）で推定された低周波数の加速度外乱を観測方程式に含めると次式のように書くことができる。
ここで、
とすると、ｙ_t を出力とする観測方程式は次式となる。
ただし、ここでｈ_t（ｘ_t）は次式のようになっている。

第１の実施の形態の方法と同様に行列Ｆ_t及びＨ_tを次式のように定義する。
以上のように定義された行列と式（５０）、式（５３）を用いた拡張カルマンフィルタアルゴリズムは以下に示す式で与えられる。

［シミュレーション２］
以下においては、本発明の拡張フィルタアルゴリズムによって正常に低周波数の加速度の外乱が推定されていることを確認するために移動体に一定値の低周波数の加速度外乱を加え、それが正しく推定されていることをシミュレーションにより確認する。本シミュレーションでは、機体が停止状態から最大加速度で加速し、一定時間の移動の後、最大加速度で原則すると仮定し、印加される外乱の大きさは０．２Ｇとした。また、各アルゴリズムの性能を明確にするため、加減速による加速度外乱の印加時間は実フライトよりも十分に長い２０秒間に設定した。図１４は本シミュレーションに用いた各軸の加速度データを示す。図１４中において、実線はＸ軸方向の加速度データを示し、一点鎖線はＹ軸方向の加速度データを示し、点線はＺ軸方向の加速度データを示す。図１４により、開始から約１５秒後にｘ軸加速度に一定値の外乱が負方向及び正方向に１回ずつ印加されていることが分かる。ｙ軸には加速度がなく、ｚ軸には重力加速度が印加されている。図１５は各アルゴリズムの姿勢推定結果を示し、図１５（ａ）はロール方向における姿勢推定結果を示し、図１５（ｂ）はピッチ方向における姿勢推定結果を示し、図１５（ｃ）はヨー方向における姿勢推定結果を示す。また、図１５において、Ｔｒｕｅの表示は姿勢推定結果の真値を実線で示し、Ｐｒｅｖの表示は従来のアルゴリズムによる姿勢推定結果を点線（細線）示し、Ｎｅｗの表示は本発明において状態量に加速度外乱を含めた拡張カルマンフィルタのアルゴリズムによる姿勢推定結果を点線（太線）示す。図１６は本発明のアルゴリズムにおける加速度外乱推定値と加速度外乱の真値との比較を示す図であり、図１６（ａ）はＸ方向における加速度外乱の比較結果を示し、図１６（ｂ）はＹ方向における加速度外乱の比較結果を示し、図１６（ｃ）はＺ方向における加速度外乱の比較結果を示す。図１６において、Ｔｒｕｅの表示は加速度外乱の真値を実線で示し、Ｎｅｗの表示は本発明のアルゴリズムによる加速度外乱推定結果を点線（太線）で示す。

上述のシミュレーションにおいて、計測される加速度は重力と移動による加速度外乱であり、図１５の真値から姿勢には変動がないことが分かる。図１４の加速度と、重力の各軸成分が、（g_x g_y g_z）＝［０，０，１］［Ｇ］であることから、時刻１５［ｓｅｃ］から３５［ｓｅｃ］にかけてＸ軸には加速による−０．２［Ｇ］の外乱が印加され、また、時刻５０［ｓｅｃ］から７０［ｓｅｃ］にかけてＸ軸には減速による０．２［Ｇ］の外乱が印加されているが、図１６の本発明のアルゴリズムによる加速度外乱推定値は真値によく一致している。図１５の各アルゴリズムによる姿勢推定値を比較すると、従来アルゴリズムによる姿勢推定ではピッチ角に最大１２［ｄｅｇ］の誤差を生じているのに対して、提案した２種類のアルゴリズムでは共に誤差が０．１［ｄｅｇ］以内に収まっており、その有効性が確認できる。従来アルゴリズムにおける姿勢推定誤差は、印加された加速度外乱をピッチ角が傾いたときにＸ軸へ印加される重力成分として扱ったために生じていると考えられる。

［実験２］
実際の加速度外乱環境下における測定結果を基に、従来の姿勢推定アルゴリズムと本発明の姿勢推定アルゴリズムの性能比較を行う。アルゴリズムを実装するハードウェアとして本発明の小型姿勢センサを用いるが、この小型姿勢センサに搭載されているマイクロプロセッサ（ＭＰＵ）は小型のため、複数種類のアルゴリズムを同時にリアルタイムで演算することはできない。そこで、カルマンフィルタを用いた加速度外乱推定を行うアルゴリズムを実装し、測定によって得られた加速度、角速度、磁気データを基にオフラインで推定演算を行った。演算に用いる行列の次数からカルマンフィルタを用いた加速度外乱推定アルゴリズムの演算量が多いのは明らかであるため、本手法がＭＰＵに実装可能であれば、他の手法においても十分に実現可能である。姿勢推定アルゴリズムの比較検証を行うための真値は、Ｃｒｏｓｓｂｏｗ社製の高精度姿勢センサＡＨＲＳ４４０から取得する。

この実験では、車載による姿勢計測実験を行うこととし、一般車両に各センサ及びデータ収集用ノートパソコン、電源を搭載し、走行実験を行った結果を図１７及び図１８に示す。本来ならば、上記センサ等は小型無人ヘリコプターに搭載して測定することが望ましいが、姿勢センサＡＨＲＳ４４０は小型無人ヘリコプターへの搭載に適切な重量ではない。そこで、移動体へ適用した際の有効性を確かめる手段として、車両への搭載を選択した。試験は平坦な道路上を直線的に走行し、４０ｋｍ/ｈへ到達した段階で、急減速により停止する。このとき、センサの姿勢変動はほとんどなく、前後方向に大きな加速度外乱が印加される。

図１７及び図１８は上記実験におけるセンサによる姿勢計測結果を表す図である。図１７は各アルゴリズムの姿勢計測結果を示し、図１７（ａ）はロール方向における姿勢計測結果を示し、図１７（ｂ）はピッチ方向における姿勢計測結果を示し、図１７（ｃ）はヨー方向における姿勢計測結果を示す。また、図１７において、Ｒｅｆの表示はＣｒｏｓｓｂｏｗ社製の高精度姿勢センサＡＨＲＳ４４０による姿勢計測結果（真値）を実線で示し、Ｐｒｅｖの表示は従来のアルゴリズムによる姿勢計測結果を点線（細線）で示し、Ｎｅｗの表示は本発明の拡張カルマンフィルタのアルゴリズムによる姿勢計測結果を点線（太線）で示す。図１８は本発明のアルゴリズムにおける加速度外乱推定値と加速度外乱の真値との比較を示す図であり、図１８（ａ）はＸ方向における加速度外乱の比較結果を示し、図１８（ｂ）はＹ方向における加速度外乱の比較結果を示し、図１８（ｃ）はＺ方向における加速度外乱の比較結果を示す。図１８において、Ｔｒｕｅの表示は加速度外乱の真値を実線で示し、Ｎｅｗの表示は本発明のアルゴリズムによる加速度外乱推定結果を点線（太線）で示す。

図１７から、従来アルゴリズムでは加速度外乱の印加に応じてピッチ角に最大１８［ｄｅｇ］の誤差を生じており、さらに８秒付近からヨー角の揺動も見られる。他方、本発明の拡張カルマンフィルタのアルゴリズムによる加速度外乱推定では図１８から明らかなように、Ｃｒｏｓｓｂｏｗ社製の高精度姿勢センサＡＨＲＳ４４０と同等の外乱抑制効果が得られており、その結果図１７においても、ロール角（図１７（ａ））、ピッチ角（図１７（ｂ））、ヨー角（図１７（ｃ））のいずれにおいても誤差が０．１［ｄｅｇ］以内に収まっている。これにより、本発明の拡張カルマンフィルタのアルゴリズムによれば、正しく姿勢推定が行われていることが分かり、その有効性が確認できる。

Claims (4)

1. 移動物体の独立な３軸上の角速度を計測する角速度センサと、
   移動物体の独立な３軸上の加速度を計測する加速度センサと、
   移動物体の独立な３軸上の地磁気を計測する磁気センサと、
   前記角速度センサ、前記加速度センサ、及び前記磁気センサの計測値を基にクォータニオンの推定値を算出する演算処理部とを備え、
   前記演算処理部は、予め推定されたクォータニオン推定値を用いて座標変換行列を算出し、当該座標変換行列を用いて地上固定座標系成分の地磁気ベクトル及び重力加速度ベクトルを変換し、現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを推定する座標変換部と、
   前記磁気センサ及び前記加速度センサから現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを取得する磁気・加速度センサデータ取得部と、
   前記座標変換部で推定した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルと、前記磁気・加速度センサデータ取得部で取得した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルとの誤差を算出するベクトル誤差算出部と、
   前記角速度センサから現ステップのセンサ座標系成分の角速度ベクトルを取得する角速度センサデータ取得部と、
   前記取得した角速度データについて、バイアス誤差、スケールファクタ誤差、ミスアラインメント誤差、ノイズの要素を含む角速度センサ誤差を演算により求める角速度データ誤差算出部と、
   前ステップのクォータニオンと現ステップのクォータニオンの時間変化を求めるクォータニオン算出部と
   前記座標変換部、前記磁気・加速度センサデータ取得部、前記ベクトル誤差算出部、前記角速度データ誤差算出部、及び前記クォータニオン算出部を基に現在の姿勢を表すクォータニオンおよび角速度誤差を推定する拡張カルマンフィルタを構成し、
   前記拡張カルマンフィルタによるクォータニオン推定値を現在の姿勢として出力することを特徴とする小型姿勢センサ。
2. 前記拡張カルマンフィルタは、
   の式および、
   低周波数の加速度外乱
   ａ_dr＝［ａ_bx ａ_by ａ_bz］^T
   に対して
   のダイナミクスを定義し、式（６１）およびこのダイナミクスについて、
   を状態量としたときの状態方程式
   ただし、前記状態方程式において各行列は次式のようになっている、
   を演算することを特徴とする請求項１に記載の小型姿勢センサ。
3. 前記拡張カルマンフィルタは、請求項２の演算に追加して、式（６２）の状態方程式を離散化して、
   ただし、ｘ_t はt ステップ目の状態量であり、Δt はサンプリングタイムである、
   の式を求め、ここで、改めて
   とおいて、離散時間の状態方程式を、
   の形に求め、続いて、式（６２）の状態方程式で推定された低周波数の加速度外乱を観測方程式に含めて、
   なる観測方程式を求め、
   ここで、
   として、
   ただし、ここでｈ_t（ｘ_t）は、
   なる形の、ｙ_t を出力とする観測方程式を求め、この観測方程式により推定値を演算することを特徴とする請求項１記載の小型姿勢センサ。
4. 前記拡張カルマンフィルタは、請求項３の演算に追加して、
   行列Ｆ_t及びＨ_tを、
   のように定義し、
   以上のように定義された行列と式（６４）、式（６７）を用いた拡張カルマンフィルタアルゴリズムを以下に示す式で与え、
   これらのアルゴリズムにより、演算処理することを特徴とする請求項１に記載の小型姿勢センサ。