JPH0259287A - マニピュレータの制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〈産業上の利用分野〉 本発明はマニピュレータ又はハンドが作業対象物に接近
し、接触後、力制御を行う賜金のマニピュレータの制御
方法に関するものである。

〈従来の技術及び発明が解決しようとする課題〉マニピ
ュレータ又はハンドが、位置制御により作業対象物に接
近し、接触後、力制御を行う場合、従来は位置制御時に
常に接触力をモニターし、接触力検出後、比例制御等の
力制御を行っていた。このため、マニピュレータ又はハ
ンドが作業対象物と接触した時に接触力が力制御の目標
値に較へ大幅にオーバーシュートし、作業対象物を変形
又は破壊する危険が存在するという欠点があった。

この欠点を改善するために、力制御に、接触時のマニピ
ュレータ又はハンドの速度に比例した補償入力を加え、
接触力のオーバーシュートを力ＩＩＪ御の目標値以下に
し、作業対象物を変形又は破壊する危険を回避するとい
う制御方法がある。しかし、この制御方法では補償入力
を常にマニピュレータまたはハンドに加えているために
、力制御の目標値に較べ比較的大きな定常偏差が残存す
るという欠点があった。

この欠点を改善するために先の補償入力を加えるかわり
に微分制御を比例制御に加えるという力制御方法もある
が、この制御方法では接触力の時間微分を行うために雑
音により制御系が不安定になり易いという欠点が有った
。さらに、高周波領域の雑音除去のために、低域フィル
ターを用いると制御系の帯域が狭くなり、制御系の応答
が悪（なるという欠点が有った。

本発明の目的は、従来技術の状況に鑑み、位置制御から
力制御移行時に発生する接触力のオーバーシュートを力
制御の目標値以下にし、また力制御の定常偏差を力制御
の目標値に比較して小さくでき、しかも雑音に比較的強
し）マニピュレータの制御方法を提供することにある。

く課題を解決するための手段〉 本発明は、前記の従来技術の欠点を解決するために、マ
ニピュレータ又はハンドが位置制御により作業対象物に
接近し、接触後接触力制御を行う時、接触後の接触力と
力制御の目標値が一致するような補償入力を力制御に加
えた移行モードを、位置制御から力制御への移行時に新
たに加え、接触力の最大値検出後補償入力を０にし、力
制御モードへ移行させることを基本アルゴリズムとする
。

〈実　施　例〉 本発明の主要な原理と従来技術の差異を作業対象物の弾
性の作る物理ポテンシャル及び制御による人工ポテンシ
ャルを用いて説明する。まず、本発明である接触力制御
法に用いられる制御則を以下に示す。

〔位置制御）　　　Ｖ（ｋ＋１）＝ＰＩＤ（（Ｘｄ−Ｘ
（ｋｌＨ１ｌｌ（Ｘ、＝Ｆ／に、） 〔移行モーＦ）　　　Ｖ　（ｋ＋１）＝Ｖ、＋に、（Ｆ
、−ｆ（ｋｌ）　＋Ｖ、ｆ２）（Ｖ０＝　Ｆ　／Ｔ、） 〔力制御）　　　Ｖ　（ｋ＋１１＝Ｖ０＋に、（Ｆｄ−
ｆ（ｋｌ＋　　　　（３１但し、■はモータへの入力電
圧、Ｘはマニピュレータまたはハンドの変位　１（、は
作業対象物のバネ定数、Ｆｄは力制御の目標値、Ｋ−よ
比例制御のゲイン、■（は補償入力、Ｔはモータのトル
ク定数、ｆは接触力である。またＰＩＤは比例、積分２
ｗｉ分動作制御を示しており、ｋはディジタルサンプリ
ング時刻を示すものである。第１図に各制御モードの切
り換えの概念図を示す。図に示す様に、まず位置制御を
用いて対象物に接近し、作業対象物に接触した後移行モ
ードを用いて接触によるオーパーンニートのない力制御
モードへと移行する。

この切り換え時期及び移行モードで用いられるオーバー
・レユートを抑制ずろ補償入力についてはポテンシャル
を用いて次に説明する。

また、本発明により定常偏差の比較的少ない制御系が設
計できることについても併せて説明する。

衝突問題でのポテンシャルエネルギーとして作業対象物
の弾性によって作られろ物理ポテンシャルを考慮する必
要がある。また、Ｐ制御によりマニピュレータまたはハ
ンドが発生する力もマニピュレータまたはハンドの変位
により決定することが出来るのでポテンシャルを責える
ことができろ。以下、この２つのポテンシャルを用いて
本発明の特徴と従来技術との差異について説明する。マ
ニピュレータ位置の方向はマニピュレータがら作業対象
物に向かう方向を正とし、位置の基準をカが加わってい
ない時の接触面の位置とした。

また、ポテンシャルの基準としてＸ＝Ｘ、（平衡点二力
制御の目標値を発生する変位）でのポテンシャルをＵｏ
とすると力制御モード時及び移行モード時のポテンシャ
ル曲線は次式の様になる。但し、制御のサンプリング時
間は十分短いものとし以下アナログ系と等価とみなして
議論する。

Ｃ力１ｅｌｌｉａ〕　　Ｕ４に、（Ｔ、に、＋１）（Ｘ
−Ｘｄ）”＋Ｕ。　　　　（４）ＣｅモーＦ〕　Ｕ−Ｖ
２に、Ｃｒ、に、＋１）（Ｘ−Ｘｄ）２＋Ｔ、Ｖ、（Ｘ
−Ｘ、）＋Ｕ０（５１ここでＵは変位Ｘでのポテンシャ
ルエネルギ−である。式（５）よりｖｆを変化させるこ
とによりポテンシャル面を変化させ接触力のオーバーシ
ュートを変化させることが可能である。

式＋４１　（５１で示されろポテンシャル面を接触力の
時間変化と共に示したのが第２図である。

図中実線は本発明の接触力制御法を用いた場合の接触力
の時間変化、破線は接触力制御法を用いない場合の接触
力の時間変化を示している。また、ポテンシャル１は式
（５）で表されろ移行モードのポテンシャル曲面、ポテ
ンシャル２は式（４）で表されろ力制御モードのボテン
ンヤル曲面である。第２図では接触力の変化を模凝的に
黒のボールの軌跡として示しているが、接触力は作業対
象物と接触後移行モードのポテンシャル１の曲面内を推
移し、ポテンシャルの平衡点を過ぎて後最大値（オーバ
ーシュート量）を示す。この時、移行モードから力制御
へのモードの切り換えを行わないと図中破線で示す様に
振動しながらポテンシャル１の平衡点に収束する。この
現象は、一般的に特別な制御を行わない場合の結果であ
り第１図の破線の応答に対応する。本発明では第１図の
実線に示す様な接触力の応答を得るために、以下に示す
ように制御則を移行させた。ポテンシャル面１で想定さ
れる接触力の最大値とポテンシャル面２で規定される力
制御の目標値（平衡点）が一致する様に補償力■、を選
定し、接触力が最大値を示す点で制御系のダイナミック
スをポテンシャル面１からポテンシャル面２へ移行させ
る。この時、ポテンシャル面１で接触力が最大値を示す
点の系の速度は０なので、この時この接触力に対応する
平衡点を有するポテンシャル面２に系のダイナミックス
は移行する。このため切り換え後運動は生ぜず、目標値
と一致する平衡点で安定な状態となる。この時接触力の
軌跡は第２図のポテンシャル面２の実線に示す様に振動
のない理想的なものとなる。上記のポテンシャル面を用
いた制御系の切り換又方法は、実際には以下の様に切り
換えろ。マニピュレータが対象物接触後、式（２）の移
行モードの制御則で制御を行う。マニピュレータに取り
つけな力センサで接触力を検知し、接触力が最大値をし
めした時点で、式（３）に示す制御則に切り換える。ポ
テンシャル２の平衡点と力制御の目標値が一致している
ために定常偏差を０にすることが出来る。最大値と力制
御の目標値が一致する様な■、は次の様に求めろことが
出来る。系のダンピングによるとエネルギーの散逸が無
視できる場合、力学的エネルギー保存の法則より次式が
成立する。

ＩｈＫ、（Ｔ、に、＋　１）　Ｘ、”　＋Ｔ、Ｖ、Ｘｄ
＋１７ｍｖ２＝Ｕ　　　（６１（旦しｍはマニピュレー
タまたはハンドの質量・Ｖはマユビニレータまた（よＡ
ンドカ（作業対象物と衝突した時の速度である。式（６
）より■、を求めると以下の様になる。

Ｖ、＝−Ｋｍｖ２／（２ＦｄＴ、）−Ｖ２Ｆ、（Ｋ、＋
１／Ｔ、、）　　　（７）式（７）より補償力は接触力
の微分を必要としないため、雑音による影響を受けにく
いことが分かる。式（７）の結果を用いて移行モード時
の制御則（式（２））を変形すると Ｖ　（ｋ＋１）　−Ｖ２Ｖｏ＋に、　（ｖ２Ｆｄ−ｆ（
ｋ））　＋Ｋ　ＶＫ　＝−に、ｍｖ２／　（２Ｆ、Ｔ、
）　　　　　　　　　　（８１となる。式（８）を用い
て移行モード時の制御を行又は接触力の最大値と力制御
の目標値は一致する。

以上説明した様にマニピュレータ又はハンドが位置制譚
により作業対象物に接近し、接触後方制御を行う時、本
発明である接触力制御法をもちいれば、オーパーンニー
トを抑制し、定常偏差が比較的小さい雑音に強い制御系
を設計することが出来る。

第２図は本発明の具体的な実施例装置であって、１は作
業対象物、２は３自由度マニピュレータ、３はコントロ
ーラである。第４図は本発明である接触力制御法の制御
フローである。まず、位置制御によりマニピュレータは
接触力を検知するまで作業対策物に接近し、接触力を検
知後移行モードに切り換える。その後接触力の最大値を
検出するまで移行モ−ドを繰り返し、最大値検知検力制
御に切り換える。以下に本実施例で用いた制御則を示す
。

〔位置制御）　　　Ｖ［旬＝Ｐ　Ｉ　Ｄ　（Ｘ、、１−
Ｘ）　　　　　　　　　　　＋９１（Ｘｄ＝　Ｆ　／Ｋ
　、） Ｃ利ｑＴモ　Ｆ〕　　Ｖ（ｋｌｌ）＝１，４Ｖ。＋に、
、（ｖ２Ｆ、−ｆ（ｋｌｌ＋Ｋ　　ｖ２（１０）〔力制
御）　　　Ｖ（ｋｌ＝Ｖｏ＋に、□（Ｆｄ−ｆ（ｋｌｌ
　　　　　　　　（１１）但し、■はマニピュレータの
接触時の速度、Ｋは補償ゲインである。

作業対象物のバネ定数が既知であり、ダンピングが無視
でき、かつ制御系の非線形要素による影響が無視できろ
時には前項式（８）の補償ゲインＫを用いれば、力制御
の目標値と接触力の最大値は一致し良好な制御を実現ず
ろことが出来ろ。しかし、作業対象物のバネ定数が既知
であるという仮定は制約があまりにも太き（、また制御
系の非線形要素、ダンピングの影響も無視出来ない。そ
こで本実施例では、実験的に力制御の目標値と接触力の
最大値が一致する様な補償ゲインに、　Ａ求める方法を
示す。第５図に補償ゲインＫ、を変化させた時のオーバ
ーシュート量の変化を示す。図に示す様に補償ゲインが
大きくなるにつれてオーバーシュート量は正の値から負
の値へ単調に減少し、補償ゲインによりオーバーシュー
ト量を比較的広範囲に変化させることが可能であること
が分かる。このことから、力制御の目標値と接触力の最
大値を一致させろためにはオーバーシュート量がＯにな
る補償ゲインを用いれば良い。この結果を基に本発明の
接触力制御法を用いた場合と用いなかった場合について
比較したのが第６図である。但し、図中の実線は本発明
の接触力制御法を用いた場合、図中の破線は移行モード
を用いず、直ちに位置制御モードから力制御モードへ切
り換えた場合をそれぞれ示している。図に示す様に本発
明の接触力制御法を用いなかった場合は接触力のオーバ
ーシュートは力制御の目標値に比較してかなり大きいこ
とが分かる。

一方、本発明の接触力制御法を用いた場合、比較的小さ
な振動が残存するもののオーバーシュート及び定常偏差
は力制御の目標値に較べ小さく良好な制御が実現出来る
ことが分かる。

以上水した様に本発明の接触力制御法を用いてマニピュ
レータ又はハンドが位置制御により作業対象物に接近し
、接触後方制御を行う時、オーバーシュートを抑制し、
定常偏差が比較的小さり２蓮音に強い制御系を設計する
ことが出来ることが分かる。

〈発明の効果〉 以上説明した様にマニピュレータ又はハンドが位置制御
により作業対象物に接近し、接触後方制御を行う時、接
触後の接触力の最大値と力制御の目標値が一致するよう
な補償入力を力制御に加えた制御モードを位置制御から
力制御への移行時に新たに加え、接触力の最大値検出段
補償入力を０にする接触力制御をおこなえば、オーバー
シュートを抑制し、定常偏差が比較的小さく雑音に強い
制御系を設計することが出来る。また、本発明の接触力
制御法は制御アルゴリズムが単純であるため、コントロ
ーラの負担を増加することなく良好な制御を行うことが
できる利点を有している。

【図面の簡単な説明】

第１図は各制御モードの切り換えの概念を示す概念図、
第２図はポテンシャル面を示す説明図、第３図は本発明
の実施例を示す構成図、第４図は制御フローを示すフロ
ー図、第５図は補償ゲインとオーバーシュート量との関
係を示す特性図、第６図は接触力の時間応答例を示す特
性図である。 図　　面　　中、 １は作業対象物、 ２は３自由度マニピュレータ、 ３はコントローラである。 特　　許　　出　　願　　人 日本電信電話株式会社 代　　　　理　　　　人

Claims (1)

1. 【特許請求の範囲】 マニピュレータまたはハンドを位置制御により作業対象
   物に接近させ、マニピュレータまたはハンドが作業対象
   物に接触したら力制御を行う制御方法において、 接触後は、接触力と力制御の目標値が一致するような力
   制御入力に、接触力の最大値と力制御の目標値が一致す
   るような補償入力を加えて力制御をし、 その後、接触力の最大値が検出されたら、前記補償入力
   を零にし、前記力制御入力により力制御をすることを特
   徴とするマニピュレータの制御方法。