JPH1131008A - 一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体 - Google Patents

Abstract

(57)【要約】 【課題】 直線補間動作においてアーム先端がロッキン
グ点を速度過大を生ずることなく通過するようにするこ
と。 【解決手段】 一平面内多自由度スカラ型ロボットの軌
跡制御方法において、直線補間動作を行うに際してアー
ム先端の位置増分量を下式により行う。すなわち、Ｖｎ
＝Ｌ₁ ・ ｓｉｎ（θ₂ ) ・Ｊ₁ であり、但し、Ｖｎ：ア
ーム先端の位置増分量，Ｌ₁ ：第１アーム長，θ₂ ：第
２関節角度，Ｊ₁ ：ユーザ指定の第１関節角速度指令値
である。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は、スカラ型ロボッ
トの軌跡制御装置および軌跡制御方法、並びに、スカラ
型ロボットの軌跡制御プログラムを記録したコンピュー
タ読み取り可能な記録媒体に関し、さらに詳細には直線
補間動作を行う一平面内多自由度スカラ型ロボットの軌
跡制御装置および軌跡制御方法、並びに、一平面内多自
由度スカラ型ロボットの軌跡制御プログラムを記録した
コンピュータ読み取り可能な記録媒体に関するものであ
る。

【０００２】

【従来の技術】ロボット制御において、直線補間による
軌跡制御とは、ロボットアーム先端の軌跡が直線にな
り、その速度が例えば台形速度パターンとなるように制
御することである。

【０００３】図７は、水平面内２自由度スカラ型ロボッ
トを示している。自由度スカラ型ロボット２０は、第１
関節軸２１を中心として一平面（ＸＹ平面）内を回動可
能な第１アーム２４と、第２関節軸２２を中心として一
平面（ＸＹ平面）内を回動可能な第２アーム２５とを有
し、第２アーム２５のアーム先端（アーム先端）２７が
ロボット制御の対象とする座標点である。

【０００４】図８は、マイクロコンピュータによるロボ
ット制御装置を示している。ロボット制御装置は、中央
処理装置１と、入力装置２および出力装置３と中央処理
装置１とを接続する入出力インターフェイス４と、メモ
リ５と、サーボアンプインターフェイス６と、エンコー
ダインターフェイス７とを有している。メモリ５は、制
御プログラム記憶領域８と、アーム先端位置記憶領域９
と、関節角度記憶領域１０と、終点位置記憶領域１１
と、アーム長記憶領域１２とを含んでいる。

【０００５】サーボアンプインターフェイス６にはロボ
ット２０の各アームを駆動するために各関節軸に設けら
れるサーボモータを駆動するサーボアンプ３１が接続さ
れ、エンコーダインターフェイス７にはロボット２０の
各アームの角度を検出するために各関節軸に設けられる
関節エンコーダ３２が接続されている。

【０００６】従来におけるスカラ型ロボットの直線補間
による軌跡制御方法を図９を参照して説明する。まず、
中央処理装置１が直線補間を開始する（ステップＳ１０
１）。この直線補間の手続きは、上位機器の作業計画な
どの手続きから呼び出される。この手続きが呼び出され
るときには、上位機器の手続きによって現在位置と最終
目標位置が確定する。

【０００７】直線補間を開始すると、先ずメモリ５に記
憶された直線補間を行う経路の始点位置、終点位置、指
定速度からアーム先端位置増分を求める（ステップＳ１
０２）。このアーム先端位置増分は、指定速度にサンプ
リング時間を乗じて、各サンプリング時刻でのアーム先
端位置の始点と終点を結ぶ方向への位置増分として求め
る。

【０００８】つぎに、メモリ５の中に設けられているア
ーム先端位置記憶領域９に記憶された現在（現時点）の
アーム先端位置に、ステップＳ１０２にて求めたアーム
先端位置増分を加えて、次サンプリング時刻にて到達す
べきアーム先端位置を求める（ステップＳ１０３）。つ
ぎに、ステップＳ１０３にて求めた次サンプリング時刻
でのアーム先端位置から関節−アーム先端位置の逆変換
を行って、次サンプリング時刻における各関節の角度を
求める（ステップＳ１０４）。

【０００９】つぎに、ステップＳ１０４にて求めた次サ
ンプリング時刻における各関節の角度からメモリ５に設
けられている関節角度記憶領域１０に記憶された現在の
関節角度を減じて、現在のサンプリング時間に進めるべ
き関節角度を求める（ステップＳ１０５）。つぎに、サ
ーボインターフェイス６を通じてサーボアンプ３１にス
テップＳ１０５で求めた駆動すべき関節角度を指示し、
ロボット２０の各関節に設けられているサーボモータを
駆動する（ステップＳ１０６）。

【００１０】つぎに、時刻が次サンプリング時刻に達し
た時点で、エンコーダインターフェイス７を通じて、ロ
ボット２０の各関節に設けられている関節エンコーダ３
２により各アームの関節角度を検出し、関節角度記憶領
域１０の現在の各アームの関節角度の値を更新する（ス
テップＳ１０７）。つぎに、現在の関節角度から、関節
角度−アーム先端位置の正変換によって現在のアーム先
端位置を求める（ステップＳ１０８）。

【００１１】つぎに、ステップＳ１０８で求めた現在の
アーム先端位置によってメモリ５の中に設けられたアー
ム先端位置記憶領域９に書き込まれているアーム先端位
置の値を更新する（ステップＳ１０９）。つぎに、メモ
リ５に設けられている終点位置記憶領域１１に記憶され
ている終点位置の座標とステップ１０８で求められた現
在のアーム先端位置とを比較する（ステップＳ１１
０）。現在位置が終点位置に達していれば、終了（ステ
ップＳ１１１）へ分岐し、到達していなければ、ステッ
プＳ１０３へ分岐し、以下のステップを再度実行する。

【００１２】つぎに、具体的な２自由度スカラ型ロボッ
トの動作を図１０の動作説明図と図１１、図１２に示さ
れているグラフとを用いて説明する。

【００１３】ここで、２自由度スカラ型ロボットの緒元
を、 第１アーム長さＬ₁ → ３２０ｍｍ 第２アーム長さＬ₂ → ３２０ｍｍ 第１関節最大定格角速度 → ４．１２ｒａｄ／ｓｅｃ 第２関節最大定格角速度 → ４．１２ｒａｄ／ｓｅｃ サンプリング時間 → ２８．４ｍｓｅｃ のように設定する。

【００１４】図１０では動作の具体的な例として、Ｘ座
標５００ｍｍ；Ｙ座標−１００ｍｍ（始点位置）からＸ
軸に平行にＸ座標−５００ｍｍ；Ｙ座標−１００ｍｍ
（終点位置）までを直線補間で動作する場合を示す。図
１０では、アーム先端位置速度が３９０ｍｍ／ｓｅｃ、
すなわちサンプリング時間ごとにアーム先端位置の増分
が１１ｍｍである場合を０．２８４秒毎：１０サンプリ
ング時刻毎のロボットアームの状態（姿勢）を示した。
ここで、速度は関節の動作速度が最大定格を越えないな
かで最も大きくなるものを選んでいる。

【００１５】図１０に示されているように、第１軸（第
１関節軸２１）は、始点位置（Ｘ座標５００ｍｍ；Ｙ座
標−１００ｍｍ）から、符号Ｒで示されているアーム状
態：第２アーム２５と進行方向とが直角をなす姿勢にな
るまで、まず反時計廻り方向に動く。この状態から第１
軸は、時計廻り方向に動き、終点位置（Ｘ座標−５００
ｍｍ；Ｙ座標−１００ｍｍ）まで動く。このＲ点（一つ
のアーム状態点）は、第１軸の回転方向が逆転する。す
なわち、第１軸、第１アーム２４、第２軸（第２関節軸
２２）、第２アーム２５、アーム先端と直線経路で構成
されるスライダ・クランク機構のロッキング点（揺動
点）である。

【００１６】図１１は、横軸にアーム先端のＸ座標、縦
軸にサンプリング時間でのアーム先端位置増分、第１軸
角度、第１軸角速度を取った状態図である。図１１で、
黒丸「●」は第１軸角度を、白四角「□」は第１軸角速
度を、黒三角「黒△」は先端軸のサンプリング時間あた
りのアーム先端位置増分をそれぞれ示している。

【００１７】図１２は、横軸にサンプリング時刻、縦軸
には図１１と同様にサンプリング時間でのアーム先端位
置増分、第１軸角度、第１軸角速度を取った状態図であ
る。図１２で、黒丸「●」は第１軸角度を、白四角
「□」は第１軸角速度を、黒三角「黒△」は先端軸のサ
ンプリング時間あたりのアーム先端位置増分をそれぞれ
示している。

【００１８】図１１からアーム先端位置が原点近くに来
た時点：ｘ座標が０に近づいた時点で、第１軸の角速度
が最大となる。このときに、第１軸の角速度が最大定格
を越えないようにするために、指令速度が制約を受けて
いる。このために、Ｘ＝０ｍｍ近傍以外では、第１軸は
能力の半分程度しか使われていない。

【００１９】なお、図１１、図１２に示されている第１
軸の角速度は実際の角速度であり、１サンプリング時間
（２８．４ｍｓｅｃ）当たりの第１軸角度の変化量を１
サンプリング時間（２８．４ｍｓｅｃ）で割り算するこ
とにより求められる。

【００２０】スカラ型ロボットが、塗装作業などＣＰ制
御（連続経路制御）に適した作業を行う場合には、アー
ム先端の速度を一定に保つことは非常に重要な制御の目
的の一つである。

【００２１】

【発明が解決しようとする課題】しかしながら、スカラ
型ロボットがピックアンドプレース制御を主体する組み
立て作業に用いられる場合には、アーム先端の軌跡が直
線であることと同時に速度が一定に保たれることは必須
の条件ではない。むしろ、作業の効率を高めるために、
始点位置から終点位置までを短時間で直線動作すること
が強く求められる。このような場合には、スカラ型ロボ
ットのアーム先端速度を一定にするのではなく、第１関
節軸２１の能力を一杯に使って作業効率を向上させるこ
とが優先される。

【００２２】このようなことから、第１関節軸２１の角
速度が一定で、第１関節軸２１を駆動するサーボモータ
の能力を全て使って高速動作させる直線補間制御方法が
考えられる。

【００２３】第１関節軸２１の角速度を一定にしてアー
ム先端が直線上を動作する直線補間制御方法を図１３を
参照して説明する。

【００２４】直線補間を開始する（ステップＳ１０
１）。この直線補間の手続きは、上位機器の作業計画な
どの手続きから呼び出される。この手続きが呼び出され
るときには、上位機器の手続きによって現在位置と最終
目標位置が確定する。また、上位機器の手続きにより当
該直線補間動作における第１関節の角速度が与えられ
る。直線補間を開始すると、第１関節の角速度一定の条
件元でのアーム先端位置増分を求める（ステップＳ１１
２）。

【００２５】図７に示されている２自由度スカラ型ロボ
ットのアーム先端位置（Ｘ，Ｙ）の時間微分、すなわち
アーム先端位置速度と、関節角度（θ₁ , θ₂ ) の時間
微分、すなわち関節角速度の関係は、式（２）で表され
る。ここで、Ｊは、式（３）で表されるヤコビ行列であ
る。

【００２６】

【数１】

【００２７】

【数２】

【００２８】この逆関係は式（４）で表される。

【００２９】

【数３】

【００３０】ここでＪｒは、式（５）で表される逆ヤコ
ビ行列である。

【００３１】

【数４】

【００３２】式（４）は、２元連立方程式である。この
第１式より、第１軸の角速度とアーム先端位置の速度の
関係は、式（６）で表される。

【００３３】

【数５】

【００３４】ここで、サンプリング時間当たりの第１軸
の角度増分をＪ₁ 、アーム先端位置のＸ軸方向の位置増
分をＶｘ、Ｙ軸方向の位置増分をＶｙとする。角度増分
Ｊ₁とアーム先端位置の増分Ｖｘ、Ｖｙとの関係は、式
（７）で表される。

【００３５】

【数６】

【００３６】図１４に示されているように、現在位置を
Ｐ、終点位置をＥとし、アーム先端が直線ＰＥに沿って
動作するものとする。現在位置Ｐのアーム先端位置増分
をＶｎ、Ｘ軸方向の増分をＶｘ、Ｙ軸方向の増分をＶｙ
とする。現在位置Ｐから終点位置Ｅまで移動すべき残距
離をＤｎ、Ｘ方向の残距離の成分をＰｒｘ、Ｙ方向の残
距離の成分をＰｒｙとする。

【００３７】図１４に示すように、残距離Ｄｎ、Ｐｒｘ
と移動増分Ｖｎ、Ｖｘとの間には、線分ＰＥを斜辺とす
る直角三角形と位置増分ベクトルＶｎを斜辺とする直角
三角形が相似であることから、式（８）に示す関係が成
り立つ。

【００３８】

【数７】

【００３９】したがって、式（９）の関係が成り立つ。
同様にＹ軸方向成分についても式（１０）が成り立つ。

【００４０】

【数８】

【００４１】式（９）と式（１０）を式（７）に代入し
て整理すると、式（１１）が成り立つ。

【００４２】

【数９】

【００４３】式（１１）をアーム先端位置増分Ｖｎにつ
いて解くと、式（１２）が成り立つ。

【００４４】

【数１０】

【００４５】式（１２）の第１軸角度増分Ｊ₁ に指令関
節角速度を代入することにより、第１軸角速度一定の条
件での直線軌跡で、アーム先端を制御するためのアーム
先端位置増分Ｖｎが求まる。アーム先端位置増分Ｖｎを
式（８）と式（９）へ代入することにより、アーム先端
位置座標の増分が求まる。

【００４６】上述のようにして、位置増分が求められる
と、メモリ５に設けられたアーム先端位置記憶領域９に
記憶されたアーム先端位置座標に上述のようにして求め
られた位置増分を加えて、次サンプル時刻で到達するべ
きアーム先端位置を求める（ステップＳ１０３）。

【００４７】図１３のフローチャートのステップＳ１０
４からステップＳ１１１は、図９に示されているアーム
先端速度一定の直線補間制御のフローチャートのものと
同じであるから、その説明は省略する。

【００４８】つぎに、具体的なスカラ型ロボットの動作
を図１５の動作説明図と、図１６、図１７に示されてい
るグラフを用いて説明する。スカラ型ロボットの緒元
は、前述のものと同じであるものとする。

【００４９】図１５では動作の具体的な例として、Ｘ座
標５００ｍｍ；Ｙ座標−１００ｍｍ（始点位置）からＸ
軸に平行にＸ座標−５００ｍｍ；Ｙ座標−１００ｍｍ
（終点位置）までを直線補間で動作する場合を示す。図
１５では、第１軸角速度が、２．３ｒａｄ／ｓｅｃであ
る場合を０．２８４秒毎：１０サンプリング時刻毎のロ
ボットアームを示した。ここで、速度は関節の動作速度
が最大定格を越えないなかで最も大きくなるものを選ん
でいる。

【００５０】図１５に示されているように、第１軸は、
始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）か
ら符号Ｒで示されているアーム状態：第２アーム２５と
進行方向が直角をなす姿勢になるまで、まず反時計廻り
方向に動く。この状態から第１軸は、時計廻り方向に動
き終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍ
ｍ）まで動く。このＲ点では、第１軸の回転方向が逆転
する。すなわち、第１軸、第１アーム２４、第２軸、第
２アーム２５、アーム先端と直線経路で構成されるスラ
イダ・クランク機構のロッキング点である。

【００５１】図１６は、横軸にアーム先端のＸ座標、縦
軸にサンプリング時間でのアーム先端位置増分、第１軸
角度、第１軸角速度を取った状態図である。図１６で、
黒丸「●」は第１軸角度を、白四角「□」は第１軸角速
度（実速度）を、黒三角「黒△」はアーム先端軸のサン
プリング時間あたりのアーム先端位置増分をそれぞれ示
している。

【００５２】図１７は、横軸にサンプリング時刻、縦軸
には図１６と同様にサンプリング時間でのアーム先端位
置増分、第１軸角度、第１軸角速度を取った状態図であ
る。図１７で、黒丸「●」は第１軸角度を、白四角
「□」は第１軸角速度（実速度）を、黒三角「黒△」は
アーム先端軸のサンプリング時間あたりのアーム先端位
置増分をそれぞれ示している。

【００５３】図１６にてＸ座標が２３２ｍｍのロッキン
グ点に至るまでは、白四角「□」で表される第１軸角速
度は、＋２．３ｒａｄ／ｓｅｃ近傍で、ロッキング点を
超えると、−２．３ｒａｄ／ｓｅｃの角速度でほぼ定角
速度で動作している。

【００５４】アーム先端位置速度一定の直線補間動作で
は、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍ
ｍ）から終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１０
０ｍｍ）の１０００ｍｍの区間を２．５６秒要して移動
していたが、角速度一定で制御すると、１．４８秒で移
動を完了している。

【００５５】従来のアーム先端速度一定の直線補間方法
では、関節駆動モータの能力を活用し、タクトタイムの
短縮を図ることはできないが、上述のような関節角速度
一定の直線補間制御方法を採用すれば、関節駆動モータ
の能力を活用してタクトタイムの短縮を図ることができ
る。

【００５６】しかし、上述の関節角速度一定の直線補間
制御方法では、アーム先端位置のサンプリング時間にお
ける位置の増分が式（１２）によって決定されるため、
関節軸の回転方向が逆転するロッキング点にて分母がゼ
ロとなると、アーム先端位置増分Ｖｎが無限大へ発散し
てしまうと云う不具合がある。

【００５７】このため、図１６における黒三角「黒△」
で示されるアーム先端軸のサンプリング時間あたりのア
ーム先端位置増分の線図は、このロボットにおけるロッ
キング点（Ｘ＝２３２ｍｍ）近傍で、無限大に発散して
いる。

【００５８】また、この発散の影響を受けて、白四角
「□」で示された第１軸角速度が、指定された角速度の
２倍の回転を行ってしまう。逆に、この部分があるため
に最大定格回転数式を超えることが生じないように指定
角速度を低くしなければならないと云う不具合がある。

【００５９】この発明は、上述のような問題点を解決す
るためになされたものであり、アーム先端がロッキング
点を速度過大を生ずることなく通過するように改善され
た一平面内多自由度スカラ型ロボットの軌跡制御装置お
よび軌跡制御方法、並びに、一平面内多自由度スカラ型
ロボットの軌跡制御プログラムを記録したコンピュータ
読み取り可能な記録媒体を得ることを目的としている。

【００６０】

【課題を解決するための手段】上述の目的を達成するた
めに、この発明による一平面内多自由度スカラ型ロボッ
トの軌跡制御装置は、アーム先端位置増分量演算のため
の一つのパラメータとして指定される第１関節角速度指
令値を記憶する第１関節角速度記憶部と、直線補間動作
を行うに際してアーム先端の位置増分量を第１アーム長
と第２関節角度の正弦と前記第１関節角速度記憶部に記
憶されている第１関節角速度指令値との乗算により算出
するアーム先端の位置増分量演算部とを有しているもの
である。

【００６１】この発明による一平面内多自由度スカラ型
ロボットの軌跡制御装置では、第１関節角速度記憶部が
アーム先端位置増分量演算のための一つのパラメータと
して指定される第１関節角速度指令値を記憶し、アーム
先端の位置増分量演算部が直線補間動作を行うに際して
アーム先端の位置増分量を第１アーム長と第２関節角度
の正弦と第１関節角速度記憶部に記憶されている第１関
節角速度指令値との乗算により算出し、直線補間制御を
行う。これにより全直線補間動作域に亙ってアーム先端
位置増分が無限大となることがない。

【００６２】なお、アーム先端の位置増分量をＶｎ、第
１アーム長をＬ₁ 、 第２関節角度をθ₂ 、 第１関節角速
度指令値をＪ₁ とすると、位置増分量演算部は、Ｖｎ＝
Ｌ₁・ ｓｉｎ（θ₂ ) ・Ｊ₁ なる演算式によりアーム先
端の位置増分量Ｖｎを演算する。

【００６３】つぎの発明による一平面内多自由度スカラ
型ロボットの軌跡制御装置は、上述の発明による一平面
内多自由度スカラ型ロボットの軌跡制御装置において、
前記一平面内多自由度スカラ型ロボットは２自由度ある
いは３自由度のスカラ型ロボットであるものである。

【００６４】この発明による一平面内多自由度スカラ型
ロボットの軌跡制御装置では、２自由度あるいは３自由
度のスカラ型ロボットにおいて、位置増分量演算部が直
線補間動作を行うに際してアーム先端の位置増分量を第
１アーム長と第２関節角度の正弦と第１関節角速度記憶
部に記憶されている第１関節角速度指令値との乗算によ
り算出し、直線補間制御を行う。これにより２自由度あ
るいは３自由度のスカラ型ロボットにおいて、全直線補
間動作域に亙ってアーム先端位置増分が無限大となるこ
とがない。

【００６５】また、上述の目的を達成するために、この
発明による一平面内多自由度スカラ型ロボットの軌跡制
御方法は、アーム先端位置増分量演算のための一つのパ
ラメータとして指定される第１関節角速度指令値を記憶
し、直線補間動作を行うに際してアーム先端の位置増分
量を第１アーム長と第２関節角度の正弦と前記第１関節
角速度指令値との乗算により算出するものである。

【００６６】この発明による一平面内多自由度スカラ型
ロボットの軌跡制御方法では、アーム先端位置増分量演
算のための一つのパラメータとして指定される第１関節
角速度指令値を記憶し、直線補間動作を行うに際してア
ーム先端の位置増分量を第１アーム長と第２関節角度の
正弦と第１関節角速度指令値との乗算により算出して直
線補間制御を行う。これにより全直線補間動作域に亙っ
てアーム先端位置増分が無限大となることがない。

【００６７】また、上述の目的を達成するために、この
発明による一平面内多自由度スカラ型ロボットの軌跡制
御方法は、一平面内多自由度スカラ型ロボットの軌跡制
御方法において、直線補間動作を行うに際してアーム先
端の位置増分量を下式（１）により行うことものであ
る。

【００６８】 Ｖｎ＝Ｌ₁ ・ ｓｉｎ（θ₂ ) ・Ｊ₁ …（１）再掲 但し、Ｖｎ：アーム先端の位置増分量，Ｌ₁ ：第１アー
ム長，θ₂ ：第２関節角度，Ｊ₁ ：ユーザ指定の第１関
節角速度指令値である。

【００６９】この発明による一平面内多自由度スカラ型
ロボットの軌跡制御方法では、Ｖｎ＝Ｌ₁ ・ ｓｉｎ（θ
₂ ) ・Ｊ₁ なる演算式によりアーム先端の位置増分量Ｖ
ｎを演算して直線補間制御を行う。これにより全直線補
間動作域に亙ってアーム先端位置増分が無限大となるこ
とがない。

【００７０】つぎの発明による一平面内多自由度スカラ
型ロボットの軌跡制御方法は、上述の発明による一平面
内多自由度スカラ型ロボットの軌跡制御方法において、
前記一平面内多自由度スカラ型ロボットは２自由度ある
いは３自由度のスカラ型ロボットであるものである。

【００７１】この発明による一平面内多自由度スカラ型
ロボットの軌跡制御方法では、２自由度あるいは３自由
度のスカラ型ロボットにおいて、位置増分量演算部が直
線補間動作を行うに際してアーム先端の位置増分量を第
１アーム長と第２関節角度の正弦と第１関節角速度指令
値との乗算により算出し、直線補間制御を行う。これに
より２自由度あるいは３自由度のスカラ型ロボットにお
いて、全直線補間動作域に亙ってアーム先端位置増分が
無限大となることがない。

【００７２】また上述の目的を達成するために、この発
明による一平面内多自由度スカラ型ロボットの軌跡制御
プログラムを記録したコンピュータ読み取り可能な記録
媒体は、アーム先端位置増分量演算のための一つのパラ
メータとして指定される第１関節角速度指令値を記憶す
る第１関節角速度記憶手順と、直線補間動作を行うに際
してアーム先端の位置増分量を第１アーム長と第２関節
角度の正弦と前記第１関節角速度記憶部に記憶されてい
る第１関節角速度指令値との乗算により算出するアーム
先端位置増分量演算手順とをコンピュータに実行させる
ためのプログラムを記録したものである。

【００７３】この発明によるコンピュータ読み取り可能
な記録媒体は、アーム先端位置増分量演算のための一つ
のパラメータとして指定される第１関節角速度指令値を
記憶する第１関節角速度記憶手順と、直線補間動作を行
うに際してアーム先端の位置増分量を第１アーム長と第
２関節角度の正弦と第１関節角速度記憶部に記憶されて
いる第１関節角速度指令値との乗算により算出するアー
ム先端位置増分量演算手順とをコンピュータに実行させ
るプログラムを格納しており、この記録媒体に記録され
ているプログラムをコンピュータが実行することによ
り、全直線補間動作域に亙ってアーム先端位置増分が無
限大となることなく直線補間動作が行われる。

【００７４】

【発明の実施の形態】以下に添付の図を参照してこの発
明に係る一平面内多自由度スカラ型ロボットの軌跡制御
装置および軌跡制御方法、並びに、一平面内多自由度ス
カラ型ロボットの軌跡制御プログラムを記録したコンピ
ュータ読み取り可能な記録媒体の実施の形態を詳細に説
明する。なお、以下に説明するこの発明の実施の形態に
おいて上述の従来例と同一構成の部分は、上述の従来例
に付した符号と同一の符号を付してその説明を省略す
る。

【００７５】本明細書において、「一平面」とは、水平
面や垂直面等の一つの平面のことであり、アームの運動
がこの水平面等の一つの平面内で行われる。したがっ
て、アームの回転軸の方向は鉛直方向等全て同じとな
る。また、「多自由度」とは、多関節（ａｒｔｉｃｕｌ
ａｔｅｄ）と同じ意味である。水平多関節型ロボットは
一般的であり、これがスカラ型と呼ばれる。因に、スカ
ラ（ＳＣＡＲＡ）とは、Ｓｅｌｅｃｔｉｖｅ Ｃｏｍｐ
ｌｉａｎｃｅ Ａｓｓｅｍｂｌｙ Ｒｏｂｏｔ Ａｒｍ
の略称である。

【００７６】実施の形態１．図１はこの発明による軌跡
制御装置（ロボット制御装置）の一つの実施の形態を示
している。この軌跡制御装置は、メモリ５に、制御プロ
グラム記憶領域８、アーム先端位置記憶領域９、関節角
度記憶領域１０、終点位置記憶領域１１、アーム長記憶
領域１２に加えて第１関節角速度記憶領域１３を有して
いる。

【００７７】第１関節角速度記憶領域１３は入力装置２
より一つのパラメータとして指定入力されるユーザ指定
の第１関節角速度指令値（指定角速度）を記憶する。

【００７８】アーム先端の位置増分量演算部は、メモリ
５の制御プログラム記憶領域８に格納されている制御プ
ログラムを中央処理装置１が実行することにより具現化
され、直線補間動作を行うに際してアーム先端の位置増
分量を第１アーム長と第２関節角度の正弦と第１関節角
速度記憶領域１３に記憶されている第１関節角速度指令
値との乗算により算出する。

【００７９】この演算は、アーム先端の位置増分量をＶ
ｎ、第１アーム長をＬ₁ 、 第２関節角度をθ₂ 、 第１関
節角速度指令値をＪ₁ とすると、Ｖｎ＝Ｌ₁ ・ ｓｉｎ
（θ₂) ・Ｊ₁ なる演算式により行われる。

【００８０】入力装置２は、アーム先端位置増分量演算
のための一つのパラメータとして指定される第１関節角
速度指令値を第１関節角速度記憶領域１３に記憶する第
１関節角速度記憶手順と、直線補間動作を行うに際して
アーム先端の位置増分量を第１アーム長と第２関節角度
の正弦と第１関節角速度記憶領域１３に記憶されている
第１関節角速度指令値との乗算により算出するアーム先
端位置増分量演算手順とをコンピュータに実行させるた
めの軌跡制御プログラムを記録したコンピュータ読み取
り可能な記録媒体より軌跡制御プログラムを読み取り、
これを制御プログラム記憶領域８に格納する。

【００８１】つぎに、この発明による本発明による軌跡
制御方法を図２を参照して説明する。中央処理装置１が
直線補間を開始する（ステップＳ１０１）。この直線補
間の手続きは、上位機器の作業計画などの手続きから呼
び出される。この手続きが呼び出されるときには、上位
機器の手続きによって現在位置と最終目標位置が確定す
る。また、上位機器の手続きにより当該直線補間動作に
おける第１関節の角速度Ｊ₁ が与えられる。

【００８２】直線補間を開始すると、ユーザ指定の第１
関節角速度指令値Ｊ₁ および式（１）によってアーム先
端位置増分Ｖｎを演算、決定する（ステップＳ２０
２）。

【００８３】 Ｖｎ＝Ｌ₁ ・ ｓｉｎ（θ₂ ) ・Ｊ₁ …（１）再掲 但し、Ｖｎ：アーム先端の位置増分量，Ｌ₁ ：第１アー
ム長，θ₂ ：第２関節角度，Ｊ₁ ：ユーザ指定の第１関
節角速度指令値である。

【００８４】また、上記式（１２）は、式（１３）のよ
うに変形することができる。

【００８５】

【数１１】

【００８６】式（１３）の分母は、すべての動作範囲に
おいて、０以上１以下の値をとるものである。ロッキン
グ点にて、この分母の部分がゼロになるために、アーム
先端位置増分量が、無限大へ発散する不具合が生じるも
のである。

【００８７】そこで、式（１３）の分母を１に固定する
ことにより、式（１）が導出される。式（１）の値は、
必ず式（１３）の値を超えることはない。これにより、
ステップＳ１０５にてアーム先端位置から逆変換によっ
て関節角度を求め、ステップＳ１０６で関節角度増分を
求めた場合に、関節の角速度一定で制御した場合より小
さな角度増分が決定される。このために式（１）におい
て指令角速度Ｊ₁ を定格最大角速度としても、それを超
える関節角度指令が生成されることはない。

【００８８】図２のフローチャートのステップＳ１０３
からステップＳ１１１は、図１３に示されている角速度
一定の直線補間制御のフローチャートのものと同じであ
るから、その説明は省略する。

【００８９】つぎに、具体的なスカラ型ロボットの動作
を図３の動作説明図と図４、図５に示されているグラフ
とを用いて説明する。なお、スカラ型ロボットの緒元
は、前述のものと同じであるものとする。

【００９０】ここで、図３では動作の具体的な例とし
て、Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ（始点位
置）からＸ軸に平行にＸ座標−５００ｍｍ；Ｙ座標−１
００ｍｍ（終点位置）までを直線補間で動作する場合を
示す。図３では、第１軸角速度指令値が定格最大角速度
４．１２ｒａｄ／ｓｅｃである場合を０．２８４秒毎：
１０サンプリング時刻毎のロボットアームを示した。

【００９１】図３に示されているように、第１軸は、始
点位置（Ｘ座標５００ｍｍ；Ｙ座標−１００ｍｍ）から
符号Ｒで示されているアーム状態：第２アーム２５と進
行方向が直角をなす姿勢になるまで、まず反時計廻り方
向に動く。この状態から第１軸は、時計廻り方向に動き
終点位置（Ｘ座標−５００ｍｍ；Ｙ座標−１００ｍｍ）
まで動く。このＲ点では、第１軸の回転方向が逆転す
る。すなわち、第１軸、第１アーム２４、第２軸、第２
アーム２５、アーム先端と直線経路で構成されるスライ
ダ・クランク機構のロッキング点である。

【００９２】図４は、横軸にアーム先端のＸ座標、縦軸
にサンプリング時間でのアーム先端位置増分、第１軸角
度、第１軸角速度を取った状態図である。図４で、黒丸
「●」は第１軸角度を、白四角「□」は第１軸角速度
（実速度）を、黒三角「黒△」はアーム先端軸のサンプ
リング時間あたりのアーム先端位置増分をそれぞれ示し
ている。つぎに、図４のグラフの作成手順について説明
する。

【００９３】（１）Ｘ０= ５００の位置における第２関
節角度θ₂ をもとに、式（１３）の分母を１とし、アー
ム先端の位置増分量Ｖｎを算出する。Ｖｎ＝ほぼ３６ｍ
ｍをこれは式（１）を使用して計算したものと同じ結果
である。

【００９４】なお、第１アーム長Ｌ₁ は３２０ｍｍ、第
１関節角速度指令値Ｊ₁ は４．１２ｒａｄ／ｓｅｃであ
り、サンプリング時間は２８．４ｍｓｅｃであり、第１
軸角度（第１関節角度）θ₁ と第２関節角度θ₂ は、ア
ーム先端の位置Ｘ，Ｙ（−１００ｍｍ固定）と、第１お
よび第２アーム長（ともに３２０ｍｍ）から決まる。

【００９５】Ｘ０= ５００の位置における第１軸角度θ
₁ はほぼ０．４５２ｒａｄであり、第２関節角度θ₂ は
ほぼ１．３００ｒａｄである。

【００９６】（２）２４ｍｓｅｃ後（１サンプリング時
間後）のＸ₁ 位置を算出する。 Ｘ₁ = ５００−Ｖｎ＝５００−ほぼ３６＝ほぼ４６４ｍ
ｍ Ｘ₁ 位置における第２関節角度θ₂ をもとに、つぎの位
置増分量Ｖｎを算出する。また第１軸角度θ₁ はＸ₁ 位
置から逆算できる。 アーム先端の位置増分量Ｖｎ＝ほぼ３７．２ｍｍ 第１軸角度θ₁ = ほぼ０．５２３ｒａｄ 第１軸角速度（実速度）＝第１軸角度変化量（ほぼ０．
５２３ｒａｄ−ほぼ０．４５２ｒａｄ）／２８．４ｍｓ
ｅｃ

【００９７】以上の計算をＸ₁ 位置が−５００ｍｍ付近
まで繰り返し、アーム先端の位置増分量、第１軸角度、
第１軸角速度（実速度）をグラフにプロットすると、図
４が得られる。

【００９８】なお、式（１３）の分母に、θ₁ 、 θ₂ の
値が関係するため、実際の第１軸角速度は第１関節角速
度指令値Ｊ₁ より小さい値になる。

【００９９】図５は、横軸にサンプリング時刻、縦軸に
は図４と同様にサンプリング時間でのアーム先端位置増
分、第１軸角度、第１軸角速度を取った状態図である。
図１２で、黒丸「●」は第１軸角度を、白四角「□」は
第１軸角速度（実速度）を、黒三角「黒△」はアーム先
端軸のサンプリング時間あたりのアーム先端位置増分を
それぞれ示している。

【０１００】図４にてＸ座標が２３２ｍｍのロッキング
点においても、黒三角「黒△」で示されるアーム先端軸
のサンプリング時間あたりのアーム先端位置増分の線図
は、滑らかに変化し、図１６、図１７に示されているよ
うに、無限大へ発散することはない。

【０１０１】また、白四角「□」で示された実際の第１
軸角速度も滑らかな変化をしており、図１６、図１７に
示されているように、急激な変化を生じることがない。
このため、、式（１）で用いる指令角速度指令値Ｊ₁ を
定格最大角速度としても、角速度過大を生ずることなく
ロボットを運転することが可能となる。

【０１０２】また、アーム先端位置速度一定の直線補間
動作では、、始点位置（Ｘ座標５００ｍｍ；Ｙ座標−１
００ｍｍ）から終点位置（Ｘ座標−５００ｍｍ；Ｙ座標
−１００ｍｍ）の１０００ｍｍの区間を２．５６秒要し
て移動していたが、角速度一定で制御すると、１．３３
秒で移動を完了している。

【０１０３】また、角速度一定では、式（１２）によっ
て、位置増分量を決定するので、計算量が、加減算３、
乗算５、除算１、三角関数式３の計算が必要であるが、
この発明による軌跡制御方法では、位置増分量を決定す
る計算量が、乗算２、三角関数式１で済む。具体的に計
算量を比較すると、インテル社の８０３８６マイクロプ
ロセッサと８０３８７数式値演算プロセッサを用いる
と、加減算には４５クロック、乗算には６５クロック、
除算には１０２クロック、三角関数式には７７１クロッ
クが必要であるので、角速度一定方式では、２８７５ク
ロックが必要であるのに対して、この発明による軌跡制
御方法では、計算負荷の大きな除算、三角関数式の分量
が少ないので、９０１クロックで計算が完了し、所用時
間はおよそ１／３とすることが可能となる。

【０１０４】なお、上述の実施の形態は、図７で示す２
自由度スカラ型ロボットの場合であるが、この発明は図
６に示されているような３自由度スカラ型ロボットにも
拡張して適用することが可能である。図６は図７に示さ
れている２自由度スカラ型ロボットのアーム先端に、第
３関節２３を加え、その第３関節２３によってハンド２
６を駆動することにより、もう一つの自由度を加えたも
のであり、アーム先端の位置に加えて姿勢（角度Ａ）を
制御するものである。

【０１０５】この３自由度スカラ型ロボットでも、図２
に示されているアルゴリズムにて直線補間制御を行う。
従って、ｂの場合も、式（１）によってアーム先端位置
増分Ｖｎを演算、決定する。

【０１０６】 Ｖｎ＝Ｌ₁ ・ ｓｉｎ（θ₂ ) ・Ｊ₁ …（１）再掲

【０１０７】ここで、３自由度スカラ型ロボットにおけ
る式（１）の導出について説明する。

【０１０８】以後、ＪＩＳ（日本工業規格）に従い、ア
ーム先端の位置と姿勢を合わせたものをポーズとよぶ。
図６に示されている３自由度スカラ型ロボットでは、ポ
ーズは、アーム先端の位置のＸ軸成分、Ｙ軸成分と姿勢
角度Ａの３要素から成り立つベクトルである。

【０１０９】３自由度スカラ型ロボットのポーズと関節
角度のそれぞれの時間微分量の関係は、式（１４）によ
り表される。

【０１１０】

【数１２】

【０１１１】ここで、Ｊは式（１５）で表されるヤコビ
行列である。

【０１１２】

【数１３】

【０１１３】従って、この逆関係は式（１６）で表され
る。

【０１１４】

【数１４】

【０１１５】ここで、Ｊｒは式（１７）で表される逆ヤ
コビ行列である。

【０１１６】

【数１５】

【０１１７】式（１６）は３元線形連立方程式である。
この第１式から、第１軸の角速度とアーム先端のポーズ
の時間微分（すなわちアーム先端の速度と姿勢の角速
度）の関係は、式（１８）にて表される。

【０１１８】

【数１６】

【０１１９】ここで、サンプリング時間当たりの第１軸
の角度の増分をＪ₁ 、 アーム先端位置のＸ軸方向の位置
の増分をＶｘ、Ｙ軸方向の位置の増分をＶｙ、姿勢軸の
角度の増分をＶＡとする。第１軸の角度とアーム先端ポ
ーズの増分の関係は、式（１９）で表される。

【０１２０】

【数１７】

【０１２１】２自由度スカラ型ロボットの場合に図１４
に示したように定義した現在位置Ｐから終点までの残距
離Ｄｎをアーム先端の姿勢を考慮し拡張する。現在ポー
ズから終点までの移動するべき残距離Ｄｍを式（２０）
のように定義する。

【０１２２】

【数１８】

【０１２３】ここで、右辺かっこ内の第２項は、残姿勢
角度ＶＡに式（２１）で定義する所の角度＝距離換算係
数式Ｊｎｍを乗じたものである。すなわち、式（２１）
は、アーム先端の残距離もしくは、残姿勢角度のうち、
いずれか大きい値のものをＤｍｎとして採用するもので
ある。この換算係数式は、アーム先端の定格最大速度Ｓ
ｐｍａｘを、姿勢を制御する第３軸の定格最大角速度で
除したものである。

【０１２４】

【数１９】

【０１２５】式（９）、式（１０）と同様にして、アー
ム先端位置の増分量Ｖｎとポーズのそれぞれの成分の関
係は、式（２２）、式（２３）、式（２４）で表され
る。

【０１２６】

【数２０】

【０１２７】

【数２１】

【０１２８】

【数２２】

【０１２９】式（２２）、式（２３）、式（２４）を式
（１９）へ代入し、整理すると、式（２５）が得られ
る。

【０１３０】

【数２３】

【０１３１】これを、アーム先端位置増分量Ｖｎについ
て解くと、式（２６）が得られる。

【０１３２】

【数２４】

【０１３３】この式は、２自由度スカラ型ロボットの場
合の式（１２）に相当するものである。この式により求
められるアーム先端位置増分量Ｖｎは、式（１２）と同
様に角速度一定で直線補間を行う場合を３自由度スカラ
型ロボットに拡張したものである。

【０１３４】式（２６）によれば、２自由度スカラ型ロ
ボットの場合と同様に、ロッキング点においてアーム先
端位置増分の発散が生じてしまう。

【０１３５】これは、式（２６）の分母の部分がロッキ
ング点でゼロの値を取るためである。この分母を拡張さ
れた残距離Ｄｍで除した値は、始点位置では１となり、
終点位置では０となるため、不等式（式（２７））が成
り立つ。

【０１３６】

【数２５】

【０１３７】ここで、式（２７）の項を最大値の１に置
き換えて式（２６）へ代入すると、結局、式（１）が得
られる。

【０１３８】この実施の形態では、直線動作におけるア
ーム先端位置増分を式（１）にて決定するため、ロッキ
ング点でのアーム先端速度の発散を生じることがない。

【０１３９】アーム先端位置増分量を角速度一定として
求める式（２６）によれば、乗算７、除算１、加算２、
三角関数式４の演算が必要であり、インテル社の８０３
８６マイクロプロセッサーと８０３８７数式値演算プロ
セッサーを用いると、３７３１クロックが必要である。
これに対し式（１）によれば、９０１クロックで演算が
終了するので、大幅な演算時間の短縮を図ることができ
る。演算時間の短縮により、サンプリング時間の短縮が
可能となり、軌跡精度の向上などが期待できる。

【０１４０】

【発明の効果】以上の説明から理解される如く、この発
明による一平面内多自由度スカラ型ロボットの軌跡制御
装置では、第１関節角速度記憶部がアーム先端位置増分
量演算のための一つのパラメータとして指定される第１
関節角速度指令値を記憶し、アーム先端の位置増分量演
算部が直線補間動作を行うに際してアーム先端の位置増
分量を第１アーム長と第２関節角度の正弦と第１関節角
速度記憶部に記憶されている第１関節角速度との乗算に
より算出し、直線補間制御を行うから、全直線補間動作
域に亙ってアーム先端位置増分が無限大となることがな
く、ロッキング点通過のために指令速度を小さく設定す
ることなく高速で動作させることが可能となる。加え
て、アーム先端位置速度を一定に保つ従来の直線補間の
ように、アーム先端が原点近傍を通過する場合に大きな
第１軸の角速度を必要としないので、原点近傍通過のた
めに指令速度を小さく設定することなく高速で動作させ
ることが可能となる。また、計算量を少なくすることが
できるので、サンプリング時間の短縮が可能となり、軌
跡精度の向上が期待できる。

【０１４１】つぎの発明による一平面内多自由度スカラ
型ロボットの軌跡制御装置によれば、２自由度あるいは
３自由度のスカラ型ロボットにおいて、位置増分量演算
部が直線補間動作を行うに際してアーム先端の位置増分
量を第１アーム長と第２関節角度の正弦と第１関節角速
度記憶部に記憶されている第１関節角速度指令値との乗
算により算出し、直線補間制御を行うから、２自由度あ
るいは３自由度のスカラ型ロボットにおいて、全直線補
間動作域に亙ってアーム先端位置増分が無限大となるこ
とがなく、ロッキング点通過のために指令速度を小さく
設定することなく高速で動作させることが可能となる。

【０１４２】つぎの発明による一平面内多自由度スカラ
型ロボットの軌跡制御方法によれば、アーム先端位置増
分量演算のための一つのパラメータとして指定される第
１関節角速度指令値を記憶し、直線補間動作を行うに際
してアーム先端の位置増分量を第１アーム長と第２関節
角度の正弦と第１関節角速度指令値との乗算により算出
して直線補間制御を行うから、全直線補間動作域に亙っ
てアーム先端位置増分が無限大となることがなく、ロッ
キング点通過のために指令速度を小さく設定することな
く高速で動作させることが可能となる。加えて、アーム
先端位置速度を一定に保つ従来の直線補間のように、ア
ーム先端が原点近傍を通過する場合に大きな第１軸の角
速度を必要としないので、原点近傍通過のために指令速
度を小さく設定することなく高速で動作させることが可
能となる。また、計算量を少なくすることができるの
で、サンプリング時間の短縮が可能となり、軌跡精度の
向上が期待できる。

【０１４３】つぎの発明による一平面内多自由度スカラ
型ロボットの軌跡制御方法によれば、Ｖｎ＝Ｌ₁ ・ ｓｉ
ｎ（θ₂ ) ・Ｊ₁ なる演算式によりアーム先端の位置増
分量Ｖｎを演算して直線補間制御を行うから、全直線補
間動作域に亙ってアーム先端位置増分が無限大となるこ
とがなく、ロッキング点通過のために指令速度を小さく
設定することなく高速で動作させることが可能となる。
加えて、アーム先端位置速度を一定に保つ従来の直線補
間のように、アーム先端が原点近傍を通過する場合に大
きな第１軸の角速度を必要としないので、原点近傍通過
のために指令速度を小さく設定することなく高速で動作
させることが可能となる。また、計算量を少なくするこ
とができるので、サンプリング時間の短縮が可能とな
り、軌跡精度の向上が期待できる。

【０１４４】つぎの発明による一平面内多自由度スカラ
型ロボットの軌跡制御方法によれば、２自由度あるいは
３自由度のスカラ型ロボットにおいて、位置増分量演算
部が直線補間動作を行うに際してアーム先端の位置増分
量を第１アーム長と第２関節角度の正弦と第１関節角速
度指令値との乗算により算出し、直線補間制御を行うか
ら、２自由度あるいは３自由度のスカラ型ロボットにお
いて、全直線補間動作域に亙ってアーム先端位置増分が
無限大となることがなく、ロッキング点通過のために指
令速度を小さく設定することなく高速で動作させること
が可能となる。

【０１４５】つぎの発明によるコンピュータ読み取り可
能な記録媒体によれば、当該記録媒体に記録されている
プログラムをコンピュータが実行することにより、全直
線補間動作域に亙ってアーム先端位置増分が無限大とな
ることなく直線補間動作が行われ、ロッキング点通過の
ために指令速度を小さく設定することなく高速で動作さ
せることが可能となる。加えて、アーム先端位置速度を
一定に保つ従来の直線補間のように、アーム先端が原点
近傍を通過する場合に大きな第１軸の角速度を必要とし
ないので、原点近傍通過のために指令速度を小さく設定
することなく高速で動作させることが可能となる。ま
た、計算量を少なくすることができるので、サンプリン
グ時間の短縮が可能となり、軌跡精度の向上が期待でき
る。

【図面の簡単な説明】

【図１】 この発明による一平面内多自由度スカラ型ロ
ボットの軌跡制御装置の一つの実施の形態を示すブロッ
ク線図である。

【図２】 この発明による一平面内多自由度スカラ型ロ
ボットの軌跡制御方法の一つの実施の形態を示すフロー
チャートである。

【図３】 この発明による一平面内多自由度スカラ型ロ
ボットの軌跡制御におけるアーム動作経路を示す説明図
である。

【図４】 この発明による一平面内多自由度スカラ型ロ
ボットの軌跡制御におけるアーム先端位置に対するアー
ム先端位置増分量、第１軸角度、第１軸角速度を示すグ
ラフである。

【図５】 この発明による一平面内多自由度スカラ型ロ
ボットの軌跡制御におけるサンプリング時刻に対するア
ーム先端位置増分量、第１軸角度、第１軸角速度を示す
グラフである。

【図６】 姿勢軸を有する一平面内３自由度スカラ型ロ
ボットを示す機構図である。

【図７】 水平面内２自由度スカラ型ロボットを示す機
構図である。

【図８】 ロボット制御装置の概略構成を示すブロック
線図である。

【図９】 アーム先端位置速度一定の直線補間動作示す
フローチャートである。

【図１０】 アーム先端位置速度一定の直線補間動作に
おけるアーム動作経路を示す説明図である。

【図１１】 アーム先端位置速度一定の直線補間動作に
おけるアーム先端位置に対するアーム先端位置増分量、
第１軸角度、第１軸角速度を示すグラフである。

【図１２】 アーム先端位置速度一定の直線補間動作に
おけるサンプリング時刻に対するアーム先端位置増分
量、第１軸角度、第１軸角速度を示すグラフである。

【図１３】 角速度一定制御によるロボット制御の直線
補間制御を示すフローチャートである。

【図１４】 直線補間動作のアーム先端位置の増分量を
示す説明図である。

【図１５】 角速度一定の直線補間動作におけるアーム
動作経路を示す説明図である。

【図１６】 角速度一定の直線補間動作におけるアーム
先端位置に対するアーム先端位置増分量、第１軸角度、
第１軸角速度を示すグラフである。

【図１７】 角速度一定の直線補間動作におけるサンプ
リング時刻に対するアーム先端位置増分量、第１軸角
度、第１軸角速度を示すグラフである。

【符号の説明】

１ 中央処理装置，２ 入力装置，３ 出力装置，４
入出力インターフェイス，５ メモリ，６ サーボアン
プインターフェイス，７ エンコーダインターフェイ
ス，８ 制御プログラム記憶領域，９ アーム先端位置
記憶領域，１０関節角度記憶領域，１１ 終点位置記憶
領域，１２ アーム長記憶領域，１３第１関節角速度記
憶領域，２０ ロボット，２１ 第１関節軸，２２ 第
２関節軸，２３ 第３関節軸，２４ 第１アーム，２５
第２アーム，２６ ハンド，２７ アーム先端，３１
サーボアンプ，３２ 関節エンコーダ。

Claims (6)

【特許請求の範囲】
1. 【請求項１】 一平面内多自由度スカラ型ロボットの軌
   跡制御装置において、 アーム先端位置増分量演算のための一つのパラメータと
   して指定される第１関節角速度指令値を記憶する第１関
   節角速度記憶部と、 直線補間動作を行うに際してアーム先端の位置増分量を
   第１アーム長と第２関節角度の正弦と前記第１関節角速
   度記憶部に記憶されている第１関節角速度指令値との乗
   算により算出するアーム先端の位置増分量演算部と、 を有していることを特徴とする一平面内多自由度スカラ
   型ロボットの軌跡制御装置。
2. 【請求項２】 前記一平面内多自由度スカラ型ロボット
   は２自由度あるいは３自由度のスカラ型ロボットである
   ことを特徴とする請求項１に記載の一平面内多自由度ス
   カラ型ロボットの軌跡制御装置。
3. 【請求項３】 一平面内多自由度スカラ型ロボットの軌
   跡制御方法において、 アーム先端位置増分量演算のための一つのパラメータと
   して指定される第１関節角速度指令値を記憶し、直線補
   間動作を行うに際してアーム先端の位置増分量を第１ア
   ーム長と第２関節角度の正弦と前記第１関節角速度指令
   値との乗算により算出することを特徴とする一平面内多
   自由度スカラ型ロボットの軌跡制御方法。
4. 【請求項４】 一平面内多自由度スカラ型ロボットの軌
   跡制御方法において、 直線補間動作を行うに際してアーム先端の位置増分量を
   式（１）により行うことを特徴とする一平面内多自由度
   スカラ型ロボットの軌跡制御方法。 Ｖｎ＝Ｌ₁ ・ ｓｉｎ（θ₂ ) ・Ｊ₁ …（１） 但し、Ｖｎ：アーム先端の位置増分量，Ｌ₁ ：第１アー
   ム長，θ₂ ：第２関節角度，Ｊ₁ ：ユーザ指定の第１関
   節角速度指令値である。
5. 【請求項５】 前記一平面内多自由度スカラ型ロボット
   は２自由度あるいは３自由度のスカラ型ロボットである
   ことを特徴とする請求項３または４に記載の一平面内多
   自由度スカラ型ロボットの軌跡制御方法。
6. 【請求項６】 一平面内多自由度スカラ型ロボットの軌
   跡制御プログラムを記録したコンピュータ読み取り可能
   な記録媒体であって、 アーム先端位置増分量演算のための一つのパラメータと
   して指定される第１関節角速度指令値を記憶する第１関
   節角速度記憶手順と、 直線補間動作を行うに際してアーム先端の位置増分量を
   第１アーム長と第２関節角度の正弦と前記第１関節角速
   度記憶部に記憶されている第１関節角速度指令値との乗
   算により算出するアーム先端位置増分量演算手順と、 をコンピュータに実行させるためのプログラムを記録し
   たコンピュータ読み取り可能な記録媒体。