(57)【要約】【課題】  本発明の目的は、運動学的及び動力学的に望ましく、動作移動の効率を考慮した経路を計画することが可能な経路計画方法を提供することにある。【解決手段】  本発明の経路計画方法は、初期位置Ｓと最終位置Ｇ（Ｇｉ）との間に中間点Ｍｉを確率的に生成させ、中間点Ｍｉを用いて初期位置Ｓから最終位置Ｇ（Ｇｉ）までの間に経路を計画するものであり、一つの初期位置Ｓと共に複数の最終位置Ｇ（Ｇｉ）を予め設定し、初期位置Ｓ及び最終位置Ｇ（Ｇｉ）に対して上述した経路を生成することを特徴としている。（ｉ＝１，２，…）【選択図】      図１１
2006048372.tif 000002
【特許請求の範囲】
【請求項１】
初期位置と最終位置との間に中間点を確率的に生成させ、該中間点を用いて前記初期位置から前記最終位置までの間に経路を計画する複数目標に対する経路計画方法において、
一つの前記初期位置と共に複数の前記最終位置を予め設定し、前記初期位置及び前記最終位置に対して前記経路を生成することを特徴とする複数目標に対する経路計画方法。
【請求項２】
前記初期位置及び前記最終位置の双方からそれぞれ前記経路を生成するもので、前記最終位置からの前記経路は、（１）少なくとも一つの中間点を生成し、（２）生成された前記中間点に基づいて複数の前記最終位置から一つを選択し、（３）前記中間点と選択された前記最終位置との間に前記経路の少なくとも一部を生成する、ことを少なくとも一回以上行うことで生成されることを特徴とする請求項１に記載の複数目標に対する経路計画方法。
【請求項３】
初期位置と最終位置との間に中間点を確率的に生成させ、該中間点を用いて前記初期位置から前記最終位置までの間に、自動機械の全体又は一部分の経路を計画する複数目標に対する経路計画方法において、
前記自動機械が存在する実空間上に作業空間を定義すると共に、該作業空間における前記自動機械の状態が一点として表される多次元座標空間を定義し、
前記作業空間上の前記初期位置と前記最終位置との間の前記経路設定時に、前記多次元座標空間上における前記初期位置に対応する点と前記最終位置に対応する点とを結ぶ経路を、前記作業空間における距離と前記多次元座標空間における距離とを用いて生成することを特徴とする複数目標に対する経路計画方法。
【請求項４】
前記作業空間における距離を用いて前記中間点の設定を行い、該中間点に向けた前記経路の生成を前記多次元座標空間における距離を用いて決定することを特徴とする請求項３に記載の複数目標に対する経路計画方法。
【請求項５】
前記経路は、（１）少なくとも一つの中間点を前記多次元座標空間上に生成し、（２）該多次元座標空間上における前記初期位置又は前記最終位置あるいは既に生成されている前記経路上の点の中から、生成された前記中間点と前記作業空間上で最も近い距離にある点を経路点として選択し、（３）前記多次元座標空間における距離を用いて、前記経路点から前記中間点への間で障害物と生成中の経路との衝突判定を行い、（４）衝突がない場合に次の経路点を設定する、ことを少なくとも一回以上行うことで、生成されることを特徴とする請求項４に記載の複数目標に対する経路計画方法。
【請求項６】
初期位置と最終位置との間に中間点を確率的に生成させ、該中間点を用いて前記初期位置から前記最終位置までの間に経路を計画する複数目標に対する経路計画方法において、
前記中間点の候補となる中間点候補が同時に複数生成され、複数の該中間点候補の中から所定の選択基準に基づいて一つの中間点を選択することを特徴とする複数目標に対する経路計画方法。
【請求項７】
前記選択基準が、運動学的可操作性又は動力学的可操作性に基づくものであることを特徴とする請求項６に記載の複数目標に対する経路計画方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自動機械の全体又は一部分などの移動経路を計画する複数目標に対する経路計画方法に関する。
【背景技術】
【０００２】
ロボットを移動させる際や、ロボットアームを自動操作する際には、その経路を計画する必要が生じる。下記［非特許文献１］には、確率機構を用いた経路計画について記載されている。［非特許文献１］の方法では、一つのスタート位置（初期位置：初期姿勢）に対して一つのゴール位置（最終位置：最終姿勢）が設定され、この間に一つ以上の中間点を経由する経路が設定される。このとき、中間点に基づいて経路を計画する際の基準が関節空間などの探索空間であり、中間点は一度に一つのみ設定される。
【０００３】
なお、関節空間などの探索空間を用いた経路設定については、下記［非特許文献２］に記載がある。［非特許文献２］には、空間内でのロボットの望ましい経路を決定する方法として、空間的・時間的な経路を関節角の関数として記述する関節空間法などが記載されている。関節空間法では、経路のスタート位置（初期位置）とゴール位置（最終位置）との間に一以上の中間点を設定し、これらをスプライン曲線で接続した経路を作成する。
【０００４】
【非特許文献１】Ｒａｎｄｏｍｉｚｅｄ  Ｋｉｎｏｄｙｎａｍｉｃ  Ｐｌａｎｎｉｎｇ  ：  Ｓ．Ｍ．ＬａＶａｌｌｅ  ａｎｄ  Ｊ．Ｊ．Ｋｕｆｆｎｅｒ，Ｊｒ．  ：  Ｐｒｏｃ  ｏｆ  ＩＥＥＥ  Ｉｎｔ．  Ｃｏｎｆ．  ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ，  １９９９
【非特許文献２】ロボティクス−機構・力学・制御−（第７章）、Ｊｏｈｎ  Ｊ．Ｃｒａｉｇ  著、三浦宏文・下山勲  訳、共立出版株式会社、１０９６４１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
［非特許文献１］の方法では、ロボットアームなどの経路を計画する場合などは、最終姿勢が一つであるため、この設定された一つの最終姿勢で経路設定が不可能だった場合に経路をどのように設定するかという問題が残る。実際には、最終姿勢は複数とれる場合が多く、ある最終姿勢で経路計算不可でも、別の最終姿勢では経路計算可能な場合がある。また、［非特許文献１］の方法では、関節空間などの探索空間における距離と自動機械が動く現実の作業空間における距離とは対応していないため、作業空間で無駄な動きを生じさせる経路を生成してしまうおそれがあった。
【０００６】
さらに、経路生成途中で設定される中間点はランダムに一つずつ生成されるため、運動学的に望ましくない中間点への無駄な経路計算が行われる場合があった。従って、本発明の目的は、運動学的及び動力学的に望ましく、動作移動の効率を考慮した経路を計画することが可能な経路計画方法を提供することにある。
【課題を解決するための手段】
【０００７】
請求項１に記載の複数目標に対する経路計画方法は、初期位置と最終位置との間に中間点を確率的に生成させ、中間点を用いて初期位置から最終位置までの間に経路を計画するものであり、一つの初期位置と共に複数の最終位置を予め設定し、初期位置及び最終位置に対して上述した経路を生成することを特徴としている。
【０００８】
請求項２に記載の発明は、請求項１に記載の複数目標に対する経路計画方法において、初期位置及び前記最終位置の双方からそれぞれ経路を生成するもので、最終位置からの経路は、（１）少なくとも一つの中間点を生成し、（２）生成された中間点に基づいて複数の最終位置から一つを選択し、（３）中間点と選択された最終位置との間に経路の少なくとも一部を生成する、ことを少なくとも一回以上行うことで生成されることを特徴としている。
【０００９】
請求項３に記載の複数目標に対する経路計画方法は、初期位置と最終位置との間に中間点を確率的に生成し、中間点を用いて初期位置から最終位置までの間に、自動機械の全体又は一部分の経路を計画するもので、自動機械が存在する実空間上に作業空間を定義すると共に、この作業空間における自動機械の状態が一点として表される多次元座標空間を定義し、作業空間上の初期位置と最終位置との間の経路設定時に、多次元座標空間上における初期位置に対応する点と最終位置に対応する点とを結ぶ経路を、作業空間における距離と多次元座標空間における距離とを用いて生成することを特徴としている。
【００１０】
請求項４に記載の発明は、請求項３に記載の複数目標に対する経路計画方法において、作業空間における距離を用いて中間点の設定を行い、この中間点に向けた経路の生成を多次元座標空間における距離を用いて決定することを特徴としている。
【００１１】
請求項５に記載の発明は、請求項４に記載の複数目標に対する経路計画方法において、経路は、（１）少なくとも一つの中間点を多次元座標空間上に生成し、（２）多次元座標空間上における初期位置又は最終位置あるいは既に生成されている経路上の点の中から、生成された中間点と作業空間上で最も近い距離にある点を経路点として選択し、（３）多次元座標空間における距離を用いて、経路点から中間点への間で障害物と生成中の経路との衝突判定を行い、（４）衝突がない場合に次の経路点を設定する、ことを少なくとも一回以上行うことで、生成されることを特徴としている。
【００１２】
請求項６に記載の発明は、初期位置と最終位置との間に中間点を確率的に生成し、中間点を用いて初期位置から最終位置までの間に経路を計画するもので、中間点の候補となる中間点候補が同時に複数生成され、複数の中間点候補の中から所定の選択基準に基づいて一つの中間点を選択することを特徴としている。
【００１３】
請求項７に記載の発明は、請求項６に記載の複数目標に対する経路計画方法において、選択基準が、運動学的可操作性又は動力学的可操作性に基づくものであることを特徴としている。
【発明の効果】
【００１４】
請求項１に記載の複数目標に対する経路計画方法によれば、経路の最終位置を予め複数設定し、これらに対して経路計算を行うことで、取りうる経路が多くなるためより適切な経路を設定しやすくなる。この結果、より効率よく、無駄のない経路を計画することができる。また、請求項２に記載の複数目標に対する経路計画方法によれば、予め設定されたすべての最終位置に対して経路の全行程を計算する場合よりも少ない計算量で経路を計画することができる。
【００１５】
請求項３に記載の複数目標に対する経路計画方法によれば、自動装置（自動装置自体の移動経路やロボットアームの移動経路など）の状態（姿勢など）を多次元座標空間上の一点で表し、実際の作業空間と多次元座標空間とのそれぞれの空間上での距離とを使って自動装置の経路を計画する。このように、実際の作業空間における距離と多次元座標空間における距離とをそれぞれ適切に使い分けることで、動作移動の効率を考慮した無駄のない経路を計画することができる。
【００１６】
また、請求項４に記載の複数目標に対する経路計画方法によれば、自動機械の経路上に位置することとなる中間点の設定には実際の作業空間の距離を用いることで、より効率よく、無駄のない経路を計画することができる。また、設定された中間点に向けて経路を順次伸ばして行く過程では多次元座標空間における距離を用いることで演算負荷を軽減することができる。
【００１７】
また、請求項５に記載の複数目標に対する経路計画方法によれば、中間点との間に経路を計画する経路点（又は、初期位置／最終位置）の選択には、実際の作業空間の距離を用いることで、より効率よく、無駄のない経路を計画することができる。また、設定された中間点に向けて経路を順次伸ばして行く過程では多次元座標空間における距離を用いて衝突判定（作業空間上で自動機械が障害物と衝突するか否かの判定）を行うことで演算負荷を軽減することができる。
【００１８】
請求項６に記載の複数目標に対する経路計画方法によれば、中間点となる候補が複数生成され、この中から所定の選定基準に基づいて中間点が決定される。このため、運動学的に望ましく、無駄のない経路を計画することができる。また、請求項７に記載の複数目標に対する経路計画方法によれば、選択基準が自動装置の操作性を考慮した可操作性（運動学的又は動力学的）に基づくものであるため、より一層、運動学的及び動力学的に望ましい経路を計画することができる。
【発明を実施するための最良の形態】
【００１９】
本発明の複数目標に対する経路計画方法の一実施形態について以下に説明する。まず、本実施形態の複数目標に対する経路計画方法を演算する装置（経路生成手段）の構成を図１に示す。経路生成手段１は、具体的にはコンピュータによって構成されている。図１に示されるように、経路生成手段１は外部環境情報を入力する手段である外部環境情報入力手段２と、生成過程にある経路や最終的に生成した（設定した）経路を出力する経路出力手段３とを有している。
【００２０】
外部環境情報入力手段２は、移動経路を生成する自動機械（ロボット自体やロボットアームなど）周辺の情報、特に障害物情報を外部環境として取得する手段であり、カメラや各種センサなどの情報取得デバイスが例として挙げられる。あるいは、外部環境情報入力手段２は、キーボードや光ディスクドライブなどであっても良く、オペレータが手動で外部環境情報を入力しても良いし、すでにデータ化されて光ディスクに格納されている外部環境情報を入力しても良い。経路出力手段３は、具体的には、モニタやプリンタ、記憶可能な媒体を扱うドライブなどである。
【００２１】
外部環境情報入力手段２から入力された外部環境情報は、環境情報記憶手段４に蓄積される。環境情報記憶手段４は、具体的にはハードディスクやＲＡＭなどである。環境情報記憶手段４には、知識としての環境情報（すでに知識として保持している情報）と、上述したような、外部環境情報入力手段２によって新たに入力された環境情報とを保持する。外部環境情報入力手段２によって新たに入力された環境情報は、その後は知識としての環境情報として蓄積される。経路生成手段１は、経路のスタート位置（初期位置）とゴール位置（最終位置）とを最終的に決定するスタート・ゴール位置決定手段５や、スタート位置とゴール位置との間に中間点を設定（仮設定）する中間点生成手段６なども有している。中間点生成手段６などは、ハードディスク内やＲＯＭ内に格納されたプログラムとこれを実行するＣＰＵなどによって実現されている。
【００２２】
次に、経路生成の手順について、図２のフローチャートを参照しつつつ、順を追って説明する。ここでは、自動機械がロボットアームである場合を例にして説明する。図３は、ロボットアーム１００を模式的に示した図である。このロボットアーム１００は、図３に示されるように、複数のリンク（線状の部材）１０１と、二つのリンク１０１の間に配置された関節ジョイント１０２とからなっている。各関節ジョイント１０２は一自由度の関節である。また、アーム１００の先端には、把持部１０３が取り付けられている。ここでは、把持部１０３の根本、即ち、最も末端側のリンク１０１の先端部分（手首位置Ｘ）の移動軌跡を生成する。
【００２３】
実際には各関節ジョイント１０２にアクチュエータが内蔵されており、その関節ジョイント１０２に接合している一対のリンク１０１によって形成される角度を変更することができる。なお、ここでは、根本側のリンク１０１の基端部と、末端側リンク１０１の把持部１０３との接合部（手首位置Ｘ）はリジッドであるものとして説明する。なお、ロボットアーム１００が実存している空間を、以下、作業空間と言うこととする。
【００２４】
また、ロボットアーム１００の状態、即ち、ロボットアーム１００がどのような姿勢であるかは、各関節ジョイント１０２の角度が分かれば把握することができる。今ここで、図３中の三つの各関節ジョイント１０２での角度θ１，θ２，θ３それぞれ座標軸として持つ多次元座標空間（この例では三次元座標空間）を設定すれば、ロボットアーム１００の姿勢は、この多次元座標空間上の一点［座標（θ１，θ２，θ３）］によって表すことができる。
【００２５】
作業空間に対してｘ，ｙ座標軸を設定した場合、ロボットアーム１００の姿勢を表す手首位置Ｘの座標（ｘ，ｙ）は、多次元座標空間上の座標（θ１，θ２，θ３）を変換することで容易に得ることができる。なお、ここでは、ロボットアーム１００の状態（姿勢）を三次元座標で表したが、二次元や、より多次元の座標空間上で表すこともできる。例えば、上述した関節ジョイント１０２が七つあるようなロボットアームであれば、七次元座標空間上の一点としてその状態（姿勢）を表すことができる。あるいは、三つの関節ジョイントと一つの伸縮部とを持つようなロボットアームであれば、四次元座標空間上の一点としてその状態（姿勢）を表すことができる。
【００２６】
経路生成に際しては、まず、外部環境情報入力手段２によって外部環境情報が入力される。すでに入力された環境情報記憶手段４に蓄積された情報のみで不足がない場合は、新たな外部環境情報の入力は必ずしも必要ない。次に、経路のスタート位置とゴール位置とを上述した作業空間上にオペレータがセットする。これらの位置があらかじめプリセットされているのであれば、オペレータによる入力は必要ない。入力は、キーボードから座標位置を入力するなどして設定することができる。あるいは、経路生成手段１にロボットアーム１００を接続し、ロボットアーム１００を実際にスタート・ゴール位置に位置させたときの座標などを経路生成手段１に読み込ませるようにしても良い。また、装置に組み込まれたカメラや各種センサによって外部環境情報が自動又は半自動で取得されるようにしても良い。
【００２７】
このとき、ゴール位置は複数設定される。例えば、ここではロボットアーム１００について説明しているが、最終位置で対象物を把持するとした場合に、対象物を把持する際のロボットアーム１００の姿勢（手首位置Ｘ）はいくつか考えられる。対象物を上方からつかんでも良いし、右／左側方からつかんでもよい。このようなバリエーションを考えて、ゴール位置を予め複数設定する。ロボットアーム以外の自動機械の場合であっても、ゴール位置の取り方にバリエーションを持たせられる場合が多いので、このような場合もゴール位置を複数設定する。
【００２８】
スタート・ゴール位置決定手段５は、環境情報記憶手段４に蓄積された情報と照らし合わせて、入力されたスタート・ゴール位置が障害物と干渉していないか否かを判断し、干渉していない場合はこれらをスタート・ゴール位置として決定する（ステップ３００）。なお、干渉の有無の検証については、追って詳しく説明する。もし、入力されたスタート・ゴール位置が障害物と干渉している場合は、干渉しているゴール位置については削除するなどすると共に、必要であればオペレータに対して再入力を促し、再入力されたスタート・ゴール位置について同様の判定を行う。この手順は、スタート・ゴール位置が決定されるまで反復される。
【００２９】
決定された作業空間上のスタート・ゴール位置は、多次元座標空間上に変換される。なお、ロボットアーム１００で対象物を把持する際には、つかむ方向の違いによる手首位置Ｘの違いによって、ロボットアーム１００のアームの姿勢にはバリエーションが生まれる。また、ゴール位置の手首位置Ｘが一つ決まったときであっても、そのときのロボットアーム１００のアームの姿勢（各関節角度の組み合わせ）にはバリエーションが生まれる。ここでは、これらのバリエーションも含めて、多次元座標空間上に複数のゴール位置が設定される。このため、多次元座標空間上には複数のゴール位置が設定される。ここでは、説明の便宜上、多次元座標空間を二次元座標空間θｘ−θｙ（関節ジョイント１０２が二つのロボットアームの場合に相当）として説明する。図４に示されるように、ここでは、二次元座標空間θｘ−θｙ上に一つのスタート位置Ｓと複数のゴール位置Ｇとが設定される。なお、スタート・ゴール位置は、はじめから多次元座標空間上の座標として入力されても構わない。
【００３０】
次に、確率機構を用いた公知の手法（上記［非特許文献１］などを参照）によって、多次元座標空間上で、スタート・ゴール位置の間に複数の中間点が仮設定（複数の中間点候補ｍ１が設定）される（中間点候補設定行程：ステップ３０５）。複数の中間点候補ｍ１を上述した二次元座標空間θｘ−θｙに設定した状態を図５に例示する。この中間点候補ｍ１の設定（中間点の仮設定）は、中間点生成手段６によって行われる。図５に示されるように、複数の中間点候補ｍ１が設定されたら、次に、この設定された中間点候補ｍ１の中から、経路設定に利用するために好適なものを中間点Ｍ１として設定する（中間点決定行程）。
【００３１】
上述したように、複数の中間点候補ｍ１の中から好適なものを中間点Ｍ１として設定するが、ここでは中間点候補ｍ１のすべてに関して可操作度を算出し、可操作度の最も高い、即ち、可操作性が最も良い中間点候補ｍ１を中間点Ｍ１として採用する（ステップ３１０）。二次元座標空間θｘ−θｙにおいて、複数の中間点候補ｍ１の中から一つの中間点Ｍ１を設定した状態を図６に示す。なお、可操作性については、「コンピュータ制御機械システムシリーズ１０  ロボット制御基礎理論」（第４章）  吉川恒夫著  コロナ社  に記載されている。可操作性とは、自動装置（ここではロボットアーム１００の手先部分）をどの程度自由に操作できるかということであり、この操作能力を表すものが可操作度である。可操作性には、運動学的可操作性と動力学的可操作性とがあるが、ここでは何れの可操作性に基づいても良い。
【００３２】
複数の中間点候補ｍ１から最も可操作性の良いものを中間点Ｍ１として採用した後は、作業空間上での複数のゴール位置の中から、中間点Ｍ１に対応する作業空間上の点と最も近い距離（作業空間上での距離）にあるものを選択する（ステップ３１５，３２０）。上述したように、二次元座標空間θｘ−θｙ上の中間点Ｍ１は、実際の作業空間上にある手首位置Ｘを定義し、また、上述した二次元座標空間θｘ−θｙ上の複数のゴール位置Ｇも、実際の作業空間上にある手首位置Ｘを定義する。ここでは、作業空間上において、中間点Ｍ１に最も近いゴール位置Ｇの定める手首位置Ｘを経路生成に利用するものとして選択する（説明の便宜上、選択されたゴール位置ＧをＧ１と呼ぶこととする）。なお、選択されなかったゴール位置Ｇは、後工程で利用する可能性があるため保持される。図７に上述した手順で一つのゴール位置Ｇ１が二次元座標空間θｘ−θｙ上に設定された状態を示す。
【００３３】
これで、スタート位置Ｓ・中間点Ｍ１・ゴール位置Ｇ１が一つずつ決定／選択されたこととなるので、スタート位置Ｓ−中間点Ｍ１と中間点Ｍ１−ゴール位置Ｇ１とを二次元座標空間θｘ−θｙ上で最短距離（関節ジョイント１０２に関する二次元座標空間θｘ−θｙでの距離）で結ぶ。なお、作業空間及び多次元座標空間のそれぞれの距離は、距離を定義する距離関数であるならば何でもよく、例えば、ユークリッド距離関数などでもよい。図８ではそれぞれ線分で表されている。そして、まず、二次元座標空間θｘ−θｙ上で、ゴール位置Ｇ１側の線分（ベクトル）についてゴール位置Ｇ１から所定長さ分が切り取られ、この所定長さ分に関して衝突判定が行われる（ステップ３２５）。衝突判定は、生成する経路が作業空間上で障害物と干渉しないかどうかを判定するものである。
【００３４】
図８に、ゴール位置Ｇ１側及びスタート位置Ｓ側の衝突判定が終了した時点での二次元座標空間θｘ−θｙを示す。衝突判定は、切り取られた所定長さ分の線分（ベクトル）をさらに微小な区間に分割し、その微小区間の境界部毎に行われる。この微小区間の判定は、ゴール位置Ｇ１側から中間点Ｍ１に向けて順次行われる。なお、ここでの「所定長さ」や「微小区間」は、二次元座標空間θｘ−θｙ上の距離（ユークリッド距離）に基づいている。このようにすることで、演算負荷を軽減することができる。一方、上述したように、複数のゴール位置Ｇから最も近い距離のゴール位置を選択するときは実際の作業空間における距離に基づいている。これは、生成される経路を無駄のないものとするためである。
【００３５】
このように、作業空間における距離と二次元座標空間θｘ−θｙにおける距離とを、必要に応じて使い分けることで、無駄のない経路生成と演算負荷の低減とを両立させている。ここで、実際の衝突判定についてであるが、上述した微小区間の境界部の二次元座標空間θｘ−θｙ上の位置座標が実際の作業空間に変換され、予め入力されている障害物の情報と比較され、作業空間上で干渉の有無が判定される。干渉がなければ、上述した所定長さ分の端部まで経路が生成される（ステップ３３０）。ゴール位置Ｇ１側の所定長さ分に関する衝突判定が済んだら、同様にスタート位置Ｓ側の衝突判定が行われる（ステップ３３５，３４０，３４５）。スタート位置Ｓ側に関しても干渉がなければ、上述した所定長さ分の端部まで経路が生成される（ステップ３５０）。
【００３６】
ゴール位置Ｇ１側及びスタート位置Ｓ側の各所定長さ分に関して衝突がなければ、中間点Ｍ１をそのままにして、さらに所定長さ分進んで衝突判定を行う（ステップ３５５−ｎｏ）。一方、このように衝突判定を進めつつ、経路を生成していくと、生成中の経路が作業空間上で障害物と衝突すると判定される場合が生じる。このように、衝突判定中にその所定長さ分について衝突があると判定された場合は、その所定長さ分（本実施形態では、スタート位置Ｓ側とゴール位置Ｇｉとの双方）については破棄され、新たな中間点Ｍｉが設定され、それ以降はこの新たな中間点Ｍｉを利用して同様の経路生成が続行される（ステップ３２５，３４５−ｎｏ）。
【００３７】
以下、中間点Ｍ１による二回目の所定長さについての経路生成中に衝突判定によって新たな中間点Ｍ２を設定する必要が生じた場合を例に説明する。新たな中間点Ｍ２を設定する場合、まず、図９に示されるように、複数の中間点候補ｍ２を二次元座標空間θｘ−θｙ上にランダムに設定する（ステップ３０５）。そして、図１０に示されるように、上述した中間点候補ｍ１の場合と同様に、中間点候補ｍ２のすべてに関して可操作度を算出し、可操作度の最も高い、即ち、可操作性が最も良い中間点候補ｍ２を中間点Ｍ２として採用する（ステップ３１０）。
【００３８】
図１０において、複数の中間点候補ｍ２から最も可操作性の良いものを中間点Ｍ２として採用した後は、上述したゴール位置Ｇ１・このゴール位置Ｇ１から生成された経路上の点Ｇ１’（ここではこれ一つであるが他にもあれば複数の経路点のすべて）・採用されていなかった残りのゴール位置Ｇの中から、作業空間上での中間点Ｍ２に対して最も近い距離にあるものを経路生成に利用するものとして選択する（説明の便宜上、選択されたゴール位置ＧをＧ２と呼ぶこととする）（ステップ３１５，３２０）。なお、ここでは、先ほど生成したゴール位置Ｇ１からの経路は破棄されずに残される。さらに中間点Ｍｉを設定する際に、ゴール位置Ｇ１や経路点Ｇ１’が再度利用されることがあり得るからである。しかし、最終的に、ゴール位置Ｇ１からの経路が利用されずに終わる可能性もある。ただし、この場合は、作業空間上で中間点Ｍｉから遠いからであり、最終的に採用される経路は無駄のないものとなるので構わない。また、一度も選択されていないゴール位置Ｇも後工程で利用する可能性があるため保持される。
【００３９】
なお、ここで中間点Ｍ２を用いて経路計画を進める際のスタート側に関しても、当初のスタート位置Ｓやこのスタート位置Ｓから既に経路設定された点（ノード）Ｓ’のうち、中間点Ｍ２からの距離が最も近いもの（ここでは点Ｓ’）から経路計画が行われる（ステップ３４０）。そしてその後も、図１１に示されるように同様な手順を繰り返し、最終的にスタート位置Ｓ側の経路とゴール位置Ｇ側の経路とが合流し、経路全体が決定される（ステップ３５５−ｙｅｓ）。このようにして生成された経路は、省略できる経路点を省略して経路の単純化を図る平坦化や、経路が直線の組み合わせでなく滑らかなものとなるように処理する平滑化が行われ、最終的に決定される（平坦化及び平滑化も、ここでは多次元座標空間上で行われる）。これらの平坦化や平滑化には公知の方法を用いることができる。このため、ここでは、これらの平坦化や平滑化についての詳しい説明は省略する。
【００４０】
決定された二次元座標空間θｘ−θｙ上の経路は、作業区間上の経路に変換される。最終的な経路は、経路出力手段３によって出力される。なお、スタート位置Ｓ側の経路とゴール位置Ｇ側の経路のどちらかが早く中間点Ｍｉに達し、かつ、さらに中間点Ｍｉ＋１を設定する必要が生じた場合には、中間点Ｍｉは経路点の一つとして次の中間点Ｍｉ＋１が設定される。上述したように、複数のゴール位置Ｇを設定して上述したような経路生成を行うことで、滑らかな経路設定を実現することができる。また、中間点Ｍｉの設定時に予め複数の候補ｍｉを設定してから可操作度に基づいて一つを選択／決定することで、自動機械（ここではロボットアーム１００）に無理な動きや姿勢をさせないで済む。
【００４１】
なお、本実施形態では、ゴール位置Ｇｉ側の所定長さ分に関して衝突していると判定された場合は、その所定長さ分の始点に戻って破棄している。ただし、衝突していると判定される直前の微小区間までは経路として利用しても良い。また、本実施形態では、ゴール位置Ｇｉ側の所定長さ分に関して衝突していると判定された場合は、それに対応するスタート位置Ｓ側の衝突判定は行わずに次の中間点Ｍｉ＋１を設定することとしているが、対応するスタート側Ｓの衝突判定を行い、衝突していなければそれを経路として採用するようにしても良い。さらに、スタート位置Ｓ側の所定長さ分に関して衝突していると判定された場合は、それに対応する既に終了しているゴール位置Ｇｉ側の経路は利用しても良いし、破棄しても良い。
【００４２】
本発明の複数目標に対する経路計画方法は上述した実施形態に限定されるものではない。例えば、上述した実施形態では、説明を分かりやすくするため、自動機械（ロボットアーム１００）の状態（姿勢）を座標軸上の一点として表す多次元座標軸を二次元としたが、三次元以上の多次元座標軸であっても構わない。また、自動機械は、ロボットアームではなくても良い。さらに、計画する経路は、ロボットアームなどの自動機械の一部ではなく、移動可能な自動機械の移動経路であっても良い。
【図面の簡単な説明】
【００４３】
【図１】本発明の複数目標に対する経路計画方法の一実施形態を実施する装置の構成図である。
【図２】本発明の複数目標に対する経路計画方法の一実施形態による経路計画のフローチャートである。
【図３】本発明の複数目標に対する経路計画方法の一実施形態によって制御される多関節ロボットアームを示す図である。
【図４】経路生成時（１）の多次元座標空間を示す図である。
【図５】経路生成時（２）の多次元座標空間を示す図である。
【図６】経路生成時（３）の多次元座標空間を示す図である。
【図７】経路生成時（４）の多次元座標空間を示す図である。
【図８】経路生成時（５）の多次元座標空間を示す図である。
【図９】経路生成時（６）の多次元座標空間を示す図である。
【図１０】経路生成時（７）の多次元座標空間を示す図である。
【図１１】経路生成時（８）の多次元座標空間を示す図である。
【符号の説明】
【００４４】
１…経路生成手段、２…外部環境情報入力手段、３…経路出力手段、４…環境情報記憶手段、５…スタート・ゴール位置決定手段、６…中間点生成手段、１００…多関節ロボットアーム、１０１…リンク、１０２…関節ジョイント、１０３…把持部。
【図１】
2006048372.tif 000003
【図２】
2006048372.tif 000004
【図３】
2006048372.tif 000005
【図４】
2006048372.tif 000006
【図５】
2006048372.tif 000007
【図６】
2006048372.tif 000008
【図７】
2006048372.tif 000009
【図８】
2006048372.tif 000010
【図９】
2006048372.tif 000011
【図１０】
2006048372.tif 000012
【図１１】
2006048372.tif 000013
【手続補正書】
【提出日】平成16年8月17日(2004.8.17)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
初期位置と最終位置との間に中間点を確率的に生成させ、該中間点を用いて前記初期位置から前記最終位置までの間に経路を計画する経路計画方法において、
一つの前記初期位置と共に複数の前記最終位置を予め設定し、前記初期位置及び前記最終位置に対して前記経路を生成することを特徴とする経路計画方法。
【請求項２】
前記初期位置及び前記最終位置の双方からそれぞれ前記経路を生成するもので、前記最終位置からの前記経路は、（１）少なくとも一つの中間点を生成し、（２）生成された前記中間点に基づいて複数の前記最終位置から一つを選択し、（３）前記中間点と選択された前記最終位置との間に前記経路の少なくとも一部を生成する、ことを少なくとも一回以上行うことで生成されることを特徴とする請求項１に記載の経路計画方法。
【請求項３】
初期位置と最終位置との間に中間点を確率的に生成させ、該中間点を用いて前記初期位置から前記最終位置までの間に、自動機械の全体又は一部分の経路を計画する経路計画方法において、
前記自動機械が存在する実空間上に作業空間を定義すると共に、該作業空間における前記自動機械の状態が一点として表される多次元座標空間を定義し、
前記作業空間上の前記初期位置と前記最終位置との間の前記経路設定時に、前記多次元座標空間上における前記初期位置に対応する点と前記最終位置に対応する点とを結ぶ経路を、前記作業空間における距離と前記多次元座標空間における距離とを用いて生成することを特徴とする経路計画方法。
【請求項４】
前記作業空間における距離を用いて前記中間点の設定を行い、該中間点に向けた前記経路の生成を前記多次元座標空間における距離を用いて決定することを特徴とする請求項３に記載の経路計画方法。
【請求項５】
前記経路は、（１）少なくとも一つの中間点を前記多次元座標空間上に生成し、（２）該多次元座標空間上における前記初期位置又は前記最終位置あるいは既に生成されている前記経路上の点の中から、生成された前記中間点と前記作業空間上で最も近い距離にある点を経路点として選択し、（３）前記多次元座標空間における距離を用いて、前記経路点から前記中間点への間で障害物と生成中の経路との衝突判定を行い、（４）衝突がない場合に次の経路点を設定する、ことを少なくとも一回以上行うことで、生成されることを特徴とする請求項４に記載の経路計画方法。
【請求項６】
初期位置と最終位置との間に中間点を確率的に生成させ、該中間点を用いて前記初期位置から前記最終位置までの間に経路を計画する経路計画方法において、
前記中間点の候補となる中間点候補が同時に複数生成され、複数の該中間点候補の中から所定の選択基準に基づいて一つの中間点を選択することを特徴とする経路計画方法。
【請求項７】
前記選択基準が、運動学的可操作性又は動力学的可操作性に基づくものであることを特徴とする請求項６に記載の経路計画方法。
【手続補正３】
【補正対象書類名】明細書
【補正対象項目名】０００１
【補正方法】変更
【補正の内容】
【０００１】
本発明は、自動機械の全体又は一部分などの移動経路を計画する経路計画方法に関する。
【手続補正４】
【補正対象書類名】明細書
【補正対象項目名】０００７
【補正方法】変更
【補正の内容】
【０００７】
請求項１に記載の経路計画方法は、初期位置と最終位置との間に中間点を確率的に生成させ、中間点を用いて初期位置から最終位置までの間に経路を計画するものであり、一つの初期位置と共に複数の最終位置を予め設定し、初期位置及び最終位置に対して上述した経路を生成することを特徴としている。
【手続補正５】
【補正対象書類名】明細書
【補正対象項目名】０００８
【補正方法】変更
【補正の内容】
【０００８】
請求項２に記載の発明は、請求項１に記載の経路計画方法において、初期位置及び前記最終位置の双方からそれぞれ経路を生成するもので、最終位置からの経路は、（１）少なくとも一つの中間点を生成し、（２）生成された中間点に基づいて複数の最終位置から一つを選択し、（３）中間点と選択された最終位置との間に経路の少なくとも一部を生成する、ことを少なくとも一回以上行うことで生成されることを特徴としている。
【手続補正６】
【補正対象書類名】明細書
【補正対象項目名】０００９
【補正方法】変更
【補正の内容】
【０００９】
請求項３に記載の経路計画方法は、初期位置と最終位置との間に中間点を確率的に生成し、中間点を用いて初期位置から最終位置までの間に、自動機械の全体又は一部分の経路を計画するもので、自動機械が存在する実空間上に作業空間を定義すると共に、この作業空間における自動機械の状態が一点として表される多次元座標空間を定義し、作業空間上の初期位置と最終位置との間の経路設定時に、多次元座標空間上における初期位置に対応する点と最終位置に対応する点とを結ぶ経路を、作業空間における距離と多次元座標空間における距離とを用いて生成することを特徴としている。
【手続補正７】
【補正対象書類名】明細書
【補正対象項目名】００１０
【補正方法】変更
【補正の内容】
【００１０】
請求項４に記載の発明は、請求項３に記載の経路計画方法において、作業空間における距離を用いて中間点の設定を行い、この中間点に向けた経路の生成を多次元座標空間における距離を用いて決定することを特徴としている。
【手続補正８】
【補正対象書類名】明細書
【補正対象項目名】００１１
【補正方法】変更
【補正の内容】
【００１１】
請求項５に記載の発明は、請求項４に記載の経路計画方法において、経路は、（１）少なくとも一つの中間点を多次元座標空間上に生成し、（２）多次元座標空間上における初期位置又は最終位置あるいは既に生成されている経路上の点の中から、生成された中間点と作業空間上で最も近い距離にある点を経路点として選択し、（３）多次元座標空間における距離を用いて、経路点から中間点への間で障害物と生成中の経路との衝突判定を行い、（４）衝突がない場合に次の経路点を設定する、ことを少なくとも一回以上行うことで、生成されることを特徴としている。
【手続補正９】
【補正対象書類名】明細書
【補正対象項目名】００１２
【補正方法】変更
【補正の内容】
【００１２】
請求項６に記載の経路計画方法は、初期位置と最終位置との間に中間点を確率的に生成し、中間点を用いて初期位置から最終位置までの間に経路を計画するもので、中間点の候補となる中間点候補が同時に複数生成され、複数の中間点候補の中から所定の選択基準に基づいて一つの中間点を選択することを特徴としている。
【手続補正１０】
【補正対象書類名】明細書
【補正対象項目名】００１３
【補正方法】変更
【補正の内容】
【００１３】
請求項７に記載の発明は、請求項６に記載の経路計画方法において、選択基準が、運動学的可操作性又は動力学的可操作性に基づくものであることを特徴としている。
【手続補正１１】
【補正対象書類名】明細書
【補正対象項目名】００１４
【補正方法】変更
【補正の内容】
【００１４】
請求項１に記載の経路計画方法によれば、経路の最終位置を予め複数設定し、これらに対して経路計算を行うことで、取りうる経路が多くなるためより適切な経路を設定しやすくなる。この結果、より効率よく、無駄のない経路を計画することができる。また、請求項２に記載の経路計画方法によれば、予め設定されたすべての最終位置に対して経路の全行程を計算する場合よりも少ない計算量で経路を計画することができる。
【手続補正１２】
【補正対象書類名】明細書
【補正対象項目名】００１５
【補正方法】変更
【補正の内容】
【００１５】
請求項３に記載の経路計画方法によれば、自動装置（自動装置自体の移動経路やロボットアームの移動経路など）の状態（姿勢など）を多次元座標空間上の一点で表し、実際の作業空間と多次元座標空間とのそれぞれの空間上での距離とを使って自動装置の経路を計画する。このように、実際の作業空間における距離と多次元座標空間における距離とをそれぞれ適切に使い分けることで、動作移動の効率を考慮した無駄のない経路を計画することができる。
【手続補正１３】
【補正対象書類名】明細書
【補正対象項目名】００１６
【補正方法】変更
【補正の内容】
【００１６】
また、請求項４に記載の経路計画方法によれば、自動機械の経路上に位置することとなる中間点の設定には実際の作業空間の距離を用いることで、より効率よく、無駄のない経路を計画することができる。また、設定された中間点に向けて経路を順次伸ばして行く過程では多次元座標空間における距離を用いることで演算負荷を軽減することができる。
【手続補正１４】
【補正対象書類名】明細書
【補正対象項目名】００１７
【補正方法】変更
【補正の内容】
【００１７】
また、請求項５に記載の経路計画方法によれば、中間点との間に経路を計画する経路点（又は、初期位置／最終位置）の選択には、実際の作業空間の距離を用いることで、より効率よく、無駄のない経路を計画することができる。また、設定された中間点に向けて経路を順次伸ばして行く過程では多次元座標空間における距離を用いて衝突判定（作業空間上で自動機械が障害物と衝突するか否かの判定）を行うことで演算負荷を軽減することができる。
【手続補正１５】
【補正対象書類名】明細書
【補正対象項目名】００１８
【補正方法】変更
【補正の内容】
【００１８】
請求項６に記載の経路計画方法によれば、中間点となる候補が複数生成され、この中から所定の選定基準に基づいて中間点が決定される。このため、運動学的に望ましく、無駄のない経路を計画することができる。また、請求項７に記載の経路計画方法によれば、選択基準が自動装置の操作性を考慮した可操作性（運動学的又は動力学的）に基づくものであるため、より一層、運動学的及び動力学的に望ましい経路を計画することができる。
【手続補正１６】
【補正対象書類名】明細書
【補正対象項目名】００１９
【補正方法】変更
【補正の内容】
【００１９】
本発明の経路計画方法の一実施形態について以下に説明する。まず、本実施形態の経路計画方法を演算する装置（経路生成手段）の構成を図１に示す。経路生成手段１は、具体的にはコンピュータによって構成されている。図１に示されるように、経路生成手段１は外部環境情報を入力する手段である外部環境情報入力手段２と、生成過程にある経路や最終的に生成した（設定した）経路を出力する経路出力手段３とを有している。
【手続補正１７】
【補正対象書類名】明細書
【補正対象項目名】００４２
【補正方法】変更
【補正の内容】
【００４２】
本発明の経路計画方法は上述した実施形態に限定されるものではない。例えば、上述した実施形態では、説明を分かりやすくするため、自動機械（ロボットアーム１００）の状態（姿勢）を座標軸上の一点として表す多次元座標軸を二次元としたが、三次元以上の多次元座標軸であっても構わない。また、自動機械は、ロボットアームではなくても良い。さらに、計画する経路は、ロボットアームなどの自動機械の一部ではなく、移動可能な自動機械の移動経路であっても良い。
【手続補正１８】
【補正対象書類名】明細書
【補正対象項目名】００４３
【補正方法】変更
【補正の内容】
【００４３】
【図１】本発明の経路計画方法の一実施形態を実施する装置の構成図である。
【図２】本発明の経路計画方法の一実施形態による経路計画のフローチャートである。
【図３】本発明の経路計画方法の一実施形態によって制御される多関節ロボットアームを示す図である。
【図４】経路生成時（１）の多次元座標空間を示す図である。
【図５】経路生成時（２）の多次元座標空間を示す図である。
【図６】経路生成時（３）の多次元座標空間を示す図である。
【図７】経路生成時（４）の多次元座標空間を示す図である。
【図８】経路生成時（５）の多次元座標空間を示す図である。
【図９】経路生成時（６）の多次元座標空間を示す図である。
【図１０】経路生成時（７）の多次元座標空間を示す図である。
【図１１】経路生成時（８）の多次元座標空間を示す図である。