(57)【要約】【課題】ロボットの動作制御の精度を向上させることができる周辺地図作成方法、自己位置推定方法および自己位置推定装置を提供する。【解決手段】移動ロボット１の自己位置推定装置に、距離データを入力するLRF距離データ入力部１１ａと、移動ロボット１の移動量および角度変化量を入力するオドメトリデータ入力部１１ｂと、距離データを基にして障害物なしの領域および障害物有りの領域を、サブピクセル処理を用いて描画した局所地図を生成する局所地図生成部１１ｃと、局所地図と予め取得された自己位置・姿勢データを利用してサブピクセル処理を用いて大域地図を生成する大域地図生成部１１ｄと、距離データを大域地図とマッチングすることにより直前の自己位置・姿勢に位置・姿勢変化量データを反映して更新したデータを補正して、最新の自己位置・姿勢データを作成する自己位置推定部１１ｅとを設けた。【選択図】図２
2017097402.tif 000002
【特許請求の範囲】
【請求項１】
移動経路上にある物体までの距離を計測する距離センサ、並びに、自己の移動量および移動方向の角度変化量を検出する位置・姿勢変化量検出手段を搭載した移動体の自己位置推定装置であって、
前記距離センサにより取得した距離データを入力する距離データ入力部と、
前記位置・姿勢変化量検出手段により取得した位置・姿勢変化量データを入力する位置・姿勢変化量データ入力部と、
前記距離データを基にして前記距離センサにより距離を検出できる範囲についてサブピクセル処理を用いて物体なしの領域および物体ありの領域を描画した局所地図を生成する局所地図生成部と、
前記局所地図並びにサブピクセル処理を用いて取得した自己の位置及び姿勢を利用して大域地図を生成する大域地図生成部と、
前記距離データを前記大域地図とマッチングすることにより直前に作成した自己位置・姿勢データに位置・姿勢変化量データを反映して最新の自己位置・姿勢データを更新する自己位置推定部と
を備える
ことを特徴とする自己位置推定装置。
【請求項２】
請求項１記載の自己位置推定装置において、
前記局所地図生成部は、物体なしの領域および物体ありの領域を描画する際に画像上の色と色の境界線を滑らかにするアンチエイリアシング処理を施す
ことを特徴とする自己位置推定装置。
【請求項３】
請求項１又は請求項２記載の自己位置推定装置において、
前記自己位置推定部は、前記大域地図を用いて、大まかな自己位置・姿勢から疑似的な距離データをサブピクセル処理により作成し、前記疑似的な距離データと実際の距離データとをスキャンマッチングして位置・姿勢を補正する
ことを特徴とする自己位置推定装置。
【請求項４】
移動経路上にある物体までの距離を検出する距離センサおよび自己の移動量および移動方向の角度変化量を検出する位置・姿勢変化量検出手段を搭載した移動体の周辺地図を作成する方法であって、
前記距離センサにより取得した距離データを入力する第一の工程と、
前記位置・姿勢変化量検出手段により取得した前記移動体の移動量および角度変化量を入力する第二の工程と、
前記距離データを基にして前記距離センサにより距離を取得できる範囲についてサブピクセル処理を用いて物体なしの領域および物体ありの領域を描画した局所地図を生成する第三の工程と、
前記局所地図とサブピクセル処理を用いて取得した自己の位置及び姿勢とを利用して大域地図を生成する第四の工程と
を備える
ことを特徴とする周辺地図作成方法。
【請求項５】
請求項４記載の周辺地図作成方法において、
前記第三の工程では、物体なしの領域および物体ありの領域を描画する際に画像上の色と色の境界線を滑らかにするアンチエイリアシング処理を施す
ことを特徴とする周辺地図作成方法。
【請求項６】
請求項４又は請求項５記載の周辺地図作成方法に続いて、
前記距離データを前記大域地図とマッチングすることにより直前に作成した自己位置・姿勢データに位置・姿勢変化量データを反映して最新の自己位置・姿勢データを更新する第五の工程を備える
ことを特徴とする自己位置推定方法。
【請求項７】
請求項６記載の自己位置推定方法において、
前記第五の工程では、
大域地図上でサブピクセル処理を用いて疑似的な距離データを取得し、
実際の距離データを取得し、
前記疑似的な距離データと前記実際の距離データとのスキャンマッチングを行い、
前記スキャンマッチングの結果に基づいて前記大域地図上の位置・姿勢を補正する
ことを特徴とする自己位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、レーザレンジファインダ（LRF）を用いて周辺環境を計測しながら動作するロボットについて、その周辺環境の地図生成と自己位置の推定を行う周辺地図作成方法、自己位置推定方法および自己位置推定装置に関する。
【背景技術】
【０００２】
下記特許文献１〜９及び非特許文献１には、レーザレンジファインダ（LRF）を用いてロボット等の周辺環境を距離計測して局所地図を作成し、その局所地図を用いて全体の大域地図を生成する方法が記載されている。
このうち、局所地図を作る方法については非特許文献１に開示されている。
【０００３】
非特許文献１では、レーザレンジファインダによって計測した距離データ（以下、LRF距離データ）に基づく局所地図の生成、及び、この局所地図を用いた大域地図の生成の方法として、LRF距離データを画像データと同じピクセルデータに変換して扱っている。たとえば図２１に示すようなLRF距離データが得られた場合、LRF距離データを基にした物体（障害物等）のピクセルデータは図２２に黒色で示す部分となる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１０−０６１３５５号公報
【特許文献２】特開２０１０−０６１４４２号公報
【特許文献３】特開２０１０−０６６９３２号公報
【特許文献４】特開２０１０−０７２７６２号公報
【特許文献５】特開２０１０−０７９８６９号公報
【特許文献６】特開２０１４−００６８３３号公報
【特許文献７】特開２０１４−００６８３５号公報
【特許文献８】特開２０１０−２６２５４６号公報
【特許文献９】特開２０１２−１８５２０２号公報
【非特許文献】
【０００５】
【非特許文献１】根岸、三浦、白井著、「全方位ステレオとレーザレンジファインダの統合による移動ロボットの地図生成」、日本ロボット学会誌、２００３年、Ｖｏｌ．２１、Ｎｏ．６、ｐ．６９０−６９６
【発明の概要】
【発明が解決しようとする課題】
【０００６】
以下に地図生成の一例について簡単に説明する。地図生成では、次の手順でまず局所地図を作成する。
（１）局所地図全体を「不明領域」として輝度0.5（例えば、グレー）に初期化する。
（２）物体がないところを「物体なし領域」として輝度0.0（例えば、黒）で描画する。
（３）レーザレンジファインダの計測点（物体のある点）間を、「物体あり領域」として距離に応じた輝度0.5〜0.0の間（グレー〜白）で描画する。これにより、計測点間が直線状に描画される。
【０００７】
続いて、局所地図を用いて大域地図を作成する方法は次の通りとなる。
（１）局所地図と大域地図の向きが異なる場合、自己位置の向きに合わせてレーザレンジファインダの中心を軸に局所地図を回転し、局所地図の向きを修正する。
（２）局所地図の座標系を大域地図の座標系に合わせる。
（３）局所地図を大域地図と同じ向きの座標系で再描画する。
（４）推定した自己位置に基づき局所地図を大域地図に重ね合わせ、大域地図を更新する。
【０００８】
また、大域地図から自己位置を推定する方法は以下の通りとなる。
自己位置を推定するためには、現在のLRF距離データと大域地図上の物体とのマッチングを行う必要がある。マッチングの方法には、パーティクルフィルタを用いる方法やスキャンマッチング（ICP Scan MatchingやPolar Scan Matching等）を用いる方法、またはこれら両方を用いる方法などがある。
【０００９】
パーティクルフィルタを用いる方法では、N個のパーティクルPi(i=1,2,…,N)それぞれが大域地図Mapiと現在の自己位置・姿勢pi(xi,yi,θi)を持つ。ロボットが移動したときにはオドメトリデータなどから得られる移動量・姿勢変化量Δp(Δx,Δy,Δθ)から、パーティクル毎に自己位置・姿勢を更新する。その際には、パーティクル毎にランダムなノイズ(δix,δiy,δiθ)を加えるので、新しい自己位置・姿勢pi'(xi',yi',θi')は、下式（１）となる。
【００１０】
【数１】
2017097402.tif 000003
【００１１】
その後、LRF距離データから局所地図を作成し、パーティクル毎に局所地図を大域地図Mapiに適用するが、その適用時に局所地図上の物体と大域地図上の物体の重なり度合いから尤度を求める。この尤度の最も高いパーティクルが最も精度の高い地図と自己位置・姿勢を持っていることとなる。
【００１２】
また、スキャンマッチングを用いる方法では、大域地図の物体座標データと、LRF距離データのそれぞれの形状から位置合わせを行う。
例えば、Polar Scan Matching（以下、PSM）の場合、以下の手順となる。
（１）大域地図上の物体に対して、推定した自己位置からレーザレンジファインダのレーザー放射角と同じように各角度方向に探索して物体までの距離を求め、疑似的なLRF距離データを作成する。
（２）実際のLRF距離データと最も誤差が少なくなるように回転・重ね合わせ、自己位置・姿勢を補正する。
【００１３】
ここで、局所地図作成時に例えばLRF距離データに基づき隣り合う二つの計測点（物体のある点）間を直線状に描画する場合、本来の座標系が浮動小数点で扱っているのに対し、ピクセルレベルで描画する際に小数点以下を切り捨てる（または四捨五入する）と、その分の誤差が生じてしまう。例えば、局所地図の解像度を0.1m/pixelとした場合ではx方向・y方向それぞれで-0.05m〜0.05mの誤差が生じることになる。
【００１４】
同様に局所地図を大域地図に重ね合わせて大域地図を更新するときも、実際の自己位置座標を大域地図座標に変換した場合には浮動小数点以下を切り捨てる（または四捨五入する）ため、大域地図の解像度分の誤差が生じる。例えば大域地図の解像度を0.1m/pixelとした場合ではx方向・y方向それぞれで-0.05m〜0.05mの誤差が生じる。
【００１５】
さらに、自己位置推定においてPSMを使用して疑似的なLRF距離データを作成する場合、推定した自己位置から物体までのピクセルを角度方向に探索するが、ここでもピクセル分の誤差が生じる。
【００１６】
上記のように、地図生成および自己位置推定の際にピクセル分の誤差が複数個所で起こるため、地図作成時の精度が十分に得られないおそれがあるという問題があった。
たとえば、0.1m/pixelの解像度の地図を使用した場合には、ロボットの動作制御は良くても-0.05m〜0.05mの精度しか得られない。
【００１７】
一方、地図の解像度を上げればその分動作制御の精度は改善できるが、解像度を上げることにより保持する大域地図画像データが大きなものになるほか、処理速度が大きく低下する。そのため単純に解像度を上げることができないという問題もあった。
【００１８】
このようなことから本発明は、地図の解像度を上げることなくロボットの動作制御の精度を向上させることができる周辺地図作成方法、自己位置推定方法および自己位置推定装置を提供することを目的とする。
【課題を解決するための手段】
【００１９】
上記の課題を解決するための第１の発明に係る自己位置推定装置は、
移動経路上にある物体までの距離を計測する距離センサ、並びに、自己の移動量および移動方向の角度変化量を検出する位置・姿勢変化量検出手段を搭載した移動体の自己位置推定装置であって、
前記距離センサにより取得した距離データを入力する距離データ入力部と、
前記位置・姿勢変化量検出手段により取得した位置・姿勢変化量データを入力する位置・姿勢変化量データ入力部と、
前記距離データを基にして前記距離センサにより距離を検出できる範囲についてサブピクセル処理を用いて物体なしの領域および物体ありの領域を描画した局所地図を生成する局所地図生成部と、
前記局所地図並びにサブピクセル処理を用いて取得した自己の位置及び姿勢を利用して大域地図を生成する大域地図生成部と、
前記距離データを前記大域地図とマッチングすることにより直前に作成した自己位置・姿勢データに位置・姿勢変化量データを反映して最新の自己位置・姿勢データを更新する自己位置推定部と
を備える
ことを特徴とする。
【００２０】
上記の課題を解決するための第２の発明に係る自己位置推定装置は、
前記局所地図生成部は、物体なしの領域および物体ありの領域を描画する際に画像上の色と色の境界線を滑らかにするアンチエイリアシング処理を施す
ことを特徴とする。
【００２１】
上記の課題を解決するための第３の発明に係る自己位置推定装置は、
前記自己位置推定部は、前記大域地図を用いて、大まかな自己位置・姿勢から疑似的な距離データをサブピクセル処理により作成し、前記疑似的な距離データと実際の距離データとをスキャンマッチングして位置・姿勢を補正する
ことを特徴とする。
【００２２】
上記の課題を解決するための第４の発明に係る周辺地図作成方法は、
移動経路上にある物体までの距離を検出する距離センサおよび自己の移動量および移動方向の角度変化量を検出する位置・姿勢変化量検出手段を搭載した移動体の周辺地図を作成する方法であって、
前記距離センサにより取得した距離データを入力する第一の工程と、
前記位置・姿勢変化量検出手段により取得した前記移動体の移動量および角度変化量を入力する第二の工程と、
前記距離データを基にして前記距離センサにより距離を取得できる範囲についてサブピクセル処理を用いて物体なしの領域および物体ありの領域を描画した局所地図を生成する第三の工程と、
前記局所地図とサブピクセル処理を用いて取得した自己の位置及び姿勢とを利用して大域地図を生成する第四の工程と
を備える
ことを特徴とする。
【００２３】
上記の課題を解決するための第５の発明に係る周辺地図作成方法は、
前記第三の工程では、物体なしの領域および物体ありの領域を描画する際に画像上の色と色の境界線を滑らかにするアンチエイリアシング処理を施す
ことを特徴とする。
【００２４】
上記の課題を解決するための第６の発明に係る自己位置推定方法は、
前記距離データを前記大域地図とマッチングすることにより直前に作成した自己位置・姿勢データに位置・姿勢変化量データを反映して最新の自己位置・姿勢データを更新する第五の工程を備える
ことを特徴とする。
【００２５】
上記の課題を解決するための第７の発明に係る自己位置推定方法は、
前記第五の工程では、
大域地図上でサブピクセル処理を用いて疑似的な距離データを取得し、
実際の距離データを取得し、
前記疑似的な距離データと前記実際の距離データとのスキャンマッチングを行い、
前記スキャンマッチングの結果に基づいて前記大域地図上の位置・姿勢を補正する
ことを特徴とする。
【発明の効果】
【００２６】
本発明に係る周辺地図作成方法、自己位置推定方法および自己位置推定装置によれば、地図の解像度を上げることなく高精度に局所地図を作成することが可能になる。これにより、大域地図の精度改善が可能になり、よって、高精度な自己位置推定が可能になる。
【図面の簡単な説明】
【００２７】
【図１】本発明の実施例に係る自己位置推定装置による距離計測の例を示す説明図である。
【図２】本発明の実施例に係る自己位置推定装置の構成を示すブロック図である。
【図３】本発明の実施例に係る地図生成処理の流れを示すフローチャートである。
【図４】局所地図を初期化した例を示す説明図である。
【図５】図４に示す局所地図に「物体なし領域」を描画してアンチエイリアシング処理を施した例を示す説明図である。
【図６】図６（ａ）はアンチエイリアシング処理を施した例を示す説明図、図６（ｂ）はアンチエイリアシング処理を施していない画像の例を示す説明図である。
【図７】サブピクセル位置及びピクセル位置の例を示す説明図である。
【図８】サブピクセル処理により輝度値1.0の点を描画する例を示す説明図である。
【図９】サブピクセル位置及びピクセル位置での輝度値取得の例を示す説明図である。
【図１０】サブピクセル処理を用いて取得した輝度値の例を示す説明図である。
【図１１】図１１（ａ）は輝度値のピークの探索例を示す説明図、図１１（ｂ）はサブピクセルレベルでパラボラフィッティングにより輝度値のピークを求める例を示す説明図である。
【図１２】図５に示す局所地図に「物体あり領域」を描画しアンチエイリアシング処理を施した例を示す説明図である。
【図１３】図１３（ａ）は局所地図の例を示す説明図、図１３（ｂ）は大域地図上での自己位置推定結果例を示す説明図である。
【図１４】局所地図の向きを修正する例を示す説明図である。
【図１５】局所地図の座標系を大域地図の座標系に合わせる例を示す説明図である。
【図１６】大域地図と同じ向きの座標系で局所地図を再描画する例を示す説明図である。
【図１７】局所地図を大域地図に重ね合わせ、大域地図を更新する例を示す説明図である。
【図１８】本発明の実施例に係る自己位置推定処理の流れを示すフローチャートである。
【図１９】図１９（ａ）は疑似的なLRF距離データを作成する例を示す説明図、図１９（ｂ）は図１９（ａ）から抽出した疑似的なLRF距離データを示す説明図である。
【図２０】実際のLRF距離データに合わせて疑似的なLRF距離データの位置及び姿勢を補正する例を示す説明図である。
【図２１】LRF距離データの例を示す説明図である。
【図２２】図２１に示すLRF距離データを基にした物体のピクセルデータの例を示す説明図である。
【発明を実施するための形態】
【００２８】
以下、図面を参照しつつ本発明に係る周辺地図作成方法、自己位置推定方法および自己位置推定装置について説明する。
【実施例】
【００２９】
図１から図２０を用いて、本発明の実施例に係る周辺地図作成方法、自己位置推定方法および自己位置推定装置を詳細に説明する。なお、以下の実施例においては、輝度値0.0を白、輝度値1.0を黒とし、輝度値0.0と輝度値1.0との間については輝度値が0.0に近いほど白いグレー、輝度値が1.0に近いほど黒いグレーとして説明する。
【００３０】
図１に示すように、本実施例に係る自己位置推定装置１１は、距離センサとしてのレーザレンジファインダ（LRF）１２を用いて周辺環境を計測しながら動作する移動ロボット１に搭載され、レーザレンジファインダ１２により取得した距離データを用いて移動ロボット１の周辺環境の地図作成と自己位置の推定を行うものである。
【００３１】
図２に示すように、本実施例において自己位置推定装置１１は、LRF距離データ入力部１１ａと、オドメトリデータ入力部１１ｂと、局所地図生成部１１ｃと、大域地図生成部１１ｄと、自己位置推定部１１ｅと、記憶部１１ｆとを備えている。
【００３２】
LRF距離データ入力部１１ａは、レーザレンジファインダ１２から距離データを入力し、記憶部１１ｆに保管する。
オドメトリデータ入力部１１ｂは、移動ロボット１の位置・姿勢変化量検出手段としての駆動部１３から移動ロボット１の移動量および角度変化量を入力して、位置・姿勢変化量データとして記憶部１１ｆに保管する。
【００３３】
局所地図生成部１１ｃは、記憶部１１ｆから読み出した距離データを基にして現在レーザレンジファインダ１２で距離データが得られる範囲の局所地図３（図１２等参照）を作成し、記憶部１１ｆに保管する。
大域地図生成部１１ｄは、記憶部１１ｆから読み出した局所地図３と後で詳述する自己位置・姿勢データとから大域地図４（図１７等参照）を作成し、記憶部１１ｆに保管する。
【００３４】
自己位置推定部１１ｅは、記憶部１１ｆから読み出した大域地図４と距離データとをマッチングすることにより、記憶部１１ｆから読み出した位置・姿勢変化量データを直前の自己位置・姿勢に反映して更新したデータを補正して、最新の自己位置・姿勢データを作成し、記憶部１１ｆに保管する。
記憶部１１ｆは、距離データ、位置・姿勢変化量データ、自己位置・姿勢データ、局所地図３、大域地図４等を保管する。
【００３５】
レーザレンジファインダ１２は、水平方向に一定角度Δθの間隔（角度分解能：例えばΔθ＝１度やΔθ＝０．２５度など）でレーザー光１２ａを照射し、このレーザー光１２ａの物体（障害物２ａや壁２ｂ等。以下、総称する場合は物体２と称する）による反射光を検出して物体２までの距離を計測する。なお、図１中において、点Ｐはレーザレンジファインダ１２のレーザー光１２ａによる物体計測点を示す。
【００３６】
以下、図３から図１７を用いて自己位置推定装置１１による地図生成の処理の流れを説明する。
【００３７】
図３に示すように、自己位置推定装置１１ではまず、LRF距離データ入力部１１ａにより、レーザレンジファインダ１２から距離データを取得し（ステップＳ１１）、オドメトリデータ入力部１１ｂにより、移動ロボット１の駆動部１３から移動量および角度変化量（位置・姿勢変化量データ）を取得する（ステップＳ１２）。
【００３８】
続いて、局所地図生成部１１ｃにより局所地図３の描画（生成）を行う（ステップＳ１３）。すなわち、記憶部１１ｆから距離データを読み出し、この距離データを基にして現在レーザレンジファインダ１２により距離データを取得できる範囲の局所地図３を生成する。
【００３９】
局所地図生成部１１ｃにおける処理をより詳しく説明すると、まず、図４に示すように、局所地図３全体を不明領域Ａに対応する輝度値0.5で塗りつぶし、初期化する（ステップＳ１３ａ）。続いて、図５に示すように、LRF距離データを基に物体がない部分を物体なし領域Ｂとして輝度値1.0で描画し、不明領域Ａとの境界に対してアンチエイリアシング処理を行う（ステップＳ１３ｂ）。
【００４０】
ここで、アンチエイリアシング処理とは、画像上の色と色の境界線を滑らかにする処理である。通常アンチエイリアシング処理なしで線を引くと図６（ｂ）に示すように境界線がギザギザ（ジャギー）となる。これに対し、図６（ａ）に示すようにアンチエイリアシング処理は、ジャギーを軽減するように境界線で二つの色の中間色を使用する。
【００４１】
また、物体２の位置は、サブピクセル処理により取得する。
サブピクセル処理とは、ピクセルレベル（整数）で扱う画像に対して、ピクセルサイズ以下の精度の値（浮動小数点）を扱う仕組みであり、本実施例のサブピクセル処理には、サブピクセルレベルでの描画処理、サブピクセルレベルでの輝度値取得処理、サブピクセルレベルでの輝度値ピーク位置の取得処理の三つの処理が含まれる。
【００４２】
〔サブピクセルレベルでの描画処理〕
例えば図７に示すように座標p(2.3,2.8)に点を描画する場合、サブピクセルを用いないピクセルレベルでは図７中に実線で示すように座標を四捨五入して座標p(2,3)に点を描画することになる。
これに対して本実施例では、図７中に太線の破線で示すようにサブピクセルを考慮するため、浮動小数点で表示した座標p(x+Δx,y+Δy)（ただし、切り捨てによる整数部分を(x,y)、小数部分を(Δx,Δy)）に対して輝度値lの点を描画する場合、四つの座標(x,y)、(x+1,y)、(x,y+1)、および(x+1,y+1)に対しそれぞれ以下の式（２）〜（５）により求めた輝度値lx,y、lx+1,y、lx,y+1、lx+1,y+1の点を描画する。
【００４３】
lx,y=(1-Δx)(1-Δy)l  …（２）
lx+1,y=Δx(1-Δy)l  …（３）
lx,y+1=(1-Δx)Δyl  …（４）
lx+1,y+1=ΔxΔyl  …（５）
【００４４】
上式（２）〜（４）に基づいて、輝度値1.0の点を座標p(2.3,2.8)に描画すると、l2,2=0.14，l3,2=0.06，l2,3=0.56，l3,3=0.24となり、図８に示すようになる。
【００４５】
〔サブピクセルレベルでの輝度値取得〕
また、例えば図９に示すように座標p(2.3,2.8)の輝度値を取得したい場合、サブピクセルを用いないピクセルレベルでは座標を四捨五入したp(2,3)の輝度値を取得することになる。
【００４６】
これに対して本実施例ではサブピクセルを考慮するため、浮動小数点で表示した座標p(x+Δx,y+Δy)（ただし、切り捨てによる整数部分を(x,y)、小数部分を(Δx,Δy)）から輝度値lを取得したい場合、この輝度値lを、四つの点(x,y)、(x+1,y)、（x,y+1）、および(x+1,y+1)でのそれぞれの輝度値lx,y、lx+1,y、lx,y+1、lx+1,y+1から以下の式（６）により求める。
l=(1-Δy)[(1-Δx)lx,y+Δxlx+1,y]+Δy[(1-Δx)lx,y+1+Δxlx+1,y+1]  …（６）
【００４７】
上式（６）に基づいて、図９に示す座標p(2.3,2.8)における輝度値lを求めると、l2,2=1.0，l3,2=1.0，l2,3=0.0，l3,3=1.0からl=0.44となり、図１０に示すようになる。
【００４８】
〔サブピクセルレベルでの輝度値ピーク位置の取得〕
また、例えば図１１（ａ）に示すように探索を行い、輝度値のピークの位置（ここでは、最も輝度値が低い位置）を求める場合、サブピクセルを用いないピクセルレベルでは、座標(1,2)、(2,2)と探索し、座標(3,2)の位置が輝度値のピークとなる。
【００４９】
これに対して本実施例ではサブピクセルを考慮するため、パラボラフィッティングにより輝度値のピーク位置を算出する。パラボラフィッティングはピークの値（ピーク位置xの輝度値lx）とその前後の値（輝度値lx-1とlx+1）から、輝度値のピーク位置のズレΔxを以下の式（７）により求める。
【００５０】
【数２】
2017097402.tif 000004
【００５１】
上記の例で、サブピクセルレベルで輝度値のピーク位置を求める場合、輝度値のグラフは図１１（ｂ）に示すようになり、Δxの値は約0.24となる。結果、輝度値のピーク位置は図１１（ａ）に示すように座標(3.2,2)になる。
【００５２】
ステップＳ１３ｂに続いては、図１２に示すようにレーザレンジファインダ１２により取得した計測点（物体のある点）間を、物体あり領域Ｃとして隣り合う計測点間の距離に応じた輝度0.5〜0.0で描画して直線状の線を引き、アンチエイリアシング処理を施す（ステップＳ１３ｃ）。
ここで、隣り合う計測点間の距離に応じた輝度0.5〜0.0とは、隣り合う計測点間の距離が短いほど輝度0.0に近く、隣り合う計測点間の距離が長いほど輝度0.5に近くなるように設定されるものとする。
【００５３】
続いて、大域地図生成部１１ｄにより、大域地図４の更新（生成）を行う（ステップＳ１４）。すなわち、記憶部１１ｆから後述する自己位置・姿勢データおよび局所地図３を読み出し、これらを用いて大域地図４を更新（生成）する。
【００５４】
大域地図生成部１１ｄにおける処理を図１３から図１７を用いてより詳しく説明する。なお、以下では、局所地図が図１３（ａ）に示す状態である場合を例に説明するものとする。また、図１３から図１７に示す局所地図及び大域地図は実際にはアンチエイリアシング処理が施されているものとする。
【００５５】
大域地図生成部１１ｄではまず、自己推定位置に基づく大域地図４上の位置を、整数成分(x,y)と小数部分(Δx,Δy)に分ける（ステップＳ１４ａ）。
【００５６】
続いて、自己位置推定に基づくレーザレンジファインダ１２の向きに基づき局所地図３を回転し、上記の小数部分(Δx,Δy)に従い平行移動をする（ステップＳ１４ｂ）。この時、新しく作られる回転・平行移動した局所地図３は、サブピクセルレベルでの輝度値取得で得た輝度値で描画する。この時点で小数部分(Δx,Δy)の平行移動を加えることで、自己位置座標の切り捨てによる誤差を無くす。
【００５７】
例えば、局所地図３上での自己位置の向きが図１３（ａ）では正面を向いているのに対し、大域地図４上での現在の自己位置の向きの推定結果が図１３（ｂ）に矢印で示す向きになっている場合、図１４に示すように局所地図３の向きを大域地図４上での自己位置の向きに合わせて修正する。より具体的には、レーザレンジファインダ１２の中心を軸に局所地図３を回転させることにより、局所地図３の向きを修正する。
【００５８】
続いて、上記の座標の整数部分(x,y)に基づき、局所地図３の座標系を、図１５に示すように大域地図４の座標系に合わせ、図１６に示すように大域地図４と同じ向きの座標系で局所地図３を再描画する。
【００５９】
ステップＳ１４ｂに続いては、大域地図４と同じ向きの座標系で再描画した局所地図３を、ステップＳ１４ａでサブピクセル処理により推定した自己位置に基づき図１７に示すように大域地図４に重ね合わせる（ステップＳ１４ｃ）。これにより、大域地図４が更新される。
【００６０】
次に、図１８から図２０を用いて自己位置推定装置１１による自己位置推定の処理の流れを説明する。
【００６１】
自己位置推定装置１１ではまず、図１９（ａ）に示すように、自己位置推定部１１ｅによりレーザレンジファインダ１２のサブピクセルレベルでの位置から、レーザー光１２ａそれぞれの角度ごとに、１ピクセル分ずつ物体にぶつかるまで探索し、輝度値lがピークになる部分を探索する（ステップＳ２１）。この時点の輝度値lの取得は、サブピクセルレベルで行う。
【００６２】
続いて、ピークの輝度値lxと、その前後の輝度値lx-1,lx+1からパラボラフィッティングによりサブピクセルレベルでの輝度値lxのピーク位置pを算出する（ステップＳ２２）。
続いて、大域地図４上で疑似的なLRF距離データを取得する（ステップＳ２３）。すなわち、ステップＳ２１、ステップＳ２２を全ての角度で行い、レーザレンジファインダ１２の中心からの距離を算出することで、疑似的なLRF距離データを得る。
【００６３】
その後、実際のLRF距離データを取得し（ステップＳ２４）、スキャンマッチングを行い（ステップＳ２５）、大域地図４上の位置・姿勢を補正する（ステップＳ２６）。これにより、最新の自己位置・姿勢を取得する。
【００６４】
すなわち、自己位置推定を行うためには、現在のLRF距離データと、大域地図４上の物体とのマッチングを行う必要がある。マッチング方法としては、パーティクルフィルタを用いる方法やスキャンマッチング（ICP Scan MatchingやPSM等）を用いる方法、または両方を用いる方法などがある。
【００６５】
例えば、パーティクルフィルタを用いる方法では、Ｎ個のパーティクルPi(i=1,2,…,N)それぞれが大域地図４(Mapi)と現在の自己位置・姿勢pi(xi,yi,zi)を持つ。移動ロボット１が移動したときにはオドメトリデータなどから得られる移動量、姿勢変化量Δp(Δx,Δy,Δθ)から、パーティクル毎に自己位置・姿勢を更新する。その際には、パーティクル毎にランダムなノイズ（δix,δiy,δiθ）を加えるので、新しい自己位置・姿勢pi'(xi',yi',zi')は、上述したように式（１）で表される。
【００６６】
その後、LRF距離データから局所地図３を作成し、パーティクル毎に局所地図３を大域地図４(Mapi)に適用するが、その適用時に局所地図３上の物体２と大域地図４上の物体２の重なり度合いから尤度を求める。この尤度の最も高いパーティクルが最も精度の高い地図と自己位置・姿勢を持っていることとなる。
【００６７】
スキャンマッチングを用いる方法では、大域地図４の物体座標データと、LRF距離データのそれぞれの形状から位置合わせを行う。
【００６８】
スキャンマッチングとしてPSMを適用する場合、以下の手順となる。
まず、上述したように大域地図４上の物体に対して、推定した自己位置からレーザレンジファインダ１２のレーザー放射角と同じように各角度方向に探索して物体までの距離を求め、図１９（ｂ）に示すような疑似的なLRF距離データを作成する。なお、図１９中に示す符号１４は疑似レーザレンジファインダを示し、点Ｐ'は疑似的な物体計測点を示す。
そして、図２０に示すように、実際のLRF距離データと最も誤差が少なくなるように疑似的なLRF距離データを回転させ、実際のLRF距離データに重ね合わせて、自己位置・姿勢を補正することにより、最新の自己位置・姿勢が得られる。
【００６９】
このような本実施例に係る周辺地図作成方法、自己位置推定方法および自己位置推定装置によれば、局所地図３を作成する際、座標の値を浮動小数点のサブピクセルレベルで行うことで、精度の高い局所地図３の作成が可能となる。また、局所地図３を作成する際、アンチエイリアシング処理を行うことにより、不明領域Ａ、物体なし領域Ｂ、物体あり領域Ｃの相互の境界線を滑らかに表示することができる。
また、大域地図更新時、上述したように精度を改善した局所地図３を用いて、更に局所地図３の位置合わせをサブピクセルレベルで行うことで、大域地図の精度改善が可能となる。
また、自己位置推定時、疑似的なLRF距離データを作成する際に、上記の精度改善した大域地図４を用いて、さらにサブピクセルレベルでの物体探索を行うことで、高精度な自己位置推定が可能になる。
【産業上の利用可能性】
【００７０】
本発明は、周辺地図作成方法、自己位置推定方法および自己位置推定装置に適用することができる。
【符号の説明】
【００７１】
１  移動ロボット
２ａ  障害物
２ｂ  壁
３  局所地図
４  大域地図
１１  自己位置推定装置
１１ａ  LRF距離データ入力部
１１ｂ  オドメトリデータ入力部
１１ｃ  局所地図生成部
１１ｄ  大域地図生成部
１１ｅ  自己位置推定部
１２  レーザレンジファインダ
１３  駆動部
１４  疑似レーザレンジファインダ
Ａ  不明領域
Ｂ  物体なし領域
Ｃ  物体あり領域
Ｐ  物体計測点
Ｐ'  疑似的な物体計測点
【図１】
2017097402.tif 000005
【図２】
2017097402.tif 000006
【図３】
2017097402.tif 000007
【図４】
2017097402.tif 000008
【図５】
2017097402.tif 000009
【図６】
2017097402.tif 000010
【図７】
2017097402.tif 000011
【図８】
2017097402.tif 000012
【図９】
2017097402.tif 000013
【図１０】
2017097402.tif 000014
【図１１】
2017097402.tif 000015
【図１２】
2017097402.tif 000016
【図１３】
2017097402.tif 000017
【図１４】
2017097402.tif 000018
【図１５】
2017097402.tif 000019
【図１６】
2017097402.tif 000020
【図１７】
2017097402.tif 000021
【図１８】
2017097402.tif 000022
【図１９】
2017097402.tif 000023
【図２０】
2017097402.tif 000024
【図２１】
2017097402.tif 000025
【図２２】
2017097402.tif 000026