JP5747752B2 - 姿勢推定装置、姿勢推定方法、姿勢推定プログラム - Google Patents

Description

本発明は、姿勢推定装置、姿勢推定方法、及び、姿勢推定プログラムに関する。

この種の技術として、特許文献１は、チルト検出手段と角速度検出手段を有した傾動可能な物体の姿勢を推定する方法を開示している。

特表２００３−５２１６９７号公報

ところで、本願出願人は、移動ロボットの姿勢を推定する技術を開発しており、具体的には、移動ロボットに、姿勢角度を計測する傾斜センサ（例えば３軸加速度センサ）と、姿勢角度の変化である姿勢角速度を計測するジャイロセンサとを設けている。そして、傾斜センサの計測結果と、ジャイロセンサの計測結果とをフィルタ処理により合成することで、移動ロボットの姿勢を精度よく推定することとしている。即ち、移動ロボットの姿勢の推定は、以下の式により行われる。以下の式（１）〜（３）は、オイラー角表現による１つの軸についてのフィルタ処理を示している。

α^ドット _measuredは、ジャイロセンサによる姿勢角速度の計測結果である。
α_measuredは、傾斜センサによる姿勢角度の計測結果である。
α_predictは、ジャイロセンサによる姿勢角速度の計測結果を用いて予測した、移動ロボットの姿勢角度の予測値である。
α_nは、移動ロボットの姿勢角度の予測値に、傾斜センサによる姿勢角度の計測結果を加味した、移動ロボットの姿勢角度の推定値である。
b_nは、ジャイロセンサの姿勢角速度バイアスの推定値である。
b_n-1は、前回のジャイロセンサの姿勢角速度バイアスの推定値である。
dtは、サンプリング周期である。
K₀は、カルマンフィルタ係数である。
K₁は、カルマンフィルタ係数である。

上記式（２）は１つの軸についてのフィルタ処理を示しているので、３つの軸については、上記フィルタ処理を３回繰り返すことになり、計算コストが高い。これに対し、本願出願人は、四元数の概念を導入して、上記式（２）の計算コストを抑えることに成功している。詳しくは以下の通りである。

上記式（２）を変形すると、下記式（４）となる。下記式（４）においてα_nは、α_predictとα_measuredという２つの角度を(1-K₀):K₀の比率で補間する角度に相当している。αをオイラー角による１つの軸の角度としてではなく四元数であるとすると、下記式（４）の演算は球面線形補間（slerp：Spherical linear interpolation）と同一の形として捉えることができる。

ここで、q₀とq₁の２つの四元数の間を変数tで補間する関数slerp(q₀,q₁,t)は理論上、下記式（５）によって定義される。

上記式（５）は、q₀とq₁の間の弧の長さであるωを利用して下記式（６）で代替することができる。

そして、２つの四元数の間の三角関数は下記式（７）で簡単に求められる。

従って、下記式（８）のようになる。

以上のように、αをオイラー角による１つの軸の姿勢角度ではなく四元数であるとして四元数特有の球面線形補間を利用することにより、上記式（２）を低い計算コストで実行することが可能となる。このような上記式（２）についての効率的な計算手法については、本願出願人が既に特許出願している。

一方で、上記式（３）の演算については改善の余地が残されていた。詳しくは、上記式（３）については、上記式（２）と違って、四元数特有の球面線形補間を利用するかたちには持っていくことができない。従って、α_measured等が既に四元数であった場合には、一度オイラー角表現に戻して演算を実行し、再度四元数に戻すといった計算が必要となる。そして、四元数とオイラー角の間の変換には、三角関数や逆関数といった時間がかかる関数を多用するため、結果として上記式（３）の演算には多くの時間を費やしていた。

本願発明の目的は、姿勢角速度バイアスを推定する計算の効率を向上する技術を提供することにある。

本願発明の第１の観点によれば、傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定装置であって、前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成する第１姿勢角度四元数生成手段と、前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成する第２姿勢角度四元数生成手段と、フィルタ係数K₁を生成するフィルタ係数生成手段と、下記式（９）により姿勢角速度バイアス四元数b_nを推定する姿勢角速度バイアス四元数推定手段と、を備えた姿勢推定装置が提供される。ただし、b_n-1は、前回の姿勢角速度バイアス四元数である。以上の構成によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま前記姿勢角速度バイアス四元数b_nを推定することができる。従って、前記角速度バイアス四元数b_nを高い計算効率で推定することができる。

好ましくは、前記姿勢角速度バイアス推定手段は、前記フィルタ係数K₁が負である場合は、前記フィルタ係数K₁の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数b_nを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。

好ましくは、前記姿勢角速度バイアス推定手段は、前記フィルタ係数K₁の絶対値が１を超える場合は、前記フィルタ係数K₁の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数b_nを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。

本願発明の第２の観点によれば、傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定方法であって、前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成する第１姿勢角度四元数生成ステップと、前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成する第２姿勢角度四元数生成ステップと、フィルタ係数K₁を生成するフィルタ係数生成ステップと、上記式（９）により姿勢角速度バイアス四元数b_nを推定する姿勢角速度バイアス四元数推定ステップと、を備えた姿勢推定方法が提供される。ただし、b_n-1は、前回の姿勢角速度バイアス四元数である。

また、コンピュータに、上記の姿勢推定方法を実行させるための姿勢推定プログラムが提供される。

本願発明によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま前記姿勢角速度バイアス四元数b_nを推定することができる。従って、前記角速度バイアス四元数b_nを高い計算効率で推定することができる。

図１は、倒立二輪車本体のブロック図である。 図２は、姿勢推定部のブロック図である。 図３は、姿勢角速度バイアス四元数推定部のブロック図である。 図４は、倒立二輪車の制御フローである。

以下、図１〜４を参照して、本願発明の実施形態を説明する。

倒立二輪車１（移動ロボット、機器）は、倒立二輪車本体２（機器本体）と、図示しない台車、ハンドル、車輪等を備えて構成されている。

倒立二輪車本体２は、傾斜角センサ３と、ジャイロセンサ４と、CPU５（Central Processing Unit）と、RAM６（Random Access Memory）と、ROM７（Read Only Memory）と、によって構成されている。ROM７には、姿勢推定プログラムと姿勢制御プログラムが記憶されている。姿勢推定プログラムと姿勢制御プログラムは、CPU５に読み込まれ、CPU５上で実行されることで、CPU５等のハードウェアに、姿勢推定部８（姿勢推定装置）及び姿勢制御部９としての機能を発揮させるようになっている。

姿勢推定部８は、倒立二輪車本体２の姿勢を傾斜角センサ３の計測結果とジャイロセンサ４の計測結果に基づいて推定するものである。なお、「倒立二輪車本体２の姿勢」は、「倒立二輪車本体２の姿勢角度」と「倒立二輪車本体２の姿勢角速度」を含む概念である。

姿勢制御部９は、姿勢推定部８によって推定された倒立二輪車本体２の姿勢に基づいて、倒立二輪車本体２の姿勢を制御するものである。

図２に示すように、姿勢推定部８は、第１姿勢角度四元数生成部２０（第１姿勢角度四元数生成手段）と、第２姿勢角度四元数生成部２１（第２姿勢角度四元数生成手段）と、フィルタ係数生成部２２（フィルタ係数生成手段）と、姿勢角度四元数合成部２３と、姿勢角度四元数正規化部２４と、オイラー角変換部２５と、姿勢角速度バイアス四元数推定部２６（姿勢角速度バイアス四元数推定手段）と、姿勢角速度バイアス四元数正規化部２７と、によって構成されている。

図３に示すように、姿勢角速度バイアス四元数推定部２６は、差分演算部４０（差分演算手段）と、フィルタ演算部４１（フィルタ演算手段）と、積分演算部４２（積分演算手段）と、によって構成されている。

（傾斜角センサ３）
図１に戻り、傾斜角センサ３は、倒立二輪車本体２の現在の姿勢角度を計測するためのものである。本実施形態において傾斜角センサ３は、３軸加速度センサと地磁気センサの組み合わせによって構成されている。３軸加速度センサは、倒立二輪車本体２の姿勢角度のうちロール角度とピッチ角度を計測可能である。地磁気センサは、倒立二輪車本体２の姿勢角度のうちヨー角度を計測可能である。傾斜角センサ３は、計測により得られた姿勢角度信号を姿勢推定部８の第１姿勢角度四元数生成部２０に出力する。

（ジャイロセンサ４）
ジャイロセンサ４は、倒立二輪車本体２の現在の姿勢角速度を計測するためのものである。本実施形態においてジャイロセンサ４は、MEMSジャイロによって構成されている。ジャイロセンサ４は、計測により得られた姿勢角速度信号を姿勢推定部８の第２姿勢角度四元数生成部２１に出力する。

（第１姿勢角度四元数生成部２０）
図２の第１姿勢角度四元数生成部２０は、傾斜角センサ３の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成するものである。具体的には以下の通りである。

（→フィルタ処理）
先ず、第１姿勢角度四元数生成部２０は、傾斜角センサ３から受信した姿勢角度信号をフィルタ処理して、姿勢角度信号に含まれる望ましくない周波数帯域をカットする。

（→取り付け角度補正処理）
次に、第１姿勢角度四元数生成部２０は、倒立二輪車本体２に対する３軸加速度センサの取り付け角度に基づいて、姿勢角度信号を補正する。これにより、姿勢角度信号から、３軸加速度センサの取り付け角度による影響がキャンセルされる。

（→自加速度キャンセル処理）
次に、第１姿勢角度四元数生成部２０は、車輪の回転数を微分して得られる倒立二輪車本体２の加速度に基づいて、姿勢角度信号を補正する。これにより、姿勢角度信号から、自加速度による影響がキャンセルされる。

（→姿勢角度行列変換処理）
次に、第１姿勢角度四元数生成部２０は、姿勢角度信号（加速度センサベクトル）にヨー角（方位角）を加えた回転を姿勢角度行列に変換する。

（→四元数変換処理）
次に、第１姿勢角度四元数生成部２０は、姿勢角度行列を、姿勢角度の四元数表現である第１姿勢角度四元数α_measuredに変換する。

そして、第１姿勢角度四元数生成部２０は、生成した第１姿勢角度四元数α_measuredを、姿勢角度四元数合成部２３と姿勢角速度バイアス四元数推定部２６に出力する。

（第２姿勢角度四元数生成部２１）
第２姿勢角度四元数生成部２１は、ジャイロセンサ４の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成するものである。具体的には以下の通りである。

（→フィルタ処理）
先ず、第２姿勢角度四元数生成部２１は、ジャイロセンサ４から受信した姿勢角速度信号をフィルタ処理して、姿勢角速度信号に含まれる望ましくない周波数帯域をカットする。

（→予測処理）
次に、第２姿勢角度四元数生成部２１は、RAM６から、前回の姿勢角速度バイアスb_n-1を取得した上で、姿勢角速度信号α^ドット _measuredから前回の姿勢角速度バイアスb_n-1を引き算し、これにサンプリング周期dtを掛け、前回の姿勢角度α_n-1に積算し、結果を四元数に変換する。これにより、第２姿勢角度四元数α_predictが求められる。なお、この予測処理は、始めから四元数で演算してもよいし、とりあえずオイラー角で演算して最後に四元数変換することとしてもよい。オイラー角で演算する場合は、下記式（１）による。

そして、第２姿勢角度四元数生成部２１は、生成した第２姿勢角度四元数α_predictを姿勢角度四元数合成部２３と姿勢角速度バイアス四元数推定部２６に出力する。

（フィルタ係数生成部２２）
フィルタ係数生成部２２は、状態フィードバックを実現する際のオブザーバの一種として、カルマンフィルタ係数K₀、K₁を生成する。カルマンフィルタ係数K₀は姿勢角の計測ノイズをフィルタして姿勢角にフィードバックするための係数であり、カルマンフィルタ係数K₁は姿勢角の計測ノイズをフィルタして姿勢角速度バイアスにフィードバックするための係数である。

（姿勢角度四元数合成部２３）
姿勢角度四元数合成部２３は、第１姿勢角度四元数α_measuredと第２姿勢角度四元数α_predictをカルマンフィルタ係数K₀でのフィルタ処理により合成して、倒立二輪車本体２の現在の姿勢角度α_n（推定値）を生成する。この姿勢角度四元数合成部２３による合成処理は、前述したように球面線形補間を利用することで効率良く実行できることを示した。必要であれば、オイラー角で表現した式（２）を参照されたい。姿勢角度四元数合成部２３は、生成した倒立二輪車本体２の現在の姿勢角度α_n（推定値）を姿勢角度四元数正規化部２４に出力する。

（姿勢角度四元数正規化部２４）
姿勢角度四元数正規化部２４は、浮動小数点演算の誤差の蓄積を回避すべく、倒立二輪車本体２の現在の姿勢角度α_nを正規化する。姿勢角度四元数正規化部２４は、正規化した姿勢角度α_nをオイラー角変換部２５に出力する。

（オイラー角変換部２５）
オイラー角変換部２５は、姿勢角度α_nをオイラー角表現に変換した上で姿勢制御部９に出力する。

（姿勢角速度バイアス四元数推定部２６：図３）
姿勢角速度バイアス四元数推定部２６は、下記式（９）により角速度バイアス四元数b_nを推定する。具体的には以下の通りである。

姿勢角速度バイアスb_nは、前述したように下記式（３）により求められる。

上記式（３）は、前述した式（２）のようには球面線形補間を利用できるかたちに変形することができない。そこで、姿勢角速度バイアス四元数推定部２６は、四元数の累乗（べき乗）という概念を導入して、上記式（３）を効率的に解く。詳しく説明すると、q^tは四元数qのt乗を意味するが、t=0のときq⁰=1、即ち恒等四元数(1,0,0,0)を示す。また、t=1のときq¹=qとなり、実数と同等の振る舞いをする。注意すべきは、t<0の場合とt>1の場合であり、四元数は360度以上の回転を示すことができないので、tを大きくして複数回転を表すようなことはできない。(a^s)^t=a^stのようなスカラーの累乗に関する代数が当てはまらないことがある。しかし、今回のようなフィルタ演算の場合は、ノイズをフィルタリングして小さくするという意味からK₁の絶対値は必ず1より小さい値になる上に、第１姿勢角度四元数α_measuredも第２姿勢角度四元数α_predictもある1つの角度を示すことから累乗の乗数が０〜１の範囲で使用することができる。なお、四元数の累乗は乗数が０以上１以下の範囲で使用することが一般的である。

さて、オイラー角表現での足し算は四元数表現では外積（演算子は×）であり、オイラー角表現での引き算は四元数表現では共役四元数（演算子は^−１）との外積である。また、回転のスカラーの掛け算は、四元数表現での累乗に相当する。従って、上記式（３）は、上記式（９）に置き換えることができる。上記式（９）の意味としては、姿勢角度四元数αのノイズ成分をカルマンフィルタ係数K₁でフィルタリングし、これを用いて姿勢角速度バイアス四元数b_n-1を回転させて修正する、ということになる。

従って、図３において、差分演算部４０は、第１姿勢角度四元数生成部２０から取得した第１姿勢角度四元数α_measuredと、第２姿勢角度四元数生成部２１から取得した第２姿勢角度四元数α_predictと、の差分を求めて姿勢角度四元数αのノイズ成分を算出する。

次に、フィルタ演算部４１は、差分演算部４０が算出した姿勢角度四元数αのノイズ成分をカルマンフィルタ係数K₁でフィルタ処理する。このとき、カルマンフィルタ係数K₁が負である場合は、フィルタ演算部４１は、カルマンフィルタ係数K₁の正負を反転させた上で、姿勢角度四元数αのノイズ成分をフィルタ処理する。即ち、上記式（９）に代えて下記式（１０）を用いる。

また、フィルタの方式としてカルマンフィルタなどの係数が変化するタイプが存在する。例えば逐次カルマンフィルタではK₀及びK₁の絶対値が一時的に１より大きな値になることがある。この場合、フィルタ演算部４１は、カルマンフィルタ係数K₁の絶対値を１にした上で、前記の式（９）又は（１０）により姿勢角速度バイアス四元数b_nを推定すればよい。カルマンフィルタ係数K₀についても同様である。

次に、積分演算部４２は、フィルタ処理後の値を、前回の姿勢角速度バイアス四元数b_n-1に積算して、現在の姿勢角速度バイアス四元数b_nを求める。積分演算部４２は、求めた姿勢角速度バイアス四元数b_nを姿勢角速度バイアス四元数正規化部２７に出力する。

（姿勢角速度バイアス四元数正規化部２７）
姿勢角速度バイアス四元数正規化部２７は、浮動小数点演算の誤差の蓄積を回避すべく、姿勢角速度バイアス四元数b_nを正規化する。そして、姿勢角速度バイアス四元数正規化部２７は、正規化した姿勢角速度バイアス四元数b_nをRAM６に保存する。

次に、図４を参照して、主として姿勢推定部８の作動を説明する。

（傾斜角センサによる計測ステップ）
倒立二輪車１の電源を投入すると（S300）、傾斜角センサ３は、倒立二輪車本体２の現在の姿勢角度を計測して、姿勢角度信号を第１姿勢角度四元数生成部２０に出力する（S310）。次に、ジャイロセンサ４は、倒立二輪車本体２の現在の姿勢角速度を計測して、姿勢角速度信号を第２姿勢角度四元数生成部２１に出力する（S320）。次に、第１姿勢角度四元数生成部２０は、傾斜角センサ３から受信した姿勢角度信号に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成する（S330）。次に、第２姿勢角度四元数生成部２１は、ジャイロセンサ４から受信した姿勢角速度信号に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成する（S340）。次に、フィルタ係数生成部２２は、カルマンフィルタ係数K₀、K₁を生成する（S350）。なお、S310とS320の処理は可換であり、S330〜S350の処理は適宜順番を入れ替えても良い。次に、姿勢角速度バイアス四元数推定部２６は、下記式（９）により姿勢角速度バイアス四元数b_nを推定する（S360）。そして、処理をS310に戻して次の周期に備える。

以上に本願発明の好適な実施形態を説明したが、上記実施形態は、要するに、以下の特長を有している。

姿勢推定部８（姿勢推定装置）は、傾斜角センサ３とジャイロセンサ４を備えた倒立二輪車本体２（機器本体）の姿勢を傾斜角センサ３の計測結果とジャイロセンサ４の計測結果に基づいて推定するものである。姿勢推定部８は、傾斜角センサ３の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成する第１姿勢角度四元数生成部２０（第１姿勢角度四元数生成手段）と、ジャイロセンサ４の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成する第２姿勢角度四元数生成部２１（第２姿勢角度四元数生成手段）と、カルマンフィルタ係数K₁（フィルタ係数K₁）を生成するフィルタ係数生成部２２（フィルタ係数生成手段）と、下記式（９）により姿勢角速度バイアス四元数b_nを推定する姿勢角速度バイアス四元数推定部２６（姿勢角速度バイアス四元数推定手段）と、を備える。ただし、b_n-1は、前回の姿勢角速度バイアス四元数である。

以上の構成によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま姿勢角速度バイアス四元数b_nを推定することができる。従って、角速度バイアス四元数b_nを高い計算効率で推定することができる。

また、姿勢角速度バイアス姿勢推定部８は、カルマンフィルタ係数K₁が負である場合は、カルマンフィルタ係数K₁の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数b_nを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。

また、姿勢角速度バイアス姿勢推定部８は、カルマンフィルタ係数K₁の絶対値が１を超える場合は、カルマンフィルタ係数K₁の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数b_nを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。

なお、角速度バイアスb_nの推定を始めるに際し、角速度バイアスb_nの初期値は「回転しない」ことを表す恒等四元数（1,0,0,0）とすることが考えれる。この場合、倒立二輪車１の状態（温度や各角速度センサのバイアス固有値）によりいずれある値に収束していくことになる。好ましくは、倒立二輪車１を起動し、姿勢推定のフィルタ演算を開始するときに姿勢角速度バイアスb_nの初期値を恒等四元数から開始することに代えて、前回の起動後に収束した値を初期値として採用すれば、一層安定した姿勢角速度バイアスb_nの推定を行うことができる。

１ 倒立二輪車
２ 倒立二輪車本体
３ 傾斜角センサ
４ ジャイロセンサ
８ 姿勢推定部
９ 姿勢制御部
２６ 姿勢角速度バイアス四元数推定部

Claims (5)

1. 傾斜角センサとジャイロセンサを有する機器本体において、ジャイロセンサの静止状態における出力値の四元数表現である角速度バイアス四元数を推定するバイアス四元数推定装置であって、
   前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第1姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成手段と、
   前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成手段と、
   フィルタ係数K1を生成するフィルタ係数生成手段と、
   下記式により角速度バイアス四元数bnを推定する角速度バイアス四元数推定手段と、
   を備えたバイアス四元数推定装置。
   

   

   ただし、bn-1は、前回の角速度バイアス四元数である。
2. 請求項１に記載のバイアス四元数推定装置であって、
   前記第２姿勢角度四元数生成手段は、前記ジャイロセンサの計測結果から前回の角速度バイアスを引き算し、引き算した結果にサンプリング周期を掛け算し、前記機器本体の前回の姿勢角度に積算することで、前記第２姿勢角度四元数αpredictを生成する、
   バイアス四元数推定装置。
3. 請求項１又は２に記載の姿勢推定装置であって、
   前記角速度バイアス四元数推定手段は、前記フィルタ係数K1が負である場合は、前記フィルタ係数K1の正負を反転させた上で、請求項１の式により前記角速度バイアス四元数bnを推定する、
   バイアス四元数推定装置。
4. 請求項１〜３の何れかに記載の姿勢推定装置であって、
   前記角速度バイアス四元数推定手段は、前記フィルタ係数K1の絶対値が１を超える場合は、前記フィルタ係数K1の絶対値を１にした上で、請求項１の式により前記角速度バイアス四元数bnを推定する、
   バイアス四元数推定装置。
5. 傾斜角センサとジャイロセンサを有する機器本体において、ジャイロセンサの静止状態における出力値の四元数表現である角速度バイアス四元数を推定するバイアス四元数推定方法であって、
   前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第1姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成ステップと、
   前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成ステップと、
   フィルタ係数K1を生成するフィルタ係数生成ステップと、
   下記式により角速度バイアス四元数bnを推定する角速度バイアス四元数推定ステップと、
   を含む、バイアス四元数推定方法。
   

   

   ただし、bn-1は、前回の角速度バイアス四元数である。

Images