JP4170284B2

JP4170284B2 - 移動物体の進行方向の推定方法およびシステム

Info

Publication number: JP4170284B2
Application number: JP2004323963A
Authority: JP
Inventors: 雄權; 慶植盧; 宇燮韓; 營輔沈; セルゲイボルディレフ
Original assignee: Samsung Electronics Co Ltd
Current assignee: Samsung Electronics Co Ltd
Priority date: 2003-11-08
Filing date: 2004-11-08
Publication date: 2008-10-22
Anticipated expiration: 2024-11-08
Also published as: US7543392B2; CN1651862A; JP2005140789A; KR100580628B1; CN100562711C; US20050125108A1; EP1530024A1; KR20050044967A

Description

本発明は、移動物体の位置推定に係り、特に、磁気コンパスデータを利用した移動物体の進行方向の推定方法およびシステムに関する。

移動物体の姿勢の推定は、絶対センサーまたは相対センサーを利用して物体の位置および方向を推定する問題であって、図１に示されるように、２次元平面で動く移動ロボットの場合には（ｘ,ｙ）位置および方向θを推定する問題である。絶対センサーは、ロボットまたは物体の相対的な動きの代わりに、絶対的な位置または姿勢を測定できるセンサーであって、カメラ、レーザースキャナ、センサー、全地球測位システム（ＧＰＳ:ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ）、磁気コンパスなどがある。これに対して、相対センサーは、ロボットまたは物体の相対的な変化量を測定し、これを統合することによって位置または姿勢を測定できるセンサーであって、ジャイロ、加速度計、走行距離計（モーターに装着されたエンコーダ）などが該当する。

絶対センサーのうち、カメラは周辺環境の照明状態に敏感であり、不確実な情報のデータを与える可能性が高く、レーザースキャナは正確なデータの獲得が可能であるが、コストが極めて高く、障害物が多い場合には測定が困難となるという短所がある。また、ソナーはデータの正確性に劣り、ＧＰＳは室外でのみ使用可能であり、しかも精度に劣り、磁気コンパスは室内に混在する外乱磁場の影響を受ける可能性が高い短所がある。一方、ジャイロ、走行距離計、加速度計などの相対センサーは、変化量のみを測定するので、その変化量の積分によって発生する積分誤差を避けられず、ジャイロ、加速度計に固有に存在するドリフト誤差は回避できない。

既存の推測航法の姿勢推定方式は、使われるセンサーによって、走行距離計のみを使用した方式、ジャイロおよび走行距離計を使用した方式、磁気コンパスおよび走行距離計を使用した方式に区分される。走行距離計のみを使用した方式は、最も簡単な方式であるが、スリッページエラー、バンプ衝突、空中移動の問題などに対処できず、エラーが累積し続けるので、限界のないエラーを有する。

これらの問題を解決するために、走行距離計にジャイロを追加して推測航法を行う方式が開発された。しかし、この方式は、走行距離計、すなわちモーターエンコーダのみを単独使用する方式に比べて正確な結果が得られるが、エンコーダとジャイロの何れも相対センサーであるので、長期的にはやはりエラーの限界を保証できない。ジャイロの代りに、絶対センサーである磁気コンパスを導入して長期的には進行方向を安定的に検出できる方式も開発されたが、家庭環境、オフィス環境などに常に存在している外乱磁場の影響を受けやすいので、この方式はそのまま実用化され難い。

最近、ジャイロ、磁気コンパス、走行距離計の全てを利用する方式も提案されている。この方式は、ジャイロと磁気コンパスとを同時に使用するので、相互補完可能な長所を有しているが、現在までに開発されたほとんどの方式は、統計に基づいたセンサー統合（ｆｕｓｉｏｎ）方法である既存のカルマンフィルタをそのまま使用することによって、カルマンフィルタ固有の短所による様々な限界を有する。すなわち、システム雑音および測定雑音がガウス白色雑音特性を有しなければならないので、磁場、障害物、滑りやすい床などの各種の不確実な状況に対処し難い。その他にも、センサーのエラーモデルに対する正確な情報およびエラーソース間に相互相関がないという仮定が必要であるが、これは現実的に実現不可能である。また、センサーの性能がセンサー統合に重要な要素として作用するので、この方式では、主に高価なセンサーが使用されている。

本発明が解決しようとする技術的課題は、移動物体の進行方向の角度を正確に、かつ堅実に推定して物体の姿勢情報を算出できる統合センサー（sensor fusion）を利用した移動物体の進行方向の推定方法およびシステムを提供することである。

本発明が解決しようとする他の技術的課題は、前記の移動物体の進行方向の推定方法をコンピュータで実行させるためのプログラムを記録したコンピュータ読取可能な記録媒体を提供することである。

本発明は、絶対センサーのうち磁気コンパスを利用し、相対センサーのうちジャイロおよび走行距離計を利用しながら同時に統合センサーを具現して前記短所を補完するものである。
すなわち、前記技術的課題を解決するために、本発明による移動物体の進行方向の推定方法は、移動物体に装着された磁気コンパスから磁場情報を求める第１段階と、前記移動物体の磁場を所定値と比較し、その結果によって移動物体が存在する地点が特定領域に属するか否かを判断する第２段階と、前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して移動物体の方向を推定する第３段階と、を含み、前記所定値は地球磁場のサイズであり、前記特定領域は地球磁気が作用する領域であり、前記移動物体に２つの磁気コンパスが設置されており、前記第２段階では、移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場のサイズと比較して、その差が１つでも所定の限界値より小さな場合には移動物体が存在する地点が特定領域に属すると判断し、両方とも所定の限界値より小さくない場合には移動物体が存在する地点が特定領域に属していないと判断することを特徴とする。

前記第３段階は、移動物体が存在する地点が特定領域に属すると判断される場合、磁気コンパスの最終方位角を求め、前記方位角を測定入力として採択したカルマンフィルタを利用して最適の進行方向角度を推定し、移動物体が存在する地点が特定領域に属していないと判断される場合、ジャイロの角速度と走行距離計のセンサーデータに基づいて得られた角速度とを比較し、その差が所定値より小さい場合、走行距離計のセンサーデータに基づいて得られた角速度を測定入力として採択したカルマンフィルタを利用して最適の進行方向を推定し、前記差が所定値より小さくない場合、ジャイロの角速度を積分して進行方向を推定する段階であることが望ましい。

前記第３段階は、移動物体が存在する地点が特定領域に属すると判断される場合において、前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場のサイズと比較してその差が両方とも所定の限界値より小さな場合に、前記第１磁気コンパスの方位角と第２磁気コンパスの方位角との差を求める段階と、前記方位角の差が所定値より小さい場合には、地球磁場のサイズと磁気コンパスの磁場サイズとの差によって加重値を異ならせて移動物体の磁気コンパス方位角を求める加重値段階と、前記方位角の差が所定値より小さくない場合には、移動物体に対するジャイロの角速度と磁気コンパスの方位角に対する角速度との差をそれぞれ求めてそのサイズによって移動物体の磁気コンパス方位角を求める角速度段階と、を含み、前記加重値は、前記第１磁気コンパスの方位角θ _ｃ１及び前記第２磁気コンパスの方位角θ _ｃ２に基づいて移動物体の磁気コンパス方位角を求める際に用いられる値であって、前記第１磁気コンパスの方位角θ _ｃ１に乗算される第１の加重値と、前記第２磁気コンパスの方位角θ _ｃ２に乗算される第２の加重値と、からなり、移動物体の磁気コンパス方位角は、前記第１磁気コンパスの方位角θ _ｃ１と前記第１の加重値との積と、前記第２磁気コンパスの方位角θ _ｃ２と前記第２の加重値との積と、の和であることを特徴とする。

前記加重値段階は、Δθ_c1、Δθ_c2をサンプリング周期の間の第１磁気コンパスおよび第２磁気コンパスの方位角変化量、ω_gはジャイロの角速度、Δｔをサンプリング時間とし、前記第１磁気コンパスの磁場サイズと地球磁場のサイズとの差をΔＨ_1E、前記第２磁気コンパスの磁場サイズと地球磁場のサイズとの差をΔＨ_2Eとする時、前記ΔＨ_1EとΔＨ_2Eとの積を求めてその値が負数であれば、数式３により求められる値を磁気コンパスの最終方位角θ_cとし、もし、前記ΔＨ_1EとΔＨ_2Eとの積が正数であれば、数式４により求められる値を磁気コンパスの最終方位角θ_cとし、もし、前記積の値が０であれば、数式５により求められる値を磁気コンパスの最終方位角θ_cとすることが望ましい。

前記角速度段階では、Δθ_c1、Δθ_c2をサンプリング周期の間の第１磁気コンパスおよび第２磁気コンパスの方位角変化量、ω_gはジャイロの角速度、Δｔをサンプリング時間とし、前記第１磁気コンパスの方位角θ_c1と第２磁気コンパスの方位角θ_c2との差が所定値より小さくない場合には、数式６を満足するか否かをチェックし、前記数式６を満足すれば、第１磁気コンパスの方位角θ_c1を磁気コンパスの最終方位角θ_cとし、もし、数式６を満足せずに数式７を満足すれば、第２磁気コンパスの方位角θ_c2を磁気コンパスの最終方位角θ_cとし、もし、前記数式６および数式７を満足しない場合には、数式５により求められる値を磁気コンパスの最終方位角θ_cとすることが望ましい。
数式３

数式４

数式５

数式６

数式７

前記第３段階では、移動物体が存在する地点が特定領域に属すると判断される場合において、前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場のサイズと比較して、その差のうち１つのみ所定の限界値より小さな場合に、前記第１磁気コンパス及び前記第２磁気コンパスのうち、前記所定の限界値よりも磁場サイズが小さな磁気コンパスの方位角を磁気コンパスの最終方位角とすることが望ましい。

前記第３段階は、移動物体が存在する地点が特定領域に属していないと判断される場合に、移動物体の走行距離計のセンサーデータに基づいて得られた角速度を計算する段階と、前記移動物体のジャイロ角速度と前記走行距離計のセンサーデータに基づいて得られた角速度との差を求める段階と、前記ジャイロ角速度と前記走行距離計のセンサーデータに基づいて得られた角速度との差が所定値より小さければ、走行距離計のセンサーデータに基づいて得られた角速度を測定入力として採択したカルマンフィルタを利用して最適の進行方向角度を推定する段階と、前記ジャイロ角速度と前記走行距離計のセンサーデータに基づいて得られた角速度との差が所定値より小さくなければ、カルマンフィルタを使用せずにジャイロの角速度を積分して進行方向角度を推定する積分推定段階と、を含むことが望ましい。前記積分推定段階は、所定時間内に前記積分推定段階が所定回数以上行われる場合にはシステムを終了する段階をさらに具備することを特徴とする。

前記移動物体の進行方向の推定方法は、エラー状態フィードバックカルマンフィルタを通じて移動物体の進行方向角度をフィルタリングして、移動物体に対する最適の進行方向推定値を計算する段階をさらに具備することを特徴とする。

前記技術的課題を解決するために、本発明による移動物体の進行方向の推定システムは、ジャイロ、走行距離計および磁気コンパスが設置された移動物体において、前記磁気コンパスが設置された移動物体の磁場サイズを計算する磁場算出部と、前記磁場サイズと地球磁場のサイズとの差を求めて所定の限界値と比較する磁場比較部と、前記比較結果によって移動物体が存在する地点が地球磁気が作用する領域に属するか否かを判断する地球磁気領域判断部と、前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して、移動物体の方向を推定する進行方向推定部と、を含み、前記移動物体に２つの磁気コンパスが設置されており、前記地球磁気領域判断部は、移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズと前記地球磁場のサイズとの差をそれぞれ求めて、その差が１つでも所定の限界値より小さな場合には、移動物体が存在する地点が、地球磁気が作用する領域に属すると判断し、両方とも所定の限界値より小さくない場合には、移動物体が存在する地点が、地球磁気が作用する領域に属していないと判断することを特徴とする。

そして、前記記載された方法をコンピュータで実行させるためのプログラムを記録したコンピュータ読取可能な記録媒体を提供する。

本発明による移動物体の進行方向の推定方法およびシステムは、センサーの特性よりセンサーの統合が重要な要素として作用するので、高性能の仕様のセンサーを必要とせず、廉価なセンサーで高性能を具現可能である。ジャイロ、磁気コンパスおよび走行距離計センサーのそれぞれの短所をカバーし、長所を活用することによって移動物体の移動時に発生する可能性がある多くの突発状況に対して対処でき、多様な種類のエラー源を推定できる。また、センサーのエラー特性に対する正確な情報を必要とせず、バイアス、有色ノイズ、非システム的なエラーなどに対する対処が可能であるなどカルマンフィルタが有している様々な固有の限界を克服できる。付加的な効果としてシステムの誤動作の感知機能が具現可能になる。

以下、図面を参照して本発明による方法および装置について詳細に説明する。
物体のロールとピッチは加速度計およびジャイロを利用して正確に計算できるが、物体のヨー値を求めることは極めて難しい問題として知られている。本発明では、移動物体の進行方向を正確に、かつ堅実に推定し、物体の姿勢情報を算出できる新たな統合センサーを利用して移動物体の進行方向を推定する。すなわち、磁気コンパス、ジャイロ、走行距離計などの絶対センサーと相対センサーとを組合わせて、磁場、障害物、滑りやすい床などの各種の不確実な環境に効果的に対処しながら移動物体のヨー角度を獲得して位置および姿勢を最適に推定する。

図２は、本発明の実施形態による移動物体の進行方向推定のために、移動物体に設置された統合センサーシステムを示したものであって、移動物体２００は、２つの磁気コンパス２１０，２２０と、ジャイロ２３０と、２つのホイールエンコーダ２４０，２５０（走行距離計）とを具備している。

使われたセンサーは、それぞれ次のような特性を有する。
ジャイロ２３０は、バイアスドリフトエラーと角度値とを計算するために必要な積分過程で長期的には積分エラーが発生するが、短期的には比較的正確な角度値を検出できる。一方、磁気コンパス２１０，２２０は、長期的には絶対方位角を与えるので、正確な情報を提供するが、短期的には生活環境で存在する磁場撹乱によるエラーを発生させる。一方、移動物体の駆動部（ロボットの場合には車輪）に設けられたホイールエンコーダ２４０，２５０は、長期的には滑り（slippage）によるエラーとホイールサイズおよび整列の如何などの動力学的な条件によるエラーが累積されてやはり制限されないエラー特性を示し、また、短期的にもバンプ衝突や空中移動のような突発的な状況に対処できない限界を有している。しかし、ほとんどの期間、比較的正確な情報を与える。

前記磁気コンパス２１０，２２０からは、磁場の方向と磁場サイズのデータとが得られる。前記ジャイロ２３０は角速度を検出でき、ホイールエンコーダ２４０，２５０からホイールの回転速度を検出して走行距離計を利用した移動物体の角速度を求めることができる。

本発明は、磁気コンパス、ジャイロ、走行距離計などの絶対センサーと相対センサーとを組合わせて、磁場、障害物、滑りやすい床などの各種の不確実な環境に効果的に対処しながら、物体のヨー角度を求めて位置および姿勢を最適に推定するために、規則基盤のカルマンフィルタを利用する。

図３は、本発明による移動物体の進行方向の推定システムに係る一実施形態を示したブロック図であって、磁場算出部３００、磁場比較部３１０、地球磁気領域判断部３２０および進行方向推定部３３０を含む。前記移動物体には、ジャイロ、走行距離計および磁気コンパスが設置されている。

移動物体は、磁気コンパス、ジャイロおよび走行距離計からセンサーデータを得る。そのうち磁気コンパス情報から磁場が求められる。磁場算出部３００は、前記磁気コンパスが設置された移動物体の磁場サイズを計算する。磁場比較部３１０は、前記磁場サイズと地球磁場のサイズとの差を求めて所定値ε_Hと比較する。地球磁気領域判断部３２０は、前記比較結果によって移動物体が存在する地点が地球磁気が作用する領域に属するか否かを判断する。進行方向推定部３３０は、前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して移動物体の方向を推定する。２つの磁気コンパスから得た磁場と、地球磁場との差を前記磁場比較部３１０で比較した結果、何れか１つでも前記所定値より小さな場合には、磁気コンパスデータから得た方位角を移動物体の方向推定に利用し、そうでない場合には、ジャイロの角速度と走行距離計のセンサーデータに基づいて得られた角速度とを利用して移動物体の方向推定に利用する。以下、その方向推定についてより詳細に説明する。

図４は、本発明による移動物体の進行方向の推定方法についての一実施形態を示したフローチャートであって、これに基づいて本発明を具体的に説明する。

磁気コンパス、ジャイロ、走行距離計のセンサーデータを読取る（４００段階）。まず、前記センサーデータのうち磁気コンパスデータから磁場を求める。図５は、磁気コンパス情報から磁場を求める方法を説明する図面である。磁気コンパスを通じて得られるｘ軸成分の磁場Ｈ_xとｙ軸成分の磁場Ｈ_yとから磁場サイズ｜Ｈ｜が求められる。ここで、ΔＨ_iEを数式１のように定義する。ここで、ｉは磁気コンパスの識別記号であり、Ｈ_Eは地球磁場を示す。
数式１

そして、数式２のように定義する。
もし
数式２

であれば、

ここで、ε_Hは臨界値を示し、Ｓ_Hは地球磁気が作用する領域であって、成功領域を示す。

数式１と数式２のように定義した後、図６に示したように、第１磁気コンパス１と第２磁気コンパス２のデータを読取ってＨ₁およびＨ₂をそれぞれ求め（６００段階）、前記Ｈ₁とＨ₂がＳ_Hに属するか否かをチェックする（図４の４１０段階）。

前記移動物体の第１磁気コンパス磁場Ｈ₁のサイズおよび第２磁気コンパス磁場Ｈ₂のサイズをそれぞれ前記地球磁場Ｈ_Eのサイズと比較し、その差が両方とも所定の限界値より小さな場合（６１０段階）には、図７に示されたように、加重値割方式（weighted partition）を使用して磁気コンパスの最終方位角を計算する（図４の４２０段階）。前記第１磁気コンパスの方位角θ_c1と第２磁気コンパスの方位角θ_c2との差を求める（７００段階）。前記方位角の差が所定値ε_cより小さい場合には、地球磁場と磁気コンパスの磁場との差によって加重値を異ならせて移動物体の磁気コンパス方位角を求める。すなわち、ΔＨ_1EとΔＨ_2Eとの積を求め、その値が負数であれば（７１０段階）、数式３により求められる値を磁気コンパスの最終方位角θ_cとする（７２０段階）。
数式３

もし、前記７１０段階で求められた値が正数であれば（７３０段階）、数式４により求められる値を磁気コンパスの最終方位角θ_cとする（７４０段階）。
数式４

もし、前記７１０段階でΔＨ_1EとΔＨ_2Eとの積を求め、その値が０であれば、数式５により求められる値を磁気コンパスの最終方位角θ_cとする（７５０段階）。
数式５

一方、前記７００段階で、前記第１磁気コンパスの方位角θ_c1と第２磁気コンパスの方位角θ_c2との差が所定値ε_cより小さくない場合には、数式６を満足するか否かをチェックする（７６０段階）。
数式６

前記数式６を満足すれば、第１磁気コンパス１の方位角θ_c1を磁気コンパスの最終方位角θ_cとする（７７０段階）。もし、数式６を満足せずに数式７を満足すれば（７８０段階）、第２磁気コンパス２の方位角θ_c2を磁気コンパスの最終方位角θ_cとする（７９０段階）。
数式７

もし、７６０段階および７８０段階で等式が成立すれば、７５０段階と同様に数式５により求められる値を磁気コンパスの最終方位角θ_cとする。

前記数式３ないし７で、Δθ_c1、Δθ_c2はサンプリング周期間の第１磁気コンパス１および第２磁気コンパス２の方位角変化量であり、ω_gはジャイロの角速度であり、Δｔはサンプリング時間を意味する。
一方、図４の４５０段階について、図６および図８によって、より詳細に説明する。図６において、前記移動物体の第１磁気コンパス磁場Ｈ₁のサイズおよび第２磁気コンパス磁場Ｈ₂のサイズをそれぞれ前記地球磁場Ｈ_Eのサイズと比較して、第１磁気コンパス磁場Ｈ₁のサイズは所定の限界値より小さいが、第２磁気コンパス磁場Ｈ₂のサイズは所定の限界値より小さくない場合には（６２０段階）、図８に示されたように第１磁気コンパス１の方位角Δθ_c1を磁気コンパスの最終方位角θ_cとする（８００段階）。

そして、図６において、前記移動物体の第１磁気コンパス磁場Ｈ₁のサイズおよび第２磁気コンパス磁場Ｈ₂のサイズをそれぞれ前記地球磁場Ｈ_Eのサイズと比較して、第２磁気コンパス磁場Ｈ₁のサイズは所定の限界値より小さいが、第１磁気コンパス磁場Ｈ₂のサイズは所定の限界値より小さくない場合には（６３０段階）、第２磁気コンパス２の方位角Δθ_c2を磁気コンパスの最終方位角θ_cとする（８１０段階）。

そして、前記方法により求められた磁気コンパスの最終方位角θ_cは、エラー状態フィードバックカルマンフィルタを通じて（４３０段階）、最適に推定される進行方向を求めるのに利用される（４４０段階）。

一方、図６で前記移動物体の第１磁気コンパス磁場Ｈ₁のサイズおよび第２磁気コンパス磁場Ｈ₂のサイズをそれぞれ前記地球磁場Ｈ_Eのサイズと比較して、両方とも所定の限界値より小さくない場合には磁気コンパスデータを使用せずにジャイロの角速度と走行距離計のセンサーデータに基づいて得られた角速度との差をチェックする（４６０段階、８２０段階）。

図４の４６０段階ないし４９５段階について、図９によって、より詳細に説明する。図９は、ジャイロの角速度と走行距離計のセンサーデータに基づいて得られた角速度とをチェックして移動物体の進行方向を推定する過程を示したフローチャートである。まず、数式８で走行距離計から角速度ω_ｏを計算する（９００段階）。
数式８

ここで、

はホイールの速度であり、ｌはトレッド長さ、すなわちホイール間の距離を示す。

ジャイロの角速度ω_gと走行距離計のセンサーデータに基づいて得られた角速度ω_ｏとの差が所定値ε_ω以内であるか否かをチェックし（９１０段階）、所定値以内であれば、走行距離計データでエラー状態フィードバックカルマンフィルタを駆動する（９２０段階）。その後、最適な進行方向を推定する（９３０段階）。

そして、前記ジャイロの角速度と走行距離計のセンサーデータに基づいて得られた角速度との差が所定値以内であるか否かをチェックし（９１０段階）、所定値より小さくなければ、カルマンフィルタを使用せずにジャイロの角速度を積分して（９４０段階）、副最適進行方向を計算する（９５０段階）。そして、失敗インデックスの値を増加させ（９６０段階）、前記インデックスが所定の臨界値を超過すれば（９７０段階）、警告してシステムの作動を中断させる（９８０段階）。そして、前記失敗インデックスが所定の臨界値と等しいかまたは所定の臨界値より小さければ、磁気コンパス、ジャイロおよび走行距離計のセンサーデータを読取る。

一方、走行距離計と移動物体の一例に該当するロボットのモデリングとを次のように行うことができる。
図１０は、ロボットの位置を２次元座標で示したものであって、Ｔ_sをサンプリング時間とする時、前記ロボットをモデリングすれば、数式９のように表わされる。
数式９

そして、走行距離計をモデリングすれば、ホイールの直径を

ホイールの１回転当たりのパルス数であるエンコーダ解像度（resolution）を

ギア比をｎ、サンプリング間隔内のパルス数を

とする時、数式１０のように表わされる。
数式１０

そして、走行距離計の角速度は数式１１のように表わされる。
数式１１

次に、ジャイロをモデリングすれば、次の通りである。まず、ジャイロの実際モデルは、移動物体の進行方向をθ、ジャイロドリフト率をｂ、ジャイロ出力をω_g、分散σθを有するガウス白色雑音を

分散σ_bを有するガウス白色雑音を

とする時、数式１２のように表わされる。
数式１２

そして、推定された進行方向を

推定されたジャイロドリフトを

（定数と仮定）とする時、ジャイロの推定モデル、すなわち積分器の出力は、数式１３のように表わされる。
数式１３

次に、エラー状態をモデリングすれば、次の通りである。まず、エラー状態を数式１４のように定義する。
数式１４

外部センサー測定モデルは、測定された進行方向をθ_m、分散

を有するガウス白色雑音を

とする時、数式１５のように表わすことができる。ここで、外部センサー測定入力は、前記方法による磁気コンパスの最終方位角、および走行距離計角速度から選択される。
数式１５

そして、エラー状態モデルは、数式１３から数式１２を減算して、数式１６のように表わされる。
数式１６

図１１は、エラー状態測定モデルを示したものである。

以上のモデルを離散モデルで表現すれば、次の通りである。まず、離散エラー状態を説明する。離散エラー状態は、数式１７のように定義できる。
数式１７

前記数式１２の離散形態は、数式１８のように表わされる。
数式１８

前記数式１３の離散形態は、数式１９で表わされる。
数式１９

前記数式１８および前記数式１９から数式２０が導出できる。
数式２０

数式１５の離散形態は、数式２１のように表現できる。
数式２１

次に、離散エラー状態カルマンフィルタについて詳細に説明する。初期条件は、数式２２および数式２３のように表わされる。数式２２は根源的に平均値が０であるガウスランダム変数であり、数式２３は初期値θ、ｂの分散を表したものである。
数式２２

数式２３

ガウス雑音の離散形態は、数式２４のように表現できる。
数式２４

時間更新に対する離散形態は、数式２５および数式２６のように表わされる。
数式２５

数式２６

測定更新に対する離散形態は、数式２７、数式２８および数式２９のように表わされる。
数式２７

数式２８

数式２９

数式１９から数式３０を導出でき、数式２８から数式３１を導出できる。
数式３０

数式３１

と定義する時、数式３０を数式３１に加算することによって、数式３２が得られる。
数式３２

初期条件は、数式３３のようになる。
数式３３

数式３２は、本発明で使われるエラー状態のフィードバックカルマンフィルタの代表数式を示す。前記方法によって磁気コンパスの方位角、走行距離計の角速度のうちの１つの測定入力を選択して数式３２のθ_mに代入すれば、移動方向の最適推定値

を求めることができる。

本発明は、コンピュータ読取可能な記録媒体にコンピュータ（情報処理機能を有する装置を何れも含む）が読取可能なコードとして具現することが可能である。コンピュータが読取可能な記録媒体は、コンピュータシステムによって読取れるデータが保存される全種の記録装置を含む。コンピュータが読取可能な可読記録装置の例としては、ＲＯＭ、ＲＡＭ、ＣＤ−ＲＯＭ、磁気テープ、フロッピーディスク、光データ保存装置などがある。

本発明は図面に示された実施形態を参考に説明されたが、これは例示的なものに過ぎず、本発明の技術分野の当業者であれば、これより多様な変形および均等な他の実施形態が可能であることが理解できる。したがって、本発明の真の技術的保護範囲は、特許請求の範囲の技術的思想により定められねばならない。

本発明は、移動物体の位置推定に関わり、ヨー角度を効果的に求められることによって、３Ｄ角度情報を堅実に提供するので、絶対位置および姿勢推定が必要な全分野に適用できる。適用可能分野には、自律走行車両、知能型車両、車両のナビゲーションシステム、医療ロボット、バーチャルリアリティ(virtual reality)装置、エンターテインメント装置、無人飛行機、個人用ナビゲーションシステムなどがある。

（ｘ,ｙ）位置および方向θを推定するための２次元平面で動く移動ロボットを示す図面である。本発明の実施形態による統合センサーシステムを示す概念図である。本発明による移動物体の進行方向の推定システムに係る一実施形態を示すブロック図である。本発明による移動物体の進行方向の推定方法に係る一実施形態を示すフローチャートである。磁気コンパス情報から磁場を求めることを示す図面である。第１磁気コンパス１と第２磁気コンパス２とにより求められたＨ₁とＨ₂とがＳ_Hに属するか否かを示すフローチャートである。加重値割方式を使用して移動物体の進行方向を計算するプロセスを示すフローチャートである。図４で求めた磁気コンパスの磁場と地球磁場との差が所定値より小さくない場合、移動物体の進行方向の決定を示すフローチャートである。ジャイロの角速度と走行距離計のセンサーデータに基づいて得られた角速度とをチェックして移動物体の進行方向を推定する過程を示すフローチャートである。ロボットの位置を２次元座標で示す図面である。エラー状態の測定モデルを示す図面である。

Claims

移動物体に装着された磁気コンパスから磁場情報を求める第１段階と、
前記移動物体の磁場を所定値と比較し、その結果によって移動物体が存在する地点が特定領域に属するか否かを判断する第２段階と、
前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して移動物体の方向を推定する第３段階と、
を含み、
前記所定値は地球磁場のサイズであり、
前記特定領域は地球磁気が作用する領域であり、
前記移動物体に２つの磁気コンパスが設置されており、
前記第２段階では、
移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場のサイズと比較して、その差が１つでも所定の限界値より小さな場合には移動物体が存在する地点が特定領域に属すると判断し、両方とも所定の限界値より小さくない場合には移動物体が存在する地点が特定領域に属していないと判断する
ことを特徴とする移動物体の進行方向の推定方法。
前記第３段階は、
移動物体が存在する地点が特定領域に属すると判断される場合、磁気コンパスの最終方位角を求め、前記方位角を測定入力として採択したカルマンフィルタを利用して最適の進行方向角度を推定し、
移動物体が存在する地点が特定領域に属していないと判断される場合、ジャイロの角速度と走行距離計のセンサーデータに基づいて得られた角速度とを比較し、その差が所定値より小さい場合、走行距離計のセンサーデータに基づいて得られた角速度を測定入力として採択したカルマンフィルタを利用して最適の進行方向を推定し、前記差が所定値より小さくない場合、ジャイロの角速度を積分して進行方向を推定する
段階である
ことを特徴とする請求項１に記載の移動物体の進行方向の推定方法。
前記第３段階は、
移動物体が存在する地点が特定領域に属すると判断される場合において、前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場のサイズと比較してその差が両方とも所定の限界値より小さな場合に、
前記第１磁気コンパスの方位角と第２磁気コンパスの方位角との差を求める段階と、
前記方位角の差が所定値より小さい場合には、地球磁場のサイズと磁気コンパスの磁場サイズとの差によって加重値を異ならせて移動物体の磁気コンパス方位角を求める加重値段階と、
前記方位角の差が所定値より小さくない場合には、移動物体に対するジャイロの角速度と磁気コンパスの方位角に対する角速度との差をそれぞれ求めてそのサイズによって移動物体の磁気コンパス方位角を求める角速度段階と、
を含み、
前記加重値は、前記第１磁気コンパスの方位角θ _ｃ１及び前記第２磁気コンパスの方位角θ _ｃ２に基づいて移動物体の磁気コンパス方位角を求める際に用いられる値であって、前記第１磁気コンパスの方位角θ _ｃ１に乗算される第１の加重値と、前記第２磁気コンパスの方位角θ _ｃ２に乗算される第２の加重値と、からなり、
移動物体の磁気コンパス方位角は、前記第１磁気コンパスの方位角θ _ｃ１と前記第１の加重値との積と、前記第２磁気コンパスの方位角θ _ｃ２と前記第２の加重値との積と、の和である
ことを特徴とする請求項１に記載の移動物体の進行方向の推定方法。
前記加重値段階は、Δθ_c1、Δθ_c2をサンプリング周期の間の第１磁気コンパスおよび第２磁気コンパスの方位角変化量、ω_gはジャイロの角速度、Δｔをサンプリング時間とし、前記第１磁気コンパスの磁場サイズと地球磁場のサイズとの差をΔＨ_1E、前記第２磁気コンパスの磁場サイズと地球磁場のサイズとの差をΔＨ_2Eとする時、
前記ΔＨ_1EとΔＨ_2Eとの積を求めてその値が負数であれば、数式３により求められる値を磁気コンパスの最終方位角θ_cとし、
数式３

もし、前記ΔＨ_1EとΔＨ_2Eとの積が正数であれば、数式４により求められる値を磁気コンパスの最終方位角θ_cとし、
数式４

もし、前記積の値が０であれば、数式５
数式５

により求められる値を磁気コンパスの最終方位角θ_cとする
ことを特徴とする請求項３に記載の移動物体の進行方向の推定方法。
前記角速度段階では、
Δθ_c1、Δθ_c2をサンプリング周期の間の第１磁気コンパスおよび第２磁気コンパスの方位角変化量、ω_gはジャイロの角速度、Δｔをサンプリング時間とし、
前記第１磁気コンパスの方位角θ_c1と第２磁気コンパスの方位角θ_c2との差が所定値より小さくない場合には、数式６を満足するか否かをチェックし、
数式６

前記数式６を満足すれば、第１磁気コンパスの方位角θ_c1を磁気コンパスの最終方位角θ_cとし、
もし、数式６を満足せずに数式７を満足すれば、第２磁気コンパスの方位角θ_c2を磁気コンパスの最終方位角θ_cとし、
数式７

もし、前記数式６および数式７を満足しない場合には、数式５により求められる値を
数式５

磁気コンパスの最終方位角θ_cとする
ことを特徴とする請求項３に記載の移動物体の進行方向の推定方法。
前記第３段階では、
移動物体が存在する地点が特定領域に属すると判断される場合において、前記移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズをそれぞれ前記地球磁場のサイズと比較して、その差のうち１つのみ所定の限界値より小さな場合に、
前記第１磁気コンパス及び前記第２磁気コンパスのうち、前記所定の限界値よりも磁場サイズが小さな磁気コンパスの方位角を磁気コンパスの最終方位角とする
ことを特徴とする請求項１に記載の移動物体の進行方向の推定方法。
前記第３段階は、
移動物体が存在する地点が特定領域に属していないと判断される場合に、
移動物体の走行距離計のセンサーデータに基づいて得られた角速度を計算する段階と、
前記移動物体のジャイロ角速度と前記走行距離計のセンサーデータに基づいて得られた角速度との差を求める段階と、
前記ジャイロ角速度と前記走行距離計のセンサーデータに基づいて得られた角速度との差が所定値より小さければ、走行距離計のセンサーデータに基づいて得られた角速度を測定入力として採択したカルマンフィルタを利用して最適の進行方向角度を推定する段階と、
前記ジャイロ角速度と前記走行距離計のセンサーデータに基づいて得られた角速度との差が所定値より小さくなければ、カルマンフィルタを使用せずにジャイロの角速度を積分して進行方向角度を推定する積分推定段階と、
を含むことを特徴とする請求項１に記載の移動物体の進行方向の推定方法。
前記積分推定段階は、
所定時間内に前記積分推定段階が所定回数以上行われる場合にはシステムを終了する段階をさらに具備する
ことを特徴とする請求項７に記載の移動物体の進行方向の推定方法。
エラー状態フィードバックカルマンフィルタを通じて移動物体の進行方向角度をフィルタリングして、移動物体に対する最適の進行方向推定値を計算する段階をさらに具備する
ことを特徴とする請求項３または請求項６に記載の移動物体の進行方向の推定方法。
請求項１ないし請求項９のいずれか１項に記載の移動物体の進行方向の推定方法をコンピュータで実行させるためのプログラムを記録したコンピュータによって読取可能な記録媒体。
ジャイロ、走行距離計および磁気コンパスが設置された移動物体において、
前記磁気コンパスが設置された移動物体の磁場サイズを計算する磁場算出部と、
前記磁場サイズと地球磁場のサイズとの差を求めて所定の限界値と比較する磁場比較部と、
前記比較結果によって移動物体が存在する地点が地球磁気が作用する領域に属するか否かを判断する地球磁気領域判断部と、
前記判断結果によって磁気コンパスの方位角を移動物体の方向推定に利用するか否かを決定して、移動物体の方向を推定する進行方向推定部と、
を含み、
前記移動物体に２つの磁気コンパスが設置されており、
前記地球磁気領域判断部は、
移動物体の第１磁気コンパスの磁場サイズおよび第２磁気コンパスの磁場サイズと前記地球磁場のサイズとの差をそれぞれ求めて、その差が１つでも所定の限界値より小さな場合には、移動物体が存在する地点が、地球磁気が作用する領域に属すると判断し、両方とも所定の限界値より小さくない場合には、移動物体が存在する地点が、地球磁気が作用する領域に属していないと判断する
ことを特徴とする移動物体の進行方向の推定システム。