JP2001280970A - 姿勢計測装置 - Google Patents

Abstract

(57)【要約】 【課題】 高速ハードウェアを用いることなく、高速
に、高精度の姿勢角を得る。 【解決手段】 ジャイロ１１の出力に対し方向余弦行列
計算部１３で高次の近似式を用いて方向余弦行列Ｃ_n を
計算すると共に、ジャイロ出力に対し、第２方向余弦行
列計算部３３で低次の近似式を用いて、方向余弦行列計
算部１３よりも短かい周期で方向余弦行列Ｂ_m を計算
し、Ｃ_n が得られるごとに、そのＣ_n を用いて第２方向
余弦行列計算部３３の計算ごとの初期値を補正する。Ｂ
_m を用いてこれが得られるごとに姿勢角を姿勢角計算部
２３で計算する。

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 の計算におけるsi
n，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 ・ΔＢ２・ΔＢ３）が計算される。この
ようにして第２方向余弦行列計算部３３は周期Ｔ₁ ／４
ごとに次数の小さい近似式で第２方向余弦行列計算を行
うが、その計算の初期値が、周期Ｔ₁ ごとに方向余弦行
列計算部１３で次数の高い近似式で計算された精度の高
い方向余弦行列によって補正されるため、周期Ｔ₁ ／４
ごとに比較的高い精度の姿勢角を得ることができる。ち
なみに、４００°／ｓで１軸回転しているときの１／
（２００Ｈｚ）間の回転軸の角度誤差は下記の通りであ
る。

【００１７】 テーラー展開の次数 方向余弦行列誤差（度） １次 −８．１×１０^-4 ２次 ４．１×１０^-4 ３次 ９．９×１０^-8 ４次 −２．５×１０^-8 上記条件は通常はない大きな値であるが、近似次数と誤
差はこのような関係にあり、例えば方向余弦行列Ｃ_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. 【請求項２】 上記演算誤差補正部は、上記第１演算部
   での姿勢基準の計算に用いたジャイロ出力が入力されて
   から、上記第１演算部での上記姿勢基準の計算結果が得
   られ、上記初期設定するまでの、上記第２演算部におけ
   る姿勢基準計算結果の変動分の総てを求める変動分検出
   部と、その変動分の総てを上記第１演算部の上記姿勢基
   準の計算結果に補正演算して上記第２演算部にその計算
   ごとの初期値として設定する補正演算部とよりなること
   を特徴とする請求項１記載の姿勢計測装置。