JPS6316982A

JPS6316982A - 多関節マニピユレ−タの制御装置

Info

Publication number: JPS6316982A
Application number: JP15760086A
Authority: JP
Inventors: 正憲鈴木; 関戸　明仁; 栄杉山
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 1986-07-04
Filing date: 1986-07-04
Publication date: 1988-01-23
Anticipated expiration: 2009-12-21
Also published as: JPH06104307B2

Abstract

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

Description

【発明の詳細な説明】〔産業上の利用分野〕本発明は多関節マニピュレータの制御装置に係り、特に
マスクスレーブ方式で遠隔より機器の分解・組立等の作
業を実施するのに好適なマニピュレータの制御装置に関
するものである。

〔従来の技術〕

従来のマスク・スレーゾ式多関節マニビ、レーク装置に
は、特公昭５９−４６７５３号に記載のように、マスタ
アームの制御装置に非線形変換器を付加し、スレーブア
ームが高剛性の物体に作用したときに生じる反力を小さ
くしてマスタアームに伝達するようにしたものがある。

これはマスタアームに生じるトルクの急激な変化を小さ
くするためのものである。

〔発明が解決しようとする問題点〕

上記従来技術では、スレーブアーム自体の剛性の制御は
されておらず、マスクに生じている力よりも大きい力が
スレーブに生じるという問題があった。マニピュレータ
に高速な位置応答をもたせるためにはマニピュレータの
サーボ剛性を大きくする必要があるが、他方、マニピュ
レータによって嵌め合い作業などを実施する場合には、
マニピュレータのサーボ剛性が高いと、極めて正確な位
置決め精度が要求されるという問題がある。しかし、マ
スタ・スレーブマニピュレータの場合、極めて正確な高
い位置決め精度はマスク・マニピュレータの操縦者にと
って大きな負担となり、マニピュレータの操作性悪化の
原因となる。

本発明の目的は、軽負荷を扱っているときは操縦者の位
置指令に高速で応答させ（すなわちマニピュレータのサ
ーボ剛性を大にし）、慣性の大きい対象物に作用すると
きは、マニピュレータを柔軟にして、マニピュレータの
操作性を向上させることにある。

〔問題点を解決するだめの手段〕

上記目的を達成するため、本発明は、関節角度検出手段
の出力と関節目標角度設定手段の出力との偏差に応じて
関節駆動モータを該偏差を零にする方向に駆動する位置
サーボ系ルーゾを形成している多関節マニピュレータの
制御装置において、関節角速度検出手段と、規定の慣性
モーメントの負荷を扱っているときの上記マニピュレー
タの関節角速度を推定するオブザーバと、この推定され
た関節角速度と上記関節角速度検出手段で検出された関
節角速度との差の絶対値が所定値を超えたとき上記位置
サーボ系ループのループケ゛インヲ減少させる可変係数
手段と、を備えたことを特徴とするものである。

〔作用〕

マニピュレータが規定以上の慣性モーメントを有する対
象物に作用するときには、位置サー日ζ系のルージグイ
ンが減少し、その結果、サーボ剛性が低下するので、マ
ニピュレータの各関節角を柔軟に制御できるようになる
。一方、マニピュレータの負荷が規定の慣性モーメント
以下のとき、ルージグインは位置応答が臨界制動状態に
なるルージグインに等しくされ、それによって高速な位
置応答を得ることができる。

〔実施例〕

以下、マスク・スレーブ式マニピュレータにおける本発
明の一実施例を第１図〜第３図を用いて説明する。

第３図は制御装置の全体構成を示したものである。アー
ム２１・ｌの一端は架台１９に固定されている。アーム
２１・１の他の一端には関節２０・２があり、アーム２
１・２がアーム２１・ｌに対して関節２０・２を回転中
心として旋回できるように接続されている。以下同様に
、アーム２１・１はアーム２１・ト１に対して関節２０
・１を中心に旋回できるように接続されており、このよ
うにして全体がｎヶの関節を有するスレーブアーム２を
構成している。各スレーブ関節２０−１には、モータ３
５が減速機３６を介して接続されており、モータ３５の
出力軸の回転によってアーム２１・ｌが関節２０・１を
中心に旋回するように構成されている。エンコーダ３４
は、モータ３５の軸に接続されており、モータ３５の回
転角を検出し、位置カウンタ３２と速度カウンタ３１に
よって関節角Ｘと関節角速度−をディジタル値の形で検
出することができる。

カウンタ３１．３２は計算機３０に入力される。

計算機３０の出力の一つは、Ｄ／Ａ変換器３３を介して
モータ３５に入力されるように構成されている。他の関
節２０・１〜２０＠ｉ−１，２０・１＋１〜２０・ｎの
駆動機構は、関節２０・量のそれと基本的に同一である
ので説明は省略する。

一方、アーム４１・１は架台３９に固定されており、一
端には関節４０・２が結合されている。アーム４１・２
は、関節４０・２を中心としてアーム４０・ｌに対して
旋回できるように、関節４０・２を介してアーム４０・
ｌに結合されている。以下同様にアーム４１・ｌは関節
４０−１を中心としてアーム４０・ｉ−１に対して旋回
できるように、関節４０・１を介してアーム４０−１−
１に結合され、このようにして全体がｎヶの関節を有す
るマスク・アーム４を構成しく６）ている。各マスタ関節４０・１にはエンコーダ３７が装
着されており、マスク関節角を検出できるようになって
いる。エンコーダ３７からの信号はカウンタ３８を介し
て計算機３０に入力されている。

他のマスク関節も同じ構成である。

マスタアーム４とスレーブアーム２とは相似形であり、
マスク関節４０１とスレーブ関節２０１とが対となって
位置制御系が構成されている。

操縦者が関節角がｘｒとなるようにマスタ関節４０・ｌ
を回転させると、その回転角はエンコーダ３７によって
検出され、カウンタ３８を介して計算機３０に入力され
、スレーブ関節２０・１の関節角の目標値ｘｒとなる。

同時に計算機３０に入力されたスレーブ関節角Ｘと上記
ｘｒとの差に応じたスレーブトルク指令値τが計算機３
０から出力される。スレーブトルク指令値τ、はＤ／Ａ
変換器３３を介してモータ３５に与えられる。モータ３
５はマスタ関節４０・１とスレーブ関節２０・ｌの関節
角の差Ｘｒ−Ｘが減少する方向に回転する。このように
して、対をなすスレーブ関節角がマスク関節角に追従す
る位置制御系が構成されている。この位置制御系を計算
機内部での演算を含めてブロック線図で表わしたものを
第１図に示す。

第１図において、破線で囲まれた部分３０は、計算機３
０で行なわれる演算を示す。スレーブ関節角の目標値Ｘ
ｒは、マスク関節角を表わすカウンタ３８からサンプラ
５ｆを介して得られた信号である。目標値Ｘｒおよびサ
ンシラ５ｃから得られたスレーブ関節角Ｘは減算器８ａ
に入力され、その差ｘｒ−ｘが減算器８ａから出力され
、可変係数器７に入力される。可変係数器７で信号はＡ
倍され、その出力Ａ（ｘｒ−ｘ）はサンシラ５ｂから得
られたスレーブ関節角速度；とともに減算器８Ｃに入力
される。減算器８ｃからはそれらの差Ａ（ｘｒ−ｘ）−
ｘが出力される（この出力をｉと表わす）。係数器９＆
はこの姿、を入力とし、トルク目標値τ、を出力する。

このトルク目標値τはサンプラ５ａを介してＤ／Ａ変換
器３３ｍである０次ホールドに入力される。その出力は
図示していないアンプを介してモータ３５に入力されて
いる。破線で囲んだ部分３５はモータ３５及びその負荷
を数学的に積分器１０ｍ、１０ｃで表わしたものであり
、実際の積分器ではない。積分器１０ｍはトルクと角速
度の関係を示したものであり、Ｊはモータを含めた負荷
の関節軸上の慣性モーメントである。積分器１０ｃは角
速度と角度の関係を表わしたものである。

以上の構成により、目標値ｘｒにスレーブ関節角Ｘが追
従する位置制御系が構成されている。

また、第１図においてオブザーバ６は目標値Ｘ。

を入力とし、推定量である角速度量を出力する。

０　。

減算器８ｅは、前記角速度量と推定角速度；　を入力と
し、その差（角速度推定誤差）Δ＝、、　−ｘｏを出力
する。出力Δはサンプラ５ｅを介して可変係数器７に入
力される。

オブザーバ６の内部は、前述の位置制御系をシミュレー
トした本のであり、構成もほぼ同じである。異なる点は
慣性モーメントの値を規定の慣性モーメントの負荷を持
った場合の値Ｊ０とした点である。Ｊの値は任意に設定
可能である。

以下オブザーバ６の構成を説明する。減算器８ｂは、目
標値ＩＩｒとサンプラ５ｇを介して得られる角度推定量
Ｉ　とを入力とし、その差ｘｒ−Ｘ０を出力する・減算
器８ｂの出力Ｘ　ｒ　　Ｘ　ｏは係ａ器９ｂに入力され
る。係数器９ｂのゲインはＡ。である。このゲインＡ０
の値は、マニピュレータの負荷が規定の慣性モーメント
を有するとき位置応答を臨界制動もしくは臨界制動に近
い状態にするようなルーゾｒインが得られる値に設定す
る。減Ｘ器８ｄは、係数器９ｂの出力Ａｏ　（ｘ　ｒ−
ｘ　ｏ　）とサンプラ５ｈを介して得られる角速度推定
量−とを入力し、その差Ａｏ（ｘｒ−１０）−Ｘ、を出
力する（この出力をｉ。と衣わす）、係数器９ｃは減算
器８　ｄ’の出力＝、０を入力とし、これを３０倍し、
サンプラ５ｄを介して０次ホールド３３ｂに出力する。

０次ホールド３３ｂの出力は積分器１０ｂに入力される
。積分器１０ｂは、慣性モーメントＪ０を有する系のト
ルクと角速度の関係を表わしたものであり、ここから角
速度推定誤差が出力される。積分器１０ｄは角速度と角
度の関係を表わしたものであり、ここから角度推定量Ｘ
　が出力される。

可変係数器７は、第２図に示されるように、飽和要素を
有する非線形回路１１と乗算器１２とから構成されてい
る。非線形回路１１は角速度推定誤差Δを入力され、ｒ
インＡを出力する。乗算器１２はダインＡと角度偏差ｘ
−ｘを入力し、その積Ａ（ｘｒ−ｘ）を第１図中の減算
器８Ｃへ出力する。

非線形回路１１の出力Ａは、入力Δがゼロのときオブザ
ーバの前記係数器９ｂのゲインＡに等しく、角速度推定
誤差Δの絶対値が後記ゆらぎ分Δ。

より小さい場合はぼ八〇に等しく、１Δ１がΔ。を超え
るとΔに関して単調減少関数となり、１Δ１が太きくな
るにつれてＡより小さい一定値Ａｌに漸近するようにな
っている。

次に動作について説明する。減算器８ｃ、係数器９ｍ、
サンプラ５ａ、０次ホールド３３１、積分器１０ｍ、サ
ンシラ５ｂで構成される閉ループは、局所速度フィード
バック系を構成しており、目標角速度−と角速度−の差
が０になるように制御される。この局所速度フィードバ
ック系を含んだ減算器８ｍ、可変係数器７、積分器１０
　ｃ、サンプラ５ｃからなる閉ループは前述のように位
置フィードバック系を構成しており、目標角度ｘｒと角
度Ｘとの差がゼロになるように制御される。

この場合、系の剛性は、静的には −Ｘｒで与えられる。

また、位置フィードバック系の伝達関数Ｇ（Ｓ’）は、
簡単のため０次ホールドを省略するとＧ（Ｓ）＝□　　　　　　　■ Ｊ８　　＋ＢＳ＋Ａと２次遅れ系で表わされる。一般に２次遅れ系（固有角
周波数ω、減衰係数ζ） ω７２における臨界制動状態（応答の立ち上りが速く、オーバ
シュートがない状態）はζ＝ｌ／／ｒ２で与えられる。

０．０式を比較して・。−ム丙　　　　　　　　　　■ ζ＝Ｂ／２ω。　　　　　　　　　　　　　　■ζ＝１
／６　　と選ぶとするとＢは次式で与えられる。

すなわち、オブザーバ６の係数器９ｃのゲインＢ。

は０式からに選定する。ここに、Ｊｏはスレーブアーム２が把持し
ている負荷を含めた慣性モーメントとする。

スレーブアーム２が通常の状態のとき、すなわち規定以
下の慣性モーメントの負荷と接しているときは、角速度
−とオブザーバ推定角速度餐　との差は理論的には零で
ある。しかし、両者は関節駆動機構の摩擦やアーム自重
などの外乱により僅かにずれることが予想される。そこ
で、この僅かなすれ即ちゆらぎ分をΔ。とじて、角速度
；とその推定量；。の誤差Δ＝；−；。がΔ。以下のと
きには前記の如くゲインＡをＡｏに等しくしておくこと
によって、位置フィードバック系は臨界制動状態で動作
する。高速で応答させるためにはダインＡを大きくすれ
ばよい。

一方、スレーブアーム２が通常の状態でないとき、すな
わち、慣性モーメントの大きい対象物に接触していると
きには、系の応答は次のように変化する。負荷の慣性モ
ーメン）Ｊが規定の慣性モーメントＪ０に比べて非常に
大きくなると、推定誤差Δの絶対値は非常に大きくカリ
、Δ。を越えると、前記の如くｒインＡはΔの絶対値と
ともに小さくなる。ｒインＡが減少すると、■、■式よ
り固有角周波数ω。は小さくなり、減衰係数ζも小さく
なるので系の応答はゆるやかになる。すると推定誤差Δ
はますます大きくなシ、ｒインＡの減少を加速する。し
かし、このままではｒインＡは限りなく小さくなってし
まうので、やがてスレーブの関節は動かなく彦ってしま
う。そこで、前記の如くゲインＡをある値ＡＩ以下にな
らないようにすることによって、上記の事態は避けるこ
とができる。

Ａ１の値はマニピュレータの作業内容に応じて適宜選ぶ
ようにする。

このような機態は、例えばボルト等を穴に挿入する作業
などの嵌め合い作業をスレーブアームを用いて行なう際
に効果がある。すなわち、ボルト等を単に持っていると
きは高剛性で高速応答をし、固定された穴の周辺にＣル
トが触れるとスレーブアームは柔軟になり、可変コンプ
ライアンスな嵌め合いが可能になる。

以上詳述したように、本実施例によれば加速的にアーム
の柔軟さを早め、かつ一定のアームの柔軟さを得ること
ができる効果がある。

第４図は変形実施例であり、第１図中、可変係数器７を
係数器９ｄで置きかえ、係数器９ａを可変係数器７′で
置きかえ、ゲインＡを一定にし、可変係数器７′のｒイ
ンＢをＢｏからＢ＋　’＆で変えるように構成した以外
は前述の実施例と同じである。

本変形実施例によれば、固有角周波数ω。を一定にでき
る（°、“■式）という効果がある。

前記の実施例では計算機内部の計算時間の遅れが生じて
、ゲインＡをあまり大きくできない、しかし、極めて高
速な計算機及びＤ／Ａ変換器を用いれば、サンプリング
周期Ｔを極めて短くできるので完全な２次遅れ系となっ
て、第５図のように第１図のブロック線図から０次ホー
ルド回路３３ｍ。

３３ｂを除去した構成の実施例が可能である。本実施例
によれば、ゲインＡを任意に太きくできるので、極めて
高剛性のアームが実現できる効果がある。

なお、前述の推定誤差Δの代りに、角度Ｘと角度推定量
Ｘ　との差Ｘ　−Ｘｏを用いても類似の効果が期待でき
る。

以上においては計算機を用いた制御系を構成したが、ア
ナリグ素子で制御系を構成しても同一の効果が得られる
。

〔発明の効果〕

本発明によれば、負荷の慣性モーメントの大きさＫよっ
てマニピュレータの関節の位置制御系のルーググインを
変えることができるので、マニピュレータが慣性モーメ
ントの小さい負荷を有する場合にはマニピュレータのス
レーブの各関節が高速に位置応答し、マニピュレータが
慣性モーメントの大きい負荷を有する場合にはマニビュ
レーク各関節を柔軟にする効果がある。従って、マニピ
ュレータで機器の分解や組立などの作業を行う場合に、
操作者に大きな負担をかけずにマニピュレータの操作性
が向上する利点がある。

【図面の簡単な説明】

第１図は本発明の一実施例の構成を示すブロック線図、
第２図はその可変係数器７の構成を示すブロック線図、
第３図は、本発明の適用される多関節マニピュレータの
一実施例の全体構成図、第４図は本発明の変形例を示す
ブロック縮図、第５図は本発明の更に他の実施例を示す
ブロック線図である。２…スレーブアーム、４…マスターアーム、５ａ〜５ｇ
・・・サンシラ、６・・・オシデーパ、７．７′・・・
可変係数器、１１・・・非線形回路、１２・・・乗算器
、　　　　３０・・・計算機。

Claims

【特許請求の範囲】１、関節角度検出手段の出力と関節目標角度設定手段の
出力との偏差に応じて関節駆動モータを該偏差を零にす
る方向に駆動する位置サーボ系ループを形成している多
関節マニピュレータの制御装置において、関節角速度検
出手段と、規定の慣性モーメントの負荷を扱っていると
きの上記マニピュレータの関節角速度を推定するオブザ
ーバと、この推定された関節角速度と上記関節角速度検
出手段で検出された関節角速度との差の絶対値が所定値
を超えたとき上記位置サーボ系ループのループゲインを
減少させる可変係数手段と、を備えたことを特徴とする
多関節マニピュレータの制御装置。２、上記可変係数手段は、前記推定された関節角速度と
関節角速度検出手段で検出された関節角速度の差の絶対
値が前記所定値を超えたとき、前記ループゲインを該絶
対値の増大につれて零でない一定値に漸近する単調減少
関数となるように変化させる特許請求の範囲第１項記載
の多関節マニピュレータの制御装置。