JPH058187A

JPH058187A - ロボツト

Info

Publication number: JPH058187A
Application number: JP16257491A
Authority: JP
Inventors: Yasuyuki Nakada; 康之中田
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1991-07-03
Filing date: 1991-07-03
Publication date: 1993-01-19

Abstract

(57)【要約】【目的】力制御の負荷を軽減してロボット先端の振動発
生を抑制し、力制御の一層の高速化達成を可能にするこ
とを目的とする。【構成】ロボット本体から延びたマニプレータの先端
に、力覚センサ及びハンドを取り付け、前記力覚センサ
によってハンドに働く外力を検出し、該検出値及び任意
の指令値に基づいて、前記ハンドと物体間の接触力をコ
ントロールするロボットにおいて、前記マニプレータの
先端と力覚センサの間を制御型の直動機構で連結し、前
記検出値及び指令値に基づいて演算される制御量で、該
直動機構を駆動するように構成したことを特徴とする。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ロボット、特に力覚セ
ンサを備えたロボットであって倣い作業に好適なロボッ
トに関する。

【０００２】ロボットによる倣い作業、すなわち対象物
体の表面形状に倣いながらその形状を正確になぞる作業
として、ロボットと物体間の接触力を一定に保つような
作業が行われる。こうした作業に力覚センサを備えたロ
ボットは好適であり、物体からの反力を力覚センサで検
出し、この検出値（以下、力検出値）が常に目標値と一
致するように制御（いわゆる力制御）できる。

【０００３】ところで、力検出値は、物体表面上の接触
点における直角座標系として与えられ、その座標成分
は、物体の法線方向の軸（以下Ｘ軸）、物体表面の接線
方向の軸（以下Ｙ軸）、そして、これら２軸と直交する
軸（以下、Ｚ軸）の３軸で表される。

【０００４】ここで、任意の曲率をもつ湾曲面上の複数
点ごとの直角座標系は互いに異なる系を構成する。これ
は、それぞれの点における法線方向が一致しないから
で、各座標系の成分方向（軸）が異なるからである。

【０００５】このため、倣い作業用ロボットにあって
は、物体表面の各点ごとの座標系教示（ティーチング）
が必須であり、座標系設定の容易化技術が求められる。

【０００６】

【従来の技術】そこで、本願発明者は先に、座標系設定
を容易化した「倣い制御装置」（特願平１−２４１８９
７号、出願日平成元年９月２０日）を提案している。

【０００７】この先願の装置（以下、先願装置）では、
図１１に示すような構造のロボットを使用する。すなわ
ち、このロボットは、本体底面の中心Ｏを基準座標系の
原点とする基準座標軸Ｘ_O、Ｙ_O及びＺ_Oを設定し、座標
軸Ｚ_O上に第１関節１０を設け、この第１関節１０に、
第１アーム１１、第２関節１２、第２アーム１３及び第
３関節１４を含むマニプレータ１５を連結し、第３関節
１４を介して力覚センサ１６及びハンド（ロボット先端
と言うこともある）１７を連結する構造を有している。
マニプレータ１５の各関節１０、１２及び１６が、図示
しない制御装置からの信号に応答してきわめて微小なス
テップで回転し、ロボット先端の位置及び接触力が複合
的に制御される。

【０００８】図１１はハンド１７が任意曲率表面の物体
１９に接触している状態を示しており、その接触点Ｏ_W
を原点とする直角座標系（以下、倣い座標系）の座標軸
は、物体１８の内向き法線と一致する軸Ｘ_W、ハンド１
７の移動方向に一致する軸Ｙ_W及びこれらの２軸に直交
する方向の軸Ｚ_Wであり、各軸方向の単位ベクトルはそ
れぞれｎ、ｏ、ａで示されている。なお、正しいベクト
ル表記は、符号の頭に矢印（→）が付くが、本明細書中
では省略している。

【０００９】ｎはハンド１７によって物体１９を垂直に
押す力（以下、押圧力）、ｏはハンド１７の移動力であ
り、何れも力制御の制御対象（ハンド１７）の状態量で
ある。なお、ａはこれら状態量の外積（ｎ×ｏ）で与え
られる。

【００１０】先願装置の特徴は、物体表面上の任意点の
位置情報、その点の力情報及び倣い動作の完了点の位置
情報に基づいて、任意点におけるｎ及びｏを逐次求めて
いくアルゴリズムにある。

【００１１】すなわち、アルゴリズムは、図１２に示す
ように、力覚センサ１７からの力情報により任意点Ｏ
_Wiにおける反力Ｆ_iを求める、反力Ｆ_iからＯ_Wi座標系
の単位ベクトルｎ_iを求める、このｎ_iを基準座標系の
単位ベクトルｎ_iに変換する、任意点Ｏ_Wiと倣い動作
の完了点Ｐ_ENDとを結ぶ直線上の単位ベクトルＬ_iを求め
る、このＬ_iと先のｎ_iとの外積により単位ベクトルａ
_iを求める、このａ_iとｎ_iとの外積により単位ベクト
ルｏ_iを求める、という手順を含むものである。

【００１２】これによれば、図１３に示すように、任意
点Ｏ_W1から完了点Ｐ_ENDに向かう単位ベクトルＬ₁とその
点Ｏ_W1における反力Ｆ₁とにより、点Ｏ_W1の各座標成分
（ｎ ₁、ａ₁、ｏ₁）が求められる。これは、倣い動作の
進行に伴って点が移動した場合でも同様である。例えば
点Ｏ_W2では、そのＯ_W2から完了点Ｐ_ENDに向かう単位ベ
クトルＬ₂とその点Ｏ_W2における反力Ｆ₂とにより、点Ｏ
_W2の各座標成分（ｎ₂、ａ₂、ｏ₂）が求められる。

【００１３】したがって、各点の座標系が自動設定され
るので、倣い動作の開始点や完了点及びその間の目標押
圧力等の最小限度のティーチングで済ますことができ
る。

【００１４】

【発明が解決しようとする課題】ところで、上記の先願
装置は、必要最小限度のティーチングによって倣い作業
を実行できる点で有効なものの、制御速度を高速化しよ
うとすると、ロボット先端に振動が発生して押圧力の方
向を正確にコントロールし難くなる欠点があり、作業ス
ピードのより一層の高速化を図るという観点から見た場
合に改善すべき余地があった。

【００１５】すなわち、先願装置は、マニプレータ１５
の３つの関節で与えられる３自由度の動き（Ａ、Ｂ、
Ｃ）により、ロボット先端の位置、姿勢及び接触力を複
合的にコントロールするものであるが、特に、物体表面
への鉛直方向の押圧力を微妙、且つ高速にコントロール
する力制御においては、ロボット本体の第１関節１０
（換言すればマニプレータ１５の根元部分の関節）によ
って与えられる自由度Ａに主として依存するため、この
第１関節１０が、ハンド１８、力覚センサ１７及びマニ
プレータ１５を含む全ての可動部分（質量が大きくしか
も剛性が低い）を駆動しなければならず、駆動速度を一
層高めようとすると、大きな慣性力が発生してロボット
先端に振動が発生し易くなるといった問題点がある。

【００１６】図１４は振動発生時におけるロボット先端
のふるまいを示す例である。一般に、長尺体の振動現象
は、その物体の一端を揺動支点とする首振り運動であ
り、ロボット先端のふるまいも、程度こそ違うものの例
外ではない。すなわち、図１４において、任意の曲面を
有する物体１９の表面に接触する長尺体（この場合ハン
ド１８）に振動が発生していなければ、図中のイに示す
ように、力制御によってハンド１８の押圧力Ｆ_r（以
下、制御押圧力）の方向が正しくコントロールされ、物
体１９の内部に向かう法線ｎの方向と正確に一致する
が、ハンド１８に振動が発生すると、図中のロに示すよ
うに、制御押圧力Ｆ_rの方向と法線ｎの方向とが一致し
なくなるため、Ｆ_rから２つの分力（Ｆ_rn及びＦ_ro）が
発生し、法線ｎと一致する分力Ｆ_rnによって制御押圧力
Ｆ_rよりも小さい押圧力（実際の接触力）が作用すると
ともに、法線ｎと直交するＦ_roによってハンド１８の先
端に不本意な移動力（すなわちハンド１８の先端をずれ
させる力）が作用することになる。すなわち、ハンド１
８の先端に振動が発生して制御押圧力Ｆ_rの方向と法線
ｎの方向とが一致しなくなると、その不一致の程度に応
じて物体１９の表面に作用する接触力が減少したり、し
かもハンド１８の先端に不本意な移動力が作用したりす
るという好ましくない現象を生ずるのである。

【００１７】そこで、本発明の目的は、力制御の負荷を
軽減してロボット先端の振動発生を抑制し、力制御の一
層の高速化達成を可能にすることにある。

【００１８】

【課題を解決するための手段】本発明は、上記目的を達
成するためその原理図を図１に示すように、ロボット本
体から延びたマニプレータの先端に、力覚センサ及びハ
ンドを取り付け、前記力覚センサによってハンドに働く
外力を検出し、該検出値及び任意の指令値に基づいて、
前記ハンドと物体間の接触力をコントロールするロボッ
トにおいて、前記マニプレータの先端と力覚センサの間
を制御型の直動機構で連結し、前記検出値及び指令値に
基づいて演算される制御量で、該直動機構を駆動するよ
うに構成したことを特徴とする。

【００１９】

【作用】本発明では、ハンドと物体間の接触力制御すな
わち力制御の制御対象がマニプレータ先端の直動機構と
なり、この直動機構は、力覚センサ及びハンドといった
最小限の部分を駆動すればよいから、力制御の負担を軽
減してロボット先端の振動発生を抑制することができ、
力制御の一層の高速化達成を可能にすることができる。

【００２０】

【実施例】以下、本発明の実施例を図面に基づいて説明
する。図２〜図６は本発明に係るロボットの一実施例を
示す図であり、倣い作業用のロボットに適用した例であ
る。

【００２１】まず、構成を説明する。図２において、２
０はロボットであり、ロボット２０は、本体底面の中心
Ｏを「基準座標系」の原点とする基準座標軸Ｘ_O、Ｙ_O及
びＺ _Oを設定し、座標軸Ｚ_O上に第１関節２１を設け、こ
の第１関節２１に、第１アーム２２、第２関節２３、第
２アーム２４及び第３関節２５を含むマニプレータ２６
を連結し、さらに第３関節２５を介して制御型の直動機
構２７、力覚センサ２８及びハンド（ロボット先端と言
うこともある）２９を連結する構造である。

【００２２】図示しない制御部からの信号によって、上
記３つの関節２１、２３及び２５の回転速度及び回転量
を複合的にコントロールし、これにより、マニプレータ
２６の先端位置（図中の点Ｏ_L参照）を制御する「位置
制御」と、同じく制御部からの別の信号によって、上記
直動機構２７の直動速度及び直動量をコントロールし、
これにより、ハンド２９と物体３０間の接触力を制御す
る「力制御」とを同時並行的に行う。

【００２３】図２は、ハンド２９が任意曲率表面の物体
３０に接触する状態を示している。ここに、接触点Ｏ_W
を原点とする直角座標系「以下、倣い座標系」の座標軸
は、まず、物体３０の内向き法線と一致する軸Ｘ_W、そ
れにハンド２９の移動方向と一致する軸Ｙ_W及びこれら
の２軸に直交する方向の軸Ｚ_Wの３つの軸であり、各軸
方向の単位ベクトルは、それぞれｎ_W、ｏ_W、ａ_Wで示さ
れる。ｎ_Wはハンド２９によって物体３０を垂直に押す
力（以下、制御押圧力）であり、またｏ_Wはハンド２９
の移動力である。ａ_Wはこれら状態量の外積（ｎ_W×
ｏ_W）で与えられる。

【００２４】一方、マニプレータ２６の先端位置Ｏ_Lを
原点とする直角座標系「以下、手首座標系」の座標軸
は、直動機構２７のスライド方向と一致する軸Ｘ_L、ハ
ンド２９の移動方向と並行する軸Ｙ_L及びこれらの２軸
に直交する方向の軸Ｚ_Lであり、各軸方向の単位ベクト
ルはそれぞれｎ_L、ｏ_L、ａ_Lで示される。

【００２５】図３は制御部を含むシステムの概略ブロッ
ク図である。このシステムでは、マニプレータ２６の各
関節を制御対象とする第１の制御ループＬＰ₁と、ハン
ド２９の手首に取り付けられた直動機構２７を制御対象
とする第２の制御ループＬＰ ₂とが形成され、それぞれ
の制御ループには、制御指令生成部４０からの位置指令
値「^oＸ_r」及び位置制御パラメータ「Ｃ_P、^oｏ、^oａ」
が与えられるとともに、さらにＬＰ₂には、力指令値「^o
Ｆ_r」及び力制御パラメータ「Ｃ_f、^oｎ」が与えられ
る。なお、左上の添字ｏは、その添字を付した値が基準
座標系の記述によることを示している。

【００２６】第１の制御ループＬＰ₁は、位置制御部４
１、操作部４２及び位置検出部４３を構成に含み、これ
らの各部によって、マニプレータ２６の先端位置をフィ
ードバック制御する。一方、第２の制御ループＬＰ
₂は、位置制御部４４、操作部４５、位置検出部４６及
び力制御部４７を構成に含み、これらの各部によって、
直動機構２７に接続されたハンド２９の先端位置やハン
ド２９と物体間に働く接触力をフィードバック制御す
る。

【００２７】力覚センサ２８の力検出値は、力検出部４
８の内部で座標変換（力覚センサ座標系から基準座標系
へ）された後、法線ベクトル算出部４９に送られ、この
法線ベクトル算出部４９によって、接触点における法線
方向の単位ベクトル「^oｎ_W」が求められるとともに、移
動方向ベクトル算出部５０によって、接触点における移
動方向の単位ベクトル「^oｏ_W」が求められる。

【００２８】図４は制御部を含むシステムの詳細ブロッ
ク図である。この図において、４２は、補償器４２ａ、
Ｄ／Ａ変換器４２ｂ及びパワーアンプ４２ｃを含むとと
もに、マニプレータ２６を駆動するためのサーボモータ
４２ｄを含む第１の制御ループＬＰ₁側の操作部であ
り、サーボモータ４２ｄの回転情報すなわちマニプレー
タ２６の各関節の回転角度θ_sが、タコメータ４３ａ及
びエンコーダ＆カウンタ４３ｂを含む位置検出部４３に
よって検出される。なお、５１は、θ_sをマニプレータ
２６の先端位置Ｘ_oに変換する座標変換部、５２、５３
は、それぞれ加算部、５４は加算部５３の出力Ｖ_kをマ
ニプレータ２６の各関節の回転速度θ（ドット付）に変
換する逆ヤコビ変換部、Ｖ_Pは、位置制御部４１で作ら
れる位置指令信号である。

【００２９】一方、４５は、補償器４５ａ、Ｄ／Ａ変換
器４５ｂ及びパワーアンプ４５ｃを含むとともに、直動
機構２７を駆動するためのサーボモータ４５ｄを含む第
２の制御ループＬＰ₂側の操作部であり、サーボモータ
４５ｄの回転情報、すなわち直動機構２７のスライド情
報ｘが、速度計４６ａ及び位置センサ４６ｂを含む位置
検出部４６によって検出される。なお、５５は、ｘをハ
ンド２９の先端位置Ｘ _oに変換する座標変換部、５６〜
５８は、それぞれ加算部、Ｖ_fは力制御部４７で作られ
る速度指令信号、Ｖ_Pは、位置制御部４４で作られる位
置指令信号である。次に、作用を説明する。

【００３０】この実施例では、倣い動作の開始点
（Ｐ_B）と終了点（Ｐ_E）が与えられると、対象物体に一
定の接触力を加えながら、且つ、倣い動作時の接触点と
終点Ｐ_Eとを結ぶ直線と接触点の法線ベクトルｎとのな
す平面上に、ロボット先端の移動方向ベクトルｏが常に
存在するように制御が行われる。

【００３１】始点Ｐ_Bでの法線ベクトルｎ_Bは、始点にお
いてロボットのハンドが対象物から受ける反力Ｆに従っ
て求められる。図６（ｂ）はロボットのハンドが対象物
から受ける反力Ｆと力覚センサ座標系のＯ_s−Ｘ_s、
Ｙ_s、Ｚ_sの関係を示す図であり、力覚センサによれば、
反力Ｆの各センサ座標軸方向のそれぞれの分力ｆ_x、
ｆ_y、ｆ_zが検出されるため、反力^sＦをベクトル表示す
ると次式で与えられる。

【００３２】^sＦ＝（ｆ_x ｆ_y ｆ_z）^T ……（１）ここで、ｓは反力Ｆが力覚センサ座標系Ｏ_s−Ｘ_s、
Ｙ_s、Ｚ_sで記述されていることを示している。なお、こ
こでは説明の簡単化のために力覚センサが検出するトル
ク成分については省略する。

【００３３】力覚センサ座標系で表される法線ベクトル
^sｎ_Bは、^sＦと逆向きのベクトルであって、成分表示す
ると次式で与えられる。ここで、Ａはｆ_x ²＋ｆ_y ²＋ｆ_z ²であり、また、ベクトル
Ｆの大きさは｜^sＦ｜で与えられる。

【００３４】力覚センサ座標系で表された法線ベクトル
^sｎ_Bを基準座標系で表すためには、力覚センサ座標系か
ら基準座標系への座標変換行列が必要である。力覚セン
サ座標系の各座標軸Ｘ_s、Ｘ_s、Ｘ_sの方向の単位ベクト
ルを基準座標系で成分表示したものが次式で与えられる
とする。

【００３５】^o ｎ_s＝（ｎ_sx ｎ_sy ｎ_sz）^T ^o ｏ_s＝（ｏ_sx ｏ_sy ｏ_sz）^T ……（３）^o ａ_s＝（ａ_sx ａ_sy ａ_sz）^T ここで、例えば力覚センサ座標系の座標軸Ｘ_s方向の単
位ベクトル^oｎ_sのｏは、基準座標系で記述されているこ
とを示している。上式（３）から座標変換行列は次式で
与えられる。

【００３６】^o Ａ_s＝（^oｎ_s ^oｏ_s ^oａ_s） ……（４）この座標変換行列を用いると、基準座標系表示の法線ベ
クトル^oｎ_Bは次式で与えられる。

【００３７】^oｎ_B＝^oＡ_s ^sｎ_B ……（５）このようにして法線ベクトル^oｎ_Bを求めた後は、以下の
演算によって始点Ｐ_Bにおけるロボットハンドの移動方
向ベクトルｏ_Bを算出する。移動方向ベクトルｏ_Bは、法
線ベクトルｎ_Bに直交し、且つ始点Ｐ_Bと終点Ｐ_Eとを結
ぶ直線と、法線ベクトルｎ_Bとのなす平面上に存在する
ベクトルである。始点Ｐ_Bと終点Ｐ_Eとを結ぶ直線の方向
の単位ベクトルＬ_Bは、基準座標系における始点Ｐ_Bと終
点Ｐ_Eの位置ベクトル^oＰ_B、^oＰ_Eとを用いて次式で与え
られている。

【００３８】^o Ｌ_B＝（^oＰ_E−^oＰ_B）／｜^oＰ_E−^oＰ_B｜ ……（６）この時、倣い座標系の座標軸方向の単位ベクトルのひと
つである^oａ_Bは、始点Ｐ _Bにおいて対象物体表面に接す
る接平面上にあり、^oｎ_Bと^oＬ_Bとを用いて次式で与えら
れている。

【００３９】^o ａ_B＝（^oｎ_B×^oＬ_B）／｜^oｎ_B×^oＬ_B｜ ……（７）但し、^oｎ_B≠^oＬ_B さらに、ロボットハンドの移動方向ベクトル^oｏ_Bは、同
様に接平面上のベクトルとして次式で与えられる。

【００４０】^oｏ_B＝^oａ_B×^oｎ_B ……（８）本実施例の力制御ロボットでは、図４の制御指令生成部
４０からの制御指令がマニプレータ２６を操作する第１
の制御ループＬＰ₁の位置制御部４１に与えられると共
に、直動機構２７を操作する第２の制御ループＬＰ₂の
位置制御部４４及び力制御部４７にも与えられるが、力
指令値だけについては第２の制御ループＬＰ₂のみに与
えられる。

【００４１】力指令値^oＦ_rは、図６（ｂ）に示すよう
に、力の大きさがＦ_r、方向がｎ_Bの力であり、次式で与
えられる。^o Ｆ_r＝Ｆ_r×^oｎ_B ……（９）また、始点Ｐ_Bでのロボットの位置指令値は、始点Ｐ_Bか
らの相対位置指令として与えられる。始点Ｐ_Bからの移
動方向は、前式（８）の移動方向ベクトル^oｎ_Bで与えら
れており、これを用いて位置指令値は次式で与えられ
る。

【００４２】^oＸ_r＝α×^oｏ_B ……（１０）なお、αは、倣い座標系の修正を行うべき距離に相当す
る適当な定数である。以上は、始点Ｐ_Bにおける法線ベ
クトルｎ_B、移動方向ベクトルｏ_B、力指令値^oＦ_r及び位
置指令値^oＸ_rの設定であるが、始点Ｐ_B以後の倣い動作
途中点における法線ベクトルｎ_i、移動方向ベクトル
ｏ_i、力指令値^oＦ_r及び位置指令値^oＸ_rの設定も同様な
手順で行われる。すなわち、図５にその演算フローを示
すように、まず、ステップ６０で各種の初期パラメータ
を取込んだ後、ステップ６１で力覚センサによって反力
Ｆを検出する。次いで、ステップ６２で式（２）を用い
て法線ベクトル^sｎ_Bを算出し、ステップ６３で座標変換
行列^oＡ_sを算出する。そして、ステップ６４で式（４）
により^sｎ_Bを座標変換して^oｎ_Bを算出し、ステップ６５
で異常発生を判定して正常であれば、ステップ６６で式
（７）により単位ベクトル^oａ_Bを算出する。次に、式
（８）を用いて移動方向ベクトル^oｏ_Bを算出し、制御指
令生成部４０に転送する。ステップ６８では、制御指令
生成部４０で手首機構制御用の力指令値Ｆ_r及び位置指
令値を発生し、第２の制御ループＬＰ₂の力制御部４７
及び位置制御部４４に転送して、これらの制御部により
直動機構２７を駆動する。さらに、ステップ６９では、
法線ベクトル^oｎ_Bに基づいてマニプレータ２６の位置指
令値等を発生し、これを第１の制御ループＬＰ₁の位置
制御部に転送してマニプレータ２６を駆動する。

【００４３】以上のように、本実施例では、マニプレー
タ２６の先端に取り付けた直動機構２７を唯一のアクチ
ュエータとする力制御としたので、この直動機構２７は
力覚センサ２８やハンド２９といった最小限の部分、言
い替えれば軽量且つ剛性の高い部分だけを負担して駆動
すればよいから、ロボット先端の振動発生を抑制するこ
とができ、力制御の一層の高速化達成を可能とすること
ができる。

【００４４】なお、上記の実施例では、アーム剛性の高
いロボットを対象としているが、これに限るものではな
い。例えば、ハンドにあたかもバネを有するが如き弾性
的な反応を起こさせる、いわゆるコンプライアンス制御
機能を備えたロボットにも適用できる。図７〜図１０は
コンプライアンス制御型ロボットへの適用実施例図であ
り、上記の実施例と相違する部分には新たな符号を付し
てある。

【００４５】一般に、コンプライアンス制御では、次式
（１１）に示すように、ハンドの目標先端位置ｘｄとハ
ンドの実際の先端位置との差（ｘｄ−ｘ）に応じた接触
力Ｆを発生し、実際の接触力＝目標接触力となるように
（Ｆ＝Ｆ_r）目標位置ｘｄを微修正しながら接触力Ｆを
制御する。

【００４６】Ｆ＝ｋ_p（ｘｄ−ｘ） ……（１１）但し、ｋ_pは位置フィードバックゲインである。

【００４７】こうしたコンプライアンス制御型のロボッ
トにおいても、ロボット先端に振動が発生すると押圧力
の方向が法線方向からずれ易くなる。すなわち、図１０
に示すように、先端位置ｘと目標位置ｘｄとを結ぶ直線
が物体表面の法線方向ｎと異なっていると、実際に物体
に加えられる力は、押圧力Ｆ_rのｎ方向への分力Ｆ_rnと
なり、Ｆ≠Ｆ_rnとなって正しい接触力を得られない。

【００４８】そこで、検出された先端位置ｘと力の目標
値Ｆ_rとにより、次式（１２）に従って目標位置ｘｄを
求める。ｘｄ＝ｘ＋Ｆ_r／ｋ_p ……（１２）このコンプライアンス制御型ロボットは、図７に示すよ
うに、第２の制御ループＬＰ₂にコンプライアンス制御
部７０を備える点で前記実施例と相違する。すなわち、
図７の詳細構成を示す図８において、コンプライアンス
制御部７０は、位置フィードバックゲインｋ_p設定部７
１、速度フィードバックゲインｋ_v設定部７２、力フィ
ードバックゲインｋ_f設定部７３及び加・減算部７４〜
７８を含み、目標位置Ｘ_dと実際位置Ｘ_oとの偏差にゲイ
ンｋ_pを与えるとともに、目標速度Ｘ_(dot)dと実際速度
Ｘ_(dot)との偏差にゲインｋ_vを与えてそれらの加算値ａ
を求め、そしてその加算値ａと実際の力の検出値Ｆ_oと
の偏差に対してゲインｋ_fを与えた後、再び加算値ａと
加算してその加算値ｂから力の制御量Ｆ’を生成する。
なお、図７において、７９は直動機構２７のスライド量
Ｘを基準座標系に変換してハンドの位置Ｘ_oを求める座
標変換部、８０はスライド速度を基準座標系に変換して
ハンドの速度Ｘ_(dot)oを求める座標変換部、８１は制御
値Ｆ’を直動機構２７の駆動力に変換する座標変換行列
部である。

【００４９】ここで、制御値Ｆ’は、（ａ−Ｆ_o）＋ａ×ｋ_f ……（１３）で与えられる。すなわち、（１３）式は力の検出値Ｆ_o
が大きいほど力の制御値Ｆ’が小さくなることを示して
いるから、直動機構２７を含むハンドは、あたかもバネ
を有する如き弾性的な反応を示すこととなり、いわゆる
コンプライアンス制御機能を発揮するのである。

【００５０】図９はコンプライアンス制御を行う場合の
フロー図である。前記実施例のフロー図（図５参照）と
の相違点は、ステップ８０、８１において、現在位置Ｘ
_oを検出し、目標位置Ｘ_dを算出した後、これらのＸ_d、
Ｘ_(dot)dを転送して直動機構２７を含むハンドを駆動す
る点に集約される。なお、６０番台のステップは図５と
共通である。

【００５１】以上のように、本発明を適用したコンプラ
イアンス制御型のロボットにおいても、マニプレータ２
６の先端に取り付けた直動機構２７を唯一のアクチュエ
ータとしてコンプライアンス制御を行うことができ、駆
動負担を軽減してロボット先端の振動発生を抑制でき
る。したがって、実際に物体に加えられる力の方向と法
線方向とを一致させることができ、設定値どおりの正し
い接触力を与えることができる。

【００５２】

【発明の効果】本発明によれば、マニプレータの先端と
力覚センサの間を制御型の直動機構で連結したので、力
制御の負荷を軽減してロボット先端の振動発生を抑制で
き、力制御の一層の高速化達成を可能にすることができ
る。

【図面の簡単な説明】

【図１】本発明の原理図である。

【図２】一実施例の構成図である。

【図３】一実施例の制御系ブロック図である。

【図４】一実施例の詳細ブロック図である。

【図５】一実施例の処理フロー図である。

【図６】一実施例の力の関係を示す図である。

【図７】他の実施例の制御系ブロック図である。

【図８】他の実施例の詳細ブロック図である。

【図９】他の実施例の処理フロー図である。

【図１０】コンプライアンス制御における不具合説明図
である。

【図１１】従来例の構成図である。

【図１２】従来例の座標系設定アルゴリズムの概念図で
ある。

【図１３】従来例の倣い動作の概念図である。

【図１４】従来例の不具合説明図である。

【符号の説明】

２０：ロボット２６：マニプレータ２８：力覚センサ２９：ハンド２７：直動機構

Claims

【特許請求の範囲】【請求項１】ロボット本体から延びたマニプレータの先
端に、力覚センサ及びハンドを取り付け、前記力覚セン
サによってハンドに働く外力を検出し、該検出値及び任
意の指令値に基づいて、前記ハンドと物体間の接触力を
コントロールするロボットにおいて、前記マニプレータの先端と力覚センサの間を制御型の直
動機構で連結し、前記検出値及び指令値に基づいて演算
される制御量で、該直動機構を駆動するように構成した
ことを特徴とするロボット。