JP2798518B2 - マニピュレータの制御装置 - Google Patents
マニピュレータの制御装置Info
- Publication number
- JP2798518B2 JP2798518B2 JP3071295A JP7129591A JP2798518B2 JP 2798518 B2 JP2798518 B2 JP 2798518B2 JP 3071295 A JP3071295 A JP 3071295A JP 7129591 A JP7129591 A JP 7129591A JP 2798518 B2 JP2798518 B2 JP 2798518B2
- Authority
- JP
- Japan
- Prior art keywords
- manipulator
- force
- work handling
- control device
- target
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Description
【０００１】
【産業上の利用分野】本発明は，マニピュレータのワー
ク取扱い部を目標とする動力学インピーダンスで動作さ
せるためのトルクを上記ワーク取扱い部に与えるいわゆ
るインピーダンス制御を行う制御装置に係り，特にワー
ク取扱い部をその目標軌道に精度良く追随させることの
できるマニピュレータの制御装置に関する。
ク取扱い部を目標とする動力学インピーダンスで動作さ
せるためのトルクを上記ワーク取扱い部に与えるいわゆ
るインピーダンス制御を行う制御装置に係り，特にワー
ク取扱い部をその目標軌道に精度良く追随させることの
できるマニピュレータの制御装置に関する。
【０００２】
【従来の技術】上記したようなマニピュレータの制御装
置としては，特開平２−２０５４８９号公報に開示され
た装置がある。上記制御装置に対応する制御ブロック図
を図５に示す。同図において，マニピュレータの制御装
置１a は，マニピュレータのワーク取扱い部２０の目標
とする動力学的な挙動を表現する動力学インピーダンス
の設定部３を備えている。上記動力学インピーダンス設
定部３には，目標弾性係数行列４（ｋ），目標粘性係数
行列５（ｂ），目標慣性係数行列６（ｍ）が予め設定さ
れている。上記マニピュレータ２０の動作は次の運動方
程式で与えられる。 Ｍ（θ）・ｄ2 θ／ｄｔ2 ＋Ｃ（ｄθ／ｄｔ，θ） ＝τ＋Ｊ（θ）T ・Ｆ＋Ｄ …（１） ここで， θ：関節角（ベクトル） Ｍ（θ）：慣性項（行列） Ｃ（ｄθ／ｄｔ，θ）：コリオリ力，求心力，粘性，ク
ーロン摩擦等の非線形項（行列） τ：関節モータに与えられるトルク Ｄ：トルクリップル等の外乱 Ｊ（θ）T ：ヤコビアンの転置行列 Ｆ：ワーク取扱い部が外部環境から受ける反力 そして，上記制御装置１a は，マニピュレータ２０の関
節モータの，例えば角度検出器より出力された関節の揺
動角（角度θ）に基づいて，上記ワーク取扱い部の状態
量としての位置ｘ，速度ｄｘ／ｄｔ，加速度ｄ2 ｘ／ｄ
ｔ2 をディジタルフィルタ１１，変換部２を用いて演算
する。更に，上記制御装置１a は，上記位置ｘと，目標
位置設定部７より設定された目標位置ｘ0 との偏差ｘ−
ｘ0 と上記演算された速度ｄｘ／ｄｔ，加速度ｄ2 ｘ／
ｄｔ2 と，上記動力学インピーダンス設定部３の各係数
行列４，５，６とよりそれぞれ求められた力の総和とし
ての設定インピーダンスｆを求めるようになっている。
上記設定インピーダンスｆは，ワーク取扱い部に与えら
れるべき付与力を示し，変換部８において上記角度θの
関数に変換される。即ち，上記設定インピーダンスｆを
求める構成が本発明にいう付与力演算手段である。一
方，逆動力学演算器１０は，ワーク取扱い部に係る慣性
項やコリオリ力，求心力，粘性，クーロン摩擦等の項
（以下，非線形項という）を求めるための数学モデルを
備えている。上記慣性項や非線形項は，上記関節の角
度，角速度，角加速度に基づいて上記数学モデルを解い
て得た推定値である。そして，これらの各項の推定値に
よりワーク取扱い部自身の慣性力，粘性力，クーロン摩
擦力等が求められ，これらの力と上記設定インピーダン
スｆとに基づいて，ワーク取扱い部の動力学特性が上記
動力学インピーダンス設定部３に設定された目標弾性・
粘性・慣性ｋ，ｂ，ｍになるようなトルクτが求められ
る。ちなみに，上記制御装置１a に設定され，上記トル
クτに対応する演算式を以下の（２）式に示す。 τ＝Ｍ* （θ）・ｄ2 θ／ｄｔ2 ＋Ｃ* （θ，ｄθ／ｄｔ） −Ｊ（θ）T ・ｆ …（２） 但し， Ｍ* （θ）：慣性項の推定値 Ｃ* （θ，ｄθ／ｄｔ）：上記非線形項の推定値 又，上記設定インピーダンスｆは次式で表される。 ｆ＝ｆ（ｄ2 ｘ／ｄｔ2 ，ｄｘ／ｄｔ，ｘ，ｄ2 ｘ0 ／ｄｔ2 ， ｄｘ0 ／ｄｔ，ｘ0 ） ＝ｍ・ｄ2 ｘ／ｄｔ2 ＋ｂ・ｄｘ／ｄｔ＋ｋ（ｘ−ｘ0 ）…（３） 上記制御装置１a により制御されるマニピュレータ２０
を用いて，予め設定された目標軌道に対するワーク取扱
い部の倣い動作を行った例を図６に示す。同図におい
て，ワーク取扱い部の目標軌道Ｐ0 上には障害物４０が
存在している。上記ワーク取扱い部は，予め設定された
目標軌道Ｐ0 に追随するように，関節モータの駆動によ
り始点Ｓから図中時計廻りに移動する。このときのワー
ク取扱い部の実際の移動軌跡を実線Ｐ3 で示す。そし
て，ワーク取扱い部が障害物４０に接触すると，ワーク
取扱い部は障害物４０の表面に沿って移動し，同時に障
害物４０からの反力Ｆを受ける。上記反力Ｆを示す複数
の直線は，それぞれの直線が交差する障害物４０上から
の反力のベクトルを示す。このようなマニピュレータの
制御装置１a は，ワーク取扱い部に係る外力を検出する
力センサやトルクセンサを用いることなく，関節モータ
のトルクを制御することができるため構成が簡単になる
といった利点がある。
置としては，特開平２−２０５４８９号公報に開示され
た装置がある。上記制御装置に対応する制御ブロック図
を図５に示す。同図において，マニピュレータの制御装
置１a は，マニピュレータのワーク取扱い部２０の目標
とする動力学的な挙動を表現する動力学インピーダンス
の設定部３を備えている。上記動力学インピーダンス設
定部３には，目標弾性係数行列４（ｋ），目標粘性係数
行列５（ｂ），目標慣性係数行列６（ｍ）が予め設定さ
れている。上記マニピュレータ２０の動作は次の運動方
程式で与えられる。 Ｍ（θ）・ｄ2 θ／ｄｔ2 ＋Ｃ（ｄθ／ｄｔ，θ） ＝τ＋Ｊ（θ）T ・Ｆ＋Ｄ …（１） ここで， θ：関節角（ベクトル） Ｍ（θ）：慣性項（行列） Ｃ（ｄθ／ｄｔ，θ）：コリオリ力，求心力，粘性，ク
ーロン摩擦等の非線形項（行列） τ：関節モータに与えられるトルク Ｄ：トルクリップル等の外乱 Ｊ（θ）T ：ヤコビアンの転置行列 Ｆ：ワーク取扱い部が外部環境から受ける反力 そして，上記制御装置１a は，マニピュレータ２０の関
節モータの，例えば角度検出器より出力された関節の揺
動角（角度θ）に基づいて，上記ワーク取扱い部の状態
量としての位置ｘ，速度ｄｘ／ｄｔ，加速度ｄ2 ｘ／ｄ
ｔ2 をディジタルフィルタ１１，変換部２を用いて演算
する。更に，上記制御装置１a は，上記位置ｘと，目標
位置設定部７より設定された目標位置ｘ0 との偏差ｘ−
ｘ0 と上記演算された速度ｄｘ／ｄｔ，加速度ｄ2 ｘ／
ｄｔ2 と，上記動力学インピーダンス設定部３の各係数
行列４，５，６とよりそれぞれ求められた力の総和とし
ての設定インピーダンスｆを求めるようになっている。
上記設定インピーダンスｆは，ワーク取扱い部に与えら
れるべき付与力を示し，変換部８において上記角度θの
関数に変換される。即ち，上記設定インピーダンスｆを
求める構成が本発明にいう付与力演算手段である。一
方，逆動力学演算器１０は，ワーク取扱い部に係る慣性
項やコリオリ力，求心力，粘性，クーロン摩擦等の項
（以下，非線形項という）を求めるための数学モデルを
備えている。上記慣性項や非線形項は，上記関節の角
度，角速度，角加速度に基づいて上記数学モデルを解い
て得た推定値である。そして，これらの各項の推定値に
よりワーク取扱い部自身の慣性力，粘性力，クーロン摩
擦力等が求められ，これらの力と上記設定インピーダン
スｆとに基づいて，ワーク取扱い部の動力学特性が上記
動力学インピーダンス設定部３に設定された目標弾性・
粘性・慣性ｋ，ｂ，ｍになるようなトルクτが求められ
る。ちなみに，上記制御装置１a に設定され，上記トル
クτに対応する演算式を以下の（２）式に示す。 τ＝Ｍ* （θ）・ｄ2 θ／ｄｔ2 ＋Ｃ* （θ，ｄθ／ｄｔ） −Ｊ（θ）T ・ｆ …（２） 但し， Ｍ* （θ）：慣性項の推定値 Ｃ* （θ，ｄθ／ｄｔ）：上記非線形項の推定値 又，上記設定インピーダンスｆは次式で表される。 ｆ＝ｆ（ｄ2 ｘ／ｄｔ2 ，ｄｘ／ｄｔ，ｘ，ｄ2 ｘ0 ／ｄｔ2 ， ｄｘ0 ／ｄｔ，ｘ0 ） ＝ｍ・ｄ2 ｘ／ｄｔ2 ＋ｂ・ｄｘ／ｄｔ＋ｋ（ｘ−ｘ0 ）…（３） 上記制御装置１a により制御されるマニピュレータ２０
を用いて，予め設定された目標軌道に対するワーク取扱
い部の倣い動作を行った例を図６に示す。同図におい
て，ワーク取扱い部の目標軌道Ｐ0 上には障害物４０が
存在している。上記ワーク取扱い部は，予め設定された
目標軌道Ｐ0 に追随するように，関節モータの駆動によ
り始点Ｓから図中時計廻りに移動する。このときのワー
ク取扱い部の実際の移動軌跡を実線Ｐ3 で示す。そし
て，ワーク取扱い部が障害物４０に接触すると，ワーク
取扱い部は障害物４０の表面に沿って移動し，同時に障
害物４０からの反力Ｆを受ける。上記反力Ｆを示す複数
の直線は，それぞれの直線が交差する障害物４０上から
の反力のベクトルを示す。このようなマニピュレータの
制御装置１a は，ワーク取扱い部に係る外力を検出する
力センサやトルクセンサを用いることなく，関節モータ
のトルクを制御することができるため構成が簡単になる
といった利点がある。
【０００３】
【発明が解決しようとする課題】ところで，上記従来の
マニピュレータの制御装置１a において，上記慣性項や
非線形項の各推定値Ｍ* ，Ｃ* を用いてトルクを演算す
る場合，演算されたトルク（（２）式）には，上記各推
定値の推定誤差ΔＭ（θ）（＝Ｍ（θ）−Ｍ
* （θ）），ΔＣ（θ）（＝Ｃ（ｄθ／ｄｔ，θ）−Ｃ
* （ｄθ／ｄｔ，θ））（以下，モデル化誤差という。
ただし，Ｍ（θ），Ｃ（ｄθ／ｄｔ，θ）は未知であっ
て，上記各項の真値を示す。）が当然ながら含まれる。
そのため，上記設定インピーダンスｆは上記モデル化誤
差により見掛け上大きく演算される。従って，上記ワー
ク取扱い部の移動軌跡Ｐ3 は図示の如く目標軌道Ｐ0 か
らずれるという問題があった。そこで，上記制御装置１
a では，上記設定部３の各係数行列４，５，６（ｋ，
ｂ，ｍ）の値を大きく設定変更することにより，上記目
標軌道Ｐ0 に対する移動軌跡の追随精度を高くすること
ができる。例えば，目標弾性・粘性・慣性ｋ，ｂ，ｍの
値を図６の場合と比べて３倍大きく設定したときのワー
ク取扱い部の倣い動作を図７に示す。図示のように，上
記移動軌跡Ｐ4 は目標軌道Ｐ0 に近付いており，その追
随精度が向上している。しかしながら，上記ｋ，ｂ，ｍ
の値の設定変更により，ワーク取扱い部が受ける反力Ｆ
も約３倍の大きさになる。これでは，上記ワーク取扱い
部の目標軌道に対する追随精度と所定の動力学特性に対
応した動作とを同時に満足させることができない。従っ
て，本発明の目的とするところは，ワーク取扱い部が設
定された目標軌道に精度良く追随し且つ設定された動力
学特性で動作することのできるマニピュレータの制御装
置を提供することにある。
マニピュレータの制御装置１a において，上記慣性項や
非線形項の各推定値Ｍ* ，Ｃ* を用いてトルクを演算す
る場合，演算されたトルク（（２）式）には，上記各推
定値の推定誤差ΔＭ（θ）（＝Ｍ（θ）−Ｍ
* （θ）），ΔＣ（θ）（＝Ｃ（ｄθ／ｄｔ，θ）−Ｃ
* （ｄθ／ｄｔ，θ））（以下，モデル化誤差という。
ただし，Ｍ（θ），Ｃ（ｄθ／ｄｔ，θ）は未知であっ
て，上記各項の真値を示す。）が当然ながら含まれる。
そのため，上記設定インピーダンスｆは上記モデル化誤
差により見掛け上大きく演算される。従って，上記ワー
ク取扱い部の移動軌跡Ｐ3 は図示の如く目標軌道Ｐ0 か
らずれるという問題があった。そこで，上記制御装置１
a では，上記設定部３の各係数行列４，５，６（ｋ，
ｂ，ｍ）の値を大きく設定変更することにより，上記目
標軌道Ｐ0 に対する移動軌跡の追随精度を高くすること
ができる。例えば，目標弾性・粘性・慣性ｋ，ｂ，ｍの
値を図６の場合と比べて３倍大きく設定したときのワー
ク取扱い部の倣い動作を図７に示す。図示のように，上
記移動軌跡Ｐ4 は目標軌道Ｐ0 に近付いており，その追
随精度が向上している。しかしながら，上記ｋ，ｂ，ｍ
の値の設定変更により，ワーク取扱い部が受ける反力Ｆ
も約３倍の大きさになる。これでは，上記ワーク取扱い
部の目標軌道に対する追随精度と所定の動力学特性に対
応した動作とを同時に満足させることができない。従っ
て，本発明の目的とするところは，ワーク取扱い部が設
定された目標軌道に精度良く追随し且つ設定された動力
学特性で動作することのできるマニピュレータの制御装
置を提供することにある。
【０００４】
【課題を解決するための手段】上記目的を達成するため
に，本発明が採用する主たる手段は，その要旨とすると
ころが，複数のリンクを関節を介して連結してなるマニ
ピュレータの，あるリンクに設けられたワーク取扱い部
の位置，速度，加速度と上記ワーク取扱い部の設定され
た動力学特性とに基づいて上記ワーク取扱い部に与える
べき付与力を求める付与力演算手段を少なくとも備え，
少なくとも上記求められた付与力を用いて上記関節を駆
動する駆動源のトルクを演算し出力する制御装置におい
て，上記ワーク取扱い部にかかる反力を検出する力セン
サと，上記求められた付与力と上記力センサにより検出
された反力との差にゲインを乗じた値を上記付与力に加
算する制御ループとを具備してなる点に係るマニピュレ
ータの制御装置として構成されている。
に，本発明が採用する主たる手段は，その要旨とすると
ころが，複数のリンクを関節を介して連結してなるマニ
ピュレータの，あるリンクに設けられたワーク取扱い部
の位置，速度，加速度と上記ワーク取扱い部の設定され
た動力学特性とに基づいて上記ワーク取扱い部に与える
べき付与力を求める付与力演算手段を少なくとも備え，
少なくとも上記求められた付与力を用いて上記関節を駆
動する駆動源のトルクを演算し出力する制御装置におい
て，上記ワーク取扱い部にかかる反力を検出する力セン
サと，上記求められた付与力と上記力センサにより検出
された反力との差にゲインを乗じた値を上記付与力に加
算する制御ループとを具備してなる点に係るマニピュレ
ータの制御装置として構成されている。
【０００５】
【作用】本発明のマニピュレータの制御装置では，マニ
ピュレータの関節を駆動する駆動源に出力されるトルク
の演算の際に，付与力演算手段により求められた付与力
が用いられる。上記付与力には一般的に外乱等の誤差要
因が含まれるので，ワーク取扱い部が上記トルクに基づ
いて駆動された結果外部から受けた反力にも上記誤差要
因が反映される。即ち，力センサにより検出された反力
と上記ワーク取扱い部に与えられるべく求められた付与
力との差は上記誤差要因に対応する。そこで，上記反力
と上記付与力との差にゲインを乗じ，このゲインを設定
変更することにより上記付与力に含まれる誤差要因を調
整することができる。従って，上記制御装置は，ワーク
取扱い部が設定された動力学特性に基づく動作になるよ
うに，精度良く駆動制御することができる。
ピュレータの関節を駆動する駆動源に出力されるトルク
の演算の際に，付与力演算手段により求められた付与力
が用いられる。上記付与力には一般的に外乱等の誤差要
因が含まれるので，ワーク取扱い部が上記トルクに基づ
いて駆動された結果外部から受けた反力にも上記誤差要
因が反映される。即ち，力センサにより検出された反力
と上記ワーク取扱い部に与えられるべく求められた付与
力との差は上記誤差要因に対応する。そこで，上記反力
と上記付与力との差にゲインを乗じ，このゲインを設定
変更することにより上記付与力に含まれる誤差要因を調
整することができる。従って，上記制御装置は，ワーク
取扱い部が設定された動力学特性に基づく動作になるよ
うに，精度良く駆動制御することができる。
【０００６】
【実施例】以下添付図面を参照して，本発明を具体化し
た実施例につき説明し，本発明の理解に供する。尚，以
下の実施例は，本発明を具体化した一例であって，本発
明の技術的範囲を限定する性格のものではない。ここ
に，図１は本発明の一実施例に係るマニピュレータを示
す斜視図，図２は上記マニピュレータの制御装置を示す
ブロック構成図，図３は上記マニピュレータのワーク取
扱い部の目標軌道に対する倣い動作を示す説明図，図４
は上記ワーク取扱い部の目標軌道に対する倣い動作の別
例を示す説明図である。但し，図５に示した上記従来の
マニピュレータの制御装置１a と共通する要素には，同
一の符号を使用すると共に，その詳細な説明は省略す
る。本実施例に係るマニピュレータ２０は，図１に示す
ように，図示せぬ基台側に固定された第１の関節モータ
９a と，この関節モータ９a の回転駆動軸に固設された
第１のリンク２３と，このリンク２３の先端に固設され
た第２の関節モータ９b と，この関節モータ９b の回転
駆動軸に設けられた第２のリンク２４と，リンク２４の
先端に配備されたワーク取扱い部２５とよりなってい
る。上記関節モータ９a 及び９b には，それぞれエンコ
ーダ（３６００００ｐ／回転）が一体的に設けられてお
り，上記基台に対して揺動するリンク２３の角度θ a や
リンク２３の軸芯に対して揺動するリンク２４の角度θ
b を検出し出力するようになっている。そして，上記リ
ンク２３及び２４のリンク長は予め決まっているので，
これらの角度θa 及びθb により上記ワーク取扱い部２
５の，例えばｘ方向の位置が決定される。なお，図１中
の関節モータ９は上記関節モータ９a 及び９b のいずれ
も含むものであって，同じく角度θは上記角度θa 及び
θb を表わしている。そして，上記マニピュレータ２０
を制御する制御装置１は，図２に示すように，上記従来
の制御装置１a と基本的構成をほぼ同様とし，上記従来
の制御装置１ a と構造上異なる点は，マニピュレータ２
０のワーク取扱い部２５にかかる例えば障害物４０から
の反力Ｆを検出する力センサ１４と，上記目標インピー
ダンス設定部３からの力の総和として求められた設定イ
ンピーダンスｆ（付与力）と上記力センサ１４により検
出された反力Ｆとの差（ｆ−Ｆ）を求める減算器１７
と，上記差（ｆ−Ｆ）に乗じられるゲインＧを設定する
ゲイン設定部１５と，上記差にゲインＧを乗じた値Ｇ
（ｆ−Ｆ）を上記設定インピーダンスｆに加算する加算
器１６とを備え，上記減算器１７，ゲイン設定部１５，
加算器１６よりなる制御ループ構成が採用されたことで
ある。上記力センサ１４は，例えば歪ゲージよりなり，
ワーク取扱い部２５に設けられている。又，上記制御装
置１では，ワーク取扱い部２５の目標速度ｄｘ0 ／ｄｔ
を設定する目標速度設定部１２と，目標加速度ｄ2 ｘ0
／ｄｔ2 を設定する目標加速度設定部１３とが設けら
れ，上記設定インピーダンスｆの演算において上記位
置，速度，加速度とそれぞれの目標値との偏差が用いら
れるようになっている。従って，上記設定インピーダン
スｆは次の（４）式のように示される。 ｆ＝ｍ（ｄ2 ｘ／ｄｔ2 −ｄ2 ｘ0 ／ｄｔ2 ）＋ｂ（ｄｘ／ｄｔ −ｄｘ0 ／ｄｔ）＋ｋ（ｘ−ｘo ） …（４） そして，上記設定インピーダンスｆは変換部８により角
度θの関数Ｊ（θ）T ・ｆに変換される。
た実施例につき説明し，本発明の理解に供する。尚，以
下の実施例は，本発明を具体化した一例であって，本発
明の技術的範囲を限定する性格のものではない。ここ
に，図１は本発明の一実施例に係るマニピュレータを示
す斜視図，図２は上記マニピュレータの制御装置を示す
ブロック構成図，図３は上記マニピュレータのワーク取
扱い部の目標軌道に対する倣い動作を示す説明図，図４
は上記ワーク取扱い部の目標軌道に対する倣い動作の別
例を示す説明図である。但し，図５に示した上記従来の
マニピュレータの制御装置１a と共通する要素には，同
一の符号を使用すると共に，その詳細な説明は省略す
る。本実施例に係るマニピュレータ２０は，図１に示す
ように，図示せぬ基台側に固定された第１の関節モータ
９a と，この関節モータ９a の回転駆動軸に固設された
第１のリンク２３と，このリンク２３の先端に固設され
た第２の関節モータ９b と，この関節モータ９b の回転
駆動軸に設けられた第２のリンク２４と，リンク２４の
先端に配備されたワーク取扱い部２５とよりなってい
る。上記関節モータ９a 及び９b には，それぞれエンコ
ーダ（３６００００ｐ／回転）が一体的に設けられてお
り，上記基台に対して揺動するリンク２３の角度θ a や
リンク２３の軸芯に対して揺動するリンク２４の角度θ
b を検出し出力するようになっている。そして，上記リ
ンク２３及び２４のリンク長は予め決まっているので，
これらの角度θa 及びθb により上記ワーク取扱い部２
５の，例えばｘ方向の位置が決定される。なお，図１中
の関節モータ９は上記関節モータ９a 及び９b のいずれ
も含むものであって，同じく角度θは上記角度θa 及び
θb を表わしている。そして，上記マニピュレータ２０
を制御する制御装置１は，図２に示すように，上記従来
の制御装置１a と基本的構成をほぼ同様とし，上記従来
の制御装置１ a と構造上異なる点は，マニピュレータ２
０のワーク取扱い部２５にかかる例えば障害物４０から
の反力Ｆを検出する力センサ１４と，上記目標インピー
ダンス設定部３からの力の総和として求められた設定イ
ンピーダンスｆ（付与力）と上記力センサ１４により検
出された反力Ｆとの差（ｆ−Ｆ）を求める減算器１７
と，上記差（ｆ−Ｆ）に乗じられるゲインＧを設定する
ゲイン設定部１５と，上記差にゲインＧを乗じた値Ｇ
（ｆ−Ｆ）を上記設定インピーダンスｆに加算する加算
器１６とを備え，上記減算器１７，ゲイン設定部１５，
加算器１６よりなる制御ループ構成が採用されたことで
ある。上記力センサ１４は，例えば歪ゲージよりなり，
ワーク取扱い部２５に設けられている。又，上記制御装
置１では，ワーク取扱い部２５の目標速度ｄｘ0 ／ｄｔ
を設定する目標速度設定部１２と，目標加速度ｄ2 ｘ0
／ｄｔ2 を設定する目標加速度設定部１３とが設けら
れ，上記設定インピーダンスｆの演算において上記位
置，速度，加速度とそれぞれの目標値との偏差が用いら
れるようになっている。従って，上記設定インピーダン
スｆは次の（４）式のように示される。 ｆ＝ｍ（ｄ2 ｘ／ｄｔ2 −ｄ2 ｘ0 ／ｄｔ2 ）＋ｂ（ｄｘ／ｄｔ −ｄｘ0 ／ｄｔ）＋ｋ（ｘ−ｘo ） …（４） そして，上記設定インピーダンスｆは変換部８により角
度θの関数Ｊ（θ）T ・ｆに変換される。
【０００７】一方，逆動力学演算器１０（推定値演算手
段）は上記関節モータ９のエンコーダから出力された角
度θ，及びこの角度θからディジタルフィルタ１１によ
り演算された角速度，角加速度に基づいて，慣性項の推
定値Ｍ* （θ）と，上記非線形項の推定値Ｃ* （θ，ｄ
θ／ｄｔ）との和を求める。そして，上記関節モータ９
に与えられるべきトルクτは次の（５）式により求めら
れる。即ち（５）式が上記マニピュレータ２０の制御則
を表わす。 τ＝Ｍ* （θ）・ｄ2 θ／ｄｔ2 ＋Ｃ* （θ，ｄθ／ｄｔ） −Ｊ（θ）T ・｛ｆ＋Ｇ（ｆ−Ｆ）｝ …（５） 従って，上記ワーク取扱い部２５にかかる反力Ｆは，
（１）式，（５）式及びモデル化誤差を示す式（ΔＭ＝
Ｍ（θ）−Ｍ* （θ），ΔＣ＝Ｃ（ｄθ／ｄｔ，θ）−
Ｃ* （ｄθ／ｄｔ，θ））より，次の（６）式のように
示される。 Ｆ＝ｆ＋｛Ｉ＋Ｇ｝-1｛Ｊ（θ）T ｝-1｛ΔＭ（θ）・ｄ2 θ／ｄｔ2 ＋ΔＣ（ｄθ／ｄｔ，θ）−Ｄ｝ …（６） ただし，Ｉは単位行列である。上記（６）式において，
右辺第２項は，モデル化誤差及び外乱Ｄによる反力Ｆへ
の影響を表わしている。そこで，上記ゲイン設定部１５
からゲインＧを設定変更することにより，上記モデル化
誤差及び外乱の影響を調整することができる。例えば，
上記ゲインＧを十分大きくすることにより上記モデル化
誤差及び外乱の影響を極めて小さくすることが可能で，
これによって反力Ｆを上記設定インピーダンスに限りな
く近づけることができる。なお，上記変換部２，動力学
インピーダンス設定部３，変換部８，逆動力学演算器１
０，ディジタルフィルタ１１，ゲイン設定部１５，加算
器１６，減算器１７は，図外のＣＰＵ上で動くソフトウ
ェアにより全て構成され，プログラムとして同じく図外
のメモリに格納されている。また，上記マニピュレータ
２０は制御装置１のＣＰＵにより４ミリ秒のサンプリン
グ周期で制御される。なお，上記逆動力学演算器１０の
数学モデルにおいて，関節モータ９の駆動軸の粘性とク
ーロン摩擦とは停止中の駆動軸に力（トルク）をステッ
プ状に変えて与え，上記駆動軸が動き始めた時のトルク
を実測しこれに基づいて予め求めた。また，上記慣性項
に係る係数は，上記関節モータ９に白色ノイズ状のトル
クを与え，このとき同時に計測して得た。
段）は上記関節モータ９のエンコーダから出力された角
度θ，及びこの角度θからディジタルフィルタ１１によ
り演算された角速度，角加速度に基づいて，慣性項の推
定値Ｍ* （θ）と，上記非線形項の推定値Ｃ* （θ，ｄ
θ／ｄｔ）との和を求める。そして，上記関節モータ９
に与えられるべきトルクτは次の（５）式により求めら
れる。即ち（５）式が上記マニピュレータ２０の制御則
を表わす。 τ＝Ｍ* （θ）・ｄ2 θ／ｄｔ2 ＋Ｃ* （θ，ｄθ／ｄｔ） −Ｊ（θ）T ・｛ｆ＋Ｇ（ｆ−Ｆ）｝ …（５） 従って，上記ワーク取扱い部２５にかかる反力Ｆは，
（１）式，（５）式及びモデル化誤差を示す式（ΔＭ＝
Ｍ（θ）−Ｍ* （θ），ΔＣ＝Ｃ（ｄθ／ｄｔ，θ）−
Ｃ* （ｄθ／ｄｔ，θ））より，次の（６）式のように
示される。 Ｆ＝ｆ＋｛Ｉ＋Ｇ｝-1｛Ｊ（θ）T ｝-1｛ΔＭ（θ）・ｄ2 θ／ｄｔ2 ＋ΔＣ（ｄθ／ｄｔ，θ）−Ｄ｝ …（６） ただし，Ｉは単位行列である。上記（６）式において，
右辺第２項は，モデル化誤差及び外乱Ｄによる反力Ｆへ
の影響を表わしている。そこで，上記ゲイン設定部１５
からゲインＧを設定変更することにより，上記モデル化
誤差及び外乱の影響を調整することができる。例えば，
上記ゲインＧを十分大きくすることにより上記モデル化
誤差及び外乱の影響を極めて小さくすることが可能で，
これによって反力Ｆを上記設定インピーダンスに限りな
く近づけることができる。なお，上記変換部２，動力学
インピーダンス設定部３，変換部８，逆動力学演算器１
０，ディジタルフィルタ１１，ゲイン設定部１５，加算
器１６，減算器１７は，図外のＣＰＵ上で動くソフトウ
ェアにより全て構成され，プログラムとして同じく図外
のメモリに格納されている。また，上記マニピュレータ
２０は制御装置１のＣＰＵにより４ミリ秒のサンプリン
グ周期で制御される。なお，上記逆動力学演算器１０の
数学モデルにおいて，関節モータ９の駆動軸の粘性とク
ーロン摩擦とは停止中の駆動軸に力（トルク）をステッ
プ状に変えて与え，上記駆動軸が動き始めた時のトルク
を実測しこれに基づいて予め求めた。また，上記慣性項
に係る係数は，上記関節モータ９に白色ノイズ状のトル
クを与え，このとき同時に計測して得た。
【０００８】上記した構成のマニピュレータ２０の制御
装置１を用いて，ワーク取扱い部２５の目標軌道に対す
る倣い動作を行った例に付き図３に示す。この場合，上
記制御装置１には，上記ワーク取扱い部２５の移動する
目標位置ｘ0 として従来と同様の目標軌道Ｐ0 が予め設
定されている。又，上記動力学インピーダンス設定部３
の目標弾性ｋ，目標粘性ｂ，目標慣性ｍとしては，例え
ばｋ＝８００Ｎ／ｍ，ｂ＝１６０Ｎｓ／ｍ，ｍ＝８Kgが
設定されている。これらは従来（図６）において設定さ
れたものと同じ値である。更に，上記ゲインＧとして
は，Ｇ＝２Ｉが設定されている。従来方式であれば上記
ゲインＧはＧ＝０に相当するが，上記ゲインＧ（２Ｉ）
の設定により，上記（６）式の右辺第２項の値が従来と
比べて１／３になり，制御精度が３倍になる。そこで，
上記マニピュレータ２０の関節モータ９に上記（５）式
により求めたトルクτを与えると，ワーク取扱い部２５
が始点Ｓから移動を始める。このとき，上記ワーク取扱
い部２５が移動した移動軌跡Ｐ1 及びワーク取扱い部２
５が受けた反力Ｆを同図に示す。それによれば，上記マ
ニピュレータ２０のワーク取扱い部２５は，従来（図
６）と比べて，同等の反力Ｆを障害物４０から受けつつ
目標軌道Ｐ0 により近い移動軌跡Ｐ1 で移動している。
更に，上記ゲインＧをＧ＝４Ｉに設定した場合のワーク
取扱い部２５の倣い動作を図４に示す。この場合，上記
制御装置１の制御精度はＧ＝０のときと比べて５倍にな
る。そのため，上記ワーク取扱い部２５は上記した場合
と同等の反力Ｆを受けつつ，目標軌道Ｐ0 により一層近
い移動軌跡Ｐ2 で移動する。上記したように，本実施例
のマニピュレータの制御装置１は，上記モデル化誤差や
外乱の影響をゲインＧの調整によって任意に小さくする
ことにより，ワーク取扱い部２５を設定されたインピー
ダンスに対応した動作で駆動し得る制御手法を採用して
おり，ワーク取扱い部２５の目標軌道追随精度を飛躍的
に改善することができる。しかも，従来と比べて，減算
（減算器１７），加算（加算器１６），乗算（ゲイン設
定部Ｇ）をひとつずつ増やすだけですむので，ＣＰＵ負
荷は大きく増大しない。又，一般的に設定インピーダン
スｆの設定範囲はワーク取扱い部２５の位置，速度，加
速度のフィードバック量の上限により制限される。しか
しながら，本実施例装置によれば，ゲインＧを変化させ
ることにより設定インピーダンスｆと上記フィードバッ
ク量との関係を従来よりも自由に決定することができ
る。従って，上記ゲインＧを設定してフィードバック量
の上限を超えないようにすることにより，設定インピー
ダンスｆの設定範囲を広げることができる。一方，ワー
ク取扱い部２５が，例えば硬い外部環境と接触する場
合，上記目標インピーダンス設定部３の目標弾性ｋの値
を小さく設定する必要がある。このような場合，従来手
法によれば，上記目標弾性ｋを小さく設定すると目標軌
道追随精度が極めて悪くなる。ところが，本願手法によ
れば，上記目標弾性ｋの値が小さくとも上記ゲインＧを
大きく設定することにより，良好な追随精度でインピー
ダンス制御を行うことができる。従って，硬い外部環境
と接触しながら高い追随精度が要求される作業や，例え
ば破損しやすい部品の組立作業を精度良く行うことがで
きる。尚，上記制御装置１から逆動力学演算器１０が省
略された構成である場合，上記推定値Ｍ* ，Ｃ* は演算
されることがなく，これによって上記トルクτが補償さ
れないので，ワーク取扱い部の制御精度は悪くなる。し
かしながら，このような場合にも上記ゲインＧの調整に
よって上記ワーク取扱い部の追随精度を上げることがで
きる。
装置１を用いて，ワーク取扱い部２５の目標軌道に対す
る倣い動作を行った例に付き図３に示す。この場合，上
記制御装置１には，上記ワーク取扱い部２５の移動する
目標位置ｘ0 として従来と同様の目標軌道Ｐ0 が予め設
定されている。又，上記動力学インピーダンス設定部３
の目標弾性ｋ，目標粘性ｂ，目標慣性ｍとしては，例え
ばｋ＝８００Ｎ／ｍ，ｂ＝１６０Ｎｓ／ｍ，ｍ＝８Kgが
設定されている。これらは従来（図６）において設定さ
れたものと同じ値である。更に，上記ゲインＧとして
は，Ｇ＝２Ｉが設定されている。従来方式であれば上記
ゲインＧはＧ＝０に相当するが，上記ゲインＧ（２Ｉ）
の設定により，上記（６）式の右辺第２項の値が従来と
比べて１／３になり，制御精度が３倍になる。そこで，
上記マニピュレータ２０の関節モータ９に上記（５）式
により求めたトルクτを与えると，ワーク取扱い部２５
が始点Ｓから移動を始める。このとき，上記ワーク取扱
い部２５が移動した移動軌跡Ｐ1 及びワーク取扱い部２
５が受けた反力Ｆを同図に示す。それによれば，上記マ
ニピュレータ２０のワーク取扱い部２５は，従来（図
６）と比べて，同等の反力Ｆを障害物４０から受けつつ
目標軌道Ｐ0 により近い移動軌跡Ｐ1 で移動している。
更に，上記ゲインＧをＧ＝４Ｉに設定した場合のワーク
取扱い部２５の倣い動作を図４に示す。この場合，上記
制御装置１の制御精度はＧ＝０のときと比べて５倍にな
る。そのため，上記ワーク取扱い部２５は上記した場合
と同等の反力Ｆを受けつつ，目標軌道Ｐ0 により一層近
い移動軌跡Ｐ2 で移動する。上記したように，本実施例
のマニピュレータの制御装置１は，上記モデル化誤差や
外乱の影響をゲインＧの調整によって任意に小さくする
ことにより，ワーク取扱い部２５を設定されたインピー
ダンスに対応した動作で駆動し得る制御手法を採用して
おり，ワーク取扱い部２５の目標軌道追随精度を飛躍的
に改善することができる。しかも，従来と比べて，減算
（減算器１７），加算（加算器１６），乗算（ゲイン設
定部Ｇ）をひとつずつ増やすだけですむので，ＣＰＵ負
荷は大きく増大しない。又，一般的に設定インピーダン
スｆの設定範囲はワーク取扱い部２５の位置，速度，加
速度のフィードバック量の上限により制限される。しか
しながら，本実施例装置によれば，ゲインＧを変化させ
ることにより設定インピーダンスｆと上記フィードバッ
ク量との関係を従来よりも自由に決定することができ
る。従って，上記ゲインＧを設定してフィードバック量
の上限を超えないようにすることにより，設定インピー
ダンスｆの設定範囲を広げることができる。一方，ワー
ク取扱い部２５が，例えば硬い外部環境と接触する場
合，上記目標インピーダンス設定部３の目標弾性ｋの値
を小さく設定する必要がある。このような場合，従来手
法によれば，上記目標弾性ｋを小さく設定すると目標軌
道追随精度が極めて悪くなる。ところが，本願手法によ
れば，上記目標弾性ｋの値が小さくとも上記ゲインＧを
大きく設定することにより，良好な追随精度でインピー
ダンス制御を行うことができる。従って，硬い外部環境
と接触しながら高い追随精度が要求される作業や，例え
ば破損しやすい部品の組立作業を精度良く行うことがで
きる。尚，上記制御装置１から逆動力学演算器１０が省
略された構成である場合，上記推定値Ｍ* ，Ｃ* は演算
されることがなく，これによって上記トルクτが補償さ
れないので，ワーク取扱い部の制御精度は悪くなる。し
かしながら，このような場合にも上記ゲインＧの調整に
よって上記ワーク取扱い部の追随精度を上げることがで
きる。
【発明の効果】本発明によれば，複数のリンクを関節を
介して連結してなるマニピュレータの，あるリンクに設
けられたワーク取扱い部の位置，速度，加速度と上記ワ
ーク取扱い部の設定された動力学特性とに基づいて上記
ワーク取扱い部に与えるべき付与力を求める付与力演算
手段を少なくとも備え，少なくとも上記求められた付与
力を用いて上記関節を駆動する駆動源のトルクを演算し
出力する制御装置において，上記ワーク取扱い部にかか
る反力を検出する力センサと，上記求められた付与力と
上記力センサにより検出された反力との差にゲインを乗
じた値を上記付与力に加算する制御ループとを具備して
なることを特徴とするマニピュレータの制御装置が提供
される。従って，上記ゲインを調整することにより，上
記設定された動力学特性に依存することなく制御精度を
向上させることができる。それにより，上記ワーク取扱
い部は上記設定された動力学特性で精度良く駆動制御さ
れる。
介して連結してなるマニピュレータの，あるリンクに設
けられたワーク取扱い部の位置，速度，加速度と上記ワ
ーク取扱い部の設定された動力学特性とに基づいて上記
ワーク取扱い部に与えるべき付与力を求める付与力演算
手段を少なくとも備え，少なくとも上記求められた付与
力を用いて上記関節を駆動する駆動源のトルクを演算し
出力する制御装置において，上記ワーク取扱い部にかか
る反力を検出する力センサと，上記求められた付与力と
上記力センサにより検出された反力との差にゲインを乗
じた値を上記付与力に加算する制御ループとを具備して
なることを特徴とするマニピュレータの制御装置が提供
される。従って，上記ゲインを調整することにより，上
記設定された動力学特性に依存することなく制御精度を
向上させることができる。それにより，上記ワーク取扱
い部は上記設定された動力学特性で精度良く駆動制御さ
れる。
【図１】 本発明の一実施例に係るマニピュレータを示
す斜視図。
す斜視図。
【図２】 上記マニピュレータの制御装置を示すブロッ
ク構成図。
ク構成図。
【図３】 上記マニピュレータのワーク取扱い部の目標
軌道に対する倣い動作を示す説明図。
軌道に対する倣い動作を示す説明図。
【図４】 上記ワーク取扱い部の目標軌道に対する倣い
動作の別例を示す説明図。
動作の別例を示す説明図。
【図５】 本発明の背景の一例となる従来のマニピュレ
ータの制御装置を示すブロック構成図。
ータの制御装置を示すブロック構成図。
【図６】 上記従来のマニピュレータのワーク取扱い部
の倣い動作を示す説明図。
の倣い動作を示す説明図。
【図７】 図６の倣い動作とは異なる設定条件により行
った倣い動作を示す説明図。
った倣い動作を示す説明図。
１，１a …制御装置 ３…動力学インピーダンス設定部 ４…目標弾性係数行列 ５…目標粘性係数行列 ６…目標慣性係数行列 ９，９a ，９b …関節モータ １４…力センサ １５…ゲイン設定部 １６…加算器 １７…減算器 ２０…マニピュレータ ２３…リンク ２４…リンク ２５…ワーク取扱い
部
部
フロントページの続き (56)参考文献 特開 平２−205489（ＪＰ，Ａ) 特開 昭64−44510（ＪＰ，Ａ) 特開 平１−142812（ＪＰ，Ａ) 特開 平１−210281（ＪＰ，Ａ) 特開 平４−152082（ＪＰ，Ａ) 実開 平２−61586（ＪＰ，Ｕ) (58)調査した分野(Int.Cl.6，ＤＢ名) G05D 3/12 B25J 9/10 B25J 13/00
Claims (1)
- 【請求項１】 複数のリンクを関節を介して連結してな
るマニピュレータの，あるリンクに設けられたワーク取
扱い部の位置，速度，加速度と上記ワーク取扱い部の設
定された動力学特性とに基づいて上記ワーク取扱い部に
与えるべき付与力を求める付与力演算手段を少なくとも
備え，少なくとも上記求められた付与力を用いて上記関
節を駆動する駆動源のトルクを演算し出力する制御装置
において，上記ワーク取扱い部にかかる反力を検出する
力センサと，上記求められた付与力と上記力センサによ
り検出された反力との差にゲインを乗じた値を上記付与
力に加算する制御ループとを具備してなることを特徴と
するマニピュレータの制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP3071295A JP2798518B2 (ja) | 1991-04-04 | 1991-04-04 | マニピュレータの制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP3071295A JP2798518B2 (ja) | 1991-04-04 | 1991-04-04 | マニピュレータの制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH04306710A JPH04306710A (ja) | 1992-10-29 |
| JP2798518B2 true JP2798518B2 (ja) | 1998-09-17 |
Family
ID=13456541
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP3071295A Expired - Fee Related JP2798518B2 (ja) | 1991-04-04 | 1991-04-04 | マニピュレータの制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2798518B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103203756A (zh) * | 2012-01-17 | 2013-07-17 | 精工爱普生株式会社 | 机器人控制装置、机器人系统以及机器人控制方法 |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7469166B2 (en) * | 2001-06-29 | 2008-12-23 | Honda Motor Co., Ltd. | System and method of predicting novel motion in a serial chain system |
-
1991
- 1991-04-04 JP JP3071295A patent/JP2798518B2/ja not_active Expired - Fee Related
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103203756A (zh) * | 2012-01-17 | 2013-07-17 | 精工爱普生株式会社 | 机器人控制装置、机器人系统以及机器人控制方法 |
| CN103203756B (zh) * | 2012-01-17 | 2016-12-28 | 精工爱普生株式会社 | 机器人控制装置、机器人系统以及机器人控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JPH04306710A (ja) | 1992-10-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US5994864A (en) | Robot controller | |
| EP0280324B1 (en) | Nonlinear control unit for a multi-degree-of freedom manipulator | |
| US4617502A (en) | Method and apparatus for controlling a robot hand along a predetermined path | |
| EP0321579A1 (en) | Articulated robot controller | |
| WO2014091840A1 (ja) | サーボ制御装置 | |
| JP2604929B2 (ja) | ロボットの制御装置 | |
| JP4639417B2 (ja) | ロボットの制御装置 | |
| JP4361285B2 (ja) | 数値制御装置 | |
| JP2798518B2 (ja) | マニピュレータの制御装置 | |
| CN115336167B (zh) | 摩擦补偿装置、碰撞检测装置、扭矩前馈运算装置及机器人控制装置以及摩擦补偿方法 | |
| JP3460761B2 (ja) | ロボットの制御装置 | |
| JP2906256B2 (ja) | サーボ制御装置 | |
| JP4134369B2 (ja) | ロボットの制御装置 | |
| JPH04306712A (ja) | マニピュレータの制御装置 | |
| JPH07185817A (ja) | 多軸ロボットのウィービング制御方法 | |
| JP2960232B2 (ja) | ロボットの制御装置 | |
| JPH06332535A (ja) | ロボットの制御装置 | |
| JP4507071B2 (ja) | モータ制御装置 | |
| JP2960233B2 (ja) | ロボットの制御装置 | |
| JPS61217802A (ja) | ロボツト制御装置 | |
| JP2682977B2 (ja) | ロボットの制御方法および装置 | |
| JPH06104307B2 (ja) | 多関節マニピユレ−タの制御装置 | |
| JPH09185416A (ja) | マニピュレータの力制御装置 | |
| JPH0769734B2 (ja) | マニピュレータ装置 | |
| JPH0245806A (ja) | 多自由作業機械の位置と力の制御装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |