JPH1039916A - 多関節ロボットの加減速制御方法 - Google Patents

Abstract

(57)【要約】 【課題】 制御装置の演算部を圧迫することなく、簡便
に各駆動軸の許容最大角加速度α_maxiを導出することが
できるような多関節ロボットの加減速制御方法を提供す
る。 【解決手段】 アンバランストルクＰ_i 、及びモータと
減速機の最大許容トルクＴ_splyi とにより最小トルク余
裕率ξ_min を算出し、これにイナーシャＪ_i を考慮して
許容最大角加速度α_maxiを算出する。また、教示点の記
録情報を基にティーチングにより設定された複数の教示
点間の中間位置θ_i ^halfにおける予想最大角速度Ｖ_maxi
を算出した後、中間位置θ_i ^halfでの遠心力及びコリオ
リ力の予想最大値Ｃ_maxiを算出する。ここで遠心力及び
コリオリ力の予想最大値Ｃ_maxiが限界値（Ｔ_splyi −Ｐ
_i ）より大きい場合は予想最大角速度Ｖ_maxiを補正し、
これを許容最大角速度Ｖ_maxiとする。算出された各駆動
軸の許容最大角加速度α_maxi及び許容最大角速度Ｖ_maxi
に基づいて多関節ロボットの加減速制御を行う。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】複数の駆動軸を有する多関節
ロボットの加減速時における制御方法に関し、特に、動
作中の各駆動軸の角度、角速度、及び角加速度が、他の
駆動軸の動作から受ける力すなわち反力の影響を受ける
ように構成されている多関節ロボットの加減速制御方法
に関する。

【０００２】

【従来の技術】複数の駆動軸を有する多関節ロボットに
おいては、駆動軸モータの特性やアームの形態等から各
駆動軸の許容最大角加速度α_maxiを算出し、動作中のロ
ボットの各駆動軸の角加速度α_i がこの許容最大角加速
度α_maxiを超えないように制御するようにしている。そ
して、この許容最大角加速度α_maxiは、駆動軸の発生ト
ルクτ_i がこの駆動軸の最大許容トルクＴ_splyi を超え
ないように制限されている。ここで、各駆動軸の最大許
容トルクＴ_splyi は、各駆動軸を駆動する駆動軸モータ
の発生トルク限界Ｔ_miと減速機の許容トルクＴ_giのうち
いづれか値の小さい方のトルク値によって与えられる。

【０００３】一般に、ｎ軸の自由度を有する多関節ロボ
ットの第ｉ駆動軸（ｉは駆動軸ナンバー）における発生
トルクτ_i は、この第ｉ駆動軸のイナーシャをＪ_i （θ
_1,θ_2,・・・，θ_n ）、角加速度をα_i 、遠心力及びコ
リオリ力によるトルクをＣ_i（ω_1,ω_2,・・・，ω_n 、
θ_1,θ_2,・・・，θ_n ）、アンバランストルクをＰ
_i（θ_1,θ_2,・・・，θ_n ）とすると、式（１）により
与えられる。

【０００４】

【数１】

【０００５】式（１）において、（θ_1,θ_2,・・・，θ
_n ）は第１〜第ｎ駆動軸の角度であり、（ω_1,ω_2,・・
・，ω_n ）は第１〜第ｎ駆動軸の角速度である。式
（１）の左辺に第ｉ駆動軸の最大許容トルクＴ_splyi を
代入すると、この第ｉ駆動軸における許容最大角加速度
α_maxiは式（２）に示すようになる。

【０００６】

【数２】

【０００７】多関節ロボットの実際の加減速制御では、
全ての駆動軸において、各駆動軸の角加速度α_i がこの
許容最大角加速度α_maxiを超えないように制御されるこ
とになる。式（２）には全ての駆動軸の姿勢や角速度が
考慮されているので、ロボットアームを比較的低速で動
作させる場合においては、式（２）により算出された各
駆動軸の許容最大角加速度α_maxiに基づいて各駆動軸を
制御することにより、減速機の破損及び寿命低下を防止
することが可能となる。

【０００８】しかし、式（２）は他の駆動軸の動作から
受ける力すなわち反力については考慮されていないの
で、複数の駆動軸が高速で同時に動作する場合や急激な
加減速を行う場合など、他の駆動軸から受ける反力の影
響を無視できないような動作態様においては、式（２）
により算出された各駆動軸の許容最大角加速度α_maxiに
基づいて各駆動軸の角加速度α_i を制御するようにする
と、他の駆動軸から受ける反力の影響により、各駆動軸
の最大許容トルクＴ_splyi を超すような場合が生じると
いう問題がある。極端な場合には、動作中の第ｉ駆動軸
の角加速度α_i が０の場合でも、この第ｉ駆動軸以外の
駆動軸から受ける反力の影響により、第ｉ駆動軸の発生
トルクτ_i がこの駆動軸の最大許容トルクＴ_splyi を超
すような場合が発生する。

【０００９】そこで、出願人が先に出願した特願平８−
３０１２０号では、複数の駆動軸が高速で同時に動作す
る場合や急激な加減速を行う場合など、他の駆動軸の動
作から受ける反力を無視できないような動作態様におい
ても、各駆動軸の発生トルクτ_i がこの駆動軸の最大許
容トルクＴ_splyi を超えることなく、減速機の破損及び
寿命低下を防止することができるような多関節ロボット
の加減速制御方法を提供した。

【００１０】すなわち、特願平８−３０１２０号では、
動作中の多関節ロボットの各駆動軸の角度θ_i 、角速度
ω_i 、及び角加速度α_i が他の駆動軸の動作から受ける
力すなわち反力の影響を受けるようにされている多関節
ロボットの加減速制御方法において、各駆動軸につい
て、遠心力及びコリオリ力によるトルクＣ_i 、アンバラ
ンストルクＰ_i 、及びモータの発生トルク限界Ｔ_miと減
速機の許容トルクＴ_giのうちいづれか値の小さい方のト
ルク値すなわち最大許容トルクＴ_splyi とにより、各駆
動軸のトルク余裕率ξ_i を算出し、算出された全ての駆
動軸のトルク余裕率のうち最小の値をもつものを最小ト
ルク余裕率ξ_min として抽出し、前記最大許容トルクＴ
_splyi 、最小トルク余裕率ξ_min 、及び駆動軸のイナー
シャＪ_i とにより、各駆動軸の許容最大角加速度α_maxi
を算出し、動作中の多関節ロボットの全ての駆動軸につ
いて、角加速度α_i が許容最大角加速度α_maxiを超えな
いように制御することにより、減速機の破損及び寿命低
下を防止することができるようにした。

【００１１】上記加減速制御方法を図３に示すフローチ
ャートに基づいて具体的に説明する。まず、ロボットが
適正に動作するための各駆動軸のアーム角度としての指
令位置θ_i を出力する指令位置発生装置からこの指令位
置θ_i を得（ステップ３１）、この指令位置θ_i と、メ
モリに記憶しておいたθ_i よりも１演算周期前（１スキ
ャンタイム前）の指令位置θ_i ′より、各駆動軸の角速
度ω_i を算出する（ステップ３２）。この各駆動軸の指
令位置θ_i 及び角速度ω_i より、各駆動軸のトルク余裕
率ξ_i を式（３）により算出する（ステップ３３）。

【００１２】

【数３】

【００１３】式（３）において、Ｃ_i （ω_1,ω_2,・・
・，ω_n 、θ_1,θ_2,・・・，θ_n ）は第ｉ駆動軸におけ
る遠心力及びコリオリ力によるトルクであり、Ｐ_i （θ
_1,θ_2,・・・，θ_n ）はこの第ｉ駆動軸におけるアンバ
ランストルクである。また、Ｔ_splyi は第ｉ駆動軸にお
ける最大許容トルクであり、この値はこの駆動軸のモー
タの発生トルク限界Ｔ_miと減速機の許容トルクＴ_giのう
ちいづれか値の小さい方のトルク値によって与えられ
る。

【００１４】次に、式（４）に示すように、式（３）よ
り算出された各駆動軸のトルク余裕率ξ_i のうち最小の
値をもつものを最小トルク余裕率ξ_min として抽出する
（ステップ３４）。

【００１５】

【数４】

【００１６】次に、式（４）より算出された最小トルク
余裕率ξ_min 、各駆動軸の最大許容トルクＴ_splyi 及び
イナーシャＪ_i （θ_1,θ_2,・・・，θ_n ）とにより、各
駆動軸の許容最大角加速度α_maxiを式（５）により算出
する（ステップ３５）。

【００１７】

【数５】

【００１８】最後に、式（５）により算出された各駆動
軸の許容最大角加速度α_maxiを、前述の各駆動軸の指令
位置θ_i を出力する指令位置発生装置に対して出力する
（ステップ３６）。

【００１９】

【発明が解決しようとする課題】しかし、各駆動軸の許
容最大角加速度α_maxiを導出するための上記の処理過程
においては、遠心力及びコリオリ力によるトルクＣ_i 、
アンバランストルクＰ_i、及びイナーシャＪ_i の各値を
正確に求めなければならず、多関節ロボットの場合これ
らの計算には複雑な座標変換処理が必要であり、これに
よる制御装置の演算部の処理能力への圧迫に対処するた
めに、演算周期（スキャンタイム）を延ばす等の対応を
余儀なくされていた。したがって、特願平８−３０１２
０号の方法では、駆動軸モーターの出し得るトルク内で
の加減速制御が保証されることによりロボットの軌跡精
度の向上を実現できるという利点がある反面、制御装置
の演算周期が延びることによって、短ピッチ移動性能の
低下やリアルタイムトラッキング系の追従性能の低下等
の副作用を招いていた。

【００２０】ここで、短ピッチ移動性能とは短ピッチす
なわち短い距離を移動する際の移動性能のことであり、
演算周期が延びると、移動指令の出力にかかる所要時間
が延びることになり、その結果短ピッチの移動性能が低
下することになる。また、リアルタイムトラッキング系
の追従性能とは、例えば物品の搬送にハンドリングロボ
ットとコンベア装置とを組み合わせた場合のロボットの
コンベア同期機能について、リアルタイムにコンベア装
置から入力されるコンベアの位置信号に従ってロボット
の再生位置をシフト移動させる際の追従性能のことであ
り、制御装置の演算周期が延びると、ロボットの再生位
置を算出するための計算のインターバルが延びることに
なり、その結果動作中のロボットに脈動現象が発生し、
追従性能が低下することになる。

【００２１】本発明はこのような従来技術の問題点を解
決するためになされたものであり、制御装置の演算部の
処理能力を圧迫することなく、簡便に各駆動軸の許容最
大角加速度α_maxiを導出することができるような多関節
ロボットの加減速制御方法を提供することを目的として
いる。

【００２２】

【課題を解決するための手段】上述の遠心力及びコリオ
リ力によるトルクＣ_i 、アンバランストルクＰ_i 、及び
イナーシャＪ_i のそれぞれの算出に要する時間の比は、
おおよそ４：１：１であることを出願人は知得した。そ
こで本発明では、これらの中で最も算出時間を要する遠
心力及びコリオリ力によるトルクＣ_i の算出による、制
御装置の演算部の処理能力への圧迫に対処するための演
算処理方法を提供することにより、上記の課題を解決す
ることにした。すなわち本発明における演算処理方法で
は、図３に示した従来技術の許容最大角加速度α_maxiの
算出過程から、遠心力及びコリオリ力によるトルクＣ_i
の算出処理を排除し、その代わりに、ティーチングによ
って与えられた複数の教示点について、各教示点間の中
間位置における許容最大角速度Ｖ_maxiの算出過程を新た
に設定することとし、これら２つの算出過程により算出
された各駆動軸の許容最大角加速度α_maxi及び許容最大
角速度Ｖ_maxiを、各駆動軸の指令位置θ_i を出力する指
令位置発生装置へ出力し、これらの値を上限値として各
駆動軸の角加速度及び角速度を規制するようにした。

【００２３】具体的な構成として、請求項１に記載され
ているように、許容最大角加速度α_maxiの算出過程で
は、各駆動軸について、駆動軸に対する指令位置θ_i に
より規定されるアンバランストルクＰ_i 、及びモータの
発生トルク限界Ｔ_miと減速機の許容トルクＴ_giのうちい
づれか値の小さい方のトルク値すなわち最大許容トルク
Ｔ_splyi とによりトルク余裕率ξ_i を算出し（ステップ
１２）、算出された全ての駆動軸のトルク余裕率のうち
最小の値をもつものを最小トルク余裕率ξ_min として抽
出し（ステップ１３）、各駆動軸について、最大許容ト
ルクＴ_splyi 、最小トルク余裕率ξ_min 、及びイナーシ
ャＪ_i とにより、許容最大角加速度α_maxiを算出する
（ステップ１４）。ここで、駆動軸に対する指令位置θ
_i とは、各駆動軸を動作させるサーボモータに対して制
御装置の指令位置発生装置から出力される駆動軸の角度
データをいう。

【００２４】一方、許容最大角速度Ｖ_maxiの算出過程で
は、ティーチングにより設定された複数の教示点につい
て、各教示点間の中間位置θ_i ^halfにおける角速度を教
示点の記録情報から算出し、これを予想最大角速度Ｖ
_maxiとして設定し（ステップ１５）、中間位置θ_i ^half
及び予想最大角速度Ｖ_maxiの各データより、中間位置θ
_i ^halfでの遠心力及びコリオリ力の予想最大値Ｃ_maxiを
算出し（ステップ１６）、最大許容トルクＴ_splyi 及び
アンバランストルクＰ_i より遠心力及びコリオリ力の限
界値（Ｔ_splyi −Ｐ_i ）を算出し、この限界値（Ｔ
_splyi −Ｐ_i ）と予想最大値Ｃ_maxiとの比（（Ｔ_splyi
−Ｐ_i ）／Ｃ_maxi）を両者の比率ρ_i として設定し（ス
テップ１７）、この比率ρ_i が１未満のときは予想最大
角速度Ｖ_maxiに比率ρ_i の平方根（√ρ_i ）を乗じて得
た値を許容最大角速度Ｖ_maxiとして置き換え、一方、比
率ρ_i が１以上のときは予想最大角速度Ｖ_maxiの値をそ
のまま許容最大角速度Ｖ_maxiとする（ステップ１８〜１
９）。

【００２５】ここで、教示点の記録情報とは、ティーチ
ングにより教示点を指定する際に設定する各種パラメー
タであり、具体的には、教示点の位置データ、教示点ま
での移動速度データ、教示点での位置決め精度データ、
教示点への移動における補間等の移動形態データなどで
ある。本発明では各教示点間の中間位置θ_i ^halfにおけ
る予想最大角速度Ｖ_maxiはこれらの記録情報を基に算出
する。また、予想最大角速度Ｖ_maxiを各教示点間の中間
位置θ_i ^halfにおける角速度とした点については、教示
点間の移動において最も角速度が大きくなるのは、一般
に教示点間の中間位置においてであることによる。

【００２６】最後に、これら２つの算出過程により算出
された各駆動軸の許容最大角加速度α_maxi及び許容最大
角速度Ｖ_maxiに基づいて、各駆動軸の角加速度α_i が許
容最大角加速度α_maxiを超えず、かつ各駆動軸の角速度
ω_i が許容最大角速度Ｖ_maxiを超えないように制御する
ことにより、減速機の破損及び寿命低下を防止するよう
にした。

【００２７】ここで、上記の請求項１に記載の構成にお
いて、比率ρ_i が１未満のときは予想最大角速度Ｖ_maxi
に比率ρ_i の平方根（√ρ_i ）を乗じて得た値を許容最
大角速度Ｖ_maxiとして置き換え、一方、比率ρ_i が１以
上のときは予想最大角速度Ｖ_maxiの値をそのまま許容最
大角速度Ｖ_maxiとした点について説明する。予想最大角
速度Ｖ_maxiは前述のように教示点の記録情報から算出さ
れており、この教示点の記録情報には駆動軸の許容トル
クについては加味されていない。よって、教示点の記録
情報のみから許容最大角速度を設定するようにすると、
動作形態によっては駆動軸が許容トルクオーバーとなる
場合が生ずる。具体的に言うと、比率ρ_i が１未満のと
き、すなわち遠心力及びコリオリ力によるトルクが占め
ることのできる限界値（Ｔ_splyi −Ｐ_i ）よりも予想最
大値Ｃ_maxiが大きい場合は、中間位置θ_i ^halfにおける
遠心力及びコリオリ力によるトルクによって、第ｉ駆動
軸が許容トルクオーバー状態となる可能性がある。

【００２８】そこで比率ρ_i が１未満の場合は、遠心力
及びコリオリ力によるトルクは概ね角速度の２乗に比例
することを考慮し、予想最大角速度Ｖ_maxiに√（ρ_i ）
を乗じた値を許容最大角速度として置き換えることによ
り補正することにした。一方、比率ρ_i が１以上のとき
は、遠心力及びコリオリ力によるトルクの予想最大値Ｃ
_maxiが、限界値（Ｔ_splyi −Ｐ_i ）以内に治まっている
わけであるから、この場合は修正の必要はなく、予想最
大角速度Ｖ_maxiの値をそのまま許容最大角速度とするこ
とにした。

【００２９】上記の構成によれば、遠心力及びコリオリ
力によるトルクＣ_maxiは、各教示点間の中間位置θ_i
^halfと、この中間位置θ_i ^halfでの予想最大角速度Ｖ
_maxiとにより算出されており、結局のところＣ_maxiは各
教示点間の移動の際に１回だけ算出すればよいことにな
る。一方、従来技術では、遠心力及びコリオリ力による
トルクＣ_i は、指令位置θ_i と、メモリに記憶しておい
たθ_i よりも１演算周期前（１スキャンタイム前）の指
令位置θ_i ′とにより求められた角速度ω_i により算出
されており、結局のところＣ_i は演算周期毎（スキャン
タイム毎）に算出しなければならなかった。一般に、制
御装置の演算部における演算周期（スキャンタイム）は
数十ｍ秒であるが、各教示点間の移動に要する時間は数
百ｍ秒〜数秒であるので、演算量の大きい遠心力及びコ
リオリ力によるトルクの算出では、本発明の方が従来技
術よりも制御装置に対する演算負荷が格段に小さくな
る。

【００３０】

【発明の実施の形態】以下、本発明の実施形態について
図面を参照しながら具体的に説明する。図２は本発明の
制御方法が適用されるシステムの一例を示すブロック図
であり、図中１はロボット本体、２はサーボアンプ、３
は指令位置発生装置、４は目標位置発生装置、５は許容
最大角加速度α_maxi及び許容最大角速度Ｖ_maxiの計算装
置である。目標位置発生装置４は、メモリーに記録され
た位置または計算により求められた位置を、ロボットが
到達すべき目標位置として指令位置発生装置３へ出力す
る。指令位置発生装置３は、目標位置、指定速度、現在
位置、及び加速度制限等の値から、ロボットが適正に動
作するための時々刻々の各駆動軸の指令角度としての指
令位置θ_i を算出し、サーボアンプ２へ出力する。

【００３１】許容最大角加速度α_maxi及び許容最大角速
度Ｖ_maxiの計算装置５は、本発明の制御方法において算
出される許容最大角加速度α_maxi及び許容最大角速度Ｖ
_maxiの計算装置であり、指令位置発生装置３の出力であ
る各駆動軸のアーム角度としての指令位置θ_i を入力
し、各駆動軸の減速機の許容トルクＴ_gi及び駆動軸モー
タの発生トルク限界Ｔ_miより許容最大角加速度α_maxi及
び許容最大角速度Ｖ_maxiを算出し、指令位置発生装置３
へ出力するループを形成する。サーボアンプ２は、指令
位置発生装置３からの指令位置θ_i の入力を受け、ロボ
ット本体１を動作させるための動作指令を出力する。ロ
ボット本体１には図示しない駆動軸モータ、エンコー
ダ、減速機等が取り付けられており、これらはサーボル
−プを構成する。

【００３２】図１は、図２において示した許容最大角加
速度α_maxi及び許容最大角速度Ｖ_maxiの計算装置５にて
行われる、各駆動軸の許容最大角加速度α_maxi及び許容
最大角速度Ｖ_maxiの算出処理の流れを示すフローチャー
トである。まず、指令位置発生装置３からロボットが適
正に動作するための各駆動軸の指令位置θ_i を得る（ス
テップ１１）。この各駆動軸の指令位置θ_i より、各駆
動軸のトルク余裕率ξ_i を式（６）により算出する（ス
テップ１２）。

【００３３】

【数６】

【００３４】式（６）において、Ｐ_i （θ_1,θ_2,・・
・，θ_n ）は第ｉ駆動軸（ｉは駆動軸ナンバー）におけ
るアンバランストルクである。また、Ｔ_splyi はこの第
ｉ駆動軸における最大許容トルクであり、この値は第ｉ
駆動軸のモータの発生トルク限界Ｔ_miと減速機の許容ト
ルクＴ_giのうちいづれか値の小さい方のトルク値によっ
て与えられる。

【００３５】このトルク余裕率ξ_i の算出において、本
実施形態におけるステップ１２での処理と、先行技術で
ある特願平８−３０１２０号におけるステップ３３での
処理とを比較すると、特願平８−３０１２０号では式
（３）で示したように、第ｉ駆動軸における遠心力及び
コリオリ力によるトルクであるＣ_i （ω_1,ω_2,・・・，
ω_n 、θ_1,θ_2,・・・，θ_n ）を考慮しているが、本実
施形態ではこれを考慮していない。本実施形態では、第
ｉ駆動軸における遠心力及びコリオリ力によるトルクで
あるＣ_i は、後述する許容最大角速度Ｖ_maxiの算出過程
で考慮するようにしている。

【００３６】次に、式（７）に示すように、式（６）よ
り算出された各駆動軸のトルク余裕率ξ_i のうち最小の
値をもつものを最小トルク余裕率ξ_min として抽出する
（ステップ１３）。

【００３７】

【数７】

【００３８】次に、式（７）より算出された最小トルク
余裕率ξ_min 、第ｉ駆動軸における最大許容トルクＴ
_splyi 及びイナーシャをＪ_i （θ_1,θ_2,・・・，θ_n ）
とにより、第ｉ駆動軸における許容最大角加速度α_maxi
を式（８）により算出する（ステップ１４）。

【００３９】

【数８】

【００４０】一方、前述のステップ１２において考慮に
いれなかった遠心力及びコリオリ力によるトルクＣ
_i を、上記許容最大角加速度α_maxiの算出過程とは別過
程にて考慮しているのが図１に示すフローチャートのス
テップ１５以降の処理ステップである。遠心力及びコリ
オリ力によるトルクＣ_i は、位置と角速度に依存し、一
般に角速度が大きいほど大きくなる。そこで、各駆動軸
において、ティーチングにより設定された複数の教示点
について、隣接する教示点間の移動の際に角速度が最も
大きくなる位置すなわち各教示点間の丁度中間の位置θ
_i ^halfを代表点として抽出し、教示点の記録情報からこ
の中間位置θ_i ^halfでのおおよその最大角速度すなわち
予想最大角速度Ｖ_maxiを算出する（ステップ１５）。こ
こで、教示点の記録情報とは、ティーチングにより教示
点を指定する際に設定する各種パラメータであり、具体
的には、教示点の位置データ、教示点までの移動速度デ
ータ、教示点での位置決め精度データ、教示点への移動
における補間等の移動形態データなどである。

【００４１】この中間位置θ_i ^half及び予想最大角速度
Ｖ_maxiから、式（９）により、第ｉ駆動軸における遠心
力及びコリオリ力によるトルクＣ_maxiを算出する（ステ
ップ１６）。ここで示されるＣ_maxiは中間位置θ_i ^half
での遠心力及びコリオリ力によるトルクの予想最大値と
なる。

【００４２】

【数９】

【００４３】ここで、従来技術と本実施形態における遠
心力及びコリオリ力によるトルクの算出過程を比較す
る。従来技術における遠心力及びコリオリ力によるトル
クＣ_iは、指令位置θ_i と、メモリに記憶しておいたθ
_i よりも１演算周期前（１スキャンタイム前）の指令位
置θ_i ′とにより求められた角速度ω_i により算出され
ており、結局のところＣ_i は演算周期毎（スキャンタイ
ム毎）に算出しなければならなかった。一方、本実施形
態における遠心力及びコリオリ力によるトルクＣ
_maxiは、各教示点間の中間位置θ_i ^halfと、この中間位
置θ_i ^halfでの予想最大角速度Ｖ_maxiとにより算出され
ており、結局のところＣ_maxiは各教示点間の移動の際に
１回だけ算出すればよいことになる。一般に、制御装置
の演算部における演算周期（スキャンタイム）は数十ｍ
秒であるが、各教示点間の移動に要する時間は数百ｍ秒
〜数秒であるので、演算量の大きい遠心力及びコリオリ
力によるトルクの算出では、本実施形態の方が制御装置
に対する演算負荷が格段に小さくなる。

【００４４】この中間位置θ_i ^halfにおける第ｉ駆動軸
のアンバランストルクＰ_i 及び最大許容トルクＴ_splyi
とから、同時刻において遠心力及びコリオリ力によるト
ルクが占めることのできる限界値（Ｔ_splyi −Ｐ_i ）を
求めることができるので、この限界値と式（９）より求
められた中間位置θ_i ^halfでの遠心力及びコリオリ力に
よるトルクの予想最大値Ｃ_maxiとを比較する。具体的に
は、式（１０）により、比率ρ_i を算出する（ステップ
１７）。

【００４５】

【数１０】

【００４６】式（１０）により算出された比率ρ_i が１
未満のとき、すなわち遠心力及びコリオリ力によるトル
クが占めることのできる限界値（Ｔ_splyi −Ｐ_i ）より
も予想最大値Ｃ_maxiが大きい場合は、中間位置θ_i ^half
における遠心力及びコリオリ力によるトルクによって、
第ｉ駆動軸が許容トルクオーバー状態となる可能性があ
る。そこで比率ρ_i が１未満の場合は、遠心力及びコリ
オリ力によるトルクは概ね角速度の２乗に比例するもの
と考え、ρ_i ←√（ρ_i ）と置き換えたうえで（ステッ
プ１８）、先に算出した中間位置θ_i ^halfでのおおよそ
の予想最大角速度Ｖ_maxiを、式（１１）により修正し、
修正後の値を許容最大角速度とすることにする（ステッ
プ１９）。一方、比率ρ_i が１以上のときは、遠心力及
びコリオリ力によるトルクの予想最大値Ｃ_maxiが、限界
値（Ｔ_splyi −Ｐ_i ）以内に治まっているわけであるか
ら、この場合は修正の必要はなく、実際の演算処理では
式（１１）においてρ_i ＝１とすることになる。

【００４７】

【数１１】

【００４８】式（８）により算出された許容最大角加速
度α_maxi及び式（１１）により修正された中間位置θ_i
^halfでの許容最大角速度Ｖ_maxiは、指令位置発生装置３
へ出力され（ステップ２０）、指令位置発生装置３はこ
れらの値を角加速度及び角速度の上限値として、次回の
指令位置を導出する。

【００４９】以上の処理のうち、ステップ１１〜１４
は、指令位置θ_i と、メモリに記憶しておいたθ_i より
も１演算周期前（１スキャンタイム前）の指令位置
θ_i ′のデータに基づいて処理しているので、演算周期
毎に行う必要があるが、ステップ１５〜１９は、各教示
点の中間位置における角速度に基づいているので、各教
示点間の移動の際に１回だけ行えばよい。これにより遠
心力及びコリオリ力によるトルクを算出する過程が、従
来技術の演算周期毎から、本実施形態の各教示点間の移
動毎に１回になったことにより、制御装置の演算負荷を
軽減することができる。

【００５０】

【発明の効果】本発明における多関節ロボットの加減速
制御方法によれば、遠心力及びコリオリ力によるトルク
は、各教示点間の中間位置と、この中間位置での予想最
大角速度とにより算出されるようにし、この算出過程は
各教示点間の移動の際に１回だけ行えばよいので、従来
技術のように演算周期毎に算出する必要はなくなり、簡
便に各駆動軸の許容最大角加速度α_maxi及び許容最大角
速度Ｖ_maxiを算出できるようになった。そして、この許
容最大角加速度α_maxi及び許容最大角速度Ｖ_maxiを用い
て多関節ロボットを加減速制御することにより、減速器
の破損及び寿命低下を防止できるようになった。

【００５１】また、制御装置の演算負荷が軽減され、演
算周期が延びることがなくなったことにより、短ピッチ
移動性能の低下やリアルタイムトラッキング系の追従性
能の低下等の従来技術の不具合が解消されることとなっ
た。

【図面の簡単な説明】

【図１】本発明の一実施形態における、許容最大角加速
度α_maxi及び許容最大角速度Ｖ_maxiの算出処理の流れを
示すフローチャートである。

【図２】本発明の一実施形態における、加減速制御方法
が適用されるシステムの一例を示すブロック図である。

【図３】従来技術における許容最大角加速度α_maxiの算
出処理の流れを示すフローチャートである。

Claims (1)

【特許請求の範囲】
1. 【請求項１】動作中の多関節ロボットの各駆動軸の角
   度、角速度、及び角加速度が他の駆動軸の動作から受け
   る力すなわち反力の影響を受けるようにされている多関
   節ロボットの加減速制御方法において、 各駆動軸について、駆動軸に対する指令位置θ_i により
   規定されるアンバランストルクＰ_i 、及びモータの発生
   トルク限界Ｔ_miと減速機の許容トルクＴ_giのうちいづれ
   か値の小さい方のトルク値すなわち最大許容トルクＴ
   _splyi とによりトルク余裕率ξ_i を算出し、 算出された全ての駆動軸のトルク余裕率のうち最小の値
   をもつものを最小トルク余裕率ξ_min として抽出し、 各駆動軸について、前記最大許容トルクＴ_splyi 、前記
   最小トルク余裕率ξ_min 、及びイナーシャＪ_i とによ
   り、許容最大角加速度α_maxiを算出し、 一方、ティーチングにより設定された複数の教示点につ
   いて、各教示点間の中間位置θ_i ^halfにおける角速度を
   教示点の記録情報から算出し、これを予想最大角速度Ｖ
   _maxiとして設定し、 前記中間位置θ_i ^half及び予想最大角速度Ｖ_maxiより、
   中間位置θ_i ^halfでの遠心力及びコリオリ力の予想最大
   値Ｃ_maxiを算出し、 前記最大許容トルクＴ_splyi 及びアンバランストルクＰ
   _i より遠心力及びコリオリ力の限界値（Ｔ_splyi −
   Ｐ_i ）を算出し、該限界値（Ｔ_splyi −Ｐ_i ）と前記予
   想最大値Ｃ_maxiとの比（（Ｔ_splyi −Ｐ_i ）／Ｃ_maxi）
   を両者の比率ρ_i として設定し、 該比率ρ_i が１未満のときは前記予想最大角速度Ｖ_maxi
   に比率ρ_i の平方根（√ρ_i ）を乗じて得た値を許容最
   大角速度Ｖ_maxiとして置き換え、一方、比率ρ_i が１以
   上のときは前記予想最大角速度Ｖ_maxiの値をそのまま許
   容最大角速度Ｖ_maxiとし、 動作中の多関節ロボットの全ての駆動軸について、各駆
   動軸の角加速度α_i が前記許容最大角加速度α_maxiを超
   えず、かつ各駆動軸の角速度ω_i が前記許容最大角速度
   Ｖ_maxiを超えないように制御することにより、減速機の
   破損及び寿命低下を防止するようにしたことを特徴とす
   る多関節ロボットの加減速制御方法。