JPH07200777A

JPH07200777A - 移動体における位置推定方法

Info

Publication number: JPH07200777A
Application number: JP5335665A
Authority: JP
Inventors: Haruo Takeda; 晴夫武田; Shinichiro Miyaoka; 伸一郎宮岡
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 1993-12-28
Filing date: 1993-12-28
Publication date: 1995-08-04

Abstract

(57)【要約】【目的】予め与えられた環境マップと、環境センサに
よる観測データを基に、高い精度で移動体の推定位置の
最尤推定結果を得ることを可能とする、移動体における
位置推定方法を提供すること。【構成】環境マップと、環境センサにより環境内の複
数の方向にそれぞれ最も近い障害物までの距離を計測し
たデータと、同じ領域内では前記光線と障害物との対応
関係が等しくなるように、前記移動体のコンフィギュレ
ーション空間を分割した領域データが与えられたとき、
各領域単位に、前記計測データと環境マップとのマッチ
ング誤差が最小となる領域内誤差最小コンフィギュレー
ションを計算し、これら複数の最小コンフィギュレーシ
ョンの誤差の値のうち最も小さい値に対応する全域的誤
差最小コンフィギュレーションを求めることを特徴とす
る移動体における位置推定方法。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は移動体における位置推定
方法に関し、特に、予め与えられた環境マップと、環境
センサによる計測データを基に自らの位置を推定する、
移動体における位置推定方法に関する。

【０００２】

【従来の技術】従来、工場や一般オフィス内での自律型
移動ロボット，一般家庭内での自動移動掃除機，屋外の
自動運転自動車など、環境マップを先見知識とする移動
体のナビゲーションを実現するためには、環境センサに
よる観測データと、予め与えられている環境マップとか
ら、移動体の位置推定を如何に正確に行うかが基本的課
題となる。移動体の環境は、一般に可視グラフ、ボロノ
イ図など計算幾何学的手法(例えば、文献：Ｈerbert Ｅ
delsbrunner, “Ａlgorithms in ＣombinatorialＧeome
try”Ｓpringer-Ｖerlag, 1987 参照)の容易さなどの理
由から、２次元平面の線分の集合で記述されることが多
い。一方、環境センサは、放射方向に複数の光線を持
ち、各光線に沿って最も近い障害物までの距離を計測す
る距離センサの集合で、通常モデル化できる。このと
き、上述の位置推定問題は、観測距離の組と環境マップ
の線分の集合との誤差が小さくなるような、移動***置
とセンサ方向を求めるパターンマッチング問題に帰着す
る。

【０００３】このようなマッチングで、従来、代表的な
方法として、例えば、文献：ＨaruoＴakeda and Ｊean-
Ｃlaude Ｌatombe, “Ｓensory Ｕncertainty Ｆield f
orＭobile Ｒobot Ｎavigation,”ＩＥＥＥＩnternati
onal Ｃonference onＲobotics and Ａutomation, Ｎic
e, Ｆrance, pp.2465-2472, 1992 に記述されている方
法がある。この方法は、まず、観測点の列から直線を構
成すると考えられる点の部分集合を選び、各部分集合に
最小２乗法を用いて直線を当てはめ、更に、各直線が矛
盾なく環境マップに合致する対応を求めることによって
マッチングを実現する方法である。

【０００４】

【発明が解決しようとする課題】点列に直線を当てはめ
る方法としての、上記従来技術に示した最小２乗法は、
直線の法線方向への観測データの誤差が正規分布に従う
場合に、その観測値の同時確率分布を最大とする最尤推
定直線を与える。但し、上述の２ステップからなる従来
技術では、前段のステップで考慮された推定直線と観測
点列の垂直距離誤差の２乗和を、後段のステップにおい
て直線の組と環境マップの線分の組とのマッチングの誤
差に反映することができなかった。また、観測点の列か
ら１本の直線を構成する点の部分集合を選択する処理
に、マッチングの誤差を反映するのが困難であった。こ
のため、得られる移動体の推定位置は、一般に最尤推定
結果とはならず、移動体の正確なナビゲーションに支障
を生じていた。本発明は上記事情に鑑みてなされたもの
で、その目的とするところは、従来の技術における上述
の如き問題を解消し、移動体の推定位置の最尤推定結果
を得ることを可能とする、移動体における位置推定方法
を提供することにある。

【０００５】

【課題を解決するための手段】本発明の上記目的は、環
境マップと、環境センサにより環境内の複数の方向にそ
れぞれ最も近い障害物までの距離を計測したデータと、
同じ領域内では前記光線と障害物との対応関係が等しく
なるように、前記移動体のコンフィギュレーション空間
を分割した領域データが与えられたとき、各領域単位
に、前記計測データと環境マップとのマッチング誤差が
最小となる領域内誤差最小コンフィギュレーションを計
算し、これら複数の最小コンフィギュレーションの誤差
の値のうち最も小さい値に対応する全域的誤差最小コン
フィギュレーションを求めることを特徴とする移動体に
おける位置推定方法によって達成される。

【０００６】

【作用】本発明に係る移動体における位置推定方法にお
いては、移動体は、環境センサによって環境内の複数の
方向にそれぞれ最も近い障害物までの距離を計測して得
られたデータが与えられると、同じ領域内では光線と障
害物との対応関係が等しくなるように移動体のコンフィ
ギュレーション空間を分割した領域単位に、前記計測デ
ータと仮定した移動体のコンフィギュレーションとか
ら、環境マップとのマッチング誤差を計算して、前記各
領域内で上述のマッチング誤差が最小となる移動体のコ
ンフィギュレーションを探索し、このようにして得られ
た複数の領域内誤差最小値の中で最も小さい値に対応す
るコンフィギュレーションを求める全域的誤差最小コン
フィギュレーションを選択することによって移動***置
を推定する。

【０００７】

【実施例】以下、本発明の実施例を図面に基づいて詳細
に説明する。図１は、本発明に係る移動***置推定環境
の一例を示すものである。図において、１は移動体であ
り、環境のセンサ２を装備している。また、３〜７は環
境内に存在する物体(障害物)、８は目標を示している。
このような環境内で、例えば、移動体１を、物体３〜７
に衝突することなく、また、自らの位置を見失うことな
く目標８まで導く場合などに、与えられたセンサデータ
から、各時点での移動体の位置を正確に推定することが
必須となる。図２は、上述のようなセンサ２を装備した
移動体１の具体的構成例を示すものである。この例で
は、センサ２は、環境内の物体に対して水平平面光線を
放射する光源２ａと、このような平面光線が物体上に作
る線Ｌを観測するためのカメラ２ｂから成る。

【０００８】カメラ２ｂで撮像される視野Ｖにおいて、
直線Ｌ位置の、視野高さＨに対する相対値を観測するこ
とによって、センサ２と物体との絶対距離を計算するこ
とができる。ここで、このような計算を、特定の間隔で
離散化した各方向について行うことによって、放射方向
の距離データの組を得ることができる。なお、環境まで
の距離センサとしては、超音波や赤外線，レーザ光など
による反射波を観測する方法も、実際に多く用いられて
いる。このようなセンサは、通常、移動体の周囲に放射
方向の光軸を向けて配置されるが、その場合には、本発
明に関しては、上述のカメラセンサと全く同じものとし
て、以下、説明を続けることができる。

【０００９】図３は、本発明の一実施例に係る移動***
置推定方法の全体のフローチャートである。本実施例は
大きく２つの処理から成る。すなわち、ステップ１０か
ら３０は環境のマップが与えられたときに行われる前処
理、ステップ４０から６０は実際にセンサの計測データ
が与えられたときに行われる位置推定処理である。図３
において、まず、ステップ１０で、環境マップが移動体
の情報処理手段に入力される。環境マップは、本実施例
では２次元平面における線分の集合とし、例えば、線分
の始点および終点の２次元座標のリストＥ_j＝(Ｅｘ_1j，
Ｅｙ_1j，Ｅｘ_2j，Ｅｙ_2j)、但しｊ＝１，２，・・・・，Ｍ
(Ｍは線分の本数)として表現する。データ入力の手段と
しては、キーボードや通信による直接入力や、データベ
ースなど既に定義されているデータの参照によるもの、
画像認識など他のプログラムからの入力など、様々な形
態が考えられる。ステップ２０では離散化した環境の位
置と方向が更新される。

【００１０】第ｋ番目の位置と方向を、以下、コンフィ
ギュレーションＱ_k＝(ｘ_k，ｙ_k，θ_k)(但しｋ＝１，２，・・・・，Ｋ) と書く。ここで、Ｋは環境内の離散化コンフィギュレー
ションの個数であり、離散化の解像度が細かいほど大き
くなる。ｘ_kとｙ_kはそれぞれ環境におけるｘ，ｙの絶対
座標、θ_kは環境に固定された座標軸を基準とする方向
を表わす。本ステップでは、例えば、ｋを初期値を１と
して１づつインクリメントする。ステップ２１では、処
理の対象とする光線を更新選択する。以下の説明中で
は、第ｉ番目の光線のセンサの基準方向に対する相対方
向を、θ_i(但しｉ＝１，２，・・・・，Ｎ)とする。なお、
Ｎは光線の本数を表わす。ステップ２２では、センサの
各光線がどの線分と最初に交わるかを計算する。

【００１１】なお、この処理は、点(ｘ_k，ｙ_k)を始点と
し、方向θ_k＋ｔ_iに伸びる半直線と交わるすべての線分
について、その交点が最も(ｘ_k，ｙ_k)に近い線分を選択
することによって実現できる。上述のステップ２２をす
べての光線について繰り返し、第ｋコンフィギュレーシ
ョンにおける光線と線分の対応関係を求め、更に、この
処理をすべてのｋについて繰り返すことによって、コン
フィギュレーションＱ_kと光線ｉに対する可視線分番号
ｊの関数、すなわち、ｊ＝ｆ(ｋ，ｉ)を求めることがで
きる。前処理の最終ステップ３０では、このような対応
関係がすべての光線について等しくなるようなコンフィ
ギュレーションを連結し、コンフィギュレーション空
間、すなわち、位置方向の３次元空間を領域分割する。

【００１２】後に詳述するように、このとき、各領域上
の任意の２点はＱ_kの近傍をＱＮ_k＝{ｋ’｜‖Ｑ_k’−Ｑ_k‖≦‖(Ｄｘ，Ｄｙ，Ｄｔ)
‖，ｋ’≠ｋ} (但し、Ｄｘ，Ｄｙ，Ｄｔはそれぞれｘ，ｙ，θの解像
度、‖‖はベクトルの最大値ノルム(‖(ｘ，ｙ，ｚ)‖
＝max(｜ｘ｜，｜ｙ｜，｜ｚ｜)) と定義するとき、このような近傍のみへの移動によって
到達可能なもの) すなわち、各領域が連結であるように分割する。連結領
域ラベリングは画像処理の基本的処理であり、その具体
的アルゴリズムは、例えば、文献：武田晴夫「ＭＲ符号
からの画像連結成分ラベリング」電子情報通信学会論文
誌Ｄ-II,1993年11月などに記述されている。

【００１３】次に、実際のセンサ計測データが与えられ
たときに、上述の領域分割結果を利用して、移動***置
を推定する方法について説明する。ステップ４０で処理
対象の光線を更新選択し、ステップ４１で距離データを
前述の情報処理手段に獲得する。距離データは、第ｉ番
目の光線の方向で最も近い線分までの距離ｒ_iで表現す
る。この値には誤差が含まれている。以下、この距離
データの組と上記線分データとのマッチングの処理を、
上で予め求めた領域単位に行う。具体的には、まず、ス
テップ５０で領域の更新を行う。これは、例えば、上記
領域ラベリング結果のラベルの順序に、選択する領域の
ラベル番号を順次更新すればよい。

【００１４】次に、ステップ５１では、後に詳述するよ
うに、所定の初期コンフィギュレーションから、所定の
手続きによって位置決めの候補となるコンフィギュレー
ションが順次更新される。ステップ５２では、更新され
たコンフィギュレーションにおいて上記ステップ２２と
同様にして各光線と可視線分の交点を求め、これとステ
ップ４０で獲得した観測データとの誤差を求める。誤差
の定義についても、後に詳述する。このように、ステッ
プ５１，５２をその領域内で誤差が最小値になるまで繰
り返し、領域内で観測データと環境が最もマッチする位
置を求める。この処理を、ラベルを付された全領域につ
いて行い、ステップ６０ですべての領域内最小値のうち
最も小さい値を選択し、これに対応する位置データを全
域的最小値としてこのセンサ位置すなわち移動体の位置
と推定する。以上の４０から６０のステップは、必要に
応じて何回でも繰り返すことができる。

【００１５】以上の処理を、更に、詳しく説明する。図
４に、以下の説明で必要となる記号の定義を示す。図に
おいて、Ｏは環境マップの原点、(ｘ，ｙ)はセンサ位置
を表わす。また、Ｒ₁からＲ₄はセンサの光線であり、
(ｘ，ｙ)を中心に反時計回りのインデクス１−４が付さ
れている。光線本数Ｎは、ここでは４である。まず、最
小インデクスを持つ光線Ｒ₁の方向をセンサの基準方向
θと定める。θは環境に定義された絶対座標系ｘ，ｙ
に関する絶対方向とする。基準光線Ｒ₁に対する第ｉ光
線Ｒ_iの相対方向をｔ_iとする。従って、第ｉ光線の絶対
方向はθ＋ｔ_iとなる。また、環境に存在する第ｊ線分
は、以下の説明の都合上特に線分を問題にしない場合
には、以下、直線(ρ_j，α_j)として扱う。ここに、ρ_j
は原点Ｏから第ｊ線分に下した垂線の長さ、α_jはそ
の垂線の環境に対する方向を表わす。

【００１６】初等数学で周知のように、線分端点(Ｅｘ
_1j，Ｅｙ_1j)と(Ｅｘ_2j，Ｅｙ_2j)の間には、 (Ｅｙ_2j−Ｅｙ_1j) sinα_j＋(Ｅｘ_2j−Ｅｘ_1j) cosα_j＝
０および ρ_j｛(Ｅｙ_2j−Ｅｙ_1j) cosα_j−(Ｅｘ_2j−Ｅｘ_1j) sin
α_j｝＝Ｅｘ_1jＥｙ_2j−Ｅｘ_2jＥｙ_1j なる関係が存在する。次に、ステップ３０で行われる位
置(ｘ，ｙ)および方向θの３次元空間の分割について
説明する。図５は、分割された３次元空間を、ある平面
θ＝ｃ(ｃは定数)で切断した切り口を表わす。本例で
は、環境は太線で示されるＥ₁からＥ₅の５本の線分から
構成される。環境センサは破線で示されるＲ₁からＲ₄ま
での４本の光線から成る。特に本図は、θ＝０すなわ
ちセンサの基準方向である光線Ｒ₁の方向が、ｘ軸に一
致する空間の断面を示している。

【００１７】例えば、このような姿勢にあるセンサが、
図の円で示される位置に存在する場合には、光線Ｒ₄〜
Ｒ₁からは、それぞれ、線分Ｅ₃、Ｅ₃、Ｅ₅、Ｅ₂が可視
状態となる。この状態を４次元ベクトル(３，３，５，
２)と書くことにする。状態ベクトルが同一となる領域
で、本空間を分割すると、その領域境界は、図中に細い
実線で示されるものとなる。各領域に付された４次元ベ
クトルは、その領域中における上記姿勢のセンサの光線
と線分との対応を示す。例えば、上部の領域(３，３，
３，２)に上記姿勢のセンサが置かれれば、領域内の位
置に係らず、光線Ｒ₄からＲ₁からは、それぞれ線分
Ｅ₃，Ｅ₃，Ｅ₃，Ｅ₂が可視状態となる。このような分割
は、各線分の端点から各光線方向を反転した方向に半直
線を開始し、これがいずれかの線分にあたるまで伸ばす
ことによって求めることができる。

【００１８】このような分割は、実際には上述のよう
に、離散化したコンフィギュレーション空間での上記Ｎ
次元状態ベクトルの連結領域をラベリングすることによ
って求める。更に、ステップ３０では各領域の代表点と
して、同じラベルを持ち、かつ連結した領域の重心を求
める。ここで、ｘ，ｙについては平均値、θについては
単位方向ベクトルの総和の方向を重心の座標とする。図
６に、このようにして計算した各領域の重心を示す。こ
こに示す例では、四角形環境の中で始終点座標を乱数に
よって発生し、総計１０本の線分から成る環境を作成
し、その中で領域分割を行った結果を示す。図中の各短
ベクトルが重心の位置方向を示す。位置または方向の小
さな動きに対して可視線分の組合せが変化する位置では
ベクトルが密、すなわち、領域が細かく分割される。こ
れらのベクトルは、領域内最小値探索においてステップ
５１の位置方向の初期値として利用される。

【００１９】次に、ステップ５２で行われるマッチング
誤差の計算方法について説明する。コンフィギュレーシ
ョン(ｘ，ｙ，θ)にあるセンサの第ｉ光線が、当該領域
で見る線分の番号をｊ_i、センサ誤差がない場合の第ｉ
光線のセンサ計測距離をｒ_0iとすると、ｒ_0i＝｜{ρ_jｘ cosα_j−ｙ sinα_j}／cos(θ＋ｔ_i−α_j)}｜・・・・(１) となる。これは、一般に２本の直線ｘ cosα＋ｙ sinα＝ρ およびｙ−ｂ＝(ｘ−ａ) tanθ の交点がｘ＝{ρ−ｂ sinα＋ａ sinα tanθ}／{cosα＋sinα tanθ｝ｙ＝{ρ−ｂ sinα−ａ cosα}tan θ／{cosα＋sinα tanθ}＋ｂであるから、点（ａ，ｂ）と上記交点との距離は｜{ρ−ａ cosα−ｂ sinα}／cos(θ−α)｜となることからただちに導くことができる。

【００２０】ここで、第ｉ光線のセンサ計測距離をｒ_i
とするとき、計測距離と上記真の距離の誤差をと定義する。これは計測距離と真の距離の差を真の距離
で正規化したものの２乗和であり、多くのセンサの特性
に合致したものである。上式に(１)を代入することによ
って、を得る。ステップ５０で領域を与えられることによって
ｊ_iが一意に決まり、環境マップからα_jとρ_jが決定す
る。

【００２１】ステップ５１で、位置(ｘ，ｙ)および方向
θが与えられる。従って、ステップ５２では、上記ｐの
値を計算することができる。図７に、誤差の値の分布を
示す。図において、４本の放射線が、真のセンサのコン
フィギュレーションを示す。また、破線はコンフィギュ
レーション空間における領域の境界を示す。濃いグレー
で示される部分が誤差が小さい部分、淡いグレーで示さ
れる部分が誤差が大きい部分を示す。上記ｐの領域内の
最小値は、ステップ５１で位置，方向を、例えば次のよ
うに更新することによって探索する。ステップ５０で領
域が与えられると、ステップ３０で計算された領域重心
(図６参照)を初期値として、探索を開始する。初期値
(ｘ₀，ｙ₀，θ₀)に対してｐ(ｘ₀，ｙ₀，θ₀)の値ととも
に、その点での偏微分ｋ＝∂ｐ／∂ｘ｜ｘ＝ｘ₀ およびｈ＝∂ｐ／∂ｙ｜ｙ＝ｙ₀ の値を計算する。

【００２２】次に、θ＝θ₀において、２次元ベクトル
(ｈ，ｋ)方向にその領域の境界Ｑ_a0および、Ｑ_b0を求
める。これは、図８に示すように、その領域において、
各光線が、それぞれの可視線分の端点を通る位置に平行
移動したときの、(ｘ，ｙ)の値(図では線分Ｅｊと光線
Ｒｉの組(Ｅ_j，Ｒ_i)で示される区間で表示)のうち、最
も内側の位置(上記各区間の共通部分でハッチングを施
した区間)を求めることによって得ることができる。こ
こで求めたＱ_aとＱ_bの区間で、(ｘ₀，ｙ₀)を初期値とす
るベクトル(ｈ，ｋ)方向の１次元の最小値探索を行う。
１次元の関数の最小値探索のアルゴリズムとしては、例
えば、広く知られているＢrentの方法(Ｗ.Ｐress, Ｂ.
Ｆlannery, Ｓ.Ｔeukolsky, Ｗ.Ｖetterling,“Ｎumeri
cal Ｒecipes in Ｃ” Ｃambridge Ｕniversity Ｐres
s, pp.299-302,1988)を利用することができる。

【００２３】次に、ここで求めた最小点(ｘ₁，ｙ₁)に対
して、θ方向の最小値探索を行う。まず、点(ｘ₁，ｙ₁)
において、θ方向の、その領域の境界ＱｃおよびＱｄを
求める。これは、図９に示すように、その領域において
各光線がそれぞれの可視線分の端点を通る位置に回転移
動したときの θの値(図では線分Ｅ_jと光線Ｒ_iの組
(Ｅ_j，Ｒ_i)で示される円周上の区間で表示)のうち、も
っとも内側の位置(上記各区間の共通部分でハッチング
を施した区間)を求めることによって得ることができ
る。ここで求めたＱｃとＱｄの区間で、θ₁＝θ₀を初期
値とするθ方向の１次元の最小値探索を行う。本１次元
探索にも、上と同じアルゴリズムを用いることができ
る。

【００２４】以下ステップ５１で、(ｘ，ｙ)方向の最小
値探索とθ方向の最小値探索を交互にくり返しながら、
領域内の最小点を求める。ステップ５３では、上述の、
引き続く２回の１次元の最小値探索において、いずれの
探索においても初期値から最小値までの移動距離がある
しきい値よりも小さい場合に、最小値に到達したと判定
する。図１０に、実際のパターンマッチングの過程の例
を示す。利用した環境は、図６に示すものと全く同じも
のである。(ａ)は観測距離に誤差が全く含まれない場合
の１例である。Ｑ₀が領域重心(初期値)、Ｑ₁が最初の
探索処理結果、Ｑ_Gがマッチング最終結果であり、各１
次元探索結果の(ｘ，ｙ，θ)に観測結果をマッピングし
た４本の光線の状態を、マッチングの途中経過として示
している。

【００２５】ＱＧの最終結果における各観測点がそれぞ
れの線分に完全に一致していることが示されている。
(ｂ)は誤差をもつ観測データに対するマッチング結果の
１例を示す。誤差をもつデータは、観測距離に真の長
さの０.１倍の標準偏差をもつ正規乱数を加えることに
よって生成した。本マッチング結果は、上記(２)の基準
を最小にする最適なものと言える。また、(ｃ)は複数の
結果が存在するケースを示しており、２つの最小点に対
して２つの領域で行われた探索結果が異なる点の同じ最
小値に到達することを示している。(ｄ)は４本の光線が
それぞれ異なる４つの線分に対応するケースを示してい
る。

【００２６】上記実施例によれば、光線と線分の対応が
同一であるようなコンフィギュレーションのクラス単位
にマッチングの誤差が最小となるような点を求め、これ
を可能性のあるすべての光線と線分の対応について行っ
て、全域的な最小値を求めるので、各光線の計測誤差の
同時確率分布を最小とする移動***置の最尤推定が可能
となり、精度の高い移動***置推定が実現できる効果が
ある。なお、上記実施例は本発明の一例を示したもので
あり、本発明はこれに限定されるべきものではないこと
は言うまでもないことである。例えば、上述の実施例で
は、コンフィギュレーション空間の分割データを環境マ
ップから自動的に生成したが、これは、環境マップから
手計算によってキーボードから与えても、以下の位置推
定の処理を全く同様に行うことができる。

【００２７】また、以上の処理を行う情報処理手段は、
必ずしも移動体上に物理的に装着されている必要はな
く、例えば、無線モデムによって移動体自身とは論理的
に接続された遠隔のコンピュータにあってもよい。更
に、上記実施例においては２次元平面の環境マップと、
３次元の移動体コンフィギュレーションを前提とした
が、これを、３次元空間の環境マップと４次元以上のコ
ンフィギュレーションを前提とする移動体に適用するこ
とも、同様にして可能である。なお、本実施例では、環
境の障害物は線分に限ったが、これを任意の形状の障害
物に拡張することができる。また、以上の実施例では３
次元コンフィギュレーション空間の分割を、離散化して
求めた空間の点のラベルを連結成分ラベリング手法によ
って統合する手続きによって行ったが、これを逆に各線
分の端点から逆光線方向に分割線を発することによっ
て、解析的に求めることもできる。

【００２８】また、連結領域であっても、図１１に示す
ように領域内極小点を複数生じる場合が存在するので、
領域形状の判定結果に基づいて領域を細分割し、特殊な
状況を解消することができる。また、分割された領域の
代表点、すなわち、前述のステップ５０における初期値
は、必ずしも各領域の重心である必要はなく、領域内の
任意の点に設定しても、同様のマッチング結果を得るこ
とができる。従って、領域の代表点は、光線と線分のす
べての対応関係について、そのような各状態を実現可能
なコンフィギュレーションの探索を行うことによって、
より完全な領域分割を行うことができる。また、本実施
例では、マッチングの誤差を式(１)に従って定義した
が、これを通常の最小２乗法で用いられる誤差、すなわ
ち、線分からデータ点までの垂直距離の２乗和で定義し
ても、以下、全く同様にして、この基準での最適なマッ
チング結果を得ることができる。

【００２９】また、本実施例では、最小値の探索におい
て、各１次元探索で境界を求めてチェックを行ったが、
探索の終了条件などの設定方法によっては、あらかじめ
境界を求めておくことによって処理を効率化できる場合
もある。更に、本実施例では、ステップ５０ではすべて
の領域について探索を行うように領域を更新したが、実
際の移動体のナビゲーションでは過去の位置姿勢の履歴
や、制御コマンド情報を利用して、デッドレコニング手
法や、カルマンフィルタ手法などにより、環境センサ情
報以前に、ある程度の位置方向の情報が得られる場合が
多い。このような場合には、得られた位置方向情報の特
定の周囲のみの領域を探索することによって、計算時間
を大幅に短縮することが可能である。

【００３０】

【発明の効果】以上、詳細に説明した如く、本発明によ
れば、移動体の推定位置の最尤推定結果を得ることを可
能とする、移動体における位置推定方法を実現できると
いう顕著な効果を奏するものである。

【図面の簡単な説明】

【図１】本発明に係る移動***置推定環境の一例を示す
図である。

【図２】実施例に係る移動体の具体的構成例を示す図で
ある。

【図３】実施例に係る移動体の位置推定方法の全体フロ
ーチャートである。

【図４】記号の定義を示す図である。

【図５】３次元領域分割の１断面を示す図である。

【図６】領域重心の例を示す図である。

【図７】誤差関数の空間分布を示す図である。

【図８】ｘｙ平面における領域境界探索を示す図であ
る。

【図９】ｔ方向への領域境界探索を示す図である。

【図１０】パターンマッチング実例を示す図である。

【図１１】領域内極小点が複数存在する例を示す図であ
る。

【符号の説明】

１移動体２環境のセンサ３〜７環境内に存在する物体(障害物) ８目標１０環境マップの入力手段２２光線と線分の対応計算手段３０３次元コンフィギュレーション空間分割手段４１距離データの獲得手段５２マッチング誤差の計算手段６０最小値選択手段

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁶ 識別記号庁内整理番号ＦＩ技術表示箇所Ｇ０５Ｄ 1/02 ＫＪＧ０６Ｔ 7/60

Claims

【特許請求の範囲】

【請求項１】予め与えられた複数の障害物から成る環
境マップと、光線を用いる環境センサによる計測データ
とを基に自らの位置を推定する、移動体における位置推
定方法であって、前記環境マップと、前記環境センサに
より環境内の複数の方向にそれぞれ最も近い障害物まで
の距離を計測したデータと、同じ領域内では前記光線と
障害物との対応関係が等しくなるように、前記移動体の
コンフィギュレーション空間を分割した領域データが与
えられたとき、各領域単位に、前記計測データと環境マ
ップとのマッチング誤差が最小となる領域内誤差最小コ
ンフィギュレーションを計算し、これら複数の最小コン
フィギュレーションの誤差の値のうち最も小さい値に対
応する全域的誤差最小コンフィギュレーションを求める
ことを特徴とする移動体における位置推定方法。
【請求項２】前記環境マップは２次元平面で定義さ
れ、該環境マップにおいて各障害物は前記２次元平面上
の線分で表現されており、また、前記コンフィギュレー
ション空間は前記２次元平面上の位置と方向との３次元
で定義されており、前記領域データは各領域の代表コン
フィギュレーションの集合であり、前記領域内誤差最小
コンフィギュレーションは初期値をその領域の代表コン
フィギュレーションとして、領域境界を超えないようチ
ェックしながらマッチング誤差を減少する方向へのコン
フィギュレーションの更新を繰返すことを特徴とする請
求項１記載の移動体における位置推定方法。
【請求項３】同じ領域内では、前記領域データは前記
環境マップから計算することを特徴とする請求項１また
は２記載の移動体の位置推定方法。