姿勢角安定化装置及び方法
【課題】高精度速度計測装置に組み込まれて、移動体の静止時における位置、速度及び方位の変化を解消すると共に、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うことができるようにする姿勢角安定化装置及び方法を提供すること。
【解決手段】姿勢角安定化装置１００は、移動体の角速度とフィードバックした補正値とから姿勢角の変化量を算出し、算出した姿勢角の変化量を積分した姿勢角と、カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し出力する。そして、動作時と判定した場合に生成した姿勢行列のラッチを行い、静止時と判定した場合に静止時であると判定する前の姿勢行列のラッチを保持し、ラッチしている姿勢行列と、生成した姿勢行列とから姿勢角誤差を算出し、算出した姿勢角誤差と生成した姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、フィードバックする。
【解決手段】姿勢角安定化装置１００は、移動体の角速度とフィードバックした補正値とから姿勢角の変化量を算出し、算出した姿勢角の変化量を積分した姿勢角と、カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し出力する。そして、動作時と判定した場合に生成した姿勢行列のラッチを行い、静止時と判定した場合に静止時であると判定する前の姿勢行列のラッチを保持し、ラッチしている姿勢行列と、生成した姿勢行列とから姿勢角誤差を算出し、算出した姿勢角誤差と生成した姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、フィードバックする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、姿勢角安定化装置及び方法に関する。
【背景技術】
【０００２】
従来より、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）／ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）複合航法によって移動体の位置、速度及び方位等を演算するＧＰＳ複合航法装置では、誤差の推定にカルマンフィルタが広く使用されている。カルマンフィルタでは、移動体の位置、速度及び方位等をモデルによって推定するが、移動体の移動中と静止中とで、モデルが大きく異なる。特に、移動体の静止時において、カルマンフィルタは観測量の可観測性問題によって推定精度が劣化してしまう。移動体が静止し続けた場合（例えば、移動体が赤信号で止まる等）、移動体の位置、速度及び方位等が次第にドリフトするという不具合が生じる。また、移動中と静止中とでモデルを切り替えると、静止時から動的状態に切り替わるときに姿勢角、速度の応答性が劣化して非連続な出力となってしまう。
【０００３】
このような不具合について、ＧＰＳ複合航法装置において、移動体の静止時における推定位置、推定速度及び推定方位の変化を解消すると同時に、静止時から移動時における応答性の劣化を解消する技術を開示する特許文献１が知られている。特許文献１に開示されたＧＰＳ複合航法装置は、移動体の静止を判定する静止判定部を設け、該静止判定部で静止状態と判定されたとき、カルマンフィルタの観測更新に用いる観測モデルを変更すると共に、更新による誤差共分散行列の変化分を補正する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００８−２３２８６９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
このような技術が提案されているが、ＧＰＳ／ＩＭＵ複合航法を利用した高精度速度計測装置において、移動体の静止時における位置、速度及び方位の変化を解消し、静止時から移動時に切り替わるときであっても、従来技術よりもさらに連続的な姿勢角及び速度応答出力を行うことができるようにする装置が求められている。
【０００６】
本発明は、ＧＰＳ受信機を利用し、カルマンフィルタを用いる高精度速度計測装置に組み込まれて、移動体の静止時における位置、速度及び方位の変化を解消すると共に、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うことができるようにする姿勢角安定化装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明では、以下のような解決手段を提供する。
【０００８】
（１） ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置であって、前記ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出手段と、前記姿勢角変化量算出手段によって算出された姿勢角の変化量を積分する積分処理手段と、前記積分処理手段によって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理手段と、前記ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、前記ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定手段と、前記速度旋回判定手段によって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチ手段と、前記姿勢角ラッチ手段によってラッチされている姿勢行列の転置行列を算出する転置処理手段と、前記転置処理手段によって転置された姿勢行列と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理手段と、前記姿勢角誤差演算処理手段によって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出手段にフィードバックする前記補正値とする補正制御手段と、を備える姿勢角安定化装置。
【０００９】
（１）の構成によれば、本発明に係る姿勢角安定化装置は、ＩＭＵを使用して計測された移動体の角速度と、フィードバックした補正値とから姿勢角の変化量を算出し、算出した姿勢角の変化量を積分し、積分した姿勢角と、カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力する。そして、姿勢角安定化装置は、ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定し、速度又は角速度が一定の閾値以上であると判定した場合に、生成した姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定した場合に、生成した姿勢行列のラッチを行わずに閾値未満であると判定する前の姿勢行列のラッチを保持し、ラッチしている姿勢行列の転置行列を算出し、算出した転置行列と、生成した姿勢行列との乗算処理を行い、姿勢角誤差を算出し、算出した姿勢角誤差と、生成した姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、フィードバックする補正値とする。
【００１０】
すなわち、本発明に係る姿勢角安定化装置は、移動体の移動と静止との判定を行い、移動体が静止すると、移動から静止に移る前の姿勢行列を保持し、保持した姿勢行列を用いて姿勢角を補正制御し、移動し始めると、保持した姿勢行列を用いて姿勢行列を生成し始める。したがって、本発明に係る姿勢角安定化装置は、ＧＰＳ受信機を利用し、カルマンフィルタを用いる高精度速度計測装置に組み込まれて、移動体の静止時における位置、速度及び方位が変化することを解消すると共に、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うようにすることができる。
【００１１】
（２） ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置が実行する方法であって、前記ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出ステップと、前記姿勢角変化量算出ステップによって算出された姿勢角の変化量を積分する積分処理ステップと、前記積分処理ステップによって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理ステップと、前記ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、前記ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定ステップと、前記速度旋回判定ステップによって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチステップと、前記姿勢角ラッチステップによってラッチされている姿勢行列の転置行列を算出する転置処理ステップと、前記転置処理ステップによって求められた転置行列と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理ステップと、前記姿勢角誤差演算処理ステップによって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出ステップにフィードバックする前記補正値とする補正制御ステップと、を備える方法。
【００１２】
したがって、（１）と同様に、本発明に係る方法は、ＧＰＳ受信機を利用し、カルマンフィルタを用いる高精度速度計測装置に組み込まれた姿勢角安定化装置によって実行され、移動体の静止時における位置、速度及び方位が変化することを解消すると共に、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うようにすることができる。
【発明の効果】
【００１３】
本発明は、ＧＰＳ受信機及びＩＭＵを使用して計測した速度からカルマンフィルタ及び自律航法アルゴリズムによって移動体の速度を算出する装置において、静止状態で算出する移動体の姿勢角を一定に安定させることができる。さらに、本発明は、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うようにすることができ、その結果、低速度、低旋回時の測定精度を向上させることが可能である。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施形態である姿勢角安定化装置の構成を示すブロック図である。
【図２】本発明の一実施形態に係る姿勢角安定化装置のハードウェア構成の一例を示す図である。
【図３】本発明の一実施形態に係る姿勢角安定化装置の処理内容を示すフローチャートである。
【図４】本発明の一実施形態に係る姿勢角安定化装置における姿勢角の出力の変化を示す図である。
【発明を実施するための形態】
【００１５】
以下、本発明の実施形態について図を参照しながら説明する。
【００１６】
図１は、本発明の一実施形態である姿勢角安定化装置１００の構成を示すブロック図である。姿勢角安定化装置１００は、姿勢角変化量算出部１０１と、積分処理部１０２と、カルマンフィルタ姿勢補正処理部１０３と、速度旋回判定部１０４と、姿勢角ラッチ部１０５と、転置処理部１０６と、姿勢角誤差演算処理部１０７と、補正制御部１０８とを備える。姿勢角安定化装置１００は、高精度速度計測装置（図示せず）の自律航法アルゴリズムを実行するストラップダウンナビゲータ内部に組み込まれる。高精度速度計測装置は、ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と計測速度とを融合する際に、姿勢角安定化装置１００に、３軸ジャイロセンサによる角速度（ω）と、カルマンフィルタからの姿勢角補正値（δθ）と、ＧＰＳ／ＩＭＵの統合速度（Ｖ）とを入力し、姿勢角安定化装置１００から方向余弦マトリクスで表される姿勢行列（Ｒｎｅｗ）を取得して、自律航法アルゴリズムによって移動体の速度を算出する。
【００１７】
高精度速度計測装置において、３軸ジャイロセンサは、移動体の３次元の角速度を計測する。カルマンフィルタは、姿勢角から姿勢角の補正値を推定演算する。
【００１８】
姿勢角変化量算出部１０１は、ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する。すなわち、姿勢角変化量算出部１０１は、数式１を計算することで、姿勢角変化量を算出する。
【００１９】
【数１】
【００２０】
積分処理部１０２は、姿勢角変化量算出部１０１によって算出された姿勢角の変化量を積分する。積分処理部１０２は、数式２の積分処理を行うが、例えば数式３のように１次近似し、離散積分処理を実施する。ここで、Ｒｎ＋１は１ステップ演算後の姿勢角であり、Ｒｎが１ステップ前の姿勢角となる。
【００２１】
【数２】
【００２２】
カルマンフィルタ姿勢補正処理部１０３は、積分処理部１０２によって積分された姿勢角と、カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力する。カルマンフィルタ姿勢補正処理部１０３は、数式４により、姿勢角をＧＰＳの位置及び速度を基準として補正処理する。ここで、Ｒｎｅｗはカルマンフィルタ姿勢補正処理後の姿勢行列、Ｉは３ｘ３の単位行列、δθはカルマンフィルタによって推定された姿勢角誤差、×は外積を意味する。
【００２３】
【数３】
【００２４】
速度旋回判定部１０４は、ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する。すなわち、速度旋回判定部１０４は、ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、ＩＭＵを使用して計測された角速度が一定の閾値未満である場合に、静止状態と判定する。同様に、速度旋回判定部１０４は、ＧＰＳ受信機によって計測された速度が一定の閾値以上であるか、又は、ＩＭＵを使用して計測された角速度が一定の閾値以上である場合に、動的状態と判定する。
【００２５】
例えば、速度旋回判定部１０４は、ＧＰＳ／ＩＭＵによる統合速度の平均値Ｖがｘｋｍ／ｈ未満、かつ、３軸ジャイロの角速度出力ωの平均値がすべて、ｙｄｅｇ／ｓ未満である場合、静止状態と判定し、静止状態フラグを出力する。速度旋回判定部１０４は、ＧＰＳ／ＩＭＵによる統合速度の平均値Ｖがｘｋｍ／ｈ以上、又は、３軸ジャイロの角速度出力ωの平均値がすべて、ｙｄｅｇ／ｓ以上である場合、動的状態と判定し、動的状態フラグを出力する。
【００２６】
姿勢角ラッチ部１０５は、速度旋回判定部１０４によって、速度又は角速度が一定の閾値以上であると判定された場合に、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列Ｒｎｅｗのラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列Ｒｎｅｗのラッチを行わずに閾値未満であると判定される前の姿勢行列のラッチを保持する。すなわち、姿勢角ラッチ部１０５は、速度旋回判定部１０４の出力結果によって、姿勢行列Ｒｎｅｗのラッチを行うか否かを決定し、姿勢行列をラッチする。例えば、速度旋回判定部１０４によって動的状態フラグが出力されている場合、姿勢角ラッチ部１０５は、姿勢行列Ｒｎｅｗを生成されるごとにラッチする。速度旋回判定部１０４によって静止状態フラグが出力されている場合、姿勢角ラッチ部１０５は、生成された姿勢行列Ｒｎｅｗのラッチを行わずに、静止状態フラグが出力される前の動的状態フラグが出力されている場合にラッチした姿勢行列を保持する。
【００２７】
転置処理部１０６は、姿勢角ラッチ部１０５によってラッチされている姿勢行列の転置行列ＲｎｅｗＴを算出する。
【００２８】
姿勢角誤差演算処理部１０７は、転置処理部１０６によって算出された転置行列ＲｎｅｗＴと、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列Ｒｎｅｗとの乗算処理を行い、姿勢角誤差を算出する。ここで、動的状態時では、姿勢角ラッチ部１０５がアクティブであり、姿勢行列Ｒｎｅｗをラッチするため、転置処理部１０６は、姿勢角ラッチ部１０５がラッチした姿勢行列ＲｎｅｗをＲｎｅｗＴに常に更新し、現在の姿勢行列Ｒｎｅｗを単純に転置した転置行列を算出する。すなわち、ＲｎｅｗＴとＲｎｅｗとは直交関係が維持されているので、数式５によって算出されるＩは、３ｘ３の単位行列である。
【００２９】
【数４】
【００３０】
一方、静止状態時では、姿勢角ラッチ部１０５は、動的状態から静止状態に遷移する前の姿勢行列Ｒを保持しているため、転置処理部１０６は、動的状態から静止状態に遷移する前の姿勢行列Ｒの転置行列ＲＴを算出する。すなわち、姿勢行列Ｒｎｅｗは、ジャイロのオフセット誤差とカルマンフィルタの補正とにより、次第に正しい姿勢からドリフトしていくのに対して、転置処理部１０６が算出したＲＴは、動的状態から静止状態に遷移する前の姿勢行列Ｒの転置行列であるので、ＲＴとＲｎｅｗとの乗算処理（数式６）を行うと、上記ドリフト分の誤差量を算出することができる。
【００３１】
【数５】
【００３２】
ここで、δＲｎｅｗは姿勢角誤差行列であり、δθ１１、δθ１２、δθ１３、δθ２１、δθ２２、δθ２３、δθ３１、δθ３２、δθ３３、がそれぞれの行列要素における誤差である。
【００３３】
補正制御部１０８は、姿勢角誤差演算処理部１０７によって算出された姿勢角誤差と、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列Ｒｎｅｗとに基づいて姿勢角を補正するための補正制御値を算出し、姿勢角変化量算出部１０１にフィードバックする補正値とする。例えば、補正制御部１０８は、姿勢角誤差演算処理部１０７によって算出されたドリフト誤差δＲｎｅｗを、生成された姿勢行列Ｒｎｅｗに基づいてＰＩ制御によって修正する。すなわち、補正制御部１０８は静止時においてのみ動作する。補正制御部１０８は、動的状態から静止状態に遷移する時の姿勢角を保持するように、数式７及び数式８によってＰＩ制御によるフィードバック制御を行う。
【００３４】
【数６】
【００３５】
数式７において、ＫＰはＰゲイン、ＫＩはＩゲインである。ｄｉｆｆＤＣＭはＰＩ制御によるフィードバック値であり、姿勢角変化量算出部１０１にフィードバックするＲＦＢに補正される。ここでＰＩ制御のフィードバックゲインはカルマンフィルタの補正とジャイロオフセットドリフトの補正をキャンセルする程度の微小な値とする。これにより、姿勢角安定化装置１００は、静止状態から動的状態に切り替わるとき、上記フィードバックの効果が得られる前に動的状態への切り替わりを行うことが可能なため、従来の技術と比較してより連続的な姿勢角及び速度応答出力を行うことができる。
【００３６】
図２は、本発明の一実施形態に係る姿勢角安定化装置１００のハードウェア構成の一例を示す図である。姿勢角安定化装置１００は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０１０、バスライン１００５、メモリ１０５０及び入出力Ｉ／Ｆ１０４０を備える。
【００３７】
ＣＰＵ１０１０は、姿勢角安定化装置１００を統括的に制御する部分であり、メモリ１０５０に記憶された各種プログラムを適宜読み出して実行することにより、上述したハードウェアと協働し、本発明に係る各種機能を実現している。
【００３８】
メモリ１０５０は、適宜読み出して実行されるプログラムを記憶し、プログラムの実行によって作成される種々の情報を記憶する。例えば、メモリ１０５０は、ＰＩ制御のためのゲイン値や、姿勢角の変化量等の各種演算結果を記憶する。
【００３９】
入出力Ｉ／Ｆ１０４０は、姿勢角安定化装置１００が高精度速度計測装置から３軸ジャイロセンサによる角速度（ω）と、カルマンフィルタからの姿勢角補正値（δθ）と、ＧＰＳ／ＩＭＵの統合速度（Ｖ）とを入力し、高精度速度計測装置に姿勢角を出力するためのインターフェースである。
【００４０】
図３は、本発明の一実施形態に係る姿勢角安定化装置１００の処理内容を示すフローチャートである。
【００４１】
ステップＳ１０１において、ＣＰＵ１０１０は、高精度速度計測装置から角速度（ω）と、カルマンフィルタからの姿勢角補正値（δθ）と、統合速度（Ｖ）とを入力する。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。
【００４２】
ステップＳ１０２において、ＣＰＵ１０１０（姿勢角変化量算出部１０１）は、入力した角速度（ω）と、フィードバックされた補正値とから姿勢角の変化量を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０３に移す。
【００４３】
ステップＳ１０３において、ＣＰＵ１０１０（積分処理部１０２）は、ステップＳ１０２によって算出された姿勢角の変化量を積分する。その後、ＣＰＵ１０１０は、処理をステップＳ１０４に移す。
【００４４】
ステップＳ１０４において、ＣＰＵ１０１０（カルマンフィルタ姿勢補正処理部１０３）は、ステップＳ１０３によって積分された姿勢角と、カルマンフィルタからの姿勢角補正値（δθ）とから姿勢行列を生成し、高精度速度計測装置に出力する。その後、ＣＰＵ１０１０は、処理をステップＳ１０５に移す。
【００４５】
ステップＳ１０５において、ＣＰＵ１０１０（速度旋回判定部１０４）は、静止状態か否かを判断する。より具体的には、ＣＰＵ１０１０は、入力した統合速度（Ｖ）が一定の閾値未満であり、かつ、入力した角速度（ω）が一定の閾値未満であるか否かを判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１０６に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１０７に移す。
【００４６】
ステップＳ１０６において、ＣＰＵ１０１０（姿勢角ラッチ部１０５）は、ステップＳ１０４によって生成された姿勢行列Ｒｎｅｗを記憶しないで、閾値未満であると判定される前の姿勢行列の記憶を保持する。その後、ＣＰＵ１０１０は、処理をステップＳ１０８に移す。
【００４７】
ステップＳ１０７において、ＣＰＵ１０１０（姿勢角ラッチ部１０５）は、ステップＳ１０４によって生成された姿勢行列Ｒｎｅｗを記憶する。その後、ＣＰＵ１０１０は、処理をステップＳ１０８に移す。
【００４８】
ステップＳ１０８において、ＣＰＵ１０１０（転置処理部１０６）は、ステップＳ１０６又はステップＳ１０７によって記憶されている姿勢行列の転置行列ＲｎｅｗＴを算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０９に移す。
【００４９】
ステップＳ１０９において、ＣＰＵ１０１０（姿勢角誤差演算処理部１０７）は、ステップＳ１０８において算出された転置行列ＲｎｅｗＴと、ステップＳ１０４において生成された姿勢行列Ｒｎｅｗとの乗算処理を行い、姿勢角誤差を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１１０に移す。
【００５０】
ステップＳ１１０において、ＣＰＵ１０１０（補正制御部１０８）は、ステップＳ１０９において算出された姿勢角誤差と、ステップＳ１０４において生成された姿勢行列Ｒｎｅｗとに基づいて姿勢角を補正するための補正制御値を算出し、ステップＳ１０２にフィードバックする補正値とする。その後、ＣＰＵ１０１０は、処理をステップＳ１０１に移す。
【００５１】
図４は、本発明の一実施形態に係る姿勢角安定化装置１００における姿勢角の出力の変化を示す図である。図４（ａ）は、移動体の速度の変化を示し、静止状態の時間を示している。図４（ｂ）は、従来において算出される姿勢角の変化を示す図である。図４（ｃ）は、姿勢角安定化装置１００において算出される姿勢角の変化を示す図である。
【００５２】
図４（ｂ）の静止状態において、姿勢角の角度ドリフトは次第に大きくなっていることが示されている。一方、図４（ｃ）の静止状態において、姿勢角安定化装置１００によって、移動体の静止時における角度ドリフトが次第に大きくなることが解消され、静止状態であっても角度ドリフトは小さく、一定幅で安定している。
【００５３】
本実施例によれば、姿勢角安定化装置１００は、ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出部１０１と、姿勢角変化量算出部１０１によって算出された姿勢角の変化量を積分する積分処理部１０２と、積分処理部１０２によって積分された姿勢角と、カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理部１０３と、ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定部１０４と、速度旋回判定部１０４によって、速度又は角速度が一定の閾値以上であると判定された場合に、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、生成された姿勢行列のラッチを行わずに閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチ部１０５と、姿勢角ラッチ部１０５によってラッチされている姿勢行列の転置行列を算出する転置処理部１０６と、転置処理部１０６によって算出された転置行列と、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理部１０７と、姿勢角誤差演算処理部１０７によって算出された姿勢角誤差と、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、姿勢角変化量算出部１０１にフィードバックする補正値とする補正制御部１０８と、を備える。したがって、姿勢角安定化装置１００は、ＧＰＳ受信機を利用し、カルマンフィルタを用いる高精度速度計測装置に組み込まれて、移動体の静止時における位置、速度及び方位が変化することを解消すると共に、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うようにすることができ、その結果、低速度、低旋回時の測定精度を向上させることが可能である。
【００５４】
以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限るものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されるものではない。
【符号の説明】
【００５５】
１００ 姿勢角安定化装置
１０１ 姿勢角変化量算出部
１０２ 積分処理部
１０３ カルマンフィルタ姿勢補正処理部
１０４ 速度旋回判定部
１０５ 姿勢角ラッチ部
１０６ 転置処理部
１０７ 姿勢角誤差演算処理部
１０８ 補正制御部
【技術分野】
【０００１】
本発明は、姿勢角安定化装置及び方法に関する。
【背景技術】
【０００２】
従来より、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）／ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）複合航法によって移動体の位置、速度及び方位等を演算するＧＰＳ複合航法装置では、誤差の推定にカルマンフィルタが広く使用されている。カルマンフィルタでは、移動体の位置、速度及び方位等をモデルによって推定するが、移動体の移動中と静止中とで、モデルが大きく異なる。特に、移動体の静止時において、カルマンフィルタは観測量の可観測性問題によって推定精度が劣化してしまう。移動体が静止し続けた場合（例えば、移動体が赤信号で止まる等）、移動体の位置、速度及び方位等が次第にドリフトするという不具合が生じる。また、移動中と静止中とでモデルを切り替えると、静止時から動的状態に切り替わるときに姿勢角、速度の応答性が劣化して非連続な出力となってしまう。
【０００３】
このような不具合について、ＧＰＳ複合航法装置において、移動体の静止時における推定位置、推定速度及び推定方位の変化を解消すると同時に、静止時から移動時における応答性の劣化を解消する技術を開示する特許文献１が知られている。特許文献１に開示されたＧＰＳ複合航法装置は、移動体の静止を判定する静止判定部を設け、該静止判定部で静止状態と判定されたとき、カルマンフィルタの観測更新に用いる観測モデルを変更すると共に、更新による誤差共分散行列の変化分を補正する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００８−２３２８６９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
このような技術が提案されているが、ＧＰＳ／ＩＭＵ複合航法を利用した高精度速度計測装置において、移動体の静止時における位置、速度及び方位の変化を解消し、静止時から移動時に切り替わるときであっても、従来技術よりもさらに連続的な姿勢角及び速度応答出力を行うことができるようにする装置が求められている。
【０００６】
本発明は、ＧＰＳ受信機を利用し、カルマンフィルタを用いる高精度速度計測装置に組み込まれて、移動体の静止時における位置、速度及び方位の変化を解消すると共に、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うことができるようにする姿勢角安定化装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明では、以下のような解決手段を提供する。
【０００８】
（１） ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置であって、前記ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出手段と、前記姿勢角変化量算出手段によって算出された姿勢角の変化量を積分する積分処理手段と、前記積分処理手段によって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理手段と、前記ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、前記ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定手段と、前記速度旋回判定手段によって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチ手段と、前記姿勢角ラッチ手段によってラッチされている姿勢行列の転置行列を算出する転置処理手段と、前記転置処理手段によって転置された姿勢行列と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理手段と、前記姿勢角誤差演算処理手段によって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出手段にフィードバックする前記補正値とする補正制御手段と、を備える姿勢角安定化装置。
【０００９】
（１）の構成によれば、本発明に係る姿勢角安定化装置は、ＩＭＵを使用して計測された移動体の角速度と、フィードバックした補正値とから姿勢角の変化量を算出し、算出した姿勢角の変化量を積分し、積分した姿勢角と、カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力する。そして、姿勢角安定化装置は、ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定し、速度又は角速度が一定の閾値以上であると判定した場合に、生成した姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定した場合に、生成した姿勢行列のラッチを行わずに閾値未満であると判定する前の姿勢行列のラッチを保持し、ラッチしている姿勢行列の転置行列を算出し、算出した転置行列と、生成した姿勢行列との乗算処理を行い、姿勢角誤差を算出し、算出した姿勢角誤差と、生成した姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、フィードバックする補正値とする。
【００１０】
すなわち、本発明に係る姿勢角安定化装置は、移動体の移動と静止との判定を行い、移動体が静止すると、移動から静止に移る前の姿勢行列を保持し、保持した姿勢行列を用いて姿勢角を補正制御し、移動し始めると、保持した姿勢行列を用いて姿勢行列を生成し始める。したがって、本発明に係る姿勢角安定化装置は、ＧＰＳ受信機を利用し、カルマンフィルタを用いる高精度速度計測装置に組み込まれて、移動体の静止時における位置、速度及び方位が変化することを解消すると共に、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うようにすることができる。
【００１１】
（２） ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置が実行する方法であって、前記ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出ステップと、前記姿勢角変化量算出ステップによって算出された姿勢角の変化量を積分する積分処理ステップと、前記積分処理ステップによって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理ステップと、前記ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、前記ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定ステップと、前記速度旋回判定ステップによって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチステップと、前記姿勢角ラッチステップによってラッチされている姿勢行列の転置行列を算出する転置処理ステップと、前記転置処理ステップによって求められた転置行列と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理ステップと、前記姿勢角誤差演算処理ステップによって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出ステップにフィードバックする前記補正値とする補正制御ステップと、を備える方法。
【００１２】
したがって、（１）と同様に、本発明に係る方法は、ＧＰＳ受信機を利用し、カルマンフィルタを用いる高精度速度計測装置に組み込まれた姿勢角安定化装置によって実行され、移動体の静止時における位置、速度及び方位が変化することを解消すると共に、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うようにすることができる。
【発明の効果】
【００１３】
本発明は、ＧＰＳ受信機及びＩＭＵを使用して計測した速度からカルマンフィルタ及び自律航法アルゴリズムによって移動体の速度を算出する装置において、静止状態で算出する移動体の姿勢角を一定に安定させることができる。さらに、本発明は、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うようにすることができ、その結果、低速度、低旋回時の測定精度を向上させることが可能である。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施形態である姿勢角安定化装置の構成を示すブロック図である。
【図２】本発明の一実施形態に係る姿勢角安定化装置のハードウェア構成の一例を示す図である。
【図３】本発明の一実施形態に係る姿勢角安定化装置の処理内容を示すフローチャートである。
【図４】本発明の一実施形態に係る姿勢角安定化装置における姿勢角の出力の変化を示す図である。
【発明を実施するための形態】
【００１５】
以下、本発明の実施形態について図を参照しながら説明する。
【００１６】
図１は、本発明の一実施形態である姿勢角安定化装置１００の構成を示すブロック図である。姿勢角安定化装置１００は、姿勢角変化量算出部１０１と、積分処理部１０２と、カルマンフィルタ姿勢補正処理部１０３と、速度旋回判定部１０４と、姿勢角ラッチ部１０５と、転置処理部１０６と、姿勢角誤差演算処理部１０７と、補正制御部１０８とを備える。姿勢角安定化装置１００は、高精度速度計測装置（図示せず）の自律航法アルゴリズムを実行するストラップダウンナビゲータ内部に組み込まれる。高精度速度計測装置は、ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と計測速度とを融合する際に、姿勢角安定化装置１００に、３軸ジャイロセンサによる角速度（ω）と、カルマンフィルタからの姿勢角補正値（δθ）と、ＧＰＳ／ＩＭＵの統合速度（Ｖ）とを入力し、姿勢角安定化装置１００から方向余弦マトリクスで表される姿勢行列（Ｒｎｅｗ）を取得して、自律航法アルゴリズムによって移動体の速度を算出する。
【００１７】
高精度速度計測装置において、３軸ジャイロセンサは、移動体の３次元の角速度を計測する。カルマンフィルタは、姿勢角から姿勢角の補正値を推定演算する。
【００１８】
姿勢角変化量算出部１０１は、ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する。すなわち、姿勢角変化量算出部１０１は、数式１を計算することで、姿勢角変化量を算出する。
【００１９】
【数１】
【００２０】
積分処理部１０２は、姿勢角変化量算出部１０１によって算出された姿勢角の変化量を積分する。積分処理部１０２は、数式２の積分処理を行うが、例えば数式３のように１次近似し、離散積分処理を実施する。ここで、Ｒｎ＋１は１ステップ演算後の姿勢角であり、Ｒｎが１ステップ前の姿勢角となる。
【００２１】
【数２】
【００２２】
カルマンフィルタ姿勢補正処理部１０３は、積分処理部１０２によって積分された姿勢角と、カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力する。カルマンフィルタ姿勢補正処理部１０３は、数式４により、姿勢角をＧＰＳの位置及び速度を基準として補正処理する。ここで、Ｒｎｅｗはカルマンフィルタ姿勢補正処理後の姿勢行列、Ｉは３ｘ３の単位行列、δθはカルマンフィルタによって推定された姿勢角誤差、×は外積を意味する。
【００２３】
【数３】
【００２４】
速度旋回判定部１０４は、ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する。すなわち、速度旋回判定部１０４は、ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、ＩＭＵを使用して計測された角速度が一定の閾値未満である場合に、静止状態と判定する。同様に、速度旋回判定部１０４は、ＧＰＳ受信機によって計測された速度が一定の閾値以上であるか、又は、ＩＭＵを使用して計測された角速度が一定の閾値以上である場合に、動的状態と判定する。
【００２５】
例えば、速度旋回判定部１０４は、ＧＰＳ／ＩＭＵによる統合速度の平均値Ｖがｘｋｍ／ｈ未満、かつ、３軸ジャイロの角速度出力ωの平均値がすべて、ｙｄｅｇ／ｓ未満である場合、静止状態と判定し、静止状態フラグを出力する。速度旋回判定部１０４は、ＧＰＳ／ＩＭＵによる統合速度の平均値Ｖがｘｋｍ／ｈ以上、又は、３軸ジャイロの角速度出力ωの平均値がすべて、ｙｄｅｇ／ｓ以上である場合、動的状態と判定し、動的状態フラグを出力する。
【００２６】
姿勢角ラッチ部１０５は、速度旋回判定部１０４によって、速度又は角速度が一定の閾値以上であると判定された場合に、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列Ｒｎｅｗのラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列Ｒｎｅｗのラッチを行わずに閾値未満であると判定される前の姿勢行列のラッチを保持する。すなわち、姿勢角ラッチ部１０５は、速度旋回判定部１０４の出力結果によって、姿勢行列Ｒｎｅｗのラッチを行うか否かを決定し、姿勢行列をラッチする。例えば、速度旋回判定部１０４によって動的状態フラグが出力されている場合、姿勢角ラッチ部１０５は、姿勢行列Ｒｎｅｗを生成されるごとにラッチする。速度旋回判定部１０４によって静止状態フラグが出力されている場合、姿勢角ラッチ部１０５は、生成された姿勢行列Ｒｎｅｗのラッチを行わずに、静止状態フラグが出力される前の動的状態フラグが出力されている場合にラッチした姿勢行列を保持する。
【００２７】
転置処理部１０６は、姿勢角ラッチ部１０５によってラッチされている姿勢行列の転置行列ＲｎｅｗＴを算出する。
【００２８】
姿勢角誤差演算処理部１０７は、転置処理部１０６によって算出された転置行列ＲｎｅｗＴと、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列Ｒｎｅｗとの乗算処理を行い、姿勢角誤差を算出する。ここで、動的状態時では、姿勢角ラッチ部１０５がアクティブであり、姿勢行列Ｒｎｅｗをラッチするため、転置処理部１０６は、姿勢角ラッチ部１０５がラッチした姿勢行列ＲｎｅｗをＲｎｅｗＴに常に更新し、現在の姿勢行列Ｒｎｅｗを単純に転置した転置行列を算出する。すなわち、ＲｎｅｗＴとＲｎｅｗとは直交関係が維持されているので、数式５によって算出されるＩは、３ｘ３の単位行列である。
【００２９】
【数４】
【００３０】
一方、静止状態時では、姿勢角ラッチ部１０５は、動的状態から静止状態に遷移する前の姿勢行列Ｒを保持しているため、転置処理部１０６は、動的状態から静止状態に遷移する前の姿勢行列Ｒの転置行列ＲＴを算出する。すなわち、姿勢行列Ｒｎｅｗは、ジャイロのオフセット誤差とカルマンフィルタの補正とにより、次第に正しい姿勢からドリフトしていくのに対して、転置処理部１０６が算出したＲＴは、動的状態から静止状態に遷移する前の姿勢行列Ｒの転置行列であるので、ＲＴとＲｎｅｗとの乗算処理（数式６）を行うと、上記ドリフト分の誤差量を算出することができる。
【００３１】
【数５】
【００３２】
ここで、δＲｎｅｗは姿勢角誤差行列であり、δθ１１、δθ１２、δθ１３、δθ２１、δθ２２、δθ２３、δθ３１、δθ３２、δθ３３、がそれぞれの行列要素における誤差である。
【００３３】
補正制御部１０８は、姿勢角誤差演算処理部１０７によって算出された姿勢角誤差と、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列Ｒｎｅｗとに基づいて姿勢角を補正するための補正制御値を算出し、姿勢角変化量算出部１０１にフィードバックする補正値とする。例えば、補正制御部１０８は、姿勢角誤差演算処理部１０７によって算出されたドリフト誤差δＲｎｅｗを、生成された姿勢行列Ｒｎｅｗに基づいてＰＩ制御によって修正する。すなわち、補正制御部１０８は静止時においてのみ動作する。補正制御部１０８は、動的状態から静止状態に遷移する時の姿勢角を保持するように、数式７及び数式８によってＰＩ制御によるフィードバック制御を行う。
【００３４】
【数６】
【００３５】
数式７において、ＫＰはＰゲイン、ＫＩはＩゲインである。ｄｉｆｆＤＣＭはＰＩ制御によるフィードバック値であり、姿勢角変化量算出部１０１にフィードバックするＲＦＢに補正される。ここでＰＩ制御のフィードバックゲインはカルマンフィルタの補正とジャイロオフセットドリフトの補正をキャンセルする程度の微小な値とする。これにより、姿勢角安定化装置１００は、静止状態から動的状態に切り替わるとき、上記フィードバックの効果が得られる前に動的状態への切り替わりを行うことが可能なため、従来の技術と比較してより連続的な姿勢角及び速度応答出力を行うことができる。
【００３６】
図２は、本発明の一実施形態に係る姿勢角安定化装置１００のハードウェア構成の一例を示す図である。姿勢角安定化装置１００は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０１０、バスライン１００５、メモリ１０５０及び入出力Ｉ／Ｆ１０４０を備える。
【００３７】
ＣＰＵ１０１０は、姿勢角安定化装置１００を統括的に制御する部分であり、メモリ１０５０に記憶された各種プログラムを適宜読み出して実行することにより、上述したハードウェアと協働し、本発明に係る各種機能を実現している。
【００３８】
メモリ１０５０は、適宜読み出して実行されるプログラムを記憶し、プログラムの実行によって作成される種々の情報を記憶する。例えば、メモリ１０５０は、ＰＩ制御のためのゲイン値や、姿勢角の変化量等の各種演算結果を記憶する。
【００３９】
入出力Ｉ／Ｆ１０４０は、姿勢角安定化装置１００が高精度速度計測装置から３軸ジャイロセンサによる角速度（ω）と、カルマンフィルタからの姿勢角補正値（δθ）と、ＧＰＳ／ＩＭＵの統合速度（Ｖ）とを入力し、高精度速度計測装置に姿勢角を出力するためのインターフェースである。
【００４０】
図３は、本発明の一実施形態に係る姿勢角安定化装置１００の処理内容を示すフローチャートである。
【００４１】
ステップＳ１０１において、ＣＰＵ１０１０は、高精度速度計測装置から角速度（ω）と、カルマンフィルタからの姿勢角補正値（δθ）と、統合速度（Ｖ）とを入力する。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。
【００４２】
ステップＳ１０２において、ＣＰＵ１０１０（姿勢角変化量算出部１０１）は、入力した角速度（ω）と、フィードバックされた補正値とから姿勢角の変化量を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０３に移す。
【００４３】
ステップＳ１０３において、ＣＰＵ１０１０（積分処理部１０２）は、ステップＳ１０２によって算出された姿勢角の変化量を積分する。その後、ＣＰＵ１０１０は、処理をステップＳ１０４に移す。
【００４４】
ステップＳ１０４において、ＣＰＵ１０１０（カルマンフィルタ姿勢補正処理部１０３）は、ステップＳ１０３によって積分された姿勢角と、カルマンフィルタからの姿勢角補正値（δθ）とから姿勢行列を生成し、高精度速度計測装置に出力する。その後、ＣＰＵ１０１０は、処理をステップＳ１０５に移す。
【００４５】
ステップＳ１０５において、ＣＰＵ１０１０（速度旋回判定部１０４）は、静止状態か否かを判断する。より具体的には、ＣＰＵ１０１０は、入力した統合速度（Ｖ）が一定の閾値未満であり、かつ、入力した角速度（ω）が一定の閾値未満であるか否かを判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１０６に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１０７に移す。
【００４６】
ステップＳ１０６において、ＣＰＵ１０１０（姿勢角ラッチ部１０５）は、ステップＳ１０４によって生成された姿勢行列Ｒｎｅｗを記憶しないで、閾値未満であると判定される前の姿勢行列の記憶を保持する。その後、ＣＰＵ１０１０は、処理をステップＳ１０８に移す。
【００４７】
ステップＳ１０７において、ＣＰＵ１０１０（姿勢角ラッチ部１０５）は、ステップＳ１０４によって生成された姿勢行列Ｒｎｅｗを記憶する。その後、ＣＰＵ１０１０は、処理をステップＳ１０８に移す。
【００４８】
ステップＳ１０８において、ＣＰＵ１０１０（転置処理部１０６）は、ステップＳ１０６又はステップＳ１０７によって記憶されている姿勢行列の転置行列ＲｎｅｗＴを算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０９に移す。
【００４９】
ステップＳ１０９において、ＣＰＵ１０１０（姿勢角誤差演算処理部１０７）は、ステップＳ１０８において算出された転置行列ＲｎｅｗＴと、ステップＳ１０４において生成された姿勢行列Ｒｎｅｗとの乗算処理を行い、姿勢角誤差を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１１０に移す。
【００５０】
ステップＳ１１０において、ＣＰＵ１０１０（補正制御部１０８）は、ステップＳ１０９において算出された姿勢角誤差と、ステップＳ１０４において生成された姿勢行列Ｒｎｅｗとに基づいて姿勢角を補正するための補正制御値を算出し、ステップＳ１０２にフィードバックする補正値とする。その後、ＣＰＵ１０１０は、処理をステップＳ１０１に移す。
【００５１】
図４は、本発明の一実施形態に係る姿勢角安定化装置１００における姿勢角の出力の変化を示す図である。図４（ａ）は、移動体の速度の変化を示し、静止状態の時間を示している。図４（ｂ）は、従来において算出される姿勢角の変化を示す図である。図４（ｃ）は、姿勢角安定化装置１００において算出される姿勢角の変化を示す図である。
【００５２】
図４（ｂ）の静止状態において、姿勢角の角度ドリフトは次第に大きくなっていることが示されている。一方、図４（ｃ）の静止状態において、姿勢角安定化装置１００によって、移動体の静止時における角度ドリフトが次第に大きくなることが解消され、静止状態であっても角度ドリフトは小さく、一定幅で安定している。
【００５３】
本実施例によれば、姿勢角安定化装置１００は、ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出部１０１と、姿勢角変化量算出部１０１によって算出された姿勢角の変化量を積分する積分処理部１０２と、積分処理部１０２によって積分された姿勢角と、カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理部１０３と、ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定部１０４と、速度旋回判定部１０４によって、速度又は角速度が一定の閾値以上であると判定された場合に、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、生成された姿勢行列のラッチを行わずに閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチ部１０５と、姿勢角ラッチ部１０５によってラッチされている姿勢行列の転置行列を算出する転置処理部１０６と、転置処理部１０６によって算出された転置行列と、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理部１０７と、姿勢角誤差演算処理部１０７によって算出された姿勢角誤差と、カルマンフィルタ姿勢補正処理部１０３によって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、姿勢角変化量算出部１０１にフィードバックする補正値とする補正制御部１０８と、を備える。したがって、姿勢角安定化装置１００は、ＧＰＳ受信機を利用し、カルマンフィルタを用いる高精度速度計測装置に組み込まれて、移動体の静止時における位置、速度及び方位が変化することを解消すると共に、静止状態から動的状態に切り替わるときであっても、連続的な姿勢角及び速度応答出力を行うようにすることができ、その結果、低速度、低旋回時の測定精度を向上させることが可能である。
【００５４】
以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限るものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されるものではない。
【符号の説明】
【００５５】
１００ 姿勢角安定化装置
１０１ 姿勢角変化量算出部
１０２ 積分処理部
１０３ カルマンフィルタ姿勢補正処理部
１０４ 速度旋回判定部
１０５ 姿勢角ラッチ部
１０６ 転置処理部
１０７ 姿勢角誤差演算処理部
１０８ 補正制御部
【特許請求の範囲】
【請求項１】
ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置であって、
前記ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出手段と、
前記姿勢角変化量算出手段によって算出された姿勢角の変化量を積分する積分処理手段と、
前記積分処理手段によって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理手段と、
前記ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、前記ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定手段と、
前記速度旋回判定手段によって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチ手段と、
前記姿勢角ラッチ手段によってラッチされている姿勢行列の転置行列を算出する転置処理手段と、
前記転置処理手段によって算出された転置行列と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理手段と、
前記姿勢角誤差演算処理手段によって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出手段にフィードバックする前記補正値とする補正制御手段と、
を備える姿勢角安定化装置。
【請求項２】
ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置が実行する方法であって、
前記ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出ステップと、
前記姿勢角変化量算出ステップによって算出された姿勢角の変化量を積分する積分処理ステップと、
前記積分処理ステップによって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理ステップと、
前記ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、前記ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定ステップと、
前記速度旋回判定ステップによって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチステップと、
前記姿勢角ラッチステップによってラッチされている姿勢行列の転置行列を算出する転置処理ステップと、
前記転置処理ステップによって算出された転置行列と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理ステップと、
前記姿勢角誤差演算処理ステップによって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出ステップにフィードバックする前記補正値とする補正制御ステップと、
を備える方法。
【請求項１】
ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置であって、
前記ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出手段と、
前記姿勢角変化量算出手段によって算出された姿勢角の変化量を積分する積分処理手段と、
前記積分処理手段によって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理手段と、
前記ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、前記ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定手段と、
前記速度旋回判定手段によって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチ手段と、
前記姿勢角ラッチ手段によってラッチされている姿勢行列の転置行列を算出する転置処理手段と、
前記転置処理手段によって算出された転置行列と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理手段と、
前記姿勢角誤差演算処理手段によって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出手段にフィードバックする前記補正値とする補正制御手段と、
を備える姿勢角安定化装置。
【請求項２】
ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置が実行する方法であって、
前記ＩＭＵを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出ステップと、
前記姿勢角変化量算出ステップによって算出された姿勢角の変化量を積分する積分処理ステップと、
前記積分処理ステップによって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理ステップと、
前記ＧＰＳ受信機によって計測された速度が一定の閾値未満であり、かつ、前記ＩＭＵを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定ステップと、
前記速度旋回判定ステップによって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチステップと、
前記姿勢角ラッチステップによってラッチされている姿勢行列の転置行列を算出する転置処理ステップと、
前記転置処理ステップによって算出された転置行列と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理ステップと、
前記姿勢角誤差演算処理ステップによって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出ステップにフィードバックする前記補正値とする補正制御ステップと、
を備える方法。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２０１２−４７４９５（Ｐ２０１２−４７４９５Ａ）
【公開日】平成２４年３月８日（２０１２．３．８）
【国際特許分類】
【出願番号】特願２０１０−１８７５５３（Ｐ２０１０−１８７５５３）
【出願日】平成２２年８月２４日（２０１０．８．２４）
【出願人】（０００１４５８０６）株式会社小野測器 (230)
【Ｆターム（参考）】
【公開日】平成２４年３月８日（２０１２．３．８）
【国際特許分類】
【出願日】平成２２年８月２４日（２０１０．８．２４）
【出願人】（０００１４５８０６）株式会社小野測器 (230)
【Ｆターム（参考）】
[ Back to top ]