姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラム
【課題】３次元の姿勢角を求めるための計算を効率よく行うことができ、演算時間を短縮でき、かつ演算速度を向上することができる姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラムを提供する。
【解決手段】姿勢制御装置は、状態フィードバックを用いて移動体の姿勢の推定を行う。この姿勢制御装置は、姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部１０１と、四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部１０２と、フィルタ演算により得られた姿勢に基づき移動体の姿勢制御を行う姿勢制御部１０４とを有し、フィルタ演算部１０２は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する。
【解決手段】姿勢制御装置は、状態フィードバックを用いて移動体の姿勢の推定を行う。この姿勢制御装置は、姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部１０１と、四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部１０２と、フィルタ演算により得られた姿勢に基づき移動体の姿勢制御を行う姿勢制御部１０４とを有し、フィルタ演算部１０２は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラムに関し、特に、四元数を用いて姿勢制御する姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラム関する。
【背景技術】
【０００２】
ある空間において、ある物体がどのような姿勢にあるかを表現するために、３つの手法が編み出されている。３つの手法とは、オイラー角、姿勢行列、四元数である。オイラー角は、右手系の座標軸では、図６のように表わされる。すなわち、親指、人差し指、中指をそれぞれ９０度ずつ離して指したとき、正面から水平方向に延びる方向がロール軸、左側に向く方向がピッチ軸、垂直方向上向きがヨー軸方向となる。オイラー角による表現は大変わかりやすいという長所を持つ半面、ジンバルロックが生じる等の欠点がある。
【０００３】
姿勢行列による表現では、３×３の行列を用いて姿勢（回転）を表現する。姿勢行列による表現は、わかりにくい、演算誤差の蓄積で不正な形式になってしまう等の欠点があるが、ベクトルの回転が即座に利用できるというメリットがある。
【０００４】
四元数（クオタニオン：Quaternion）は、虚数を３Ｄ（３次元：three dimensions）に拡張するものであり、１８４３年に、アイルランドのウィリアム・ハミルトンによって発案された。四元数には、１つのスカラー要素と１つの３Ｄベクトル要素が含まれ、ｑ＝（ω，ｘ，ｙ，ｚ）＝ω＋ｘｉ＋ｙｊ＋ｚｋで表現される。
ｉ２＝ｊ２＝ｋ２＝−１
ｉｊ＝ｋ，ｊｉ＝−ｋ，ｋｊ＝−ｉ，ｉｋ＝−ｊ
【０００５】
四元数は、回転を表わすので、あるベクトルｎの周りの回転θを四元数で表わすと、下記の式となる。
【０００６】
【数１】
【０００７】
四元数は、わかりにくいという欠点があるものの、特異点がない、すなわち、ジンバルロックしない、高速演算ができる等の長所がある。
【０００８】
移動ロボットの姿勢を計測し、これを表現するのに、オイラー角又はカルダン角と呼ばれる表現手法を用いると、ロール、ピッチ、ヨーという人間が感覚的に分かりやすい表現となるが、オイラー角表現は、上述のように数学的な特異点を有するため、ジンバルロックという問題が発生する。そこで、ジンバルロックを回避できる姿勢の表現手法として、上記の姿勢行列を用いる方法、四元数を用いる方法などが従来提案されている。
【０００９】
例えば、四元数を使用した姿勢制御として特許文献１が公知である。特許文献１における姿勢推定では、ジャイロ及びチルトセンサから出力された信号に基づいて傾動可能な物体の姿勢を追跡して制御する。ジャイロから出力された信号を変換し統合し、それにより、推定位置情報をヨー成分が値０に拘束される変形四元数の形態で得る。また、同一形態の変形四元数情報をチルトセンサから出力された信号から生成し、これを利用して推定位置情報中の誤差成分を検出してこれを補正する。
【先行技術文献】
【特許文献】
【００１０】
【特許文献１】特表２００３−５２１６９７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１１】
ところで、移動ロボットの姿勢を計測するときに、十分な精度を有する高価なセンサ、例えば光ファイバジャイロを使用することができない場合がある。コストやサイズが大きな理由となる。この問題を解決するため、安価で小型のＭＥＭＳセンサを複数種類使用し、これをディジタルフィルタで処理して合成を行い、単独のＭＥＭＳセンサでは得られない高精度での姿勢計測を実現する手法が従来提案されている。
【００１２】
しかし、これまでのフィルタ手法は、オイラー角による姿勢角表現をもとに考えられたもので、原理的にジンバルロックが発生してしまう、回避のために、ジンバルロックが発生する特異点付近では、一旦姿勢計測をやめ、特異点から遠ざかった姿勢になってから姿勢計測を再開するなどの手法をとるしかなく、計測できない姿勢が存在するという問題点もある。
【００１３】
すなわち、上述したように、移動ロボットの姿勢を表現する手法としてオイラー角による表現を行うと、必ず、ジンバルロックという問題が付随する、ジンバルロックは数学的には、特異点と呼ばれる非連続なポイントが存在することが原因である。また、安価で小型のＭＥＭＳセンサを複数種類使用する場合に、姿勢角度を直接計測できる傾斜センサ（通常は、３軸加速度センサを用いられることが多い）と、姿勢角度の変化である姿勢角速度を計測するジャイロセンサをフィルタ処理によって合成を行うが、特異点を有する非連続な姿勢角度と、特異点を持たない連続な姿勢角速度を合成することは、当然ながら無理があり、合成の際に複雑な条件分岐を行う必要があるという問題点がある。
【００１４】
本発明は、このような問題点を解決するためになされたものであり、３次元の姿勢角を求めるための計算を効率よく行うことができ、演算時間を短縮でき、かつ演算速度を向上することができる姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１５】
本発明に係る姿勢推定装置は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定装置であって、姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部と、前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部とを有し、前記フィルタ演算部は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するものである。
【００１６】
本発明においては、姿勢推定を行うためのフィルタ演算にｓｌｅｒｐ（球面線形補間：spherical linear interpolation）を利用するため、ジンバルロックが生じることなく、効率よく演算を行うことができる。
【００１７】
また、前記フィルタ演算部は、前記四元数に変換された姿勢及び姿勢変化率の合成を行うことができる。更に、前記フィルタ演算部は、前記四元数に変換された姿勢及び姿勢変化率の積分値の合成を行うことができる。これにより、姿勢及び姿勢変化率の合成にｓｌｅｒｐを使用することができ、演算時間を短縮することができる。
【００１８】
更にまた、前記フィルタ演算後の四元数で表現された姿勢をオイラー変換するオイラー変換部を更に有することができ、姿勢制御の際には、四元数を一旦オイラー角に変換することができる。
【００１９】
また、前記フィルタ演算前の姿勢角は傾斜センサの値であって、前記姿勢変化率は、ロール角速度、ピッチ角速度、及びヨー角速度とすることができる。
【００２０】
更に、前記入力部には、前記姿勢及び姿勢変化率と、方角の情報とが入力するものとすることができる。なお、方角情報を使用せずに演算することも可能である。
【００２１】
更にまた、前記方角の情報は、方位角センサの値、又は移動体がタイヤを有する場合はそのタイヤの向きを示す情報とすることができる。方位角センセを使用せず、移動体のタイヤの方向を使用することも可能である。
【００２２】
また、フィルタとしては、カルマンフィルタ又は周波数フィルタを使用することができる。
【００２３】
本発明にかかる姿勢制御装置は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢制御装置であって、姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部と、前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部と、前記フィルタ演算により得られた姿勢に基づき前記移動体の姿勢制御を行う姿勢制御部とを有し、前記フィルタ演算部は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するものである。
【００２４】
本発明に係る姿勢推定方法は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定方法であって、姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程とを有し、前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するものである。
【００２５】
本発明にかかる姿勢制御方法は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定方法であって、姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程と、前記フィルタ演算により得られた姿勢に基づき前記移動体の姿勢制御を行う姿勢制御工程とを有し、前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するものである。
【００２６】
本発明においては、四元数を用いて姿勢推定を行うため、フィルタ演算に、球面線形補間を利用することができる。四元数を用いるため、オイラー角表現を用いた場合に生じるジンバルロックの問題を回避でき、また姿勢行列を用いる場合のように、演算誤差の蓄積で不正な形式となってしまうような問題が生じにくい。
【００２７】
また、本発明に係るプログラムは、上述した姿勢制御をコンピュータに実行させるものである。
【発明の効果】
【００２８】
本発明によれば、３次元の姿勢角を求めるための計算を効率よく行うことができ、演算時間を短縮でき、かつ演算速度を向上することができる姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラムを提供することができる。
【図面の簡単な説明】
【００２９】
【図１】本発明の実施の形態に係る倒立二輪車の構成を示す図である。
【図２】本発明の実施の形態に係る制御装置を説明するための図である。
【図３】本発明の実施の形態に係る姿勢制御装置を示す模式図である。
【図４】本発明の実施の形態に係る姿勢制御装置の詳細を示すブロック図、
【図５】本発明の実施の形態における姿勢角度と姿勢角速度の合成を行う際のフィルタリング手法を示すフローチャートである。
【図６】オイラー角表現の右手座標軸を示す模式図である。
【発明を実施するための形態】
【００３０】
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、四元数を用いて移動ロボットの姿勢の推定を行い、推定した姿勢を使用して移動ロボットの姿勢を安定化制御する代表例として同軸二輪車の倒立制御に適用したものである。なお、本実施の形態においては、同軸二輪車として説明するが、その他脚型又は車輪型ロボット等に本発明を適用することができることはいうまでもない。
【００３１】
図１は、本発明に係る実施の形態として、本発明に係る倒立二輪車の一実施形態の構成を示す図である。尚、図１（ａ）は正面図を示し、図１（ｂ）は側面図を示す。図１において、本実施の形態に係る走行装置は、乗員が立つ部分である本体１に対して、同軸芯線上に平行に車輪３Ａ、３Ｂを有する同軸二輪車である。
【００３２】
尚、以下の説明で用いる同軸二輪車の車両の全体に対する各座標系を、図中に記載のように、車軸に対して垂直方向をＸ軸、車軸方向をＹ軸、鉛直方向をＺ軸、車軸周り（Ｙ軸周り）をピッチ軸、車両上面視においてＸ−Ｙ平面上の回転方向をヨー軸とする。
【００３３】
本実施の形態に係る走行装置は、本体１と、本体１に同軸上に取り付けられた１対の駆動ユニット２Ａ及び２Ｂと、駆動ユニット２Ａ及び２Ｂによりそれぞれ回転駆動される車輪３Ａ及び３Ｂと、乗員がつかまるＴ字型のハンドル４と、本体１の前後（Ｙ軸周り）の傾きを検出する姿勢検出装置４と、旋回操作を指示するための旋回操作装置６と、を備えている。また、本体１には、図示しないが、後述する車両の制御を行う制御装置が設けられている。尚、本体１には乗員の乗車を識別するセンサ、或いはスイッチ（図示せず）を内蔵していてもよい。
【００３４】
この倒立二輪車は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定装置を搭載する。姿勢制御装置は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する。これについての詳細は後述する。
【００３５】
次に、本実施の形態にかかる倒立二輪車の制御装置について説明する。図２は、本実施の形態にかかる制御装置を説明するための図である。図２に示すように、制御装置１０は、例えば、マイクロコンピュータ（ＣＰＵ（Central Processing Unit））を有する演算回路１１と、プログラムメモリやデータメモリその他のＲＡＭ（Random Access Memory）やＲＯＭ（Read Only Memory等を有する記憶装置１２等を備えている。制御装置１０には、バッテリー２６と一対の駆動回路２８Ｌ，２８Ｒが接続されていて、それらは非常停止スイッチ２７を介しても接続されている。一対の駆動回路２８Ｌ，２８Ｒは、一対の車輪の回転速度や回転方向等を個別に制御するもので、これらに一対の車輪駆動ユニット２９Ｌ，２９Ｒが個別に接続されている。
【００３６】
この制御装置１０には、１５の傾斜角度を検出する角度検出センサ２３からの検出信号と、姿勢センサユニット２２からの検出信号と、降車スイッチ２４からの降車補助開始トリガ信号、ステップセンサ２５から足検出信号が供給される。制御装置１０は、通常の姿勢制御に加えて、降車補助開始トリガ信号や足検出信号に基づいて降車補助制御を実行する。
【００３７】
姿勢センサユニット２１は、同軸二輪車の走行時における角速度や加速度を検出してその角速度や走行加速度を制御するために用いられるもので、例えば、ジャイロセンサと、加速度センサとから構成される。ハンドルを前方または後方に傾けると、ステップ部が同方向に傾くことになるが、この姿勢センサユニット２２は、かかる傾斜に対応した角速度や加速度を検出する。そして、制御装置１０は、姿勢センサユニット２２によって検出された角速度や加速度に基づいて、ハンドル２３の傾斜方向に車両が移動するように、車輪駆動ユニット２９Ｌ，２９Ｒを駆動制御する。
【００３８】
次に、制御装置１０に搭載される姿勢制御装置について説明する。図３は、姿勢制御装置を示す模式図である。姿勢推定装置は、状態フィードバックを用いて移動体の姿勢の推定を行う。この姿勢推定装置は、姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部１０１と、四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部１０２と、フィルタ演算後の四元数で表現された姿勢をオイラー変換するオイラー変換部１０３と、フィルタ演算により得られた姿勢に基づき移動体の姿勢制御を行う姿勢制御部１０４とを有する。
【００３９】
フィルタ演算部１０２は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する。フィルタ演算部１０２は、四元数に変換された姿勢及び姿勢変化率の積分値の合成を行う。ここで、姿勢は、角速度センサの値であり、姿勢変化率は、ロール角速度、ピッチ角速度、ヨー角速度等である。また、入力部１０１には、姿勢及び姿勢変化率の他、方角の情報も入力される。
【００４０】
次に、姿勢制御装置の具体例について説明する。図４は、姿勢制御装置の詳細を示すブロック図、図５は、オイラー各表現による１つの軸についての姿勢角度と姿勢角速度の合成を行う際のフィルタリング手法を示すフローチャートである。
【００４１】
図４に示すように、姿勢制御装置は、角速度センサ２２ａ、加速度センサ２２ｂ、方位角センサ２２ｃ、バイアス修正部３１、姿勢角算出部３２、積分演算部３３、フィルタ３４、３５、バイアス推定値算出部３６及び姿勢角推定値算出部３７を有する。
【００４２】
角速度センサ２２ａ、加速度センサ２２ｂ及び方位角センサ２２ｃは、姿勢検出センサユニット２２に含まれる。角速度センサ２２ａは、ロール、ピッチ、ヨーのそれぞれの角速度を検出して出力する（ステップＳ１０４）。加速度センサ２２ｂは、ｘ、ｙ、ｚ方向の加速度を検出して出力する（ステップＳ１０１）。方位角センサ２２ｃは、方角を検出して出力する。なお、方位角の代わりに、タイヤの向きなどを利用することも可能である。また、特許文献１に記載されているように、方位角を使用しない方法も適用可能である。
【００４３】
バイアス修正部３１は、角度センサ２２ａからの角速度が入力され、ジャイロセンサのずれ量（バイアス）をバイアス推定値に基づき修正する（ステップＳ１０５、式（２））。具体的には、角速度計測値から前回のバイアス推定値を引く。前回のバイアス推定値は、前回の周期のバイアス推定値演算部３６の値である。これに、サンプリング時間をかけて微小な変化率を求める（ステップＳ１０６）。そして、ジャイロセンサで計測した角速度を、３軸微小速度を微小回転四元数に変換することで、四元数に変換する（ステップＳ１０７）。次に、ジャイロの初期取り付け角度の補正（回転）を行う。これにより、ジャイロセンサによって観測した角速度をボディ基準座標系の各変位に変換することができる。
【００４４】
姿勢角算出部３２は、加速度センサ２２ｂ及び方位角センサ２２ｃからの加速度及び方位角に基づき、姿勢角を算出する。先ず、加速度センサの値から自身の加速度ベクトルをキャンセルする。次に、加速度センサベクトルに方位角を加えた回転を姿勢行列に変換する（ステップＳ１０２）。そして、この姿勢行列を四元数に変換する（ステップＳ１０３）。
【００４５】
積分演算部３３では、ジャイロ計測値を積分することで、バイアス値を予測する（ステップＳ１０８、式（２））。すなわち、前回の周期の姿勢を表わす四元数に今回の周期で観測した角変位を積分することで、今回の周期の姿勢を表わす四元数を予測する。今回の周期の予測値αｐｒｅｄｉｃｔとすると、下記により求まる。
【００４６】
【数２】
【００４７】
フィルタ３４、３５は、四元数を使用してカルマンフィルタ演算を実施する（ステップＳ１０９）。オイラー角による３次元姿勢角度表現を用いる場合は、上記の演算を３つ直交する軸（代表的なのは、ロール、ピッチ、ヨー軸）についいて繰り返すことになる。このとき、角速度は、バイアスを取り除いた後にセンサが取り付けられているボディ座標系と重力を基準として水平基準座標系の間で回転変換を行い、水平基準座標系の角度で積分を行う必要がある。
【００４８】
これに対し、実施の形態においては、この演算を、オイラー角を使用して３つの軸に対して３回のフィルタ演算を行うのではなく、四元数を使用して一度に行う。オイラー角で１つの軸に対して足し算を行う、ということは、その軸に関する回転を行うことであり、３つの軸に対して同時に足し算を行うということは、四元数に対して回転の演算を行うことになる。
この時、角度の推定を行う下記の式に注目する。
【００４９】
【数３】
この式を変形すると、
【００５０】
【数４】
αｎは、αｐｒｅｄｉｃｔとαｍｅａｓｕｒｅｄという２つの角度を、（１−Ｋ０）：Ｋの比率で補間する角度という意味となる。別の表現では、αｐｒｅｄｉｃｔを始点として、αｍｅａｓｕｒｅｄとαｐｒｅｄｉｃｔの差分のＫ０倍を加えるということになる。αをオイラー角により１つの軸の角度ではなく、四元数と考えると、この演算は、ｓｌｅｒｐ（球面線形補間：spherical linear interpolation）と呼ばれる演算手法と同一となる。すなわち、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用することができる。
【００５１】
ｑ０とｑ１の２つの四元数の間を変数ｔで補間する関数ｓｌｅｒｐ（ｑ０，ｑ１，ｔ）は、理論上は下記の式によって定義される。
【００５２】
【数５】
計算機で実際に演算を行う際には、ｑ０とｑ１との間の弧の長さであるωを使用して、更に効率のよい下記のテクニックを利用することができる。
【００５３】
【数６】
２つの四元数間の角度のサインとコサインは、下記の式で簡単に求めることができる。
ｑ０＝（ｗ０，ｘ０，ｙ０，ｚ０）＝（ｗ１，ｘ１，ｙ１，ｚ１）とした時、
ｃｏｓω＝ｗ０・ｗ１＋ｘ０・ｘ１＋ｙ０・ｙ１＋ｚ０・ｚ１
ｓｉｎω＝ｓｑｒｔ（１−ｃｏｓ２ω）
したがって、ｓｌｅｒｐの答えをｑ＝（ｗ，ｘ，ｙ，ｚ）とすると下記のような演算を行うことができる。
ω＝αｔａｎ２（ｓｉｎω，ｃｏｓω）
ｋ０＝ｓｉｎ（（１．０−ｔ）ω）／ｓｉｎω
ｋ１＝ｓｉｎ（ｔω）／ｓｉｎω
ｗ＝ｗ０・ｋ０＋ｗ１・ｋ１
ｘ＝ｘ０・ｋ０＋ｘ１・ｋ１
ｙ＝ｙ０・ｋ０＋ｙ１・ｋ１
ｚ＝ｚ０・ｋ０＋ｚ１＋ｋ１
なお、最後に、浮動小数点演算の誤差が蓄積しないように、正規化を行う（ステップＳ１１１）。
【００５４】
バイアス推定値３６は、バイアスにフィードバックする（ステップＳ１１２）ために、四元数を一旦オイラー角に戻す。なお、バイアスをオイラー角でなく四元数で保持するようにすれば、ｓｌｅｒｐを使用することができ、演算効率を向上することができる。
【００５５】
姿勢角推定値算出部３７は、次回周期の演算と姿勢制御演算のために値を保存しておく。一定時間が経過すると、加速度センサとジャイロセンサの想定から次の周期が開始する（ステップＳ１１３）。
【００５６】
以上のようにして、ノイズの大きな角度センサ（加速度センサ）と、ドリフトの大きな角速度センサ（ジャイロ）の信号を適切なフィルタにより、合成し、ノイズの少ない姿勢角とバイアス（ドリフト）の推定値を得る。適用可能なフィルタの種類としては、周波数フィルタ（ＬＰＦ（low pass filter）にＨＰＦ（high pass filter）を組み合わせたもの）や、カルマンフィルタを利用することができる。本実施の形態においては、四元数をカルマンフィルタにてフィルタ処理する際に、球面線形補間（slerp）を使用することで、フィルタ処理の精度とフィルタ演算の効率を向上させることができる。
【００５７】
なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
【００５８】
例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００５９】
２２ａ 角速度センサ
２２ｂ 加速度センサ
２２ｃ 方位角センサ
３１ バイアス修正部
３２ 姿勢角算出部
３３ 積分演算部
３４ フィルタ
３５ フィルタ
３６ バイアス推定値算出部
３７ 姿勢角推定値算出部
【技術分野】
【０００１】
本発明は、姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラムに関し、特に、四元数を用いて姿勢制御する姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラム関する。
【背景技術】
【０００２】
ある空間において、ある物体がどのような姿勢にあるかを表現するために、３つの手法が編み出されている。３つの手法とは、オイラー角、姿勢行列、四元数である。オイラー角は、右手系の座標軸では、図６のように表わされる。すなわち、親指、人差し指、中指をそれぞれ９０度ずつ離して指したとき、正面から水平方向に延びる方向がロール軸、左側に向く方向がピッチ軸、垂直方向上向きがヨー軸方向となる。オイラー角による表現は大変わかりやすいという長所を持つ半面、ジンバルロックが生じる等の欠点がある。
【０００３】
姿勢行列による表現では、３×３の行列を用いて姿勢（回転）を表現する。姿勢行列による表現は、わかりにくい、演算誤差の蓄積で不正な形式になってしまう等の欠点があるが、ベクトルの回転が即座に利用できるというメリットがある。
【０００４】
四元数（クオタニオン：Quaternion）は、虚数を３Ｄ（３次元：three dimensions）に拡張するものであり、１８４３年に、アイルランドのウィリアム・ハミルトンによって発案された。四元数には、１つのスカラー要素と１つの３Ｄベクトル要素が含まれ、ｑ＝（ω，ｘ，ｙ，ｚ）＝ω＋ｘｉ＋ｙｊ＋ｚｋで表現される。
ｉ２＝ｊ２＝ｋ２＝−１
ｉｊ＝ｋ，ｊｉ＝−ｋ，ｋｊ＝−ｉ，ｉｋ＝−ｊ
【０００５】
四元数は、回転を表わすので、あるベクトルｎの周りの回転θを四元数で表わすと、下記の式となる。
【０００６】
【数１】
【０００７】
四元数は、わかりにくいという欠点があるものの、特異点がない、すなわち、ジンバルロックしない、高速演算ができる等の長所がある。
【０００８】
移動ロボットの姿勢を計測し、これを表現するのに、オイラー角又はカルダン角と呼ばれる表現手法を用いると、ロール、ピッチ、ヨーという人間が感覚的に分かりやすい表現となるが、オイラー角表現は、上述のように数学的な特異点を有するため、ジンバルロックという問題が発生する。そこで、ジンバルロックを回避できる姿勢の表現手法として、上記の姿勢行列を用いる方法、四元数を用いる方法などが従来提案されている。
【０００９】
例えば、四元数を使用した姿勢制御として特許文献１が公知である。特許文献１における姿勢推定では、ジャイロ及びチルトセンサから出力された信号に基づいて傾動可能な物体の姿勢を追跡して制御する。ジャイロから出力された信号を変換し統合し、それにより、推定位置情報をヨー成分が値０に拘束される変形四元数の形態で得る。また、同一形態の変形四元数情報をチルトセンサから出力された信号から生成し、これを利用して推定位置情報中の誤差成分を検出してこれを補正する。
【先行技術文献】
【特許文献】
【００１０】
【特許文献１】特表２００３−５２１６９７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１１】
ところで、移動ロボットの姿勢を計測するときに、十分な精度を有する高価なセンサ、例えば光ファイバジャイロを使用することができない場合がある。コストやサイズが大きな理由となる。この問題を解決するため、安価で小型のＭＥＭＳセンサを複数種類使用し、これをディジタルフィルタで処理して合成を行い、単独のＭＥＭＳセンサでは得られない高精度での姿勢計測を実現する手法が従来提案されている。
【００１２】
しかし、これまでのフィルタ手法は、オイラー角による姿勢角表現をもとに考えられたもので、原理的にジンバルロックが発生してしまう、回避のために、ジンバルロックが発生する特異点付近では、一旦姿勢計測をやめ、特異点から遠ざかった姿勢になってから姿勢計測を再開するなどの手法をとるしかなく、計測できない姿勢が存在するという問題点もある。
【００１３】
すなわち、上述したように、移動ロボットの姿勢を表現する手法としてオイラー角による表現を行うと、必ず、ジンバルロックという問題が付随する、ジンバルロックは数学的には、特異点と呼ばれる非連続なポイントが存在することが原因である。また、安価で小型のＭＥＭＳセンサを複数種類使用する場合に、姿勢角度を直接計測できる傾斜センサ（通常は、３軸加速度センサを用いられることが多い）と、姿勢角度の変化である姿勢角速度を計測するジャイロセンサをフィルタ処理によって合成を行うが、特異点を有する非連続な姿勢角度と、特異点を持たない連続な姿勢角速度を合成することは、当然ながら無理があり、合成の際に複雑な条件分岐を行う必要があるという問題点がある。
【００１４】
本発明は、このような問題点を解決するためになされたものであり、３次元の姿勢角を求めるための計算を効率よく行うことができ、演算時間を短縮でき、かつ演算速度を向上することができる姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１５】
本発明に係る姿勢推定装置は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定装置であって、姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部と、前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部とを有し、前記フィルタ演算部は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するものである。
【００１６】
本発明においては、姿勢推定を行うためのフィルタ演算にｓｌｅｒｐ（球面線形補間：spherical linear interpolation）を利用するため、ジンバルロックが生じることなく、効率よく演算を行うことができる。
【００１７】
また、前記フィルタ演算部は、前記四元数に変換された姿勢及び姿勢変化率の合成を行うことができる。更に、前記フィルタ演算部は、前記四元数に変換された姿勢及び姿勢変化率の積分値の合成を行うことができる。これにより、姿勢及び姿勢変化率の合成にｓｌｅｒｐを使用することができ、演算時間を短縮することができる。
【００１８】
更にまた、前記フィルタ演算後の四元数で表現された姿勢をオイラー変換するオイラー変換部を更に有することができ、姿勢制御の際には、四元数を一旦オイラー角に変換することができる。
【００１９】
また、前記フィルタ演算前の姿勢角は傾斜センサの値であって、前記姿勢変化率は、ロール角速度、ピッチ角速度、及びヨー角速度とすることができる。
【００２０】
更に、前記入力部には、前記姿勢及び姿勢変化率と、方角の情報とが入力するものとすることができる。なお、方角情報を使用せずに演算することも可能である。
【００２１】
更にまた、前記方角の情報は、方位角センサの値、又は移動体がタイヤを有する場合はそのタイヤの向きを示す情報とすることができる。方位角センセを使用せず、移動体のタイヤの方向を使用することも可能である。
【００２２】
また、フィルタとしては、カルマンフィルタ又は周波数フィルタを使用することができる。
【００２３】
本発明にかかる姿勢制御装置は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢制御装置であって、姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部と、前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部と、前記フィルタ演算により得られた姿勢に基づき前記移動体の姿勢制御を行う姿勢制御部とを有し、前記フィルタ演算部は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するものである。
【００２４】
本発明に係る姿勢推定方法は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定方法であって、姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程とを有し、前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するものである。
【００２５】
本発明にかかる姿勢制御方法は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定方法であって、姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程と、前記フィルタ演算により得られた姿勢に基づき前記移動体の姿勢制御を行う姿勢制御工程とを有し、前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するものである。
【００２６】
本発明においては、四元数を用いて姿勢推定を行うため、フィルタ演算に、球面線形補間を利用することができる。四元数を用いるため、オイラー角表現を用いた場合に生じるジンバルロックの問題を回避でき、また姿勢行列を用いる場合のように、演算誤差の蓄積で不正な形式となってしまうような問題が生じにくい。
【００２７】
また、本発明に係るプログラムは、上述した姿勢制御をコンピュータに実行させるものである。
【発明の効果】
【００２８】
本発明によれば、３次元の姿勢角を求めるための計算を効率よく行うことができ、演算時間を短縮でき、かつ演算速度を向上することができる姿勢推定装置及び方法、姿勢制御装置及び方法、並びにプログラムを提供することができる。
【図面の簡単な説明】
【００２９】
【図１】本発明の実施の形態に係る倒立二輪車の構成を示す図である。
【図２】本発明の実施の形態に係る制御装置を説明するための図である。
【図３】本発明の実施の形態に係る姿勢制御装置を示す模式図である。
【図４】本発明の実施の形態に係る姿勢制御装置の詳細を示すブロック図、
【図５】本発明の実施の形態における姿勢角度と姿勢角速度の合成を行う際のフィルタリング手法を示すフローチャートである。
【図６】オイラー角表現の右手座標軸を示す模式図である。
【発明を実施するための形態】
【００３０】
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、四元数を用いて移動ロボットの姿勢の推定を行い、推定した姿勢を使用して移動ロボットの姿勢を安定化制御する代表例として同軸二輪車の倒立制御に適用したものである。なお、本実施の形態においては、同軸二輪車として説明するが、その他脚型又は車輪型ロボット等に本発明を適用することができることはいうまでもない。
【００３１】
図１は、本発明に係る実施の形態として、本発明に係る倒立二輪車の一実施形態の構成を示す図である。尚、図１（ａ）は正面図を示し、図１（ｂ）は側面図を示す。図１において、本実施の形態に係る走行装置は、乗員が立つ部分である本体１に対して、同軸芯線上に平行に車輪３Ａ、３Ｂを有する同軸二輪車である。
【００３２】
尚、以下の説明で用いる同軸二輪車の車両の全体に対する各座標系を、図中に記載のように、車軸に対して垂直方向をＸ軸、車軸方向をＹ軸、鉛直方向をＺ軸、車軸周り（Ｙ軸周り）をピッチ軸、車両上面視においてＸ−Ｙ平面上の回転方向をヨー軸とする。
【００３３】
本実施の形態に係る走行装置は、本体１と、本体１に同軸上に取り付けられた１対の駆動ユニット２Ａ及び２Ｂと、駆動ユニット２Ａ及び２Ｂによりそれぞれ回転駆動される車輪３Ａ及び３Ｂと、乗員がつかまるＴ字型のハンドル４と、本体１の前後（Ｙ軸周り）の傾きを検出する姿勢検出装置４と、旋回操作を指示するための旋回操作装置６と、を備えている。また、本体１には、図示しないが、後述する車両の制御を行う制御装置が設けられている。尚、本体１には乗員の乗車を識別するセンサ、或いはスイッチ（図示せず）を内蔵していてもよい。
【００３４】
この倒立二輪車は、状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定装置を搭載する。姿勢制御装置は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する。これについての詳細は後述する。
【００３５】
次に、本実施の形態にかかる倒立二輪車の制御装置について説明する。図２は、本実施の形態にかかる制御装置を説明するための図である。図２に示すように、制御装置１０は、例えば、マイクロコンピュータ（ＣＰＵ（Central Processing Unit））を有する演算回路１１と、プログラムメモリやデータメモリその他のＲＡＭ（Random Access Memory）やＲＯＭ（Read Only Memory等を有する記憶装置１２等を備えている。制御装置１０には、バッテリー２６と一対の駆動回路２８Ｌ，２８Ｒが接続されていて、それらは非常停止スイッチ２７を介しても接続されている。一対の駆動回路２８Ｌ，２８Ｒは、一対の車輪の回転速度や回転方向等を個別に制御するもので、これらに一対の車輪駆動ユニット２９Ｌ，２９Ｒが個別に接続されている。
【００３６】
この制御装置１０には、１５の傾斜角度を検出する角度検出センサ２３からの検出信号と、姿勢センサユニット２２からの検出信号と、降車スイッチ２４からの降車補助開始トリガ信号、ステップセンサ２５から足検出信号が供給される。制御装置１０は、通常の姿勢制御に加えて、降車補助開始トリガ信号や足検出信号に基づいて降車補助制御を実行する。
【００３７】
姿勢センサユニット２１は、同軸二輪車の走行時における角速度や加速度を検出してその角速度や走行加速度を制御するために用いられるもので、例えば、ジャイロセンサと、加速度センサとから構成される。ハンドルを前方または後方に傾けると、ステップ部が同方向に傾くことになるが、この姿勢センサユニット２２は、かかる傾斜に対応した角速度や加速度を検出する。そして、制御装置１０は、姿勢センサユニット２２によって検出された角速度や加速度に基づいて、ハンドル２３の傾斜方向に車両が移動するように、車輪駆動ユニット２９Ｌ，２９Ｒを駆動制御する。
【００３８】
次に、制御装置１０に搭載される姿勢制御装置について説明する。図３は、姿勢制御装置を示す模式図である。姿勢推定装置は、状態フィードバックを用いて移動体の姿勢の推定を行う。この姿勢推定装置は、姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部１０１と、四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部１０２と、フィルタ演算後の四元数で表現された姿勢をオイラー変換するオイラー変換部１０３と、フィルタ演算により得られた姿勢に基づき移動体の姿勢制御を行う姿勢制御部１０４とを有する。
【００３９】
フィルタ演算部１０２は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する。フィルタ演算部１０２は、四元数に変換された姿勢及び姿勢変化率の積分値の合成を行う。ここで、姿勢は、角速度センサの値であり、姿勢変化率は、ロール角速度、ピッチ角速度、ヨー角速度等である。また、入力部１０１には、姿勢及び姿勢変化率の他、方角の情報も入力される。
【００４０】
次に、姿勢制御装置の具体例について説明する。図４は、姿勢制御装置の詳細を示すブロック図、図５は、オイラー各表現による１つの軸についての姿勢角度と姿勢角速度の合成を行う際のフィルタリング手法を示すフローチャートである。
【００４１】
図４に示すように、姿勢制御装置は、角速度センサ２２ａ、加速度センサ２２ｂ、方位角センサ２２ｃ、バイアス修正部３１、姿勢角算出部３２、積分演算部３３、フィルタ３４、３５、バイアス推定値算出部３６及び姿勢角推定値算出部３７を有する。
【００４２】
角速度センサ２２ａ、加速度センサ２２ｂ及び方位角センサ２２ｃは、姿勢検出センサユニット２２に含まれる。角速度センサ２２ａは、ロール、ピッチ、ヨーのそれぞれの角速度を検出して出力する（ステップＳ１０４）。加速度センサ２２ｂは、ｘ、ｙ、ｚ方向の加速度を検出して出力する（ステップＳ１０１）。方位角センサ２２ｃは、方角を検出して出力する。なお、方位角の代わりに、タイヤの向きなどを利用することも可能である。また、特許文献１に記載されているように、方位角を使用しない方法も適用可能である。
【００４３】
バイアス修正部３１は、角度センサ２２ａからの角速度が入力され、ジャイロセンサのずれ量（バイアス）をバイアス推定値に基づき修正する（ステップＳ１０５、式（２））。具体的には、角速度計測値から前回のバイアス推定値を引く。前回のバイアス推定値は、前回の周期のバイアス推定値演算部３６の値である。これに、サンプリング時間をかけて微小な変化率を求める（ステップＳ１０６）。そして、ジャイロセンサで計測した角速度を、３軸微小速度を微小回転四元数に変換することで、四元数に変換する（ステップＳ１０７）。次に、ジャイロの初期取り付け角度の補正（回転）を行う。これにより、ジャイロセンサによって観測した角速度をボディ基準座標系の各変位に変換することができる。
【００４４】
姿勢角算出部３２は、加速度センサ２２ｂ及び方位角センサ２２ｃからの加速度及び方位角に基づき、姿勢角を算出する。先ず、加速度センサの値から自身の加速度ベクトルをキャンセルする。次に、加速度センサベクトルに方位角を加えた回転を姿勢行列に変換する（ステップＳ１０２）。そして、この姿勢行列を四元数に変換する（ステップＳ１０３）。
【００４５】
積分演算部３３では、ジャイロ計測値を積分することで、バイアス値を予測する（ステップＳ１０８、式（２））。すなわち、前回の周期の姿勢を表わす四元数に今回の周期で観測した角変位を積分することで、今回の周期の姿勢を表わす四元数を予測する。今回の周期の予測値αｐｒｅｄｉｃｔとすると、下記により求まる。
【００４６】
【数２】
【００４７】
フィルタ３４、３５は、四元数を使用してカルマンフィルタ演算を実施する（ステップＳ１０９）。オイラー角による３次元姿勢角度表現を用いる場合は、上記の演算を３つ直交する軸（代表的なのは、ロール、ピッチ、ヨー軸）についいて繰り返すことになる。このとき、角速度は、バイアスを取り除いた後にセンサが取り付けられているボディ座標系と重力を基準として水平基準座標系の間で回転変換を行い、水平基準座標系の角度で積分を行う必要がある。
【００４８】
これに対し、実施の形態においては、この演算を、オイラー角を使用して３つの軸に対して３回のフィルタ演算を行うのではなく、四元数を使用して一度に行う。オイラー角で１つの軸に対して足し算を行う、ということは、その軸に関する回転を行うことであり、３つの軸に対して同時に足し算を行うということは、四元数に対して回転の演算を行うことになる。
この時、角度の推定を行う下記の式に注目する。
【００４９】
【数３】
この式を変形すると、
【００５０】
【数４】
αｎは、αｐｒｅｄｉｃｔとαｍｅａｓｕｒｅｄという２つの角度を、（１−Ｋ０）：Ｋの比率で補間する角度という意味となる。別の表現では、αｐｒｅｄｉｃｔを始点として、αｍｅａｓｕｒｅｄとαｐｒｅｄｉｃｔの差分のＫ０倍を加えるということになる。αをオイラー角により１つの軸の角度ではなく、四元数と考えると、この演算は、ｓｌｅｒｐ（球面線形補間：spherical linear interpolation）と呼ばれる演算手法と同一となる。すなわち、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用することができる。
【００５１】
ｑ０とｑ１の２つの四元数の間を変数ｔで補間する関数ｓｌｅｒｐ（ｑ０，ｑ１，ｔ）は、理論上は下記の式によって定義される。
【００５２】
【数５】
計算機で実際に演算を行う際には、ｑ０とｑ１との間の弧の長さであるωを使用して、更に効率のよい下記のテクニックを利用することができる。
【００５３】
【数６】
２つの四元数間の角度のサインとコサインは、下記の式で簡単に求めることができる。
ｑ０＝（ｗ０，ｘ０，ｙ０，ｚ０）＝（ｗ１，ｘ１，ｙ１，ｚ１）とした時、
ｃｏｓω＝ｗ０・ｗ１＋ｘ０・ｘ１＋ｙ０・ｙ１＋ｚ０・ｚ１
ｓｉｎω＝ｓｑｒｔ（１−ｃｏｓ２ω）
したがって、ｓｌｅｒｐの答えをｑ＝（ｗ，ｘ，ｙ，ｚ）とすると下記のような演算を行うことができる。
ω＝αｔａｎ２（ｓｉｎω，ｃｏｓω）
ｋ０＝ｓｉｎ（（１．０−ｔ）ω）／ｓｉｎω
ｋ１＝ｓｉｎ（ｔω）／ｓｉｎω
ｗ＝ｗ０・ｋ０＋ｗ１・ｋ１
ｘ＝ｘ０・ｋ０＋ｘ１・ｋ１
ｙ＝ｙ０・ｋ０＋ｙ１・ｋ１
ｚ＝ｚ０・ｋ０＋ｚ１＋ｋ１
なお、最後に、浮動小数点演算の誤差が蓄積しないように、正規化を行う（ステップＳ１１１）。
【００５４】
バイアス推定値３６は、バイアスにフィードバックする（ステップＳ１１２）ために、四元数を一旦オイラー角に戻す。なお、バイアスをオイラー角でなく四元数で保持するようにすれば、ｓｌｅｒｐを使用することができ、演算効率を向上することができる。
【００５５】
姿勢角推定値算出部３７は、次回周期の演算と姿勢制御演算のために値を保存しておく。一定時間が経過すると、加速度センサとジャイロセンサの想定から次の周期が開始する（ステップＳ１１３）。
【００５６】
以上のようにして、ノイズの大きな角度センサ（加速度センサ）と、ドリフトの大きな角速度センサ（ジャイロ）の信号を適切なフィルタにより、合成し、ノイズの少ない姿勢角とバイアス（ドリフト）の推定値を得る。適用可能なフィルタの種類としては、周波数フィルタ（ＬＰＦ（low pass filter）にＨＰＦ（high pass filter）を組み合わせたもの）や、カルマンフィルタを利用することができる。本実施の形態においては、四元数をカルマンフィルタにてフィルタ処理する際に、球面線形補間（slerp）を使用することで、フィルタ処理の精度とフィルタ演算の効率を向上させることができる。
【００５７】
なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
【００５８】
例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００５９】
２２ａ 角速度センサ
２２ｂ 加速度センサ
２２ｃ 方位角センサ
３１ バイアス修正部
３２ 姿勢角算出部
３３ 積分演算部
３４ フィルタ
３５ フィルタ
３６ バイアス推定値算出部
３７ 姿勢角推定値算出部
【特許請求の範囲】
【請求項１】
状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定装置であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部とを有し、
前記フィルタ演算部は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する、姿勢推定装置。
【請求項２】
前記フィルタ演算部は、前記四元数に変換された姿勢及び姿勢変化率の合成を行う、請求項１記載の姿勢推定装置。
【請求項３】
前記フィルタ演算部は、前記四元数に変換された姿勢及び姿勢変化率の積分値の合成を行う、請求項１又は２記載の姿勢推定装置。
【請求項４】
前記フィルタ演算後の四元数で表現された姿勢をオイラー変換するオイラー変換部を更に有する、請求項１乃至３のいずれか１項記載の姿勢推定装置。
【請求項５】
前記フィルタ演算前の姿勢角は傾斜センサの値であって、前記姿勢変化率は、ロール角速度、ピッチ角速度、及びヨー角速度である、請求項１乃至４のいずれか１項記載の姿勢推定装置。
【請求項６】
前記入力部には、前記姿勢及び姿勢変化率と、方角の情報とが入力される、請求項１乃至５のいずれか１項記載の姿勢推定装置。
【請求項７】
前記方角の情報は、方位角センサの値、又は移動体がタイヤを有する場合はそのタイヤの向きを示す情報である、請求項６に記載の姿勢推定装置。
【請求項８】
状態フィードバックを用いて移動体の姿勢の推定を行う姿勢制御装置であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部と、
前記フィルタ演算により得られた姿勢に基づき前記移動体の姿勢制御を行う姿勢制御部とを有し、
前記フィルタ演算部は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する、姿勢制御装置。
【請求項９】
前記フィルタ演算後の四元数で表現された姿勢をオイラー変換するオイラー変換部を更に有する、請求項８記載の姿勢推定装置。
【請求項１０】
前記フィルタ演算前の姿勢角は傾斜センサの値であって、前記姿勢変化率は、ロール角速度、ピッチ角速、及びヨー角速度である、請求項８又は９項記載の姿勢制御装置。
【請求項１１】
前記入力部には、前記姿勢及び姿勢変化率と、方角の情報とが入力される、請求項８乃至１０のいずれか１項記載の姿勢推定装置。
【請求項１２】
前記フィルタは、カルマンフィルタ又は周波数フィルタである、請求項１乃至１１のいずれか１項記載の姿勢推定装置。
【請求項１３】
状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定方法であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程とを有し、
前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する、姿勢推定方法。
【請求項１４】
前記フィルタ演算工程後に四元数で表現された姿勢をオイラー変換するオイラー変換工程を更に有する、請求項１３項記載の姿勢推定方法。
【請求項１５】
前記フィルタ演算前の姿勢角は傾斜センサの値であって、前記姿勢変化率は、ロール角速度、ピッチ角速、及びヨー角速度である、請求項１３又は１４記載の姿勢推定方法。
【請求項１６】
前記姿勢及び姿勢変化率と共に、方角の情報とが入力される、請求項１３乃至１５のいずれか１項記載の姿勢推定方法。
【請求項１７】
状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定方法であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程と、
前記フィルタ演算により得られた姿勢に基づき前記移動体の姿勢制御を行う姿勢制御工程とを有し、
前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する、姿勢制御方法。
【請求項１８】
状態フィードバックを用いて移動体の姿勢の推定処理をコンピュータに実行させるためのプログラムであって、
を行う姿勢推定方法であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程とを有し、
前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するプログラム。
【請求項１】
状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定装置であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部とを有し、
前記フィルタ演算部は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する、姿勢推定装置。
【請求項２】
前記フィルタ演算部は、前記四元数に変換された姿勢及び姿勢変化率の合成を行う、請求項１記載の姿勢推定装置。
【請求項３】
前記フィルタ演算部は、前記四元数に変換された姿勢及び姿勢変化率の積分値の合成を行う、請求項１又は２記載の姿勢推定装置。
【請求項４】
前記フィルタ演算後の四元数で表現された姿勢をオイラー変換するオイラー変換部を更に有する、請求項１乃至３のいずれか１項記載の姿勢推定装置。
【請求項５】
前記フィルタ演算前の姿勢角は傾斜センサの値であって、前記姿勢変化率は、ロール角速度、ピッチ角速度、及びヨー角速度である、請求項１乃至４のいずれか１項記載の姿勢推定装置。
【請求項６】
前記入力部には、前記姿勢及び姿勢変化率と、方角の情報とが入力される、請求項１乃至５のいずれか１項記載の姿勢推定装置。
【請求項７】
前記方角の情報は、方位角センサの値、又は移動体がタイヤを有する場合はそのタイヤの向きを示す情報である、請求項６に記載の姿勢推定装置。
【請求項８】
状態フィードバックを用いて移動体の姿勢の推定を行う姿勢制御装置であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する入力部と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算部と、
前記フィルタ演算により得られた姿勢に基づき前記移動体の姿勢制御を行う姿勢制御部とを有し、
前記フィルタ演算部は、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する、姿勢制御装置。
【請求項９】
前記フィルタ演算後の四元数で表現された姿勢をオイラー変換するオイラー変換部を更に有する、請求項８記載の姿勢推定装置。
【請求項１０】
前記フィルタ演算前の姿勢角は傾斜センサの値であって、前記姿勢変化率は、ロール角速度、ピッチ角速、及びヨー角速度である、請求項８又は９項記載の姿勢制御装置。
【請求項１１】
前記入力部には、前記姿勢及び姿勢変化率と、方角の情報とが入力される、請求項８乃至１０のいずれか１項記載の姿勢推定装置。
【請求項１２】
前記フィルタは、カルマンフィルタ又は周波数フィルタである、請求項１乃至１１のいずれか１項記載の姿勢推定装置。
【請求項１３】
状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定方法であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程とを有し、
前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する、姿勢推定方法。
【請求項１４】
前記フィルタ演算工程後に四元数で表現された姿勢をオイラー変換するオイラー変換工程を更に有する、請求項１３項記載の姿勢推定方法。
【請求項１５】
前記フィルタ演算前の姿勢角は傾斜センサの値であって、前記姿勢変化率は、ロール角速度、ピッチ角速、及びヨー角速度である、請求項１３又は１４記載の姿勢推定方法。
【請求項１６】
前記姿勢及び姿勢変化率と共に、方角の情報とが入力される、請求項１３乃至１５のいずれか１項記載の姿勢推定方法。
【請求項１７】
状態フィードバックを用いて移動体の姿勢の推定を行う姿勢推定方法であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程と、
前記フィルタ演算により得られた姿勢に基づき前記移動体の姿勢制御を行う姿勢制御工程とを有し、
前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用する、姿勢制御方法。
【請求項１８】
状態フィードバックを用いて移動体の姿勢の推定処理をコンピュータに実行させるためのプログラムであって、
を行う姿勢推定方法であって、
姿勢及び姿勢変化率が入力され、これらを四元数に変換する変換工程と、
前記四元数に変換された姿勢及び姿勢変化率を使用してフィルタ演算するフィルタ演算工程とを有し、
前記フィルタ演算工程では、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用するプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１１−２２０８２５（Ｐ２０１１−２２０８２５Ａ）
【公開日】平成２３年１１月４日（２０１１．１１．４）
【国際特許分類】
【出願番号】特願２０１０−９０２７０（Ｐ２０１０−９０２７０）
【出願日】平成２２年４月９日（２０１０．４．９）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２３年１１月４日（２０１１．１１．４）
【国際特許分類】
【出願日】平成２２年４月９日（２０１０．４．９）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]