姿勢推定装置、姿勢推定方法、姿勢推定プログラム
【課題】姿勢角速度バイアスを推定する計算の効率を向上する技術を提供する。
【解決手段】姿勢推定部８は、傾斜角センサの計測結果に基づいて倒立二輪車本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成部２０と、ジャイロセンサの計測結果に基づいて倒立二輪車本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成部２１と、カルマンフィルタ係数K1を生成するフィルタ係数生成部２２と、下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定部２６と、を備える。ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【解決手段】姿勢推定部８は、傾斜角センサの計測結果に基づいて倒立二輪車本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成部２０と、ジャイロセンサの計測結果に基づいて倒立二輪車本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成部２１と、カルマンフィルタ係数K1を生成するフィルタ係数生成部２２と、下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定部２６と、を備える。ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、姿勢推定装置、姿勢推定方法、及び、姿勢推定プログラムに関する。
【背景技術】
【０００２】
この種の技術として、特許文献１は、チルト検出手段と角速度検出手段を有した傾動可能な物体の姿勢を推定する方法を開示している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特表２００３−５２１６９７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、本願出願人は、移動ロボットの姿勢を推定する技術を開発しており、具体的には、移動ロボットに、姿勢角度を計測する傾斜センサ（例えば３軸加速度センサ）と、姿勢角度の変化である姿勢角速度を計測するジャイロセンサとを設けている。そして、傾斜センサの計測結果と、ジャイロセンサの計測結果とをフィルタ処理により合成することで、移動ロボットの姿勢を精度よく推定することとしている。即ち、移動ロボットの姿勢の推定は、以下の式により行われる。以下の式（１）〜（３）は、オイラー角表現による１つの軸についてのフィルタ処理を示している。
【０００５】
【数１】
【０００６】
【数２】
【０００７】
【数３】
【０００８】
αドットmeasuredは、ジャイロセンサによる姿勢角速度の計測結果である。
αmeasuredは、傾斜センサによる姿勢角度の計測結果である。
αpredictは、ジャイロセンサによる姿勢角速度の計測結果を用いて予測した、移動ロボットの姿勢角度の予測値である。
αnは、移動ロボットの姿勢角度の予測値に、傾斜センサによる姿勢角度の計測結果を加味した、移動ロボットの姿勢角度の推定値である。
bnは、ジャイロセンサの姿勢角速度バイアスの推定値である。
bn-1は、前回のジャイロセンサの姿勢角速度バイアスの推定値である。
dtは、サンプリング周期である。
K0は、カルマンフィルタ係数である。
K1は、カルマンフィルタ係数である。
【０００９】
上記式（２）は１つの軸についてのフィルタ処理を示しているので、３つの軸については、上記フィルタ処理を３回繰り返すことになり、計算コストが高い。これに対し、本願出願人は、四元数の概念を導入して、上記式（２）の計算コストを抑えることに成功している。詳しくは以下の通りである。
【００１０】
上記式（２）を変形すると、下記式（４）となる。下記式（４）においてαnは、αpredictとαmeasuredという２つの角度を(1-K0):K0の比率で補間する角度に相当している。αをオイラー角による１つの軸の角度としてではなく四元数であるとすると、下記式（４）の演算は球面線形補間（slerp：Spherical linear interpolation）と同一の形として捉えることができる。
【００１１】
【数４】
【００１２】
ここで、q0とq1の２つの四元数の間を変数tで補間する関数slerp(q0,q1,t)は理論上、下記式（５）によって定義される。
【００１３】
【数５】
【００１４】
上記式（５）は、q0とq1の間の弧の長さであるωを利用して下記式（６）で代替することができる。
【００１５】
【数６】
【００１６】
そして、２つの四元数の間の三角関数は下記式（７）で簡単に求められる。
【００１７】
【数７】
【００１８】
従って、下記式（８）のようになる。
【００１９】
【数８】
【００２０】
以上のように、αをオイラー角による１つの軸の姿勢角度ではなく四元数であるとして四元数特有の球面線形補間を利用することにより、上記式（２）を低い計算コストで実行することが可能となる。このような上記式（２）についての効率的な計算手法については、本願出願人が既に特許出願している。
【００２１】
一方で、上記式（３）の演算については改善の余地が残されていた。詳しくは、上記式（３）については、上記式（２）と違って、四元数特有の球面線形補間を利用するかたちには持っていくことができない。従って、αmeasured等が既に四元数であった場合には、一度オイラー角表現に戻して演算を実行し、再度四元数に戻すといった計算が必要となる。そして、四元数とオイラー角の間の変換には、三角関数や逆関数といった時間がかかる関数を多用するため、結果として上記式（３）の演算には多くの時間を費やしていた。
【００２２】
本願発明の目的は、姿勢角速度バイアスを推定する計算の効率を向上する技術を提供することにある。
【課題を解決するための手段】
【００２３】
本願発明の第１の観点によれば、傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定装置であって、前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成手段と、前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成手段と、フィルタ係数K1を生成するフィルタ係数生成手段と、下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定手段と、を備えた姿勢推定装置が提供される。ただし、bn-1は、前回の姿勢角速度バイアス四元数である。以上の構成によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま前記姿勢角速度バイアス四元数bnを推定することができる。従って、前記角速度バイアス四元数bnを高い計算効率で推定することができる。
【００２４】
【数９】
【００２５】
好ましくは、前記姿勢角速度バイアス推定手段は、前記フィルタ係数K1が負である場合は、前記フィルタ係数K1の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。
【００２６】
好ましくは、前記姿勢角速度バイアス推定手段は、前記フィルタ係数K1の絶対値が１を超える場合は、前記フィルタ係数K1の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。
【００２７】
本願発明の第２の観点によれば、傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定方法であって、前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成ステップと、前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成ステップと、フィルタ係数K1を生成するフィルタ係数生成ステップと、上記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定ステップと、を備えた姿勢推定方法が提供される。ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【００２８】
また、コンピュータに、上記の姿勢推定方法を実行させるための姿勢推定プログラムが提供される。
【発明の効果】
【００２９】
本願発明によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま前記姿勢角速度バイアス四元数bnを推定することができる。従って、前記角速度バイアス四元数bnを高い計算効率で推定することができる。
【図面の簡単な説明】
【００３０】
【図１】図１は、倒立二輪車本体のブロック図である。
【図２】図２は、姿勢推定部のブロック図である。
【図３】図３は、姿勢角速度バイアス四元数推定部のブロック図である。
【図４】図４は、倒立二輪車の制御フローである。
【発明を実施するための形態】
【００３１】
以下、図１〜４を参照して、本願発明の実施形態を説明する。
【００３２】
倒立二輪車１（移動ロボット、機器）は、倒立二輪車本体２（機器本体）と、図示しない台車、ハンドル、車輪等を備えて構成されている。
【００３３】
倒立二輪車本体２は、傾斜角センサ３と、ジャイロセンサ４と、CPU５（Central Processing Unit）と、RAM６（Random Access Memory）と、ROM７（Read Only Memory）と、によって構成されている。ROM７には、姿勢推定プログラムと姿勢制御プログラムが記憶されている。姿勢推定プログラムと姿勢制御プログラムは、CPU５に読み込まれ、CPU５上で実行されることで、CPU５等のハードウェアに、姿勢推定部８（姿勢推定装置）及び姿勢制御部９としての機能を発揮させるようになっている。
【００３４】
姿勢推定部８は、倒立二輪車本体２の姿勢を傾斜角センサ３の計測結果とジャイロセンサ４の計測結果に基づいて推定するものである。なお、「倒立二輪車本体２の姿勢」は、「倒立二輪車本体２の姿勢角度」と「倒立二輪車本体２の姿勢角速度」を含む概念である。
【００３５】
姿勢制御部９は、姿勢推定部８によって推定された倒立二輪車本体２の姿勢に基づいて、倒立二輪車本体２の姿勢を制御するものである。
【００３６】
図２に示すように、姿勢推定部８は、第１姿勢角度四元数生成部２０（第１姿勢角度四元数生成手段）と、第２姿勢角度四元数生成部２１（第２姿勢角度四元数生成手段）と、フィルタ係数生成部２２（フィルタ係数生成手段）と、姿勢角度四元数合成部２３と、姿勢角度四元数正規化部２４と、オイラー角変換部２５と、姿勢角速度バイアス四元数推定部２６（姿勢角速度バイアス四元数推定手段）と、姿勢角速度バイアス四元数正規化部２７と、によって構成されている。
【００３７】
図３に示すように、姿勢角速度バイアス四元数推定部２６は、差分演算部４０（差分演算手段）と、フィルタ演算部４１（フィルタ演算手段）と、積分演算部４２（積分演算手段）と、によって構成されている。
【００３８】
（傾斜角センサ３）
図１に戻り、傾斜角センサ３は、倒立二輪車本体２の現在の姿勢角度を計測するためのものである。本実施形態において傾斜角センサ３は、３軸加速度センサと地磁気センサの組み合わせによって構成されている。３軸加速度センサは、倒立二輪車本体２の姿勢角度のうちロール角度とピッチ角度を計測可能である。地磁気センサは、倒立二輪車本体２の姿勢角度のうちヨー角度を計測可能である。傾斜角センサ３は、計測により得られた姿勢角度信号を姿勢推定部８の第１姿勢角度四元数生成部２０に出力する。
【００３９】
（ジャイロセンサ４）
ジャイロセンサ４は、倒立二輪車本体２の現在の姿勢角速度を計測するためのものである。本実施形態においてジャイロセンサ４は、MEMSジャイロによって構成されている。ジャイロセンサ４は、計測により得られた姿勢角速度信号を姿勢推定部８の第２姿勢角度四元数生成部２１に出力する。
【００４０】
（第１姿勢角度四元数生成部２０）
図２の第１姿勢角度四元数生成部２０は、傾斜角センサ３の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成するものである。具体的には以下の通りである。
【００４１】
（→フィルタ処理）
先ず、第１姿勢角度四元数生成部２０は、傾斜角センサ３から受信した姿勢角度信号をフィルタ処理して、姿勢角度信号に含まれる望ましくない周波数帯域をカットする。
【００４２】
（→取り付け角度補正処理）
次に、第１姿勢角度四元数生成部２０は、倒立二輪車本体２に対する３軸加速度センサの取り付け角度に基づいて、姿勢角度信号を補正する。これにより、姿勢角度信号から、３軸加速度センサの取り付け角度による影響がキャンセルされる。
【００４３】
（→自加速度キャンセル処理）
次に、第１姿勢角度四元数生成部２０は、車輪の回転数を微分して得られる倒立二輪車本体２の加速度に基づいて、姿勢角度信号を補正する。これにより、姿勢角度信号から、自加速度による影響がキャンセルされる。
【００４４】
（→姿勢角度行列変換処理）
次に、第１姿勢角度四元数生成部２０は、姿勢角度信号（加速度センサベクトル）にヨー角（方位角）を加えた回転を姿勢角度行列に変換する。
【００４５】
（→四元数変換処理）
次に、第１姿勢角度四元数生成部２０は、姿勢角度行列を、姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredに変換する。
【００４６】
そして、第１姿勢角度四元数生成部２０は、生成した第１姿勢角度四元数αmeasuredを、姿勢角度四元数合成部２３と姿勢角速度バイアス四元数推定部２６に出力する。
【００４７】
（第２姿勢角度四元数生成部２１）
第２姿勢角度四元数生成部２１は、ジャイロセンサ４の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成するものである。具体的には以下の通りである。
【００４８】
（→フィルタ処理）
先ず、第２姿勢角度四元数生成部２１は、ジャイロセンサ４から受信した姿勢角速度信号をフィルタ処理して、姿勢角速度信号に含まれる望ましくない周波数帯域をカットする。
【００４９】
（→予測処理）
次に、第２姿勢角度四元数生成部２１は、RAM６から、前回の姿勢角速度バイアスbn-1を取得した上で、姿勢角速度信号αドットmeasuredから前回の姿勢角速度バイアスbn-1を引き算し、これにサンプリング周期dtを掛け、前回の姿勢角度αn-1に積算し、結果を四元数に変換する。これにより、第２姿勢角度四元数αpredictが求められる。なお、この予測処理は、始めから四元数で演算してもよいし、とりあえずオイラー角で演算して最後に四元数変換することとしてもよい。オイラー角で演算する場合は、下記式（１）による。
【００５０】
【数１０】
【００５１】
そして、第２姿勢角度四元数生成部２１は、生成した第２姿勢角度四元数αpredictを姿勢角度四元数合成部２３と姿勢角速度バイアス四元数推定部２６に出力する。
【００５２】
（フィルタ係数生成部２２）
フィルタ係数生成部２２は、状態フィードバックを実現する際のオブザーバの一種として、カルマンフィルタ係数K0、K1を生成する。カルマンフィルタ係数K0は姿勢角の計測ノイズをフィルタして姿勢角にフィードバックするための係数であり、カルマンフィルタ係数K1は姿勢角の計測ノイズをフィルタして姿勢角速度バイアスにフィードバックするための係数である。
【００５３】
（姿勢角度四元数合成部２３）
姿勢角度四元数合成部２３は、第１姿勢角度四元数αmeasuredと第２姿勢角度四元数αpredictをカルマンフィルタ係数K0でのフィルタ処理により合成して、倒立二輪車本体２の現在の姿勢角度αn（推定値）を生成する。この姿勢角度四元数合成部２３による合成処理は、前述したように球面線形補間を利用することで効率良く実行できることを示した。必要であれば、オイラー角で表現した式（２）を参照されたい。姿勢角度四元数合成部２３は、生成した倒立二輪車本体２の現在の姿勢角度αn（推定値）を姿勢角度四元数正規化部２４に出力する。
【００５４】
（姿勢角度四元数正規化部２４）
姿勢角度四元数正規化部２４は、浮動小数点演算の誤差の蓄積を回避すべく、倒立二輪車本体２の現在の姿勢角度αnを正規化する。姿勢角度四元数正規化部２４は、正規化した姿勢角度αnをオイラー角変換部２５に出力する。
【００５５】
（オイラー角変換部２５）
オイラー角変換部２５は、姿勢角度αnをオイラー角表現に変換した上で姿勢制御部９に出力する。
【００５６】
（姿勢角速度バイアス四元数推定部２６：図３）
姿勢角速度バイアス四元数推定部２６は、下記式（９）により角速度バイアス四元数bnを推定する。具体的には以下の通りである。
【００５７】
【数１１】
【００５８】
姿勢角速度バイアスbnは、前述したように下記式（３）により求められる。
【００５９】
【数１２】
【００６０】
上記式（３）は、前述した式（２）のようには球面線形補間を利用できるかたちに変形することができない。そこで、姿勢角速度バイアス四元数推定部２６は、四元数の累乗（べき乗）という概念を導入して、上記式（３）を効率的に解く。詳しく説明すると、qtは四元数qのt乗を意味するが、t=0のときq0=1、即ち恒等四元数(1,0,0,0)を示す。また、t=1のときq1=qとなり、実数と同等の振る舞いをする。注意すべきは、t<0の場合とt>1の場合であり、四元数は360度以上の回転を示すことができないので、tを大きくして複数回転を表すようなことはできない。(as)t=astのようなスカラーの累乗に関する代数が当てはまらないことがある。しかし、今回のようなフィルタ演算の場合は、ノイズをフィルタリングして小さくするという意味からK1の絶対値は必ず1より小さい値になる上に、第１姿勢角度四元数αmeasuredも第２姿勢角度四元数αpredictもある1つの角度を示すことから累乗の乗数が０〜１の範囲で使用することができる。なお、四元数の累乗は乗数が０以上１以下の範囲で使用することが一般的である。
【００６１】
さて、オイラー角表現での足し算は四元数表現では外積（演算子は×）であり、オイラー角表現での引き算は四元数表現では共役四元数（演算子は−１）との外積である。また、回転のスカラーの掛け算は、四元数表現での累乗に相当する。従って、上記式（３）は、上記式（９）に置き換えることができる。上記式（９）の意味としては、姿勢角度四元数αのノイズ成分をカルマンフィルタ係数K1でフィルタリングし、これを用いて姿勢角速度バイアス四元数bn-1を回転させて修正する、ということになる。
【００６２】
従って、図３において、差分演算部４０は、第１姿勢角度四元数生成部２０から取得した第１姿勢角度四元数αmeasuredと、第２姿勢角度四元数生成部２１から取得した第２姿勢角度四元数αpredictと、の差分を求めて姿勢角度四元数αのノイズ成分を算出する。
【００６３】
次に、フィルタ演算部４１は、差分演算部４０が算出した姿勢角度四元数αのノイズ成分をカルマンフィルタ係数K1でフィルタ処理する。このとき、カルマンフィルタ係数K1が負である場合は、フィルタ演算部４１は、カルマンフィルタ係数K1の正負を反転させた上で、姿勢角度四元数αのノイズ成分をフィルタ処理する。即ち、上記式（９）に代えて下記式（１０）を用いる。
【００６４】
【数１３】
【００６５】
また、フィルタの方式としてカルマンフィルタなどの係数が変化するタイプが存在する。例えば逐次カルマンフィルタではK0及びK1の絶対値が一時的に１より大きな値になることがある。この場合、フィルタ演算部４１は、カルマンフィルタ係数K1の絶対値を１にした上で、前記の式（９）又は（１０）により姿勢角速度バイアス四元数bnを推定すればよい。カルマンフィルタ係数K0についても同様である。
【００６６】
次に、積分演算部４２は、フィルタ処理後の値を、前回の姿勢角速度バイアス四元数bn-1に積算して、現在の姿勢角速度バイアス四元数bnを求める。積分演算部４２は、求めた姿勢角速度バイアス四元数bnを姿勢角速度バイアス四元数正規化部２７に出力する。
【００６７】
（姿勢角速度バイアス四元数正規化部２７）
姿勢角速度バイアス四元数正規化部２７は、浮動小数点演算の誤差の蓄積を回避すべく、姿勢角速度バイアス四元数bnを正規化する。そして、姿勢角速度バイアス四元数正規化部２７は、正規化した姿勢角速度バイアス四元数bnをRAM６に保存する。
【００６８】
次に、図４を参照して、主として姿勢推定部８の作動を説明する。
【００６９】
（傾斜角センサによる計測ステップ）
倒立二輪車１の電源を投入すると（S300）、傾斜角センサ３は、倒立二輪車本体２の現在の姿勢角度を計測して、姿勢角度信号を第１姿勢角度四元数生成部２０に出力する（S310）。次に、ジャイロセンサ４は、倒立二輪車本体２の現在の姿勢角速度を計測して、姿勢角速度信号を第２姿勢角度四元数生成部２１に出力する（S320）。次に、第１姿勢角度四元数生成部２０は、傾斜角センサ３から受信した姿勢角度信号に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する（S330）。次に、第２姿勢角度四元数生成部２１は、ジャイロセンサ４から受信した姿勢角速度信号に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する（S340）。次に、フィルタ係数生成部２２は、カルマンフィルタ係数K0、K1を生成する（S350）。なお、S310とS320の処理は可換であり、S330〜S350の処理は適宜順番を入れ替えても良い。次に、姿勢角速度バイアス四元数推定部２６は、下記式（９）により姿勢角速度バイアス四元数bnを推定する（S360）。そして、処理をS310に戻して次の周期に備える。
【００７０】
【数１４】
【００７１】
以上に本願発明の好適な実施形態を説明したが、上記実施形態は、要するに、以下の特長を有している。
【００７２】
姿勢推定部８（姿勢推定装置）は、傾斜角センサ３とジャイロセンサ４を備えた倒立二輪車本体２（機器本体）の姿勢を傾斜角センサ３の計測結果とジャイロセンサ４の計測結果に基づいて推定するものである。姿勢推定部８は、傾斜角センサ３の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成部２０（第１姿勢角度四元数生成手段）と、ジャイロセンサ４の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成部２１（第２姿勢角度四元数生成手段）と、カルマンフィルタ係数K1（フィルタ係数K1）を生成するフィルタ係数生成部２２（フィルタ係数生成手段）と、下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定部２６（姿勢角速度バイアス四元数推定手段）と、を備える。ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【００７３】
【数１５】
【００７４】
以上の構成によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま姿勢角速度バイアス四元数bnを推定することができる。従って、角速度バイアス四元数bnを高い計算効率で推定することができる。
【００７５】
また、姿勢角速度バイアス姿勢推定部８は、カルマンフィルタ係数K1が負である場合は、カルマンフィルタ係数K1の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。
【００７６】
また、姿勢角速度バイアス姿勢推定部８は、カルマンフィルタ係数K1の絶対値が１を超える場合は、カルマンフィルタ係数K1の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。
【００７７】
なお、角速度バイアスbnの推定を始めるに際し、角速度バイアスbnの初期値は「回転しない」ことを表す恒等四元数（1,0,0,0）とすることが考えれる。この場合、倒立二輪車１の状態（温度や各角速度センサのバイアス固有値）によりいずれある値に収束していくことになる。好ましくは、倒立二輪車１を起動し、姿勢推定のフィルタ演算を開始するときに姿勢角速度バイアスbnの初期値を恒等四元数から開始することに代えて、前回の起動後に収束した値を初期値として採用すれば、一層安定した姿勢角速度バイアスbnの推定を行うことができる。
【符号の説明】
【００７８】
１ 倒立二輪車
２ 倒立二輪車本体
３ 傾斜角センサ
４ ジャイロセンサ
８ 姿勢推定部
９ 姿勢制御部
２６ 姿勢角速度バイアス四元数推定部
【技術分野】
【０００１】
本発明は、姿勢推定装置、姿勢推定方法、及び、姿勢推定プログラムに関する。
【背景技術】
【０００２】
この種の技術として、特許文献１は、チルト検出手段と角速度検出手段を有した傾動可能な物体の姿勢を推定する方法を開示している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特表２００３−５２１６９７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、本願出願人は、移動ロボットの姿勢を推定する技術を開発しており、具体的には、移動ロボットに、姿勢角度を計測する傾斜センサ（例えば３軸加速度センサ）と、姿勢角度の変化である姿勢角速度を計測するジャイロセンサとを設けている。そして、傾斜センサの計測結果と、ジャイロセンサの計測結果とをフィルタ処理により合成することで、移動ロボットの姿勢を精度よく推定することとしている。即ち、移動ロボットの姿勢の推定は、以下の式により行われる。以下の式（１）〜（３）は、オイラー角表現による１つの軸についてのフィルタ処理を示している。
【０００５】
【数１】
【０００６】
【数２】
【０００７】
【数３】
【０００８】
αドットmeasuredは、ジャイロセンサによる姿勢角速度の計測結果である。
αmeasuredは、傾斜センサによる姿勢角度の計測結果である。
αpredictは、ジャイロセンサによる姿勢角速度の計測結果を用いて予測した、移動ロボットの姿勢角度の予測値である。
αnは、移動ロボットの姿勢角度の予測値に、傾斜センサによる姿勢角度の計測結果を加味した、移動ロボットの姿勢角度の推定値である。
bnは、ジャイロセンサの姿勢角速度バイアスの推定値である。
bn-1は、前回のジャイロセンサの姿勢角速度バイアスの推定値である。
dtは、サンプリング周期である。
K0は、カルマンフィルタ係数である。
K1は、カルマンフィルタ係数である。
【０００９】
上記式（２）は１つの軸についてのフィルタ処理を示しているので、３つの軸については、上記フィルタ処理を３回繰り返すことになり、計算コストが高い。これに対し、本願出願人は、四元数の概念を導入して、上記式（２）の計算コストを抑えることに成功している。詳しくは以下の通りである。
【００１０】
上記式（２）を変形すると、下記式（４）となる。下記式（４）においてαnは、αpredictとαmeasuredという２つの角度を(1-K0):K0の比率で補間する角度に相当している。αをオイラー角による１つの軸の角度としてではなく四元数であるとすると、下記式（４）の演算は球面線形補間（slerp：Spherical linear interpolation）と同一の形として捉えることができる。
【００１１】
【数４】
【００１２】
ここで、q0とq1の２つの四元数の間を変数tで補間する関数slerp(q0,q1,t)は理論上、下記式（５）によって定義される。
【００１３】
【数５】
【００１４】
上記式（５）は、q0とq1の間の弧の長さであるωを利用して下記式（６）で代替することができる。
【００１５】
【数６】
【００１６】
そして、２つの四元数の間の三角関数は下記式（７）で簡単に求められる。
【００１７】
【数７】
【００１８】
従って、下記式（８）のようになる。
【００１９】
【数８】
【００２０】
以上のように、αをオイラー角による１つの軸の姿勢角度ではなく四元数であるとして四元数特有の球面線形補間を利用することにより、上記式（２）を低い計算コストで実行することが可能となる。このような上記式（２）についての効率的な計算手法については、本願出願人が既に特許出願している。
【００２１】
一方で、上記式（３）の演算については改善の余地が残されていた。詳しくは、上記式（３）については、上記式（２）と違って、四元数特有の球面線形補間を利用するかたちには持っていくことができない。従って、αmeasured等が既に四元数であった場合には、一度オイラー角表現に戻して演算を実行し、再度四元数に戻すといった計算が必要となる。そして、四元数とオイラー角の間の変換には、三角関数や逆関数といった時間がかかる関数を多用するため、結果として上記式（３）の演算には多くの時間を費やしていた。
【００２２】
本願発明の目的は、姿勢角速度バイアスを推定する計算の効率を向上する技術を提供することにある。
【課題を解決するための手段】
【００２３】
本願発明の第１の観点によれば、傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定装置であって、前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成手段と、前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成手段と、フィルタ係数K1を生成するフィルタ係数生成手段と、下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定手段と、を備えた姿勢推定装置が提供される。ただし、bn-1は、前回の姿勢角速度バイアス四元数である。以上の構成によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま前記姿勢角速度バイアス四元数bnを推定することができる。従って、前記角速度バイアス四元数bnを高い計算効率で推定することができる。
【００２４】
【数９】
【００２５】
好ましくは、前記姿勢角速度バイアス推定手段は、前記フィルタ係数K1が負である場合は、前記フィルタ係数K1の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。
【００２６】
好ましくは、前記姿勢角速度バイアス推定手段は、前記フィルタ係数K1の絶対値が１を超える場合は、前記フィルタ係数K1の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。
【００２７】
本願発明の第２の観点によれば、傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定方法であって、前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成ステップと、前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成ステップと、フィルタ係数K1を生成するフィルタ係数生成ステップと、上記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定ステップと、を備えた姿勢推定方法が提供される。ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【００２８】
また、コンピュータに、上記の姿勢推定方法を実行させるための姿勢推定プログラムが提供される。
【発明の効果】
【００２９】
本願発明によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま前記姿勢角速度バイアス四元数bnを推定することができる。従って、前記角速度バイアス四元数bnを高い計算効率で推定することができる。
【図面の簡単な説明】
【００３０】
【図１】図１は、倒立二輪車本体のブロック図である。
【図２】図２は、姿勢推定部のブロック図である。
【図３】図３は、姿勢角速度バイアス四元数推定部のブロック図である。
【図４】図４は、倒立二輪車の制御フローである。
【発明を実施するための形態】
【００３１】
以下、図１〜４を参照して、本願発明の実施形態を説明する。
【００３２】
倒立二輪車１（移動ロボット、機器）は、倒立二輪車本体２（機器本体）と、図示しない台車、ハンドル、車輪等を備えて構成されている。
【００３３】
倒立二輪車本体２は、傾斜角センサ３と、ジャイロセンサ４と、CPU５（Central Processing Unit）と、RAM６（Random Access Memory）と、ROM７（Read Only Memory）と、によって構成されている。ROM７には、姿勢推定プログラムと姿勢制御プログラムが記憶されている。姿勢推定プログラムと姿勢制御プログラムは、CPU５に読み込まれ、CPU５上で実行されることで、CPU５等のハードウェアに、姿勢推定部８（姿勢推定装置）及び姿勢制御部９としての機能を発揮させるようになっている。
【００３４】
姿勢推定部８は、倒立二輪車本体２の姿勢を傾斜角センサ３の計測結果とジャイロセンサ４の計測結果に基づいて推定するものである。なお、「倒立二輪車本体２の姿勢」は、「倒立二輪車本体２の姿勢角度」と「倒立二輪車本体２の姿勢角速度」を含む概念である。
【００３５】
姿勢制御部９は、姿勢推定部８によって推定された倒立二輪車本体２の姿勢に基づいて、倒立二輪車本体２の姿勢を制御するものである。
【００３６】
図２に示すように、姿勢推定部８は、第１姿勢角度四元数生成部２０（第１姿勢角度四元数生成手段）と、第２姿勢角度四元数生成部２１（第２姿勢角度四元数生成手段）と、フィルタ係数生成部２２（フィルタ係数生成手段）と、姿勢角度四元数合成部２３と、姿勢角度四元数正規化部２４と、オイラー角変換部２５と、姿勢角速度バイアス四元数推定部２６（姿勢角速度バイアス四元数推定手段）と、姿勢角速度バイアス四元数正規化部２７と、によって構成されている。
【００３７】
図３に示すように、姿勢角速度バイアス四元数推定部２６は、差分演算部４０（差分演算手段）と、フィルタ演算部４１（フィルタ演算手段）と、積分演算部４２（積分演算手段）と、によって構成されている。
【００３８】
（傾斜角センサ３）
図１に戻り、傾斜角センサ３は、倒立二輪車本体２の現在の姿勢角度を計測するためのものである。本実施形態において傾斜角センサ３は、３軸加速度センサと地磁気センサの組み合わせによって構成されている。３軸加速度センサは、倒立二輪車本体２の姿勢角度のうちロール角度とピッチ角度を計測可能である。地磁気センサは、倒立二輪車本体２の姿勢角度のうちヨー角度を計測可能である。傾斜角センサ３は、計測により得られた姿勢角度信号を姿勢推定部８の第１姿勢角度四元数生成部２０に出力する。
【００３９】
（ジャイロセンサ４）
ジャイロセンサ４は、倒立二輪車本体２の現在の姿勢角速度を計測するためのものである。本実施形態においてジャイロセンサ４は、MEMSジャイロによって構成されている。ジャイロセンサ４は、計測により得られた姿勢角速度信号を姿勢推定部８の第２姿勢角度四元数生成部２１に出力する。
【００４０】
（第１姿勢角度四元数生成部２０）
図２の第１姿勢角度四元数生成部２０は、傾斜角センサ３の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成するものである。具体的には以下の通りである。
【００４１】
（→フィルタ処理）
先ず、第１姿勢角度四元数生成部２０は、傾斜角センサ３から受信した姿勢角度信号をフィルタ処理して、姿勢角度信号に含まれる望ましくない周波数帯域をカットする。
【００４２】
（→取り付け角度補正処理）
次に、第１姿勢角度四元数生成部２０は、倒立二輪車本体２に対する３軸加速度センサの取り付け角度に基づいて、姿勢角度信号を補正する。これにより、姿勢角度信号から、３軸加速度センサの取り付け角度による影響がキャンセルされる。
【００４３】
（→自加速度キャンセル処理）
次に、第１姿勢角度四元数生成部２０は、車輪の回転数を微分して得られる倒立二輪車本体２の加速度に基づいて、姿勢角度信号を補正する。これにより、姿勢角度信号から、自加速度による影響がキャンセルされる。
【００４４】
（→姿勢角度行列変換処理）
次に、第１姿勢角度四元数生成部２０は、姿勢角度信号（加速度センサベクトル）にヨー角（方位角）を加えた回転を姿勢角度行列に変換する。
【００４５】
（→四元数変換処理）
次に、第１姿勢角度四元数生成部２０は、姿勢角度行列を、姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredに変換する。
【００４６】
そして、第１姿勢角度四元数生成部２０は、生成した第１姿勢角度四元数αmeasuredを、姿勢角度四元数合成部２３と姿勢角速度バイアス四元数推定部２６に出力する。
【００４７】
（第２姿勢角度四元数生成部２１）
第２姿勢角度四元数生成部２１は、ジャイロセンサ４の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成するものである。具体的には以下の通りである。
【００４８】
（→フィルタ処理）
先ず、第２姿勢角度四元数生成部２１は、ジャイロセンサ４から受信した姿勢角速度信号をフィルタ処理して、姿勢角速度信号に含まれる望ましくない周波数帯域をカットする。
【００４９】
（→予測処理）
次に、第２姿勢角度四元数生成部２１は、RAM６から、前回の姿勢角速度バイアスbn-1を取得した上で、姿勢角速度信号αドットmeasuredから前回の姿勢角速度バイアスbn-1を引き算し、これにサンプリング周期dtを掛け、前回の姿勢角度αn-1に積算し、結果を四元数に変換する。これにより、第２姿勢角度四元数αpredictが求められる。なお、この予測処理は、始めから四元数で演算してもよいし、とりあえずオイラー角で演算して最後に四元数変換することとしてもよい。オイラー角で演算する場合は、下記式（１）による。
【００５０】
【数１０】
【００５１】
そして、第２姿勢角度四元数生成部２１は、生成した第２姿勢角度四元数αpredictを姿勢角度四元数合成部２３と姿勢角速度バイアス四元数推定部２６に出力する。
【００５２】
（フィルタ係数生成部２２）
フィルタ係数生成部２２は、状態フィードバックを実現する際のオブザーバの一種として、カルマンフィルタ係数K0、K1を生成する。カルマンフィルタ係数K0は姿勢角の計測ノイズをフィルタして姿勢角にフィードバックするための係数であり、カルマンフィルタ係数K1は姿勢角の計測ノイズをフィルタして姿勢角速度バイアスにフィードバックするための係数である。
【００５３】
（姿勢角度四元数合成部２３）
姿勢角度四元数合成部２３は、第１姿勢角度四元数αmeasuredと第２姿勢角度四元数αpredictをカルマンフィルタ係数K0でのフィルタ処理により合成して、倒立二輪車本体２の現在の姿勢角度αn（推定値）を生成する。この姿勢角度四元数合成部２３による合成処理は、前述したように球面線形補間を利用することで効率良く実行できることを示した。必要であれば、オイラー角で表現した式（２）を参照されたい。姿勢角度四元数合成部２３は、生成した倒立二輪車本体２の現在の姿勢角度αn（推定値）を姿勢角度四元数正規化部２４に出力する。
【００５４】
（姿勢角度四元数正規化部２４）
姿勢角度四元数正規化部２４は、浮動小数点演算の誤差の蓄積を回避すべく、倒立二輪車本体２の現在の姿勢角度αnを正規化する。姿勢角度四元数正規化部２４は、正規化した姿勢角度αnをオイラー角変換部２５に出力する。
【００５５】
（オイラー角変換部２５）
オイラー角変換部２５は、姿勢角度αnをオイラー角表現に変換した上で姿勢制御部９に出力する。
【００５６】
（姿勢角速度バイアス四元数推定部２６：図３）
姿勢角速度バイアス四元数推定部２６は、下記式（９）により角速度バイアス四元数bnを推定する。具体的には以下の通りである。
【００５７】
【数１１】
【００５８】
姿勢角速度バイアスbnは、前述したように下記式（３）により求められる。
【００５９】
【数１２】
【００６０】
上記式（３）は、前述した式（２）のようには球面線形補間を利用できるかたちに変形することができない。そこで、姿勢角速度バイアス四元数推定部２６は、四元数の累乗（べき乗）という概念を導入して、上記式（３）を効率的に解く。詳しく説明すると、qtは四元数qのt乗を意味するが、t=0のときq0=1、即ち恒等四元数(1,0,0,0)を示す。また、t=1のときq1=qとなり、実数と同等の振る舞いをする。注意すべきは、t<0の場合とt>1の場合であり、四元数は360度以上の回転を示すことができないので、tを大きくして複数回転を表すようなことはできない。(as)t=astのようなスカラーの累乗に関する代数が当てはまらないことがある。しかし、今回のようなフィルタ演算の場合は、ノイズをフィルタリングして小さくするという意味からK1の絶対値は必ず1より小さい値になる上に、第１姿勢角度四元数αmeasuredも第２姿勢角度四元数αpredictもある1つの角度を示すことから累乗の乗数が０〜１の範囲で使用することができる。なお、四元数の累乗は乗数が０以上１以下の範囲で使用することが一般的である。
【００６１】
さて、オイラー角表現での足し算は四元数表現では外積（演算子は×）であり、オイラー角表現での引き算は四元数表現では共役四元数（演算子は−１）との外積である。また、回転のスカラーの掛け算は、四元数表現での累乗に相当する。従って、上記式（３）は、上記式（９）に置き換えることができる。上記式（９）の意味としては、姿勢角度四元数αのノイズ成分をカルマンフィルタ係数K1でフィルタリングし、これを用いて姿勢角速度バイアス四元数bn-1を回転させて修正する、ということになる。
【００６２】
従って、図３において、差分演算部４０は、第１姿勢角度四元数生成部２０から取得した第１姿勢角度四元数αmeasuredと、第２姿勢角度四元数生成部２１から取得した第２姿勢角度四元数αpredictと、の差分を求めて姿勢角度四元数αのノイズ成分を算出する。
【００６３】
次に、フィルタ演算部４１は、差分演算部４０が算出した姿勢角度四元数αのノイズ成分をカルマンフィルタ係数K1でフィルタ処理する。このとき、カルマンフィルタ係数K1が負である場合は、フィルタ演算部４１は、カルマンフィルタ係数K1の正負を反転させた上で、姿勢角度四元数αのノイズ成分をフィルタ処理する。即ち、上記式（９）に代えて下記式（１０）を用いる。
【００６４】
【数１３】
【００６５】
また、フィルタの方式としてカルマンフィルタなどの係数が変化するタイプが存在する。例えば逐次カルマンフィルタではK0及びK1の絶対値が一時的に１より大きな値になることがある。この場合、フィルタ演算部４１は、カルマンフィルタ係数K1の絶対値を１にした上で、前記の式（９）又は（１０）により姿勢角速度バイアス四元数bnを推定すればよい。カルマンフィルタ係数K0についても同様である。
【００６６】
次に、積分演算部４２は、フィルタ処理後の値を、前回の姿勢角速度バイアス四元数bn-1に積算して、現在の姿勢角速度バイアス四元数bnを求める。積分演算部４２は、求めた姿勢角速度バイアス四元数bnを姿勢角速度バイアス四元数正規化部２７に出力する。
【００６７】
（姿勢角速度バイアス四元数正規化部２７）
姿勢角速度バイアス四元数正規化部２７は、浮動小数点演算の誤差の蓄積を回避すべく、姿勢角速度バイアス四元数bnを正規化する。そして、姿勢角速度バイアス四元数正規化部２７は、正規化した姿勢角速度バイアス四元数bnをRAM６に保存する。
【００６８】
次に、図４を参照して、主として姿勢推定部８の作動を説明する。
【００６９】
（傾斜角センサによる計測ステップ）
倒立二輪車１の電源を投入すると（S300）、傾斜角センサ３は、倒立二輪車本体２の現在の姿勢角度を計測して、姿勢角度信号を第１姿勢角度四元数生成部２０に出力する（S310）。次に、ジャイロセンサ４は、倒立二輪車本体２の現在の姿勢角速度を計測して、姿勢角速度信号を第２姿勢角度四元数生成部２１に出力する（S320）。次に、第１姿勢角度四元数生成部２０は、傾斜角センサ３から受信した姿勢角度信号に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する（S330）。次に、第２姿勢角度四元数生成部２１は、ジャイロセンサ４から受信した姿勢角速度信号に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する（S340）。次に、フィルタ係数生成部２２は、カルマンフィルタ係数K0、K1を生成する（S350）。なお、S310とS320の処理は可換であり、S330〜S350の処理は適宜順番を入れ替えても良い。次に、姿勢角速度バイアス四元数推定部２６は、下記式（９）により姿勢角速度バイアス四元数bnを推定する（S360）。そして、処理をS310に戻して次の周期に備える。
【００７０】
【数１４】
【００７１】
以上に本願発明の好適な実施形態を説明したが、上記実施形態は、要するに、以下の特長を有している。
【００７２】
姿勢推定部８（姿勢推定装置）は、傾斜角センサ３とジャイロセンサ４を備えた倒立二輪車本体２（機器本体）の姿勢を傾斜角センサ３の計測結果とジャイロセンサ４の計測結果に基づいて推定するものである。姿勢推定部８は、傾斜角センサ３の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成部２０（第１姿勢角度四元数生成手段）と、ジャイロセンサ４の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成部２１（第２姿勢角度四元数生成手段）と、カルマンフィルタ係数K1（フィルタ係数K1）を生成するフィルタ係数生成部２２（フィルタ係数生成手段）と、下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定部２６（姿勢角速度バイアス四元数推定手段）と、を備える。ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【００７３】
【数１５】
【００７４】
以上の構成によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま姿勢角速度バイアス四元数bnを推定することができる。従って、角速度バイアス四元数bnを高い計算効率で推定することができる。
【００７５】
また、姿勢角速度バイアス姿勢推定部８は、カルマンフィルタ係数K1が負である場合は、カルマンフィルタ係数K1の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。
【００７６】
また、姿勢角速度バイアス姿勢推定部８は、カルマンフィルタ係数K1の絶対値が１を超える場合は、カルマンフィルタ係数K1の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。
【００７７】
なお、角速度バイアスbnの推定を始めるに際し、角速度バイアスbnの初期値は「回転しない」ことを表す恒等四元数（1,0,0,0）とすることが考えれる。この場合、倒立二輪車１の状態（温度や各角速度センサのバイアス固有値）によりいずれある値に収束していくことになる。好ましくは、倒立二輪車１を起動し、姿勢推定のフィルタ演算を開始するときに姿勢角速度バイアスbnの初期値を恒等四元数から開始することに代えて、前回の起動後に収束した値を初期値として採用すれば、一層安定した姿勢角速度バイアスbnの推定を行うことができる。
【符号の説明】
【００７８】
１ 倒立二輪車
２ 倒立二輪車本体
３ 傾斜角センサ
４ ジャイロセンサ
８ 姿勢推定部
９ 姿勢制御部
２６ 姿勢角速度バイアス四元数推定部
【特許請求の範囲】
【請求項１】
傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定装置であって、
前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成手段と、
前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成手段と、
フィルタ係数K1を生成するフィルタ係数生成手段と、
下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定手段と、
を備えた姿勢推定装置。
ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【請求項２】
請求項１に記載の姿勢推定装置であって、
前記姿勢角速度バイアス推定手段は、前記フィルタ係数K1が負である場合は、前記フィルタ係数K1の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する、
姿勢推定装置。
【請求項３】
請求項１又は２に記載の姿勢推定装置であって、
前記姿勢角速度バイアス推定手段は、前記フィルタ係数K1の絶対値が１を超える場合は、前記フィルタ係数K1の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する、
姿勢推定装置。
【請求項４】
傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定方法であって、
前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成ステップと、
前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成ステップと、
フィルタ係数K1を生成するフィルタ係数生成ステップと、
下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定ステップと、
を備えた姿勢推定方法。
ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【請求項５】
コンピュータに、請求項４に記載の姿勢推定方法を実行させるための姿勢推定プログラム。
【請求項１】
傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定装置であって、
前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成手段と、
前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成手段と、
フィルタ係数K1を生成するフィルタ係数生成手段と、
下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定手段と、
を備えた姿勢推定装置。
ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【請求項２】
請求項１に記載の姿勢推定装置であって、
前記姿勢角速度バイアス推定手段は、前記フィルタ係数K1が負である場合は、前記フィルタ係数K1の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する、
姿勢推定装置。
【請求項３】
請求項１又は２に記載の姿勢推定装置であって、
前記姿勢角速度バイアス推定手段は、前記フィルタ係数K1の絶対値が１を超える場合は、前記フィルタ係数K1の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数bnを推定する、
姿勢推定装置。
【請求項４】
傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定方法であって、
前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成ステップと、
前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成ステップと、
フィルタ係数K1を生成するフィルタ係数生成ステップと、
下記式（９）により姿勢角速度バイアス四元数bnを推定する姿勢角速度バイアス四元数推定ステップと、
を備えた姿勢推定方法。
ただし、bn-1は、前回の姿勢角速度バイアス四元数である。
【請求項５】
コンピュータに、請求項４に記載の姿勢推定方法を実行させるための姿勢推定プログラム。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２０１３−５４００９（Ｐ２０１３−５４００９Ａ）
【公開日】平成２５年３月２１日（２０１３．３．２１）
【国際特許分類】
【出願番号】特願２０１１−１９４２４６（Ｐ２０１１−１９４２４６）
【出願日】平成２３年９月６日（２０１１．９．６）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２５年３月２１日（２０１３．３．２１）
【国際特許分類】
【出願日】平成２３年９月６日（２０１１．９．６）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]