JP4533659B2 - レーザー計測により地図画像を生成する装置及び方法 - Google Patents

Description

本発明は、障害物までの方位と距離の計測が可能なレーザー距離センサを移動させながら計測することにより、移動するセンサの周囲の環境における障害物の位置を画像として自動的に生成及び／又は表示する装置及び方法に関する。

レーザー距離センサを移動させながらレーザー距離センサの周囲の環境を複数回計測し、各計測毎の値から環境の地図を画像として生成する手法としては、1回の計測で得られる或る領域の計測値と、他の1回の計測で得られる或る領域の計測値のと差の二乗和を評価値として、この評価値を最小とするような計測値間の変位を算出して（いわゆる、最小二乗法の算出手法によって）、これをセンサの移動距離とし、この移動距離に従って各回の計測値を共通の座標系に記録することで、地図状の画像として生成する手法が提案されている（例えば、非特許文献１と２を参照）。
根岸善朗、三浦純、 白井良明、「全方位ステレオとレーザレンジファインダの統合による移動ロボットの地図生成」、日本ロボット学会誌、Vol.21，No.6，pp.110-116，2003． D. Hahnel、W. Burgard、「Probabilistic Matching for 3D Scan Registration」、In Proc of the VDI-Conference Robotik 2002，2002．

しかしながら、上述した非特許文献１，２に記載されているような従来技術におけるレーザー距離センサ移動前後の計測値間の変位を最小二乗法算出手法で評価すると、必ずしも常に最適解が求まるということにならず、結果的に局所解を求めてしまうことが発生するという課題が生じていた。すなわち、センサ移動前後の計測値で評価した環境中の障害物の照合が、局所解によって正確に実施されない（センサ移動前後で或る障害物の照合が一致しない）という不都合が発生する。

本発明の目的は、センサの移動前後における画像間の相対位置と姿勢を精密に算出した場合における誤差の発生を抑制して、地図画像を精度良く生成又は表示する地図画像生成装置を提供することにある。

前記課題を解決するために、本発明は主として次のような構成を採用する。
障害物までの方位と距離を用いて前記距離センサ周囲の地図画像を生成する画像生成装置であって、
前記距離センサを移動させて前記障害物までの方位と距離の計測値を前記移動の前後のそれぞれで取得し、
前記計測値に基づいて前記距離センサの周囲における障害物の有無を、前記距離センサの分解能で決められたグリッド毎に割り当てられた占有（画像において障害物が占有）、非占有（画像において障害物が非占有）、未知（画像において障害物が占有しているかが未知）の状態によって表したグリッド画像を生成し、
前記計測値でそれぞれ得られた各グリッド画像において、障害物で占有された状態のグリッドを中心として或る範囲を示すウインドウを設け、前記ウインドウ内のグリッドの状態を占有、非占有、未知で表した際の各状態の出現頻度をヒストグラムで表したときに、占有状態のグリッドが多くて障害物の有無が未知な状態のグリッドが少ないウインドウを特徴点として抽出し、
現在の計測により求められたグリッド画像と前回の計測により求められたグリッド画像の２つのグリッド画像間において対応する前記特徴点の比較に際して、前記抽出された各グリッド画像の前記特徴点における占有グリッドの数と前記特徴点間の距離を各グリッド画像間で比較することで、各グリッド画像間の前記特徴点を照合し、
前記照合の結果に基づいて特徴点の最も良く一致するグリッド画像間で重ね合わせてグリッド画像を仮配置し、
前記仮配置したグリッド画像間において前記計測値によって前記グリッド画像間の相対位置と姿勢を演算して前記地図画像を生成する構成とする。

レーザー距離センサによる複数回の計測値から環境における障害物の有無を示す地図を画像として生成する画像生成方法であって、
レーザーにより前記障害物までの方位と距離の計測値を取得する取得工程と、
前記取得工程から取得された前記計測値に基づいて前記レーザー距離センサの周囲における障害物の有無を、前記距離センサの分解能で決められたグリッド毎に割り当てられた占有（画像において障害物が占有）、非占有（画像において障害物が非占有）、未知（画像において障害物が占有しているかが未知）の状態によって表したグリッド画像を生成する生成工程と、
前記生成工程でそれぞれ得られた各グリッド画像において、障害物で占有された状態のグリッドを中心として或る範囲を示すウインドウを設け、前記ウインドウ内のグリッドの状態を占有、非占有、未知で表した際の各状態の出現頻度をヒストグラムで表したときに、占有状態のグリッドが多くて障害物の有無が未知な状態のグリッドが少ないウインドウを特徴点として抽出する抽出工程と、
現在の計測により求められたグリッド画像と前回の計測により求められたグリッド画像の２つのグリッド画像間において対応する前記特徴点の比較に際して、前記抽出工程で得られた各グリッド画像の前記特徴点における占有グリッドの数と前記特徴点間の距離を各グリッド画像間で比較し照合することで、各グリッド画像間で一致する前記特徴点の識別を行う識別工程と、
前記識別工程で得られた照合結果に基づきグリッド画像間の前記特徴点が最も良く一致するときの相対位置と姿勢に従って各グリッド画像を共通の座標系に重ね合わせて仮配置し、仮配置された各グリッド画像間で重複する領域を算出する算出工程と、
前記算出工程で得られた前記重複領域に含まれる前記レーザー距離センサの計測値のみを用いてグリッド画像間の相対位置と姿勢を演算する演算工程と、
前記演算工程で得られたグリッド画像間の相対位置と姿勢に従って複数の画像を１枚の画像に合成する生成工程と、からなる画像生成方法。

本発明によれば、移動するレーザー距離センサにより複数回の計測毎に得られる環境中での障害物の占有する領域（占有グリッド）を示す画像において共通な特徴点を抽出し照合して、１枚の環境地図の画像として自動的に合成及び／又は表示することが可能となる。

また、より具体的には、複数回の計測毎に生成される複数の画像のそれぞれからヒストグラムの特徴点を抽出し、各画像間でヒストグラムとその相対位置を照合することにより、画像間の相対位置と姿勢を算出し、この相対位置と姿勢により求められる画像間のおよその重複領域を算出し、重複領域に含まれる方位と距離の計測値のみを用いることにより画像間において対応がとれない計測値を減らし、より精度良く画像間の相対位置と姿勢を算出することが可能となり、精度の良い環境地図画像の自動生成及び／又は表示が可能となる。

本発明の地図画像を生成する画像生成装置に係る実施の形態について、実施例を挙げて図面を参照しながら以下詳細に説明する。本発明の概要は、レーザー距離センサが移動した周囲の環境の障害物の有無を示す地図を精度良く生成及び／又は表示するという目的のために、計測毎にグリッド画像を生成して一致する特徴点を検出して、およその画像間の相対位置と姿勢を求める前処理を施す（具体的には、センサの移動前後でグリッド画像間の仮の位置合わせを行う）ことにより、グリッド画像間の相対位置と姿勢を精密に算出した場合における誤差の発生を抑制し（例えば、後述する図９に示す局所解９０４（最適解９０３ではない）への落ち込みを防ぎ）、環境地図画像を精度良く生成及び／又は表示するものである。

本発明の地図画像生成装置に係る実施例を図面を用いて説明する。ここでは、特に、水平面の走査を行うことで障害物に対する方位と距離の計測が可能なレーザー距離センサを車輪移動型のロボットに搭載して移動させ、複数回の計測を行い、この計測値を用いてセンサが移動した環境における障害物の存在の有無を画像として生成及び／又は表示する装置を例示して説明する。

図１は本発明の地図画像生成装置に係る実施例における処理の流れの概要を示す。処理が開始されると（１０１）、レーザー距離センサから環境の地図を生成及び／又は表示する処理（１０２〜１０９）とロボット制御（１１１）の処理が開始される。レーザー距離センサによる地図生成及び／又は表示の処理では、まずレーザー距離センサを制御することにより方位と距離の計測値の入力を行い（１０２）、入力された計測値より環境中の障害物の方位と距離を示す画像であるグリッド画像（後述する図５に示す枡目で示した全体の画像）を生成する（１０３）。

次いで、画像において障害物の占有する領域（占有グリッド）が多く、また占有されているかが未知の領域（未知グリッド）が少ない点を特徴点として抽出し（１０４）、画像間の特徴点のヒストグラムと特徴点の相対位置が画像間で保持されているかを基準として画像間の照合を行い、照合結果のうち画像間で最も特徴点が保持されるような画像の相対位置と姿勢を記録し（１０５）、この相対位置と姿勢に従って画像同士を共通の座標系に配置したときに重複する領域を算出する（１０６）。

さらに、重複する領域内に含まれる各画像の方位と距離の実測値を用いることにより画像間の相対位置と姿勢を精密に算出し（１０７）、算出された画像間の相対位置と姿勢に従って各画像を共通の座標系に配置して環境地図の画像を生成し（１０８）、生成された環境地図画像を表示する（１０９）。

以上のレーザー距離センサから環境の地図を生成及び／又は表示する処理（１０２〜１０９）と並行してロボット制御（１１１）の処理により、操作者の指令もしくは予め指定された経路に沿って移動する。目的とする環境の地図が生成及び／又は表示が終了してからプログラムを終了する（１１０）。

以上説明した処理の流れの概略を踏まえ、図１の処理の流れをより詳細にしたものを図２に示す。図１の１０２が図２の２０２に、図１の１０３が図２の２０３に、図１の１０４が図２の２０４〜２０６に、図１の１０５が図２の２０７に、図１の１０６が図２の２０８〜２０９に、図１の１０７が図２の２１０に、図１の１０８が図２の２１１に、図１の１０９が図２の２１２に、それぞれ対応している。

また、図２の２０７の処理を詳細に示したものを図３に示す。さらに、以上の処理を行うにあたり、ロボットが環境中を移動し、レーザー距離センサにより環境中の障害物の方位と位置を走査する様子を図４に示す。

図２において、プログラムが開始されると（２０１）、レーザー距離センサによりセンサから障害物に対する方位と距離の計測を行い、計測値を取り込む（２０２）。ここでは、図４に示される（４０５）の位置においてロボットが（４０４）で示されるように１８０度の方向にレーザー距離センサによる走査を行い、（４０１）の斜線の領域で示される環境中の障害物に対して、（４０２）のように図中で太線で示される範囲が障害物として検出されたものとし、同様の計測をロボットが（４０３）の位置に移動して繰り返し行って、地図を画像として表示する例を示す。なお、図４に示す図示例で最上部における障害物に対して太線を付していないのはセンサから遠隔地のために省略している。

このようにして移動するロボット上のレーザー距離センサで取り込んだ計測値からグリッド画像を生成する（２０３）。ここでの計測値とは（４０４）で示されるように一定の角度の範囲を走査して得られた各方位における障害物までの距離の集合であるとする。この計測値をもとにグリッド画像はセンサの位置を基準とする座標系においてセンサの計測誤差をもとに決められたグリッド（図５に示す１つの枡目）を設定し、このグリッドに計測値から推定されるグリッドの状態を示す３種類の色を割り当てることによって生成される。

図５にグリッド画像生成（１０３）の処理（２０３）の例を示す。図５は（５０７）の位置にセンサが配置されている状態で１回の計測につき水平面の１８０度の範囲にレーザー光線（５０５）による走査を行うことで、走査した範囲内で計測された障害物の方位と距離にもとづいて画像としたものである。図５において（５０１）が実際の計測値（５０７と５０１を結ぶ直線５０５における方位と距離）であるとする。

グリッド画像の各グリッドは３値の状態を持ち、それぞれ占有グリッド（５０３）、非占有グリッド（５０４）、未知グリッド（５０２）の値を持つ。占有グリッドは障害物があることが判明している領域を示す。非占有グリッドは、センサ位置と占有グリッドを結ぶ直線（５０５）上のグリッドには障害物が無いと仮定して非占有グリッドとする。センサ位置と占有グリッドを結ぶ直線上で占有グリッドを基準として、センサから離れる方向にあるグリッドは障害物の有無が判明していない未知の領域であるため未知グリッドとして設定する。なお、グリッドの大きさはセンサの計測誤差より決定される。ここではセンサの計測誤差が前後左右方向に±ｄと仮定し、１つのグリッドの大きさを２ｄ×２ｄとする。これにより後述する画像の特徴点の照合時において誤差を考慮した照合を行うものとする。また、各グリッドは３値のいずれかの値（占有、非占有、未知）とともに（５０１）で示されるセンサの実測値（方位と距離）を保持するものとする。

次に、グリッド画像特徴点の抽出（１０４）を行う。図５はグリッド画像特徴点の抽出（１０４）の処理（２０４〜２０５）の例を示すものである。レーザー距離センサ値の入力（１０２）により入力された計測値から生成されるグリッド画像において、占有グリッドを中心とするウィンドウ（５０６）を設けてヒストグラム（占有、非占有、未知のそれぞれのグリッド数を示す数表）を作成し、これを特徴点とする。各特徴点のヒストグラムを比較し、占有グリッドが多い特徴点を選択し（２０４）、その内で未知グリッドが少ない特徴点を選択する（２０５）ことにより特徴点の抽出が行われる。次いで抽出された各特徴点のヒストグラム（後述する図６に示す表）と座標が記録される（２０６）。

図６は記録される特徴点のヒストグラムの値を表にしたものを示す。特徴点に固有の識別子として１〜５が割り振られたときの各特徴点のグリッドの状態の出現頻度を示す値が例として示されている。

次に、グリッド画像の特徴点照合（１０５）を行う。グリッド画像の特徴点照合は現在の計測により求められたグリッド画像と前回の計測にもとづくグリッド画像の２つのグリッド画像間において対応する特徴点の算出を行うものであり、図３に示される処理により行われる。ここでは、グリッド画像の特徴点照合（１０５）の処理の例として、図７を用いて図３の処理の説明を行う。図７の（Ａ）は図４の位置（４０５）にて環境を計測したときのグリッド画像であり、また、図７の（Ｂ）は図４の位置（４０３）にて計測したときのグリッド画像の例を示している。なお、レーザー距離センサを搭載したロボットは自分自身の絶対位置を認識できないということが前提であり、ロボットの移動前後において自分で観察できる風景が描ければ地図を生成できるのである。

まず、各グリッド画像において抽出された特徴点のうち２つを選択し、これらの特徴点を通る座標系（７０１）を設定する（３０１）。この座標系における他の特徴点の座標を求める（３０２）。同様に他のグリッド画像においても基準とする座標系を設定し（７０２）、他の特徴点の座標を求める。次に、基準とする座標系の原点と姿勢が一致するように各グリッド画像の特徴点を配置することを考える（３０３）。このとき、各グリッド画像の特徴点のヒストグラムと相対位置の比較を行う（３０４）。より具体的には、ヒストグラムにおける占有グリッドの数の差が閾値以下である特徴点（図７に示すウインドウを指す）を選択し、その相対距離（ウインドウの中心グリッド位置の相対距離）が閾値以下の場合に限り、特徴点が一致したものとしてカウントする。この処理を２つの特徴点からなる全ての組み合わせの座標系に対して行う（３０５）。

グリッド画像の特徴点照合（１０５）の処理において特徴点の一致率が最も多かったときに選択された座標系を記録しておき（３０６）、各画像の座標系同士の原点と軸の方向が一致する（図７に示す座標系７０１と７０２を重ね合わせる）ようにして、各画像を同じ座標系に配置する（２０８）。処理（２０８）による配置は、後述する処理（２１０）による画像間の相対位置と姿勢を精密に算出するための前処理（仮の位置合わせ処理）であり、処理（２０７）による相対位置・姿勢の値は最終的な画像間の相対位置・姿勢として採用しないため（最終的な画像間相対位置・姿勢として採用するのは、後述する処理（２１０）の値である）、処理（２０８）による画像の配置は処理（２０９）を行うための仮配置（仮の位置合わせ）である。

処理（２０８）により同一座標系に仮配置された画像間の重複領域を算出する（２０９）。図８に処理（２０８）により仮配置された画像とその重複領域（８０１）を示す。この重複領域（８０１）に含まれる各占有グリッドに対応する方位と距離の実測値によって画像間の相対位置・姿勢を精密に算出する。ここでの画像間の相対位置と姿勢はＩｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ（ＩＣＰ）により算出する（２１０）。

図９において、横軸を位置・姿勢の解とし、縦軸を誤差とする座標系（９０１）において誤差の評価関数（９０２）を仮定した場合、単純に最小二乗法などにより位置と姿勢の解を求めようとした場合に局所解（９０４）を解として算出してしまう可能性があるが、前述のようにグリッド画像同士を仮配置して重複領域の値のみを用いることにより（図８に示す重複領域８０１以外のウインドウの計測値を採用しないことにより）、余分な計測値を間引き、画像間の位置・姿勢の最適解（９０３）がより求めやすくなる。換言すると、重複領域を一応確保した場合には、図９の横軸のＯ点より左のＰ点で解を求めることとなり、局所解（９０４）に落ち込むことなく最適解（９０３）が求められ得る。

次いで、処理（２１０）により算出された画像間の相対位置・姿勢にもとづいて各画像が重複するように１枚の画像を生成し、これを環境地図画像とする（２１１）。続いて生成された環境地図画像を表示する（２１２）。新たにセンサ値の入力がなければ（２１３）、プログラムは終了する（２１３）。

次に、図１０を用いて本発明の地図画像生成装置に係る実施例の構成を説明する。本実施例は、プログラムにもとづいてデータの処理を行うメインプロセッサ（１００１）、プログラムとデータを保持するメモリ等の主記憶（１００９）とハードディスク等の補助記憶（１００８）、ディスプレイ（１００２）とグラフィックボード（１００３）、障害物までの方位・距離の計測値の入力（１０２）を行うためのレーザー距離センサ（１００４）とレーザー距離センサ制御ボード（１００５）、センサを移動させるための台車となるロボット（１００６）、ロボット制御ボード（１００７）を備える。

図１０に示す構成は、ハードウェア的には、プロセッサ、メモリ、その他のＬＳＩで実現でき、ソフトウェア的にはメモリにロードされたプログラムなどによって実現されるが、図１０にはそれらの連携によって実現される機能ブロックを記載している。したがって、これらの機能ブロックがハードウェアのみ、ソフトウェアのみ、またはそれらの組み合わせによっていろいろな形で実現できるものである。

主記憶（１００９）にて動作するプログラムとデータについて説明をする。オペレーティングシステム（１０１１）は、プログラムの実行等を管理する基本ソフトウェアである。方位と距離の計測値から環境地図画像を生成するプログラムとしては、全体の処理の流れを制御するメインプログラム（１０１２）、図１におけるレーザー距離センサ値取得（１０２）の処理に相当するレーザー距離センサ値取得プログラム（１０１３）、グリッド画像生成（１０３）の処理に相当するグリッド画像生成プログラム（１０１４）、グリッド画像の特徴点抽出（１０３）の処理に相当するグリッド画像特徴点抽出プログラム（１０１５）、グリッド画像の特徴点照合（１０５）の処理に相当するグリッド画像特徴点照合プログラム（１０１６）、グリッド画像間重複領域算出（１０６）の処理に相当するグリッド画像間重複領域算出プログラム（１０１７）、画像間相対位置と姿勢算出（１０７）の処理に相当する画像間相対位置・姿勢算出プログラム（１０１８）、環境地図画像生成（１０８）の処理に相当する環境地図画像生成プログラム（１０１９）、環境地図画像表示（１０９）の処理に相当する環境地図画像表示プログラム（１０２０）が存在する。

また、画像処理にあたって処理結果を保持する画像バッファ（１０２１）がある。また、補助記憶（１００８）には主記憶（１００９）に読み込まれる各種プログラム（１０１１〜１０２２）がプログラム（１０１０）として記憶されている。これらのプログラムは、メモリに読み込まれ、プロセッサによって実行されることによって処理が行われる。

各プログラムの処理について説明する。メインプログラム（１０１２）は図１〜図９に示したレーザー距離センサ値取得から環境地図画像の生成及び／又は表示までのプログラム全体の処理の流れを制御する。レーザー距離センサ値取得プログラム（１０１３）はレーザー距離センサ値取得（１０２）としてレーザー距離センサ制御ボード（１００７）とレーザー距離センサ（１００４）を制御することにより、レーザー距離センサにより得られる方位と距離の計測値を主記憶（１００９）のバッファ（１０２１）に取り込む（図２の２０２）。なお、バッファ（１０２１）は以下における各種プログラムの作業領域としても用いられる。

次に、グリッド画像生成プログラム（１０１４）はレーザー距離センサ値取得（１０２）により取り込まれた方位・距離計測値よりグリッド画像の生成を行い（図２の処理２０３）、グリッド画像特徴点抽出プログラム（１０１５）は画像内の占有グリッドに設けたウィンドウ内のヒストグラムを特徴点として抽出し、ヒストグラムと座標を記録する（図２の処理２０４〜２０６）。

グリッド画像特徴点照合プログラム（１０１６）はグリッド画像間において特徴点のヒストグラムと座標の比較にもとづいた照合を行う（図２の処理２０７）。より具体的には、グリッド画像において選択した２つの特徴点を基準とした座標系を設定し、その座標系における他の特徴点の座標とヒストグラムをグリッド画像間で比較していき、画像間におけるヒストグラムの差と相対位置の差が閾値以下であれば特徴が一致するとみなし、一致する特徴点の数をカウントする。これにより最も一致率が高くなるようなグリッド画像間の相対位置と姿勢関係を算出する。

グリッド画像間重複領域算出プログラム（１０１７）はグリッド画像特徴点照合プログラム（１０１６）により得られた画像の特徴点の一致率が最大となるときの相対位置と姿勢に従って各グリッド画像を共通の座標系に仮配置し、仮配置された画像間で重複する領域を算出する。画像間相対位置・姿勢算出プログラム（１０１８）により重複領域に含まれる各グリッド画像の占有グリッドの方位と距離の実測値を用いてグリッド画像間の相対位置と姿勢を精密に算出する。

環境地図画像生成プログラム（１０１９）は求められた相対位置と姿勢に従って各グリッド画像を共通の座標系に配置し、環境地図画像を生成する。次いで環境地図画像表示プログラム（１０２０）は環境地図画像生成プログラム（１０１９）により生成された環境地図画像を表示する（図２の処理２１２）。

なお、前述した本発明の実施例の機能を実現するソフトウェアのプログラムを記録した記録媒体を、システムあるいは装置に供給し、そのシステムあるいは装置のプロセッサが記録媒体に格納されたプログラムを読み出し実行することによっても、本発明の目的が達成されることはいうまでもない。

以上の説明においては、本発明の実施例として、特にレーザー距離センサを方位と距離の計測機器とする構成例を示したが、計測値を保存しているネットワーク上の記憶装置などから計測値を入力した場合でも、上述した構成例と同様の結果を得ることが可能である。また、複数回の環境の計測に伴うレーザー距離センサの移動方法についてはセンサをロボット以外の車両などに取り付けた場合、あるいはセンサを取り付けた三脚を人間が計測地点を変えるなどした場合においても類似した特徴が観測される範囲において計測地点を変えて計測を行うならば、同様の結果を得ることが可能である。また、本実施例では水平に１８０度の範囲内を走査するレーザー距離センサの例を示したが、走査範囲がより広い場合、また狭い場合においても同様の結果を得ることが可能である。

また、本発明の地図画像生成装置の産業応用の例としては、自動車や船舶などの移動物体に搭載されたレーザー距離センサにより撮影された複数回の方位と距離の計測値から、障害物が占有する領域を示す画像を生成し、１枚の環境地図の画像を自動的に生成及び／又は表示を行ったり、複数のレーザー距離センサを並べて得られる複数の画像から１枚の環境地図の画像を自動的に生成及び／又は表示することが可能となり、作業の省力化が期待できる。このように、本発明の実施例によれば、障害物の配置が未知の環境において、人力もしくは車両などの移動体によりレーザー距離センサを移動させながら一定の領域の計測を行っていくことにより、センサが移動した周囲の環境の地図を精度良く生成し、表示することが可能となる。

以上説明したように、本発明の特徴を取り纏めると、レーザー距離センサによる障害物までの方位と距離を複数回計測し、各計測により得られる障害物の位置を表す画像からヒストグラムによる特徴点を抽出し、各計測により生成される各画像の特徴点を計測前後の画像間で照合することにより画像間で共通する重複領域を算出し、この重複領域算出を元にして画像間の相対位置・姿勢を算出することによって、レーザー距離センサが移動した範囲の環境における障害物の位置を示す1枚の画像として生成し、表示するものである。

本発明の特徴について、敷衍して説明すると、障害物までの方位と距離の計測が可能なレーザー距離センサもしくはそれに類する機器、あるいは既に計測値が保存されている記憶装置を制御することにより計測値の入力を行い、入力された計測値から環境における障害物の占有位置を示す画像を生成し、複数回の計測に相当するそれぞれの画像における占有領域（占有グリッド）を中心とするある範囲（ウインドウ）を特徴点として抽出し、特徴点のヒストグラムと位置を記録し、特徴点のヒストグラムと特徴点同士の相対的な位置が類似するような特徴点を持つ画像間において、相対位置関係が保持されている特徴点のみを算出し、同じ特徴点を持つ画像間のおよその相対位置と姿勢を算出し、算出された相対位置と姿勢において画像同士を配置した場合に重複する領域を算出し、算出された重複領域に含まれる画像における占有領域（占有グリッド）のみを用いて各画像における占有領域（占有グリッド）間の距離の差の二乗が最小となるような相対位置と姿勢を精密に算出し、この相対位置と姿勢を画像間の相対位置と姿勢として、画像を重ね合わせて環境地図画像を生成し、表示するものである。

本発明の地図画像生成装置に係る実施例における処理の流れの概要を示す図である。 本実施例における全体の処理における詳細な流れを示す図である。 図２に示す全体の処理の流れの内でグリッド画像の特徴点照合に関する詳細な処理の流れを示す図である。 ロボットに搭載されたレーザ距離センサを用いて環境を走査している様子を示す図である。 図２に示す全体の処理の流れの内でグリッド画像生成に関わる処理とグリッド画像特徴点抽出に関する説明図である。 グリッド画像の特徴点のヒストグラムを表で示した図である。 図２に示す全体の処理の流れの内でグリッド画像特徴点照合に関する説明図である。 図２に示す全体の処理の流れの内でグリッド画像の仮配置と重複領域の算出に関する説明図である。 図８に示す仮配置したことによるグリッド画像間相対位置と姿勢算出時における効果を示す説明図である。 本発明の地図画像生成装置に係る実施例における全体構成を示す図である。

符号の説明

１０１ プログラムの開始処理
１０２ レーザー距離センサ値取得処理
１０３ グリッド画像生成処理
１０４ グリッド画像特徴点抽出処理
１０５ グリッド画像特徴点照合処理
１０６ グリッド画像間重複領域算出処理
１０７ グリッド画像間相対位置・姿勢算出処理
１０８ 環境地図画像生成処理
１０９ 環境地図画像表示処理
１１０ プログラムの終了処理
４０１ 環境中の障害物
４０２ 検出された障害物
４０３ 移動後のロボット位置
４０４ 走査範囲
４０５ 移動前のロボット位置
５０１ 実際の計測値
５０２ 未知グリッド
５０３ 占有グリッド
５０４ 非占有グリッド
５０５ レーザ光線
５０６ ウインドウ
５０７ センサ配置位置

Claims (4)

1. 障害物までの方位と距離を用いて前記距離センサ周囲の地図画像を生成する画像生成装置であって、
   前記距離センサを移動させて前記障害物までの方位と距離の計測値を前記移動の前後のそれぞれで取得し、
   前記計測値に基づいて前記距離センサの周囲における障害物の有無を、前記距離センサの分解能で決められたグリッド毎に割り当てられた占有（画像において障害物が占有）、非占有（画像において障害物が非占有）、未知（画像において障害物が占有しているかが未知）の状態によって表したグリッド画像を生成し、
   前記計測値でそれぞれ得られた各グリッド画像において、障害物で占有された状態のグリッドを中心として或る範囲を示すウインドウを設け、前記ウインドウ内のグリッドの状態を占有、非占有、未知で表した際の各状態の出現頻度をヒストグラムで表したときに、占有状態のグリッドが多くて障害物の有無が未知な状態のグリッドが少ないウインドウを特徴点として抽出し、
   現在の計測により求められたグリッド画像と前回の計測により求められたグリッド画像の２つのグリッド画像間において対応する前記特徴点の比較に際して、前記抽出された各グリッド画像の前記特徴点における占有グリッドの数と前記特徴点間の距離を各グリッド画像間で比較することで、各グリッド画像間の前記特徴点を照合し、
   前記照合の結果に基づいて特徴点の最も良く一致するグリッド画像間で重ね合わせてグリッド画像を仮配置し、
   前記仮配置したグリッド画像間において前記計測値によって前記グリッド画像間の相対位置と姿勢を演算して前記地図画像を生成する
   ことを特徴とする画像生成装置。
2. 障害物までの方位と距離を計測可能なレーザー距離センサの計測値から前記レーザー距離センサ周囲の地図の画像を生成する画像生成装置であって、
   前記障害物までの方位と距離の計測値を取得可能なレーザー距離センサ値取得手段と、
   前記レーザー距離センサ値取得手段から取得された計測値に基づいて前記距離センサの周囲における障害物の有無を、前記距離センサの分解能で決められたグリッド毎に割り当てられた占有（画像において障害物が占有）、非占有（画像において障害物が非占有）、未知（画像において障害物が占有しているかが未知）の状態によって表したグリッド画像を生成するグリッド画像生成手段と、
   前記計測値でそれぞれ得られた各グリッド画像において、障害物で占有された状態のグリッドを中心として或る範囲を示すウインドウを設け、前記ウインドウ内のグリッドの状態を占有、非占有、未知で表した際の各状態の出現頻度をヒストグラムで表したときに、占有状態のグリッドが多くて障害物の有無が未知な状態のグリッドが少ないウインドウを特徴点として抽出するグリッド画像特徴点抽出手段と、
   現在の計測により求められたグリッド画像と前回の計測により求められたグリッド画像の２つのグリッド画像間において対応する前記特徴点の比較に際して、前記抽出された各グリッド画像の前記特徴点における占有グリッドの数と前記特徴点間の距離を各グリッド画像間で比較することで、各グリッド画像間の前記特徴点を照合するグリッド画像特徴点照合手段と、
   前記グリッド画像特徴点照合手段で得られた照合結果に基づきグリッド画像間の前記特徴点が最も良く一致するときの相対位置と姿勢に従って各グリッド画像を共通の座標系に重ね合わせて仮配置し、仮配置された各グリッド画像間で重複する領域を算出するグリッド画像間重複領域算出手段と、
   前記グリッド画像間重複領域算出手段で得られた前記重複領域に含まれる前記レーザー距離センサの計測値のみを用いてグリッド画像間の相対位置と姿勢を演算するグリッド画像相対位置・姿勢演算手段と、
   前記グリッド画像相対位置・姿勢演算手段で得られたグリッド画像間の相対位置と姿勢に従って複数の画像を１枚の画像に合成する地図画像生成手段と、を有する
   ことを特徴とする画像生成装置。
3. レーザー距離センサによる複数回の計測値から環境における障害物の有無を示す地図を画像として生成する画像生成方法であって、
   レーザーにより前記障害物までの方位と距離の計測値を取得する取得工程と、
   前記取得工程から取得された前記計測値に基づいて前記レーザー距離センサの周囲における障害物の有無を、前記距離センサの分解能で決められたグリッド毎に割り当てられた占有（画像において障害物が占有）、非占有（画像において障害物が非占有）、未知（画像において障害物が占有しているかが未知）の状態によって表したグリッド画像を生成する生成工程と、
   前記生成工程でそれぞれ得られた各グリッド画像において、障害物で占有された状態のグリッドを中心として或る範囲を示すウインドウを設け、前記ウインドウ内のグリッドの状態を占有、非占有、未知で表した際の各状態の出現頻度をヒストグラムで表したときに、占有状態のグリッドが多くて障害物の有無が未知な状態のグリッドが少ないウインドウを特徴点として抽出する抽出工程と、
   現在の計測により求められたグリッド画像と前回の計測により求められたグリッド画像の２つのグリッド画像間において対応する前記特徴点の比較に際して、前記抽出工程で得られた各グリッド画像の前記特徴点における占有グリッドの数と前記特徴点間の距離を各グリッド画像間で比較し照合することで、各グリッド画像間で一致する前記特徴点の識別を行う識別工程と、
   前記識別工程で得られた照合結果に基づきグリッド画像間の前記特徴点が最も良く一致するときの相対位置と姿勢に従って各グリッド画像を共通の座標系に重ね合わせて仮配置し、仮配置された各グリッド画像間で重複する領域を算出する算出工程と、
   前記算出工程で得られた前記重複領域に含まれる前記レーザー距離センサの計測値のみを用いてグリッド画像間の相対位置と姿勢を演算する演算工程と、
   前記演算工程で得られたグリッド画像間の相対位置と姿勢に従って複数の画像を１枚の画像に合成する生成工程と、からなる
   ことを特徴とする画像生成方法。
4. レーザーにより障害物までの方位と距離を計測可能なセンサの計測値からセンサ周囲の地図の画像を生成するコンピュータのプログラムであって、
   前記レーザーにより前記障害物までの方位と距離の計測値を取得する取得工程と、
   前記取得工程から取得された前記計測値に基づいて前記レーザー距離センサの周囲における障害物の有無を、前記距離センサの分解能で決められたグリッド毎に割り当てられた占有（画像において障害物が占有）、非占有（画像において障害物が非占有）、未知（画像において障害物が占有しているかが未知）の状態によって表したグリッド画像を生成する生成工程と、
   前記生成工程でそれぞれ得られた各グリッド画像において、障害物で占有された状態のグリッドを中心として或る範囲を示すウインドウを設け、前記ウインドウ内のグリッドの状態を占有、非占有、未知で表した際の各状態の出現頻度をヒストグラムで表したときに、占有状態のグリッドが多くて障害物の有無が未知な状態のグリッドが少ないウインドウを特徴点として抽出する抽出工程と、
   現在の計測により求められたグリッド画像と前回の計測により求められたグリッド画像の２つのグリッド画像間において対応する前記特徴点の比較に際して、前記抽出工程で得られた各グリッド画像の前記特徴点における占有グリッドの数と前記特徴点間の距離を各グリッド画像間で比較し照合することで、各グリッド画像間で一致する前記特徴点の識別を行う識別工程と、
   前記識別工程で得られた照合結果に基づきグリッド画像間の前記特徴点が最も良く一致するときの相対位置と姿勢に従って各グリッド画像を共通の座標系に重ね合わせて仮配置し、仮配置された各グリッド画像間で重複する領域を算出する算出工程と、
   前記算出工程で得られた前記重複領域に含まれる前記レーザー距離センサの計測値のみを用いてグリッド画像間の相対位置と姿勢を演算する演算工程と、
   前記演算工程で得られたグリッド画像間の相対位置と姿勢に従って複数の画像を１枚の画像に合成する生成工程と、を前記コンピュータに実行させる
   ことを特徴とする画像生成プログラム。

Images