JP2004252814A - 多関節型ロボットの速度制御法および制御装置 - Google Patents
多関節型ロボットの速度制御法および制御装置 Download PDFInfo
- Publication number
- JP2004252814A JP2004252814A JP2003043686A JP2003043686A JP2004252814A JP 2004252814 A JP2004252814 A JP 2004252814A JP 2003043686 A JP2003043686 A JP 2003043686A JP 2003043686 A JP2003043686 A JP 2003043686A JP 2004252814 A JP2004252814 A JP 2004252814A
- Authority
- JP
- Japan
- Prior art keywords
- interpolation
- speed
- joint
- point
- section
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
【課題】移動区間ごとに記録された補間種別が異なる経路の継ぎ目（教示点）で、ＴＣＰ速度に非連続性が生じないようにして動作の機敏化、ロボットアームへの加振抑制、作業中のぶれ防止等を図った多関節型ロボットの速度制御法を提供すること。
【解決手段】あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別し、その２つの移動区間の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定し（速度結合処理可否判定部２）、速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせるような挙動をさせるための補間点を関節変数により決定する（速度結合演算部３）ようにしている。
【選択図】 図１
【解決手段】あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別し、その２つの移動区間の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定し（速度結合処理可否判定部２）、速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせるような挙動をさせるための補間点を関節変数により決定する（速度結合演算部３）ようにしている。
【選択図】 図１
Description
【０００１】
【発明の属する技術分野】
本発明は多関節型ロボットの速度制御法および制御装置に係り、詳しくは、教示点から次の教示点までの区間に幾つかの補間点を与え、移動区間ごとに予め計算されている個々の速度関数に基づき関節補間動作や直線補間動作によって移動させようとするとき、あい前後する移動区間の補間種別が異なる場合でも、前区間の減速域と後区間の加速域における速度の結合処理が可能となるようにした制御方法および装置に関するものである。
【０００２】
【従来の技術】
例えば６本のアームをそれぞれ連結する軸部に各１つのモータが装着され、ロボット先端に取りつけられたトーチなどのツールもしくは溶接が施されるワークピース（これらエンドエフェクタの中心を以下ＴＣＰという）を動かすなどする多関節型ロボットは、一般にティーチングプレイバック方式で動作が再現され、予め教示したとおりの経路をロボットがたどるようになっている。
【０００３】
３点以上の教示点からなる経路で多関節型ロボットを動作させる場合、前後２つの教示点で形成される１つの移動区間においては、その途中に幾つもの補間点を与えて指定した移動速度を満たす例えば台形波パターンに従う速度でもって移動される。次の移動区間においてもそれにふさわしい台形状に変化する速度により移動され、これを繰り返してツール等に目標の位置と姿勢が与えられ、ワークピースの何箇所かに溶接等の作業を施すといったことができる。
【０００４】
ところで、補間動作には通常直線補間、円弧補間、関節補間の３種がある。直線補間はＣＰ制御 ｃｏｎｔｉｎｕｏｕｓ ｐａｔｈ ｃｏｎｔｒｏｌと言われ、教示された２点間を最短コースでたどり、すなわち予め記録した速度で目標点に直進させる移動方法である。円弧補間は意図的に２点間の直路から外れた位置に参照ポイントを与え、３点を通る一義的な円弧に沿って予め記録された速度で教示点へ向かわせる方法である。この円弧補間は直線補間と同じ考え方の速度関数を使用するので、直線補間と実質的に変わるところがない。
【０００５】
関節補間はＰＴＰ制御 ｐｏｓｅ−ｔｏ−ｐｏｓｅ ｃｏｎｔｒｏｌ と言われ、移動経路の直線性や円弧性は問わず、目標の関節角度に向かって各軸が原則的に同時に動き出し、同時に目標点に到達する移動方法である。ロボットの性能は関節ごとにどのような容量のモータが搭載されているか、すなわちどれだけ重いアームを動かすことができるかで決まる。ＰＴＰ制御によれば各関節の１００％の動作性能を発揮する制御が容易であり、高速動作が必要な移動区間で多用される。
【０００６】
これによれば教示点から次の教示点へ最短時間で移すことができ、ロボット性能や稼働中の応答性の向上に貢献する。ただし、関節ごとの速度制御となるために、ＴＣＰの軌道や速度はオペレータが簡単に予測できるものでない。ロボットはこのＰＴＰ制御に限ることなく、その移動の都度その間の動作や目的にふさわしい補間方式が採択される。
【０００７】
例えば図２１の（ａ）に示すように、ＴＣＰ（図示せず）が退避位置Ｐ１ からワークピース５１に接近して作業態勢に入るための準備位置Ｐ３ （第１アプローチ点Ｐ２ を経由した第２アプローチ点）まで移動する間は関節補間動作により、作業準備位置Ｐ３ から最初の作業点Ｐ４ へおよび次の作業点Ｐ５ 、それから少し離れた作業解放位置Ｐ６ （第１リターン点）まで移動する間は直線補間動作により、作業解放位置Ｐ６ から退避位置Ｐ７ （第２リターン点＝Ｐ１ ）に戻るまでは関節補間動作により移動させるようにしておくと、ロボットを機敏に動作させかつ作業部位では位置姿勢を高い精度で与えて停止させたり所望作業速度を与えたりすることができる。
【０００８】
補間動作は簡単に説明すると、図２１の（ａ）において点Ｐ２ からＰ３ の移動区間と点Ｐ３ からＰ４ の移動区間のみに幾つかの補間点を付したが、その補間点を以下の要領で定め、それをたどる動作をさせることである。図においては前者の移動区間で関節補間が適用されているが、予め決められた補間周期ごとにその区間に与えられた速度関数に従い補間点が演算される。ｉ番目の補間は点Ｐ２ から出発して補間周期×ｉ＝ＪＴｉ の時間が経過したときの補間点で与えられ、（ｉ＋１）番目の補間は点Ｐ２ から見て補間周期×（ｉ＋１）＝ＪＴｉ＋１ が経過したときの補間点で与えられる。なお、ｉ＝４とした場合について、後述する図２３の（ａ）の左側の台形波パターン（速度関数Ｊ１ｖ２ ）上に補間点をＪθ１２４ としてその関節変数を示した。影の施された面積が教示点Ｐ２ （関節変数はＪ１ｓ２ ）からの移動量Ｊ１ｒ２４であり、時間ＪＴ４ 経過時の補間点の関節変数Ｊθ１２４ はＪ１ｓ２ にＪ１ｒ２４を加えて与えられる。
【０００９】
図２１の（ａ）中の点Ｐ３ からＰ４ の移動区間は直線補間であるが、同様の考え方で点Ｐ３ から見たｉ番目の補間は時間ＬＴｉ 経過したときの補間点で、（ｉ＋１）番目のそれは時間ＬＴｉ＋１ が経過したときの補間点で与えられる。いずれの補間法においてもその移動区間における補間開始点すなわちその移動区間における前側の教示点を基点にして補間点を演算し、その点を実現するようにサーボモータを動かせてＴＣＰを移行させるようにしている。ちなみに、補間周期は例えば５ないし２０ｍｓｅｃ程度で与えられる。
【００１０】
ところで、各移動区間における補間動作は、図２１の（ｂ）に示すように、補間種別に関係なく加速・定速・減速からなる台形波パターンの速度関数で与えられるのが一般的である。例えばどの教示点においても一旦停止して作業させようとする場合には、徐々に速度を上げてまた徐々に速度を下げるようにしておけばＴＣＰの停止位置精度は高くなり、ロボットのアームやＴＣＰに無用の振動を及ぼすことも少なくなる。
【００１１】
しかし、上記した点のうち作業点では高い精度の停止が要求されるが、作業準備位置や作業解放位置は単にその位置もしくはその近傍を通過させるだけでよい場合が多い。作業準備位置近傍での厳格な減速・停止・加速は無用であるだけでなく、この部分での緩慢であったり停滞気味の挙動は幾度となく繰り返されるロボットの反復動作の時間を長引かせることになる。もちろん、作業時間節約のために急減速や急加速させるとすれば、それは振動を助長させるもとになり好ましいことではない。
【００１２】
そこで、ＣＰ動作とＣＰ動作とをポーズ精度 ｐｏｓｅ ａｃｃｕｒａｃｙの高く要求されていない教示点を挟んで行わせる場合、前の移動区間の減速域を後の移動区間の加速域と重ね合わせて、両区間の繋ぎ点もしくはその近傍を前の移動区間の減速中の速度と後の移動区間の加速中の速度との和で通過させれば、次の教示点に到達させる時間を縮めることができる。
【００１３】
このような速度の重ね処理は前の移動区間の速度関数と後の移動区間のそれとが同種である場合、その速度関数自体を重ね合わせることによってある意味では容易に達成される。ＣＰ動作とＣＰ動作の場合には前の並進速度関数と後の並進速度関数とを、前の回転速度関数と後の回転速度関数とを、それぞれ減速域と加速域でそれぞれの全部もしくは一部を重ねればよい。
【００１４】
ＰＴＰ動作は、６軸ロボットであれば図２２に示すように関節（軸）ごとに速度関数が与えられる。この場合でもＰＴＰ動作同士の速度の重ね処理は、第１関節の前の速度関数Ｊ１ｖ２ の減速域と後の速度関数Ｊ１ｖ３ の加速域とを、第２関節の前の速度関数Ｊ２ｖ２ と後の速度関数Ｊ２ｖ３ とをといったように関節ごとを対象にして行われる。従って、６自由度のマニピュレータであれば、６つの組合せ全部で重ねられることになる。
【００１５】
図２３は第１関節のみを対象として表したものであるが、非重ね状態の（ａ）に対して、速度重ね処理における重ね度合いとしては（ｂ）から（ｅ）までのような幾つかが考えられる。ちなみに、（ｃ）は加速域を減速域に含ませた場合、（ｄ）は減速域を後の移動区間の定速域まで侵入させて重ねた場合、（ｅ）は両移動区間を定速域まで侵入させた場合である。
【００１６】
各速度関数を表す線が横軸とで囲む面積はＴＣＰの移動量を表すが、（ｂ）から（ｅ）において途中でＪ１ｖ２−３ をたどらせたとしても、その移動量は（ａ）の場合と同じになることは簡単な幾何学で理解できるところである（図２１の（ｃ）の右部分を例にすると、□ＡＣＥＤ＝△ＡＤＣ＋△ＣＤＥ＝△ＡＢＣ＋△ＣＤＥである）。（ｅ）のように重ね量を多くすれば、移動量が同じであるにもかかわらず、点Ｐ４ への到達時間は時間Ｔｓｅ（＝ＪＴ２２＋ＪＴ１６−ＪＴ３０）も早められることが分かる。
【００１７】
しかし、（ｄ）や（ｅ）のように減速域や加速域を越えてまで重ねることは妥当でない。速度関数Ｊ１ｖ２ によって与えられている定速度が第１関節のサーボモータの例えば許容最高速度である場合には、（ｅ）中に表したＶａ分大きいＪ１ｖ２−３ なる速度をもはや与えることができなくなるからである。これは、（ｄ）の場合に、後の移動区間の定速度が前の移動区間のそれより大きくかつ第１関節のサーボモータの許容最高速度もしくはそれに近い速度で与えられているようなときにも言えることであり、これらは速度の重ね処理に不向きであることに注意すべきである。
【００１８】
ところで、速度の重ね処理をするにおいて無視することができないのは、上でも少し触れたポーズ精度である。図２４の（ａ）は重ね処理をしていない場合であるが、この場合には点Ｐ３ で停止させるので、点Ｐ２ ，Ｐ３ ，Ｐ４ が直線的に並んでいても「く字状」に折れて並んでいても、同じ距離を同じ時間を掛けて移動することになる。もちろんＰ３ での停止位置を忠実に再現させる意味でもポーズ精度は高く与えられる。
【００１９】
一方、速度重ね処理をする場合、図２３の（ｃ）の例を適用すると、図２４の（ｂ）のようになる。速度を重ねるということは点Ｐ３ で停止させないことだけでなく、点Ｐ３ を厳密には通過させなくてもよいことを意味する。図２４の（ａ）は速度を重ねる前の状態を表し、減速域には補間点が８つあって減速時間Ｔｄｅは補間周期ｔｃ ×８、加速域は６つあって加速時間Ｔａｃはｔｃ ×６となっている例で表されている。図２４の（ｂ）は補間点が６つある加速域全部を、補間点８つある減速域の後半の６つの部分の領域に重ねた形態となっている。
【００２０】
この場合、前後の移動区間はいずれも関節補間であるので、その速度関数Ｊ１−６ｖ２ とＪ１−６ｖ３ とを数学的に重ねることができる。重ねる前の図２４の（ａ）では、Ｊ１−６ｖ２ とＪ１−６ｖ３ とが速度０を挟んで繋がった速度関数となって補間数２２個（ＪＴ２２）と１６個（ＪＴ１６）からなる２つの移動区間を纏めた点Ｐ２ ・Ｐ４ 間に適用されていると見るなら、重ねた場合には、図２４の（ｂ）のように、Ｊ１−６ｖ２ とＪ１−６ｖ２−３ とＪ１−６ｖ３ とが繋がった速度関数が補間数３２個分の移動区間域に割り当てられていると見ることができる。なお、添え字のうちの一部の１−６ は、６軸分を一括表記したことを意味している。
【００２１】
その結果、この（ｂ）での重ね部の移動時間は、図２４の（ａ）に影をつけて表した時間ＴＲ１２ の半分であるＴＲ６となる。一方、速度重ね処理は前の移動区間の終り部分の６補間分と後の移動区間の始め部分の６補間分であって移動距離は（ａ）も（ｂ）も変わるものでない。とすると、点Ｐ２ ，Ｐ３ ，Ｐ４ が直線的に並んでいる場合、Ｐ３ のポーズ精度が低くてもＴＣＰはＰ３ を通過することになる。
【００２２】
一方、Ｐ３ で「く字状」に折れている経路の場合、（ｂ）の軌道では図から明らかなように曲線を描かざるを得なく、点Ｐ３ をかすめた点Ｐ３ａを通過することになる。移動距離が同じであるにもかかわらずその軌道が内のり軌跡で描かれているのは、６軸ロボットの場合ならその全軸の動きを考慮したものであるからである。すなわち、Ｐ３ で「く字状」に折れるということは６つのうちの幾つかの軸が負の速度を持っているはずであり、それゆえポーズ精度内で点Ｐ３ からΔＰずれた点Ｐ３ａが通過点となる。
【００２３】
この曲線軌跡は（ｂ）の右下部に表したように重ね処理部の補間数が少なくなればなるほど小さくなることは容易に理解でき、通過点Ｐ３ａは教示点Ｐ３ に近接する。しかし、その分、重ねは速度の低いところでしか処理できないことになるから通過速度も低いものにとどまる。これは常識で想定される人間や車の動作と何ら変わりがない。逆に、重ね処理部の補間数が多くなれば軌跡の孤は大きくなり、通過点は教示点Ｐ３ からますます離反する。その分、重ねは速度の高いところで可能となり、ロボットは機敏に動く。
【００２４】
ところで、特公平１ー２７４４３号公報には、２つの移動区間がＶ字状をなす経路を形成している場合に、折り返し点での速度が低下しないようにした例が記載されている。これは、正しく上記した図２４の（ｂ）で説明したものであり、速度重ね処理によって簡単に言えば結果的には前後２つの速度関数を１つの速度関数に作り変えたかのように見ることができる。この考え方は当該公報の発行により公知になっているが、あい前後する２つの移動区間における補間種別は直線補間動作であり、それ以上の説明は見当たらない。
【００２５】
【特許文献１】
特公平１ー２７４４３号公報
【００２６】
【発明が解決しようとする課題】
しかし、図２１の（ａ）で示した第１アプローチ点Ｐ２ から第２アプローチ点Ｐ３ までは関節補間動作で、第２アプローチ点Ｐ３ から最初の作業点Ｐ４ へ向かうまでが直線補間動作である場合、前者の各関節の速度関数と後者の並進速度関数や回転速度関数とは異質なものであり、重ねることができない。また前者は軸（関節）の数だけ速度関数が存在するが、後者は並進と回転の２つの動作しかなく、数のうえからも合わせようがない。
【００２７】
関節補間は各軸の動作量から求められる移動量だけを扱うものであり、直線補間は同次変換行列でＴＣＰの並進と回転の量だけを扱う。従って、演算対象が全く異なる。前者は各軸の関節の運動であり、後者はＴＣＰの運動であるからであるが、いずれにしても、移動区間の前後で補間種別が異なる場合には速度関数を重ねるという概念の導入は不可能であり、繋ぎ点前後で減速・停止・加速の動作は依然として余儀なくされる。
【００２８】
図２１の例で言えば、点Ｐ２ から点Ｐ３ へはＴＣＰが一気に進出してくるが、点Ｐ３ では一旦停止してから点Ｐ４ に向かうことになり、ＴＣＰの滑らかな移動は期待し得ない。また、点Ｐ５ から点Ｐ６ へ移動すると、そこでもやはり一旦停止してから点Ｐ７ に向かう。演算上の事情を知らない人の目には、ＴＣＰがワークピース５１の前後各１箇所でぎこちない動きをしているように映る。
【００２９】
一旦静止するということは無用な力（慣性力）をアームに及ぼすことになるから動き続けさせた方が、しかもそれが滑らかなものにできれば、ロボットに無用な衝撃を与えなく、振動発生の原因排除もしやすくなる。これが可能になれば振動低減の意味からも性能向上の点からも極めて有用なロボットとなるわけで、このような滑らかで迅速な動作をすることができる優れたロボットの出現が望まれる。
【００３０】
本発明は上記の問題に鑑みなされたもので、その目的は、移動区間ごとに記録された補間種別が異なる経路の継ぎ目（教示点）で、ＴＣＰ速度に非連続性が生じないようにすること、速度の連続性を高めると共に速度性能を上げて動作の機敏化、ロボットアームへの加振抑制、作業中のぶれ防止等を実現し、それによって作業能率の向上を目指すことができる多関節型ロボットの速度制御法および制御装置を提供することである。
【００３１】
【課題を解決するための手段】
本発明は、教示点から次の教示点までの区間に幾つかの補間点を与え、移動区間ごとに演算された個々の速度関数に従って関節補間動作や直線補間動作により移動させるようにした多関節型ロボットの速度制御装置に適用される。その特徴とするところは、図１を参照して、あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別すると共に、その２つの移動区間の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定する速度結合処理可否判定部２と、その速度結合処理可否判定部２により速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせたように挙動させるための補間点を演算する速度結合演算部３とを具備したことである。
【００３２】
制御方法の発明は、あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別し、その２つの移動区間の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定し、速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせるような挙動をさせるべく関節変数を決定するようにしたことである。
【００３３】
異種補間動作が関節補間から直線補間に移る場合の速度結合は、現移動区間における関節補間動作の現時刻に達成されるロボット全軸の関節変数Ｊθ１ｊｉ 〜Ｊθ６ｊｉ を算出し、これを順演算して得られるロボット基準座標系から見たＴＣＰの同次変換行列を直線補間動作する次の移動区間の始点と見たてる。その直線補間動作の現時刻で達成される並進速度関数Ｌｖｐｊ＋１ と回転速度関数Ｌｖｆｊ＋１ から算出した並進運動量Ｌｐ（ｊ＋１）ｉと回転運動量Ｌｆ（ｊ＋１）ｉにより、始点とされた同次変換行列ＪＸ（ｊ＋１） から見た同次変換行列ＬＡ（ｊ＋１）ｉを算出し、始点を表す同次変換行列ＪＸ（ｊ＋１） に並進運動量Ｌｐ（ｊ＋１）ｉと回転運動量Ｌｆ（ｊ＋１）ｉとから算出された同次変換行列ＬＡ（ｊ＋１）ｉを乗じて、１補間周期の間に補間動作させたときの到達点のロボット基準座標系から見た同次変換行列ＬＸ（ｊ＋１）ｉを算出する。これを逆演算して関節変数Ｌθ１（ｊ＋１）ｉ 〜Ｌθ６（ｊ＋１）ｉ を求め、これによって関節補間による補間量に直線補間による補間量を加えた移動量を得ることにより処理するようにしたことである。
【００３４】
異種補間動作が直線補間から関節補間に移る場合の速度結合は、現移動区間における直線補間動作の現時刻に到達するＴＣＰのロボット基準座標系から見た同次変換行列ＬＸｊｉを算出し、これを逆演算して得られる関節変数を関節補間動作する次の移動区間の始点と見たてる。その関節補間動作の全軸の速度関数Ｊ１ｖｊ＋１ 〜Ｊ６ｖｊ＋１ をもとに現時刻で補間動作を実行したときの各軸の補間移動量Ｊ１ｒ（ｊ＋１）ｉ〜Ｊ６ｒ（ｊ＋１）ｉを算出し、始点を表す全軸の関節変数Ｌθ１（ｊ＋１）ｉ 〜Ｌθ６（ｊ＋１）ｉ に補間動作を実行したときの各軸の補間移動量Ｊ１ｒ（ｊ＋１）ｉ〜Ｊ６ｒ（ｊ＋１）ｉを加算して、１補間周期の間に補間動作させたときのロボットの全軸の関節変数Ｊθ１（ｊ＋１）ｉ 〜Ｊθ６（ｊ＋１）ｉ を算出する。これによって直線補間による補間量に関節補間による補間量を加えた移動量を得ることにより処理される。
【００３５】
速度の結合処理の可否は、現移動区間において減速状態にあり、かつその移動区間における残存減速時間が次の移動区間の加速時間以下となっているかどうかで判定するようにしたことである。
【００３６】
異種補間動作における速度の結合処理が適用される速度関数は、現移動区間の減速時間と次の移動区間の加速時間とを一致させておくとよい。次の移動区間の加速時間が現移動区間の減速時間よりも短い場合には、次の移動区間の加速時間を減速時間に一致させ、現移動区間の減速時間が次の移動区間の加速時間よりも短い場合は、現移動区間の減速時間を加速時間に一致させればよい。
【００３７】
【発明の実施の形態】
以下に、本発明に係る多関節型ロボットの速度制御法および制御装置を、図面に基づいて詳細に説明する。図１は速度制御装置１を含む再生動作ならびに速度制御のための構成を示し、図２は制御の概略を表したフローチャートである。これは、教示点から次の教示点までの区間に幾つかの補間点を与え、移動区間ごとに演算された個々の速度関数に従って関節補間動作または直線補間動作により移動させる際に、異なる補間種別の繋ぎ部分で速度の結合処理を可能にしたものである。
【００３８】
図１において装置の主たるところを概略的に述べる。産業用ロボットの教示されたプログラムの再生動作中に、異なる補間種別で教示された動作の繋ぎ目で、ＴＣＰの動きを止めることなく制御できるようにするため、速度結合処理への移行が可能であるかどうかを判定する手段が備えられる。これは速度結合処理可否判定部２として表され、現在解析中の移動区間における補間動作が減速域で行われており、かつ現解析中の補間動作の残り時間が次の解析対象区間の補間動作の加速時間以下であることを確認する機能を有する。
【００３９】
すなわち、あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に、または直線補間動作から関節補間動作に移り変わるかどうかを判別すると共に、その２つの移動区間の繋ぎ点を通過させる際に速度の結合処理が可能かどうかを判定するようになっている。そして、この速度結合処理可否判定部２により速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせたように挙動させることができるようにするための速度結合演算部３が設けられる。
【００４０】
この速度結合演算部３を含む軌道解析部４は、速度結合処理可否判定部２からの速度の結合処理が可能であるとの信号を受けると、補間動作演算部５における今までの補間種別による補間のための演算に加えて、速度結合演算部３において次の移動区間のための異なる補間種別を絡めた補間演算も同時に実行する。
【００４１】
例えば、関節補間動作から直線補間動作に移行する際の速度結合処理においては、現軌道での関節補間中の現時刻における関節補間点を求め、次の軌道の直線補間を開始する時点の同時変換行列には、今求められた関節補間点を順キネマティクスされたものを使用し、重ね合わせることができない速度関数を直接突き合わせることなく、ＴＣＰの移動量の積み重ねにより速度の結合を実現する。
【００４２】
一方、直線補間動作から関節補間動作に移行するときの速度結合処理においては、現軌道での直線補間中の現時刻において到達したＴＣＰの同次変換行列を求め、次の軌道の関節補間を開始する時点の関節変数には、今求められた同次変換行列を逆キネマティクスしたものを使用し、関節変数の積み重ねにより速度の結合を実現する。これもまた、速度関数を重ね合わせるものではなく、ＴＣＰの移動量の積み重ねにより速度の結合を果たす。いずれの場合も、異種補間動作間の速度を重ね合わせたように挙動させるための補間点を演算するという制御をするようになっている。
【００４３】
ちなみに、一般的な産業用ロボットの制御装置はティーチングプレイバック方式であり、オペレータはティーチペンダントを使用して、所望する作業をロボットコントローラに教示作業プログラムとして記録し、またオペレータからの起動指示信号を受けて、記録済みの教示作業プログラムが再生動作されるようになっている。
【００４４】
ロボットコントローラは、教示作業プログラムを記録するためのハードディスク、操作者が教示作業プログラムを教示するためのティーチペンダント、オペレータが起動指示信号を発信するための操作ボックス、ロボットの手動運転や再生運転を制御するロボット制御プログラム、ロボット制御プログラムが格納されているＲＯＭ、ロボット制御プログラムが実行されるＣＰＵ、ロボット制御プログラム実行中に計算の途中経過を一時的に格納するためのＲＡＭなどから構成される。ロボットの各関節最高速度やロボットアームの長さ、補間周期といった諸元もロボットコントローラ内のＲＯＭに格納される。
【００４５】
ここで、ロボット再生動作制御を簡単に触れる。図２に示すように、ハードディスクから読み出された教示作業プログラムよりロボットの補間動作命令が取り出され、補間種別と動作速度ならびに目標の位置を表すためのロボット全軸の関節角度が、先入れ・先出し（ＦＩＦＯ）処理式の動作命令バッファに格納される（ステップ１、以下Ｓ１などと記す）。動作命令バッファから動作コマンドを取り出した軌道生成部６（図１を参照）は、ロボットの動作コマンドのパラメータとして格納されている関節目標点、動作速度および補間種別、さらには必要に応じて現在関節角度をも考慮し、それらから補間開始点の同時変換行列や各移動区間における速度関数を生成し、前記同様ＦＩＦＯ形式の軌道バッファ７に格納する（Ｓ２）。
【００４６】
本発明は、産業用ロボットの教示作業プログラム再生動作中の速度を、異なる補間種別間であっても途切らせることなく滑らかに変化させること、すなわち教示点の前後で補間種別が違う場合でもＴＣＰ速度に非連続性が発生しないようにするものである。これはロボットの再生動作制御の一機能として実行されるが、以下には特に関連するところにとどめ、すでに知られた再生動作の詳しいアルゴリズムの説明は省く。
【００４７】
軌道解析部４は、軌道バッファ７から軌道データを取り出し、これから補間周期（例えば５ｍｓｅｃ）ごとのサーボドライバの目標関節値となる関節変数を生成する。すなわち、本発明による異種補間動作の速度結合処理がここで実行される（Ｓ４〜Ｓ１０）。なお、サーボ出力部８は補間周期ごとに発せられるトリガ信号によって起動され、サーボドライバ９１ ，９２ ，・・・，９６ に対して一定周期で指令値の更新を実行し、サーボモータ１０１ ，１０２ ，・・・，〜１０６ を駆動して各軸を動かし、アームを介してＴＣＰを変位させる。
【００４８】
もう少し軌道解析部４を詳しく述べる。図１に示すように、これは前記した速度結合処理可否判定部２，速度結合演算部３のほかに、補間種別判定部１１や関節補間動作演算部５Ａ、直線補間動作演算部５Ｂを備える。そして、速度結合演算部３は、関節補間から直線補間への速度結合演算部３Ａと、直線補間から関節補間への速度結合演算部３Ｂを有する。速度結合演算部３での演算結果は関節補間点バッファ１２に格納され、サーボ出力部８からの取り出し指令を受けてサーボドライバ９１ ，９２ ，・・・，９６ に送り出されるようになっている。
【００４９】
上記した関節補間から直線補間への速度結合演算部３Ａは、速度結合処理可否判定部２で現在の移動区間において補間動作が減速域に入っており、かつその移動区間における残存減速時間が次の移動区間の加速時間以下となっていると判定されたとき、速度の結合処理を実行する。
【００５０】
速度結合処理可否判定部２が速度結合処理可能判定を出すまでは、関節補間動作演算部５Ａが補間のための関節変数を算出し、それを関節補間点バッファ１２へ出力する。しかし、速度結合処理可否判定部２が速度結合処理可能判定を出せば、速度結合演算部３Ａは関節補間動作演算部５Ａで得た関節変数を取り入れると共に直線補間動作演算部５Ｂからも次の移動区間のための軌道データを取り込む。現移動区間での関節補間中の現時刻における関節補間点を求め、次の移動区間の直線補間を開始する時点の同時変換行列にはその関節補間点を順キネマティクスして求めたものを使用し、結果として得られる関節変数を関節補間点バッファ１２へ送り出す。
【００５１】
フローチャートを参照しながら後で詳しく述べるが、以下の処理がその根幹をなす。まず、現移動区間における関節補間動作の現時刻に達成されるロボット全軸の関節変数Ｊθ１ｊｉ 〜Ｊθ６ｊｉ が算出される。これを順演算して得られるロボット基準座標系から見たＴＣＰの同次変換行列を直線補間動作する次の移動区間の始点と見たてることにして、その直線補間動作の現時刻で達成される並進速度関数Ｌｖｐｊ＋１ と回転速度関数Ｌｖｆｊ＋１ から算出した並進運動量Ｌｐ（ｊ＋１）ｉと回転運動量Ｌｆ（ｊ＋１）ｉにより、始点とされた同次変換行列ＪＸ（ｊ＋１） から見た同次変換行列ＬＡ（ｊ＋１）ｉが算出される。始点を表す同次変換行列ＪＸ（ｊ＋１） に並進運動量Ｌｐ（ｊ＋１）ｉと回転運動量Ｌｆ（ｊ＋１）ｉとから算出された同次変換行列ＬＡ（ｊ＋１）ｉを乗じて、１補間周期の間に補間動作させたときの到達点のロボット基準座標系から見た同次変換行列ＬＸ（ｊ＋１）ｉが算出されれば、これを逆演算して関節変数Ｌθ１（ｊ＋１）ｉ 〜Ｌθ６（ｊ＋１）ｉ を求める。これによって関節補間による補間量に直線補間による補間量を加えた移動量を得ることにより、速度結合処理が行われることになる。
【００５２】
上記した直線補間から関節補間への速度結合演算部３Ｂも、速度結合処理可否判定部２で現在の移動区間において減速状態にあり、かつその移動区間における残存減速時間が次の移動区間の加速時間以下となっていると判定されると、速度の結合処理を実行する。
【００５３】
速度結合処理可否判定部２が速度結合処理可能判定を出すまでは、直線補間動作演算部５Ｂが補間のための関節変数を算出し、それを関節補間点バッファ１２に出力する。しかし、速度結合処理可否判定部２が速度結合処理可能判定を出せば、速度結合演算部３Ｂは直線補間動作演算部５Ｂで得た関節変数を取り入れると共に関節補間動作演算部５Ａからも次の移動区間のための軌道データを取り込む。現移動区間での直線補間中の現時刻において到達したＴＣＰの同次変換行列を求め、次の移動区間の関節補間を開始する時点の関節変数にはその同次変換行列を逆キネマティクスして求めたものを使用し、結果として得られる関節変数を関節補間点バッファ１２へ送り出す。
【００５４】
この場合の処理は、以下の点が主たるところである。まず、現移動区間における直線補間動作の現時刻に到達するＴＣＰのロボット基準座標系から見た同次変換行列ＬＸｊｉが算出される。これを逆演算して得られる関節変数を関節補間動作する次の移動区間の始点と見たて、その関節補間動作の全軸の速度関数Ｊ１ｖｊ＋１ 〜Ｊ６ｖｊ＋１ をもとに現時刻で補間動作を実行したときの各軸の補間移動量Ｊ１ｒ（ｊ＋１）ｉ〜Ｊ６ｒ（ｊ＋１）ｉが算出される。始点を表す全軸の関節変数Ｌθ１（ｊ＋１）ｉ 〜Ｌθ６（ｊ＋１）ｉ に補間動作を実行したときの各軸の補間移動量Ｊ１ｒ（ｊ＋１）ｉ〜Ｊ６ｒ（ｊ＋１）ｉを加算して、１補間周期の間に補間動作させたときのロボットの全軸の関節変数Ｊθ１（ｊ＋１）ｉ 〜Ｊθ６（ｊ＋１）ｉ が求められる。これによって、直線補間による補間量に関節補間による補間量を加えた移動量が得られることになる。
【００５５】
以下に図４からのフローチャートを参照し、異種補間動作時の速度結合に関して具体的に説明する。なお、関節補間動作から直線補間動作への乗り移りによる速度結合処理や直線補間動作から関節補間動作への乗り移りによる処理をする場合の特徴を把握しやすくするため、速度結合処理しない場合も併せて述べる。その説明順序は次に掲げる図３の（ｂ）のタイムチャートに従う。また、後で述べる事項との関連も予めとっておく都合上、速度結合処理前になされる軌道データの生成についても触れる。
【００５６】
まず、従来技術の項で述べた図２１の（ａ）の動きを想定したタイムチャートは図２１の（ｂ）であるが、本発明は（ｃ）にあるように、一連の速度関数のうちの２か所に見られる速度の結合処理を行う。図３の（ａ）は図２１の（ｂ）の順序を一部入れ替えてフローチャートの説明順に合わせたものである。この一連の動きにおいて、本発明に係る速度結合処理を行わせようとしたものが図３の（ｂ）である。
【００５７】
その速度結合処理は、関節補間動作から直線補間動作へ移るところとその逆の移り変わりをするところの２か所に現れている。図中に付されたＳ番号はフローチャートに付したステップ番号である。その番号群は、例えば「関節補間動作」から「直線補間動作」へ移るところの「速度結合処理」とその前後を含む動作がフローチャートのどの部分に当たるかを示している。
【００５８】
この再現動作では、ロボットによる作業点をＰ３ とＰ５ の２点としている。まず、退避位置Ｐ１ から作業第１準備位置Ｐ２ までは、関節補間動作により迅速にワークピースへ接近させる。点Ｐ２ で一旦停止させた後、第１作業点Ｐ３ に向けては直線補間動作によって前進させる。この点Ｐ３ へは高いポーズ精度でもって到達させ、ここでも停止させる。
【００５９】
第１作業点Ｐ３ から例えば別のワークピースのために設けた作業第２準備位置Ｐ４ までは関節補間動作により迅速に移動させ、その点では静止させることなく直線補間動作によって第２作業点Ｐ５ に向けて前進させる。この点Ｐ５ では高いポーズ精度でもって静止させる。点Ｐ５ からは直線補間動作で作業解放位置Ｐ６ へ移動させ、そこでは停止させることなく関節補間動作によって迅速に退避位置Ｐ１ （＝Ｐ７ ）へ戻すというプロセスとなっている。
【００６０】
以下、フローチャートに基づいて詳細に説明する。まず、再現動作するにあたり教示作業プログラムが読み出される（Ｓ１０１）。このときカウンタｊが０にセットされる（Ｓ１０２）。このカウンタｊは移動区間の順番を表すものである。教示作業プログラムには、移動中にたどる各教示点Ｐｊ （１≦ｊ≦Ｎ）での全軸の関節角度θ１ｊ〜θ６ｊ、関節補間される各移動区間（例えば教示点Ｐｊ ・Ｐｊ＋１ 間）での全軸の動作速度と直線補間される他の各移動区間でのＴＣＰの並進速度といった動作速度ｖ１ｊ〜ｖ６ｊ、サーボモータの仕様やアームの重量によって決まる各軸での許容最大速度ならびに各軸に適用される加速度・減速度、各移動区間での補間種別、各教示点でのポーズ精度等が含まれており、これらが動作命令バッファに格納される（Ｓ１０３）。なお、上記のＮは図３（ｂ）の例で言えば７に当たり、教示点Ｐ１ ，Ｐ７ はロボットの退避位置を意味する。
【００６１】
これらを基にして軌道生成が行われるが、それは点Ｐ１ からＰ２ である最初の移動区間から始められる。そのため移動区間の番号を表すべくカウンタｊに１が加えられ（Ｓ１０４）、始点Ｐｊ （＝Ｐ１ ）と終点Ｐｊ＋１ （＝Ｐ２ ）からなる移動区間の補間種別が「関節補間」かどうか問われる（Ｓ１０５）。「関節補間」であればステップ２０１へ移行し、「直線補間」であれば（Ｓ１０６）、図９のステップ２０６へ移行される。いずれでもなければ「円弧補間」とみなして、そのための処理が実行される（Ｓ１０７）。なお、上でも少し触れたが円弧補間は実質的に直線補間と同じであるので、以後の直線補間の説明でもってそれに充てることにし、その説明を省く。
【００６２】
ステップ２０１へ進むと、軌道生成部が動作命令バッファから動作命令を取り出し、教示データを解析してロボットの軌道が生成される。すなわち、１番目の教示点における全軸の関節角度θ１ １ 〜θ６ １ （＝θ１ ｊ 〜θ６ ｊ ＠ｊ＝１）から、その関節変数Ｊ１ｓ１ 〜Ｊ６ｓ１ （＝Ｊ１ｓｊ 〜Ｊ６ｓｊ ＠ｊ＝１）を生成する。ちなみに、６軸ロボットを例にしているので、関節角度はθ１ １ からθ６ １ の６つ設けられる。次に、本移動区間の始点Ｐ１ の関節角度θ１ １ 〜θ６ １ と終点Ｐ２ の関節角度θ１ ２ 〜θ６ ２ （＝θ１ ｊ＋１ 〜θ６ ｊ＋１ ）を比較し、指定された全軸の動作速度ｖ１ １ 〜ｖ６ １ （＝ｖ１ ｊ 〜ｖ６ ｊ ＠ｊ＝１）や各軸の加速度・減速度に基づく本移動区間での全軸の速度関数Ｊ１ｖ１ 〜Ｊ６ｖ１ （＝Ｊ１ｖｊ 〜Ｊ６ｖｊ ＠ｊ＝１）を生成する。さらに、この速度関数Ｊ１ｖ１ 〜Ｊ６ｖ１ から本移動区間における加速時間および減速時間を算出する。最後に、ロボットコントローラ内のＲＯＭに格納されている補間周期ｔｃ を呼び出し、これをもとにこの最初の（ｊ＝１）移動区間での補間回数Ｉ１ も算出される。
【００６３】
このようにして得られた最初の教示点の関節変数、最初の移動区間での速度関数、加速時間・減速時間、補間種別、補間回数、終点である教示点Ｐ２ のポーズ精度等の軌道データが、軌道バッファに格納される（Ｓ２０２）。
【００６４】
２番目の教示点Ｐ２ および２番目の移動区間についての軌道データを求めて軌道バッファに格納すべく、カウンタｊに１が加えられて２とされる（Ｓ２０３）。ｊは６である（Ｎ−１）に到っていなく（Ｓ２０４）、図４のステップ１０５へ戻される。図３の（ｂ）から分かるように、２番目の移動区間は「直線補間」が適用されているから、ステップ１０６を経て図９のステップ２０６からの処理が始められる。
【００６５】
ステップ２０６では軌道生成部が動作命令バッファから動作命令を取り出し、教示データを解析してロボットの軌道を生成する。すなわち、２番目の教示点Ｐ２ における全軸の関節角度θ１ ２ 〜θ６ ２ からその関節変数Ｌ１ｓ２ 〜Ｌ６ｓ２ を生成し、その関節変数を順演算して２番目の教示点の同次変換行列ＬＡ２ が算出される。次に、この移動区間の始点Ｐ２ の同次変換行列ＬＡ２ と終点Ｐ３ の同次変換行列ＬＡ３ 、上記関節角度θ１ ２ 〜θ６ ２ や指定された並進速度ｖ１ ２ 〜ｖ６ ２ さらには各軸の加速度・減速度とから、この移動区間での並進速度関数Ｌｖｐ２ と回転速度関数Ｌｖｆ２ とが計算される。さらに、並進速度関数Ｌｖｐ２ と回転速度関数Ｌｖｆ２ からこの移動区間における加速時間および減速時間を算出する。最後に、補間周期ｔｃ をもとに、本移動区間での補間回数Ｉ２ も算出される。
【００６６】
このようにして得られた２番目の教示点の同時変換行列、この移動区間での並進速度関数・回転速度関数、加速時間・減速時間、補間種別、補間回数、終点である教示点Ｐ３ のポーズ精度等の軌道データが、軌道バッファに格納される（Ｓ２０７）。
【００６７】
３番目の教示点Ｐ３ および３番目の移動区間についての軌道データを求めて軌道バッファに格納すべく、カウンタｊに１が加えられて３とされる（Ｓ２０８）。ｊは６である（Ｎ−１）に到っていなく（Ｓ２０９）、図４のステップ１０５に戻される。図３の（ｂ）から分かるように３番目の移動区間は関節補間が適用されるから、ステップ２０１から前述した処理が繰り返される。このようにしてｊが６となる６番目の移動区間の関節補間のための軌道データが生成され、それが格納されるとステップ２０３または２０８で１が加えられる。これによってｊが６を越えることになるので（Ｓ２０４ またはＳ２０９ ） 、ｊを０にリセットして（Ｓ２０５ またはＳ２１０）、次の処理のための準備が整えられる。
【００６８】
上記したステップ２０２や２０７で格納される軌道データは、全ての移動区間についての全ての補間分となるとかなりの量になる。通常軌道バッファはＦＩＦＯ処理式のものが採用され、順次格納しては残りの演算中に格納中の古いものから順に実際の動作のため次々と引き出されていくようにしていることが多い。しかし、このフローチャートでは全ての移動区間の軌道データが一旦蓄えられるものとして、すなわちバッファに容量の大きいものが採用されているとして、以後の説明を続けることにする。
【００６９】
次の処理であるステップ３０１以降は速度結合処理において特殊な配慮を施す場合に適用されるもので、その説明は後に譲り、ステップ４０１以降の動作を説明する。ここからは、軌道バッファに蓄えられた軌道データをもとに、ロボットを動かすべく補間動作する制御に入る。最初に行われる処理の対象は１番目（ｊ＝１）の移動区間であり、その補間種別が「関節補間」であるので、そのための動作が説明される。
【００７０】
その後、２番目（ｊ＝２）の移動区間における「直線補間」による補間動作が述べられる。ここまでは従来からなされている補間動作である。その後、ｊ＝３の移動区間における「関節補間」の途中までの補間動作処理、その残りの減速域の全部もしくは一部での関節補間動作と４番目の移動区間の「直線補間」の加速域の全部もしくは一部における直線補間動作との重合計算により実行される速度結合処理、および４番目の移動区間の残りの直線補間動作処理からなる一連の処理が説明される。この動作は言うまでもなく本発明の主題とするところである。同様に、ｊ＝５の移動区間における「直線補間」の途中までの補間動作処理、その残りの減速域の全部もしくは一部の直線補間動作と６番目の移動区間の「関節補間」の加速域の全部もしくは一部の関節補間動作との重合計算により実行される速度結合処理、および６番目の移動区間の残りの関節補間動作処理からなる一連の処理が説明される。
【００７１】
先ず図６のステップ４０１において最初の移動区間を処理すべく、ステップ２０５または２１０で０にリセットされているカウンタｊに１が加えられる。これによって移動区間が１番目のものであると特定され、この移動区間での例えば５ｍｓｅｃなる補間周期による補間のための演算が開始される。その最初の補間に入る前にカウンタｉが０と置かれる（Ｓ４０２）。説明を後回しするカウンタｋも０と置かれる（Ｓ４０３）。
【００７２】
補間のための演算には、当然のことながらこの移動区間のための軌道データを取り込む必要がある。そこで、軌道解析部に１番目の移動区間のための解析対象軌道Ｔ１ が取り出されているか確認される（Ｓ４０４）。今は最初であるので軌道バッファより１番目の移動区間のための軌道データが解析対象軌道Ｔ１ として取り出される（Ｓ４０５）。その軌道データから、始点Ｐ１ と終点Ｐ２ からなるこの１番目の移動区間の補間種別は「関節補間」であるかどうか問われる（Ｓ４０６）。「直線補間」なら図１０のステップ６０１へ移行されるが、１番目の移動区間は「関節補間」されるものであるのでステップ５０１へ進む。
【００７３】
１番目の移動区間の始点Ｐ１ における全軸の関節変数Ｊ１ｓ１ 〜Ｊ６ｓ１ が軌道データから取り出される（Ｓ５０１）。この移動区間における全軸での速度関数Ｊ１ｖ１ 〜Ｊ６ｖ１ も軌道データから取り出される（Ｓ５０２）。ここで、１番目の移動区間の最初の補間動作のための演算を始めるべく、カウンタｉに１が加えられて（Ｓ５０３）、１番目の補間のための演算準備に入る。１番目の移動区間における全軸の速度関数Ｊ１ｖ１ 〜Ｊ６ｖ１ をもとにして、最初の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ １ １〜Ｊ６ｒ １ １が算出される（Ｓ５０４）。これは、１番目の補間動作で達成される各軸の移動量を示す。
【００７４】
次に、始点Ｐ１ における全軸の関節変数Ｊ１ｓ１ 〜Ｊ６ｓ１ に、最初の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ １ １〜Ｊ６ｒ １ １を加算して、その補間動作を実行したときのロボットの全軸の関節変数Ｊθ１ １ １ 〜Ｊθ６ １ １ が算出される（Ｓ５０５）。これは、この補間動作で到達すべき全軸の目標関節変数である。そして、これら全軸の関節変数Ｊθ１ １ １ 〜Ｊθ６ １ １ が補間点である各関節の指令値として関節補間点バッファに格納される（Ｓ５０６）。
【００７５】
今は図３の（ｂ）の最初の移動区間における一連の補間動作を説明しているので、「速度結合処理」は必要でなく、後で説明するステップ７０１を素通りしてステップ５０７へ跳ぶ。内蔵タイマ等により教えられる補間周期ごとのトリガ信号に基づき、関節補間点バッファ１２から全軸の関節変数Ｊθ１ １ １ 〜Ｊθ６ １ １ を各軸の指令値にして図１で説明したサーボ出力部８へ送出される。これによって、各関節が１番目の補間目標関節角度θ１ １ １ 〜θ６ １ １ に向けて移動される（Ｓ５０８）。
【００７６】
フローチャートの今の説明では、全軸の関節変数Ｊθ１ １ １ 〜Ｊθ６ １ １ の指令値が関節補間点バッファに格納されると、２番目の補間のための演算が開始される前に、１番目の補間のための関節変数がサーボ出力部へ送出されるように表わした。実際は、関節補間点バッファも先に述べたＦＩＦＯ処理式のものが採用されていることが多く、その場合には、幾つかの補間のための各関節の指令値が蓄積されてから、格納の早い順に補間周期ごとに出力されていくことになる。
【００７７】
上の説明で最初の補間が済んだのでカウンタｉに１が加えられ（Ｓ５０９）、２となったｉでもって、ステップ５１０を経て図６のステップ５０４に戻される。このステップ５０４から５０８までは上記と同じ処理であり、その関節変数Ｊθ１ １ ２ 〜Ｊθ６ １ ２ が出力されると、各関節が２番目の補間目標関節角度θ１ １ ２ 〜θ６ １ ２ に向けて移動される。
【００７８】
順次カウンタｉに１が加えられ（Ｓ５０９）、その値がこの移動区間における補間数のＩ１ に到達するまで繰り返される。ｉがそれを越えた時点で（Ｓ５１０）、１番目の移動区間の移動が完了したと判断され、カウンタｊに１が加えられ（Ｓ５１１）る。ステップ５１２を経て、図６のステップ４０２に戻される。ｉが０にリセットされ、前記したステップ４０４を経てステップ４０６へ進められ、２番目の移動区間が如何なる補間種別であるかが問われる。図３の（ｂ）から分かるように「直線補間動作」させるべき移動区間であるから、図１０のステップ６０１へ移行される。
【００７９】
２番目の移動区間の始点Ｐ２ を表す同次変換行列ＬＡ２ が軌道データから取り出される（Ｓ６０１）。この移動区間における並進速度関数Ｌｖｐ２ と回転速度関数Ｌｖｆ２ も軌道データから取り出される（Ｓ６０２）。０にリセットされていたカウンタｉに１が加えられ（Ｓ６０３）、２番目の移動区間の最初の補間のための演算準備に入る。
【００８０】
２番目の移動区間における並進速度関数Ｌｖｐ２ と回転速度関数Ｌｖｆ２ とから１番目の補間動作を実行させたときの並進運動量Ｌｐ２ １ と回転運動量Ｌｆ２ １ が算出される（Ｓ６０４）。これは、１番目の補間動作で達成される補間点の並進と回転の運動量を示す。
【００８１】
次に、並進運動量Ｌｐ２ １ と回転運動量Ｌｆ２ １ より、１番目の補間動作を実行させたときの始点Ｐ２ から見た補間点の同次変換行列ＬＡ２ １ を算出する（Ｓ６０５）。これは、今演算された補間動作で到達すべき補間点の始点Ｐ２ から見た同次変換行列である。
【００８２】
始点Ｐ２ の同次変換行列ＬＡ２ に同次変換行列ＬＡ２ １ を乗じて、１番目の補間動作を実行させたときのロボット基準座標系から見た到達点の同次変換行列ＬＸ２ １ を算出する（Ｓ６０６）。これは、最初の補間動作で達成されるＴＣＰのロボット基準座標系から見た同次変換行列である。
【００８３】
この同次変換行列ＬＸ２ １ を逆演算して、その関節変数Ｌθ１ ２ １ 〜Ｌθ６ ２ １ が求められる（Ｓ６０７）。すなわち、これが最初の補間動作で達成される全軸の目標関節変数となる。これら全軸の関節変数Ｌθ１ ２ １ 〜Ｌθ６ ２ １ が、補間点である各関節の指令値として関節補間点バッファに格納される（Ｓ６０８）。
【００８４】
今は図３の（ｂ）の２番目の移動区間における補間動作を説明しているので、「速度結合処理」は必要でなく、ステップ９０１を素通りしてステップ６０９へ跳ぶ。内蔵タイマ等により指示される補間周期ごとのトリガ信号に基づき、関節補間点バッファ１２から全軸の関節変数Ｌθ１ ２ １ 〜Ｌθ６ ２ １ を各軸の指令値にしてサーボ出力部８へ送出する。これによって、各関節が１番目の補間目標関節角度θ１ ２ １ 〜θ６ ２ １ に向けて移動される（Ｓ６１０）。この場合も、ＦＩＦＯ処理式のものが採用されていれば、幾つかの指令値が蓄積されてから補間周期ごとに順次出力されることは上記のとおりである。
【００８５】
最初の補間が済んだのでカウンタｉに１が加えられ（Ｓ６１１）、２となったｉでもって、ステップ６１２を経て図１０のステップ６０４からの動作に戻される。以後同様の要領で処理がステップ６１０まで進められる。２番目の補間が済むとカウンタｉに１が加えられ（Ｓ６１１）、３となったｉでもって、ステップ６１２を経てステップ６０４からの動作が繰り返される。この移動区間における補間数はＩ２ であるのでｉがそれに到達するまで繰り返され、ｉがＩ２ を越えると（Ｓ６１２）、この２番目の移動区間の移動のための補間演算は完了する。
【００８６】
ステップ６１２において２番目の移動区間の移動が完了したと判断されると、カウンタｊに１が加えられ（Ｓ６１３）、ステップ６１４を経て図６のステップ４０２へ戻される。ｉが０にリセットされ、ステップ４０３，４０５を経てステップ４０６へ進められ、３番目の移動区間が如何なる補間種別であるかが問われる。図３の（ｂ）から分かるように「関節補間」すべき移動区間であるから、ステップ５０１へ移行される。ここからが、本発明に係る「速度結合処理」を含む制御動作の説明となる。図１６と図１７も併せて参照する。
【００８７】
ステップ５０１以降ステップ５１０までの動作が、前述した要領で繰り返される。図１６は教示点Ｐ３ ，Ｐ４ ，Ｐ５ について表わされており、その左側の点Ｐ３ ・Ｐ４ 間の台形波パターンで表された速度関数Ｊ１−６ ｖ３の部分で、その進展の様子を見ることができる。（ａ）は１番目の補間をした様子を示し、その間の移動量は影を施した三角形の面積のＪ１−６ ｒ３ １であり、その補間点を表す関節変数Ｊθ１−６ ３ １ はＪ１−６ ｓ３とＪ１−６ ｒ３ １との和で与えられる。なお、添え字のうちの１−６ は６軸分を一括して表記したことを意味する。
【００８８】
（ｂ）は４番目の補間を施した時点の進み状態を表している。（ｃ）は加速域を脱して定速域に入っている様子を示している。図の台形波パターンは波形のコンパクト化を図るために敢えて短く表されている。（ｄ）は減速域に入ろうとする時点であり、補間は図において１４回目（ＪＴ１４＝補間周期ｔｃ ×１４）に至っている。このｉ＝１４の演算において、ステップ５０４から５０６までの処理をした後にステップ７０１に進んでも、そこは素通りしてステップ５０７へ跳ばされる。
【００８９】
しかし、ｉ＝１５の場合にステップ７０１に至ると、今までも処理はされていたが説明を省いていた以下の判定がなされ、その判定どおりであるとステップ７０２以降の処理へ進められることになる。これを図１６に存在しないｉ＝１５の処理で説明する。１５番目の補間演算としてステップ５０４から５０５に至り、ステップ５０６で全軸の関節変数Ｊθ１ ３ １５〜Ｊθ６ ３ １５を補間点としてバッファに格納する。
【００９０】
ところで、３番目の移動区間における補間回数Ｉ３ や減速時間Ｔｄｅ３ は、３番目の移動区間の補間処理に入った時点のステップ４０５で解析対象軌道Ｔ３ としてすでに取り出されている。従って、定速域の最終補間は、補間回数Ｉ３ −（減速時間Ｔｄｅ３ ／補間周期ｔｃ ）の番目である。これをＩ３ｃとすれば、３番目の移動区間における補間動作の１５番目を意味する１５は、３番目の移動区間における定速域最後の補間までの補間回数Ｉ３ｃ＝１４より大きいので、この１５番目の補間から減速域へ入ったと判断され、ステップ７０２へ進められる。
【００９１】
ステップ７０２では、軌道解析部に３番目の次の移動区間である４番目のための解析対象軌道Ｔ４ が取り出されているかが調べられる。このステップへ進んだのは初めてであるから取り出しは未だ行われていなく、そこで軌道バッファより４番目の移動区間のための軌道データが解析対象軌道Ｔ４ として取り出される（Ｓ７０３）。その軌道データの点Ｐ４ に課せられたポーズ精度の大小を見て「速度結合処理」が可能かどうか判定される（Ｓ７０４）。
【００９２】
ポーズ精度が高ければ速度結合処理が見合わされ、ステップ５０７へ進められる。ポーズ精度が低く予め決められた精度以下であると「速度結合処理」が可能と判断される。いまの解析対象軌道Ｔ３ における残存減速時間ｔＲ ＝（Ｉ３ −ｉ）×ｔｃ ＝（２２−１５）×５ｍｓｅｃが計算される（Ｓ７０５）。その残存減速時間ｔＲ が解析対象軌道Ｔ４ の加速時間Ｔａｃ４ より少ないかどうかが判定される（Ｓ７０６）。ｔＲ がＴａｃ４ より大きければ、ステップ５０７へ進められる。このような処理は図１６で言えば（ｅ）である１７番目の補間まで続けられる。
【００９３】
補間のための演算が繰り返されｉがステップ５０９で１８にまで進むと、図１７の（ａ）に示すようにｔＲ ＝Ｔａｃ４ となる（Ｓ７０６）。しかし、この場合もステップ５０７へ先送りされる。図から分かるように４番目の移動区間の速度関数Ｌｖｐｆ４は０であり、そこでの補間量は計算するまでもなく０である。なお、添え字のうちのｐｆは、並進と回転の運動を一括して表記したことを意味する。
【００９４】
ｉが１９まで進められると図１７の（ｂ）に示すようにｔＲ ＜Ｔａｃ４ となり、ステップ７０７へ進められる。ここでは、解析対象軌道Ｔ４ での補間種別が「直線補間」であるかどうかが問われる。「直線補間」でなければすなわち「関節補間」であるなら、従来技術の項で述べた「関節補間」同士の「速度重のね処理」が実行される（Ｓ７０８）。
【００９５】
「直線補間」であれば、「関節補間」と「直線補間」との「速度結合処理」の実行に入り、ステップ４０３で０と置かれていたｋに１が加えられる（Ｓ７０９）。なお、ｋの意味するところは、３番目の移動区間の補間をｉで積み重ねて進ませようとするのに対して、その途中から行われる４番目の移動区間の補間処理をｋの積み重ねで進ませようとするものである。いまｋが１になったばかりであり、ステップ７１０ではｋ＝１が肯定され、いままで積み重ねられてきたｉの１９がｇに置き換えられて（Ｓ７１１）、図１２のステップ８０１へ進められる。
【００９６】
ステップ５０５で得られたロボットの全軸の関節変数Ｊθ１ ３ １９〜Ｊθ６ ３ １９を順演算し、その結果得られるロボット基準座標系から見た同次変換行列ＪＸ３ １９が算出され、これを４番目の移動区間においてこれから行おうとする補間の始点と見たててＪＸ４ と置かれる（Ｓ８０１）。
【００９７】
４番目の移動区間における並進速度関数Ｌｖｐ４ と回転速度関数Ｌｖｆ４ が軌道データから取り出される（Ｓ８０２）。ここで、１と置かれているカウンタｋをカウンタｉに置き替える（Ｓ８０３）。いま置かれたｉによって、４番目の移動区間の補間の進行を１から順次増やして使用することができるようになる。次に、説明を後に譲るが、ｍが０と置かれる（Ｓ８０４）。
【００９８】
その状態で、４番目の移動区間における並進速度関数Ｌｖｐ４ と回転速度関数Ｌｖｆ４ とから１番目の補間動作を実行させたときの並進運動量Ｌｐ４ １ と回転運動量Ｌｆ４ １ を算出する（Ｓ８０５）。これは「速度結合処理」の開始から最初の補間動作で達成される補間点の並進と回転の運動量を示す。ここで、ｍが０であるかどうか問われる（Ｓ８０６）。
【００９９】
ｍがいま０と置かれている状態にあるので、ステップ８０７へ進められる。ここでは、並進運動量Ｌｐ４ １ と回転運動量Ｌｆ４ １ より、１番目の補間動作を実行させたときの「同次変換行列ＪＸ４ から見た補間点の同次変換行列ＬＡ４１」が算出される。これは最初の補間動作で到達すべき補間点の同次変換行列ＪＸ４ から見た同次変換行列である。
【０１００】
次に、この同次変換行列ＪＸ４ に同次変換行列ＬＡ４１を乗じて、１番目の補間動作を実行させたときの「ロボット基準座標系から見た到達点の同次変換行列ＬＸ４１」が算出される（Ｓ８０８）。これは、最初の補間動作で達成される点のロボット基準座標系から見た同次変換行列である。
【０１０１】
この同次変換行列ＬＸ４１を逆演算して、その関節変数Ｌθ１ ４ １ 〜Ｌθ６ ４ １ が求められる（Ｓ８０９）。すなわち、１番目の補間動作で達成される全軸の目標関節変数であり、これがステップ８１０において全軸の補間点（各関節の指令値）として関節補間点バッファに格納される（Ｓ８１０）。これは、「速度結合処理」に基づき演算された「関節補間」と「直線補間」の速度を重ね合わせたかのように挙動させるための補間点である。
【０１０２】
補間周期ごとのトリガ信号が発せられると（Ｓ８１１）、図１７の（ｂ）に示すように、関節補間点バッファから全軸の関節変数Ｌθ１ ４ １ 〜Ｌθ６ ４ １ を各軸の指令値にしてサーボ出力部へ送出され（Ｓ８１２）、各関節が１番目の補間目標関節角度θ１ ４ １ 〜θ６ ４ １ に向けて移動を開始する。すなわち、ＴＣＰがその関節角度となるように動かされる。図中のＳ４ １ は４番目の移動区間の１番目の補間による移動量であり、これが３番目の移動区間における１９番目の補間による移動量のＪθ１−６ ３ １９すなわちＪＸ４ に上乗せしたように与えられる。図中に表したＳ４ １ ａ をＳ４ １ に等しく描けば、そのＬＸ４ １ を指している矢印のところの縦座標値Ｖｍ１９が、そのときの速度を表すことになる。
【０１０３】
いまはｍ＝０であるので（Ｓ８１３）、ステップ８１４に進んでｉがｇ＋ｋと置かれる。すなわちｉはステップ７１１で１９と置かれたｇと１のｋとが足され、２０とされる。この操作によってｉは３番目の移動区間の補間の回数に戻されたことになる。そこで、ｉが３番目の移動区間の補間回数のＩ３ ＝２２より大きくなっているかが問われる（Ｓ８１５）。
【０１０４】
そうでなければ図６のステップ５０４へ戻され、３番目の移動区間における２０番目の補間のための演算が開始される。以後、ステップ５０５に至ることによってＪθ１ ３ ２０〜Ｊθ６ ３ ２０が求められ、それがバッファに格納された後（Ｓ５０６）、ステップ７０１に進んでステップ７０９でｋに１が加えられて２とされる。ｋはもはや１でないのでステップ７１１は跳ばされ、ｇは従前の１９に据え置かれる。
【０１０５】
図１２のステップ８０１に入るとステップ５０５で得られたロボットの全軸の関節変数Ｊθ１ ３ ２０〜Ｊθ６ ３ ２０を順演算し、その結果得られるロボット基準座標系から見た同次変換行列ＪＸ３ ２０が算出され、これを４番目の移動区間においてこれから行おうとする２番目の補間の始点と見たててＪＸ４ と置かれる。
【０１０６】
ステップ８０３へ至ると２と置かれているカウンタｋがカウンタｉに置き替えられる。いま置かれたｉ（＝２）によって、４番目の移動区間の２番目の補間の処理がなされる準備が整う。そしてステップ８１２まで進展され、３番目の移動区間における２０番目の補間による移動量に４番目の移動区間における２番目の補間による移動量が上乗せされ、それが全軸の関節変数Ｌθ１ ４ ２ 〜Ｌθ６ ４ ２ なるかたちで各軸の指令値としてサーボ出力部へ送出される（Ｓ８１２）。各関節は２番目の補間目標関節角度θ１ ４ ２ 〜θ６ ４ ２ に向けて移動することになる。以上の動作は図１７の（ｃ）に示されている。
【０１０７】
このようにして３番目の移動区間を補間処理するためのｉが２２になるまで、同じく４番目の移動区間を補間処理するためのｋが４となるまで、すなわち図１７の（ｄ）の状態になるまで繰り返される。ステップ８１４でｉ＝１９＋４＝２３となるとｉが２２のＩｊ を越える（Ｓ８１５）。すなわち、もはや３番目の移動区間の補間は演算するに及ばなくなる。これは「速度結合処理」から解放されることを意味する。しかし、４番目の移動区間の残りの補間はＴＣＰが４番目の移動区間の終点Ｐ５ に至るまで続けられねばならない。
【０１０８】
速度重ね処理時の（ｊ＋１）番目の移動区間の始点は、ｊ番目の移動区間におけるｉ番目の補間動作により到達しようとする点が採用されている。従って、３番目の移動区間の補間が完了した後の４番目の移動区間における残りの補間動作の始点は、３番目の移動区間の補間動作が完了したとき到達している点である。これは言うまでもなく教示点Ｐ４ である。
【０１０９】
そこで、これ以後４番目の移動区間の残りの補間動作における始点はＰ４ となる。これはステップ７０３（図７を参照）で取り出したＴ４ の軌道データから、その始点Ｐ４ を表す同次変換行列ＬＡ４ で与えられる。言うまでもなく、３番目の移動区間の全軸の関節変数Ｊθ１ ３ ２２〜Ｊθ６ ３ ２２を順演算して得られるロボット基準座標系から見た同次変換行列ＪＸ３ ２２と同じものである。そこで、いずれをとってもよいが、ステップ８１６（図１３を参照）では単純にＪＸ４ ＝ＬＡ４ としてＬＡ４ をＪＸ４ にあてがうことにする。これを前提にして以後図１７の（ｅ）のように、３番目の移動区間によって達成された移動量に４番目の移動区間である濃い影の施された部分の移動量が重ねられるようにして、４番目の移動区間の終点Ｐ５ まで順次補間演算が続けられる。
【０１１０】
ステップ８１７へ進んでｉは４であるｋに１が加えられて５とされ（Ｓ８１７）、０のままのｍに１が加えられて１と置かれる（Ｓ８１８）。ｍが１になったということは、「速度結合処理」を実質的に脱して、４番目の移動区間の残りの補間のみの演算に入っていることを教える。いまｉが５であり４番目の移動区間の補間回数Ｉｊ＋１ の１６を越えていないので（Ｓ８１９）、図１２のステップ８０５に戻り、並進運動量Ｌｐ４ ５ と回転運動量Ｌｆ４ ５ が算出される。
【０１１１】
ｍは１であるので、ステップ８０６を経てステップ８２０へ進められる。ここでは上記の説明に基づいて、並進運動量Ｌｐ４ ５ と回転運動量Ｌｆ４ ５ より、５番目の補間動作を実行させたときの同次変換行列ＬＡ４ から見た補間点の同次変換行列ＬＡ４ ５ が算出される。これは、５番目の補間動作で到達すべき補間点の同次変換行列ＬＡ４ から見た同次変換行列である。そして、同次変換行列ＬＡ４ に同次変換行列ＬＡ４ ５ を乗じて、５番目の補間動作を実行させたときのロボット基準座標系から見た到達点の同次変換行列ＬＸ４ ５ が算出される（Ｓ８２１）。これは、５番目の補間動作で達成される点のロボット基準座標系から見た同次変換行列である。
【０１１２】
その後はステップ８０９から８１３までの処理がなされ、５であるｉに１が加えられて６と置かれる（Ｓ８２２）。このような処理を繰り返すうちにｉがＩ４ ＝１６を越えると（Ｓ８１９）、４番目の移動区間の補間のための演算が完了する。いまｊは３のまま来ており、速度結合のために４番目の移動区間まで処理したので、２を加えて５と置かれる（Ｓ８２３）。ｊはＮ−１＝６を越えていなく（Ｓ８２４）、図６のステップ４０２へ戻される。これによって図３の（ｂ）で言えば５番目の直線補間される移動区間の処理に入ることになる。
【０１１３】
その補間処理は、５番目の移動区間での「直線補間」、教示点Ｐ６ を繋ぎ点として５番目の移動区間の「直線補間」と６番目の移動区間の「関節補間」との「速度結合」、そして６番目の移動区間における残りを処理する「関節補間」からなる。その進展要領は３番目の移動区間を４番目の移動区間と繋ぐ説明と同じであるので、フローチャートには表しておくが、以下の要点部分のみの説明にとどめる。
【０１１４】
そのフローチャートは、ステップ４０２（図６を参照）から開始され、ステップ４０６から図１０のステップ６０１へ、そしてステップ６０８から６０９へ直行して６１１で補間順の数字を１つ上げて図１０のステップ６０４へ戻る通常の「直線補間」が繰り返される。何回かの補間が繰り返され、例えば１８番目の補間においてステップ６０８まで到達し、ステップ９０１によって減速域に突入したことが確認され、またポーズ精度から「速度結合処理」の可否を判断し（Ｓ９０４）、残存減速時間ｔＲ が６番目の移動区間の加速時間以下となり（Ｓ９０６）、その補間種別が「関節補間」であると判別されたときは（Ｓ９０７）、カウンタに上記した繰上処理を順次加えながら図１４のステップ１００１へ進められる。
【０１１５】
この場合の「速度結合」のためにはステップ６０７（図１０を参照）で得られたロボットの全軸の関節変数Ｌθ１ ５ １８〜Ｌθ６ ５ １８が、６番目の移動区間においてこれから行おうとする補間の始点と見たてられる。そして、６番目の移動区間における全軸での速度関数Ｊ１ｖ６ 〜Ｊ６ｖ６ が軌道データから取り出される（Ｓ１００２） 。ここで、ステップ９０９（図１１を参照）で１と置かれているカウンタｋがカウンタｉに置き替えられる（Ｓ１００３） 。いま置かれたｉによって、６番目の移動区間の補間の進行を１から順次増やして使用することができる。次に、「速度結合処理」を実質的に脱したかどうかの指標を与えるカウンタｍが０と置かれる（Ｓ１００４） 。
【０１１６】
その状態で、６番目の移動区間における全軸の速度関数Ｊ１ｖ６ 〜Ｊ６ｖ６ をもとに、１番目の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ６ １ 〜Ｊ６ｒ６ １ が算出される（Ｓ１００５） 。これは最初の補間で達成される各軸の移動量を表す。その後に、ｍが０であるかどうか問われる（Ｓ１００６） 。
【０１１７】
ｍがいま０と置かれた状態にあるので、ステップ１００７へ進められる。ここでは、始点と見たてられた全軸の関節変数Ｌθ１ ５ １８〜Ｌθ６ ５ １８に、１番目の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ ６ １〜Ｊ６ｒ ６ １を加算して、１番目の補間動作を実行したときのロボットの全軸の関節変数Ｊθ１ ６ １ 〜Ｊθ６ ６ １ が算出される。これは最初の補間動作で到達すべき全軸の目標関節変数である。この全軸の関節変数Ｊθ１ ６ １ 〜Ｊθ６ ６ １ が補間点（各関節の指令値）として関節補間点バッファに格納される（Ｓ１００８） 。これは、「速度結合処理」に基づき演算された「直線補間」と「関節補間」の速度を重ね合わせたかのように挙動させるための補間点である。
【０１１８】
関節補間点バッファから全軸の関節変数Ｌθ１ ６ １ 〜Ｌθ６ ６ １ を各軸の指令値にしてサーボ出力部へ送出され（Ｓ１０１０） 、各関節が１番目の補間目標関節角度θ１ ６ １ 〜θ６ ６ １ に向けて移動を開始する。すなわち、ＴＣＰがその関節角度となるように動かされる。この後のステップ１０１１から１０１７までの動作は先に「関節補間」から「直線補間」への繋ぎにおける「速度結合処理」のところで述べたステップ８１３（図１３を参照）から８１９までと同じ要領であるので、その説明は省く。ただし、ステップ１０１４はステップ８１６と、ステップ１０１８はステップ８２０や８２１とは異なるので、その部分について以下に述べる。
【０１１９】
ステップ１０１４は「速度結合処理」から抜け出て６番目の移動区間における「直線補間」を主体にした補間に入るときの処理であり、以下のとおりである。速度重ね処理時の（ｊ＋１）番目の移動区間の始点は、ｊ番目の移動区間におけるｉ番目の補間動作により到達しようとする点が採用されている。従って、５番目の移動区間の補間が完了した後の６番目の移動区間における残りの補間動作の始点は、５番目の移動区間の補間動作が完了したとき到達している点である。これは言うまでもなく教示点Ｐ６ である。
【０１２０】
そこで、これ以後６番目の移動区間の残りの補間動作における始点はＰ６ となる。これはステップ９０３（図１０を参照）で取り出したＴ６ の軌道データからその始点Ｐ６ を表す全軸の関節変数Ｊ１ｓ６ 〜Ｊ６ｓ６ で与えられる。それゆえ、Ｓ１００１でのＬθ１ ５ １８〜Ｌθ６ ５ １８がＪ１ｓ６ 〜Ｊ６ｓ６ に置き替えられる。
【０１２１】
ステップ１０１８は上記のステップ１０１４による準備が済んだ後の６番目の移動区間の「関節補間」のみによる処理をするもので、次のように行われる。この時点でｋが７（これは５番目の移動区間の補間回数に直して言えば１８＋７＝２５番目の補間に当たる）とすると、教示点Ｐ６ を表す全軸の関節変数Ｊ１ｓ６ 〜Ｊ６ｓ６ に７番目の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ６ ７ 〜Ｊ６ｒ６ ７ を加算して、その補間動作を実行したときのロボットの全軸の関節変数Ｊθ１ ６ ７ 〜Ｊθ６ ６ ７ が算出される。これは７番目の補間動作で到達すべき全軸の目標関節変数であることは言うまでもない。以上までが「速度結合処理」の補間手順である。
【０１２２】
上でも少し触れたが、「円弧補間動作」は実質的に「直線補間動作」と同じ思想の並進速度関数と回転速度関数とが採用される。従って、「円弧補間」と「関節補間」との間で「速度結合処理」するにあたっては、既に述べた「直線補間」から「関節補間」への速度結合や、「直線補間」から「関節補間」への速度結合の動作を適用することができる。そこで、「円弧補間」は「直線補間」に含まれるものであると見なせば、本発明は、速度繋ぎ部分において「順変換−逆変換」もしくは「逆変換」の処理を伴うものを対象にしているということが分かる。同種の補間種別に対する速度結合、すなわち「関節補間」から「関節補間」への繋ぎ、「直線補間」から「直線補間」へ、「円弧補間」から「円弧補間」へ、「円弧補間」から「直線補間」へ、「直線補間」から「円弧補間」への繋ぎは、速度関数の直接的な重ね合わせにより達成できるので、本発明の思想を導入するまでもないからである。
【０１２３】
上記の説明では、ｊ番目の移動区間において減速状態に入っており、かつその移動区間における残存減速時間が（ｊ＋１）番目の移動区間の加速時間以下となっているかどうかで判定するようにしている。すなわち、前後する移動区間のラップ時間帯の長短を左右するのは後側の移動区間の加速時間ということになる。一方、ユーザによっては加速時間の全てまたはその多くを減速域に持ち込みたくないという場合がある。例えば１５０ｍｓｅｃ以上は重ねたくないという何らかの事情がある場合には、そのような条件下において、予め加速時間が１５０ｍｓｅｃより大きいかどうかを調べるようにすればよい。小さければ加速時間帯の全部を減速時間と重ねるべく図１７の（ａ）の矢印のように前送りすればよい。
【０１２４】
１５０ｍｓｅｃより大きければ加速時間帯を前送りするに際して、加速時間帯のうち前半に存在する１５０ｍｓｅｃ分だけにとどめるという配慮を施すステップを加入してやればよい。具体的には、残存減速時間が１５０ｍｓｅｃ以下となった以降に加速時間帯との結合処理を開始させるように、条件として時間幅制限を加重させるのである。
【０１２５】
ところで、図１８は、速度結合処理が施された場合でも、様々な速度変化を呈することを示す。この図はすべて「関節補間」から「直線補間」へ移行する例で表されているが、（ａ）は後側の移動区間の加速域の全部が前側の移動区間の減速域に含まれるように重ねた場合、（ｂ）は前側の移動区間の減速域の全部が後側の移動区間の加速域に含まれるように重ねた場合、（ｃ）は減速時間と加速時間が同じでぴたりと重ね合う場合を示している。いずれも前側の移動区間における速度が後側のそれよりも大きく与えられたものである。（ｄ）は（ａ）と、（ｅ）は（ｂ）と、（ｆ）は（ｃ）と同じ趣旨であるが、これらの場合、後側の移動区間における速度が前側のそれよりも大きくなっている。
【０１２６】
この図を眺めてみると、（ｂ）と（ｄ）とは速度結合処理の部分で速度変化が減増した谷が形成されている。この結合処理部分でのＴＣＰの動きとしては、常に滑らかな速度変化で与えられるに越したことはない。その点で、（ａ），（ｃ），（ｅ）および（ｆ）は減少の一途をたどるか増加の一途をたどるから好ましいと言える。ところが、（ａ）と（ｅ）は途中で加速度が変わっている。その折れ方が弱い場合はよいが、折れ点で加速度が急激に変化することもあり得る。結局は、加速度が一定となる（ｃ）および（ｆ）がロボットにとって都合よいと言える。
【０１２７】
このことを念頭におくと、異種補間動作における速度の結合処理が適用される速度関数は、現移動区間の減速域の時間と次の移動区間の加速域の時間とを一致させたものとしておくことができれば、本発明によって一層滑らかな速度結合を実現できることが分かる。しかし、速度関数はステップ２０１（図４を参照）やステップ２０６（図９を参照）ですでに生成されたものであり、かつ各移動区間における動作速度も異なるのが一般的であるから、常に減速時間と加速時間とが一致するとは限らない。
【０１２８】
そこで、本発明においては、可能な場合にこの両時間を意図的に一致させる配慮も払われる。それは、加速時間を減速時間に合わせたり、減速時間を加速時間に合わせるという処置を採ることになる。これらの場合のうち、加速時間が減速時間より長い場合に、長い加速時間を縮めて減速時間に合わせるということは避けた方がよい。それは不可能なことでないが、ＴＣＰにとってすでに適切な加速度が選定されているのであり、またサーボモータの容量を考慮したうえで決められた加速度である場合が多いから、加速時間を低減させたいという理由だけで加速度を増大させることは好ましくない。
【０１２９】
従って、本発明においては 減速時間が加速時間より長い場合は、加速時間を減速時間に合わせるように引き延ばし、減速時間が加速時間より短い場合には、減速時間を加速時間に合わせるように引き延ばすことにする。いずれにしても、その変更をどの時点でしておくかということになる。バッファがＦＩＦＯ処理式の場合には、かなり以前に立ち戻ることは容易でないことが多い。その場合にはバッファに格納されている範囲で処置することになる。
【０１３０】
上記したフローチャートにそれを加味すると交錯の激しい表記となるので、動作命令バッファの容量が大きいと仮定して、その減速加速の時間の長さ合わせ処理を簡略化して表したステップ３０１から３１５の記載に基づいて説明する。なお、移動区間の数（Ｎ＝７）とその補間種別および速度結合処理の要否は、図３の（ｂ）のタイムチャートに従うものとする。ちなみに、この処理は、図１のブロック図において「異種補間速度結合性改善用速度関数再生成部」１３で行われる。
【０１３１】
まず、１番目の移動区間の軌道データを軌道バッファ７から取り出すために、すでにステップ２０５（図４を参照）または２１０（図９を参照）において０と置かれているｊに１が加えられる（Ｓ３０１）。これによって、最初の移動区間における減速時間ｔｄｅ１ と補間種別が取り出される（Ｓ３０２）。この移動区間における後の教示点であるＰ２ （作業第１準備位置）のポーズ精度から見て「速度結合処理」が可能かどうか判断される（Ｓ３０３）。
【０１３２】
ポーズ精度が低くて予め決められた精度以下であると「速度結合処理」の可能性があることになるが、この移動区間においては図３の（ｂ）によればその必要のない扱いとなっている。ステップ３０４へ進んでｊに１が加えられ、（Ｎ−１）＝７−１＝６より小さいことが確認され（Ｓ３０５）、２番目の移動区間以降の処理のための準備が整えられる。ステップ３０２において２番目の移動区間における減速時間ｔｄｅ２ と補間種別が取り出される。この移動区間における後の教示点であるＰ３ は図３の（ｂ）によれば第１作業点であり、高いポーズ精度で停止させることにしている。ステップは３０３から再度ステップ３０４へ進められる。ｊに１が加えられ、ステップ３０５を経て３番目の移動区間以降の処理に仕向けられる。
【０１３３】
ステップ３０２において３番目の移動区間における減速時間ｔｄｅ３ と補間種別として指定されている「関節補間動作」が取り出される。この移動区間における後の教示点であるＰ４ は図３の（ｂ）によれば作業第２準備位置であり、ポーズ精度は低く与えられている。ステップ３０６へ進められ、そこで現在３であるｊに１が加えられて４とされる。これに基づき、４番目の移動区間における加速時間ｔａｃ４ と補間種別として指定されている「直線補間動作」が取り出される（Ｓ３０７）。
【０１３４】
ステップ３０８であい前後する移動区間は異種補間であると判別され、加速時間ｔａｃ４ が減速時間ｔｄｅ３ より小さいかどうか問われる（Ｓ３０９）。図３の（ｂ）の該当部分を拡大した図１９の（ｂ）によれば、減速時間ｔｄｅ３ の方が大きくなっている。（ａ）には、ｔａｃ４ ＜ｔｄｅ３ となる４番目の移動区間の例が、一点鎖線と二点鎖線により２つ描かれている。その２つは、減速度の絶対値に比較して加速度が大きい場合と小さい場合とを表している。（ｂ）はそのうちの一点鎖線の移動区間を採りあげ、３番目の移動区間と速度結合処理しようとしたものである。
【０１３５】
この場合はステップ３１０に進んで、４番目の移動区間の加速時間ｔａｃ４ を減速時間ｔｄｅ３ と等しくなるように、当該速度関数Ｌｖｐｆ ４ が図１９の（ｃ）に表した加速の弱められた台形状の速度関数Ｌｖｐｆ４ｍ となるように生成し直される。すなわち、加速を示す線はａからｂに変えられる。しかし、４番目の移動区間による移動量はあくまでも同じでなければならないから、線ａ，ｂが交差して上下に形成される三角形の面積が等しくなるように、線ｂの位置が決められる。
【０１３６】
このように変形させた後に、図１８の（ｃ）のようにする意図で、図１９の（ｄ）のように、４番目の移動区間の変形された速度関数Ｌｖｐｆ４ｍ が前へ寄せられる。その結果、教示点Ｐ５ は（ａ）や（ｃ）の場合より早く到達することはもちろんのこと、（ｂ）の場合よりも早く到達できることになる（（ｂ）と（ｄ）の図中の白抜き矢印５２，５３の長短を参照）。すなわち、ＴＣＰの動きはますます機敏になることが分かる。もちろん速度結合処理部における速度は（ｄ）中に記載したｃのように可及的に変化の少ないものであり、かつ３番目の移動区間の定速度と４番目の移動区間の定速度とを結ぶ全体から見ても速度変動の少ない運動をさせることができるようになる。この再生成された速度関数Ｌｖｐｆ４ｍ は、従前の速度関数Ｌｖｐｆ４に置き替えられて軌道バッファ７に格納される（Ｓ３１１）。
【０１３７】
ステップ３１２で、現在のｊが（Ｎ−２）＝７−２＝５以下であると判断されて制御はステップ３０２へ戻される。いまｊは４とされたままであるので、ステップ３０２の処理が行われる。図３の（ｂ）によれば４番目の移動区間の後の教示点Ｐ５ においては、停止して作業をさせるようになっている。従って、ステップ３０３では速度結合の可能性のないとの判断からステップ３０４へ回される。ここで、現在の４であるｊが５に繰り上げられ、（Ｎ−１）＝７−１＝６より小さいことが確認され（Ｓ３０５）、５番目の移動区間以降の処理のための準備が整えられる。
【０１３８】
ステップ３０２に戻って５番目の移動区間における減速時間ｔｄｅ５ と補間種別の「直線補間」が取り出される。この移動区間における後の教示点であるＰ６ は図３の（ｂ）によれば作業解放位置であり、近傍を通過させればよい程度の点であることが分かる。ステップは３０３を経て３０６へ進み、１が加えられてｊは６となり、６番目の移動区間における加速時間ｔａｃ６ と補間種別として指定されている「関節補間」が取り出される（Ｓ３０７）。
【０１３９】
ステップ３０８であい前後する移動区間は異種補間であると判別され、加速時間ｔａｃ６ が減速時間ｔｄｅ５ より小さいかどうか問われる（Ｓ３０９）。図３の（ｂ）の該当部分を拡大した図２０の（ｂ）によれば、減速時間ｔｄｅ５ の方が小さくなっている。（ａ）の図はｔａｃ６ ＞ｔｄｅ５ となる６番目の移動区間の例が先の例と同じく加速度の大小を分けて一点鎖線と二点鎖線の２つで挙げられている。（ｂ）はそのうちの一点鎖線の移動区間を採りあげ、この６番目の移動区間との速度結合処理をしようとしたものである。
【０１４０】
この場合はステップ３１３に進んで、５番目の移動区間の減速時間ｔｄｅ５ を加速時間ｔａｃ６ と等しくなるように当該速度関数Ｊ１−６ ｖ ５ が、図２０の（ｃ）に表したような台形状の速度関数Ｊ１−６ ｖ ５ｍとなるように生成し直される。減速を示す線はｐからｑに変えられるが、あくまでも５番目の移動区間による移動量は同じでなければならないから、線ｐ，ｑが交差して上下に形成される三角形の面積が等しくなるように線ｑの配置が決められる。
【０１４１】
このように変形させた後に、図１８の（ｆ）のようにする意図で、図２０の（ｄ）のように、６番目の移動区間の変形されなかった速度関数Ｌｖｐｆ ６ が、５番目の移動区間の変形された速度関数Ｊ１−６ ｖ ５ｍへ寄せられる。その結果、教示点Ｐ７ すなわち退避位置へは、（ｂ）の場合よりも早く到達することになる。すなわち、ＴＣＰの動きは機敏となる。もちろん、速度結合処理部における速度は（ｄ）中に記載したｒのように可及的に変化の少ないものであり、かつ５番目の移動区間の定速度と６番目の移動区間の定速度とを結ぶ全体から見ても速度変動の少ない運動をさせることができる準備が整う。この再生成された速度関数Ｊ１−６ ｖ ５ｍは、従前の速度関数Ｊ１−６ ｖ ５ に置き替えられて軌道バッファ７に格納される（Ｓ３１４）。
【０１４２】
ステップ３１２で、現在６であるｊが（Ｎ−２）＝７−２＝５以下でないと判断されて、この加減速時間の変更制御は終了し、ｊは０にリセットされる（Ｓ３１５）。なお、図３の（ｂ）に記載されていないが、５番目と６番目の移動区間の間で速度結合処理する必要がなかったときは、ｊ＝５の状態でステップ３０４にたどりつくとｊが６に繰り上げられ、その状態で再度ステップ３０４に至り、ｊが７に繰り上げられると、ステップ３０５でｊが（Ｎ−１）＝７−１＝６より大きいと判断され、ステップ３１５へ導かれて、この処理は終了する。
【０１４３】
このようにして加減速時間を変更した速度関数が軌道バッファ７に格納されると、制御はフローチャートのステップ４０１からの処理に入り、必要な箇所での加減速時間の一致の図られた速度関数に基づいて、「速度結合処理」が施されることになる。
【０１４４】
ところで、いままでは各移動区間における速度関数は台形波パターンを例にして説明してきた。しかし、これに限らず、図３の（ａ）の一部に重なるようにして描いた細かい破線表示の速度関数Ｊｍ １−６ ｖ ３ やＬｍ ｖｐｆ ４ のように、Ｓ字波形の速度変化を持った場合にも適用することができる。このような速度関数が採用されている場合に、上記したステップ３０１から３１４の処理を施すなら、結合速度はますます滑らかなものとなる。ちなみに、以上の説明は６軸ロボットを例にしたが、２軸以上であれば、本発明を適用することができるのは言うまでもない。
【０１４５】
【発明の効果】
本発明によれば、補間種別の移り変わりを判別すると共に繋ぎ点もしくはその近傍における速度の結合処理が可能かどうかを判定する速度結合処理可否判定部と、異種補間動作間の速度を重ね合わせたように挙動させるための補間点を演算する速度結合演算部とを備えるので、３点以上の教示点からなる経路（曲折しているかどかは問題でない）上での速度制御において、教示点ごとに記録された補間種別が異なる場合でも、教示点でロボットのＴＣＰ速度を低下させることなく通過させることができる。
【０１４６】
ロボットが機敏に動けば作業能率は上がり、タクトタイムは短縮される。滑らかな速度変化が助長されることになり、無用の衝撃や力さらには振動がアームやＴＣＰに及ぶこともなくなる。ロボットによる作業の正確さ精密さが向上し、ＴＣＰの動きもスマートなものとなる。
【０１４７】
速度制御法の一連の発明においても、上記の効果が発揮されることは言うまでもない。その制御は異種補間での速度結合を行うものであるが、従前から行われている同種補間における速度重ね処理の思想とは異なり、異種補間での速度結合を補間点の重合により達成した。これは同種補間の場合の速度重ね処理と組み合わせて使用するにおいて弊害をきたす要因はなく、従ってロボットの速度制御の多様化が図られることにもなる。
【０１４８】
速度の結合処理の可否は、現移動区間において減速状態にあり、かつその移動区間における残存減速時間が次の移動区間の加速時間以下となっているかどうかで判定するようにしておけば、結果として生じる結合された速度がサーボモータの能力を越えるようなことはなくなり、駆動系に損傷をきさずまたロボットアーム等に対する安全性も向上させやすくなる。
【０１４９】
異種補間動作における速度の結合処理が適用される速度関数を生成するにおいて、現移動区間の減速時間と次の移動区間の加速時間とを一致させるようにしておくと、結合された速度の滑らかさは格段に向上する。その場合、次の移動区間の加速時間が現移動区間の減速時間よりも短かければ、次の移動区間の加速時間を減速時間に一致させ、現移動区間の減速時間が次の移動区間の加速時間よりも短かければ、現移動区間の減速時間を前記加速時間に一致させておくようにすることができる。その結果、加速度の過剰な増大を防いで、サーボモータに大きな負担を強いらずまた振動助長原因も可及的に少なくしておくことができる。
【図面の簡単な説明】
【図１】本発明に係る多関節型ロボットの速度制御を実現する制御装置のブロック図。
【図２】多関節型ロボットの速度制御法の全体を簡潔に表したフローチャート。
【図３】フローチャートを説明するための移動区間の変遷と速度結合処理箇所を示したタイムチャート。
【図４】ないし
【図１５】教示作業プログラムの読み出しに始まり、軌道データの生成、加減速時間の変更処理した速度関数の再生成、軌道データを基にした軌道解析すなわち補間のための演算ならびに速度結合処理を表したフローチャート。
【図１６】関節補間と直線補間があい前後して並んだ場合の前側の移動区間における補間動作の説明図。
【図１７】関節補間と直線補間が並んだ場合の繋ぎ点近傍での速度結合処理を施した場合の補間動作説明図。
【図１８】２つの移動区間の繋ぎ点で速度結合処理した場合に起こり得る速度の変化の種類を示した説明図。
【図１９】加減速時間を一致させるための処理を表したもので、加速時間を減速時間に合わせるときの説明図。
【図２０】加減速時間を一致させるための処理を表したもので、減速時間を加速時間に合わせるときの説明図。
【図２１】ワークピースに対して作業を施す教示点と、その軌道の補間の種類や補間法を説明するための作業モデル図およびそのタイムチャート。
【図２２】３つの教示点間で関節補間されるときに使用される関節速度関数を表したタイムチャート。
【図２３】関節補間同士の速度重ね処理を説明するタイムチャート。
【図２４】関節補間同士の速度重ね処理をした場合の実際にたどる経路とポーズ精度との関係を表した説明図。
【符号の説明】１…速度制御装置、２…速度結合処理可否判定部、３…速度結合演算部、３Ａ…関節補間から直線補間への速度結合演算部、３Ｂ…直線補間から関節補間への速度結合演算部、４…軌道解析部、５…補間動作演算部、５Ａ…関節補間動作演算部、５Ｂ…直線補間動作演算部、６…軌道生成部、７…軌道バッファ、８…サーボ出力部、９１ 〜９６ …サーボドライバ、１０１ 〜１０６ …サーボモータ、１１…補間種別判定部、１２…関節補間点バッファ、１３…異種補間速度結合性改善用速度関数再生成部、５１…ワークピース、Ｐｊ …教示点。
【発明の属する技術分野】
本発明は多関節型ロボットの速度制御法および制御装置に係り、詳しくは、教示点から次の教示点までの区間に幾つかの補間点を与え、移動区間ごとに予め計算されている個々の速度関数に基づき関節補間動作や直線補間動作によって移動させようとするとき、あい前後する移動区間の補間種別が異なる場合でも、前区間の減速域と後区間の加速域における速度の結合処理が可能となるようにした制御方法および装置に関するものである。
【０００２】
【従来の技術】
例えば６本のアームをそれぞれ連結する軸部に各１つのモータが装着され、ロボット先端に取りつけられたトーチなどのツールもしくは溶接が施されるワークピース（これらエンドエフェクタの中心を以下ＴＣＰという）を動かすなどする多関節型ロボットは、一般にティーチングプレイバック方式で動作が再現され、予め教示したとおりの経路をロボットがたどるようになっている。
【０００３】
３点以上の教示点からなる経路で多関節型ロボットを動作させる場合、前後２つの教示点で形成される１つの移動区間においては、その途中に幾つもの補間点を与えて指定した移動速度を満たす例えば台形波パターンに従う速度でもって移動される。次の移動区間においてもそれにふさわしい台形状に変化する速度により移動され、これを繰り返してツール等に目標の位置と姿勢が与えられ、ワークピースの何箇所かに溶接等の作業を施すといったことができる。
【０００４】
ところで、補間動作には通常直線補間、円弧補間、関節補間の３種がある。直線補間はＣＰ制御 ｃｏｎｔｉｎｕｏｕｓ ｐａｔｈ ｃｏｎｔｒｏｌと言われ、教示された２点間を最短コースでたどり、すなわち予め記録した速度で目標点に直進させる移動方法である。円弧補間は意図的に２点間の直路から外れた位置に参照ポイントを与え、３点を通る一義的な円弧に沿って予め記録された速度で教示点へ向かわせる方法である。この円弧補間は直線補間と同じ考え方の速度関数を使用するので、直線補間と実質的に変わるところがない。
【０００５】
関節補間はＰＴＰ制御 ｐｏｓｅ−ｔｏ−ｐｏｓｅ ｃｏｎｔｒｏｌ と言われ、移動経路の直線性や円弧性は問わず、目標の関節角度に向かって各軸が原則的に同時に動き出し、同時に目標点に到達する移動方法である。ロボットの性能は関節ごとにどのような容量のモータが搭載されているか、すなわちどれだけ重いアームを動かすことができるかで決まる。ＰＴＰ制御によれば各関節の１００％の動作性能を発揮する制御が容易であり、高速動作が必要な移動区間で多用される。
【０００６】
これによれば教示点から次の教示点へ最短時間で移すことができ、ロボット性能や稼働中の応答性の向上に貢献する。ただし、関節ごとの速度制御となるために、ＴＣＰの軌道や速度はオペレータが簡単に予測できるものでない。ロボットはこのＰＴＰ制御に限ることなく、その移動の都度その間の動作や目的にふさわしい補間方式が採択される。
【０００７】
例えば図２１の（ａ）に示すように、ＴＣＰ（図示せず）が退避位置Ｐ１ からワークピース５１に接近して作業態勢に入るための準備位置Ｐ３ （第１アプローチ点Ｐ２ を経由した第２アプローチ点）まで移動する間は関節補間動作により、作業準備位置Ｐ３ から最初の作業点Ｐ４ へおよび次の作業点Ｐ５ 、それから少し離れた作業解放位置Ｐ６ （第１リターン点）まで移動する間は直線補間動作により、作業解放位置Ｐ６ から退避位置Ｐ７ （第２リターン点＝Ｐ１ ）に戻るまでは関節補間動作により移動させるようにしておくと、ロボットを機敏に動作させかつ作業部位では位置姿勢を高い精度で与えて停止させたり所望作業速度を与えたりすることができる。
【０００８】
補間動作は簡単に説明すると、図２１の（ａ）において点Ｐ２ からＰ３ の移動区間と点Ｐ３ からＰ４ の移動区間のみに幾つかの補間点を付したが、その補間点を以下の要領で定め、それをたどる動作をさせることである。図においては前者の移動区間で関節補間が適用されているが、予め決められた補間周期ごとにその区間に与えられた速度関数に従い補間点が演算される。ｉ番目の補間は点Ｐ２ から出発して補間周期×ｉ＝ＪＴｉ の時間が経過したときの補間点で与えられ、（ｉ＋１）番目の補間は点Ｐ２ から見て補間周期×（ｉ＋１）＝ＪＴｉ＋１ が経過したときの補間点で与えられる。なお、ｉ＝４とした場合について、後述する図２３の（ａ）の左側の台形波パターン（速度関数Ｊ１ｖ２ ）上に補間点をＪθ１２４ としてその関節変数を示した。影の施された面積が教示点Ｐ２ （関節変数はＪ１ｓ２ ）からの移動量Ｊ１ｒ２４であり、時間ＪＴ４ 経過時の補間点の関節変数Ｊθ１２４ はＪ１ｓ２ にＪ１ｒ２４を加えて与えられる。
【０００９】
図２１の（ａ）中の点Ｐ３ からＰ４ の移動区間は直線補間であるが、同様の考え方で点Ｐ３ から見たｉ番目の補間は時間ＬＴｉ 経過したときの補間点で、（ｉ＋１）番目のそれは時間ＬＴｉ＋１ が経過したときの補間点で与えられる。いずれの補間法においてもその移動区間における補間開始点すなわちその移動区間における前側の教示点を基点にして補間点を演算し、その点を実現するようにサーボモータを動かせてＴＣＰを移行させるようにしている。ちなみに、補間周期は例えば５ないし２０ｍｓｅｃ程度で与えられる。
【００１０】
ところで、各移動区間における補間動作は、図２１の（ｂ）に示すように、補間種別に関係なく加速・定速・減速からなる台形波パターンの速度関数で与えられるのが一般的である。例えばどの教示点においても一旦停止して作業させようとする場合には、徐々に速度を上げてまた徐々に速度を下げるようにしておけばＴＣＰの停止位置精度は高くなり、ロボットのアームやＴＣＰに無用の振動を及ぼすことも少なくなる。
【００１１】
しかし、上記した点のうち作業点では高い精度の停止が要求されるが、作業準備位置や作業解放位置は単にその位置もしくはその近傍を通過させるだけでよい場合が多い。作業準備位置近傍での厳格な減速・停止・加速は無用であるだけでなく、この部分での緩慢であったり停滞気味の挙動は幾度となく繰り返されるロボットの反復動作の時間を長引かせることになる。もちろん、作業時間節約のために急減速や急加速させるとすれば、それは振動を助長させるもとになり好ましいことではない。
【００１２】
そこで、ＣＰ動作とＣＰ動作とをポーズ精度 ｐｏｓｅ ａｃｃｕｒａｃｙの高く要求されていない教示点を挟んで行わせる場合、前の移動区間の減速域を後の移動区間の加速域と重ね合わせて、両区間の繋ぎ点もしくはその近傍を前の移動区間の減速中の速度と後の移動区間の加速中の速度との和で通過させれば、次の教示点に到達させる時間を縮めることができる。
【００１３】
このような速度の重ね処理は前の移動区間の速度関数と後の移動区間のそれとが同種である場合、その速度関数自体を重ね合わせることによってある意味では容易に達成される。ＣＰ動作とＣＰ動作の場合には前の並進速度関数と後の並進速度関数とを、前の回転速度関数と後の回転速度関数とを、それぞれ減速域と加速域でそれぞれの全部もしくは一部を重ねればよい。
【００１４】
ＰＴＰ動作は、６軸ロボットであれば図２２に示すように関節（軸）ごとに速度関数が与えられる。この場合でもＰＴＰ動作同士の速度の重ね処理は、第１関節の前の速度関数Ｊ１ｖ２ の減速域と後の速度関数Ｊ１ｖ３ の加速域とを、第２関節の前の速度関数Ｊ２ｖ２ と後の速度関数Ｊ２ｖ３ とをといったように関節ごとを対象にして行われる。従って、６自由度のマニピュレータであれば、６つの組合せ全部で重ねられることになる。
【００１５】
図２３は第１関節のみを対象として表したものであるが、非重ね状態の（ａ）に対して、速度重ね処理における重ね度合いとしては（ｂ）から（ｅ）までのような幾つかが考えられる。ちなみに、（ｃ）は加速域を減速域に含ませた場合、（ｄ）は減速域を後の移動区間の定速域まで侵入させて重ねた場合、（ｅ）は両移動区間を定速域まで侵入させた場合である。
【００１６】
各速度関数を表す線が横軸とで囲む面積はＴＣＰの移動量を表すが、（ｂ）から（ｅ）において途中でＪ１ｖ２−３ をたどらせたとしても、その移動量は（ａ）の場合と同じになることは簡単な幾何学で理解できるところである（図２１の（ｃ）の右部分を例にすると、□ＡＣＥＤ＝△ＡＤＣ＋△ＣＤＥ＝△ＡＢＣ＋△ＣＤＥである）。（ｅ）のように重ね量を多くすれば、移動量が同じであるにもかかわらず、点Ｐ４ への到達時間は時間Ｔｓｅ（＝ＪＴ２２＋ＪＴ１６−ＪＴ３０）も早められることが分かる。
【００１７】
しかし、（ｄ）や（ｅ）のように減速域や加速域を越えてまで重ねることは妥当でない。速度関数Ｊ１ｖ２ によって与えられている定速度が第１関節のサーボモータの例えば許容最高速度である場合には、（ｅ）中に表したＶａ分大きいＪ１ｖ２−３ なる速度をもはや与えることができなくなるからである。これは、（ｄ）の場合に、後の移動区間の定速度が前の移動区間のそれより大きくかつ第１関節のサーボモータの許容最高速度もしくはそれに近い速度で与えられているようなときにも言えることであり、これらは速度の重ね処理に不向きであることに注意すべきである。
【００１８】
ところで、速度の重ね処理をするにおいて無視することができないのは、上でも少し触れたポーズ精度である。図２４の（ａ）は重ね処理をしていない場合であるが、この場合には点Ｐ３ で停止させるので、点Ｐ２ ，Ｐ３ ，Ｐ４ が直線的に並んでいても「く字状」に折れて並んでいても、同じ距離を同じ時間を掛けて移動することになる。もちろんＰ３ での停止位置を忠実に再現させる意味でもポーズ精度は高く与えられる。
【００１９】
一方、速度重ね処理をする場合、図２３の（ｃ）の例を適用すると、図２４の（ｂ）のようになる。速度を重ねるということは点Ｐ３ で停止させないことだけでなく、点Ｐ３ を厳密には通過させなくてもよいことを意味する。図２４の（ａ）は速度を重ねる前の状態を表し、減速域には補間点が８つあって減速時間Ｔｄｅは補間周期ｔｃ ×８、加速域は６つあって加速時間Ｔａｃはｔｃ ×６となっている例で表されている。図２４の（ｂ）は補間点が６つある加速域全部を、補間点８つある減速域の後半の６つの部分の領域に重ねた形態となっている。
【００２０】
この場合、前後の移動区間はいずれも関節補間であるので、その速度関数Ｊ１−６ｖ２ とＪ１−６ｖ３ とを数学的に重ねることができる。重ねる前の図２４の（ａ）では、Ｊ１−６ｖ２ とＪ１−６ｖ３ とが速度０を挟んで繋がった速度関数となって補間数２２個（ＪＴ２２）と１６個（ＪＴ１６）からなる２つの移動区間を纏めた点Ｐ２ ・Ｐ４ 間に適用されていると見るなら、重ねた場合には、図２４の（ｂ）のように、Ｊ１−６ｖ２ とＪ１−６ｖ２−３ とＪ１−６ｖ３ とが繋がった速度関数が補間数３２個分の移動区間域に割り当てられていると見ることができる。なお、添え字のうちの一部の１−６ は、６軸分を一括表記したことを意味している。
【００２１】
その結果、この（ｂ）での重ね部の移動時間は、図２４の（ａ）に影をつけて表した時間ＴＲ１２ の半分であるＴＲ６となる。一方、速度重ね処理は前の移動区間の終り部分の６補間分と後の移動区間の始め部分の６補間分であって移動距離は（ａ）も（ｂ）も変わるものでない。とすると、点Ｐ２ ，Ｐ３ ，Ｐ４ が直線的に並んでいる場合、Ｐ３ のポーズ精度が低くてもＴＣＰはＰ３ を通過することになる。
【００２２】
一方、Ｐ３ で「く字状」に折れている経路の場合、（ｂ）の軌道では図から明らかなように曲線を描かざるを得なく、点Ｐ３ をかすめた点Ｐ３ａを通過することになる。移動距離が同じであるにもかかわらずその軌道が内のり軌跡で描かれているのは、６軸ロボットの場合ならその全軸の動きを考慮したものであるからである。すなわち、Ｐ３ で「く字状」に折れるということは６つのうちの幾つかの軸が負の速度を持っているはずであり、それゆえポーズ精度内で点Ｐ３ からΔＰずれた点Ｐ３ａが通過点となる。
【００２３】
この曲線軌跡は（ｂ）の右下部に表したように重ね処理部の補間数が少なくなればなるほど小さくなることは容易に理解でき、通過点Ｐ３ａは教示点Ｐ３ に近接する。しかし、その分、重ねは速度の低いところでしか処理できないことになるから通過速度も低いものにとどまる。これは常識で想定される人間や車の動作と何ら変わりがない。逆に、重ね処理部の補間数が多くなれば軌跡の孤は大きくなり、通過点は教示点Ｐ３ からますます離反する。その分、重ねは速度の高いところで可能となり、ロボットは機敏に動く。
【００２４】
ところで、特公平１ー２７４４３号公報には、２つの移動区間がＶ字状をなす経路を形成している場合に、折り返し点での速度が低下しないようにした例が記載されている。これは、正しく上記した図２４の（ｂ）で説明したものであり、速度重ね処理によって簡単に言えば結果的には前後２つの速度関数を１つの速度関数に作り変えたかのように見ることができる。この考え方は当該公報の発行により公知になっているが、あい前後する２つの移動区間における補間種別は直線補間動作であり、それ以上の説明は見当たらない。
【００２５】
【特許文献１】
特公平１ー２７４４３号公報
【００２６】
【発明が解決しようとする課題】
しかし、図２１の（ａ）で示した第１アプローチ点Ｐ２ から第２アプローチ点Ｐ３ までは関節補間動作で、第２アプローチ点Ｐ３ から最初の作業点Ｐ４ へ向かうまでが直線補間動作である場合、前者の各関節の速度関数と後者の並進速度関数や回転速度関数とは異質なものであり、重ねることができない。また前者は軸（関節）の数だけ速度関数が存在するが、後者は並進と回転の２つの動作しかなく、数のうえからも合わせようがない。
【００２７】
関節補間は各軸の動作量から求められる移動量だけを扱うものであり、直線補間は同次変換行列でＴＣＰの並進と回転の量だけを扱う。従って、演算対象が全く異なる。前者は各軸の関節の運動であり、後者はＴＣＰの運動であるからであるが、いずれにしても、移動区間の前後で補間種別が異なる場合には速度関数を重ねるという概念の導入は不可能であり、繋ぎ点前後で減速・停止・加速の動作は依然として余儀なくされる。
【００２８】
図２１の例で言えば、点Ｐ２ から点Ｐ３ へはＴＣＰが一気に進出してくるが、点Ｐ３ では一旦停止してから点Ｐ４ に向かうことになり、ＴＣＰの滑らかな移動は期待し得ない。また、点Ｐ５ から点Ｐ６ へ移動すると、そこでもやはり一旦停止してから点Ｐ７ に向かう。演算上の事情を知らない人の目には、ＴＣＰがワークピース５１の前後各１箇所でぎこちない動きをしているように映る。
【００２９】
一旦静止するということは無用な力（慣性力）をアームに及ぼすことになるから動き続けさせた方が、しかもそれが滑らかなものにできれば、ロボットに無用な衝撃を与えなく、振動発生の原因排除もしやすくなる。これが可能になれば振動低減の意味からも性能向上の点からも極めて有用なロボットとなるわけで、このような滑らかで迅速な動作をすることができる優れたロボットの出現が望まれる。
【００３０】
本発明は上記の問題に鑑みなされたもので、その目的は、移動区間ごとに記録された補間種別が異なる経路の継ぎ目（教示点）で、ＴＣＰ速度に非連続性が生じないようにすること、速度の連続性を高めると共に速度性能を上げて動作の機敏化、ロボットアームへの加振抑制、作業中のぶれ防止等を実現し、それによって作業能率の向上を目指すことができる多関節型ロボットの速度制御法および制御装置を提供することである。
【００３１】
【課題を解決するための手段】
本発明は、教示点から次の教示点までの区間に幾つかの補間点を与え、移動区間ごとに演算された個々の速度関数に従って関節補間動作や直線補間動作により移動させるようにした多関節型ロボットの速度制御装置に適用される。その特徴とするところは、図１を参照して、あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別すると共に、その２つの移動区間の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定する速度結合処理可否判定部２と、その速度結合処理可否判定部２により速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせたように挙動させるための補間点を演算する速度結合演算部３とを具備したことである。
【００３２】
制御方法の発明は、あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別し、その２つの移動区間の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定し、速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせるような挙動をさせるべく関節変数を決定するようにしたことである。
【００３３】
異種補間動作が関節補間から直線補間に移る場合の速度結合は、現移動区間における関節補間動作の現時刻に達成されるロボット全軸の関節変数Ｊθ１ｊｉ 〜Ｊθ６ｊｉ を算出し、これを順演算して得られるロボット基準座標系から見たＴＣＰの同次変換行列を直線補間動作する次の移動区間の始点と見たてる。その直線補間動作の現時刻で達成される並進速度関数Ｌｖｐｊ＋１ と回転速度関数Ｌｖｆｊ＋１ から算出した並進運動量Ｌｐ（ｊ＋１）ｉと回転運動量Ｌｆ（ｊ＋１）ｉにより、始点とされた同次変換行列ＪＸ（ｊ＋１） から見た同次変換行列ＬＡ（ｊ＋１）ｉを算出し、始点を表す同次変換行列ＪＸ（ｊ＋１） に並進運動量Ｌｐ（ｊ＋１）ｉと回転運動量Ｌｆ（ｊ＋１）ｉとから算出された同次変換行列ＬＡ（ｊ＋１）ｉを乗じて、１補間周期の間に補間動作させたときの到達点のロボット基準座標系から見た同次変換行列ＬＸ（ｊ＋１）ｉを算出する。これを逆演算して関節変数Ｌθ１（ｊ＋１）ｉ 〜Ｌθ６（ｊ＋１）ｉ を求め、これによって関節補間による補間量に直線補間による補間量を加えた移動量を得ることにより処理するようにしたことである。
【００３４】
異種補間動作が直線補間から関節補間に移る場合の速度結合は、現移動区間における直線補間動作の現時刻に到達するＴＣＰのロボット基準座標系から見た同次変換行列ＬＸｊｉを算出し、これを逆演算して得られる関節変数を関節補間動作する次の移動区間の始点と見たてる。その関節補間動作の全軸の速度関数Ｊ１ｖｊ＋１ 〜Ｊ６ｖｊ＋１ をもとに現時刻で補間動作を実行したときの各軸の補間移動量Ｊ１ｒ（ｊ＋１）ｉ〜Ｊ６ｒ（ｊ＋１）ｉを算出し、始点を表す全軸の関節変数Ｌθ１（ｊ＋１）ｉ 〜Ｌθ６（ｊ＋１）ｉ に補間動作を実行したときの各軸の補間移動量Ｊ１ｒ（ｊ＋１）ｉ〜Ｊ６ｒ（ｊ＋１）ｉを加算して、１補間周期の間に補間動作させたときのロボットの全軸の関節変数Ｊθ１（ｊ＋１）ｉ 〜Ｊθ６（ｊ＋１）ｉ を算出する。これによって直線補間による補間量に関節補間による補間量を加えた移動量を得ることにより処理される。
【００３５】
速度の結合処理の可否は、現移動区間において減速状態にあり、かつその移動区間における残存減速時間が次の移動区間の加速時間以下となっているかどうかで判定するようにしたことである。
【００３６】
異種補間動作における速度の結合処理が適用される速度関数は、現移動区間の減速時間と次の移動区間の加速時間とを一致させておくとよい。次の移動区間の加速時間が現移動区間の減速時間よりも短い場合には、次の移動区間の加速時間を減速時間に一致させ、現移動区間の減速時間が次の移動区間の加速時間よりも短い場合は、現移動区間の減速時間を加速時間に一致させればよい。
【００３７】
【発明の実施の形態】
以下に、本発明に係る多関節型ロボットの速度制御法および制御装置を、図面に基づいて詳細に説明する。図１は速度制御装置１を含む再生動作ならびに速度制御のための構成を示し、図２は制御の概略を表したフローチャートである。これは、教示点から次の教示点までの区間に幾つかの補間点を与え、移動区間ごとに演算された個々の速度関数に従って関節補間動作または直線補間動作により移動させる際に、異なる補間種別の繋ぎ部分で速度の結合処理を可能にしたものである。
【００３８】
図１において装置の主たるところを概略的に述べる。産業用ロボットの教示されたプログラムの再生動作中に、異なる補間種別で教示された動作の繋ぎ目で、ＴＣＰの動きを止めることなく制御できるようにするため、速度結合処理への移行が可能であるかどうかを判定する手段が備えられる。これは速度結合処理可否判定部２として表され、現在解析中の移動区間における補間動作が減速域で行われており、かつ現解析中の補間動作の残り時間が次の解析対象区間の補間動作の加速時間以下であることを確認する機能を有する。
【００３９】
すなわち、あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に、または直線補間動作から関節補間動作に移り変わるかどうかを判別すると共に、その２つの移動区間の繋ぎ点を通過させる際に速度の結合処理が可能かどうかを判定するようになっている。そして、この速度結合処理可否判定部２により速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせたように挙動させることができるようにするための速度結合演算部３が設けられる。
【００４０】
この速度結合演算部３を含む軌道解析部４は、速度結合処理可否判定部２からの速度の結合処理が可能であるとの信号を受けると、補間動作演算部５における今までの補間種別による補間のための演算に加えて、速度結合演算部３において次の移動区間のための異なる補間種別を絡めた補間演算も同時に実行する。
【００４１】
例えば、関節補間動作から直線補間動作に移行する際の速度結合処理においては、現軌道での関節補間中の現時刻における関節補間点を求め、次の軌道の直線補間を開始する時点の同時変換行列には、今求められた関節補間点を順キネマティクスされたものを使用し、重ね合わせることができない速度関数を直接突き合わせることなく、ＴＣＰの移動量の積み重ねにより速度の結合を実現する。
【００４２】
一方、直線補間動作から関節補間動作に移行するときの速度結合処理においては、現軌道での直線補間中の現時刻において到達したＴＣＰの同次変換行列を求め、次の軌道の関節補間を開始する時点の関節変数には、今求められた同次変換行列を逆キネマティクスしたものを使用し、関節変数の積み重ねにより速度の結合を実現する。これもまた、速度関数を重ね合わせるものではなく、ＴＣＰの移動量の積み重ねにより速度の結合を果たす。いずれの場合も、異種補間動作間の速度を重ね合わせたように挙動させるための補間点を演算するという制御をするようになっている。
【００４３】
ちなみに、一般的な産業用ロボットの制御装置はティーチングプレイバック方式であり、オペレータはティーチペンダントを使用して、所望する作業をロボットコントローラに教示作業プログラムとして記録し、またオペレータからの起動指示信号を受けて、記録済みの教示作業プログラムが再生動作されるようになっている。
【００４４】
ロボットコントローラは、教示作業プログラムを記録するためのハードディスク、操作者が教示作業プログラムを教示するためのティーチペンダント、オペレータが起動指示信号を発信するための操作ボックス、ロボットの手動運転や再生運転を制御するロボット制御プログラム、ロボット制御プログラムが格納されているＲＯＭ、ロボット制御プログラムが実行されるＣＰＵ、ロボット制御プログラム実行中に計算の途中経過を一時的に格納するためのＲＡＭなどから構成される。ロボットの各関節最高速度やロボットアームの長さ、補間周期といった諸元もロボットコントローラ内のＲＯＭに格納される。
【００４５】
ここで、ロボット再生動作制御を簡単に触れる。図２に示すように、ハードディスクから読み出された教示作業プログラムよりロボットの補間動作命令が取り出され、補間種別と動作速度ならびに目標の位置を表すためのロボット全軸の関節角度が、先入れ・先出し（ＦＩＦＯ）処理式の動作命令バッファに格納される（ステップ１、以下Ｓ１などと記す）。動作命令バッファから動作コマンドを取り出した軌道生成部６（図１を参照）は、ロボットの動作コマンドのパラメータとして格納されている関節目標点、動作速度および補間種別、さらには必要に応じて現在関節角度をも考慮し、それらから補間開始点の同時変換行列や各移動区間における速度関数を生成し、前記同様ＦＩＦＯ形式の軌道バッファ７に格納する（Ｓ２）。
【００４６】
本発明は、産業用ロボットの教示作業プログラム再生動作中の速度を、異なる補間種別間であっても途切らせることなく滑らかに変化させること、すなわち教示点の前後で補間種別が違う場合でもＴＣＰ速度に非連続性が発生しないようにするものである。これはロボットの再生動作制御の一機能として実行されるが、以下には特に関連するところにとどめ、すでに知られた再生動作の詳しいアルゴリズムの説明は省く。
【００４７】
軌道解析部４は、軌道バッファ７から軌道データを取り出し、これから補間周期（例えば５ｍｓｅｃ）ごとのサーボドライバの目標関節値となる関節変数を生成する。すなわち、本発明による異種補間動作の速度結合処理がここで実行される（Ｓ４〜Ｓ１０）。なお、サーボ出力部８は補間周期ごとに発せられるトリガ信号によって起動され、サーボドライバ９１ ，９２ ，・・・，９６ に対して一定周期で指令値の更新を実行し、サーボモータ１０１ ，１０２ ，・・・，〜１０６ を駆動して各軸を動かし、アームを介してＴＣＰを変位させる。
【００４８】
もう少し軌道解析部４を詳しく述べる。図１に示すように、これは前記した速度結合処理可否判定部２，速度結合演算部３のほかに、補間種別判定部１１や関節補間動作演算部５Ａ、直線補間動作演算部５Ｂを備える。そして、速度結合演算部３は、関節補間から直線補間への速度結合演算部３Ａと、直線補間から関節補間への速度結合演算部３Ｂを有する。速度結合演算部３での演算結果は関節補間点バッファ１２に格納され、サーボ出力部８からの取り出し指令を受けてサーボドライバ９１ ，９２ ，・・・，９６ に送り出されるようになっている。
【００４９】
上記した関節補間から直線補間への速度結合演算部３Ａは、速度結合処理可否判定部２で現在の移動区間において補間動作が減速域に入っており、かつその移動区間における残存減速時間が次の移動区間の加速時間以下となっていると判定されたとき、速度の結合処理を実行する。
【００５０】
速度結合処理可否判定部２が速度結合処理可能判定を出すまでは、関節補間動作演算部５Ａが補間のための関節変数を算出し、それを関節補間点バッファ１２へ出力する。しかし、速度結合処理可否判定部２が速度結合処理可能判定を出せば、速度結合演算部３Ａは関節補間動作演算部５Ａで得た関節変数を取り入れると共に直線補間動作演算部５Ｂからも次の移動区間のための軌道データを取り込む。現移動区間での関節補間中の現時刻における関節補間点を求め、次の移動区間の直線補間を開始する時点の同時変換行列にはその関節補間点を順キネマティクスして求めたものを使用し、結果として得られる関節変数を関節補間点バッファ１２へ送り出す。
【００５１】
フローチャートを参照しながら後で詳しく述べるが、以下の処理がその根幹をなす。まず、現移動区間における関節補間動作の現時刻に達成されるロボット全軸の関節変数Ｊθ１ｊｉ 〜Ｊθ６ｊｉ が算出される。これを順演算して得られるロボット基準座標系から見たＴＣＰの同次変換行列を直線補間動作する次の移動区間の始点と見たてることにして、その直線補間動作の現時刻で達成される並進速度関数Ｌｖｐｊ＋１ と回転速度関数Ｌｖｆｊ＋１ から算出した並進運動量Ｌｐ（ｊ＋１）ｉと回転運動量Ｌｆ（ｊ＋１）ｉにより、始点とされた同次変換行列ＪＸ（ｊ＋１） から見た同次変換行列ＬＡ（ｊ＋１）ｉが算出される。始点を表す同次変換行列ＪＸ（ｊ＋１） に並進運動量Ｌｐ（ｊ＋１）ｉと回転運動量Ｌｆ（ｊ＋１）ｉとから算出された同次変換行列ＬＡ（ｊ＋１）ｉを乗じて、１補間周期の間に補間動作させたときの到達点のロボット基準座標系から見た同次変換行列ＬＸ（ｊ＋１）ｉが算出されれば、これを逆演算して関節変数Ｌθ１（ｊ＋１）ｉ 〜Ｌθ６（ｊ＋１）ｉ を求める。これによって関節補間による補間量に直線補間による補間量を加えた移動量を得ることにより、速度結合処理が行われることになる。
【００５２】
上記した直線補間から関節補間への速度結合演算部３Ｂも、速度結合処理可否判定部２で現在の移動区間において減速状態にあり、かつその移動区間における残存減速時間が次の移動区間の加速時間以下となっていると判定されると、速度の結合処理を実行する。
【００５３】
速度結合処理可否判定部２が速度結合処理可能判定を出すまでは、直線補間動作演算部５Ｂが補間のための関節変数を算出し、それを関節補間点バッファ１２に出力する。しかし、速度結合処理可否判定部２が速度結合処理可能判定を出せば、速度結合演算部３Ｂは直線補間動作演算部５Ｂで得た関節変数を取り入れると共に関節補間動作演算部５Ａからも次の移動区間のための軌道データを取り込む。現移動区間での直線補間中の現時刻において到達したＴＣＰの同次変換行列を求め、次の移動区間の関節補間を開始する時点の関節変数にはその同次変換行列を逆キネマティクスして求めたものを使用し、結果として得られる関節変数を関節補間点バッファ１２へ送り出す。
【００５４】
この場合の処理は、以下の点が主たるところである。まず、現移動区間における直線補間動作の現時刻に到達するＴＣＰのロボット基準座標系から見た同次変換行列ＬＸｊｉが算出される。これを逆演算して得られる関節変数を関節補間動作する次の移動区間の始点と見たて、その関節補間動作の全軸の速度関数Ｊ１ｖｊ＋１ 〜Ｊ６ｖｊ＋１ をもとに現時刻で補間動作を実行したときの各軸の補間移動量Ｊ１ｒ（ｊ＋１）ｉ〜Ｊ６ｒ（ｊ＋１）ｉが算出される。始点を表す全軸の関節変数Ｌθ１（ｊ＋１）ｉ 〜Ｌθ６（ｊ＋１）ｉ に補間動作を実行したときの各軸の補間移動量Ｊ１ｒ（ｊ＋１）ｉ〜Ｊ６ｒ（ｊ＋１）ｉを加算して、１補間周期の間に補間動作させたときのロボットの全軸の関節変数Ｊθ１（ｊ＋１）ｉ 〜Ｊθ６（ｊ＋１）ｉ が求められる。これによって、直線補間による補間量に関節補間による補間量を加えた移動量が得られることになる。
【００５５】
以下に図４からのフローチャートを参照し、異種補間動作時の速度結合に関して具体的に説明する。なお、関節補間動作から直線補間動作への乗り移りによる速度結合処理や直線補間動作から関節補間動作への乗り移りによる処理をする場合の特徴を把握しやすくするため、速度結合処理しない場合も併せて述べる。その説明順序は次に掲げる図３の（ｂ）のタイムチャートに従う。また、後で述べる事項との関連も予めとっておく都合上、速度結合処理前になされる軌道データの生成についても触れる。
【００５６】
まず、従来技術の項で述べた図２１の（ａ）の動きを想定したタイムチャートは図２１の（ｂ）であるが、本発明は（ｃ）にあるように、一連の速度関数のうちの２か所に見られる速度の結合処理を行う。図３の（ａ）は図２１の（ｂ）の順序を一部入れ替えてフローチャートの説明順に合わせたものである。この一連の動きにおいて、本発明に係る速度結合処理を行わせようとしたものが図３の（ｂ）である。
【００５７】
その速度結合処理は、関節補間動作から直線補間動作へ移るところとその逆の移り変わりをするところの２か所に現れている。図中に付されたＳ番号はフローチャートに付したステップ番号である。その番号群は、例えば「関節補間動作」から「直線補間動作」へ移るところの「速度結合処理」とその前後を含む動作がフローチャートのどの部分に当たるかを示している。
【００５８】
この再現動作では、ロボットによる作業点をＰ３ とＰ５ の２点としている。まず、退避位置Ｐ１ から作業第１準備位置Ｐ２ までは、関節補間動作により迅速にワークピースへ接近させる。点Ｐ２ で一旦停止させた後、第１作業点Ｐ３ に向けては直線補間動作によって前進させる。この点Ｐ３ へは高いポーズ精度でもって到達させ、ここでも停止させる。
【００５９】
第１作業点Ｐ３ から例えば別のワークピースのために設けた作業第２準備位置Ｐ４ までは関節補間動作により迅速に移動させ、その点では静止させることなく直線補間動作によって第２作業点Ｐ５ に向けて前進させる。この点Ｐ５ では高いポーズ精度でもって静止させる。点Ｐ５ からは直線補間動作で作業解放位置Ｐ６ へ移動させ、そこでは停止させることなく関節補間動作によって迅速に退避位置Ｐ１ （＝Ｐ７ ）へ戻すというプロセスとなっている。
【００６０】
以下、フローチャートに基づいて詳細に説明する。まず、再現動作するにあたり教示作業プログラムが読み出される（Ｓ１０１）。このときカウンタｊが０にセットされる（Ｓ１０２）。このカウンタｊは移動区間の順番を表すものである。教示作業プログラムには、移動中にたどる各教示点Ｐｊ （１≦ｊ≦Ｎ）での全軸の関節角度θ１ｊ〜θ６ｊ、関節補間される各移動区間（例えば教示点Ｐｊ ・Ｐｊ＋１ 間）での全軸の動作速度と直線補間される他の各移動区間でのＴＣＰの並進速度といった動作速度ｖ１ｊ〜ｖ６ｊ、サーボモータの仕様やアームの重量によって決まる各軸での許容最大速度ならびに各軸に適用される加速度・減速度、各移動区間での補間種別、各教示点でのポーズ精度等が含まれており、これらが動作命令バッファに格納される（Ｓ１０３）。なお、上記のＮは図３（ｂ）の例で言えば７に当たり、教示点Ｐ１ ，Ｐ７ はロボットの退避位置を意味する。
【００６１】
これらを基にして軌道生成が行われるが、それは点Ｐ１ からＰ２ である最初の移動区間から始められる。そのため移動区間の番号を表すべくカウンタｊに１が加えられ（Ｓ１０４）、始点Ｐｊ （＝Ｐ１ ）と終点Ｐｊ＋１ （＝Ｐ２ ）からなる移動区間の補間種別が「関節補間」かどうか問われる（Ｓ１０５）。「関節補間」であればステップ２０１へ移行し、「直線補間」であれば（Ｓ１０６）、図９のステップ２０６へ移行される。いずれでもなければ「円弧補間」とみなして、そのための処理が実行される（Ｓ１０７）。なお、上でも少し触れたが円弧補間は実質的に直線補間と同じであるので、以後の直線補間の説明でもってそれに充てることにし、その説明を省く。
【００６２】
ステップ２０１へ進むと、軌道生成部が動作命令バッファから動作命令を取り出し、教示データを解析してロボットの軌道が生成される。すなわち、１番目の教示点における全軸の関節角度θ１ １ 〜θ６ １ （＝θ１ ｊ 〜θ６ ｊ ＠ｊ＝１）から、その関節変数Ｊ１ｓ１ 〜Ｊ６ｓ１ （＝Ｊ１ｓｊ 〜Ｊ６ｓｊ ＠ｊ＝１）を生成する。ちなみに、６軸ロボットを例にしているので、関節角度はθ１ １ からθ６ １ の６つ設けられる。次に、本移動区間の始点Ｐ１ の関節角度θ１ １ 〜θ６ １ と終点Ｐ２ の関節角度θ１ ２ 〜θ６ ２ （＝θ１ ｊ＋１ 〜θ６ ｊ＋１ ）を比較し、指定された全軸の動作速度ｖ１ １ 〜ｖ６ １ （＝ｖ１ ｊ 〜ｖ６ ｊ ＠ｊ＝１）や各軸の加速度・減速度に基づく本移動区間での全軸の速度関数Ｊ１ｖ１ 〜Ｊ６ｖ１ （＝Ｊ１ｖｊ 〜Ｊ６ｖｊ ＠ｊ＝１）を生成する。さらに、この速度関数Ｊ１ｖ１ 〜Ｊ６ｖ１ から本移動区間における加速時間および減速時間を算出する。最後に、ロボットコントローラ内のＲＯＭに格納されている補間周期ｔｃ を呼び出し、これをもとにこの最初の（ｊ＝１）移動区間での補間回数Ｉ１ も算出される。
【００６３】
このようにして得られた最初の教示点の関節変数、最初の移動区間での速度関数、加速時間・減速時間、補間種別、補間回数、終点である教示点Ｐ２ のポーズ精度等の軌道データが、軌道バッファに格納される（Ｓ２０２）。
【００６４】
２番目の教示点Ｐ２ および２番目の移動区間についての軌道データを求めて軌道バッファに格納すべく、カウンタｊに１が加えられて２とされる（Ｓ２０３）。ｊは６である（Ｎ−１）に到っていなく（Ｓ２０４）、図４のステップ１０５へ戻される。図３の（ｂ）から分かるように、２番目の移動区間は「直線補間」が適用されているから、ステップ１０６を経て図９のステップ２０６からの処理が始められる。
【００６５】
ステップ２０６では軌道生成部が動作命令バッファから動作命令を取り出し、教示データを解析してロボットの軌道を生成する。すなわち、２番目の教示点Ｐ２ における全軸の関節角度θ１ ２ 〜θ６ ２ からその関節変数Ｌ１ｓ２ 〜Ｌ６ｓ２ を生成し、その関節変数を順演算して２番目の教示点の同次変換行列ＬＡ２ が算出される。次に、この移動区間の始点Ｐ２ の同次変換行列ＬＡ２ と終点Ｐ３ の同次変換行列ＬＡ３ 、上記関節角度θ１ ２ 〜θ６ ２ や指定された並進速度ｖ１ ２ 〜ｖ６ ２ さらには各軸の加速度・減速度とから、この移動区間での並進速度関数Ｌｖｐ２ と回転速度関数Ｌｖｆ２ とが計算される。さらに、並進速度関数Ｌｖｐ２ と回転速度関数Ｌｖｆ２ からこの移動区間における加速時間および減速時間を算出する。最後に、補間周期ｔｃ をもとに、本移動区間での補間回数Ｉ２ も算出される。
【００６６】
このようにして得られた２番目の教示点の同時変換行列、この移動区間での並進速度関数・回転速度関数、加速時間・減速時間、補間種別、補間回数、終点である教示点Ｐ３ のポーズ精度等の軌道データが、軌道バッファに格納される（Ｓ２０７）。
【００６７】
３番目の教示点Ｐ３ および３番目の移動区間についての軌道データを求めて軌道バッファに格納すべく、カウンタｊに１が加えられて３とされる（Ｓ２０８）。ｊは６である（Ｎ−１）に到っていなく（Ｓ２０９）、図４のステップ１０５に戻される。図３の（ｂ）から分かるように３番目の移動区間は関節補間が適用されるから、ステップ２０１から前述した処理が繰り返される。このようにしてｊが６となる６番目の移動区間の関節補間のための軌道データが生成され、それが格納されるとステップ２０３または２０８で１が加えられる。これによってｊが６を越えることになるので（Ｓ２０４ またはＳ２０９ ） 、ｊを０にリセットして（Ｓ２０５ またはＳ２１０）、次の処理のための準備が整えられる。
【００６８】
上記したステップ２０２や２０７で格納される軌道データは、全ての移動区間についての全ての補間分となるとかなりの量になる。通常軌道バッファはＦＩＦＯ処理式のものが採用され、順次格納しては残りの演算中に格納中の古いものから順に実際の動作のため次々と引き出されていくようにしていることが多い。しかし、このフローチャートでは全ての移動区間の軌道データが一旦蓄えられるものとして、すなわちバッファに容量の大きいものが採用されているとして、以後の説明を続けることにする。
【００６９】
次の処理であるステップ３０１以降は速度結合処理において特殊な配慮を施す場合に適用されるもので、その説明は後に譲り、ステップ４０１以降の動作を説明する。ここからは、軌道バッファに蓄えられた軌道データをもとに、ロボットを動かすべく補間動作する制御に入る。最初に行われる処理の対象は１番目（ｊ＝１）の移動区間であり、その補間種別が「関節補間」であるので、そのための動作が説明される。
【００７０】
その後、２番目（ｊ＝２）の移動区間における「直線補間」による補間動作が述べられる。ここまでは従来からなされている補間動作である。その後、ｊ＝３の移動区間における「関節補間」の途中までの補間動作処理、その残りの減速域の全部もしくは一部での関節補間動作と４番目の移動区間の「直線補間」の加速域の全部もしくは一部における直線補間動作との重合計算により実行される速度結合処理、および４番目の移動区間の残りの直線補間動作処理からなる一連の処理が説明される。この動作は言うまでもなく本発明の主題とするところである。同様に、ｊ＝５の移動区間における「直線補間」の途中までの補間動作処理、その残りの減速域の全部もしくは一部の直線補間動作と６番目の移動区間の「関節補間」の加速域の全部もしくは一部の関節補間動作との重合計算により実行される速度結合処理、および６番目の移動区間の残りの関節補間動作処理からなる一連の処理が説明される。
【００７１】
先ず図６のステップ４０１において最初の移動区間を処理すべく、ステップ２０５または２１０で０にリセットされているカウンタｊに１が加えられる。これによって移動区間が１番目のものであると特定され、この移動区間での例えば５ｍｓｅｃなる補間周期による補間のための演算が開始される。その最初の補間に入る前にカウンタｉが０と置かれる（Ｓ４０２）。説明を後回しするカウンタｋも０と置かれる（Ｓ４０３）。
【００７２】
補間のための演算には、当然のことながらこの移動区間のための軌道データを取り込む必要がある。そこで、軌道解析部に１番目の移動区間のための解析対象軌道Ｔ１ が取り出されているか確認される（Ｓ４０４）。今は最初であるので軌道バッファより１番目の移動区間のための軌道データが解析対象軌道Ｔ１ として取り出される（Ｓ４０５）。その軌道データから、始点Ｐ１ と終点Ｐ２ からなるこの１番目の移動区間の補間種別は「関節補間」であるかどうか問われる（Ｓ４０６）。「直線補間」なら図１０のステップ６０１へ移行されるが、１番目の移動区間は「関節補間」されるものであるのでステップ５０１へ進む。
【００７３】
１番目の移動区間の始点Ｐ１ における全軸の関節変数Ｊ１ｓ１ 〜Ｊ６ｓ１ が軌道データから取り出される（Ｓ５０１）。この移動区間における全軸での速度関数Ｊ１ｖ１ 〜Ｊ６ｖ１ も軌道データから取り出される（Ｓ５０２）。ここで、１番目の移動区間の最初の補間動作のための演算を始めるべく、カウンタｉに１が加えられて（Ｓ５０３）、１番目の補間のための演算準備に入る。１番目の移動区間における全軸の速度関数Ｊ１ｖ１ 〜Ｊ６ｖ１ をもとにして、最初の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ １ １〜Ｊ６ｒ １ １が算出される（Ｓ５０４）。これは、１番目の補間動作で達成される各軸の移動量を示す。
【００７４】
次に、始点Ｐ１ における全軸の関節変数Ｊ１ｓ１ 〜Ｊ６ｓ１ に、最初の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ １ １〜Ｊ６ｒ １ １を加算して、その補間動作を実行したときのロボットの全軸の関節変数Ｊθ１ １ １ 〜Ｊθ６ １ １ が算出される（Ｓ５０５）。これは、この補間動作で到達すべき全軸の目標関節変数である。そして、これら全軸の関節変数Ｊθ１ １ １ 〜Ｊθ６ １ １ が補間点である各関節の指令値として関節補間点バッファに格納される（Ｓ５０６）。
【００７５】
今は図３の（ｂ）の最初の移動区間における一連の補間動作を説明しているので、「速度結合処理」は必要でなく、後で説明するステップ７０１を素通りしてステップ５０７へ跳ぶ。内蔵タイマ等により教えられる補間周期ごとのトリガ信号に基づき、関節補間点バッファ１２から全軸の関節変数Ｊθ１ １ １ 〜Ｊθ６ １ １ を各軸の指令値にして図１で説明したサーボ出力部８へ送出される。これによって、各関節が１番目の補間目標関節角度θ１ １ １ 〜θ６ １ １ に向けて移動される（Ｓ５０８）。
【００７６】
フローチャートの今の説明では、全軸の関節変数Ｊθ１ １ １ 〜Ｊθ６ １ １ の指令値が関節補間点バッファに格納されると、２番目の補間のための演算が開始される前に、１番目の補間のための関節変数がサーボ出力部へ送出されるように表わした。実際は、関節補間点バッファも先に述べたＦＩＦＯ処理式のものが採用されていることが多く、その場合には、幾つかの補間のための各関節の指令値が蓄積されてから、格納の早い順に補間周期ごとに出力されていくことになる。
【００７７】
上の説明で最初の補間が済んだのでカウンタｉに１が加えられ（Ｓ５０９）、２となったｉでもって、ステップ５１０を経て図６のステップ５０４に戻される。このステップ５０４から５０８までは上記と同じ処理であり、その関節変数Ｊθ１ １ ２ 〜Ｊθ６ １ ２ が出力されると、各関節が２番目の補間目標関節角度θ１ １ ２ 〜θ６ １ ２ に向けて移動される。
【００７８】
順次カウンタｉに１が加えられ（Ｓ５０９）、その値がこの移動区間における補間数のＩ１ に到達するまで繰り返される。ｉがそれを越えた時点で（Ｓ５１０）、１番目の移動区間の移動が完了したと判断され、カウンタｊに１が加えられ（Ｓ５１１）る。ステップ５１２を経て、図６のステップ４０２に戻される。ｉが０にリセットされ、前記したステップ４０４を経てステップ４０６へ進められ、２番目の移動区間が如何なる補間種別であるかが問われる。図３の（ｂ）から分かるように「直線補間動作」させるべき移動区間であるから、図１０のステップ６０１へ移行される。
【００７９】
２番目の移動区間の始点Ｐ２ を表す同次変換行列ＬＡ２ が軌道データから取り出される（Ｓ６０１）。この移動区間における並進速度関数Ｌｖｐ２ と回転速度関数Ｌｖｆ２ も軌道データから取り出される（Ｓ６０２）。０にリセットされていたカウンタｉに１が加えられ（Ｓ６０３）、２番目の移動区間の最初の補間のための演算準備に入る。
【００８０】
２番目の移動区間における並進速度関数Ｌｖｐ２ と回転速度関数Ｌｖｆ２ とから１番目の補間動作を実行させたときの並進運動量Ｌｐ２ １ と回転運動量Ｌｆ２ １ が算出される（Ｓ６０４）。これは、１番目の補間動作で達成される補間点の並進と回転の運動量を示す。
【００８１】
次に、並進運動量Ｌｐ２ １ と回転運動量Ｌｆ２ １ より、１番目の補間動作を実行させたときの始点Ｐ２ から見た補間点の同次変換行列ＬＡ２ １ を算出する（Ｓ６０５）。これは、今演算された補間動作で到達すべき補間点の始点Ｐ２ から見た同次変換行列である。
【００８２】
始点Ｐ２ の同次変換行列ＬＡ２ に同次変換行列ＬＡ２ １ を乗じて、１番目の補間動作を実行させたときのロボット基準座標系から見た到達点の同次変換行列ＬＸ２ １ を算出する（Ｓ６０６）。これは、最初の補間動作で達成されるＴＣＰのロボット基準座標系から見た同次変換行列である。
【００８３】
この同次変換行列ＬＸ２ １ を逆演算して、その関節変数Ｌθ１ ２ １ 〜Ｌθ６ ２ １ が求められる（Ｓ６０７）。すなわち、これが最初の補間動作で達成される全軸の目標関節変数となる。これら全軸の関節変数Ｌθ１ ２ １ 〜Ｌθ６ ２ １ が、補間点である各関節の指令値として関節補間点バッファに格納される（Ｓ６０８）。
【００８４】
今は図３の（ｂ）の２番目の移動区間における補間動作を説明しているので、「速度結合処理」は必要でなく、ステップ９０１を素通りしてステップ６０９へ跳ぶ。内蔵タイマ等により指示される補間周期ごとのトリガ信号に基づき、関節補間点バッファ１２から全軸の関節変数Ｌθ１ ２ １ 〜Ｌθ６ ２ １ を各軸の指令値にしてサーボ出力部８へ送出する。これによって、各関節が１番目の補間目標関節角度θ１ ２ １ 〜θ６ ２ １ に向けて移動される（Ｓ６１０）。この場合も、ＦＩＦＯ処理式のものが採用されていれば、幾つかの指令値が蓄積されてから補間周期ごとに順次出力されることは上記のとおりである。
【００８５】
最初の補間が済んだのでカウンタｉに１が加えられ（Ｓ６１１）、２となったｉでもって、ステップ６１２を経て図１０のステップ６０４からの動作に戻される。以後同様の要領で処理がステップ６１０まで進められる。２番目の補間が済むとカウンタｉに１が加えられ（Ｓ６１１）、３となったｉでもって、ステップ６１２を経てステップ６０４からの動作が繰り返される。この移動区間における補間数はＩ２ であるのでｉがそれに到達するまで繰り返され、ｉがＩ２ を越えると（Ｓ６１２）、この２番目の移動区間の移動のための補間演算は完了する。
【００８６】
ステップ６１２において２番目の移動区間の移動が完了したと判断されると、カウンタｊに１が加えられ（Ｓ６１３）、ステップ６１４を経て図６のステップ４０２へ戻される。ｉが０にリセットされ、ステップ４０３，４０５を経てステップ４０６へ進められ、３番目の移動区間が如何なる補間種別であるかが問われる。図３の（ｂ）から分かるように「関節補間」すべき移動区間であるから、ステップ５０１へ移行される。ここからが、本発明に係る「速度結合処理」を含む制御動作の説明となる。図１６と図１７も併せて参照する。
【００８７】
ステップ５０１以降ステップ５１０までの動作が、前述した要領で繰り返される。図１６は教示点Ｐ３ ，Ｐ４ ，Ｐ５ について表わされており、その左側の点Ｐ３ ・Ｐ４ 間の台形波パターンで表された速度関数Ｊ１−６ ｖ３の部分で、その進展の様子を見ることができる。（ａ）は１番目の補間をした様子を示し、その間の移動量は影を施した三角形の面積のＪ１−６ ｒ３ １であり、その補間点を表す関節変数Ｊθ１−６ ３ １ はＪ１−６ ｓ３とＪ１−６ ｒ３ １との和で与えられる。なお、添え字のうちの１−６ は６軸分を一括して表記したことを意味する。
【００８８】
（ｂ）は４番目の補間を施した時点の進み状態を表している。（ｃ）は加速域を脱して定速域に入っている様子を示している。図の台形波パターンは波形のコンパクト化を図るために敢えて短く表されている。（ｄ）は減速域に入ろうとする時点であり、補間は図において１４回目（ＪＴ１４＝補間周期ｔｃ ×１４）に至っている。このｉ＝１４の演算において、ステップ５０４から５０６までの処理をした後にステップ７０１に進んでも、そこは素通りしてステップ５０７へ跳ばされる。
【００８９】
しかし、ｉ＝１５の場合にステップ７０１に至ると、今までも処理はされていたが説明を省いていた以下の判定がなされ、その判定どおりであるとステップ７０２以降の処理へ進められることになる。これを図１６に存在しないｉ＝１５の処理で説明する。１５番目の補間演算としてステップ５０４から５０５に至り、ステップ５０６で全軸の関節変数Ｊθ１ ３ １５〜Ｊθ６ ３ １５を補間点としてバッファに格納する。
【００９０】
ところで、３番目の移動区間における補間回数Ｉ３ や減速時間Ｔｄｅ３ は、３番目の移動区間の補間処理に入った時点のステップ４０５で解析対象軌道Ｔ３ としてすでに取り出されている。従って、定速域の最終補間は、補間回数Ｉ３ −（減速時間Ｔｄｅ３ ／補間周期ｔｃ ）の番目である。これをＩ３ｃとすれば、３番目の移動区間における補間動作の１５番目を意味する１５は、３番目の移動区間における定速域最後の補間までの補間回数Ｉ３ｃ＝１４より大きいので、この１５番目の補間から減速域へ入ったと判断され、ステップ７０２へ進められる。
【００９１】
ステップ７０２では、軌道解析部に３番目の次の移動区間である４番目のための解析対象軌道Ｔ４ が取り出されているかが調べられる。このステップへ進んだのは初めてであるから取り出しは未だ行われていなく、そこで軌道バッファより４番目の移動区間のための軌道データが解析対象軌道Ｔ４ として取り出される（Ｓ７０３）。その軌道データの点Ｐ４ に課せられたポーズ精度の大小を見て「速度結合処理」が可能かどうか判定される（Ｓ７０４）。
【００９２】
ポーズ精度が高ければ速度結合処理が見合わされ、ステップ５０７へ進められる。ポーズ精度が低く予め決められた精度以下であると「速度結合処理」が可能と判断される。いまの解析対象軌道Ｔ３ における残存減速時間ｔＲ ＝（Ｉ３ −ｉ）×ｔｃ ＝（２２−１５）×５ｍｓｅｃが計算される（Ｓ７０５）。その残存減速時間ｔＲ が解析対象軌道Ｔ４ の加速時間Ｔａｃ４ より少ないかどうかが判定される（Ｓ７０６）。ｔＲ がＴａｃ４ より大きければ、ステップ５０７へ進められる。このような処理は図１６で言えば（ｅ）である１７番目の補間まで続けられる。
【００９３】
補間のための演算が繰り返されｉがステップ５０９で１８にまで進むと、図１７の（ａ）に示すようにｔＲ ＝Ｔａｃ４ となる（Ｓ７０６）。しかし、この場合もステップ５０７へ先送りされる。図から分かるように４番目の移動区間の速度関数Ｌｖｐｆ４は０であり、そこでの補間量は計算するまでもなく０である。なお、添え字のうちのｐｆは、並進と回転の運動を一括して表記したことを意味する。
【００９４】
ｉが１９まで進められると図１７の（ｂ）に示すようにｔＲ ＜Ｔａｃ４ となり、ステップ７０７へ進められる。ここでは、解析対象軌道Ｔ４ での補間種別が「直線補間」であるかどうかが問われる。「直線補間」でなければすなわち「関節補間」であるなら、従来技術の項で述べた「関節補間」同士の「速度重のね処理」が実行される（Ｓ７０８）。
【００９５】
「直線補間」であれば、「関節補間」と「直線補間」との「速度結合処理」の実行に入り、ステップ４０３で０と置かれていたｋに１が加えられる（Ｓ７０９）。なお、ｋの意味するところは、３番目の移動区間の補間をｉで積み重ねて進ませようとするのに対して、その途中から行われる４番目の移動区間の補間処理をｋの積み重ねで進ませようとするものである。いまｋが１になったばかりであり、ステップ７１０ではｋ＝１が肯定され、いままで積み重ねられてきたｉの１９がｇに置き換えられて（Ｓ７１１）、図１２のステップ８０１へ進められる。
【００９６】
ステップ５０５で得られたロボットの全軸の関節変数Ｊθ１ ３ １９〜Ｊθ６ ３ １９を順演算し、その結果得られるロボット基準座標系から見た同次変換行列ＪＸ３ １９が算出され、これを４番目の移動区間においてこれから行おうとする補間の始点と見たててＪＸ４ と置かれる（Ｓ８０１）。
【００９７】
４番目の移動区間における並進速度関数Ｌｖｐ４ と回転速度関数Ｌｖｆ４ が軌道データから取り出される（Ｓ８０２）。ここで、１と置かれているカウンタｋをカウンタｉに置き替える（Ｓ８０３）。いま置かれたｉによって、４番目の移動区間の補間の進行を１から順次増やして使用することができるようになる。次に、説明を後に譲るが、ｍが０と置かれる（Ｓ８０４）。
【００９８】
その状態で、４番目の移動区間における並進速度関数Ｌｖｐ４ と回転速度関数Ｌｖｆ４ とから１番目の補間動作を実行させたときの並進運動量Ｌｐ４ １ と回転運動量Ｌｆ４ １ を算出する（Ｓ８０５）。これは「速度結合処理」の開始から最初の補間動作で達成される補間点の並進と回転の運動量を示す。ここで、ｍが０であるかどうか問われる（Ｓ８０６）。
【００９９】
ｍがいま０と置かれている状態にあるので、ステップ８０７へ進められる。ここでは、並進運動量Ｌｐ４ １ と回転運動量Ｌｆ４ １ より、１番目の補間動作を実行させたときの「同次変換行列ＪＸ４ から見た補間点の同次変換行列ＬＡ４１」が算出される。これは最初の補間動作で到達すべき補間点の同次変換行列ＪＸ４ から見た同次変換行列である。
【０１００】
次に、この同次変換行列ＪＸ４ に同次変換行列ＬＡ４１を乗じて、１番目の補間動作を実行させたときの「ロボット基準座標系から見た到達点の同次変換行列ＬＸ４１」が算出される（Ｓ８０８）。これは、最初の補間動作で達成される点のロボット基準座標系から見た同次変換行列である。
【０１０１】
この同次変換行列ＬＸ４１を逆演算して、その関節変数Ｌθ１ ４ １ 〜Ｌθ６ ４ １ が求められる（Ｓ８０９）。すなわち、１番目の補間動作で達成される全軸の目標関節変数であり、これがステップ８１０において全軸の補間点（各関節の指令値）として関節補間点バッファに格納される（Ｓ８１０）。これは、「速度結合処理」に基づき演算された「関節補間」と「直線補間」の速度を重ね合わせたかのように挙動させるための補間点である。
【０１０２】
補間周期ごとのトリガ信号が発せられると（Ｓ８１１）、図１７の（ｂ）に示すように、関節補間点バッファから全軸の関節変数Ｌθ１ ４ １ 〜Ｌθ６ ４ １ を各軸の指令値にしてサーボ出力部へ送出され（Ｓ８１２）、各関節が１番目の補間目標関節角度θ１ ４ １ 〜θ６ ４ １ に向けて移動を開始する。すなわち、ＴＣＰがその関節角度となるように動かされる。図中のＳ４ １ は４番目の移動区間の１番目の補間による移動量であり、これが３番目の移動区間における１９番目の補間による移動量のＪθ１−６ ３ １９すなわちＪＸ４ に上乗せしたように与えられる。図中に表したＳ４ １ ａ をＳ４ １ に等しく描けば、そのＬＸ４ １ を指している矢印のところの縦座標値Ｖｍ１９が、そのときの速度を表すことになる。
【０１０３】
いまはｍ＝０であるので（Ｓ８１３）、ステップ８１４に進んでｉがｇ＋ｋと置かれる。すなわちｉはステップ７１１で１９と置かれたｇと１のｋとが足され、２０とされる。この操作によってｉは３番目の移動区間の補間の回数に戻されたことになる。そこで、ｉが３番目の移動区間の補間回数のＩ３ ＝２２より大きくなっているかが問われる（Ｓ８１５）。
【０１０４】
そうでなければ図６のステップ５０４へ戻され、３番目の移動区間における２０番目の補間のための演算が開始される。以後、ステップ５０５に至ることによってＪθ１ ３ ２０〜Ｊθ６ ３ ２０が求められ、それがバッファに格納された後（Ｓ５０６）、ステップ７０１に進んでステップ７０９でｋに１が加えられて２とされる。ｋはもはや１でないのでステップ７１１は跳ばされ、ｇは従前の１９に据え置かれる。
【０１０５】
図１２のステップ８０１に入るとステップ５０５で得られたロボットの全軸の関節変数Ｊθ１ ３ ２０〜Ｊθ６ ３ ２０を順演算し、その結果得られるロボット基準座標系から見た同次変換行列ＪＸ３ ２０が算出され、これを４番目の移動区間においてこれから行おうとする２番目の補間の始点と見たててＪＸ４ と置かれる。
【０１０６】
ステップ８０３へ至ると２と置かれているカウンタｋがカウンタｉに置き替えられる。いま置かれたｉ（＝２）によって、４番目の移動区間の２番目の補間の処理がなされる準備が整う。そしてステップ８１２まで進展され、３番目の移動区間における２０番目の補間による移動量に４番目の移動区間における２番目の補間による移動量が上乗せされ、それが全軸の関節変数Ｌθ１ ４ ２ 〜Ｌθ６ ４ ２ なるかたちで各軸の指令値としてサーボ出力部へ送出される（Ｓ８１２）。各関節は２番目の補間目標関節角度θ１ ４ ２ 〜θ６ ４ ２ に向けて移動することになる。以上の動作は図１７の（ｃ）に示されている。
【０１０７】
このようにして３番目の移動区間を補間処理するためのｉが２２になるまで、同じく４番目の移動区間を補間処理するためのｋが４となるまで、すなわち図１７の（ｄ）の状態になるまで繰り返される。ステップ８１４でｉ＝１９＋４＝２３となるとｉが２２のＩｊ を越える（Ｓ８１５）。すなわち、もはや３番目の移動区間の補間は演算するに及ばなくなる。これは「速度結合処理」から解放されることを意味する。しかし、４番目の移動区間の残りの補間はＴＣＰが４番目の移動区間の終点Ｐ５ に至るまで続けられねばならない。
【０１０８】
速度重ね処理時の（ｊ＋１）番目の移動区間の始点は、ｊ番目の移動区間におけるｉ番目の補間動作により到達しようとする点が採用されている。従って、３番目の移動区間の補間が完了した後の４番目の移動区間における残りの補間動作の始点は、３番目の移動区間の補間動作が完了したとき到達している点である。これは言うまでもなく教示点Ｐ４ である。
【０１０９】
そこで、これ以後４番目の移動区間の残りの補間動作における始点はＰ４ となる。これはステップ７０３（図７を参照）で取り出したＴ４ の軌道データから、その始点Ｐ４ を表す同次変換行列ＬＡ４ で与えられる。言うまでもなく、３番目の移動区間の全軸の関節変数Ｊθ１ ３ ２２〜Ｊθ６ ３ ２２を順演算して得られるロボット基準座標系から見た同次変換行列ＪＸ３ ２２と同じものである。そこで、いずれをとってもよいが、ステップ８１６（図１３を参照）では単純にＪＸ４ ＝ＬＡ４ としてＬＡ４ をＪＸ４ にあてがうことにする。これを前提にして以後図１７の（ｅ）のように、３番目の移動区間によって達成された移動量に４番目の移動区間である濃い影の施された部分の移動量が重ねられるようにして、４番目の移動区間の終点Ｐ５ まで順次補間演算が続けられる。
【０１１０】
ステップ８１７へ進んでｉは４であるｋに１が加えられて５とされ（Ｓ８１７）、０のままのｍに１が加えられて１と置かれる（Ｓ８１８）。ｍが１になったということは、「速度結合処理」を実質的に脱して、４番目の移動区間の残りの補間のみの演算に入っていることを教える。いまｉが５であり４番目の移動区間の補間回数Ｉｊ＋１ の１６を越えていないので（Ｓ８１９）、図１２のステップ８０５に戻り、並進運動量Ｌｐ４ ５ と回転運動量Ｌｆ４ ５ が算出される。
【０１１１】
ｍは１であるので、ステップ８０６を経てステップ８２０へ進められる。ここでは上記の説明に基づいて、並進運動量Ｌｐ４ ５ と回転運動量Ｌｆ４ ５ より、５番目の補間動作を実行させたときの同次変換行列ＬＡ４ から見た補間点の同次変換行列ＬＡ４ ５ が算出される。これは、５番目の補間動作で到達すべき補間点の同次変換行列ＬＡ４ から見た同次変換行列である。そして、同次変換行列ＬＡ４ に同次変換行列ＬＡ４ ５ を乗じて、５番目の補間動作を実行させたときのロボット基準座標系から見た到達点の同次変換行列ＬＸ４ ５ が算出される（Ｓ８２１）。これは、５番目の補間動作で達成される点のロボット基準座標系から見た同次変換行列である。
【０１１２】
その後はステップ８０９から８１３までの処理がなされ、５であるｉに１が加えられて６と置かれる（Ｓ８２２）。このような処理を繰り返すうちにｉがＩ４ ＝１６を越えると（Ｓ８１９）、４番目の移動区間の補間のための演算が完了する。いまｊは３のまま来ており、速度結合のために４番目の移動区間まで処理したので、２を加えて５と置かれる（Ｓ８２３）。ｊはＮ−１＝６を越えていなく（Ｓ８２４）、図６のステップ４０２へ戻される。これによって図３の（ｂ）で言えば５番目の直線補間される移動区間の処理に入ることになる。
【０１１３】
その補間処理は、５番目の移動区間での「直線補間」、教示点Ｐ６ を繋ぎ点として５番目の移動区間の「直線補間」と６番目の移動区間の「関節補間」との「速度結合」、そして６番目の移動区間における残りを処理する「関節補間」からなる。その進展要領は３番目の移動区間を４番目の移動区間と繋ぐ説明と同じであるので、フローチャートには表しておくが、以下の要点部分のみの説明にとどめる。
【０１１４】
そのフローチャートは、ステップ４０２（図６を参照）から開始され、ステップ４０６から図１０のステップ６０１へ、そしてステップ６０８から６０９へ直行して６１１で補間順の数字を１つ上げて図１０のステップ６０４へ戻る通常の「直線補間」が繰り返される。何回かの補間が繰り返され、例えば１８番目の補間においてステップ６０８まで到達し、ステップ９０１によって減速域に突入したことが確認され、またポーズ精度から「速度結合処理」の可否を判断し（Ｓ９０４）、残存減速時間ｔＲ が６番目の移動区間の加速時間以下となり（Ｓ９０６）、その補間種別が「関節補間」であると判別されたときは（Ｓ９０７）、カウンタに上記した繰上処理を順次加えながら図１４のステップ１００１へ進められる。
【０１１５】
この場合の「速度結合」のためにはステップ６０７（図１０を参照）で得られたロボットの全軸の関節変数Ｌθ１ ５ １８〜Ｌθ６ ５ １８が、６番目の移動区間においてこれから行おうとする補間の始点と見たてられる。そして、６番目の移動区間における全軸での速度関数Ｊ１ｖ６ 〜Ｊ６ｖ６ が軌道データから取り出される（Ｓ１００２） 。ここで、ステップ９０９（図１１を参照）で１と置かれているカウンタｋがカウンタｉに置き替えられる（Ｓ１００３） 。いま置かれたｉによって、６番目の移動区間の補間の進行を１から順次増やして使用することができる。次に、「速度結合処理」を実質的に脱したかどうかの指標を与えるカウンタｍが０と置かれる（Ｓ１００４） 。
【０１１６】
その状態で、６番目の移動区間における全軸の速度関数Ｊ１ｖ６ 〜Ｊ６ｖ６ をもとに、１番目の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ６ １ 〜Ｊ６ｒ６ １ が算出される（Ｓ１００５） 。これは最初の補間で達成される各軸の移動量を表す。その後に、ｍが０であるかどうか問われる（Ｓ１００６） 。
【０１１７】
ｍがいま０と置かれた状態にあるので、ステップ１００７へ進められる。ここでは、始点と見たてられた全軸の関節変数Ｌθ１ ５ １８〜Ｌθ６ ５ １８に、１番目の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ ６ １〜Ｊ６ｒ ６ １を加算して、１番目の補間動作を実行したときのロボットの全軸の関節変数Ｊθ１ ６ １ 〜Ｊθ６ ６ １ が算出される。これは最初の補間動作で到達すべき全軸の目標関節変数である。この全軸の関節変数Ｊθ１ ６ １ 〜Ｊθ６ ６ １ が補間点（各関節の指令値）として関節補間点バッファに格納される（Ｓ１００８） 。これは、「速度結合処理」に基づき演算された「直線補間」と「関節補間」の速度を重ね合わせたかのように挙動させるための補間点である。
【０１１８】
関節補間点バッファから全軸の関節変数Ｌθ１ ６ １ 〜Ｌθ６ ６ １ を各軸の指令値にしてサーボ出力部へ送出され（Ｓ１０１０） 、各関節が１番目の補間目標関節角度θ１ ６ １ 〜θ６ ６ １ に向けて移動を開始する。すなわち、ＴＣＰがその関節角度となるように動かされる。この後のステップ１０１１から１０１７までの動作は先に「関節補間」から「直線補間」への繋ぎにおける「速度結合処理」のところで述べたステップ８１３（図１３を参照）から８１９までと同じ要領であるので、その説明は省く。ただし、ステップ１０１４はステップ８１６と、ステップ１０１８はステップ８２０や８２１とは異なるので、その部分について以下に述べる。
【０１１９】
ステップ１０１４は「速度結合処理」から抜け出て６番目の移動区間における「直線補間」を主体にした補間に入るときの処理であり、以下のとおりである。速度重ね処理時の（ｊ＋１）番目の移動区間の始点は、ｊ番目の移動区間におけるｉ番目の補間動作により到達しようとする点が採用されている。従って、５番目の移動区間の補間が完了した後の６番目の移動区間における残りの補間動作の始点は、５番目の移動区間の補間動作が完了したとき到達している点である。これは言うまでもなく教示点Ｐ６ である。
【０１２０】
そこで、これ以後６番目の移動区間の残りの補間動作における始点はＰ６ となる。これはステップ９０３（図１０を参照）で取り出したＴ６ の軌道データからその始点Ｐ６ を表す全軸の関節変数Ｊ１ｓ６ 〜Ｊ６ｓ６ で与えられる。それゆえ、Ｓ１００１でのＬθ１ ５ １８〜Ｌθ６ ５ １８がＪ１ｓ６ 〜Ｊ６ｓ６ に置き替えられる。
【０１２１】
ステップ１０１８は上記のステップ１０１４による準備が済んだ後の６番目の移動区間の「関節補間」のみによる処理をするもので、次のように行われる。この時点でｋが７（これは５番目の移動区間の補間回数に直して言えば１８＋７＝２５番目の補間に当たる）とすると、教示点Ｐ６ を表す全軸の関節変数Ｊ１ｓ６ 〜Ｊ６ｓ６ に７番目の補間動作を実行したときの各軸の補間移動量Ｊ１ｒ６ ７ 〜Ｊ６ｒ６ ７ を加算して、その補間動作を実行したときのロボットの全軸の関節変数Ｊθ１ ６ ７ 〜Ｊθ６ ６ ７ が算出される。これは７番目の補間動作で到達すべき全軸の目標関節変数であることは言うまでもない。以上までが「速度結合処理」の補間手順である。
【０１２２】
上でも少し触れたが、「円弧補間動作」は実質的に「直線補間動作」と同じ思想の並進速度関数と回転速度関数とが採用される。従って、「円弧補間」と「関節補間」との間で「速度結合処理」するにあたっては、既に述べた「直線補間」から「関節補間」への速度結合や、「直線補間」から「関節補間」への速度結合の動作を適用することができる。そこで、「円弧補間」は「直線補間」に含まれるものであると見なせば、本発明は、速度繋ぎ部分において「順変換−逆変換」もしくは「逆変換」の処理を伴うものを対象にしているということが分かる。同種の補間種別に対する速度結合、すなわち「関節補間」から「関節補間」への繋ぎ、「直線補間」から「直線補間」へ、「円弧補間」から「円弧補間」へ、「円弧補間」から「直線補間」へ、「直線補間」から「円弧補間」への繋ぎは、速度関数の直接的な重ね合わせにより達成できるので、本発明の思想を導入するまでもないからである。
【０１２３】
上記の説明では、ｊ番目の移動区間において減速状態に入っており、かつその移動区間における残存減速時間が（ｊ＋１）番目の移動区間の加速時間以下となっているかどうかで判定するようにしている。すなわち、前後する移動区間のラップ時間帯の長短を左右するのは後側の移動区間の加速時間ということになる。一方、ユーザによっては加速時間の全てまたはその多くを減速域に持ち込みたくないという場合がある。例えば１５０ｍｓｅｃ以上は重ねたくないという何らかの事情がある場合には、そのような条件下において、予め加速時間が１５０ｍｓｅｃより大きいかどうかを調べるようにすればよい。小さければ加速時間帯の全部を減速時間と重ねるべく図１７の（ａ）の矢印のように前送りすればよい。
【０１２４】
１５０ｍｓｅｃより大きければ加速時間帯を前送りするに際して、加速時間帯のうち前半に存在する１５０ｍｓｅｃ分だけにとどめるという配慮を施すステップを加入してやればよい。具体的には、残存減速時間が１５０ｍｓｅｃ以下となった以降に加速時間帯との結合処理を開始させるように、条件として時間幅制限を加重させるのである。
【０１２５】
ところで、図１８は、速度結合処理が施された場合でも、様々な速度変化を呈することを示す。この図はすべて「関節補間」から「直線補間」へ移行する例で表されているが、（ａ）は後側の移動区間の加速域の全部が前側の移動区間の減速域に含まれるように重ねた場合、（ｂ）は前側の移動区間の減速域の全部が後側の移動区間の加速域に含まれるように重ねた場合、（ｃ）は減速時間と加速時間が同じでぴたりと重ね合う場合を示している。いずれも前側の移動区間における速度が後側のそれよりも大きく与えられたものである。（ｄ）は（ａ）と、（ｅ）は（ｂ）と、（ｆ）は（ｃ）と同じ趣旨であるが、これらの場合、後側の移動区間における速度が前側のそれよりも大きくなっている。
【０１２６】
この図を眺めてみると、（ｂ）と（ｄ）とは速度結合処理の部分で速度変化が減増した谷が形成されている。この結合処理部分でのＴＣＰの動きとしては、常に滑らかな速度変化で与えられるに越したことはない。その点で、（ａ），（ｃ），（ｅ）および（ｆ）は減少の一途をたどるか増加の一途をたどるから好ましいと言える。ところが、（ａ）と（ｅ）は途中で加速度が変わっている。その折れ方が弱い場合はよいが、折れ点で加速度が急激に変化することもあり得る。結局は、加速度が一定となる（ｃ）および（ｆ）がロボットにとって都合よいと言える。
【０１２７】
このことを念頭におくと、異種補間動作における速度の結合処理が適用される速度関数は、現移動区間の減速域の時間と次の移動区間の加速域の時間とを一致させたものとしておくことができれば、本発明によって一層滑らかな速度結合を実現できることが分かる。しかし、速度関数はステップ２０１（図４を参照）やステップ２０６（図９を参照）ですでに生成されたものであり、かつ各移動区間における動作速度も異なるのが一般的であるから、常に減速時間と加速時間とが一致するとは限らない。
【０１２８】
そこで、本発明においては、可能な場合にこの両時間を意図的に一致させる配慮も払われる。それは、加速時間を減速時間に合わせたり、減速時間を加速時間に合わせるという処置を採ることになる。これらの場合のうち、加速時間が減速時間より長い場合に、長い加速時間を縮めて減速時間に合わせるということは避けた方がよい。それは不可能なことでないが、ＴＣＰにとってすでに適切な加速度が選定されているのであり、またサーボモータの容量を考慮したうえで決められた加速度である場合が多いから、加速時間を低減させたいという理由だけで加速度を増大させることは好ましくない。
【０１２９】
従って、本発明においては 減速時間が加速時間より長い場合は、加速時間を減速時間に合わせるように引き延ばし、減速時間が加速時間より短い場合には、減速時間を加速時間に合わせるように引き延ばすことにする。いずれにしても、その変更をどの時点でしておくかということになる。バッファがＦＩＦＯ処理式の場合には、かなり以前に立ち戻ることは容易でないことが多い。その場合にはバッファに格納されている範囲で処置することになる。
【０１３０】
上記したフローチャートにそれを加味すると交錯の激しい表記となるので、動作命令バッファの容量が大きいと仮定して、その減速加速の時間の長さ合わせ処理を簡略化して表したステップ３０１から３１５の記載に基づいて説明する。なお、移動区間の数（Ｎ＝７）とその補間種別および速度結合処理の要否は、図３の（ｂ）のタイムチャートに従うものとする。ちなみに、この処理は、図１のブロック図において「異種補間速度結合性改善用速度関数再生成部」１３で行われる。
【０１３１】
まず、１番目の移動区間の軌道データを軌道バッファ７から取り出すために、すでにステップ２０５（図４を参照）または２１０（図９を参照）において０と置かれているｊに１が加えられる（Ｓ３０１）。これによって、最初の移動区間における減速時間ｔｄｅ１ と補間種別が取り出される（Ｓ３０２）。この移動区間における後の教示点であるＰ２ （作業第１準備位置）のポーズ精度から見て「速度結合処理」が可能かどうか判断される（Ｓ３０３）。
【０１３２】
ポーズ精度が低くて予め決められた精度以下であると「速度結合処理」の可能性があることになるが、この移動区間においては図３の（ｂ）によればその必要のない扱いとなっている。ステップ３０４へ進んでｊに１が加えられ、（Ｎ−１）＝７−１＝６より小さいことが確認され（Ｓ３０５）、２番目の移動区間以降の処理のための準備が整えられる。ステップ３０２において２番目の移動区間における減速時間ｔｄｅ２ と補間種別が取り出される。この移動区間における後の教示点であるＰ３ は図３の（ｂ）によれば第１作業点であり、高いポーズ精度で停止させることにしている。ステップは３０３から再度ステップ３０４へ進められる。ｊに１が加えられ、ステップ３０５を経て３番目の移動区間以降の処理に仕向けられる。
【０１３３】
ステップ３０２において３番目の移動区間における減速時間ｔｄｅ３ と補間種別として指定されている「関節補間動作」が取り出される。この移動区間における後の教示点であるＰ４ は図３の（ｂ）によれば作業第２準備位置であり、ポーズ精度は低く与えられている。ステップ３０６へ進められ、そこで現在３であるｊに１が加えられて４とされる。これに基づき、４番目の移動区間における加速時間ｔａｃ４ と補間種別として指定されている「直線補間動作」が取り出される（Ｓ３０７）。
【０１３４】
ステップ３０８であい前後する移動区間は異種補間であると判別され、加速時間ｔａｃ４ が減速時間ｔｄｅ３ より小さいかどうか問われる（Ｓ３０９）。図３の（ｂ）の該当部分を拡大した図１９の（ｂ）によれば、減速時間ｔｄｅ３ の方が大きくなっている。（ａ）には、ｔａｃ４ ＜ｔｄｅ３ となる４番目の移動区間の例が、一点鎖線と二点鎖線により２つ描かれている。その２つは、減速度の絶対値に比較して加速度が大きい場合と小さい場合とを表している。（ｂ）はそのうちの一点鎖線の移動区間を採りあげ、３番目の移動区間と速度結合処理しようとしたものである。
【０１３５】
この場合はステップ３１０に進んで、４番目の移動区間の加速時間ｔａｃ４ を減速時間ｔｄｅ３ と等しくなるように、当該速度関数Ｌｖｐｆ ４ が図１９の（ｃ）に表した加速の弱められた台形状の速度関数Ｌｖｐｆ４ｍ となるように生成し直される。すなわち、加速を示す線はａからｂに変えられる。しかし、４番目の移動区間による移動量はあくまでも同じでなければならないから、線ａ，ｂが交差して上下に形成される三角形の面積が等しくなるように、線ｂの位置が決められる。
【０１３６】
このように変形させた後に、図１８の（ｃ）のようにする意図で、図１９の（ｄ）のように、４番目の移動区間の変形された速度関数Ｌｖｐｆ４ｍ が前へ寄せられる。その結果、教示点Ｐ５ は（ａ）や（ｃ）の場合より早く到達することはもちろんのこと、（ｂ）の場合よりも早く到達できることになる（（ｂ）と（ｄ）の図中の白抜き矢印５２，５３の長短を参照）。すなわち、ＴＣＰの動きはますます機敏になることが分かる。もちろん速度結合処理部における速度は（ｄ）中に記載したｃのように可及的に変化の少ないものであり、かつ３番目の移動区間の定速度と４番目の移動区間の定速度とを結ぶ全体から見ても速度変動の少ない運動をさせることができるようになる。この再生成された速度関数Ｌｖｐｆ４ｍ は、従前の速度関数Ｌｖｐｆ４に置き替えられて軌道バッファ７に格納される（Ｓ３１１）。
【０１３７】
ステップ３１２で、現在のｊが（Ｎ−２）＝７−２＝５以下であると判断されて制御はステップ３０２へ戻される。いまｊは４とされたままであるので、ステップ３０２の処理が行われる。図３の（ｂ）によれば４番目の移動区間の後の教示点Ｐ５ においては、停止して作業をさせるようになっている。従って、ステップ３０３では速度結合の可能性のないとの判断からステップ３０４へ回される。ここで、現在の４であるｊが５に繰り上げられ、（Ｎ−１）＝７−１＝６より小さいことが確認され（Ｓ３０５）、５番目の移動区間以降の処理のための準備が整えられる。
【０１３８】
ステップ３０２に戻って５番目の移動区間における減速時間ｔｄｅ５ と補間種別の「直線補間」が取り出される。この移動区間における後の教示点であるＰ６ は図３の（ｂ）によれば作業解放位置であり、近傍を通過させればよい程度の点であることが分かる。ステップは３０３を経て３０６へ進み、１が加えられてｊは６となり、６番目の移動区間における加速時間ｔａｃ６ と補間種別として指定されている「関節補間」が取り出される（Ｓ３０７）。
【０１３９】
ステップ３０８であい前後する移動区間は異種補間であると判別され、加速時間ｔａｃ６ が減速時間ｔｄｅ５ より小さいかどうか問われる（Ｓ３０９）。図３の（ｂ）の該当部分を拡大した図２０の（ｂ）によれば、減速時間ｔｄｅ５ の方が小さくなっている。（ａ）の図はｔａｃ６ ＞ｔｄｅ５ となる６番目の移動区間の例が先の例と同じく加速度の大小を分けて一点鎖線と二点鎖線の２つで挙げられている。（ｂ）はそのうちの一点鎖線の移動区間を採りあげ、この６番目の移動区間との速度結合処理をしようとしたものである。
【０１４０】
この場合はステップ３１３に進んで、５番目の移動区間の減速時間ｔｄｅ５ を加速時間ｔａｃ６ と等しくなるように当該速度関数Ｊ１−６ ｖ ５ が、図２０の（ｃ）に表したような台形状の速度関数Ｊ１−６ ｖ ５ｍとなるように生成し直される。減速を示す線はｐからｑに変えられるが、あくまでも５番目の移動区間による移動量は同じでなければならないから、線ｐ，ｑが交差して上下に形成される三角形の面積が等しくなるように線ｑの配置が決められる。
【０１４１】
このように変形させた後に、図１８の（ｆ）のようにする意図で、図２０の（ｄ）のように、６番目の移動区間の変形されなかった速度関数Ｌｖｐｆ ６ が、５番目の移動区間の変形された速度関数Ｊ１−６ ｖ ５ｍへ寄せられる。その結果、教示点Ｐ７ すなわち退避位置へは、（ｂ）の場合よりも早く到達することになる。すなわち、ＴＣＰの動きは機敏となる。もちろん、速度結合処理部における速度は（ｄ）中に記載したｒのように可及的に変化の少ないものであり、かつ５番目の移動区間の定速度と６番目の移動区間の定速度とを結ぶ全体から見ても速度変動の少ない運動をさせることができる準備が整う。この再生成された速度関数Ｊ１−６ ｖ ５ｍは、従前の速度関数Ｊ１−６ ｖ ５ に置き替えられて軌道バッファ７に格納される（Ｓ３１４）。
【０１４２】
ステップ３１２で、現在６であるｊが（Ｎ−２）＝７−２＝５以下でないと判断されて、この加減速時間の変更制御は終了し、ｊは０にリセットされる（Ｓ３１５）。なお、図３の（ｂ）に記載されていないが、５番目と６番目の移動区間の間で速度結合処理する必要がなかったときは、ｊ＝５の状態でステップ３０４にたどりつくとｊが６に繰り上げられ、その状態で再度ステップ３０４に至り、ｊが７に繰り上げられると、ステップ３０５でｊが（Ｎ−１）＝７−１＝６より大きいと判断され、ステップ３１５へ導かれて、この処理は終了する。
【０１４３】
このようにして加減速時間を変更した速度関数が軌道バッファ７に格納されると、制御はフローチャートのステップ４０１からの処理に入り、必要な箇所での加減速時間の一致の図られた速度関数に基づいて、「速度結合処理」が施されることになる。
【０１４４】
ところで、いままでは各移動区間における速度関数は台形波パターンを例にして説明してきた。しかし、これに限らず、図３の（ａ）の一部に重なるようにして描いた細かい破線表示の速度関数Ｊｍ １−６ ｖ ３ やＬｍ ｖｐｆ ４ のように、Ｓ字波形の速度変化を持った場合にも適用することができる。このような速度関数が採用されている場合に、上記したステップ３０１から３１４の処理を施すなら、結合速度はますます滑らかなものとなる。ちなみに、以上の説明は６軸ロボットを例にしたが、２軸以上であれば、本発明を適用することができるのは言うまでもない。
【０１４５】
【発明の効果】
本発明によれば、補間種別の移り変わりを判別すると共に繋ぎ点もしくはその近傍における速度の結合処理が可能かどうかを判定する速度結合処理可否判定部と、異種補間動作間の速度を重ね合わせたように挙動させるための補間点を演算する速度結合演算部とを備えるので、３点以上の教示点からなる経路（曲折しているかどかは問題でない）上での速度制御において、教示点ごとに記録された補間種別が異なる場合でも、教示点でロボットのＴＣＰ速度を低下させることなく通過させることができる。
【０１４６】
ロボットが機敏に動けば作業能率は上がり、タクトタイムは短縮される。滑らかな速度変化が助長されることになり、無用の衝撃や力さらには振動がアームやＴＣＰに及ぶこともなくなる。ロボットによる作業の正確さ精密さが向上し、ＴＣＰの動きもスマートなものとなる。
【０１４７】
速度制御法の一連の発明においても、上記の効果が発揮されることは言うまでもない。その制御は異種補間での速度結合を行うものであるが、従前から行われている同種補間における速度重ね処理の思想とは異なり、異種補間での速度結合を補間点の重合により達成した。これは同種補間の場合の速度重ね処理と組み合わせて使用するにおいて弊害をきたす要因はなく、従ってロボットの速度制御の多様化が図られることにもなる。
【０１４８】
速度の結合処理の可否は、現移動区間において減速状態にあり、かつその移動区間における残存減速時間が次の移動区間の加速時間以下となっているかどうかで判定するようにしておけば、結果として生じる結合された速度がサーボモータの能力を越えるようなことはなくなり、駆動系に損傷をきさずまたロボットアーム等に対する安全性も向上させやすくなる。
【０１４９】
異種補間動作における速度の結合処理が適用される速度関数を生成するにおいて、現移動区間の減速時間と次の移動区間の加速時間とを一致させるようにしておくと、結合された速度の滑らかさは格段に向上する。その場合、次の移動区間の加速時間が現移動区間の減速時間よりも短かければ、次の移動区間の加速時間を減速時間に一致させ、現移動区間の減速時間が次の移動区間の加速時間よりも短かければ、現移動区間の減速時間を前記加速時間に一致させておくようにすることができる。その結果、加速度の過剰な増大を防いで、サーボモータに大きな負担を強いらずまた振動助長原因も可及的に少なくしておくことができる。
【図面の簡単な説明】
【図１】本発明に係る多関節型ロボットの速度制御を実現する制御装置のブロック図。
【図２】多関節型ロボットの速度制御法の全体を簡潔に表したフローチャート。
【図３】フローチャートを説明するための移動区間の変遷と速度結合処理箇所を示したタイムチャート。
【図４】ないし
【図１５】教示作業プログラムの読み出しに始まり、軌道データの生成、加減速時間の変更処理した速度関数の再生成、軌道データを基にした軌道解析すなわち補間のための演算ならびに速度結合処理を表したフローチャート。
【図１６】関節補間と直線補間があい前後して並んだ場合の前側の移動区間における補間動作の説明図。
【図１７】関節補間と直線補間が並んだ場合の繋ぎ点近傍での速度結合処理を施した場合の補間動作説明図。
【図１８】２つの移動区間の繋ぎ点で速度結合処理した場合に起こり得る速度の変化の種類を示した説明図。
【図１９】加減速時間を一致させるための処理を表したもので、加速時間を減速時間に合わせるときの説明図。
【図２０】加減速時間を一致させるための処理を表したもので、減速時間を加速時間に合わせるときの説明図。
【図２１】ワークピースに対して作業を施す教示点と、その軌道の補間の種類や補間法を説明するための作業モデル図およびそのタイムチャート。
【図２２】３つの教示点間で関節補間されるときに使用される関節速度関数を表したタイムチャート。
【図２３】関節補間同士の速度重ね処理を説明するタイムチャート。
【図２４】関節補間同士の速度重ね処理をした場合の実際にたどる経路とポーズ精度との関係を表した説明図。
【符号の説明】１…速度制御装置、２…速度結合処理可否判定部、３…速度結合演算部、３Ａ…関節補間から直線補間への速度結合演算部、３Ｂ…直線補間から関節補間への速度結合演算部、４…軌道解析部、５…補間動作演算部、５Ａ…関節補間動作演算部、５Ｂ…直線補間動作演算部、６…軌道生成部、７…軌道バッファ、８…サーボ出力部、９１ 〜９６ …サーボドライバ、１０１ 〜１０６ …サーボモータ、１１…補間種別判定部、１２…関節補間点バッファ、１３…異種補間速度結合性改善用速度関数再生成部、５１…ワークピース、Ｐｊ …教示点。
Claims (8)
- 教示点から次の教示点までの区間に幾つかの補間点を与え、移動区間ごとに演算された個々の速度関数に従って関節補間動作や直線補間動作により移動させるようにした多関節型ロボットの速度制御装置において、
あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別すると共に、その２つの移動区間の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定する速度結合処理可否判定部と、
該速度結合処理可否判定部により速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせたように挙動させるための補間点を演算する速度結合演算部と、
を具備することを特徴とする多関節型ロボットの速度制御装置。 - 教示点から次の教示点までの区間に幾つかの補間点を与え、移動区間ごとに演算された個々の速度関数に従って関節補間動作や直線補間動作により移動させるようにした多関節型ロボットの速度制御法において、
あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別し、
その２つの移動区間の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定し、
速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせるような挙動をさせるべく関節変数を決定するようにしたことを特徴とする多関節型ロボットの速度制御法。 - 前記異種補間動作が関節補間から直線補間に移る場合の速度結合処理は、現移動区間における関節補間動作の現時刻に達成されるロボット全軸の関節変数を算出し、これを順演算して得られるロボット基準座標系から見たＴＣＰの同次変換行列を直線補間動作する次の移動区間の始点と見たて、該直線補間動作の現時刻で達成される並進速度関数と回転速度関数から算出した並進運動量と回転運動量により、始点とされた同次変換行列から見た同次変換行列を算出し、始点を表す同次変換行列に並進運動量と回転運動量とから算出された同次変換行列を乗じて、１補間周期の間に補間動作させたときの到達点のロボット基準座標系から見た同次変換行列を算出し、これを逆演算して関節変数を求め、これによって関節補間による補間量に直線補間による補間量を加えた移動量を得ることにより行われることを特徴とする請求項２に記載された多関節型ロボットの速度制御法。
- 前記異種補間動作が直線補間から関節補間に移る場合の速度結合処理は、現移動区間における直線補間動作の現時刻に到達するＴＣＰのロボット基準座標系から見た同次変換行列を算出し、これを逆演算して得られる関節変数を関節補間動作する次の移動区間の始点と見たて、該関節補間動作の全軸の速度関数をもとに現時刻で補間動作を実行したときの各軸の補間移動量を算出し、始点を表す全軸の関節変数に補間動作を実行したときの各軸の補間移動量を加算して、１補間周期の間に補間動作させたときのロボットの全軸の関節変数を算出し、これによって直線補間による補間量に関節補間による補間量を加えた移動量を得ることにより行われることを特徴とする請求項２に記載された多関節型ロボットの速度制御法。
- 前記速度の結合処理の可否は、現移動区間において減速状態にあり、かつその移動区間における残存減速時間が次の移動区間の加速時間以下となっているかどうかで判定するようにしたことを特徴とする請求項２ないし請求項４のいずれか一項に記載された多関節型ロボットの速度制御法。
- 異種補間動作における速度の結合処理が適用される速度関数は、現移動区間の減速時間と次の移動区間の加速時間とを一致させておくことを特徴とする請求項２ないし請求項５のいずれか一項に記載された多関節型ロボットの速度制御法。
- 次の移動区間の加速時間が現移動区間の減速時間よりも短い場合は、次の移動区間の加速時間を前記減速時間に一致させておくことを特徴とする請求項６に記載された多関節型ロボットの速度制御法。
- 現移動区間の減速時間が次の移動区間の加速時間よりも短い場合は、現移動区間の減速時間を前記加速時間に一致させておくことを特徴とする請求項６に記載された多関節型ロボットの速度制御法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003043686A JP2004252814A (ja) | 2003-02-21 | 2003-02-21 | 多関節型ロボットの速度制御法および制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003043686A JP2004252814A (ja) | 2003-02-21 | 2003-02-21 | 多関節型ロボットの速度制御法および制御装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2004252814A true JP2004252814A (ja) | 2004-09-09 |
Family
ID=33026616
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2003043686A Pending JP2004252814A (ja) | 2003-02-21 | 2003-02-21 | 多関節型ロボットの速度制御法および制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2004252814A (ja) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5208325B1 (ja) * | 2011-09-15 | 2013-06-12 | 三菱電機株式会社 | 数値制御装置、加工システム、および数値制御方法 |
| JP2014534083A (ja) * | 2011-11-02 | 2014-12-18 | サムスン ヘビー インダストリーズ カンパニー リミテッド | リアルタイムウィービングモーション制御装置およびその方法 |
| JP2015104773A (ja) * | 2013-11-29 | 2015-06-08 | 株式会社ダイヘン | マニピュレータの移動制御装置及び移動制御方法 |
| CN105511407A (zh) * | 2014-10-14 | 2016-04-20 | 发那科株式会社 | 数值控制装置 |
| KR20190112643A (ko) | 2018-03-26 | 2019-10-07 | 니혼 덴산 가부시키가이샤 | 로봇 제어 장치, 로봇 제어 방법, 프로그램 |
| KR20190112644A (ko) | 2018-03-26 | 2019-10-07 | 니혼 덴산 가부시키가이샤 | 로봇 제어 장치, 로봇 제어 방법, 프로그램 |
| JP2020019125A (ja) * | 2018-08-03 | 2020-02-06 | パナソニックＩｐマネジメント株式会社 | ロボット制御方法 |
| CN112486034A (zh) * | 2020-11-09 | 2021-03-12 | 北京配天技术有限公司 | 验证轨迹规划的方法、电子设备及存储介质 |
-
2003
- 2003-02-21 JP JP2003043686A patent/JP2004252814A/ja active Pending
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9678500B2 (en) | 2011-09-15 | 2017-06-13 | Mitsubishi Electric Corporation | Machining program creating device numerical control device, machining system, machining program creating method, numerical control method, and machining program |
| JP5208325B1 (ja) * | 2011-09-15 | 2013-06-12 | 三菱電機株式会社 | 数値制御装置、加工システム、および数値制御方法 |
| JP2014534083A (ja) * | 2011-11-02 | 2014-12-18 | サムスン ヘビー インダストリーズ カンパニー リミテッド | リアルタイムウィービングモーション制御装置およびその方法 |
| US9302390B2 (en) | 2011-11-02 | 2016-04-05 | Samsung Heavy Ind. Co., Ltd. | Device and method for controlling weaving motion in real time |
| JP2015104773A (ja) * | 2013-11-29 | 2015-06-08 | 株式会社ダイヘン | マニピュレータの移動制御装置及び移動制御方法 |
| US10514681B2 (en) | 2014-10-14 | 2019-12-24 | Fanuc Corporation | Numerical controller including overlap function between arbitrary blocks by common acceleration/deceleration control unit |
| CN105511407A (zh) * | 2014-10-14 | 2016-04-20 | 发那科株式会社 | 数值控制装置 |
| JP2016081172A (ja) * | 2014-10-14 | 2016-05-16 | ファナック株式会社 | 共通加減速制御部による任意ブロック間のオーバラップ機能を備えた数値制御装置 |
| CN105511407B (zh) * | 2014-10-14 | 2018-01-12 | 发那科株式会社 | 数值控制装置 |
| KR20190112643A (ko) | 2018-03-26 | 2019-10-07 | 니혼 덴산 가부시키가이샤 | 로봇 제어 장치, 로봇 제어 방법, 프로그램 |
| KR20190112644A (ko) | 2018-03-26 | 2019-10-07 | 니혼 덴산 가부시키가이샤 | 로봇 제어 장치, 로봇 제어 방법, 프로그램 |
| US11185981B2 (en) | 2018-03-26 | 2021-11-30 | Nidec Corporation | Robot control apparatus, robot control method, and program |
| JP2020019125A (ja) * | 2018-08-03 | 2020-02-06 | パナソニックＩｐマネジメント株式会社 | ロボット制御方法 |
| JP7108893B2 (ja) | 2018-08-03 | 2022-07-29 | パナソニックＩｐマネジメント株式会社 | ロボット制御方法 |
| CN112486034A (zh) * | 2020-11-09 | 2021-03-12 | 北京配天技术有限公司 | 验证轨迹规划的方法、电子设备及存储介质 |
| CN112486034B (zh) * | 2020-11-09 | 2023-11-10 | 北京配天技术有限公司 | 验证轨迹规划的方法、电子设备及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP3566822B1 (en) | Robot joint space point-to-point movement trajectory planning method | |
| JP4922584B2 (ja) | ロボットシステム | |
| JP2514490B2 (ja) | 産業用ロボットの連動手動操作による教示制御方法 | |
| EP1477284A1 (en) | Drive control method and drive controller | |
| JP6501470B2 (ja) | 軌道生成方法、軌道生成装置、ロボット装置、プログラム及び記録媒体 | |
| JPH06198584A (ja) | 産業用ロボットの手動操作による教示制御装置および教示方法 | |
| JP2004252814A (ja) | 多関節型ロボットの速度制御法および制御装置 | |
| CN103778843B (zh) | 一种工业机器人示教再现方法 | |
| KR0160998B1 (ko) | 로보트의 구동경로 계획방법 | |
| JP5858168B2 (ja) | リアルタイムウィービングモーション制御装置およびその方法 | |
| JP2007000954A (ja) | ロボット教示装置及び方法 | |
| JP2014161917A (ja) | ロボット制御システム、ロボット、ロボット制御方法及びプログラム | |
| JP6123595B2 (ja) | ２軸ロボットの速度制御方法 | |
| JP3689194B2 (ja) | ロボットの制御方法 | |
| JPH02219107A (ja) | 数値制御装置 | |
| JPH10286788A (ja) | 軌跡制御装置 | |
| JPH1076489A (ja) | ロボットアーム教示方法及び装置 | |
| JPH08123531A (ja) | 軌道の制御方法 | |
| JP6046994B2 (ja) | 制御装置 | |
| JP2020097083A (ja) | ロボットの制御装置及び制御方法 | |
| JPH1148091A (ja) | 軌跡制御装置 | |
| JPS6116305A (ja) | ロボツト制御方法およびその装置 | |
| JPS63150183A (ja) | ロボツトの運動制御方法 | |
| JPS6115207A (ja) | ロボツト制御方法およびその装置 | |
| JPS63106010A (ja) | ロボツトの経路移動制御方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20060201 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20080704 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20080708 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20080827 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20081022 |