【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、多関節型ロボットの軌跡制御方式に関し、特に、６自由度多関節型ロボットを直交座標系でスムーズに移動させる軌跡制御方式の改良に関する。 ［背景説明］一般に、各関節の運動が互いに影響し合う多関節型ロボットの軌跡制御においては、マトリクスを用いた演算、すなわち座標変換行列によって各関節の動きを計算し、この演算結果に基づいて腕や手先の位置を目標軌跡に一致させる制御方式を採用する。

【０００２】座標変換演算は、例えば「人工の手の運動制御に関する研究」日本機械学論文集（Ｃ編）、４５巻３９１号（昭和５４年３月）Ｐ３１４〜Ｐ３２２、に詳しく紹介されているように、目標とする手先の位置・姿勢ベクトルＰを与えるのに必要な各々の関節角度ベクトルΘを、各関節に固定された座標系間のトータルな座標変換行列を使用して求めるものである。

【０００３】しかし、かかる座標変換行列によって求められたΘの解は、存在範囲を次式に制限してモード８
通りある。 この解の種類は、ロボットに行わせる作業内容によって決めなければならないが、解の種類を変更するためには、特異点を必ず通過する。 −π＜θ _i ≦π（ｉ＝１〜６） …… 但し、θ _i ：関節ｉの関節角度 特異点は解の種類の変化点であるから、この点を境にΘ
の動きが不連続となり、また、特異点近傍でもＰの微小変化によってΘが大きく変化するといった軌跡制御の不連続性問題が生じてしまう。 本発明は、特に後者の「特異点近傍における不連続性問題」を解消するための有用な技術を提供するものである。

【０００４】

【従来の技術】かかる特異点近傍における不連続性問題を解消するための従来技術としては、例えば特公平３−
５６０５公報に記載された技術がある。 この従来技術は、多関節ロボットのベース（基台）に定義した直交座標系にてあらかじめ指定されている始点より逐次所定の時間後に通過するべき点（以下、補間点）を演算し、それぞれの補間点における関節角と移動中の現時点での関節角との差から、補間点に達するまでの関節駆動速度を求めて、その関節駆動速度が所定値よりも大きくなったときに特異点に接近したとみなし、この特異点近傍では、上記関節駆動速度をアクチュエータの最高速度Ｖ
_maxで制限すると共に、補間点の間隔（以下、補間間隔）を上記関節駆動速度とアクチュエータの最高速度Ｖ
_maxの比に応じて引き延ばすようにしたものである。

【０００５】これによれば、特異点近傍においても、与えられた直交座標から各関節の関節角度への変換処理をスムーズに行うことができ、６自由度多関節型ロボットの機構に固有の特異点近傍における不連続性問題を解決できる。

【０００６】

【発明が解決しようとする課題】しかしながら、かかる従来のロボットの軌跡制御方式にあっては、特異点近傍における関節速度をアクチュエータの最高速度Ｖ _maxで制限するので、特異点近傍から離脱する際、すなわち、
関節角がある値から他の値へと変化する際の変化曲線の傾き（Ｖ _maxで決まる）が急となり、他の値に収束する際に大きな振動（ハンティング）を発生して軌跡制御の安定性を損なうといった問題点があった。 また、特異点近傍における補間間隔を上記関節駆動速度とアクチュエータの最高速度Ｖ _maxの比に応じて引き延ばすので、軌跡の非補償時間が長くなり、軌跡精度が低下するといった問題点があった。 ［目的］そこで、本発明は、特異点近傍における関節駆動速度と補間間隔を適正化することにより、振動発生を回避して軌跡制御の安定性を向上すると共に、軌跡の非補償時間を短くして軌跡精度を高めることを目的とする。

【０００７】

【課題を解決するための手段】本発明は、上記目的を達成するために、多関節型ロボットのベースに定義した直交座標系にてあらかじめ指定されている始点より逐次所定の時間後に通過するべき補間点を演算し、前記各々の補間点におけるロボットの関節角を前記直交座標系からロボットの形態に固有の関節座標系への座標変換演算により求め、前記補間点における関節角と移動中の現時点での関節角との差から補間点に達するまでに関節を駆動させる速度を求める演算を行って、ロボット先端を終点まで移動させる制御方式であって、前記関節角の差が所定量よりも大きくなったときは、関節角が不連続に変化する特異点近傍に近付いたとみなし、該特異点近傍では、前記関節を駆動させる速度を関節アクチュエータの最高速度に相当する速度から該速度よりも低い速度へと段階的若しくは連続的に変化させると共に、該速度の変化に対応させて前記補間点の間隔を短くすることを特徴とする。

【０００８】

【作用】本発明では、特異点近傍から離脱する際、すなわち、関節角がある値から他の値へと変化する際の変化曲線が、関節アクチュエータの最高速度に相当する急な傾斜から、該速度よりも低い速度に相当する緩やかな傾斜へと段階的若しくは連続的にスムーズに連続する。 従って、上記他の値に収束する際の大きな振動（ハンティング）が回避され、軌跡制御の安定性向上が図られる。
また、軌跡の非補償時間が短縮化され、軌跡精度が高められる。

【０００９】

【実施例】以下、本発明の実施例を図面に基づいて説明する。 図１〜図１２は本発明に係るロボットの軌跡制御方式の一実施例を示す図であり、６自由度多関節型ロボットへの適用例である。 まず、構成を説明する。 図１において、ロボット１は、ベース（基台）２と手先（ハンド）３の間を連結する複数の関節（第１関節〜第６関節）を備え、各関節内部のアクチュエータ（例えばモータ）を回転させることにより、手先の位置・姿勢ベクトル（以下、手先位置と略す）Ｐを自在に変更できる。

【００１０】［Ｏ−ｘｙｚ］はロボット１のベース２に定義した直交座標系であり、各関節や手先３に固定の座標系で表現された位置情報等を座標変換する際の基準となる座標系である。 図２は、各関節の制御系の概念ブロック図である。 制御系は、目標関節角度と現在の関節角度とに基づいて関節角度の指令値を演算する演算処理装置１０、該指令値に応じた関節速度の制御値を発生する速度関数発生部１１、及び、該制御値に従って関節を回転駆動するサーボ機構１２を含み、全体でクローズドループ制御系を構成する。

【００１１】図３は、制御系の詳細ブロック図である。
１０ａは現在位置から指定された目標位置まであらかじめ定められた補間方式と速度で手先位置を計算する第１
軌跡補間部、１０ｂは第１軌跡補間部１０ａで計算された手先位置を関節角度に座標変換する座標変換部、１０
ｃは各関節の現在の関節角度を取得する現在位置読込部、１０ｄは後述の速度計算部１１ａで算出された速度及び移動時間に基づいて手先の補間間隔を伸縮する位置と伸縮量を設定する速度変化点抽出部、１０ｅは速度変化点抽出部１０ｄで求めた変化点と伸縮量に従って所定の補間演算を実行する第２軌跡補間部、１１ａは座標変換部１０ｂで計算された各関節角度と現在の関節角度に基づいて関節速度を算出すると共に、各関節の最高速度を越えた場合は移動時間を延ばすための計算を行う速度計算部、１２ａは各関節速度に従ってロボットを実際に動作させるサーボ制御部である。

【００１２】次に、作用を説明する。 図４において、Ｐ
_Sは多関節型ロボットのベースに定義した直交座標系にてあらかじめ指定されている始点、Ｐ _Eは軌跡制御の最終目標位置となる終点である。 始点Ｐ _Sと終点Ｐ _Eの間のＰ ₁ 、Ｐ ₂ 、Ｐ ₃ 、……Ｐ _i 、……は、それぞれが所定の時間後に通過するべき補間点であり、これら補間点の位置は演算によって逐次求められる。

【００１３】軌跡制御は、直前の補間点（例えばＰ _i ）
におけるロボットの関節角θ _iを直交座標系からロボットの形態に固有の関節座標系への座標変換演算により求めると共に、その補間点Ｐ _iにおける関節角θ _i （演算関節角）と移動中の現時点での関節角θ _i '（実関節角）との差から補間点Ｐ _iに達するまでに関節を駆動させる速度（以下、関節速度Ｖ _i ）を求める演算を行い、
これを補間点ごとに繰り返してロボット先端を終点まで移動させる一連の制御方式である。

【００１４】ロボットの手先を始点Ｐ _Sから終点Ｐ _Eまで直線補間で軌跡制御する場合、図５に示す台形速度曲線の一定時間間隔Ｄ _th （補間点の間隔に相当；以下、補間間隔）ごとに、補間点Ｐ _jにおける手先位置から各関節角度θを求め、その位置を各関節が通過するように、
次式に従って各関節角度Ｖ _iを演算し、その速度で補間点を通過するようにアクチュエータを制御する。

【００１５】Ｖ _i ＝（θ _i −θ _i '）／Ｄ _tr …… Ｖ _i ：関節_iの速度 θ _i ：Ｐ _j点における関節_iの関節角度 θ _i '：関節_iの現在位置（関節角度） Ｄ _tr ：一定時間間隔（制御間隔に相当する、通常は＝Ｄ
_th ） ここで、関節角が不連続に変化する特異点は、前述したように関節角度ベクトルΘの解の種類の変化点であり、
演算で求めた関節速度Ｖ _iが急激に大きくなるから、このＶ _iと所定値、例えばアクチュエータ等の最高速度Ｖ
_maxiとを比較することにより、特異点近傍への接近を検出できる。

【００１６】本発明では、特異点近傍以外のときは制御間隔Ｄ _trと補間間隔Ｄ _thとを等値に設定するが、特異点近傍に接近したときは補間間隔Ｄ _thを縮小方向に変化させる。 なお、縮小に伴って目標位置への到達時間が長くなるため、特異点近傍の通過後は、制御間隔Ｄ _trを元の値に戻すという手法をとる（但し、特異点近傍で停止するときは縮小したままとする）。

【００１７】速度変化点抽出部１０ｄでは、現在位置から目標位置までの補間間隔ごとに各関節速度を計算するが、特異点近傍通過時における計算では、以下に示す複数のパラメータを求める。 Ｔ _c ；制御間隔Ｄ _trを延ばす場合、その延ばしの開始点（動作開始からの時間）を示すパラメータ、Ｔ _m ；制御間隔Ｄ _trが最大に延びた点（動作開始からの時間）を示すパラメータ、Ｄ _tm ；上記Ｔ _mにおける補間間隔であり、次式で与えられる。

【００１８】Ｄ _tm ＝Ｄ _th ＊Ｖ _maxi ／Ｖ _i …… Ｖ _maxi ：関節_iの最大速度 Ｖ _i ：次の補間点に移動するための速度 Ｔ _d ；関節速度Ｖ _iが急激に低下したときに、その制御間隔Ｄ _trでは現在速度から計算値Ｖ _iまで落とせないといったことが発生した時間を示すパラメータ、なお、計算値まで落とせないという判断は、現在位置読み込み時の速度をＶ _i 'とすると、減速するまでに要する時間Ｄ
_taは次式で与えられる。

【００１９】 Ｄ _ta ＝（Ｖ _i '−Ｖ _i ）／ＶＫ _maxi …… ＶＫ _maxi ：関節_iの最大加速度 ここで、次の条件を満たす場合が、計算値まで落とせない場合である。 （ａ）Ｖ _i '＜０．０ かつ Ｖ _i ＜Ｖ _i ' かつ Ｄ
_ta ＞Ｄ _tr （ｂ）Ｖ _i '≧０．０ かつ Ｖ _i ＞Ｖ _i ' かつ Ｄ
_ta ＞Ｄ _tr従って、補間時間内で計算値まで落とすためには、１補間間隔前において次式で求められる速度Ｖ _i ”まで減速しておく必要がある。

【００２０】 Ｖ _i ”＝Ｖ _i ±ＶＫ _maxi ＊Ｄ _th （Ｖ _i ＞０で−，Ｖ _i ＜０で＋） …… すなわち、１補間間隔前のロボットの制御間隔をＤ _tr '
とすると、 Ｄ _thd ＝Ｄ _tr '＊｜Ｖ _i '／Ｖ _i ”｜ …… となる。

【００２１】第２軌跡補間部１０ｅは以上の各パラメータを利用し、以下の規則に従って手先の補間位置を求めていく（ｔは補間計算上の経過時間）。 （ア）０≦ｔ≦Ｔ _c ｔ＝ｔ＋Ｄ _th （Ｄ _th ＝Ｄ
_tr ） （イ）Ｔ _c ＜ｔ≦Ｔ _m時間内でＤ _thからＤ _tmに至るまで段階的若しくは連続的に徐々に手先補間時間を縮める。 徐々に落とす際は、必ず補間間隔Ｄ _tmになるようにする。 （ウ）Ｔ _m ＜ｔ≦Ｔ _d時間内でＤ _tmから段階的若しくは連続的に徐々に手先補間間隔をＤ _thdに至らせる。
徐々に落とす際は、必ず補間間隔Ｄ _thdになるようにする。 （エ）Ｔ _d ＜ｔ≦Ｔ _e時間内でＤ _tdから段階的若しくは連続的に徐々に手先補間間隔をＤ _thに至らせる。 徐々に落とす際は、必ず補間間隔Ｄ _thになるようにする。

【００２２】図６は、特異点近傍を通過する際の関節角速度の変化曲線である。 所定の角速度から急激に立ち上がった後、所定の角速度に安定する。 立上りの開始点（Ｔ _c ）までは上記（ア）の規則が適用され、開始点（Ｔ _c ）からほぼ中間点（Ｔ _m ）までは上記（イ）の規則が適用される。 また、中間点（Ｔ _m ）から終了点（Ｔ
_d ）までは上記（ウ）の規則が適用され、終了点（Ｔ _d ）以降は上記（エ）の規則が適用される。 すなわち、特異点近傍を通過する際は、Ｔ _mのときにＤ _tm 、Ｔ
_dのときにＤ _thd 、Ｔ _d以降ではＤ _th （本来の補間間隔）となるように、手先補間時間が徐々に伸縮される。
従って、補間間隔が長くなり過ぎないから、軌跡の非補償時間を短くすることができ、軌跡精度を高めることができる。

【００２３】以上述べたように、本実施例では、特異点を通過する際の時点Ｔ _mにおいて、関節速度を最大速度Ｖ _maxiに設定すると共に、その後の時点Ｔ _d （の１補間間隔前）において前式で求められる速度Ｖ _i ”まで減速しておくので、各関節や手先の振動を回避することができ、軌跡制御の安定性を向上することができる。すなわち、図７〜図９は、かかる速度コントロールを行わない場合（言い替えれば従来例）の関節速度（実線）と関節角度（破線）の特性図であり、この例では、第４関節（図８（ｂ）参照）と第６関節（図９（ｂ）参照）に大きな振動が認められる。これは、特異点近傍を通過する際の関節速度が、相当に大きな値から急にゼロになるためである。図１０〜図１２は速度コントロールを行う場合の特性図であり、図７〜図９と同一動作条件の図である。これによれば、第４関節（図１１（ｂ）参照）と第６関節（図１２（ｂ）参照）に振動は認められない。速度コントロールにより、関節速度が比較的緩やかにゼロとなるからである。

【００２４】本実施例では、従来例に比べて、特異点近傍通過時の補間回数が大幅に増加し（実験によれば１２
２回→１８８回）、軌跡精度を格段に高めることができる。 これにより、特異点近傍での作業が可能になり、また、補間制御を行いながら任意の関節を最大速度で動作させるといった高速制御も可能になる。 なお、ロボットを従来方式で１度動作させながら、速度変化点や補間間隔量を収集しておき、以後、同一動作を行う際に本発明の方式を適用するようにしてもよい。

【００２５】また、ロボットの動作前に中央処理装置内で仮想的に（シミュレーション的に）動作させ、手先補間間隔を縮めるための各パラメータを収集してもよい。
また、複数の演算装置を用いれば、速度変換点抽出部の計算を先に実行すると共に、遅れて軌跡制御部を動作させることも可能であり、ロボットを動作させながらスムーズに補間することができる。

【００２６】また、本方式では、（ア）〜（ウ）の４段階判定例を示したが、これらに加えてさらに途中点を設定し、その途中点での補間時間を正確に求めることにより、特異点近傍における関節角速度の変化傾向をきめ細かく知ることができ、さらに正確な制御を行うことが可能である。

【００２７】

【発明の効果】本発明によれば、特異点近傍における関節駆動速度と補間間隔を適正化したので、振動発生を回避して軌跡制御の安定性を向上することができると共に、軌跡の非補償時間を短くして軌跡精度を高めることができる。

【図面の簡単な説明】

【図１】一実施例のロボットの概念構成図である。

【図２】一実施例の制御系の概念ブロック図である。

【図３】一実施例の制御系の詳細ブロック図である。

【図４】一実施例の軌跡経路上に設定する補間点を示す図である。

【図５】一実施例の台形速度曲線図である。

【図６】一実施例の特異点近傍通過時における関節角速度特性図である。

【図７】本発明を適用しない場合の第１関節と第２関節の関節速度及び関節角度の制御特性図である。

【図８】本発明を適用しない場合の第３関節と第４関節の関節速度及び関節角度の制御特性図である。

【図９】本発明を適用しない場合の第５関節と第６関節の関節速度及び関節角度の制御特性図である。

【図１０】本発明を適用した場合の第１関節と第２関節の関節速度及び関節角度の制御特性図である。

【図１１】本発明を適用した場合の第３関節と第４関節の関節速度及び関節角度の制御特性図である。

【図１２】本発明を適用した場合の第５関節と第６関節の関節速度及び関節角度の制御特性図である。

【符号の説明】

Ｐ _S ：始点 Ｐ ₁ 、Ｐ ₂ 、……：補間点 Ｐ _E ：終点 １：多関節型ロボット ２：ベース ３：手先 １０：演算処理装置 １０ａ：第１軌跡補間部 １０ｂ：座標変換部 １０ｃ：現在位置読込部 １０ｄ：速度変化点抽出部 １０ｅ：第２軌跡補間部 １１：速度関数発生部 １１ａ：速度計算部 １２：サーボ機構 １２ａ：サーボ制御部