(57)【特許請求の範囲】
【請求項１】
障害物が存在する環境化において、与えられた経路に沿って移動する自律走行制御装置を備えた移動ロボットであって、
上記移動ロボットは、該ロボットを移動させる走行制御系と、移動ロボットの周囲の固定物および障害物の相対位置を観測する手段と、経路計画エンジンと、制御エンジンとを備え、
上記制御エンジンは、移動ロボットの走行距離・走行方向と、上記観測した固定物もしくは障害物の相対位置から現在のロボットの位置を推定し、また、予め格納された移動ロボットの走行領域の固定物の位置を表すグリッドマップと上記観測した障害物の情報とを重ね合わせて統合マップを作成し、
現在のロボットの位置と該統合マップを上記経路計画エンジンに渡して経路計画をリクエストし、
経路計画エンジンは、上記リクエストがあったとき、障害物がないときは、上記統合マップ上で、設定された経路に従って仮想ロボットの通過点を求め、
障害物があるときは、仮想的距離センサにより前方の障害物との距離を求め、移動ロボットの走行領域の固定物の位置を表すグリッドマップと上記観測した障害物の情報とを重ね合わせた統合マップ上で、仮想ロボットと周りの障害物、壁との距離を求め、
この距離と障害物および／または壁に応じて設定された重みとにより定まる値と、閾値とを比較して、仮想ロボットの前方の開いている自由空間を求め、該自由空間の方向を移動先として仮想ロボットを移動させて仮想ロボットの通過点を求め、
上記通過点の点列を上記制御エンジンに渡し、
上記制御エンジンは、上記通過点の点列に沿って移動ロボットが移動するように上記走行制御系を制御する
ことを特徴とする自律走行機能を備えた移動ロボット。
【請求項２】
障害物が存在する環境化において、与えられた経路に沿って移動する自律走行制御装置を備えた移動ロボットの制御方法であって、
上記移動ロボットは、該ロボットを移動させる走行制御系と、移動ロボットの周囲の固定物および障害物の相対位置を観測する手段を備え、
移動ロボットの走行距離・走行方向と、上記観測した固定物もしくは障害物の相対位置から現在のロボットの位置を推定し、
また、予め格納された移動ロボットの走行領域の固定物の位置を表すグリッドマップと上記観測した障害物の情報とを重ね合わせて統合マップを作成し、
仮想ロボットを、障害物がないときは、上記統合マップ上で、設定された経路に従って仮想ロボットの通過点を求め、
障害物があるときは、仮想的距離センサにより前方の障害物との距離を求め、移動ロボットの走行領域の固定物の位置を表すグリッドマップと上記観測した障害物の情報とを重ね合わせた統合マップ上で、仮想ロボットと周りの障害物、壁との距離を求め、
この距離と障害物および／または壁に応じて設定された重みとにより定まる値と、閾値とを比較して、仮想ロボットの前方の開いている自由空間を求め、該自由空間の方向を移動先として仮想ロボットを移動させて仮想ロボットの通過点を求め、
この通過点の点列に沿って移動ロボットが移動するように上記走行制御系を制御する
ことを特徴とする移動ロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、与えられた経路に沿って移動する自律移動ロボットに関し、特に、障害物がある環境下において、障害物をよけながら予め定められた経路に沿って自律移動することができる移動ロボットおよびその制御方法に関するものである。
【背景技術】
【０００２】
従来から、予め設定された走行経路に沿って移動ロボットを移動させるようにした技術が種々提案されている（例えば特許文献１、特許文献２参照）。
例えば、特許文献１には、予定走行経路を示す地点を点列として与え、移動ロボットが自分の位置・姿勢を計測し、上記点列のうちの目標地点と移動ロボットの現在位置との差に基づき、進行方向を決めるようにしたものが記載されている。
一方、障害物のある環境下で、障害物を避ける経路を生成し、障害物を避けながらロボットを移動させるようにした障害物回避経路方式が種々提案されている（例えば特許文献３、特許文献４参照）。
【特許文献１】特開平１−２８０８０７号公報
【特許文献２】特開平８−１２３５４７号公報
【特許文献３】特開平７−１２９２３８号公報
【特許文献４】特開平５−２５００２３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
予め設定された走行経路に沿って移動させる移動ロボットとしては、前記した特許文献１に記載されたものや、与えられた曲線に沿って移動させるようにした曲線追従方式が知られている。これらのうち、前記特許文献１に記載されるように、ロボットの自分の位置・姿勢と次のサブゴールとを比較して進行方向を決めるものでは、必ずしも、安定な制御を行なうことができず、例えば次のサブゴールに到達できない場合には、途中で止まってしまうといった場合も生ずる。また、与えられた曲線に沿って移動させるようにした曲線追従方式は、煩雑な処理を必要とし、組み込み制御システムに応用しにくい。
一方、障害物を回避させながらロボットを移動させ、回避後に経路に復帰させるようにしたものも考えられているが、グローバル的最適な経路を生成するものではなく、複雑な環境下でロボットは極小点に落ち込む可能性が高い。
本発明は、上記事情に鑑みなされたものであって、折線を複数の線分に分解し、ロボットの追従線分の切り替えを制御することにより、情報処理が簡単で、リアルタイム性が高く、折線に沿って滑らかに移動させることでき、さらに、障害物があっても、障害物を回避しながらグローバル的最適な経路を生成して自律的に移動することが可能な移動ロボットおよび移動ロボットの制御方法を提供することである。
【課題を解決するための手段】
【０００４】
本発明においては、サブコールをつなぐ直線からなる折線を移動経路として移動ロボットに与える。また、環境情報地図と、ステレオビジョンセンサで観測した情報を統合して、固定物の位置や障害物の位置を示す統合マップを作成する。そして、仮想ロボットを、上記統合マップ上で上記障害物を回避させながら、前記予め定められた経路に沿って移動させ、仮想ロボットの通過点を求め、この通過点の点列沿って移動ロボットが移動するように上記走行制御系を制御する。
以上のように、本発明では、以下のようにして前記課題を解決する。
障害物が存在する環境化において、与えられた経路に沿って移動する自律走行制御装置を備えた移動ロボットにおいて、該移動ロボットに、走行制御系と、移動ロボットの周囲の固定物および障害物の相対位置を観測する手段と、経路計画エンジンと、制御エンジンとを設ける。
上記制御エンジンは、移動ロボットの走行距離・走行方向と、上記観測した固定物もしくは障害物の相対位置から現在のロボットの位置を推定し、また、予め格納された移動ロボットの走行領域の固定物の位置を表すグリッドマップと上記観測した障害物の情報とを重ね合わせて統合マップを作成し、現在のロボットの位置と該統合マップを上記経路計画エンジンに渡して経路計画をリクエストする。
経路計画エンジンは、上記リクエストがあったとき、仮想ロボットを、上記統合マップ上で上記障害物を回避させながら、前記予め定められた経路に沿って移動させ、仮想ロボットの通過点を求め、この通過点の点列を上記制御エンジンに渡し、上記制御エンジンは、上記通過点の点列に沿って移動ロボットが移動するように上記走行制御系を制御する。
すなわち、経路計画エンジンは、仮想的距離センサにより前方の障害物との距離を求め、移動ロボットの走行領域の固定物の位置を表すグリッドマップと上記観測した障害物の情報とを重ね合わせた統合マップ上で、仮想ロボットと周りの障害物、壁との距離を求め、この距離と障害物および／または壁に応じて設定された重みとにより定まる値と、閾値とを比較して、仮想ロボットの前方の開いている自由空間を求め、該自由空間の方向を移動先として仮想ロボットを移動させる。
【発明の効果】
【０００５】
本発明においては、以下の効果を得ることができる。
（１）グラフ探索を利用して、障害物を回避させながら、既定の経路に追従してロボットを走行させるようにしたので、複雑な環境下においても、最適な経路の探索が可能となり、経路追従と障害物回避とを統合させてロボットを走行させることができる。
（２）環境情報地図と、ステレオビジョンセンサで観測した情報を統合して、固定物の位置や障害物の位置を把握し、この地図上で仮想ロボットを移動させて、障害物を回避させながら上記折線に沿って移動する経路を求め、この経路を走行制御系に与えてロボットを走行させているので、グローバル的な最適な経路を生成してロボットを走行させることができる。
また、サブゴールの近傍で、仮想ロボットが追従する線分を切り換えるようにしたので、サブゴールの近くでもロボットを滑らかに移動させることができで、サブゴールでロボットが停止してしまうこともない。
（３）従来では、ロボットが自由に移動できる空間に関する情報の取得には、大規模な距離をセンシングする装置が必要であった。また、単純なステレオビジョン計測の場合では、空間情報を欠如するという問題があった。本発明では、環境情報地図と、ステレオビジョンセンサで観測した情報を統合して、固定物の位置や障害物の位置を把握しているので、ロボットが自由に移動できる空間に関する情報を比較的容易に取得することが可能となり、障害物のある環境下においても、障害物を回避させながらロボットを移動させることが可能となる。
【発明を実施するための最良の形態】
【０００６】
図１は本発明の実施例の移動ロボットの制御系の概略構成例を示す図である。
図１に示す移動ロボット（以下単にロボットという）１００は４つの車輪１０１と、これを駆動するモータ１０２を有し、モータ１０２にはエンコーダ１０３が設けられている。エンコーダ１０３の出力は移動量算出部１０４に入力され、移動量算出部１０４で、車輪１０２の回転量、すなわち、ロボットの移動量を算出する。従って、ロボットの出発位置（初期位置）が与えられれば、上記移動量からロボットの大まかな位置を求めることができる。
また、本実施例のロボット１００は、ステレオビジョンセンサ４０が設けられ、ステレオビジョンセンサ４０により、例えば図２に示すようにロボット１００の移動方向の固定物（障害物）との距離ａ１，ａ２，ａ３などを求めることができる。
さらに、ロボット１００は、移動する領域の固定物などの位置を事前に記録した環境地図情報を有する。図３はロボット１００が有する環境地図情報の一例を示す図であり、同図に示すように、環境地図情報は、例えばロボットの走行領域のグリッドマップであり、壁面などの固定物の位置が記録されている。ここでグリッドマップは、図３に示すように、水平面をある大きさのブロックで分割した平面からなる地図であり、固定物、障害物のあるグリッドには、例えば０以上の値、障害物のないグリッドには、０が与えられている。
したがって、上記グリッドマップ上の固定物の位置と、上記ステレオビジョンセンサ４０で観測したロボットの周囲の固定物との距離とを照合することで、ロボットの位置をさらに正確に把握することができる。
【０００７】
また、ロボットの走行方向にある障害物の位置は、図４に示すようにグリッドマップ上に反映させる。
すなわち、図４（ａ）のように新しい環境情報を観測した時に、障害物の位置が記録されたグリッドマップをコピーし、同図（ｂ）に示すように、ロボットのステレオビジョンセンサ４０の視野内のデータをクリアする。
ついで、同図（ｃ）に示すように、ステレオビジョンセンサ４０で観測して障害物の位置を得て、同図（ｄ）に示すように（ｂ）（ｃ）のデータを統合し、新しい環境情報が書き込まれたグリッドマップを生成する。これにより、ロボットの走行領域の環境情報をグリッドマップで表現することができる。
さらに、上記事前に作られた環境地図情報とステレオビジョンセンサ４０で障害物の位置情報を統合して、図５に示すように、経路計画用のグリッドマップを生成する。
すなわち、事前に固定物の位置が記録された環境地図情報と、上記のようにステレオビジョンセンサ４０で観測した障害物の位置を示す地図情報を重ね合わせ、統合したグリッドマップを生成する。後述する経路計画においては、この統合グリッドマップを使用して、ロボットの走行方向を選定する。
【０００８】
図１に戻り、１０は自律走行モジュール、２０は走行制御系、３０はＣＰＵであり、ＣＰＵ３０から例えば、ロボットの初期位置とロボットのグローバル経路を入力する。
ロボット１００に与えられる経路情報は、例えば図６（ａ）に示すように、ロボットの現在位置から目的地までのパスを、サブゴール（同図の通過点Ａ，Ｂ，Ｃ…）をつなぐ折線で表したものである。
上記サブゴールをつなぐ折線は、図７に示すように線分１〜線分ｎに分解されてサブゴールスタックに格納される。
【０００９】
ロボット１００の自律走行モジュール１０は、前述したように、ロボットの初期位置からの移動量、ステレオビジョンセンサ４０により観測された周囲の固定物との距離、および環境地図情報に基づき、ロボットの現在位置を求める。
そして、障害物がないときには、上記折線とのずれによって、既定経路に復帰するローカルな図６（ｂ）に示すようなパスを生成し、また、図８に示すように経路上に障害物があるとき、この障害物を回避するパスを生成しながら、既定の折線の分解された線分に対して追従するようなグローバル的な最適なパスを生成し、このパスに沿ってロボット１００が移動するように、走行制御系２０に制御信号を送出する。
走行制御系２０は、前記モータ１０３を駆動してロボットを移動させる。
なお、図１では車輪によってロボットを移動させる場合について示したが、ロボットの走行手段は上記に限られず、その他の走行手段を有するものであってもよい。
【００１０】
図９に、本実施例の自律走行モジュール１０のブロック図を示す。同図に示すように、自律走行モジュール１０は、制御エンジン１１と経路計画エンジン１２の２つから構成される。
制御エンジン１１は、ロボット位置推定部１１ａと、経路計画要求部１１ｂと、走行コマンド生成部１１ｃと、統合マップ作成部１１ｄを具備し、環境情報の収集、ロボットの自己位置推定、経路計画エンジンを呼び出し、計画した経路の走行系への送信、及びロボットの追従する線分の切り替えなどを、ロボットが最終ゴールに到達まで繰り返し実行する。
すなわち、ロボット位置推定部１１ａは、ステレオビジョンセンサ４０と、前記移動量算出部１０４で求めたロボットの大まかな位置を、前述したようにステレオビジョンセンサ４０の観測値とグリッドマップ１４（図３参照）とに基づき補正して、ロボットの現在位置を推定する。
【００１１】
また、統合マップ作成部１１ｄは、前記図３で説明したように環境地図情報と、ステレオビジョンセンサ４０で観測した障害物の位置を示す地図情報を重ね合わせ、統合したグリッドマップを生成する。
一方、前述したようにＣＰＵ３０からロボットのグローバル移動経路であるサブゴールをつなぐ折線が与えられる。この折線は分解されてサブゴールスタック１３に格納されている。
ロボット位置推定部１１ａは、ロボットの現在位置が上記サブゴールあるいは最終ゴールに達したか判定し、達していなければ上記サブゴールスタック１３に格納された追従すべき線分を経路計画エンジン１２に渡す。経路計画要求部１１ｂは経路計画エンジン１２に対して経路計画を要求する。
【００１２】
経路計画エンジン１２は、制御エンジン１１からの追従すべき折線の情報とロボットの現在姿勢を受信して、仮想的な距離センサを持つ仮想ロボットを、前述した統合したグリッドマップ上で走行させて、経路追跡と障害物回避を統合したグローバル的最適な経路を生成する。
すなわち、障害物の近傍にロボットがいないときは、制御エンジン１１からの追従すべき折線の情報とロボットの現在姿勢を受信して、折線を追従するように仮想ロボットを走行させる。
このため仮想ロボットが追従すべき折線を複数線分の追従に分解する。例えば、仮想ロボットがＡ点からスタートし、追従する折線をＡＢＣとする場合、制御は線分をＡＢ、ＢＣに分解し、それぞれの線分に沿って仮想ロボットが走行するように制御する。ただし、後述するように、途中で追従する線分を切り替える。
すなわち、経路計画エンジン１２の経路計画部１２ａは、仮想ロボットの位置と上記追従すべき線分とのエラーに基づき仮想ロボットの速度と角度を決定する。そして、仮想ロボットを上記のように走行させて、仮想ロボットの通過点の点列を求める。
【００１３】
また、ロボットの近傍に障害物があるときは、障害物をよける行動を探索する。図１０（ａ）に示すように、仮想ロボットは、その胴体の正面に５度刻みで計３６個の仮想的距離センサを持ち、これによって、グリッドマップから仮想ロボットと周りの障害物、壁との距離を取得する。そして、障害物の種類に応じた重みと距離の逆数との積を計算する。例えば、壁に対して１．０の重みを使用する。人間に対して０．５を使用する。
これによって、ロボットと各物体との安全な距離を設定する。この値に対して、図１０（ｂ）に示すように閾値を設定して、ロボットの前に開いている自由空間を決める。
そして、すべての開いている自由空間を移動先とするが、各選択肢にコストを付けて、グラブとしてつなげていき、このグラフに対してＡスターの探索方法（例えば、P. E. Hart, N. J. Nilsson, B. Raphael: A Formal Basis for the Heuristic Determ ination of Minimum Cost Paths, IEEE Transactions on Systems Science  and Cybern etics SSC4 (2), pp. 100-107, 1968. 参照）を利用して最適な経路を選択する。Ａスターの探索方法は、ノードからリンクから構成されるグラフ探索の一つのアルゴリズムであり、Ａスター探索方法を利用することにより、スタートノードからゴールノードまでの累積コストが最小となる経路を決定することができる。
【００１４】
図１１に上記折線追従と障害物回避を統合させた最適経路の計画方法の概念図を示す。  同図に示すように、障害物がないときは、折線に沿ってロボットは移動する。また、前方に障害物があるときは、障害物を回避する最適な経路を選択して移動する。
例えば、前方に障害物があるノードＡにロボットがあるとき、障害物を回避して移動できる選択肢はノードＢ，Ｃであるが、ノードＢ，Ｃの内、コストが小さいノードＢへの経路を選択する。以下同様に、コストが小さくなる経路を選択することにより、障害物を回避する最適な経路を選択する。
経路計画エンジン１２は上記のようにして仮想ロボットの移動経路を求め、経路計画として制御エンジン１１に渡す。
制御エンジン１１の走行コマンド生成部１１ｃは、上記経路計画エンジン１２から得た経路計画に基づき、走行制御系を駆動する走行コマンドを生成する。
この走行コマンドに基づき走行制御系２０は、前記したようにモータ１０２を駆動し、ロボット１００を移動させる。
ここで、上記各ノードはグリッドマップ上のグリッドに対応しており、隣り合うグリッドはリンクにより結合されている。そして、例えばノードＢ，Ｃに対応するグリッドは、ノードＡに対応するグリッドから見て、障害物を回避して移動可能なノードＡに対応するグリッドに隣接するグリッドである。
あるグリッド上にいるロボットが隣のグリッド上に移動するとき、コストが発生する。このコストとしては、例えば、障害物に接近して生じる危険の度合い、ゴールとの距離、旋回角度の大きさ、目標地点への旋回角度である。
【００１５】
図１２は、上記制御エンジン１１における処理内容を示すフローチャートである。
同図に示すように、制御エンジン１１は、ロボットの現在位置Ｐを読み込み、現在位置Ｐが最終ゴールに達したかを判定する（ステップＳ１，Ｓ２）。
ロボットの位置が最終ゴールに達した場合は、処理を終了する。
また、最終ゴールに達していない場合は、前記したように、ステレオビジョンセンサ４０で観測した周囲の状況とグリッドマップ１４を重ね合わせて、現在位置を推定して補正量ΔＰを求め、移動量算出部１０４で推定したロボットの現在位置Ｐを補正量ΔＰで補正する（ステップＳ３）。また、前記図５で説明したように、環境地図情報と、ステレオビジョンセンサ４０で観測した障害物の位置を示す地図情報を重ね合わせ、統合したグリッドマップを生成する（ステップＳ４）。
ついで、現在位置Ｐがサブゴールに達したかを判定し、サブゴールに達した場合は、サブゴールスタックから次のサブゴールを取り出し（ステップＳ５，Ｓ６）、ステップＳ７に行く。また、現在位置Ｐがサブゴールに達していない場合には、ステップＳ５からステップＳ７に行き、経路計画エンジン１２にパス・ プラニング（経路計画）を要求する（ステップＳ７）。
そして、経路計画エンジン１２からパス・プラニングを取得し（ステップＳ８）、走行制御系２０に走行コマンドを送出する。
【００１６】
図１３は、上記経路計画エンジン１２における処理内容を示すフローチャートである。  経路計画エンジン１２は、探索グラフを作成し、前述したＡスターの探索方法を利用して仮想ロボットの最適な経路を選択する。ここで、探索グラフは、前記図１１に示したように、仮想ロボットの移動先の選択肢をグラフとして表したものであり、このグラフ上でコストの最も小さい経路を選択する。
図１３において、制御エンジン１１から経路計画エンジン１２に対して経路計画の要求があると、探索グラフを作成し（ステップＳ１，Ｓ２）、このグラフ上のルートノード上に、現在のロボット位置をセットする（ステップＳ３）。また、オープンリストにこのルートノードを追加する（ステップＳ４）。
ついで、上記オープンリストからノードを取得し、このノード上に仮想ロボットをセットし（ステップＳ５，Ｓ６）、仮想ロボットが最終ゴールに達したかを判定する（ステップＳ７）。仮想ロボットが最終ゴールに達したら処理を終わる。
最終ゴールに到達していなければ、サブゴールを切り換えるか、すなわち、追従する線分を切り換えるかを判定する。
【００１７】
折線のサブゴールはスタックに保存されており、ロボットの線分追跡の完了次第、サブゴールをスタックから吐き出し、次に追跡する線分に切り替える。
追従する線分を切り替える条件としては、図１４（ａ）に示すように、ロボットと現在向かっているサブゴールＢとの距離ｄが閾値より小さい場合、あるいは図１４（ｂ）に示すように、仮想ロボットの現在追従している線分からの誤差εABより、次の線分からの誤差εBCが小さい場合である。
サブゴールを切り換える場合には、サブゴールスタックから次のサブゴールを取り出す（ステップＳ８，Ｓ９）。また、サブゴールを切り換えない場合には、ステップＳ１０に行き、ループ回数が閾値ＴＨを越えたか否かを判定する。ループ回数が閾値ＴＨを越えたら、上記処理により得られた仮想ロボットの走行経路の点列を制御エンジン１１に渡す。なお、この閾値ＴＨは、制御エンジン１１に渡す点列の数を設定するものであり、この閾値に応じた数の点列が制御エンジンに渡される。
制御エンジン１１は、経路計画エンジン１２から受け取った点列（通過点ポイント）の数が０以上の場合、走行制御系２０に走行コマンドを与える。ポイントの数が０である場合は、ロボットの移動先がないと意味するため、走行制御系２０に走行停止を指示する。  制御エンジン１１が次の循環に入る。各循環では経路計画エンジン１２から生成した通過点の数が連続的に０である場合、ロボットを現在のサブゴールに向け、ランダムに向きを変え、経路を探す。
【００１８】
ループ回数が閾値ＴＨを越えていなければ、前記図１０で説明した仮想センサアレイにより、グリッドマップ周囲の距離取得する（ステップＳ１２）。そして、ロボットが障害物の近傍にいるかを判定する（ステップＳ１３）。
ロボットが障害物の近傍にいない場合には、ステップＳ１４にいき、仮想ロボットの位置と上記追従すべき線分とのエラーを求め、このエラーに基づき仮想ロボットの速度と角度を決定し（ステップＳ１５）、ステップＳ１７に行く。
【００１９】
また、ロボットが障害物の近傍にいるときには、障害物をよける行動を探索する。すなわち、前記図１０で説明したようにロボットの前に開いている自由空間を求め、移動可能な選択肢を求める。
ついで、ステップＳ１７にいき、移動可能な選択肢を親ノードに子ノードとして追加し、コスト計算をする（ステップＳ１８）。
ついで、オープンリストに全ての子ノードを追加し、コストによりオープンリストをソートする（ステップＳ１９，Ｓ２０）。
図１５は、上記ステップＳ１７からＳ２０の処理を説明する図である。
同図に示すように選択肢となる子ノードが３つある場合、親ノードａ１に３つの子ノードｂ１，ｂ２，ｂ３が追加され、それぞれの子ノードのコストが計算される。コストは、前述したように、例えばロボットが移動先へ移動する際の旋回角度の大きさ、次の移動先の障害物との間の距離、ゴールとなるサブゴールとの間の距離、現在のロボットの向きとゴール方向との角度の差等から計算される。例えば、旋回角度が小さく、次の移動先と障害物との距離が遠く、ゴールとの距離が近く、現在のロボットの向きとゴール方向との角度の差が小さい場合にコストが小さくなる。
図１３に戻り、コストが計算されると、子ノードがオープンリストに追加され、オープンリストがコストによりソートされ、コストの少ない子ノードが次の親ノードとなる。
ついで、ステップＳ５に戻り、上記ノードをオープンリストから取得し、上記処理を繰り返す。
【００２０】
図１６に、上記仮想ロボットを線分に追従させるための制御則の一例を示す。
本実施例においては、図１６（ｂ）の（１）（２）（３）式の制御則により、現在の仮想ロボットの進行方向に対する旋回角度ζおよび仮想ロボットの単位時間当たりの移動距離νを求める（定数ｋθ（θは下添え字）、ｋe 、Ｍは定数）。
ここで、同図（ａ）はサブゴールを結ぶ線分と、仮想ロボットの位置と進行方向の関係を示し、Ａw （ｘa ，ｙa ）、Ｂw （ｘb ，ｙb ）は、それぞれ、スタートのサブゴール、目標となるサブゴールの位置座標（ｗの添え字を付したものはワールド座標）、Ｒw （ｘr ，ｙr ）はロボットの現在位置の位置座標（ワールド座標）、Ｘr はロボットの移動方向、Ｙr はロボットの移動する方向に直交する方向、ｄb はロボットとゴールとなるサブゴールとの間の距離、αはロボットの進行方向と、ゴールとなるサブゴールとロボットを結ぶ線との間の角度である。
また、同図（ｂ）において、（４）式は、ワールド座標系（添え字ｗの座標系）とロボット座標系（添え字ｒの座標系）の変換式、（５）（９）式は、それぞれ追従すべき線分の傾き、この線分のＸ軸に対する角度θを示し、また、（６）（７）（８）式におけるεABはロボット位置と線分との誤差を示す。
【図面の簡単な説明】
【００２１】
【図１】本発明の実施例の移動ロボットの制御系の概略構成例を示す図である。
【図２】ステレオビジョンセンサによる周囲状況の観測を説明する図である。
【図３】環境地図情報の一例を示す図である。
【図４】ステレオビジョンセンサによる観測マップの作成を説明する図である。
【図５】経路計画用グリッドマップの生成を説明する図である。
【図６】本発明において、ロボットに与えられる経路情報を示す図である。
【図７】サブゴールをつなぐ折線の格納を説明する図である。
【図８】経路追従と障害物回避を説明する図である。
【図９】本発明の実施例の自律走行モジュールのブロック図である。
【図１０】仮想ロボットと仮想センサによる自由空間の検出を説明する図である。
【図１１】折線追従と障害物回避を統合させた最適経路の計画方法の概念図である。
【図１２】制御エンジンにおける処理内容を示すフローチャートである。
【図１３】経路計画エンジンにおける処理内容を示すフローチャートである。
【図１４】サブゴール近くでの追従する線分の切り換えを説明する図である。
【図１５】図１３のフローチャートにおける処理を説明する図である。
【図１６】仮想ロボットを線分に追従させるための制御則を説明する図である。
【符号の説明】
【００２２】
１０      自律走行モジュール
１１      制御エンジン
１２      経路計画エンジン
１３      サブゴールスタック
１４      グリッドマップ
２０      走行制御系
３０      ＣＰＵ
４０      ステレオビジョンセンサ
１００    移動ロボット
１０１    車輪
１０２    モータ
１０３    エンコーダ
１０４    移動量算出部
【図１】
0004467534.tif 000002
【図２】
0004467534.tif 000003
【図３】
0004467534.tif 000004
【図４】
0004467534.tif 000005
【図５】
0004467534.tif 000006
【図６】
0004467534.tif 000007
【図７】
0004467534.tif 000008
【図８】
0004467534.tif 000009
【図９】
0004467534.tif 000010
【図１０】
0004467534.tif 000011
【図１１】
0004467534.tif 000012
【図１２】
0004467534.tif 000013
【図１３】
0004467534.tif 000014
【図１４】
0004467534.tif 000015
【図１５】
0004467534.tif 000016
【図１６】
0004467534.tif 000017