JPH079609B2 - マニピューレータの力と位置の混成制御方法 - Google Patents

Description

【発明の詳細な説明】 （イ）発明の目的 ［産業上の利用分野］ この発明はマニピュレータの力と位置の混成制御方法に
関するものである。

人間の手作業代行のため、多自由度のリンクからなるマ
ニピュレータが開発され、人間の行うたくみな動作を実
現するため、位置制御、力制御など、対象とする作業に
応じて適当な方式がとられている。ところで、人間の行
う作業の多くは、作業を行う空間に設定される作業座標
系の各軸ごとに異なる制御方式をとることを要求される
ことが多い。また、その各軸の制御方式は、ある作業で
は力制御、ある作業では位置制御というように、両制御
方式を切り換えて用いることが求められる。

例えば、文字を書く動作では、紙面に垂直な方向の動作
については筆圧に相当する力制御、紙面に平行な２自由
度の動作については筆の運びに相当する位置制御が適し
ている。また、マニピュレータがドアのノブを回す動作
を考えると、ノブを破壊しないようにするために、ノブ
の回転軸に垂直な方向に対しては一定の大きさ以上の力
が加わらないような力制御が必要であり、かつ、軸まわ
りの運動にはノブを規定量回転させるための回転位置制
御が必要になる。

多自由度のマニピュレータでこれを実現するには、一般
にマニピュレータの各関節の自由度が各々独立に作業座
標系の自由度に対応していないため、マニピュレータの
各関節の動作の制御には、力と位置を同時に制御する混
成制御が必要になっている。

［従来の技術］ 従来の位置と力を併用する制御では第８図に示すよう
に、位置・角度センサと力・トルクセンサを設け、場合
に応じてセンサを切換えて、力・トルクサーボ系、位置
・角度サーボ系を構成できるようにしている。こうした
方式は、例えば第９図に示すように制御したい力や位置
の方向が一方向であり、しかも、これを制御するモータ
が一つの場合には有効である。しかし、ロボット・マニ
ピュレータの制御では、例えば、第10図に示すように物
体表面に一定の力を加えつつその表面をトレースすると
いうような場合に見られるように、その時の作業内容に
応じて、ハンド５の手先に設定される座標系の各軸方向
に対し、力制御、位置制御に関する個別の制御が要求さ
れる。第10図の場合では、物体100表面に垂直な方向に
対し、力制御、物体表面に平行な方向に対しては、位置
制御が必要とされる。そのように力・位置出力が発生す
るように、各ロボット・マニピュレータの関節に構成さ
れているアクチュエータ（モータ）を駆動するわけであ
るが、この時に第８図に示すように、各関節の制御系が
構成されているだけでは目的を達成できない。

例えば、第10図においてモータ101を位置制御、モータ1
02を力制御モードとするなど種々の制御モードの組合せ
を考えても、各関節の運動が創成する手先の運動方向
は、手先の力・位置制御が要求される座標系の方向と異
なっているから、第10図に示すような手先の力制御、位
置制御は実現できない。

［発明が解決しようとする問題点］ このように、従来の位置制御、力制御を併用する考え方
は、基本的に異なる制御系を切換えて使用することによ
り行うものであるから困難が生じている。

この発明は、上記の如き事情に鑑みてなされたものであ
って、関節のコンプライアンスを外部信号によって可変
にする機構を導入することにより、関節に位置制御系を
構成するだけで、手先の任意の座標軸方向に対し、力制
御、位置制御を可能にするマニピュレータの制御方法を
提供することを目的とするものである。

（ロ）発明の構成 ［問題を解決するための手段］ この目的に対応して、この発明のマニピュレータの力と
位置の混成制御方法は、マニピュレータの関節を駆動す
るアクチュエータをコンプライアンスを調整可能なアク
チュエータで構成し、前記マニピュレータの関節の力目
標値をＰ_ｆ、手先に拘束が加わる目標位置までの位置目
標値をｒとするとき、前記関節を駆動するアクチュエー
タのコンプライアンスをｋとして、Ｐ_ｆ・ｋ＝Δｒなる
位置偏差を前記位置目標値ｒに加えた（ｒ＋Δｒ）を位
置目標値として前記アクチュエータを位置制御すること
によって前記マニピユレータの関節が目標位置に達して
前記手先が拘束されたときの力制御を位置制御に置換す
ることにより、位置制御と力制御の混成を位置制御だけ
で達成することを特徴としている。

以下、この発明の詳細を実施例を示す図面について説明
する。

第１図に１軸のアクチュエータ系による基本原理を示
す。この第１図ではアクチュエータとしては、サーボ弁
を用いる油圧系を用いているが、電動モータを含む系で
も以下の議論は同様に成り立つ。

第１図において、シリンダ７、位置検出器８、アンプ1
0、サーボ弁３により、位置サーボ系が構成されてい
る。すなわち、ａより位置目標値が加えられると、それ
が位置検出器８によって検出されたピストン６の動きと
引算器２で比較され、その差がサーボ弁３に加えられ
る。

サーボ弁３は加えられる差信号に応じて、その内部のス
プールを動かして、それによって差信号が零になる方向
にピストンを動かす。結果的に、ピストン６はａから加
えられる位置指令に応じた量だけ動く、これは通常位置
サーボ機構とよばれているが、本発明では、この機構の
ピストンの一端に図に示すように、可変コンプライアン
ス要素４を接続する。そしてこの可変コンプライアンス
要素４を介して例えばハンド５を駆動するようにする。
可変コンプライアンスとは、そのスチフネス（ばね定
数）ｋを外部からの電気信号によって変化させることの
できる要素である。

このように構成したシステムでは、コンプライアンスｋ
を適切な値に設定することによって、第８図のように位
置検出器、力検出器の切換えを行わなくても、任意サー
ボ系だけで力制御モード、位置制御モードを実現でき
る。

すなわち、位置制御とする場合は、可変コンプライアン
ス要素４のコンプライアンスを小、すなわちばねを固く
する。それによってａから位置目標値が加えられると、
それに応じてピストン６が動き、アクチュエータ変位Ｐ
_ａはｒに等しくなる。可変コンプライアンス要素４のコ
ンプライアンスｋは、小であるから、ハンド５に変位中
に何か外乱が加わったとしても、それはほとんど変形す
ることなく、常にＰ_ａとハンド変位Ｐ_ｈは等しく動く、
すなわち、常にＰ_ａ＝Ｐ_ｈとなるため、結果的にハンド
は位置目標値ｒに従って正確に動くことになる。ハンド
が位置目標値ｒだけ動いて目標位置に達して、例えば壁
９や対象物体のようなものに接触したことは例えばセン
サによって検出される。

一方、力制御をする場合には、ハンドが位置目標値ｒだ
け動いて目標位置に達してハンド５が壁９或は対象物体
のようなものに接触していて、その壁９や対象物のよう
なものから拘束されたときに、それにある力Ｆを作用さ
せるよう要求されている場合を想定する。この時、可変
コンプライアンス要素４のコンプライアンスｋをある程
度大なる値に設定し、ばねを柔かくする。すると、も
し、アクチュエータ変位をＰ_ａとし、可変コンプライア
ンス要素４が自然長にあって、ハンド５が壁９に接触し
ているとすると、この状態から、アクチュエータ（ピス
トン）６がΔｒ変位し、Ｐ_ａがＰ_ａ＋Δｒになると、壁
９にはハンドを介してＦ＝（1/k）Δｒなる力が加わ
る。これは見方を変えて、壁９にＦなる力を作用させた
いとした時に、アクチュエータ（ピストン）を、Δｒ＝
ｋ・Ｆ変化させればよいことに相当する。従って位置サ
ーボ系に位置目標値ｒを加えた時に、アクチュエータ
（ピストン）６がＰ_ａで静止しているならば、この状態
において位置サーボ系に、ｒ＋Δｒを加えて、アクチュ
エータ６をＰ_ａ＋Δｒ変化させれば、ハンド５にＦなる
力を発生させることができる。すなわち、以上より、位
置サーボ系だけで、そのアクチュエータに接続された可
変コンプライアンス要素４のコンプライアンスｋと位置
サーボ系の位置目標値を適切に設定するだけで、力制
御、位置制御の両モードを実現することができる。ここ
でコンプライアンスｋの値を力制御モードにおいて大に
とることは重要である。すなわち、ｋを小にとると希望
する発生力Ｆに対し、Δｒ＝ｋ・Ｆの関係からΔｒの値
が小になり、位置サーボ系に高分解能な制御が要求され
ることになる。また、位置制御モードではｋをできるだ
け小としないと、多少の外乱がハンド５に加わる時で
も、コンプライアンス要素がたわみ、アクチュエータ変
位とハンドの変位が一対一に対応しなくなり、正確なハ
ンドの位置制御が期待できない。このコンプライアンス
要素の実現は、ソフトウェアサーボの手法などを適用す
れば従来技術で実現できる。

また、第１図に示すような直動運動アクチュエータだけ
でなく、回転型アクチュエータに対しても第２図に示す
ように、可変コンプライアンス要素としてねじりばね要
素を用いかつ変位を回転変位におきかえれば上述と同様
の議論が成立する。

可変コンプライアンス要素としてのねじりばね要素は、
その一はトルクモータを使用して次のように構成する。
ここでトルクとはコイルに流す電流に比例したトルクを
発生する電動モータである。まず第11図に示すように角
度センサ付のトルクモータを用意する。

このトルクモータ・センサ系に第11図に示すような角度
サーボ系を構成し、目標値を０［Ｖ］に固定する。これ
に外力トルクτ_ｘを加えると、トルクモータはその力に
よって変位するが、その変位Δθはτ_ｘにつりあうトル
クをモータが発生するところでとまる。すなわち、モー
タが０からΔθまで変位したときに発生するモータのト
ルクτは、目標値が０であるから、 τ＝αＫΔθ となり、τ＝τ_ｘの時、つりあって停止する。すなわ
ち、停止位置では τ_ｘ＝αＫΔθ の関係が成り立つ。ここで、αはトルクモータのトルク
定数である。

よって第11図の系は外力τ_ｘに比例したΔθを発生する
（比例定数αＫ）ので、αＫはフックの法則のバネ定数
の役割をはたす。

すなわち、Ｋをコンピュータ等で変化させることにより
可変バネ系ができる。

次に、こうした位置と力の制御方式を１軸ではなく、一
般的なロボット・マニピュレータに適用することを考え
る。第３図にロボット・マニピュレータの関節に第１図
に示したような可変コンプライアンス要素を含む位置サ
ーボ系を装備したシステムを示す。一般的マニピユレー
タには最小限６自由度が必要であるから、ここで説明す
るマニピュレータの関節も６個有すると仮定する。こう
した６個の関節に可変コンプライアンスと位置サーホ系
を備えたマニピュレータでは、例えばハンドの部分Ｐに
種々の方向にある力を加えてマニピュレータを変位させ
ようとした場合、各関節のコンプライアンスの値に応じ
て、ある柔かさを示す。すなわち、各関節ののコンプラ
イアンスを小とすれば、ハンド部分Ｐをどの方向から押
しても固くてあまり動かない。一方、コンプライアンス
を大とすれば柔かくなり、すぐ外力に応じて動くように
なる。各関節の角度をθ_１〜θ_６、そのコンプライアン
スをk₁〜k₆、マニピュレータの固定台に固定された直交
座標系をＯ、作業空間に作業内容に応じて任意にとられ
た座標系を作業座標系と名付けＯ′とする。また手先に
固定された座標系をＰとすると、作業座標系Ｏ′の各直
交軸方向及び各軸まわりのスチフネス（コンプライアン
スの逆数）と、各軸間のスチフネスは次のように求めら
れる。

この式（＊）は次のように誘導する。

各関節に発生するトルクをτ_ｉ（ｉ＝１〜６）、τ_ｉを
要素とするベクトルを次のように書く。

またマニピュレータ先端に作用する力・モーメントの成
分をｆ_ｉ（ｉ＝１〜６）、ｆ_ｉを要素とするベクトル
とし、このを次のように書く。

ここでをに変換する行列をＱとすると ＝Ｑ …（３） （この式は仮想仕事の原理から容易に導かれる。） また関節のバネの剛性をＣ_ｉ（ｉ＝１〜６）、それを対
角にもつ行列をとすると 各関節のバネ変位をΔθ_ｉ（ｉ＝１〜６）、これを要素
にもつベクトルをΔとすると、 フックの法則により関節トルクベクトルとΔの関係
が次のように求まる。

＝Δ …（６） 一方、関節の変位Δに対応するアーム先端の変位を手
先座標系Ｐで記述したものをΔＸ′とすると ΔＸ′＝ＪΔ …（７） 但し、Ｊはヤコビ行列で、関節の変化Δとそれに対応
する手先座標系におけるマニピュレータ先端変位ΔＸ′
を関係づける行列である。

又、手先座標系Ｐで記述される変位を作業座標系で記述
される変位に変換する行列をR^-1、作業座標系で記述さ
れたΔＸ′をΔＸとすると、 ΔＸ＝R^-1ΔＸ′ よってΔＸ′＝ＲΔＸ …（８） （３）と（６）より ＝Q^-1Δ…（９）
（９）と（７）より ＝Q^-1J^-1ΔＸ′ …（10）
（10）と（８）より ＝Q^-1J^-1RΔＸ …（11）
（11）式は作業座標系Ｏ′で表された外力とそれに対
応する変位ΔＸの関係を表しており、 とすると， は剛性行列となる。これは上記の式（＊）を意味する。

ここでＱは作業座標系Ｏ′の各軸方向に作用する力及び
各軸まわりのモーメントをマニピュレータの関節トルク
に変換する６×６行列、Ｊはマニピュレータの関節の微
小変化を、手先の手先座標系Ｐにおける各軸方向及び各
軸まわりの変化に交換する６×６行列、Ｒは作業座標系
Ｏ′と手先座標系Ｐの関係を記述する６×６変換行列で
ある。Q,Jはマニピュレータの関節角度（θ_１〜θ_６）
を、またＲは作業座標系Ｏ′と手先座標系Ｐの対応する
座標軸間の角度を変数として含む。

一方、はマニピュレータの関節のスチフネス1/k₁,1/k
₂,〜1/k₆を対角成分に含む６×６の対角行列 である。さらに式（＊）左辺は、第４図に示すように座
標系を定めた時に、例えば、 Ｋ_xx…Ｋ_zz、Ｋ_αα…Ｋ_γγは、ｘ〜ｙ軸方向、α、
β、γ回転方向の剛性を、また、Ｋ_ij，（ｉ＝ｘ〜z,α
〜γ、ｊ＝ｘ〜z,α〜γ:i≠ｊ）はｉとｊ間の剛性を示
し、ｊ方向にΔεの変位（角変位）を加えた時に、ｉ方
向にＫ_ij・Δεの力（モーメント）を発生することを意
味する。明らかな如く、（＊）式の左辺は関節の剛性K₁
〜K₆、すなわち、コンプライアンスk₁〜k₆の関数であ
る。従って関節の可変コンプライアンス機構によって、
その関節のコンプライアンスを調節すれば作業座標各軸
方向の剛性Ｋ_xx,K_yy,K_zz,K_αα,K_ββ,K_γγを設定でき
る。

次にこの作業座標系の各方向に対し、第５図に示すよう
に手先（ハンド）の力制御、位置制御を個別に行うこと
を考えると、ある座標軸方向に対し、位置制御を行う時
は１軸の場合に対して説明した原理に基づき、その方向
のコンプライアンスを小、すなわち剛性を大にする。そ
して、その座標軸方向に指定の位置指令だけハンドが変
位するように関節の位置サーボ系に指令値を送る。一
方、力制御したい作業座標軸の方向に対しては、そのコ
ンプライアンスをある程度大、すなわち剛性を小にして
おく。そして、Ｆという力を発生したいならば、その座
標軸の方向にＦを発生するに必要な関節トルクを座標変
換によって求め、そうして求められた関節トルクτ₁,τ
₂,…τ_６に対し、その関節のコンプライアンスk₁,k₂…k
₆から、Δθ_１＝k₁τ₁,Δθ_２＝k₂τ₂,…Δθ_６＝k₆τ
_６によって、その関節トルクを発生するに必要なアクチ
ュエータ（モータ）の角度増分Δθ₁,Δθ₂,…Δθ_６を
求める。

例えば第６図において、壁に垂直にとられた作業座標系
ｚ軸の方向に一定の力Ｆ_ｚを作用させ、かつ、壁に平行
にとられたｙ軸方向にｒ_ｙだけハンドを移動させたいと
すると、ハンドの位置移動量は、作業座標系の位置・角
度増分ベクトルを である。

作業座標系の各軸方向の移動量増分を関節角度の増分へ
変換する行列をとすると によってｙ軸方向にハンドをry移動させるに必要な関節
角増分を求めることができる。この増分を実行するとハ
ンドは第６図の点線の位置に移動し、関節角はもとの関
節角をθ₁,θ₂,…θ_６とすと、θ_１＋Δε₁,θ_２＋Δε
₂,…θ_６＋Δε_６になる。また上式のはθ_１〜θ_６の
関数である。ここで、さらにハンドは壁にFzを作用させ
なければならない。

行列は作業座標系の変位を関節座標に変換するヤコビ
行列の逆行列であり、その構造はマニピュレータの幾何
学的パラメータと作業座標系の配置で決まる。その構造
はかなり複雑になるが、但し誘導法はロボット工学では
既知である。一般に逆ヤコビ行列の存在は６自由度のマ
ニピュレータでは特異姿勢でない限り、解の存在は保証
される。よってマニピュレータが特異姿勢にない限り解
の存在は保証される。

作業座標系における力トルクベクトルを このＦ_ｚが作用している状態は 作業座標系に作用する力・トルクを関節トルクτ_１〜τ
_６に変換する行列をとすると によりτ_１〜τ_６が求まる。ここで関節に設定されてい
るコンプライアンスをk₁〜k₆とすれば によって、関節にτ_１〜τ_６を発生するに要する、従っ
て作業座標系に を発生するに要する各関節の角度増分量Δθ₁,Δθ₂,…
Δθ_６が得られる。結果的に上述の位置制御の場合と総
合して、各関節に θ_１＋Δε_１＋Δθ₁,θ_２＋Δε_１＋Δθ₂,…θ_６＋Δ
ε_６＋Δθ_６なる角度指令を与えれば、第６図において
ハンドはｙ軸方向にｒ_ｙ動き、かつｚ方向に壁に対し、
Ｆ_ｚの力を発生することになる。

上記のＢ（θ）は作業座標系Ｏ′で記述されたマニピュ
レータ先端変位と関節変位の関係を記述するヤコビ行列
をＪとすれば、 Ｂ（θ）Ｊ^Ｔ となる。ここでＴは行列Ｊの転置を示す。

Ｊ^Ｔは常に存在するので、τ_ｉ〜τ_６の一義性は保証さ
れる。

以上の過程を実行するシステムを第７図に示す。第７図
において２は位置制御、力制御選択装置である。

なる対角行列で構成されている。ここで、Ｓ_ｉは０か１
の値をとる。

一方、_ｉは Ｓ_ｉ＝１ならば_ｉ＝０ Ｓ_ｉ＝０ならば_ｉ＝１ なる値をとる。但し、ｉ＝ｘ〜z,α〜γであり、作業座
標系のｘ…α…γ軸に対応している。このＳ_ｉ，_ｉは
位置、力選択信号により例えば位置制御したい軸に対し
てＳ_ｉ＝１（従って_ｉ＝０）、力制御したい軸には対
してはＳ_ｉ＝０（従って_ｉ＝１）とする。

例えば は作業座標系のx,z,γ方向に対し位置制御、y,α，β方
向に対し力制御することを意味する。

これに位置増分指令、力・トルク指令 加える。この時に位置制御をしない座標軸の位置指令
値、力制御をしない座標軸の力指令値（例えば今の例で
はｒ_ｙ、ｒ_α,r_β,F_ｘ,F_ｚ,F_γ）は結果的に無視される
ので、どんな値をセットしてもよい。

13に 14に なる出力を得る。

各関節のコンプライアンス指令値23を可変コンプライア
ンス機構16に伝達して関節のコンプライアンスを調整す
るとともに、その調整値k₁〜k₆を11へ送り、24によって
関節トルクに変換された力指令を関節角増分指令に変換
する。

このコンプライアンス指定値及び関節コンプライアンス
調整値ｋ_ｉ〜k₆は力指令を変位指令に変換するものであ
るから、理論的には０或いは∞でない限りどんな値でも
よい。便益上は、作業座標系においてどの軸方向に対し
ても等しい値をもつことが望ましい。作業座標系のコン
プライアンスはマニピュレータの姿勢に応じて変化する
から、この実施例では予め指定するようにしてある。

一方、位置増分指令は６で関節角増分指令に変換された
後、11で求められた力指令に対応する関節増分指令と、
加算器で加算される。

この３の出力が指令された位置・力制御を達成するに必
要な関節角増分になる。これに関節角現在値を加えて各
関節アクチュエータの位置サーボ系に出力すれば目的が
達成される。

Ｓとを今の例とは別にセットすればそれに応じて作業
座標軸方向の力制御・位置制御が実現できる。

（ハ）発明の効果 このように、この発明によれば、マニピュレータの各関
節を可変コンプライアンスをもつアクチュエータで構成
し、力制御、位置制御の両者を好適に組合せ、作業座標
系における安定な力と位置のハイブリット制御を精度良
く実現することができるマニピュレータの混成制御方法
を得ることができる。

【図面の簡単な説明】

第１図はこの発明を１軸直線運動アクチュエータを使用
した制御系に適用した例を示す構成説明図、第２図は１
軸回転運動アクチュエータを使用した制御系に適用した
例を示す構成説明図、第３図は多関節ロボット・マニピ
ュレータを示す構成説明図、第４図は作業座標系におけ
る剛性を示す説明図、第５図は作業座標系の各方向への
ハンドの制御を示す説明図、第６図はハンドの位置変位
を示す説明図、第７図はこの発明を多関節マニピュレー
タの制御系に適用した例を示す構成説明図、第８図は分
離された力・トルク制御モードと位置・角度制御モード
を示す説明図、第９図は従来の制御系の一例を示す構成
説明図、第10図は従来の制御系の他の例をを示す構成説
明図、及び第11図は角度サーボ系を示す構成説明図であ
る。 ２……引算器、３……サーボ弁、４……可変コンプライ
アンス要素、５……ハンド、６……アクチュエータ、７
……シリンダ、８……位置検出器、９……壁、10……ア
ンプ、11……位置偏差演算装置、12……座標変換装置、
13……位置制御系、14……力制御系、15……駆動要素、
16……コンプライアンス要素、17……関節、18……手
先、21……作業対象物、22……出力軸（アクチュエータ
15の）、100……物体、101……モータ、102……モータ

Claims (1)

【特許請求の範囲】
1. 【請求項１】マニピュレータの関節を駆動するアクチュ
   エータをコンプライアンスを調整可能なアクチュエータ
   で構成し、前記マニピュレータの関節の力目標値を
   Ｐ_ｆ、手先に拘束が加わる目標位置までの位置目標値を
   ｒとするとき、前記関節を駆動するアクチュエータのコ
   ンプライアンスをｋとして、Ｐ_ｆ・ｋ＝Δｒなる位置偏
   差を前記位置目標値ｒに加えた（ｒ＋Δｒ）を位置目標
   値として前記アクチュエータを位置制御することによっ
   て前記マニピュレータの関節が目標位置に達して前記手
   先が拘束されたときの力制御を位置制御に置換すること
   により、位置制御と力制御の混成を位置制御だけで達成
   することを特徴とするマニピュレータの力と位置の混成
   制御方法