姿勢角演算装置、姿勢角演算方法、及びプログラム
【課題】高速で姿勢角度を算出すること。
【解決手段】姿勢角演算装置は、移動体の状態量を検出する状態検出手段と、状態検出手段により検出された移動体の状態量に基づいて、移動体の姿勢を示す四元数を算出する四元数算出手段と、四元数算出手段により算出された四元数におけるヨー軸成分をキャンセルして四元数の修正を行った修正四元数（Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋ）を算出する修正手段と、修正手段により算出された修正四元数に基づいて、近似式を用いて移動体のロール角α及びピッチ角βを高速に算出する姿勢角計算手段と、を備えている。
【解決手段】姿勢角演算装置は、移動体の状態量を検出する状態検出手段と、状態検出手段により検出された移動体の状態量に基づいて、移動体の姿勢を示す四元数を算出する四元数算出手段と、四元数算出手段により算出された四元数におけるヨー軸成分をキャンセルして四元数の修正を行った修正四元数（Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋ）を算出する修正手段と、修正手段により算出された修正四元数に基づいて、近似式を用いて移動体のロール角α及びピッチ角βを高速に算出する姿勢角計算手段と、を備えている。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットなどの移動体の姿勢角度を高速に算出できる姿勢角演算装置、姿勢角演算方法、及びプログラムに関するものである。
【背景技術】
【０００２】
近年、スカラー部分と複素数ベクトル部分とから成る四元数（クォータニオン）を用いて、ロボットなどの移動体の姿勢角度を算出する技術が知られている（例えば、特許文献１参照）。この技術により、ロール角、ピッチ角、及びヨー角の姿勢角度において生じるジンバルロックを抑制することができる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特表２００３−５２１６９７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記特許文献１においては、四元数に基づいて、三角関数を用いて姿勢角度を算出している。このため、その計算に多くの時間を要することとなる。
【０００５】
本発明は、このような問題点を解決するためになされたものであり、高速で姿勢角度を算出できる姿勢角演算装置、姿勢角演算方法、及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、移動体の状態量を検出する状態検出手段と、前記状態検出手段により検出された前記移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する四元数算出手段と、前記四元数算出手段により算出された前記四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する修正手段と、前記修正手段により算出された前記修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する姿勢角計算手段と、
を備える、ことを特徴とする姿勢角演算装置である。β≒２・ｙ／ｗ、α≒２・ｘ／ｗ、上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。なお、上記近似式は、右手座標系に関するものであるが、左手座標系に関しても右手座標系と同様に適用可能である。
この一態様において、前記修正手段は、座標系をｘ軸、ｙ軸、及びｚ軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、該算出した四元数と、前記四元数算出手段により算出された四元数と、を乗算することで、前記修正四元数を算出してもよい。
この一態様において、前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出してもよい。
この一態様において、前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出してもよい。
この一態様において、前記移動体は倒立型移動体であり、前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサと、前記移動体の加速度を検出する加速度センサと、各車輪の速度検出する速度検出器と、を含み、前記四元数算出手段は、前記ジャイロセンサにより検出された角速度、前記加速度センサにより検出された加速度、及び前記速度検出器により検出された車輪速度、に基づいた車体加速度、遠心力、及び旋回加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出してもよい。
他方、上記目的を達成するための本発明の一態様は、移動体の状態量を検出するステップと、前記検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出するステップと、前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出するステップと、前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出するステップと、を含む、ことを特徴とする姿勢角演算方法であってもよい。β≒２・ｙ／ｗ、α≒２・ｘ／ｗ、上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。なお、上記近似式は、右手座標系に関するものであるが、左手座標系に関しても右手座標系と同様に適用可能である。
この一態様において、座標系をｘ軸、ｙ軸、及びｚ軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、該算出した四元数と、前記算出された四元数と、を乗算することで、前記修正四元数を算出してもよい。
この一態様において、ジャイロセンサにより検出された角速度と、加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出してもよい。
さらに、上記目的を達成するための本発明の一態様は、検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する処理と、前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する処理と、前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する処理と、をコンピュータに実行させる、ことを特徴とするプログラムであってもよい。β≒２・ｙ／ｗ、α≒２・ｘ／ｗ、上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。なお、上記近似式は、右手座標系に関するものであるが、左手座標系に関しても右手座標系と同様に適用可能である。
【発明の効果】
【０００７】
本発明によれば、高速で姿勢角度を算出できる姿勢角演算装置、姿勢角演算方法、及びプログラムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の実施の形態１に係る姿勢角演算装置の概略的な構成を示すブロック図である。
【図２】本発明の実施の形態１に係るヨー軸成分キャンセル部による演算処理の一例を示すフローチャートである。
【図３】本発明の実施の形態２に係る姿勢角演算装置の概略的なシステム構成を示すブロック図である。
【図４】本発明の実施の形態２に係る姿勢角演算装置を搭載した倒立型二輪車の概略的なシステム構成を示すブロック図である。
【図５】本発明の実施の形態２に係る倒立二輪車の概略的な構成を示す図である。
【発明を実施するための形態】
【０００９】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の実施の形態１に係る姿勢角演算装置の概略的な構成を示すブロック図である。本実施の形態１に係る姿勢角演算装置１０は、四元数計算部１と、ヨー軸成分キャンセル部２と、姿勢角計算部３と、センサ４と、を備えている。
【００１０】
なお、姿勢角演算装置１０は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバス等によって相互に接続されている。
【００１１】
四元数計算部１は、四元数算出手段の一具体例であり、例えば、ロボットや倒立二輪車などの移動体に搭載された各種センサ４から出力されるセンサ値に基づいて、ロボットなどの移動体の姿勢を示す四元数Ｑ（Ｑｕａｔｅｒｎｉｏｎ）を算出する。ここで、四元数とは、一つの実部と３つの虚数単位からなる虚部を持つ、複素数の一種である。３つの虚数単位をｉ、ｊ、ｋとしたとき、任意の四元数Ｑは実数ｗ、ｘ、ｙ、ｚを用いて、下記（１）式のように表される。
【００１２】
四元数計算部１は、例えば、ジャイロセンサにより検出された角速度と、加速度センサにより検出された加速度と、に基づいて、下記（１）式を用いて、ロボットなどの姿勢を示す四元数Ｑを算出する。
Ｑ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋ （１）式
【００１３】
ここで、右手座標系において、基準座標系から見てｘ軸周りにα［ｒａｄ］だけロール回転させる。また、その状態から基準座標系から見てｙ軸周りにβ［ｒａｄ］だけピッチ回転させる。さらに、その状態から基準座標系から見てｚ軸周りにγ[ｒａｄ]だけヨー回転させると、その回転姿勢を表わす姿勢行列（回転行列）は、下記（２）式のように表わすことができる。なお、以下、右手座標系を用いた一例を説明するが、左手座標系に関しても右手座標系と同様の方法を適用することができる。
【数１】
【００１４】
また、四元数Ｑを上記（１）式のように表わすと、上記姿勢行列Ｒは、ｗ、ｘ、ｙ、ｚを用いて、下記（３）式のように表わすことができる。
【数２】
四元数計算部１にはヨー軸成分キャンセル部２が接続されており、四元数計算部１は算出した四元数Ｑをヨー軸成分キャンセル部２に対して出力する。
【００１５】
ヨー軸成分キャンセル部２は、修正手段の一具体例であり、四元数計算部１により算出された四元数Ｑから、ヨー軸成分をキャンセルした修正四元数Ｑｍｏｄを算出する。これにより、四元数Ｑの要素の一つであるヨー軸成分をキャンセルすることで、計算量を大幅に低減し、姿勢角度の演算速度を高速化することができる。
【００１６】
ここで、修正四元数Ｑｍｏｄの算出方法について、詳細に説明する。図２は、本実施の形態１に係るヨー軸成分キャンセル部による演算処理の一例を示すフローチャートである。
まず、ヨー軸成分キャンセル部２は、下記（４）式を用いてｓｉｎβを算出する（ステップＳ１０１）。
ｓｉｎβ＝−２・（ｘ・ｚ−ｗ・ｙ） （４）式
【００１７】
次に、ヨー軸成分キャンセル部２は、上記算出したｓｉｎβに基づいて、ｓｉｎβ＞０．９９９９又はｓｉｎβ＜−０．９９９９を満たし、ジンバルロック状態であるか否かを判断する（ステップＳ１０２）。
【００１８】
ヨー軸成分キャンセル部２は、ジンバルロック状態であると判断したとき（ｓｉｎβの絶対値が１に近いとき）（ステップＳ１０２のＹＥＳ）、下記（５）式を用いて、ｓｉｎγ及びｃｏｓγを算出する（ステップＳ１０３）。
ｓｉｎγ＝２・（ｗ・ｚ−ｘ・ｙ） （５）式
ｃｏｓγ＝１−２・ｘ２−２・ｚ２
なお、上記（２）式においてα＝０とした姿勢行列Ｒと、上記（３）式に示す姿勢行列Ｒと、を比較することで、上記（５）式を求めることができる。
【００１９】
一方、ヨー軸成分キャンセル部２は、ジンバルロック状態でないと判断したとき（ｓｉｎβの絶対値が１に近くないとき）（ステップＳ１０２のＮＯ）、下記（６）式を用いて、ｓｉｎγｃｏｓβ、ｃｏｓγｃｏｓβを算出する（ステップＳ１０４）。
ｓｉｎγｃｏｓβ＝２・（ｘ・ｙ＋ｗ・ｚ） （６）式
ｃｏｓγｃｏｓβ＝１−２．ｙ２−２・ｚ２
【００２０】
なお、上記（２）式に示す姿勢行列Ｒと上記（３）式に示す姿勢行列Ｒとを比較することで、上記（４）式及び（６）式を求めることができる。
【００２１】
次に、ヨー軸成分キャンセル部２は、上記（４）式により算出したｓｉｎβに基づいて、下記（７）式を用いて、ｃｏｓβを算出する（ステップＳ１０５）。
ｃｏｓβ＝ｓｑｒｔ（１−ｓｉｎβ２） （７）式
【００２２】
ヨー軸成分キャンセル部２は、上記（６）式及び（７）式の演算結果に基づいて、下記（８）式を用いて、ｓｉｎγ、ｃｏｓγを算出する（ステップＳ１０６）。
ｓｉｎγ＝ｓｉｎγｃｏｓβ／ｃｏｓβ （８）式
ｃｏｓγ＝ｃｏｓγｃｏｓβ／ｃｏｓβ
【００２３】
ヨー軸成分キャンセル部２は、上記（５）式又は（８）式の演算結果に基づいて、下記（９）式を用いて、ｓｉｎ（γ／２）、ｃｏｓ（γ／２）を算出する（ステップＳ１０７）。
ｓｉｎ（γ／２）＝ｓｑｒｔ（（１−ｃｏｓγ）／２） （９）式
ｃｏｓ（γ／２）＝ｓｑｒｔ（（１＋ｃｏｓγ）／２）
なお、ヨー軸成分キャンセル部２は、ｓｉｎγ＜０であると判断したとき、ｓｉｎ（γ／２）＝−ｓｉｎ（γ／２）とする。
【００２４】
ヨー軸成分キャンセル部２は、上記（９）式の演算結果に基づいて、下記（１０）式を用いて、ヨー軸成分をキャンセルするための四元数を算出する（ステップＳ１０８）。
Ｑｙａｗ．ｗ＝ｃｏｓ（γ／２）
Ｑｙａｗ．ｘ＝０ （１０）式
Ｑｙａｗ．ｙ＝０
Ｑｙａｗ．ｚ＝−ｓｉｎ（γ／２）
【００２５】
ヨー軸成分キャンセル部２は、上記（１０）式に示す四元数をキャンセルする四元数Ｑｙａｗと、四元数計算部１により算出された四元数Ｑと、を、下記（１１）式を用いて乗算し、ヨー軸成分がキャンセルされた修正四元数Ｑｍｏｄを算出する（ステップＳ１０９）。
Ｑｍｏｄ＝Ｑｙａｗ・Ｑ （１１）式
【００２６】
ここで、上述のようにヨー軸成分をキャンセルし０に拘束して、Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとすると、下記（１２）式が成立する。
ｗ＝ｃｏｓ（α／２）ｃｏｓ（β／２）
ｘ＝ｓｉｎ（α／２）ｃｏｓ（β／２） （１２）式
ｙ＝ｃｏｓ（α／２）ｓｉｎ（β／２）
ｚ＝−ｓｉｎ（α／２）ｓｉｎ（β／２）
【００２７】
さらに、ロール角度α、及びピッチ角度βを大きくない角度と仮定すると、下記（１３）式に示す近似式を用いて、ロール角度α及びピッチ角度βを算出することができる。
β≒２ｔａｎ(β/２)＝２・ｓｉｎ(β/２)/ｃｏｓ(β/２)＝２・ｙ/ｗ
α≒２ｔａｎ(α/２)＝２・ｓｉｎ(α/２)/ｃｏｓ(α/２)＝２・ｘ/ｗ （１３）式
なお、上記（１３）式は、右手座標系を適用した場合の式であるが、左手座標系を適用した場合の近似式も、右手座標系と同様に算出することができる。また、上記（１３）式に示す近似式を用いた場合、例えば、姿勢角度が２０[ｄｅｇ]のとき、約１％の誤差が生じ、姿勢角度が３０[ｄｅｇ]のとき、約２．３％の誤差が生じる。一方で、姿勢角度に対して、一般的に求められる精度は、１〜２％程度の誤差であるため、本実施の形態に係る上記近似式（１３）は、この精度を満たすこととなる。
ヨー軸成分キャンセル部２には姿勢角計算部３が接続されており、ヨー軸成分キャンセル部２は、算出した修正四元数Ｑｍｏｄを姿勢角計算部３に対して出力する。
【００２８】
姿勢角計算部３は、姿勢角計算手段の一具体例であり、ヨー軸成分キャンセル部２により算出された修正四元数Ｑｍｏdに基づいて求めた上記（１３）式を用いて、三角関数を用いることなく、ロール角度α及びピッチ角度βを算出することができる。このように三角関数を用いることなく姿勢角度を算出することで、計算量を大幅に低減できる。
【００２９】
なお、ヨー軸成分キャンセル部２により算出された修正四元数Ｑｍｏｄのｗ成分が０の場合（又は０に極めて近い場合）は、上記（１３）式を用いて、ロール角度α及びピッチ角度βを算出することができない。したがって、ｗが０に極めて近い値の場合（ｗ≒０）は、例えば、下記（１４）式のように、ロール軸をπに固定してピッチ角度βのみを算出することができる。
β≒−２・ｚ／ｘ （１４）式
α＝０
ここで、ｃｏｓα／２＝０、ｓｉｎα／２＝１に基づいて、ｗ＝０、ｘ＝ｃｏｓ（β／２）、ｙ＝０、ｚ＝−ｓｉｎ（β／２）が成立し、上記（１４）式が導出される。
【００３０】
以上により、姿勢角計算部３は、修正四元数Ｑｍｏｄのｗ成分が０に極めて近い（ｗ≒０）と判断したときは、上記（１４）式を用いて、ロール角度α及びピッチ角度βを算出する。一方、姿勢角計算部３は、修正四元数Ｑｍｏｄのｗ成分が０に近くないと判断したときは、上記（１３）式を用いて、ロール角度α及びピッチ角度βを算出する。
【００３１】
以上、本実施の形態１に係る姿勢角演算装置１０において、四元数Ｑからヨー軸成分をキャンセルした修正四元数Ｑｍｏｄを算出し、該算出した修正四元数Ｑｍｏｄに基づいて上記近似式（１３）を用いてロール角度α及びピッチ角度βを算出する。これにより、計算量を大幅に低減し、姿勢角度を高速に算出することができる。
【００３２】
実施の形態２．
本発明の実施の形態２に係る姿勢角演算装置２０において、四元数計算部１は、ストラップダウン演算を用いて、ロボットなどの移動体の姿勢を示す四元数Ｑを算出してもよい。図３は、本実施の形態２に係る姿勢角演算装置の概略的なシステム構成を示すブロック図である。
【００３３】
四元数計算部１は、例えば、ジャイロセンサ５により検出された角速度と、加速度センサ６により検出された加速度と、算出されフィードバックされた四元数Ｑと、に基づいて、ロボットなどの姿勢を示す四元数Ｑを算出する。
なお、上記ストラップダウン演算方法の詳細は、例えば、特表２００９−５０７２７２号公報などに詳細に記載されており、これを援用することができる。本実施の形態２において、他の構成は上記実施の形態１と略同一であるため、詳細な説明は省略する。
【００３４】
本実施の形態２に係る姿勢角演算装置２０は、例えば、倒立型二輪車に搭載されてもよい。図４は、本実施の形態２に係る姿勢角演算装置を搭載した倒立型二輪車の概略的なシステム構成を示すブロック図である。図５は、本実施の形態２に係る倒立二輪車の概略的な構成を示す図である。
【００３５】
本実施の形態に係る倒立型二輪車１００は、例えば、図５に示すように、左右一対の車輪１０１と、搭乗者が搭乗するステップ１０２と、搭乗者が把持し操作するハンドル１０３と、を備えている。また、本実施の形態に係る倒立型二輪車１００は、例えば、図４に示すように、ジャイロセンサ５と、加速度センサ６と、一対の車輪速度検出器７、８と、演算部９と、姿勢角演算装置２０と、センサ値変換部１１と、ＰＩＤ制御部１２と、旋回指令値生成部１３と、目標車輪速度生成部１４と、一対の駆動アンプ１５、１６と、一対のモータ１７、１８と、を備えている。
【００３６】
ジャイロセンサ５は、例えば、３軸レートジャイロセンサが用いられており、検出した３軸角速度を姿勢角演算装置２０及びセンサ値変換部１１に出力する。加速度センサ６は、例えば、３軸加速度センサであり、検出した３軸加速度を姿勢角演算装置２０に対して出力する。
【００３７】
一対の車輪速度検出器（速度検出器）７、８は、ロータリーエンコーダやレゾルバーなどを用いて、左右一対の車輪の車輪速度を夫々検出し、検出した各車輪速度を演算部９に対して出力する。
【００３８】
演算部９は、各車輪速度検出器７、８から出力された各車輪速度に基づいて、倒立型二輪車の車体加速度、遠心力、及び旋回加速度を算出し、算出した車体加速度、遠心力、及び旋回加速度を姿勢角演算装置２０の四元数計算部１に対して出力する。
【００３９】
四元数計算部１は、ジャイロセンサ５から出力された角速度と、加速度センサ６から出力された加速度と、演算部９から出力された車体加速度、遠心力、及び旋回加速度と、算出されフィードバックされた四元数Ｑと、に基づいて、ストラップダウン演算を行い、倒立型二輪車１００の姿勢を示す四元数Ｑを算出し、ヨー軸成分キャンセル部２に対して出力する。
【００４０】
なお、四元数計算部１は、各車輪速度に基づいた車体加速度、遠心力、及び旋回加速度を用いて、ストラップダウン演算を行うことで、その演算精度を向上させている。
【００４１】
ヨー軸成分キャンセル部２は、四元数計算部１から出力された四元数Ｑから、ヨー軸成分をキャンセルした修正四元数Ｑｍｏｄを算出し、センサ値変換部１１及び姿勢角計算部３に対して出力する。
【００４２】
センサ値変換部１１は、ジャイロセンサ５から出力された角速度と、ヨー軸成分キャンセル部２から出力された修正四元数Ｑｍｏｄと、に基づいて、水平座標系へ各センサ値を変換して、倒立型二輪車１００のピッチ角速度（βの微分値）を算出し、ＰＩＤ制御部１２に対して出力する。
【００４３】
姿勢角計算部３は、ヨー軸成分キャンセル部２から出力された修正四元数Ｑｍｏｄに基づいて、倒立型二輪車１００のロール角度α及びピッチ角度βを算出し、算出したピッチ角度βをＰＩＤ制御部１２に対して出力する。
【００４４】
ＰＩＤ制御部１２は、センサ値変換部１１から出力されたピッチ角速度（βの微分値）と、姿勢角計算部３から出力されたピッチ角度βと、目標ピッチ角度と、に基づいて、ＰＩＤ（Proportinal Integral Differential）制御を行い、目標速度（例えば、左右速度の平均値）を算出し、目標車輪速度生成部１４に対して出力する。
【００４５】
旋回指令値生成部１３は、搭乗者の旋回操作に応じて、その旋回量を示す旋回量指令値を生成し、目標車輪速度生成部１４に対して出力する。
【００４６】
目標車輪速度生成部１４は、ＰＩＤ制御部１２から出力された目標速度と、旋回指令値生成部１３から出力された目標速度と、に基づいて、左右車輪に対する目標車輪速度を算出し、各駆動アンプ１５、１６に対して出力する。
【００４７】
各駆動アンプ１５、１６は、目標車輪速度生成部１４から出力された各目標速度に対応するモータ電流を、夫々各モータ１７、１８に対して出力する。各モータ１７、１８は、各駆動アンプ１５、１６から出力されたモータ電流に応じて、各車輪を回転駆動する。これにより、倒立型二輪車１００は倒立状態を維持しつつ、所望の走行を行うことができる。
【００４８】
以上、本実施の形態２に係る姿勢角演算装置２０において、ストラップダウン演算を用いて算出した四元数Ｑからヨー軸成分をキャンセルした修正四元数Ｑｍｏｄを算出し、該算出した修正四元数Ｑｍｏｄに基づいて上記近似式（１３）を用いてロール角度α及びピッチ角度βを算出する。これにより、姿勢角度を高精度かつ高速に算出することができる。
【００４９】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、例えば、図２に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００５０】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ）を含む。
【００５１】
上記プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００５２】
１ 四元数計算部
２ ヨー軸成分キャンセル部
３ 姿勢角計算部
４ 各種センサ
５ ジャイロセンサ
６ 加速度センサ
７、８ 車輪速度検出器
９ 演算部
１０、２０ 姿勢角演算装置
１１ センサ値変換部
１２ ＰＩＤ制御部
１３ 旋回指令値生成部
１４ 目標車輪速度生成部
１５、１６ 駆動アンプ
１７、１８ モータ
【技術分野】
【０００１】
本発明は、ロボットなどの移動体の姿勢角度を高速に算出できる姿勢角演算装置、姿勢角演算方法、及びプログラムに関するものである。
【背景技術】
【０００２】
近年、スカラー部分と複素数ベクトル部分とから成る四元数（クォータニオン）を用いて、ロボットなどの移動体の姿勢角度を算出する技術が知られている（例えば、特許文献１参照）。この技術により、ロール角、ピッチ角、及びヨー角の姿勢角度において生じるジンバルロックを抑制することができる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特表２００３−５２１６９７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記特許文献１においては、四元数に基づいて、三角関数を用いて姿勢角度を算出している。このため、その計算に多くの時間を要することとなる。
【０００５】
本発明は、このような問題点を解決するためになされたものであり、高速で姿勢角度を算出できる姿勢角演算装置、姿勢角演算方法、及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、移動体の状態量を検出する状態検出手段と、前記状態検出手段により検出された前記移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する四元数算出手段と、前記四元数算出手段により算出された前記四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する修正手段と、前記修正手段により算出された前記修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する姿勢角計算手段と、
を備える、ことを特徴とする姿勢角演算装置である。β≒２・ｙ／ｗ、α≒２・ｘ／ｗ、上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。なお、上記近似式は、右手座標系に関するものであるが、左手座標系に関しても右手座標系と同様に適用可能である。
この一態様において、前記修正手段は、座標系をｘ軸、ｙ軸、及びｚ軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、該算出した四元数と、前記四元数算出手段により算出された四元数と、を乗算することで、前記修正四元数を算出してもよい。
この一態様において、前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出してもよい。
この一態様において、前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出してもよい。
この一態様において、前記移動体は倒立型移動体であり、前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサと、前記移動体の加速度を検出する加速度センサと、各車輪の速度検出する速度検出器と、を含み、前記四元数算出手段は、前記ジャイロセンサにより検出された角速度、前記加速度センサにより検出された加速度、及び前記速度検出器により検出された車輪速度、に基づいた車体加速度、遠心力、及び旋回加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出してもよい。
他方、上記目的を達成するための本発明の一態様は、移動体の状態量を検出するステップと、前記検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出するステップと、前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出するステップと、前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出するステップと、を含む、ことを特徴とする姿勢角演算方法であってもよい。β≒２・ｙ／ｗ、α≒２・ｘ／ｗ、上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。なお、上記近似式は、右手座標系に関するものであるが、左手座標系に関しても右手座標系と同様に適用可能である。
この一態様において、座標系をｘ軸、ｙ軸、及びｚ軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、該算出した四元数と、前記算出された四元数と、を乗算することで、前記修正四元数を算出してもよい。
この一態様において、ジャイロセンサにより検出された角速度と、加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出してもよい。
さらに、上記目的を達成するための本発明の一態様は、検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する処理と、前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する処理と、前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する処理と、をコンピュータに実行させる、ことを特徴とするプログラムであってもよい。β≒２・ｙ／ｗ、α≒２・ｘ／ｗ、上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。なお、上記近似式は、右手座標系に関するものであるが、左手座標系に関しても右手座標系と同様に適用可能である。
【発明の効果】
【０００７】
本発明によれば、高速で姿勢角度を算出できる姿勢角演算装置、姿勢角演算方法、及びプログラムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の実施の形態１に係る姿勢角演算装置の概略的な構成を示すブロック図である。
【図２】本発明の実施の形態１に係るヨー軸成分キャンセル部による演算処理の一例を示すフローチャートである。
【図３】本発明の実施の形態２に係る姿勢角演算装置の概略的なシステム構成を示すブロック図である。
【図４】本発明の実施の形態２に係る姿勢角演算装置を搭載した倒立型二輪車の概略的なシステム構成を示すブロック図である。
【図５】本発明の実施の形態２に係る倒立二輪車の概略的な構成を示す図である。
【発明を実施するための形態】
【０００９】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の実施の形態１に係る姿勢角演算装置の概略的な構成を示すブロック図である。本実施の形態１に係る姿勢角演算装置１０は、四元数計算部１と、ヨー軸成分キャンセル部２と、姿勢角計算部３と、センサ４と、を備えている。
【００１０】
なお、姿勢角演算装置１０は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバス等によって相互に接続されている。
【００１１】
四元数計算部１は、四元数算出手段の一具体例であり、例えば、ロボットや倒立二輪車などの移動体に搭載された各種センサ４から出力されるセンサ値に基づいて、ロボットなどの移動体の姿勢を示す四元数Ｑ（Ｑｕａｔｅｒｎｉｏｎ）を算出する。ここで、四元数とは、一つの実部と３つの虚数単位からなる虚部を持つ、複素数の一種である。３つの虚数単位をｉ、ｊ、ｋとしたとき、任意の四元数Ｑは実数ｗ、ｘ、ｙ、ｚを用いて、下記（１）式のように表される。
【００１２】
四元数計算部１は、例えば、ジャイロセンサにより検出された角速度と、加速度センサにより検出された加速度と、に基づいて、下記（１）式を用いて、ロボットなどの姿勢を示す四元数Ｑを算出する。
Ｑ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋ （１）式
【００１３】
ここで、右手座標系において、基準座標系から見てｘ軸周りにα［ｒａｄ］だけロール回転させる。また、その状態から基準座標系から見てｙ軸周りにβ［ｒａｄ］だけピッチ回転させる。さらに、その状態から基準座標系から見てｚ軸周りにγ[ｒａｄ]だけヨー回転させると、その回転姿勢を表わす姿勢行列（回転行列）は、下記（２）式のように表わすことができる。なお、以下、右手座標系を用いた一例を説明するが、左手座標系に関しても右手座標系と同様の方法を適用することができる。
【数１】
【００１４】
また、四元数Ｑを上記（１）式のように表わすと、上記姿勢行列Ｒは、ｗ、ｘ、ｙ、ｚを用いて、下記（３）式のように表わすことができる。
【数２】
四元数計算部１にはヨー軸成分キャンセル部２が接続されており、四元数計算部１は算出した四元数Ｑをヨー軸成分キャンセル部２に対して出力する。
【００１５】
ヨー軸成分キャンセル部２は、修正手段の一具体例であり、四元数計算部１により算出された四元数Ｑから、ヨー軸成分をキャンセルした修正四元数Ｑｍｏｄを算出する。これにより、四元数Ｑの要素の一つであるヨー軸成分をキャンセルすることで、計算量を大幅に低減し、姿勢角度の演算速度を高速化することができる。
【００１６】
ここで、修正四元数Ｑｍｏｄの算出方法について、詳細に説明する。図２は、本実施の形態１に係るヨー軸成分キャンセル部による演算処理の一例を示すフローチャートである。
まず、ヨー軸成分キャンセル部２は、下記（４）式を用いてｓｉｎβを算出する（ステップＳ１０１）。
ｓｉｎβ＝−２・（ｘ・ｚ−ｗ・ｙ） （４）式
【００１７】
次に、ヨー軸成分キャンセル部２は、上記算出したｓｉｎβに基づいて、ｓｉｎβ＞０．９９９９又はｓｉｎβ＜−０．９９９９を満たし、ジンバルロック状態であるか否かを判断する（ステップＳ１０２）。
【００１８】
ヨー軸成分キャンセル部２は、ジンバルロック状態であると判断したとき（ｓｉｎβの絶対値が１に近いとき）（ステップＳ１０２のＹＥＳ）、下記（５）式を用いて、ｓｉｎγ及びｃｏｓγを算出する（ステップＳ１０３）。
ｓｉｎγ＝２・（ｗ・ｚ−ｘ・ｙ） （５）式
ｃｏｓγ＝１−２・ｘ２−２・ｚ２
なお、上記（２）式においてα＝０とした姿勢行列Ｒと、上記（３）式に示す姿勢行列Ｒと、を比較することで、上記（５）式を求めることができる。
【００１９】
一方、ヨー軸成分キャンセル部２は、ジンバルロック状態でないと判断したとき（ｓｉｎβの絶対値が１に近くないとき）（ステップＳ１０２のＮＯ）、下記（６）式を用いて、ｓｉｎγｃｏｓβ、ｃｏｓγｃｏｓβを算出する（ステップＳ１０４）。
ｓｉｎγｃｏｓβ＝２・（ｘ・ｙ＋ｗ・ｚ） （６）式
ｃｏｓγｃｏｓβ＝１−２．ｙ２−２・ｚ２
【００２０】
なお、上記（２）式に示す姿勢行列Ｒと上記（３）式に示す姿勢行列Ｒとを比較することで、上記（４）式及び（６）式を求めることができる。
【００２１】
次に、ヨー軸成分キャンセル部２は、上記（４）式により算出したｓｉｎβに基づいて、下記（７）式を用いて、ｃｏｓβを算出する（ステップＳ１０５）。
ｃｏｓβ＝ｓｑｒｔ（１−ｓｉｎβ２） （７）式
【００２２】
ヨー軸成分キャンセル部２は、上記（６）式及び（７）式の演算結果に基づいて、下記（８）式を用いて、ｓｉｎγ、ｃｏｓγを算出する（ステップＳ１０６）。
ｓｉｎγ＝ｓｉｎγｃｏｓβ／ｃｏｓβ （８）式
ｃｏｓγ＝ｃｏｓγｃｏｓβ／ｃｏｓβ
【００２３】
ヨー軸成分キャンセル部２は、上記（５）式又は（８）式の演算結果に基づいて、下記（９）式を用いて、ｓｉｎ（γ／２）、ｃｏｓ（γ／２）を算出する（ステップＳ１０７）。
ｓｉｎ（γ／２）＝ｓｑｒｔ（（１−ｃｏｓγ）／２） （９）式
ｃｏｓ（γ／２）＝ｓｑｒｔ（（１＋ｃｏｓγ）／２）
なお、ヨー軸成分キャンセル部２は、ｓｉｎγ＜０であると判断したとき、ｓｉｎ（γ／２）＝−ｓｉｎ（γ／２）とする。
【００２４】
ヨー軸成分キャンセル部２は、上記（９）式の演算結果に基づいて、下記（１０）式を用いて、ヨー軸成分をキャンセルするための四元数を算出する（ステップＳ１０８）。
Ｑｙａｗ．ｗ＝ｃｏｓ（γ／２）
Ｑｙａｗ．ｘ＝０ （１０）式
Ｑｙａｗ．ｙ＝０
Ｑｙａｗ．ｚ＝−ｓｉｎ（γ／２）
【００２５】
ヨー軸成分キャンセル部２は、上記（１０）式に示す四元数をキャンセルする四元数Ｑｙａｗと、四元数計算部１により算出された四元数Ｑと、を、下記（１１）式を用いて乗算し、ヨー軸成分がキャンセルされた修正四元数Ｑｍｏｄを算出する（ステップＳ１０９）。
Ｑｍｏｄ＝Ｑｙａｗ・Ｑ （１１）式
【００２６】
ここで、上述のようにヨー軸成分をキャンセルし０に拘束して、Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとすると、下記（１２）式が成立する。
ｗ＝ｃｏｓ（α／２）ｃｏｓ（β／２）
ｘ＝ｓｉｎ（α／２）ｃｏｓ（β／２） （１２）式
ｙ＝ｃｏｓ（α／２）ｓｉｎ（β／２）
ｚ＝−ｓｉｎ（α／２）ｓｉｎ（β／２）
【００２７】
さらに、ロール角度α、及びピッチ角度βを大きくない角度と仮定すると、下記（１３）式に示す近似式を用いて、ロール角度α及びピッチ角度βを算出することができる。
β≒２ｔａｎ(β/２)＝２・ｓｉｎ(β/２)/ｃｏｓ(β/２)＝２・ｙ/ｗ
α≒２ｔａｎ(α/２)＝２・ｓｉｎ(α/２)/ｃｏｓ(α/２)＝２・ｘ/ｗ （１３）式
なお、上記（１３）式は、右手座標系を適用した場合の式であるが、左手座標系を適用した場合の近似式も、右手座標系と同様に算出することができる。また、上記（１３）式に示す近似式を用いた場合、例えば、姿勢角度が２０[ｄｅｇ]のとき、約１％の誤差が生じ、姿勢角度が３０[ｄｅｇ]のとき、約２．３％の誤差が生じる。一方で、姿勢角度に対して、一般的に求められる精度は、１〜２％程度の誤差であるため、本実施の形態に係る上記近似式（１３）は、この精度を満たすこととなる。
ヨー軸成分キャンセル部２には姿勢角計算部３が接続されており、ヨー軸成分キャンセル部２は、算出した修正四元数Ｑｍｏｄを姿勢角計算部３に対して出力する。
【００２８】
姿勢角計算部３は、姿勢角計算手段の一具体例であり、ヨー軸成分キャンセル部２により算出された修正四元数Ｑｍｏdに基づいて求めた上記（１３）式を用いて、三角関数を用いることなく、ロール角度α及びピッチ角度βを算出することができる。このように三角関数を用いることなく姿勢角度を算出することで、計算量を大幅に低減できる。
【００２９】
なお、ヨー軸成分キャンセル部２により算出された修正四元数Ｑｍｏｄのｗ成分が０の場合（又は０に極めて近い場合）は、上記（１３）式を用いて、ロール角度α及びピッチ角度βを算出することができない。したがって、ｗが０に極めて近い値の場合（ｗ≒０）は、例えば、下記（１４）式のように、ロール軸をπに固定してピッチ角度βのみを算出することができる。
β≒−２・ｚ／ｘ （１４）式
α＝０
ここで、ｃｏｓα／２＝０、ｓｉｎα／２＝１に基づいて、ｗ＝０、ｘ＝ｃｏｓ（β／２）、ｙ＝０、ｚ＝−ｓｉｎ（β／２）が成立し、上記（１４）式が導出される。
【００３０】
以上により、姿勢角計算部３は、修正四元数Ｑｍｏｄのｗ成分が０に極めて近い（ｗ≒０）と判断したときは、上記（１４）式を用いて、ロール角度α及びピッチ角度βを算出する。一方、姿勢角計算部３は、修正四元数Ｑｍｏｄのｗ成分が０に近くないと判断したときは、上記（１３）式を用いて、ロール角度α及びピッチ角度βを算出する。
【００３１】
以上、本実施の形態１に係る姿勢角演算装置１０において、四元数Ｑからヨー軸成分をキャンセルした修正四元数Ｑｍｏｄを算出し、該算出した修正四元数Ｑｍｏｄに基づいて上記近似式（１３）を用いてロール角度α及びピッチ角度βを算出する。これにより、計算量を大幅に低減し、姿勢角度を高速に算出することができる。
【００３２】
実施の形態２．
本発明の実施の形態２に係る姿勢角演算装置２０において、四元数計算部１は、ストラップダウン演算を用いて、ロボットなどの移動体の姿勢を示す四元数Ｑを算出してもよい。図３は、本実施の形態２に係る姿勢角演算装置の概略的なシステム構成を示すブロック図である。
【００３３】
四元数計算部１は、例えば、ジャイロセンサ５により検出された角速度と、加速度センサ６により検出された加速度と、算出されフィードバックされた四元数Ｑと、に基づいて、ロボットなどの姿勢を示す四元数Ｑを算出する。
なお、上記ストラップダウン演算方法の詳細は、例えば、特表２００９−５０７２７２号公報などに詳細に記載されており、これを援用することができる。本実施の形態２において、他の構成は上記実施の形態１と略同一であるため、詳細な説明は省略する。
【００３４】
本実施の形態２に係る姿勢角演算装置２０は、例えば、倒立型二輪車に搭載されてもよい。図４は、本実施の形態２に係る姿勢角演算装置を搭載した倒立型二輪車の概略的なシステム構成を示すブロック図である。図５は、本実施の形態２に係る倒立二輪車の概略的な構成を示す図である。
【００３５】
本実施の形態に係る倒立型二輪車１００は、例えば、図５に示すように、左右一対の車輪１０１と、搭乗者が搭乗するステップ１０２と、搭乗者が把持し操作するハンドル１０３と、を備えている。また、本実施の形態に係る倒立型二輪車１００は、例えば、図４に示すように、ジャイロセンサ５と、加速度センサ６と、一対の車輪速度検出器７、８と、演算部９と、姿勢角演算装置２０と、センサ値変換部１１と、ＰＩＤ制御部１２と、旋回指令値生成部１３と、目標車輪速度生成部１４と、一対の駆動アンプ１５、１６と、一対のモータ１７、１８と、を備えている。
【００３６】
ジャイロセンサ５は、例えば、３軸レートジャイロセンサが用いられており、検出した３軸角速度を姿勢角演算装置２０及びセンサ値変換部１１に出力する。加速度センサ６は、例えば、３軸加速度センサであり、検出した３軸加速度を姿勢角演算装置２０に対して出力する。
【００３７】
一対の車輪速度検出器（速度検出器）７、８は、ロータリーエンコーダやレゾルバーなどを用いて、左右一対の車輪の車輪速度を夫々検出し、検出した各車輪速度を演算部９に対して出力する。
【００３８】
演算部９は、各車輪速度検出器７、８から出力された各車輪速度に基づいて、倒立型二輪車の車体加速度、遠心力、及び旋回加速度を算出し、算出した車体加速度、遠心力、及び旋回加速度を姿勢角演算装置２０の四元数計算部１に対して出力する。
【００３９】
四元数計算部１は、ジャイロセンサ５から出力された角速度と、加速度センサ６から出力された加速度と、演算部９から出力された車体加速度、遠心力、及び旋回加速度と、算出されフィードバックされた四元数Ｑと、に基づいて、ストラップダウン演算を行い、倒立型二輪車１００の姿勢を示す四元数Ｑを算出し、ヨー軸成分キャンセル部２に対して出力する。
【００４０】
なお、四元数計算部１は、各車輪速度に基づいた車体加速度、遠心力、及び旋回加速度を用いて、ストラップダウン演算を行うことで、その演算精度を向上させている。
【００４１】
ヨー軸成分キャンセル部２は、四元数計算部１から出力された四元数Ｑから、ヨー軸成分をキャンセルした修正四元数Ｑｍｏｄを算出し、センサ値変換部１１及び姿勢角計算部３に対して出力する。
【００４２】
センサ値変換部１１は、ジャイロセンサ５から出力された角速度と、ヨー軸成分キャンセル部２から出力された修正四元数Ｑｍｏｄと、に基づいて、水平座標系へ各センサ値を変換して、倒立型二輪車１００のピッチ角速度（βの微分値）を算出し、ＰＩＤ制御部１２に対して出力する。
【００４３】
姿勢角計算部３は、ヨー軸成分キャンセル部２から出力された修正四元数Ｑｍｏｄに基づいて、倒立型二輪車１００のロール角度α及びピッチ角度βを算出し、算出したピッチ角度βをＰＩＤ制御部１２に対して出力する。
【００４４】
ＰＩＤ制御部１２は、センサ値変換部１１から出力されたピッチ角速度（βの微分値）と、姿勢角計算部３から出力されたピッチ角度βと、目標ピッチ角度と、に基づいて、ＰＩＤ（Proportinal Integral Differential）制御を行い、目標速度（例えば、左右速度の平均値）を算出し、目標車輪速度生成部１４に対して出力する。
【００４５】
旋回指令値生成部１３は、搭乗者の旋回操作に応じて、その旋回量を示す旋回量指令値を生成し、目標車輪速度生成部１４に対して出力する。
【００４６】
目標車輪速度生成部１４は、ＰＩＤ制御部１２から出力された目標速度と、旋回指令値生成部１３から出力された目標速度と、に基づいて、左右車輪に対する目標車輪速度を算出し、各駆動アンプ１５、１６に対して出力する。
【００４７】
各駆動アンプ１５、１６は、目標車輪速度生成部１４から出力された各目標速度に対応するモータ電流を、夫々各モータ１７、１８に対して出力する。各モータ１７、１８は、各駆動アンプ１５、１６から出力されたモータ電流に応じて、各車輪を回転駆動する。これにより、倒立型二輪車１００は倒立状態を維持しつつ、所望の走行を行うことができる。
【００４８】
以上、本実施の形態２に係る姿勢角演算装置２０において、ストラップダウン演算を用いて算出した四元数Ｑからヨー軸成分をキャンセルした修正四元数Ｑｍｏｄを算出し、該算出した修正四元数Ｑｍｏｄに基づいて上記近似式（１３）を用いてロール角度α及びピッチ角度βを算出する。これにより、姿勢角度を高精度かつ高速に算出することができる。
【００４９】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、例えば、図２に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００５０】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ）を含む。
【００５１】
上記プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００５２】
１ 四元数計算部
２ ヨー軸成分キャンセル部
３ 姿勢角計算部
４ 各種センサ
５ ジャイロセンサ
６ 加速度センサ
７、８ 車輪速度検出器
９ 演算部
１０、２０ 姿勢角演算装置
１１ センサ値変換部
１２ ＰＩＤ制御部
１３ 旋回指令値生成部
１４ 目標車輪速度生成部
１５、１６ 駆動アンプ
１７、１８ モータ
【特許請求の範囲】
【請求項１】
移動体の状態量を検出する状態検出手段と、
前記状態検出手段により検出された前記移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する四元数算出手段と、
前記四元数算出手段により算出された前記四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する修正手段と、
前記修正手段により算出された前記修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する姿勢角計算手段と、
を備える、ことを特徴とする姿勢角演算装置。
β≒−２・ｙ／ｗ、α≒２・ｘ／ｗ
上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。
【請求項２】
請求項１記載の姿勢角演算装置であって、
前記修正手段は、
座標系をｘ軸、ｙ軸、及びｚ軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、
該算出した四元数と、前記四元数算出手段により算出された四元数と、を乗算することで、前記修正四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項３】
請求項１又は２記載の姿勢角演算装置であって、
前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、
前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項４】
請求項１又は２記載の姿勢角演算装置であって、
前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、
前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項５】
請求項１又は２記載の姿勢角演算装置であって、
前記移動体は倒立型移動体であり、
前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサと、前記移動体の加速度を検出する加速度センサと、各車輪の速度検出する速度検出器と、を含み、
前記四元数算出手段は、前記ジャイロセンサにより検出された角速度、前記加速度センサにより検出された加速度、及び前記速度検出器により検出された車輪速度、に基づいた車体加速度、遠心力、及び旋回加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項６】
移動体の状態量を検出するステップと、
前記検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出するステップと、
前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出するステップと、
前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出するステップと、
を含む、ことを特徴とする姿勢角演算方法。
β≒２・ｙ／ｗ、α≒２・ｘ／ｗ
上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。
【請求項７】
請求項６記載の姿勢角演算装置であって、
座標系をｘ軸、ｙ軸、及びｚ軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、
該算出した四元数と、前記算出された四元数と、を乗算することで、前記修正四元数を算出する、ことを特徴とする姿勢角演算方法。
【請求項８】
請求項６又は７記載の姿勢角演算方法であって、
ジャイロセンサにより検出された角速度と、加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算方法。
【請求項９】
検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する処理と、
前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する処理と、
前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する処理と、
をコンピュータに実行させる、ことを特徴とするプログラム。
β≒２・ｙ／ｗ、α≒２・ｘ／ｗ
上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。
【請求項１】
移動体の状態量を検出する状態検出手段と、
前記状態検出手段により検出された前記移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する四元数算出手段と、
前記四元数算出手段により算出された前記四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する修正手段と、
前記修正手段により算出された前記修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する姿勢角計算手段と、
を備える、ことを特徴とする姿勢角演算装置。
β≒−２・ｙ／ｗ、α≒２・ｘ／ｗ
上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。
【請求項２】
請求項１記載の姿勢角演算装置であって、
前記修正手段は、
座標系をｘ軸、ｙ軸、及びｚ軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、
該算出した四元数と、前記四元数算出手段により算出された四元数と、を乗算することで、前記修正四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項３】
請求項１又は２記載の姿勢角演算装置であって、
前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、
前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項４】
請求項１又は２記載の姿勢角演算装置であって、
前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、
前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項５】
請求項１又は２記載の姿勢角演算装置であって、
前記移動体は倒立型移動体であり、
前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサと、前記移動体の加速度を検出する加速度センサと、各車輪の速度検出する速度検出器と、を含み、
前記四元数算出手段は、前記ジャイロセンサにより検出された角速度、前記加速度センサにより検出された加速度、及び前記速度検出器により検出された車輪速度、に基づいた車体加速度、遠心力、及び旋回加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項６】
移動体の状態量を検出するステップと、
前記検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出するステップと、
前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出するステップと、
前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出するステップと、
を含む、ことを特徴とする姿勢角演算方法。
β≒２・ｙ／ｗ、α≒２・ｘ／ｗ
上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。
【請求項７】
請求項６記載の姿勢角演算装置であって、
座標系をｘ軸、ｙ軸、及びｚ軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、
該算出した四元数と、前記算出された四元数と、を乗算することで、前記修正四元数を算出する、ことを特徴とする姿勢角演算方法。
【請求項８】
請求項６又は７記載の姿勢角演算方法であって、
ジャイロセンサにより検出された角速度と、加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算方法。
【請求項９】
検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する処理と、
前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する処理と、
前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する処理と、
をコンピュータに実行させる、ことを特徴とするプログラム。
β≒２・ｙ／ｗ、α≒２・ｘ／ｗ
上記式において、前記修正四元数Ｑｍｏｄ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋとする。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１３−１０４６６５（Ｐ２０１３−１０４６６５Ａ）
【公開日】平成２５年５月３０日（２０１３．５．３０）
【国際特許分類】
【出願番号】特願２０１１−２４６２４３（Ｐ２０１１−２４６２４３）
【出願日】平成２３年１１月１０日（２０１１．１１．１０）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２５年５月３０日（２０１３．５．３０）
【国際特許分類】
【出願日】平成２３年１１月１０日（２０１１．１１．１０）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]