JP2006038650A - 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム - Google Patents
姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム Download PDFInfo
- Publication number
- JP2006038650A JP2006038650A JP2004219288A JP2004219288A JP2006038650A JP 2006038650 A JP2006038650 A JP 2006038650A JP 2004219288 A JP2004219288 A JP 2004219288A JP 2004219288 A JP2004219288 A JP 2004219288A JP 2006038650 A JP2006038650 A JP 2006038650A
- Authority
- JP
- Japan
- Prior art keywords
- state variable
- acceleration
- angular velocity
- coordinate system
- error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Gyroscopes (AREA)
Abstract
【課題】 従来よりも短い時間で物体の姿勢又は真北を含む方位を計測することができる姿勢計測方法、姿勢計測装置、方位計及びコンピュータプログラムを提供する。
【解決手段】 本発明のカルマンフィルタの時間更新では、姿勢計測装置の姿勢をクオータニオンを用いて表した状態変数xの推定値xk(-)ハット及び誤差共分散Pk(-)は、時間に対して不変であるとして前ステップの観測更新で得られた値と等しいとする。また観測更新では、ジャイロ及び加速度センサを用いて互いに直交する各3軸回りの角速度及び各3軸方向の加速度を測定し、測定した角速度及び加速度の値を観測値zk として状態変数の推定値xk(+)ハット及び誤差共分散Pk(+)を計算する。誤差共分散が十分小さくなるまでカルマンフィルタの処理を繰り返し、姿勢を示す状態変数を得る。
【選択図】 図3
【解決手段】 本発明のカルマンフィルタの時間更新では、姿勢計測装置の姿勢をクオータニオンを用いて表した状態変数xの推定値xk(-)ハット及び誤差共分散Pk(-)は、時間に対して不変であるとして前ステップの観測更新で得られた値と等しいとする。また観測更新では、ジャイロ及び加速度センサを用いて互いに直交する各3軸回りの角速度及び各3軸方向の加速度を測定し、測定した角速度及び加速度の値を観測値zk として状態変数の推定値xk(+)ハット及び誤差共分散Pk(+)を計算する。誤差共分散が十分小さくなるまでカルマンフィルタの処理を繰り返し、姿勢を示す状態変数を得る。
【選択図】 図3
Description
本発明は、加速度及び角速度を測定することによって地表に対する姿勢を計測する方法、姿勢計測装置、真北を含む方位を計測する方位計、及びコンピュータプログラムに関する。
加速度センサ及びジャイロを用いて航空機又は船舶等の物体の加速度及び角速度を物体の内部で測定し、測定した加速度及び角速度を時間積分することによって各時刻での物体の位置・姿勢を決定する慣性航法は、移動する物体の位置を決定する方法の一つである。従来の慣性航法の技術は、例えば特許文献1及び非特許文献1に開示されている。
慣性航法により物体の位置・姿勢を正確に計測するためには、物体が地表に対して静止した状態での物体の位置・姿勢の計測(以下、アライメントという)を正確に行っておく必要がある。真北及び鉛直方向を含む方位と静止状態での緯度とを正確に計測することによって、方位に対する物体の姿勢を決定することができるので、正確なアライメントを行うことは正確な方位を計測することと同意である。
従来行われているアライメントの処理は、水平面又は鉛直方向を検出するレベリング、方位に対する物体の姿勢を簡易的に検出するコースアライメント、観測を繰り返して誤差を減少させることにより物体の姿勢を精密に計測するファインアライメントの順で行われる。
図4は、レベリング及びコースアライメントの方法の例を示す模式図である。図中には、自転軸を含む地球の断面を示している。レベリングでは、物体が静止した状態で加速度センサにより重力加速度を測定する。重力加速度の方向は鉛直方向であるので、測定した重力加速度の方向に直交する面が水平面である。次に、コースアライメントでは、物体が静止した状態でジャイロにより地球の自転角速度を測定する。自転角速度の方向は地球の自転軸に平行であり、自転軸に直交する赤道面と鉛直方向とのなす角度が物体の緯度λの定義であるので、測定した自転角速度の方向と水平面とのなす角度が緯度λとなる。更に、自転角速度のベクトルを水平面内に射影したベクトルの方向が真北方向である。鉛直方向及び真北方向から図示しない東方向が求められる。この真北方向(North)、東方向(East)、鉛直方向(Down)からなる、地表に対して固定された固定座標系をNorth−East−Down(NED)系と言う。また地表に対して固定されておらず、物体上に固定された座標系を、以下body系と言う。重力加速度の大きさをg、自転角速度の大きさをΩとする。Ωの値は、Ω=360°/day=15°/h=0.004°/sである。
次に、地表上の座標系であるNED系に対する物体の姿勢を表現する必要がある。物体の姿勢を表現するパラメータとしては、body系がNED系に対して回転した角度を示すオイラー角が従来用いられている。また、オイラー角以外で物体の姿勢を表現するパラメータとしては、クオータニオンがある。物体上に一の固定軸を定め、North軸と固定軸とのなす角をα、East軸と固定軸とのなす角をβ、Down軸と固定軸とのなす角をγ、固定軸回りの物体の回転角度をδとする。このとき、クオータニオン(q0 ,q1 ,q2 ,q3)は下記式で定義される。
またクオータニオンは、q0 2+q1 2+q2 2+q3 2=1の関係を満たす。body系上の座標(b1 ,b2 ,b3 )をNED系上の座標(N,E,D)へ変換する座標変換行列Cb nは、クオータニオンを用いて下記式で表される。
以上の如きオイラー角又はクオータニオン等のパラメータを用いて、地表に対する物体の姿勢を表すことができる。逆に、同じパラメータを用いて物体から見た真北方向等の方位を表すことができる。コースアライメントを行って得られる物体の姿勢は、オイラー角又はクオータニオン等のパラメータで表現された形で得られることとなる。
次に、従来のファインアライメントの方法の例を説明する。ファインアライメントを行う従来の方法の例として、カルマンフィルタにより誤差を減少させる方法が用いられている。カルマンフィルタは、ある系の状態を示す状態変数を定義しておき、時間の経過に伴って状態変数の推定値及び状態変数の誤差を更新する時間更新の処理、並びに観測された何らかのパラメータの値である観測値と状態変数の推定値から予測される観測値の予測値とのずれに基づいて状態変数の誤差及び状態変数の推定値を更に更新する観測更新の処理からなる。カルマンフィルタの時間更新で用いられる数式を下記に示す。
ここで、xは状態変数であり、ハットはxの値が推定値であることを示し、kは自然数であってステップ数を示し、f()は所定の関数である。また(−)は観測更新を行う前の推定値であることを示し、(+)は観測更新を行った後の推定値であることを示す。またPはxの誤差共分散であり、ΦはPを時間変化させる演算子であり、Qは系に発生するノイズを示し、Γは系のノイズを時間変化させる演算子である。(2)式は、(k−1)ステップ目の観測更新後の状態変数の推定値の関数でkステップ目の観測更新前の状態変数の推定値を計算する式であり、(3)式は(k−1)ステップ目の観測更新後の誤差共分散を時間変化させることによりkステップ目の観測更新前の誤差共分散を計算する式である。またカルマンフィルタの観測更新で用いられる数式を下記に示す。
ここで、zは観測値を示し、状態変数xの関数h(x)を用いてz=h(x)と予測される。z=h(x)は観測方程式と呼ばれる。またHは観測感度行列と呼ばれ、H=∂h(x)/∂xである。またKはカルマンゲインと呼ばれ、観測された観測値と状態変数から求められる観測値の予測値とのずれをどの程度状態変数の更新に反映させるかの重みを示し、0≦K≦1である。またRは観測のノイズであり、VはRを計算式に組み込むためにRの次元を変換する変換係数である。またIは単位行列である。(4)式は、カルマンゲインの計算式であり、(5)式は、観測された観測値zと観測更新前の状態変数xの推定値から求められる観測値の予測値とのずれに基づいてkステップ目の観測更新後の状態変数xの推定値を更新する式であり、(6)式はカルマンゲインを用いて状態変数xの誤差共分散を更新する式である。
図5は、従来のファインアライメントの方法を概念的に説明する概念図である。従来のファインアライメントでは、オイラー角又はクオータニオン等のパラメータを用いて表現した方位、物体の姿勢又は位置等を状態変数xとし、物体の実際の速度等を観測値zとする。従来の時間更新では、加速度センサ及びジャイロを用いて加速度及び角速度を測定し、測定した加速度及び角速度を積分し、時間に伴って変化する物体の姿勢、位置又は速度等を示すxk(-)ハットと誤差共分散Pk(-)とを計算する。しかし、実際にはアライメントの処理中は物体は静止状態であるので、従来の観測更新では、速度=0に対応する観測値zk の値を(5)式に代入し、xk(+)ハットを計算する。ここで、物体の速度を観測値にしている場合はzk=0を代入する。観測更新が終了した後は、kを1インクリメントし、再び時間更新の処理を行う。誤差共分散が十分に小さくなるまで処理を繰り返し、物体が静止した状態での方位又は物体の姿勢を精密に計測する。
以上の如きアライメントの技術を応用することによって、磁針又はGPS( Global Positioning System)等の外部の情報を利用する方法を用いることなく方位を精密に計測する方位計を実現することができる。特許文献2には、水平面に平行な回転面上に設けたジャイロを用いて真北を含む方位を精密に検出する技術が開示されている。
特開2002−323322号公報
特開2001−215121号公報
アウン−ファン・シン(Eun-Hwan Shin), "アキュラシー インプルヴメント オブ ロウ コスト INS/GPS フォー ランド アプリケーション(Accuracy Improvement of Low Cost INS/GPS for Land Application)", UCGE レポーツ(UCGE Reports), No.20156, [online], 2001年12月, ユニバーシティ オブ カルガリー(UNIVERSITY OF CALGARY),[平成16年 7月27日検索] インターネット<URL: http://www.geomatics.ucalgary.ca/Papers/Thesis/NES/01.20156.EHShin.pdf>
アライメントを行う又は方位を計測する従来の技術では、前述した如くレベリング、コースアライメント及びファインアライメントの三つの処理からなっており、これら三つの処理を順に行う必要があるので、従来の技術では処理に長い時間を要するという問題がある。また従来の技術では、カルマンフィルタを利用したファインアライメントの処理の際に観測データを積分する処理が含まれるので、ファインアライメントの処理が複雑で長い時間を要するという問題がある。また、方位又は物体の姿勢を表現するパラメータとしてオイラー角を用いた場合は、オイラー角には姿勢を精度良く表すことができなくなる特異点が存在するので、方位又は物体の姿勢を表現できない場合があるという問題がある。
本発明は、斯かる事情に鑑みてなされたものであって、その目的とするところは、従来に比べてより簡易なアルゴリズムを用いて方位又は物体の姿勢を計測することにより、従来よりも短い時間で方位又は物体の姿勢を計測することができる姿勢計測方法、姿勢計測装置、真北を含む方位を計測する方位計、及びコンピュータプログラムを提供することにある。
第1発明に係る姿勢計測方法は、互いに直交する3軸で表される座標系の前記3軸夫々の方向の加速度を測定する加速度測定手段と、各前記3軸回りの角速度を測定する角速度測定手段とを用い、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を反復計算することによって、前記座標系の地表に対する姿勢を計測する方法において、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値としたカルマンフィルタの処理により、状態変数の推定値及び誤差を前回の計算結果から更新する更新ステップと、該更新ステップで更新した状態変数の誤差が所定値以下であるか否かを判定する判定ステップと、該判定ステップで前記誤差が所定値以下であると判定されるまで、前記更新ステップ及び前記判定ステップの処理を反復するステップと、前記判定ステップで前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定するステップとを含むことを特徴とする。
第2発明に係る姿勢計測方法は、互いに直交する3軸で表される座標系の前記3軸夫々の方向の加速度を測定する加速度測定手段と、各前記3軸回りの角速度を測定する角速度測定手段とを用い、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測する方法において、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新ステップと、該更新ステップで更新した状態変数の誤差が所定値以下であるか否かを判定する判定ステップと、該判定ステップで前記誤差が所定値以下であると判定されるまで、前記更新ステップ及び前記判定ステップの処理を反復するステップと、前記判定ステップで前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定するステップとを含むことを特徴とする。
第3発明に係る姿勢計測方法は、前記状態変数は、クオータニオンを変数として含んでいることを特徴とする。
第4発明に係る姿勢計測方法は、前記状態変数は、緯度を変数として含んでいることを特徴とする。
第5発明に係る姿勢計測装置は、互いに直交する3軸で表される座標系の前記3軸夫々の方向の加速度を測定する加速度測定手段と、各前記3軸回りの角速度を測定する角速度測定手段とを備え、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測する装置において、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新手段と、前記更新手段が更新した状態変数の誤差が所定値以下であるか否かを判定する判定手段と、該判定手段により前記誤差が所定値以下であると判定されるまで、前記更新手段及び前記判定手段が行う処理を反復させる手段と、前記判定手段により前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定する手段とを備えることを特徴とする。
第6発明に係る方位計は、互いに直交する3軸で表される座標系の前記3軸夫々の方向の加速度を測定する加速度測定手段と、各前記3軸回りの角速度を測定する角速度測定手段とを備え、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、地表に対して固定された方位を計測する方位計において、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新手段と、前記更新手段が更新した状態変数の誤差が所定値以下であるか否かを判定する判定手段と、該判定手段により前記誤差が所定値以下であると判定されるまで、前記更新手段及び前記判定手段が行う処理を反復させる手段と、前記判定手段により前記誤差が所定値以下であると判定された場合の状態変数の推定値に基づいて、地表に対して固定された方位を求める手段とを備えることを特徴とする。
第7発明に係るコンピュータプログラムは、コンピュータに、互いに直交する3軸で表される座標系が実質的に静止した状態で、前記3軸夫々の方向の加速度及び各前記3軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測させるコンピュータプログラムにおいて、コンピュータに、前記3軸夫々の方向の加速度及び各前記3軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新させる更新手順と、コンピュータに、更新した状態変数の誤差が所定値以下であるか否かを判定させる判定手順と、コンピュータに、前記判定手順で前記誤差が所定値以下であると判定されるまで、前記更新手順及び前記判定手順での処理を反復させる手順と、コンピュータに、前記判定手順で前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定させる手順とを含むことを特徴とする。
第1、第2、第5及び第7発明においては、時間更新では、地表に対する姿勢を示す状態変数の推定値及び誤差は時間に対して不変であるとし、観測更新では、加速度測定手段及び角速度測定手段を用いて測定した3軸に係る加速度及び角速度をカルマンフィルタの測定値として状態変数の推定値及び誤差を計算し、誤差が十分小さくなるまで計算を繰り返す。
第3発明においては、状態変数は、地表に対する姿勢を示すクオータニオンを変数として含んでなる。
第4発明においては、状態変数は、地表に対する位置を示す緯度を変数として含んでなる。
第6発明においては、時間更新では、地表に対する姿勢を示す状態変数の推定値及び誤差は時間に対して不変であるとし、観測更新では、加速度測定手段及び角速度測定手段を用いて測定した3軸に係る加速度及び角速度をカルマンフィルタの測定値として状態変数の推定値及び誤差を計算し、誤差が十分小さくなるまで計算を繰り返し、誤差が十分小となった場合に、計算した状態変数から方位を求める。
第1、第2、第5及び第7発明にあっては、従来の姿勢計測方法に比べて処理内容がより単純であるので、本発明の姿勢計測方法を用いることにより、短時間で物体の姿勢を計測することができるようになる。又は、従来よりも低速な演算装置を用いて姿勢計測装置を実現することが可能となり、姿勢計測装置のコストを低減することができる。また本発明では3軸方向の加速度及び3軸回りの角速度を用いて姿勢を計測するので、3次元内の任意の姿勢を計測することが可能となり、利便性が高い。また水平面又は鉛直方向を検出するレベリングの処理を行う必要がないので、従来に比べてより短時間で姿勢を計測することができる。また姿勢計測装置内に水平面を備える必要がないので、姿勢計測装置を小型化することができる。
第3発明にあっては、状態変数はクオータニオンからなるので、姿勢を表現できなくなる特異点が存在せず、どのような状態であっても精度良く姿勢を計測することができる。
第4発明にあっては、状態変数は緯度を変数として含むので、姿勢計測装置の位置が不明である場合であっても、地表に対する姿勢及び位置を精度良く計測することができる。
第6発明にあっては、どのような姿勢からでも地表に対して固定された方位を短時間で精度良く計測することができる等、本発明は優れた効果を奏する。
以下本発明をその実施の形態を示す図面に基づき具体的に説明する。
図1は、本発明の姿勢計測装置の内部構成を示すブロック図である。姿勢計測装置のハウジング4に対して固定された互いに直交する三軸b1 軸,b2 軸,b3 軸が定められている。そして、b1軸回りの角速度ω1 を検出するジャイロ(角速度測定手段)21、b1 軸方向の加速度g1 を検出する加速度センサ(加速度測定手段)31、b2軸回りの角速度ω2 を検出するジャイロ22、b2 軸方向の加速度g2 を検出する加速度センサ32、b3軸回りの角速度ω3 を検出するジャイロ23、b3 軸方向の加速度g3 を検出する加速度センサ33がハウジング4に対して固定されている。b1軸,b2 軸,b3 軸は、地表に対して固定されておらず、姿勢計測装置に固定された座標系であるbody系を表す。
図1は、本発明の姿勢計測装置の内部構成を示すブロック図である。姿勢計測装置のハウジング4に対して固定された互いに直交する三軸b1 軸,b2 軸,b3 軸が定められている。そして、b1軸回りの角速度ω1 を検出するジャイロ(角速度測定手段)21、b1 軸方向の加速度g1 を検出する加速度センサ(加速度測定手段)31、b2軸回りの角速度ω2 を検出するジャイロ22、b2 軸方向の加速度g2 を検出する加速度センサ32、b3軸回りの角速度ω3 を検出するジャイロ23、b3 軸方向の加速度g3 を検出する加速度センサ33がハウジング4に対して固定されている。b1軸,b2 軸,b3 軸は、地表に対して固定されておらず、姿勢計測装置に固定された座標系であるbody系を表す。
姿勢計測装置は、演算を行うCPU11を備え、CPU11には、演算に伴う一時的な情報を記憶するRAM12と、姿勢計測装置を制御するための本発明のコンピュータプログラム16を記憶するROM13とが接続されている。CPU11は、ROM13が記憶しているコンピュータプログラム16に従って姿勢計測装置に必要な処理を行う。ジャイロ21,22,23及び加速度センサ31,32,33にはAD変換部15が接続されており、AD変換部15は更にCPU11に接続されている。ジャイロ21,22,23及び加速度センサ31,32,33は、検出データをAD変換部15へ出力し、AD変換部15は、入力された検出データをAD変換してCPU11へ出力する。CPU11は、AD変換部15を介して各センサから入力された検出データに基づいて姿勢計測の処理を行う。更にCPU11には、CPU11の演算結果を示すディスプレイ又はメータ等の出力部14が接続されている。
本発明の姿勢計測装置は、航空機又は船舶等の物体の各時刻での位置・姿勢を決定する慣性航法装置に組み込まれ、慣性航法のためのアライメントを行う装置として構成されるか、又は真北を含む方位を計測する方位計として単独で構成されている。
本発明においては、クオータニオンで表現した姿勢計測装置の姿勢、及び緯度を状態変数xとし、ジャイロが測定した各軸の角速度、及び加速度センサが測定した各軸の加速度を観測値zとする。状態変数x及び観測値zを下記式に示す。
ここで、λは静止状態の姿勢計測装置の位置の緯度であり、一般的には未知変数である。また、ω1 ,ω2 ,ω3 は夫々ジャイロ21,22,23が測定したb1軸回り,b2 軸回り,b3 軸回りの角速度であり、g1 ,g2 ,g3は夫々加速度センサ31,32,33が測定したb1 軸方向,b2 軸方向,b3 軸方向の加速度である。
次に、観測方程式z=h(x)の導出を説明する。NED系での静止状態の姿勢計測装置の角速度ωn 及び加速度gn は、図4より、自転角速度の大きさΩ及び重力加速度の大きさgを用いて下記式で表される。
また、NED系をbody系へ変換する座標変換行列Cn bは、(1)式で表されるCb nの逆行列であり、下記式で表される。
Cn bを用いることにより、body系の角速度ωb とNED系の角速度ωn との関係はωb=Cn bωn と表すことができる。従って、下記式が成り立つ。
またCn bを用いることにより、body系の加速度gb とNED系の加速度gn との関係はgb=Cn bgn と表すことができる。従って、下記式が成り立つ。
(7)式と(8)式とをまとめることにより、観測方程式z=h(x)が下記式で得られる。
次に、観測感度行列Hの導出を説明する。(7)式を状態変数xで偏微分することにより、下記式が成り立つ。
また、(8)式を状態変数xで偏微分することにより、下記式が成り立つ。
(10)式及び(11)式をまとめることにより、下記の(12)式で示す6行5列の観測感度行列Hが得られる。
次に、本発明で用いるカルマンフィルタの数式を説明する。本発明では、姿勢計測装置は姿勢計測の処理中には静止状態であるので、クオータニオンを用いて姿勢計測装置の姿勢及び位置を示す状態変数xの推定値と状態変数xの誤差共分散Pとは、時間経過に伴った変化を起こさないとする。従って、(2)式及び(3)式に対応する本発明での時間更新で用いる数式は下記式で表される。
また、(9)式で与えられる観測方程式h(x)及び(12)式で与えられる観測感度行列Hを用い、(4)式、(5)式及び(6)式に対応する本発明での観測更新で用いる数式は下記式で表される。
ここで、(15)式に含まれるRは、ジャイロ21,22,23及び加速度センサ31,32,33の観測ノイズの共分散行列である。本発明では、観測ノイズの互いの共分散の値を0とし、ジャイロ21,22,23の観測ノイズの分散をσω 2 、加速度センサ31,32,33の観測ノイズの分散をσg 2とする。従って、Rは下記式で表される。
(18)式の対角成分以外の成分の値は0である。ジャイロ21,22,23としては通常同一の部品を用いるので、ジャイロ21,22,23の観測ノイズの分散は同一の値σω 2 となる。同様に加速度センサ31,32,33の観測ノイズの分散は同一の値σg 2となる。またσω 2及びσg 2の値は、ジャイロ及び加速度センサの部品の仕様で定まっているので、(18)式で与えられるRは予め定められた値からなっている。なお、Rの対角成分として、互いに異なる値σω(1) 2,σω(2) 2,σω(3) 2,σg(1) 2,σg(2) 2 ,σg(3) 2 を用いてもよい。
次に、本発明の姿勢計測装置が行う処理を説明する。図2は、本発明の姿勢計測装置が行う処理の手順を示すフローチャートである。CPU11は、ROM13が記憶している本発明のコンピュータプログラム16に従って以下の処理を行う。ジャイロ21,22,23及び加速度センサ31,32,33は角速度ω1 ,ω2 ,ω3 及び加速度g1,g2 ,g3 を測定し、CPU11は測定された角速度及び加速度をAD変換部15を介して受け付ける(S1)。CPU11は、次に、測定された角速度及び加速度に基づいて、所定の方法で状態変数xの推定値の初期値x0(+)ハットと状態変数xの誤差共分散Pの初期値P0(+)とを計算する(S2)。
CPU11は、次に、ステップ数kを1に初期化する(S3)。ジャイロ21,22,23は姿勢計測装置に対して固定された各軸回りの角速度ω1 ,ω2 ,ω3 を測定し、加速度センサ31,32,33は姿勢計測装置に対して固定された各軸方向の加速度g1,g2 ,g3 を測定し、CPU11は測定された角速度及び加速度をAD変換部15を介して受け付ける(S4)。CPU11は、次に、Pk(-)=Pk-1(+)として得られるPk(-)、xk(-)ハット=xk-1(+)ハットとして得られるxk(-)ハットの各成分の値を代入した(12)式より得られるHk 、及び(18)式で得られるRを(15)式に代入することにより、カルマンゲインKk を計算する(S5)。CPU11は、次に、計算したカルマンゲインKk 、測定した角速度ω1 ,ω2 ,ω3 及び加速度g1 ,g2 ,g3からなる測定値zk 、xk(-)ハットの各成分の値を代入した(9)式より得られるh(xk(-)ハット)を用いて、(16)式により状態変数の推定値xk(+)ハットを計算し、(17)式により状態変数の誤差共分散Pk(+)を計算する(S6)。
CPU11は、次に、5行5列の行列である計算した状態変数の誤差共分散Pk(+)の対角成分の夫々が全て所定の微少値以下であるか否かを判定する(S7)。誤差共分散Pk(+)の対角成分は、夫々q0 ,q1 ,q2 ,q3 ,λの誤差分散であるので、これらの値が全て所定の微少値以下になることによって、状態変数xの誤差が十分に小さくなったと判定することができる。ステップS7で状態変数の誤差共分散Pk(+)の対角成分に所定の微少値より大きい成分があった場合は(S7:NO)、CPU11は、kの値を1インクリメントし(S8)、処理をステップS4へ戻す。CPU11は、状態変数xの誤差が十分に小さくなるまで、角速度及び加速度の測定値に基づいて状態変数の推定値及び状態変数の誤差共分散を計算するステップS4〜ステップS7の処理を繰り返す。状態変数xの誤差が十分に小さくなったときに、静止状態の姿勢計測装置の地表に対する姿勢を示すクオータニオンと静止状態の姿勢計測装置の位置を示す緯度λとが得られることとなる。
ステップS7で状態変数の誤差共分散Pk(+)の対角成分の夫々が全て所定の微少値以下であった場合は(S7:YES)、CPU11は、状態変数xの成分であるクオータニオン(q0 ,q1 ,q2 ,q3)を、NED系の各軸回りの物体の回転角度を示すロール角、ピッチ角及びヨー角等の、クオータニオンよりもわかりやすく姿勢計測装置の地表に対する姿勢を示すパラメータへ変換する(S9)。CPU11は、次に、姿勢の計測結果として、変換したパラメータで表される姿勢計測装置の地表に対する姿勢、及び姿勢計測装置の緯度λを出力部14に出力させ(S10)、処理を終了する。
本発明の姿勢計測装置が慣性航法装置に組み込まれてアライメントを行う装置である場合は、計測された姿勢及び緯度を初期値として慣性航法が行われる。また本発明の姿勢計測装置が方位計である場合は、ステップS9ではクオータニオンを用いてNED系上の(1,0,0)をbody系上の座標に変換する等の処理により真北を含む方位をbody系上で表現し、ステップS10では真北を含む方位を出力する処理を行う。
図3は、本発明の姿勢計測方法を概念的に説明する概念図である。本発明では、クオータニオン及び緯度を用いて表現した姿勢計測装置の姿勢及び位置を状態変数xとし、測定した角速度及び加速度を観測値zとする。本発明の時間更新では、状態変数xと状態変数の誤差共分散Pは時間に対して不変であるとし、状態変数の推定値xk(-)ハット及び状態変数の誤差共分散Pk(-)は前ステップの観測更新で得られた値と等しいとする。また本発明の観測更新では、ジャイロ及び加速度センサを用いて角速度及び加速度を測定し、測定した角速度及び加速度の値を(16)式に代入し、状態変数の推定値xk(+)ハット及び状態変数の誤差共分散Pk(+)を計算する。観測更新が終了した後は、kを1インクリメントし、再び時間更新の処理を行う。
本発明の姿勢計測方法は、図5に示す従来の姿勢計測方法に比べ、積分の処理を行う必要がなく、時間更新で状態変数xの推定値及び状態変数の誤差共分散Pを更新する必要がないので、処理内容がより簡単になる。更に、観測更新でジャイロ21,22,23及び加速度センサ31,32,33の検出結果を直接に観測値zとして用いて状態変数xの推定値及び状態変数の誤差共分散Pを計算する本発明の姿勢計測方法は、観測更新で速度=0を用いて状態変数xの推定値及び状態変数の誤差共分散Pを計算する従来の方法に比べて処理内容がより単純である。従って、本発明の姿勢計測方法を用いることにより、短時間で物体の姿勢を計測することができ、このことは逆に、従来よりも低速な演算装置を用いて姿勢計測装置を実現することが可能となるので、姿勢計測装置のコストを低減することができる。
また本発明の姿勢計測装置は、3軸方向の加速度及び3軸回りの角速度を用いて姿勢を計測するので、3次元内の任意の姿勢を計測することが可能となり、利便性が高い。また水平面又は鉛直方向を検出するレベリングの処理を行う必要がないので、従来に比べてより短時間で姿勢を計測することができる。また姿勢計測装置内に水平面を備える必要がないので、姿勢計測装置を小型化することができる。
また本発明の姿勢計測装置を方位計として利用した場合は、どのような姿勢からでも真北を含む方位を短時間で精度良く計測することができる。
また本発明においては、状態変数はクオータニオンからなるので、状態変数としてオイラー角を用いた場合とは異なり、姿勢を表現できなくなる特異点が存在せず、どのような状態であっても精度良く姿勢を計測することができる。また本発明にあっては、状態変数は緯度λを変数として含むので、姿勢計測装置の緯度が不明である場合であっても、地表に対する姿勢及び位置を精度良く計測することができる。
なお、本実施の形態においては、状態変数はクオータニオン及び緯度からなるとしたが、オイラー角等その他のパラメータを用いて状態変数を表現する形態であってもよい。また本実施の形態においては、互いに直交する三軸b1 軸,b2 軸,b3 軸に対してジャイロ21,22,23及び加速度センサ31,32,33を備えた形態を示したが、これに限るものではなく、本発明に係る角速度測定手段及び加速度測定手段としてより多くのジャイロ及び加速度センサを備える形態であってもよく、この場合には各ジャイロ及び各加速度センサの測定結果から各軸に係る角速度及び加速度をより精度良く測定することが可能となる。
また、本実施の形態においては、ジャイロ21,22,23及び加速度センサ31,32,33に対してAD変換部15を介してCPU11及びROM13等が接続された形態を示したが、これに限るものではなく、本発明のコンピュータプログラムを記憶したコンピュータを用い、コンピュータ外のジャイロ及び加速度センサが測定したデータをコンピュータが受け付け、本発明のコンピュータプログラムに従ってコンピュータが物体の姿勢を計測する形態であってもよい。
11 CPU
16 コンピュータプログラム
21,22,23 ジャイロ(角速度測定手段)
31,32,33 加速度センサ(加速度測定手段)
4 ハウジング
16 コンピュータプログラム
21,22,23 ジャイロ(角速度測定手段)
31,32,33 加速度センサ(加速度測定手段)
4 ハウジング
Claims (7)
- 互いに直交する3軸で表される座標系の前記3軸夫々の方向の加速度を測定する加速度測定手段と、各前記3軸回りの角速度を測定する角速度測定手段とを用い、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を反復計算することによって、前記座標系の地表に対する姿勢を計測する方法において、
前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値としたカルマンフィルタの処理により、状態変数の推定値及び誤差を前回の計算結果から更新する更新ステップと、
該更新ステップで更新した状態変数の誤差が所定値以下であるか否かを判定する判定ステップと、
該判定ステップで前記誤差が所定値以下であると判定されるまで、前記更新ステップ及び前記判定ステップの処理を反復するステップと、
前記判定ステップで前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定するステップと
を含むことを特徴とする姿勢計測方法。 - 互いに直交する3軸で表される座標系の前記3軸夫々の方向の加速度を測定する加速度測定手段と、各前記3軸回りの角速度を測定する角速度測定手段とを用い、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測する方法において、
前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新ステップと、
該更新ステップで更新した状態変数の誤差が所定値以下であるか否かを判定する判定ステップと、
該判定ステップで前記誤差が所定値以下であると判定されるまで、前記更新ステップ及び前記判定ステップの処理を反復するステップと、
前記判定ステップで前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定するステップと
を含むことを特徴とする姿勢計測方法。 - 前記状態変数は、クオータニオンを変数として含んでいることを特徴とする請求項1又は2に記載の姿勢計測方法。
- 前記状態変数は、緯度を変数として含んでいることを特徴とする請求項1乃至3のいずれかひとつに記載の姿勢計測方法。
- 互いに直交する3軸で表される座標系の前記3軸夫々の方向の加速度を測定する加速度測定手段と、各前記3軸回りの角速度を測定する角速度測定手段とを備え、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測する装置において、
前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新手段と、
前記更新手段が更新した状態変数の誤差が所定値以下であるか否かを判定する判定手段と、
該判定手段により前記誤差が所定値以下であると判定されるまで、前記更新手段及び前記判定手段が行う処理を反復させる手段と、
前記判定手段により前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定する手段と
を備えることを特徴とする姿勢計測装置。 - 互いに直交する3軸で表される座標系の前記3軸夫々の方向の加速度を測定する加速度測定手段と、各前記3軸回りの角速度を測定する角速度測定手段とを備え、前記座標系が実質的に静止した状態で、前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、地表に対して固定された方位を計測する方位計において、
前記加速度測定手段が測定した各軸方向の加速度及び前記角速度測定手段が測定した各軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新する更新手段と、
前記更新手段が更新した状態変数の誤差が所定値以下であるか否かを判定する判定手段と、
該判定手段により前記誤差が所定値以下であると判定されるまで、前記更新手段及び前記判定手段が行う処理を反復させる手段と、
前記判定手段により前記誤差が所定値以下であると判定された場合の状態変数の推定値に基づいて、地表に対して固定された方位を求める手段と
を備えることを特徴とする方位計。 - コンピュータに、互いに直交する3軸で表される座標系が実質的に静止した状態で、前記3軸夫々の方向の加速度及び各前記3軸回りの角速度に基づいて、前記座標系の地表に対する姿勢を示す状態変数の推定値及び誤差を時間経過に伴って更新する時間更新の処理と、状態変数の推定値を所定の観測方程式へ代入することによって得られる所定の観測値の予測値を実際の観測値と比較することにより状態変数の推定値及び誤差を更新する観測更新の処理とを反復するカルマンフィルタの処理によって、前記座標系の地表に対する姿勢を計測させるコンピュータプログラムにおいて、
コンピュータに、前記3軸夫々の方向の加速度及び各前記3軸回りの角速度を観測値とし、地表に対して固定された固定座標系上での加速度及び角速度を前記座標系上での加速度及び角速度へ変換する式を観測方程式としたカルマンフィルタの観測更新の処理により、時間更新の処理では前回の観測更新の結果から不変であるとした状態変数の推定値及び誤差を更新させる更新手順と、
コンピュータに、更新した状態変数の誤差が所定値以下であるか否かを判定させる判定手順と、
コンピュータに、前記判定手順で前記誤差が所定値以下であると判定されるまで、前記更新手順及び前記判定手順での処理を反復させる手順と、
コンピュータに、前記判定手順で前記誤差が所定値以下であると判定された場合の状態変数の推定値を、前記座標系の地表に対する姿勢を示す状態変数であると決定させる手順と
を含むことを特徴とするコンピュータプログラム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2004219288A JP2006038650A (ja) | 2004-07-27 | 2004-07-27 | 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2004219288A JP2006038650A (ja) | 2004-07-27 | 2004-07-27 | 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2006038650A true JP2006038650A (ja) | 2006-02-09 |
Family
ID=35903799
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2004219288A Pending JP2006038650A (ja) | 2004-07-27 | 2004-07-27 | 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2006038650A (ja) |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007322392A (ja) * | 2006-06-05 | 2007-12-13 | Nippon Telegr & Teleph Corp <Ntt> | 姿勢平滑化方法およびそのプログラム |
JP2008310767A (ja) * | 2007-06-18 | 2008-12-25 | Toyota Central R&D Labs Inc | 姿勢信号演算装置 |
US7801704B2 (en) | 2008-05-15 | 2010-09-21 | Schlumberger Technology Corporation | Method and system for azimuth measurements using gyro sensors |
JP2011017688A (ja) * | 2009-07-09 | 2011-01-27 | Jiaotong Univ | 角度測定の方法およびそれを実施するための角度測定ジャイロシステム |
CN103363993A (zh) * | 2013-07-06 | 2013-10-23 | 西北工业大学 | 一种基于无迹卡尔曼滤波的飞机角速率信号重构方法 |
JP2013543976A (ja) * | 2010-11-08 | 2013-12-09 | アルパイン リプレイ インコーポレイテッド | ジャイロセンサ較正のデバイス及び方法 |
CN109947116A (zh) * | 2019-04-18 | 2019-06-28 | 北京主线科技有限公司 | 无人驾驶汽车的定位方法及装置 |
US10451438B2 (en) | 2015-02-05 | 2019-10-22 | Alpinereplay, Inc. | Systems and methods for in-motion gyroscope calibration |
WO2020050084A1 (ja) * | 2018-09-07 | 2020-03-12 | 国立研究開発法人宇宙航空研究開発機構 | 誘導制御プログラムを格納した記憶媒体 |
US10901095B2 (en) | 2014-04-25 | 2021-01-26 | Nec Corporation | Position and attitude estimation device, image processing device, and position and attitude estimation method |
-
2004
- 2004-07-27 JP JP2004219288A patent/JP2006038650A/ja active Pending
Cited By (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007322392A (ja) * | 2006-06-05 | 2007-12-13 | Nippon Telegr & Teleph Corp <Ntt> | 姿勢平滑化方法およびそのプログラム |
JP4759446B2 (ja) * | 2006-06-05 | 2011-08-31 | 日本電信電話株式会社 | 姿勢平滑化方法およびそのプログラム |
JP2008310767A (ja) * | 2007-06-18 | 2008-12-25 | Toyota Central R&D Labs Inc | 姿勢信号演算装置 |
US7801704B2 (en) | 2008-05-15 | 2010-09-21 | Schlumberger Technology Corporation | Method and system for azimuth measurements using gyro sensors |
JP2011017688A (ja) * | 2009-07-09 | 2011-01-27 | Jiaotong Univ | 角度測定の方法およびそれを実施するための角度測定ジャイロシステム |
JP2013543976A (ja) * | 2010-11-08 | 2013-12-09 | アルパイン リプレイ インコーポレイテッド | ジャイロセンサ較正のデバイス及び方法 |
KR20140052925A (ko) * | 2010-11-08 | 2014-05-07 | 알파인리플레이 아이앤씨 | 자이로 센서의 보정 디바이스 및 보정 방법 |
US9146134B2 (en) | 2010-11-08 | 2015-09-29 | Alpinereplay, Inc. | Device and method of gyro sensor calibration |
KR101880524B1 (ko) * | 2010-11-08 | 2018-07-20 | 퀄컴 인코포레이티드 | 자이로 센서의 보정 디바이스 및 보정 방법 |
CN103363993A (zh) * | 2013-07-06 | 2013-10-23 | 西北工业大学 | 一种基于无迹卡尔曼滤波的飞机角速率信号重构方法 |
US10901095B2 (en) | 2014-04-25 | 2021-01-26 | Nec Corporation | Position and attitude estimation device, image processing device, and position and attitude estimation method |
US10451438B2 (en) | 2015-02-05 | 2019-10-22 | Alpinereplay, Inc. | Systems and methods for in-motion gyroscope calibration |
WO2020050084A1 (ja) * | 2018-09-07 | 2020-03-12 | 国立研究開発法人宇宙航空研究開発機構 | 誘導制御プログラムを格納した記憶媒体 |
CN109947116A (zh) * | 2019-04-18 | 2019-06-28 | 北京主线科技有限公司 | 无人驾驶汽车的定位方法及装置 |
CN109947116B (zh) * | 2019-04-18 | 2022-10-18 | 北京主线科技有限公司 | 无人驾驶汽车的定位方法及装置 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Wu et al. | Fast complementary filter for attitude estimation using low-cost MARG sensors | |
CN109001787A (zh) | 一种姿态角解算与定位的方法及其融合传感器 | |
US9417091B2 (en) | System and method for determining and correcting field sensors errors | |
JP4876204B2 (ja) | 小型姿勢センサ | |
Phuong et al. | A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass | |
US10788324B2 (en) | Method and apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor | |
JP6705972B2 (ja) | 姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体 | |
CN110221302B (zh) | 环境探测装置及其修正方法、***、便携设备及存储介质 | |
CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航***及方法 | |
WO2011091083A1 (en) | Apparatus and methodology for calibration of a gyroscope and a compass included in a handheld device | |
CN108731664B (zh) | 机器人状态估计方法、装置、计算机设备和存储介质 | |
US11408735B2 (en) | Positioning system and positioning method | |
JP2012173190A (ja) | 測位システム、測位方法 | |
EP2930467A1 (en) | A system and method for sensing the inclination of a moving platform with respect to gravity | |
WO2021218731A1 (zh) | Imu与刚体的位姿融合方法、装置、设备及存储介质 | |
JP2006038650A (ja) | 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム | |
CN112577518A (zh) | 一种惯性测量单元标定方法及装置 | |
JP2013096724A (ja) | 状態推定装置 | |
JP5678357B2 (ja) | 回転情報演算方法、回転情報演算プログラム、磁気型ジャイロスコープおよび移動体 | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
JP2013122384A (ja) | カルマンフィルタ、及び、状態推定装置 | |
JP2013061309A (ja) | カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム | |
Lajimi et al. | A comprehensive filter to reduce drift from Euler angles, velocity, and position using an IMU | |
KR101140379B1 (ko) | 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치 | |
Kannan et al. | Adaptive sensor fusion technology for mobile and wearable applications |