(57)【要約】【課題】  ジャイロ、コンパスおよび走行距離計センサーの短所を克服し、移動物体の移動時に発生し得る突発状況に対する対処が可能であり、多様な種類のエラー源を推定でき、システムフォールトの感知もできる移動物体の進行方向の推定方法およびシステムを提供する。【解決手段】  移動物体に装着された磁気コンパスから磁場情報を求める段階と、移動物体の磁場を所定値と比較し、その結果によって移動物体が存在する地点が特定領域に属するか否かを判断する段階と、前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して移動物体の方向を推定する段階と、を含む移動物体の進行方向の推定方法およびその方法を行うためのシステム。【選択図】  図４
2005140789.tif 000002
【特許請求の範囲】
【請求項１】
移動物体に装着された磁気コンパスから磁場情報を求める第１段階と、
前記移動物体の磁場を所定値と比較し、その結果によって移動物体が存在する地点が特定領域に属するか否かを判断する第２段階と、
前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して移動物体の方向を推定する第３段階と、を含むことを特徴とする移動物体の進行方向の推定方法。
【請求項２】
前記所定値は地球磁場のサイズであり、
前記特定領域は地球磁気が作用する領域であることを特徴とする請求項１に記載の移動物体の進行方向の推定方法。
【請求項３】
前記第２段階は、
前記移動物体に２つの磁気コンパスが設置されている場合、移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場サイズと比較して、その差が両方とも所定の限界値より小さな場合、１つのみ所定の限界値より小さな場合、両方とも所定の限界値より小さくない場合に分け、その結果によって移動物体が存在する地点が特定領域に属するか否かを判断することを特徴とする請求項２に記載の移動物体の進行方向の推定方法。
【請求項４】
前記第３段階は、
移動物体が特定領域に属して磁気コンパスの情報が有効であると判断される場合、磁気コンパスの最終方位角を求め、前記方位角を測定入力として採択したカルマンフィルタを利用して最適の進行方向角度を推定し、
移動物体が特定領域に属さずに磁気コンパスの情報が有効でないと判断される場合、ジャイロと走行距離計との角速度を比較し、その差が所定値より小さい場合、走行距離計から求めた方向を測定入力として採択したカルマンフィルタを利用して最適の進行方向を推定するか、前記差が所定値より小さくない場合、ジャイロの角速度を積分して進行方向を推定する段階であることを特徴とする請求項３に記載の移動物体の進行方向の推定方法。
【請求項５】
前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場サイズと比較してその差が両方とも所定の限界値より小さな場合の移動物体の方向推定は、
前記第１磁気コンパスの方位角と第２磁気コンパスの方位角との差を求める段階と、
前記方位角の差が所定値より小さい場合には、地球磁場と磁気コンパスとの磁場の差によって加重値を異ならせて移動物体の磁気コンパス方位角を求める加重値段階と、
前記方位角の差が所定値より小さくない場合には、移動物体に対するジャイロの角速度と磁気コンパスの方位角に対する角速度との差をそれぞれ求めてそのサイズによって移動物体の磁気コンパス方位角を求める角速度段階と、を含むことを特徴とする請求項３に記載の移動物体の進行方向の推定方法。
【請求項６】
前記加重値段階は、Δθc1、Δθc2をサンプリング周期の間の第１磁気コンパスおよび第２磁気コンパスの方位角変化量、ωgはジャイロの角速度、Δｔをサンプリング時間とし、前記第１磁気コンパスの磁場サイズと地球磁場サイズとの差をΔＨ1E、前記第２磁気コンパスの磁場サイズと地球磁場サイズとの差をΔＨ2Eとする時、
前記ΔＨ1EとΔＨ2Eとの積を求めてその値が負数であれば、数式３により求められる値を磁気コンパスの最終方位角θcとし、
数式３
【数１】
2005140789.tif 000053
もし、前記ΔＨ1EとΔＨ2Eとの積が正数であれば、数式４により求められる値を磁気コンパスの最終方位角θcとし、
数式４
【数２】
2005140789.tif 000054
もし、前記積の値が０であれば、数式５
数式５
【数３】
2005140789.tif 000055
により求められる値を磁気コンパスの最終方位角θcとすることを特徴とする請求項５に記載の移動物体の進行方向の推定方法。
【請求項７】
前記角速度段階は、Δθc1、Δθc2をサンプリング周期の間の第１磁気コンパスおよび第２磁気コンパスの方位角変化量、ωgはジャイロの角速度、Δｔをサンプリング時間とする時、
前記第１磁気コンパスの方位角θc1と第２磁気コンパスの方位角θc2との差が所定値より小さくない場合には、数式６を満足するか否かをチェックする段階と、
数式６
【数４】
2005140789.tif 000056
前記数式６を満足すれば、第１磁気コンパスの方位角θc1を磁気コンパスの最終方位角θcとする段階と、
もし、数式６を満足せずに数式７を満足すれば、第２磁気コンパスの方位角θc2を磁気コンパスの最終方位角θcとする段階と、
数式７
【数５】
2005140789.tif 000057
もし、前記数式６および数式７で両辺が相互同じであれば、数式５により求められる値を
数式５
【数６】
2005140789.tif 000058
磁気コンパスの最終方位角θcとすることを特徴とする請求項５に記載の移動物体の進行方向の推定方法。
【請求項８】
前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場サイズと比較して、その差のうち１つのみ所定の限界値より小さな場合の移動物体の方向推定は、
前記限界値より小さな磁気コンパスに対する方位角を磁気コンパスの最終方位角とすることを特徴とする請求項３に記載の移動物体の進行方向の推定方法。
【請求項９】
前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場サイズと比較し、その差が両方とも所定の限界値より小さくない場合の移動物体の方向推定は、
磁気コンパスデータを使用せずに移動物体のホイール速度に対する角速度（走行距離計により求められる角速度）を計算する段階と、
前記移動物体のジャイロ角速度と前記ホイールの角速度との差を求める段階と、
前記ジャイロ角速度とホイール角速度との差が所定値より小さければ、走行距離計から求めた方向を測定入力として採択したカルマンフィルタを利用して最適の進行方向角度を推定する段階と、
前記ジャイロ角速度とホイール角速度との差が所定値より小さくなければ、カルマンフィルタを使用せずにジャイロの角速度を積分して進行方向角度を推定する積分推定段階と、を含むことを特徴とする請求項３に記載の移動物体の進行方向の推定方法。
【請求項１０】
前記積分推定段階は、
所定時間内に前記積分推定段階が所定回数以上行われる場合にはシステムを終了する段階をさらに具備することを特徴とする請求項９に記載の移動物体の進行方向の推定方法。
【請求項１１】
エラー状態フィードバックカルマンフィルタを通じて移動物体の進行方向角度をフィルタリングして、移動物体に対する最適の進行方向推定値を計算する段階をさらに具備することを特徴とする請求項５または８に記載の移動物体の進行方向の推定方法。
【請求項１２】
請求項１ないし請求項１１のいずれか１項に記載の移動物体の進行方向の推定方法をコンピュータで実行させるためのプログラムを記録したコンピュータによって読取可能な記録媒体。
【請求項１３】
ジャイロ、走行距離計および磁気コンパスが設置された移動物体において、
前記磁気コンパスが設置された移動物体の磁場のサイズを計算する磁場算出部と、
前記磁場のサイズと地球磁場のサイズとの差を求めて所定値と比較する磁場比較部と、
前記比較結果によって移動物体が存在する地点が地球磁気が作用する領域に属するか否かを判断する地球磁気領域判断部と、
前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して、移動物体の方向を推定する進行方向推定部と、を含むことを特徴とする移動物体の進行方向の推定システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動物体の位置推定に係り、特に、磁気コンパスデータを利用した移動物体の進行方向の推定方法およびシステムに関する。
【背景技術】
【０００２】
移動物体の姿勢の推定は、絶対センサーまたは相対センサーを利用して物体の位置および方向を推定する問題であって、図１に示されるように、２次元平面で動く移動ロボットの場合には（ｘ,ｙ）位置および方向θを推定する問題である。絶対センサーは、ロボットまたは物体の相対的な動きの代わりに、絶対的な位置または姿勢を測定できるセンサーであって、カメラ、レーザースキャナ、センサー、全地球測位システム（ＧＰＳ:Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）、磁気コンパスなどがある。これに対して、相対センサーは、ロボットまたは物体の相対的な変化量を測定し、これを統合することによって位置または姿勢を測定できるセンサーであって、ジャイロ、加速度計、走行距離計（モーターに装着されたエンコーダ）などが該当する。
【０００３】
絶対センサーのうち、カメラは周辺環境の照明状態に敏感であり、不確実な情報のデータを与える可能性が高く、レーザースキャナは正確なデータの獲得が可能であるが、コストが極めて高く、障害物が多い場合には測定が困難となるという短所がある。また、ソナーはデータの正確性に劣り、ＧＰＳは室外でのみ使用可能であり、しかも精度に劣り、磁気コンパスは室内に混在する外乱磁場の影響を受ける可能性が高い短所がある。一方、ジャイロ、走行距離計、加速度計などの相対センサーは、変化量のみを測定するので、その変化量の積分によって発生する積分誤差を避けられず、ジャイロ、加速度計に固有に存在するドリフト誤差は回避できない。
【０００４】
既存の推測航法の姿勢推定方式は、使われるセンサーによって、走行距離計のみを使用した方式、ジャイロおよび走行距離計を使用した方式、磁気コンパスおよび走行距離計を使用した方式に区分される。走行距離計のみを使用した方式は、最も簡単な方式であるが、スリッページエラー、バンプ衝突、空中移動の問題などに対処できず、エラーが累積し続けるので、限界のないエラーを有する。
【０００５】
これらの問題を解決するために、走行距離計にジャイロを追加して推測航法を行う方式が開発された。しかし、この方式は、走行距離計、すなわちモーターエンコーダのみを単独使用する方式に比べて正確な結果が得られるが、エンコーダとジャイロの何れも相対センサーであるので、長期的にはやはりエラーの限界を保証できない。ジャイロの代りに、絶対センサーである磁気コンパスを導入して長期的には進行方向を安定的に検出できる方式も開発されたが、家庭環境、オフィス環境などに常に存在している外乱磁場の影響を受けやすいので、この方式はそのまま実用化され難い。
【０００６】
最近、ジャイロ、磁気コンパス、走行距離計の全てを利用する方式も提案されている。この方式は、ジャイロと磁気コンパスとを同時に使用するので、相互補完可能な長所を有しているが、現在までに開発されたほとんどの方式は、統計に基づいたセンサー統合（ｆｕｓｉｏｎ）方法である既存のカルマンフィルタをそのまま使用することによって、カルマンフィルタ固有の短所による様々な限界を有する。すなわち、システム雑音および測定雑音がガウス白色雑音特性を有しなければならないので、磁場、障害物、滑りやすい床などの各種の不確実な状況に対処し難い。その他にも、センサーのエラーモデルに対する正確な情報およびエラーソース間に相互相関がないという仮定が必要であるが、これは現実的に実現不可能である。また、センサーの性能がセンサー統合に重要な要素として作用するので、この方式では、主に高価なセンサーが使用されている。
【発明の開示】
【発明が解決しようとする課題】
【０００７】
本発明が解決しようとする技術的課題は、移動物体の進行方向の角度を正確に、かつ堅実に推定して物体の姿勢情報を算出できる統合センサー（sensor fusion）を利用した移動物体の進行方向の推定方法およびシステムを提供することである。
【０００８】
本発明が解決しようとする他の技術的課題は、前記の移動物体の進行方向の推定方法をコンピュータで実行させるためのプログラムを記録したコンピュータ読取可能な記録媒体を提供することである。
【課題を解決するための手段】
【０００９】
本発明は、絶対センサーのうち磁気コンパスを利用し、相対センサーのうちジャイロおよび走行距離計を利用しながら同時に統合センサーを具現して前記短所を補完するものである。
すなわち、前記技術的課題を解決するために、本発明による移動物体の進行方向の推定方法は、移動物体に装着された磁気コンパスから磁場情報を求める第１段階と、前記移動物体の磁場を所定値と比較し、その結果によって移動物体が存在する地点が特定領域に属するか否かを判断する第２段階と、前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して移動物体の方向を推定する第３段階と、を含むことを特徴とする。
【００１０】
前記所定値は地球磁場のサイズであり、前記特定領域は地球磁気が優勢に作用する領域であることが望ましい。前記第２段階は、前記移動物体に２つの磁気コンパスが設置されている場合、移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場サイズと比較して、その差が両方とも所定の限界値より小さな場合、１つのみ所定の限界値より小さな場合、両方とも所定の限界値より小さくない場合に分け、その結果によって移動物体が存在する地点が特定領域に属するか否かを判断することが望ましい。
【００１１】
前記第３段階は、移動物体が特定領域に属して磁気コンパスの情報が有効であると判断される場合、磁気コンパスの最終方位角を求め、前記方位角を測定入力として採択したカルマンフィルタを利用して進行方向角度を推定し、移動物体が特定領域に属さず、磁気コンパスの情報が有効でないと判断される場合、ジャイロと走行距離計との角速度を比較して、その差が所定値より小さな場合、走行距離計から求めた方向を測定入力として採択したカルマンフィルタを利用して進行方向を推定するか、前記差が所定値より小さくない場合、ジャイロの角速度を積分して進行方向を推定する段階であることが望ましい。
【００１２】
前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場サイズと比較して、その差が両方とも所定の限界値より小さな場合の移動物体の方向推定は、前記第１磁気コンパスの方位角と第２磁気コンパスの方位角との差を求める段階と、前記方位角の差が所定値より小さい場合には、地球磁場と２つの磁気コンパスの磁場との差によって加重値を異ならせて移動物体の磁気コンパス方位角を求める加重値段階と、前記方位角の差が所定値より小さくない場合には、移動物体に対するジャイロの角速度と磁気コンパスの方位角に対する角速度との差をそれぞれ求めてそのサイズによって移動物体の磁気コンパス方位角を求める角速度段階と、を含むことを特徴とする。
【００１３】
前記加重値段階は、Δθc1、Δθc2をサンプリング周期の間の第１磁気コンパスおよび第２磁気コンパスの方位角変化量、ωgはジャイロの角速度、Δｔをサンプリング時間とし、前記第１磁気コンパスの磁場サイズと地球磁場サイズとの差をΔＨ1E、前記第２磁気コンパスの磁場サイズと地球磁場サイズとの差をΔＨ2Eとする時、前記ΔＨ1EとΔＨ2Eとの積を求めてその値が負数であれば、数式３により求められる値を磁気コンパスの最終方位角θcとし、もし、前記ΔＨ1Eと前記ΔＨ2Eとの積が正数であれば、数式４により求められる値を最終方位角θcとし、もし前記積の値が０であれば、数式５により求められる値を磁気コンパスの最終方位角θcとすることが望ましい。
【００１４】
前記角速度段階は、Δθc1、Δθc2をサンプリング周期の間の第１磁気コンパスおよび第２磁気コンパスの方位角の変化量、ωgはジャイロの角速度、Δｔをサンプリング時間とする時、前記第１磁気コンパスの方位角θc1と第２磁気コンパスの方位角θc2との差が所定値より小さくない場合には数式６を満足するか否かをチェックする段階と、前記数式６を満足すれば、第１磁気コンパスの方位角θc1を磁気コンパスの最終方位角θcとする段階と、もし数式６を満足せずに数式７を満足すれば、第２磁気コンパスの方位角θc2を磁気コンパスの最終方位角θcとする段階と、もし数式６と数式７で両辺が相互に同じであれば、前記数式５により求められる値を移動物体の進行方向角度θcとすることが望ましい。
数式３
【数７】
2005140789.tif 000003
数式４
【数８】
2005140789.tif 000004
数式５
【数９】
2005140789.tif 000005
数式６
【数１０】
2005140789.tif 000006
数式７
【数１１】
2005140789.tif 000007
【００１５】
前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場サイズと比較して、その差のうち１つのみ所定の限界値より小さな場合の移動物体の方向推定は、前記限界値より小さな磁気コンパスに対する方位角を磁気コンパスの最終方位角とすることが望ましい。
【００１６】
前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場サイズと比較して、その差が両方とも所定の限界値より小さくない場合の移動物体の方向推定は磁気コンパスデータを使用せずに移動物体のホイール速度に対する角速度（すなわち、走行距離計により求められる角速度）を計算する段階と、前記移動物体のジャイロ角速度と前記ホイールの角速度との差を求める段階と、前記ジャイロ角速度とホイール角速度との差が所定値より小さければ、走行距離計から求めた方向を測定入力として採択したカルマンフィルタを利用して最適の進行方向角度を推定する段階と、前記ジャイロ角速度とホイール角速度との差が所定値より小さくなければ、カルマンフィルタを使用せずにジャイロの角速度を積分して進行方向角度を推定する積分推定段階と、を含むことが望ましい。前記積分推定段階は、所定時間内に前記積分推定段階が所定回数以上行われる場合にはシステムを終了する段階をさらに具備することを特徴とする。
【００１７】
前記移動物体の進行方向の推定方法は、エラー状態フィードバックカルマンフィルタを通じて移動物体の進行方向角度をフィルタリングして、移動物体に対する最適の進行方向推定値を計算する段階をさらに具備することを特徴とする。
【００１８】
前記技術的課題を解決するために、本発明による移動物体の進行方向の推定システムは、ジャイロ、走行距離計および磁気コンパスが設置された移動物体において、前記磁気コンパスが設置された移動物体の磁場のサイズを計算する磁場算出部と、前記磁場のサイズと地球磁場のサイズとの差を求めて所定値と比較する磁場比較部と、前記比較結果によって移動物体が存在する地点が地球磁気が作用する領域に属するか否かを判断する地球磁気領域判断部と、前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して移動物体の方向を推定する進行方向推定部と、を含むことを特徴とする。
【００１９】
そして、前記記載された方法をコンピュータで実行させるためのプログラムを記録したコンピュータ読取可能な記録媒体を提供する。
【発明の効果】
【００２０】
本発明による移動物体の進行方向の推定方法およびシステムは、センサーの特性よりセンサーの統合が重要な要素として作用するので、高性能の仕様のセンサーを必要とせず、廉価なセンサーで高性能を具現可能である。ジャイロ、磁気コンパスおよび走行距離計センサーのそれぞれの短所をカバーし、長所を活用することによって移動物体の移動時に発生する可能性がある多くの突発状況に対して対処でき、多様な種類のエラー源を推定できる。また、センサーのエラー特性に対する正確な情報を必要とせず、バイアス、有色ノイズ、非システム的なエラーなどに対する対処が可能であるなどカルマンフィルタが有している様々な固有の限界を克服できる。付加的な効果としてシステムの誤動作の感知機能が具現可能になる。
【発明を実施するための最良の形態】
【００２１】
以下、図面を参照して本発明による方法および装置について詳細に説明する。
物体のロールとピッチは加速度計およびジャイロを利用して正確に計算できるが、物体のヨー値を求めることは極めて難しい問題として知られている。本発明では、移動物体の進行方向を正確に、かつ堅実に推定し、物体の姿勢情報を算出できる新たな統合センサーを利用して移動物体の進行方向を推定する。すなわち、磁気コンパス、ジャイロ、走行距離計などの絶対センサーと相対センサーとを組合わせて、磁場、障害物、滑りやすい床などの各種の不確実な環境に効果的に対処しながら移動物体のヨー角度を獲得して位置および姿勢を最適に推定する。
【００２２】
図２は、本発明の実施形態による移動物体の進行方向推定のために、移動物体に設置された統合センサーシステムを示したものであって、移動物体２００は、２つの磁気コンパス２１０，２２０と、ジャイロ２３０と、２つのホイールエンコーダ２４０，２５０（走行距離計）とを具備している。
【００２３】
使われたセンサーは、それぞれ次のような特性を有する。
ジャイロ２３０は、バイアスドリフトエラーと角度値とを計算するために必要な積分過程で長期的には積分エラーが発生するが、短期的には比較的正確な角度値を検出できる。一方、磁気コンパス２１０，２２０は、長期的には絶対方位角を与えるので、正確な情報を提供するが、短期的には生活環境で存在する磁場撹乱によるエラーを発生させる。一方、移動物体の駆動部（ロボットの場合には車輪）に設けられたホイールエンコーダ２４０，２５０は、長期的には滑り（slippage）によるエラーとホイールサイズおよび整列の如何などの動力学的な条件によるエラーが累積されてやはり制限されないエラー特性を示し、また、短期的にもバンプ衝突や空中移動のような突発的な状況に対処できない限界を有している。しかし、ほとんどの期間、比較的正確な情報を与える。
【００２４】
前記磁気コンパス２１０，２２０からは、磁場の方向と磁場のサイズデータとが得られる。前記ジャイロ２３０は角速度を検出でき、ホイールエンコーダ２４０，２５０からホイールの回転速度を検出して走行距離計を利用した移動物体の角速度を求めることができる。
【００２５】
本発明は、磁気コンパス、ジャイロ、走行距離計などの絶対センサーと相対センサーとを組合わせて、磁場、障害物、滑りやすい床などの各種の不確実な環境に効果的に対処しながら、物体のヨー角度を求めて位置および姿勢を最適に推定するために、規則基盤のカルマンフィルタを利用する。
【００２６】
図３は、本発明による移動物体の進行方向の推定システムに係る一実施形態を示したブロック図であって、磁場算出部３００、磁場比較部３１０、地球磁気領域判断部３２０および進行方向推定部３３０を含む。前記移動物体には、ジャイロ、走行距離計および磁気コンパスが設置されている。
【００２７】
移動物体は、磁気コンパス、ジャイロおよび走行距離計からセンサーデータを得る。そのうち磁気コンパス情報から磁場が求められる。磁場算出部３００は、前記磁気コンパスが設置された移動物体の磁場のサイズを計算する。磁場比較部３１０は、前記磁場のサイズと地球磁場のサイズとの差を求めて所定値εHと比較する。地球磁気領域判断部３２０は、前記比較結果によって移動物体が存在する地点が地球磁気が作用する領域に属するか否かを判断する。進行方向推定部３３０は、前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して移動物体の方向を推定する。２つの磁気コンパスから得た磁場と、地球磁場との差を前記磁場比較部３１０で比較した結果、何れか１つでも前記所定値より小さな場合には、磁気コンパスデータから得た方位角を移動物体の方向推定に利用し、そうでない場合には、ジャイロと走行距離計との角速度を利用して移動物体の方向推定に利用する。以下、その方向推定についてより詳細に説明する。
【００２８】
図４は、本発明による移動物体の進行方向の推定方法についての一実施形態を示したフローチャートであって、これに基づいて本発明を具体的に説明する。
【００２９】
磁気コンパス、ジャイロ、走行距離計のセンサーデータを読取る（４００段階）。まず、前記センサーデータのうち磁気コンパスデータから磁場を求める。図５は、磁気コンパス情報から磁場を求める方法を説明する図面である。磁気コンパスを通じて得られるｘ軸成分の磁場Ｈxとｙ軸成分の磁場Ｈyとから磁場サイズ｜Ｈ｜が求められる。ここで、ΔＨiEを数式１のように定義する。ここで、ｉは磁気コンパスの識別記号であり、ＨEは地球磁場を示す。
数式１
【数１２】
2005140789.tif 000008
そして、数式２のように定義する。
もし
数式２
【数１３】
2005140789.tif 000009
であれば、
2005140789.tif 000010
【００３０】
ここで、εHは臨界値を示し、ＳHは地球磁気が作用する領域であって、成功領域を示す。
【００３１】
数式１と数式２のように定義した後、図６に示したように、第１磁気コンパス１と第２磁気コンパス２のデータを読取ってＨ1およびＨ2をそれぞれ求め（６００段階）、前記Ｈ1とＨ2がＳHに属するか否かをチェックする（図４の４１０段階）。
【００３２】
前記移動物体の第１磁気コンパス磁場Ｈ1のサイズおよび第２磁気コンパス磁場Ｈ2のサイズをそれぞれ前記地球磁場ＨEのサイズと比較し、その差が両方とも所定の限界値より小さな場合（６１０段階）には、図７に示されたように、加重値割方式（weighted partition）を使用して磁気コンパスの最終方位角を計算する（図４の４２０段階）。前記第１磁気コンパスの方位角θc1と第２磁気コンパスの方位角θc2との差を求める（７００段階）。前記方位角の差が所定値εcより小さい場合には、地球磁場と磁気コンパスの磁場との差によって加重値を異ならせて移動物体の磁気コンパス方位角を求める。すなわち、ΔＨ1EとΔＨ2Eとの積を求め、その値が負数であれば（７１０段階）、数式３により求められる値を磁気コンパスの最終方位角θcとする（７２０段階）。
数式３
【数１４】
2005140789.tif 000011
もし、前記７１０段階で求められた値が正数であれば（７３０段階）、数式４により求められる値を磁気コンパスの最終方位角θcとする（７４０段階）。
数式４
【数１５】
2005140789.tif 000012
もし、前記７１０段階でΔＨ1EとΔＨ2Eとの積を求め、その値が０であれば、数式５により求められる値を磁気コンパスの最終方位角θcとする（７５０段階）。
数式５
【数１６】
2005140789.tif 000013
【００３３】
一方、前記７００段階で、前記第１磁気コンパスの方位角θc1と第２磁気コンパスの方位角θc2との差が所定値εcより小さくない場合には、数式６を満足するか否かをチェックする（７６０段階）。
数式６
【数１７】
2005140789.tif 000014
【００３４】
前記数式６を満足すれば、第１磁気コンパス１の方位角θc1を磁気コンパスの最終方位角θcとする（７７０段階）。もし、数式６を満足せずに数式７を満足すれば（７８０段階）、第２磁気コンパス２の方位角θc2を磁気コンパスの最終方位角θcとする（７９０段階）。
数式７
【数１８】
2005140789.tif 000015
【００３５】
もし、７６０段階および７８０段階で等式が成立すれば、７５０段階と同様に数式５により求められる値を磁気コンパスの最終方位角θcとする。
【００３６】
前記数式３ないし７で、Δθc1、Δθc2はサンプリング周期間の第１磁気コンパス１および第２磁気コンパス２の方位角変化量であり、ωgはジャイロの角速度であり、Δｔはサンプリング時間を意味する。
一方、図４の４５０段階について、図６および図８によって、より詳細に説明する。図６において、前記移動物体の第１磁気コンパス磁場Ｈ1のサイズおよび第２磁気コンパス磁場Ｈ2のサイズをそれぞれ前記地球磁場ＨEのサイズと比較して、第１磁気コンパス磁場Ｈ1のサイズは所定の限界値より小さいが、第２磁気コンパス磁場Ｈ2のサイズは所定の限界値より小さくない場合には（６２０段階）、図８に示されたように第１磁気コンパス１の方位角Δθc1を磁気コンパスの最終方位角θcとする（８００段階）。
【００３７】
そして、図６において、前記移動物体の第１磁気コンパス磁場Ｈ1のサイズおよび第２磁気コンパス磁場Ｈ2のサイズをそれぞれ前記地球磁場ＨEのサイズと比較して、第２磁気コンパス磁場Ｈ1のサイズは所定の限界値より小さいが、第１磁気コンパス磁場Ｈ2のサイズは所定の限界値より小さくない場合には（６３０段階）、第２磁気コンパス２の方位角Δθc2を磁気コンパスの最終方位角θcとする（８１０段階）。
【００３８】
そして、前記方法により求められた磁気コンパスの最終方位角θcは、エラー状態フィードバックカルマンフィルタを通じて（４３０段階）、最適に推定される進行方向を求めるのに利用される（４４０段階）。
【００３９】
一方、図６で前記移動物体の第１磁気コンパス磁場Ｈ1のサイズおよび第２磁気コンパス磁場Ｈ2のサイズをそれぞれ前記地球磁場ＨEのサイズと比較して、両方とも所定の限界値より小さくない場合には磁気コンパスデータを使用せずにジャイロと走行距離計の角速度の差をチェックする（４６０段階、８２０段階）。
【００４０】
図４の４６０段階ないし４９５段階について、図９によって、より詳細に説明する。図９は、ジャイロと走行距離計の角速度をチェックして移動物体の進行方向を推定する過程を示したフローチャートである。まず、数式８で走行距離計から角速度ωｏを計算する（９００段階）。
数式８
【数１９】
2005140789.tif 000016
ここで、
2005140789.tif 000017
はホイールの速度であり、ｌはトレッド長さ、すなわちホイール間の距離を示す。
【００４１】
ジャイロ角速度ωgと走行距離計の角速度ωｏとの差が所定値εω以内であるか否かをチェックし（９１０段階）、所定値以内であれば、走行距離計データでエラー状態フィードバックカルマンフィルタを駆動する（９２０段階）。その後、最適な進行方向を推定する（９３０段階）。
【００４２】
そして、前記ジャイロ角速度と走行距離計の角速度との差が所定値以内であるか否かをチェックし（９１０段階）、所定値より小さくなければ、カルマンフィルタを使用せずにジャイロの角速度を積分して（９４０段階）、副最適進行方向を計算する（９５０段階）。そして、失敗インデックスの値を増加させ（９６０段階）、前記インデックスが所定の臨界値を超過すれば（９７０段階）、警告してシステムの作動を中断させる（９８０段階）。そして、前記失敗インデックスが所定の臨界値と等しいかまたは所定の臨界値より小さければ、磁気コンパス、ジャイロおよび走行距離計のセンサーデータを読取る。
【００４３】
一方、走行距離計と移動物体の一例に該当するロボットのモデリングとを次のように行うことができる。
図１０は、ロボットの位置を２次元座標で示したものであって、Ｔsをサンプリング時間とする時、前記ロボットをモデリングすれば、数式９のように表わされる。
数式９
【数２０】
2005140789.tif 000018
【００４４】
そして、走行距離計をモデリングすれば、ホイールの直径を
2005140789.tif 000019
ホイールの１回転当たりのパルス数であるエンコーダ解像度（resolution）を
2005140789.tif 000020
ギア比をｎ、サンプリング間隔内のパルス数を
2005140789.tif 000021
とする時、数式１０のように表わされる。
数式１０
【数２１】
2005140789.tif 000022
そして、走行距離計の角速度は数式１１のように表わされる。
数式１１
【数２２】
2005140789.tif 000023
【００４５】
次に、ジャイロをモデリングすれば、次の通りである。まず、ジャイロの実際モデルは、移動物体の進行方向をθ、ジャイロドリフト率をｂ、ジャイロ出力をωg、分散σθを有するガウス白色雑音を
2005140789.tif 000024
分散σbを有するガウス白色雑音を
2005140789.tif 000025
とする時、数式１２のように表わされる。
数式１２
【数２３】
2005140789.tif 000026
【００４６】
そして、推定された進行方向を
2005140789.tif 000027
推定されたジャイロドリフトを
2005140789.tif 000028
（定数と仮定）とする時、ジャイロの推定モデル、すなわち積分器の出力は、数式１３のように表わされる。
数式１３
【数２４】
2005140789.tif 000029
【００４７】
次に、エラー状態をモデリングすれば、次の通りである。まず、エラー状態を数式１４のように定義する。
数式１４
【数２５】
2005140789.tif 000030
【００４８】
外部センサー測定モデルは、測定された進行方向をθm、分散
2005140789.tif 000031
を有するガウス白色雑音を
2005140789.tif 000032
とする時、数式１５のように表わすことができる。ここで、外部センサー測定入力は、前記方法による磁気コンパスの最終方位角、および走行距離計角速度から選択される。
数式１５
【数２６】
2005140789.tif 000033
【００４９】
そして、エラー状態モデルは、数式１３から数式１２を減算して、数式１６のように表わされる。
数式１６
【数２７】
2005140789.tif 000034
図１１は、エラー状態測定モデルを示したものである。
【００５０】
以上のモデルを離散モデルで表現すれば、次の通りである。まず、離散エラー状態を説明する。離散エラー状態は、数式１７のように定義できる。
数式１７
【数２８】
2005140789.tif 000035
前記数式１２の離散形態は、数式１８のように表わされる。
数式１８
【数２９】
2005140789.tif 000036
【００５１】
前記数式１３の離散形態は、数式１９で表わされる。
数式１９
【数３０】
2005140789.tif 000037
【００５２】
前記数式１８および前記数式１９から数式２０が導出できる。
数式２０
【数３１】
2005140789.tif 000038
【００５３】
数式１５の離散形態は、数式２１のように表現できる。
数式２１
【数３２】
2005140789.tif 000039
【００５４】
次に、離散エラー状態カルマンフィルタについて詳細に説明する。初期条件は、数式２２および数式２３のように表わされる。数式２２は根源的に平均値が０であるガウスランダム変数であり、数式２３は初期値θ、ｂの分散を表したものである。
数式２２
【数３３】
2005140789.tif 000040
数式２３
【数３４】
2005140789.tif 000041
【００５５】
ガウス雑音の離散形態は、数式２４のように表現できる。
数式２４
【数３５】
2005140789.tif 000042
【００５６】
時間更新に対する離散形態は、数式２５および数式２６のように表わされる。
数式２５
【数３６】
2005140789.tif 000043
数式２６
【数３７】
2005140789.tif 000044
【００５７】
測定更新に対する離散形態は、数式２７、数式２８および数式２９のように表わされる。
数式２７
【数３８】
2005140789.tif 000045
数式２８
【数３９】
2005140789.tif 000046
数式２９
【数４０】
2005140789.tif 000047
【００５８】
数式１９から数式３０を導出でき、数式２８から数式３１を導出できる。
数式３０
【数４１】
2005140789.tif 000048
数式３１
【００５９】
【数４２】
2005140789.tif 000049
と定義する時、数式３０を数式３１に加算することによって、数式３２が得られる。
数式３２
【数４３】
2005140789.tif 000050
【００６０】
初期条件は、数式３３のようになる。
数式３３
【数４４】
2005140789.tif 000051
【００６１】
数式３２は、本発明で使われるエラー状態のフィードバックカルマンフィルタの代表数式を示す。前記方法によって磁気コンパスの方位角、走行距離計の角速度のうちの１つの測定入力を選択して数式３２のθmに代入すれば、移動方向の最適推定値
2005140789.tif 000052
を求めることができる。
【００６２】
本発明は、コンピュータ読取可能な記録媒体にコンピュータ（情報処理機能を有する装置を何れも含む）が読取可能なコードとして具現することが可能である。コンピュータが読取可能な記録媒体は、コンピュータシステムによって読取れるデータが保存される全種の記録装置を含む。コンピュータが読取可能な可読記録装置の例としては、ＲＯＭ、ＲＡＭ、ＣＤ−ＲＯＭ、磁気テープ、フロッピーディスク、光データ保存装置などがある。
【００６３】
本発明は図面に示された実施形態を参考に説明されたが、これは例示的なものに過ぎず、本発明の技術分野の当業者であれば、これより多様な変形および均等な他の実施形態が可能であることが理解できる。したがって、本発明の真の技術的保護範囲は、特許請求の範囲の技術的思想により定められねばならない。
【産業上の利用可能性】
【００６４】
本発明は、移動物体の位置推定に関わり、ヨー角度を効果的に求められることによって、３Ｄ角度情報を堅実に提供するので、絶対位置および姿勢推定が必要な全分野に適用できる。適用可能分野には、自律走行車両、知能型車両、車両のナビゲーションシステム、医療ロボット、バーチャルリアリティ(virtual reality)装置、エンターテインメント装置、無人飛行機、個人用ナビゲーションシステムなどがある。
【図面の簡単な説明】
【００６５】
【図１】（ｘ,ｙ）位置および方向θを推定するための２次元平面で動く移動ロボットを示す図面である。
【図２】本発明の実施形態による統合センサーシステムを示す概念図である。
【図３】本発明による移動物体の進行方向の推定システムに係る一実施形態を示すブロック図である。
【図４】本発明による移動物体の進行方向の推定方法に係る一実施形態を示すフローチャートである。
【図５】磁気コンパス情報から磁場を求めることを示す図面である。
【図６】第１磁気コンパス１と第２磁気コンパス２とにより求められたＨ1とＨ2とがＳHに属するか否かを示すフローチャートである。
【図７】加重値割方式を使用して移動物体の進行方向を計算するプロセスを示すフローチャートである。
【図８】図４で求めた磁気コンパスの磁場と地球磁場との差が所定値より小さくない場合、移動物体の進行方向の決定を示すフローチャートである。
【図９】ジャイロと走行距離計の角速度をチェックして移動物体の進行方向を推定する過程を示すフローチャートである。
【図１０】ロボットの位置を２次元座標で示す図面である。
【図１１】エラー状態の測定モデルを示す図面である。
【図１】
2005140789.tif 000059
【図２】
2005140789.tif 000060
【図３】
2005140789.tif 000061
【図４】
2005140789.tif 000062
【図５】
2005140789.tif 000063
【図６】
2005140789.tif 000064
【図７】
2005140789.tif 000065
【図８】
2005140789.tif 000066
【図９】
2005140789.tif 000067
【図１０】
2005140789.tif 000068
【図１１】
2005140789.tif 000069