JP4667764B2 - 経路設定方法 - Google Patents

Description

本発明は、自動機械の全体又は一部分の移動経路を設定する経路設定方法に関する。

ロボットを移動させる際や、ロボットアームを自動操作する際には、その経路を設定する必要が生じる。下記［特許文献１］には、空間内でのロボットの望ましい経路を決定する方法として、空間的・時間的な経路を関節角の関数として記述する関節空間法や、デカルト座標系での位置と姿勢の経路とを時間の関数として記述するデカルト空間法などが記載されている。関節空間法では、経路のスタート位置（初期位置）とゴール位置（最終位置）との間に一以上の中間点を設定し、これらをスプライン曲線で接続した経路を作成する。この経路によって決まる速度及び加速度を制御するために、多項式スプライン関数を用いる方法も下記［特許文献１］に紹介されている。
ロボティクス−機構・力学・制御−（第７章）、Ｊｏｈｎ Ｊ．Ｃｒａｉｇ 著、三浦宏文・下山勲 訳、共立出版株式会社、１０９６４１号公報 Ｒａｎｄｏｍｉｚｅｄ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｉｎｇ ： Ｓ．Ｍ．ＬａＶａｌｌｅ ａｎｄ Ｊ．Ｊ．Ｋｕｆｆｎｅｒ，Ｊｒ． ： Ｐｒｏｃ ｏｆ ＩＥＥＥ Ｉｎｔ． Ｃｏｎｆ． ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ， １９９９

［特許文献１］には、各点（初期位置・中間点・最終位置）を時間の関数として繋ぐ多項式スプライン補間として、特に３次多項式と５次多項式を用いた場合が紹介されている。しかし、３次多項式スプラインは速度の滑らかさまでは保証できるが、加速度の滑らかさまでは一般に保証できない。このため、３次多項式スプラインを用いて生成された経路に従って多関節ロボットアームを制御した場合に、加速度の滑らかさの欠如に起因する振動が発生し得る。そこで、［特許文献１］には、これを改善するために５次多項式スプライン補間も紹介されているが、中間点の補間後に加速度−時間グラフにおいて余計な凹凸の個数が増加する。

また、複数の障害物が配置された作業空間においてロボットアームなどの自動機械の移動経路を計画する場合、設定した経路が障害物と干渉するものであった場合は、再計算が必要となる。ここで、経路設定を最初から全てやり直す必要があると計算量が増えてしまう。このため、設定した経路が障害物と干渉する場合には、その部分（位置・時間）近傍を局所的に修正できると計算量の低減に効果がある。しかし、多項式スプライン補間では、多項式スプライン曲線が、ある時刻で障害物と干渉する場合、当該時刻近傍で局所的に修正することは一般に不可能である。従って、本発明の目的は、自動機械を円滑に動作させることができ、かつ、計算量の低減にも寄与する経路設定方法を提供することにある。

請求項１に記載の経路設定方法は、自動機械の全体又は一部分の移動経路を設定するもので、自動機械の全体又は一部分を経路設定対象部として、該経路設定対象部の初期位置から最終位置の間に複数の中間点を仮設定する中間点設定行程と、経路設定対象部が中間点の間を移動する時間を設定する時間設定行程と、所定の中間点と初期位置、所定の中間点と他の中間点、又は所定の中間点と最終位置とを結んで、障害物との干渉がない場合は、所定の中間点と初期位置との間、所定の中間点と他の中間点との間、又は所定の中間点と最終位置との間に存在する中間点を省略することで、仮設定された経路を平坦化する平坦化行程と、任意の中間点を直線的に結んだ状態の平坦化後の前記経路を、近似誤差を制御する変数に基づいて滑らかな経路に変更することで平滑化する平滑化行程と、平滑化後の経路が障害物と干渉するか否かを判定し、障害物と干渉する場合は経路上の障害物との干渉部分近傍を局所的に経路変更することで経路修正する経路修正行程とを備え、平滑化行程において、任意の中間点のうち時間ｔ _ｉ に係る中間点ｐ _ｉ を平滑化する場合、中間点ｐ _ｉ に対して近似誤差を制御する変数である誤差制御変数ｄ _ｉ が設定され、（ｔ _ｉ −ｄ _ｉ ）から（ｔ _ｉ ＋ｄ _ｉ ）の範囲で経路を無限回数微分可能な関数で近似することで平滑化が行われ、中間点ｐ _ｉ の隣の中間点ｐ _ｉ−１ と中間点ｐ _ｉ との間の時間を（ｔ _ｉ −ｔ _ｉ−１ ）とし、中間点ｐ _ｉ と当該中間点ｐ _ｉ の隣の中間点ｐ _ｉ＋１ との間の時間を（ｔ _ｉ＋１ −ｔ _ｉ ）とした場合、誤差制御変数ｄ _ｉ は、（ｔ _ｉ −ｔ _ｉ−１ ）／２と（ｔ _ｉ＋１ −ｔ _ｉ ）／２のうち、何れか小さい方の値以下の値に設定され、経路修正行程においては、誤差制御変数ｄ _ｉ を変更することで経路修正が行われることを特徴としている。

請求項２に記載の発明は、請求項１に記載の経路設定方法において、自動機械が、複数のリンク部材を関節ジョイントを介して互いに接合したアームであり、該アームの移動経路を設定する際に、時間設定行程において、各関節ジョイント部分の最大角速度を考慮して、各中間点間の移動時間が設定されることを特徴としている。なお、各関節ジョイント部分の角度・角速度・角加速度とは、当該関節ジョイントに接続される一対のリンクのなす角度と、この角度に関する角速度及び角加速度のことを指す。

請求項３に記載の発明は、請求項１又は２に記載の経路設定方法において、自動機械が、複数のリンク部材を関節ジョイントを介して互いに接合したアームであり、該アームの移動経路を設定する際に、平滑化行程において、各関節ジョイント部分の角度を時間の関数として捉え、時間の関数である各関節ジョイントの角度を無限回微分可能な関数で近似することで平滑化を行うことを特徴としている。

請求項４に記載の発明は、請求項３に記載の経路設定方法において、各関節ジョイント部分の角加速度が所定の最大加速度を超えた場合に、最大加速度を超える時刻近傍において局所的に時間設定変更を行う時間修正行程をさらに備えていることを特徴としている。

請求項１に記載の経路設定方法によれば、平坦化の後に平滑化を行うことで、滑らかな移動が可能な経路を設定することができる。また、平滑化後の経路が障害物と干渉する場合には、局所的に修正（位置的修正及び時間的修正）を行うことで、計算量を低減しつつ、経路の修正を容易に行うことができる。

請求項２に記載の経路設定方法によれば、各関節ジョイント部分の最大角速度を考慮して、ある中間点から他の中間点に移動するまでの時間が決定されるため、各関節ジョイント部分での角速度が最大角速度を超えることを抑制でき、位置的・時間的の双方で滑らかな移動経路を設定することができる。

請求項３に記載の経路設定方法によれば、平滑化を、各関節ジョイント部の角度（時間関数）を無限回微分可能な関数で近似することで、時間関数となる角速度及び角加速度も滑らかな平滑化を行うことができる。

請求項４に記載の経路設定方法によれば、各関節ジョイント部分での角加速度に関しても最大加速度を考慮し、最大加速度を超えるような場合は時間設定を変更して修正を行うので、初期位置から最終位置までの間で角加速度が過剰となることを抑止できる。

本発明の経路設定方法の一実施形態について以下に説明する。まず、本実施形態の経路設定方法を演算する装置（経路生成手段）の構成を図１に示す。経路生成手段１は、具体的にはコンピュータによって構成されている。図１に示されるように、経路生成手段１は外部環境情報を入力する手段である外部環境情報入力手段２と、生成過程にある経路や最終的に生成した（設定した）経路を出力する経路出力手段３とを有している。

外部環境情報入力手段２は、移動経路を生成する自動機械（ロボット自体やロボットアームなど）周辺の情報、特に障害物情報を外部環境として取得する手段であり、カメラや各種センサなどの情報取得デバイスが例として挙げられる。あるいは、外部環境情報入力手段２は、キーボードや光ディスクドライブなどであっても良く、オペレータが手動で外部環境情報を入力しても良いし、すでにデータ化されて光ディスクに格納されている外部環境情報を入力しても良い。経路出力手段３は、具体的には、モニタやプリンタ、記憶可能な媒体を扱うドライブなどである。

外部環境情報入力手段２から入力された外部環境情報は、環境情報記憶手段４に蓄積される。環境情報記憶手段４は、具体的にはハードディスクやＲＡＭなどである。環境情報記憶手段４には、知識としての環境情報（すでに知識として保持している情報）と、上述したような、外部環境情報入力手段２によって新たに入力された環境情報とを保持する。外部環境情報入力手段２によって新たに入力された環境情報は、その後は知識としての環境情報として蓄積される。経路生成手段１は、経路のスタート位置（初期位置）とゴール位置（最終位置）とを最終的に決定するスタート・ゴール位置決定手段５や、スタート位置とゴール位置との間に中間点を設定（仮設定）する中間点生成手段６や、時間設定手段７、平坦化手段８、平滑化手段９、修正手段１０なども有している。

時間設定手段７は、ある中間点から次の中間点までの移動にかかる時間を設定する。ここでは、ある中間点から次の中間点までの単なる位置の変化だけでなく、移動にかかる時間をも考慮する。この結果、追って説明する角速度や角加速度に関する配慮も行えるようになる。平坦化手段８は、中間点生成手段６によって仮設定された中間点の数を減らし、経路が単純になるように平坦化する役割を担っている。平滑化手段９は、平坦化後の中間点間を直線で結んだだけの経路を滑らかな曲線に平滑化する役割を担っている。修正手段１０は、平滑化後の経路に対して、様々な条件と照らして修正が必要であるか否かを判断し、必要であれば修正を行う役割を担っている。修正が必要な条件としては、障害物と干渉する場合や、後述する角速度や角加速度が所定の上限値を超えている場合などがある。これらの中間点生成手段６〜修正手段１０などは、ハードディスク内やＲＯＭ内に格納されたプログラムとこれを実行するＣＰＵなどによって実現されている。

次に、経路生成の手順について順を追って説明する。ここでは、自動機械がロボットアームである場合を例にして説明する。図２は、冗長な自由度を持つロボットアーム１００を模式的に示した図である。このロボットアーム１００は、図２に示されるように、複数のリンク（線状の部材）１０１と、二つのリンク１０１の間に配置された関節ジョイント１０２とからなっている。また、アーム１００の先端には、把持部１０３が取り付けられている。ここでは、把持部１０３の根本、即ち、最も末端側のリンク１０１の先端部分（手首位置Ｘ）の移動軌跡を生成する。

実際には各関節ジョイント１０２にアクチュエータが内蔵されており、その関節ジョイント１０２に接合している一対のリンク１０１によって形成される角度を変更することができる。なお、ここでは、根本側のリンク１０１の基端部と、末端側リンク１０１の把持部１０３との接合部（手首位置Ｘ）はリジッドであるものとして説明する。なお、ロボットアーム１００のある空間に対してｘ，ｙ座標軸を設定した場合、手首位置Ｘの座標（ｘ，ｙ）は、図２中の各関節ジョイント部分での角度θ_１，θ_２，θ_３からなるベクトル（θ_１，θ_２，θ_３）の座標位置を座標変換することで容易に得ることができる。なお、図２は説明を容易にするためにｘ，ｙ平面内のみでロボットアーム１００が動く場合を例に説明したが、三次元空間においても同様の考えが適用できることは言うまでもない。

経路生成に際しては、まず、外部環境情報入力手段２によって外部環境情報が入力される。すでに入力され、環境情報記憶手段４に蓄積された情報のみで不足がない場合は、新たな外部環境情報の入力は必ずしも必要ない。次に、経路のスタート位置とゴール位置とをオペレータがセットする。これらの位置があらかじめプリセットされているのであれば、オペレータによる入力は必要ない。入力は、キーボードから座標位置を入力するなどして設定することができる。あるいは、経路生成手段１にロボットアーム１００を接続し、ロボットアーム１００を実際にスタート・ゴール位置に位置させたときの座標などを経路生成手段１に読み込ませるようにしても良い。また、装置に組み込まれたカメラや各種センサによって外部環境情報が自動又は半自動で取得されるようにしても良い。

スタート・ゴール位置決定手段５は、環境情報記憶手段４に蓄積された情報と照らし合わせて、入力されたスタート・ゴール位置が障害物と干渉していないか否かを判断し、干渉していない場合はこれらをスタート・ゴール位置として決定する。なお、干渉の有無の検証は、既存の手法を用いることができる。もし、入力されたスタート・ゴール位置が障害物と干渉している場合はオペレータに対して再入力を促し、再入力されたスタート・ゴール位置について同様の判定を行い、スタート・ゴール位置が決定されるまで反復される。次に、公知の手法によって、スタート・ゴール位置の間に中間点が仮設定される（中間点設定行程）。この中間点の仮設定は、中間点生成手段６によって行われる。この中間点の設定方法としては、例えば、［特許文献２］に記載の確率経路探索方法などによって中間点が設定される。

なお、ここでは、最終的には手首位置Ｘでの経路を決定するため、上述した中間点は手首位置Ｘについての中間点が重要であるが、これは各関節ジョイント部分に関してスタート位置とゴール位置との中間点が設定されることによって決定される。このとき、各関節の中間点が同時に仮設定された状態では、経路についての位置のみが仮設定されただけである。即ち、ロボットアーム１００の中間姿勢が複数生成されることとなる。これら複数の中間姿勢を時間的な要素を考慮してつなぐため、ある中間点から次の中間点まで移動するまでの時間を設定する（時間設定行程）。この時間設定は、時間設定手段７によって行われる。

ここでは、ある関節ジョイント部分を例にして説明する。なお、全ての関節ジョイント部分についての経路が決定されれば、手首位置Ｘの経路が決定される。ある関節ジョイントの角度について、スタート位置での角度をθ_ｓ、ゴール位置の角度をθ_ｇとする。そして、上述した中間点生成時に、この関節ジョイント部分に関して仮設定された中間点を関節ジョイントの角度としてθ_１，θ_２，…，θ_ｎと表すものとする。例えば、ここで、θ_ｓとθ_１，θ_ｉとθ_ｉ＋１，θ_ｎとθ_ｇの間の移動にかかる時間を一定時間ｔであると仮定して、関節ジョイントの角度の時間変化をグラフにして示すと図３のようになる。

図３から分かるように、図３の角度変化を示す折れ線の傾きが角速度を表すことになる。ここでは、角速度は時間に対して一定の割合で増減するため、角加速度はゼロである。しかし、ロボットアーム１００を実際に駆動する場合には、関節ジョイント部分のアクチュエータ（モータなど）の機械的性能などから、角速度や角加速度には最大角速度や最大角加速度といった制約が存在する。これらの条件は予め経路生成手段１にインプットされており、これらの制約を考慮して時間設定が行われる。即ち、図３のように、単純に中間点間の移動にかかる時間を決めてしまうと、アクチュエータなどの機械的性能などのために実現し得ない経路設計がなされてしまう場合や、アクチュエータの駆動に不都合がある場合などがある。

そこで、ここでは、以下のように時間設定を行う。各関節ジョイントに関しては上述した制約を考慮して、最大許容角速度ｖ_ｍａｘが予め設定される。そして、θ_ｉからθ_ｉ＋１までの時間間隔（ｔ_ｉ＋１−ｔ_ｉ）として、｜（θ_ｉ＋１−θ_ｉ）／ｖ_ｍａｘ｜で定義される時間を設定すれば、角速度が許容範囲内となる時間設定を行える。二つの関節ジョイント部分における角度・角速度の時間変化の例を図４に示す。なお、図４中のΔｔ_１の長さは全ての関節ジョイント部で一致させる必要がある。図４では、関節Ａと関節Ｂとにおける各中間点での制御タイミングがすでに一致されている状態が示されている。

そこで、次に、このΔｔ_１の長さの求め方、即ち、複数の関節ジョイント部分での中間点の制御タイミングを合わせる方法について説明する。関節Ａに関しては、Δｔ_１の前後で角度がθ_１Ａからθ_２Ａに変化したとし、関節Ｂに関しては、Δｔ_１の前後で角度がθ_１Ｂからθ_２Ｂに変化したとする。このとき、上述したように、関節Ａに関しては、最大許容速度ｖ_Ａｍａｘが設定されていたとすると、関節Ａでの角速度を許容範囲内とするには、Δｔ_１は｜θ_２Ａ−θ_１Ａ｜／ｖ_Ａｍａｘ以上とする必要がある。同様に、関節Ｂに関しては、最大許容速度ｖ_Ｂｍａｘが設定されていたとすると、関節Ｂでの角速度を許容範囲内とするには、Δｔ_１は｜θ_２Ｂ−θ_１Ｂ｜／ｖ_Ｂｍａｘ以上とする必要がある。

そこで、Δｔ_１＝ｍａｘ｛｜θ_２Ａ−θ_１Ａ｜／ｖ_Ａｍａｘ，｜θ_２Ｂ−θ_１Ｂ｜／ｖ_Ｂｍａｘ｝とし、各関節で必要な時間の中で最長の時間を選択してΔｔ_１として決定する。ここでは、二つの関節を例にして説明しているが、関節が複数でも同様に行えば良い。また、Δｔ_１以降の各時間間隔についても同様にタイミングを一致させる。これで、時間設定が終了する（後で修正される場合もある）。なお、上述した例では、説明を分かりやすくするため、ロボットアーム１００が図２に示されるｘ−ｙ平面内で動く場合を例にして説明しているが、三次元的に動く場合も同様な考え方で時間設定を行えばよい。

次に、平坦化について説明する。平坦化とは、上述したように設定された中間点の中から省略可能な中間点を抽出し、抽出された中間点を削除することである。平坦化することで、経路が単純化されることになり、ロボットアーム１００の動きの無駄が低減（あるいは解消）される（平坦化行程）。平坦化を説明するための図を図５に示す。図５は、ある関節に関して上述した時間設定が行われた後の経路を示す角度−時間のチャートである。以下、この関節を例にして説明するが、最終的には全ての関節（手首位置Ｘを含む）について同様のことが同時に行われる。即ち、各巻ｓねつの角度は、同時刻において作成されるように設定するので、全ての関節（手首位置Ｘを含む）に関して省略可能な場合にのみ、その部分の平坦化を行う。

図５を参照して説明するが、まず、スタート点ｐ_１から何点先（２点以上先から端点までの間）までの平坦化を検討するか指定する。この指定は、オペレータによって入力されても良いし、予めプリセットされたデフォルト値が用いられるようにしても良い。ここでは、３点先（ｐ_４）までと指定された場合を例にして説明する。このとき、ｐ_１とｐ_４とを線分で結び、この線分上のある一定間隔毎の点が障害物と干渉するか否かを検証する。検証の結果、障害物との干渉がない場合は、ｐ_１とｐ_４とを結んだ経路を新経路とする。

一方、検証の結果、障害物と干渉する場合は、ｐ_１とｐ_３とを結んだ経路について同様の検証を行い、障害物との干渉がない場合はｐ_１とｐ_３とを結んだ経路を新経路とする。ｐ_１とｐ_３とを結んだ経路が障害物との干渉する場合は、この区間での平坦かは行われない。そして、平坦化後の終点から再度次の平坦化が行われる。例えば、ｐ_１とｐ_４とを結んだ経路を新経路とした場合は、ｐ_４をスタート点として次の平坦化処理が実行される。このようにして順次平坦化が行われる。この平坦化は、平坦化手段８によって行われる。

なお、何点先の点を指定した後、「スタート点から２点先の点から指定点までの間にある各点（指定点が２点先だと一致する）」と「スタート点」とを重複なくランダムに順次線分で結び、各線分に対して障害物との干渉を検証し、障害物と干渉しなかった場合にその線分で平坦化を行うようにしても良い。なお、障害物と干渉しない経路が発見できなかった場合は、その時間区間では平坦化処理は行われない。このようにして、仮設定された経路のスタート位置からゴール位置までの全てに関して平坦化を行う。

次に、平滑化について説明する。平滑化とは、上述したように設定された経路を滑らかな曲線にすることを指す（平滑化行程）。平坦化が終了した時点での経路は、各中間点において急激に方向が変わるものとなっている。これを、これを滑らかな曲線とすることで、ロボットアーム１００の動きが滑らかとなるようになる。また、ここでは、平滑化後の経路が無限回微分可能な関数によって近似されるため、経路自体の滑らかさに加えて、その経路上の移動速度及び加速度の滑らかさも保証される。さらに、平滑化に際して、滑らかにした経路が障害物と干渉するか検証し、干渉する場合は衝突が回避されるまで平滑化が再履行される。このとき、回避できる経路の存在は平滑化の原理（追って詳しく説明する）によって保証されている。

平滑化に際しては、まず、次の条件（ｉ）〜（ｉｉｉ）を満足するｘの関数ｇ（ｘ）を一つ選んで固定する。
（ｉ） 関数ｇは実ベクトル空間上で定義された無限回微分可能な関数であり、台（ｓｕｐｐｏｒｔ）がコンパクトな非負関数である。
（ｉｉ） 関数ｇの台（ｓｕｐｐｏｒｔ）は実ベクトル空間の原点中心の単位超球の内部に含まれている。
（ｉｉｉ）関数ｇは実ベクトル空間上で密度関数である。
この関数ｇを用いて、正数のパラメータｄに対して、関数ｇ_ｄを下記式（１）によって定義する。なお、下記式（１）中のＮは実ベクトル空間の次元数である。

実ベクトル空間Ｖ上で局所可積分な関数をｆとすると、関数ｇ_ｄと関数ｆとの合成積（関数ｆ_ｄ）は下記式（２）のように定義される。

関数ｆに関数ｆ_ｄを対応させる作用素はフリードリスクの軟化子と呼ばれている。このとき、「連続で台がコンパクトな連続関数ｆに対して、ｄが０に近づくとき、関数ｆ_ｄは関数ｆに収束する。」という事実が知られている。この事実を根拠として、区分的に線形な経路（上述した中間点で折れ曲がる経路）の平滑化を行う。

実数上で定義された関数ｇとして、例えば下記（３）式で示される関数ｇ（ｘ）を用いる。この下記式（３）の関数ｇ（ｘ）は、台（ｓｕｐｐｏｒｔ）が−１から１の間に含まれており、実数上で滑らか（無限回微分可能）な関数である。

平滑化を説明するための図を図６に示す。図６では、平坦化後・平滑化前の時間−角度の関係が点線で示されており、平滑化後の時間−角度の関係が実線で示されている。また、ｐ_１〜ｐ_４が中間点位置での時間と角度の位置を示す点である。まず、平滑化経路の近似誤差を制御する誤差制御変数｛ｄ_１，ｄ_２，ｄ_３，ｄ_４…｝の設定方法について説明する。ここでは、誤差制御変数ｄ_２について説明するが、他の誤差制御変数も同様の手法によって設定される。

点ｐ_ｉの座標を（ｔ_ｉ，θ_ｉ）とし、この点を以下ノードと呼ぶこととする。ｐ_１−ｐ_２の間の時間（ｔ_２−ｔ_１）とｐ_２−ｐ_３間の時間（ｔ_３−ｔ_２）とを比べると、ここでは、（ｔ _２ −ｔ _１ ）間の時間の方が短い。そこで、誤差制御変数ｄ_２の「最大値」を（ｔ_２−ｔ_１）／２とする。ノードｐ_２近傍の平滑化を行う場合には、制御誤差変数としてｄ_２を用いて平滑化を行う（式（１）のｄとして誤差制御変数としてｄ_２を用いる）。即ち、ノードｐ_ｉ近傍の平滑化を行う際には、誤差制御変数としてｄ_ｉを用いる。なお、平滑化を行うことで障害物と干渉するようになってしまう場合が考えられる。そこで、障害物との干渉を検証しながら平滑化が進められる（経路修正行程）。本実施形態では、平滑化行程と経路修正行程とが一体的に実施される。

ここで、ｄ_２の「最大値」は（ｔ_２−ｔ_１）／２とされたが、誤差制御変数ｄ_２の値が大きいほど、ノードｐ_２前後のより広い範囲で平滑化が行われることになり、経路はより滑らかになる。そこで、平滑化処理時には、誤差制御変数ｄ_２として小さい値から順次平滑化を行い、障害物と干渉しないもっと大きい値のｄ_２で確定する。また、ｄ_２の値を大きい値から始めて平滑化を行うようにしても良い。誤差制御変数ｄ_２を順次大きくしつつ、できる限り大きな誤差制御変数ｄ_２を採用することで、より滑らかな平滑化を行える。なお、図６には、誤差制御変数ｐ_２，ｐ_３の双方とも、最大値が採用できた場合が示されている。この平滑化は平滑化手段９によって行われる。

上述した手法によれば、時間設定の手法から分かるように角速度が許容し得る最大角速度を超えるようなことはない。しかし、角加速度に関しては、平滑化後に、許容し得る最大角加速度を超えないことは保証されていない。そこで、角速度の最大許容角速度ｖ_ｍａｘと同様に、角加速度についても最大許容角加速度ａ_ｍａｘが予め設定される。そして、角加速度がこの最大許容角加速度ａ_ｍａｘを超えないように、平滑化後の経路が修正される（時間修正行程）。この経路修正は、経路修正手段１０によって行われる。この経路修正行程では、時間スケールが局所的に修正され、これによって角加速度が許容範囲内に納められる。

なお、以下、ある関節（手首位置Ｘを含む）を例にして説明するが、最終的には全ての関節（手首位置Ｘを含む）について同様のことが行われる。時間−角度、時間−角速度、時間−角加速度の関係を示した例を図７に示す。図７中、点線が平坦化後・平滑化前の経路についての角度・角速度・角加速度を示しており、実線が平滑化後の経路についての角度・角速度・角加速度を示している（角加速度に関しては、平坦化後・平滑化前の経路ではつねに０であるので図示を省略する）。図７からも分かるように、上述したノードを提示する時刻で角加速度は極大値・極小値をとる。ノード近傍の経路は、上述した合成積の関数ｆ_ｄによって近似されているため、ｆ_ｄの時間による２回導関数（∂^２／∂ｔ^２）ｆ_ｄ（ｔ）を求め、この２回導関数に各ノードを定義する時刻｛ｔ_１，ｔ_２，ｔ_３，…，ｔ_ｎ｝を代入することで、各時刻での角加速度｛（∂^２／∂ｔ^２）ｆ_ｄ（ｔ_１），（∂^２／∂ｔ^２）ｆ_ｄ（ｔ_２），（∂^２／∂ｔ^２）ｆ_ｄ（ｔ_３），…，（∂^２／∂ｔ^２）ｆ_ｄ（ｔ_ｎ）｝を求めることができる。

そして、求められた各時刻での角加速度｛（∂^２／∂ｔ^２）ｆ_ｄ（ｔ_１），（∂^２／∂ｔ^２）ｆ_ｄ（ｔ_２），（∂^２／∂ｔ^２）ｆ_ｄ（ｔ_３），…，（∂^２／∂ｔ^２）ｆ_ｄ（ｔ_ｎ）｝のなかから、上述した最大許容角加速度ａ_ｍａｘを超えるものを抽出する。なお、ここで、合成積の積分記号下の微分定理を利用すれば、導関数を容易に求められる。例えば、時刻ｔ_２での角加速度ａ_２＝（∂^２／∂ｔ^２）ｆ_ｄ（ｔ_２）＞ａ_ｍａｘであった場合は、時刻ｔ_２−ｄ_２からｔ_２＋ｄ_２までのΔｔ＝２ｄ_２の区間の時間を（ａ_２／ａ_ｍａｘ）倍に引き延ばす。なお、ｄ_２は、ｔ_２近傍で用いられた誤差制御変数である。このように、時間軸を局所的に引き延ばすことで、角加速度は許容範囲内に修正される。

なお、ある関節について、角加速度条件から時間軸の引き延ばしが行われた場合は、同時刻における他の関節における角加速度が許容範囲内であっても、全ての関節についての時間軸も引き延ばされ、全体としての制御タイミングが一致するようになされる。この後、各関節ジョイント部１０２（手首位置Ｘを含む）に関して角度、角速度、角加速度の時間に関するグラフを作成すれば、全ての関節について角度、角速度、角加速度の滑らかな設計が完了している。

なお、本発明は上述した実施形態に限定されるものではない。例えば、上述した本実施形態では、関数ｇ（ｘ）として式（３）で示されたものを用いたが、条件（ｉ）〜（ｉｉｉ）が満たされれば、他の関数を用いても良い。また、上述した実施形態では、多関節ロボットアーム１００の経路を設定する場合を説明した。しかし、本発明は、ロボットアームの移動経路以外にも適用することができ、自動機械の全体の移動経路や、自動機械の一部（上述したアームなど）の移動経路を設定する場合などにも適用することができる。

本発明の経路設定方法の一実施形態を実施する装置の構成図である。 本発明の経路設定方法の一実施形態によって制御される他関節ロボットアームを示す図である。 各中間点間の時間を一定とした場合の時間−関節角度のグラフである。 時間設定後の時間−関節角度のグラフであり、（ａ）は関節Ａについての、（ｂ）は他の関節Ｂについてのグラフである。 平坦化を説明する時間−関節角度のグラフである。 平滑化を説明する時間−関節角度のグラフである。 （ａ）時間−角度、（ｂ）時間−角速度、（ｃ）時間−角加速度の関係を示すグラフである。

符号の説明

１…経路生成手段、２…外部環境情報入力手段、３…経路出力手段、４…環境情報記憶手段、５…スタート・ゴール位置決定手段、６…中間点生成手段、７…時間設定手段、８…平坦化手段、９…平滑化手段、１０…経路修正手段、１００…多関節ロボットアーム、１０１…リンク、１０２…関節ジョイント、１０３…把持部。

Claims (4)

1. 自動機械の全体又は一部分の移動経路を設定する経路設定方法において、
   前記自動機械の全体又は一部分を経路設定対象部として、該経路設定対象部の初期位置から最終位置の間に複数の中間点を仮設定する中間点設定行程と、
   前記経路設定対象部が前記中間点の間を移動する時間を設定する時間設定行程と、
   所定の中間点と前記初期位置、前記所定の中間点と他の中間点、又は前記所定の中間点と前記最終位置とを結んで、障害物との干渉がない場合は、前記所定の中間点と前記初期位置との間、前記所定の中間点と前記他の中間点との間、又は前記所定の中間点と前記最終位置との間に存在する中間点を省略することで、仮設定された前記経路を平坦化する平坦化行程と、
   任意の中間点を直線的に結んだ状態の平坦化後の前記経路を、近似誤差を制御する変数に基づいて滑らかな経路に変更することで平滑化する平滑化行程と、
   平滑化後の前記経路が障害物と干渉するか否かを判定し、障害物と干渉する場合は前記経路上の障害物との干渉部分近傍を局所的に変更することで経路修正する経路修正行程とを備え、
   前記平滑化行程において、前記任意の中間点のうち時間ｔ _ｉ に係る中間点ｐ _ｉ を平滑化する場合、
   前記中間点ｐ _ｉ に対して前記近似誤差を制御する変数である誤差制御変数ｄ _ｉ が設定され、（ｔ _ｉ −ｄ _ｉ ）から（ｔ _ｉ ＋ｄ _ｉ ）の範囲で前記経路を無限回数微分可能な関数で近似することで平滑化が行われ、
   前記中間点ｐ _ｉ の隣の中間点ｐ _ｉ−１ と前記中間点ｐ _ｉ との間の時間を（ｔ _ｉ −ｔ _ｉ−１ ）とし、前記中間点ｐ _ｉ と当該中間点ｐ _ｉ の隣の中間点ｐ _ｉ＋１ との間の時間を（ｔ _ｉ＋１ −ｔ _ｉ ）とした場合、
   前記誤差制御変数ｄ _ｉ は、（ｔ _ｉ −ｔ _ｉ−１ ）／２と（ｔ _ｉ＋１ −ｔ _ｉ ）／２のうち、何れか小さい方の値以下の値に設定され、
   前記経路修正行程においては、前記誤差制御変数ｄ _ｉ を変更することで経路修正が行われることを特徴とする経路設定方法。
2. 前記自動機械が、複数のリンク部材を関節ジョイントを介して互いに接合したアームであり、該アームの移動経路を設定する経路設定方法において、
   前記時間設定行程において、各関節ジョイント部分の最大角速度を考慮して、各中間点間の移動時間が設定されることを特徴とする請求項１に記載の経路設定方法。
3. 前記自動機械が、複数のリンク部材を関節ジョイントを介して互いに接合したアームであり、該アームの移動経路を設定する経路設定方法において、
   前記平滑化行程において、各関節ジョイント部分の角度を時間の関数として捉え、時間の関数である各関節ジョイントの角度を無限回微分可能な関数で近似することで平滑化を行うことを特徴とする請求項１又は２に記載の経路設定方法。
4. 各関節ジョイント部分の角加速度が所定の最大角加速度を超えた場合に、最大角加速度を超える時刻近傍において局所的に時間設定変更を行う時間修正行程をさらに備えていることを特徴とする請求項３に記載の経路設定方法。
   

Images