JPS6346521A - 物体の位置制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 技術分野 本発明は、産業用ロボットなどのような剛性の低い物体
の位置指令値に対する追従性を良くし、その結果、軌跡
精度の向上を図ることのでさる産業用ロボットなどの物
体の位置制御方式に閃する。

背景技術 ロボットを高速で動作させると、サーボ系の追従誤差が
天外くなり、軌跡精度が悪化する。これはロボットの各
関節にリンク相互の千渉力をはじめ、遠心力、フリオリ
力、重力、摩擦力等が作用するためである。これらの力
を数式モデルより算出し、フィードフォワード補償する
制御方式は一般ｌ：動的制御と呼ばれる。この制御方式
は数式モデルが系を完全に表現しているならば、系の１
１Ｊ　ｖｔ性改善に著しい効果をもたらし得る。しかし
ながら従来の動的制御は、ｆ！ｉｉ制御討宋のロボット
を剛体モデルで記述しているため、モデルと実際の系と
の誤差が太き（、動的制御本来の威力を発揮していると
はいえない、すなわち、現在の多関節ロボットは、アー
ム自身お上びモータとアーム間のトルク伝達機構の剛性
が低く、これがロボットの移動、停止の際の残留振動を
誘起している。

発明が解決すべき問題点 本発明の目的は、電動機から物体へ動力を伝達する動力
伝達機構などにおける低剛性部分に起因する加減速時の
振動を抑制することができるようにした物体の位置制御
方式を提供することである。

問題点を解決するための手段 本発明は、電動機の回転子と、位置制御されるべき物体
と、回転子から物体に動力を伝達する動力伝速Ｉｆｓ、
横とによって構成される輪糸を、ばねと剛体とから成る
系と仮定して数式モデルを作成し、物体の位置指令値を
、数式モデルに代入して求めた′Ｉｆ１ｔｉｌ指令値Ｕ
を電動機に与え、電動機を位置制御することを特徴とす
る物体の位置制御方式である。

また本発明は、電動機の回転子と、位置制御されるべ５
物体と、回転子から物体に動力を伝達する動力伝達機構
とによって構成される軸系な、ばねと剛体とから成る系
と仮定して数式モデルを作成し、 物体の位置指令値を数式モデルに代入して求めた電流指
令値Ｕを電動機に与え、 電動機の位置測定値θｍと、電動機の速度測定値θ薯と
をＩＸ帰還し、 電流指令値りと速度測定値−纏とを用いて、ばねに起因
した電動機回転子と物体との間の盃み量θｓと、その歪
み量θｓの時間変化率９ｓとを推定するオブザーバを設
け、歪み量θ３とその歪み１の時間変化率θｓとを負帰
還し、 物体の位置指令値（θａ）ｒに基づいて、電動機の出力
軸の位置指令値（θ−）ｒと、その出力輪の速度指令値
（々ｍ）ｒおよび歪み量の指令値数（θ３）ｒ、歪み量
の時間変化率の指令値（θ５）ｒを数式モデルによって
求め、これらの位置指令値く０ｍ）ｒ、速度指令値（θ
−）ｒおよび歪み量の指令値（θｓ）ｒ、歪み量の時間
変化率の指令値（Ｍｓ）ｒを前述の電流指令値Ｕに加え
ることを特徴とする物体の位置制御方式本発明に従えば
、電動機の回転子ないしは出力軸と物体との間に介在さ
れている動力伝達機構を備える軸系を、ばねと剛体とか
ら成る系と仮定して数式モデルを作成し、物体の位置指
令値を数式モデルに代入して求めた電流指令値Ｕを電動
機に与えて電動機を位置制御するようにしたので、物体
の加減速時における振動のない、しかも追従性の優れた
応答を実現することが可能となる。

また本発明では、電流指令値Ｕと電動機の速度測定値−
纏とを用いて、歪み量θｓとその歪み量θｓの時間変化
率ｆｊｓとを推定する最小次数オブザーバを用いて、サ
ーボ補償ループを併用することによって、系のパラメー
タが変動したときの安定性と信頼性を補償することがで
きる。

実施例 第１図は、本発明の一実施例の構成を簡略化して示す斜
視図である。電動機１は、駆動回路２の電流指令値Ｕに
より駆動され、その回転子ないしは、出力軸１ａは減速
機５に取付けられる。減速機５の出力軸は、トルク伝達
ｆｉｖ１６を通じてロボットのアーム７につながる。第
１図の構成を以下に制Ｗ討束と称する。

１２図は、ＩＩｔＪ１図の構成の機械的に等価な構成を
示す斜視図である。減速機５、トルク伝達機構６、およ
びアーム７の有するばね特性に着目すれば、Ｐｔ５１図
の制御対象はモータ１の出力軸１ａに結合されたばね９
と、ばね９の先端に結合された剛体７−ム１０から成る
構成と等ｔｉである。ロボットの各軸をｔＩ４２図に示
すようなばねｍｍ体系とみな１、たとき、ｎ自由度の多
ｒｍａロボットの第ｉアームの運動は以下の微分方程式
で記述される。

Ｋｅｉ（（θｍｉ／ＲＧｉ）−θａｉ）Ｄｍｉｅｍｉ　
　　　　　　　　・＝（２）上記の第１式とｔｌＳ２式
の各記号の意味は、以下のとおりである。ただし以降で
は全て第１７−ムを意味する添字のｉは省いて説明する
。

θ箇　；　電動機回転角度 ｅｖＩ　；　　電動機回転角速度 ｄ−；　電動機回転角加速度 θａ　；　アーム回転角度 Ｄａ　；　アーム回転角速度 ｊａ　；　アーム回転角加速度 Ｋｃ　；　ばね定数 ＲＧ　；　減速比 Ｄ−；　電動機側粘性摩擦係数 Ｄａ　；　アーム側粘性Ｊ？擦係数 、Ｉ−；　電動機慣性モーメント Ｎ曽　；　　電動機出力トルク ＪＪ　　ｗ　　慣性行列の成分 ｂｊｋ　　；　　遠心力・フリオリ力の項の係数ｒｇ　
　：　　！ｎ力項 動的制御に必要なＹルクを求めるには、アーム１０の加
速度指令値（θａ）ｒ、速度指令値（Ｄａ）ｒおよび位
置指令値（θａ）ｒより電動機出力トルク指令Ｉｔ１（
Ｎｓ）ｒを打出すればよい。添字「は指令値を表わす、
Ｐｔ４１式の右辺をｆ（θ）とおけば、第１式と第２式
とより、 ＋Ｄ−（Ｄ−）ｒ　　　　　　　　　・・べ３）が成立
する。「（θ）の各項は指令値が与えられれば、これを
θａ、θａ、θａに代入すれば既知となるから、あとは
電動機１の回転角度指令値、回転角速度指令値および回
転角加速度指令値（Ｄ−）ｒ、（Ｄ−）ｒ＝（θ■）ｒ
を求める必要がある。これは第１式を微分していくこと
により得られる。

（０ｍ）ｒ＝（ｆ（θ）／Ｋｃ＋（θａ）ｒ）ＲＧ　　
　−（４）（０ｍ）ｒ＝（ｆ（θ）／Ｋｅ＋（θａ）ｒ
）ＲＧ　　　−（５）（０ｍ）ｒ＝（ｆ（θ）／　Ｋ　
ｃ　＋　（θａ）ｒ）ＲＧ　　　−（６）第４式、第５
式および第６式を第３式に代入すればトルク指令値は求
まる。

さＣ）に本発明の他の実施例では第３図のように状！！
観測器１１を用いて、電動機１の出力軸１ａとアーム１
０間のばね要素の盃み、およびその微分すなわち時間変
化率のフィードバックループを構成しているため、歪み
およびその微分の指令値を計算しフィードフォワードす
る必要がある。この状態観測器１１は、電流指令値Ｕと
電動機１の速度測定値θ騰とを用いて、歪み量θｓとそ
の歪み社θｓの時間変化率Ｊｓとを推定する最小次数オ
ブザーバを用いて実現することができる。歪み１をθｓ
とおくと、 と表されるので、第１式を変形すると （θ５）ｒ＝ｆ（θ）／Ｋｃ　　　　　　　　　−（８
）が得られる。さらにこれを微分して、 （＃５）ｒ＝ｆ”（θ）／Ｋｃ　　　　　　　　　　・
＝（９）を得る３以上より、動的制御に必要なトルク指
令値と、速度、歪みお上りその微分の各フィードバック
量の指令値を加え合せた全フィードフォワード量（Ｎｍ
ｆｂは第１０式により与えられる。

（Ｎ　ｍｆ）ｒ＝　（Ｎ　ｍ）ｒ＋　　Ｋ　ｖ（０ｍ）
ｒ＋　　Ｋ　ｐｓ（θ　ｓ）ｒ＋Ｋｖｓ（θ５）ｒ ＝ｒ（θ　）／ＲＧ＋ＪｍＲＧ（θａ）ｒ＋（Ｋｖ＋　
Ｄ　ｗ）ＲＧ　（９ａ）ｒ＋　［Ｊ　ｍＲＧ　ｒ（θ　
）＋（（Ｋ　ｖ＋　Ｄ　ｍ）ＲＧ　　＋　　Ｋ　ｖｓ）
ｆ（θ　）＋Ｋｐｓｆ（θ）１／　Ｋ　ｃ　　　　　　
−（１０）ここに、Ｋｖ、Ｋｐｓ、Ｋｖｓは、係数回路
１２，１３゜１４におけるそれぞれ速度、歪みおよび歪
みの微分の各フィードバックデインである６演キ部１５
に（θ、）、、（θａ）ｒ、（θａ）ｒが与えられたと
きに、これらを第１０式に代入して得られる値を、状態
観測器１１を用いたフィードバック制Ｏ１系の駆動回路
入力に加算することにより、本発明の制御系を構成する
ことができる。この制御系のブロック図を第３図に示す
、Ｕは駆動回路入力、Ｋｐは位置ループデインである。

参照符Ｉ　Ｇ、１７．１８もよゾ１９は、演算器を示し
、参照符２０はデインＫ。

を有する係数器であり、機構２１は第１図に示される電
動機１、減速機５、トルク伝達機構６およびアーム７な
どを含む機械的？＋’ｊ成を示す。この第３図において
フィードフォワードｇ１（Ｎｍｆ）ｒは、第１１式で示
される。

（Ｎ　ｍｌ）ｒ＝　（Ｎ　ｍ）ｒ＋　　Ｋ　ｖ（ｆｌ　
　ｍ）ｒ＋　　Ｋ　９！ｌ（θ　ｓ）ｒ十Ｋ　ＶＳ（θ
ｓ）ｒ　　　　　　　　　−（１１）なお、第１０式中
、値が小さく全体への影響が少ない項Ｄ１１などを無視
して計算を簡略化することも可能である。

またここでは電動機の回転角度、回転角速度および回転
角加速度の各指令値（０ｍ）ｒｓ（９ｍａｒ、（０輪）
ｒを計算により求めたが、検出器によって測定可能なθ
−２θ鴫の実際値を用いて、動的制御に必要なトルク指
令値をオンラインで、より正確に求めることもできる。

さらに制御対果のバラ７−タの同定が十分正確であれば
、さらに他の実施例を示す第４図のように、サーボ補償
器として状態ｔ！を測器１１を用いなくても、第４図の
ように従来の０ｍ、　　θｍのフィードバックだけでよ
い、このと慇は、ばねに↓る歪みｌおよびその微分値θ
ｓ、θｓのフィードバックループを辺り、これらの指令
値Ｋ　１１９（θｓ）ｒ＋Ｋ　ｖｓＩｓ）ｒを零にし、
フィードフォワードｌ（Ｎ＋ｄ）ｒを第１２式のとおり
とすればよい。

（Ｎ　ｍｒ　ｂ＝　（Ｎ　ｍ）ｒ＋Ｋ　ｖ（ｅ　ｍｂ　
　　　　　　−（１２）効　　果 以上のよ）に本発明によれば、産業用ロボットの動力伝
達部などの低剛性部分に起因する加減速時の振動を抑制
する二とが可能となり、しかも追従性の優れた応答を実
現することができる。

【図面の簡単な説明】

第１図は本発明の一実施例の構成を簡略化して示す斜視
図、第２図は本発明のばね一慣性系のモデルを示す斜視
図、第３図は本発明の他の実施例のブロック図、第４図
は本発明のさらに池の実施例のブロック図である。 １・・・電動機、２・・・駆動回路、５・・・減速機、
６・・・トルク伝達機構、７・・・アーム、１１・・・
状！！！観測器、１２．１３，１４．２０−・・係ｒ！
Ｌ器、１５−・・演算部、１　６．１　　？、１　８．
１　９　　・・・演算器代理人　　弁理士　画数　圭一
部 第１図

Claims (2)

【特許請求の範囲】
1. （１）電動機の回転子と、位置制御されるべき物体と、
   回転子から物体に動力を伝達する動力伝達機構とによっ
   て構成される軸系を、ばねと剛体とから成る系と仮定し
   て数式モデルを作成し、 物体の位置指令値を、数式モデルに代入して求めた電流
   指令値ｕを電動機に与え、電動機を位置制御することを
   特徴とする物体の位置制御方式。
2. （２）電動機の回転子と、位置制御されるべき物体と、
   回転子から物体に動力を伝達する動力伝達機構とによっ
   て構成される軸系を、ばねと剛体とから成る系と仮定し
   て数式モデルを作成し、 物体の位置指令値を数式モデルに代入して求めた電流指
   令値ｕを電動機に与え、 電動機の位置測定値θｍと、電動機の速度測定値■ｍと
   を負帰還し、 電流指令値ｕと速度測定値■ｍとを用いて、ばねに起因
   した電動機回転子と物体との間の歪み量θｓと、その歪
   み量θｓの時間変化率■ｓとを推定するオブザーバを設
   け、歪み量θｓとその歪み量の時間変化率■ｓとを負帰
   還し、 物体の位置指令値（θａ）ｒに基づいて、電動機の出力
   軸の位置指令値（θｍ）ｒと、その出力軸の速度指令値
   （■ｍ）ｒおよび歪み量の指令値数（θｓ）ｒ、歪み量
   の時間変化率の指令値（■ｓ）ｒを数式モデルによって
   求め、これらの位置指令値（θｍ）ｒ、速度指令値（θ
   ｍ）ｒおよび歪み量の指令値（θｓ）ｒ、歪み量の時間
   変化率の指令値（■ｓ）ｒを前述の電流指令値ｕに加え
   ることを特徴とする物体の位置制御方式。