(57)【要約】【課題】最適な移動経路を設定すること。【解決手段】移動体制御装置は、移動体周囲の障害物に対する距離点データを取得する距離点取得手段と、距離点取得手段により取得された障害物の距離点データに基づいて、障害物の外形を示すポリゴンマップを生成するポリゴンマップ生成手段と、移動体の移動経路を移動体の移動環境を示す格子状のグリッドマップ上で算出する経路算出手段と、経路算出手段により算出された移動経路を修正する経路修正手段と、を備えている。経路修正手段は、経路算出手段により算出された移動経路上で、ポリゴンマップ生成手段により生成された障害物のポリゴンマップに基づいて通過不能と判断した移動経路の経路部分に対して、距離点取得手段により距離点データを取得し、取得した距離点データを記録する。【選択図】図１
2014123200.tif 000002
【特許請求の範囲】
【請求項１】
移動体周囲の障害物に対する距離点データを取得する距離点取得手段と、
前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成するポリゴンマップ生成手段と、
前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出する経路算出手段と、
前記経路算出手段により算出された移動経路を修正する経路修正手段と、を備え、
前記経路修正手段は、前記経路算出手段により算出された移動経路上で、前記ポリゴンマップ生成手段により生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に対して、前記距離点取得手段により距離点データを取得し、該取得した距離点データを記録する、ことを特徴とする移動体制御装置。
【請求項２】
請求項１記載の移動体制御装置であって、
前記経路修正手段は、前記経路算出手段により算出された移動経路で通過不能と判断した経路部分があるとき、該経路部分に対して記録されている前記距離点データと、前記距離点取得手段により再度取得された前記経路部分に対する距離点データと、を比較する、ことを特徴とする移動体制御装置。
【請求項３】
請求項２記載の移動体制御装置であって、
前記経路修正手段は、前記経路部分に記録されている距離点データと、前記距離点取得手段により再度取得された経路部分に対する距離点データと、が一致するとき、前記経路部分を通過不能な禁止領域として確定する、ことを特徴とする移動体制御装置。
【請求項４】
請求項２又は３記載の移動体制御装置であって、
前記経路修正手段が、前記経路部分に記録されている距離点データと、前記距離点取得手段により再度取得された経路部分に対する距離点データと、が一致しないと判断したとき、前記経路算出手段は、前記移動体の移動経路を再計算する、ことを特徴とする移動体制御装置。
【請求項５】
請求項１乃至４のうちいずれか１項記載の移動体制御装置であって、
前記経路修正手段は、前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記通過不能と判断した経路部分において動的障害物が存在すると判断したとき、所定時間経過後、前記移動経路の再修正を行う、ことを特徴とする移動体制御装置。
【請求項６】
請求項１乃至５のうちいずれか１項記載の移動体制御装置であって、
前記経路修正手段は、前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記通過不能と判断した経路部分において静的障害物のみが存在すると判断したとき、該経路部分に対して通過不能を示す通過不可ポテンシャルを設定する、ことを特徴とする移動体制御装置。
【請求項７】
請求項６記載の移動体制御装置であって、
前記経路修正手段は、前記経路部分に対する通過不可ポテンシャルの重みを時間経過と共に減少させ、前記経路部分を移動経路として設定可能にする、ことを特徴とする移動体制御装置。
【請求項８】
請求項５記載の移動体制御装置であって、
前記経路修正手段は、前記移動経路の再修正を行った結果、前記通過不能と判断した経路部分を再度、通過不能と判断したとき、該経路部分に対して、前記距離点取得手段により距離点データを取得し、該取得した距離点データを記録する、を特徴とする移動体制御装置。
【請求項９】
請求項５記載の移動体制御装置であって、
前記経路修正手段は、前記移動経路の再修正を行った結果、前記通過不能と判断した経路部分を、通過可能と判断したとき、該再修正後の移動経路を最終的な移動経路として設定する、ことを特徴とする移動体制御装置。
【請求項１０】
請求項１乃至９のうちいずれか１項記載の移動体制御装置であって、
前記経路修正手段により修正された移動経路に従って、前記移動体の移動を制御する移動制御手段を更に備える、ことを特徴とする移動体制御装置。
【請求項１１】
移動体周囲の障害物に対する距離点データを取得するステップと、
前記取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成するステップと、
前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出するステップと、
前記算出された移動経路を修正するステップと、
前記算出された移動経路上で、前記生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に対して、前記距離点データを取得し、該取得した距離点データを記録するステップと、を含むことを特徴とする移動体制御方法。
【請求項１２】
移動体周囲の障害物に対する距離点データを取得する処理と、
前記取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成する処理と、
前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出する処理と、
前記算出された移動経路上で、前記生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に対して、前記距離点データを取得し、該取得した距離点データを記録する処理と、
をコンピュータに実行させることを特徴とする制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の移動経路を算出し、その移動経路に従って移動体の移動を制御する移動体制御装置、移動体制御方法及び制御プログラムに関するものである。
【背景技術】
【０００２】
近年、計測センサなどを用いて移動体周囲の障害物を検出して移動経路を設定し、その移動経路に従って自律的に移動する様々な自律移動体が提案されている。例えば、３次元地図情報を生成し、生成した３次元地図情報を用いて障害物を回避しつつ移動を行う自律移動体が知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００６−２３６０９８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記特許文献１に示す自律移動体においては、移動経路上の障害物を検出しグリッドマップに設定された場合、次回、同一の経路を移動する場合もその障害物を回避する移動経路を設定することとなる。一方で、障害物には、壁などの定常的に固定された静的障害物と、椅子などの移動させることが可能な準動的障害物と、人など移動する動的障害物が存在する。このため、例えば、一定時間経過後には、障害物の種類によってはその障害物が移動し回避の必要が無くなる可能性もある。したがって、上記のように、障害物を常に回避する移動経路を設定した場合に、無駄に迂回する経路が設定されるため、最適に移動経路が設定できない可能が生じる。
【０００５】
本発明は、このような問題点を解決するためになされたものであり、最適な移動経路を設定することができる移動体制御装置、移動体制御方法及び制御プログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、移動体周囲の障害物に対する距離点データを取得する距離点取得手段と、前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成するポリゴンマップ生成手段と、前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出する経路算出手段と、前記経路算出手段により算出された移動経路を修正する経路修正手段と、を備え、前記経路修正手段は、前記経路算出手段により算出された移動経路上で、前記ポリゴンマップ生成手段により生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に対して、前記距離点取得手段により距離点データを取得し、該取得した距離点データを記録する、ことを特徴とする移動体制御装置である。
この一態様において、前記経路修正手段は、前記経路算出手段により算出された移動経路で通過不能と判断した経路部分があるとき、該経路部分に対して記録されている前記距離点データと、前記距離点取得手段により再度取得された前記経路部分に対する距離点データと、を比較してもよい。
この一態様において、前記経路修正手段は、前記経路部分に記録されている距離点データと、前記距離点取得手段により再度取得された経路部分に対する距離点データと、が一致するとき、前記経路部分を通過不能な禁止領域として確定してもよい。
この一態様において、前記経路修正手段が、前記経路部分に記録されている距離点データと、前記距離点取得手段により再度取得された経路部分に対する距離点データと、が一致しないと判断したとき、前記経路算出手段は、前記移動体の移動経路を再計算してもよい。
この一態様において、前記経路修正手段は、前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記通過不能と判断した経路部分において動的障害物が存在すると判断したとき、所定時間経過後、前記移動経路の再修正を行ってもよい。
この一態様において、前記経路修正手段は、前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記通過不能と判断した経路部分において静的障害物のみが存在すると判断したとき、該経路部分に対して通過不能を示す通過不可ポテンシャルを設定してもよい。
この一態様において、前記経路修正手段は、前記経路部分に対する通過不可ポテンシャルの重みを時間経過と共に減少させ、前記経路部分を移動経路として設定可能にしてもよい。
この一態様において、前記経路修正手段は、前記移動経路の再修正を行った結果、前記通過不能と判断した経路部分を再度、通過不能と判断したとき、該経路部分に対して、前記距離点取得手段により距離点データを取得し、該取得した距離点データを記録してもよい。
この一態様において、前記経路修正手段は、前記移動経路の再修正を行った結果、前記通過不能と判断した経路部分を、通過可能と判断したとき、該再修正後の移動経路を最終的な移動経路として設定してもよい。
この一態様において、前記経路修正手段により修正された移動経路に従って、前記移動体の移動を制御する移動制御手段を更に備えていてもよい。
他方、上記目的を達成するための本発明の一態様は、移動体周囲の障害物に対する距離点データを取得するステップと、前記取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成するステップと、前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出するステップと、前記算出された移動経路を修正するステップと、前記算出された移動経路上で、前記生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に対して、前記距離点データを取得し、該取得した距離点データを記録するステップと、を含むことを特徴とする移動体制御方法であってもよい。
さらに、上記目的を達成するための本発明の一態様は、移動体周囲の障害物に対する距離点データを取得する処理と、前記取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成する処理と、前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出する処理と、前記算出された移動経路上で、前記生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に対して、前記距離点データを取得し、該取得した距離点データを記録する処理と、をコンピュータに実行させることを特徴とする制御プログラムであってもよい。
【発明の効果】
【０００７】
本発明によれば、最適な移動経路を設定することができる移動体制御装置、移動体制御方法及び制御プログラムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施の形態に係る移動体制御装置の概略的なシステム構成を示すブロック図である。
【図２】（ａ）障害物のポリゴンマップの一例を示す図である。（ｂ）図２（ａ）に示すポリゴンマップに対応するグリッドマップを示す図である。
【図３】障害物及び移動経路が設定されたグリッドマップの一例を示す図である。
【図４】（ａ）移動経路が滑らかな曲率変化となるように修正する方法を示す図である。（ｂ）移動経路が滑らかな曲率変化となるように修正する方法を示す図である。（ｃ）移動経路が滑らかな曲率変化となるように修正する方法を示す図である。
【図５】本発明の一実施の形態に係る移動体制御方法のフローを示すフローチャートである。
【図６】袋小路の問題を説明するための図である。
【図７】（ａ）動的障害物の一例を示す図である。（ｂ）準動的障害物の一例を示す図である。（ｃ）静的障害物の一例を示す図である。
【図８】本発明の一実施の形態に係る移動体制御装置による経路計画方法のフローを示すフローチャートである。
【図９】本発明の一実施の形態に係る移動体制御装置による経路計画方法のフローを示すフローチャートである。
【発明を実施するための形態】
【０００９】
以下、図面を参照して本発明の実施の形態について説明する。
本発明の一実施の形態に係る移動体制御装置は、自律的に移動する自律移動体に搭載されており、自律移動体の移動経路を算出し、算出した移動経路に従って自律移動体の移動を制御する。なお、自律移動体としては、例えば、二足歩行ロボット、車輪駆動型ロボットなどの自律移動が可能な任意の自律型移動体に適用可能である。
【００１０】
図１は、本発明の一実施の形態に係る移動体制御装置の概略的なシステム構成を示すブロック図である。本実施の形態に係る移動体制御装置１は、距離センサ２と、ポリゴンマップ生成部３と、経路算出部４と、経路修正部５と、移動制御部６と、自己位置推定部７と、を備えている。
【００１１】
なお、移動体制御装置１は、例えば、演算処理、制御処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。
【００１２】
距離センサ２は、距離点取得手段の一具体例であり、自律移動体に設けられており、自律移動体の周囲に存在する障害物の３次元点群データを取得する。
【００１３】
ここで、距離センサ２は、平面検出処理を行うことで、取得した３次元点群データのうち、自律移動体が移動する平面（床面）を除去する。これにより、距離センサ２は障害物のみの３次元点群データを取得することができる。なお、距離センサ２の視野が狭い場合は、過去の数フレームを重畳させて視野を拡大してもよい。
【００１４】
距離センサ２は、例えば、超音波センサ、レーザセンサ、３次元画像カメラなどで構成されている。距離センサ２は取得した３次元点群データをポリゴンマップ生成部３、経路算出部４及び経路修正部５に対して出力する。
【００１５】
ポリゴンマップ生成部３は、ポリゴンマップ生成手段の一具体例であり、距離センサ２により取得された３次元点群データに基づいて、自律移動体周囲の障害物のポリゴンマップを生成する（図２（ａ））。このポリゴンマップは、格子状のグリッドマップ（図２（ｂ））と比較して障害物の外形をポリゴンで高精度に表現できる。なお、ポリゴンマップ生成部３は、例えば、距離センサ２により取得された３次元点群を平面に投影して２次元点群を生成し、その２次元点群の外側の点を直線で結ぶことで、障害物の外形を示すポリゴンマップを生成する。ポリゴンマップ生成部３は、生成した障害物のポリゴンマップを経路修正部５に対して出力する。
【００１６】
経路算出部４は、経路算出手段の一具体例であり、自律移動体１００の移動環境を示すグリッドマップを生成し、始点から終点までの最短の移動経路をグリッドマップ上で算出する。経路算出部４は、予め設定された地図情報に基づいて、位置が定常的に不動な静的障害物（壁、テーブルなど）の領域をグリッドマップ上に設定する（図３）。さらに、経路算出部４は、距離センサ２により取得された動的な障害物である動的障害物（人、動物、ロボットなど）の３次元点群を２次元平面に投影して２次元点群を生成し、その２次元点群を埋めるグリッドをその動的障害物の領域としてグリッドマップ上に逐次設定する。
【００１７】
経路算出部４は、例えば、距離センサ２により取得された障害物の３次元点群の時系列変化に基づいて、その障害物が動的障害物および静的障害物のいずれかを判断することができる。経路算出部４は、距離センサ２により取得された障害物の３次元点群に時系列変化がある場合、その障害物を動的障害物と判断し、障害物の３次元点群に時系列変化がない場合、その障害物を静的障害物と判断する。さらに、経路算出４部は、距離センサ２により取得された障害物の３次元点群データに基づいてパターン認識を行い、その障害物が何であるかを判断し、その障害物が動的障害物および静的障害物のいずれに属するかを判断してよい。
【００１８】
経路算出部４は、静的障害物及び動的障害物の領域の周りに自律移動体１００の外形分だけ自律移動体１００の移動を禁止する禁止領域を設定してもよい。これにより、自律移動体１００を１グリッドの点で表現し移動経路を容易に設定できる。経路算出部４は、算出したグリッドマップを経路修正部５に対して出力する。経路算出部４は、上記のような禁止領域を含む静的及び動的障害物を回避しつつ、最短の移動経路を算出する。
【００１９】
経路修正部５は、ポリゴンマップ生成部３により生成された障害物のポリゴンマップに基づいて、自律移動体１００が障害物に干渉せず、自律移動体１００の姿勢変化が連続的（移動経路が滑らかな曲率変化）となるように、経路算出部４に算出された移動経路の修正を行う。
【００２０】
例えば、図４（ａ）に示すように経路算出部４により算出された初期の移動経路が障害物を回避しつつ、移動経路が不連続な曲率変化となっている場合を想定する。この場合、経路修正部５は、図４（ｂ）に示すような仮想バネによる修正を行い、図４（ｃ）に示すような滑らかな曲率変化となる移動経路に修正を行う。ここで、仮想バネとして、例えば、隣接する経路点間に伸長バネ（第１バネ）及び捻りバネ（第２バネ）を設定し、自律移動体１００と障害物との間に圧縮バネ（第３バネ）を設定することで、上記修正を行う。経路修正部５は、修正した移動経路を移動体制御部６に対して出力する。
【００２１】
移動制御部６は、移動制御手段の一具体例であり、自律移動体１００が経路修正部５により修正された移動経路に従って移動するように、自律移動体１００を制御する。
自己位置推定部７は、自律移動体１００の自己位置を推定する。自己位置推定部７は、例えば、オドメトリあるいはレーザセンサによる検出結果と地図情報とのマッチングなどの周知の手法を用いて自立移動体１００の自己位置を推定することができる。
【００２２】
図５は、本実施の形態に係る移動体制御方法のフローを示すフローチャートである。距離センサ２は、自律移動体１００の周囲に存在する障害物の３次元点群データを取得し（ステップＳ１０１）、ポリゴンマップ生成部３、及び経路修正部５に対して出力する。
【００２３】
ポリゴンマップ生成部３は、距離センサ２により取得された３次元点群データに基づいて、障害物のポリゴンマップを生成し（ステップＳ１０２）、経路修正部５に対して出力する。
【００２４】
経路算出部４は、距離センサ２により取得された動的障害物の３次元点群に基づいて、グリッドマップを生成し（ステップＳ１０３）、設定された始点から終点までの最短の移動経路をグリッドマップ上で算出し（ステップＳ１０４）、経路修正部５に出力する。
【００２５】
経路修正部５は、ポリゴンマップ生成部３により生成された障害物のポリゴンマップに基づいて、経路算出部４に算出された移動経路において、自律移動体１００が障害物に干渉しないように、移動経路の修正を行い（ステップＳ１０５）、移動体制御部６に対して出力する。このように、グリッドマップによる移動経路探索と、狭路などの詳細移動に対するポリゴンマップ修正と、を行うことで、高速な広域移動と高精度な詳細移動を両立することができる。
【００２６】
移動制御部６は、経路修正部５により修正された移動経路に従って、自律移動体１００の移動を制御する（ステップＳ１０６）。
【００２７】
ところで、自律移動体１００がグリッドマップ上では狭路などを通過できると判断し、その後、より高精度なポリゴンマップを用いて同一の狭路について干渉判定を行ったところ、通過不能と判断される所謂袋小路の問題が生じている（図６）。ここで、例えば、人などの移動する動的障害物（図７（ａ））や、椅子などの静的ではあるが移動可能な準動的障害物（図７（ｂ））により、その挟路が通過不能と判断される場合がある。この場合、後に、その障害物の移動によりその狭路が通過可能となる可能性がある。一方で、壁などの定常的に固定された静的障害物（図７（ｃ））によりその挟路が通過不能と判断された場合、後に、その狭路が通過可能となる可能性は略あり得ない。したがって、通過不能なる狭路に対して、グリッドマップ上でその障害物のより詳細な情報を設定し、その後の移動経路の計算にその情報を用いるのが望ましい。
【００２８】
そこで、本実施の形態に係る移動体制御装置１において、経路修正部５は、経路算出部４により算出された移動経路上で、ポリゴンマップ生成部３により生成された障害物のポリゴンマップに基づいて通過不能と判断した経路部分に対して、距離センサ２により３次元点群データを取得し、取得した３次元点群データを記録する。
【００２９】
これにより、自律移動体１００が再度その通過不能と判断された経路部分に来たときに、記録したその経路部分の３次元点群データを用いて、経路部分に存在する動的障害物、準動的障害物、或いは静的障害物に応じたより最適な移動経路を設定できる。さらに、通過不能と判断した経路部分に対してのみ、３次元点群データを記録することから、そのデータ処理量を低減でき、処理速度の高速化を図ることができる。
【００３０】
図８及び図９は、上述した本実施の形態に係る移動体制御装置による経路計画方法のフローを示すフローチャートである。まず、図８に示すフローチャートを用いて、自律移動体が最初に挟路などの通過不能の経路部分を通過する際の処理フローについて詳細に説明する。
【００３１】
経路算出部４は、距離センサ２により取得された動的障害物の３次元点群に基づいて、グリッドマップを生成し、始点から終点までの最短の移動経路をグリッドマップ上で算出する（ステップＳ２０１）。
【００３２】
経路修正部５は、ポリゴンマップ生成部３により生成された障害物のポリゴンマップに基づいて、経路算出部４により算出された移動経路において、自律移動体１００が障害物に干渉せず、自律移動体１００の姿勢変化が連続的となるように、移動経路の修正を行う（ステップＳ２０２）。
【００３３】
経路修正部５は、上記修正において、経路算出部４により算出された移動経路で通過不能と判断した経路部分があるか否かを判断する（ステップＳ２０３）。
【００３４】
経路修正部５は、経路算出部４により算出された移動経路で通過不能と判断した経路部分があるとき（ステップＳ２０３のＹＥＳ）、距離センサ２により取得された障害物の３次元点群データに基づいて、その通過不能と判断した経路部分に静的障害物のみが存在するか否かを判断する（ステップＳ２０４）。一方、経路修正部５は、経路算出部４に算出された移動経路で通過不能と判断した経路部分がないとき（ステップＳ２０３のＮＯ）、後述の（ステップＳ２１０）に移行する。
【００３５】
経路修正部５は、距離センサ２により取得された障害物の３次元点群データに基づいて、その通過不能と判断した経路部分に動的障害物が存在すると判断したとき（ステップＳ２０４のＮＯ）、所定時間経過後、自律移動体１００が障害物に干渉しないように、移動経路の再修正を行う（ステップＳ２０５）。このように、動的障害物は、通常、一定時間経過後に移動を行うものである。したがって、その経路部分が動的障害物により通過不能の場合でも、一定時間経過後にその動的障害物が移動し、通過可能となる可能性が高い。したがって、上述のように、所定時間だけ待って移動経路の再修正を行い、その経路部分に経路設定が可能かを再確認するのである。これにより、本来通過可能な経路部分を迂回するというような無駄な移動経路の設定を排除することができる。
【００３６】
経路修正部５は、再修正後の移動経路の経路部分が通過可能か否かを判断する（ステップＳ２０６）。経路修正部５は、再修正後の移動経路の経路部分が通過不能と判断したとき（ステップＳ２０６のＮＯ）、後述の（ステップＳ２０７）の処理に移行する。一方、経路修正部５は、再修正後の移動経路の経路部分が通過可能であると判断したとき（ステップＳ２０６のＹＥＳ）、後述の（ステップＳ２１０）に移行する。
【００３７】
経路修正部５は、距離センサ２により取得された障害物の３次元点群データに基づいて、その通過不能と判断した経路部分に静的障害物のみが存在すると判断したとき（ステップＳ２０４のＹＥＳ）、距離センサ２にその経路部分の３次元点群データを取得させ、グリッドマップのその経路部分に記録する（ステップＳ２０７）。
【００３８】
経路修正部５は、グリッドマップにおける移動経路のその経路部分に通過不能を示す通過不可ポテンシャルを設定する（ステップＳ２０８）。このように、経路部分に静的障害物が存在し通過不能となっている場合、通常、静的障害物は定常的に固定されているため、その通過不能の状態は時間経過しても変わることは略あり得ない。よって、その経路部分に通過不可ポテンシャルを設定し、原則として、次回からその経路部分を回避した移動経路を設定する。これにより、通過不能な経路部分に移動経路が設定されることが無い為、無駄な移動経路の設定を排除することができる。
【００３９】
ここで、経路修正部５は、グリッドマップに設定する通過不可ポテンシャルの重みを時間経過と共に減少（漸減あるいは段階的に減少）するように設定してもよい。これは、静的障害物の中に準動的障害物が含まれている場合、一定時間経過後に、その準動的障害物が移動し（例えば、椅子など小型軽量の準動的障害物は容易に移動可能である）、一旦、通過不能と判断した経路部分が通過可能となる可能性があるからである。
【００４０】
この場合、経路算出部４は、他に最適な経路が存在しない場合、時間経過に伴ってその重みが減少した通過不可ポテンシャルの経路部分を再度、移動経路の一部として設定することとなる。これにより、通過可能となり得る経路部分を直ちに迂回するというような無駄な移動経路の設定を排除することができる。
【００４１】
経路修正部５は、通過不可ポテンシャルが設定されたグリッドマップ上で移動経路の再計算を行う（ステップＳ２０９）。
【００４２】
移動制御部６は経路修正部５により再計算された移動経路に従って移動するように、自律移動体１００の移動を制御する（ステップＳ２１０）。
【００４３】
続いて、図９に示すフローチャートを用いて、自律移動体１００が二回目以降に挟路などの通過不能の経路部分を通過する際の処理フローについて詳細に説明する。
【００４４】
経路算出部４は、距離センサ２により取得された動的障害物の３次元点群に基づいて、グリッドマップを生成し、設定された始点から終点までの最短の移動経路をグリッドマップ上で算出する（ステップＳ３０１）。
【００４５】
経路修正部５は、ポリゴンマップ生成部３により生成された障害物のポリゴンマップに基づいて、経路算出部４に算出された移動経路において、自律移動体１００が障害物に干渉しないように、移動経路の修正を行う（ステップＳ３０２）。
【００４６】
経路修正部５は、上記修正において、経路算出部４に算出された移動経路で通過不能と判断した経路部分（通過不能ポテンシャルの設定）があるか否かを判断する（ステップＳ３０３）。
【００４７】
経路修正部５は、経路算出部４に算出された移動経路で通過不能と判断した経路部分があるとき（ステップＳ３０３のＹＥＳ）、通過不能と判断した経路部分に対して３次元点群データが記録されているか否かを判断する（ステップＳ３０４）。一方、経路修正部５は、経路算出部４に算出された移動経路で通過不能と判断した経路部分がないとき（ステップＳ３０３のＮＯ）、後述の（ステップＳ３０９）の処理に移行する。
【００４８】
経路修正部５は、通過不能と判断した経路部分に対して３次元点群データが記録されていると判断したとき（ステップＳ３０４のＹＥＳ）、距離センサ２により再度取得された経路部分に対する３次元点群データと、記録されている３次元点群データとを比較する（ステップＳ３０５）。この比較により、一度、通過不能と判断した経路部分に対して、再度、障害物が移動して通過可能となったか否かを判断することができる。
【００４９】
一方、経路修正部５は、通過不能と判断した経路部分に対して３次元点群データが記録されていないと判断したとき（ステップＳ３０４のＮＯ）、上述の（ステップＳ２０４）（図８）の処理に移行する。この場合、自律移動体１００がその通過不能と判断した経路部分を、初めて通過することとなるため、上記図８に示す初回通過のフローに戻る。
【００５０】
ここで、経路修正部５は、例えば、ＩＣＰ(Interative Closest Point)アルゴリズムを用いて、距離センサ２により再度取得された経路部分に対する３次元点群データと、登録されている３次元点群データとを比較して、その一致度を算出する。
【００５１】
経路修正部５は、上記経路部分の３次元点群データの比較を行った結果、その経路部分の環境が前回と一致していると判断したとき（ステップＳ３０６のＹＥＳ）、その通過不能ポテンシャルの経路部分を通過不能領域（禁止領域）としてグリッドマップ上に確定する（ステップＳ３０７）。この場合、自律移動体１００が今回通過するときと前回通過するときとで、その経路部分の障害物の位置に変化が無い為、最終的にその経路部分を通過不能領域としてグリッドマップに確定できる。
【００５２】
一方、経路修正部５は、上記経路部分の３次元点群データの比較を行った結果、その経路部分の環境が前回と一致していないと判断したとき（ステップＳ３０６のＮＯ）、後述の（ステップＳ３０８）の処理に移行する。この場合、自律移動体１００が今回通過するときと前回通過するときとで、その経路部分の障害物が移動しその位置に変化している判断できる。したがって、後述のように、再度、移動経路の再計算を行う。
【００５３】
経路修正部５は、グリッドマップ上で移動経路の再計算を行う（ステップＳ３０８）。
移動制御部６は経路修正部５により再計算された移動経路に従って、自律移動体１００の移動を制御する（ステップＳ３０９）。
【００５４】
以上、本実施の形態に係る移動体制御装置１において、経路算出部４により算出された移動経路上で、ポリゴンマップ生成部３により生成された障害物のポリゴンマップに基づいて通過不能と判断した経路部分に対して、距離センサ２により３次元点群データを取得し、取得した３次元点群データを記録する。これにより、その経路部分の３次元点群データを用いて、経路部分に存在する障害物に応じたより最適な移動経路を設定できる。
【００５５】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【００５６】
また、本発明は、例えば、図５、図８及び図９に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００５７】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００５８】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００５９】
１    移動体制御装置
２    距離センサ
３    ポリゴンマップ生成部
４    経路算出部
５    経路修正部
６    移動制御部
７    自己位置推定部
１００    自律移動体
【図１】
2014123200.tif 000003
【図２】
2014123200.tif 000004
【図４】
2014123200.tif 000005
【図５】
2014123200.tif 000006
【図６】
2014123200.tif 000007
【図７】
2014123200.tif 000008
【図８】
2014123200.tif 000009
【図９】
2014123200.tif 000010
【図３】
2014123200.tif 000011