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

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、ロボットの各関節に作用する力を数式モデル
により算出してこれをフィードフォーワード補償する、
いわゆる「動的制御法」を用いたロボット制御方法に関
するもので、特に、マイクロプロセッサを用いて制御演
算を行なうため、制御出力が１サンプリング周期分送れ
てしまう「演算遅れ」による精度低下を補償することが
できるようにしたロボットの制御方法に関するものであ
る。

〔従来の技術〕

ロボットアームの動力学に関する運動方程式は一般に下
記の（１）式で表わされる。

ｆ−ＩＭ（ｅ）・Φ十１１１（ｅ、Φ）＋「Ｏ＋ｇ　（
ｅ）　　　　　　　　　　　・・・（１）ただし、 ｆ：関節トルクベクトル ｅ：関節変位ベクトル ＩＭ（ｅ）：慣性行列 Ｉｈ　（８，◇）：遠心力コリオリカを表わすベクトル 「：粘性摩擦係数行列 ｇ　（ｅ）　　：重カベクトル （１）式でトルクｆを計算する際、ｅＳ白はロボットア
ームからのフィードバック信号を用い、ｄについては下
記の（２）式を用いる。

Ｑ−Ｑｄ＋ＩＫｄ　１ｄ−０）＋Ｎａ　（ｅｄ−ｅ）　
　　　　　　　　　　　・・・（２）ただし、 ＋９ｄ、○ｄ、Ｑｄ：関節目標位置、速度、加速度 ＩＫｄ、　ｎａｐ　：速度、位置フィードバックゲイン 以上のように数学モデルを用いて各関節に必要なトルク
を算出するフィードフォワード補償と、サーボ演算によ
るフィードバック補償を組合わせたものが、「動的制御
法」と呼ばれている。

このような制御演算は、マイクロプロセッサを用いて行
なわれるので、演算結果は一定のサンプリング周期毎に
出力される。

第４図は上記制御演算のタイムチャートを示す。サンプ
リング周期の始めでアームの現在位置θ（ｋ）・速度ｄ
　（ｋ）を入力し、上記（１）（２）式を用いて制御ト
ルクで（ｋ）を計算する。

この計算された制御トルク？　（ｋ）は第４図に示すよ
うに１サンプリング周期後に出力され、これが「演算遅
れ」である。ロボットの制御の場合、制御演算量が多い
ため、この演算遅れ時間はかなり大きく、加減速時には
特に大きな精度低下の原因となる。

第３図はこの従来の動的制御法のブロック図であり、目
標軌道演算部１から関節の目標の位置、速度、加速度θ
ｄＳθｄ、σｄがサーボ演算部２に入力され、このサー
ボ演算部２では上記（２）式を演算し、その結果を動的
補償演算部３に人力し、ここでは上記（１）式が演算さ
れる。ついで、この結果がアンプ４を経てロボットアー
ム５を駆動するモータ６に入力され、このモータ６を所
定の角度にわたって回動する。

一方、モータ６からは関節位置、速度θ、ｅがサーボ演
算部２と動的補償演算部３ヘフイードバツクされる。

〔発明が解決しようとする課題〕

上記動的制御法はアーム間の力学的干渉を考慮に入れた
制御ができるので、高速高精度化に効果を示すが、この
制御法は上記したように、演算遅れが生じるため、その
効果を十分に発揮できなかった。

本発明は上記のことにかんがみなされたもので、動的制
御法における演算遅れを補償することができるロボット
制御方法を提供することを目的とするものである。

〔課題を解決するための手段〕

上記目的を達成するために、本発明に係るロボット制御
方法は、ロボットアームの各関節に作用する力を数式モ
デルにより算出し、フィードフォワード補償する動的制
御法を用いたロボットの制御方法において、サンプリン
グ周期毎に出力する制御トルクと、入力するロボットア
ームまたはアクチュエータの位置・速度より、次回サン
プリング周期のロボットアーム、またはアクチュエータ
の位置・速度を推定し、次回サンプリング周期の目標値
と上記推定値を用いて制御トルクを算出し、次回サンプ
リング周期にこれを出力する。

〔作　用〕

入力トルクに対するロボットアームの位置・速度が離散
時間系での状態方程式の形で表わされ、この式を用いて
マイクロプロセッサにより、リアルタイムで１サンプリ
ング周期後の状態（位置・速度）が推定され、これが制
御演算に用いることにより演算時間遅れが解決される。

〔実　施　例〕

本発明の実施例を第１図・第２図に基づいて説明する。

第１図は本発明方法を示すブロック図であり、このブロ
ック図は第３図に示す従来の方法を示すブロック図のフ
ィードバック回路に離散時間系状態方程式の演算部７が
追加されている。なお上記両ブロック図はロボットの動
的制御法の１軸分のものを示す。

状態方程式は駆動系への制御出力トルクτに対する検出
器でのアクチュエータあるいはロボットアームの位置・
速度状態量Ｘを表わすもので、サンプリング周期毎に更
新される制御出力トルクτ及び位置・速度人力信号より
次回のサンプリング周期での位置・速度を推定するため
のものである。また、マイクロプロセッサで処理しやす
いように、サンプリング周期による離散時間表現を用い
ており、（３）式のような差分方程式表現となる。

Ｓｔ　（ｋａｔ）−ＩＰＸ　（ｋ）　十Ｑｒ　（ｋ）・
・・（３） ただし、 ７（ｋ）：に番目のサンプリング周期の計測量（位置・
速度） τ　（ｋ）：に番目のサンプリング周期での制御出力ト
ルク Ｘ　（ｋａｔ）：　　（ｋａｔ）番目のサンプリング周
期の推定量（位置・速度） Ｐ：係数行列 Ｑ、係数ベクトル なお上記ＩＰＳＱは共にサンプリング周期ΔＴの関数で
ある。

ｋ番目のサンプリング周期において、推定した（ｋａｔ
）番目のサンプリング周期の位置・速度信号と、同サン
プリング周期での目標軌道より制御出力トルクを計算し
、（ｋａｔ）番目のサンプリング周期にこれを出力する
ことにより演算遅れを補正する。

上記方法をさらに具体的に示すと、ｋ番目のサンプリン
グ周期において、まず、ロボットアームの位置・速度Ｘ
　（ｋ）を入力する。このとき Ｘ　　（ｋ）　　−［θ　（ｋ）　　　　ビ９　　（ｋ
）　　コ　０であり、Ｔは転置を意味する。

Ｘ　（ｋ）とに番目で出力する制御トルクτ（ｋ）より
（３）式を用いて（ｋａｔ）番目のロボットアームの位
置・速度Ｘ（ｋａｔ）を推定する。

２（ｋａｔ）　　−［＃　　（ｋａｔ）　　Ｊ　　（ｋ
　　＋　１）　　］　　Ｔこの推定値と（ｋａｔ）番目
の目標軌道よりサーボ演算部２において、（４）式のよ
うにサーボ演算を行なう。

σ　（ｋａｔ）　　−＃　　ｄ　　（ｋａｔ）　　＋　
　Ｋ　　ｄ　　［θ　ｄ　　（ｋａｔ）Ｊ　（ｋａｔ）
　　コ　＋　Ｋｐ　　Ｅ　θ　ｄ　　（ｋａｔ）σ（ｋ
ａｔ）　］　　　　　　　　・・・（４）次に動的補償
演算部３で（５）式の演算を行ない、（ｋａｔ）番目の
制御トルクτ（ｋａｔ）を求める。

τ（ｋａｔ）　−Ｍ　［σ（ｋａｔ）　］　　・σ（ｋ
ａｔ）　＋ｈ［（σ（ｋａｔ）　　ぎ（ｋａｔ）　］　
＋ｒ　δ（ｋａｔ）　　＋　　ｇ　　［σ（ｋａｔ）　
　コ・・・（５） 以上の計算をに番目のサンプリング周期間で行ない（ｋ
ａｔ）番目のサンプリング周期の始めに制御トルクτ（
ｋａｔ）を出力することにより演算の時間遅れが補償さ
れる。

上記作用のタイムチャートを示すと第２図に示すように
なる。

〔発明の効果〕

本発明によれば、マイクロプロセッサを用いた制御装置
特有の問題点の１つである演算時間遅れの問題を解決す
ることができ、マイクロプロセッサを用いた制御装置で
の精度低下を改善できる。

また演算時間遅れの補償は、次回サンプリング周期での
位置・速度を推定することにより行なうが、機械系の特
性を離散時間系での状態方程式の形で表わして位置・速
度を推定するので、演算量が少なくてすみ、リアルタイ
ム処理が可能である。

【図面の簡単な説明】

第１図、第２図は本発明の実施例を示すもので、第１図
はブロック図、第７２図はタイムチャートである。第３
図、第４図は従来例を示すもので、第３図はブロック図
、第４図はタイムチャートである。 １は目標軌道演算部、２はサーボ演算部、３は動的補償
演算部、４はアンプ、５はアーム、６はモータ、 ７は離散時間系状態方程式の演算 部。

Claims (1)

【特許請求の範囲】
1. ロボットアームの各関節に作用する力を数式モデルによ
   り算出し、フィードフォワード補償する動的制御法を用
   いたロボットの制御方法において、サンプリング周期毎
   に出力する制御トルクと、入力するロボットアームまた
   はアクチュエータの位置・速度より、次回サンプリング
   周期のロボットアーム、またはアクチュエータの位置・
   速度を推定し、次回サンプリング周期の目標値と上記推
   定値を用いて制御トルクを算出し、次回サンプリング周
   期にこれを出力するようにしたことを特徴とするロボッ
   トの制御方法。