ヨー角初期化異常判定装置及び方法
【課題】慣性装置が搭載された車両が移動するような場合に、ヨー角初期化が異常であるか否かを容易に判定することができるヨー角初期化異常判定装置及び方法を提供すること。
【解決手段】ヨー角初期化異常判定装置２００は、ＧＰＳ受信機によって算出された方位角の平均値を算出し、ＩＭＵによって計測されたヨー角度の平均値を算出し、アルゴリズム速度によって移動体の移動方向が前進か後退かを判定し、判定した移動方向に基づいて、方位角の平均値と、ヨー角度の平均値との差分を算出し、算出した差分が閾値以上であるか未満であるかを判断する。さらに、算出した差分が閾値以上であると判断した場合にカウントをインクリメントし、算出した差分が閾値未満であると判断した場合にカウントをデクリメントし、計数したカウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する。
【解決手段】ヨー角初期化異常判定装置２００は、ＧＰＳ受信機によって算出された方位角の平均値を算出し、ＩＭＵによって計測されたヨー角度の平均値を算出し、アルゴリズム速度によって移動体の移動方向が前進か後退かを判定し、判定した移動方向に基づいて、方位角の平均値と、ヨー角度の平均値との差分を算出し、算出した差分が閾値以上であるか未満であるかを判断する。さらに、算出した差分が閾値以上であると判断した場合にカウントをインクリメントし、算出した差分が閾値未満であると判断した場合にカウントをデクリメントし、計数したカウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ヨー角初期化異常判定装置及び方法に関する。
【背景技術】
【０００２】
従来より、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）／ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）統合システムによって移動体の位置、速度及び方位等を演算するＧＰＳ／ＩＭＵ統合装置では、ＩＭＵの初期姿勢角、特にヨー角度を決定する必要がある。例えば、高精度ジャイロセンサを備えていれば、地球の自転を計測できる感度があるため、ジャイロコンパスという方法でヨー角度を高精度に決定できる。これに対して、ＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ）ＩＭＵ等の低精度ジャイロセンサしか備えていない場合、別手段で初期ヨー角度を決定する必要がある。
【０００３】
例えば、慣性装置の初期アライメント計算時においてジャイロ誤差にもとづく方位角誤差の増加を抑圧する技術を開示する特許文献１が知られている。特許文献１に開示された慣性装置は、初期アライメント計算部に方位角補正演算部を設け、初期アライメント計算中に、計算した方位角ΨＣを方位角補正演算部に入力して、その出力を姿勢基準行列計算部に帰還補正して、方位角ΨＣがゼロに収斂するようにする。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００１−１５３６８０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１の慣性装置で行われる初期アライメント計算は、慣性装置が搭載された機体が移動しないという条件下で計算されることを利用して、方位角ΨＣがゼロになるように補正量を計算するので、慣性装置が搭載された車両が移動するような場合に利用することができない。
【０００６】
例えば、移動体の進行方向とＩＭＵの軸が同軸上に配置されているＧＰＳ／ＩＭＵ統合装置において、初期アライメント時にＧＰＳ方位角からＩＭＵ初期ヨー角を近似的に決定する際、ＧＰＳ方位角がマルチパスの影響により、真の値からずれてしまっているとき、初期ヨー角は誤った値にて初期化されてしまう。また、移動体が前進方向に進まず、後退方向に進行してしまったとき、ＧＰＳ方位角と初期ヨー角は１８０°ずれてしまうため、初期ヨー角は１８０°ずれた誤った値にて初期化されてしまう。また、一般的にＧＰＳによる位置及び速度と、ＧＰＳ／ＩＭＵ統合システムによる位置及び速度との差分から、初期アライメントの異常を判定することが可能であるが、ＧＰＳによる位置及び速度と、ＧＰＳ／ＩＭＵ統合システムによる位置及び速度との差が生じずに、姿勢角だけが異常な値を出力し続けるという状況が生じてしまう可能性がある。このような場合、位置及び速度の差分から初期アライメントの異常を判定するだけでは不十分である。
【０００７】
そこで、慣性装置が搭載された車両が移動するような場合に、ヨー角初期化が異常であるか否かを容易に判定することができる装置及び方法が求められている。
【０００８】
本発明は、慣性装置が搭載された車両が移動するような場合に、ヨー角初期化が異常であるか否かを容易に判定することができるヨー角初期化異常判定装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明では、以下のような解決手段を提供する。
【００１０】
（１） ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれており、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられるヨー角初期化異常判定装置において、前記ＧＰＳ受信機によって算出された方位角の平均値を算出するＧＰＳ方位角平均値演算手段と、前記ＩＭＵによって計測されたヨー角度の平均値を算出するヨー角度平均値演算手段と、前記アルゴリズム速度によって前記移動体の移動方向が前進か後退かを判定する逆位相判定手段と、前記逆位相判定手段によって判定された前記移動方向に基づいて、前記方位角の平均値と、前記ヨー角度の平均値との差分を算出する差分演算手段と、前記差分演算手段によって算出された差分が閾値以上であるか未満であるかを判断する誤差閾値判断手段と、前記誤差閾値判断手段によって、前記算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、前記算出された差分が閾値未満であると判断された場合にカウントをデクリメントするカウント手段と、前記カウント手段によって計数された前記カウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する異常判定手段と、を備えるヨー角初期化異常判定装置。
【００１１】
（１）の構成によれば、本発明に係るヨー角初期化異常判定装置は、ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれ、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられる。そして、ヨー角初期化異常判定装置は、ＧＰＳ受信機によって算出された方位角の平均値を算出し、ＩＭＵによって計測されたヨー角度の平均値を算出し、アルゴリズム速度によって移動体の移動方向が前進か後退かを判定し、判定した移動方向に基づいて、方位角の平均値と、ヨー角度の平均値との差分を算出し、算出した差分が閾値以上であるか未満であるかを判断する。さらに、ヨー角初期化異常判定装置は、算出した差分が閾値以上であると判断した場合にカウントをインクリメントし、算出した差分が閾値未満であると判断した場合にカウントをデクリメントし、計数したカウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する。
【００１２】
すなわち、本発明に係るヨー角初期化異常判定装置は、慣性装置が搭載された車両の移動方向に基づいて、ＧＰＳによる方位角の平均値と、ＩＭＵによるヨー角度の平均値との差分を算出し、算出した差分が閾値以上であるか未満であるかによって計数したカウントが一定の値以上であると判断した場合に、異常状態と判定する。したがって、本発明に係るヨー角初期化異常判定装置は、ヨー角初期化において、車両が前後に移動した後に前進したり、後進したりする場合に、ヨー角初期化が異常であるか否かを容易に判定することができる。
【００１３】
（２） ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれており、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられるヨー角初期化異常判定装置が実行する方法において、前記ＧＰＳ受信機によって算出された方位角の平均値を算出するＧＰＳ方位角平均値演算ステップと、前記ＩＭＵによって計測されたヨー角度の平均値を算出するヨー角度平均値演算ステップと、前記アルゴリズム速度によって前記移動体の移動方向が前進か後退かを判定する逆位相判定ステップと、前記逆位相判定ステップによって判定された前記移動方向に基づいて、前記方位角の平均値と、前記ヨー角度の平均値との差分を算出する差分演算ステップと、前記差分演算ステップによって算出された差分が閾値以上であるか未満であるかを判断する誤差閾値判断ステップと、前記誤差閾値判断ステップによって、前記算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、前記算出された差分が閾値未満であると判断された場合にカウントをデクリメントするカウントステップと、前記カウントステップによって計数された前記カウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する異常判定ステップと、を備える方法。
【００１４】
したがって、（２）に係る方法は、（１）と同様に、ヨー角初期化において、車両が前後に移動した後に前進したり、後進したりする場合に、ヨー角初期化が異常であるか否かを容易に判定することができる。
【発明の効果】
【００１５】
本発明は、慣性装置が搭載された車両が前後のみに移動するような場合に、ヨー角初期化が異常であるか否かを容易に判定することができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の一実施形態であるヨー角初期化異常判定装置の構成を示すブロック図である。
【図２】本発明の一実施形態に係るヨー角初期化異常判定装置のハードウェア構成の一例を示す図である。
【図３】本発明の一実施形態に係るヨー角初期化異常判定装置の処理内容を示すフローチャートである。
【発明を実施するための形態】
【００１７】
以下、本発明の実施形態について図を参照しながら説明する。
【００１８】
図１は、本発明の一実施形態であるヨー角初期化異常判定装置２００の構成を示すブロック図である。ヨー角初期化異常判定装置２００は、ＧＰＳ方位角平均値演算部２０１と、ヨー角度平均値演算部２０２と、逆位相判定部２０３と、差分演算部２０４と、誤差閾値判断部２０５と、カウント部２０６と、異常判定部２０７とを備える。例えば、ヨー角初期化異常判定装置２００は、ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する高精度車両速度計測装置（図示せず）に組み込まれ、車両の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられる。そして、ヨー角初期化異常判定装置２００は、入力として、ＧＰＳ方位角（θＧＰＳ：−π〜π）と、ＩＭＵヨー角度（θｙａｗ：−π〜π）と、ＧＰＳ／ＩＭＵ統合速度（Ｖｂｏｄｙ：ＢＯＤＹ座標系進行方向速度）とが与えられ、出力として、ヨー角度の正常又は異常判定を行う。
【００１９】
ＧＰＳ方位角平均値演算部２０１は、ＧＰＳ受信機によって算出された方位角の平均値を算出する。すなわち、ＧＰＳ方位角平均値演算部２０１は、ＧＰＳ／ＩＭＵ統合装置（例えば、高精度車両速度計測装置）によって入力されるＧＰＳ方位角θＧＰＳのある一定時間における平均値θＧＰＳ平均を算出する。ただし、θＧＰＳは−π〜πの非連続な範囲であるため、そのままθＧＰＳを平均した場合は正しくない値となってしまう。そこで、ｓｉｎθＧＰＳ、ｃｏｓθＧＰＳした値を平均化し、その出力に対し、ｔａｎ−１変換することで、θＧＰＳの正しい平均値であるθＧＰＳ平均を求めることが可能となる。
【００２０】
ヨー角度平均値演算部２０２は、ＩＭＵによって計測されたヨー角度の平均値を算出する。すなわち、ヨー角度平均値演算部２０２は、ＧＰＳ方位角平均値演算部２０１と同様の処理を行い、ＧＰＳ／ＩＭＵ統合装置（例えば、高精度車両速度計測装置）によって入力されるヨー角度θｙａｗの一定時間における平均値θｙａｗ平均を算出する。
【００２１】
逆位相判定部２０３は、自律航法アルゴリズムによって算出された移動体のアルゴリズム速度によって移動体の移動方向が前進か後退かを判定する。すなわち、逆位相判定部２０３は、ＧＰＳ／ＩＭＵ統合速度Ｖｂｏｄｙ（進行方向速度）から、移動体が前進しているか、後退しているかを判定する。これは、ＧＰＳ方位角とＩＭＵヨー角度が本質的には等価ではないことへの対策である。もしＶｂｏｄｙが正であった場合、移動体は前進していると判定し、Ｖｂｏｄｙが負であった場合、移動体は後退していると判定することができる。そして、移動体について前進していると判定した場合、逆位相判定部２０３は、方位角の平均値をθＧＰＳ平均のままとする。移動体について後退していると判定した場合、ＧＰＳ方位角とヨー角度は１８０°ずれていることから、逆位相判定部２０３は、方位角の平均値をθＧＰＳ平均＋πとする。
【００２２】
差分演算部２０４は、逆位相判定部２０３によって判定された移動方向に基づいて、方位角の平均値と、ヨー角度の平均値との差分を算出する。すなわち、差分演算部２０４は、θｙａｗ平均と逆位相判定部２０３から出力されたθＧＰＳ平均もしくはθＧＰＳ平均＋πとの角度誤差Δθを計算する。ただし、両値ともに−π〜π範囲の非連続値であるため、差分演算部２０４は、この非線形性を考慮して差分計算を行う。すなわち、差分演算部２０４は、差分値がπより大きいとき、２πから差分値を引いた値を出力する。差分値が−πより小さいとき、差分値に２πを足した値を出力する。
【００２３】
誤差閾値判断部２０５は、差分演算部２０４によって算出された差分（角度誤差Δθ）が閾値以上であるか未満であるかを判断する。これによって、現在のＧＰＳ方位角とヨー角度との誤差の大小を判定する。
【００２４】
カウント部２０６は、誤差閾値判断部２０５によって、差分演算部２０４によって算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、差分演算部２０４によって算出された差分が閾値未満であると判断された場合にカウントをデクリメントする。すなわち、カウント部２０６は、誤差閾値判断部２０５によって差分が閾値以上であると判定された場合、カウントを＋１し、差分が閾値未満であると判定された場合、最小値を０としてカウントを−１する（すなわち、カウントが０の場合、−１しない）。
【００２５】
異常判定部２０７は、カウント部２０６によって計数されたカウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、ヨー角初期化を異常状態と判定する。すなわち、異常判定部２０７は、カウントがインクリメントされ続けて、ある一定値以上となった場合、ヨー角度を異常と判断し、ヨー角初期化の異常を検出する。
【００２６】
上記構成により、ヨー角初期化異常判定装置２００は、初期アライメント時にヨー角度が誤った値に決定されてしまったとしても、容易にその異常を検出することができる。そして、ヨー角初期化異常判定装置２００は、異常を検出した場合、再度初期アライメントを実行するように、ＧＰＳ／ＩＭＵ統合装置（例えば、高精度車両速度計測装置）に通知することにより、初期アライメント処理をアルゴリズム的に戻させることが可能となる。
【００２７】
図２は、本発明の一実施形態に係るヨー角初期化異常判定装置２００のハードウェア構成の一例を示す図である。ヨー角初期化異常判定装置２００は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０１０、バスライン１００５、メモリ１０５０及び入出力Ｉ／Ｆ１０４０を備える。
【００２８】
ＣＰＵ１０１０は、ヨー角初期化異常判定装置２００を統括的に制御する部分であり、メモリ１０５０に記憶された各種プログラムを適宜読み出して実行することにより、上述したハードウェアと協働し、本発明に係る各種機能を実現している。
【００２９】
メモリ１０５０は、適宜読み出して実行されるプログラムを記憶し、プログラムの実行によって作成される種々の情報を記憶する。例えば、メモリ１０５０は、方位角及びヨー角度の平均値や、カウント等の各種演算結果を記憶する。
【００３０】
入出力Ｉ／Ｆ１０４０は、ヨー角初期化異常判定装置２００が高精度車両速度計測装置から３軸ジャイロセンサによるヨー角度（θｙａｗ）と、ＧＰＳ受信機による方位角（θＧＰＳ）と、ＧＰＳ／ＩＭＵの統合速度（Ｖｂｏｄｙ）とを入力し、高精度車両速度計測装置に初期アライメントの異常又は正常を通知するためのインターフェースである。
【００３１】
図３は、本発明の一実施形態に係るヨー角初期化異常判定装置２００の処理内容を示すフローチャートである。
【００３２】
ステップＳ１０１において、ＣＰＵ１０１０は、方位角と、ヨー角度と、統合速度とを入力する。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。
【００３３】
ステップＳ１０２において、ＣＰＵ１０１０（ＧＰＳ方位角平均値演算部２０１、ヨー角度平均値演算部２０２）は、方位角の平均値と、ヨー角度の平均値とを算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０３に移す。
【００３４】
ステップＳ１０３において、ＣＰＵ１０１０（逆位相判定部２０３）は、移動体が前進しているか否かを判断する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０１で入力した統合速度が正の値である場合に、移動体が前進していると判断し、統合速度が負の値である場合に、後退していると判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１０５に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１０４に移す。
【００３５】
ステップＳ１０４において、ＣＰＵ１０１０（逆位相判定部２０３）は、方位角の平均値にπを加算する。その後、ＣＰＵ１０１０は、処理をステップＳ１０５に移す。
【００３６】
ステップＳ１０５において、ＣＰＵ１０１０（差分演算部２０４）は、方位角の平均値と、ヨー角度の平均値との差分を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０６に移す。
【００３７】
ステップＳ１０６において、ＣＰＵ１０１０（誤差閾値判断部２０５）は、差分が閾値以上か否かを判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１０７に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１０８に移す。
【００３８】
ステップＳ１０７において、ＣＰＵ１０１０（カウント部２０６）は、カウントをインクリメントする。その後、ＣＰＵ１０１０は、処理をステップＳ１０９に移す。
【００３９】
ステップＳ１０８において、ＣＰＵ１０１０（カウント部２０６）は、カウントをデクリメントする。その後、ＣＰＵ１０１０は、処理をステップＳ１０９に移す。
【００４０】
ステップＳ１０９において、ＣＰＵ１０１０（異常判定部２０７）は、カウントが一定値以上か否かを判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１１０に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１１１に移す。
【００４１】
ステップＳ１１０において、ＣＰＵ１０１０（異常判定部２０７部）は、異常と判定する。その後、ＣＰＵ１０１０は、処理を終了する。
【００４２】
ステップＳ１１１において、ＣＰＵ１０１０（異常判定部２０７）は、一定時間が経過か否かを判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１１２に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１０１に移す。
【００４３】
ステップＳ１１２において、ＣＰＵ１０１０（異常判定部２０７部）は、正常と判定する。その後、ＣＰＵ１０１０は、処理を終了する。
【００４４】
本実施形態によれば、ヨー角初期化異常判定装置２００は、ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する高精度車両速度計測装置に組み込まれ、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられる。そして、ヨー角初期化異常判定装置２００は、ＧＰＳ受信機によって算出された方位角の平均値を算出し、ＩＭＵによって計測されたヨー角度の平均値を算出し、アルゴリズム速度によって移動体の移動方向が前進か後退かを判定し、判定した移動方向に基づいて、方位角の平均値と、ヨー角度の平均値との差分を算出し、算出した差分が閾値以上であるか未満であるかを判断する。さらに、ヨー角初期化異常判定装置２００は、算出した差分が閾値以上であると判断した場合にカウントをインクリメントし、算出した差分が閾値未満であると判断した場合にカウントをデクリメントし、計数したカウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する。したがって、ヨー角初期化異常判定装置２００は、ヨー角初期化において、車両が前後に移動した後に前進したり、後進したりする場合に、ヨー角初期化が異常であるか否かを容易に判定することができる。
【００４５】
以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限るものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されるものではない。
【符号の説明】
【００４６】
２００ ヨー角初期化異常判定装置
２０１ ＧＰＳ方位角平均値演算部
２０２ ヨー角度平均値演算部
２０３ 逆位相判定部
２０４ 差分演算部
２０５ 誤差閾値判断部
２０６ カウント部
２０７ 異常判定部
【技術分野】
【０００１】
本発明は、ヨー角初期化異常判定装置及び方法に関する。
【背景技術】
【０００２】
従来より、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）／ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）統合システムによって移動体の位置、速度及び方位等を演算するＧＰＳ／ＩＭＵ統合装置では、ＩＭＵの初期姿勢角、特にヨー角度を決定する必要がある。例えば、高精度ジャイロセンサを備えていれば、地球の自転を計測できる感度があるため、ジャイロコンパスという方法でヨー角度を高精度に決定できる。これに対して、ＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ）ＩＭＵ等の低精度ジャイロセンサしか備えていない場合、別手段で初期ヨー角度を決定する必要がある。
【０００３】
例えば、慣性装置の初期アライメント計算時においてジャイロ誤差にもとづく方位角誤差の増加を抑圧する技術を開示する特許文献１が知られている。特許文献１に開示された慣性装置は、初期アライメント計算部に方位角補正演算部を設け、初期アライメント計算中に、計算した方位角ΨＣを方位角補正演算部に入力して、その出力を姿勢基準行列計算部に帰還補正して、方位角ΨＣがゼロに収斂するようにする。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００１−１５３６８０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１の慣性装置で行われる初期アライメント計算は、慣性装置が搭載された機体が移動しないという条件下で計算されることを利用して、方位角ΨＣがゼロになるように補正量を計算するので、慣性装置が搭載された車両が移動するような場合に利用することができない。
【０００６】
例えば、移動体の進行方向とＩＭＵの軸が同軸上に配置されているＧＰＳ／ＩＭＵ統合装置において、初期アライメント時にＧＰＳ方位角からＩＭＵ初期ヨー角を近似的に決定する際、ＧＰＳ方位角がマルチパスの影響により、真の値からずれてしまっているとき、初期ヨー角は誤った値にて初期化されてしまう。また、移動体が前進方向に進まず、後退方向に進行してしまったとき、ＧＰＳ方位角と初期ヨー角は１８０°ずれてしまうため、初期ヨー角は１８０°ずれた誤った値にて初期化されてしまう。また、一般的にＧＰＳによる位置及び速度と、ＧＰＳ／ＩＭＵ統合システムによる位置及び速度との差分から、初期アライメントの異常を判定することが可能であるが、ＧＰＳによる位置及び速度と、ＧＰＳ／ＩＭＵ統合システムによる位置及び速度との差が生じずに、姿勢角だけが異常な値を出力し続けるという状況が生じてしまう可能性がある。このような場合、位置及び速度の差分から初期アライメントの異常を判定するだけでは不十分である。
【０００７】
そこで、慣性装置が搭載された車両が移動するような場合に、ヨー角初期化が異常であるか否かを容易に判定することができる装置及び方法が求められている。
【０００８】
本発明は、慣性装置が搭載された車両が移動するような場合に、ヨー角初期化が異常であるか否かを容易に判定することができるヨー角初期化異常判定装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明では、以下のような解決手段を提供する。
【００１０】
（１） ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれており、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられるヨー角初期化異常判定装置において、前記ＧＰＳ受信機によって算出された方位角の平均値を算出するＧＰＳ方位角平均値演算手段と、前記ＩＭＵによって計測されたヨー角度の平均値を算出するヨー角度平均値演算手段と、前記アルゴリズム速度によって前記移動体の移動方向が前進か後退かを判定する逆位相判定手段と、前記逆位相判定手段によって判定された前記移動方向に基づいて、前記方位角の平均値と、前記ヨー角度の平均値との差分を算出する差分演算手段と、前記差分演算手段によって算出された差分が閾値以上であるか未満であるかを判断する誤差閾値判断手段と、前記誤差閾値判断手段によって、前記算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、前記算出された差分が閾値未満であると判断された場合にカウントをデクリメントするカウント手段と、前記カウント手段によって計数された前記カウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する異常判定手段と、を備えるヨー角初期化異常判定装置。
【００１１】
（１）の構成によれば、本発明に係るヨー角初期化異常判定装置は、ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれ、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられる。そして、ヨー角初期化異常判定装置は、ＧＰＳ受信機によって算出された方位角の平均値を算出し、ＩＭＵによって計測されたヨー角度の平均値を算出し、アルゴリズム速度によって移動体の移動方向が前進か後退かを判定し、判定した移動方向に基づいて、方位角の平均値と、ヨー角度の平均値との差分を算出し、算出した差分が閾値以上であるか未満であるかを判断する。さらに、ヨー角初期化異常判定装置は、算出した差分が閾値以上であると判断した場合にカウントをインクリメントし、算出した差分が閾値未満であると判断した場合にカウントをデクリメントし、計数したカウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する。
【００１２】
すなわち、本発明に係るヨー角初期化異常判定装置は、慣性装置が搭載された車両の移動方向に基づいて、ＧＰＳによる方位角の平均値と、ＩＭＵによるヨー角度の平均値との差分を算出し、算出した差分が閾値以上であるか未満であるかによって計数したカウントが一定の値以上であると判断した場合に、異常状態と判定する。したがって、本発明に係るヨー角初期化異常判定装置は、ヨー角初期化において、車両が前後に移動した後に前進したり、後進したりする場合に、ヨー角初期化が異常であるか否かを容易に判定することができる。
【００１３】
（２） ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれており、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられるヨー角初期化異常判定装置が実行する方法において、前記ＧＰＳ受信機によって算出された方位角の平均値を算出するＧＰＳ方位角平均値演算ステップと、前記ＩＭＵによって計測されたヨー角度の平均値を算出するヨー角度平均値演算ステップと、前記アルゴリズム速度によって前記移動体の移動方向が前進か後退かを判定する逆位相判定ステップと、前記逆位相判定ステップによって判定された前記移動方向に基づいて、前記方位角の平均値と、前記ヨー角度の平均値との差分を算出する差分演算ステップと、前記差分演算ステップによって算出された差分が閾値以上であるか未満であるかを判断する誤差閾値判断ステップと、前記誤差閾値判断ステップによって、前記算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、前記算出された差分が閾値未満であると判断された場合にカウントをデクリメントするカウントステップと、前記カウントステップによって計数された前記カウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する異常判定ステップと、を備える方法。
【００１４】
したがって、（２）に係る方法は、（１）と同様に、ヨー角初期化において、車両が前後に移動した後に前進したり、後進したりする場合に、ヨー角初期化が異常であるか否かを容易に判定することができる。
【発明の効果】
【００１５】
本発明は、慣性装置が搭載された車両が前後のみに移動するような場合に、ヨー角初期化が異常であるか否かを容易に判定することができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の一実施形態であるヨー角初期化異常判定装置の構成を示すブロック図である。
【図２】本発明の一実施形態に係るヨー角初期化異常判定装置のハードウェア構成の一例を示す図である。
【図３】本発明の一実施形態に係るヨー角初期化異常判定装置の処理内容を示すフローチャートである。
【発明を実施するための形態】
【００１７】
以下、本発明の実施形態について図を参照しながら説明する。
【００１８】
図１は、本発明の一実施形態であるヨー角初期化異常判定装置２００の構成を示すブロック図である。ヨー角初期化異常判定装置２００は、ＧＰＳ方位角平均値演算部２０１と、ヨー角度平均値演算部２０２と、逆位相判定部２０３と、差分演算部２０４と、誤差閾値判断部２０５と、カウント部２０６と、異常判定部２０７とを備える。例えば、ヨー角初期化異常判定装置２００は、ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する高精度車両速度計測装置（図示せず）に組み込まれ、車両の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられる。そして、ヨー角初期化異常判定装置２００は、入力として、ＧＰＳ方位角（θＧＰＳ：−π〜π）と、ＩＭＵヨー角度（θｙａｗ：−π〜π）と、ＧＰＳ／ＩＭＵ統合速度（Ｖｂｏｄｙ：ＢＯＤＹ座標系進行方向速度）とが与えられ、出力として、ヨー角度の正常又は異常判定を行う。
【００１９】
ＧＰＳ方位角平均値演算部２０１は、ＧＰＳ受信機によって算出された方位角の平均値を算出する。すなわち、ＧＰＳ方位角平均値演算部２０１は、ＧＰＳ／ＩＭＵ統合装置（例えば、高精度車両速度計測装置）によって入力されるＧＰＳ方位角θＧＰＳのある一定時間における平均値θＧＰＳ平均を算出する。ただし、θＧＰＳは−π〜πの非連続な範囲であるため、そのままθＧＰＳを平均した場合は正しくない値となってしまう。そこで、ｓｉｎθＧＰＳ、ｃｏｓθＧＰＳした値を平均化し、その出力に対し、ｔａｎ−１変換することで、θＧＰＳの正しい平均値であるθＧＰＳ平均を求めることが可能となる。
【００２０】
ヨー角度平均値演算部２０２は、ＩＭＵによって計測されたヨー角度の平均値を算出する。すなわち、ヨー角度平均値演算部２０２は、ＧＰＳ方位角平均値演算部２０１と同様の処理を行い、ＧＰＳ／ＩＭＵ統合装置（例えば、高精度車両速度計測装置）によって入力されるヨー角度θｙａｗの一定時間における平均値θｙａｗ平均を算出する。
【００２１】
逆位相判定部２０３は、自律航法アルゴリズムによって算出された移動体のアルゴリズム速度によって移動体の移動方向が前進か後退かを判定する。すなわち、逆位相判定部２０３は、ＧＰＳ／ＩＭＵ統合速度Ｖｂｏｄｙ（進行方向速度）から、移動体が前進しているか、後退しているかを判定する。これは、ＧＰＳ方位角とＩＭＵヨー角度が本質的には等価ではないことへの対策である。もしＶｂｏｄｙが正であった場合、移動体は前進していると判定し、Ｖｂｏｄｙが負であった場合、移動体は後退していると判定することができる。そして、移動体について前進していると判定した場合、逆位相判定部２０３は、方位角の平均値をθＧＰＳ平均のままとする。移動体について後退していると判定した場合、ＧＰＳ方位角とヨー角度は１８０°ずれていることから、逆位相判定部２０３は、方位角の平均値をθＧＰＳ平均＋πとする。
【００２２】
差分演算部２０４は、逆位相判定部２０３によって判定された移動方向に基づいて、方位角の平均値と、ヨー角度の平均値との差分を算出する。すなわち、差分演算部２０４は、θｙａｗ平均と逆位相判定部２０３から出力されたθＧＰＳ平均もしくはθＧＰＳ平均＋πとの角度誤差Δθを計算する。ただし、両値ともに−π〜π範囲の非連続値であるため、差分演算部２０４は、この非線形性を考慮して差分計算を行う。すなわち、差分演算部２０４は、差分値がπより大きいとき、２πから差分値を引いた値を出力する。差分値が−πより小さいとき、差分値に２πを足した値を出力する。
【００２３】
誤差閾値判断部２０５は、差分演算部２０４によって算出された差分（角度誤差Δθ）が閾値以上であるか未満であるかを判断する。これによって、現在のＧＰＳ方位角とヨー角度との誤差の大小を判定する。
【００２４】
カウント部２０６は、誤差閾値判断部２０５によって、差分演算部２０４によって算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、差分演算部２０４によって算出された差分が閾値未満であると判断された場合にカウントをデクリメントする。すなわち、カウント部２０６は、誤差閾値判断部２０５によって差分が閾値以上であると判定された場合、カウントを＋１し、差分が閾値未満であると判定された場合、最小値を０としてカウントを−１する（すなわち、カウントが０の場合、−１しない）。
【００２５】
異常判定部２０７は、カウント部２０６によって計数されたカウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、ヨー角初期化を異常状態と判定する。すなわち、異常判定部２０７は、カウントがインクリメントされ続けて、ある一定値以上となった場合、ヨー角度を異常と判断し、ヨー角初期化の異常を検出する。
【００２６】
上記構成により、ヨー角初期化異常判定装置２００は、初期アライメント時にヨー角度が誤った値に決定されてしまったとしても、容易にその異常を検出することができる。そして、ヨー角初期化異常判定装置２００は、異常を検出した場合、再度初期アライメントを実行するように、ＧＰＳ／ＩＭＵ統合装置（例えば、高精度車両速度計測装置）に通知することにより、初期アライメント処理をアルゴリズム的に戻させることが可能となる。
【００２７】
図２は、本発明の一実施形態に係るヨー角初期化異常判定装置２００のハードウェア構成の一例を示す図である。ヨー角初期化異常判定装置２００は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０１０、バスライン１００５、メモリ１０５０及び入出力Ｉ／Ｆ１０４０を備える。
【００２８】
ＣＰＵ１０１０は、ヨー角初期化異常判定装置２００を統括的に制御する部分であり、メモリ１０５０に記憶された各種プログラムを適宜読み出して実行することにより、上述したハードウェアと協働し、本発明に係る各種機能を実現している。
【００２９】
メモリ１０５０は、適宜読み出して実行されるプログラムを記憶し、プログラムの実行によって作成される種々の情報を記憶する。例えば、メモリ１０５０は、方位角及びヨー角度の平均値や、カウント等の各種演算結果を記憶する。
【００３０】
入出力Ｉ／Ｆ１０４０は、ヨー角初期化異常判定装置２００が高精度車両速度計測装置から３軸ジャイロセンサによるヨー角度（θｙａｗ）と、ＧＰＳ受信機による方位角（θＧＰＳ）と、ＧＰＳ／ＩＭＵの統合速度（Ｖｂｏｄｙ）とを入力し、高精度車両速度計測装置に初期アライメントの異常又は正常を通知するためのインターフェースである。
【００３１】
図３は、本発明の一実施形態に係るヨー角初期化異常判定装置２００の処理内容を示すフローチャートである。
【００３２】
ステップＳ１０１において、ＣＰＵ１０１０は、方位角と、ヨー角度と、統合速度とを入力する。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。
【００３３】
ステップＳ１０２において、ＣＰＵ１０１０（ＧＰＳ方位角平均値演算部２０１、ヨー角度平均値演算部２０２）は、方位角の平均値と、ヨー角度の平均値とを算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０３に移す。
【００３４】
ステップＳ１０３において、ＣＰＵ１０１０（逆位相判定部２０３）は、移動体が前進しているか否かを判断する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０１で入力した統合速度が正の値である場合に、移動体が前進していると判断し、統合速度が負の値である場合に、後退していると判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１０５に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１０４に移す。
【００３５】
ステップＳ１０４において、ＣＰＵ１０１０（逆位相判定部２０３）は、方位角の平均値にπを加算する。その後、ＣＰＵ１０１０は、処理をステップＳ１０５に移す。
【００３６】
ステップＳ１０５において、ＣＰＵ１０１０（差分演算部２０４）は、方位角の平均値と、ヨー角度の平均値との差分を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０６に移す。
【００３７】
ステップＳ１０６において、ＣＰＵ１０１０（誤差閾値判断部２０５）は、差分が閾値以上か否かを判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１０７に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１０８に移す。
【００３８】
ステップＳ１０７において、ＣＰＵ１０１０（カウント部２０６）は、カウントをインクリメントする。その後、ＣＰＵ１０１０は、処理をステップＳ１０９に移す。
【００３９】
ステップＳ１０８において、ＣＰＵ１０１０（カウント部２０６）は、カウントをデクリメントする。その後、ＣＰＵ１０１０は、処理をステップＳ１０９に移す。
【００４０】
ステップＳ１０９において、ＣＰＵ１０１０（異常判定部２０７）は、カウントが一定値以上か否かを判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１１０に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１１１に移す。
【００４１】
ステップＳ１１０において、ＣＰＵ１０１０（異常判定部２０７部）は、異常と判定する。その後、ＣＰＵ１０１０は、処理を終了する。
【００４２】
ステップＳ１１１において、ＣＰＵ１０１０（異常判定部２０７）は、一定時間が経過か否かを判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１１２に移し、ＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１０１に移す。
【００４３】
ステップＳ１１２において、ＣＰＵ１０１０（異常判定部２０７部）は、正常と判定する。その後、ＣＰＵ１０１０は、処理を終了する。
【００４４】
本実施形態によれば、ヨー角初期化異常判定装置２００は、ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する高精度車両速度計測装置に組み込まれ、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられる。そして、ヨー角初期化異常判定装置２００は、ＧＰＳ受信機によって算出された方位角の平均値を算出し、ＩＭＵによって計測されたヨー角度の平均値を算出し、アルゴリズム速度によって移動体の移動方向が前進か後退かを判定し、判定した移動方向に基づいて、方位角の平均値と、ヨー角度の平均値との差分を算出し、算出した差分が閾値以上であるか未満であるかを判断する。さらに、ヨー角初期化異常判定装置２００は、算出した差分が閾値以上であると判断した場合にカウントをインクリメントし、算出した差分が閾値未満であると判断した場合にカウントをデクリメントし、計数したカウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する。したがって、ヨー角初期化異常判定装置２００は、ヨー角初期化において、車両が前後に移動した後に前進したり、後進したりする場合に、ヨー角初期化が異常であるか否かを容易に判定することができる。
【００４５】
以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限るものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されるものではない。
【符号の説明】
【００４６】
２００ ヨー角初期化異常判定装置
２０１ ＧＰＳ方位角平均値演算部
２０２ ヨー角度平均値演算部
２０３ 逆位相判定部
２０４ 差分演算部
２０５ 誤差閾値判断部
２０６ カウント部
２０７ 異常判定部
【特許請求の範囲】
【請求項１】
ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれており、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられるヨー角初期化異常判定装置において、
前記ＧＰＳ受信機によって算出された方位角の平均値を算出するＧＰＳ方位角平均値演算手段と、
前記ＩＭＵによって計測されたヨー角度の平均値を算出するヨー角度平均値演算手段と、
前記アルゴリズム速度によって前記移動体の移動方向が前進か後退かを判定する逆位相判定手段と、
前記逆位相判定手段によって判定された前記移動方向に基づいて、前記方位角の平均値と、前記ヨー角度の平均値との差分を算出する差分演算手段と、
前記差分演算手段によって算出された差分が閾値以上であるか未満であるかを判断する誤差閾値判断手段と、
前記誤差閾値判断手段によって、前記算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、前記算出された差分が閾値未満であると判断された場合にカウントをデクリメントするカウント手段と、
前記カウント手段によって計数された前記カウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する異常判定手段と、
を備えるヨー角初期化異常判定装置。
【請求項２】
ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれており、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられるヨー角初期化異常判定装置が実行する方法において、
前記ＧＰＳ受信機によって算出された方位角の平均値を算出するＧＰＳ方位角平均値演算ステップと、
前記ＩＭＵによって計測されたヨー角度の平均値を算出するヨー角度平均値演算ステップと、
前記アルゴリズム速度によって前記移動体の移動方向が前進か後退かを判定する逆位相判定ステップと、
前記逆位相判定ステップによって判定された前記移動方向に基づいて、前記方位角の平均値と、前記ヨー角度の平均値との差分を算出する差分演算ステップと、
前記差分演算ステップによって算出された差分が閾値以上であるか未満であるかを判断する誤差閾値判断ステップと、
前記誤差閾値判断ステップによって、前記算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、前記算出された差分が閾値未満であると判断された場合にカウントをデクリメントするカウントステップと、
前記カウントステップによって計数された前記カウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する異常判定ステップと、
を備える方法。
【請求項１】
ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれており、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられるヨー角初期化異常判定装置において、
前記ＧＰＳ受信機によって算出された方位角の平均値を算出するＧＰＳ方位角平均値演算手段と、
前記ＩＭＵによって計測されたヨー角度の平均値を算出するヨー角度平均値演算手段と、
前記アルゴリズム速度によって前記移動体の移動方向が前進か後退かを判定する逆位相判定手段と、
前記逆位相判定手段によって判定された前記移動方向に基づいて、前記方位角の平均値と、前記ヨー角度の平均値との差分を算出する差分演算手段と、
前記差分演算手段によって算出された差分が閾値以上であるか未満であるかを判断する誤差閾値判断手段と、
前記誤差閾値判断手段によって、前記算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、前記算出された差分が閾値未満であると判断された場合にカウントをデクリメントするカウント手段と、
前記カウント手段によって計数された前記カウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する異常判定手段と、
を備えるヨー角初期化異常判定装置。
【請求項２】
ＧＰＳ受信機及びＩＭＵを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体のアルゴリズム速度を算出する装置に含まれており、移動体の進行方向とＩＭＵの軸とが同軸上に配置されている場合に用いられるヨー角初期化異常判定装置が実行する方法において、
前記ＧＰＳ受信機によって算出された方位角の平均値を算出するＧＰＳ方位角平均値演算ステップと、
前記ＩＭＵによって計測されたヨー角度の平均値を算出するヨー角度平均値演算ステップと、
前記アルゴリズム速度によって前記移動体の移動方向が前進か後退かを判定する逆位相判定ステップと、
前記逆位相判定ステップによって判定された前記移動方向に基づいて、前記方位角の平均値と、前記ヨー角度の平均値との差分を算出する差分演算ステップと、
前記差分演算ステップによって算出された差分が閾値以上であるか未満であるかを判断する誤差閾値判断ステップと、
前記誤差閾値判断ステップによって、前記算出された差分が閾値以上であると判断された場合にカウントをインクリメントし、前記算出された差分が閾値未満であると判断された場合にカウントをデクリメントするカウントステップと、
前記カウントステップによって計数された前記カウントが一定の値以上であるか否かを判断し、一定の値以上であると判断した場合に、異常状態と判定する異常判定ステップと、
を備える方法。
【図１】
【図２】
【図３】
【図２】
【図３】
【公開番号】特開２０１２−４２２８５（Ｐ２０１２−４２２８５Ａ）
【公開日】平成２４年３月１日（２０１２．３．１）
【国際特許分類】
【出願番号】特願２０１０−１８２５４４（Ｐ２０１０−１８２５４４）
【出願日】平成２２年８月１７日（２０１０．８．１７）
【出願人】（０００１４５８０６）株式会社小野測器 (230)
【Ｆターム（参考）】
【公開日】平成２４年３月１日（２０１２．３．１）
【国際特許分類】
【出願日】平成２２年８月１７日（２０１０．８．１７）
【出願人】（０００１４５８０６）株式会社小野測器 (230)
【Ｆターム（参考）】
[ Back to top ]