JP3930956B2 - 一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体 - Google Patents
一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体 Download PDFInfo
- Publication number
- JP3930956B2 JP3930956B2 JP33716797A JP33716797A JP3930956B2 JP 3930956 B2 JP3930956 B2 JP 3930956B2 JP 33716797 A JP33716797 A JP 33716797A JP 33716797 A JP33716797 A JP 33716797A JP 3930956 B2 JP3930956 B2 JP 3930956B2
- Authority
- JP
- Japan
- Prior art keywords
- arm
- joint
- axis
- angular velocity
- angle
- 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.)
- Expired - Fee Related
Links
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【０００１】
【発明の属する技術分野】
この発明は、スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体に関し、さらに詳細には直線補間動作を行う一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体に関するものである。
【０００２】
【従来の技術】
ロボット制御において、直線補間による軌跡制御とは、ロボットアーム先端の軌跡が直線になり、その速度が例えば台形速度パターンとなるように制御することである。
【０００３】
図７は、水平面内２自由度スカラ型ロボットを示している。自由度スカラ型ロボット２０は、第１関節軸２１を中心として一平面（ＸＹ平面）内を回動可能な第１アーム２４と、第２関節軸２２を中心として一平面（ＸＹ平面）内を回動可能な第２アーム２５とを有し、第２アーム２５のアーム先端（アーム先端）２７がロボット制御の対象とする座標点である。
【０００４】
図８は、マイクロコンピュータによるロボット制御装置を示している。ロボット制御装置は、中央処理装置１と、入力装置２および出力装置３と中央処理装置１とを接続する入出力インターフェイス４と、メモリ５と、サーボアンプインターフェイス６と、エンコーダインターフェイス７とを有している。メモリ５は、制御プログラム記憶領域８と、アーム先端位置記憶領域９と、関節角度記憶領域１０と、終点位置記憶領域１１と、アーム長記憶領域１２とを含んでいる。
【０００５】
サーボアンプインターフェイス６にはロボット２０の各アームを駆動するために各関節軸に設けられるサーボモータを駆動するサーボアンプ３１が接続され、エンコーダインターフェイス７にはロボット２０の各アームの角度を検出するために各関節軸に設けられる関節エンコーダ３２が接続されている。
【０００６】
従来におけるスカラ型ロボットの直線補間による軌跡制御方法を図９を参照して説明する。まず、中央処理装置１が直線補間を開始する（ステップＳ１０１）。この直線補間の手続きは、上位機器の作業計画などの手続きから呼び出される。この手続きが呼び出されるときには、上位機器の手続きによって現在位置と最終目標位置が確定する。
【０００７】
直線補間を開始すると、先ずメモリ５に記憶された直線補間を行う経路の始点位置、終点位置、指定速度からアーム先端位置増分を求める（ステップＳ１０２）。このアーム先端位置増分は、指定速度にサンプリング時間を乗じて、各サンプリング時刻でのアーム先端位置の始点と終点を結ぶ方向への位置増分として求める。
【０００８】
つぎに、メモリ５の中に設けられているアーム先端位置記憶領域９に記憶された現在（現時点）のアーム先端位置に、ステップＳ１０２にて求めたアーム先端位置増分を加えて、次サンプリング時刻にて到達すべきアーム先端位置を求める（ステップＳ１０３）。つぎに、ステップＳ１０３にて求めた次サンプリング時刻でのアーム先端位置から関節−アーム先端位置の逆変換を行って、次サンプリング時刻における各関節の角度を求める（ステップＳ１０４）。
【０００９】
つぎに、ステップＳ１０４にて求めた次サンプリング時刻における各関節の角度からメモリ５に設けられている関節角度記憶領域１０に記憶された現在の関節角度を減じて、現在のサンプリング時間に進めるべき関節角度を求める（ステップＳ１０５）。つぎに、サーボインターフェイス６を通じてサーボアンプ３１にステップＳ１０５で求めた駆動すべき関節角度を指示し、ロボット２０の各関節に設けられているサーボモータを駆動する（ステップＳ１０６）。
【００１０】
つぎに、時刻が次サンプリング時刻に達した時点で、エンコーダインターフェイス７を通じて、ロボット２０の各関節に設けられている関節エンコーダ３２により各アームの関節角度を検出し、関節角度記憶領域１０の現在の各アームの関節角度の値を更新する（ステップＳ１０７）。つぎに、現在の関節角度から、関節角度−アーム先端位置の正変換によって現在のアーム先端位置を求める（ステップＳ１０８）。
【００１１】
つぎに、ステップＳ１０８で求めた現在のアーム先端位置によってメモリ５の中に設けられたアーム先端位置記憶領域９に書き込まれているアーム先端位置の値を更新する（ステップＳ１０９）。つぎに、メモリ５に設けられている終点位置記憶領域１１に記憶されている終点位置の座標とステップ１０８で求められた現在のアーム先端位置とを比較する（ステップＳ１１０）。現在位置が終点位置に達していれば、終了（ステップＳ１１１）へ分岐し、到達していなければ、ステップＳ１０３へ分岐し、以下のステップを再度実行する。
【００１２】
つぎに、具体的な２自由度スカラ型ロボットの動作を図１０の動作説明図と図１１、図１２に示されているグラフとを用いて説明する。
【００１３】
ここで、２自由度スカラ型ロボットの緒元を、
第１アーム長さＬ1 → ３２０ｍｍ
第２アーム長さＬ2 → ３２０ｍｍ
第１関節最大定格角速度 → ４．１２ｒａｄ／ｓｅｃ
第２関節最大定格角速度 → ４．１２ｒａｄ／ｓｅｃ
サンプリング時間 → ２８．４ｍｓｅｃ
のように設定する。
【００１４】
図１０では動作の具体的な例として、Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ（始点位置）からＸ軸に平行にＸ座標−５００ｍｍ；Ｙ座標−１００ｍｍ（終点位置）までを直線補間で動作する場合を示す。図１０では、アーム先端位置速度が３９０ｍｍ／ｓｅｃ、すなわちサンプリング時間ごとにアーム先端位置の増分が１１ｍｍである場合を０．２８４秒毎：１０サンプリング時刻毎のロボットアームの状態（姿勢）を示した。ここで、速度は関節の動作速度が最大定格を越えないなかで最も大きくなるものを選んでいる。
【００１５】
図１０に示されているように、第１軸（第１関節軸２１）は、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から、符号Ｒで示されているアーム状態：第２アーム２５と進行方向とが直角をなす姿勢になるまで、まず反時計廻り方向に動く。この状態から第１軸は、時計廻り方向に動き、終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）まで動く。このＲ点（一つのアーム状態点）は、第１軸の回転方向が逆転する。すなわち、第１軸、第１アーム２４、第２軸（第２関節軸２２）、第２アーム２５、アーム先端と直線経路で構成されるスライダ・クランク機構のロッキング点（揺動点）である。
【００１６】
図１１は、横軸にアーム先端のＸ座標、縦軸にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図１１で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度を、黒三角「黒△」は先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【００１７】
図１２は、横軸にサンプリング時刻、縦軸には図１１と同様にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図１２で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度を、黒三角「黒△」は先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【００１８】
図１１からアーム先端位置が原点近くに来た時点：ｘ座標が０に近づいた時点で、第１軸の角速度が最大となる。このときに、第１軸の角速度が最大定格を越えないようにするために、指令速度が制約を受けている。このために、Ｘ＝０ｍｍ近傍以外では、第１軸は能力の半分程度しか使われていない。
【００１９】
なお、図１１、図１２に示されている第１軸の角速度は実際の角速度であり、１サンプリング時間（２８．４ｍｓｅｃ）当たりの第１軸角度の変化量を１サンプリング時間（２８．４ｍｓｅｃ）で割り算することにより求められる。
【００２０】
スカラ型ロボットが、塗装作業などＣＰ制御（連続経路制御）に適した作業を行う場合には、アーム先端の速度を一定に保つことは非常に重要な制御の目的の一つである。
【００２１】
【発明が解決しようとする課題】
しかしながら、スカラ型ロボットがピックアンドプレース制御を主体する組み立て作業に用いられる場合には、アーム先端の軌跡が直線であることと同時に速度が一定に保たれることは必須の条件ではない。むしろ、作業の効率を高めるために、始点位置から終点位置までを短時間で直線動作することが強く求められる。このような場合には、スカラ型ロボットのアーム先端速度を一定にするのではなく、第１関節軸２１の能力を一杯に使って作業効率を向上させることが優先される。
【００２２】
このようなことから、第１関節軸２１の角速度が一定で、第１関節軸２１を駆動するサーボモータの能力を全て使って高速動作させる直線補間制御方法が考えられる。
【００２３】
第１関節軸２１の角速度を一定にしてアーム先端が直線上を動作する直線補間制御方法を図１３を参照して説明する。
【００２４】
直線補間を開始する（ステップＳ１０１）。この直線補間の手続きは、上位機器の作業計画などの手続きから呼び出される。この手続きが呼び出されるときには、上位機器の手続きによって現在位置と最終目標位置が確定する。また、上位機器の手続きにより当該直線補間動作における第１関節の角速度が与えられる。直線補間を開始すると、第１関節の角速度一定の条件元でのアーム先端位置増分を求める（ステップＳ１１２）。
【００２５】
図７に示されている２自由度スカラ型ロボットのアーム先端位置（Ｘ，Ｙ）の時間微分、すなわちアーム先端位置速度と、関節角度（θ1 , θ2 ) の時間微分、すなわち関節角速度の関係は、式（２）で表される。ここで、Ｊは、式（３）で表されるヤコビ行列である。
【００２６】
【数１】
【００２７】
【数２】
【００２８】
この逆関係は式（４）で表される。
【００２９】
【数３】
【００３０】
ここでＪｒは、式（５）で表される逆ヤコビ行列である。
【００３１】
【数４】
【００３２】
式（４）は、２元連立方程式である。この第１式より、第１軸の角速度とアーム先端位置の速度の関係は、式（６）で表される。
【００３３】
【数５】
【００３４】
ここで、サンプリング時間当たりの第１軸の角度増分をＪ1 、アーム先端位置のＸ軸方向の位置増分をＶｘ、Ｙ軸方向の位置増分をＶｙとする。角度増分Ｊ1 とアーム先端位置の増分Ｖｘ、Ｖｙとの関係は、式（７）で表される。
【００３５】
【数６】
【００３６】
図１４に示されているように、現在位置をＰ、終点位置をＥとし、アーム先端が直線ＰＥに沿って動作するものとする。現在位置Ｐのアーム先端位置増分をＶｎ、Ｘ軸方向の増分をＶｘ、Ｙ軸方向の増分をＶｙとする。現在位置Ｐから終点位置Ｅまで移動すべき残距離をＤｎ、Ｘ方向の残距離の成分をＰｒｘ、Ｙ方向の残距離の成分をＰｒｙとする。
【００３７】
図１４に示すように、残距離Ｄｎ、Ｐｒｘと移動増分Ｖｎ、Ｖｘとの間には、線分ＰＥを斜辺とする直角三角形と位置増分ベクトルＶｎを斜辺とする直角三角形が相似であることから、式（８）に示す関係が成り立つ。
【００３８】
【数７】
【００３９】
したがって、式（９）の関係が成り立つ。同様にＹ軸方向成分についても式（１０）が成り立つ。
【００４０】
【数８】
【００４１】
式（９）と式（１０）を式（７）に代入して整理すると、式（１１）が成り立つ。
【００４２】
【数９】
【００４３】
式（１１）をアーム先端位置増分Ｖｎについて解くと、式（１２）が成り立つ。
【００４４】
【数１０】
【００４５】
式（１２）の第１軸角度増分Ｊ1 に指令関節角速度を代入することにより、第１軸角速度一定の条件での直線軌跡で、アーム先端を制御するためのアーム先端位置増分Ｖｎが求まる。アーム先端位置増分Ｖｎを式（９）と式（１０）へ代入することにより、アーム先端位置座標の増分が求まる。
【００４６】
上述のようにして、位置増分が求められると、メモリ５に設けられたアーム先端位置記憶領域９に記憶されたアーム先端位置座標に上述のようにして求められた位置増分を加えて、次サンプル時刻で到達するべきアーム先端位置を求める（ステップＳ１０３）。
【００４７】
図１３のフローチャートのステップＳ１０４からステップＳ１１１は、図９に示されているアーム先端速度一定の直線補間制御のフローチャートのものと同じであるから、その説明は省略する。
【００４８】
つぎに、具体的なスカラ型ロボットの動作を図１５の動作説明図と、図１６、図１７に示されているグラフを用いて説明する。スカラ型ロボットの緒元は、前述のものと同じであるものとする。
【００４９】
図１５では動作の具体的な例として、Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ（始点位置）からＸ軸に平行にＸ座標−５００ｍｍ；Ｙ座標−１００ｍｍ（終点位置）までを直線補間で動作する場合を示す。図１５では、第１軸角速度が、２．３ｒａｄ／ｓｅｃである場合を０．２８４秒毎：１０サンプリング時刻毎のロボットアームを示した。ここで、速度は関節の動作速度が最大定格を越えないなかで最も大きくなるものを選んでいる。
【００５０】
図１５に示されているように、第１軸は、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から符号Ｒで示されているアーム状態：第２アーム２５と進行方向が直角をなす姿勢になるまで、まず反時計廻り方向に動く。この状態から第１軸は、時計廻り方向に動き終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）まで動く。このＲ点では、第１軸の回転方向が逆転する。すなわち、第１軸、第１アーム２４、第２軸、第２アーム２５、アーム先端と直線経路で構成されるスライダ・クランク機構のロッキング点である。
【００５１】
図１６は、横軸にアーム先端のＸ座標、縦軸にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図１６で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度（実速度）を、黒三角「黒△」はアーム先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【００５２】
図１７は、横軸にサンプリング時刻、縦軸には図１６と同様にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図１７で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度（実速度）を、黒三角「黒△」はアーム先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【００５３】
図１６にてＸ座標が２３２ｍｍのロッキング点に至るまでは、白四角「□」で表される第１軸角速度は、＋２．３ｒａｄ／ｓｅｃ近傍で、ロッキング点を超えると、−２．３ｒａｄ／ｓｅｃの角速度でほぼ定角速度で動作している。
【００５４】
アーム先端位置速度一定の直線補間動作では、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）の１０００ｍｍの区間を２．５６秒要して移動していたが、角速度一定で制御すると、１．４８秒で移動を完了している。
【００５５】
従来のアーム先端速度一定の直線補間方法では、関節駆動モータの能力を活用し、タクトタイムの短縮を図ることはできないが、上述のような関節角速度一定の直線補間制御方法を採用すれば、関節駆動モータの能力を活用してタクトタイムの短縮を図ることができる。
【００５６】
しかし、上述の関節角速度一定の直線補間制御方法では、アーム先端位置のサンプリング時間における位置の増分が式（１２）によって決定されるため、関節軸の回転方向が逆転するロッキング点にて分母がゼロとなると、アーム先端位置増分Ｖｎが無限大へ発散してしまうと云う不具合がある。
【００５７】
このため、図１６における黒三角「黒△」で示されるアーム先端軸のサンプリング時間あたりのアーム先端位置増分の線図は、このロボットにおけるロッキング点（Ｘ＝２３２ｍｍ）近傍で、無限大に発散している。
【００５８】
また、この発散の影響を受けて、白四角「□」で示された第１軸角速度が、指定された角速度の２倍の回転を行ってしまう。逆に、この部分があるために最大定格回転数式を超えることが生じないように指定角速度を低くしなければならないと云う不具合がある。
【００５９】
この発明は、上述のような問題点を解決するためになされたものであり、アーム先端がロッキング点を速度過大を生ずることなく通過するように改善された一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体を得ることを目的としている。
【００６０】
上述の目的を達成するために、この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置にあっては、第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御装置において、前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、アーム先端の位置増分量演算のための定数角速度指令値として前記第１関節角に対し指定される第１関節角速度指令値を記憶する第１関節角速度記憶部と、直線補間動作を行うに際し、アーム先端の位置増分量を前記第１アームのアーム長と、前記第２関節角の正弦値と、前記第１関節角速度記憶部に記憶されている第１関節角速度指令値と、の乗算により算出する位置増分量演算部と、を備えたことを特徴とする。
【００６１】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置では、第１関節角速度記憶部がアーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度記憶部に記憶されている第１関節角速度指令値との乗算により算出し、直線補間制御を行う。これにより直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００６２】
なお、アーム先端の位置増分量をＶｎ、第１アーム長をＬ1 、第２関節角度をθ2 、第１関節角速度指令値をＪ1 とすると、位置増分量演算部は、Ｖｎ＝Ｌ1 ・ｓｉｎ（θ2 )・Ｊ1 なる演算式によりアーム先端の位置増分量Ｖｎを演算する。
【００６３】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御装置は、上述の発明による一平面内多自由度スカラ型ロボットの軌跡制御装置において、前記一平面内多自由度スカラ型ロボットは２自由度あるいは３自由度のスカラ型ロボットであるものである。
【００６４】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置では、２自由度あるいは３自由度のスカラ型ロボットにおいて、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度記憶部に記憶されている第１関節角速度指令値との乗算により算出し、直線補間制御を行う。これにより２自由度あるいは３自由度のスカラ型ロボットにおいて、直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００６５】
また、上述の目的を達成するために、この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法にあっては、第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御方法において、前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、アーム先端の位置増分量演算のための定数角速度指令値として前記第１関節角に対し指定される第１関節角速度指令値を記憶するとともに、直線補間動作を行うに際し、アーム先端の位置増分量を前記第１アームのアーム長と、前記第２関節角の正弦値と、前記第１関節角速度記憶部に記憶されている第１関節角速度指令値と、の乗算により算出することを特徴とする。
【００６６】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法では、アーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度指令値との乗算により算出して直線補間制御を行う。これにより直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００６７】
また、上述の目的を達成するために、この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法は、一平面内多自由度スカラ型ロボットの軌跡制御方法において、直線補間動作を行うに際してアーム先端の位置増分量を下式（１）により行うことものである。
【００６８】
Ｖｎ＝Ｌ1 ・ｓｉｎ（θ2 ) ・Ｊ1 …（１）再掲
但し、Ｖｎ：アーム先端の位置増分量，Ｌ1 ：第１アーム長，θ2 ：第２関節角，Ｊ1 ：第１関節角に対し指定されるユーザ指定の定数角速度指令値である。
【００６９】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法では、Ｖｎ＝Ｌ1 ・ｓｉｎ（θ2 ) ・Ｊ1 なる演算式によりアーム先端の位置増分量Ｖｎを演算して直線補間制御を行う。これにより直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００７０】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御方法は、上述の発明による一平面内多自由度スカラ型ロボットの軌跡制御方法において、前記一平面内多自由度スカラ型ロボットは２自由度あるいは３自由度のスカラ型ロボットであるものである。
【００７１】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法では、２自由度あるいは３自由度のスカラ型ロボットにおいて、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度指令値との乗算により算出し、直線補間制御を行う。これにより２自由度あるいは３自由度のスカラ型ロボットにおいて、直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００７２】
また、上述の目的を達成するために、この発明による一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータに読みとり可能な記録媒体にあっては、第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体であって、前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、アーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶する第１関節角速度記憶手順と、直線補間動作を行うに際し、アーム先端の位置増分量を前記第１アームのアーム長と、第２関節角の正弦値と、前記第１関節角速度記憶部に記憶されている第１関節角速度指令値と、の乗算により算出する位置増分量演算手順と、をコンピュータに実行させるためのプログラムを記録したものである。
【００７３】
この発明によるコンピュータ読み取り可能な記録媒体は、アーム先端位置増分量演算のための一つのパラメータとして指定される第１関節角速度指令値を記憶する第１関節角速度記憶手順と、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角と第１関節角速度指令値との乗算により算出するアーム先端位置増分量演算手順とをコンピュータに実行させるプログラムを格納しており、この記録媒体に記録されているプログラムをコンピュータが実行することにより、直線補間動作域に亙ってアーム先端位置増分が無限大となることなく直線補間動作が行われる。
【００７４】
【発明の実施の形態】
以下に添付の図を参照してこの発明に係る一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体の実施の形態を詳細に説明する。なお、以下に説明するこの発明の実施の形態において上述の従来例と同一構成の部分は、上述の従来例に付した符号と同一の符号を付してその説明を省略する。
【００７５】
本明細書において、「一平面」とは、水平面や垂直面等の一つの平面のことであり、アームの運動がこの水平面等の一つの平面内で行われる。したがって、アームの回転軸の方向は鉛直方向等全て同じとなる。また、「多自由度」とは、多関節（ａｒｔｉｃｕｌａｔｅｄ）と同じ意味である。水平多関節型ロボットは一般的であり、これがスカラ型と呼ばれる。因に、スカラ（ＳＣＡＲＡ）とは、Ｓｅｌｅｃｔｉｖｅ Ｃｏｍｐｌｉａｎｃｅ Ａｓｓｅｍｂｌｙ Ｒｏｂｏｔ Ａｒｍの略称である。
【００７６】
実施の形態１．
図１はこの発明による軌跡制御装置（ロボット制御装置）の一つの実施の形態を示している。この軌跡制御装置は、メモリ５に、制御プログラム記憶領域８、アーム先端位置記憶領域９、関節角度記憶領域１０、終点位置記憶領域１１、アーム長記憶領域１２に加えて第１関節角速度記憶領域１３を有している。
【００７７】
第１関節角速度記憶領域１３は入力装置２より一つのパラメータとして指定入力されるユーザ指定の第１関節角速度指令値（指定角速度）を記憶する。
【００７８】
アーム先端の位置増分量演算部は、メモリ５の制御プログラム記憶領域８に格納されている制御プログラムを中央処理装置１が実行することにより具現化され、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角度の正弦と第１関節角速度記憶領域１３に記憶されている第１関節角速度指令値との乗算により算出する。
【００７９】
この演算は、アーム先端の位置増分量をＶｎ、第１アーム長をＬ1 、 第２関節角度をθ2 、 第１関節角速度指令値をＪ1 とすると、Ｖｎ＝Ｌ1 ・ ｓｉｎ（θ2 ) ・Ｊ1 なる演算式により行われる。
【００８０】
入力装置２は、アーム先端位置増分量演算のための一つのパラメータとして指定される第１関節角速度指令値を第１関節角速度記憶領域１３に記憶する第１関節角速度記憶手順と、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角度の正弦と第１関節角速度記憶領域１３に記憶されている第１関節角速度指令値との乗算により算出するアーム先端位置増分量演算手順とをコンピュータに実行させるための軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体より軌跡制御プログラムを読み取り、これを制御プログラム記憶領域８に格納する。
【００８１】
つぎに、この発明による軌跡制御方法を図２を参照して説明する。中央処理装置１が直線補間を開始する（ステップＳ１０１）。この直線補間の手続きは、上位機器の作業計画などの手続きから呼び出される。この手続きが呼び出されるときには、上位機器の手続きによって現在位置と最終目標位置が確定する。また、上位機器の手続きにより当該直線補間動作における第１関節の角速度Ｊ1 が与えられる。
【００８２】
直線補間を開始すると、ユーザ指定の第１関節角速度指令値Ｊ1 および式（１）によってアーム先端位置増分Ｖｎを演算、決定する（ステップＳ２０２）。
【００８３】
Ｖｎ＝Ｌ1 ・ ｓｉｎ（θ2 ) ・Ｊ1 …（１）再掲
但し、Ｖｎ：アーム先端の位置増分量，Ｌ1 ：第１アーム長，θ2 ：第２関節角度，Ｊ1 ：ユーザ指定の第１関節角速度指令値である。
【００８４】
また、上記式（１２）は、式（１３）のように変形することができる。
【００８５】
【数１１】
【００８６】
式（１３）の分母は、すべての動作範囲において、０以上１以下の値をとるものである。ロッキング点にて、この分母の部分がゼロになるために、アーム先端位置増分量が、無限大へ発散する不具合が生じるものである。
【００８７】
そこで、式（１３）の分母を１に固定することにより、式（１）が導出される。式（１）の値は、必ず式（１３）の値を超えることはない。これにより、ステップＳ１０４にてアーム先端位置から逆変換によって関節角度を求め、ステップＳ１０５で関節角度増分を求めた場合に、関節の角速度一定で制御した場合より小さな角度増分が決定される。このために式（１）において指令角速度Ｊ1 を定格最大角速度としても、それを超える関節角度指令が生成されることはない。
【００８８】
図２のフローチャートのステップＳ１０３からステップＳ１１１は、図１３に示されている角速度一定の直線補間制御のフローチャートのものと同じであるから、その説明は省略する。
【００８９】
つぎに、具体的なスカラ型ロボットの動作を図３の動作説明図と図４、図５に示されているグラフとを用いて説明する。なお、スカラ型ロボットの緒元は、前述のものと同じであるものとする。
【００９０】
ここで、図３では動作の具体的な例として、Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ（始点位置）からＸ軸に平行にＸ座標−５００ｍｍ；Ｙ座標−１００ｍｍ（終点位置）までを直線補間で動作する場合を示す。図３では、第１軸角速度指令値が定格最大角速度４．１２ｒａｄ／ｓｅｃである場合を０．２８４秒毎：１０サンプリング時刻毎のロボットアームを示した。
【００９１】
図３に示されているように、第１軸は、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から符号Ｒで示されているアーム状態：第２アーム２５と進行方向が直角をなす姿勢になるまで、まず反時計廻り方向に動く。この状態から第１軸は、時計廻り方向に動き終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）まで動く。このＲ点では、第１軸の回転方向が逆転する。すなわち、第１軸、第１アーム２４、第２軸、第２アーム２５、アーム先端と直線経路で構成されるスライダ・クランク機構のロッキング点である。
【００９２】
図４は、横軸にアーム先端のＸ座標、縦軸にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図４で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度（実速度）を、黒三角「黒△」はアーム先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。つぎに、図４のグラフの作成手順について説明する。
【００９３】
（１）Ｘ０= ５００の位置における第２関節角度θ2 をもとに、式（１３）の分母を１とし、アーム先端の位置増分量Ｖｎを算出する。Ｖｎ＝ほぼ３６ｍｍをこれは式（１）を使用して計算したものと同じ結果である。
【００９４】
なお、第１アーム長Ｌ1 は３２０ｍｍ、第１関節角速度指令値Ｊ1 は４．１２ｒａｄ／ｓｅｃであり、サンプリング時間は２８．４ｍｓｅｃであり、第１軸角度（第１関節角度）θ1 と第２関節角度θ2 は、アーム先端の位置Ｘ，Ｙ（−１００ｍｍ固定）と、第１および第２アーム長（ともに３２０ｍｍ）から決まる。
【００９５】
Ｘ０= ５００の位置における第１軸角度θ1 はほぼ０．４５２ｒａｄであり、第２関節角度θ2 はほぼ１．３００ｒａｄである。
【００９６】
（２）２４ｍｓｅｃ後（１サンプリング時間後）のＸ1 位置を算出する。
Ｘ1 = ５００−Ｖｎ＝５００−ほぼ３６＝ほぼ４６４ｍｍ
Ｘ1 位置における第２関節角度θ2 をもとに、つぎの位置増分量Ｖｎを算出する。また第１軸角度θ1 はＸ1 位置から逆算できる。
アーム先端の位置増分量Ｖｎ＝ほぼ３７．２ｍｍ
第１軸角度θ1 = ほぼ０．５２３ｒａｄ
第１軸角速度（実速度）＝第１軸角度変化量（ほぼ０．５２３ｒａｄ−ほぼ０．４５２ｒａｄ）／２８．４ｍｓｅｃ
【００９７】
以上の計算をＸ1 位置が−５００ｍｍ付近まで繰り返し、アーム先端の位置増分量、第１軸角度、第１軸角速度（実速度）をグラフにプロットすると、図４が得られる。
【００９８】
なお、式（１３）の分母に、θ1 、 θ2 の値が関係するため、実際の第１軸角速度は第１関節角速度指令値Ｊ1 より小さい値になる。
【００９９】
図５は、横軸にサンプリング時刻、縦軸には図４と同様にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図５で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度（実速度）を、黒三角「黒△」はアーム先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【０１００】
図４にてＸ座標が２３２ｍｍのロッキング点においても、黒三角「黒△」で示されるアーム先端軸のサンプリング時間あたりのアーム先端位置増分の線図は、滑らかに変化し、図１６、図１７に示されているように、無限大へ発散することはない。
【０１０１】
また、白四角「□」で示された実際の第１軸角速度も滑らかな変化をしており、図１６、図１７に示されているように、急激な変化を生じることがない。このため、、式（１）で用いる指令角速度指令値Ｊ1 を定格最大角速度としても、角速度過大を生ずることなくロボットを運転することが可能となる。
【０１０２】
また、アーム先端位置速度一定の直線補間動作では、、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）の１０００ｍｍの区間を２．５６秒要して移動していたが、角速度一定で制御すると、１．３３秒で移動を完了している。
【０１０３】
また、角速度一定では、式（１２）によって、位置増分量を決定するので、計算量が、加減算３、乗算５、除算１、三角関数式３の計算が必要であるが、この発明による軌跡制御方法では、位置増分量を決定する計算量が、乗算２、三角関数式１で済む。具体的に計算量を比較すると、インテル社の８０３８６マイクロプロセッサと８０３８７数式値演算プロセッサを用いると、加減算には４５クロック、乗算には６５クロック、除算には１０２クロック、三角関数式には７７１クロックが必要であるので、角速度一定方式では、２８７５クロックが必要であるのに対して、この発明による軌跡制御方法では、計算負荷の大きな除算、三角関数式の分量が少ないので、９０１クロックで計算が完了し、所用時間はおよそ１／３とすることが可能となる。
【０１０４】
なお、上述の実施の形態は、図７で示す２自由度スカラ型ロボットの場合であるが、この発明は図６に示されているような３自由度スカラ型ロボットにも拡張して適用することが可能である。図６は図７に示されている２自由度スカラ型ロボットのアーム先端に、第３関節２３を加え、その第３関節２３によってハンド２６を駆動することにより、もう一つの自由度を加えたものであり、アーム先端の位置に加えて姿勢（角度Ａ）を制御するものである。
【０１０５】
この３自由度スカラ型ロボットでも、図２に示されているアルゴリズムにて直線補間制御を行う。従って、ｂの場合も、式（１）によってアーム先端位置増分Ｖｎを演算、決定する。
【０１０６】
Ｖｎ＝Ｌ1 ・ ｓｉｎ（θ2 ) ・Ｊ1 …（１）再掲
【０１０７】
ここで、３自由度スカラ型ロボットにおける式（１）の導出について説明する。
【０１０８】
以後、ＪＩＳ（日本工業規格）に従い、アーム先端の位置と姿勢を合わせたものをポーズとよぶ。図６に示されている３自由度スカラ型ロボットでは、ポーズは、アーム先端の位置のＸ軸成分、Ｙ軸成分と姿勢角度Ａの３要素から成り立つベクトルである。
【０１０９】
３自由度スカラ型ロボットのポーズと関節角度のそれぞれの時間微分量の関係は、式（１４）により表される。
【０１１０】
【数１２】
【０１１１】
ここで、Ｊは式（１５）で表されるヤコビ行列である。
【０１１２】
【数１３】
【０１１３】
従って、この逆関係は式（１６）で表される。
【０１１４】
【数１４】
【０１１５】
ここで、Ｊｒは式（１７）で表される逆ヤコビ行列である。
【０１１６】
【数１５】
【０１１７】
式（１６）は３元線形連立方程式である。この第１式から、第１軸の角速度とアーム先端のポーズの時間微分（すなわちアーム先端の速度と姿勢の角速度）の関係は、式（１８）にて表される。
【０１１８】
【数１６】
【０１１９】
ここで、サンプリング時間当たりの第１軸の角度の増分をＪ1 、 アーム先端位置のＸ軸方向の位置の増分をＶｘ、Ｙ軸方向の位置の増分をＶｙ、姿勢軸の角度の増分をＶＡとする。第１軸の角度とアーム先端ポーズの増分の関係は、式（１９）で表される。
【０１２０】
【数１７】
【０１２１】
２自由度スカラ型ロボットの場合に図１４に示したように定義した現在位置Ｐから終点までの残距離Ｄｎをアーム先端の姿勢を考慮し拡張する。現在ポーズから終点までの移動するべき残距離Ｄｍを式（２０）のように定義する。
【０１２２】
【数１８】
【０１２３】
ここで、右辺かっこ内の第２項は、残姿勢角度ＶＡに式（２１）で定義する所の角度＝距離換算係数式Ｊｎｍを乗じたものである。すなわち、式（２１）は、アーム先端の残距離もしくは、残姿勢角度のうち、いずれか大きい値のものをＤｍｎとして採用するものである。この換算係数式は、アーム先端の定格最大速度Ｓｐｍａｘを、姿勢を制御する第３軸の定格最大角速度で除したものである。
【０１２４】
【数１９】
【０１２５】
式（９）、式（１０）と同様にして、アーム先端位置の増分量Ｖｎとポーズのそれぞれの成分の関係は、式（２２）、式（２３）、式（２４）で表される。
【０１２６】
【数２０】
【０１２７】
【数２１】
【０１２８】
【数２２】
【０１２９】
式（２２）、式（２３）、式（２４）を式（１９）へ代入し、整理すると、式（２５）が得られる。
【０１３０】
【数２３】
【０１３１】
これを、アーム先端位置増分量Ｖｎについて解くと、式（２６）が得られる。
【０１３２】
【数２４】
【０１３３】
この式は、２自由度スカラ型ロボットの場合の式（１２）に相当するものである。この式により求められるアーム先端位置増分量Ｖｎは、式（１２）と同様に角速度一定で直線補間を行う場合を３自由度スカラ型ロボットに拡張したものである。
【０１３４】
式（２６）によれば、２自由度スカラ型ロボットの場合と同様に、ロッキング点においてアーム先端位置増分の発散が生じてしまう。
【０１３５】
これは、式（２６）の分母の部分がロッキング点でゼロの値を取るためである。この分母を拡張された残距離Ｄｍで除した値は、始点位置では１となり、終点位置では０となるため、不等式（式（２７））が成り立つ。
【０１３６】
【数２５】
【０１３７】
ここで、式（２７）の項を最大値の１に置き換えて式（２６）へ代入すると、結局、式（１）が得られる。
【０１３８】
この実施の形態では、直線動作におけるアーム先端位置増分を式（１）にて決定するため、ロッキング点でのアーム先端速度の発散を生じることがない。
【０１３９】
アーム先端位置増分量を角速度一定として求める式（２６）によれば、乗算７、除算１、加算２、三角関数式４の演算が必要であり、インテル社の８０３８６マイクロプロセッサーと８０３８７数式値演算プロセッサーを用いると、３７３１クロックが必要である。これに対し式（１）によれば、９０１クロックで演算が終了するので、大幅な演算時間の短縮を図ることができる。演算時間の短縮により、サンプリング時間の短縮が可能となり、軌跡精度の向上などが期待できる。
【０１４０】
【発明の効果】
以上の説明から理解される如く、この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置では、第１関節角速度記憶部がアーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、アーム先端の位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度記憶部に記憶されている第１関節角速度指令値との乗算により算出し、直線補間制御を行うから、全直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。加えて、アーム先端位置速度を一定に保つ従来の直線補間のように、アーム先端が原点近傍を通過する場合に大きな第１軸の角速度を必要としないので、原点近傍通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。また、計算量を少なくすることができるので、サンプリング時間の短縮が可能となり、軌跡精度の向上が期待できる。
【０１４１】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御装置によれば、２自由度あるいは３自由度のスカラ型ロボットにおいて、第１関節角速度記憶部がアーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度記憶部に記憶されている第１関節角速度指令値との乗算により算出し、直線補間制御を行うから、２自由度あるいは３自由度のスカラ型ロボットにおいて、直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。
【０１４２】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御方法によれば、アーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度指令値との乗算により算出して直線補間制御を行うから、直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。加えて、アーム先端位置速度を一定に保つ従来の直線補間のように、アーム先端が原点近傍を通過する場合に大きな第１軸の角速度を必要としないので、原点近傍通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。また、計算量を少なくすることができるので、サンプリング時間の短縮が可能となり、軌跡精度の向上が期待できる。
【０１４３】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御方法によれば、Ｖｎ＝Ｌ1 ・ｓｉｎ（θ2 ) ・Ｊ1 なる演算式によりアーム先端の位置増分量Ｖｎを演算して直線補間制御を行うから、直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。加えて、アーム先端位置速度を一定に保つ従来の直線補間のように、アーム先端が原点近傍を通過する場合に大きな第１軸の角速度を必要としないので、原点近傍通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。また、計算量を少なくすることができるので、サンプリング時間の短縮が可能となり、軌跡精度の向上が期待できる。
【０１４４】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御方法によれば、２自由度あるいは３自由度のスカラ型ロボットにおいて、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度指令値との乗算により算出し、直線補間制御を行うから、２自由度あるいは３自由度のスカラ型ロボットにおいて、直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。
【０１４５】
つぎの発明によるコンピュータ読み取り可能な記録媒体によれば、当該記録媒体に記録されているプログラムをコンピュータが実行することにより、直線補間動作域に亙ってアーム先端位置増分が無限大となることなく直線補間動作が行われ、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。加えて、アーム先端位置速度を一定に保つ従来の直線補間のように、アーム先端が原点近傍を通過する場合に大きな第１軸の角速度を必要としないので、原点近傍通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。また、計算量を少なくすることができるので、サンプリング時間の短縮が可能となり、軌跡精度の向上が期待できる。
【図面の簡単な説明】
【図１】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置の一つの実施の形態を示すブロック線図である。
【図２】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法の一つの実施の形態を示すフローチャートである。
【図３】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御におけるアーム動作経路を示す説明図である。
【図４】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御におけるアーム先端位置に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図５】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御におけるサンプリング時刻に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図６】 姿勢軸を有する一平面内３自由度スカラ型ロボットを示す機構図である。
【図７】 水平面内２自由度スカラ型ロボットを示す機構図である。
【図８】 ロボット制御装置の概略構成を示すブロック線図である。
【図９】 アーム先端位置速度一定の直線補間動作示すフローチャートである。
【図１０】 アーム先端位置速度一定の直線補間動作におけるアーム動作経路を示す説明図である。
【図１１】 アーム先端位置速度一定の直線補間動作におけるアーム先端位置に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図１２】 アーム先端位置速度一定の直線補間動作におけるサンプリング時刻に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図１３】 角速度一定制御によるロボット制御の直線補間制御を示すフローチャートである。
【図１４】 直線補間動作のアーム先端位置の増分量を示す説明図である。
【図１５】 角速度一定の直線補間動作におけるアーム動作経路を示す説明図である。
【図１６】 角速度一定の直線補間動作におけるアーム先端位置に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図１７】 角速度一定の直線補間動作におけるサンプリング時刻に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【符号の説明】
１ 中央処理装置，２ 入力装置，３ 出力装置，４ 入出力インターフェイス，５ メモリ，６ サーボアンプインターフェイス，７ エンコーダインターフェイス，８ 制御プログラム記憶領域，９ アーム先端位置記憶領域，１０関節角度記憶領域，１１ 終点位置記憶領域，１２ アーム長記憶領域，１３第１関節角速度記憶領域，２０ ロボット，２１ 第１関節軸，２２ 第２関節軸，２３ 第３関節軸，２４ 第１アーム，２５ 第２アーム，２６ ハンド，２７ アーム先端，３１ サーボアンプ，３２ 関節エンコーダ。
【発明の属する技術分野】
この発明は、スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体に関し、さらに詳細には直線補間動作を行う一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体に関するものである。
【０００２】
【従来の技術】
ロボット制御において、直線補間による軌跡制御とは、ロボットアーム先端の軌跡が直線になり、その速度が例えば台形速度パターンとなるように制御することである。
【０００３】
図７は、水平面内２自由度スカラ型ロボットを示している。自由度スカラ型ロボット２０は、第１関節軸２１を中心として一平面（ＸＹ平面）内を回動可能な第１アーム２４と、第２関節軸２２を中心として一平面（ＸＹ平面）内を回動可能な第２アーム２５とを有し、第２アーム２５のアーム先端（アーム先端）２７がロボット制御の対象とする座標点である。
【０００４】
図８は、マイクロコンピュータによるロボット制御装置を示している。ロボット制御装置は、中央処理装置１と、入力装置２および出力装置３と中央処理装置１とを接続する入出力インターフェイス４と、メモリ５と、サーボアンプインターフェイス６と、エンコーダインターフェイス７とを有している。メモリ５は、制御プログラム記憶領域８と、アーム先端位置記憶領域９と、関節角度記憶領域１０と、終点位置記憶領域１１と、アーム長記憶領域１２とを含んでいる。
【０００５】
サーボアンプインターフェイス６にはロボット２０の各アームを駆動するために各関節軸に設けられるサーボモータを駆動するサーボアンプ３１が接続され、エンコーダインターフェイス７にはロボット２０の各アームの角度を検出するために各関節軸に設けられる関節エンコーダ３２が接続されている。
【０００６】
従来におけるスカラ型ロボットの直線補間による軌跡制御方法を図９を参照して説明する。まず、中央処理装置１が直線補間を開始する（ステップＳ１０１）。この直線補間の手続きは、上位機器の作業計画などの手続きから呼び出される。この手続きが呼び出されるときには、上位機器の手続きによって現在位置と最終目標位置が確定する。
【０００７】
直線補間を開始すると、先ずメモリ５に記憶された直線補間を行う経路の始点位置、終点位置、指定速度からアーム先端位置増分を求める（ステップＳ１０２）。このアーム先端位置増分は、指定速度にサンプリング時間を乗じて、各サンプリング時刻でのアーム先端位置の始点と終点を結ぶ方向への位置増分として求める。
【０００８】
つぎに、メモリ５の中に設けられているアーム先端位置記憶領域９に記憶された現在（現時点）のアーム先端位置に、ステップＳ１０２にて求めたアーム先端位置増分を加えて、次サンプリング時刻にて到達すべきアーム先端位置を求める（ステップＳ１０３）。つぎに、ステップＳ１０３にて求めた次サンプリング時刻でのアーム先端位置から関節−アーム先端位置の逆変換を行って、次サンプリング時刻における各関節の角度を求める（ステップＳ１０４）。
【０００９】
つぎに、ステップＳ１０４にて求めた次サンプリング時刻における各関節の角度からメモリ５に設けられている関節角度記憶領域１０に記憶された現在の関節角度を減じて、現在のサンプリング時間に進めるべき関節角度を求める（ステップＳ１０５）。つぎに、サーボインターフェイス６を通じてサーボアンプ３１にステップＳ１０５で求めた駆動すべき関節角度を指示し、ロボット２０の各関節に設けられているサーボモータを駆動する（ステップＳ１０６）。
【００１０】
つぎに、時刻が次サンプリング時刻に達した時点で、エンコーダインターフェイス７を通じて、ロボット２０の各関節に設けられている関節エンコーダ３２により各アームの関節角度を検出し、関節角度記憶領域１０の現在の各アームの関節角度の値を更新する（ステップＳ１０７）。つぎに、現在の関節角度から、関節角度−アーム先端位置の正変換によって現在のアーム先端位置を求める（ステップＳ１０８）。
【００１１】
つぎに、ステップＳ１０８で求めた現在のアーム先端位置によってメモリ５の中に設けられたアーム先端位置記憶領域９に書き込まれているアーム先端位置の値を更新する（ステップＳ１０９）。つぎに、メモリ５に設けられている終点位置記憶領域１１に記憶されている終点位置の座標とステップ１０８で求められた現在のアーム先端位置とを比較する（ステップＳ１１０）。現在位置が終点位置に達していれば、終了（ステップＳ１１１）へ分岐し、到達していなければ、ステップＳ１０３へ分岐し、以下のステップを再度実行する。
【００１２】
つぎに、具体的な２自由度スカラ型ロボットの動作を図１０の動作説明図と図１１、図１２に示されているグラフとを用いて説明する。
【００１３】
ここで、２自由度スカラ型ロボットの緒元を、
第１アーム長さＬ1 → ３２０ｍｍ
第２アーム長さＬ2 → ３２０ｍｍ
第１関節最大定格角速度 → ４．１２ｒａｄ／ｓｅｃ
第２関節最大定格角速度 → ４．１２ｒａｄ／ｓｅｃ
サンプリング時間 → ２８．４ｍｓｅｃ
のように設定する。
【００１４】
図１０では動作の具体的な例として、Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ（始点位置）からＸ軸に平行にＸ座標−５００ｍｍ；Ｙ座標−１００ｍｍ（終点位置）までを直線補間で動作する場合を示す。図１０では、アーム先端位置速度が３９０ｍｍ／ｓｅｃ、すなわちサンプリング時間ごとにアーム先端位置の増分が１１ｍｍである場合を０．２８４秒毎：１０サンプリング時刻毎のロボットアームの状態（姿勢）を示した。ここで、速度は関節の動作速度が最大定格を越えないなかで最も大きくなるものを選んでいる。
【００１５】
図１０に示されているように、第１軸（第１関節軸２１）は、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から、符号Ｒで示されているアーム状態：第２アーム２５と進行方向とが直角をなす姿勢になるまで、まず反時計廻り方向に動く。この状態から第１軸は、時計廻り方向に動き、終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）まで動く。このＲ点（一つのアーム状態点）は、第１軸の回転方向が逆転する。すなわち、第１軸、第１アーム２４、第２軸（第２関節軸２２）、第２アーム２５、アーム先端と直線経路で構成されるスライダ・クランク機構のロッキング点（揺動点）である。
【００１６】
図１１は、横軸にアーム先端のＸ座標、縦軸にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図１１で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度を、黒三角「黒△」は先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【００１７】
図１２は、横軸にサンプリング時刻、縦軸には図１１と同様にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図１２で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度を、黒三角「黒△」は先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【００１８】
図１１からアーム先端位置が原点近くに来た時点：ｘ座標が０に近づいた時点で、第１軸の角速度が最大となる。このときに、第１軸の角速度が最大定格を越えないようにするために、指令速度が制約を受けている。このために、Ｘ＝０ｍｍ近傍以外では、第１軸は能力の半分程度しか使われていない。
【００１９】
なお、図１１、図１２に示されている第１軸の角速度は実際の角速度であり、１サンプリング時間（２８．４ｍｓｅｃ）当たりの第１軸角度の変化量を１サンプリング時間（２８．４ｍｓｅｃ）で割り算することにより求められる。
【００２０】
スカラ型ロボットが、塗装作業などＣＰ制御（連続経路制御）に適した作業を行う場合には、アーム先端の速度を一定に保つことは非常に重要な制御の目的の一つである。
【００２１】
【発明が解決しようとする課題】
しかしながら、スカラ型ロボットがピックアンドプレース制御を主体する組み立て作業に用いられる場合には、アーム先端の軌跡が直線であることと同時に速度が一定に保たれることは必須の条件ではない。むしろ、作業の効率を高めるために、始点位置から終点位置までを短時間で直線動作することが強く求められる。このような場合には、スカラ型ロボットのアーム先端速度を一定にするのではなく、第１関節軸２１の能力を一杯に使って作業効率を向上させることが優先される。
【００２２】
このようなことから、第１関節軸２１の角速度が一定で、第１関節軸２１を駆動するサーボモータの能力を全て使って高速動作させる直線補間制御方法が考えられる。
【００２３】
第１関節軸２１の角速度を一定にしてアーム先端が直線上を動作する直線補間制御方法を図１３を参照して説明する。
【００２４】
直線補間を開始する（ステップＳ１０１）。この直線補間の手続きは、上位機器の作業計画などの手続きから呼び出される。この手続きが呼び出されるときには、上位機器の手続きによって現在位置と最終目標位置が確定する。また、上位機器の手続きにより当該直線補間動作における第１関節の角速度が与えられる。直線補間を開始すると、第１関節の角速度一定の条件元でのアーム先端位置増分を求める（ステップＳ１１２）。
【００２５】
図７に示されている２自由度スカラ型ロボットのアーム先端位置（Ｘ，Ｙ）の時間微分、すなわちアーム先端位置速度と、関節角度（θ1 , θ2 ) の時間微分、すなわち関節角速度の関係は、式（２）で表される。ここで、Ｊは、式（３）で表されるヤコビ行列である。
【００２６】
【数１】
【数２】
この逆関係は式（４）で表される。
【００２９】
【数３】
ここでＪｒは、式（５）で表される逆ヤコビ行列である。
【００３１】
【数４】
式（４）は、２元連立方程式である。この第１式より、第１軸の角速度とアーム先端位置の速度の関係は、式（６）で表される。
【００３３】
【数５】
ここで、サンプリング時間当たりの第１軸の角度増分をＪ1 、アーム先端位置のＸ軸方向の位置増分をＶｘ、Ｙ軸方向の位置増分をＶｙとする。角度増分Ｊ1 とアーム先端位置の増分Ｖｘ、Ｖｙとの関係は、式（７）で表される。
【００３５】
【数６】
図１４に示されているように、現在位置をＰ、終点位置をＥとし、アーム先端が直線ＰＥに沿って動作するものとする。現在位置Ｐのアーム先端位置増分をＶｎ、Ｘ軸方向の増分をＶｘ、Ｙ軸方向の増分をＶｙとする。現在位置Ｐから終点位置Ｅまで移動すべき残距離をＤｎ、Ｘ方向の残距離の成分をＰｒｘ、Ｙ方向の残距離の成分をＰｒｙとする。
【００３７】
図１４に示すように、残距離Ｄｎ、Ｐｒｘと移動増分Ｖｎ、Ｖｘとの間には、線分ＰＥを斜辺とする直角三角形と位置増分ベクトルＶｎを斜辺とする直角三角形が相似であることから、式（８）に示す関係が成り立つ。
【００３８】
【数７】
したがって、式（９）の関係が成り立つ。同様にＹ軸方向成分についても式（１０）が成り立つ。
【００４０】
【数８】
式（９）と式（１０）を式（７）に代入して整理すると、式（１１）が成り立つ。
【００４２】
【数９】
式（１１）をアーム先端位置増分Ｖｎについて解くと、式（１２）が成り立つ。
【００４４】
【数１０】
式（１２）の第１軸角度増分Ｊ1 に指令関節角速度を代入することにより、第１軸角速度一定の条件での直線軌跡で、アーム先端を制御するためのアーム先端位置増分Ｖｎが求まる。アーム先端位置増分Ｖｎを式（９）と式（１０）へ代入することにより、アーム先端位置座標の増分が求まる。
【００４６】
上述のようにして、位置増分が求められると、メモリ５に設けられたアーム先端位置記憶領域９に記憶されたアーム先端位置座標に上述のようにして求められた位置増分を加えて、次サンプル時刻で到達するべきアーム先端位置を求める（ステップＳ１０３）。
【００４７】
図１３のフローチャートのステップＳ１０４からステップＳ１１１は、図９に示されているアーム先端速度一定の直線補間制御のフローチャートのものと同じであるから、その説明は省略する。
【００４８】
つぎに、具体的なスカラ型ロボットの動作を図１５の動作説明図と、図１６、図１７に示されているグラフを用いて説明する。スカラ型ロボットの緒元は、前述のものと同じであるものとする。
【００４９】
図１５では動作の具体的な例として、Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ（始点位置）からＸ軸に平行にＸ座標−５００ｍｍ；Ｙ座標−１００ｍｍ（終点位置）までを直線補間で動作する場合を示す。図１５では、第１軸角速度が、２．３ｒａｄ／ｓｅｃである場合を０．２８４秒毎：１０サンプリング時刻毎のロボットアームを示した。ここで、速度は関節の動作速度が最大定格を越えないなかで最も大きくなるものを選んでいる。
【００５０】
図１５に示されているように、第１軸は、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から符号Ｒで示されているアーム状態：第２アーム２５と進行方向が直角をなす姿勢になるまで、まず反時計廻り方向に動く。この状態から第１軸は、時計廻り方向に動き終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）まで動く。このＲ点では、第１軸の回転方向が逆転する。すなわち、第１軸、第１アーム２４、第２軸、第２アーム２５、アーム先端と直線経路で構成されるスライダ・クランク機構のロッキング点である。
【００５１】
図１６は、横軸にアーム先端のＸ座標、縦軸にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図１６で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度（実速度）を、黒三角「黒△」はアーム先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【００５２】
図１７は、横軸にサンプリング時刻、縦軸には図１６と同様にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図１７で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度（実速度）を、黒三角「黒△」はアーム先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【００５３】
図１６にてＸ座標が２３２ｍｍのロッキング点に至るまでは、白四角「□」で表される第１軸角速度は、＋２．３ｒａｄ／ｓｅｃ近傍で、ロッキング点を超えると、−２．３ｒａｄ／ｓｅｃの角速度でほぼ定角速度で動作している。
【００５４】
アーム先端位置速度一定の直線補間動作では、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）の１０００ｍｍの区間を２．５６秒要して移動していたが、角速度一定で制御すると、１．４８秒で移動を完了している。
【００５５】
従来のアーム先端速度一定の直線補間方法では、関節駆動モータの能力を活用し、タクトタイムの短縮を図ることはできないが、上述のような関節角速度一定の直線補間制御方法を採用すれば、関節駆動モータの能力を活用してタクトタイムの短縮を図ることができる。
【００５６】
しかし、上述の関節角速度一定の直線補間制御方法では、アーム先端位置のサンプリング時間における位置の増分が式（１２）によって決定されるため、関節軸の回転方向が逆転するロッキング点にて分母がゼロとなると、アーム先端位置増分Ｖｎが無限大へ発散してしまうと云う不具合がある。
【００５７】
このため、図１６における黒三角「黒△」で示されるアーム先端軸のサンプリング時間あたりのアーム先端位置増分の線図は、このロボットにおけるロッキング点（Ｘ＝２３２ｍｍ）近傍で、無限大に発散している。
【００５８】
また、この発散の影響を受けて、白四角「□」で示された第１軸角速度が、指定された角速度の２倍の回転を行ってしまう。逆に、この部分があるために最大定格回転数式を超えることが生じないように指定角速度を低くしなければならないと云う不具合がある。
【００５９】
この発明は、上述のような問題点を解決するためになされたものであり、アーム先端がロッキング点を速度過大を生ずることなく通過するように改善された一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体を得ることを目的としている。
【００６０】
上述の目的を達成するために、この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置にあっては、第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御装置において、前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、アーム先端の位置増分量演算のための定数角速度指令値として前記第１関節角に対し指定される第１関節角速度指令値を記憶する第１関節角速度記憶部と、直線補間動作を行うに際し、アーム先端の位置増分量を前記第１アームのアーム長と、前記第２関節角の正弦値と、前記第１関節角速度記憶部に記憶されている第１関節角速度指令値と、の乗算により算出する位置増分量演算部と、を備えたことを特徴とする。
【００６１】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置では、第１関節角速度記憶部がアーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度記憶部に記憶されている第１関節角速度指令値との乗算により算出し、直線補間制御を行う。これにより直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００６２】
なお、アーム先端の位置増分量をＶｎ、第１アーム長をＬ1 、第２関節角度をθ2 、第１関節角速度指令値をＪ1 とすると、位置増分量演算部は、Ｖｎ＝Ｌ1 ・ｓｉｎ（θ2 )・Ｊ1 なる演算式によりアーム先端の位置増分量Ｖｎを演算する。
【００６３】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御装置は、上述の発明による一平面内多自由度スカラ型ロボットの軌跡制御装置において、前記一平面内多自由度スカラ型ロボットは２自由度あるいは３自由度のスカラ型ロボットであるものである。
【００６４】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置では、２自由度あるいは３自由度のスカラ型ロボットにおいて、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度記憶部に記憶されている第１関節角速度指令値との乗算により算出し、直線補間制御を行う。これにより２自由度あるいは３自由度のスカラ型ロボットにおいて、直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００６５】
また、上述の目的を達成するために、この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法にあっては、第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御方法において、前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、アーム先端の位置増分量演算のための定数角速度指令値として前記第１関節角に対し指定される第１関節角速度指令値を記憶するとともに、直線補間動作を行うに際し、アーム先端の位置増分量を前記第１アームのアーム長と、前記第２関節角の正弦値と、前記第１関節角速度記憶部に記憶されている第１関節角速度指令値と、の乗算により算出することを特徴とする。
【００６６】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法では、アーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度指令値との乗算により算出して直線補間制御を行う。これにより直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００６７】
また、上述の目的を達成するために、この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法は、一平面内多自由度スカラ型ロボットの軌跡制御方法において、直線補間動作を行うに際してアーム先端の位置増分量を下式（１）により行うことものである。
【００６８】
Ｖｎ＝Ｌ1 ・ｓｉｎ（θ2 ) ・Ｊ1 …（１）再掲
但し、Ｖｎ：アーム先端の位置増分量，Ｌ1 ：第１アーム長，θ2 ：第２関節角，Ｊ1 ：第１関節角に対し指定されるユーザ指定の定数角速度指令値である。
【００６９】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法では、Ｖｎ＝Ｌ1 ・ｓｉｎ（θ2 ) ・Ｊ1 なる演算式によりアーム先端の位置増分量Ｖｎを演算して直線補間制御を行う。これにより直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００７０】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御方法は、上述の発明による一平面内多自由度スカラ型ロボットの軌跡制御方法において、前記一平面内多自由度スカラ型ロボットは２自由度あるいは３自由度のスカラ型ロボットであるものである。
【００７１】
この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法では、２自由度あるいは３自由度のスカラ型ロボットにおいて、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度指令値との乗算により算出し、直線補間制御を行う。これにより２自由度あるいは３自由度のスカラ型ロボットにおいて、直線補間動作域に亙ってアーム先端位置増分が無限大となることがない。
【００７２】
また、上述の目的を達成するために、この発明による一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータに読みとり可能な記録媒体にあっては、第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体であって、前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、アーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶する第１関節角速度記憶手順と、直線補間動作を行うに際し、アーム先端の位置増分量を前記第１アームのアーム長と、第２関節角の正弦値と、前記第１関節角速度記憶部に記憶されている第１関節角速度指令値と、の乗算により算出する位置増分量演算手順と、をコンピュータに実行させるためのプログラムを記録したものである。
【００７３】
この発明によるコンピュータ読み取り可能な記録媒体は、アーム先端位置増分量演算のための一つのパラメータとして指定される第１関節角速度指令値を記憶する第１関節角速度記憶手順と、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角と第１関節角速度指令値との乗算により算出するアーム先端位置増分量演算手順とをコンピュータに実行させるプログラムを格納しており、この記録媒体に記録されているプログラムをコンピュータが実行することにより、直線補間動作域に亙ってアーム先端位置増分が無限大となることなく直線補間動作が行われる。
【００７４】
【発明の実施の形態】
以下に添付の図を参照してこの発明に係る一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体の実施の形態を詳細に説明する。なお、以下に説明するこの発明の実施の形態において上述の従来例と同一構成の部分は、上述の従来例に付した符号と同一の符号を付してその説明を省略する。
【００７５】
本明細書において、「一平面」とは、水平面や垂直面等の一つの平面のことであり、アームの運動がこの水平面等の一つの平面内で行われる。したがって、アームの回転軸の方向は鉛直方向等全て同じとなる。また、「多自由度」とは、多関節（ａｒｔｉｃｕｌａｔｅｄ）と同じ意味である。水平多関節型ロボットは一般的であり、これがスカラ型と呼ばれる。因に、スカラ（ＳＣＡＲＡ）とは、Ｓｅｌｅｃｔｉｖｅ Ｃｏｍｐｌｉａｎｃｅ Ａｓｓｅｍｂｌｙ Ｒｏｂｏｔ Ａｒｍの略称である。
【００７６】
実施の形態１．
図１はこの発明による軌跡制御装置（ロボット制御装置）の一つの実施の形態を示している。この軌跡制御装置は、メモリ５に、制御プログラム記憶領域８、アーム先端位置記憶領域９、関節角度記憶領域１０、終点位置記憶領域１１、アーム長記憶領域１２に加えて第１関節角速度記憶領域１３を有している。
【００７７】
第１関節角速度記憶領域１３は入力装置２より一つのパラメータとして指定入力されるユーザ指定の第１関節角速度指令値（指定角速度）を記憶する。
【００７８】
アーム先端の位置増分量演算部は、メモリ５の制御プログラム記憶領域８に格納されている制御プログラムを中央処理装置１が実行することにより具現化され、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角度の正弦と第１関節角速度記憶領域１３に記憶されている第１関節角速度指令値との乗算により算出する。
【００７９】
この演算は、アーム先端の位置増分量をＶｎ、第１アーム長をＬ1 、 第２関節角度をθ2 、 第１関節角速度指令値をＪ1 とすると、Ｖｎ＝Ｌ1 ・ ｓｉｎ（θ2 ) ・Ｊ1 なる演算式により行われる。
【００８０】
入力装置２は、アーム先端位置増分量演算のための一つのパラメータとして指定される第１関節角速度指令値を第１関節角速度記憶領域１３に記憶する第１関節角速度記憶手順と、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角度の正弦と第１関節角速度記憶領域１３に記憶されている第１関節角速度指令値との乗算により算出するアーム先端位置増分量演算手順とをコンピュータに実行させるための軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体より軌跡制御プログラムを読み取り、これを制御プログラム記憶領域８に格納する。
【００８１】
つぎに、この発明による軌跡制御方法を図２を参照して説明する。中央処理装置１が直線補間を開始する（ステップＳ１０１）。この直線補間の手続きは、上位機器の作業計画などの手続きから呼び出される。この手続きが呼び出されるときには、上位機器の手続きによって現在位置と最終目標位置が確定する。また、上位機器の手続きにより当該直線補間動作における第１関節の角速度Ｊ1 が与えられる。
【００８２】
直線補間を開始すると、ユーザ指定の第１関節角速度指令値Ｊ1 および式（１）によってアーム先端位置増分Ｖｎを演算、決定する（ステップＳ２０２）。
【００８３】
Ｖｎ＝Ｌ1 ・ ｓｉｎ（θ2 ) ・Ｊ1 …（１）再掲
但し、Ｖｎ：アーム先端の位置増分量，Ｌ1 ：第１アーム長，θ2 ：第２関節角度，Ｊ1 ：ユーザ指定の第１関節角速度指令値である。
【００８４】
また、上記式（１２）は、式（１３）のように変形することができる。
【００８５】
【数１１】
式（１３）の分母は、すべての動作範囲において、０以上１以下の値をとるものである。ロッキング点にて、この分母の部分がゼロになるために、アーム先端位置増分量が、無限大へ発散する不具合が生じるものである。
【００８７】
そこで、式（１３）の分母を１に固定することにより、式（１）が導出される。式（１）の値は、必ず式（１３）の値を超えることはない。これにより、ステップＳ１０４にてアーム先端位置から逆変換によって関節角度を求め、ステップＳ１０５で関節角度増分を求めた場合に、関節の角速度一定で制御した場合より小さな角度増分が決定される。このために式（１）において指令角速度Ｊ1 を定格最大角速度としても、それを超える関節角度指令が生成されることはない。
【００８８】
図２のフローチャートのステップＳ１０３からステップＳ１１１は、図１３に示されている角速度一定の直線補間制御のフローチャートのものと同じであるから、その説明は省略する。
【００８９】
つぎに、具体的なスカラ型ロボットの動作を図３の動作説明図と図４、図５に示されているグラフとを用いて説明する。なお、スカラ型ロボットの緒元は、前述のものと同じであるものとする。
【００９０】
ここで、図３では動作の具体的な例として、Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ（始点位置）からＸ軸に平行にＸ座標−５００ｍｍ；Ｙ座標−１００ｍｍ（終点位置）までを直線補間で動作する場合を示す。図３では、第１軸角速度指令値が定格最大角速度４．１２ｒａｄ／ｓｅｃである場合を０．２８４秒毎：１０サンプリング時刻毎のロボットアームを示した。
【００９１】
図３に示されているように、第１軸は、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から符号Ｒで示されているアーム状態：第２アーム２５と進行方向が直角をなす姿勢になるまで、まず反時計廻り方向に動く。この状態から第１軸は、時計廻り方向に動き終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）まで動く。このＲ点では、第１軸の回転方向が逆転する。すなわち、第１軸、第１アーム２４、第２軸、第２アーム２５、アーム先端と直線経路で構成されるスライダ・クランク機構のロッキング点である。
【００９２】
図４は、横軸にアーム先端のＸ座標、縦軸にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図４で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度（実速度）を、黒三角「黒△」はアーム先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。つぎに、図４のグラフの作成手順について説明する。
【００９３】
（１）Ｘ０= ５００の位置における第２関節角度θ2 をもとに、式（１３）の分母を１とし、アーム先端の位置増分量Ｖｎを算出する。Ｖｎ＝ほぼ３６ｍｍをこれは式（１）を使用して計算したものと同じ結果である。
【００９４】
なお、第１アーム長Ｌ1 は３２０ｍｍ、第１関節角速度指令値Ｊ1 は４．１２ｒａｄ／ｓｅｃであり、サンプリング時間は２８．４ｍｓｅｃであり、第１軸角度（第１関節角度）θ1 と第２関節角度θ2 は、アーム先端の位置Ｘ，Ｙ（−１００ｍｍ固定）と、第１および第２アーム長（ともに３２０ｍｍ）から決まる。
【００９５】
Ｘ０= ５００の位置における第１軸角度θ1 はほぼ０．４５２ｒａｄであり、第２関節角度θ2 はほぼ１．３００ｒａｄである。
【００９６】
（２）２４ｍｓｅｃ後（１サンプリング時間後）のＸ1 位置を算出する。
Ｘ1 = ５００−Ｖｎ＝５００−ほぼ３６＝ほぼ４６４ｍｍ
Ｘ1 位置における第２関節角度θ2 をもとに、つぎの位置増分量Ｖｎを算出する。また第１軸角度θ1 はＸ1 位置から逆算できる。
アーム先端の位置増分量Ｖｎ＝ほぼ３７．２ｍｍ
第１軸角度θ1 = ほぼ０．５２３ｒａｄ
第１軸角速度（実速度）＝第１軸角度変化量（ほぼ０．５２３ｒａｄ−ほぼ０．４５２ｒａｄ）／２８．４ｍｓｅｃ
【００９７】
以上の計算をＸ1 位置が−５００ｍｍ付近まで繰り返し、アーム先端の位置増分量、第１軸角度、第１軸角速度（実速度）をグラフにプロットすると、図４が得られる。
【００９８】
なお、式（１３）の分母に、θ1 、 θ2 の値が関係するため、実際の第１軸角速度は第１関節角速度指令値Ｊ1 より小さい値になる。
【００９９】
図５は、横軸にサンプリング時刻、縦軸には図４と同様にサンプリング時間でのアーム先端位置増分、第１軸角度、第１軸角速度を取った状態図である。図５で、黒丸「●」は第１軸角度を、白四角「□」は第１軸角速度（実速度）を、黒三角「黒△」はアーム先端軸のサンプリング時間あたりのアーム先端位置増分をそれぞれ示している。
【０１００】
図４にてＸ座標が２３２ｍｍのロッキング点においても、黒三角「黒△」で示されるアーム先端軸のサンプリング時間あたりのアーム先端位置増分の線図は、滑らかに変化し、図１６、図１７に示されているように、無限大へ発散することはない。
【０１０１】
また、白四角「□」で示された実際の第１軸角速度も滑らかな変化をしており、図１６、図１７に示されているように、急激な変化を生じることがない。このため、、式（１）で用いる指令角速度指令値Ｊ1 を定格最大角速度としても、角速度過大を生ずることなくロボットを運転することが可能となる。
【０１０２】
また、アーム先端位置速度一定の直線補間動作では、、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）の１０００ｍｍの区間を２．５６秒要して移動していたが、角速度一定で制御すると、１．３３秒で移動を完了している。
【０１０３】
また、角速度一定では、式（１２）によって、位置増分量を決定するので、計算量が、加減算３、乗算５、除算１、三角関数式３の計算が必要であるが、この発明による軌跡制御方法では、位置増分量を決定する計算量が、乗算２、三角関数式１で済む。具体的に計算量を比較すると、インテル社の８０３８６マイクロプロセッサと８０３８７数式値演算プロセッサを用いると、加減算には４５クロック、乗算には６５クロック、除算には１０２クロック、三角関数式には７７１クロックが必要であるので、角速度一定方式では、２８７５クロックが必要であるのに対して、この発明による軌跡制御方法では、計算負荷の大きな除算、三角関数式の分量が少ないので、９０１クロックで計算が完了し、所用時間はおよそ１／３とすることが可能となる。
【０１０４】
なお、上述の実施の形態は、図７で示す２自由度スカラ型ロボットの場合であるが、この発明は図６に示されているような３自由度スカラ型ロボットにも拡張して適用することが可能である。図６は図７に示されている２自由度スカラ型ロボットのアーム先端に、第３関節２３を加え、その第３関節２３によってハンド２６を駆動することにより、もう一つの自由度を加えたものであり、アーム先端の位置に加えて姿勢（角度Ａ）を制御するものである。
【０１０５】
この３自由度スカラ型ロボットでも、図２に示されているアルゴリズムにて直線補間制御を行う。従って、ｂの場合も、式（１）によってアーム先端位置増分Ｖｎを演算、決定する。
【０１０６】
Ｖｎ＝Ｌ1 ・ ｓｉｎ（θ2 ) ・Ｊ1 …（１）再掲
【０１０７】
ここで、３自由度スカラ型ロボットにおける式（１）の導出について説明する。
【０１０８】
以後、ＪＩＳ（日本工業規格）に従い、アーム先端の位置と姿勢を合わせたものをポーズとよぶ。図６に示されている３自由度スカラ型ロボットでは、ポーズは、アーム先端の位置のＸ軸成分、Ｙ軸成分と姿勢角度Ａの３要素から成り立つベクトルである。
【０１０９】
３自由度スカラ型ロボットのポーズと関節角度のそれぞれの時間微分量の関係は、式（１４）により表される。
【０１１０】
【数１２】
ここで、Ｊは式（１５）で表されるヤコビ行列である。
【０１１２】
【数１３】
従って、この逆関係は式（１６）で表される。
【０１１４】
【数１４】
ここで、Ｊｒは式（１７）で表される逆ヤコビ行列である。
【０１１６】
【数１５】
式（１６）は３元線形連立方程式である。この第１式から、第１軸の角速度とアーム先端のポーズの時間微分（すなわちアーム先端の速度と姿勢の角速度）の関係は、式（１８）にて表される。
【０１１８】
【数１６】
ここで、サンプリング時間当たりの第１軸の角度の増分をＪ1 、 アーム先端位置のＸ軸方向の位置の増分をＶｘ、Ｙ軸方向の位置の増分をＶｙ、姿勢軸の角度の増分をＶＡとする。第１軸の角度とアーム先端ポーズの増分の関係は、式（１９）で表される。
【０１２０】
【数１７】
２自由度スカラ型ロボットの場合に図１４に示したように定義した現在位置Ｐから終点までの残距離Ｄｎをアーム先端の姿勢を考慮し拡張する。現在ポーズから終点までの移動するべき残距離Ｄｍを式（２０）のように定義する。
【０１２２】
【数１８】
ここで、右辺かっこ内の第２項は、残姿勢角度ＶＡに式（２１）で定義する所の角度＝距離換算係数式Ｊｎｍを乗じたものである。すなわち、式（２１）は、アーム先端の残距離もしくは、残姿勢角度のうち、いずれか大きい値のものをＤｍｎとして採用するものである。この換算係数式は、アーム先端の定格最大速度Ｓｐｍａｘを、姿勢を制御する第３軸の定格最大角速度で除したものである。
【０１２４】
【数１９】
式（９）、式（１０）と同様にして、アーム先端位置の増分量Ｖｎとポーズのそれぞれの成分の関係は、式（２２）、式（２３）、式（２４）で表される。
【０１２６】
【数２０】
【数２１】
【数２２】
式（２２）、式（２３）、式（２４）を式（１９）へ代入し、整理すると、式（２５）が得られる。
【０１３０】
【数２３】
これを、アーム先端位置増分量Ｖｎについて解くと、式（２６）が得られる。
【０１３２】
【数２４】
この式は、２自由度スカラ型ロボットの場合の式（１２）に相当するものである。この式により求められるアーム先端位置増分量Ｖｎは、式（１２）と同様に角速度一定で直線補間を行う場合を３自由度スカラ型ロボットに拡張したものである。
【０１３４】
式（２６）によれば、２自由度スカラ型ロボットの場合と同様に、ロッキング点においてアーム先端位置増分の発散が生じてしまう。
【０１３５】
これは、式（２６）の分母の部分がロッキング点でゼロの値を取るためである。この分母を拡張された残距離Ｄｍで除した値は、始点位置では１となり、終点位置では０となるため、不等式（式（２７））が成り立つ。
【０１３６】
【数２５】
ここで、式（２７）の項を最大値の１に置き換えて式（２６）へ代入すると、結局、式（１）が得られる。
【０１３８】
この実施の形態では、直線動作におけるアーム先端位置増分を式（１）にて決定するため、ロッキング点でのアーム先端速度の発散を生じることがない。
【０１３９】
アーム先端位置増分量を角速度一定として求める式（２６）によれば、乗算７、除算１、加算２、三角関数式４の演算が必要であり、インテル社の８０３８６マイクロプロセッサーと８０３８７数式値演算プロセッサーを用いると、３７３１クロックが必要である。これに対し式（１）によれば、９０１クロックで演算が終了するので、大幅な演算時間の短縮を図ることができる。演算時間の短縮により、サンプリング時間の短縮が可能となり、軌跡精度の向上などが期待できる。
【０１４０】
【発明の効果】
以上の説明から理解される如く、この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置では、第１関節角速度記憶部がアーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、アーム先端の位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度記憶部に記憶されている第１関節角速度指令値との乗算により算出し、直線補間制御を行うから、全直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。加えて、アーム先端位置速度を一定に保つ従来の直線補間のように、アーム先端が原点近傍を通過する場合に大きな第１軸の角速度を必要としないので、原点近傍通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。また、計算量を少なくすることができるので、サンプリング時間の短縮が可能となり、軌跡精度の向上が期待できる。
【０１４１】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御装置によれば、２自由度あるいは３自由度のスカラ型ロボットにおいて、第１関節角速度記憶部がアーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度記憶部に記憶されている第１関節角速度指令値との乗算により算出し、直線補間制御を行うから、２自由度あるいは３自由度のスカラ型ロボットにおいて、直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。
【０１４２】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御方法によれば、アーム先端の位置増分量演算のための定数角速度指令値として第１関節角に対し指定される第１関節角速度指令値を記憶し、直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度指令値との乗算により算出して直線補間制御を行うから、直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。加えて、アーム先端位置速度を一定に保つ従来の直線補間のように、アーム先端が原点近傍を通過する場合に大きな第１軸の角速度を必要としないので、原点近傍通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。また、計算量を少なくすることができるので、サンプリング時間の短縮が可能となり、軌跡精度の向上が期待できる。
【０１４３】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御方法によれば、Ｖｎ＝Ｌ1 ・ｓｉｎ（θ2 ) ・Ｊ1 なる演算式によりアーム先端の位置増分量Ｖｎを演算して直線補間制御を行うから、直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。加えて、アーム先端位置速度を一定に保つ従来の直線補間のように、アーム先端が原点近傍を通過する場合に大きな第１軸の角速度を必要としないので、原点近傍通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。また、計算量を少なくすることができるので、サンプリング時間の短縮が可能となり、軌跡精度の向上が期待できる。
【０１４４】
つぎの発明による一平面内多自由度スカラ型ロボットの軌跡制御方法によれば、２自由度あるいは３自由度のスカラ型ロボットにおいて、位置増分量演算部が直線補間動作を行うに際してアーム先端の位置増分量を第１アーム長と第２関節角の正弦値と第１関節角速度指令値との乗算により算出し、直線補間制御を行うから、２自由度あるいは３自由度のスカラ型ロボットにおいて、直線補間動作域に亙ってアーム先端位置増分が無限大となることがなく、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。
【０１４５】
つぎの発明によるコンピュータ読み取り可能な記録媒体によれば、当該記録媒体に記録されているプログラムをコンピュータが実行することにより、直線補間動作域に亙ってアーム先端位置増分が無限大となることなく直線補間動作が行われ、ロッキング点通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。加えて、アーム先端位置速度を一定に保つ従来の直線補間のように、アーム先端が原点近傍を通過する場合に大きな第１軸の角速度を必要としないので、原点近傍通過のために指令速度を小さく設定することなく高速で動作させることが可能となる。また、計算量を少なくすることができるので、サンプリング時間の短縮が可能となり、軌跡精度の向上が期待できる。
【図面の簡単な説明】
【図１】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御装置の一つの実施の形態を示すブロック線図である。
【図２】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御方法の一つの実施の形態を示すフローチャートである。
【図３】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御におけるアーム動作経路を示す説明図である。
【図４】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御におけるアーム先端位置に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図５】 この発明による一平面内多自由度スカラ型ロボットの軌跡制御におけるサンプリング時刻に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図６】 姿勢軸を有する一平面内３自由度スカラ型ロボットを示す機構図である。
【図７】 水平面内２自由度スカラ型ロボットを示す機構図である。
【図８】 ロボット制御装置の概略構成を示すブロック線図である。
【図９】 アーム先端位置速度一定の直線補間動作示すフローチャートである。
【図１０】 アーム先端位置速度一定の直線補間動作におけるアーム動作経路を示す説明図である。
【図１１】 アーム先端位置速度一定の直線補間動作におけるアーム先端位置に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図１２】 アーム先端位置速度一定の直線補間動作におけるサンプリング時刻に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図１３】 角速度一定制御によるロボット制御の直線補間制御を示すフローチャートである。
【図１４】 直線補間動作のアーム先端位置の増分量を示す説明図である。
【図１５】 角速度一定の直線補間動作におけるアーム動作経路を示す説明図である。
【図１６】 角速度一定の直線補間動作におけるアーム先端位置に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【図１７】 角速度一定の直線補間動作におけるサンプリング時刻に対するアーム先端位置増分量、第１軸角度、第１軸角速度を示すグラフである。
【符号の説明】
１ 中央処理装置，２ 入力装置，３ 出力装置，４ 入出力インターフェイス，５ メモリ，６ サーボアンプインターフェイス，７ エンコーダインターフェイス，８ 制御プログラム記憶領域，９ アーム先端位置記憶領域，１０関節角度記憶領域，１１ 終点位置記憶領域，１２ アーム長記憶領域，１３第１関節角速度記憶領域，２０ ロボット，２１ 第１関節軸，２２ 第２関節軸，２３ 第３関節軸，２４ 第１アーム，２５ 第２アーム，２６ ハンド，２７ アーム先端，３１ サーボアンプ，３２ 関節エンコーダ。
Claims (6)
- 第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御装置において、
前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、
アーム先端の位置増分量演算のための定数角速度指令値として前記第１関節角に対し指定される第１関節角速度指令値を記憶する第１関節角速度記憶部と、
直線補間動作を行うに際し、アーム先端の位置増分量を前記第１アームのアーム長と、前記第２関節角の正弦値と、前記第１関節角速度記憶部に記憶されている第１関節角速度指令値と、の乗算により算出する位置増分量演算部と、
を備えたことを特徴とする一平面内多自由度スカラ型ロボットの軌跡制御装置。 - 前記一平面内多自由度スカラ型ロボットは２自由度あるいは３自由度のスカラ型ロボットであることを特徴とする請求項１に記載の一平面内多自由度スカラ型ロボットの軌跡制御装置。
- 第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御方法において、
前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、
アーム先端の位置増分量演算のための定数角速度指令値として前記第１関節角に対し指定される第１関節角速度指令値を記憶するとともに、直線補間動作を行うに際し、アーム先端の位置増分量を前記第１アームのアーム長と、前記第２関節角の正弦値と、前記第１関節角速度記憶部に記憶されている第１関節角速度指令値と、の乗算により算出することを特徴とする一平面内多自由度スカラ型ロボットの軌跡制御方法。 - 第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御方法において、
前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、
直線補間動作を行うに際し、アーム先端の位置増分量を式（１）により行うことを特徴とする一平面内多自由度スカラ型ロボットの軌跡制御方法。
Ｖｎ＝Ｌ1 ・ｓｉｎ（θ2 )・Ｊ1 …（１）
但し、Ｖｎ：アーム先端の位置増分量，Ｌ1 ：第１アームのアーム長，θ2 ：第２関節角，Ｊ1 ：第１関節角に対し指定されるユーザ指定の定数角速度指令値 - 前記一平面内多自由度スカラ型ロボットは２自由度あるいは３自由度のスカラ型ロボットであることを特徴とする請求項３または４に記載の一平面内多自由度スカラ型ロボットの軌跡制御方法。
- 第１関節軸に軸支される第１アームと、該第１アームに第２関節軸を介して軸支される第２アームまたは第２アームを含む２個以上のアームと、を有する一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体であって、
前記第１関節軸に直交する任意の軸を基準軸として、該基準軸と前記第１アームとのなす角を第１関節角とし、該第１アームと前記第２アームとのなす角を第２関節角とするとき、
アーム先端の位置増分量演算のための定数角速度指令値として前記第１関節角に対し指定される第１関節角速度指令値を記憶する第１関節角速度記憶手順と、
直線補間動作を行うに際し、アーム先端の位置増分量を前記第１アームのアーム長と、前記第２関節角の正弦値と、前記第１関節角速度記憶部に記憶されている第１関節角速度指令値と、の乗算により算出する位置増分量演算手順と、
をコンピュータに実行させるためのプログラムを記録したコンピュータ読み取り可能な記録媒体。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP33716797A JP3930956B2 (ja) | 1997-05-15 | 1997-12-08 | 一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体 |
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP9-126110 | 1997-05-15 | ||
| JP12611097 | 1997-05-15 | ||
| JP33716797A JP3930956B2 (ja) | 1997-05-15 | 1997-12-08 | 一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH1131008A JPH1131008A (ja) | 1999-02-02 |
| JP3930956B2 true JP3930956B2 (ja) | 2007-06-13 |
Family
ID=26462338
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP33716797A Expired - Fee Related JP3930956B2 (ja) | 1997-05-15 | 1997-12-08 | 一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3930956B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20230099273A (ko) * | 2021-12-27 | 2023-07-04 | 한국과학기술연구원 | 스카라 로봇의 작업 공간상의 직선 궤적을 생성하는 방법 |
-
1997
- 1997-12-08 JP JP33716797A patent/JP3930956B2/ja not_active Expired - Fee Related
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20230099273A (ko) * | 2021-12-27 | 2023-07-04 | 한국과학기술연구원 | 스카라 로봇의 작업 공간상의 직선 궤적을 생성하는 방법 |
| KR102641604B1 (ko) | 2021-12-27 | 2024-02-29 | 한국과학기술연구원 | 스카라 로봇의 작업 공간상의 직선 궤적을 생성하는 방법 |
Also Published As
| Publication number | Publication date |
|---|---|
| JPH1131008A (ja) | 1999-02-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US4698777A (en) | Industrial robot circular arc control method for controlling the angle of a tool | |
| KR100253898B1 (ko) | 한 평면내 다자유도 스카라형 로봇의 궤적 제어장치 및 궤적제어방법 및 한 평면내 다자유도 스카라형 로봇의 궤적 제어프로그램을 기록한 컴퓨터 판독 가능한 기록매체 | |
| JPH079606B2 (ja) | ロボット制御装置 | |
| JP4060393B2 (ja) | ロボットの速度演算装置、およびロボットの速度演算方法 | |
| JP6584102B2 (ja) | ロボット装置、ロボット制御方法、プログラム、記録媒体、及び物品の製造方法 | |
| US20070030271A1 (en) | Robot controller and robot control method using spline interpolation | |
| Fahimi et al. | An improved inverse kinematic and velocity solution for spatial hyper-redundant robots | |
| JP2874238B2 (ja) | 多関節形ロボットの制御方法 | |
| JP2020171989A (ja) | ロボット教示システム | |
| JP3930956B2 (ja) | 一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体 | |
| JPH05345291A (ja) | ロボットの動作範囲制限方式 | |
| JP3748454B2 (ja) | 産業用ロボットの制御装置 | |
| JPH035605B2 (ja) | ||
| JPH06312392A (ja) | 多関節ロボットの制御装置 | |
| JP2610996B2 (ja) | 多関節ロボット制御装置 | |
| JP3206765B2 (ja) | マスタスレーブマニピュレータの制御方法 | |
| Ibarguren et al. | Control strategies for dual arm co-manipulation of flexible objects in industrial environments | |
| JPH01222311A (ja) | 多自由度作業機械の曲面倣い制御装置 | |
| JP3394472B2 (ja) | スプライン補間による移動経路制御方法 | |
| KR20040034167A (ko) | 6축 수직다관절 매니퓰레이터의 직선이동 제어방법 | |
| JPH058187A (ja) | ロボツト | |
| JPS59220806A (ja) | 工業用ロボツトの制御方法 | |
| JPH10244481A (ja) | ロボットの動作プログラム作成方法および装置 | |
| JP2676721B2 (ja) | 多関節型ロボットの制御装置 | |
| KR0155281B1 (ko) | 다관절 로보트의 직선보간방법 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20040622 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20040823 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20041109 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20050107 |
|
| A911 | Transfer of reconsideration by examiner before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A911 Effective date: 20050118 |
|
| A912 | Removal of reconsideration by examiner before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A912 Effective date: 20050624 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070209 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070312 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100316 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110316 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110316 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120316 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130316 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130316 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140316 Year of fee payment: 7 |
|
| LAPS | Cancellation because of no payment of annual fees |