JP2006038650A - 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム - Google Patents

Abstract

【課題】 従来よりも短い時間で物体の姿勢又は真北を含む方位を計測することができる姿勢計測方法、姿勢計測装置、方位計及びコンピュータプログラムを提供する。
【解決手段】 本発明のカルマンフィルタの時間更新では、姿勢計測装置の姿勢をクオータニオンを用いて表した状態変数ｘの推定値ｘ_k(-)ハット及び誤差共分散Ｐ_k(-)は、時間に対して不変であるとして前ステップの観測更新で得られた値と等しいとする。また観測更新では、ジャイロ及び加速度センサを用いて互いに直交する各３軸回りの角速度及び各３軸方向の加速度を測定し、測定した角速度及び加速度の値を観測値ｚ_k として状態変数の推定値ｘ_k(+)ハット及び誤差共分散Ｐ_k(+)を計算する。誤差共分散が十分小さくなるまでカルマンフィルタの処理を繰り返し、姿勢を示す状態変数を得る。
【選択図】 図３

Description

本発明は、加速度及び角速度を測定することによって地表に対する姿勢を計測する方法、姿勢計測装置、真北を含む方位を計測する方位計、及びコンピュータプログラムに関する。

加速度センサ及びジャイロを用いて航空機又は船舶等の物体の加速度及び角速度を物体の内部で測定し、測定した加速度及び角速度を時間積分することによって各時刻での物体の位置・姿勢を決定する慣性航法は、移動する物体の位置を決定する方法の一つである。従来の慣性航法の技術は、例えば特許文献１及び非特許文献１に開示されている。

慣性航法により物体の位置・姿勢を正確に計測するためには、物体が地表に対して静止した状態での物体の位置・姿勢の計測（以下、アライメントという）を正確に行っておく必要がある。真北及び鉛直方向を含む方位と静止状態での緯度とを正確に計測することによって、方位に対する物体の姿勢を決定することができるので、正確なアライメントを行うことは正確な方位を計測することと同意である。

従来行われているアライメントの処理は、水平面又は鉛直方向を検出するレベリング、方位に対する物体の姿勢を簡易的に検出するコースアライメント、観測を繰り返して誤差を減少させることにより物体の姿勢を精密に計測するファインアライメントの順で行われる。

図４は、レベリング及びコースアライメントの方法の例を示す模式図である。図中には、自転軸を含む地球の断面を示している。レベリングでは、物体が静止した状態で加速度センサにより重力加速度を測定する。重力加速度の方向は鉛直方向であるので、測定した重力加速度の方向に直交する面が水平面である。次に、コースアライメントでは、物体が静止した状態でジャイロにより地球の自転角速度を測定する。自転角速度の方向は地球の自転軸に平行であり、自転軸に直交する赤道面と鉛直方向とのなす角度が物体の緯度λの定義であるので、測定した自転角速度の方向と水平面とのなす角度が緯度λとなる。更に、自転角速度のベクトルを水平面内に射影したベクトルの方向が真北方向である。鉛直方向及び真北方向から図示しない東方向が求められる。この真北方向（Ｎｏｒｔｈ）、東方向（Ｅａｓｔ）、鉛直方向（Ｄｏｗｎ）からなる、地表に対して固定された固定座標系をＮｏｒｔｈ−Ｅａｓｔ−Ｄｏｗｎ（ＮＥＤ）系と言う。また地表に対して固定されておらず、物体上に固定された座標系を、以下ｂｏｄｙ系と言う。重力加速度の大きさをｇ、自転角速度の大きさをΩとする。Ωの値は、Ω＝３６０°／ｄａｙ＝１５°／ｈ＝０．００４°／ｓである。

次に、地表上の座標系であるＮＥＤ系に対する物体の姿勢を表現する必要がある。物体の姿勢を表現するパラメータとしては、ｂｏｄｙ系がＮＥＤ系に対して回転した角度を示すオイラー角が従来用いられている。また、オイラー角以外で物体の姿勢を表現するパラメータとしては、クオータニオンがある。物体上に一の固定軸を定め、Ｎｏｒｔｈ軸と固定軸とのなす角をα、Ｅａｓｔ軸と固定軸とのなす角をβ、Ｄｏｗｎ軸と固定軸とのなす角をγ、固定軸回りの物体の回転角度をδとする。このとき、クオータニオン（ｑ₀ ，ｑ₁ ，ｑ₂ ，ｑ₃）は下記式で定義される。

またクオータニオンは、ｑ₀ ²＋ｑ₁ ²＋ｑ₂ ²＋ｑ₃ ²＝１の関係を満たす。ｂｏｄｙ系上の座標（ｂ₁ ，ｂ₂ ，ｂ₃ ）をＮＥＤ系上の座標（Ｎ，Ｅ，Ｄ）へ変換する座標変換行列Ｃ_b ⁿは、クオータニオンを用いて下記式で表される。

以上の如きオイラー角又はクオータニオン等のパラメータを用いて、地表に対する物体の姿勢を表すことができる。逆に、同じパラメータを用いて物体から見た真北方向等の方位を表すことができる。コースアライメントを行って得られる物体の姿勢は、オイラー角又はクオータニオン等のパラメータで表現された形で得られることとなる。

次に、従来のファインアライメントの方法の例を説明する。ファインアライメントを行う従来の方法の例として、カルマンフィルタにより誤差を減少させる方法が用いられている。カルマンフィルタは、ある系の状態を示す状態変数を定義しておき、時間の経過に伴って状態変数の推定値及び状態変数の誤差を更新する時間更新の処理、並びに観測された何らかのパラメータの値である観測値と状態変数の推定値から予測される観測値の予測値とのずれに基づいて状態変数の誤差及び状態変数の推定値を更に更新する観測更新の処理からなる。カルマンフィルタの時間更新で用いられる数式を下記に示す。

ここで、ｘは状態変数であり、ハットはｘの値が推定値であることを示し、ｋは自然数であってステップ数を示し、ｆ（）は所定の関数である。また（−）は観測更新を行う前の推定値であることを示し、（＋）は観測更新を行った後の推定値であることを示す。またＰはｘの誤差共分散であり、ΦはＰを時間変化させる演算子であり、Ｑは系に発生するノイズを示し、Γは系のノイズを時間変化させる演算子である。（２）式は、（ｋ−１）ステップ目の観測更新後の状態変数の推定値の関数でｋステップ目の観測更新前の状態変数の推定値を計算する式であり、（３）式は（ｋ−１）ステップ目の観測更新後の誤差共分散を時間変化させることによりｋステップ目の観測更新前の誤差共分散を計算する式である。またカルマンフィルタの観測更新で用いられる数式を下記に示す。

ここで、ｚは観測値を示し、状態変数ｘの関数ｈ（ｘ）を用いてｚ＝ｈ（ｘ）と予測される。ｚ＝ｈ（ｘ）は観測方程式と呼ばれる。またＨは観測感度行列と呼ばれ、Ｈ＝∂ｈ（ｘ）／∂ｘである。またＫはカルマンゲインと呼ばれ、観測された観測値と状態変数から求められる観測値の予測値とのずれをどの程度状態変数の更新に反映させるかの重みを示し、０≦Ｋ≦１である。またＲは観測のノイズであり、ＶはＲを計算式に組み込むためにＲの次元を変換する変換係数である。またＩは単位行列である。（４）式は、カルマンゲインの計算式であり、（５）式は、観測された観測値ｚと観測更新前の状態変数ｘの推定値から求められる観測値の予測値とのずれに基づいてｋステップ目の観測更新後の状態変数ｘの推定値を更新する式であり、（６）式はカルマンゲインを用いて状態変数ｘの誤差共分散を更新する式である。

図５は、従来のファインアライメントの方法を概念的に説明する概念図である。従来のファインアライメントでは、オイラー角又はクオータニオン等のパラメータを用いて表現した方位、物体の姿勢又は位置等を状態変数ｘとし、物体の実際の速度等を観測値ｚとする。従来の時間更新では、加速度センサ及びジャイロを用いて加速度及び角速度を測定し、測定した加速度及び角速度を積分し、時間に伴って変化する物体の姿勢、位置又は速度等を示すｘ_k(-)ハットと誤差共分散Ｐ_k(-)とを計算する。しかし、実際にはアライメントの処理中は物体は静止状態であるので、従来の観測更新では、速度＝０に対応する観測値ｚ_k の値を（５）式に代入し、ｘ_k(+)ハットを計算する。ここで、物体の速度を観測値にしている場合はｚ_k＝０を代入する。観測更新が終了した後は、ｋを１インクリメントし、再び時間更新の処理を行う。誤差共分散が十分に小さくなるまで処理を繰り返し、物体が静止した状態での方位又は物体の姿勢を精密に計測する。

以上の如きアライメントの技術を応用することによって、磁針又はＧＰＳ（ Global Positioning System）等の外部の情報を利用する方法を用いることなく方位を精密に計測する方位計を実現することができる。特許文献２には、水平面に平行な回転面上に設けたジャイロを用いて真北を含む方位を精密に検出する技術が開示されている。
特開２００２−３２３３２２号公報 特開２００１−２１５１２１号公報 アウン−ファン・シン（Eun-Hwan Shin）, "アキュラシー インプルヴメント オブ ロウ コスト ＩＮＳ／ＧＰＳ フォー ランド アプリケーション（Accuracy Improvement of Low Cost INS/GPS for Land Application）", ＵＣＧＥ レポーツ（UCGE Reports）, No.20156, ［online］, 2001年12月, ユニバーシティ オブ カルガリー（UNIVERSITY OF CALGARY）,［平成１６年 ７月２７日検索］ インターネット＜ＵＲＬ： http://www.geomatics.ucalgary.ca/Papers/Thesis/NES/01.20156.EHShin.pdf＞

アライメントを行う又は方位を計測する従来の技術では、前述した如くレベリング、コースアライメント及びファインアライメントの三つの処理からなっており、これら三つの処理を順に行う必要があるので、従来の技術では処理に長い時間を要するという問題がある。また従来の技術では、カルマンフィルタを利用したファインアライメントの処理の際に観測データを積分する処理が含まれるので、ファインアライメントの処理が複雑で長い時間を要するという問題がある。また、方位又は物体の姿勢を表現するパラメータとしてオイラー角を用いた場合は、オイラー角には姿勢を精度良く表すことができなくなる特異点が存在するので、方位又は物体の姿勢を表現できない場合があるという問題がある。

本発明は、斯かる事情に鑑みてなされたものであって、その目的とするところは、従来に比べてより簡易なアルゴリズムを用いて方位又は物体の姿勢を計測することにより、従来よりも短い時間で方位又は物体の姿勢を計測することができる姿勢計測方法、姿勢計測装置、真北を含む方位を計測する方位計、及びコンピュータプログラムを提供することにある。

第１発明に係る姿勢計測方法は、互いに直交する３軸で表される座標系の前記３軸夫々の方向の加速度を測定する加速度測定手段と、各前記３軸回りの角速度を測定する角速度測定手段とを用い、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を反復計算することによって、前記座標系の地表に対する姿勢を計測する方法において、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値としたカルマンフィルタの処理により、状態変数の推定値及び誤差を前回の計算結果から更新する更新ステップと、該更新ステップで更新した状態変数の誤差が所定値以下であるか否かを判定する判定ステップと、該判定ステップで前記誤差が所定値以下であると判定されるまで、前記更新ステップ及び前記判定ステップの処理を反復するステップと、前記判定ステップで前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定するステップとを含むことを特徴とする。

第２発明に係る姿勢計測方法は、互いに直交する３軸で表される座標系の前記３軸夫々の方向の加速度を測定する加速度測定手段と、各前記３軸回りの角速度を測定する角速度測定手段とを用い、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測する方法において、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新ステップと、該更新ステップで更新した状態変数の誤差が所定値以下であるか否かを判定する判定ステップと、該判定ステップで前記誤差が所定値以下であると判定されるまで、前記更新ステップ及び前記判定ステップの処理を反復するステップと、前記判定ステップで前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定するステップとを含むことを特徴とする。

第３発明に係る姿勢計測方法は、前記状態変数は、クオータニオンを変数として含んでいることを特徴とする。

第４発明に係る姿勢計測方法は、前記状態変数は、緯度を変数として含んでいることを特徴とする。

第５発明に係る姿勢計測装置は、互いに直交する３軸で表される座標系の前記３軸夫々の方向の加速度を測定する加速度測定手段と、各前記３軸回りの角速度を測定する角速度測定手段とを備え、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測する装置において、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新手段と、前記更新手段が更新した状態変数の誤差が所定値以下であるか否かを判定する判定手段と、該判定手段により前記誤差が所定値以下であると判定されるまで、前記更新手段及び前記判定手段が行う処理を反復させる手段と、前記判定手段により前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定する手段とを備えることを特徴とする。

第６発明に係る方位計は、互いに直交する３軸で表される座標系の前記３軸夫々の方向の加速度を測定する加速度測定手段と、各前記３軸回りの角速度を測定する角速度測定手段とを備え、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、地表に対して固定された方位を計測する方位計において、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新手段と、前記更新手段が更新した状態変数の誤差が所定値以下であるか否かを判定する判定手段と、該判定手段により前記誤差が所定値以下であると判定されるまで、前記更新手段及び前記判定手段が行う処理を反復させる手段と、前記判定手段により前記誤差が所定値以下であると判定された場合の状態変数の推定値に基づいて、地表に対して固定された方位を求める手段とを備えることを特徴とする。

第７発明に係るコンピュータプログラムは、コンピュータに、互いに直交する３軸で表される座標系が実質的に静止した状態で、前記３軸夫々の方向の加速度及び各前記３軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測させるコンピュータプログラムにおいて、コンピュータに、前記３軸夫々の方向の加速度及び各前記３軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新させる更新手順と、コンピュータに、更新した状態変数の誤差が所定値以下であるか否かを判定させる判定手順と、コンピュータに、前記判定手順で前記誤差が所定値以下であると判定されるまで、前記更新手順及び前記判定手順での処理を反復させる手順と、コンピュータに、前記判定手順で前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定させる手順とを含むことを特徴とする。

第１、第２、第５及び第７発明においては、時間更新では、地表に対する姿勢を示す状態変数の推定値及び誤差は時間に対して不変であるとし、観測更新では、加速度測定手段及び角速度測定手段を用いて測定した３軸に係る加速度及び角速度をカルマンフィルタの測定値として状態変数の推定値及び誤差を計算し、誤差が十分小さくなるまで計算を繰り返す。

第３発明においては、状態変数は、地表に対する姿勢を示すクオータニオンを変数として含んでなる。

第４発明においては、状態変数は、地表に対する位置を示す緯度を変数として含んでなる。

第６発明においては、時間更新では、地表に対する姿勢を示す状態変数の推定値及び誤差は時間に対して不変であるとし、観測更新では、加速度測定手段及び角速度測定手段を用いて測定した３軸に係る加速度及び角速度をカルマンフィルタの測定値として状態変数の推定値及び誤差を計算し、誤差が十分小さくなるまで計算を繰り返し、誤差が十分小となった場合に、計算した状態変数から方位を求める。

第１、第２、第５及び第７発明にあっては、従来の姿勢計測方法に比べて処理内容がより単純であるので、本発明の姿勢計測方法を用いることにより、短時間で物体の姿勢を計測することができるようになる。又は、従来よりも低速な演算装置を用いて姿勢計測装置を実現することが可能となり、姿勢計測装置のコストを低減することができる。また本発明では３軸方向の加速度及び３軸回りの角速度を用いて姿勢を計測するので、３次元内の任意の姿勢を計測することが可能となり、利便性が高い。また水平面又は鉛直方向を検出するレベリングの処理を行う必要がないので、従来に比べてより短時間で姿勢を計測することができる。また姿勢計測装置内に水平面を備える必要がないので、姿勢計測装置を小型化することができる。

第３発明にあっては、状態変数はクオータニオンからなるので、姿勢を表現できなくなる特異点が存在せず、どのような状態であっても精度良く姿勢を計測することができる。

第４発明にあっては、状態変数は緯度を変数として含むので、姿勢計測装置の位置が不明である場合であっても、地表に対する姿勢及び位置を精度良く計測することができる。

第６発明にあっては、どのような姿勢からでも地表に対して固定された方位を短時間で精度良く計測することができる等、本発明は優れた効果を奏する。

以下本発明をその実施の形態を示す図面に基づき具体的に説明する。
図１は、本発明の姿勢計測装置の内部構成を示すブロック図である。姿勢計測装置のハウジング４に対して固定された互いに直交する三軸ｂ₁ 軸，ｂ₂ 軸，ｂ₃ 軸が定められている。そして、ｂ₁軸回りの角速度ω₁ を検出するジャイロ（角速度測定手段）２１、ｂ₁ 軸方向の加速度ｇ₁ を検出する加速度センサ（加速度測定手段）３１、ｂ₂軸回りの角速度ω₂ を検出するジャイロ２２、ｂ₂ 軸方向の加速度ｇ₂ を検出する加速度センサ３２、ｂ₃軸回りの角速度ω₃ を検出するジャイロ２３、ｂ₃ 軸方向の加速度ｇ₃ を検出する加速度センサ３３がハウジング４に対して固定されている。ｂ₁軸，ｂ₂ 軸，ｂ₃ 軸は、地表に対して固定されておらず、姿勢計測装置に固定された座標系であるｂｏｄｙ系を表す。

姿勢計測装置は、演算を行うＣＰＵ１１を備え、ＣＰＵ１１には、演算に伴う一時的な情報を記憶するＲＡＭ１２と、姿勢計測装置を制御するための本発明のコンピュータプログラム１６を記憶するＲＯＭ１３とが接続されている。ＣＰＵ１１は、ＲＯＭ１３が記憶しているコンピュータプログラム１６に従って姿勢計測装置に必要な処理を行う。ジャイロ２１，２２，２３及び加速度センサ３１，３２，３３にはＡＤ変換部１５が接続されており、ＡＤ変換部１５は更にＣＰＵ１１に接続されている。ジャイロ２１，２２，２３及び加速度センサ３１，３２，３３は、検出データをＡＤ変換部１５へ出力し、ＡＤ変換部１５は、入力された検出データをＡＤ変換してＣＰＵ１１へ出力する。ＣＰＵ１１は、ＡＤ変換部１５を介して各センサから入力された検出データに基づいて姿勢計測の処理を行う。更にＣＰＵ１１には、ＣＰＵ１１の演算結果を示すディスプレイ又はメータ等の出力部１４が接続されている。

本発明の姿勢計測装置は、航空機又は船舶等の物体の各時刻での位置・姿勢を決定する慣性航法装置に組み込まれ、慣性航法のためのアライメントを行う装置として構成されるか、又は真北を含む方位を計測する方位計として単独で構成されている。

本発明においては、クオータニオンで表現した姿勢計測装置の姿勢、及び緯度を状態変数ｘとし、ジャイロが測定した各軸の角速度、及び加速度センサが測定した各軸の加速度を観測値ｚとする。状態変数ｘ及び観測値ｚを下記式に示す。

ここで、λは静止状態の姿勢計測装置の位置の緯度であり、一般的には未知変数である。また、ω₁ ，ω₂ ，ω₃ は夫々ジャイロ２１，２２，２３が測定したｂ₁軸回り，ｂ₂ 軸回り，ｂ₃ 軸回りの角速度であり、ｇ₁ ，ｇ₂ ，ｇ₃は夫々加速度センサ３１，３２，３３が測定したｂ₁ 軸方向，ｂ₂ 軸方向，ｂ₃ 軸方向の加速度である。

次に、観測方程式ｚ＝ｈ（ｘ）の導出を説明する。ＮＥＤ系での静止状態の姿勢計測装置の角速度ω_n 及び加速度ｇ_n は、図４より、自転角速度の大きさΩ及び重力加速度の大きさｇを用いて下記式で表される。

また、ＮＥＤ系をｂｏｄｙ系へ変換する座標変換行列Ｃ_n ^bは、（１）式で表されるＣ_b ⁿの逆行列であり、下記式で表される。

Ｃ_n ^bを用いることにより、ｂｏｄｙ系の角速度ω_b とＮＥＤ系の角速度ω_n との関係はω_b＝Ｃ_n ^bω_n と表すことができる。従って、下記式が成り立つ。

またＣ_n ^bを用いることにより、ｂｏｄｙ系の加速度ｇ_b とＮＥＤ系の加速度ｇ_n との関係はｇ_b＝Ｃ_n ^bｇ_n と表すことができる。従って、下記式が成り立つ。

（７）式と（８）式とをまとめることにより、観測方程式ｚ＝ｈ（ｘ）が下記式で得られる。

次に、観測感度行列Ｈの導出を説明する。（７）式を状態変数ｘで偏微分することにより、下記式が成り立つ。

また、（８）式を状態変数ｘで偏微分することにより、下記式が成り立つ。

（１０）式及び（１１）式をまとめることにより、下記の（１２）式で示す６行５列の観測感度行列Ｈが得られる。

次に、本発明で用いるカルマンフィルタの数式を説明する。本発明では、姿勢計測装置は姿勢計測の処理中には静止状態であるので、クオータニオンを用いて姿勢計測装置の姿勢及び位置を示す状態変数ｘの推定値と状態変数ｘの誤差共分散Ｐとは、時間経過に伴った変化を起こさないとする。従って、（２）式及び（３）式に対応する本発明での時間更新で用いる数式は下記式で表される。

また、（９）式で与えられる観測方程式ｈ（ｘ）及び（１２）式で与えられる観測感度行列Ｈを用い、（４）式、（５）式及び（６）式に対応する本発明での観測更新で用いる数式は下記式で表される。

ここで、（１５）式に含まれるＲは、ジャイロ２１，２２，２３及び加速度センサ３１，３２，３３の観測ノイズの共分散行列である。本発明では、観測ノイズの互いの共分散の値を０とし、ジャイロ２１，２２，２３の観測ノイズの分散をσ_ω ² 、加速度センサ３１，３２，３３の観測ノイズの分散をσ_g ²とする。従って、Ｒは下記式で表される。

（１８）式の対角成分以外の成分の値は０である。ジャイロ２１，２２，２３としては通常同一の部品を用いるので、ジャイロ２１，２２，２３の観測ノイズの分散は同一の値σ_ω ² となる。同様に加速度センサ３１，３２，３３の観測ノイズの分散は同一の値σ_g ²となる。またσ_ω ²及びσ_g ²の値は、ジャイロ及び加速度センサの部品の仕様で定まっているので、（１８）式で与えられるＲは予め定められた値からなっている。なお、Ｒの対角成分として、互いに異なる値σ_ω(1) ²，σ_ω(2) ²，σ_ω(3) ²，σ_g(1) ²，σ_g(2) ² ，σ_g(3) ² を用いてもよい。

次に、本発明の姿勢計測装置が行う処理を説明する。図２は、本発明の姿勢計測装置が行う処理の手順を示すフローチャートである。ＣＰＵ１１は、ＲＯＭ１３が記憶している本発明のコンピュータプログラム１６に従って以下の処理を行う。ジャイロ２１，２２，２３及び加速度センサ３１，３２，３３は角速度ω₁ ，ω₂ ，ω₃ 及び加速度ｇ₁，ｇ₂ ，ｇ₃ を測定し、ＣＰＵ１１は測定された角速度及び加速度をＡＤ変換部１５を介して受け付ける（Ｓ１）。ＣＰＵ１１は、次に、測定された角速度及び加速度に基づいて、所定の方法で状態変数ｘの推定値の初期値ｘ₀₍₊₎ハットと状態変数ｘの誤差共分散Ｐの初期値Ｐ₀₍₊₎とを計算する（Ｓ２）。

ＣＰＵ１１は、次に、ステップ数ｋを１に初期化する（Ｓ３）。ジャイロ２１，２２，２３は姿勢計測装置に対して固定された各軸回りの角速度ω₁ ，ω₂ ，ω₃ を測定し、加速度センサ３１，３２，３３は姿勢計測装置に対して固定された各軸方向の加速度ｇ₁，ｇ₂ ，ｇ₃ を測定し、ＣＰＵ１１は測定された角速度及び加速度をＡＤ変換部１５を介して受け付ける（Ｓ４）。ＣＰＵ１１は、次に、Ｐ_k(-)＝Ｐ_k-1(+)として得られるＰ_k(-)、ｘ_k(-)ハット＝ｘ_k-1(+)ハットとして得られるｘ_k(-)ハットの各成分の値を代入した（１２）式より得られるＨ_k 、及び（１８）式で得られるＲを（１５）式に代入することにより、カルマンゲインＫ_k を計算する（Ｓ５）。ＣＰＵ１１は、次に、計算したカルマンゲインＫ_k 、測定した角速度ω₁ ，ω₂ ，ω₃ 及び加速度ｇ₁ ，ｇ₂ ，ｇ₃からなる測定値ｚ_k 、ｘ_k(-)ハットの各成分の値を代入した（９）式より得られるｈ（ｘ_k(-)ハット）を用いて、（１６）式により状態変数の推定値ｘ_k(+)ハットを計算し、（１７）式により状態変数の誤差共分散Ｐ_k(+)を計算する（Ｓ６）。

ＣＰＵ１１は、次に、５行５列の行列である計算した状態変数の誤差共分散Ｐ_k(+)の対角成分の夫々が全て所定の微少値以下であるか否かを判定する（Ｓ７）。誤差共分散Ｐ_k(+)の対角成分は、夫々ｑ₀ ，ｑ₁ ，ｑ₂ ，ｑ₃ ，λの誤差分散であるので、これらの値が全て所定の微少値以下になることによって、状態変数ｘの誤差が十分に小さくなったと判定することができる。ステップＳ７で状態変数の誤差共分散Ｐ_k(+)の対角成分に所定の微少値より大きい成分があった場合は（Ｓ７：ＮＯ）、ＣＰＵ１１は、ｋの値を１インクリメントし（Ｓ８）、処理をステップＳ４へ戻す。ＣＰＵ１１は、状態変数ｘの誤差が十分に小さくなるまで、角速度及び加速度の測定値に基づいて状態変数の推定値及び状態変数の誤差共分散を計算するステップＳ４〜ステップＳ７の処理を繰り返す。状態変数ｘの誤差が十分に小さくなったときに、静止状態の姿勢計測装置の地表に対する姿勢を示すクオータニオンと静止状態の姿勢計測装置の位置を示す緯度λとが得られることとなる。

ステップＳ７で状態変数の誤差共分散Ｐ_k(+)の対角成分の夫々が全て所定の微少値以下であった場合は（Ｓ７：ＹＥＳ）、ＣＰＵ１１は、状態変数ｘの成分であるクオータニオン（ｑ₀ ，ｑ₁ ，ｑ₂ ，ｑ₃）を、ＮＥＤ系の各軸回りの物体の回転角度を示すロール角、ピッチ角及びヨー角等の、クオータニオンよりもわかりやすく姿勢計測装置の地表に対する姿勢を示すパラメータへ変換する（Ｓ９）。ＣＰＵ１１は、次に、姿勢の計測結果として、変換したパラメータで表される姿勢計測装置の地表に対する姿勢、及び姿勢計測装置の緯度λを出力部１４に出力させ（Ｓ１０）、処理を終了する。

本発明の姿勢計測装置が慣性航法装置に組み込まれてアライメントを行う装置である場合は、計測された姿勢及び緯度を初期値として慣性航法が行われる。また本発明の姿勢計測装置が方位計である場合は、ステップＳ９ではクオータニオンを用いてＮＥＤ系上の（１，０，０）をｂｏｄｙ系上の座標に変換する等の処理により真北を含む方位をｂｏｄｙ系上で表現し、ステップＳ１０では真北を含む方位を出力する処理を行う。

図３は、本発明の姿勢計測方法を概念的に説明する概念図である。本発明では、クオータニオン及び緯度を用いて表現した姿勢計測装置の姿勢及び位置を状態変数ｘとし、測定した角速度及び加速度を観測値ｚとする。本発明の時間更新では、状態変数ｘと状態変数の誤差共分散Ｐは時間に対して不変であるとし、状態変数の推定値ｘ_k(-)ハット及び状態変数の誤差共分散Ｐ_k(-)は前ステップの観測更新で得られた値と等しいとする。また本発明の観測更新では、ジャイロ及び加速度センサを用いて角速度及び加速度を測定し、測定した角速度及び加速度の値を（１６）式に代入し、状態変数の推定値ｘ_k(+)ハット及び状態変数の誤差共分散Ｐ_k(+)を計算する。観測更新が終了した後は、ｋを１インクリメントし、再び時間更新の処理を行う。

本発明の姿勢計測方法は、図５に示す従来の姿勢計測方法に比べ、積分の処理を行う必要がなく、時間更新で状態変数ｘの推定値及び状態変数の誤差共分散Ｐを更新する必要がないので、処理内容がより簡単になる。更に、観測更新でジャイロ２１，２２，２３及び加速度センサ３１，３２，３３の検出結果を直接に観測値ｚとして用いて状態変数ｘの推定値及び状態変数の誤差共分散Ｐを計算する本発明の姿勢計測方法は、観測更新で速度＝０を用いて状態変数ｘの推定値及び状態変数の誤差共分散Ｐを計算する従来の方法に比べて処理内容がより単純である。従って、本発明の姿勢計測方法を用いることにより、短時間で物体の姿勢を計測することができ、このことは逆に、従来よりも低速な演算装置を用いて姿勢計測装置を実現することが可能となるので、姿勢計測装置のコストを低減することができる。

また本発明の姿勢計測装置は、３軸方向の加速度及び３軸回りの角速度を用いて姿勢を計測するので、３次元内の任意の姿勢を計測することが可能となり、利便性が高い。また水平面又は鉛直方向を検出するレベリングの処理を行う必要がないので、従来に比べてより短時間で姿勢を計測することができる。また姿勢計測装置内に水平面を備える必要がないので、姿勢計測装置を小型化することができる。

また本発明の姿勢計測装置を方位計として利用した場合は、どのような姿勢からでも真北を含む方位を短時間で精度良く計測することができる。

また本発明においては、状態変数はクオータニオンからなるので、状態変数としてオイラー角を用いた場合とは異なり、姿勢を表現できなくなる特異点が存在せず、どのような状態であっても精度良く姿勢を計測することができる。また本発明にあっては、状態変数は緯度λを変数として含むので、姿勢計測装置の緯度が不明である場合であっても、地表に対する姿勢及び位置を精度良く計測することができる。

なお、本実施の形態においては、状態変数はクオータニオン及び緯度からなるとしたが、オイラー角等その他のパラメータを用いて状態変数を表現する形態であってもよい。また本実施の形態においては、互いに直交する三軸ｂ₁ 軸，ｂ₂ 軸，ｂ₃ 軸に対してジャイロ２１，２２，２３及び加速度センサ３１，３２，３３を備えた形態を示したが、これに限るものではなく、本発明に係る角速度測定手段及び加速度測定手段としてより多くのジャイロ及び加速度センサを備える形態であってもよく、この場合には各ジャイロ及び各加速度センサの測定結果から各軸に係る角速度及び加速度をより精度良く測定することが可能となる。

また、本実施の形態においては、ジャイロ２１，２２，２３及び加速度センサ３１，３２，３３に対してＡＤ変換部１５を介してＣＰＵ１１及びＲＯＭ１３等が接続された形態を示したが、これに限るものではなく、本発明のコンピュータプログラムを記憶したコンピュータを用い、コンピュータ外のジャイロ及び加速度センサが測定したデータをコンピュータが受け付け、本発明のコンピュータプログラムに従ってコンピュータが物体の姿勢を計測する形態であってもよい。

本発明の姿勢計測装置の内部構成を示すブロック図である。 本発明の姿勢計測装置が行う処理の手順を示すフローチャートである。 本発明の姿勢計測方法を概念的に説明する概念図である。 レベリング及びコースアライメントの方法の例を示す模式図である。 従来のファインアライメントの方法を概念的に説明する概念図である。

符号の説明

１１ ＣＰＵ
１６ コンピュータプログラム
２１，２２，２３ ジャイロ（角速度測定手段）
３１，３２，３３ 加速度センサ（加速度測定手段）
４ ハウジング

Claims (7)

1. 互いに直交する３軸で表される座標系の前記３軸夫々の方向の加速度を測定する加速度測定手段と、各前記３軸回りの角速度を測定する角速度測定手段とを用い、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を反復計算することによって、前記座標系の地表に対する姿勢を計測する方法において、
   前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値としたカルマンフィルタの処理により、状態変数の推定値及び誤差を前回の計算結果から更新する更新ステップと、
   該更新ステップで更新した状態変数の誤差が所定値以下であるか否かを判定する判定ステップと、
   該判定ステップで前記誤差が所定値以下であると判定されるまで、前記更新ステップ及び前記判定ステップの処理を反復するステップと、
   前記判定ステップで前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定するステップと
   を含むことを特徴とする姿勢計測方法。
2. 互いに直交する３軸で表される座標系の前記３軸夫々の方向の加速度を測定する加速度測定手段と、各前記３軸回りの角速度を測定する角速度測定手段とを用い、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測する方法において、
   前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新ステップと、
   該更新ステップで更新した状態変数の誤差が所定値以下であるか否かを判定する判定ステップと、
   該判定ステップで前記誤差が所定値以下であると判定されるまで、前記更新ステップ及び前記判定ステップの処理を反復するステップと、
   前記判定ステップで前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定するステップと
   を含むことを特徴とする姿勢計測方法。
3. 前記状態変数は、クオータニオンを変数として含んでいることを特徴とする請求項１又は２に記載の姿勢計測方法。
4. 前記状態変数は、緯度を変数として含んでいることを特徴とする請求項１乃至３のいずれかひとつに記載の姿勢計測方法。
5. 互いに直交する３軸で表される座標系の前記３軸夫々の方向の加速度を測定する加速度測定手段と、各前記３軸回りの角速度を測定する角速度測定手段とを備え、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測する装置において、
   前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新手段と、
   前記更新手段が更新した状態変数の誤差が所定値以下であるか否かを判定する判定手段と、
   該判定手段により前記誤差が所定値以下であると判定されるまで、前記更新手段及び前記判定手段が行う処理を反復させる手段と、
   前記判定手段により前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定する手段と
   を備えることを特徴とする姿勢計測装置。
6. 互いに直交する３軸で表される座標系の前記３軸夫々の方向の加速度を測定する加速度測定手段と、各前記３軸回りの角速度を測定する角速度測定手段とを備え、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、地表に対して固定された方位を計測する方位計において、
   前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新手段と、
   前記更新手段が更新した状態変数の誤差が所定値以下であるか否かを判定する判定手段と、
   該判定手段により前記誤差が所定値以下であると判定されるまで、前記更新手段及び前記判定手段が行う処理を反復させる手段と、
   前記判定手段により前記誤差が所定値以下であると判定された場合の状態変数の推定値に基づいて、地表に対して固定された方位を求める手段と
   を備えることを特徴とする方位計。
7. コンピュータに、互いに直交する３軸で表される座標系が実質的に静止した状態で、前記３軸夫々の方向の加速度及び各前記３軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測させるコンピュータプログラムにおいて、
   コンピュータに、前記３軸夫々の方向の加速度及び各前記３軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新させる更新手順と、
   コンピュータに、更新した状態変数の誤差が所定値以下であるか否かを判定させる判定手順と、
   コンピュータに、前記判定手順で前記誤差が所定値以下であると判定されるまで、前記更新手順及び前記判定手順での処理を反復させる手順と、
   コンピュータに、前記判定手順で前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定させる手順と
   を含むことを特徴とするコンピュータプログラム。

Images