姿勢角演算装置、姿勢角演算方法、及びプログラム

【課題】高速で姿勢角度を算出すること。
【解決手段】姿勢角演算装置は、移動体の状態量を検出する状態検出手段と、状態検出手段により検出された移動体の状態量に基づいて、移動体の姿勢を示す四元数を算出する四元数算出手段と、四元数算出手段により算出された四元数におけるヨー軸成分をキャンセルして四元数の修正を行った修正四元数(Qmod=w+xi+yj+zk)を算出する修正手段と、修正手段により算出された修正四元数に基づいて、近似式を用いて移動体のロール角α及びピッチ角βを高速に算出する姿勢角計算手段と、を備えている。

【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ロボットなどの移動体の姿勢角度を高速に算出できる姿勢角演算装置、姿勢角演算方法、及びプログラムに関するものである。
【背景技術】
【0002】
近年、スカラー部分と複素数ベクトル部分とから成る四元数(クォータニオン)を用いて、ロボットなどの移動体の姿勢角度を算出する技術が知られている(例えば、特許文献1参照)。この技術により、ロール角、ピッチ角、及びヨー角の姿勢角度において生じるジンバルロックを抑制することができる。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特表2003−521697号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、上記特許文献1においては、四元数に基づいて、三角関数を用いて姿勢角度を算出している。このため、その計算に多くの時間を要することとなる。
【0005】
本発明は、このような問題点を解決するためになされたものであり、高速で姿勢角度を算出できる姿勢角演算装置、姿勢角演算方法、及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【0006】
上記目的を達成するための本発明の一態様は、移動体の状態量を検出する状態検出手段と、前記状態検出手段により検出された前記移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する四元数算出手段と、前記四元数算出手段により算出された前記四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する修正手段と、前記修正手段により算出された前記修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する姿勢角計算手段と、
を備える、ことを特徴とする姿勢角演算装置である。β≒2・y/w、α≒2・x/w、上記式において、前記修正四元数Qmod=w+xi+yj+zkとする。なお、上記近似式は、右手座標系に関するものであるが、左手座標系に関しても右手座標系と同様に適用可能である。
この一態様において、前記修正手段は、座標系をx軸、y軸、及びz軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、該算出した四元数と、前記四元数算出手段により算出された四元数と、を乗算することで、前記修正四元数を算出してもよい。
この一態様において、前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出してもよい。
この一態様において、前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出してもよい。
この一態様において、前記移動体は倒立型移動体であり、前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサと、前記移動体の加速度を検出する加速度センサと、各車輪の速度検出する速度検出器と、を含み、前記四元数算出手段は、前記ジャイロセンサにより検出された角速度、前記加速度センサにより検出された加速度、及び前記速度検出器により検出された車輪速度、に基づいた車体加速度、遠心力、及び旋回加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出してもよい。
他方、上記目的を達成するための本発明の一態様は、移動体の状態量を検出するステップと、前記検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出するステップと、前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出するステップと、前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出するステップと、を含む、ことを特徴とする姿勢角演算方法であってもよい。β≒2・y/w、α≒2・x/w、上記式において、前記修正四元数Qmod=w+xi+yj+zkとする。なお、上記近似式は、右手座標系に関するものであるが、左手座標系に関しても右手座標系と同様に適用可能である。
この一態様において、座標系をx軸、y軸、及びz軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、該算出した四元数と、前記算出された四元数と、を乗算することで、前記修正四元数を算出してもよい。
この一態様において、ジャイロセンサにより検出された角速度と、加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出してもよい。
さらに、上記目的を達成するための本発明の一態様は、検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する処理と、前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する処理と、前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する処理と、をコンピュータに実行させる、ことを特徴とするプログラムであってもよい。β≒2・y/w、α≒2・x/w、上記式において、前記修正四元数Qmod=w+xi+yj+zkとする。なお、上記近似式は、右手座標系に関するものであるが、左手座標系に関しても右手座標系と同様に適用可能である。
【発明の効果】
【0007】
本発明によれば、高速で姿勢角度を算出できる姿勢角演算装置、姿勢角演算方法、及びプログラムを提供することができる。
【図面の簡単な説明】
【0008】
【図1】本発明の実施の形態1に係る姿勢角演算装置の概略的な構成を示すブロック図である。
【図2】本発明の実施の形態1に係るヨー軸成分キャンセル部による演算処理の一例を示すフローチャートである。
【図3】本発明の実施の形態2に係る姿勢角演算装置の概略的なシステム構成を示すブロック図である。
【図4】本発明の実施の形態2に係る姿勢角演算装置を搭載した倒立型二輪車の概略的なシステム構成を示すブロック図である。
【図5】本発明の実施の形態2に係る倒立二輪車の概略的な構成を示す図である。
【発明を実施するための形態】
【0009】
実施の形態1.
以下、図面を参照して本発明の実施の形態について説明する。図1は、本発明の実施の形態1に係る姿勢角演算装置の概略的な構成を示すブロック図である。本実施の形態1に係る姿勢角演算装置10は、四元数計算部1と、ヨー軸成分キャンセル部2と、姿勢角計算部3と、センサ4と、を備えている。
【0010】
なお、姿勢角演算装置10は、例えば、演算処理等と行うCPU(Central Processing Unit)と、CPUによって実行される演算プログラム等が記憶されたROM(Read Only Memory)と、処理データ等を一時的に記憶するRAM(Random Access Memory)と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらCPU、ROM、及びRAMは、データバス等によって相互に接続されている。
【0011】
四元数計算部1は、四元数算出手段の一具体例であり、例えば、ロボットや倒立二輪車などの移動体に搭載された各種センサ4から出力されるセンサ値に基づいて、ロボットなどの移動体の姿勢を示す四元数Q(Quaternion)を算出する。ここで、四元数とは、一つの実部と3つの虚数単位からなる虚部を持つ、複素数の一種である。3つの虚数単位をi、j、kとしたとき、任意の四元数Qは実数w、x、y、zを用いて、下記(1)式のように表される。
【0012】
四元数計算部1は、例えば、ジャイロセンサにより検出された角速度と、加速度センサにより検出された加速度と、に基づいて、下記(1)式を用いて、ロボットなどの姿勢を示す四元数Qを算出する。
Q=w+xi+yj+zk (1)式
【0013】
ここで、右手座標系において、基準座標系から見てx軸周りにα[rad]だけロール回転させる。また、その状態から基準座標系から見てy軸周りにβ[rad]だけピッチ回転させる。さらに、その状態から基準座標系から見てz軸周りにγ[rad]だけヨー回転させると、その回転姿勢を表わす姿勢行列(回転行列)は、下記(2)式のように表わすことができる。なお、以下、右手座標系を用いた一例を説明するが、左手座標系に関しても右手座標系と同様の方法を適用することができる。
【数1】

【0014】
また、四元数Qを上記(1)式のように表わすと、上記姿勢行列Rは、w、x、y、zを用いて、下記(3)式のように表わすことができる。
【数2】

四元数計算部1にはヨー軸成分キャンセル部2が接続されており、四元数計算部1は算出した四元数Qをヨー軸成分キャンセル部2に対して出力する。
【0015】
ヨー軸成分キャンセル部2は、修正手段の一具体例であり、四元数計算部1により算出された四元数Qから、ヨー軸成分をキャンセルした修正四元数Qmodを算出する。これにより、四元数Qの要素の一つであるヨー軸成分をキャンセルすることで、計算量を大幅に低減し、姿勢角度の演算速度を高速化することができる。
【0016】
ここで、修正四元数Qmodの算出方法について、詳細に説明する。図2は、本実施の形態1に係るヨー軸成分キャンセル部による演算処理の一例を示すフローチャートである。
まず、ヨー軸成分キャンセル部2は、下記(4)式を用いてsinβを算出する(ステップS101)。
sinβ=−2・(x・z−w・y) (4)式
【0017】
次に、ヨー軸成分キャンセル部2は、上記算出したsinβに基づいて、sinβ>0.9999又はsinβ<−0.9999を満たし、ジンバルロック状態であるか否かを判断する(ステップS102)。
【0018】
ヨー軸成分キャンセル部2は、ジンバルロック状態であると判断したとき(sinβの絶対値が1に近いとき)(ステップS102のYES)、下記(5)式を用いて、sinγ及びcosγを算出する(ステップS103)。
sinγ=2・(w・z−x・y) (5)式
cosγ=1−2・x−2・z
なお、上記(2)式においてα=0とした姿勢行列Rと、上記(3)式に示す姿勢行列Rと、を比較することで、上記(5)式を求めることができる。
【0019】
一方、ヨー軸成分キャンセル部2は、ジンバルロック状態でないと判断したとき(sinβの絶対値が1に近くないとき)(ステップS102のNO)、下記(6)式を用いて、sinγcosβ、cosγcosβを算出する(ステップS104)。
sinγcosβ=2・(x・y+w・z) (6)式
cosγcosβ=1−2.y−2・z
【0020】
なお、上記(2)式に示す姿勢行列Rと上記(3)式に示す姿勢行列Rとを比較することで、上記(4)式及び(6)式を求めることができる。
【0021】
次に、ヨー軸成分キャンセル部2は、上記(4)式により算出したsinβに基づいて、下記(7)式を用いて、cosβを算出する(ステップS105)。
cosβ=sqrt(1−sinβ) (7)式
【0022】
ヨー軸成分キャンセル部2は、上記(6)式及び(7)式の演算結果に基づいて、下記(8)式を用いて、sinγ、cosγを算出する(ステップS106)。
sinγ=sinγcosβ/cosβ (8)式
cosγ=cosγcosβ/cosβ
【0023】
ヨー軸成分キャンセル部2は、上記(5)式又は(8)式の演算結果に基づいて、下記(9)式を用いて、sin(γ/2)、cos(γ/2)を算出する(ステップS107)。
sin(γ/2)=sqrt((1−cosγ)/2) (9)式
cos(γ/2)=sqrt((1+cosγ)/2)
なお、ヨー軸成分キャンセル部2は、sinγ<0であると判断したとき、sin(γ/2)=−sin(γ/2)とする。
【0024】
ヨー軸成分キャンセル部2は、上記(9)式の演算結果に基づいて、下記(10)式を用いて、ヨー軸成分をキャンセルするための四元数を算出する(ステップS108)。
Qyaw.w=cos(γ/2)
Qyaw.x=0 (10)式
Qyaw.y=0
Qyaw.z=−sin(γ/2)
【0025】
ヨー軸成分キャンセル部2は、上記(10)式に示す四元数をキャンセルする四元数Qyawと、四元数計算部1により算出された四元数Qと、を、下記(11)式を用いて乗算し、ヨー軸成分がキャンセルされた修正四元数Qmodを算出する(ステップS109)。
Qmod=Qyaw・Q (11)式
【0026】
ここで、上述のようにヨー軸成分をキャンセルし0に拘束して、Qmod=w+xi+yj+zkとすると、下記(12)式が成立する。
w=cos(α/2)cos(β/2)
x=sin(α/2)cos(β/2) (12)式
y=cos(α/2)sin(β/2)
z=−sin(α/2)sin(β/2)
【0027】
さらに、ロール角度α、及びピッチ角度βを大きくない角度と仮定すると、下記(13)式に示す近似式を用いて、ロール角度α及びピッチ角度βを算出することができる。
β≒2tan(β/2)=2・sin(β/2)/cos(β/2)=2・y/w
α≒2tan(α/2)=2・sin(α/2)/cos(α/2)=2・x/w (13)式
なお、上記(13)式は、右手座標系を適用した場合の式であるが、左手座標系を適用した場合の近似式も、右手座標系と同様に算出することができる。また、上記(13)式に示す近似式を用いた場合、例えば、姿勢角度が20[deg]のとき、約1%の誤差が生じ、姿勢角度が30[deg]のとき、約2.3%の誤差が生じる。一方で、姿勢角度に対して、一般的に求められる精度は、1〜2%程度の誤差であるため、本実施の形態に係る上記近似式(13)は、この精度を満たすこととなる。
ヨー軸成分キャンセル部2には姿勢角計算部3が接続されており、ヨー軸成分キャンセル部2は、算出した修正四元数Qmodを姿勢角計算部3に対して出力する。
【0028】
姿勢角計算部3は、姿勢角計算手段の一具体例であり、ヨー軸成分キャンセル部2により算出された修正四元数Qmodに基づいて求めた上記(13)式を用いて、三角関数を用いることなく、ロール角度α及びピッチ角度βを算出することができる。このように三角関数を用いることなく姿勢角度を算出することで、計算量を大幅に低減できる。
【0029】
なお、ヨー軸成分キャンセル部2により算出された修正四元数Qmodのw成分が0の場合(又は0に極めて近い場合)は、上記(13)式を用いて、ロール角度α及びピッチ角度βを算出することができない。したがって、wが0に極めて近い値の場合(w≒0)は、例えば、下記(14)式のように、ロール軸をπに固定してピッチ角度βのみを算出することができる。
β≒−2・z/x (14)式
α=0
ここで、cosα/2=0、sinα/2=1に基づいて、w=0、x=cos(β/2)、y=0、z=−sin(β/2)が成立し、上記(14)式が導出される。
【0030】
以上により、姿勢角計算部3は、修正四元数Qmodのw成分が0に極めて近い(w≒0)と判断したときは、上記(14)式を用いて、ロール角度α及びピッチ角度βを算出する。一方、姿勢角計算部3は、修正四元数Qmodのw成分が0に近くないと判断したときは、上記(13)式を用いて、ロール角度α及びピッチ角度βを算出する。
【0031】
以上、本実施の形態1に係る姿勢角演算装置10において、四元数Qからヨー軸成分をキャンセルした修正四元数Qmodを算出し、該算出した修正四元数Qmodに基づいて上記近似式(13)を用いてロール角度α及びピッチ角度βを算出する。これにより、計算量を大幅に低減し、姿勢角度を高速に算出することができる。
【0032】
実施の形態2.
本発明の実施の形態2に係る姿勢角演算装置20において、四元数計算部1は、ストラップダウン演算を用いて、ロボットなどの移動体の姿勢を示す四元数Qを算出してもよい。図3は、本実施の形態2に係る姿勢角演算装置の概略的なシステム構成を示すブロック図である。
【0033】
四元数計算部1は、例えば、ジャイロセンサ5により検出された角速度と、加速度センサ6により検出された加速度と、算出されフィードバックされた四元数Qと、に基づいて、ロボットなどの姿勢を示す四元数Qを算出する。
なお、上記ストラップダウン演算方法の詳細は、例えば、特表2009−507272号公報などに詳細に記載されており、これを援用することができる。本実施の形態2において、他の構成は上記実施の形態1と略同一であるため、詳細な説明は省略する。
【0034】
本実施の形態2に係る姿勢角演算装置20は、例えば、倒立型二輪車に搭載されてもよい。図4は、本実施の形態2に係る姿勢角演算装置を搭載した倒立型二輪車の概略的なシステム構成を示すブロック図である。図5は、本実施の形態2に係る倒立二輪車の概略的な構成を示す図である。
【0035】
本実施の形態に係る倒立型二輪車100は、例えば、図5に示すように、左右一対の車輪101と、搭乗者が搭乗するステップ102と、搭乗者が把持し操作するハンドル103と、を備えている。また、本実施の形態に係る倒立型二輪車100は、例えば、図4に示すように、ジャイロセンサ5と、加速度センサ6と、一対の車輪速度検出器7、8と、演算部9と、姿勢角演算装置20と、センサ値変換部11と、PID制御部12と、旋回指令値生成部13と、目標車輪速度生成部14と、一対の駆動アンプ15、16と、一対のモータ17、18と、を備えている。
【0036】
ジャイロセンサ5は、例えば、3軸レートジャイロセンサが用いられており、検出した3軸角速度を姿勢角演算装置20及びセンサ値変換部11に出力する。加速度センサ6は、例えば、3軸加速度センサであり、検出した3軸加速度を姿勢角演算装置20に対して出力する。
【0037】
一対の車輪速度検出器(速度検出器)7、8は、ロータリーエンコーダやレゾルバーなどを用いて、左右一対の車輪の車輪速度を夫々検出し、検出した各車輪速度を演算部9に対して出力する。
【0038】
演算部9は、各車輪速度検出器7、8から出力された各車輪速度に基づいて、倒立型二輪車の車体加速度、遠心力、及び旋回加速度を算出し、算出した車体加速度、遠心力、及び旋回加速度を姿勢角演算装置20の四元数計算部1に対して出力する。
【0039】
四元数計算部1は、ジャイロセンサ5から出力された角速度と、加速度センサ6から出力された加速度と、演算部9から出力された車体加速度、遠心力、及び旋回加速度と、算出されフィードバックされた四元数Qと、に基づいて、ストラップダウン演算を行い、倒立型二輪車100の姿勢を示す四元数Qを算出し、ヨー軸成分キャンセル部2に対して出力する。
【0040】
なお、四元数計算部1は、各車輪速度に基づいた車体加速度、遠心力、及び旋回加速度を用いて、ストラップダウン演算を行うことで、その演算精度を向上させている。
【0041】
ヨー軸成分キャンセル部2は、四元数計算部1から出力された四元数Qから、ヨー軸成分をキャンセルした修正四元数Qmodを算出し、センサ値変換部11及び姿勢角計算部3に対して出力する。
【0042】
センサ値変換部11は、ジャイロセンサ5から出力された角速度と、ヨー軸成分キャンセル部2から出力された修正四元数Qmodと、に基づいて、水平座標系へ各センサ値を変換して、倒立型二輪車100のピッチ角速度(βの微分値)を算出し、PID制御部12に対して出力する。
【0043】
姿勢角計算部3は、ヨー軸成分キャンセル部2から出力された修正四元数Qmodに基づいて、倒立型二輪車100のロール角度α及びピッチ角度βを算出し、算出したピッチ角度βをPID制御部12に対して出力する。
【0044】
PID制御部12は、センサ値変換部11から出力されたピッチ角速度(βの微分値)と、姿勢角計算部3から出力されたピッチ角度βと、目標ピッチ角度と、に基づいて、PID(Proportinal Integral Differential)制御を行い、目標速度(例えば、左右速度の平均値)を算出し、目標車輪速度生成部14に対して出力する。
【0045】
旋回指令値生成部13は、搭乗者の旋回操作に応じて、その旋回量を示す旋回量指令値を生成し、目標車輪速度生成部14に対して出力する。
【0046】
目標車輪速度生成部14は、PID制御部12から出力された目標速度と、旋回指令値生成部13から出力された目標速度と、に基づいて、左右車輪に対する目標車輪速度を算出し、各駆動アンプ15、16に対して出力する。
【0047】
各駆動アンプ15、16は、目標車輪速度生成部14から出力された各目標速度に対応するモータ電流を、夫々各モータ17、18に対して出力する。各モータ17、18は、各駆動アンプ15、16から出力されたモータ電流に応じて、各車輪を回転駆動する。これにより、倒立型二輪車100は倒立状態を維持しつつ、所望の走行を行うことができる。
【0048】
以上、本実施の形態2に係る姿勢角演算装置20において、ストラップダウン演算を用いて算出した四元数Qからヨー軸成分をキャンセルした修正四元数Qmodを算出し、該算出した修正四元数Qmodに基づいて上記近似式(13)を用いてロール角度α及びピッチ角度βを算出する。これにより、姿勢角度を高精度かつ高速に算出することができる。
【0049】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、例えば、図2に示す処理を、CPUにコンピュータプログラムを実行させることにより実現することも可能である。
【0050】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体(non-transitory computer readable medium)を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体(tangible storage medium)を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体(例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ)、光磁気記録媒体(例えば光磁気ディスク)、CD−ROM、CD−R、CD−R/W、半導体メモリ(例えば、マスクROM、PROM(Programmable ROM)、EPROM(Erasable PROM)、フラッシュROM、RAM)を含む。
【0051】
上記プログラムは、様々なタイプの一時的なコンピュータ可読媒体(transitory computer readable medium)によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【0052】
1 四元数計算部
2 ヨー軸成分キャンセル部
3 姿勢角計算部
4 各種センサ
5 ジャイロセンサ
6 加速度センサ
7、8 車輪速度検出器
9 演算部
10、20 姿勢角演算装置
11 センサ値変換部
12 PID制御部
13 旋回指令値生成部
14 目標車輪速度生成部
15、16 駆動アンプ
17、18 モータ

【特許請求の範囲】
【請求項1】
移動体の状態量を検出する状態検出手段と、
前記状態検出手段により検出された前記移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する四元数算出手段と、
前記四元数算出手段により算出された前記四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する修正手段と、
前記修正手段により算出された前記修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する姿勢角計算手段と、
を備える、ことを特徴とする姿勢角演算装置。
β≒−2・y/w、α≒2・x/w
上記式において、前記修正四元数Qmod=w+xi+yj+zkとする。
【請求項2】
請求項1記載の姿勢角演算装置であって、
前記修正手段は、
座標系をx軸、y軸、及びz軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、
該算出した四元数と、前記四元数算出手段により算出された四元数と、を乗算することで、前記修正四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項3】
請求項1又は2記載の姿勢角演算装置であって、
前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、
前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項4】
請求項1又は2記載の姿勢角演算装置であって、
前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサ、及び前記移動体の加速度を検出する加速度センサを含み、
前記四元数算出手段は、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項5】
請求項1又は2記載の姿勢角演算装置であって、
前記移動体は倒立型移動体であり、
前記状態検出手段は、前記移動体の角速度を検出するジャイロセンサと、前記移動体の加速度を検出する加速度センサと、各車輪の速度検出する速度検出器と、を含み、
前記四元数算出手段は、前記ジャイロセンサにより検出された角速度、前記加速度センサにより検出された加速度、及び前記速度検出器により検出された車輪速度、に基づいた車体加速度、遠心力、及び旋回加速度と、算出されフィードバックされた前記四元数と、に基づいて、ストラップダウン演算を用いて、前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算装置。
【請求項6】
移動体の状態量を検出するステップと、
前記検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出するステップと、
前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出するステップと、
前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出するステップと、
を含む、ことを特徴とする姿勢角演算方法。
β≒2・y/w、α≒2・x/w
上記式において、前記修正四元数Qmod=w+xi+yj+zkとする。
【請求項7】
請求項6記載の姿勢角演算装置であって、
座標系をx軸、y軸、及びz軸周りに夫々回転させたときの姿勢行列であって、ジンバルロック状態のときの前記姿勢行列及び前記ジンバルロック状態でないときの前記姿勢行列と、前記姿勢を示す四元数の姿勢行列と、を夫々比較してヨー軸成分をキャンセルするための四元数を算出し、
該算出した四元数と、前記算出された四元数と、を乗算することで、前記修正四元数を算出する、ことを特徴とする姿勢角演算方法。
【請求項8】
請求項6又は7記載の姿勢角演算方法であって、
ジャイロセンサにより検出された角速度と、加速度センサにより検出された加速度と、に基づいて前記移動体の姿勢を示す四元数を算出する、ことを特徴とする姿勢角演算方法。
【請求項9】
検出された移動体の状態量に基づいて、前記移動体の姿勢を示す四元数を算出する処理と、
前記算出された四元数におけるヨー軸成分をキャンセルして前記四元数の修正を行った修正四元数を算出する処理と、
前記算出された修正四元数に基づいて、下記近似式を用いて前記移動体のロール角α及びピッチ角βを算出する処理と、
をコンピュータに実行させる、ことを特徴とするプログラム。
β≒2・y/w、α≒2・x/w
上記式において、前記修正四元数Qmod=w+xi+yj+zkとする。

【図1】
image rotate

【図2】
image rotate

【図3】
image rotate

【図4】
image rotate

【図5】
image rotate


【公開番号】特開2013−104665(P2013−104665A)
【公開日】平成25年5月30日(2013.5.30)
【国際特許分類】
【出願番号】特願2011−246243(P2011−246243)
【出願日】平成23年11月10日(2011.11.10)
【出願人】(000003207)トヨタ自動車株式会社 (59,920)
【Fターム(参考)】