JPH0446785A - マニピュレータの位置と力のハイブリッド制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〈産業上の利用分腎〉 本発明は、マニピュレータに適用する位置と力のハイブ
リッド制御装置に関する。

〈従来の技術〉 関節形マニピュレータは、一般に人間の腕と似た形をし
ており、その各関節にはアクチユエータ、速度センサ、
位置センサが備えられまた、手首部には力センサが備え
られている。また、他機覆のマニピュ　レータとして、
各関節にトルクセンサが装着されている力ゝ、あるいは
モータ電流でトルクを推定するものである。

ここで従来の装置を第３図に示す。この第３図において
、位置制御系は、角度から直交座標系の位置を求める角
度位置変換手段１゜位置制御を行う方向を選択する位置
制御選択手段２、位置制御ゲイン手段３および速度角速
度変換手段４、更にはマニピュレータ本体２０内に備え
られるサーボアンプ９、アクチュエータ１０、位置セン
サ１２より構成される。

位置目標ｘ１は、マニピュレータの位置センサ１２から
出力θを角度位置変換手段１に入力してその出力として
得られる直交座標（Ｏ−ｘｙｚ座標系）の位置Ｘと比較
され、位置偏差△Ｘが求められる。

位置偏差ΔＸは位置制御選択手段２に入力され、位置制
御を行なう方向の偏差のみが位置制御選択手段２から出
力され、位置制御ゲインを乗することで位置制御のため
の修正速度Ｖｐが導出される。一方、力制輝系は、手首
座標系で検出された力を別な直交座標系（ｏ−ｘｙｚ座
標系）の力に変換する。力変換手段７、力制御を行う方
向を選択する力制御選択手段５、力制御ゲイン手段６お
よび速度角速度変換手段４、更にはマニピュレータ本体
２０内に備えられるサーボアンプ９、アクチュエータ１
０、手首センサ８より構成される。

力目標値Ｆ　はマニピュレータの手首センサ１３からの
出力Ｆ、を力変換手段７に入力してその出力として得ら
れろ直交座標系（〇−ｘＹＺ座標系）の力Ｆと比較され
力偏差ΔＦが求められる。

力偏差ΔＦは力制御選択手段５に入力され力制御を行な
う方向のみの偏差が力制御選択手段５から出力され力制
御ゲイン手段６に送られ力制御のための修正速度ｖＦが
導出される。

修正速度ｖＰとＶｌは加算され速度角速度変換手段４に
入力される。その人力転は、マニピュレータ２０の各関
節の対する速度指令シ、は速度センサ１１から検出され
る速度さと比較されサーボアンプ９は速度フィードバッ
クにより汐がさ、と一致するようにアクチュエータ１０
を駆動する。

〈発明が解決しようとする課題〉 第３図による従来の方式においては、位置制御・力制御
の方向は第２図に示す地球座標系（ｏ−ｘｙｚ座標系）
に基づく並進と回転とになる。かかる場合、ピン１３の
挿入作業を行なおうとした場合、ピンの軸方向と穴１４
の方向とを一致させる位置制御を行ない、それ息外の方
向は力制御することて達成できる。

しかしながら、この地球座標系は空間的に固定された座
標系であるので、ピン１３の挿入方向と地球座標系のＸ
軸、Ｙ軸、又はＺ軸が一致しない限り、上述の作業は行
なえない。

そこで、本発明は、地球座標系と作業方向が一致しなく
とも円滑に作業を行なうようにしたマニピュレータの位
置と力のハイブリッド制御装置の提供を目的とする。

く課題を解決するための手段〉 上述の目的を達成する本発明は、手首に力センサを備え
各関節の速度を入力する関節形マニピュレータに対して
このマニピュレータの地球座標系での位置と力の輪差を
求める手段と、この偏差を上記マニピュレータの手首座
標系からみた偏差に変換する手段と、位置制御および力
制御を行なう方向を選択する手段と、手首座標系からみ
た位置あるいは力の偏差を修正速度に変換する手段と、
上記修正速度を上記マニピュレータの各関節の回転速度
に変換する手段とからなる位置と力のハイブリッド制御
装置において、位置制御および力制御の方向がマニピュ
レータの手首座標系に基づく並進と回転となることを特
徴とする。

〈作　　　用〉 マニピュレータの地球座標系での位置と力の偏差を求め
、次にこの偏差をマニピュレータの手首座標系の偏差に
変換する。そしてこの手首座標系の偏差に対して位置制
御ゲイン、力制御ゲインを乗じろことにより手首座標系
での修正速度を求め、この修正速度をマニピュレータの
各関節の回転速度変換し関節形マニピュレータに出力す
る。

したがって、位置制御・力制御の方向は手首座標系に基
づく回転と並進となり、位置制御の方向を手首座標系の
２８軸方向、それ以外の方向を力制御とすれば、ｚｌ、
１方向以外については穴からピンが受けろ力を減少する
方向にビンがなじみながら挿入作業が行なわれる。

く実　施　例〉 ここで、第１図、第２図を参照して本発明の詳細な説明
する。実施例として第２図に示す関節形マニピュレータ
２０への適用例について述べる。この関節形マニピュレ
ータ２０におうで、第１図、第２図に示すように、各関
節には、サーボアンプ９、アクチュエータ１０、速度セ
ンサ１１、及び位置センサ１２が備えられ、また、手首
部には手首力センサ８が備えられている。この力センサ
８は手首に加わる３方向（Ｘ、方向、Ｙｕ力方向Ｚ、方
向）の力を検出するものである。

第１図にて、Ｓは位置制ｆ１１Ｉ／力制御の選択指令で
あり、Ｓ中の要素を、１とすれば力制御、０とすれば位
置制御となる。

また、位置目標値Ｘは、マニピュレータの位置センサ１
２からの出力Ｑを角度、位置変換手段に入力して得られ
る地球座標系（０−ＸＹＺ座標系）の位置Ｘと比較され
、地球座標系による位置偏差ΔＸが求められる。

次に、位置制御系にあっては、ΔＸを入力する速度逆変
換手段２４が備えられる。この速度逆変換手段２４はΔ
Ｘを手首座標系における位置偏差Δｘ２に変換する。

位置偏差ΔＸ、、ｌは、位置制御選択手段２に入力され
、この位置制御選択手段２では位置制御を行なう偏差の
みが出力される。

位置制御ゲイン手段２２では位置制御のための手首座標
系での修正速度■　が導出されろ。

ついで、速度変換手段２５では修正速度ｘｐｕが地球座
標系での修正速度■２に変換される。

力制御系について述べる。

手首座標系におけろ力目標値Ｆ１．全力変換手段７によ
り地球座標系における力目標値Ｆに変換する。そして、
この力目標値Ｆ１をマニピュレータの手首力センサ８の
出力Ｆ１．ｌから力変換手段７を通じて得られる地球座
標系の力Ｆと比較され、力偏差ΔＦが求められる。

ついで、位置制御系と同様に、力偏差ΔＦは速度逆変換
手段２４により手首座標系における力偏差ムＦ−こ変換
され、この力偏差ΔＦ。

は力制御選択手段５に入力されて力制御を行う方向の偏
差のみが出力され、ついで力制御ゲイン手段２３にゲイ
ンを乗することで、力制御のための手首座標系での修正
速度ｖ、ｌ、が導出され、この修正速度ＶＦに変換され
ろ。

位置制御系及び力制御系の各修正速度Ｖ　と■１は、互
いに直交方向の速度であり、これらは加算されて速度・
角速度変換手段４に入力される。この速度・角速度変換
手段４の出力θ、はマニピュレータでの各関節への速度
指令となる。速度指令θ２は速度センサ１１から検出さ
れる速度すと比較され、サーボアンプ９は速度フィード
バックによりδが汐　に一致するようにアクチュエータ
１０を駆動する。

第１図に実施例では、手首の力センサを備えたものであ
るが、これに代えて各関節のトルクセンサあるいはモー
タ電流でトルクを推定し、手先に加わるカを求める場合
も本発明は含まれる。

〈発明の効果〉 以上説明したように本発明によれば、位置制御系、及び
力制御系においてハイブリッド制御を提供することによ
り、地球座標系を手首座標系に基づき処理することによ
り、例えばビン挿入作業が容易となる。

【図面の簡単な説明】

第１図は本発明の一実施例の構成図、第２図は関節形マ
ニピュレータの作業説明図、第３図は従来例の構成図で
ある。 図　　　中、 ２２は位置制御ゲイン手段、 ２３は力制御ゲイン手段、 ２４は速度逆変換手段、 ２５は速度変換手段である。

Claims (1)

1. 【特許請求の範囲】 手首に力センサを備え各関節の速度を入力する関節形マ
   ニピュレータに対してこのマニピュレータの地球座標系
   での位置と力の偏差を求める手段と、 この偏差を上記マニピュレータの手首座標系からみた偏
   差に変換する手段と、 位置制御および力制御を行なう方向を選択する手段と、 手首座標系からみた位置あるいは力の偏差を修正速度に
   変換する手段と、 上記修正速度を上記マニピュレータの各関節の回転速度
   に変換する手段とからなる位置と力のハイブリッド制御
   装置において、 位置制御および力制御の方向がマニピュレータの手首座
   標系に基づく並進と回転となることを特徴とするハイブ
   リッド制御装置。