JPH07120215B2 - ロボットの制御方法 - Google Patents

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、高速回転時にトルク減小特性を示すモータの
回転にり関節を駆動するようにしたロボットの制御方法
に係り、特に制御の総合特性の向上対策に関する。

（従来の技術） 従来より、ロボットのアーム手先の運動を制御する方法
として、第９図に示すように、出発地点から所定時間の
間は速度ｖをリニアに増加つまり加速度を一定の値に保
持する加速部（ａ）と、一定速度に達するとその速度を
維持する定速部（ｂ）と、目標地点に近づくと速度をリ
ニアに減小つまり加速度を負の一定の値に保持する減速
部（ｃ）とからなる台形状の速度計画線図で表わされる
速度計画を立案し、該速度計画に基づいてロボットのア
ーム手先の運動を制御する方法は一般的に知られてい
る。

（発明が解決しようとする問題点） しかしながら、上記従来の方法では、モータの種類によ
って、以下のような問題が生ずる。

例えば、第１図に示すようなトルクＴ−モータ角速度
特性を有するリアクタンス可変形のいわゆるVR型ステッ
ピングモータの場合、高速回転ではトルクが減小する特
性を有し、回転速度が_Ｓから_Ｒ（_Ｒ＞_Ｓ）に上
昇すると、モータのトルクはT_SからT_R（T_R＜T_S）に減小
する。したがって、上記のような台形状の速度計画線図
を利用すると、高速運転時のトルクが不足して減速過程
で振動を生ずる虞れがある。その一方、高速運転時のト
ルクを十分大きくもたせるべき容量を大きく設定する
と、低速運転時には十分にその容量を利用できず、無駄
が生じると共に関節等の重量が大きくなる。さらに、そ
のような事態で回避しようとすると、加速度を十分大き
く設定できず目標値に到達するまでの時間が長くなると
いう問題がある。

本発明は斯かる点に鑑みてなされたものであり、その目
的は、トルクの減小に見合った速度計画の変更を行うこ
とにより、モータの容量を十分活用してロボット動作の
高速化を図ることにある。

（問題点を解決するための手段） 上記目的を達成するため本発明の解決手段は、回転数が
高くなるほどトルクが減小する特性を有する関節駆動用
モータの加減速を制御して、少なくとも停止状態から所
定速度に達するまでの加速部及び所定速度から停止する
までの減速部を有するようロボットの動作を制御するロ
ボット制御方法として、上記加速部及び減速部では、上
記モータのトルク特性を所定の関数関係式で近似し、こ
の関数関係式に沿って上記モータの加減速パターンを導
き出してロボットの速度計画を立案し、該速度計画に基
づきモータの回転速度を調節して、アーム手先の運動を
制御する方法である。

（作用） 以上の方法により、本発明では、ロボットアーム手先の
移動経路に対応して速度計画が立案され、該速度計画に
基づいてモータ（18）の回転が調節されてロボットの運
動が制御される。

その場合、速度計画の加減速特性がモータ（18）の高速
回転時におけるトルク減小特性に応じた所定の関数で近
似されているので、低速側では加速値を大きく高速側で
は加速値を小さくしてモータ（18）のトルク特性に対応
して設定されることになり、振動等の不安定状態の発生
を防止しながら、加減速に要する時間が短縮される。さ
らに、モータ（18）のトルク特性の有効利用によりモー
タ容量に小形化することができるに加えて、移動距離の
長短に拘らず一定のパターンで簡易に制御を行うことが
でき、ロボットの総合性能を向上させることができる。

（実施例） 以下、本発明の実施例について、図面に基づき説明す
る。

第４図は六関節マニピュレータの制御系に本発明を適用
した場合のブロック線図を示す。尚、各関節の信号伝達
系は互いに同一であるので、根元と手先の関節のみにつ
いて図示し、中間の関節については図示を省略してあ
る。同図において、（PL）は目標地点の設定信号に応じ
て、ロボットのアーム手先の最適な速度計画を作成し、
該速度計画に基づく目的の手先軌跡信号を出力するプラ
ンナ、（１）は該プランナ（PL）の目的の手先軌跡信号
に基づいて各関節の目標関節角度信号θr₁,θr₆を逐次
内挿演算して出力する目標関節角度計算器、（２）は該
目標関節角度計算器（１）の目標関節角度信号θr₁,θr
₆を伝達関数１＋s/Kp（ｓは制御理論におけるラプラス
演算子）で遅れを補償する遅れ補償回路、（３）は上記
遅れ補償回路（２）からの目標関節角度信号θ′r₁,θr
₆と実関節角度θ₁,θ_６との代数差を演算する第１加え
合せ点、（４）は該第１加え合せ点（３）の演算結果
θ′・r₁−θ₁,θ′・r₆−θ_６に関節角度ループゲイン
Kpを乗じる関節角度ループゲイン器、（５）は該関節角
度ループゲイン器（４）の乗算結果Kp（θ′・r₁−
θ_１）,Kp（θ′r₆−θ_６）と関節角速度₁,_６との
代数差を演算する第２加え合せ点、（10）は該第２加え
合せ点（５）の演算結果（以下切換え関数と記す）S₁＝
Kp（θ′・r₁−θ_１）−₁,S₆＝Kp（θ′・r₆−θ_６）
−_６に応じて出力Ｕが変化するショートステップリレ
ーであって、該ショートステップリレー（10）はその枠
内に図示されるように、正側リニア部と、負側リニア
部、上記両リニア部間で出力が非連続的に変化するステ
ップ部と、出力が一と飽和正値となる定常正値領域と、
出力が一定の飽和負値となる定常負値領域とからなる信
号を、その出力平均値まわりに移動させた出力U₁,U₆を
生成するものである。また、（11）はモーター電流の位
相遅れを補償する補償回路、（12）は制御信号U₁,U₆と
上記補償回路（11）の出力信号Uc₁,Uc₆の代数和を演算
して、最終的に後述のサーボモータ（18）を駆動するた
めの目標電流値I₁,I₆を算出する第３加え合せ点であ
る。

次に、（18）はコイルインダクタンスL₁,L₆,内部抵抗
R₁,R₆,トルク定数K₁,K₆を有するVR可変形のステッピン
グモータからなるサーボモータであって、該サーボモー
タ（18）は、第１図に示すように、回転速度に対して
２次関数的にトルクが減小するようなトルク特性を有し
ている。また、（14）は、上記サーボモータ（18）の目
標電流値I₁,I₆およびタコジェネレータ（VS）で検出さ
れたサーボモータ（18）の回転速度（角速度）₁,_６
に基づき、PWM方式により、サーボモータ（18）を制御
するためのパルス状制御信号を出力するマイクロコンピ
ュータ、（15）は該マイクロコンピュータ（14）のパル
ス信号に応じて、サーボモータ（18）の制御電流を出力
するトランジスタ装置である。

そして、（Ｆ）はアームの対応する関節の位置を制御す
る関節ダイナミックスであって、該関節ダイナミックス
（Ｆ）は、上記サーボモータ（18）のトルクを関節の角
角速度₁,_６に変換する慣性モーメント要素（19）
と、該慣性モーメント要素（19）の角加速度₁,_６を
積分して関節の角速度₁,_６を得る第１積分要素（2
0）と、該第１積分要素（20）の角速度₁,_６を積分
して関節角度θ₁,θ_２を得る第２積分要素（21）とから
なり、該各第２積分要素（21）の出力値である関節角度
θ_１〜θ_６はそれぞれアーム（22）に入力されて、三次
元座標系（X,Y,Z）に変換されるようになされている。

ここで、本発明の特徴として、上記プランナ（PL）で
は、以下のようにして、基本的な速度計画が立案され
る。

すなわち、第５図に示すごとく高速回転側で２次関数的
にトルクが減小するようなトルクＴ−速度特性を有す
るモータ（18）を使用する場合、トルクＴと角速度と
の関係を下記式 ＝（ω₀/T₀ ²）（Ｔ−T₀）（Ｔ＋T₀） （１） （ただし、T₀,ω_０は、それぞれ特性曲線とＴ軸，θ１
軸との交点における値、つまりトルクＴおよび角速度
の最大値）で近似する。

ここに、 Ｔ＝Ｉ・（Ｉは関節の慣性モーメント） であるから、結局次の方程式 ＝（T₀/）（１−／ω_０）^1/2 （２） を得る。上記微分方程式をについて解くと、 ＝Ｋ・ω_０・ｔ（１−Ｋ・t/4） （３） （ただし、Ｋ＝T₀/I・ω_０、ｔは時間である）となる。

また、角加速度は ＝Ｋ・ω_０（１−Ｋ・t/2） （４） となる。

第６図は、上記（４）式による角速度−時間ｔの関係
を示し、ゼロ点を通って時間ｔが2/Kの時に最大値をと
る放物線となる。

また、モータ（18）のトルク特性が第７図実線に示すよ
うな場合、モータ（18）のトルクＴて角速度の関係を
近似的に直線とみなす（同図破線の直線）ことができる
ので、下記直線関数 ＝（ω₀/T₀）（T₀−Ｔ） （５） により、トルク特性を近似する。すなわち、次の方程式 ＝−（Ｉ・ω₀/T₀）＋ω_０ （６） を解くことになり、上記微分方程式をについて解く
と、 ＝ω_０｛１−exp（−Ｋ・ｔ）｝ （７） を得る。

第８図は（７）式による角速度−時間ｔの関係を示
し、最終的に収束する値ω_０に対して、時定数1/Kを有
するような立上がり特性を示すものとなる。なお、減速
時にも加速時の角速度の変化特性をそのまま使用する
ようにしている。

以上のように、モータ（18）のトルク特性に応じて、放
物線状、直線状等の関数により加減速次の角速度変化特
性を近似させて速度計画を立案するようにしている。

次に、第１図の特性を有するモータ（18）について、具
体的に速度計画を立案する方法を説明するに、まず、上
記モータの特性から使用するべき最大角速度ωmaxを定
め、その角速度ωmaxを定速部として選択する。そし
て、モータ（18）のトルク特性は放物線関数で近似され
角加速度の減小率は一定であるから、その減小率の値
をｋとし角加速度の初期値をa₀とすると、一定角速度
ωmaxに達するまでの時間τは、 τ＝２ωmax/a₀（２−ｋ） （８） となり、角速度の方程式は、 ＝ｋ・a₀・t²/2τ＋a₀・ｔ （９） となる。また、上記（９）式より、角加速度は ＝−ka₀・t/τ＋a₀ （10） となる。

つまり、ｔ秒後の移動距離ｆ（ｔ）は ｆ（ｔ）＝（−ka₀/6）（t³/τ）＋（1/2）a₀t² （11） となって、一定速度に達するまでの移動距離は ｆ（τ）＝（−ka₀/6＋a₀/2）τ^２（12） により求められる。すなわち、第２図上図実線（ｉ）に
示すような放物線状の角速度の変化特性と、第２図下
図実線（ii）に示すような角加速度の変化特性とが得
られることになる。

また、減速時の角速度の変化特性も、上記加速時と同
様の（つまり対称的な）パターンで得られる。

以上により、加速と減速のパターンを同じとすると、速
度計画に基づく基本的な速度線図が求まることになる。
その場合、全移動距離をｌとすると、ｌ≧２・ｆ（τ）
のときには角速度が一定の定速部が存在し、第３図上
図に示すような角速度が放物線状に増加する加速部
（Ａ）と、一定の角速度ω_０（＝ωmax）を示す定速部
（Ｂ）と、角速度が放物線状に減小する減速部（Ｃ）
とからなる速度計画線図となる。

ここで、定速部の期間t_cは t_c＝｛ｌ−２・ｆ（τ）｝／ωmax である。

一方、ｌ≦２・ｆ（τ）のときには速度が一定の定速部
が存在せず、第３図下図に示すような加速部（Ａ′）と
減速部（Ｃ′）とからなる速度計画線図となる。

したがって、上記実施例では、ロボットのアーム手先の
移動する軌跡を算出するとき、従来のように角加速度
を一律に一定とするのではなく、モータ（18）のトルク
特性に応じ、トルクの変化特性に対応する関数で近似し
た角速度変化を生ずるようにしているので、最適な速度
計画を立案することができる。従来のように、例えば加
速部の加速値を一定とする場合には、定速部つまり最大
角速度ωmax時におけるモータ（18）のトルクＴの限界
値から加速値が定まるので、第２図上図破線（ｉ）′の
ように小さな加速値に設定しなければならず、一定速度
ωmaxに達するまでの時間が図中のτ′となる。それに
対し、本発明では、加速時の角速度変化特性をモータ
（18）のトルク特性に対応する所定の関数で近似してい
るので、第２図上図の実線（ｉ）に示すように、低速側
では加速値が大きく高速側では加速値が小さくなるよう
に変化することになり、一定角速度ωmaxに達するまで
の時間を上記τ′よりも小さな値τに短縮することがで
きる。また、高速回転側でトルクの減小に応じて回転数
が減することになり、振動等の不安定な状態を生ずるこ
とがない。さらに、モータ（18）のトルク特性の有効利
用により、モータ容量を小形化することができる。

加えて、従来の方法では、ロボットのアーム手先の移動
距離が短い場合、移動時間を短縮する目的で、加速値を
長距離時よりも大きく設定するなどの調節を頻繁に行う
必要があったが、本発明では、移動距離の長短に拘らず
一定のパターンをそのまま使用して速度計画を立案する
ことができ、簡易に制御を行うことができるなど、ロボ
ットの総合性能を向上させることができる。

なお、上記実施例では、モータ（18）のトルク特性を２
次関数（放物線）で近似した場合について速度計画線図
の作成方法を説明したが、上記直線関数による近似の場
合や、その他モータ（18）のトルク特性に応じた所定の
関数で近似した場合についても、基本的な速度計画を立
案して同様の効果を発揮することができる。

（発明の効果） 以上説明したように、本発明のロボット制御方法によれ
ば、ロボットのアーム手先の運動を制御する場合、ロボ
ットのアーム手先が移動すべき経路に対応して、モータ
のトルク特性を関数で近似し、この関数に沿ってモータ
の加減速パターンを導き出して速度計画を立案し、該沿
度計画に基づきモータの回転速度を加減速制御するよう
にしたので、振動等の不安定状態の発生を防止しながら
移動時間の短縮、モータの小形化、制御の簡易化などを
図ることができる。

【図面の簡単な説明】

第１図〜第８図は本発明の実施例を示し、第１図はモー
タのトルク特性を示す図、第２図上図はモータのトルク
特性に対応した速度計画線図、第２図下図はその速度計
画線図に対応する加速度の変化特性図、第３図上図およ
び下図はそれぞれ定速部が存在するときおよび存在しな
いときの基本的な速度計画線図のパターンを示す図、第
４図は制御装置の全体構成を示すブロック図、第５図は
モータのトルク減小特性に応じた２次関数による近似の
説明図、第６図は放物線近似によるロボットの速度変化
特性図、第７図はモータのトルク減小特性に応じた直線
関数による近似の説明図、第８図は該直線近似によるロ
ボットの速度変化特性図である。第９図は従来の方法に
よる制御時の速度計画線図である。 （18）……サーボモータ、（23）……サーボ制御装置、
（PL）……プランナ。

Claims (1)

【特許請求の範囲】
1. 【請求項１】回転数が高くなるほどトルクが減小する特
   性を有する関節駆動用モータの加減速を制御して、少な
   くとも停止状態から所定速度に達するまでの加速部及び
   所定速度から停止するまでの減速部を有するようロボッ
   トの動作を制御するロボット制御方法であって、 上記加速部及び減速部では、上記モータのトルク特性を
   所定の関数関係式で近似し、この関数関係式に沿って上
   記モータの加減速パターンを導き出してロボットの速度
   計画を立案し、該速度計画に基づきモータの回転速度を
   調節して、アーム手先の運動を制御することを特徴とす
   るロボットの制御方法。