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