JP6501470B2 - 軌道生成方法、軌道生成装置、ロボット装置、プログラム及び記録媒体 - Google Patents

Description

本発明は、ロボットの軌道を生成する軌道生成方法、軌道生成装置、ロボット装置、プログラム及び記録媒体に関する。

ロボットの補間動作には、主に直線補間、円弧補間及び関節補間の３種が知られている。直線補間は、目標点にツールセンターポイント（ＴＣＰ）を直進させる補間方法である。円弧補間は、意図的に２点間の直路から外れた位置に参照ポイントを与え、３点を通る一義的な円弧に沿ってＴＣＰを移動させる補間方法である。この円弧補間と直線補間はタスク空間で補間を行うので、本質的には同じものである。

関節補間は、ＴＣＰの直線性や円弧性は問わず、目標の関節角度に向かって各軸が原則的に同時に動き出し、同時に目標点に到達する補間方法である。すなわち、関節補間は、関節空間（コンフィグレーション空間ともいう）で経路を生成する手法である。関節補間によれば、各関節の１００％の動作性能を発揮する制御が容易であり、また、直線補間や円弧補間で問題となる、特異点問題が起きないので、高速動作が必要な移動区間で多用される。ただし、関節ごとの速度制御となるために、ＴＣＰの経路や速度はユーザが簡単に予測できるものでない。

ロボットの動作は、その移動の都度、その間の動作や目的にふさわしい補間方式が採択される。例えば、円筒部品をロボットが把持し、円筒部品を円筒状の穴の上まで移動させ、円筒部品をその穴に嵌め込む場合がある。この場合、把持した後のロボットの移動は、関節補間で高速に動作し、円筒状の穴の上から円筒部品を穴に嵌め込む動作は直線補間を用いて、真っ直ぐに嵌め込むことが望まれる。

ところで、各移動区間における補間動作は、補間経路上をどのような速度で移動するかを決める速度関数で与えられる。移動開始時と移動終了時にロボットが停止しているとすると、移動開始後は、加速をして移動速度を上げ、移動終了前には減速をして停止するという動作となる。

ここで、２つの異なる経路を連続して動かす方法として、経路のつなぎ目で停止することが考えられる。例えば、円筒部品を穴に嵌め込む例の場合、関節補間経路で加速をして移動を行い、関節補間経路の終了前に減速して経路のつなぎ目で停止し、直線補間経路で徐々に速度を上げるようにしておけば、２つの補間経路上を連続して動作させることができる。しかし、２つの補間経路の間でのロボットの厳格な停止は、無用であるだけでなく、この部分での停滞気味の挙動がロボットの動作の時間を長引かせることになる。もちろん、作業時間節約のために急減速や急加速させるとすれば、それはロボットの振動を助長させるもとになり好ましいことではない。

そこで、例えば、直線補間と直線補間とを連続して動作させる場合、それぞれの直線補間経路上での速度関数を先に導きだした後に、先の移動区間の減速域と後の移動区間の加速域とを繋ぎ合せる。そして、両区間の繋ぎ点もしくはその近傍を先の移動区間の減速中の速度と後の移動区間の加速中の速度との和で通過させればよい。これにより、次の教示点に到達させる時間を縮めることができる。このような速度の重ね処理は、先の移動区間の補間方法と後の移動区間の補間方法とが同種である場合、その速度関数自体を繋ぎ合せることによってある意味では容易に達成される。直線補間と直線補間の場合には、タスク空間上で、先の並進速度関数と後の並進速度関数とを、先の回転速度関数と後の回転速度関数とを、それぞれ減速域と加速域とでそれぞれの全部もしくは一部を重ねればよい。

関節補間で動作させる場合は、６軸ロボットであれば関節（軸）ごとに速度関数が与えられる。この場合でも関節補間同士の速度の重ね処理は、それぞれの関節補間経路上での速度関数を求めた後に、第１関節の先の速度関数の減速域と後の速度関数の加速域とを、第２関節の先の速度関数と後の速度関数とをといったように関節ごとを対象にして行えばよい。従って、６自由度のマニピュレータであれば、６つの組合せ全部で重ねられることになる。

しかし、ロボットの動作が関節補間から直線補間に移る場合は、補間をする空間がそれぞれ、関節空間とタスク空間とで異なり、関節補間の関節角速度と、直線補間のＴＣＰの並進速度及び回転速度の繋ぎ合せ処理がそのままではできない。

そこで、特許文献１では、関節補間から直線補間に移る場合、直線補間をしてＴＣＰの並進速度及び回転速度を出した後、逆キネマティクス計算を行い各軸の関節の角速度に直してから、関節補間軌道との速度の繋ぎ合せを行っている。直線補間の速度を関節の角速度に直して速度を繋ぎ合せるため、関節角速度の繋ぎ合せが可能となる。

一方、非特許文献１では、与えられた経路上で、各種物理的制約を守り、動作時間を最短にするロボットの最適な動作速度を計算する最適速度の計算方法が述べられている。与えられた経路で、ロボットの動作速度を調整し動作軌道を作成するには、通常、各種物理的制約を守りつつ、ロボットの動作時間が短くなるように、ユーザが速度値を何度も調整する必要があった。しかし、人間が速度値の調整をすると、軌道が最適にならず、動作時間が長くなってしまうと同時に、軌道開発工数がかかってしまう。そこで、各種物理的制約を守り、動作時間を最短にするロボットの最適な動作速度の計算を自動で行うことが望まれる。

ロボットの関節トルク制約を守りつつ、与えられた経路に対し、できる限り動作時間の短い軌道を生成する最適速度計算方法について非特許文献１に記載されている。

特開２００４−２５２８１４号公報

Ｖｅｒｓｃｈｅｕｒｅ，Ｄｉｅｄｅｒｉｋ，ｅｔ ａｌ． "Ｔｉｍｅ−ｏｐｔｉｍａｌ ｐａｔｈ ｔｒａｃｋｉｎｇ ｆｏｒ ｒｏｂｏｔｓ： ａ ｃｏｎｖｅｘ ｏｐｔｉｍｉｚａｔｉｏｎ ａｐｐｒｏａｃｈ．" Ａｕｔｏｍａｔｉｃ Ｃｏｎｔｒｏｌ， ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ５４．１０ （２００９）： ２３１８−２３２７．

しかしながら、上記特許文献１及び上記非特許文献１によっても、２つの補間経路を繋ぎ合せたときに物理的制約を考慮にいれた最適な軌道を生成することができなかった。

非特許文献１には、生成した２つの補間経路を、媒介変数に関して滑らかさを保って繋ぐ方法については記載されていない。また、引用文献１でも、２つの補間経路を繋げる方法は開示されているが、２つの補間経路を媒介変数ｓに関して滑らかさ（所定の階数まで微分した微分値の変化が連続な滑らかさ）をもたせたまま繋げることについては、記載されていない。

つまり、従来、第１補間方法で生成した第１補間経路と第２補間方法で生成した第２補間経路とを媒介変数ｓに関して滑らかさ（所定の階数まで微分した微分値の変化が連続な滑らかさ）をもたせたまま繋げることは、これまで行われていなかった。例えば、関節補間と直線補間という経路を補間する空間がそれぞれ違う２つの補間経路や、円弧補間と直線補間といった形状が違う２つの補間経路を媒介変数ｓに関して滑らかさをもたせたまま繋げることは、行われていなかった。

従来、２つの補間経路を媒介変数ｓに関して滑らかさをもたせたまま繋げていなかったので、ロボットへの時刻毎の関節角度の指令値を生成する、つまりロボットの軌道を生成する際に、軌道の最適性に大きな誤差を生む原因となっていた。

例えば、関節ｉ軸の関節加加速度は、ｓを用いて、

と書ける。もしｑ_ｉ（ｓ）がｓに関して滑らかでなく、単に連続である場合、ｑ_ｉ’（ｓ）、ｑ_ｉ’’（ｓ）、ｑ_ｉ’’’（ｓ）は不連続になるため、ｑ_ｉ（…）（ｓ）は、媒介変数ｓに関して不連続になる。

このような滑らかでない（所定の階数まで微分した微分値の変化が不連続である）経路で、速度の最適化計算を行った場合、一定周期毎のロボットへの関節角度の指令値を生成する際に大きな誤差が生じていた。これにより、例えば関節加加速度が制約から大幅に外れる原因となっていた。また、これは、関節加加速度だけに限定されるものではなく、関節トルク制約、関節角速度制約、関節角加速度制約、ＴＣＰ速度制約又はＴＣＰ加速度制約等の物理的制約も同様に大きな誤差を生み、制約から大幅に外れる原因となっていた。

そこで、本発明は、２つの補間経路を繋ぎ合せた経路を辿るロボットが停止しない、物理的制約を守った最適な軌道を生成することを目的とする。

本発明は、演算部が、多関節のロボットの軌道を生成する軌道生成方法であって、前記演算部が、媒介変数の関数であって、所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第１教示点から第１補助点に向かう第１補間経路を、第１補間方法により生成する第１補間経路生成工程と、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第２補助点から第２教示点に向かう第２補間経路を、第２補間方法により生成する第２補間経路生成工程と、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、前記第１補間経路上の第１途中点から前記第２補間経路上の第２途中点に向かう繋ぎ合せ経路を生成する繋ぎ合せ工程と、前記演算部が、前記第１補間経路における前記第１教示点と前記第１途中点との間の第１局所経路と、前記第２補間経路における前記第２途中点と前記第２教示点との間の第２局所経路とを、前記繋ぎ合せ経路で繋ぎ合せた経路に対し、所定の物理的制約を守るよう前記ロボットの動作時間を最適化する最適化計算によって、前記ロボットの前記軌道を求める最適化計算工程と、を備え、前記繋ぎ合せ工程では、前記演算部が、前記第１途中点から前記第１補助点までの各第１補間点に対して、前記第１教示点に近い側の第１補間点から順に、１から０に順次遷移する第１重み係数をかけ、前記第２補助点から前記第２途中点までの各第２補間点に対して、前記第２補助点に近い側の第２補間点から順に、１から前記第１重み係数を引いた値となるよう０から１に順次遷移する第２重み係数をかけ、前記第１補間点に前記第１重み係数をかけて求めた点と、前記第２補間点に前記第２重み係数をかけて求めた点とを足し合わせて、前記繋ぎ合せ経路を生成し、前記繋ぎ合せ工程では、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数による微分値の変化が連続な、第１重み関数を用いて、前記第１重み係数を求め、前記所定の階数まで前記媒介変数による微分値の変化が連続な、第２重み関数を用いて、前記第２重み係数を求めることを特徴とする。
また、本発明は、演算部が、多関節のロボットの軌道を生成する軌道生成方法であって、前記演算部が、媒介変数の関数であって、所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第１教示点から第１補助点に向かう第１補間経路を、第１補間方法により生成する第１補間経路生成工程と、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第２補助点から第２教示点に向かう第２補間経路を、第２補間方法により生成する第２補間経路生成工程と、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、前記第１補間経路上の第１途中点から前記第２補間経路上の第２途中点に向かう繋ぎ合せ経路を生成する繋ぎ合せ工程と、前記演算部が、前記第１補間経路における前記第１教示点と前記第１途中点との間の第１局所経路と、前記第２補間経路における前記第２途中点と前記第２教示点との間の第２局所経路とを、前記繋ぎ合せ経路で繋ぎ合せた経路に対し、所定の物理的制約を守るよう前記ロボットの動作時間を最適化する最適化計算によって、前記ロボットの前記軌道を求める最適化計算工程と、を備え、前記第１補間方法及び前記第２補間方法がそれぞれタスク空間で補間経路を生成する方法及び関節空間で補間経路を生成する方法のうちいずれかであり、前記繋ぎ合せ工程では、前記演算部が、前記第１補間経路及び前記第２補間経路を、前記タスク空間及び前記関節空間のうちいずれかの空間における経路に統一して、前記統一した空間において繋ぎ合せ経路の生成を行うことを特徴とする。

本発明によれば、媒介変数に関して滑らかな（所定の階数まで微分した微分値の変化が連続である）ロボットの経路を生成しているので、２つの補間経路を繋げ合せた経路を辿るロボットが停止することのない、物理的制約を守った軌道を生成することができる。

本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。 本発明の第１実施形態に係るロボット装置のロボットを示す模式図である。 本発明の第１実施形態に係る軌道生成装置の構成を示すブロック図である。 本発明の第１実施形態における重み関数を示すグラフである。 本発明の第１実施形態におけるタスク空間でのロボットの経路を示す説明図である。 本発明の第１実施形態における関節空間でのロボットの経路を示す説明図である。 本発明の第１実施形態に係る軌道生成方法の主要なステップを示すフローチャートである。 本発明の第１実施形態に係る軌道生成方法の経路生成処理を示すフローチャートである。 本発明の第２実施形態におけるタスク空間でのロボットの経路を示す説明図である。 本発明の第２実施形態における関節空間でのロボットの経路を示す説明図である。 本発明の第２実施形態に係る軌道生成方法の経路生成処理を示すフローチャートである。 本発明の第３実施形態におけるタスク空間でのロボットの経路を示す説明図である。 本発明の第３実施形態における関節空間でのロボットの経路を示す説明図である。 本発明の第３実施形態に係る軌道生成方法の経路生成処理を示すフローチャートである。 本発明の第４実施形態におけるタスク空間でのロボットの経路を示す説明図である。 本発明の第４実施形態における関節空間でのロボットの経路を示す説明図である。 本発明の第４実施形態に係る軌道生成方法の経路生成処理を示すフローチャートである。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。ロボット装置５００は、多関節（２関節以上）のロボット２００と、ロボット２００の軌道を生成する軌道生成装置１００と、生成された軌道に基づきロボット２００の動作を制御するロボットコントローラ３００と、を備えている。また、ロボット装置５００は、作業者（ユーザ）の操作により軌道生成装置１００にデータ入力等を行う操作部４１０と、軌道生成装置１００から表示データを入力して画像表示を行う表示部４２０とを有する操作装置４００を備えている。

軌道生成装置１００は、演算部であるＣＰＵ（Central Processing Unit）１０１と、種々のデータを格納する記憶部１０６と、を備えたコンピュータで構成されている。操作装置４００の表示部４２０は、ディスプレイであり、ロボットアーム２１０や作業空間を３Ｄ（３次元）グラフィックス表示して、生成された動作軌道を作業者が視認できるよう構成されている。なお、操作部４１０と表示部４２０とが１つの操作装置４００に組み込まれている場合について説明するが、操作部４１０と表示部４２０とが独立した装置で構成されていてもよい。また、操作部４１０と表示部４２０とが別々に構成されている場合について説明するが、操作部４１０と表示部４２０とがタッチパネルディスプレイ等の操作及び表示が可能な操作表示部で構成されていてもよい。

ＣＰＵ１０１は、後述するプログラムを読み出して実行することにより、キネマティクス計算部３１、経路生成部３２、経路繋ぎ合せ部３３、教示点計算部３４及び最適速度計算部３５として機能する。

図２は、本発明の第１実施形態に係るロボット装置のロボットを示す模式図である。ロボット２００は、多関節（２関節以上、第１実施形態では２関節）のロボットアーム２１０と、ロボットアーム２１０の先端に取り付けられたエンドエフェクタ、例えばロボットハンド２２０とを有している。

ロボットアーム２１０は、例えば２つの関節（軸）Ｊ_１，Ｊ_２とリンク２０１，２０２で構成される２自由度のロボットアームである。関節には回転関節や直動関節の他、球体関節などさまざまな種類があるが、第１実施形態では、関節Ｊ_１，Ｊ_２は、回転（旋回）関節である。関節の動作はモータ等の動力源で能動的に動作する場合と、動力源を持たずに受動的に動作する場合があるが、第１実施形態では、関節Ｊ_１，Ｊ_２は、動力源を有する。ロボットアームは、各関節間がリンクにより結びつけられ、関節とリンクが交互に直列されたシリアルリンク型と、関節とリンクの組合せが並列となったパラレルリンク型などがあるが、第１実施形態では、回転関節を有するシリアルリンク型とする。つまり、ロボットアーム２１０は、ロボットアーム自体が動作するような動作体として広く観念されるものであればよく、特に限定するものではない。例えば、６軸多関節ロボットアームや直動型のロボットアームを用いてもよい。

ロボットアーム２１０（ロボット２００）の自由度を表すパラメータを関節角度として、ロボットアーム２１０の２軸Ｊ_１，Ｊ_２の関節角度をそれぞれθ_１，θ_２とする。ロボットアーム２１０のコンフィグレーションは（θ_１，θ_２）で表され、関節空間上では、１つの点とみなすことができる。このように、ロボットアーム２１０の自由度を表すパラメータ（例えば、関節角度や伸縮長さ）を座標軸の値とした場合、ロボットアーム２１０のコンフィグレーションは関節空間上の点として表現することができる。つまり、関節空間は、ロボットアーム２１０（ロボット２００）の関節角度を座標軸とする空間である。

また、ロボット２００の先端、即ちロボットハンド２２０には、ツールセンターポイント（ＴＣＰ）が設定されている。ＴＣＰは、位置を表す３つのパラメータ（ｘ，ｙ，ｚ）と、姿勢（回転）を表す３つのパラメータ（α，β，γ）、即ち６つのパラメータ（ｘ，ｙ，ｚ，α，β，γ）で表され、タスク空間上では、１つの点としてみなすことができる。つまり、タスク空間は、これら６つの座標軸で規定された空間である。

図３は、本発明の第１実施形態に係る軌道生成装置１００の構成を示すブロック図である。図３に示すように、コンピュータで構成されている軌道生成装置１００は、演算部としてのＣＰＵ１０１を備えている。また軌道生成装置１００は、記憶部１０６として、ＲＯＭ（Read Only Memory）１０２、ＲＡＭ（Random Access Memory）１０３及びＨＤＤ（Hard Disk Drive）１０４を備えている。また、軌道生成装置１００は、記録ディスクドライブ１０５及び各種のインタフェース１１１〜１１３を備えている。

ＣＰＵ１０１には、ＲＯＭ１０２、ＲＡＭ１０３、ＨＤＤ１０４、記録ディスクドライブ１０５及びインタフェース１１１〜１１３が、バス１２０を介して接続されている。

ＲＯＭ１０２には、ＢＩＯＳ等の起動プログラムが格納されている。ＲＡＭ１０３は、ＣＰＵ１０１の演算処理結果等、各種データを一時的に記憶する。ＨＤＤ１０４には、プログラム１４０が格納（記録）されている。そして、ＣＰＵ１０１は、プログラム１４０を読み出して実行することにより、後述する軌道生成方法の各工程を実行する。つまり、ＣＰＵ１０１は、プログラム１４０を読み出して実行することにより、図１に示す各部３１〜３５として機能する。記録ディスクドライブ１０５は、記録ディスク１４１に記録された各種データやプログラム等を読み出すことができる。

インタフェース１１１には、上述した操作装置４００が接続されている。インタフェース１１２には、ＵＳＢメモリ等の書き換え可能な不揮発性メモリ、或いは外付けＨＤＤ等の外部記憶装置６００が接続されている。インタフェース１１３には、上述したロボットコントローラ３００が接続されている。

図１において、キネマティクス計算部３１は、ロボット２００（ロボットアーム２１０）の各関節角度情報から、ロボット２００のＴＣＰの位置及び回転（姿勢）を求める順キネマティクスを計算する。また、キネマティクス計算部３１は、ロボット２００のＴＣＰの位置及び回転（姿勢）から、ロボット２００（ロボットアーム２１０）の各関節角度を計算する逆キネマティクスを計算する。

キネマティクス計算部３１は、例えば、教示点として、コンフィグレーション（θ_１，θ_２）を与えられ、それを基準として直線補間を行う場合、コンフィグレーションを順キネマティクスによりＴＣＰの位置及び回転にする。逆に、キネマティクス計算部３１は、教示点として、ＴＣＰの位置及び回転を与えられ、関節補間をする場合は、ＴＣＰの位置及び回転を逆キネマティクスによりコンフィグレーションにする。

経路生成部３２は、教示点間を補間するロボット２００の経路（補間経路）を生成する。ロボット２００の経路とは、関節空間におけるロボット２００のコンフィグレーションの軌跡、もしくはタスク空間におけるロボット２００のＴＣＰの軌跡である。換言すれば、ロボット２００の経路とは、関節空間又はタスク空間の点（ポーズ）の順序集合である。ロボット２００の軌道とは、時間をパラメータとしてポーズ（経路）を表したものであり、第１実施形態では、時刻毎のロボット２００の関節の関節指令値（角度指令値）の集合である。ロボット２００の関節は複数あるため、各関節で関節指令値の同期がとられる。

教示点間を補間する補間方法としては、Ｓｐｌｉｎｅ補間、Ｂ−Ｓｐｌｉｎｅ補間、ベジェ曲線、直線補間、円弧補間、関節補間などがあり、媒介変数ｓの値により、ロボット２００の経路上の位置を示す。つまり、媒介変数ｓの値にロボット２００の位置（補間点）が対応付けられて、これら点の集合で一連の経路が形成される。これら補間方法により、媒介変数ｓをパラメータとした補間関数が、ロボット２００の経路（補間経路）として求められる。

関節加加速度の制約を考慮に入れる場合、媒介変数ｓに関して経路が３階まで微分した微分値の変化が連続である必要がある。例えば、３次のＢ−Ｓｐｌｉｎｅ曲線の場合、媒介変数ｓに対しＣ^３級（３階まで微分した値が連続）になる。

最適速度計算部３５は、算出されたロボット２００の経路に対し、所定の物理的制約を守りつつ、ロボット２００の動作時間を最短にするという最適化問題を解き、ロボット２００の最適速度を計算する。所定の物理的制約とは、第１実施形態では、ロボット２００の関節トルクの制約、関節角速度の制約、関節角加速度の制約、関節角加加速度の制約、ＴＣＰ速度の制約及びＴＣＰ加速度の制約である。ロボット２００のＴＣＰ速度の制約とは、ロボット２００の先端の速度の制約である。ロボット２００のＴＣＰ加速度の制約とは、ロボット２００の先端の加速度の制約である。

第１実施形態では、所定の物理的制約には、これら全ての制約が含まれる場合について説明するが、これに限定するものではない。所定の物理的制約には、少なくとも、関節角速度の制約及びＴＣＰ速度の制約のうち少なくとも１つの制約が含まれている。これに加え、所定の物理的制約には、関節トルクの制約、関節角加速度の制約、関節角加加速度の制約及びＴＣＰ加速度の制約のうち少なくとも１つの制約が含まれていればよい。それぞれの制約値の中で、最も高い階数以上の階数を持った経路を生成すれば、計算上の制約値を計算するモデルも連続となり、正確に制約値を計算することができる。

関節トルクの制約、関節角加速度の制約、関節角加加速度の制約及びＴＣＰ加速度の制約を条件にロボット２００の軌道を求める際には、媒介変数ｓで経路を１〜３階微分した微分値を用いる。このため、経路は媒介変数ｓに対し少なくともＣ^３級（３階微分した微分値の変化が連続）である必要がある例について説明する。つまり、所定の物理的制約には、経路を媒介変数ｓで１〜３階微分した微分値を用いて評価する制約が含まれており、第１実施形態では、関節トルクの制約、関節角加速度の制約、関節角加加速度の制約及びＴＣＰ加速度の制約が含まれている。

最適化問題の解き方としては、公知の方法を用いればよく、例えば、パラメータを置き換えて、問題を凸問題化し、ログバリア法とニュートン法を用いて大域的収束解に高速に収束させるといった方法を用いればよい。

経路繋ぎ合せ部３３は、２つの補間経路それぞれに繋ぎ合せ区間（重ね合せ区間）を設け、２つの補間経路の繋ぎ合せ区間上の点に重み係数を乗じて足し合せ、２つの補間経路を所定の滑らかさ（ここではＣ^３級の滑らかさ）で繋ぎ合せた経路を生成する。

関節補間及び直線補間でそれぞれ生成された、媒介変数ｓに関してＣ^３級の２つの補間経路を、経路繋ぎ合せ部３３が、媒介変数ｓに関してＣ^３級の滑らかさを保ち繋ぎ合せる例を以下に説明する。

媒介変数ｓに関して滑らかに、関節空間で補間した補間経路とタスク空間で補間した補間経路とを繋ぐためには、各補間経路に繋ぎ合せ区間を設け、徐々に関節空間補間経路からタスク空間補間経路に移り変える必要がある。徐々に移り変える際に、繋ぎ合せの重みを徐々に移していくことが、滑らかに繋ぐ上で必須となる。

重み関数の条件として、繋ぎ合せの区間の開始点と終了点で、媒介変数ｓでの１〜３階の微分値が０になり（０とみなせるものも含む）、かつ媒介変数ｓに関して少なくともＣ^３級での滑らかさが必要になる。そこで、Ｃ^∞級であるシグモイド関数を応用して重みづけをすることを考える。

シグモイド関数は、

である。しかし、このままでは、定義域が−∞≦ｘ≦∞となり扱いにくい。そこで、シグモイド関数とｔａｎ（ｘ）との合成関数を以下のように考える。

更に、

とする。但し、ａは０より大きい実数である。

この重み関数は、ａを十分大きくとれば、定義域の開始点と終了点で、１〜３階微分値がほぼ０とみなせるため、異なる補間方法で作成した補間経路を媒介変数ｓに関してほぼＣ^３級を保ち滑らかに繋ぐ重みづけに利用できる。

図４は、ａ＝５の時の第１重み関数ｆ_ｓｉｇ１（ｘ）、第２重み関数ｆ_ｓｉｇ２（ｘ）のグラフであり、図４（ａ）は第１重み関数ｆ_ｓｉｇ１（ｘ）のグラフ、図４（ｂ）は第２重み関数ｆ_ｓｉｇ２（ｘ）のグラフである。

なお、第１重み関数はｘの増加に応じて０から１に順次変化する関数であり、第２重み関数はｘの増加に応じて１から０に順次変化する関数であって、同じｘ値のときの第１重み関数の第１重み係数と、第２重み関数の第２重み係数との合計が１であればよい。このような関数であれば第１重み関数及び第２重み関数は、上記の関数に限定されるものではなく、多項式やその他の関数を用いてもよい。その際、第１重み関数は、媒介変数ｓの関数であって、所定の階数（例えば３階）まで媒介変数ｓによる微分で連続な関数であってもよい。また、第２重み関数は、媒介変数ｓの関数であって、所定の階数（例えば３階）まで媒介変数ｓによる微分で連続な関数であってもよい。

図５は、第１実施形態におけるタスク空間でのロボットの経路を示す説明図であり、図６は、第１実施形態における関節空間でのロボットの経路を示す説明図である。

但し、図５中のＰ_２１，Ｐ_２２は、直線補間経路を生成するための教示点であり、図６中のＱ_１１，Ｑ_１２は、関節補間経路を生成するための教示点である。また、図６中のＱ_１１，Ｑ_１２のタスク空間上のＴＣＰの位置及び回転をそれぞれ図５中のＰ_１１，Ｐ_１２とする。ＴＣＰが図５中のＰ_２１，Ｐ_２２の位置及び回転にあるときのロボット２００（ロボットアーム２１０）のコンフィグレーションをそれぞれ図６中のＱ_２１，Ｑ_２２とする。

また、関節空間上の教示点Ｑ_１１，Ｑ_１２により関節補間をした、関節補間経路上のロボット２００のコンフィグレーションを図６中のｑ_１（ｓ_１）（０≦ｓ_１≦１）とする。タスク空間上の教示点Ｐ_２１，Ｐ_２２により直線補間をした、直線補間経路上にＴＣＰを位置付けるロボット２００のコンフィグレーションを図６中のｑ_２（ｓ_２）（０≦ｓ_２≦１）とする。但し、ｓ_１、ｓ_２は、それぞれ関節補間経路、直線補間経路の媒介変数とする。

また、ロボット２００のコンフィグレーションがｑ_１（ｓ_１）の時のＴＣＰの位置及び回転を表すベクトルを図５中のｐ_１（ｓ_１）とする。ロボット２００のコンフィグレーションがｑ_２（ｓ_２）の時のＴＣＰの位置及び回転を表すベクトルを図５中のｐ_２（ｓ_２）とする。

第１教示点である教示点Ｐ_１１（又は教示点Ｑ_１１）及び第２教示点である教示点Ｐ_２２（又は教示点Ｑ_２２）は、ロボット２００を通過させる点である。これら２つの教示点Ｐ_１１，Ｐ_２２（又は教示点Ｑ_１１，Ｑ_２２）は、ユーザが操作部４１０を操作して直接、又はユーザが操作部４１０を操作して記憶部１０６に記憶させて、ＣＰＵ１０１に指定（設定）する。

一方、第１補助点である教示点Ｐ_１２（又は教示点Ｑ_１２）及び第２補助点である教示点Ｐ_２１（又は教示点Ｑ_２１）は、ロボット２００が通過しない点であって、補助的な点である。第１実施形態では、教示点Ｐ_１２（又は教示点Ｑ_１２）は、予め設定される点であり、教示点Ｐ_２１（又は教示点Ｑ_２１）は、演算により求める点である。

今、関節補間により求めた第１補間経路である関節補間経路を、直線補間により求めた第２補間経路である直線補間経路に繋ぎ合せることを考える。そこで、関節補間経路（第１補間経路）上のｓ_１＝ｓ_１ｓを繋ぎ合せ区間の開始点とする。このｓ_１＝ｓ_１ｓのとき点は、関節補間経路上の途中点（第１途中点）である。また、関節補間経路上のｓ_１＝ｓ_１ｅを繋ぎ合せ区間の終了点とする。

また、直線補間経路（第２補間経路）上のｓ_２＝ｓ_２ｓを繋ぎ合せ区間の開始点とし、直線補間経路上のｓ_２＝ｓ_２ｅを繋ぎ合せ区間の終了点とする。ｓ_２＝ｓ_２ｅのとき点は、直線補間経路上の途中点（第２途中点）である。今簡単のために各定数を、ｓ_１ｅ＝１、ｓ_２ｓ＝０、ｓ_２ｅ−ｓ_２ｓ＝ｓ_１ｅ−ｓ_１ｓつまりｓ_２ｅ＝１−ｓ_１ｓとする。

ユーザは、各補間経路の繋ぎ合せ区間とそうでない区間の境目となる第１途中点及び第２途中点である教示点Ｐ_１ａ，Ｐ_２ａ（又は教示点Ｑ_１ａ，Ｑ_２ａ）のうち少なくとも１つの途中点のパラメータ値を設定（指定）する。教示点計算部３４は、その点を基に、経路補間に用いる教示点と繋ぎ合せ区間を逆算する。

第１実施形態の繋ぎ合せ区間は、関節補間と直線補間とが繋ぎ合さった繋ぎ合せ経路となるため、直線にはならず、ユーザの意図した動きにならない可能性がある。

そこで、ユーザが、直線補間経路のここからは直進性を確保したいという点を、直線補間経路上から指定（設定）する。その点から、教示点計算部３４により、経路補間に用いる適切な教示点と、繋ぎ合せ区間を算出する。

第１実施形態では、直進性を確保できる図５中の教示点Ｐ_２ａをユーザが指示（設定）し、そこから逆算して、教示点Ｐ_２１と、定数ｓ_２ｅを教示点計算部３４により逆算を行う。具体的には、直線補間が媒介変数ｓ＝ｓ_２ｅの時のＴＣＰの位置及び回転ｐ_１（ｓ）が教示点Ｐ_２ａと重なるように、教示点Ｐ_２１と、定数ｓ_２ｅを逆算する。

なお、定数ｓ_２ｅが求まれば、ｓ_２ｅ＝１−ｓ_１ｓであるので、定数ｓ_１ｓが求まる。

第１実施形態では、直線補間経路の教示点及び繋ぎ合せ区間の調整のみを説明したが、それに限定するものではない。関節補間部分も、ユーザが同様に繋ぎ合せ区間とそうでない区間の境目である教示点Ｐ_１ａ（教示点Ｑ_１ａ）を指定（設定）して、関節補間の特性が欲しい区間をユーザが指示できるようにしても良い。

ｓは、２つの補間経路を繋ぎ合せたロボット２００の経路の媒介変数である。この媒介変数ｓの値にロボット２００の経路上の点が対応している。つまり、媒介変数ｓの値を決めると、その値に対応するロボット２００の経路上の点が決まる。第１実施形態では、媒介変数ｓは、０から１＋ｓ_１までの範囲の値である。ｓ＝０からｓ＝１＋ｓ_１まで時刻に応じて媒介変数ｓの値が増加するように変化させることにより、ロボット２００のＴＣＰは、教示点Ｐ_１１から教示点Ｐ_２２まで経路上を移動することとなる。

０≦ｓ＜ｓ_１ｓの局所区間を関節補間区間、ｓ_１ｓ≦ｓ＜１の区間を繋ぎ合せ区間、１≦ｓ＜１＋ｓ_１ｓの局所区間を直線補間区間とする。このときの２つの補間経路を繋ぎ合せたときの関節空間上の経路ｑ（ｓ）は以下のようになる。

ここで、ｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））は第１重み関数、ｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）は第２重み関数である。これら関数のｓに数値を代入することで、第１重み係数及び第２重み係数が求まる。

媒介変数ｓに関してＣ^３級で滑らかさを保つｑ（ｓ）を用いれば、連続なｑ’（ｓ），ｑ’’（ｓ），ｑ’’’（ｓ）を求めることが可能となる。また、ｑ（ｓ）を用いれば、最適速度計算（ロボット２００の動作時間を最適にする最適化計算）をした後に、それに基づき、一定時間ごとの関節指令値（つまり軌道）を生成することが可能となる。

式（５）に則り、図６における、繋ぎ合せ区間での繋ぎ合せ経路上の点を算出する式は図４のグラフ上の値を用いて以下のように書ける。

ここで、ｑ_１１，ｑ_１２，ｑ_１３，ｑ_１４，ｑ_１５は、図６中、第１補間経路である関節補間経路ｑ_１（ｓ_１）上の教示点Ｑ_１ａから教示点Ｑ_１２までの複数の補間点（第１補間点）である。また、ｑ_２１，ｑ_２２，ｑ_２３，ｑ_２４，ｑ_２５は、図６中、第２補間経路である直線補間経路ｑ_２（ｓ_２）上の教示点Ｑ_２１から教示点Ｑ_２ａまでの複数の補間点（第２補間点）である。

図７は、第１実施形態に係る軌道生成方法の主要なステップを示すフローチャートである。以下、軌道生成装置１００のＣＰＵ１０１により行われるロボット２００の軌道生成方法の各工程について説明する。

まず、ＣＰＵ１０１は、ロボット２００が作業を行う作業空間での、ロボット２００のモデル及び制約条件（所定の物理的制約）を記憶部１０６（例えばＨＤＤ１０４）から読み込む（Ｓ１０１）。

ロボット２００のモデルは、ロボット２００を構成する部品の設計情報であり、各リンクの慣性モーメント、質量、位置、回転、軸数等である。制約条件は、ロボット２００の物理的制約を定義したもので、ロボット２００の関節トルクの制約、関節角速度の制約、関節角加速度の制約、関節角加加速度の制約、ＴＣＰ速度の制約及びＴＣＰ加速度の制約である。

次に、ＣＰＵ１０１は、ロボット２００の教示点及び補間方法を取得する（Ｓ１０２）。例えば、ロボット２００の教示点は、ＴＣＰの位置及び回転、又は関節角度により決定することができる。教示点と同時に、どういった補間方法で、その教示点まで移動するかの情報を読み込む。

第１実施形態でＣＰＵ１０１が取得する教示点は、第１教示点である教示点Ｐ_１１（又は教示点Ｑ_１１）、第１補助点である教示点Ｐ_１２（又は教示点Ｑ_１２）、第２教示点である教示点Ｐ_２２（又は教示点Ｑ_２２）である。

また、第１実施形態では、ユーザが関節補間と直線補間を指定するので、第１実施形態でＣＰＵ１０１が取得する補間方法は、第１補間方法である関節補間、第２補間方法である直線補間である。これら教示点及び補間方法は、記憶部１０６（例えばＨＤＤ１０４）に記憶されたロボットプログラムにて指定（設定）されている。

次に、ＣＰＵ１０１は、途中点である教示点Ｐ_１ａ，Ｐ_２ａのうち、少なくとも一方、第１実施形態では教示点Ｐ_２ａがユーザにより設定（指定）され、繋ぎ合せ区間を設定する（Ｓ１０３：設定工程）。第１実施形態では、関節補間のｓ_１ｓ≦ｓ_１＜１の区間と、直線補間の０＜ｓ_２＜ｓ_２ｅの区間を繋ぎ合せ区間とする。繋ぎ合せ区間は、任意であるが、ユーザが指定した教示点から直進性を確保する場合、教示点計算部３４により、それに合わせて直線補間の経路生成に用いる教示点と繋ぎ合せ区間を調整する。

具体的に説明すると、定数ｓ_１ｅは１に、定数ｓ_２ｓは０に設定される。つまり、これら定数ｓ_１ｅ，ｓ_２ｓは、予め記憶部１０６に記憶されており、ＣＰＵ１０１が記憶部１０６から読み出して設定する。

第２補助点から第２教示点に向かう第２補間経路上の第２途中点である教示点Ｐ_２ａ（パラメータ値）は、ユーザが設定（指定）する。これにより、ＣＰＵ１０１は、第２補助点である教示点Ｐ_２１及び定数ｓ_２ｅ，ｓ_１ｓを演算する。なお、第１実施形態では、第１教示点から第１補助点に向かう第１補間経路上の第１途中点である教示点Ｐ_１ａは、予め記憶部１０６に記憶されており、ＣＰＵ１０１が記憶部１０６から読み出して設定する。

以上、ステップＳ１０３により、繋ぎ合せ区間、つまり定数ｓ_１ｓ，ｓ_２ｅが設定（初期化）される。

次に、ＣＰＵ１０１は、第１補間経路である関節補間経路と第２補間経路である直線補間経路とを繋ぎ合せたロボット２００の経路ｑ（ｓ）を生成する経路生成処理を実行する（Ｓ１０４）。具体的には、図６において、関節補間経路における教示点Ｑ_１１と教示点Ｑ_１ａとの間の第１局所経路と、直線補間経路における教示点Ｑ_２ａと教示点Ｑ_２２との間の第２局所経路とを、繋ぎ合せ区間の繋ぎ合せ経路で繋ぎ合せた経路ｑ（ｓ）を生成する。ここで、第１局所経路は、ｑ_１（ｓ）（但し０≦ｓ＜ｓ_１ｓ）である。第２局所経路は、ｑ_２（ｓ−ｓ_１ｓ）（但し１≦ｓ＜１＋ｓ_１ｓ）である。繋ぎ合せ経路は、ｑ_１（ｓ）×ｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））＋ｑ_２（ｓ−ｓ_１ｓ）×ｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）（但しｓ_１ｓ≦ｓ＜１）である（式（５）参照）。

次に、ＣＰＵ１０１は、速度の最適化の対象となる２つの補間経路を繋いだ経路ｑ（ｓ）上の評価点を決定する（Ｓ１０５）。例えば、ｓ＝０からｓ＝１＋ｓ_１ｓまで、ある一定のΔｓずつ等分割したｓの列を評価点とする。

次に、ＣＰＵ１０１は、補間関数、つまり経路ｑ（ｓ）を使って、媒介変数ｓに関する数値微分により、評価点におけるｑ’（ｓ），ｑ’’（ｓ），ｑ’’’（ｓ）を求める（Ｓ１０６）。

次にＣＰＵ１０１は、ｑ’（ｓ），ｑ’’（ｓ），ｑ’’’（ｓ）を用い、関節トルク制約、関節角速度制約、関節角加速度制約、関節角加加速度制約、ＴＣＰ速度制約及びＴＣＰ加速度制約の制約を条件に最短時間で移動する最適速度を計算する（Ｓ１０７）。

次に、ＣＰＵ１０１は、式（１４）のように、最適速度計算により計算したｓ（・）（ｓ）から、ｓ（ｔ）を計算し、ｑ（ｓ）から一定時間Δｔ毎の関節指令値（角度指令値）を求める（Ｓ１０８）。但し、ｓ（０）＝０とする。

以上のステップＳ１０５〜Ｓ１０８が最適化計算工程である。つまり、ステップＳ１０５〜Ｓ１０８により、経路ｑ（ｓ）に対し、所定の物理的制約を守るようロボット２００の動作時間を最適化する最適化計算によって、時刻に対応する媒介変数ｓの値（つまりｓ（ｔ））を求めて、ロボット２００の軌道を求める。

媒介変数ｓに関してＣ^３級の滑らかさをもつｑ（ｓ）を用いれば、ｑ’（ｓ）、ｑ’’（ｓ）、ｑ’’’（ｓ）が連続となり、ロボット２００への関節指令値を生成する際の誤差を大幅に減らすことができる。

次に、ステップＳ１０４の経路生成処理について具体的に説明する。図８は、媒介変数ｓを入力してコンフィグレーションｑ（ｓ）を出力する、図７のステップＳ１０４における経路生成処理を示すフローチャートである。

まずＣＰＵ１０１は、媒介変数ｓの数値を取得（入力）する（Ｓ２０１）。ＣＰＵ１０１は、入力した媒介変数ｓの数値で場合分けを行う（Ｓ２０２）。つまり、ＣＰＵ２０１は、このステップＳ２０２で、媒介変数ｓの数値の判別を行う。０≦ｓ＜ｓ_１ｓの場合は、関節補間区間、ｓ_１ｓ≦ｓ＜１の場合は、関節補間と直線補間の繋ぎ合せ区間、１≦ｓ＜１＋ｓ_１ｓの場合は、直線補間区間である。

ＣＰＵ１０１は、ステップＳ２０２の判別で０≦ｓ＜ｓ_１ｓの場合（補間方法が関節補間の場合）、教示点Ｑ_１１，Ｑ_１２により関節補間を行い、媒介変数ｓの関数となる関節補間経路であるｑ_１（ｓ）を求める（Ｓ２０３）。つまりＣＰＵ１０１は、媒介変数ｓの関数であり、所定の階数（例えば３階）微分した微分値の変化が連続な、教示点Ｑ_１１から教示点Ｑ_１２に向かう関節補間経路ｑ_１（ｓ）を、関節補間により生成する（第１補間経路生成工程）。０≦ｓ＜ｓ_１ｓの区間では、第１補間経路中の教示点Ｑ_１１と教示点Ｑ_１ａとの間の第１局所経路ｑ_１（ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ_１（ｓ）となる。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ２０４）。

ＣＰＵ１０１は、ステップＳ２０２の判別でｓ_１ｓ≦ｓ＜１の場合（関節補間と直線補間の繋ぎ合せ区間の場合）、教示点Ｑ_１１，Ｑ_１２により関節補間を行い、ｑ_１（ｓ）を求める（Ｓ２０５：第１補間経路生成工程）。

ＣＰＵ１０１は、教示点Ｐ_２１，Ｐ_２２によりＴＣＰを直線補間し、ｐ_２（ｓ−ｓ_１ｓ）を求める（Ｓ２０６）。つまりＣＰＵ１０１は、媒介変数ｓの関数であり、教示点Ｐ_２１から教示点Ｐ_２２に向かう第２補間経路である直線補間経路ｐ_２（ｓ−ｓ_１ｓ）を、関節補間により生成する（第２補間経路生成工程）。このステップＳ２０６で生成する直線補間経路ｐ_２（ｓ−ｓ_１ｓ）は、所定の階数（例えば３階）まで媒介変数ｓにより微分した微分値の変化が連続な、媒介変数ｓの関数である。

ＣＰＵ１０１は、ｐ_２（ｓ−ｓ_１ｓ）に対し、キネマティクス計算部３１により逆キネマティクス計算を行い、コンフィグレーションｑ_２（ｓ−ｓ_１ｓ）を求める（Ｓ２０７）。つまり、ＣＰＵ１０１は、関節補間経路及び直線補間経路を、タスク空間及び関節空間のうちいずれかの空間（第１実施形態では、関節空間）における経路に統一する。

次に、ＣＰＵ１０１は、関節補間したコンフィグレーションｑ_１（ｓ）に重みづけするための第１重み関数ｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））から、第１重み係数を求める（Ｓ２０８）。また、ＣＰＵ１０１は、直線補間したコンフィグレーションｑ_２（ｓ）に重みづけするための第２重み関数ｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）から第２重み係数を求める（Ｓ２０９）。

ＣＰＵ１０１は、ｑ_１（ｓ）にｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））を掛け、ｑ_２（ｓ−ｓ_１ｓ）にｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）を掛けて和をとって、繋ぎ合せ経路ｑ（ｓ）（但しｓ_１ｓ≦ｓ＜１）を求める（Ｓ２１０）。

ｑ_１（ｓ）（但しｓ_１ｓ≦ｓ＜１）は、式（６）〜式（１０）に示す第１補間点ｑ_１１，ｑ_１２，ｑ_１３，ｑ_１４，ｑ_１５の集合である。ＣＰＵ１０１は、各補間点ｑ_１１，ｑ_１２，ｑ_１３，ｑ_１４，ｑ_１５に各第１重み係数ｒ_１１，ｒ_１２，ｒ_１３，ｒ_１４，ｒ_１５を掛け算する。

同様に、ｑ_２（ｓ−ｓ_１ｓ）（但しｓ_１ｓ≦ｓ＜１）は、式（６）〜式（１０）に示す第２補間点ｑ_２１，ｑ_２２，ｑ_２３，ｑ_２４，ｑ_２５の集合である。ＣＰＵ１０１は、各補間点ｑ_２１，ｑ_２２，ｑ_２３，ｑ_２４，ｑ_２５に各第２重み係数ｒ_２１，ｒ_２２，ｒ_２３，ｒ_２４，ｒ_２５を掛け算する。ここで、ｒ_１１＋ｒ_２１＝１，ｒ_１２＋ｒ_２２＝１，ｒ_１３＋ｒ_２３＝１，ｒ_１４＋ｒ_２４＝１，ｒ_１５＋ｒ_２５＝１である。

各第１重み係数ｒ_１１，ｒ_１２，ｒ_１３，ｒ_１４，ｒ_１５は、媒介変数ｓが大きくなるに従って１から０に順次遷移する。つまり、ｒ_１１（＝１）＞ｒ_１２＞ｒ_１３＞ｒ_１４＞ｒ_１５（＝０）である。

また、各第２重み係数ｒ_２１，ｒ_２２，ｒ_２３，ｒ_２４，ｒ_２５は、１から各第１重み係数ｒ_１１，ｒ_１２，ｒ_１３，ｒ_１４，ｒ_１５を引いた値となるよう媒介変数ｓが大きくなるに従って０から１に順次遷移する。つまり、ｒ_２１（＝０）＜ｒ_２２＜ｒ_２３＜ｒ_２４＜ｒ_２５（＝１）である。

即ち、ＣＰＵ１０１は、ステップＳ２１０では、補間点ｑ_１１，ｑ_１２，ｑ_１３，ｑ_１４，ｑ_１５に対して、教示点Ｑ_１１に近い側の補間点から順に、１から０に順次遷移する重み係数ｒ_１１，ｒ_１２，ｒ_１３，ｒ_１４，ｒ_１５をかける。また、ＣＰＵ１０１は、補間点ｑ_２１，ｑ_２２，ｑ_２３，ｑ_２４，ｑ_２５に対して、教示点Ｑ_２１に近い側の補間点から順に、０から１に順次遷移する重み係数ｒ_２１，ｒ_２２，ｒ_２３，ｒ_２４，ｒ_２５をかける。ＣＰＵ１０１は、補間点ｑ_１ｎ（ｎ＝１〜５、以下同じ）に重み係数ｒ_１ｎをかけて求めた点と、補間点ｑ_２ｎに重み係数ｒ_２ｎをかけて求めた点とを足し合わせて、繋ぎ合せ経路ｑ_１，ｑ_２，ｑ_３，ｑ_４，ｑ_５を生成する（図７、式（６）〜式（１０））。

以上のステップＳ２０６〜Ｓ２１０により、ＣＰＵ１０１は、媒介変数ｓの関数であって、所定の階数（例えば３階）まで媒介変数ｓにより微分した微分値の変化が連続な、教示点Ｑ_１ａから教示点Ｑ_２ａに向かう繋ぎ合せ経路を生成する（繋ぎ合せ工程）。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ２１１）。

なお、重み係数ｒ_１ｎ，ｒ_２ｎは、ステップＳ２０８，Ｓ２０９で重み関数から求める場合について説明したが、これに限定するものではなく、予め重み係数ｒ_１ｎ，ｒ_２ｎを計算しておいて記憶部１０６に記憶させておいてもよい。その際、ＣＰＵ１０１は、ステップＳ２０８，Ｓ２０９では、演算を行わずに記憶部１０６から重み係数ｒ_１ｎ，ｒ_２ｎのデータを読み出すだけでよい。

次に、ＣＰＵ１０１は、ステップＳ２０２の判別で１≦ｓ＜１＋ｓ_１ｓの場合（補間方法が直線補間の場合）、教示点Ｐ_２１，Ｐ_２２によりＴＣＰを直線補間し、ｐ_２（ｓ−ｓ_１ｓ）を求める（Ｓ２１２：第２補間経路生成工程）。

ＣＰＵ１０１は、ｐ_２（ｓ−ｓ_１ｓ）に対し、逆キネマティクス計算を行い、関節空間上のコンフィグレーションであるｑ_２（ｓ−ｓ_１ｓ）を求める（Ｓ２１３）。１≦ｓ＜１＋ｓ_１ｓの区間では、第２補間経路中の教示点Ｑ_２ａと教示点Ｑ_２２との間の第２局所経路ｑ_２（ｓ−ｓ_１ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ_２（ｓ−ｓ_１ｓ）である。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ２１４）。

以上のステップＳ２０１〜Ｓ２１４は、媒介変数ｓの数値を、０から１＋ｓ_１ｓまで変化させて繰り返し行う。これらステップＳ２０１〜Ｓ２１４により、式（５）に示す、ロボット２００の関節空間においてＣ^３級の経路ｑ（ｓ）が求まる。

以上、第１実施形態によれば、ロボット２００が関節補間と直線補間という異なる２つの補間経路を繋げ合せた際に、媒介変数ｓに関してＣ^３級で滑らかなロボット２００の関節空間における経路ｑ（ｓ）を生成している。これにより、２つの補間経路を繋げ合せた経路ｑ（ｓ）を辿るロボット２００が停止することのない、物理的制約を守った軌道を生成することができる。

特に、ステップＳ２１０では、ＣＰＵ１０１が、ｑ_１ｎ×ｒ_１ｎ＋ｑ_２ｎ×ｒ_２ｎ（ｎ＝１〜５）を計算して、繋ぎ合せ経路ｑ_ｎを生成している。これにより、繋ぎ合せ経路は、媒介変数ｓに関してＣ^３級で滑らかな経路として生成される。なお、第１実施形態では、説明の都合上、ｎ＝１〜５としたが、これに限定するものではない。ｎの数は、多ければ多いほど精度が向上するため、繋ぎ合せ経路として、多数の点を生成するのがよい。第１実施形態では、第１及び第２重み関数に基づいて第１及び第２重み係数を計算しているので、予め大量の重み係数のデータを記憶部１０６に記憶させておく必要がない。よって、記憶部１０６のメモリ領域が圧迫されることがなく、メモリ領域を有効活用できる。

また、第１実施形態によれば、ユーザが教示点Ｐ_２ａ（Ｑ_２ａ）を設定（指定）しているため、経路の繋ぎ合せ区間を意識せずに、直進性が必要な区間をユーザが指定でき、使用性が上がるという効果がある。

［第２実施形態］
本発明の第２実施形態に係る軌道生成方法について説明する。なお、軌道生成装置、即ちロボット装置の構成は、上記第１実施形態と同様であり、説明を省略する。第２実施形態では、上記第１実施形態と異なる補間方法の組み合わせ、即ち円弧補間と直線補間の２種類の補間方法が指定されている。

図９は、第２実施形態におけるタスク空間でのロボットアームの経路を示す説明図であり、図１０は、第２実施形態における関節空間でのロボットアームの経路を示す説明図である。

第２実施形態では、第１補間方法を円弧補間とし、第２補間方法を、上記第１実施形態と同様、直線補間としたものである。円弧補間をした時のコンフィグレーションをｑ_１（ｓ）とすると、上記第１実施形態と同様、式（５）により、経路の滑らかさを保った繋ぎ合せが可能となる。式（５）に則り、図１０における、繋ぎ合せ区間での繋ぎ合せ経路上の点ｑ_１〜ｑ_５を算出する式は、上記第１実施形態と同様、式（６）〜式（１０）になる。

上記第１実施形態と違い、円弧補間した補間経路は、タスク空間上のＴＣＰの経路となるため、関節空間上で繋ぎ合せるためには、逆キネマティクスを行い、コンフィグレーションを求めて円弧補間経路ｑ_１（ｓ）を求める。

以上を考慮にいれ、媒介変数ｓからコンフィグレーションｑ（ｓ）を求める流れを説明する。図１１は、媒介変数ｓを入力してコンフィグレーションｑ（ｓ）を出力する経路生成処理のフローチャートである。なお、第２実施形態では、第１補間方法がタスク空間で補間経路を生成する方法であるため、求めた第１補間経路である円弧補間経路を逆キネマティクス計算により関節空間における経路に変換する点で、上記第１実施形態と異なる。それ以外は、上記第１実施形態と同様であるため、以下、経路生成処理に関する各工程を簡略して説明する。

まずＣＰＵ１０１は、媒介変数ｓを取得（入力）する（Ｓ３０１）。ＣＰＵ１０１は、入力された媒介変数ｓの区間で場合分けを行う（Ｓ３０２）。０≦ｓ＜ｓ_１ｓの場合は、円弧補間区間、ｓ_１ｓ≦ｓ＜１の場合は、円弧補間と直線補間の繋ぎ合せ区間、１≦ｓ＜１＋ｓ_１ｓの場合は、直線補間区間である。

ＣＰＵ１０１は、ステップＳ３０２の判別で０≦ｓ＜ｓ_１ｓの場合、教示点Ｐ_１１，Ｐ_１２によりＴＣＰを円弧補間し、ｐ_１（ｓ）を求める（Ｓ３０３）。ＣＰＵ１０１は、ｐ_１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ_１（ｓ）を求める（Ｓ３０４）。０≦ｓ＜ｓ_１ｓの区間では、第１局所経路ｑ_１（ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ_１（ｓ）となる。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ３０５）。

次に、ＣＰＵ１０１は、ステップＳ３０２の判別でｓ_１ｓ≦ｓ＜１であった場合、まず教示点Ｐ_１１，Ｐ_１２によりＴＣＰを円弧補間し、ｐ_１（ｓ）を求める（Ｓ３０６）。ＣＰＵ１０１は、ｐ_１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ_１（ｓ）を求める（Ｓ３０７）。

ＣＰＵ１０１は、教示点Ｐ_２１，Ｐ_２２によりＴＣＰを直線補間し、ｐ_２（ｓ−ｓ_１ｓ）を求める（Ｓ３０８）。ＣＰＵ１０１は、ｐ_２（ｓ−ｓ_１ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ_２（ｓ−ｓ_１ｓ）を求める（Ｓ３０９）。

ＣＰＵ１０１は、円弧補間したコンフィグレーションｑ_１（ｓ）に重みづけするための重み関数ｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））を求める（Ｓ３１０）。ＣＰＵ１０１は、直線補間したコンフィグレーションｑ_２（ｓ）に重みづけするための重み関数ｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）を求める（Ｓ３１１）。

ＣＰＵ１０１は、ｑ_１（ｓ）にｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））を掛け、ｑ_２（ｓ−ｓ_１ｓ）にｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）を掛けて和をとって、ｑ（ｓ）を求める（Ｓ３１２）。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ３１３）。

ＣＰＵ１０１は、ステップＳ３０２の判別で１≦ｓ＜１＋ｓ_１ｓであった場合、教示点Ｐ_２１，Ｐ_２２によりＴＣＰを直線補間し、ｐ_２（ｓ−ｓ_１ｓ）を求める（Ｓ３１４）。ＣＰＵ１０１は、ｐ_２（ｓ−ｓ_１ｓ）に対し、逆キネマティクス計算を行い、関節空間上のコンフィグレーションであるｑ_２（ｓ−ｓ_１ｓ）を求める（Ｓ３１５）。１≦ｓ＜１＋ｓ_１ｓの区間では、第２局所経路ｑ_２（ｓ−ｓ_１ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ_２（ｓ−ｓ_１ｓ）である。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ３１６）。

以上、円弧補間と直線補間とをＣ^３級を保って繋ぎ合せたコンフィグレーションｑ（ｓ）が求められるので、上記第１実施形態と同様、図７の流れで、最適速度を計算することができる。

また、図７のステップＳ１０３において、ユーザが円弧補間経路の円弧性と、直線補間経路の直進性を確保したい点Ｐ_１ａ，Ｐ_２ａ（又は点Ｑ_１ａ，Ｑ_２ａ）をそれぞれ指定（設定）する。教示点計算部３４として機能するＣＰＵ１０１は、その点Ｐ_１ａ，Ｐ_２ａ（又は点Ｑ_１ａ，Ｑ_２ａ）を基に、円弧補間及び直線補間の教示点及び繋ぎ合せ区間を逆算する。

繋ぎ合せ区間は、円弧補間と直線補間が繋ぎ合わさった経路となるため、純粋な円弧もしくは直線にはならず、ユーザの意図した動きにならない可能性がある。そこで、ユーザが、ここまでは円弧性を確保し、ここからは、直進性を確保したいという点を指定し、それから、教示点計算部３４により適切な教示点Ｐ_１２，Ｐ_２１（又は教示点Ｑ_１２，Ｑ_２１）を演算する。

つまり、第２実施形態では、円弧性を確保する図９上の点Ｐ_１ａ、直進性を確保できる図９上の点Ｐ_２ａをユーザが指示する。ＣＰＵ１０１は、そこから逆算して、それぞれ適当なＰ_１２，Ｐ_２１，ｓ_１ｓ，ｓ_２ｅを求める。具体的には、円弧補間経路が、ｓ_１＝ｓ_１ｓのときのＴＣＰの位置及び回転ｐ_１（ｓ）が、Ｐ_１ａと重なり、直線補間経路がｓ_２＝ｓ_２ｅの時のＴＣＰの位置及び回転ｐ_２（ｓ_２）が、Ｐ_２１と重なるようにＰ_１２，Ｐ_２１，ｓ_１ｓ，ｓ_２ｅを逆算する。

以上説明したように、第２実施形態によれば、上記第１実施形態の効果に加えて、教示点に付随する補間方法が、円弧補間から直線補間に切り替わる場合にも、経路の切り替え時に停止せずに、ロボット２００の各種物理的制約を守った最適な軌道を生成できる。また、第２実施形態によれば、経路の繋ぎ合せ区間を意識せずに、円弧性及び直進性が必要な区間をユーザが指定できるため、使用性が上がるという効果がある。

［第３実施形態］
本発明の第３実施形態に係る軌道生成方法について説明する。なお、軌道生成装置、即ちロボット装置の構成は、上記第１，第２実施形態と同様であり、説明を省略する。第３実施形態では、上記第１，第２実施形態と異なる補間方法の組み合わせ、即ち円弧補間と関節補間の２種類の補間方法が指定されている。

図１２は、第３実施形態におけるタスク空間でのロボットアームの経路を示す説明図であり、図１３は、第３実施形態における関節空間でのロボットアームの経路を示す説明図である。

第３実施形態では、第１補間方法を円弧補間、第２補間方法を関節補間としたものである。円弧補間をした時のコンフィグレーション（補間経路）をｑ_１（ｓ）とし、関節補間した時のコンフィグレーション（補間経路）をｑ_２（ｓ）すると、上記第１実施形態と同様、式（５）により、経路の滑らかさを保った繋ぎ合せが可能となる。式（５）に則り、図１３における、繋ぎ合せ区間での経路上の点ｑ_１〜ｑ_５を算出する式は、上記第１実施形態と同様、式（６）〜式（１０）になる。

上記第２実施形態と同様、円弧補間した経路は、タスク空間上のＴＣＰの経路となるため、関節空間上で繋ぎ合せるためには、逆キネマティクスを行い、コンフィグレーションを求めて円弧補間経路ｑ_１（ｓ）を求める。以上を考慮にいれ、媒介変数ｓからコンフィグレーションｑ（ｓ）を求める流れを説明する。

図１４は、媒介変数ｓを入力してコンフィグレーションｑ（ｓ）を出力する経路生成処理のフローチャートである。なお、第３実施形態では、第２補間方法が関節空間で補間経路を生成する方法であるため、第２補間経路である関節補間経路を関節空間で求める点で、上記第２実施形態と異なる。それ以外は、上記第２実施形態と同様であるため、以下、経路生成処理に関する各工程を簡略して説明する。

まずＣＰＵ１０１は、媒介変数ｓを取得（入力）する（Ｓ４０１）。ＣＰＵ１０１は、入力された媒介変数ｓの区間で場合分けを行う（Ｓ４０２）。０≦ｓ＜ｓ_１ｓの場合は、円弧補間区間、ｓ_１ｓ≦ｓ＜１の場合は、円弧補間と関節補間の繋ぎ合せ区間、１≦ｓ＜１＋ｓ_１ｓの場合は、関節補間区間である。

ＣＰＵ１０１は、ステップＳ４０２の判別で０≦ｓ＜ｓ_１ｓの場合、教示点Ｐ_１１，Ｐ_１２によりＴＣＰを円弧補間し、ｐ_１（ｓ）を求める（Ｓ４０３）。ＣＰＵ１０１は、ｐ_１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ_１（ｓ）を求める（Ｓ４０４）。０≦ｓ＜ｓ_１ｓの区間では、第１局所経路ｑ_１（ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ_１（ｓ）となる。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ４０５）。

次に、ＣＰＵ１０１は、ステップＳ４０２の判別でｓ_１ｓ≦ｓ＜１の場合、まず教示点Ｐ_１１，Ｐ_１２によりＴＣＰを円弧補間し、ｐ_１（ｓ）を求める（Ｓ４０６）。ＣＰＵ１０１は、ｐ_１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ_１（ｓ）を求める（Ｓ４０７）。

ＣＰＵ１０１は、教示点Ｑ_２１，Ｑ_２２により関節補間し、ｑ_２（ｓ−ｓ_１ｓ）を求める（Ｓ４０８）。

ＣＰＵ１０１は、円弧補間したコンフィグレーションｑ_１（ｓ）に重みづけするための重み関数ｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））を求める（Ｓ４０９）。ＣＰＵ１０１は、直線補間したコンフィグレーションｑ_２（ｓ）に重みづけするための重み関数ｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）を求める（Ｓ４１０）。

ＣＰＵ１０１は、ｑ_１（ｓ）にｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））を掛け、ｑ_２（ｓ−ｓ_１ｓ）にｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）を掛けて和をとって、ｑ（ｓ）を求める（Ｓ４１１）。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ４１２）。

ＣＰＵ１０１は、ステップＳ４０２の判別で１≦ｓ＜１＋ｓ_１ｓの場合、教示点Ｑ_２１，Ｑ_２２により関節補間し、ｑ_２（ｓ−ｓ_１ｓ）を求める（Ｓ４１３）。１≦ｓ＜１＋ｓ_１ｓの区間では、第２局所経路ｑ_２（ｓ−ｓ_１ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ_２（ｓ−ｓ_１ｓ）である。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ４１４）。

以上、円弧補間と関節補間とをＣ^３級を保って繋ぎ合せたコンフィグレーションｑ（ｓ）を求められるので、上記第１実施形態と同様、図７の流れで、最適速度を計算することができる。

また、図７のステップＳ１０３において、ユーザが円弧補間経路の円弧性を確保したい点Ｐ_１ａ（又は点Ｑ_１ａ）をそれぞれ指定（設定）する。教示点計算部３４として機能するＣＰＵ１０１は、その点Ｐ_１ａ（又は点Ｑ_１ａ）を基に、円弧補間及び関節補間の教示点及び繋ぎ合せ区間を逆算する。

繋ぎ合せ区間は、円弧補間と関節補間が繋ぎ合わさった経路となるため、純粋な円弧にはならず、ユーザの意図した動きにならない可能性がある。そこで、ユーザが、ここまでは円弧性を確保したいという点を指定する。そこから教示点計算部３４により適切な教示点Ｐ_１２（又は教示点Ｑ_１２）を演算する。

つまり、第３実施形態では、円弧性を確保する図１２上の点Ｐ_１ａをユーザが指示する。ＣＰＵ１０１は、そこから逆算して、適当なＰ_１２とｓ_１ｓを求める。具体的には、円弧補間経路が、ｓ_１＝ｓ_１ｓのときのＴＣＰの位置及び回転ｐ_１（ｓ_１）が、Ｐ_１ａと重なるようにＰ_１２、ｓ_１ｓを逆算する。

以上説明したように、第３実施形態によれば、上記第１実施形態の効果に加えて、教示点に付随する補間方法が、円弧補間から関節補間に切り替わる場合にも、経路の切り替え時に停止せずに、ロボット２００の各種物理的制約を守った最適な軌道を生成できる。また、第３実施形態によれば、経路の繋ぎ合せ区間を意識せずに、円弧性が必要な区間をユーザが指定できるため、使用性が上がるという効果がある。

［第４実施形態］
本発明の第４実施形態に係る軌道生成方法について説明する。なお、軌道生成装置、即ちロボット装置の構成は、上記第１，第２及び第３実施形態と同様であり、説明を省略する。第４実施形態では、上記第１，第２，第３実施形態と異なる補間方法の組み合わせ、即ち直線補間と直線補間の補間方法が指定されている。

図１５は、第４実施形態におけるタスク空間でのロボットアームの経路を示す説明図であり、図１６は、第４実施形態における関節空間でのロボットアームの経路を示す説明図である。

第４実施形態では、第１補間方法を第１直線補間、第２補間方法を第２直線補間としたものである。第１直線補間をした時のコンフィグレーションをｑ_１（ｓ）とし、第２直線補間をした時のコンフィグレーションをｑ_２（ｓ）すると、上記第１実施形態と同様、式（５）により、経路の滑らかさを保った繋ぎ合せが可能となる。式（５）に則り、図１６における、繋ぎ合せ区間での経路上の点ｑ_１〜ｑ_５を算出する式は、上記第１実施形態と同様、式（６）〜式（１０）になる。

上記第１実施形態と同様、直線補間した経路は、タスク空間上のＴＣＰの経路となるため、関節空間上で繋ぎ合せるためには、逆キネマティクスを行い、コンフィグレーションを求めて直線補間経路ｑ_１（ｓ），ｑ_２（ｓ−ｓ_１ｓ）を求める。

以上を考慮にいれ、媒介変数ｓからコンフィグレーションｑ（ｓ）を求める流れを説明する。図１７は、媒介変数ｓを入力してコンフィグレーションｑ（ｓ）を出力する経路生成処理のフローチャートである。なお、第４実施形態では、第１補間方法がタスク空間で補間経路を生成する方法であるため、求めた第１補間経路である直線補間経路を逆キネマティクス計算により関節空間における経路に変換する点で、上記第１実施形態と異なる。それ以外は、上記第１実施形態と同様であるため、以下、経路生成処理に関する各工程を簡略して説明する。

まずＣＰＵ１０１は、媒介変数ｓを取得（入力）する（Ｓ５０１）。ＣＰＵ１０１は、入力された媒介変数ｓの区間で場合分けを行う（Ｓ５０２）。０≦ｓ＜ｓ_１ｓの場合は、第１直線補間区間、ｓ_１ｓ≦ｓ＜１の場合は、第１直線補間と第２直線補間の繋ぎ合せ区間、１≦ｓ＜１＋ｓ_１ｓの場合は、第２直線補間区間である。

ＣＰＵ１０１は、ステップＳ５０２の判別で０≦ｓ＜ｓ_１ｓの場合、教示点Ｐ_１１，Ｐ_１２によりＴＣＰを直線補間し、ｐ_１（ｓ）を求める（Ｓ５０３）。ＣＰＵ１０１は、ｐ_１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ_１（ｓ）を求める（Ｓ５０４）。０≦ｓ＜ｓ_１ｓの区間では、第１局所経路ｑ_１（ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ_１（ｓ）となる。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ５０５）。

次に、ＣＰＵ１０１は、ステップＳ５０２の判別でｓ_１ｓ≦ｓ＜１の場合、まず教示点Ｐ_１１，Ｐ_１２によりＴＣＰを直線補間し、ｐ_１（ｓ）を求める（Ｓ５０６）。ＣＰＵ１０１は、ｐ_１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ_１（ｓ）を求める（Ｓ５０７）。

ＣＰＵ１０１は、教示点Ｐ_２１，Ｐ_２２によりＴＣＰを直線補間し、ｐ_２（ｓ−ｓ_１ｓ）を求める（Ｓ５０８）。ＣＰＵ１０１は、ｐ_２（ｓ−ｓ_１ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ_２（ｓ−ｓ_１ｓ）を求める（Ｓ５０９）。

ＣＰＵ１０１は、直線補間したコンフィグレーションｑ_１（ｓ）に重みづけするための重み関数ｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））を求める（Ｓ５１０）。ＣＰＵ１０１は、直線補間したコンフィグレーションｑ_２（ｓ）に重みづけするための重み関数ｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）を求める（Ｓ５１１）。

ＣＰＵ１０１は、ｑ_１（ｓ）にｆ_ｓｉｇ１（（ｓ−ｓ_１ｓ）／（１−ｓ_１ｓ））を掛け、ｑ_２（ｓ−ｓ_１ｓ）にｆ_ｓｉｇ２（（ｓ−ｓ_１ｓ）／ｓ_２ｅ）を掛けて和をとって、ｑ（ｓ）を求める（Ｓ５１２）。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ５１３）。

ＣＰＵ１０１は、ステップＳ５０２の判別で１≦ｓ＜１＋ｓ_１ｓの場合、教示点Ｐ_２１，Ｐ_２２によりＴＣＰを直線補間し、ｐ_２（ｓ−ｓ_１ｓ）を求める（Ｓ５１４）。ＣＰＵ１０１は、ｐ_２（ｓ−ｓ_１ｓ）に対し、逆キネマティクス計算を行い、関節空間上のコンフィグレーションであるｑ_２（ｓ−ｓ_１ｓ）を求める（Ｓ５１５）。１≦ｓ＜１＋ｓ_１ｓの区間では、第２局所経路ｑ_２（ｓ−ｓ_１ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ_２（ｓ−ｓ_１ｓ）である。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ５１６）。

第４実施形態によれば、上記第１実施形態の効果に加えて、教示点に付随する補間方法が、第１直線補間から第２直線補間に切り替わる場合にも、経路の切り替え時に停止せずに、ロボット２００の各種物理的制約を守った最適な軌道を生成できる。これは、背景技術で述べた直線補間から直線補間に移る手法と比較して、各種物理的制約を守った最適な軌道を生成できるという点で有利である。また、第４実施形態によれば、経路の繋ぎ合せ区間を意識せずに、直進性が必要な区間をユーザが指定できるため、使用性が上がるという効果がある。

なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。

なお、上記実施形態では、関節空間での繋ぎ合せで説明したが、それに限定されるものではなく、タスク空間で同様の繋ぎ合せを行ってもよい。例えば、ＴＣＰの位置及び回転に対し、同様の重み関数を用いて経路の繋ぎ合せを行ってもよい。この場合、最終的には、タスク空間上の経路を、関節空間上の経路に変換して、軌道を生成すればよい。

また、上記実施形態においては、軌道生成装置１００が１つのコンピュータにより構成される場合について説明したが、複数のコンピュータにより構成される場合についても本発明は適用可能である。この場合、複数のＣＰＵで本発明の演算部が構成されていてもよい。また、軌道生成装置１００とロボットコントローラ３００とが別のコンピュータで構成される場合について説明したが、１つのコンピュータで構成されていてもよい。この場合、該コンピュータのＣＰＵが、ロボットの経路を生成して、生成した経路に従ってロボットの動作を制御することとなる。

上記実施形態の各処理動作は具体的にはＣＰＵ１０１により実行されるものである。従って上述した機能を実現するプログラムを記録した記録媒体を軌道生成装置１００に供給し、軌道生成装置１００を構成するコンピュータ（ＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。

また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ１０４であり、ＨＤＤ１０４にプログラム１４０が格納される場合について説明したが、これに限定するものではない。プログラム１４０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図３に示すＲＯＭ１０２や外部記憶装置６００、記録ディスク１４１等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることができる。また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。

また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。

さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。

また、上記実施形態では、コンピュータが記録媒体や記憶装置に記録されたプログラムを実行することにより、処理を行う場合について説明したが、これに限定するものではない。プログラムに基づいて動作する演算部の一部又は全部の機能をＡＳＩＣやＦＰＧＡ等の専用ＬＳＩで構成してもよい。なお、ＡＳＩＣはApplication Specific Integrated Circuit、ＦＰＧＡはField-Programmable Gate Arrayの頭字語である。

１００…軌道生成装置、１０１…ＣＰＵ（演算部）、１４０…プログラム、２００…ロボット、５００…ロボット装置、Ｐ_１１，Ｑ_１１…第１教示点、Ｐ_１２，Ｑ_１２…第１補助点、Ｐ_１ａ，Ｑ_１ａ…第１途中点、Ｐ_２１，Ｑ_２１…第２補助点、Ｐ_２２，Ｑ_２２…第２教示点、Ｐ_２ａ，Ｑ_２ａ…第２途中点、ｓ…媒介変数

Claims (11)

1. 演算部が、多関節のロボットの軌道を生成する軌道生成方法であって、
   前記演算部が、媒介変数の関数であって、所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第１教示点から第１補助点に向かう第１補間経路を、第１補間方法により生成する第１補間経路生成工程と、
   前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第２補助点から第２教示点に向かう第２補間経路を、第２補間方法により生成する第２補間経路生成工程と、
   前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、前記第１補間経路上の第１途中点から前記第２補間経路上の第２途中点に向かう繋ぎ合せ経路を生成する繋ぎ合せ工程と、
   前記演算部が、前記第１補間経路における前記第１教示点と前記第１途中点との間の第１局所経路と、前記第２補間経路における前記第２途中点と前記第２教示点との間の第２局所経路とを、前記繋ぎ合せ経路で繋ぎ合せた経路に対し、所定の物理的制約を守るよう前記ロボットの動作時間を最適化する最適化計算によって、前記ロボットの前記軌道を求める最適化計算工程と、を備え、
   前記繋ぎ合せ工程では、前記演算部が、前記第１途中点から前記第１補助点までの各第１補間点に対して、前記第１教示点に近い側の第１補間点から順に、１から０に順次遷移する第１重み係数をかけ、前記第２補助点から前記第２途中点までの各第２補間点に対して、前記第２補助点に近い側の第２補間点から順に、１から前記第１重み係数を引いた値となるよう０から１に順次遷移する第２重み係数をかけ、前記第１補間点に前記第１重み係数をかけて求めた点と、前記第２補間点に前記第２重み係数をかけて求めた点とを足し合わせて、前記繋ぎ合せ経路を生成し、
   前記繋ぎ合せ工程では、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数による微分値の変化が連続な、第１重み関数を用いて、前記第１重み係数を求め、前記所定の階数まで前記媒介変数による微分値の変化が連続な、第２重み関数を用いて、前記第２重み係数を求めることを特徴とする軌道生成方法。
2. 前記第１補間方法及び前記第２補間方法がそれぞれタスク空間で補間経路を生成する方法及び関節空間で補間経路を生成する方法のうちいずれかであり、
   前記繋ぎ合せ工程では、前記演算部が、前記第１補間経路及び前記第２補間経路を、前記タスク空間及び前記関節空間のうちいずれかの空間における経路に統一して、前記統一した空間において繋ぎ合せ経路の生成を行うことを特徴とする請求項１記載の軌道生成方法。
3. 演算部が、多関節のロボットの軌道を生成する軌道生成方法であって、
   前記演算部が、媒介変数の関数であって、所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第１教示点から第１補助点に向かう第１補間経路を、第１補間方法により生成する第１補間経路生成工程と、
   前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第２補助点から第２教示点に向かう第２補間経路を、第２補間方法により生成する第２補間経路生成工程と、
   前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、前記第１補間経路上の第１途中点から前記第２補間経路上の第２途中点に向かう繋ぎ合せ経路を生成する繋ぎ合せ工程と、
   前記演算部が、前記第１補間経路における前記第１教示点と前記第１途中点との間の第１局所経路と、前記第２補間経路における前記第２途中点と前記第２教示点との間の第２局所経路とを、前記繋ぎ合せ経路で繋ぎ合せた経路に対し、所定の物理的制約を守るよう前記ロボットの動作時間を最適化する最適化計算によって、前記ロボットの前記軌道を求める最適化計算工程と、を備え、
   前記第１補間方法及び前記第２補間方法がそれぞれタスク空間で補間経路を生成する方法及び関節空間で補間経路を生成する方法のうちいずれかであり、
   前記繋ぎ合せ工程では、前記演算部が、前記第１補間経路及び前記第２補間経路を、前記タスク空間及び前記関節空間のうちいずれかの空間における経路に統一して、前記統一した空間において繋ぎ合せ経路の生成を行うことを特徴とする軌道生成方法。
4. 前記所定の階数は、３であることを特徴とする請求項１乃至３のいずれか１項に記載の軌道生成方法。
5. 前記繋ぎ合せ工程では、前記演算部が、シグモイド関数を用いて、前記第１重み係数を求め、シグモイド関数を用いて、前記第２重み係数を求めることを特徴とする請求項１又は２に記載の軌道生成方法。
6. 前記演算部が、前記第１途中点及び前記第２途中点のうち、少なくとも一方の途中点がユーザにより設定される設定工程を更に備えたことを特徴とする請求項１乃至５のいずれか１項に記載の軌道生成方法。
7. 前記所定の物理的制約は、前記ロボットの関節角速度の制約及び前記ロボットの先端の速度の制約のうち少なくとも１つを含むほか、前記ロボットの関節トルクの制約、前記ロボットの関節角加速度の制約、前記ロボットの関節角加加速度の制約及び前記ロボットの先端の加速度の制約のうち少なくとも１つを含むことを特徴とする請求項１乃至６のいずれか１項に記載の軌道生成方法。
8. 請求項１乃至７のいずれか１項に記載の軌道生成方法の各工程を実行する前記演算部を備えた軌道生成装置。
9. 前記多関節のロボットと、請求項８に記載の軌道生成装置と、を備え、前記軌道生成装置により生成した軌道に基づき、前記ロボットの動作を制御することを特徴とするロボット装置。
10. コンピュータに、請求項１乃至７のいずれか１項に記載の軌道生成方法の各工程を実行させるためのプログラム。
11. 請求項１０に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images