JP2019160147A - 自己位置検出装置 - Google Patents

Abstract

【課題】検出精度を向上した自己位置検出装置を提供する。
【解決手段】自己位置検出装置の演算処理部は、予め記憶された周囲の環境情報（第１環境情報）と移動体１に設けたセンサ１４で取得された周方向角度毎の周囲の環境情報（第２環境情報）とを比較する処理を含む演算処理により、移動体１の自己位置を検出する。演算処理部は、周方向角度毎に、第２環境情報に基づき、自己位置の検出精度の高さに対応付けた指標値を算出する。演算処理部は、当該指標値の内、所定値以上の指標値が複数存在した場合又は相対的に高い指標値を複数特定できた場合は、次の処理を行う。即ち、演算処理部は、これら複数の指標値のそれぞれに対応する周方向角度の第２環境情報から算出した自己位置の中から、相対的に他から離れた自己位置を除去する。演算処理部は、除去した残りの自己位置が複数存在する場合は、これらの平均値又は中央値を前記移動体の自己位置として検出する。
【選択図】図１

Description

本発明は、自己位置検出装置に関する。

特許文献１には、環境地図に示される点群と距離センサから得られた点群をマッチングして移動体の自己位置を推定する自己位置推定方法が開示されている。この方法では、距離センサの計測方向毎に、距離センサから取得される点群について、予め定められた指標に基づく好適度を算出している。そして、この方法では、好適度が予め定められた基準を満たすような点群が取得される計測方向において距離センサから取得される点群を、環境地図に示される点群とのマッチングに用いている。

特開２０１７−１４６８９３号公報

しかしながら、特許文献１に記載の方法では、自己位置推定（検出）時の環境によっては、好適度が上記基準を満たすような計測方向の点群が、マッチングに適した点群（つまり自己位置推定精度が高い点群）とはならない場合がある。よって、移動体の自己位置の検出精度をより高めることが求められる。

そこで、本発明の目的は、移動体の自己位置を予め記憶された環境情報との比較により検出するに際し、自己位置の検出精度を向上させることが可能な自己位置検出装置を提供することにある。

上記目的を達成するための本発明の一態様は、
予め記憶された周囲の環境情報である第１環境情報を、移動体に搭載されたメモリ又は前記移動体の外部の装置から取得するデータ取得部と、
前記移動体に設けられ、周方向角度毎の周囲の環境情報である第２環境情報を取得するセンサと、
前記第１環境情報と前記第２環境情報とを比較する比較処理を含む演算処理によって、前記移動体の自己位置を検出する演算処理部と、
を備え、
前記演算処理部は、前記周方向角度毎に、前記第２環境情報に基づき、前記自己位置の検出精度の高さに対応付けた指標値を算出し、
当該指標値の内、所定値以上の指標値が複数存在した場合、又は、相対的に高い指標値を複数特定できた場合は、これら複数の指標値のそれぞれに対応する周方向角度の前記第２環境情報から算出した自己位置の中から、相対的に他の自己位置から離れている自己位置を除去し、
除去した残りの自己位置が複数存在する場合は、これらの平均値又は中央値を前記移動体の自己位置として検出する、
自己位置検出装置である。

この一態様に係る自己位置検出装置では、センサで周方向角度毎に計測した周囲の環境情報が自己位置の検出に好適なものであるか否かの指標値を検出処理に導入している。さらに、この一態様に係る自己位置検出装置では、好適な複数の指標値のそれぞれに対応する周方向角度について計測した周囲の環境情報から算出した自己位置の中から、相対的に他の自己位置から離れているものを除去して移動体の自己位置を検出している。よって、この一態様によれば、移動体の自己位置を予め記憶された環境情報との比較により検出するに際し、自己位置の検出精度を向上させることが可能な自己位置検出装置を提供することができる。

本発明によれば、移動体の自己位置を予め記憶された環境情報との比較により検出するに際し、自己位置の検出精度を向上させることが可能な自己位置検出装置を提供することができる。

本発明の一実施形態に係る自己位置検出装置を備えた移動体の一構成例を示す斜視図である。 図１の移動体における自己位置検出処理の一例を説明するためのフロー図である。 図１の移動体におけるセンサの姿勢と本体部との関係の一例を示す。 図２の自己位置検出処理において算出された、センサ姿勢に対する好適度の関係の一例を示す図である。 図４の関係に対して行った極大点抽出処理の一例を説明するための図である。 図４の関係に対して行った極大点抽出処理の他の例を説明するための図である。 図６の一部を拡大した図である。 図４の関係に対して行った閾値処理の一例を説明するための図である。 図４の関係に対して行った好適な極大点の抽出処理の一例を説明するための図である。 図９の処理により好適とされた各センサ姿勢について自己位置推定を行った結果の一例をプロットした図である。 図１０の自己位置推定結果から外れ値を除外した結果の一例をプロットした図である。

以下、本発明の一実施形態に係る自己位置検出装置について、図面を参照しながら説明する。

図１は、本実施形態に係る自己位置検出装置を備えた移動体の一構成例を示す斜視図である。図１に示すように、本実施形態に係る移動体１は、その全体を制御する制御部１０、本体部１１、センサ設置部１２、可動部１３、及びセンサ１４を備えることができる。

ここでは、移動体１は、センサ１４の検出結果に基づき自己位置を検出して自律的に移動させるように構成すること、つまり自律走行が可能な自律移動体とすることができる。この場合、センサ１４は、このような自律制御に用いることから、自律センサと称することもできる。なお、自律移動の制御は、制御部１０で行うように構成することができるが、別途設けた自律移動制御部で行うように構成することもできる。

本実施形態に係る自己位置検出装置は、主に、センサ１４及び制御部１０で構成することができる。センサ１４は、移動体１に設けられ、周方向角度毎の周囲の環境情報（例えば、点群の情報）を取得する。なお、周方向角度毎とは、センサ１４が周方向に移動可能な最小単位の角度毎とすることもできるが、最小単位の角度を正の整数倍した所定角度毎とすることもできる。例えば、センサ１４は、周囲３６０度のうち前方（正面）に向かって１８０度を４分割して４５度毎に環境情報を取得することができる。

センサ１４は、例えば、ライダー等の３Ｄ距離センサ（測距センサ）、カメラ、又は２つを組み合わせたものとすることができる。なお、ライダーは、ＬＩＤＡＲ（Light Detection and Ranging、Laser Imaging Detection and Ranging）を指す。

センサ１４は、センサ設置部１２に設置され、可動部１３での回転等の動作により、センサ設置部１２とともに少なくとも周方向に移動（回転）可能となっている。このようなセンサ１４の周方向の回転移動は、制御部１０により制御することができる。つまり、制御部１０は、センサ移動制御部を有すると言える。

制御部１０は、例えば、ＣＰＵ（Central Processing Unit）、作業用メモリ、及び不揮発性の記憶装置などによって、或いは集積回路（Integrated Circuit）によって実現される。この記憶装置にＣＰＵによって実行される制御プログラムを格納しておき、ＣＰＵがその制御プログラムを作業用メモリに読み出して実行することで、制御部１０の機能を果たすことができる。また、制御部１０は、後述するデータ取得部の一部として、無線通信アダプタ等の通信インタフェースを備えることができる。

そして、制御部１０は、次に説明するデータ取得部及び演算処理部（図示せず）を有する。データ取得部は、予め記憶された周囲の環境情報を、移動体１に搭載されたメモリ又は移動体１の外部の装置（例えば外部サーバ）から取得する。なお、このメモリは制御部１０の内部に具備することもできる。

また、上記予め記憶された周囲の環境情報は、例えば、上記予め記憶された周囲の環境情報は、移動体１が移動する可能性のある範囲について、センサ１４又は別の移動体のセンサなどを使用して作成した環境地図とすることができる。この環境地図は、測距した結果の点群の情報を含むことができる。なお、別の移動体のセンサは、センサ１４と同種のセンサであることが望ましく、同一のセンサであることがより望ましい。

以下、区別のため、データ取得部で取得される周囲の環境情報を第１環境情報と称し、自己位置を検出する際にセンサ１４で取得される周囲の環境情報を第２環境情報と称す。

演算処理部は、第１環境情報と第２環境情報とを比較する比較処理を含む演算処理によって、移動体１の自己位置を検出する。このように演算処理部は、センサ１４から得た第２環境情報と環境地図等の第１環境情報とを照合し、移動体１の自己位置を検出（推定により検出）する自己位置検出部（自己位置推定部）として機能する。

次に、演算処理部における演算処理の詳細について説明する。
演算処理部は、周方向角度毎に、第２環境情報に基づき、自己位置の検出精度の高さに対応付けた指標値（以下、好適度Ｃ）を算出する。好適度Ｃは、例えば、第２環境情報に含まれる点群の重心が移動体１から（センサ１４から）遠い程、高くすることができ、またその点群の分布が広い程、高くすることができる。なお、上述したように、センサ１４の周方向角度、つまりセンサ１４の周方向角度で表されるセンサ１４の姿勢は、制御部１０により変化させることができるため、周方向角度毎に第２環境情報を取得することができる。

演算処理部は、当該指標値の内、所定値以上の指標値が複数存在した場合、又は、相対的に高い指標値を複数特定できた場合は、これら複数の指標値のそれぞれに対応する周方向角度の第２環境情報から自己位置を算出する。なお、周方向角度毎の自己位置は、指標値の判定前に、対象となる全ての周方向角度のそれぞれについて算出しておくこともできる。自己位置の算出は、第１環境情報との比較により行うことができる。

次いで、演算処理部は、これら複数の指標値のそれぞれに対応して算出した自己位置の中から、相対的に他の自己位置から離れている自己位置を除去する。

そして、演算処理部は、除去した残りの自己位置が複数存在する場合は、これらの平均値又は中央値を移動体１の自己位置として検出する。一方で、演算処理部は、除去した残りの自己位置が１つである場合は、この自己位置を移動体１の自己位置として検出することができる。

次に、図２〜図１１を参照しながら、主に制御部１０の演算処理部における演算処理として実行される自己位置検出処理の一例について説明する。図２は、移動体１における自己位置検出処理の一例を説明するためのフロー図である。

まず、制御部１０は、センサ１４の姿勢を変化させる制御を行いながら、姿勢毎にセンサ１４から第２環境情報（以下、センサデータとも称す）を取得し、センサデータに基づき好適度Ｃを計算する（ステップＳ１）。ステップＳ１の処理例については後述する。

次いで、制御部１０は、好適度Ｃに基づき、好適なセンサ１４の姿勢（以下、センサ姿勢）を数点抽出し（ステップＳ２）、好適なセンサ姿勢の数が閾値以上であるか否かを判定する（ステップＳ３）。ステップＳ２の処理例については後述する。ステップＳ３でＮＯの場合、制御部１０は、自己位置の検出（推定）が困難である旨を、移動体１の自律移動を制御する部位又は上位の処理系に出力し（ステップＳ９）、処理を終了する。

ステップＳ３でＹＥＳの場合、制御部１０は、好適な各センサ姿勢のそれぞれに対して、そのセンサ姿勢で取得された第２環境情報を第１環境情報と照合することにより自己位置を推定する（ステップＳ４）。

次いで、制御部１０は、ステップＳ４で推定された自己位置群（自己位置推定結果群）から外れ値を除去する（ステップＳ５）。そして、制御部１０は、自己位置推定結果の分布より自己位置推定結果の分散を計算し（ステップＳ６）、算出した分散が闘値以下であるか否かを判定する（ステップＳ７）。ステップＳ５〜Ｓ７の処理例については後述する。

ステップＳ７でＮＯの場合、制御部１０は、ステップＳ９の処理後、処理を終了する。一方で、ステップＳ７でＹＥＳの場合、制御部１０は、残った自己位置推定結果の平均を計算し（ステップＳ８）、処理を終了する。ステップＳ８で計算される値が制御部１０による自己位置検出結果となり、自律移動を制御する部位に出力される。なお、ステップＳ８では、残った自己位置推定結果が１つの場合、その結果が示す値が出力される。また、ステップＳ８の処理例についても後述する。

ステップＳ１の処理例について、図３及び図４を併せて参照しながら説明する。
制御部１０では、センサ移動制御部がセンサ１４を動かす制御を行いながら、センサ１４に計測を実行させ、センサ１４から第２環境情報を取得し、姿勢毎に好適度Ｃを計算する。これにより、制御部１０は、センサ１４の姿勢と好適度Ｃの組み合わせを得ることができる。

好適度Ｃは、使用するセンサ１４と好適とする指標により任意に設定することができる。以下、センサ１４として３Ｄセンサを使用した場合の好適度Ｃの例と、センサ１４としてカメラを使用した場合の好適度Ｃの例について挙げる。

センサ１４として３Ｄセンサを使用した場合、環境地図の３次元点群とセンサ１４で得られた３次元点群をマッチングすることで、姿勢毎の自己位置推定を行うことができる。その際、センサ１４の点群中に動的物体を含む割合が小さいであろうことを好適とする（好適度Ｃが高いとする）ことができる。好適度Ｃは、例えば、センサ１４の点群の重心がセンサ１４より遠くにあり且つ点群が広く分布していると、高い傾向となるように設定することができる。このような設定の例については、特許文献１に記載されている。

センサ１４としてカメラを使用した場合、環境地図を予め撮影した画像列とし、カメラで得られた画像と最も近い画像とマッチングすることで、姿勢毎の自己位置推定を行うことができる。この場合、画像が他と弁別し易く特徴的であることを好適とする（好適度Ｃが高いとする）ことができる。例えば、画像から得られる特徴点の数を好適度Ｃとして設定することができる。なお、上記特徴点は、画像中のコーナ等の特徴的な箇所を指す。上記特徴点としては、その特徴を記述する特徴量の例としてＳＩＦＴ（Scale-Invariant Feature Transform）、ＳＵＲＦ（Speeded Up Robust Features）等が挙げられる。

ステップＳ１で得られる好適度Ｃとセンサ姿勢の関係の例について、図３及び図４を参照しながら説明する。ここでは、センサ１４を鉛直軸中心に回転させたセンサ姿勢θと好適度Ｃとの関係を見ることとする。図３は、センサ１４の姿勢（周方向角度）θと本体部１１との関係の一例を示す図である。図３において、Ｆは移動体１の正面の方向を示している。図４は、図２の自己位置検出処理中であるステップＳ１で算出された、センサ姿勢θに対する好適度Ｃの関係の一例を示す図である。

図４において、θとＣの関係をプロットしたように、制御部１０は、好適度Ｃを各センサ姿勢θについて得ることができる。この例では、概略的には、移動体１の本体部１１の正面（θ＝０［ｄｅｇ］）を中心に高く且つほぼ一定の周期性をもつような好適度Ｃが得られている。

ステップＳ２の処理例について、図５を併せて参照しながら説明する。図５は、図４の関係に対して行った極大点抽出処理の一例を説明するための図である。制御部１０は、ステップＳ１で得られたセンサ姿勢θと好適度Ｃの関係のプロットより、図５で例示するような極大点となる部分のθを抽出し、好適なセンサ姿勢の候補とする。

実際には好適度Ｃのプロットは離散的なものであり、計測誤差により細かいピークが多数立つことがある。そのような場合に対応可能な極大点抽出処理（離散的な好適度プロットからの極大点抽出処理）の例について、図６及び図７を参照しながら説明する。図６は、図４の関係に対して行った極大点抽出処理の他の例を説明するための図で、図７は、図６の符号６０で示す部分を拡大した図である。

まず、制御部１０は、図６の縦破線で示すようにセンサ姿勢θの一定区間でプロットを区切り、各区間でＮ次関数によるフィッティングを行う。Ｎは正の整数である。制御部１０は、予め定めたＮでフィッティングを行うことができる。或いは、制御部例えばＮを１からインクリメントしながらフィッティングを行うことができる。図７では、図６における符号６０付近の区間のプロットに対して３次関数でフィッティングを行った結果を示している。その後、制御部１０は、フィッティングを行ったＮ次関数の極大値となるθ（図７の黒丸で示すプロットに対応するθ）を、好適なセンサ姿勢の候補とする。

ステップＳ２では、図５〜図７で例示したような極大点抽出処理の後、閾値処理を行うことができる。この閾値処理について図８を参照しながら説明する。図８は、図４の関係に対して行った閾値処理の一例を説明するための図である。

図８に示すように、制御部１０は、抽出された極大点群の中から、好適度Ｃが高いものを自己位置推定に使用するために、好適度Ｃが闘値Ｃｔｈ以上となるもののみを抽出する。図８の閾値処理の例では、結果的に極大点群のうちセンサ１４が正面に近い４点のセンサ姿勢の候補が抽出されている。

また、ステップＳ２では、抽出されたセンサ姿勢の候補の中から、他の姿勢と近いものを除外する処理を行うことができる。この処理について図９を参照しながら説明する。図９は、図４の関係に対して行った好適な極大点の抽出処理の一例を説明するための図である。

図９に示すように、制御部１０は、近いセンサ姿勢の重複を防ぐため、好適度Ｃが高いとして抽出された極大点ｉに相当するセンサ姿勢θ_ｉ同士の距離を判定し、互いの距離がθ_ｔｈ以下であれば、好適度Ｃの高い方を残し低い方を削除することができる。

図９の例では、制御部１０は、｜θ_１−θ_２｜＜θ_ｔｈ、｜θ_２−θ_３｜≧θ_ｔｈ、｜θ_３−θ_４｜≧θ_ｔｈといった距離判定処理（閾値処理）を行う。次いで、制御部１０は、閾値θ_ｔｈを下回るθ_１，θ_２の組み合わせのうち、Ｃ（θ_１）＞Ｃ（θ_２）であるため、θ_１を残し、θ_２を削除する。このような処理の結果、制御部１０は、図９において黒丸で示す点群を、自己位置推定に好適な極大点群、つまり自己位置推定に好適なセンサ姿勢群とすることができる。このように、ステップＳ２では、以上の過程で残ったセンサ姿勢を自己位置推定に好適なセンサ姿勢群とすることができる。

次に、ステップＳ５〜Ｓ７の処理例について図１０及び図１１を併せて参照しながら説明する。図１０は、図９の処理により好適とされた各センサ姿勢についてステップＳ４の自己位置推定処理を行った結果の一例をプロットした図で、図１１は、図１０の自己位置推定結果から外れ値を除外した結果の一例をプロットした図である。

ステップＳ２で好適とされた各センサ姿勢でそれぞれステップＳ４の自己位置推定処理を行った結果をプロットすると、図１０に示すようになったとして説明する。図１０において、予め作成された環境地図の原点を基準にして、矢印の根元が推定された自己位置（移動体１の位置）を示し、矢印の向きが移動体１の向きを示すものとする。

制御部１０は、ステップＳ４において、センサ１４がセンサ姿勢θ_ｉで計測したデータ（第２環境情報）から自己位置を推定することになる。このようにセンサ姿勢θ_ｉでの計測データから推定された自己位置を、Ｐ_ｉ＝（ｘ_ｉ，ｙ_ｉ，Φ_ｉ）とする。なお、Ｐ_ｉは成分をもつことから明らかなようにベクトルとする。

制御部１０は、ステップＳ５において、各Ｐ_ｉが外れ値であるか否かの外れ値検出を行い、検出された点を除外（除去）する。このようにして除外される自己位置推定結果の例は、図１０におけるＰａ，Ｐｂで示している。Ｐａは、位置が他と比べて離れているため除外される対象となり、Ｐｂは角度が他と比べて大きく異なるため除外される対象となる。Ｐａ，Ｐｂを除外した結果、図１１に示すような自己位置推定結果が残ることになる。このように、制御部１０は、自己位置が相対的に他と離れている情報だけでなく、移動体１の向きが相対的に他と異なる情報も除外することが望ましい。

制御部１０は、ステップＳ６において、ステップＳ５の外れ値除去処理の後に残った自己位置推定結果群を元に、推定結果の位置、姿勢の分散をそれぞれ計算する。制御部１０は、ステップＳ７において、計算した各分散のいずれかが、設定した闘値以上となった場合は、有効な推定結果が得られず自己位置推定が困難であるとする。つまり、推定位置の各次元に対する分散をＶで表し、その閾値を添え字ＴＨ付きで表すと、次の場合に自己位置推定を困難とみなすことができる。

Ｖ（ｘ）＞Ｖ_ＸＴＨ
又は、Ｖ（ｙ）＞Ｖ_ＹＴＨ
又は、Ｖ（Φ）＞Ｖ_ΦＴＨ

なお、制御部１０は、自己位置推定が困難とした場合、ステップＳ９において、その旨を例えば自律移動を制御する部位又は上位の処理系に伝達することになる。

次に、ステップＳ８の処理例について説明する。
制御部１０は、ステップＳ６の処理の後に残された自己位置推定結果群の平均を取ったものを最終的な自己位置推定結果とする。例えば、制御部１０は、各自己位置推定結果Ｐ_ｉを計算した際の尤度をＬ_ｉとすると、最終的な自己位置推定結果Ｐ_ｏｕｔは尤度の重み付け平均として求め、下式のように算出することができる。

Ｐ_ｏｕｔ＝（Ｌ_１Ｐ_１＋Ｌ_２Ｐ_２＋・・・＋Ｌ_ＮＰ_Ｎ）／（Ｌ_１＋Ｌ_２＋・・・＋Ｌ_Ｎ）
なお、自己位置推定における尤度が求められない場合は、センサ姿勢に対応する好適度Ｃ_ｉを尤度として重み付け平均を求めてもよい。

以上のように、本実施形態に係る自己位置検出装置では、センサ１４で周方向角度毎に計測した周囲の環境情報が自己位置の検出（自己位置推定）に好適なものであるか否かの指標値（好適度Ｃ）を検出処理に導入している。つまり、この検出処理では、センサ姿勢、センサデータ、及び好適度Ｃの組み合わせを収集して使用している。

さらに、本実施形態に係る自己位置検出装置では、好適な複数の指標値のそれぞれに対応する周方向角度について計測した周囲の環境情報から算出した自己位置の中から、相対的に他の自己位置から離れているものを除去して移動体の自己位置を検出している。つまり、この検出処理では、予め候補とする周方向角度を複数残しておき、これらを統計処理している。よって、このような検出処理では、環境から受ける影響を低減することができる。

具体的に説明すると、このような検出処理を行わない場合、検出環境（計測環境）によっては好適度Ｃが最大値を取る周方向角度に対応する自己位置が、最も精度の良い自己位置にならない場合がある。例えば、好適度Ｃが最大となる視点で計測した距離センサデータに動的物体が混ざっている場合には、適切に自己位置を推定することができない恐れがある。好適度Ｃはあくまで環境の変動や動的物体が混ざっている割合が小さいと見込まれる指標であるためである。また、好適度Ｃが最大となる視点で計測した距離センサデータと合致する環境地図上の形状が複数存在する場合にも、適切に自己位置を推定できない恐れがある。

しかし、自己位置推定の計算を好適度Ｃが最大となる単一のセンサ姿勢についてのみではなく、好適度が高めの他のセンサ姿勢でのセンサデータも統合してから行う方が、動的物体の混入等による自己位置推定誤差を小さくし高い精度とすることができる。

よって、本実施形態のような検出処理を行うことで、例えば、好適度Ｃが最大値を取る周方向角度に対応する自己位置が環境の影響を受けた外れ値であったとしても、それを除去したデータの中から最も精度の良い自己位置を抽出できる。よって、本実施形態のような検出処理では、環境の影響を低減した精度の良い（信頼性の高い）自己位置検出が可能となる。さらに、本実施形態のように好適なデータが得られるセンサ姿勢を選定すること（外れ値を除去すること）により、除去しない場合に比べて処理時間を短縮することができるといった効果も奏する。

（代替例）
次に、本実施形態における代替例について説明する。
移動体１が自律移動体であることを前提にして説明したが、自律移動ができない移動体であっても同様に本実施形態に係る自己位置検出装置を搭載することができる。また、図１では、２輪の移動体１を図示しているが、上記自己位置検出装置を搭載する移動体の車輪の数はこれに限ったものではなく、また、車輪を備えない駆動機構を採用した移動体であってもよい。

また、指標値（好適度Ｃ）は、第２環境情報だけでなく第１環境情報も用いて算出することもできる。例えば、好適度Ｃは、周方向角度毎の第２環境情報を第１環境情報と比較（照合）して、その照合度合い（例えば、或る周方向角度での第２環境情報に含まれる点群のうち第１環境情報に含まれる点群と一致する割合など）に応じて算出することもできる。

また、除去した残りの自己位置が複数存在する場合について、これらの平均値又は中央値を移動体の自己位置として検出したが、他の統計値を採用することもできる。

以上に、本実施形態について説明したが、上記実施形態は、以下の特徴を有する。
即ち、上記実施形態に係る自己位置検出装置は、データ取得部、センサ１４、及び演算処理部を備える。データ取得部は、予め記憶された周囲の環境情報である第１環境情報を、移動体１に搭載されたメモリ又は移動体１の外部の装置から取得する。センサ１４は、移動体１に設けられ、周方向角度毎の周囲の環境情報である第２環境情報を取得する。演算処理部は、第１環境情報と第２環境情報とを比較する比較処理を含む演算処理によって、移動体１の自己位置を検出する。演算処理部は、周方向角度毎に、第２環境情報に基づき、自己位置の検出精度の高さに対応付けた指標値を算出する。演算処理部は、当該指標値の内、所定値以上の指標値が複数存在した場合、又は、相対的に高い指標値を複数特定できた場合は、次の処理を行う。即ち、演算処理部は、これら複数の指標値のそれぞれに対応する周方向角度の第２環境情報から算出した自己位置の中から、相対的に他の自己位置から離れている自己位置を除去する。演算処理部は、除去した残りの自己位置が複数存在する場合は、これらの平均値又は中央値を前記移動体の自己位置として検出する。

以上の構成の自己位置検出装置では、センサ１４で周方向角度毎に計測した周囲の環境情報が自己位置の検出に好適なものであるか否かの指標値を検出処理に導入している。さらに、この自己位置検出装置では、好適な複数の指標値のそれぞれに対応する周方向角度について計測した周囲の環境情報から算出した自己位置の中から、相対的に他の自己位置から離れているものを除去して移動体の自己位置を検出している。よって、この自己位置検出装置によれば、移動体の自己位置を予め記憶された環境情報との比較により検出するに際し、自己位置の検出精度を向上させることが可能になる。

１ 移動体
１０ 制御部
１１ 本体部
１２ センサ設置部
１３ 可動部
１４ センサ

Claims (1)

1. 予め記憶された周囲の環境情報である第１環境情報を、移動体に搭載されたメモリ又は前記移動体の外部の装置から取得するデータ取得部と、
   前記移動体に設けられ、周方向角度毎の周囲の環境情報である第２環境情報を取得するセンサと、
   前記第１環境情報と前記第２環境情報とを比較する比較処理を含む演算処理によって、前記移動体の自己位置を検出する演算処理部と、
   を備え、
   前記演算処理部は、前記周方向角度毎に、前記第２環境情報に基づき、前記自己位置の検出精度の高さに対応付けた指標値を算出し、
   当該指標値の内、所定値以上の指標値が複数存在した場合、又は、相対的に高い指標値を複数特定できた場合は、これら複数の指標値のそれぞれに対応する周方向角度の前記第２環境情報から算出した自己位置の中から、相対的に他の自己位置から離れている自己位置を除去し、
   除去した残りの自己位置が複数存在する場合は、これらの平均値又は中央値を前記移動体の自己位置として検出する、
   自己位置検出装置。

Images