JP5304534B2

JP5304534B2 - 自己位置推定装置、自己位置推定方法及びプログラム

Info

Publication number: JP5304534B2
Application number: JP2009191758A
Authority: JP
Inventors: 崇小倉; 英典藪下; 佳昭朝原
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2009-08-21
Filing date: 2009-08-21
Publication date: 2013-10-02
Anticipated expiration: 2029-08-21
Also published as: JP2011043405A

Description

本発明は、物体との距離情報を検出する距離センサを備え、距離センサにより検出された距離情報に基づいて、自己位置を推定する自己位置推定装置、自己位置推定方法及びプログラムに関するものである。

センサからロボット等の観測対象の情報を取得して、観測対象の自己位置等を認識するシステムが知られている（例えば、特許文献１参照）。また、レーザレンジファインダなどの距離センサにより検出された距離情報と、予め記憶した地図情報とのマッチングにより自己位置を推定する自己位置推定装置が知られている。例えば、当該自己位置推定装置は、地図情報上にない物体が存在する場合、距離情報と地図情報とのマッチ度（信頼度）を利用して、自己位置が正常に推定されているか否かを判定することができる。また、自己位置推定装置は、距離センサの全検出領域のマッチ度を求め、このマッチ度に基づいて、自己位置が正常に推定されているか否かを判定することができる。

特開平８−２１２３２９号公報

しかしながら、距離センサの検出領域において、例えば、一部の領域に対するマッチ度が高いにもかかわらず、その他の領域に対するマッチ度が低い場合、検出領域全体に対するマッチ度は平均化され大きく低下する可能性があり、自己位置の推定が正確に行えない可能性がある。

本発明は、このような問題点を解決するためになされたものであり、自己位置の推定を高精度に行うことができる自己位置推定装置、自己位置推定方法及びプログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、検出領域内の物体との距離情報を検出する距離センサを備え、前記距離センサにより検出された前記物体との距離情報に基づいて、自己位置を推定する自己位置推定装置であって、前記距離センサの検出領域を複数の小領域に分割し、前記距離センサにより検出された前記物体の距離情報と、地図情報とに基づいて、前記小領域毎に自己位置を判定する小領域判定手段と、前記小領域判定手段により判定された前記小領域毎の自己位置の判定に基づいて、自己位置が正常であるか否かを最終的に判定する自己位置判定手段と、を備える、ことを特徴とする自己位置推定装置である。

また、この一態様において、前記小領域判定手段は、前記小領域毎に、前記距離センサにより検出された前記物体の距離情報と、地図情報とのマッチ度を算出し、該算出した小領域のマッチ度が所定値以上となるとき、該小領域に対する自己位置が正常であると判定してもよい。

さらに、この一態様において、前記自己位置判定手段は、前記小領域判定手段により判定された前記小領域毎の自己位置の判定と、前記距離センサの冗長性と、に基づいて、自己位置が正常であるか否かを判定してもよい。

さらにまた、この一態様において、前記自己位置判定手段は、（前記小領域判定手段により自己位置が正常と判定された前記小領域の数／分割された前記小領域の総数）＞（１／前記距離センサの冗長性）となるとき、自己位置が正常であると判定してもよい。

なお、この一態様において、前記自己位置判定手段により前記自己位置が異常であると判定されたとき、正常な自己位置へ復帰させるための処理を行う復帰処理手段を更に備えていてもよい。

また、この一態様において、前記復帰処理手段は、パーティクルフィルタを用いて所定の領域内にパーティクルを散布し、該散布したパーティクルのうち、前記自己位置判定手段により自己位置が正常であると判定される前記パーティクルを、復帰位置として抽出してもよい。

他方、上記目的を達成するための本発明の一態様は、距離センサの検出領域内の物体との距離情報に基づいて、自己位置を推定する自己位置推定方法であって、前記距離センサの検出領域を複数の小領域に分割し、前記距離センサにより検出された前記物体の距離情報と、地図情報とに基づいて、前記小領域毎に自己位置を判定する工程と、前記判定された前記小領域毎の自己位置の判定に基づいて、自己位置が正常であるか否かを最終的に判定する工程と、を含む、ことを特徴とする自己位置推定方法であってもよい。

また、上記目的を達成するための本発明の一態様は、距離センサの検出領域内の物体との距離情報に基づいて、自己位置を推定する自己位置推定プログラムであって、前記距離センサの検出領域を複数の小領域に分割し、前記距離センサにより検出された前記物体の距離情報と、地図情報とに基づいて、前記小領域毎に自己位置を判定する処理と、前記判定された前記小領域毎の自己位置の判定に基づいて、自己位置が正常であるか否かを最終的に判定する処理と、をコンピュータに実行させる、ことを特徴とする自己位置推定プログラムであってもよい。

本発明によれば、自己位置の推定を高精度に行うことができる自己位置推定装置、自己位置推定方法及びプログラムを提供できる。

本発明の実施形態１に係る自己位置推定装置の概略的なシステム構成を示すブロック図である。距離センサの検出領域及びその小領域の一例を示す図である。本発明の実施形態１に係る自己位置推定装置の演算装置の概略的なシステム構成を示すブロック図である。ステレオカメラの撮影画像に形成される小領域の一例を示す図である。（ａ）地図情報の一例を示す図である。（ｂ）距離センサにより検出された物体の距離情報と、地図情報とのマッチングの一例を示す図である。距離センサの検出領域の各小領域に対するマッチ度及び判定結果の一例を示す図である。小領域判定部による判定結果及びその小領域の数の一例を示す図である。本発明の実施形態１に係る自己位置推定装置による自己位置推定方法の一例を示すフローチャートである。移動体が交互に配置された障害物間をスラローム状に移動する場合の一例を示す図である。本発明の実施形態２に係る自己位置推定装置の概略的なシステム構成を示すブロック図である。復帰処理部がパーティクルを散布する領域の一例を示す図である。移動体を正常な自己位置へ復帰させるための処理の一例を示す図である。

実施形態１

以下、図面を参照して本発明の実施形態について説明する。図１は、本実施形態１に係る自己位置推定装置の概略的なシステム構成を示すブロック図である。本実施形態１に係る自己位置推定装置１０は、例えば、自律移動型又は操作型のロボットや車両等の移動体１に搭載され、その移動体１の自己位置を高精度に推定することができる。なお、ロボットには、例えば、ヒューマノイドロボット（二足歩行型ロボット）、４足歩行型ロボット、倒立二輪ロボット等の車輪型ロボット、などが含まれ、車両には、自動車、自動搬送車などが含まれる。

本実施形態１に係る自己位置推定装置１０は、少なくとも１つの距離センサ１１と、距離センサ１１等のセンサ情報に基づいて、移動体１の自己位置の推定等を行う演算装置１２と、を備えている。

距離センサ１１は、例えば、移動体１に装着され、移動体１周辺の検出領域Ｓ内にある物体と移動体１との距離情報を検出することができる（図２）。また、距離センサ１１として、例えば、レーザレンジファインダが用いられているが、これに限らず、超音波センサ、赤外線センサ、カメラ等の任意の距離センサを用いることができる。例えば、レーザレンジファインダは、移動体１前方の検出領域Ｓへ放射状にレーザ光を放射し、その検出領域Ｓ内の物体からの反射光を受光することで、その物体の距離情報を検出することができる。距離センサ１１は、検出した距離情報を演算装置１２に対して出力する。

演算装置１２は、距離センサ１１により検出された距離情報に基づいて、移動体１の自己位置の推定を行う。なお、演算装置１２は、演算処理等と行うＣＰＵ（Central Processing Unit）１２ａと、ＣＰＵ１２ａによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）１２ｂと、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）１２ｃと、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ１２ａ、ＲＯＭ１２ｂ、及びＲＡＭ１２ｃは、データバス等によって相互に接続されている。

図３は、本実施形態１に係る自己位置推定装置１０の演算装置１２の概略的なシステム構成を示すブロック図である。演算装置１２は、距離センサ１１の検出領域Ｓ内における小領域Ｓ_ｄ毎に、移動体１の自己位置が正常であるか否かを判定する小領域判定部１３と、小領域Ｓ_ｄ毎の自己位置の判定に基づいて、最終的に自己位置が正常であるか否かを判定する自己位置判定部１４と、を有している。

小領域判定部１３は、まず、距離センサ１１の検出領域Ｓを複数の小領域Ｓ_ｄに分割し、距離センサ１１により検出された各小領域Ｓ_ｄ内の物体の距離情報と、地図情報とに基づいて、小領域Ｓ_ｄ毎にそのマッチ度（信頼度）を算出する。ここで、地図情報は、例えば、路面情報、障害物情報、経路情報等が含まれており、ＲＡＭ１２ｃ等に予め記憶されている。

また、例えば、図２に示すように、距離センサ１１の検出領域Ｓが略扇状に形成され、その扇形状を円周方向へ略等分割することで、小領域Ｓ_ｄ（１）〜（５）は形成されているが、これに限らず、形成される小領域Ｓ_ｄの形状及び数は任意でよい。さらに、距離センサ１１として、例えば、ステレオカメラが用いられた場合、図４に示すように、ステレオカメラにより取得された撮影画像（検出領域Ｓ）を分割することで、小領域Ｓ_ｄ（１）〜（４）を形成してもよい。

小領域判定部１３は、距離センサ１１により検出された各小領域Ｓ_ｄ内の物体の距離情報と、ＲＡＭ１２ｃに予め記憶された地図情報とを比較し、所謂マッチング処理を行うことで、小領域Ｓ_ｄ毎に、距離センサ１１の距離情報と地図情報とのマッチ度（一致度）を算出する。小領域判定部１３は、このマッチング処理において、例えば、小領域Ｓ_ｄ全体のうち、距離センサ１１により検出された物体の距離情報と、ＲＡＭ１２ｃに予め記憶された地図情報と、の一致部分の割合（％）を算出する。例えば、ある小領域Ｓ_ｄにおいて、地図情報にない複数の障害物等が存在し、距離センサ１１により検出されたその小領域Ｓ_ｄ内の物体の距離情報（位置情報）と、地図情報との一致部分が減少すると、そのマッチ度も減少する。

さらに、小領域判定部１３は、算出した各小領域Ｓ_ｄにおける、距離センサ１１の距離情報と地図情報とのマッチ度に基づいて、各小領域Ｓ_ｄに対する自己位置が正常であるか否かを判定する。例えば、小領域判定部１３は、複数の小領域Ｓ_ｄのうち、算出したマッチ度が所定値Ｎ以上（例えば、７０％以上）になる小領域Ｓ_ｄを、その小領域Ｓ_ｄに対する自己位置が正常であると判定する。一方、小領域判定部１３は、算出したマッチ度が所定値Ｎ未満になる小領域Ｓ_ｄを、その小領域Ｓ_ｄに対する自己位置が異常であると判定する。

ここで、各小領域Ｓ_ｄに対する自己位置が正常であるとは、例えば、その自己位置が適正に認識されている状態を指し、各小領域Ｓ_ｄに対する自己位置が異常であるとは、その自己位置が適正に認識されない、所謂ＬＯＳＴ状態を指す。また、この所定値Ｎは、任意の値に変更可能であり、所定値Ｎが増加すると自己位置の推定精度が増加する。

具体的には、図５（ａ）に示すような地図情報に対して、図５（ｂ）に示すような検出領域Ｓ及びその小領域Ｓ_ｄが形成され、小領域Ｓ_ｄ（４）及び（５）内において地図情報に存在しない障害物が距離センサ１１によって検出された場合を想定する。

この場合、図６に示すように、小領域Ｓ_ｄ（１）〜（３）において、距離センサ１１により検出されたその小領域Ｓ_ｄ（１）〜（３）内の物体の位置情報と、地図情報とが略一致するため、そのマッチ度は夫々、１００％、１００％、８０％となる。従って、小領域判定部１３は、その小領域Ｓ_ｄ（１）〜（３）に対する自己位置を正常と判定する。

一方、小領域Ｓ_ｄ（４）及び（５）において、地図情報に存在しない障害物が距離センサ１１により検出されるため、距離センサ１１により検出されたその小領域Ｓ_ｄ（４）及び（５）内の物体の位置情報と、地図情報とが不一致となり、そのマッチ度は夫々０％となる。従って、小領域判定部１３は、その小領域Ｓ_ｄ（４）及び（５）に対する自己位置を異常と判定する。このように、小領域判定部１３は、まず、検出領域Ｓの各小領域Ｓ_ｄのマッチ度に基づいて、各小領域Ｓ_ｄに対する自己位置が正常であるか否かの判定を個別に行う。

ところで、距離センサ１１等のセンサ情報は、一般に、冗長な情報を含んでいるため、必ずしも、全ての小領域Ｓ_ｄに対して、正しく上記マッチングが取れている必要がない。そこで、自己位置判定部１４は、距離センサ１１の冗長性を考慮しつつ、各小領域Ｓ_ｄに対する自己位置の判定を総合的に判断して、最終的な自己位置の判定を行う。すなわち、自己位置判定部１４は、小領域判定部１３により判定された小領域Ｓ_ｄ毎の自己位置の判定と、距離センサ１１の冗長性と、に基づいて、移動体１の自己位置が正常であるか否かを判定する。これにより、移動体１の自己位置を高精度に推定することができる。

例えば、自己位置判定部１４は、（小領域判定部１３により自己位置が正常と判定された小領域Ｓ_ｄの数／分割された小領域Ｓ_ｄの総数）＞（１／距離センサ１１の冗長性）となるとき、自己位置が正常であると判定する。一方、自己位置判定部１４は、（小領域判定部１３により自己位置が異常と判定された小領域Ｓ_ｄの数／分割された小領域Ｓ_ｄの総数）＞（１／距離センサ１１の冗長性）となるとき、自己位置が異常（ＬＯＳＴ）であると判定する。

なお、自己位置判定部１４は、移動体１が一定距離を移動する期間、上記正常又は異常の判定が継続した場合に、その判定を最終的な判定結果とする。また、距離センサ１１の冗長性は、使用されるセンサ特性に応じて最適値が設定される。このように、距離センサ１１の冗長性を考慮して、さらに、上述のような各小領域Ｓ_ｄに対する判定結果の多数決を用いて、移動体１の自己位置の最終的な判定を行うことで、例えば、検出領域Ｓ内の各小領域Ｓ_ｄ間においてマッチ度の差が大きく生じても、高精度に自己位置の判定を行うことができる。

例えば、小領域判定部１３が図６に示すように各小領域Ｓ_ｄに対して自己位置の判定を行い、かつ、距離センサ１１の上記冗長性を２倍とした場合、図７に示すように、自己位置判定部１４は、（３／５）＞（１／２）により、自己位置を正常であると最終的に判定する。

なお、従来技術のように、各小領域のマッチ度を平均して上記判定を行った場合、そのマッチ度の平均値は５６％となり大きく低下するため、その自己位置は異常と誤判定される可能性がある。

次に、本実施形態１に係る自己位置推定装置１０による自己位置推定方法について、詳細に説明する。図８は、本実施形態１に係る自己位置推定装置１０による自己位置推定方法の一例を示すフローチャートである。

まず、距離センサ１１は、移動体１周辺の検出領域Ｓ内にある物体と移動体１との距離情報を検出し（ステップＳ１０１）、検出した距離情報を演算装置１２の小領域判定部１３に対して、出力する。

次に、小領域判定部１３は、距離センサ１１の検出領域Ｓを複数の小領域Ｓ_ｄに分割し（ステップＳ１０２）、距離センサ１１により検出された各小領域Ｓ_ｄ内の物体の距離情報と、ＲＡＭ１２ｃに予め記憶された地図情報とを比較し、所謂マッチング処理を行うことで、各小領域Ｓ_ｄ内の物体の距離情報と地図情報とのマッチ度を算出する（ステップＳ１０３）。

さらに、小領域判定部１３は、算出した各小領域Ｓ_ｄのマッチ度が所定値Ｎ以上となり、各小領域Ｓ_ｄに対する自己位置が正常であるか否かを判定する（ステップＳ１０４）。

その後、自己位置判定部１４は、（小領域判定部１３により自己位置が正常と判定された小領域Ｓ_ｄの数／分割された小領域Ｓ_ｄの総数）＞（１／距離センサ１１の冗長性）となり、自己位置が最終的に正常であるか否かを判定する（ステップＳ１０５）。自己位置判定部１４は、自己位置が正常であると判定したとき（ステップＳ１０５のＹＥＳ）、本処理を終了する。一方、自己位置判定部１４は、自己位置が異常であると判定したとき（ステップＳ１０５のＮＯ）、上記（ステップＳ１０１）に戻る。

以上、本実施形態１に係る自己位置推定装置１０において、小領域判定部１３が、各小領域Ｓ_ｄにおける、距離センサ１１の距離情報と地図情報とのマッチ度に基づいて、各小領域Ｓ_ｄに対する自己位置が正常であるか否かを個別に判定する。そして、自己位置判定部１４は、小領域判定部１３により判定された各小領域Ｓ_ｄに対する自己位置の判定と、距離センサ１１の冗長性と、に基づいて、自己位置が正常であるか否かを最終的に判定する。これにより、例えば、距離センサ１１の検出領域Ｓの各小領域Ｓ_ｄ間においてマッチ度の差が大きく生じた場合でも、各小領域Ｓ_ｄに対する判定結果の多数決によって最終的な自己位置の判定をより正確に行うことができる。したがって、移動体１の自己位置の判定を高精度に行うことができ、自己位置を高精度に推定することができる。

例えば、図９に示すように、移動体１が交互に配置された障害物間をスラローム状に移動する場合などの複雑な環境下においても、自己位置の信頼度を必要以上に低下させることなく、移動体１の自己位置を高精度に推定することができる。

実施形態２
図１０は、本発明の実施形態２に係る自己位置推定装置の概略的なシステム構成を示すブロック図である。本実施形態２に係る自己位置推定装置２０の演算装置２１は、上記実施形態１に係る自己位置推定装置１０の演算装置１２の構成に加えて、正常な自己位置へ復帰させるための処理を行う復帰処理部２２を、更に備えている。復帰処理部２２は、自己位置判定部１４により自己位置が異常であると判定されたとき、正常な自己位置へ復帰させるための処理を行う。

図１２は、移動体を正常な自己位置へ復帰させるための処理の一例を示す図である。復帰処理部２２は、まず、図１１に示すように、自己位置が異常であると推定された推定地点Ｘ１と、自己位置が異常であると確定された確定地点Ｘ２と、を夫々決定する（ステップＳ２０１）。ここで、推定地点Ｘ１とは、例えば、自己位置判定部１４により最後に自己位置が異常であると判定された領域において、小領域判定部１３により小領域Ｓ_ｄが最初に異常であると判定された地点であり、自己位置をロストしたと推定される地点を指す。また、確定地点Ｘ２とは、自己位置判定部１４により自己位置が異常であると判定された地点であり、自己位置をロストしたと確定される地点を指す。

次に、復帰処理部２２は、推定地点Ｘ１から確定地点Ｘ２までの移動量を半径Ｒとして描いた略半円形状の領域（所定の領域）Ｐを、形成する（ステップＳ２０２）。さらに、復帰処理部２２は、周知のパーティクルフィルタを用いて、形成した領域Ｐ内に複数のパーティクルをランダムに散布する（ステップＳ２０３）。このように、本実施形態２によれば、パーティクルを散布する領域Ｐを最小限に限定することで、従来のように検出領域全体にパーティクルを散布する場合と比較して、効率的かつ高精度に、移動体１を正常な自己位置へ復帰させることができる。

ここで、パーティクルフィルタとは、逐次型のモンテカルロ法であり、確率分布をパーティクルと呼ばれるサンプル集合で表すフィルタである。なお、復帰処理部２２は、形成した領域Ｐ内を必要な精度のグリッドで分割し、このグリッドに沿って各パーティクルを散布してもよく、任意の散布方法を適用することができる。

復帰処理部２２は、上記自己位置推定方法を用いて、最適なパーティクルを抽出する（ステップＳ２０４）。例えば、復帰処理部２２は、散布した複数のパーティクルのうち、自己位置判定部１４により自己位置が正常であると最初に判定された、パーティクルを抽出する。このとき、小領域判定部１３は、上記所定値Ｎを上記自己位置推定時のときの所定値Ｎ（例えば、７０％）よりも増加させるのが好ましい。これにより、上記判定時のチャタリングを防止することができる。なお、復帰処理部２２は、任意の方法を用いて、自己位置判定部１４により自己位置が正常であると判定される、最適なパーティクルを抽出することができる。

復帰処理部２２は、上述のように、自己位置が正常であると判定される最適なパーティクルを抽出できた場合、そのパーティクルの位置を、自己位置を正常に認識できる復帰位置として決定する（ステップＳ２０５）。一方、復帰処理部２２は、自己位置が正常であると判定されるパーティクルを抽出できない場合、パーティクル散布の領域Ｐを順次拡大し、その拡大した領域で、最適なパーティクルを抽出する。

以上、本実施形態２に係る自己位置推定装置２０において、復帰処理部２２は、自己位置判定部１４により移動体１の自己位置が異常であると判定されたとき、パーティクルの散布領域Ｐを最小限に限定し、効率的かつ高精度に、移動体１を正常な自己位置へ復帰させることができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

また、上述の実施形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、任意の処理を、ＣＰＵ（Central Processing Unit）１２ａにコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、記録媒体に記録して提供することも可能であり、また、インターネットその他の通信媒体を介して伝送することにより提供することも可能である。また、記憶媒体には、例えば、フレキシブルディスク、ハードディスク、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ、ＲＯＭカートリッジ、バッテリバックアップ付きＲＡＭメモリカートリッジ、フラッシュメモリカートリッジ、不揮発性ＲＡＭカートリッジ等が含まれる。また、通信媒体には、電話回線等の有線通信媒体、マイクロ波回線等の無線通信媒体等が含まれる。

１移動体
１０、２０自己位置推定装置
１１距離センサ
１２、２１演算装置
１３小領域判定部
１４自己位置判定部
２２復帰処理部

Claims

検出領域内の物体との距離情報を検出する距離センサを備える自己位置推定装置であって、
前記距離センサの検出領域を複数の小領域に分割し、前記小領域毎に、前記距離センサにより検出された前記物体の距離情報と、地図情報とのマッチ度を算出し、該算出した小領域のマッチ度に基づいて前記小領域毎に自己位置が適正に認識される状態である正常であるか否かを判定する小領域判定手段と、
前記小領域判定手段により判定された前記小領域毎の自己位置の判定結果の多数決と、前記距離センサのセンサ情報に含まれる冗長な情報を示し前記距離センサのセンサ特性に応じて設定される値である前記距離センサの冗長性と、に基づいて、自己位置が適正に認識される状態である正常であるか否かを最終的に判定する自己位置判定手段と、
を備える、ことを特徴とする自己位置推定装置。
請求項１記載の自己位置推定装置であって、
前記小領域判定手段は、前記小領域毎に、前記距離センサにより検出された前記物体の距離情報と、地図情報とのマッチ度を算出し、該算出した小領域のマッチ度が所定値以上となるとき、該小領域に対する自己位置が適正に認識される状態である正常であると判定する、ことを特徴とする自己位置推定装置。
請求項１又は２記載の自己位置推定装置であって、
前記自己位置判定手段は、（前記小領域判定手段により自己位置が正常と判定された前記小領域の数／分割された前記小領域の総数）＞（１／前記距離センサの冗長性）となるとき、自己位置が正常であると判定する、ことを特徴とする自己位置推定装置。
請求項１記載の自己位置推定装置であって、
パーティクルフィルタを用いて所定の領域内にパーティクルを散布し、前記小領域判定手段により、前記小領域毎に、前記距離センサにより検出された前記物体の距離情報と、地図情報とのマッチ度が算出され、該算出された小領域のマッチ度に基づいて前記小領域毎に自己位置が正常であるか否かが前記散布したパーティクルについて夫々判定され、前記小領域判定手段により判定された前記小領域毎の自己位置の判定結果の多数決と前記距離センサの冗長性と、に基づいて、自己位置が正常であると前記自己位置判定手段により判定される前記パーティクルの位置を、自己位置を正常に認識できる復帰位置として抽出する復帰処理手段を更に備える、ことを特徴とする自己位置推定装置。
検出領域内の物体との距離情報を検出する距離センサの前記検出領域を複数の小領域に分割し、前記小領域毎に、前記距離センサにより検出された前記物体の距離情報と、地図情報とのマッチ度を算出し、該算出した小領域のマッチ度に基づいて前記小領域毎に自己位置が適正に認識される状態である正常であるか否かを判定する工程と、
前記判定された前記小領域毎の自己位置の判定結果の多数決と、前記距離センサのセンサ情報に含まれる冗長な情報を示し前記距離センサのセンサ特性に応じて設定される値である前記距離センサの冗長性と、に基づいて、自己位置が適正に認識される状態である正常であるか否かを最終的に判定する工程と、
を含む、ことを特徴とする自己位置推定方法。
検出領域内の物体との距離情報を検出する距離センサの前記検出領域を複数の小領域に分割し、前記小領域毎に、前記距離センサにより検出された前記物体の距離情報と、地図情報とのマッチ度を算出し、該算出した小領域のマッチ度に基づいて、前記小領域毎に自己位置が適正に認識される状態である正常であるか否かを判定する処理と、
前記判定された前記小領域毎の自己位置の判定結果の多数決と、前記距離センサのセンサ情報に含まれる冗長な情報を示し前記距離センサのセンサ特性に応じて設定される値である前記距離センサの冗長性と、に基づいて、自己位置が適正に認識される状態である正常であるか否かを最終的に判定する処理と、
をコンピュータに実行させる、ことを特徴とする自己位置推定プログラム。