JP3991097B2 - 移動体経路制御装置 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は、工場内外での点検・保守・搬送などの作業を行うための走行車あるいは産業用ロボットのハンド等が予定の経路から外れたときに位置誘導を行って予定経路に復帰させる移動体経路制御装置に関する。
【０００２】
【従来の技術】
従来、産業用ロボットがプレイバック中に非常停止したときに早急に原点に復帰して再作動させる方法（例えば特開昭６１−６０１０２号公報）や、所定の動作開始点まで復帰する方法（例えば特開昭６１−２３２１０号公報）、あるいは非常事態が起こり手動でロボットを操作して教示経路から外した後で元の位置に復帰させる方法（例えば特開昭５７−１６８３０６号公報）などが知られている。
【０００３】
これらはロボットや自律走行車が停止した後の作業再開を効率的に行う方法ではあるが、いずれもプレイバックを中断して停止した位置あるいは作業開始位置に戻す方法であって、プレイバックを継続しながら教示経路にスムーズに復帰する方法ではない。
したがって、たとえば教示経路中に生じた障害物を回避するためロボットが自律的にあるいはオペレータの操作により回避行為として教示経路を外した場合などにも、わざわざ元の位置まで戻って再起動する必要があった。
【０００４】
なお、特開平８−２４１１２３号公報には、巡回ロボットなどが異常処理のために巡回経路を離脱し処理を終了した後に元の巡回経路に復帰する方法として、遠隔操作により走行してきた地点を逆に辿る方法のほかに現在位置から巡回経路上の最も近い地点まで直線的に戻る方法が開示されている。この場合、巡回ロボットなどが走行する経路は現地点から走行経路への垂直線分になり急激な方向転換と速度変化を生じるため滑らかな復帰動作にはならない。
【０００５】
また、障害物回避のために経路をはずれたときに、次の目標位置に向かう方向の目標速度指令ベクトルを演算して目標位置に直線的に近付くようにする方法が知られている。しかしこの方法では平方根演算を含むため演算に時間がかかり、制御性が良くない。
【０００６】
特公平７−５８４４２号公報には、この欠点を解消するため、前の目標点と次の目標点を結ぶ直線に対して平行な向きに一定の速さを有し、かつ現在の位置からその直線に立てた垂線の向きに垂線の長さに比例する速さを有するようなベクトル速度を用いて経路復帰させるようにしたものが開示されている。この方法によれば、平行速度成分が一定であり複雑な平方根演算を要しないので演算時間を短縮して十分リアルタイムにロボット軌道制御することができる。
しかし、この方法では現時点における速度にかかわりなく区間毎に決められた一定速度で運動するので、障害物回避を行った後の移動体の速度に影響を受けて滑らかな復帰運動状態を得ることができない。さらに、復帰開始点で大きな加速度を受ける場合には、安全装置により緊急停止をする事態が発生する可能性がある。
【０００７】
【発明が解決しようとする課題】
そこで、本発明が解決しようとする課題は、産業用ロボットや自律走行車などが自動運転中に自律的にあるいは遠隔操作により教示経路から外れた場合に、これら移動体を停止させることなくそのまま自動運転を継続しながら教示経路にスムーズに復帰する移動体経路制御装置を提供することである。
【０００８】
【課題を解決するための手段】
上記課題を解決するため、本発明の移動体経路制御装置は、移動体の現在位置を測定もしくは算出して求める位置認識装置と、移動体の予定経路を各点における移動方向と速度と共に記憶する記憶装置と、演算装置と駆動制御装置とを備え、演算装置が現在位置から垂線を下ろして決まる最短距離にある予定経路上の対応点における移動方向と速度を基準としてこの基準値に徐々に近づくような移動体移動方向と速度を設定し、駆動制御装置がこの設定方向と速度に基づいて移動体を予定経路に復帰させるように移動制御することを特徴とする。
【０００９】
移動体の現在位置から予定した経路の情報を参照し、現在速度に基づいて予定経路上で計画された速度に徐々に近付けるように移動体の運動を制御するので、移動体は激しく速度や方向を変化させることなく徐々に予定経路に近づき極めてスムーズに予定経路とその時点における予定速度状態に復帰することができる。
なお、設定移動方向と速度は、予定経路上の移動方向と平行な成分と垂直な成分に分解して、それぞれの差が徐々に縮小するように設定することが好ましい。
【００１０】
さらに、移動体の復帰を始める時点における移動体位置と予定経路上の復帰目標点を結ぶ緩和曲線を設定し、移動体の現在位置から緩和曲線に垂線を下ろしてその垂線の長さが徐々に減少するように働く速度成分を算出して、予定経路上の対応点における値を基準として算出された移動体移動方向と速さを修正するようにしてもよい。
このように、復帰目標点までの経路に拘束される成分を加えることにより、移動体を予定経路上の目標点で予定経路に復帰するようにすることができる。
【００１１】
設定移動速度の予定経路上の対応点における移動方向に垂直な成分、あるいは目標点までを結ぶ緩和曲線に立てた垂線の長さを減少させる速度成分は、移動体の現在位置と予定経路上の対応点との距離または垂線の長さをＤ、時間をｔ、またｃとｋを係数として、運動方程式
ｄ²Ｄ／ｄｔ²＋ｃｄＤ／ｄｔ＋ｋＤ＝０
の解として求めるようにすることができる。
これにより、移動体と予定経路の間にダンパー付きバネを仮想的に介装したと同様の効果をもたらし、予定経路への復帰を単純な演算で実現することができ、かつその運動形態を容易に予測することができる。
【００１２】
さらにまた、上記運動方程式の解を、ζ＝ｃ／２ｋ^1/2≧１ なる条件の下で求めることがより好ましい。運動方程式の係数をこのような値にすることにより、ロボットの軌跡が予定経路を行き過ぎて振動的になるようなことがなく、しかもその値により予定経路に復帰する様子を予想することができるようになる。
なお、演算は所定の間隔ごとに行うようにすることができる。
本発明の移動体経路制御装置は、自律走行車を走行経路に復帰させるために使用したり、ロボットハンドを教示経路に復帰させるために使用するときに、大きな効果がある。
【００１３】
本発明の移動体経路制御装置により、ロボットや走行車を停止させることなく自動で教示経路に復帰させることが可能である。復帰経路は非常に滑らかであり、ロボットや移動体が急に動き出したり激しい姿勢変化をしたりしないため周囲の人に対して安全で装置にかける負担も少ない。また、ビジュアルサーボを用いて誘導する場合にも視界が急激に変化しないためランドマークを見失うことが無く、デッドレコニングによる場合にも自己位置推定累積誤差が小さいためより正確な誘導が可能になる。
【００１４】
【発明の実施の形態】
以下、図面を参照して本発明の詳細を実施例に基づいて説明する。
図１は本発明の移動体経路制御装置の１実施例を表すブロック図、図２は本実施例における演算手順を示すブロック図、図３と図４は演算のアルゴリズムを説明する図表、図５と図６は演算に使用する係数の例を示す図表、図７は移動体の収束状況を示す図表である。図８は本発明をビジュアルサーボに適用した場合を説明する図表である。
【００１５】
【実施例１】
移動体経路制御装置は、図１に示すように、移動体の現在位置を求める位置認識装置１と、移動体の予定経路を各点における移動方向と速度と共に記憶する経路記憶装置２と、演算装置３と駆動制御装置４とを備える。
移動体の予定経路および移動速度は、ロボットハンドの場合はオペレータがハンドを手に持ってガイドすることにより経路教示部５から教示することができる。また、ＮＣ装置から機械的にあるいはオペレータコンソールから手動で通過点とそこにおける作業内容を教示するようにもできる。なお、初めから記憶装置に収納しておいても良い。
【００１６】
記憶装置２は、教示された移動体の経路と経路上の点における移動体の速度に関する情報を記憶する。経路の情報は２次元的にあるいは３次元的に展開して示すことができる。
移動体は予め決められた自律移動経路Ｌをたどって自律的に移動しているが、移動体に搭載された物体識別装置が移動経路中に障害物Ｂを検出すると、自動的にあるいは監視室のオペレータによる遠隔操作で移動体を予定経路から逸脱させてこの障害物を回避する。
回避行動Ｌｄが終了した時の移動体の位置Ｐは、移動体に付設した位置センサで測定して求めることができ、また駆動装置により移動させた量から算出することもできる。
【００１７】
演算装置３は、回避行動を終えた時に位置認識装置１から伝達される移動体の位置情報と経路記憶装置２から与えられる移動体予定経路の情報を用いて、移動体の現在位置座標Ｐから予定経路の軌跡Ｌに垂線を下ろして予定経路上の対応点Ｐｒを求める。こうして求めた垂線の足は予定経路中で移動体の現在位置に最も近い点であり、垂線の長さＤは予定経路までの最短距離である。
【００１８】
この対応点Ｐｒにおける予定された移動方向と速度Ｖｐｒを基準として、この基準値に徐々に近づくような移動体移動方向と速度Ｖｍを算出して、駆動制御装置４にこれらの値を設定値として伝送する。なお、通常、対応点における移動方向は予定経路の接線方向と同じである。
駆動制御装置４は、この設定方向と速度に基づいて移動体を駆動制御して予定経路に復帰させる。
【００１９】
図２は演算装置３が実行する演算手順例を説明するフロー図、図３および図４は演算原理を説明するための図表である。
障害物回避行動が完了した時点で、移動体は継続して予定の教示経路に復帰するための行動を開始する。
予定経路への復帰は滑らかであることが好ましく、移動体が予定経路上の点Ｒに復帰する間急激な速度変化を生ぜずかつ迅速に予定経路に近づき、しかも予定経路上に戻ったときに予定経路上の点における運動と移動体の運動の相対速度ＲＶがゼロになるようにしたい。
演算装置３は、移動体が逸脱位置から円滑に元の予定経路上の点Ｒに復帰し、かつその点Ｒにおいて期待される速度を持つようにするために、復帰工程中の最適な速度Ｖｍを算出する。
【００２０】
演算装置３は、まず、移動体の現在位置座標Ｐから予定経路の軌跡に垂線を下ろしこの垂線の足Ｐｒを求める（Ｓ１）。次に、この点Ｐｒにおいて期待される移動体の速度Ｖｐｒを求めて、移動体の現状における速度Ｖｐと比較する。こうして予定経路上の対応点における期待速度に対する移動体Ｐの相対速度ＲＶを求める（Ｓ２）。
相対速度ＲＶはベクトル速度であって、予定経路上の点Ｐｒにおける接線方向の速度成分と移動体の同じ方向の速度成分との差ＲＶｔとこれに垂直な方向における速度成分ＲＶｎに分解することができる（Ｓ３）。なお、予定経路中の期待速度の垂直成分はゼロであるから、移動体の速度成分ＲＶｎは移動体Ｐと予定経路上の垂線の足Ｐｒまでの距離Ｄの時間微分値に当たることになる。
【００２１】
まず接線方向速度差ＲＶｔは現状における値を徐々に減少させていって復帰時点ではゼロにする。このため、復帰工程開始時点から徐々に減少するような関数で表される係数αを現在の速度差ＲＶｔに掛けて次の期間の接線方向速度差ＲＶｔｏを算出するようにする（Ｓ４）。このような係数αとして例えば図５に示したような１以下の値を有する単純減少関数を用いれば、予定経路上に戻るまでの速度変化も円滑に最終的な期待速度に復帰することができる。なお、この関数を適当に選択することにより復帰するまでの時間や速度変化状態を調整することができる。
【００２２】
また、接線に垂直な方向の速度差ＲＶｎも現状における値を徐々に減少させていって復帰時点ではゼロになるように算定する（Ｓ５）。本実施例の移動体経路制御装置では、概念的には図６に示すように、予定経路上の垂線の足Ｐｒを平衡位置とし移動体Ｐまでの長さＤだけ変位させた仮想的なバネ・ダンパ系を想定し下のような運動方程式（１）を立てて、これに基づいてパラメータを適当に選択することにより予定経路に復帰させるようにした。
ｄ²Ｄ／ｄｔ²＋ｃｄＤ／ｄｔ＋ｋＤ＝０ （１）
ここで、時間をｔ、またダンパ係数をｃ、バネ定数をｋとする。また次の期間の垂線方向速度差ＲＶｎｏはこの解を用いてｄＤ／ｄｔとして求まる。なお、ｄＤ／ｄｔの初期値をＰ点における移動体の速度成分ＲＶｎとする。
【００２３】
この運動方程式は、また共振周波数ωｎと減衰係数ζを用いた下の式（２）に整理できる。
ｄ²Ｄ／ｄｔ²＋２ζωｎｄＤ／ｄｔ＋ωｎ²Ｄ＝０ （２）
このような運動方程式は工学的によく解析されており、解を求めることが容易でかつ系の挙動が正確に予想できる。
【００２４】
すなわち、バネ定数ｋとダンパ係数ｃの関数として減衰係数ζ＝ｃ／２ｋ^1/2が求まり、減衰係数ζが与えられれば予定経路上の点に当たる平衡位置に対する移動体の運動状況が予測できる。
減衰係数ζが小さければ運動は振動的になるが、減衰係数が１以上であれば非振動的となり予定経路を行き過ぎることなく円滑に近づくようになる。このときｃ²−４ｋ≧１ となる。
【００２５】
図７は移動体の垂線方向初期速度差ＲＶｎｉの正負に従って距離Ｄの経時変化の様相が異なることを示すグラフである。初期速度差ＲＶｎｉが予定経路から離れる方向に向いている場合でも、距離Ｄは徐々に小さくなって、やがて移動体は予定経路上に復帰する。
なお、減衰係数が大きいほど速度変化は小さいが平衡位置に収束する時間が長くなるので、適当な減衰係数を選択する。
【００２６】
このようにして決定する接線方向速度差ＲＶｔと垂直方向速度差ＲＶｎを現状における対応点速度Ｖｐｒに加算してその後の移動体のベクトル速度Ｖｍを算出し、移動体の動きを制御する駆動制御装置４に設定値として伝送する（Ｓ６）。
駆動制御装置４は上記設定速度Ｖｍに従って移動体の速度を制御し（Ｓ７）、次第に予定経路に近付けるようにする。
移動体が教示経路に復帰するまで（Ｓ８）、これらの工程を繰り返す。
【００２７】
なお、これら各装置は電子計算機の部分として構成することもできる。また、各装置における操作を適当な間隔で繰り返し行うようにすることができるが、この場合は演算装置３における演算はその間隔を１倍を含む整数倍した時間後までの状態を規制するものとして、その間の制御条件を一定に保持するようにしても良い。
【００２８】
本発明の移動体経路制御装置をビジュアルサーボにより誘導する自律走行車に付設し、自律走行車がコースを逸脱したときに予定の走行経路に復帰させるために使用することができる。
予定の経路ＬはランドマークＭをプロットした地図上に記述されている。
障害物を回避した後の現在位置Ｐはランドマークの撮像画面を解析することにより求められるので、図８に示したように、その位置Ｐから地図上の予定経路Ｌに垂線を下ろす。垂線の長さが走行車から予定経路までの距離Ｄである。
【００２９】
垂線の足Ｐｒにおける予定速度Ｖｐｒに収束するように接線方向の速度差ＲＶｔに１以下の係数を掛けて新しい速度ＲＶｔｏを求め、上記の距離Ｄが短くなるように減衰係数ζを用いて新しい垂線方向速度差ＲＶｎｏを求め、これらから設定速度Ｖｍを決め次の目標位置Ｐｏまで走行車を駆動する。
【００３０】
次のサンプル時点が来ると、自律走行車の新しい位置Ｐｏに基づいてさらに次のサンプル時点までの速度条件を決めて走行車を移動させる。こうして走行車はやがて予定経路Ｌ上に復帰し、しかも復帰時の速度は予定されたものと同じ値になっている。
サンプル間隔が短いときには、サンプル時点間の走行条件を一定に保持するようにしても滑らかな運動が実現できる。
【００３１】
ビジュアルサーボを用いて誘導する場合には幾つかのランドマークＭを視野中にとらえ続けなければならないが、本発明により予定経路に復帰する間は視界が急激に変化しないためランドマークを見失うことが無い。
なお、デッドレコニングによる場合にも本発明の移動体経路制御装置を適用すれば、自己位置推定累積誤差が小さいのでより正確な誘導が可能になる。
【００３２】
また、本発明の移動体経路制御装置をロボットハンドの教示経路復帰に使用することができる。
ロボットハンドの教示経路は３次元的になることが多いが、本発明の移動体経路制御装置は３次元マップにも適用できるから、ロボットハンドを停止させることなく自動的に教示経路に復帰させることが可能である。復帰途中の経路は非常に滑らかであり、ロボットハンドが急に動き出したり急激な姿勢変化をしたりしないため周囲の人に対して安全で装置にかける負担も少ない。
【００３３】
【実施例２】
本実施例の移動体経路制御装置は、移動体の回避行動が終了して元の自律移動経路すなわち教示経路に復帰させるときに、教示経路への復帰行動を開始した地点と教示経路上の復帰目標点を結んだ適当な曲線と移動体の現在位置との関係をも参酌するようにした点のみが第１の実施例の制御装置と異なる。
【００３４】
第１の実施例では、ロボット現在位置Ｐから教示経路へ垂線を下ろし、その垂線垂線の足ＰＴとロボット現在位置Ｐの間に適当なダンパ係数ｃｔとバネ定数ｋｔを持つバネ系が介在すると仮想して運動方程式を解き、移動体の運動を決定した。
第１実施例の移動体経路制御装置を用いると、教示経路に復帰することはできるが、教示経路上の復帰位置を指定することはできず成り行きに任せるほかない。
【００３５】
ただし、運動方程式のパラメータを用いて復帰位置を推定することは可能であるから、指定した目標位置に復帰できるようなダンパ係数ｃとバネ定数ｋなどのパラメータを算出してこれを使用することにより、目標地点において教示経路に復帰するようにすることができる。
【００３６】
しかし、この方法は、ある時刻におけるロボット位置に対してパラメータｃ、ｋを変えながら何度も自動復帰計算を行って、目標点に最も近い点に復帰した時のパラメータを次の時刻における目標位置計算に使用するものであって、最適パラメータを算出するために大量の計算が必要となる。
また、この計算量はロボットの現在位置と復帰目標点の距離が離れるに従って著しく増大していく。
【００３７】
本実施例の移動体経路制御装置は、最適パラメータの算出を行う手順を経ることなくより簡単な計算により、目標点に確実に復帰するようにしたものである。
図９に表したように、本実施例は、復帰行動開始時点における初期位置ＰＯと教示経路上に定めた復帰目標点ＰＲの間を適当な緩和曲線で結び、この緩和曲線と移動体の現在位置Ｐとの関係を規制することにより移動体を目標点まで導く。
【００３８】
すなわち、ロボット現在位置Ｐから上記緩和曲線に垂線を下ろし、この垂線の足ＰＬとロボット現在位置Ｐの間に適当なダンパ係数ｃｌとバネ定数ｋｌを有する新しいバネ系が介在すると仮想して運動方程式を解き、先に述べた教示経路との間に導入したバネ系に係る運動方程式の解と合成して、移動体の次のステップにおける目標位置を決定する。
すると、移動体は復帰目標点ＰＲにおいて教示経路に復帰するようになる。
【００３９】
図では上記緩和曲線として最も単純な直線を選んでいるが、教示経路の状態と対応させて例えば放物線や指数関数曲線などを選択しても良い。
【００４０】
本実施例では、２つの仮想バネ系（ｃｌ、ｋｌ）、（ｃｔ、ｋｔ）に関する運動方程式を解く。図１０は、本実施例における運動方程式の解の関係を説明する図である。
運動方程式を表現する座標系は、仮想バネ系（ｃｌ、ｋｌ）については点ＰＬを原点とし線分ＰＯ−ＰＲに垂直な方向に主軸をとり、仮想バネ系（ｃｔ、ｋｔ）については点ＰＴを原点とし教示経路に垂直な方向に主軸をとっている。
【００４１】
仮想バネ系（ｃｔ、ｋｔ）については、第１実施例におけると同じで、教示経路に平行なベクトル成分ＶＴ−ｈは教示速度との差が徐々に減少していくように相対速度に減衰係数を掛けたものを使用し、教示経路に垂直なベクトル成分ＶＴ−ｖは前出の式（１）と同じようにして仮想バネ系の運動方程式を立てて解き、これらの結果を合成して速度ベクトル（ＶＴ−ｈ＋ＶＴ−ｖ）を求める。
【００４２】
また、仮想バネ系（ｃｌ、ｋｌ）については現在位置Ｐと垂線の足ＰＬの間に挿入したバネ系として運動方程式を立てて求めた主軸上の速度ＶＬを、仮想バネ系（ｃｔ、ｋｔ）の座標系成分、すなわち教示経路に平行な成分ＶＬ−ｈと垂直な成分ＶＬ−ｖとに分解する。
そして、この垂直成分ＶＬ−ｖを先に仮想バネ系（ｃｔ、ｋｔ）について求めた速度ベクトル（ＶＴ−ｈ＋ＶＴ−ｖ）に加え合わせた（ＶＴ−ｈ＋ＶＴ−ｖ＋ＶＬ−ｖ）を最終的な復帰運動速度ベクトルとする。
【００４３】
なお、ここで上記平行成分ＶＬ−ｈを無視するのは、既に減衰係数に基づいた速度変化が予定されている教示経路に平行な速度成分まで現在位置と点ＰＬとの距離に基づいて変化する修正項を持ち込む必要がなかったからである。
【００４４】
図１１と図１２は、上記アルゴリズムに基づいて教示経路に復帰する様子を表したグラフである。横軸に教示経路上に投影した距離を表し、縦軸に移動体から教示経路までの距離を表示している。
【００４５】
図１１は、仮想バネ系（ｃｌ、ｋｌ）のダンパ係数ｃｌを５．０、バネ定数ｋｌを０．０６、仮想バネ系（ｃｔ、ｋｔ）のダンパ係数ｃｔを７．０、バネ定数ｋｔを０．０６、教示速度１０ｃｍ／ｓｅｃ、初期速度の教示経路に平行な成分を１０ｃｍ／ｓｅｃ、初期速度の教示経路に垂直な成分を０ｃｍ／ｓｅｃとし、復帰目標位置を４００ｃｍ先に設定して、教示経路から１００ｃｍ離れた地点から教示経路上に復帰させたときの様子を示す。与えられた目標位置で教示経路に復帰していることが分かる。
【００４６】
また、図１２は、図１１で用いた条件中、初期速度の教示経路に平行な成分を１００ｃｍ／ｓｅｃに変え、復帰目標位置を３００ｃｍ先に設定したときの復帰状態を示すものである。
復帰動作の初めの段階では初期速度の影響のため教示経路方向へ近づく程度が緩やかであるが、その後は滑らかに復帰目標点に復帰できていることが分かる。
【００４７】
このように、本実施例の移動体経路制御装置は教示経路とロボット現在位置を結ぶ仮想バネ系の拘束に、復帰目標位置に関する仮想バネ系の拘束を加味したもので、複雑な演算を大量に行わなくても、教示経路から離脱した移動体を復帰動作開始時点で決めた目標点で教示経路に復帰させることができるので、実地における運行管理上極めて好ましい。
また、本実施例の移動体経路制御装置をロボットハンドの教示経路復帰に使用することもできる。
【００４８】
【発明の効果】
以上説明した通り、本発明の移動体経路制御装置により、産業用ロボットや自律走行車などが自動運転中に自律的にあるいは遠隔操作により教示経路から外れたときにも、移動体の運動を中断させずにそのまま自動運転しながらスムーズに教示経路に復帰することができるので、移動体の移動効率が大幅に向上する。
また、教示経路上に決めた復帰目標位置で正しく復帰するようにもすることができる。
【図面の簡単な説明】
【図１】本発明の移動体経路制御装置の１実施例を説明するブロック図である。
【図２】本実施例における演算手順を示すフロー図である。
【図３】本実施例におけるアルゴリズムを説明する図面である。
【図４】図３の一部を詳細に示した図面である。
【図５】本実施例における演算に使用する係数の例を示すグラフである。
【図６】本実施例における演算の概念を示す図面である。
【図７】本実施例を用いた移動体が予定経路に接近していく状況を説明するグラフである。
【図８】本実施例をビジュアルサーボで用いた場合について説明する図面である。
【図９】本発明の移動体経路制御装置の第２の実施例を説明する線図である。
【図１０】本実施例におけるアルゴリズムを説明する図面である。
【図１１】本実施例による作用結果を説明するグラフである。
【図１２】本実施例による別の作用結果を説明するグラフである。
【符号の説明】
１ 位置認識装置
２ 経路記憶装置
３ 演算装置
４ 駆動制御装置
５ 経路教示部

Claims (8)

1. 移動体の現在位置を求める位置認識装置と、移動体の予定経路を各点における移動方向と速さと共に記憶する記憶装置と、演算装置と、駆動制御装置とを備え、演算装置が現在位置から最短距離にある予定経路上の対応点における移動方向と速さを求めて移動体の現在の移動方向と速さとの差が単純減少するような値に移動体移動方向と速さを設定し、駆動制御装置がこの設定方向と速さに基づいて移動体を移動制御することにより、予定経路に復帰させることを特徴とする移動体経路制御装置。
2. 前記設定移動方向と速さは、予定経路上の移動方向に平行な成分と垂直な成分に分解して、それぞれの差が単純減少するような値に設定することを特徴とする請求項１記載の移動体経路制御装置。
3. さらに、前記演算装置が、移動体の復帰を始める時点における移動体位置と予定経路上に設定された復帰目標点を結ぶ緩和曲線を算出して前記記憶装置に記憶させ、移動体の現在位置から最短距離にある該緩和曲線上の対応点までの距離が単純減少するように働く速度成分を算出して、該速度成分を用いて前記予定経路上の対応点における値を基準として算出された移動体移動方向と速さを修正することを特徴とする請求項１または２記載の移動体経路制御装置。
4. 請求項１記載の移動体経路制御装置において、前記演算装置が、現在位置から最短距離にある予定経路上の対応点における移動方向と速さと移動体の現在の移動方向と速さとの差が単純減少するような値に移動体移動方向と速さを設定する代わりに、前記設定移動速度の予定経路上の対応点における移動方向に垂直な成分、あるいは前記緩和曲線上の対応点までの距離が減少するようにする速度成分を、移動体の現在位置と予定経路上または緩和曲線上の対応点との距離をＤ、時間をｔ、またｃとｋを係数として、運動方程式
   ｄ^２Ｄ／ｄｔ^２＋ｃｄＤ／ｄｔ＋ｋＤ＝０
   の解として求めることを特徴とする移動体経路制御装置。
5. 前記運動方程式の解を、ζ＝ｃ／２ｋ^１／２≧１ なる条件の下で求めることを特徴とする請求項４記載の移動体経路制御装置。
6. 前記演算装置における演算は所定の間隔ごとに行うことを特徴とする請求項１から５のいずれかに記載の移動体経路制御装置。
7. 前記移動体が自律走行車であって走行経路に復帰するように制御することを特徴とする請求項１から６のいずれかに記載の移動体経路制御装置。
8. 前記移動体がロボットハンドであって教示経路に復帰するように制御することを特徴とする請求項１から６のいずれかに記載の移動体経路制御装置。

Images