(57)【特許請求の範囲】
【請求項１】
演算部が、多関節のロボットの軌道を生成する軌道生成方法であって、
前記演算部が、媒介変数の関数であって、所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第１教示点から第１補助点に向かう第１補間経路を、第１補間方法により生成する第１補間経路生成工程と、
前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第２補助点から第２教示点に向かう第２補間経路を、第２補間方法により生成する第２補間経路生成工程と、
前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、前記第１補間経路上の第１途中点から前記第２補間経路上の第２途中点に向かう繋ぎ合せ経路を生成する繋ぎ合せ工程と、
前記演算部が、前記第１補間経路における前記第１教示点と前記第１途中点との間の第１局所経路と、前記第２補間経路における前記第２途中点と前記第２教示点との間の第２局所経路とを、前記繋ぎ合せ経路で繋ぎ合せた経路に対し、所定の物理的制約を守るよう前記ロボットの動作時間を最適化する最適化計算によって、前記ロボットの前記軌道を求める最適化計算工程と、を備え、
前記繋ぎ合せ工程では、前記演算部が、前記第１途中点から前記第１補助点までの各第１補間点に対して、前記第１教示点に近い側の第１補間点から順に、１から０に順次遷移する第１重み係数をかけ、前記第２補助点から前記第２途中点までの各第２補間点に対して、前記第２補助点に近い側の第２補間点から順に、１から前記第１重み係数を引いた値となるよう０から１に順次遷移する第２重み係数をかけ、前記第１補間点に前記第１重み係数をかけて求めた点と、前記第２補間点に前記第２重み係数をかけて求めた点とを足し合わせて、前記繋ぎ合せ経路を生成し、
前記繋ぎ合せ工程では、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数による微分値の変化が連続な、第１重み関数を用いて、前記第１重み係数を求め、前記所定の階数まで前記媒介変数による微分値の変化が連続な、第２重み関数を用いて、前記第２重み係数を求めることを特徴とする軌道生成方法。
【請求項２】
前記第１補間方法及び前記第２補間方法がそれぞれタスク空間で補間経路を生成する方法及び関節空間で補間経路を生成する方法のうちいずれかであり、
前記繋ぎ合せ工程では、前記演算部が、前記第１補間経路及び前記第２補間経路を、前記タスク空間及び前記関節空間のうちいずれかの空間における経路に統一して、前記統一した空間において繋ぎ合せ経路の生成を行うことを特徴とする請求項１記載の軌道生成方法。
【請求項３】
演算部が、多関節のロボットの軌道を生成する軌道生成方法であって、
前記演算部が、媒介変数の関数であって、所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第１教示点から第１補助点に向かう第１補間経路を、第１補間方法により生成する第１補間経路生成工程と、
前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第２補助点から第２教示点に向かう第２補間経路を、第２補間方法により生成する第２補間経路生成工程と、
前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、前記第１補間経路上の第１途中点から前記第２補間経路上の第２途中点に向かう繋ぎ合せ経路を生成する繋ぎ合せ工程と、
前記演算部が、前記第１補間経路における前記第１教示点と前記第１途中点との間の第１局所経路と、前記第２補間経路における前記第２途中点と前記第２教示点との間の第２局所経路とを、前記繋ぎ合せ経路で繋ぎ合せた経路に対し、所定の物理的制約を守るよう前記ロボットの動作時間を最適化する最適化計算によって、前記ロボットの前記軌道を求める最適化計算工程と、を備え、
前記第１補間方法及び前記第２補間方法がそれぞれタスク空間で補間経路を生成する方法及び関節空間で補間経路を生成する方法のうちいずれかであり、
前記繋ぎ合せ工程では、前記演算部が、前記第１補間経路及び前記第２補間経路を、前記タスク空間及び前記関節空間のうちいずれかの空間における経路に統一して、前記統一した空間において繋ぎ合せ経路の生成を行うことを特徴とする軌道生成方法。
【請求項４】
前記所定の階数は、３であることを特徴とする請求項１乃至３のいずれか１項に記載の軌道生成方法。
【請求項５】
前記繋ぎ合せ工程では、前記演算部が、シグモイド関数を用いて、前記第１重み係数を求め、シグモイド関数を用いて、前記第２重み係数を求めることを特徴とする請求項１又は２に記載の軌道生成方法。
【請求項６】
前記演算部が、前記第１途中点及び前記第２途中点のうち、少なくとも一方の途中点がユーザにより設定される設定工程を更に備えたことを特徴とする請求項１乃至５のいずれか１項に記載の軌道生成方法。
【請求項７】
前記所定の物理的制約は、前記ロボットの関節角速度の制約及び前記ロボットの先端の速度の制約のうち少なくとも１つを含むほか、前記ロボットの関節トルクの制約、前記ロボットの関節角加速度の制約、前記ロボットの関節角加加速度の制約及び前記ロボットの先端の加速度の制約のうち少なくとも１つを含むことを特徴とする請求項１乃至６のいずれか１項に記載の軌道生成方法。
【請求項８】
請求項１乃至７のいずれか１項に記載の軌道生成方法の各工程を実行する前記演算部を備えた軌道生成装置。
【請求項９】
前記多関節のロボットと、請求項８に記載の軌道生成装置と、を備え、前記軌道生成装置により生成した軌道に基づき、前記ロボットの動作を制御することを特徴とするロボット装置。
【請求項１０】
コンピュータに、請求項１乃至７のいずれか１項に記載の軌道生成方法の各工程を実行させるためのプログラム。
【請求項１１】
請求項１０に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの軌道を生成する軌道生成方法、軌道生成装置、ロボット装置、プログラム及び記録媒体に関する。
【背景技術】
【０００２】
ロボットの補間動作には、主に直線補間、円弧補間及び関節補間の３種が知られている。直線補間は、目標点にツールセンターポイント（ＴＣＰ）を直進させる補間方法である。円弧補間は、意図的に２点間の直路から外れた位置に参照ポイントを与え、３点を通る一義的な円弧に沿ってＴＣＰを移動させる補間方法である。この円弧補間と直線補間はタスク空間で補間を行うので、本質的には同じものである。
【０００３】
関節補間は、ＴＣＰの直線性や円弧性は問わず、目標の関節角度に向かって各軸が原則的に同時に動き出し、同時に目標点に到達する補間方法である。すなわち、関節補間は、関節空間（コンフィグレーション空間ともいう）で経路を生成する手法である。関節補間によれば、各関節の１００％の動作性能を発揮する制御が容易であり、また、直線補間や円弧補間で問題となる、特異点問題が起きないので、高速動作が必要な移動区間で多用される。ただし、関節ごとの速度制御となるために、ＴＣＰの経路や速度はユーザが簡単に予測できるものでない。
【０００４】
ロボットの動作は、その移動の都度、その間の動作や目的にふさわしい補間方式が採択される。例えば、円筒部品をロボットが把持し、円筒部品を円筒状の穴の上まで移動させ、円筒部品をその穴に嵌め込む場合がある。この場合、把持した後のロボットの移動は、関節補間で高速に動作し、円筒状の穴の上から円筒部品を穴に嵌め込む動作は直線補間を用いて、真っ直ぐに嵌め込むことが望まれる。
【０００５】
ところで、各移動区間における補間動作は、補間経路上をどのような速度で移動するかを決める速度関数で与えられる。移動開始時と移動終了時にロボットが停止しているとすると、移動開始後は、加速をして移動速度を上げ、移動終了前には減速をして停止するという動作となる。
【０００６】
ここで、２つの異なる経路を連続して動かす方法として、経路のつなぎ目で停止することが考えられる。例えば、円筒部品を穴に嵌め込む例の場合、関節補間経路で加速をして移動を行い、関節補間経路の終了前に減速して経路のつなぎ目で停止し、直線補間経路で徐々に速度を上げるようにしておけば、２つの補間経路上を連続して動作させることができる。しかし、２つの補間経路の間でのロボットの厳格な停止は、無用であるだけでなく、この部分での停滞気味の挙動がロボットの動作の時間を長引かせることになる。もちろん、作業時間節約のために急減速や急加速させるとすれば、それはロボットの振動を助長させるもとになり好ましいことではない。
【０００７】
そこで、例えば、直線補間と直線補間とを連続して動作させる場合、それぞれの直線補間経路上での速度関数を先に導きだした後に、先の移動区間の減速域と後の移動区間の加速域とを繋ぎ合せる。そして、両区間の繋ぎ点もしくはその近傍を先の移動区間の減速中の速度と後の移動区間の加速中の速度との和で通過させればよい。これにより、次の教示点に到達させる時間を縮めることができる。このような速度の重ね処理は、先の移動区間の補間方法と後の移動区間の補間方法とが同種である場合、その速度関数自体を繋ぎ合せることによってある意味では容易に達成される。直線補間と直線補間の場合には、タスク空間上で、先の並進速度関数と後の並進速度関数とを、先の回転速度関数と後の回転速度関数とを、それぞれ減速域と加速域とでそれぞれの全部もしくは一部を重ねればよい。
【０００８】
関節補間で動作させる場合は、６軸ロボットであれば関節（軸）ごとに速度関数が与えられる。この場合でも関節補間同士の速度の重ね処理は、それぞれの関節補間経路上での速度関数を求めた後に、第１関節の先の速度関数の減速域と後の速度関数の加速域とを、第２関節の先の速度関数と後の速度関数とをといったように関節ごとを対象にして行えばよい。従って、６自由度のマニピュレータであれば、６つの組合せ全部で重ねられることになる。
【０００９】
しかし、ロボットの動作が関節補間から直線補間に移る場合は、補間をする空間がそれぞれ、関節空間とタスク空間とで異なり、関節補間の関節角速度と、直線補間のＴＣＰの並進速度及び回転速度の繋ぎ合せ処理がそのままではできない。
【００１０】
そこで、特許文献１では、関節補間から直線補間に移る場合、直線補間をしてＴＣＰの並進速度及び回転速度を出した後、逆キネマティクス計算を行い各軸の関節の角速度に直してから、関節補間軌道との速度の繋ぎ合せを行っている。直線補間の速度を関節の角速度に直して速度を繋ぎ合せるため、関節角速度の繋ぎ合せが可能となる。
【００１１】
一方、非特許文献１では、与えられた経路上で、各種物理的制約を守り、動作時間を最短にするロボットの最適な動作速度を計算する最適速度の計算方法が述べられている。与えられた経路で、ロボットの動作速度を調整し動作軌道を作成するには、通常、各種物理的制約を守りつつ、ロボットの動作時間が短くなるように、ユーザが速度値を何度も調整する必要があった。しかし、人間が速度値の調整をすると、軌道が最適にならず、動作時間が長くなってしまうと同時に、軌道開発工数がかかってしまう。そこで、各種物理的制約を守り、動作時間を最短にするロボットの最適な動作速度の計算を自動で行うことが望まれる。
【００１２】
ロボットの関節トルク制約を守りつつ、与えられた経路に対し、できる限り動作時間の短い軌道を生成する最適速度計算方法について非特許文献１に記載されている。
【先行技術文献】
【特許文献】
【００１３】
【特許文献１】特開２００４−２５２８１４号公報
【非特許文献】
【００１４】
【非特許文献１】Ｖｅｒｓｃｈｅｕｒｅ，Ｄｉｅｄｅｒｉｋ，ｅｔ  ａｌ．  “Ｔｉｍｅ−ｏｐｔｉｍａｌ  ｐａｔｈ  ｔｒａｃｋｉｎｇ  ｆｏｒ  ｒｏｂｏｔｓ：  ａ  ｃｏｎｖｅｘ  ｏｐｔｉｍｉｚａｔｉｏｎ  ａｐｐｒｏａｃｈ．”  Ａｕｔｏｍａｔｉｃ  Ｃｏｎｔｒｏｌ，  ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ  ５４．１０  （２００９）：  ２３１８−２３２７．
【発明の概要】
【発明が解決しようとする課題】
【００１５】
しかしながら、上記特許文献１及び上記非特許文献１によっても、２つの補間経路を繋ぎ合せたときに物理的制約を考慮にいれた最適な軌道を生成することができなかった。
【００１６】
非特許文献１には、生成した２つの補間経路を、媒介変数に関して滑らかさを保って繋ぐ方法については記載されていない。また、引用文献１でも、２つの補間経路を繋げる方法は開示されているが、２つの補間経路を媒介変数ｓに関して滑らかさ（所定の階数まで微分した微分値の変化が連続な滑らかさ）をもたせたまま繋げることについては、記載されていない。
【００１７】
つまり、従来、第１補間方法で生成した第１補間経路と第２補間方法で生成した第２補間経路とを媒介変数ｓに関して滑らかさ（所定の階数まで微分した微分値の変化が連続な滑らかさ）をもたせたまま繋げることは、これまで行われていなかった。例えば、関節補間と直線補間という経路を補間する空間がそれぞれ違う２つの補間経路や、円弧補間と直線補間といった形状が違う２つの補間経路を媒介変数ｓに関して滑らかさをもたせたまま繋げることは、行われていなかった。
【００１８】
従来、２つの補間経路を媒介変数ｓに関して滑らかさをもたせたまま繋げていなかったので、ロボットへの時刻毎の関節角度の指令値を生成する、つまりロボットの軌道を生成する際に、軌道の最適性に大きな誤差を生む原因となっていた。
【００１９】
例えば、関節ｉ軸の関節加加速度は、ｓを用いて、
【００２０】
【数１】
0006501470.tif 000002
と書ける。もしｑｉ（ｓ）がｓに関して滑らかでなく、単に連続である場合、ｑｉ’（ｓ）、ｑｉ’’（ｓ）、ｑｉ’’’（ｓ）は不連続になるため、ｑｉ（…）（ｓ）は、媒介変数ｓに関して不連続になる。
【００２１】
このような滑らかでない（所定の階数まで微分した微分値の変化が不連続である）経路で、速度の最適化計算を行った場合、一定周期毎のロボットへの関節角度の指令値を生成する際に大きな誤差が生じていた。これにより、例えば関節加加速度が制約から大幅に外れる原因となっていた。また、これは、関節加加速度だけに限定されるものではなく、関節トルク制約、関節角速度制約、関節角加速度制約、ＴＣＰ速度制約又はＴＣＰ加速度制約等の物理的制約も同様に大きな誤差を生み、制約から大幅に外れる原因となっていた。
【００２２】
そこで、本発明は、２つの補間経路を繋ぎ合せた経路を辿るロボットが停止しない、物理的制約を守った最適な軌道を生成することを目的とする。
【課題を解決するための手段】
【００２３】
本発明は、演算部が、多関節のロボットの軌道を生成する軌道生成方法であって、前記演算部が、媒介変数の関数であって、所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第１教示点から第１補助点に向かう第１補間経路を、第１補間方法により生成する第１補間経路生成工程と、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第２補助点から第２教示点に向かう第２補間経路を、第２補間方法により生成する第２補間経路生成工程と、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、前記第１補間経路上の第１途中点から前記第２補間経路上の第２途中点に向かう繋ぎ合せ経路を生成する繋ぎ合せ工程と、前記演算部が、前記第１補間経路における前記第１教示点と前記第１途中点との間の第１局所経路と、前記第２補間経路における前記第２途中点と前記第２教示点との間の第２局所経路とを、前記繋ぎ合せ経路で繋ぎ合せた経路に対し、所定の物理的制約を守るよう前記ロボットの動作時間を最適化する最適化計算によって、前記ロボットの前記軌道を求める最適化計算工程と、を備え、前記繋ぎ合せ工程では、前記演算部が、前記第１途中点から前記第１補助点までの各第１補間点に対して、前記第１教示点に近い側の第１補間点から順に、１から０に順次遷移する第１重み係数をかけ、前記第２補助点から前記第２途中点までの各第２補間点に対して、前記第２補助点に近い側の第２補間点から順に、１から前記第１重み係数を引いた値となるよう０から１に順次遷移する第２重み係数をかけ、前記第１補間点に前記第１重み係数をかけて求めた点と、前記第２補間点に前記第２重み係数をかけて求めた点とを足し合わせて、前記繋ぎ合せ経路を生成し、前記繋ぎ合せ工程では、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数による微分値の変化が連続な、第１重み関数を用いて、前記第１重み係数を求め、前記所定の階数まで前記媒介変数による微分値の変化が連続な、第２重み関数を用いて、前記第２重み係数を求めることを特徴とする。
また、本発明は、演算部が、多関節のロボットの軌道を生成する軌道生成方法であって、前記演算部が、媒介変数の関数であって、所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第１教示点から第１補助点に向かう第１補間経路を、第１補間方法により生成する第１補間経路生成工程と、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、第２補助点から第２教示点に向かう第２補間経路を、第２補間方法により生成する第２補間経路生成工程と、前記演算部が、前記媒介変数の関数であって、前記所定の階数まで前記媒介変数で微分した微分値の変化が連続な、前記第１補間経路上の第１途中点から前記第２補間経路上の第２途中点に向かう繋ぎ合せ経路を生成する繋ぎ合せ工程と、前記演算部が、前記第１補間経路における前記第１教示点と前記第１途中点との間の第１局所経路と、前記第２補間経路における前記第２途中点と前記第２教示点との間の第２局所経路とを、前記繋ぎ合せ経路で繋ぎ合せた経路に対し、所定の物理的制約を守るよう前記ロボットの動作時間を最適化する最適化計算によって、前記ロボットの前記軌道を求める最適化計算工程と、を備え、前記第１補間方法及び前記第２補間方法がそれぞれタスク空間で補間経路を生成する方法及び関節空間で補間経路を生成する方法のうちいずれかであり、前記繋ぎ合せ工程では、前記演算部が、前記第１補間経路及び前記第２補間経路を、前記タスク空間及び前記関節空間のうちいずれかの空間における経路に統一して、前記統一した空間において繋ぎ合せ経路の生成を行うことを特徴とする。
【発明の効果】
【００２４】
本発明によれば、媒介変数に関して滑らかな（所定の階数まで微分した微分値の変化が連続である）ロボットの経路を生成しているので、２つの補間経路を繋げ合せた経路を辿るロボットが停止することのない、物理的制約を守った軌道を生成することができる。
【図面の簡単な説明】
【００２５】
【図１】本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。
【図２】本発明の第１実施形態に係るロボット装置のロボットを示す模式図である。
【図３】本発明の第１実施形態に係る軌道生成装置の構成を示すブロック図である。
【図４】本発明の第１実施形態における重み関数を示すグラフである。
【図５】本発明の第１実施形態におけるタスク空間でのロボットの経路を示す説明図である。
【図６】本発明の第１実施形態における関節空間でのロボットの経路を示す説明図である。
【図７】本発明の第１実施形態に係る軌道生成方法の主要なステップを示すフローチャートである。
【図８】本発明の第１実施形態に係る軌道生成方法の経路生成処理を示すフローチャートである。
【図９】本発明の第２実施形態におけるタスク空間でのロボットの経路を示す説明図である。
【図１０】本発明の第２実施形態における関節空間でのロボットの経路を示す説明図である。
【図１１】本発明の第２実施形態に係る軌道生成方法の経路生成処理を示すフローチャートである。
【図１２】本発明の第３実施形態におけるタスク空間でのロボットの経路を示す説明図である。
【図１３】本発明の第３実施形態における関節空間でのロボットの経路を示す説明図である。
【図１４】本発明の第３実施形態に係る軌道生成方法の経路生成処理を示すフローチャートである。
【図１５】本発明の第４実施形態におけるタスク空間でのロボットの経路を示す説明図である。
【図１６】本発明の第４実施形態における関節空間でのロボットの経路を示す説明図である。
【図１７】本発明の第４実施形態に係る軌道生成方法の経路生成処理を示すフローチャートである。
【発明を実施するための形態】
【００２６】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００２７】
［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。ロボット装置５００は、多関節（２関節以上）のロボット２００と、ロボット２００の軌道を生成する軌道生成装置１００と、生成された軌道に基づきロボット２００の動作を制御するロボットコントローラ３００と、を備えている。また、ロボット装置５００は、作業者（ユーザ）の操作により軌道生成装置１００にデータ入力等を行う操作部４１０と、軌道生成装置１００から表示データを入力して画像表示を行う表示部４２０とを有する操作装置４００を備えている。
【００２８】
軌道生成装置１００は、演算部であるＣＰＵ（Central  Processing  Unit）１０１と、種々のデータを格納する記憶部１０６と、を備えたコンピュータで構成されている。操作装置４００の表示部４２０は、ディスプレイであり、ロボットアーム２１０や作業空間を３Ｄ（３次元）グラフィックス表示して、生成された動作軌道を作業者が視認できるよう構成されている。なお、操作部４１０と表示部４２０とが１つの操作装置４００に組み込まれている場合について説明するが、操作部４１０と表示部４２０とが独立した装置で構成されていてもよい。また、操作部４１０と表示部４２０とが別々に構成されている場合について説明するが、操作部４１０と表示部４２０とがタッチパネルディスプレイ等の操作及び表示が可能な操作表示部で構成されていてもよい。
【００２９】
ＣＰＵ１０１は、後述するプログラムを読み出して実行することにより、キネマティクス計算部３１、経路生成部３２、経路繋ぎ合せ部３３、教示点計算部３４及び最適速度計算部３５として機能する。
【００３０】
図２は、本発明の第１実施形態に係るロボット装置のロボットを示す模式図である。ロボット２００は、多関節（２関節以上、第１実施形態では２関節）のロボットアーム２１０と、ロボットアーム２１０の先端に取り付けられたエンドエフェクタ、例えばロボットハンド２２０とを有している。
【００３１】
ロボットアーム２１０は、例えば２つの関節（軸）Ｊ１，Ｊ２とリンク２０１，２０２で構成される２自由度のロボットアームである。関節には回転関節や直動関節の他、球体関節などさまざまな種類があるが、第１実施形態では、関節Ｊ１，Ｊ２は、回転（旋回）関節である。関節の動作はモータ等の動力源で能動的に動作する場合と、動力源を持たずに受動的に動作する場合があるが、第１実施形態では、関節Ｊ１，Ｊ２は、動力源を有する。ロボットアームは、各関節間がリンクにより結びつけられ、関節とリンクが交互に直列されたシリアルリンク型と、関節とリンクの組合せが並列となったパラレルリンク型などがあるが、第１実施形態では、回転関節を有するシリアルリンク型とする。つまり、ロボットアーム２１０は、ロボットアーム自体が動作するような動作体として広く観念されるものであればよく、特に限定するものではない。例えば、６軸多関節ロボットアームや直動型のロボットアームを用いてもよい。
【００３２】
ロボットアーム２１０（ロボット２００）の自由度を表すパラメータを関節角度として、ロボットアーム２１０の２軸Ｊ１，Ｊ２の関節角度をそれぞれθ１，θ２とする。ロボットアーム２１０のコンフィグレーションは（θ１，θ２）で表され、関節空間上では、１つの点とみなすことができる。このように、ロボットアーム２１０の自由度を表すパラメータ（例えば、関節角度や伸縮長さ）を座標軸の値とした場合、ロボットアーム２１０のコンフィグレーションは関節空間上の点として表現することができる。つまり、関節空間は、ロボットアーム２１０（ロボット２００）の関節角度を座標軸とする空間である。
【００３３】
また、ロボット２００の先端、即ちロボットハンド２２０には、ツールセンターポイント（ＴＣＰ）が設定されている。ＴＣＰは、位置を表す３つのパラメータ（ｘ，ｙ，ｚ）と、姿勢（回転）を表す３つのパラメータ（α，β，γ）、即ち６つのパラメータ（ｘ，ｙ，ｚ，α，β，γ）で表され、タスク空間上では、１つの点としてみなすことができる。つまり、タスク空間は、これら６つの座標軸で規定された空間である。
【００３４】
図３は、本発明の第１実施形態に係る軌道生成装置１００の構成を示すブロック図である。図３に示すように、コンピュータで構成されている軌道生成装置１００は、演算部としてのＣＰＵ１０１を備えている。また軌道生成装置１００は、記憶部１０６として、ＲＯＭ（Read  Only  Memory）１０２、ＲＡＭ（Random  Access  Memory）１０３及びＨＤＤ（Hard  Disk  Drive）１０４を備えている。また、軌道生成装置１００は、記録ディスクドライブ１０５及び各種のインタフェース１１１〜１１３を備えている。
【００３５】
ＣＰＵ１０１には、ＲＯＭ１０２、ＲＡＭ１０３、ＨＤＤ１０４、記録ディスクドライブ１０５及びインタフェース１１１〜１１３が、バス１２０を介して接続されている。
【００３６】
ＲＯＭ１０２には、ＢＩＯＳ等の起動プログラムが格納されている。ＲＡＭ１０３は、ＣＰＵ１０１の演算処理結果等、各種データを一時的に記憶する。ＨＤＤ１０４には、プログラム１４０が格納（記録）されている。そして、ＣＰＵ１０１は、プログラム１４０を読み出して実行することにより、後述する軌道生成方法の各工程を実行する。つまり、ＣＰＵ１０１は、プログラム１４０を読み出して実行することにより、図１に示す各部３１〜３５として機能する。記録ディスクドライブ１０５は、記録ディスク１４１に記録された各種データやプログラム等を読み出すことができる。
【００３７】
インタフェース１１１には、上述した操作装置４００が接続されている。インタフェース１１２には、ＵＳＢメモリ等の書き換え可能な不揮発性メモリ、或いは外付けＨＤＤ等の外部記憶装置６００が接続されている。インタフェース１１３には、上述したロボットコントローラ３００が接続されている。
【００３８】
図１において、キネマティクス計算部３１は、ロボット２００（ロボットアーム２１０）の各関節角度情報から、ロボット２００のＴＣＰの位置及び回転（姿勢）を求める順キネマティクスを計算する。また、キネマティクス計算部３１は、ロボット２００のＴＣＰの位置及び回転（姿勢）から、ロボット２００（ロボットアーム２１０）の各関節角度を計算する逆キネマティクスを計算する。
【００３９】
キネマティクス計算部３１は、例えば、教示点として、コンフィグレーション（θ１，θ２）を与えられ、それを基準として直線補間を行う場合、コンフィグレーションを順キネマティクスによりＴＣＰの位置及び回転にする。逆に、キネマティクス計算部３１は、教示点として、ＴＣＰの位置及び回転を与えられ、関節補間をする場合は、ＴＣＰの位置及び回転を逆キネマティクスによりコンフィグレーションにする。
【００４０】
経路生成部３２は、教示点間を補間するロボット２００の経路（補間経路）を生成する。ロボット２００の経路とは、関節空間におけるロボット２００のコンフィグレーションの軌跡、もしくはタスク空間におけるロボット２００のＴＣＰの軌跡である。換言すれば、ロボット２００の経路とは、関節空間又はタスク空間の点（ポーズ）の順序集合である。ロボット２００の軌道とは、時間をパラメータとしてポーズ（経路）を表したものであり、第１実施形態では、時刻毎のロボット２００の関節の関節指令値（角度指令値）の集合である。ロボット２００の関節は複数あるため、各関節で関節指令値の同期がとられる。
【００４１】
教示点間を補間する補間方法としては、Ｓｐｌｉｎｅ補間、Ｂ−Ｓｐｌｉｎｅ補間、ベジェ曲線、直線補間、円弧補間、関節補間などがあり、媒介変数ｓの値により、ロボット２００の経路上の位置を示す。つまり、媒介変数ｓの値にロボット２００の位置（補間点）が対応付けられて、これら点の集合で一連の経路が形成される。これら補間方法により、媒介変数ｓをパラメータとした補間関数が、ロボット２００の経路（補間経路）として求められる。
【００４２】
関節加加速度の制約を考慮に入れる場合、媒介変数ｓに関して経路が３階まで微分した微分値の変化が連続である必要がある。例えば、３次のＢ−Ｓｐｌｉｎｅ曲線の場合、媒介変数ｓに対しＣ３級（３階まで微分した値が連続）になる。
【００４３】
最適速度計算部３５は、算出されたロボット２００の経路に対し、所定の物理的制約を守りつつ、ロボット２００の動作時間を最短にするという最適化問題を解き、ロボット２００の最適速度を計算する。所定の物理的制約とは、第１実施形態では、ロボット２００の関節トルクの制約、関節角速度の制約、関節角加速度の制約、関節角加加速度の制約、ＴＣＰ速度の制約及びＴＣＰ加速度の制約である。ロボット２００のＴＣＰ速度の制約とは、ロボット２００の先端の速度の制約である。ロボット２００のＴＣＰ加速度の制約とは、ロボット２００の先端の加速度の制約である。
【００４４】
第１実施形態では、所定の物理的制約には、これら全ての制約が含まれる場合について説明するが、これに限定するものではない。所定の物理的制約には、少なくとも、関節角速度の制約及びＴＣＰ速度の制約のうち少なくとも１つの制約が含まれている。これに加え、所定の物理的制約には、関節トルクの制約、関節角加速度の制約、関節角加加速度の制約及びＴＣＰ加速度の制約のうち少なくとも１つの制約が含まれていればよい。それぞれの制約値の中で、最も高い階数以上の階数を持った経路を生成すれば、計算上の制約値を計算するモデルも連続となり、正確に制約値を計算することができる。
【００４５】
関節トルクの制約、関節角加速度の制約、関節角加加速度の制約及びＴＣＰ加速度の制約を条件にロボット２００の軌道を求める際には、媒介変数ｓで経路を１〜３階微分した微分値を用いる。このため、経路は媒介変数ｓに対し少なくともＣ３級（３階微分した微分値の変化が連続）である必要がある例について説明する。つまり、所定の物理的制約には、経路を媒介変数ｓで１〜３階微分した微分値を用いて評価する制約が含まれており、第１実施形態では、関節トルクの制約、関節角加速度の制約、関節角加加速度の制約及びＴＣＰ加速度の制約が含まれている。
【００４６】
最適化問題の解き方としては、公知の方法を用いればよく、例えば、パラメータを置き換えて、問題を凸問題化し、ログバリア法とニュートン法を用いて大域的収束解に高速に収束させるといった方法を用いればよい。
【００４７】
経路繋ぎ合せ部３３は、２つの補間経路それぞれに繋ぎ合せ区間（重ね合せ区間）を設け、２つの補間経路の繋ぎ合せ区間上の点に重み係数を乗じて足し合せ、２つの補間経路を所定の滑らかさ（ここではＣ３級の滑らかさ）で繋ぎ合せた経路を生成する。
【００４８】
関節補間及び直線補間でそれぞれ生成された、媒介変数ｓに関してＣ３級の２つの補間経路を、経路繋ぎ合せ部３３が、媒介変数ｓに関してＣ３級の滑らかさを保ち繋ぎ合せる例を以下に説明する。
【００４９】
媒介変数ｓに関して滑らかに、関節空間で補間した補間経路とタスク空間で補間した補間経路とを繋ぐためには、各補間経路に繋ぎ合せ区間を設け、徐々に関節空間補間経路からタスク空間補間経路に移り変える必要がある。徐々に移り変える際に、繋ぎ合せの重みを徐々に移していくことが、滑らかに繋ぐ上で必須となる。
【００５０】
重み関数の条件として、繋ぎ合せの区間の開始点と終了点で、媒介変数ｓでの１〜３階の微分値が０になり（０とみなせるものも含む）、かつ媒介変数ｓに関して少なくともＣ３級での滑らかさが必要になる。そこで、Ｃ∞級であるシグモイド関数を応用して重みづけをすることを考える。
【００５１】
シグモイド関数は、
【００５２】
【数２】
0006501470.tif 000003
である。しかし、このままでは、定義域が−∞≦ｘ≦∞となり扱いにくい。そこで、シグモイド関数とｔａｎ（ｘ）との合成関数を以下のように考える。
【００５３】
【数３】
0006501470.tif 000004
【００５４】
更に、
【００５５】
【数４】
0006501470.tif 000005
とする。但し、ａは０より大きい実数である。
【００５６】
この重み関数は、ａを十分大きくとれば、定義域の開始点と終了点で、１〜３階微分値がほぼ０とみなせるため、異なる補間方法で作成した補間経路を媒介変数ｓに関してほぼＣ３級を保ち滑らかに繋ぐ重みづけに利用できる。
【００５７】
図４は、ａ＝５の時の第１重み関数ｆｓｉｇ１（ｘ）、第２重み関数ｆｓｉｇ２（ｘ）のグラフであり、図４（ａ）は第１重み関数ｆｓｉｇ１（ｘ）のグラフ、図４（ｂ）は第２重み関数ｆｓｉｇ２（ｘ）のグラフである。
【００５８】
なお、第１重み関数はｘの増加に応じて０から１に順次変化する関数であり、第２重み関数はｘの増加に応じて１から０に順次変化する関数であって、同じｘ値のときの第１重み関数の第１重み係数と、第２重み関数の第２重み係数との合計が１であればよい。このような関数であれば第１重み関数及び第２重み関数は、上記の関数に限定されるものではなく、多項式やその他の関数を用いてもよい。その際、第１重み関数は、媒介変数ｓの関数であって、所定の階数（例えば３階）まで媒介変数ｓによる微分で連続な関数であってもよい。また、第２重み関数は、媒介変数ｓの関数であって、所定の階数（例えば３階）まで媒介変数ｓによる微分で連続な関数であってもよい。
【００５９】
図５は、第１実施形態におけるタスク空間でのロボットの経路を示す説明図であり、図６は、第１実施形態における関節空間でのロボットの経路を示す説明図である。
【００６０】
但し、図５中のＰ２１，Ｐ２２は、直線補間経路を生成するための教示点であり、図６中のＱ１１，Ｑ１２は、関節補間経路を生成するための教示点である。また、図６中のＱ１１，Ｑ１２のタスク空間上のＴＣＰの位置及び回転をそれぞれ図５中のＰ１１，Ｐ１２とする。ＴＣＰが図５中のＰ２１，Ｐ２２の位置及び回転にあるときのロボット２００（ロボットアーム２１０）のコンフィグレーションをそれぞれ図６中のＱ２１，Ｑ２２とする。
【００６１】
また、関節空間上の教示点Ｑ１１，Ｑ１２により関節補間をした、関節補間経路上のロボット２００のコンフィグレーションを図６中のｑ１（ｓ１）（０≦ｓ１≦１）とする。タスク空間上の教示点Ｐ２１，Ｐ２２により直線補間をした、直線補間経路上にＴＣＰを位置付けるロボット２００のコンフィグレーションを図６中のｑ２（ｓ２）（０≦ｓ２≦１）とする。但し、ｓ１、ｓ２は、それぞれ関節補間経路、直線補間経路の媒介変数とする。
【００６２】
また、ロボット２００のコンフィグレーションがｑ１（ｓ１）の時のＴＣＰの位置及び回転を表すベクトルを図５中のｐ１（ｓ１）とする。ロボット２００のコンフィグレーションがｑ２（ｓ２）の時のＴＣＰの位置及び回転を表すベクトルを図５中のｐ２（ｓ２）とする。
【００６３】
第１教示点である教示点Ｐ１１（又は教示点Ｑ１１）及び第２教示点である教示点Ｐ２２（又は教示点Ｑ２２）は、ロボット２００を通過させる点である。これら２つの教示点Ｐ１１，Ｐ２２（又は教示点Ｑ１１，Ｑ２２）は、ユーザが操作部４１０を操作して直接、又はユーザが操作部４１０を操作して記憶部１０６に記憶させて、ＣＰＵ１０１に指定（設定）する。
【００６４】
一方、第１補助点である教示点Ｐ１２（又は教示点Ｑ１２）及び第２補助点である教示点Ｐ２１（又は教示点Ｑ２１）は、ロボット２００が通過しない点であって、補助的な点である。第１実施形態では、教示点Ｐ１２（又は教示点Ｑ１２）は、予め設定される点であり、教示点Ｐ２１（又は教示点Ｑ２１）は、演算により求める点である。
【００６５】
今、関節補間により求めた第１補間経路である関節補間経路を、直線補間により求めた第２補間経路である直線補間経路に繋ぎ合せることを考える。そこで、関節補間経路（第１補間経路）上のｓ１＝ｓ１ｓを繋ぎ合せ区間の開始点とする。このｓ１＝ｓ１ｓのとき点は、関節補間経路上の途中点（第１途中点）である。また、関節補間経路上のｓ１＝ｓ１ｅを繋ぎ合せ区間の終了点とする。
【００６６】
また、直線補間経路（第２補間経路）上のｓ２＝ｓ２ｓを繋ぎ合せ区間の開始点とし、直線補間経路上のｓ２＝ｓ２ｅを繋ぎ合せ区間の終了点とする。ｓ２＝ｓ２ｅのとき点は、直線補間経路上の途中点（第２途中点）である。今簡単のために各定数を、ｓ１ｅ＝１、ｓ２ｓ＝０、ｓ２ｅ−ｓ２ｓ＝ｓ１ｅ−ｓ１ｓつまりｓ２ｅ＝１−ｓ１ｓとする。
【００６７】
ユーザは、各補間経路の繋ぎ合せ区間とそうでない区間の境目となる第１途中点及び第２途中点である教示点Ｐ１ａ，Ｐ２ａ（又は教示点Ｑ１ａ，Ｑ２ａ）のうち少なくとも１つの途中点のパラメータ値を設定（指定）する。教示点計算部３４は、その点を基に、経路補間に用いる教示点と繋ぎ合せ区間を逆算する。
【００６８】
第１実施形態の繋ぎ合せ区間は、関節補間と直線補間とが繋ぎ合さった繋ぎ合せ経路となるため、直線にはならず、ユーザの意図した動きにならない可能性がある。
【００６９】
そこで、ユーザが、直線補間経路のここからは直進性を確保したいという点を、直線補間経路上から指定（設定）する。その点から、教示点計算部３４により、経路補間に用いる適切な教示点と、繋ぎ合せ区間を算出する。
【００７０】
第１実施形態では、直進性を確保できる図５中の教示点Ｐ２ａをユーザが指示（設定）し、そこから逆算して、教示点Ｐ２１と、定数ｓ２ｅを教示点計算部３４により逆算を行う。具体的には、直線補間が媒介変数ｓ＝ｓ２ｅの時のＴＣＰの位置及び回転ｐ１（ｓ）が教示点Ｐ２ａと重なるように、教示点Ｐ２１と、定数ｓ２ｅを逆算する。
【００７１】
なお、定数ｓ２ｅが求まれば、ｓ２ｅ＝１−ｓ１ｓであるので、定数ｓ１ｓが求まる。
【００７２】
第１実施形態では、直線補間経路の教示点及び繋ぎ合せ区間の調整のみを説明したが、それに限定するものではない。関節補間部分も、ユーザが同様に繋ぎ合せ区間とそうでない区間の境目である教示点Ｐ１ａ（教示点Ｑ１ａ）を指定（設定）して、関節補間の特性が欲しい区間をユーザが指示できるようにしても良い。
【００７３】
ｓは、２つの補間経路を繋ぎ合せたロボット２００の経路の媒介変数である。この媒介変数ｓの値にロボット２００の経路上の点が対応している。つまり、媒介変数ｓの値を決めると、その値に対応するロボット２００の経路上の点が決まる。第１実施形態では、媒介変数ｓは、０から１＋ｓ１までの範囲の値である。ｓ＝０からｓ＝１＋ｓ１まで時刻に応じて媒介変数ｓの値が増加するように変化させることにより、ロボット２００のＴＣＰは、教示点Ｐ１１から教示点Ｐ２２まで経路上を移動することとなる。
【００７４】
０≦ｓ＜ｓ１ｓの局所区間を関節補間区間、ｓ１ｓ≦ｓ＜１の区間を繋ぎ合せ区間、１≦ｓ＜１＋ｓ１ｓの局所区間を直線補間区間とする。このときの２つの補間経路を繋ぎ合せたときの関節空間上の経路ｑ（ｓ）は以下のようになる。
【００７５】
【数５】
0006501470.tif 000006
【００７６】
ここで、ｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））は第１重み関数、ｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）は第２重み関数である。これら関数のｓに数値を代入することで、第１重み係数及び第２重み係数が求まる。
【００７７】
媒介変数ｓに関してＣ３級で滑らかさを保つｑ（ｓ）を用いれば、連続なｑ’（ｓ），ｑ’’（ｓ），ｑ’’’（ｓ）を求めることが可能となる。また、ｑ（ｓ）を用いれば、最適速度計算（ロボット２００の動作時間を最適にする最適化計算）をした後に、それに基づき、一定時間ごとの関節指令値（つまり軌道）を生成することが可能となる。
【００７８】
式（５）に則り、図６における、繋ぎ合せ区間での繋ぎ合せ経路上の点を算出する式は図４のグラフ上の値を用いて以下のように書ける。
【００７９】
【数６】
0006501470.tif 000007
【００８０】
ここで、ｑ１１，ｑ１２，ｑ１３，ｑ１４，ｑ１５は、図６中、第１補間経路である関節補間経路ｑ１（ｓ１）上の教示点Ｑ１ａから教示点Ｑ１２までの複数の補間点（第１補間点）である。また、ｑ２１，ｑ２２，ｑ２３，ｑ２４，ｑ２５は、図６中、第２補間経路である直線補間経路ｑ２（ｓ２）上の教示点Ｑ２１から教示点Ｑ２ａまでの複数の補間点（第２補間点）である。
【００８１】
図７は、第１実施形態に係る軌道生成方法の主要なステップを示すフローチャートである。以下、軌道生成装置１００のＣＰＵ１０１により行われるロボット２００の軌道生成方法の各工程について説明する。
【００８２】
まず、ＣＰＵ１０１は、ロボット２００が作業を行う作業空間での、ロボット２００のモデル及び制約条件（所定の物理的制約）を記憶部１０６（例えばＨＤＤ１０４）から読み込む（Ｓ１０１）。
【００８３】
ロボット２００のモデルは、ロボット２００を構成する部品の設計情報であり、各リンクの慣性モーメント、質量、位置、回転、軸数等である。制約条件は、ロボット２００の物理的制約を定義したもので、ロボット２００の関節トルクの制約、関節角速度の制約、関節角加速度の制約、関節角加加速度の制約、ＴＣＰ速度の制約及びＴＣＰ加速度の制約である。
【００８４】
次に、ＣＰＵ１０１は、ロボット２００の教示点及び補間方法を取得する（Ｓ１０２）。例えば、ロボット２００の教示点は、ＴＣＰの位置及び回転、又は関節角度により決定することができる。教示点と同時に、どういった補間方法で、その教示点まで移動するかの情報を読み込む。
【００８５】
第１実施形態でＣＰＵ１０１が取得する教示点は、第１教示点である教示点Ｐ１１（又は教示点Ｑ１１）、第１補助点である教示点Ｐ１２（又は教示点Ｑ１２）、第２教示点である教示点Ｐ２２（又は教示点Ｑ２２）である。
【００８６】
また、第１実施形態では、ユーザが関節補間と直線補間を指定するので、第１実施形態でＣＰＵ１０１が取得する補間方法は、第１補間方法である関節補間、第２補間方法である直線補間である。これら教示点及び補間方法は、記憶部１０６（例えばＨＤＤ１０４）に記憶されたロボットプログラムにて指定（設定）されている。
【００８７】
次に、ＣＰＵ１０１は、途中点である教示点Ｐ１ａ，Ｐ２ａのうち、少なくとも一方、第１実施形態では教示点Ｐ２ａがユーザにより設定（指定）され、繋ぎ合せ区間を設定する（Ｓ１０３：設定工程）。第１実施形態では、関節補間のｓ１ｓ≦ｓ１＜１の区間と、直線補間の０＜ｓ２＜ｓ２ｅの区間を繋ぎ合せ区間とする。繋ぎ合せ区間は、任意であるが、ユーザが指定した教示点から直進性を確保する場合、教示点計算部３４により、それに合わせて直線補間の経路生成に用いる教示点と繋ぎ合せ区間を調整する。
【００８８】
具体的に説明すると、定数ｓ１ｅは１に、定数ｓ２ｓは０に設定される。つまり、これら定数ｓ１ｅ，ｓ２ｓは、予め記憶部１０６に記憶されており、ＣＰＵ１０１が記憶部１０６から読み出して設定する。
【００８９】
第２補助点から第２教示点に向かう第２補間経路上の第２途中点である教示点Ｐ２ａ（パラメータ値）は、ユーザが設定（指定）する。これにより、ＣＰＵ１０１は、第２補助点である教示点Ｐ２１及び定数ｓ２ｅ，ｓ１ｓを演算する。なお、第１実施形態では、第１教示点から第１補助点に向かう第１補間経路上の第１途中点である教示点Ｐ１ａは、予め記憶部１０６に記憶されており、ＣＰＵ１０１が記憶部１０６から読み出して設定する。
【００９０】
以上、ステップＳ１０３により、繋ぎ合せ区間、つまり定数ｓ１ｓ，ｓ２ｅが設定（初期化）される。
【００９１】
次に、ＣＰＵ１０１は、第１補間経路である関節補間経路と第２補間経路である直線補間経路とを繋ぎ合せたロボット２００の経路ｑ（ｓ）を生成する経路生成処理を実行する（Ｓ１０４）。具体的には、図６において、関節補間経路における教示点Ｑ１１と教示点Ｑ１ａとの間の第１局所経路と、直線補間経路における教示点Ｑ２ａと教示点Ｑ２２との間の第２局所経路とを、繋ぎ合せ区間の繋ぎ合せ経路で繋ぎ合せた経路ｑ（ｓ）を生成する。ここで、第１局所経路は、ｑ１（ｓ）（但し０≦ｓ＜ｓ１ｓ）である。第２局所経路は、ｑ２（ｓ−ｓ１ｓ）（但し１≦ｓ＜１＋ｓ１ｓ）である。繋ぎ合せ経路は、ｑ１（ｓ）×ｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））＋ｑ２（ｓ−ｓ１ｓ）×ｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）（但しｓ１ｓ≦ｓ＜１）である（式（５）参照）。
【００９２】
次に、ＣＰＵ１０１は、速度の最適化の対象となる２つの補間経路を繋いだ経路ｑ（ｓ）上の評価点を決定する（Ｓ１０５）。例えば、ｓ＝０からｓ＝１＋ｓ１ｓまで、ある一定のΔｓずつ等分割したｓの列を評価点とする。
【００９３】
次に、ＣＰＵ１０１は、補間関数、つまり経路ｑ（ｓ）を使って、媒介変数ｓに関する数値微分により、評価点におけるｑ’（ｓ），ｑ’’（ｓ），ｑ’’’（ｓ）を求める（Ｓ１０６）。
【００９４】
【数７】
0006501470.tif 000008
【００９５】
次にＣＰＵ１０１は、ｑ’（ｓ），ｑ’’（ｓ），ｑ’’’（ｓ）を用い、関節トルク制約、関節角速度制約、関節角加速度制約、関節角加加速度制約、ＴＣＰ速度制約及びＴＣＰ加速度制約の制約を条件に最短時間で移動する最適速度を計算する（Ｓ１０７）。
【００９６】
次に、ＣＰＵ１０１は、式（１４）のように、最適速度計算により計算したｓ（・）（ｓ）から、ｓ（ｔ）を計算し、ｑ（ｓ）から一定時間Δｔ毎の関節指令値（角度指令値）を求める（Ｓ１０８）。但し、ｓ（０）＝０とする。
【００９７】
【数８】
0006501470.tif 000009
【００９８】
以上のステップＳ１０５〜Ｓ１０８が最適化計算工程である。つまり、ステップＳ１０５〜Ｓ１０８により、経路ｑ（ｓ）に対し、所定の物理的制約を守るようロボット２００の動作時間を最適化する最適化計算によって、時刻に対応する媒介変数ｓの値（つまりｓ（ｔ））を求めて、ロボット２００の軌道を求める。
【００９９】
媒介変数ｓに関してＣ３級の滑らかさをもつｑ（ｓ）を用いれば、ｑ’（ｓ）、ｑ’’（ｓ）、ｑ’’’（ｓ）が連続となり、ロボット２００への関節指令値を生成する際の誤差を大幅に減らすことができる。
【０１００】
次に、ステップＳ１０４の経路生成処理について具体的に説明する。図８は、媒介変数ｓを入力してコンフィグレーションｑ（ｓ）を出力する、図７のステップＳ１０４における経路生成処理を示すフローチャートである。
【０１０１】
まずＣＰＵ１０１は、媒介変数ｓの数値を取得（入力）する（Ｓ２０１）。ＣＰＵ１０１は、入力した媒介変数ｓの数値で場合分けを行う（Ｓ２０２）。つまり、ＣＰＵ２０１は、このステップＳ２０２で、媒介変数ｓの数値の判別を行う。０≦ｓ＜ｓ１ｓの場合は、関節補間区間、ｓ１ｓ≦ｓ＜１の場合は、関節補間と直線補間の繋ぎ合せ区間、１≦ｓ＜１＋ｓ１ｓの場合は、直線補間区間である。
【０１０２】
ＣＰＵ１０１は、ステップＳ２０２の判別で０≦ｓ＜ｓ１ｓの場合（補間方法が関節補間の場合）、教示点Ｑ１１，Ｑ１２により関節補間を行い、媒介変数ｓの関数となる関節補間経路であるｑ１（ｓ）を求める（Ｓ２０３）。つまりＣＰＵ１０１は、媒介変数ｓの関数であり、所定の階数（例えば３階）微分した微分値の変化が連続な、教示点Ｑ１１から教示点Ｑ１２に向かう関節補間経路ｑ１（ｓ）を、関節補間により生成する（第１補間経路生成工程）。０≦ｓ＜ｓ１ｓの区間では、第１補間経路中の教示点Ｑ１１と教示点Ｑ１ａとの間の第１局所経路ｑ１（ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ１（ｓ）となる。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ２０４）。
【０１０３】
ＣＰＵ１０１は、ステップＳ２０２の判別でｓ１ｓ≦ｓ＜１の場合（関節補間と直線補間の繋ぎ合せ区間の場合）、教示点Ｑ１１，Ｑ１２により関節補間を行い、ｑ１（ｓ）を求める（Ｓ２０５：第１補間経路生成工程）。
【０１０４】
ＣＰＵ１０１は、教示点Ｐ２１，Ｐ２２によりＴＣＰを直線補間し、ｐ２（ｓ−ｓ１ｓ）を求める（Ｓ２０６）。つまりＣＰＵ１０１は、媒介変数ｓの関数であり、教示点Ｐ２１から教示点Ｐ２２に向かう第２補間経路である直線補間経路ｐ２（ｓ−ｓ１ｓ）を、関節補間により生成する（第２補間経路生成工程）。このステップＳ２０６で生成する直線補間経路ｐ２（ｓ−ｓ１ｓ）は、所定の階数（例えば３階）まで媒介変数ｓにより微分した微分値の変化が連続な、媒介変数ｓの関数である。
【０１０５】
ＣＰＵ１０１は、ｐ２（ｓ−ｓ１ｓ）に対し、キネマティクス計算部３１により逆キネマティクス計算を行い、コンフィグレーションｑ２（ｓ−ｓ１ｓ）を求める（Ｓ２０７）。つまり、ＣＰＵ１０１は、関節補間経路及び直線補間経路を、タスク空間及び関節空間のうちいずれかの空間（第１実施形態では、関節空間）における経路に統一する。
【０１０６】
次に、ＣＰＵ１０１は、関節補間したコンフィグレーションｑ１（ｓ）に重みづけするための第１重み関数ｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））から、第１重み係数を求める（Ｓ２０８）。また、ＣＰＵ１０１は、直線補間したコンフィグレーションｑ２（ｓ）に重みづけするための第２重み関数ｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）から第２重み係数を求める（Ｓ２０９）。
【０１０７】
ＣＰＵ１０１は、ｑ１（ｓ）にｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））を掛け、ｑ２（ｓ−ｓ１ｓ）にｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）を掛けて和をとって、繋ぎ合せ経路ｑ（ｓ）（但しｓ１ｓ≦ｓ＜１）を求める（Ｓ２１０）。
【０１０８】
ｑ１（ｓ）（但しｓ１ｓ≦ｓ＜１）は、式（６）〜式（１０）に示す第１補間点ｑ１１，ｑ１２，ｑ１３，ｑ１４，ｑ１５の集合である。ＣＰＵ１０１は、各補間点ｑ１１，ｑ１２，ｑ１３，ｑ１４，ｑ１５に各第１重み係数ｒ１１，ｒ１２，ｒ１３，ｒ１４，ｒ１５を掛け算する。
【０１０９】
同様に、ｑ２（ｓ−ｓ１ｓ）（但しｓ１ｓ≦ｓ＜１）は、式（６）〜式（１０）に示す第２補間点ｑ２１，ｑ２２，ｑ２３，ｑ２４，ｑ２５の集合である。ＣＰＵ１０１は、各補間点ｑ２１，ｑ２２，ｑ２３，ｑ２４，ｑ２５に各第２重み係数ｒ２１，ｒ２２，ｒ２３，ｒ２４，ｒ２５を掛け算する。ここで、ｒ１１＋ｒ２１＝１，ｒ１２＋ｒ２２＝１，ｒ１３＋ｒ２３＝１，ｒ１４＋ｒ２４＝１，ｒ１５＋ｒ２５＝１である。
【０１１０】
各第１重み係数ｒ１１，ｒ１２，ｒ１３，ｒ１４，ｒ１５は、媒介変数ｓが大きくなるに従って１から０に順次遷移する。つまり、ｒ１１（＝１）＞ｒ１２＞ｒ１３＞ｒ１４＞ｒ１５（＝０）である。
【０１１１】
また、各第２重み係数ｒ２１，ｒ２２，ｒ２３，ｒ２４，ｒ２５は、１から各第１重み係数ｒ１１，ｒ１２，ｒ１３，ｒ１４，ｒ１５を引いた値となるよう媒介変数ｓが大きくなるに従って０から１に順次遷移する。つまり、ｒ２１（＝０）＜ｒ２２＜ｒ２３＜ｒ２４＜ｒ２５（＝１）である。
【０１１２】
即ち、ＣＰＵ１０１は、ステップＳ２１０では、補間点ｑ１１，ｑ１２，ｑ１３，ｑ１４，ｑ１５に対して、教示点Ｑ１１に近い側の補間点から順に、１から０に順次遷移する重み係数ｒ１１，ｒ１２，ｒ１３，ｒ１４，ｒ１５をかける。また、ＣＰＵ１０１は、補間点ｑ２１，ｑ２２，ｑ２３，ｑ２４，ｑ２５に対して、教示点Ｑ２１に近い側の補間点から順に、０から１に順次遷移する重み係数ｒ２１，ｒ２２，ｒ２３，ｒ２４，ｒ２５をかける。ＣＰＵ１０１は、補間点ｑ１ｎ（ｎ＝１〜５、以下同じ）に重み係数ｒ１ｎをかけて求めた点と、補間点ｑ２ｎに重み係数ｒ２ｎをかけて求めた点とを足し合わせて、繋ぎ合せ経路ｑ１，ｑ２，ｑ３，ｑ４，ｑ５を生成する（図７、式（６）〜式（１０））。
【０１１３】
以上のステップＳ２０６〜Ｓ２１０により、ＣＰＵ１０１は、媒介変数ｓの関数であって、所定の階数（例えば３階）まで媒介変数ｓにより微分した微分値の変化が連続な、教示点Ｑ１ａから教示点Ｑ２ａに向かう繋ぎ合せ経路を生成する（繋ぎ合せ工程）。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ２１１）。
【０１１４】
なお、重み係数ｒ１ｎ，ｒ２ｎは、ステップＳ２０８，Ｓ２０９で重み関数から求める場合について説明したが、これに限定するものではなく、予め重み係数ｒ１ｎ，ｒ２ｎを計算しておいて記憶部１０６に記憶させておいてもよい。その際、ＣＰＵ１０１は、ステップＳ２０８，Ｓ２０９では、演算を行わずに記憶部１０６から重み係数ｒ１ｎ，ｒ２ｎのデータを読み出すだけでよい。
【０１１５】
次に、ＣＰＵ１０１は、ステップＳ２０２の判別で１≦ｓ＜１＋ｓ１ｓの場合（補間方法が直線補間の場合）、教示点Ｐ２１，Ｐ２２によりＴＣＰを直線補間し、ｐ２（ｓ−ｓ１ｓ）を求める（Ｓ２１２：第２補間経路生成工程）。
【０１１６】
ＣＰＵ１０１は、ｐ２（ｓ−ｓ１ｓ）に対し、逆キネマティクス計算を行い、関節空間上のコンフィグレーションであるｑ２（ｓ−ｓ１ｓ）を求める（Ｓ２１３）。１≦ｓ＜１＋ｓ１ｓの区間では、第２補間経路中の教示点Ｑ２ａと教示点Ｑ２２との間の第２局所経路ｑ２（ｓ−ｓ１ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ２（ｓ−ｓ１ｓ）である。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ２１４）。
【０１１７】
以上のステップＳ２０１〜Ｓ２１４は、媒介変数ｓの数値を、０から１＋ｓ１ｓまで変化させて繰り返し行う。これらステップＳ２０１〜Ｓ２１４により、式（５）に示す、ロボット２００の関節空間においてＣ３級の経路ｑ（ｓ）が求まる。
【０１１８】
以上、第１実施形態によれば、ロボット２００が関節補間と直線補間という異なる２つの補間経路を繋げ合せた際に、媒介変数ｓに関してＣ３級で滑らかなロボット２００の関節空間における経路ｑ（ｓ）を生成している。これにより、２つの補間経路を繋げ合せた経路ｑ（ｓ）を辿るロボット２００が停止することのない、物理的制約を守った軌道を生成することができる。
【０１１９】
特に、ステップＳ２１０では、ＣＰＵ１０１が、ｑ１ｎ×ｒ１ｎ＋ｑ２ｎ×ｒ２ｎ（ｎ＝１〜５）を計算して、繋ぎ合せ経路ｑｎを生成している。これにより、繋ぎ合せ経路は、媒介変数ｓに関してＣ３級で滑らかな経路として生成される。なお、第１実施形態では、説明の都合上、ｎ＝１〜５としたが、これに限定するものではない。ｎの数は、多ければ多いほど精度が向上するため、繋ぎ合せ経路として、多数の点を生成するのがよい。第１実施形態では、第１及び第２重み関数に基づいて第１及び第２重み係数を計算しているので、予め大量の重み係数のデータを記憶部１０６に記憶させておく必要がない。よって、記憶部１０６のメモリ領域が圧迫されることがなく、メモリ領域を有効活用できる。
【０１２０】
また、第１実施形態によれば、ユーザが教示点Ｐ２ａ（Ｑ２ａ）を設定（指定）しているため、経路の繋ぎ合せ区間を意識せずに、直進性が必要な区間をユーザが指定でき、使用性が上がるという効果がある。
【０１２１】
［第２実施形態］
本発明の第２実施形態に係る軌道生成方法について説明する。なお、軌道生成装置、即ちロボット装置の構成は、上記第１実施形態と同様であり、説明を省略する。第２実施形態では、上記第１実施形態と異なる補間方法の組み合わせ、即ち円弧補間と直線補間の２種類の補間方法が指定されている。
【０１２２】
図９は、第２実施形態におけるタスク空間でのロボットアームの経路を示す説明図であり、図１０は、第２実施形態における関節空間でのロボットアームの経路を示す説明図である。
【０１２３】
第２実施形態では、第１補間方法を円弧補間とし、第２補間方法を、上記第１実施形態と同様、直線補間としたものである。円弧補間をした時のコンフィグレーションをｑ１（ｓ）とすると、上記第１実施形態と同様、式（５）により、経路の滑らかさを保った繋ぎ合せが可能となる。式（５）に則り、図１０における、繋ぎ合せ区間での繋ぎ合せ経路上の点ｑ１〜ｑ５を算出する式は、上記第１実施形態と同様、式（６）〜式（１０）になる。
【０１２４】
上記第１実施形態と違い、円弧補間した補間経路は、タスク空間上のＴＣＰの経路となるため、関節空間上で繋ぎ合せるためには、逆キネマティクスを行い、コンフィグレーションを求めて円弧補間経路ｑ１（ｓ）を求める。
【０１２５】
以上を考慮にいれ、媒介変数ｓからコンフィグレーションｑ（ｓ）を求める流れを説明する。図１１は、媒介変数ｓを入力してコンフィグレーションｑ（ｓ）を出力する経路生成処理のフローチャートである。なお、第２実施形態では、第１補間方法がタスク空間で補間経路を生成する方法であるため、求めた第１補間経路である円弧補間経路を逆キネマティクス計算により関節空間における経路に変換する点で、上記第１実施形態と異なる。それ以外は、上記第１実施形態と同様であるため、以下、経路生成処理に関する各工程を簡略して説明する。
【０１２６】
まずＣＰＵ１０１は、媒介変数ｓを取得（入力）する（Ｓ３０１）。ＣＰＵ１０１は、入力された媒介変数ｓの区間で場合分けを行う（Ｓ３０２）。０≦ｓ＜ｓ１ｓの場合は、円弧補間区間、ｓ１ｓ≦ｓ＜１の場合は、円弧補間と直線補間の繋ぎ合せ区間、１≦ｓ＜１＋ｓ１ｓの場合は、直線補間区間である。
【０１２７】
ＣＰＵ１０１は、ステップＳ３０２の判別で０≦ｓ＜ｓ１ｓの場合、教示点Ｐ１１，Ｐ１２によりＴＣＰを円弧補間し、ｐ１（ｓ）を求める（Ｓ３０３）。ＣＰＵ１０１は、ｐ１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ１（ｓ）を求める（Ｓ３０４）。０≦ｓ＜ｓ１ｓの区間では、第１局所経路ｑ１（ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ１（ｓ）となる。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ３０５）。
【０１２８】
次に、ＣＰＵ１０１は、ステップＳ３０２の判別でｓ１ｓ≦ｓ＜１であった場合、まず教示点Ｐ１１，Ｐ１２によりＴＣＰを円弧補間し、ｐ１（ｓ）を求める（Ｓ３０６）。ＣＰＵ１０１は、ｐ１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ１（ｓ）を求める（Ｓ３０７）。
【０１２９】
ＣＰＵ１０１は、教示点Ｐ２１，Ｐ２２によりＴＣＰを直線補間し、ｐ２（ｓ−ｓ１ｓ）を求める（Ｓ３０８）。ＣＰＵ１０１は、ｐ２（ｓ−ｓ１ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ２（ｓ−ｓ１ｓ）を求める（Ｓ３０９）。
【０１３０】
ＣＰＵ１０１は、円弧補間したコンフィグレーションｑ１（ｓ）に重みづけするための重み関数ｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））を求める（Ｓ３１０）。ＣＰＵ１０１は、直線補間したコンフィグレーションｑ２（ｓ）に重みづけするための重み関数ｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）を求める（Ｓ３１１）。
【０１３１】
ＣＰＵ１０１は、ｑ１（ｓ）にｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））を掛け、ｑ２（ｓ−ｓ１ｓ）にｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）を掛けて和をとって、ｑ（ｓ）を求める（Ｓ３１２）。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ３１３）。
【０１３２】
ＣＰＵ１０１は、ステップＳ３０２の判別で１≦ｓ＜１＋ｓ１ｓであった場合、教示点Ｐ２１，Ｐ２２によりＴＣＰを直線補間し、ｐ２（ｓ−ｓ１ｓ）を求める（Ｓ３１４）。ＣＰＵ１０１は、ｐ２（ｓ−ｓ１ｓ）に対し、逆キネマティクス計算を行い、関節空間上のコンフィグレーションであるｑ２（ｓ−ｓ１ｓ）を求める（Ｓ３１５）。１≦ｓ＜１＋ｓ１ｓの区間では、第２局所経路ｑ２（ｓ−ｓ１ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ２（ｓ−ｓ１ｓ）である。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ３１６）。
【０１３３】
以上、円弧補間と直線補間とをＣ３級を保って繋ぎ合せたコンフィグレーションｑ（ｓ）が求められるので、上記第１実施形態と同様、図７の流れで、最適速度を計算することができる。
【０１３４】
また、図７のステップＳ１０３において、ユーザが円弧補間経路の円弧性と、直線補間経路の直進性を確保したい点Ｐ１ａ，Ｐ２ａ（又は点Ｑ１ａ，Ｑ２ａ）をそれぞれ指定（設定）する。教示点計算部３４として機能するＣＰＵ１０１は、その点Ｐ１ａ，Ｐ２ａ（又は点Ｑ１ａ，Ｑ２ａ）を基に、円弧補間及び直線補間の教示点及び繋ぎ合せ区間を逆算する。
【０１３５】
繋ぎ合せ区間は、円弧補間と直線補間が繋ぎ合わさった経路となるため、純粋な円弧もしくは直線にはならず、ユーザの意図した動きにならない可能性がある。そこで、ユーザが、ここまでは円弧性を確保し、ここからは、直進性を確保したいという点を指定し、それから、教示点計算部３４により適切な教示点Ｐ１２，Ｐ２１（又は教示点Ｑ１２，Ｑ２１）を演算する。
【０１３６】
つまり、第２実施形態では、円弧性を確保する図９上の点Ｐ１ａ、直進性を確保できる図９上の点Ｐ２ａをユーザが指示する。ＣＰＵ１０１は、そこから逆算して、それぞれ適当なＰ１２，Ｐ２１，ｓ１ｓ，ｓ２ｅを求める。具体的には、円弧補間経路が、ｓ１＝ｓ１ｓのときのＴＣＰの位置及び回転ｐ１（ｓ）が、Ｐ１ａと重なり、直線補間経路がｓ２＝ｓ２ｅの時のＴＣＰの位置及び回転ｐ２（ｓ２）が、Ｐ２１と重なるようにＰ１２，Ｐ２１，ｓ１ｓ，ｓ２ｅを逆算する。
【０１３７】
以上説明したように、第２実施形態によれば、上記第１実施形態の効果に加えて、教示点に付随する補間方法が、円弧補間から直線補間に切り替わる場合にも、経路の切り替え時に停止せずに、ロボット２００の各種物理的制約を守った最適な軌道を生成できる。また、第２実施形態によれば、経路の繋ぎ合せ区間を意識せずに、円弧性及び直進性が必要な区間をユーザが指定できるため、使用性が上がるという効果がある。
【０１３８】
［第３実施形態］
本発明の第３実施形態に係る軌道生成方法について説明する。なお、軌道生成装置、即ちロボット装置の構成は、上記第１，第２実施形態と同様であり、説明を省略する。第３実施形態では、上記第１，第２実施形態と異なる補間方法の組み合わせ、即ち円弧補間と関節補間の２種類の補間方法が指定されている。
【０１３９】
図１２は、第３実施形態におけるタスク空間でのロボットアームの経路を示す説明図であり、図１３は、第３実施形態における関節空間でのロボットアームの経路を示す説明図である。
【０１４０】
第３実施形態では、第１補間方法を円弧補間、第２補間方法を関節補間としたものである。円弧補間をした時のコンフィグレーション（補間経路）をｑ１（ｓ）とし、関節補間した時のコンフィグレーション（補間経路）をｑ２（ｓ）すると、上記第１実施形態と同様、式（５）により、経路の滑らかさを保った繋ぎ合せが可能となる。式（５）に則り、図１３における、繋ぎ合せ区間での経路上の点ｑ１〜ｑ５を算出する式は、上記第１実施形態と同様、式（６）〜式（１０）になる。
【０１４１】
上記第２実施形態と同様、円弧補間した経路は、タスク空間上のＴＣＰの経路となるため、関節空間上で繋ぎ合せるためには、逆キネマティクスを行い、コンフィグレーションを求めて円弧補間経路ｑ１（ｓ）を求める。以上を考慮にいれ、媒介変数ｓからコンフィグレーションｑ（ｓ）を求める流れを説明する。
【０１４２】
図１４は、媒介変数ｓを入力してコンフィグレーションｑ（ｓ）を出力する経路生成処理のフローチャートである。なお、第３実施形態では、第２補間方法が関節空間で補間経路を生成する方法であるため、第２補間経路である関節補間経路を関節空間で求める点で、上記第２実施形態と異なる。それ以外は、上記第２実施形態と同様であるため、以下、経路生成処理に関する各工程を簡略して説明する。
【０１４３】
まずＣＰＵ１０１は、媒介変数ｓを取得（入力）する（Ｓ４０１）。ＣＰＵ１０１は、入力された媒介変数ｓの区間で場合分けを行う（Ｓ４０２）。０≦ｓ＜ｓ１ｓの場合は、円弧補間区間、ｓ１ｓ≦ｓ＜１の場合は、円弧補間と関節補間の繋ぎ合せ区間、１≦ｓ＜１＋ｓ１ｓの場合は、関節補間区間である。
【０１４４】
ＣＰＵ１０１は、ステップＳ４０２の判別で０≦ｓ＜ｓ１ｓの場合、教示点Ｐ１１，Ｐ１２によりＴＣＰを円弧補間し、ｐ１（ｓ）を求める（Ｓ４０３）。ＣＰＵ１０１は、ｐ１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ１（ｓ）を求める（Ｓ４０４）。０≦ｓ＜ｓ１ｓの区間では、第１局所経路ｑ１（ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ１（ｓ）となる。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ４０５）。
【０１４５】
次に、ＣＰＵ１０１は、ステップＳ４０２の判別でｓ１ｓ≦ｓ＜１の場合、まず教示点Ｐ１１，Ｐ１２によりＴＣＰを円弧補間し、ｐ１（ｓ）を求める（Ｓ４０６）。ＣＰＵ１０１は、ｐ１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ１（ｓ）を求める（Ｓ４０７）。
【０１４６】
ＣＰＵ１０１は、教示点Ｑ２１，Ｑ２２により関節補間し、ｑ２（ｓ−ｓ１ｓ）を求める（Ｓ４０８）。
【０１４７】
ＣＰＵ１０１は、円弧補間したコンフィグレーションｑ１（ｓ）に重みづけするための重み関数ｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））を求める（Ｓ４０９）。ＣＰＵ１０１は、直線補間したコンフィグレーションｑ２（ｓ）に重みづけするための重み関数ｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）を求める（Ｓ４１０）。
【０１４８】
ＣＰＵ１０１は、ｑ１（ｓ）にｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））を掛け、ｑ２（ｓ−ｓ１ｓ）にｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）を掛けて和をとって、ｑ（ｓ）を求める（Ｓ４１１）。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ４１２）。
【０１４９】
ＣＰＵ１０１は、ステップＳ４０２の判別で１≦ｓ＜１＋ｓ１ｓの場合、教示点Ｑ２１，Ｑ２２により関節補間し、ｑ２（ｓ−ｓ１ｓ）を求める（Ｓ４１３）。１≦ｓ＜１＋ｓ１ｓの区間では、第２局所経路ｑ２（ｓ−ｓ１ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ２（ｓ−ｓ１ｓ）である。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ４１４）。
【０１５０】
以上、円弧補間と関節補間とをＣ３級を保って繋ぎ合せたコンフィグレーションｑ（ｓ）を求められるので、上記第１実施形態と同様、図７の流れで、最適速度を計算することができる。
【０１５１】
また、図７のステップＳ１０３において、ユーザが円弧補間経路の円弧性を確保したい点Ｐ１ａ（又は点Ｑ１ａ）をそれぞれ指定（設定）する。教示点計算部３４として機能するＣＰＵ１０１は、その点Ｐ１ａ（又は点Ｑ１ａ）を基に、円弧補間及び関節補間の教示点及び繋ぎ合せ区間を逆算する。
【０１５２】
繋ぎ合せ区間は、円弧補間と関節補間が繋ぎ合わさった経路となるため、純粋な円弧にはならず、ユーザの意図した動きにならない可能性がある。そこで、ユーザが、ここまでは円弧性を確保したいという点を指定する。そこから教示点計算部３４により適切な教示点Ｐ１２（又は教示点Ｑ１２）を演算する。
【０１５３】
つまり、第３実施形態では、円弧性を確保する図１２上の点Ｐ１ａをユーザが指示する。ＣＰＵ１０１は、そこから逆算して、適当なＰ１２とｓ１ｓを求める。具体的には、円弧補間経路が、ｓ１＝ｓ１ｓのときのＴＣＰの位置及び回転ｐ１（ｓ１）が、Ｐ１ａと重なるようにＰ１２、ｓ１ｓを逆算する。
【０１５４】
以上説明したように、第３実施形態によれば、上記第１実施形態の効果に加えて、教示点に付随する補間方法が、円弧補間から関節補間に切り替わる場合にも、経路の切り替え時に停止せずに、ロボット２００の各種物理的制約を守った最適な軌道を生成できる。また、第３実施形態によれば、経路の繋ぎ合せ区間を意識せずに、円弧性が必要な区間をユーザが指定できるため、使用性が上がるという効果がある。
【０１５５】
［第４実施形態］
本発明の第４実施形態に係る軌道生成方法について説明する。なお、軌道生成装置、即ちロボット装置の構成は、上記第１，第２及び第３実施形態と同様であり、説明を省略する。第４実施形態では、上記第１，第２，第３実施形態と異なる補間方法の組み合わせ、即ち直線補間と直線補間の補間方法が指定されている。
【０１５６】
図１５は、第４実施形態におけるタスク空間でのロボットアームの経路を示す説明図であり、図１６は、第４実施形態における関節空間でのロボットアームの経路を示す説明図である。
【０１５７】
第４実施形態では、第１補間方法を第１直線補間、第２補間方法を第２直線補間としたものである。第１直線補間をした時のコンフィグレーションをｑ１（ｓ）とし、第２直線補間をした時のコンフィグレーションをｑ２（ｓ）すると、上記第１実施形態と同様、式（５）により、経路の滑らかさを保った繋ぎ合せが可能となる。式（５）に則り、図１６における、繋ぎ合せ区間での経路上の点ｑ１〜ｑ５を算出する式は、上記第１実施形態と同様、式（６）〜式（１０）になる。
【０１５８】
上記第１実施形態と同様、直線補間した経路は、タスク空間上のＴＣＰの経路となるため、関節空間上で繋ぎ合せるためには、逆キネマティクスを行い、コンフィグレーションを求めて直線補間経路ｑ１（ｓ），ｑ２（ｓ−ｓ１ｓ）を求める。
【０１５９】
以上を考慮にいれ、媒介変数ｓからコンフィグレーションｑ（ｓ）を求める流れを説明する。図１７は、媒介変数ｓを入力してコンフィグレーションｑ（ｓ）を出力する経路生成処理のフローチャートである。なお、第４実施形態では、第１補間方法がタスク空間で補間経路を生成する方法であるため、求めた第１補間経路である直線補間経路を逆キネマティクス計算により関節空間における経路に変換する点で、上記第１実施形態と異なる。それ以外は、上記第１実施形態と同様であるため、以下、経路生成処理に関する各工程を簡略して説明する。
【０１６０】
まずＣＰＵ１０１は、媒介変数ｓを取得（入力）する（Ｓ５０１）。ＣＰＵ１０１は、入力された媒介変数ｓの区間で場合分けを行う（Ｓ５０２）。０≦ｓ＜ｓ１ｓの場合は、第１直線補間区間、ｓ１ｓ≦ｓ＜１の場合は、第１直線補間と第２直線補間の繋ぎ合せ区間、１≦ｓ＜１＋ｓ１ｓの場合は、第２直線補間区間である。
【０１６１】
ＣＰＵ１０１は、ステップＳ５０２の判別で０≦ｓ＜ｓ１ｓの場合、教示点Ｐ１１，Ｐ１２によりＴＣＰを直線補間し、ｐ１（ｓ）を求める（Ｓ５０３）。ＣＰＵ１０１は、ｐ１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ１（ｓ）を求める（Ｓ５０４）。０≦ｓ＜ｓ１ｓの区間では、第１局所経路ｑ１（ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ１（ｓ）となる。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ５０５）。
【０１６２】
次に、ＣＰＵ１０１は、ステップＳ５０２の判別でｓ１ｓ≦ｓ＜１の場合、まず教示点Ｐ１１，Ｐ１２によりＴＣＰを直線補間し、ｐ１（ｓ）を求める（Ｓ５０６）。ＣＰＵ１０１は、ｐ１（ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ１（ｓ）を求める（Ｓ５０７）。
【０１６３】
ＣＰＵ１０１は、教示点Ｐ２１，Ｐ２２によりＴＣＰを直線補間し、ｐ２（ｓ−ｓ１ｓ）を求める（Ｓ５０８）。ＣＰＵ１０１は、ｐ２（ｓ−ｓ１ｓ）に対し、逆キネマティクス計算を行い、コンフィグレーションｑ２（ｓ−ｓ１ｓ）を求める（Ｓ５０９）。
【０１６４】
ＣＰＵ１０１は、直線補間したコンフィグレーションｑ１（ｓ）に重みづけするための重み関数ｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））を求める（Ｓ５１０）。ＣＰＵ１０１は、直線補間したコンフィグレーションｑ２（ｓ）に重みづけするための重み関数ｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）を求める（Ｓ５１１）。
【０１６５】
ＣＰＵ１０１は、ｑ１（ｓ）にｆｓｉｇ１（（ｓ−ｓ１ｓ）／（１−ｓ１ｓ））を掛け、ｑ２（ｓ−ｓ１ｓ）にｆｓｉｇ２（（ｓ−ｓ１ｓ）／ｓ２ｅ）を掛けて和をとって、ｑ（ｓ）を求める（Ｓ５１２）。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ５１３）。
【０１６６】
ＣＰＵ１０１は、ステップＳ５０２の判別で１≦ｓ＜１＋ｓ１ｓの場合、教示点Ｐ２１，Ｐ２２によりＴＣＰを直線補間し、ｐ２（ｓ−ｓ１ｓ）を求める（Ｓ５１４）。ＣＰＵ１０１は、ｐ２（ｓ−ｓ１ｓ）に対し、逆キネマティクス計算を行い、関節空間上のコンフィグレーションであるｑ２（ｓ−ｓ１ｓ）を求める（Ｓ５１５）。１≦ｓ＜１＋ｓ１ｓの区間では、第２局所経路ｑ２（ｓ−ｓ１ｓ）が、ロボット２００の経路であり、ｑ（ｓ）＝ｑ２（ｓ−ｓ１ｓ）である。ＣＰＵ１０１は、コンフィグレーションｑ（ｓ）を出力する（Ｓ５１６）。
【０１６７】
第４実施形態によれば、上記第１実施形態の効果に加えて、教示点に付随する補間方法が、第１直線補間から第２直線補間に切り替わる場合にも、経路の切り替え時に停止せずに、ロボット２００の各種物理的制約を守った最適な軌道を生成できる。これは、背景技術で述べた直線補間から直線補間に移る手法と比較して、各種物理的制約を守った最適な軌道を生成できるという点で有利である。また、第４実施形態によれば、経路の繋ぎ合せ区間を意識せずに、直進性が必要な区間をユーザが指定できるため、使用性が上がるという効果がある。
【０１６８】
なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。
【０１６９】
なお、上記実施形態では、関節空間での繋ぎ合せで説明したが、それに限定されるものではなく、タスク空間で同様の繋ぎ合せを行ってもよい。例えば、ＴＣＰの位置及び回転に対し、同様の重み関数を用いて経路の繋ぎ合せを行ってもよい。この場合、最終的には、タスク空間上の経路を、関節空間上の経路に変換して、軌道を生成すればよい。
【０１７０】
また、上記実施形態においては、軌道生成装置１００が１つのコンピュータにより構成される場合について説明したが、複数のコンピュータにより構成される場合についても本発明は適用可能である。この場合、複数のＣＰＵで本発明の演算部が構成されていてもよい。また、軌道生成装置１００とロボットコントローラ３００とが別のコンピュータで構成される場合について説明したが、１つのコンピュータで構成されていてもよい。この場合、該コンピュータのＣＰＵが、ロボットの経路を生成して、生成した経路に従ってロボットの動作を制御することとなる。
【０１７１】
上記実施形態の各処理動作は具体的にはＣＰＵ１０１により実行されるものである。従って上述した機能を実現するプログラムを記録した記録媒体を軌道生成装置１００に供給し、軌道生成装置１００を構成するコンピュータ（ＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。
【０１７２】
また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ１０４であり、ＨＤＤ１０４にプログラム１４０が格納される場合について説明したが、これに限定するものではない。プログラム１４０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図３に示すＲＯＭ１０２や外部記憶装置６００、記録ディスク１４１等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることができる。また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。
【０１７３】
また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。
【０１７４】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。
【０１７５】
また、上記実施形態では、コンピュータが記録媒体や記憶装置に記録されたプログラムを実行することにより、処理を行う場合について説明したが、これに限定するものではない。プログラムに基づいて動作する演算部の一部又は全部の機能をＡＳＩＣやＦＰＧＡ等の専用ＬＳＩで構成してもよい。なお、ＡＳＩＣはApplication  Specific  Integrated  Circuit、ＦＰＧＡはField-Programmable  Gate  Arrayの頭字語である。
【符号の説明】
【０１７６】
１００…軌道生成装置、１０１…ＣＰＵ（演算部）、１４０…プログラム、２００…ロボット、５００…ロボット装置、Ｐ１１，Ｑ１１…第１教示点、Ｐ１２，Ｑ１２…第１補助点、Ｐ１ａ，Ｑ１ａ…第１途中点、Ｐ２１，Ｑ２１…第２補助点、Ｐ２２，Ｑ２２…第２教示点、Ｐ２ａ，Ｑ２ａ…第２途中点、ｓ…媒介変数
【図１】
0006501470.tif 000010
【図２】
0006501470.tif 000011
【図３】
0006501470.tif 000012
【図４】
0006501470.tif 000013
【図５】
0006501470.tif 000014
【図６】
0006501470.tif 000015
【図７】
0006501470.tif 000016
【図８】
0006501470.tif 000017
【図９】
0006501470.tif 000018
【図１０】
0006501470.tif 000019
【図１１】
0006501470.tif 000020
【図１２】
0006501470.tif 000021
【図１３】
0006501470.tif 000022
【図１４】
0006501470.tif 000023
【図１５】
0006501470.tif 000024
【図１６】
0006501470.tif 000025
【図１７】
0006501470.tif 000026