WO1997001801A1 - Method of setting acceleration/deceleration operation of robot - Google Patents

Description

明 細 書

ロボ ッ ト の加減速動作の設定方法

技 術 分 野

本発明は産業用ロ ボッ 卜の加減速動作の設定方法に関 し、 更に詳 し く 言えば、 軌道計画時に、 ロ ボッ トの動特 性を考慮 して最大加速度を定める よ う に した加減速動作 の設定方法に関する。

背 景 技 術

ロボ ッ トを効率良 く 動作さ せるためには、 加速/減速 時に駆動 ト ルク の最大値 （最大 ト ルク ） を発生させるよ う な加減速動作を実現させる こ とが望ま しい。 しか し、 ロ ボッ ト はその構造上、 動作時のイ ナ一 シ ャ変動や他の 軸の動作によ って生 じる干渉 トルクが大き く 、 動摩擦の 影響も無視出来ないため、 精度良 く 最大 ト ルク を発生さ せるよ う な加減速制御を行な う こ とは容易ではなかつた。

即ち、 従来よ り広 く 採用 されてい る最大加速度制御の 方法では、 軌道計画時に加減速動作の最大加速度を決定 するに際 して、 干渉 トルクが考慮されていなかった。 ま た、 動摩擦の影響については これを固定的な値で評価 し、 その分だけモー タの最大 ト ルク を予め減じて設定してお く と言う方法がと られていた。

上記従来の方法では、 干渉 ト ルク が考慮されていない ため、 計算上では最大 ト ルク を発生するよ う に加減速制 御の条件設定 （最大加速度の算定とそれに基づく 時定数 の決定） を行な って も、 実際の動作時に必要とする ト ル P9

2 クがモータが出力 し得る最大 トルク を超過 したり 、 ある いは逆に、 モー タ の最大 トルクが発揮されなかった りす る こ とが珍 し く ない。

前者のケース （ ト ルク飽和） では ロボッ 卜が動作指令 に追従出来な く なるために軌跡精度が悪化 し、 後者のケ ースではロ ボッ 卜 の性能が十分に発揮されず、 作業効率 が低下する。 ま た、 動摩擦の影響の考慮が一方的である ため、 減速時に必要以上に小さな ト ルクが使用される と い う問題点があ る。

発 明 の 開 示

本発明は、 干渉 ト ルク と動摩擦の影響が適正に考慮さ れる ロ ボッ トの加減速動作の設定方法を提供 し、 それに よ ってロボッ 卜 の軌跡精度と作業効率の向上を図る もの し⁵ある。

本発明では、 軌道計画時に、 加減速時に ロボッ 卜 の軸 にかかる ト ルク が最大と な る位置を逐次計算法を用いて 推定 し、 それに基づいて加減速動作を設定する。 ま た、 更に加減速時に動摩擦の影響を逆符号方向に考慮する こ とによ り 、 ロ ボ ッ ト の加減速動作時に、 よ り高精度で最 大 トルクを発揮させ得る よ う に した ものである。

本発明に従えば、 軌道計画時に下記の 3 条件を考慮 し て、 加減速動作の条件が設定される。

1 ) ロボッ 卜 の目標軌道を実現するための各軸の拘束 条件式。 通常、 これは直交座標系上におけ る ツール先端 点の位置及び姿勢と それに対応する各軸値との変換関係 を規定するヤコ ビア ン と して与え られる。

2 ) ロボッ ト の運動方程式。 こ れに基づいて干渉 ト ル クが計算される。

3 ) ロ ボッ 卜 の最大 ト ルク に関する拘束条件式。 動摩 擦の影響を考慮する際には、 こ の拘束条件式の中で動摩 擦の影響を逆符号方向に評価する。

上記 1 ) 〜 3 ) を考慮 し、 次のよ う な処理 A 〜 F を加 速動作と減速動作に関 して実行する こ とで、 適正な加減 速動作条件の設定が行なわれる。

A . 目標軌道上のある位置を設定 し、 上記ロボッ トの 運動方程式に基づいて最大 ト ルク を発生するよ う な加速 度を計算する。

B . 目標軌道上をその加速度で動作 （加速動作ま たは 減速動作） した時、 指令 トルクが最大とな る位置を計算 する。

C . 上記 2 ) の運動方程式を用い、 指令 ト ルクが最大 と な る位置とその時の速度及び加速度から ト ルクを計算 する。 こ の トルク には干渉 ト ノレク が含まれる。

D . 計算された ト ルク が上記 3 ) の条件を実質的に満 た しているか判断する。

E . 満た していない場合には、 こ れを満たすよ う に加 速度値を更新 した上で、 上記 B の処理へ戻る。 以下、 上 記 B 〜 E の処理サイ ク ルは、 上記 Dの処理で上記 3 ) の 条件が実質的に満た されたと判断される まで繰り返され る。 F . 上記 Dの処理で上記 3 ) の条件が実質的に満たさ れたと判断されたな らば、 その時の加速度値を加減速動 作の加速度と して定め、 これに基づいて加減速動作の設 定を行な う。 通常は、 指令速度値を定められた加速度値 で除して加減速制御の時定数の設定が行なわれる。

本発明の方法では、 ロ ボッ ト制御装置内で軌道計画を 立てる際に、 ロ ボッ ト の 目標軌道を実現するための各軸 の拘束条件式と、 ロ ボッ 卜の運動方程式と、 各軸の最大 ト ルク に関する拘束条件を考慮 して、 逐次計算処理に基 づいて加減速動作が設定される。 通常、 ロボッ ト の目標 軌道は動作プロ グラ ムデータ に基づく 補間計算で定めら れ、 こ れを実現するための各軸の拘束条件式はヤコ ビア ンで与え られる。 ヤコ ビア ンは、 ロボッ 卜の構'造を反映 してお り 、 個々 の点においてヤコ ビア ンの値の計算に必 要なデータは、 予めロボ ッ ト制御装置に設定される。

ロボ ッ トの運動方程式に含まれる各種パラ メ ータや各 軸の最大 ト ルク に関する拘束条件を規定するパラメ 一タ について も、 同様に、 予めロ ボッ ト制御装置に設定され る o

本発明で実行される逐次計算処理は、 トルク飽和を起 こ さない範囲で最大加速度を近似的に求めるための もの である。 こ のよ う な最大加速度が定め られれば、 指令速 度のデー タ と合わせて加減速動作の時定数が決定される。 こ の時定数の下で計算された各軸に関する移動指令を各 軸のサーボ制御系へ出力すれば、 加減速動作の中で最大 の トルク が必要と される位置において も、 トルク飽和は 回避される。 ま た、 上記最大加速度は、 ロ ボッ トの動特 性を考慮に入れて求め られた ト ルク飽和を起こ さない臨 界的な条件と して計算される。 従って、 その大き さ （絶 対値） が過小に算出され、 動作時間が無用に長引 く こ と も回避される。

動摩擦が各軸の出力 ト ルク に与える影響は、 加速動作 と減速動作では向きが逆である。 即ち、 加速動作時には 出力 トルク を減殺する方向 （加速を妨げる方向） に作用 し、 加速動作時には出力 トルクを幫助する方向 （減速を 促進する方向） に作用する。 本発明では こ の事実を勘案 し、 各軸の最大 トルク に関する拘束条件の中で動摩擦の 影響を加速動作と減速動作について逆符号方向に評価す る よ う に した。 これによ り 、 加速動作時の トルク飽和と 減速動作時の動作の遅延化がよ り確実に回避される。

図面の簡単な説明

図 1 は、 本発明の方法を適用する 4軸水平多関節ロボ ッ トの軸構成を示 した図、

図 2 は、 ロボッ 卜 の制御に用い られる一般的な システ ムの概略構成の一例を示 したブロ ッ ク 図、

図 3 は、 本実施例で加減速動作設定の対象と される位 置決め移動区間について、 加速度の微分値 j (上段） 、 加速度 a (中断） 、 速度 V (下段） の各プロ フ ァ イ ルを 共通の時間軸 （横軸） を用いてチ ャ ー ト表示 した もの、 図 4 は、 始点から終点に至るま での速度と ト ルク の関 係の推移を記すグラ フ、

図 5 は、 本発明を適用する 4軸水平多関節ロボッ 卜の 軸変数と構造パラ メ 一タを説明する図、

図 6 は、 本実施例で実行される処理の概要を記したフ ロ ーチ ャ ー ト 、

図 7 a及び図 7 b は夫々、 従来方法及び本発明の方法 によ っ て図 1 に示 した軸構成を有する ロボ ッ トを動作さ せたと きの結果を示すグラ フ、

図 8 は、 図 7 a及び図 7 b のグラ フで対象と された口 ボッ ト動作を表わす図である。

発明を実施するための最良の形態 本発明の方法を適用する ロ ボッ ト と して、 こ こ では図 1 に示 したよ う な 4 軸水平多関節ロ ボッ ト を想定する。 図 1 において、 4軸水平多関節ロ ボ ッ トは、 ベース側か ら順に、 第 1 軸 1 〜第 4軸 4 を有 している。 第 1 軸 1 は 鉛直方向の直動軸、 第 2軸 2 、 第 3 軸 3 は各々水平面内 で動作する リ ン クを駆動する旋回軸である。 第 4軸 4 は 水平面内でロボッ 卜 の手先のエ ン ドェフ エ ク タ 5 を回転 させる軸であ る。 符号 6 はエ ン ドエフ ヱ ク タ 5 の先端位 置に設定されたツール先端点を表わ してい る。 なお、 こ こでは軸変数を J 1〜 J 4で表示 した。

こ の よ う な軸構成を有する ロボ ッ 卜 の場合、 第 2 軸 2 〜第 4 軸 4 の干渉 ト ルク 、 特に第 3 軸 3 か ら第 2軸 2 へ の干渉 トルクが大き い。 そのため、 従来方法で加減速動 作の設定を行な う と、 ト ルク の使用効率がかな り悪く な り 、 それだけ動作時間が延びていた。

図 2 に、 ロボッ 卜 の制御に用い られる一般的な システ ムの概略構成の一例をブロ ッ ク 図で示 した。 図 2に記し たよ う に、 制御 システム全体はホス ト C P U 1 1、 サ一 ボ C P U 1 3 と、 各 C P U間の指令、 デー タ等を媒介す る共有 R A M 1 2、 並びにパルス コーダ 1 5が付設され たモー タ 1 4で構成される。

次に、 本発明の実施例における加減速動作の設定つい て、 図 3を参照図に加えて説明する。 図 3 は、 本実施例 で加減速動作設定の対象と される位置決め移動区間につ いて、 加速度の微分値 j (上段） 、 加速度 a (中段） 、 速度 V (下段） の各プロ フ ァ イ ルを共通の時間軸 （横軸） を用いてチ ャ ー ト表示 した ものである。 本施例では、 次 の仮定 I 〜仮定 IVの下に動作設定が行われる。 これら仮 定を成立させる に必要な設定については、 処理の実行に 必要な他のパラ メ 一 夕の設定と と もに完了済みとする。

仮定 I ： 加速及び減速の比率は一定とする。 即ち、 加 々速度のプロフ ァ イ ルは図 3の上段チヤ一 ト に示 したパ ター ンで与え られ、 t l : t 2 - t 1： t 3— t 2=—定、 且つ、 t 4— t 3 : t 5- t 4： t 6— t 5=—定、 であ る も の とする。 こ の仮定の下で、 加速度 a 、 速度 V のプロ フ ア イ ルは、 夫々 図 3の中段、 下段チ ャ ー ト に示 したバタ ー ンを持つ こ と にな る。 但 し、 ロ ングモー シ ョ ンの移動 時には、 速度 V のプロフ ァ イ ルがビー ク Cの代わり にプ ラ ト 一領域 （指令速度を保つ領域） を持つよ う にな る。 仮定 Π ： 上記仮定 I の下で、 ト ルク が最大となる点は 点 Α (加速度が最高加速値から下がり始める点） とする。 即ち、 加速度最大の条件で、 速度が最大と なる点で最も 大きな トルク 出力が要求される と仮定する。 これは、 通 常、 減速機の動摩擦等の影響によ り 、 同一の加速度の下 で も速度が大きい方が大きな トルクが要求される と仮定 したこ とに相当する。 こ の仮定を入れて、 始点から終点 に至る までの速度と トルクの関係の推移を記すと、 図 4 に示 したよ う なループにな る。 こ こ で、 符号 D はモー タ 単体の最大 ト ルク の特性曲線 （ ト ルク カーブ） を表わ し ており 、 仮定 Π によ り 、 点 Aで上記ループと接 している。

仮定 ΠΙ ： ロボッ ト は各軸動作を行な う ものとする。 仮定 IV ： ロボ ッ 卜 は指令軌道上を移動する。 これは、 始点と終点が与え られれば、 全移動量に対する比率 r を 用いて全ての動作中の位置がシステムで計算される とい う こ とを意味する。 即ち、 始点の各軸位置を q s、 終点 の各軸位置を q dと した時、 動作中の任意の点の位置は、 次式 （ 1 ) で与え られる。

q = q s+ r ( q d- q s) ( 1 ) —般に、 ロ ボ ッ ト を動作させるための処理の基本的な 流れは次のよ う な ものである。

i ) 始点、 終点の位置及び指令速度を決める処理。 こ れは、 プロ グラ ムデー タ に基づいてホス ト C P U 1 1 が 行な う処理であ る。

ii) 処理 i ) で指定された動作に応 じた加速度を決め 処理で、 本発明の固有の技術思想が含まれる。 こ の処理 は、 図 2 に示 した システムのホス ト C P U 1 1 が司る。

iii) 動作を実行する処理。 ホス ト C P U 1 1 から共 有 R A M I 2 を介 して各軸のサーボ C P U 1 3 に移動指 令を渡 し、 これに基づいてモータ 1 4 を制御する処理で ある。 パルスコ ーダ 1 5 の出力信号は、 位置ループや速 度ループのフ ィ 一 ドバッ ク信号の生成に用い られる。

上記 i ) 及び iii) の処理については周知であ るから こ こでは詳 しい説明は省略 し、 本発明の固有の技術思想 を含む上記 ii) の処理に関連 した事項について述べる。 本発明では、 上記 i i ) の加速度決定の過程でロボッ 卜の 動特性を考慮する こ とで、 モータ トルクが十分に出 し切 るよ う な加減速の動作設定が行なわれる。 以下、 本実施 例における加速度決定の過程について詳 し く 説明する。

[動特性を考慮 して加速度を定める方法]

動作時の ト ルク に関連する条件は 2 つあ り 、 ロボ ッ ト の第 i 軸にか力、る ト ルク て i及び最大 ト ルク て maxiに関 する次式 （ 2 ) 及び （ 3 ) で表わ される。

T i= M i( q ) q + h i( q , q ) ( 2 ) て maxi= g i( v i)— sgn(^ i) i | υ i I ( 3 ) 先ず、 式 （ 2 ) は ロボ ッ ト の運動方程式から得られる 条件式であ り 、 一般のロ ボッ 卜の動特性がこ の条件式に よ って記述される。 こ こ で使用されている諸記号の定義 並びに具体的な内容については、 説明の便宜上、 補足説 明で後述する。 前記仮定 Πか ら、 図 3 中で示 した点 Α に注目する と、 式 （ 2 ) は次式 （ 4 ) で表わされる。

て i_a= M i( q a) q a+ h i(q a， <¾ a) ( 4 ) こ こで、 q aは点 A に対応する位置である。 指令速度 V cmdは既知なので、 仮定 I 、 Π及び mから、 aは次式 ( 5 ) で決定される （即ち、 既知の一定値とな る） 。 t 1、 t 2は、 図 3 に示 した時間値である。 ま た、 ij aは、 q a及び ή aから次の式 （ 6 ) で決定される。 q a= V cmd ( 1.0- ) ( 5 ) て i_a— h i( q a, q a)

q a = . . r ( 6

Μ 1 ( q a)

従って、 この条件式は q aのみに関する式と な る。 更 に、 前述の式 （ 1 ) の関係か ら、 始点 q sか ら点 Aに対 応する位置までの全移動量に対する比率を r aと して、 q a = q s+ r a ^ q d— q s) ( 7 ) とな る。 以上のこ とか ら、 条件式 （ 4 ) は移動比率 r aのみに関する式と見て次式 （ 8 ) で表わすこ とが出来 る

τ i_a = f i( r a) ( 8 ) 次に 2 つ目の条件式 （ 3 ) に注目する と、 こ の式はモ — 夕の速度 · ト ルク カーブ及び動摩擦項か らな る式であ る。 第 1 項目の g i ( v i) はモー タ単体の最大 トルク の 特性曲線を表わす関数で、 図 4 に示 し た符号 D に対応す る ものである。 v iはモ一 夕速度で、 <¾ iを減速比で除 し た ものであ る。

そ して、 第 2 項目の sgn ( V i I は動摩擦を表 わす項であ る。 sgn( v i)は、 摩擦のかかる方向を規定 し、 β i I v i | は摩擦の大き さを規定する。 z iは第 i 軸の 動摩擦係数であ り 、 | v i | はモー タ速度の絶対値であ る。 こ の項は、 モー タの加速を妨げ、 減速を助けるよ う に働く 。 条件式 （ 3 ) の場合と同様に、 仮定 Π から、 図 3 中で示 した点 Aに注目する と、 次式 （ 9 ) が得られる。

て maxi , = g ( V i , )— sgn ( i _a ) i I v i, |

( 9 ) こ こ で、 τ maxi.は点 A における最大 トルクであ り 、 V i ,は点 Aにおけるモー タ速度である。 <¾ aと同様に、 v i,は既知の一定値とな る。

以上のこ とか ら、 次式 （ 1 0 ) を満たす移動比率 r a を求めれば良い こ とが判る （図 4 に示 したよ う に、 最大 トルク が要求される点 Aでモー タ の能力一杯の トルクが 発揮される） 。

て i,一 て maxi .≡ f i ' ( r a) = 0 ( 10)

[式 （ 1 0 ) を満たす r aを求める計算処理]

式 （ 1 0 ) を満たす r a= r solを求める為に、 下記の 手順か らなる近似計算処理を実行する。 図 5 の フ ロ ーチ ヤ ー ト はこれをま と めた ものであ る。 なお、 指標 i で代 表される第 i 軸についての r solの第 k近似 （ k = 0 , l， 2 , · · · ) を i r _kで表わす （ k の初期値は k = 0 ； 指 標 i の初期値は i = 1 、 図 1 に示 した ロボ ッ トでは最大 値 i = 4である） 。

ステ ッ プ S I ； 第 0次近似解 （初期値） を， r 。= 0 と する （但し、 他の値の設定も可） 。

ステ ッ プ S 2 ； 位置 i r 。において運動方程式を計算し、 最大 トルク を発生するよ う に加速度を求める。 計算式は 次式 （ 1 1 ) で与え られる。

τ imax _a― h ( q o , q a)

q l = ( 11)

M ( q o)

ステ ッ プ S 3 ； 計算された加速度を用いた時に ト ルク が最大にな る位置を求め、 i r _{k + 1}とする。 理論上の計算 式は次式 （ 1 2 ) で与え られるが、 実際の計算処理は差 分を用いた式 （ 1 3 ) で表わされる。

ί Θ r _k) )一

r _k)

d r

( 12)

■ r i r _k - ノ (

^:/ ( i r k ) - f ' ( i r _k _ ! )

( 13) ス テ ッ プ S 4 ； 計算 さ れた i r _{k + 1}と前回計算された i r _kの差 I i r _{k + 1}— i r _k i を計算 し、 予め設定された微 小値 £ (例えば、 £ = 0 .0 1 ； 全移動距離の 1 %の誤 差） を上回るか否かをチェ ッ クする。 イ エスであればス テ ツ プ S 3 へ戻る。 ノ 一であれば、 i r sol i r k + iと し て記憶 し、 ステ ッ プ S 5 へ進む。

ステ ッ プ S 5 ； 指標 i を 1 ア ッ プし、 k を k = 0 にク リ アする。 ステ ッ プ S 6 ； 全軸の計算未完了であれば、 ステ ッ プ S 1 へ戻る。 全軸の計算完了であれば、 ステ ッ プ S 7へ ステ ッ プ S 7 ； i r sol ( i = 1 , 2 , 3 , 4 ) の内、 最 大の ものを r solと して記憶する。

ステ ップ S 8 ； r solに対応する加速度を全軸に関 し て求め、 各軸における加速動作の条件を設定する （加速 制御の時定数を設定する） 。 以上の処理によ り 、 各軸に ついて、 図 4 における点 Aの時間軸上の位置、 加速度プ 口 フ ァ イ ル （中段のチャ ー ト ） における最大加速度 （点 Aの縦軸上の位置） が決定されたこ と にな る。 また、 以 上の説明で点 Aの代わり に点 Bを考え、 同様の処理を行 なえば減速動作について、 各軸における加速動作の条件 が設定出来る。 その際、 最大 トルク に関する条件式 （ 3) の定め方によ り 、 動摩擦の影響が加速の時とは逆方向に 評価され、 加速度の絶対値が従来に比 して大きめに設定 される こ と にな る。

本発明の方法を図 1 に示 した軸構成を有する実際の口 ボッ 卜 に適用 した結果について従来方法によ る結果と対 比 して図 7 a及び図 7 b のグラ フを参照 して説明する。 各グラ フ には、 第 2 軸と第 3 軸 （干渉 ト ルクが問題とな る軸） について発生 トルクが示されている。 対象と した ロ ボッ ト動作は図 8 に示されているよ う な ものであ る。 なお、 両グラ フ において、 横軸は時間を表わ している。 第 2 軸の最大 ト ルク は ± maxtrq2で示されている。 今、 第 2軸に注目する と、 こ の動作においては、 第 3 軸や第 4軸 （特に第 3軸） の動作によ る干渉 ト ルクが第 2軸に大き く かかる為、 従来方法では、 符号 Eで示 した よ う に、 トルク の飽和が観測される。

また、 従来方法は動摩擦を考慮 していない為、 符号 F で示 したよ う に、 減速時の ト ルクが加速時に比 して小さ く なつている。 これに対 して本発明の方法では、 干渉 ト ルクが考慮されている為、 従来方法に比べて加速時の ト ルク はやや抑え られてお り、 符号 Gで示 したよ う に、 ト ルクの飽和が回避される。 そ して、 本発明の方法では、 動摩擦が考慮されている為、 符号 Hで示 したよ う に、 減 速時の トルク も有効に使用されている。

この結果、 本発明の方法を適用 した場合、 加速時の最 大 トルク と減速時の最大 トルク がほぼ同 じ とな り、 加減 速について均整のとれた トルク が発生 している と言う こ とが出来る。 こ の時、 全体の動作時間は従来に比 して遅 延する こ とはな く 、 む しろ短縮される傾向にあ る。 こ の よ う に、 本発明の方法では加減速動作全体について無理 な く 、 効率的な ト ルク使用が実現される。

[運動方程式に関する補足説明 ]

本実施例における対象ロボ ッ トであ る図 1 に示 した 4 軸水平多関節ロ ボッ トの第 2軸〜第 4軸の運動方程式は、 次式 （ 1 4 ) ~ ( 1 9 ) で与え られる。 ま た、 各係数は、 式 （ 2 0 ) 〜 （ 4 0 ) で与え られる。

て 2 = a ₂₂ q 2 + a ₂₃q 3-}- a 2₄ q 4— b ₂33 <¾ 3²— b ₂ ^s I + (， 0， ^ε + + )丛 + ^s5^e ui = _Β q_Z ( Z ) " B = " B

(LZ ) ^{t s z} q = ^{z v z} q

(92 ) (" S H ="³ q

(9Z )

( ⁸ s ^ζδ*5 + ^ε s ^εδ^ζη 丛 +^s s H ^s ui)z =^{ε 3 Ζ} q oz (H ) (^{t e} s s "δ⁸δ) Λ = q ε S ³5 + ⁸ s ^εδ^ζδ ) + ⁶ S ⁸⁸5⁸5^E ^ =^{8 e z} q ill ) (^fr I +，^e O H +， 0 ^e8 +₃，3) A =

(n ) (^ε I +，^ε o +"つ， δ^επ 9ΐ

+ ^s H + 5 +_s ^e ) A + (^{3 a}H +_z'^eiJ) ^e ui = ^{S B} B (02 )

(61 ) ( ' b )i₇ q + ( )i₇ ≡ 01

(81 ) g b g ^z^ q -_zg

ill ) ( * b )g q + ( b)g ^i =

(91 ) ……

(si ) ( z q + ¾ ( ^b )zw≡

(H ) ……

Zb ^ b ^{Z f z} q — ^ b g b ^ez q — g 2 ξ> ^{8 z z} q — _z

SI

6I8I0/96dT/X3d I08I0/Z.6 OAV …… (29) a 34 =W (fi₄ ²+ ¾₃fi₄C ₄)+ I 4 (30) b 3 2 2 = Π1 3 δ₂δ_{3 β} S 3 + W (fiaSa S 3 + fi₄fi₂ S 34 )

(31) b 344 = W δ S 4 (32) b 3 34 - 2 b 3₄4 ( 33) b 342 = b 3 3 4 ( 34 ) a 4 2 =W ( fi ₄ ² + S3 fi4 C ₄ + fi ₄ fi 2 C ₃ 4 + I ₂) (35) a 3 = W (fi₄ ² + δ₃54 C 4 + I 3 ) ( 36) a 44 = W ¾ ₄ ² ( 37 ) b 42 2 = W (fi₃fi₄ S ₄ + δ₄β₂ S 3 4 ) (38) b = W S₃fi₄ S ₄ (39) b ₄₂₃ = 2 b 4 3 3 (40) こ こ で、 て ₂〜 て ₄は各々第 2軸〜第 4軸にかかる トル ク を表わ している。 また、 C i、 S i、 C i + j、 S i + jは、 各々 cos(q i) 、 sin(q i)、 cos(q iり'）、 sin(q i + j)を 表わ している。 軸変数と構造パラ メ ー タの と り 方は、 図 5 に示 した通り であ る。 即ち、 m iは第 i リ ンク の質量、 S iは第 i リ ンク の リ ンク長、 2 i ₈は第 i 軸の回転中心軸 か ら第 i リ ン ク の重心位置までの距離、 I iは第 i リ ン ク の第 i 軸の回転中心軸周 り のイ ナ一 シャ を各々表わす _c 更に、 Wは ロ ボ ッ 卜 に支持されたエ ン ドエフ ヱ ク タの質 量であ る。 これらの運動方程式を用いる こ と によ り 、 各 軸の位置、 速度、 加速度が決められた時の各軸の ト ルク を計算する こ とが出来る。 TJP9

17 上記図 7 に示 した結果から も判るよ う に、 本発明によ れば、 干渉 トルク と動摩擦の影響が適正に考慮される 口 ボ ッ ト の加減速動作の設定方法が提供される。 また、 そ れによ ってロボ ッ 卜の軌跡精度と作業効率の向上が図ら れる。

Claims

請 求 の 範 囲

1 . ロボッ トの制御装置内で軌道計画を立てる際にロボ ッ トの目標軌道を実現するための各軸の拘束条件と、 口 ボ ッ トの運動方程式と、 各軸の最大 トルク に関する拘束 条件を考慮 した逐次計算処理に基づいて加減速動作を設 定するよ う に した、 ロボ ッ 卜 の加減速動作の設定方法で あ って、 前記逐次計算処理が、

( a ) 目標軌道上のあ る位置を設定し、 前記ロボッ ト の運動方程式に基づいて最大 ト ルク を発生する よ う な加 速度を計算するステ ッ プと、

( b ) 目標軌道上をその加速度で動作 した時、 指令 ト ルクが最大となる位置を計算するステ ッ プと、

( c ) 前記運動方程式を用い、 指令 ト ルクが最大とな る位置とその時の速度及び加速度から干渉 トルクを含む ト ルク を計算するステ ッ プと 、

( d ) 計算された トルク が各軸の最大 ト ルク に関する 前記拘束条件を実質的に満た しているか否かを判断する ステ ッ プと 、

( e ) 前記ステ ッ プ （ d ) で前記拘束条件が実質的に 満たされていないと判断された場合に、 これを満たすよ う に加速度値を更新 して前記ステ ッ プ （ b ) へ戻るステ ッ ブと、

( f ) 前記ステ ッ プ （ d ) で前記拘束条件が実質的に 満たされている と判断された場合に、 その時の加速度値 を加減速動作の加速度と して定め、 これに基づいて加減 速動作の設定を行な う ステ ッ プとを含み、

前記ステ ッ プ （ b ) 〜 （ e ) は、 前記ステ ッ プ （ d ) で前記拘束条件が実質的に満たされている と判断される まで繰り返 し実行される、 前記ロ ボッ 卜 の加減速動作の 設定方法。

2 . 前記各軸の最大 トルク に関する拘束条件が、 加速動 作と減速動作について逆符号方向に動摩擦の影響を評価 する条件を含んでいる、 請求の範囲第 1 項に記載された ロ ボッ トの加減速動作の設定方法。