JP2008105132A - アームの関節空間における経路を生成する方法と装置 - Google Patents

Abstract

【課題】 関節空間の次数が大きい場合や、関節空間を詳細に離散化して表現した場合であっても、少ない計算コストで関節空間におけるアームの経路を生成することが可能な技術を提供する。
【解決手段】 複数の関節を備えるアームの関節空間における経路を生成する方法であって、経路の開始点の位置を決定する工程と、経路の目標点の位置を決定する工程と、障害物の位置と形状を決定する工程と、経路の開始点を評価対象点とする工程と、（Ａ１）評価対象点と目標点の間に障害物が存在しない場合に、評価対象点から目標点まで直線的に経路を進展させる工程と、（Ａ２）評価対象点と目標点の間に障害物が存在する場合に、評価対象点から見た目標点の方向から傾いた方向に直線的に経路を進展させて、進展させた経路の先端を新たな評価対象点とする工程と、上記の（Ａ１）と（Ａ２）の各工程を、開始点から進展した経路が目標点に到達するまで繰り返し行う工程を備える。
【選択図】 図２

Description

本発明は、複数の関節を備えるアームの関節空間における経路を生成する方法と装置に関する。なお、本明細書ではアームが存在する現実の空間を作業空間と呼び、アームの各関節の関節角度あるいは関節伸縮長さを元とする仮想的な空間を関節空間と呼ぶ。例えばアームの各関節の自由度の合計がｎである場合には、関節空間はｎ次元の空間として規定される。関節空間上の点はアームの各関節の関節角度あるいは関節伸縮長さを表現しており、関節空間上の一点が定まると作業空間におけるアームの姿勢が定まる。アームの関節空間における経路が定まると、アームの作業空間における姿勢の変化態様が定まる。

現在、家庭やオフィスなどで人間の代わりに作業したり、人間が行う作業の補助をすることを目的としたパートナーロボットの開発が盛んに行われている。このようなロボットでは、人間と同じ様に、種々の動作を行うことが期待されている。例えば、人間の代わりに荷物を搬送する動作をロボットが行う場合、まずロボット本体がその荷物の近傍まで移動し、次いでロボット本体に搭載されているアームを駆動してその荷物を把持し、荷物を把持した状態でロボット本体を移動する。このような動作を実現するためには、ロボット本体を目標物に向けて移動させる技術に加えて、ロボット本体に搭載されているアームを目標物に向けて移動させる技術が必要とされる。

アームの経路を生成する場合、移動体そのものの経路を生成する場合とは事情が異なる。アームの経路の生成では、周囲の障害物との干渉を判断する際に、アームの先端だけではなく、移動体からアームの先端に至るまでのアーム全体での障害物との干渉を考慮しなければならない。従って、作業空間においてアームの経路を生成する際には、マルコフ性が保証されない場合が多く、最適な経路を生成することが困難な場合が多い。そこで、移動体に搭載されたアームについては、ＲＲＴ（Rapidly-exploring Random Tree）法を用いて、関節空間における経路を生成する手法が従来より用いられている。

ＲＲＴ法では、関節空間における経路の開始点と目標点のそれぞれからツリーを進展させていく。それぞれのツリーは関節空間にランダムに設定された中間点に向けて、障害物と干渉しない限り進展していく。ツリーの進展途中に、障害物と干渉するか、中間点まで到達すると、新たな中間点をランダムに設定する。これを繰り返すことによって、開始点から伸びるツリーと目標点から伸びるツリーが徐々に成長していく。開始点から伸びるツリーと目標点から伸びるツリーが交差した時点で、開始点から目標点に至る１つの経路が決定される。この手法によれば、関節空間内に障害物が存在する場合であっても、開始点から目標点に至る経路を、簡単なアルゴリズムで生成することができる。なお関節空間における障害物とは、各関節の可動範囲を超える領域や、作業空間において現実の障害物あるいはアームそれ自身と干渉してしまうアームの姿勢に相当する領域を意味する。ＲＲＴ法の詳細については、例えば非特許文献１や非特許文献２に記載されている。

James J. Kuffner, "Efficient Optimal Search of Uniform-Cost Grids and Lattices", Proc. Of IEEE Intl'l Conf. on Robotics and Automation (ICRA2004), September 2004 J. Kuffner and S.M. LaValle, "RRT-Connect: An efficient approach to single-query path planning", Video Proc. of IEEE Int'l Conf. on Robotics and Automation (ICRA'2000), April 2000

ＲＲＴ法によって経路を生成する場合、周囲の状況と関係なく、関節空間においてランダムに中間点を設定する。従って、関節空間の次数が大きい場合や、関節空間を詳細に離散化して表現した場合には、多数の候補点の中からランダムに中間点が設定され、ツリーが関節空間の全方向に向かってばらばらに進展し、最終的な経路の生成に要する計算コストが膨大なものとなってしまう。このため、ＲＲＴ法による経路の生成では、関節空間において開始点と目標点の間にそれほど大きな障害物が存在しない場合であっても、経路を生成するために膨大な計算を必要とする。従って、より少ない計算コストで経路を生成することが可能な技術が待望されている。

本発明は、上述した問題を解決するためのものであり、関節空間の次数が大きい場合や、関節空間を詳細に離散化して表現した場合であっても、少ない計算コストで関節空間におけるアームの経路を生成することが可能な技術を提供する。

本発明は複数の関節を備えるアームの関節空間における経路を生成する方法として具現化される。本発明の経路生成方法は、経路の開始点の位置を決定する工程と、経路の目標点の位置を決定する工程と、障害物の位置と形状を決定する工程と、経路の開始点を評価対象点とする工程と、（Ａ１）評価対象点と目標点の間に障害物が存在しない場合に、評価対象点から目標点まで直線的に経路を進展させる工程と、（Ａ２）評価対象点と目標点の間に障害物が存在する場合に、評価対象点から見た目標点の方向から傾いた方向に直線的に経路を進展させて、進展させた経路の先端を新たな評価対象点とする工程と、上記の（Ａ１）と（Ａ２）の各工程を、開始点から進展した経路が目標点に到達するまで繰り返し行う工程を備えている。

本発明の経路生成方法では、関節空間における開始点の位置と、目標点の位置と、障害物の位置と形状をそれぞれ決定した後に、開始点から目標点に向けて経路を進展させる。本発明の経路生成方法では、始点から伸びる経路の先端を評価対象点として、評価対象点から経路を進展させ、進展した経路の先端を新たに評価対象点として、順次経路を進展させていく。評価対象点から目標点に向けて直線的に経路を進展させることができる場合には、その評価対象点から目標点に向けて直線的に経路を進展させる。評価対象点から目標点に向けて直線的に経路を進展させることができない場合には、その評価対象点から見た目標点の方向よりも傾いた方向に経路を進展させる。このような経路の進展を繰り返すことによって、開始点から伸びる経路は障害物を迂回しながら目標点に到達する。

本発明の経路生成方法は、関節空間における開始点と目標点の間にそれほど大きな障害物が存在しない場合に、特に効果的である。ＲＲＴ法を用いた経路生成方法では、開始点と目標点の間にそれほど大きな障害物が存在しない場合であっても、常に中間点をランダムに設定していくために、関節空間の次数が大きい場合や、関節空間を詳細に離散化して表現した場合には、経路を生成するために膨大な計算を必要とする。それに対して、本発明の経路生成方法では、開始点と目標点の間にそれほど大きな障害物が存在しない場合には、迅速に経路を生成することが可能である。関節空間の次数が大きい場合や、関節空間を詳細に離散化して表現した場合であっても、少ない計算コストで経路を生成することができる。

上記した経路生成方法においては、前記（Ａ２）の工程が、評価対象点と目標点の間に障害物が存在する場合に、以下の（Ｂ１）から（Ｂ３）の各工程を行う工程を備えることが好ましい。ここで、（Ｂ１）の工程とは、評価対象点から見た目標点の方向に沿って、評価対象点から障害物までの距離を算出する工程であり、（Ｂ２）の工程とは、評価対象点から見た目標点の方向から所定角度傾いた方向に、評価対象点から前記距離だけ経路を進展させる工程であり、（Ｂ３）の工程とは、進展させた経路の先端を新たな評価対象点とする工程である。

上記の経路生成方法では、目標点の方向よりも傾いた方向に経路を進展させる場合に、進展させる経路の長さを、評価対象点から目標点の方向に経路を進展させた場合に障害物と干渉するまでの距離としている。このように経路を生成することで、評価対象点から障害物までの距離が近いほど、経路を小刻みに進展させていくため、障害物の形状に沿って経路を迂回させることができる。より無駄の少ない経路を生成することができる。

本発明は、複数の関節を備えるアームの関節空間における経路を生成する装置としても具現化される。本発明の経路生成装置は、経路の開始点の位置を決定する手段と、経路の目標点の位置を決定する手段と、障害物の位置と形状を決定する手段と、開始点から目標点へ到達する経路を決定する手段を備えている。開始点から目標点へ到達する経路を決定する手段は、経路の開始点を評価対象点とする工程と、（Ａ１）評価対象点と目標点の間に障害物が存在しない場合に、評価対象点から目標点まで直線的に経路を進展させる工程と、（Ａ２）評価対象点と目標点の間に障害物が存在する場合に、評価対象点から見た目標点の方向から傾いた方向に直線的に経路を進展させて、進展させた経路の先端を新たな評価対象点とする工程と、上記の（Ａ１）と（Ａ２）の各工程を、開始点から進展した経路が目標点に到達するまで繰り返し行う工程を実行する。

本発明の方法と装置によれば、関節空間の次数が大きい場合や、関節空間を詳細に離散化して表現した場合であっても、少ない計算コストで関節空間におけるアームの経路を生成することができる。

本発明の好適な実施形態を例示する。
（形態１）アームは移動体に搭載されている。
一般に、移動体の位置決め精度はアームの位置決め精度に比べて低く、移動体の位置決め誤差がアームの作業空間における環境の変化として認識される場合がある。このような場合でも、本発明の経路生成方法によれば、適切なアームの経路を少ない計算コストで生成することができる。
（形態２）関節空間における障害物の位置と形状は、移動体から周囲の環境までの距離の計測値に基づいて決定される。

（第１実施例）
本発明の経路生成方法および装置の実施例について、図面を参照しながら説明する。図１は本実施例に係るロボット１００の構成を示している。ロボット１００は、カメラ１０２、１０４、画像処理部１０６、主制御部１１０、アーム制御部１１２、車輪制御部１１４、アクチュエータ群１１６、モータ１１８、１２０、アーム１２２、右車輪１２４、左車輪１２６、エンコーダ群１２８、エンコーダ１３０、１３２を備えている。

ロボット１００の本体は、右車輪１２４、左車輪１２６および図示されない補助輪によって支持されており、右車輪１２４、左車輪１２６をそれぞれモータ１１８、モータ１２０によって回転させることによって、転倒することなく移動することができる。右車輪１２４はモータ１１８の駆動によって回転し、左車輪１２６はモータ１２０の駆動によって回転する。右車輪１２４と左車輪１２６は互いに独立して回転可能である。右車輪１２４と左車輪１２６の回転を制御することによって、ロボット１００の本体は前進したり、後進したり、旋回したりすることができる。

車輪制御部１１４は、モータ１１８およびモータ１２０の駆動を制御する。車輪制御部１１４は、主制御部１１０から右車輪１２４と左車輪１２６の回転角度の時系列データが入力されると、その回転角度の時系列データに従って、モータ１１８およびモータ１２０を駆動する。

右車輪１２４にはエンコーダ１３０が設けられている。左車輪１２６にはエンコーダ１３２が設けられている。エンコーダ１３０およびエンコーダ１３２は、それぞれ車輪の回転角度を検出して、車輪制御部１１４へ出力する。車輪制御部１１４はそれぞれの車輪の回転角度を主制御部１１０へ出力する。

アーム１２２は、複数の回転関節を備えるリンク機構であり、その基部は回転関節によってロボット１００の本体に対して揺動可能となっている。アーム１２２は、その先端に把持部（図示されない）を備えている。アーム１２２の把持部は、それぞれが複数の回転関節を備える指部材を備えている。ロボット１００は、アーム１２２の各回転関節をアクチュエータ群１１６によって回転させることで、ロボット１００の本体に対するアーム１２２の姿勢を変化させることができる。以下ではアーム１２２の各回転関節の自由度の合計をｎとして、アーム１２２の各関節の関節角度を（θ１、θ２、・・・、θｎ）で表現する。

アーム制御部１１２は、アクチュエータ群１１６の駆動を制御する。アーム制御部１１２は、主制御部１１０からアーム１２２の各関節の関節角度の時系列データが入力されると、その関節角度の時系列データに従って、アクチュエータ群１１６を駆動する。

アーム１２２の各関節には、それぞれの関節に対応するエンコーダ群１２８が設けられている。エンコーダ群１２８は各関節の関節角度を検出して、アーム制御部１１２へ出力する。アーム制御部１１２は、各関節の関節角度を主制御部１１０へ出力する。

カメラ１０２とカメラ１０４は、一般的なＣＣＤカメラである。カメラ１０２とカメラ１０４は、ロボット１００の本体に、左右に並んで搭載されている。カメラ１０２およびカメラ１０４は、所定の時間間隔で撮影を行う。カメラ１０２およびカメラ１０４で撮影された画像は、画像処理部１０６へ送信される。

画像処理部１０６は、カメラ１０２とカメラ１０４が同時刻に撮影した画像から、ステレオ視の原理によって、ロボット１００から周囲の環境までの距離を計測する。周囲の環境までの距離は、ロボット１００の本体に設けられている基準点からの距離として算出される。本実施例では、ロボット１００の基準点はロボット１００の本体に対して固定されている。また画像処理部１０６は、カメラ１０２とカメラ１０４で撮影された画像から、目標物の位置と形状を認識する。さらに画像処理部１０６は、ロボット１００から周囲の環境までの距離と、目標物の位置と形状に基づいて、アーム１２２を目標物に向けて駆動する際の障害物の位置と形状を決定する。画像処理部１０６は、カメラ１０２およびカメラ１０４で撮影された画像を表現するデータと、それらの画像に基づいて取得された目標物の位置と形状のデータと、障害物の位置と形状のデータを、主制御部１１０へ送信する。

主制御部１１０は、アーム１２２の把持部によって目標物を把持するために、アーム１２２の経路の生成を行う。本実施例でアーム１２２の経路とは、アーム１２２の各関節の関節角度（θ１、θ２、・・・、θｎ）を元とする関節空間における経路のことをいう。以下では図２のフローチャートを参照しながら、主制御部１１０が行うアーム１２２の経路生成処理について説明する。

ステップＳ２０２では、関節空間における経路の開始点Ｓの位置として、アーム１２２の初期関節角度（θ１Ｓ、θ２Ｓ、・・・、θｎＳ）を決定する。本実施例では、アーム１２２の初期関節角度（θ１Ｓ、θ２Ｓ、・・・、θｎＳ）は、エンコーダ群１１８から入力されるアーム１２２の各関節の関節角度の計測値に設定される。

ステップＳ２０４では、作業空間における障害物の位置と形状と、作業空間における目標物の位置と形状から、関節空間における経路の目標点Ｇの位置として、アーム１２２の最終関節角度（θ１Ｅ、θ２Ｅ、・・・、θｎＥ）を決定する。本実施例では、アーム１２２の最終関節角度（θ１Ｅ、θ２Ｅ、・・・、θｎＥ）は、アーム１２２が最終的に目標物を把持する際の各関節の関節角度に設定される。アーム１２２が最終的に目標物を把持する際の各関節の関節角度は、例えば作業空間における障害物の位置と形状と、作業空間における目標物の位置と形状に基づいて、アーム１２２が障害物と干渉することなく目標物を把持している状態を想定して、その状態におけるアーム１２２についての逆キネマティクスを解くことによって、決定することができる。

ステップＳ２０６では、作業空間における障害物の位置と形状と、アーム１２２の各関節の可動範囲から、関節空間における障害物Ｏの位置と形状を決定する。

図３はステップＳ２０２からステップＳ２０６の処理によって決定された、関節空間における開始点Ｓと、目標点Ｇと、障害物Ｏを例示している。図３では図示の簡略化のために、関節空間をθ１とθ２の二次元で表現しているが、実際の処理では関節空間はθ１、θ２、・・・、θｎのｎ次元で表現されることに留意されたい。

図２のステップＳ２０８では、評価対象点Ｔを開始点Ｓに設定する。ステップＳ２１０では、中間点の順序を示す整数ｉを０に設定する。

ステップＳ２１２では、評価対象点Ｔから経路を進展させる方向（経路進展方向）γｉを、評価対象点Ｔから目標点Ｇへの方向に設定する。

ステップＳ２１４では、評価対象点Ｔから経路進展方向γｉに沿って経路を伸ばす場合に、目標点Ｇへ到達する前に障害物Ｏと干渉するか否かを判断する。障害物Ｏと干渉する場合（ステップＳ２１４でＹＥＳの場合）、評価対象点Ｔから目標点Ｇへは直線的な経路では到達できないため、新たな中間点Ｐｉを生成するために処理はステップＳ２１６へ進む。障害物Ｏと干渉しない場合（ステップＳ２１４でＮＯの場合）、評価対象点Ｔから目標点Ｇへは直線的な経路で到達できるため、これ以上中間点Ｐｉを生成する必要がなく、処理はステップＳ２２８へ進む。

ステップＳ２１６では、評価対象点Ｔから経路進展方向γｉに沿って伸ばした経路が障害物Ｏと干渉する点を、干渉点Ｑｉとして算出する。

ステップＳ２１８では、評価対象点Ｔから干渉点Ｑｉまでの距離Ｒｉを算出する。

ステップＳ２２０では、評価対象点Ｔから障害物Ｏを迂回して経路を伸ばすために、経路進展方向γｉを修正する。ステップＳ２２０では、ステップＳ２１２で設定された経路進展方向γｉを、所定の回転ベクトルΔγ（以下では迂回ベクトルと記載する）によって傾ける修正を行う。迂回ベクトルΔγは、経路を傾ける向きと傾ける角度を規定するベクトル量である。図面中では、図示の簡略化のために、便宜上、迂回ベクトルによって傾ける角度をΔγと示している。

ステップＳ２２２では、評価対象点Ｔから修正後の経路進展方向γｉに沿って距離Ｒｉだけ進んだ点を、中間点Ｐｉ＋１として決定する。

ステップＳ２２４では、評価対象点Ｔを、新たに中間点Ｐｉ＋１に設定する。ステップＳ２２６でｉを１増やした後、処理はステップＳ２１２へ移行して、上述の処理を繰り返し実行する。

ステップＳ２２８では、生成された経路における屈曲を低減するために、中間点Ｐ１、Ｐ２、・・・について整理を行う。ステップＳ２２８では、中間点Ｐ１、Ｐ２、・・・のそれぞれについて、目標点Ｇから近い順に、開始点Ｓから直線的に接続可能か否かを判断する。開始点Ｓから直線的に接続可能な中間点が存在する場合には、開始点Ｓからその中間点までの間に直線的な経路を新たに生成し、開始点Ｓとその中間点との間に存在する中間点を経路から削除する。その後、その中間点を評価開始点Ｅとして、再度、残余の中間点それぞれについて、目標点Ｇから近い順に、評価開始点Ｅから直線的に接続可能か否かを判断する。評価開始点Ｅから直線的に接続可能な中間点が存在する場合には、評価開始点Ｅからその中間点までの間に直線的な経路を新たに生成し、その中間点を新たな評価開始点Ｅとする。評価開始点Ｅが目標点Ｇに到達すれば、中間点の整理を終了する。上記の中間点の整理を行うことで、開始点Ｓから目標点Ｇまで到達する元の経路より短い１つの経路が決定される。

ステップＳ２３０では、開始点Ｓと、中間点Ｐ１、Ｐ２、・・・と、目標点Ｇの位置を出力する。上述した処理を実行した後、主制御部１１０はアーム１２２の経路生成処理を終了する。

図３と図４に、上記の処理によって経路が生成される様子を模式的に示している。図３に示すように、関節空間において開始点Ｓ、目標点Ｇ、障害物Ｏがそれぞれ決定されると、まず評価対象点Ｔが開始点Ｓに設定され、経路進展方向γ０が開始点Ｓから目標点Ｇへの方向に設定される。図３に示す例では、開始点Ｓから経路進展方向γ０に沿って伸びる経路が障害物Ｏと干渉点Ｑ０で干渉するため、経路進展方向γ０が所定の迂回ベクトルΔγによって修正される。図３では、修正後の経路進展方向をγ’０と図示している。そして、開始点Ｓから修正後の経路進展方向γ’０に沿って、距離Ｒ０だけ進んだ位置に中間点Ｐ１が決定される。距離Ｒ０は、開始点Ｓから干渉点Ｑ０までの距離に等しい。その後、図４に示すように、評価対象点Ｔが中間点Ｐ１に移り、経路進展方向γ１が中間点Ｐ１から目標点Ｇへの方向に設定される。図４に示す例では、中間点Ｐ１から経路進展方向γ１に沿って伸びる経路も障害物Ｏと干渉点Ｑ１で干渉するため、経路進展方向γ１が修正されて、修正された経路進展方向に沿って距離Ｒ１だけ進んだ位置に中間点Ｐ２が決定される。距離Ｒ１は、中間点Ｐ１から干渉点Ｑ１までの距離に等しい。その後、評価対象点Ｔが中間点Ｐ２に移る。中間点Ｐ２からは、障害物Ｏと干渉することなく目標点Ｇまで直線的に到達することができる。これによって、開始点Ｓから中間点Ｐ１、Ｐ２を経て目標点Ｇへ到達する経路が生成されたことになる。

図２の処理によって出力された開始点Ｓ、中間点Ｐ１、Ｐ２、・・・、目標点Ｇで表現される経路は、迂回ベクトルΔγの値に応じて生成されたものであり、迂回ベクトルΔγを異なる値として図２の処理を実施すると、生成される経路も異なるものになる。本実施例では、主制御部１１０は迂回ベクトルΔγを所定のステップ幅で変化させながら上記した経路生成処理を繰り返し実施し、複数の経路を生成する。そして、生成された複数の経路のうちで、開始点Ｓから目標点Ｇまでの関節空間における距離が最短となるものを、アーム１２２の経路として決定する。

主制御部１１０は、開始点Ｓ、中間点Ｐ１、Ｐ２、・・・、目標点Ｇの順に、それぞれの点が表現する各関節の関節角度が順に実現されていくように、アーム制御部１１２に各関節の関節角度の時系列データを出力する。これによって、アーム１２２は関節空間における経路に沿った動作態様で動作し、作業空間において障害物と衝突することなく、目標物を把持する動作を実現することができる。

本実施例の経路生成方法によって、アーム１２２の関節空間における開始点Ｓから目標点Ｇに至る経路を取得することができる。本実施例の経路生成方法において、経路が生成されるまでに必要な計算量は、関節空間の次数ｎや、関節空間の分解能にそれほど依存せず、障害物Ｏの位置と形状に依存する。障害物Ｏが存在しない場合や、障害物Ｏがそれほど大きくない場合には、少ない計算量で速やかに経路を生成することができる。

（第２実施例）
本実施例は、第１実施例とほぼ同様であるが、アーム１２２の経路生成処理において、関節空間における開始点Ｓの位置と、目標点Ｇの位置と、障害物Ｏの位置と形状が取得された後の処理が異なる。以下では第１実施例と相違する点についてのみ詳細に説明し、第１実施例と共通する点については、同じ参照符号を付して詳細な説明を省略する。

本実施例の主制御部１１０が行うアーム１２２の経路の生成処理について、図５を参照しながら説明する。

ステップＳ２０２からステップＳ２０６では、主制御部１１０は関節空間における開始点Ｓの位置と、目標点Ｇの位置と、障害物Ｏの位置と形状を決定する。

ステップＳ２０８では、評価対象点Ｔを開始点Ｓに設定する。ステップＳ２１０では、中間点の順序を示す整数ｉを０に設定する。

ステップＳ２１２では、評価対象点Ｔからの経路進展方向γｉを、評価対象点Ｔから目標点Ｇへの方向に設定する。

ステップＳ５１４では、評価対象点Ｔから経路進展方向γｉに沿って所定の経路長ΔＬだけ経路を伸ばす場合に、途中で障害物Ｏと干渉するか否かを判断する。障害物Ｏと干渉しない場合（ステップＳ５１４でＮＯの場合）、経路進展方向γｉを修正する必要がないため、処理はステップＳ５１６へ進む。障害物Ｏと干渉する場合（ステップＳ５１４でＹＥＳの場合）、経路進展方向γｉを修正する必要があるため、処理はステップＳ５２０へ進む。

ステップＳ５１６では、評価対象点Ｔから経路進展方向γｉに沿って経路長ΔＬだけ経路を進展させることで、目標点Ｇに到達するか否かを判断する。目標点Ｇに到達する場合（ステップＳ５１６でＹＥＳの場合）、これ以上中間点を生成する必要がないから、処理はステップＳ５３２へ進む。目標点Ｇに到達しない場合（ステップＳ５１６でＮＯの場合）、経路進展方向γｉに沿って経路を進展させるために、処理はステップＳ５２６へ進む。

ステップＳ５２６では、評価対象点Ｔから経路進展方向γｉに沿って経路長ΔＬだけ進んだ点を、中間点Ｐｉ＋１として決定する。

ステップＳ５２８では、評価対象点Ｔを、新たに中間点Ｐｉ＋１に設定する。ステップＳ５３０でｉを１増やした後、処理はステップＳ２１２へ移行して、上述の処理を繰り返し実行する。

ステップＳ５１４で障害物と干渉すると判断された場合（ＹＥＳの場合）、ステップＳ５１８以降の処理を実行して、既に生成されている経路を一旦後戻りして、障害物を迂回する方向に改めて経路を進展させる。

ステップＳ５１８では、中間点の順序を示す整数ｉを、所定のバックトラック量ＮＢだけ減ずる。

ステップＳ５２０では、評価対象点Ｔを中間点Ｐｉに改めて設定する。

ステップＳ５２２では、評価対象点Ｔから障害物Ｏを迂回して経路を伸ばすために、経路進展方向γｉを修正する。ステップＳ５２２では、既に設定されている経路進展方向γｉを、所定の迂回ベクトルΔγによって傾ける修正を行う。経路進展方向γｉを修正した後、処理はステップＳ５２６へ移行し、改めて中間点Ｐｉ＋１を設定する。ステップＳ５２８で評価対象点Ｔを改めて設定された中間点Ｐｉ＋１に設定し、ステップＳ５３０でｉを１増加させて、ステップＳ２１２に戻る。

ステップＳ５１６で目標点Ｇに到達すると判断された場合（ＹＥＳの場合）、ステップＳ５３２で生成された経路について中間点の整理を行った後に、ステップＳ２３０で生成された経路を出力する。
ステップＳ５３２では、生成された経路における屈曲を低減するために、中間点Ｐ１、Ｐ２、・・・について整理を行う。ステップＳ５３２では、中間点Ｐ１、Ｐ２、・・・のうちで、経路が屈曲している点（以下では屈曲中間点という）を抽出する。ある中間点Ｐｉで経路が屈曲しているか否かは、その中間点Ｐｉでの経路進展方向γｉが、その直前の中間点Ｐｉ−１での経路進展方向γｉ−１と相違するか否かによって判断される。そして、抽出された屈曲中間点のそれぞれについて、目標点Ｇから近い順に、開始点Ｓから直線的に接続可能か否かを判断する。開始点Ｓから直線的に接続可能な屈曲中間点が存在する場合には、開始点Ｓからその屈曲中間点までの間に直線的な経路を新たに生成して、開始点Ｓとその屈曲中間点の間の中間点を、その直線的な経路上に再配置する。その後、その屈曲中間点を新たな評価開始点Ｅとして、再度、目標点Ｇから近い順に、それぞれの屈曲中間点について、評価開始点Ｅから直線的に接続可能か否かを判断する。評価開始点Ｅから直線的に接続可能な屈曲中間点が存在する場合には、評価開始点Ｅからその屈曲中間点までの間に直線的な経路を新たに生成して、評価開始点Ｅとその屈曲中間点の間の中間点を、同様にその直線的な経路上に再配置する。さらにその後、その屈曲中間点を新たな評価開始点Ｅとして、上記した中間点の整理を繰り返す。評価開始点Ｅが目標点Ｇに到達すれば、中間点の整理を終了する。上記の中間点の整理を行うことで、開始点Ｓから目標点Ｇまで到達する元の経路より短い１つの経路が決定される。

ステップＳ２３０では、開始点Ｓ、中間点Ｐ１、Ｐ２、・・・、目標点Ｇを出力する。上述した処理を実行した後、主制御部１１０はアーム１２２の経路生成処理を終了する。

図６から図８に、上記の処理によって経路が生成される様子を模式的に示している。図６に示すように、関節空間において開始点Ｓ、目標点Ｇ、障害物Ｏがそれぞれ決定されると、まず評価対象点Ｔが開始点Ｓに設定されて、開始点Ｓから目標点Ｇに向けて直線的に経路が進展していく。この際に、経路長ΔＬ毎に中間点Ｐ１、Ｐ２、Ｐ３、Ｐ４、Ｐ５が設定されていき、評価対象点Ｔは新たに設定された中間点へと順に移行していく。

図７に示すように、評価対象点Ｔが中間点Ｐ５に設定された時点で、中間点Ｐ５から目標点Ｇの方向に経路を伸ばすと障害物Ｏと干渉するため、評価対象点Ｔは中間点Ｐ５から中間点Ｐ３まで後戻りして（バックトラック量ＮＢ＝２に相当する）、中間点Ｐ３から経路を迂回させる。中間点Ｐ３における経路進展方向γ３が迂回ベクトルΔγによって修正されて、修正後の経路進展方向に沿って中間点Ｐ’４、Ｐ’５が新たに設定され、その後の中間点Ｐ６、Ｐ７、Ｐ８、Ｐ９が設定されていく。なお図７では最初に設定された中間点Ｐ４，Ｐ５との区別をつけるために、新たに設定された中間点をＰ’４、Ｐ’５と表記しているが、実際の処理においては中間点Ｐ４、Ｐ５は新たに設定された中間点Ｐ’４、Ｐ’５に置き換えられている。評価対象点Ｔが中間点Ｐ９に設定されると、中間点Ｐ９から目標点Ｇまでは直線的に経路を接続することが可能であるから、中間点Ｐ９から目標点Ｇまで、経路長ΔＬ毎に中間点Ｐ１０からＰ１６が設定されていく。

開始点Ｓから中間点Ｐ１、Ｐ２、・・・、Ｐ１６を順に経由して目標点Ｇに至る経路が生成されると、図８に示すように中間点Ｐ１、Ｐ２、・・・、Ｐ１６の整理が行われる。中間点Ｐ１、Ｐ２、・・・、Ｐ１６のうち、経路が屈曲している屈曲中間点はＰ３とＰ９である。このうち、中間点Ｐ９は開始点Ｓと直線的に接続可能であるから、開始点Ｓと中間点Ｓ９を直線的に接続する経路が新たに設定されて、新たに設定された経路上に、中間点Ｐ”１、Ｐ”２、・・・、Ｐ”８が設定される。それまでに設定されていた中間点Ｐ１、Ｐ２、・・・、Ｐ８は、新たに設定された中間点Ｐ”１、Ｐ”２、・・・、Ｐ”８に置き換えられる。これによって、開始点Ｓから、中間点Ｐ”１、Ｐ”２、・・・、Ｐ”８、Ｐ９、Ｐ１０、・・・Ｐ１６を順に経由して、目標点Ｇに至る経路が生成されたことになる。このように中間点の整理を行うことによって、より短い距離の経路を生成することができる。

図５の処理によって出力された開始点Ｓ、中間点Ｐ１、Ｐ２、・・・、目標点Ｇで表現される経路は、迂回ベクトルΔγ、バックトラック量ＮＢ、経路長ΔＬの取り方によって異なる。本実施例では、主制御部１１０は迂回ベクトルΔγ、バックトラック量ＮＢ、経路長ΔＬをそれぞれ所定のステップ幅で変化させながら図５の経路生成処理を繰り返し実施し、複数の経路を生成する。そして、生成された経路のうちで、開始点Ｓから目標点Ｇまでの関節空間における距離が最短となるものを、アーム１２２の経路として決定する。

本実施例の経路生成方法によっても、アーム１２２の関節空間における開始点Ｓから目標点Ｇに至る経路を取得することができる。本実施例の経路生成方法において、経路が生成されるまでに必要な計算量は、関節空間の次数ｎや、関節空間の分解能にそれほど依存せず、障害物Ｏの位置と形状に依存する。障害物Ｏが存在しない場合や、障害物Ｏがそれほど大きくない場合には、少ない計算量で速やかに経路を生成することができる。

上記した第１実施例および第２実施例では、周囲の環境までの距離を計測する際に、２台のカメラ１０２、１０４を利用したステレオ視によって距離を計測する例を説明した。周囲の環境までの距離を計測する手法はこれに限らず、例えばレーザースキャナを用いた３次元計測によって、周囲の環境までの距離を計測してもよい。

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時の請求項に記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

図１はロボット１００の構成を概略的に示す図である。 図２は関節空間におけるアーム１２２の経路を生成する方法のフローチャートである。（第１実施例） 図３は関節空間におけるアーム１２２の経路の生成の様子を模式的に示す図である。（第１実施例） 図４は関節空間におけるアーム１２２の経路の生成の様子を模式的に示す図である。（第１実施例） 図５は関節空間におけるアーム１２２の経路を生成する方法のフローチャートである。（第２実施例） 図６は関節空間におけるアーム１２２の経路の生成の様子を模式的に示す図である。（第２実施例） 図７は関節空間におけるアーム１２２の経路の生成の様子を模式的に示す図である。（第２実施例） 図８は関節空間におけるアーム１２２の経路の生成の様子を模式的に示す図である。（第２実施例）

符号の説明

１００：ロボット
１０２、１０４：カメラ
１０６：画像処理部
１１０：主制御部
１１２：アーム制御部
１１４：車輪制御部
１１６：アクチュエータ群
１１８、１２０：モータ
１２２：アーム
１２４：右車輪
１２６：左車輪
１２８：エンコーダ群
１３０、１３２：エンコーダ

Claims (3)

1. 複数の関節を備えるアームの関節空間における経路を生成する方法であって、
   経路の開始点の位置を決定する工程と、
   経路の目標点の位置を決定する工程と、
   障害物の位置と形状を決定する工程と、
   経路の開始点を評価対象点とする工程と、
   （Ａ１）評価対象点と目標点の間に障害物が存在しない場合に、評価対象点から目標点まで直線的に経路を進展させる工程と、
   （Ａ２）評価対象点と目標点の間に障害物が存在する場合に、評価対象点から見た目標点の方向から傾いた方向に直線的に経路を進展させて、進展させた経路の先端を新たな評価対象点とする工程と、
   上記の（Ａ１）と（Ａ２）の各工程を、開始点から進展した経路が目標点に到達するまで繰り返し行う工程を備える、経路生成方法。
2. 前記（Ａ２）の工程が、
   評価対象点と目標点の間に障害物が存在する場合に、以下の（Ｂ１）から（Ｂ３）の各工程を行う工程、すなわち
   （Ｂ１）評価対象点から見た目標点の方向に沿って、評価対象点から障害物までの距離を算出する工程と、
   （Ｂ２）評価対象点から見た目標点の方向から所定角度傾いた方向に、評価対象点から前記距離だけ経路を進展させる工程と、
   （Ｂ３）進展させた経路の先端を新たな評価対象点とする工程
   を行う工程を備える、請求項１の経路生成方法。
3. 複数の関節を備えるアームの関節空間における経路を生成する装置であって、
   経路の開始点の位置を決定する手段と、
   経路の目標点の位置を決定する手段と、
   障害物の位置と形状を決定する手段と、
   開始点から目標点へ到達する経路を決定する手段を備えており、前記経路を決定する手段が、
   経路の開始点を評価対象点とする工程と、
   （Ａ１）評価対象点と目標点の間に障害物が存在しない場合に、評価対象点から目標点まで直線的に経路を進展させる工程と、
   （Ａ２）評価対象点と目標点の間に障害物が存在する場合に、評価対象点から見た目標点の方向から傾いた方向に直線的に経路を進展させて、進展させた経路の先端を新たな評価対象点とする工程と、
   上記の（Ａ１）と（Ａ２）の各工程を、開始点から進展した経路が目標点に到達するまで繰り返し行う工程を実行することを特徴とする、経路生成装置。

Images