JP6601178B2 - 局所地図作成装置および局所地図作成方法 - Google Patents

Description

本発明は、局所地図作成装置および局所地図作成方法に関し、とくにレーザレンジファインダ（ＬＲＦ）を用いて周辺環境を計測しながら動作するロボットについて、その周辺環境の地図作成を行う局所地図作成装置および局所地図作成方法に関する。

従来、ロボット等の周辺環境を、レーザレンジファインダ（ＬＲＦ）を用いて距離計測して局所地図を生成し、その局所地図を用いて全体の大域地図を生成するものが公知となっている（例えば、下記特許文献１〜９および非特許文献１等参照）。
このうち、局所地図を作る方法については下記非特許文献１に開示された方法が知られている。

図１に示すように、レーザレンジファインダ１１は、水平面に一定角度Δθの間隔（角度分解能：例えばΔθ＝１度やΔθ＝０．２５度など）でレーザー光１１ａを照射し、その反射光を検出して障害物２ａや壁２ｂなどの物体（以下、総称する場合は物体２と称する）までの距離を計測している。レーザレンジファインダ１１を搭載し、その距離データを利用して周辺環境を理解して移動するロボット１では、この距離データを地図化する必要がある。

非特許文献１では、図８（ａ）に示すようにレーザレンジファインダ１１などの距離センサによって得られる距離データｒを基に物体２までの距離を取得し、図８（ｂ）に示すように物体２があるところを「１．０」、物体２がない自由空間を「０．０」、未観測または不明を中間の「０．５」の確率で表し、これをグリッド状の確率的障害物地図に変換している。ここでレーザレンジファインダ１１から照射される各レーザー光１１ａには間隔があり連続的ではない。そのため角度分解能をΔθとするとき、そのΔθ分の隙間を埋める必要があり、図８（ｃ）に示すように、一つの距離データｒに対して扇形の領域Ｅで、物体２の有無を表している。なお、図８（ｃ）に示すＦは、未観測領域を示している。

この扇形の領域Ｅを図９に示すようにグリッド状の地図に障害物の確率分布として当てはめ、グリッド状の地図４を作成する。なお、図９に示す例では、未観測または不明の部分をグレーの領域Ａ、障害物がない部分を黒の領域Ｂ、障害物がある部分を白の領域Ｃで示している。
これをすべての方向の距離データに適用し、確率的障害物地図を作成する。これが周辺環境の「局所地図」となる。

また、ロボット１が走行する範囲全体の地図である「大域地図」は、上記の局所地図をつなぎ合せて作成する。
このつなぎ合わせでは、複数の局所地図を、それぞれロボット１の移動した量だけずらして重ね合わせることになるが、このロボット１の移動量に誤差が含まれる場合、精度のよい大域地図が生成できない。

従来、パーティクルフィルタを用いた方法では、各パーティクルに位置・姿勢、大域地図の情報をそれぞれ持たせ、パーティクル毎にロボット１の移動・姿勢変化量（オドメトリからの推定量など）に加え、ランダムなノイズと局所地図を与えて、大域地図と局所地図のマッチングを行う。このマッチングでは、下式(1)により間違いの割合ｄを計算する。

そして、上記間違いの割合の値ｄからパーティクル毎の尤度ｌ_pに対して、下式(2)として尤度を更新する。

この尤度の値の大きいパーティクルが最も良い大域地図と位置姿勢を保持しているということになる。

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

根岸、三浦、白井著、「全方位ステレオとレーザレンジファインダの統合による移動ロボットの地図生成」、日本ロボット学会誌、２００３年、Ｖｏｌ．２１、Ｎｏ．６、ｐ．６９０−６９６

しかしながら、非特許文献１の方法では、図１０に示すように壁２ｂに対してレーザレンジファインダ１１で距離を計測した場合、壁２ｂに対するレーザー光１１ａの角度Θが浅くなるにつれ隣り合う障害物計測点Ｐの間隔が空いてしまい、図１１に破線で囲んで示すように実際には壁２ｂがあるのにもかかわらず「物体なし」となる部分が多くなるほか、図１１に一点鎖線で囲んで示すように、本来障害物２ｂがない部分に障害物があるという結果になるなど、実際の周辺環境と異なる部分がでてきてしまう。

このような地図を重ね合わせて大域地図を構成しようとしても精度良く構築できないことがある。これは、以下の点により上式(1)で得られる間違いの割合ｄの値が正しく得られなくなるためである。
１．本来障害物がない部分が「物体あり」となった局所地図が、過去に大域地図に反映されてしまったこと
２．計測点が飛び飛びとなる部分のため、計測点間が「物体なし」となる部分が多くなること

このようなことから本発明は、レーザレンジファインダのレーザー光の障害物に対する照射角度によらず、「物体なし」の領域及び「物体あり」の領域を高精度に描画し、大域地図を高精度に生成することができる局所地図作成装置および局所地図作成方法を提供することを目的とする。

上記の課題を解決するための第１の発明に係る局所地図作成装置は、
一定角度間隔で放射状に距離を計測する距離センサから距離データを入力する距離データ入力部と、
前記距離データに基づいて隣り合う二つの計測点間の距離を計算する二点間距離計算部と、
前記二つの計測点間の距離に基づいて隣り合う二つの計測点間に物体が存在する確率を計算する二点間確率計算部と、
前記距離データに基づいて物体の存在が不明な領域を予め設定した「不明領域」に対応する輝度で描画し、前記距離センサから前記計測点までの間を予め設定した「物体なし領域」に対応する輝度で描画する一方、隣り合う二つの計測点間を「物体あり領域」として前記確率に基づく値に応じた輝度で描画してグリッド状の局所地図を生成する局所地図生成部と
を備えることを特徴とする。

上記の課題を解決するための第２の発明に係る局所地図作成装置は、
前記局所地図生成部が、前記確率に基づき、隣り合う二つの前記計測点間の距離が短いほど前記「物体あり領域」に近い輝度で隣り合う二つの計測点間を描画し、当該二点間の距離が長いほど前記「不明領域」に近い輝度で隣り合う二つの計測点間を描画する
ことを特徴とする。

上記の課題を解決するための第３の発明に係る局所地図作成方法は、
一定角度間隔で放射状に距離を計測する距離センサから距離データを入力する第一の工程と、
前記距離データに基づいて隣り合う二つの計測点間の距離を計算する第二の工程と、
前記二つの計測点間の距離に基づいて隣り合う二つの計測点間に物体が存在する確率を計算する第三の工程と、
前記距離データに基づいて物体の存在が不明な領域を予め設定した「不明領域」に対応する輝度で描画し、前記距離センサから前記計測点までの間を予め設定した「物体なし領域」に対応する輝度で描画する一方、隣り合う二つの計測点間を「物体あり領域」として前記確率に基づく値に応じた輝度で描画してグリッド状の局所地図を生成する第四の工程と
を備えることを特徴とする。

上記の課題を解決するための第４の発明に係る局所地図作成方法は、
前記第四の工程では、前記確率に基づき、当該二点間の距離が短いほど前記「物体あり領域」に近い輝度で隣り合う二つの計測点間を描画し、当該二点間の距離が長いほど前記「不明領域」に近い輝度で隣り合う二つの計測点間を描画する
ことを特徴とする。

本発明によれば、レーザレンジファインダのレーザー光の障害物に対する照射角度によらず、「物体なし」の領域及び「物体あり」の領域を高精度に描画し、大域地図を高精度に生成することができる。

レーザレンジファインダにより物体までの距離を計測する例を示す説明図である。 本発明の実施例に係る局所地図作成部の構成を示すブロック図である。 本発明の実施例に係る局所地図作成の流れを示すフローチャートである。 局所地図を初期化した状態を示す説明図である。 局所地図に「物体なし」の領域を描画した例を示す説明図である。 局所地図に「物体あり」の直線を描画した例を示す説明図である。 分岐のある通路を局所地図化した例を示す説明図である。 図８（ａ）は距離データの一例を示す説明図、図８（ｂ）は物体の有無を確率で示した例を示す説明図、図８（ｃ）は一つの距離データを従来の手法によりモデル化した例を示す説明図である。 図８に示すモデルからグリッド状の地図を作成した例を示す説明図である。 壁に対する距離計測の例を示す説明図である。 図１０から得られる従来のグリッド状地図を示す説明図である。

以下、図面を参照しつつ本発明に係る局所地図作成装置および局所地図作成方法について説明する。

図１から図７を用いて本発明の一実施例に係る局所地図作成装置および局所地図作成方法の詳細を説明する。

図１に示すように、本実施例に係る局所地図作成装置１２は、距離センサとしてのレーザレンジファインダ（LRF）１１を用いて周辺環境を計測しながら動作するロボット１に搭載され、レーザレンジファインダ１１により取得した距離データを用いてロボット１の周辺環境の地図作成を行うものである。

レーザレンジファインダ１１は、水平面に一定角度Δθの間隔（角度分解能：例えばΔθ＝１度やΔθ＝０．２５度など）でレーザー光１１ａを照射し、このレーザー光１１ａの障害物２ａや壁２ｂ等（物体２）による反射光を検出して物体２までの距離を計測する。なお、図１中に示す点Ｐはレーザレンジファインダ１１のレーザー光１１ａによる障害物計測点である。

局所地図作成部１２は、図２に示すように、ＬＲＦデータ入力部１２ａ、二点間距離計算部１２ｂ、二点間確率計算部１２ｃ、局所地図生成部１２ｄ、および記憶部１２ｅを備えている。

ＬＲＦデータ入力部１２ａは、レーザレンジファインダ１１から距離データを入力し、記憶部１２ｅに保管する。
二点間距離計算部１２ｂは、記憶部１２ｅから読み出した距離データの隣り合う二点間の距離を計算し、結果を記憶部１２ｅに保管する。
二点間確率計算部１２ｃは、記憶部１２ｅから読み出した二点間距離を基に、その二点間の「物体あり」の確率を計算する。求めた確率は、二点間確率として記憶部１２ｅに保管する。

局所地図生成部１２ｄは、距離データと二点間確率を基に、グリッド状の局所地図データを生成する部分であり、「物体なし」領域描画部１２ｄａと、「物体あり」直線描画部１２ｄｂとを備えている。「物体なし」領域描画部１２ｄａは、初期化されたグリッド状の局所地図データに対し、「物体なし」領域を描画する。「物体あり」直線描画部１２ｄｂは、グリッド状の局所地図データに対し、距離データと二点間確率を用いて、「物体あり」を示す直線を二点間確率の値に応じた輝度で描画する。生成された局所地図は記憶部１２ｅに保管する。
記憶部１２ｅは、距離データ、二点間距離、二点間確率、局所地図等を保管する。

次に、図３を用いて局所地図作成部１２による局所地図作成の流れを説明する。
図３に示すように、局所地図作成部１２では、まず、ＬＲＦデータ入力部１２ａによりレーザレンジファインダ１１から当該レーザレンジファインダ１１によって取得した距離データを入力する（ステップＳ１）。
続いて、二点間距離計算部１２ｂにより、記憶部１２ｅから読み出した距離データに基づいて距離データの隣り合う二点間の距離を算出する（ステップＳ２）。すなわち、角度分解能Δθのレーザレンジファインダ１１で取得した計測データのうち、それぞれ物体２を計測した隣り合う距離データｒ_i，ｒ_i+1に対し、二点間の距離ｌ_iを下式(3)により求める。

なお、角度分解能Δθが十分に小さい場合は、cosΔθ→１より、下式(4)により二点間の距離を求めることができる。

ステップＳ２に続いては、ステップＳ２で求めた二点間の距離ｌ_iから二点間の「物体あり」の確率を算出する（ステップＳ３）。すなわち、上記ステップＳ２で求めた距離ｌ_iが短いほど「物体あり」（１．０）に近く、距離が長いほど「不明領域」の輝度（０．５）に近くなるように、以下の式(5)により二点間を接続する線の確率値ｐ_iを求める。

続いて、局所地図生成部１２ｄにより局所地図の生成を行う（ステップＳ４）。局所地図の生成は、図４に示すように局所地図全体を「不明領域Ａ」として輝度０．５で初期化した状態で、図５に示すように、「物体なし」領域描画部１２ｄａにより障害物がないところ、つまり隣り合う二つの計測点とレーザレンジファインダ１１の原点からなる三角形を「物体なし領域Ｂ」として輝度０．０で描画する（ステップＳ４ａ）。その後、図６に示すように、「物体あり」直線描画部１２ｄｂにより障害物があるところ、つまり隣り合う障害物計測点間を距離に基づいて算出した確率値ｐ_iに応じた部分を「物体あり領域Ｃ」として０．５より大きく１．０未満の輝度で直線状に描画する（ステップＳ４ｂ）。
以上により、局所地図の生成が終了する。

ここで、図７は、分岐などで手前の物体と奥の物体との間で距離が大きく変化する場合の局所地図の例である。図７に破線で囲んで示すように、本実施例の局所地図作成装置を用いると、二点間の距離が大きく変化する部分は、ほぼ「不明領域Ａ」（輝度０．５）に近いグレーとなっていることが分かる。

上述した本実施例に係る局所地図作成装置および局所地図作成方法によれば、本来障害物がない部分を正確に「物体なし」と描画することができ、計測点間の距離が長い場合にはその長さに応じた輝度で「物体あり」を描画するため、上式(1)の間違いの割合をより正確に算出できることになる。
すなわち、レーザレンジファインダ１１のレーザー光１１ａの障害物２に対する照射角度Θが浅い場合であっても、「物体なし」の領域及び「物体あり」の領域を高精度に描画することができ、よって、大域地図を高精度に生成することが可能となる。
なお、上述した実施例では「不明領域Ａ」を輝度０．５で描画し、「物体なし領域Ｂ」を輝度０．０で描画し、「物体あり領域Ｃ」を０．５より大きく１．０未満の輝度で描画する例を示したが、例えば、「不明領域Ａ」及び「物体なし領域Ｂ」を予め設定された異なる色で描画し、「物体あり領域Ｃ」を二点間距離が小さいほど「物体あり領域Ｃ」に近く、二点間距離が大きいほど「不明領域Ａ」に近い色で描画するようにしてもよい。

本発明は、局所地図作成装置および局所地図作成方法に適用することができる。

１ ロボット
２ 物体
２ａ 障害物
２ｂ 壁
３ 局所地図
１１ レーザレンジファインダ（ＬＲＦ）
１１ａ レーザー光
１２ 局所地図作成部
１２ａ ＬＲＦデータ入力部
１２ｂ 二点間距離計算部
１２ｃ 二点間確率計算部
１２ｄ 局所地図生成部
１２ｄａ 「物体なし」領域描画部
１２ｄｂ 「物体あり」直線描画部
１２ｅ 記憶部
Ａ 不明領域
Ｂ 物体なし領域
Ｃ 物体あり領域
Ｄ 距離データ
Ｅ 扇形領域
Ｆ 未観測領域
Ｐ 障害物計測点

Claims (4)

1. 一定角度間隔で放射状に距離を計測する距離センサから距離データを入力する距離データ入力部と、
   前記距離データに基づいて隣り合う二つの計測点間の距離を計算する二点間距離計算部と、
   前記二つの計測点間の距離に基づいて隣り合う二つの計測点間に物体が存在する確率を計算する二点間確率計算部と、
   前記距離データに基づいて物体の存在が不明な領域を予め設定した「不明領域」に対応する輝度で描画し、前記距離センサから前記計測点までの間を予め設定した「物体なし領域」に対応する輝度で描画する一方、隣り合う二つの計測点間を「物体あり領域」として前記確率に基づく値に応じた輝度で描画してグリッド状の局所地図を生成する局所地図生成部と
   を備えることを特徴とする局所地図作成装置。
2. 前記局所地図生成部が、前記確率に基づき、隣り合う二つの前記計測点間の距離が短いほど前記「物体あり領域」に近い輝度で隣り合う二つの計測点間を描画し、当該二点間の距離が長いほど前記「不明領域」に近い輝度で隣り合う二つの計測点間を描画する
   ことを特徴とする請求項１記載の局所地図作成装置。
3. 一定角度間隔で放射状に距離を計測する距離センサから距離データを入力する第一の工程と、
   前記距離データに基づいて隣り合う二つの計測点間の距離を計算する第二の工程と、
   前記二つの計測点間の距離に基づいて隣り合う二つの計測点間に物体が存在する確率を計算する第三の工程と、
   前記距離データに基づいて物体の存在が不明な領域を予め設定した「不明領域」に対応する輝度で描画し、前記距離センサから前記計測点までの間を予め設定した「物体なし領域」に対応する輝度で描画する一方、隣り合う二つの計測点間を「物体あり領域」として前記確率に基づく値に応じた輝度で描画してグリッド状の局所地図を生成する第四の工程と
   を備えることを特徴とする局所地図作成方法。
4. 前記第四の工程では、前記確率に基づき、当該二点間の距離が短いほど前記「物体あり領域」に近い輝度で隣り合う二つの計測点間を描画し、当該二点間の距離が長いほど前記「不明領域」に近い輝度で隣り合う二つの計測点間を描画する
   ことを特徴とする請求項３記載の局所地図作成方法。