JP4287975B2 - 姿勢計測装置 - Google Patents

Description

【０００１】
【発明の属する技術分野】
この発明は移動体の角速度をジャイロで検出し、そのジャイロの出力を用いて姿勢基準計算を行い、その計算結果から移動体の姿勢角を計測する姿勢計測装置に関する。
【０００２】
【従来の技術】
図３に従来の姿勢計測装置を示す。この種の装置は例えば特開平８−２１７４０号公報に詳細に説明されているから、以下では簡単に述べる。ジャイロ１１から移動体の各軸まわりの角速度がジャイロデータとして出力され、このジャイロデータはジャイロデータ補正計算部１２で、ジャイロ１１のバイアス誤差、ミスアライメント誤差、スケールファクタの誤差が補正されて、姿勢基準計算、例えば方向余弦行列計算部１３へ供給される。
【０００３】
一方、移動体の各軸方向の加速度が加速度計１４で検出され、加速度計データとして加速度計補正計算部１５に入力され、加速度計自体の誤差が補正されて、座標変換計算部１６に入力され、方向余弦行列を用いて、機軸加速度が基準座標軸加速度に座標変換される。この基準座標軸加速度から速度計算部１７で各軸の速度が計算され、更に位置計算部１８で各軸上の位置が計算される。速度計算部１７よりの速度と位置計算部１８よりの位置とが航法座標軸回転量計算部１９に入力され、航法座標軸回転量計算部１９で移動体の移動（トランスポートレート）と地球自転（アースレート）に基づくジャイロデータへの影響（航法座標軸回転量）が計算され、これらの影響が方向余弦行列計算部１３においてジャイロデータから除去される。また前記航法座標軸回転量と、速度計算部１７の速度とがコリオリ加速度計算部２１に入力され、座標軸回転により生じるコリオリ加速度が計算され、これにより速度計算部１７に対する補正が行われる。
【０００４】
初期アライメント計算部２２でジャイロ１１、加速度計１４が計測する地球自転角速度、地球重力データに基づいて方向余弦行列初期値、速度初期値が計算され、それぞれ方向余弦行列計算部１３、速度計算部１７に設定される。
方向余弦行列計算部１３においては、時刻ｎにおける方向余弦行列Ｃ_n が、時刻ｎ−１での方向余弦行列Ｃ_n-1 に対し、次式に示す方向余弦行列増分ΔＣ_n を掛算して計算される。
【０００５】
Ｃ_n ＝Ｃ_n-1 ・ΔＣ_n …（１）
ΔＣ_n ＝I+((sin Φ）／Φ）［Φ_m ×］+((1−cos Φ）／Φ² )［Φ_m ×］² …（２）
Ｉは単位行列、
Φ_m ＝［Φ_x Φ_y Φ_z ］^T …（３）
Φ_i はｉ軸（ｉ＝ｘ，ｙ，ｚ）ジャイロデータ、［ ］^T は行列の転置、
【０００６】
【数１】

【０００７】
方向余弦行列Ｃ_n の計算結果が得られるごとに、そのＣ_n の行列の要素を用いて姿勢角計算部２３で次の計算をして出力する。
ロール角＝tan ^-1（Ｃ₃₂／Ｃ₃₃）
ピッチ角＝tan ^-1（Ｃ₃₁／√（１−Ｃ₃₁ ² ））又はsin ^-1（−Ｃ₃₁）
方位角＝tan ^-1（Ｃ₂₁／Ｃ₁₁）
【０００８】
【発明が解決しようとする課題】
方向余弦行列計算部１３において方向余弦行列増分ΔＣ_n の計算におけるsin ，cos を含む計算法を、下記に示すように比較的高次のテーラー展開で展開した近似式により行っている。
（sin Φ）／Φ≒１−（Φ／３！）² ＋（Φ／５！）⁴ …（６）
（１−cos Φ）／Φ² ≒（１／２！）−（Φ² ／４！） …（７）
方向余弦行列は積分系であるため、近似計算に基づく計算誤差の増大をおさえるように、従来においては比較的高次の近似式を用いていた。よって計算量が多くなり、比較的高い精度で姿勢角及び方位角を高速に計算することが困難であった。
【０００９】
【課題を解決するための手段】
この発明によれば、第１演算部において比較的次数が大きい近似式を用いて姿勢基準、例えば方向余弦行列が比較的遅い速度で計算され、第２演算部において、第１演算部よりも次数が小さい近似式を用いて姿勢基準が第１演算部より高速に計算され、その姿勢基準にもとづき移動体の姿勢角が計算され、演算誤差補正部により、第１演算部で姿勢基準計算の結果が得られるごとに、その結果が第２演算部での姿勢基準計算ごとの初期値として設定される。その第２演算部で姿勢基準計算結果が得られるごとにそれを用いて移動体の姿勢角を計算する。
【００１０】
特に、第１演算部にジャイロ出力が入力されてから、その姿勢基準の計算結果が得られ、初期値設定するまでに、第２演算部での姿勢基準計算結果の各前回の計算結果に対する変動分の総てを求め、この総ての変動分で前回の第１演算部の姿勢基準計算結果に補正演算した値が第２演算部の各姿勢基準の初期値として用いられる。
【００１１】
【発明の実施の形態】
図１にこの発明の実施例を示し、図３と対応する部分に同一番号を付けてある。ジャイロデータ補正計算部１２、方向余弦行列計算部１３、加速度計補正計算部１５、座標変換計算部１６、速度計算部１７、位置計算部１８、航法座標軸回転量計算部１９、コリオリ加速度計算部２１により第１演算部３１が構成される。第１演算部３１は少なくともジャイロ１１よりのジャイロデータが入力され、比較的高次の近似式により高い精度で方向余弦行列、つまり姿勢基準を計算する。
【００１２】
この発明では第２演算部３２が設けられ、ジャイロ１１からジャイロデータが入力され、第１演算部３１で行う方向余弦行列計算よりも、次数が小さい近似式により第２方向余弦行列の計算が、第２方向余弦行列計算部３３で第１演算部３１の方向余弦行列計算の繰返しよりも速く繰返される。
更に演算誤差補正部３４が設けられ、第１演算部３１の方向余弦行列計算部１３にジャイロデータが入力されるごとに、その入力から、その方向余弦行列計算部１３による方向余弦行列計算結果が得られるまでに、第２方向余弦行列計算部３３で計算された方向余弦行列の変動分の総てに対応した値が変動分検出部３５で計算され、方向余弦行列計算部１３で計算された方向余弦行列が、変動分検出部３５からの変動分の総てに対応した値で補正演算部３６において補正演算され、その演算結果が第２方向余弦行列計算部３３の第２方向余弦行列計算ごとの初期値として設定される。第２方向余弦行列計算部３３で計算された第２方向余弦行列を用いて姿勢角計算部２３により姿勢角が計算される。
【００１３】
例えば図２に示すように、方向余弦行列計算部１３では周期Ｔ₁ ごとに方向余弦行列の計算を、例えば従来の技術の項で説明したように、式（１）を、式（６）及び式（７）の近似式を用いて計算する。一方第２方向余弦行列計算部３３では周期Ｔ₁ ／４ごとに、方向余弦行列の計算を次式により行う。
Ｂ_m ＝Ｂ_m-1 ・ΔＢ_m …（８）
ΔＢ_m ＝Ｉ＋［Φ_m ×］＋（１／２）・［Φ_m ×］² …（９）
Ｂ_m は時刻ｍでの第２方向余弦行列
Ｂ_m-1 は時刻ｍ-１ での第２方向余弦行列
ΔＢ_m は第２方向余弦行列の変化量
Φ_m は式（３）と、［Φ_m ×］は式（５）と同一である。つまり
（sin Φ）／Φ≒１ …（10）
（１−cos Φ）／Φ²≒１／２！ …（11）
なるテーラー展開の各１次の近次式を用いて式（８）を計算する。このように次数が小さい近次式を用いるため、第２方向余弦行列Ｂ_m の計算量が少なく、短時間でＢ_m を計算することができる。
【００１４】
図２では方向余弦行列計算部１３の計算と第２方向余弦行列計算部３３の計算とを共通のＣＰＵ（中央処理ユニット）で行うようにさせ、同時にジャイロデータが入力された場合は、第２方向余弦行列計算部３３の計算を優先させた場合である。図中の各斜線を施している部分が、それぞれ計算を行っている期間を示す。方向余弦行列計算部１３と第２方向余弦行列計算部３３への各ジャイロデータの入力は同期しているが、前者の入力周期はＴ₁ であり後者の入力周期はＴ₁ ／４である。従って例えば時刻ｎにジャイロデータが両計算部１３と３３に同時に入力され、第２方向余弦行列計算部３３はそのＴ₁ ／４前に計算された第２方向余弦行列Ｂ０＝Ｂ４を初期値として増分ΔＢ１が掛算され、第２方向余弦行列Ｂ１＝Ｂ０・ΔＢ１が計算される。時刻ｎ＝ｎ₀ からＴ₁ ／４経過したサブ時刻ｎ₁ においてもＢ１を初期値として増分ΔＢ２が掛算され、第２方向余弦行列Ｂ２＝Ｂ１・ΔＢ２（＝Ｂ０・ΔＢ１・ΔＢ２）が計算される。この第２方向余弦行列の前回の計算値に対する変動分ΔＢ２が変動分検出部３５で計算される。
【００１５】
次のサブ時刻ｎ₂ において第２方向余弦行列が計算されるが、サブ時刻ｎ₂ の前に、方向余弦行列計算部１３で、時刻ｎに入力されたジャイロデータに対する方向余弦行列の計算結果Ｃ_n ＝Ｃ_n-1 ・ΔＣ_n が得られているから、その演算結果Ｃ_nが、変動分検出部３５で求めた、時刻ｎからｎ₁ までの変動分の総て、この例ではΔＢ２で補正演算部３６において補正演算され、この演算結果値Ｃ_n・ΔＢ２が第２方向余弦行列計算部３３にその計算の初期値として設定される。つまり第２方向余弦行列計算部３３で計算された第２方向余弦行列Ｂ２はＣ_n・ΔＢ２に補正される。このようにして時刻ｎに入力されたデータに基づく精度の高い演算結果Ｃ_n が、時刻ｎに入力されたデータに基づき計算された第２方向余弦行列Ｂ１に置きかえられたことになる。
【００１６】
よってサブ時刻ｎ₂ における第２方向余弦行列計算は補正されたＢ２を初期値としてＢ３＝Ｂ２・ΔＢ３（＝Ｃ_n ・ΔＢ２・ΔＢ３）が計算される。このようにして第２方向余弦行列計算部３３は周期Ｔ₁ ／４ごとに次数の小さい近似式で第２方向余弦行列計算を行うが、その計算の初期値が、周期Ｔ₁ ごとに方向余弦行列計算部１３で次数の高い近似式で計算された精度の高い方向余弦行列によって補正されるため、周期Ｔ₁ ／４ごとに比較的高い精度の姿勢角を得ることができる。ちなみに、４００°／ｓで１軸回転しているときの１／（２００Ｈｚ）間の回転軸の角度誤差は下記の通りである。
【００１７】

上記条件は通常はない大きな値であるが、近似次数と誤差はこのような関係にあり、例えば方向余弦行列Ｃ_n の計算に４次の近似式を用いて誤差は、周期が１／（５０Ｈｚ）であれば、１０^-8度のオーダという高い精度の値が得られ、第２方向余弦行列Ｂ_m の計算に１次の近似式を用いてもサブ時刻周期を１／（２００Ｈｚ）とした場合誤差が１０^-4度のオーダの精度となるが、この誤差が積分されるのは、方向余弦行列Ｃ_n の計算周期の間だけで、Ｃ_n が得られるごとにＢ_m の計算の初期値がＣ_n により補正され、結果として常に比較的高い精度の姿勢角が高速（短かい周期）で得られる。
【００１８】
なお姿勢角の計算は第２方向余弦行列
【００１９】
【数２】

【００２０】
に対し、ロール角＝tan ^-1（Ｂ₃₂／Ｂ₃₃）
ピッチ角＝tan ^-1（Ｂ₃₁／√（１−Ｂ₃₁ ² ））又はsin ^-1（−Ｂ₃₁）
方位角＝tan ^-1（Ｂ₂₁／Ｂ₁₁）
で行うことは従来と同様である。
方向余弦行列Ｃ_n の計算周期は例えば１／（５０Ｈｚ）、第２方向余弦行列Ｂ_m の計算周期は例えば１／（２００Ｈｚ）が考えられるが、これらの値は任意に選ぶことができ、少くとも、方向余弦行列Ｃ_n の計算周期に対し、第２方向余弦行列Ｂ_m の計算周期は１／２以下であればよい。方向余弦行列Ｃ_n の計算周期に対し、第２方向余弦行列Ｂ_m の計算周期は整数分の１が好ましい。かつ方向余弦行列計算部１３はジャイロデータを取込む際に、第２方向余弦行列計算部３３にジャイロデータを取込むような周期関係をもたせると同時にジャイロデータが入力されることが好ましいが、必ずしもその必要はない。方向余弦行列Ｃ_n が得られた時は、その計算のためのジャイロデータを取込んだ時刻に近い、サブ時刻での第２方向余弦行列Ｂ_m をＣ_n で置きかえたと等価になるように補正すればよい。また方向余弦行列の計算結果Ｃ_n が得られると直に第２方向余弦行列計算部３３の初期値を設定する必要もない。つまり方向余弦行列Ｃ_n を計算するためにジャイロデータを取込んだ時刻から、その計算結果Ｃ_n が得られた後の適当な時刻（サブ時刻）までの第２方向余弦行列Ｂ_m の変動分の総てを用いてＣ_n に対し補正演算してその演算結果を、次の第２方向余弦行列Ｂ_m の計算の初期値（直前に得られた第２方向余弦行列Ｂ_m-1 ）とすればよい。
【００２１】
方向余弦行列Ｃ_n の計算、第２方向余弦行列Ｂ_m の計算にそれぞれ用いる近似式は前記例に限らず前者に次数が３〜６次の高次のテーラー展開式を用い、後者は次数が１〜２次の低次のテーラー展開式を用いることができるが、前者の次数より後者の次数は必ず低くすればよい。近似式もテーラー展開式に限らず、他の近似式を用いてもよい。
ジャイロデータ補正計算部には、比較的精度が悪いジャイロ１１を使用しても、その補正は例えば０．１°／１時間程度である１秒間当りの補正は０．００００２７°／秒程度の補正となり、第２方向余弦行列計算部３３における計算ごとの初期値が、方向余弦行列計算部１３で得られた高い精度の計算結果Ｃ_n により周期的に補正されるため、第２方向余弦行列計算部３３へ供給するジャイロデータに対してはジャイロデータ補正計算を行わなくても十分であるが、必要に応じて行ってもよい。同様に航法座標軸回転量計算部１９によるジャイロデータに対する補正は長い周期で行えばよく、従ってこの補正は、第２方向余弦行列計算部３３に入力するジャイロデータに対しては行わなくてもよいが、必要に応じて行ってもよい。
【００２２】
なお上述では姿勢基準を方向余弦行列により求めたが、コータニオン方法による場合もｓｉｎ，ｃｏｓの関数を含む計算であるため、この発明を適用して、高い精度で、かつ短かい周期で姿勢角を測定することができる。
【００２３】
【発明の効果】
以上述べたようにこの発明によれば第１演算部により高次の近似式を用いて高い精度の姿勢基準を計算すると共に、第１演算部よりも次数が低い近似式を用いて第２演算部でも姿勢基準を計算し、この計算は用いる近似式の次数が小さいため短時間に計算することができ、従って、短かい周期で姿勢基準を得ることができ、この第２演算部のその計算ごとの初期値を、第１演算部で高い精度の姿勢基準が得られるごとに補正するため、結果として、可成り精度の高い姿勢角を短かい周期で得ることができる。従って演算器（一般にＣＰＵ）として比較的低速度の安価なものを使用することができ、全体としても安価に構成することができる。このように高速度に姿勢角、方位角が得られるため、これらを用いるシステムに対し、時間遅れによる誤差などを低減することができる。
【図面の簡単な説明】
【図１】この発明の実施例の機能構成を示すブロック図。
【図２】図１に示した実施例における方向余弦行列の計算タイミングと更新タイミングの例を示すタイムチャート。
【図３】従来の姿勢計測装置を示すブロック図。

Claims (2)

1. 移動体の角速度を検出するジャイロと、
   上記ジャイロの出力を用いて、方向余弦行列増分ΔＣ _ｎ の計算における正弦と余弦三角関数を含む計算をｎ次関数の近似演算により、周波数ｍＨｚで姿勢基準計算を行う第１演算部と、
   上記ジャイロの出力を用いて、上記ｎより小さい次数のｐ次の近似演算により、ｍＨｚより大きい周波数ｒＨｚで姿勢基準計算を行って移動体の姿勢角を計測する第２演算部と、
   上記第１演算部で方向余弦行列Ｃ _ｎ が計算されるごとに、その方向余弦行列Ｃ _ｎ に第２演算部で計算した方向余弦行列の変動分ΔＢｍを乗算して補正する演算誤差補正部と
   を具備する姿勢計測装置。
2. 上記演算誤差補正部は、上記第１演算部での姿勢基準の計算に用いたジャイロ出力が入力されてから、上記第１演算部での上記姿勢基準の計算結果が得られ、上記初期設定するまでの、上記第２演算部における姿勢基準計算結果の前回の計算値に対する変動分ΔＢｍを求める変動分検出部と、その変動分ΔＢｍを上記第１演算部の方向余弦行列に補正演算して上記第２演算部の方向余弦行列の初期値として設定する補正演算部とよりなることを特徴とする請求項１記載の姿勢計測装置。

Images