JPS63276607A

JPS63276607A - マニピュレータ装置

Info

Publication number: JPS63276607A
Application number: JP9982687A
Authority: JP
Inventors: Ritsu Aoki; 青木　立; Taro Iwamoto; 太郎岩本; Yoshio Nakajima; 吉男中島; Hiroshi Yamamoto; 広志山本
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 1987-04-24
Filing date: 1987-04-24
Publication date: 1988-11-14
Anticipated expiration: 2010-07-31
Also published as: JPH0769734B2

Abstract

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

Description

【発明の詳細な説明】〔産業上の利用分野〕本発明は、マニピュレータの座標変換装置に係り、特に
、逆変換演算式が複雑で数式的に解けない場合に高速に
逆変換を行うのに好適な座標変換装置に関する。

〔従来の技術〕

マニピュレータが行うべき作業を記述するのに関節角座
標系は不便であり、一般に、ベースに設定した直交座標
系（基準座標系）に対して作業を記述する。一方、マニ
ピュレータは各関節ごとにサーボ制御するので、マニピ
ュレータの作業は関節座標系で記述する必要がある。そ
こで、基準座標系に対して求められたエンドエフェクタ
の位置及び姿勢から各関節角を求める逆変換が必要とな
る。マニピュレータの関節角を８．先端の位置及び姿勢
をＸとすると、一般に、次式の関係がある。

Ｐ＝Ｘ　（θ）　　　　　　　　　　　　　・・・（１
）この演算を正変換と言う。

ここで、Ｘは、アームの軸構成、形状によって定まる関
数である。式（１）より、マニピュレータの各関節角θ
が与えられると先端の位置及び姿勢は一意に定まる。マ
ニピュレータを制御するためには、与えられた先端の目
標点Ｘに対し逆変換すなわち、 θ＝Ｘ−１（Ｐ）　　　　　　　　　　　　　　　　　
・・・（２）を行い目標関節角を求め制御する必要があ
る。特別な機構のアームに対しては式（２）の解を数式
的に求めることができるが、一般には、数式的に求まら
ない。産業用ロボットでは、多くの場合、数式解が求ま
るようなアームの構造を採用している。

一方、多自由度を有するマニピュレータにおいては、高
機能動作を高速で実行させるために、特開昭６１−９４
１０６号公報に記載されているものがある。

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

上記従来技術では、ヤコビ行列の要素に０を多くし、演
算式の項の数が極力少なくなるように軸構成をとる必要
があり、アーム設計上の大きな制約となっていた。また
、任意の軸構成、形状のアームに対し、逆変換演算式が
求まる保証がないため、制御装置の汎用性が損われると
いう問題があった。

本発明の目的は、任意の軸構成、形状のアームに対して
高速に逆変換を行うことができるマニピュレータの座標
変換装置を提供することにある。

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

本発明の上記の目的は、マニピュレータの先端の座標を
変換するものにおいて、マニピュレータの先端の目標値
を入力する目標値入力部と、前記目標値と現在値との偏
差を求める比較部と、その偏差をマニピュレータの先端
にかかる力に変換する比例ゲイン部と、前記力と静力学
的に釣り合うマニピュレータの関節トルクを求めるため
のヤコビ行列演算部と、前記関節トルクを関節角に変換
する力学的モデル部と、正変換演算部とを備えることに
より達成される。

〔作用〕

比較部は目標値と現在値との偏差を求める。この偏差は
比例ゲイン部によって、マニピュレータの先端にかかる
力に変換される。この力はヤコビ行列演算部によって関
節トルクに変換される。この関節トルクは力学的モデル
部によって関節角に変換される。この結果、任意の軸構
成、形状のマニピュレータに対して関節角を高速に変換
することができる。

〔実施例〕

以下本発明の実施例を図面を用いて説明する。

第１図は本発明の装置の一実施例を備えたマニピュレー
タの制御装置を示すもので、この図において、１はマニ
ピュレータ、２はマニピュレータ１のエンドエフェクタ
、３Ａ〜３Ｃはマニピュレータ１の各関節部ＩＡ〜ＩＣ
を駆動するモータ、４Ａ〜４Ｃは多関節部ＩＡ〜ＩＣの
関節角を検出するエンコーダ、５はエンドエフェクタ２
の目標位置、姿勢等の目標値を入力する目標値入力部、
６は本発明に係る座標変換装置、７はマニピュレータ１
の駆動制御装置で、この駆動制御装置７は各関節部ＩＡ
〜ＩＣに対応して目標関節角と実際の関節角とを比較す
る比較部７０Ａ〜７０Ｃと、比例ゲイン部７１Ａ〜７１
Ｃと、Ｄ／Ａ変換器７２Ａ〜７２Ｃと、パワーアンプ７
３Ａ〜７３Ｇとで構成されている。

前述した座標変換袋Ｎ６の一例の構成を第２図を用いて
説明する。この第２図において第１図と同符号のものは
同一部分である。６０は比較部で、この比較部６０はエ
ンドエフェクタ２の目標位置及び姿勢の目標値と、現在
の位置及び姿勢の実際値とを比較する。６１は比例ゲイ
ン部、６２はヤコビ行列演算部、６３は力学的モデル部
、６４は正変換演算部で、この正変換演算部６４は関節
角を基準座標系値に変換する。前述した力学的モデル部
６３は比較部６３１と、慣性要素６３２と、積分要素６
３３，６３４と、粘性要素６３５とで構成されている。

この力学的モデル部６３により関節角が求められる理由
に関して説明する。

マニピュレータの先端に加わる力とモーメントをｆとす
ると、このｆと平衡状態を保つためにマニピュレータの
関節部に加えなければならないトルクτは、次式で与え
られる。

τ＝Ｊ”ｆ　（−Ｔ”；ヤコビ行列の転置行列）・・・
（３）第２図に示すようにエンドエフェクタ２先端の目
標位置及び姿勢の目標値と現在の位置及び姿勢の実際値
の偏差ｄＰに比例した力ｆを先端に加えるとすると、式
（３）により、各関節部にトルクτを発生させなければ
ならない。ここで、第２図に示すように、マニピュレー
タの力学モデルとして各関節部に慣性モーメントＭ４．
粘性係数ＣＪ　を持つモデルを導入する。各関節部にト
ルクτを加えたときの運動方程式は次式で表わされる。

Ｍθ＋Ｃθ＝τ　　　　　　　　　　　　・・・（４）
τ（τ工・・・τＢ）Ｔ；関節トルクそして、式（４）を２階積分することにより関節角θを
求めることができる。

次に上述した本発明の一実施例の動作を、第３図を用い
て説明する。

座標変換装置６には目標値入力部５からエンドエフェク
タ２の先端の位置及び姿勢の目標値’５Ｃｒ　ｅ　＊が
与えられる（処理３０）。次に関節角θの初期値として
関節角の現在値が与えられる（処理３１）。

次に、正変換演算部６４は現在の関節角θに基づいて正
変換によりエンドエフェクタ２の先端の位置、姿勢の実
際値Ｘを求める（処理３２）。次に、比較部６０は目標
値ｘｒｅｉと実際値Ｘとの偏差ｄｘを求める。この偏差
ｄｘは比例ゲイン部６１によりに倍されて、エンドエフ
ェクタの先端にがかる力ｆを求める（処理３３）。そし
て、前述した偏差ｄｘは通常許容値より大きいので（処
理３４）、前述した力ｆはヤコビ行列演算部６２におい
て、関節角θに基づいて求められたヤコビ行列Ｊの転置
行列（処理３５）と前述した式（３）とにより、各関節
部に発生させるべきトルクτを求める（処理３６）。次
に、力学的モデル部６３は式（４）とトルクτとにより
、関節角θを求める（処理３７）。この関節角θを第２
図に示す正変換演算部６４により、エンドエフェクタ２
の先端の位置Ｘを求め、これを、第２図に示す比較部６
０にフィードバックして偏差ｄｘが許容値内に納まるま
で繰り返される。そして、偏差ｄｘが許容値以下になっ
たときの関節角θは第１図に示すマニピュレータ１の駆
動制御装置７に入力される。

この駆動制御装置７はその比較部７０Ａ〜７０Ｃにより
、求めた関節角θとエンコーダ４Ａ〜４Ｃにより検出し
た現在の関節角との偏差を求め、その偏差に比例ゲイン
部７１Ａ〜７１．　ＣによりゲインＫを乗じたものをモ
ータ３Ａ〜３Ｃに出力する。

これにより、エンドエフェクタ２は目標値に一致するよ
うに位置決めされる。

前述したように、本発明においては、偏差ｄｘが許容値
以下になるまで繰り返し演算されるが、整定するのに必
要な繰り返し回数を、アームの姿勢に応じて最小にする
ように、比例ゲイン部６１のゲインＫを設定すればよい
。すなわち、まず、アームの動作領域をいくつかの小領
域に分割し、各々の小領域内で、その領域内での位置、
姿勢を代表する点を１点ずつ設定する。次にシミュレー
ションにより、それぞれの点に関し、繰り返し回数が最
も少なくなるゲインＫを求め、演算装置内に予めテーブ
ルとして格納しておく。アーム先端の目標値が与えられ
たとき、それがどの領域に当たるかを不等式により判断
し、その領域に対するゲインをテーブルにより求めるこ
とにより可能である。

また、ゲインを最適に調整する方法として、ヤコビ行列
の各成分の値に基づいて、ゲインＫを調整する方法も可
能である。

次に、本発明の他の実施例を第４図を用いて説明する。

この実施例は力学的モデル部６３を、第２図に示す力学
的モデル部６３における積分要素６３４を取り除いて構
成したものである。この力学的モデル部６３は次式で表
わされる。

阿θ十Ｃθ＝τ　　　　　　　　　　　　　・・・（５
）τ　（τ１・・・τ６）Ｔ；関節トルクこの実施例に
よれば、前述した式（５）に示されるように、１階積分
することにより関節角θが求まるので、関節部のトルク
を関節角に変換することができる。これにより、積分に
要する演算時間の分だけ演算時間を短縮できる。

次に、本発明のさらに他の実施例を第５図を用いて説明
する。この実施例は力学的モデル部６３を、ゲイン要素
６３５と、比較部６３６と、時定数要素６３７と、積分
要素６３８とで構成したものである。前述した時定数要
素６３７は次のように表わされる。

ｊ＝１〜６Ｍｌ；慣性要素Ｃに粘性要素この実施例によれば、慣性要素、粘性要素を用いた場合
に比べ、演算回数が少なくなるので、より高速に逆変換
を行うことができる。

〔発明の効果〕

以上述べたように、本発明によれば、任意の軸構成、形
状のアームに関し、逆ヤコビ行列を求めることなく高速
に逆変換を行うことができる。従って、演算時間を逆ヤ
コビ行列法に対し大幅に短縮できる。

【図面の簡単な説明】第１図は本発明の装置を備えたマニピュレータの制御装
置を示す図、第２図は本発明の装置の一実施例の構成を
示すブロック図、第３図は本発明の装置の動作を説明す
るためのフローチャート図、第４図および第５図はそれ
ぞれ本発明を構成する力学的モデル部の実施例を示すブ
ロック図である。１・・・マニピュレータ、２・・・エンドエフエタク、
３Ａ〜３Ｃ・・・モータ、４Ａ〜４Ｃ・・・エンコーダ
、５・・・目標値入力部、６・・・座標変換部、７・・
・駆動制御装置。

Claims

【特許請求の範囲】１、マニピュレータの先端の座標を変換するものにおい
て、マニピュレータの先端の目標値を入力する目標値入
力部と、前記目標値と現在値との偏差を求める比較部＃
とその偏差をマニピュレータの先端にかかる力に変換す
る比例ゲイン部と、前記力と静力学的に釣り合うマニピ
ュレータの関節トルクを求めるためのヤコビ行列演算部
と、前記関節トルクを関節角に変換する力学的モデル部
と、正変換演算部とを備えたことを特徴とするマニピュ
レータの座標変換装置。２、比例ゲイン部はマニピュレータの姿勢に応じてその
ゲインを最適に調整することを特徴とする特許請求の範
囲第１項記載のマニピュレータの座標変換装置。３、力学的モデル部は、比較部と、慣性要素と、粘性要
素と、少なくとも１つの積分要素とで構成したことを特
徴とする特許請求の範囲第２項記載のマニピュレータの
座標変換装置。４、力学的モデル部は、ゲイン要素と、比較部と、時定
数要素と、積分要素とで構成したことを特徴とする特許
請求の範囲第２項記載のマニピュレータの座標変換装置
。