JPS6277608A - 移動体制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔目次〕 卑既要 産業上の利用分野 従来の技術 発明が解決しようとする問題点 問題点を解決するための手段　（第１図）作用 実施例 （ａ）一実施例の構成の説明 （第２図、第３図） （ｂｌ−実施例の動作の説明Ｃ （Ｃ）適用例の説明　　　（第４図） （ｄ）他の実施例の説明　（第５図） 発明の効果 〔概要〕 移動体を駆動する駆動手段を移動指令と力指令とに基づ
いて制御する移動体制御方式において、制御対象のモデ
ルと、制御対象の偏差を積分する積分手段を設け、状態
量をモデルから得るとともに、外力に釣り合うカを積分
手段から得るようにして、高速で安定な移動及び力制御
を可能とするものである。

〔産業上の利用分野〕

本発明はロボット等の移動体を移動指令と力指令とによ
って制御するだめの移動体制御方式に関し、特に高帯域
までサーボ特性を向上することのできる移動体制御方式
に関する。

近年、人間の行っている作業を自動化するためロボット
が開発され、更にロボットを人間の手や腕と同等な働き
に制御する技術の開発努力がなされている。″ ロボットに人間の手や腕と同様な働きを行わせることが
できれば、それだけ高度な作業が出来、極めて便利であ
る。

一般にロボットの制御はアクチュエータ（アーム）の位
置制御又は速度制御の移動制御が基本であり、アームの
先端の作業用ハンドを指令位置に位置決めして作業を行
わせている。

このような位置又は速度の制御のみでは、柔軟性に欠け
るため、人間の腕、手と同等の働きを行わせることは不
十分であり、他の物体から影響を受ける作業には適応性
がない。このためアクチュエータの出す力（又は外から
の反力）を検出して、アクチュエータを力制御する必要
がある。

〔従来の技術〕

力を制御するものとして、従来は位置制御用のアクチュ
エータに一定の剛性を持たせ、仮想的に目標値を先にお
き、仮想目標値と現在値との間に一定の偏差を持たせる
事により、力を出力する方法が知られている。

又、ロボット自体に力検知機能がついているものでは、
検知した力を偏差に換算して、仮想目標値から引く等の
方法がとられる。

一方、人間の腕、手は、位置決め等の移動機能の他に、
必要な力制御機能及び触覚機能を有しているから、これ
ら機能をロボットに持たせ、作業に応じて適時これら機
能を働かせる必要がある。

例えば、２つの部材をはめ合せるには、固定位置にある
一方の部材に対し他方の部材をロボットにより位置決め
し、更に触覚機能によって正確に位置合せした後、所定
の押し付は力ではめ合せを行う。

このような作業を行うには、位置決め時と、位置合せ時
と、はめ合せ時とではアクチュエータの制御態様を変更
する必要がある。

しかしながら、従来の方法では、力が剛性にと位置偏差
との積で与えられるので、剛性が大きいとわずかの偏差
でも大きな力が発生してしまい、剛性はあまり大きくで
きない。一方、剛性が小さいと振動し易く、高速で駆動
できない。また、重力等の外乱に弱く、位置決め精度が
悪くなる。しだがって、従来の方法では位置−力併用制
御の取り扱いが複雑であり、きめ細かな位置・力制御が
できないという問題があった。

このため、本発明者等は、位置及び力をきめ細かに制御
できる制御方式を特許出願昭５．９−１０９９８０号（
昭和５９年５月３０日出１ｉｔ）によって提案した。

既提案の内容は、アクチュエータに付与されている力を
検出手段によって検出し、指令力との差を得て力制御を
行いうるようにするとともに検出手段からの現在位置と
指令位置との差を得て位置制御を可能とするとともに、
これらに所定の重みを付した和を基本制御量としてアク
チュエータを制御するものである。

既提案の内容を第６図によって説明する。

図中、１は多関節型５軸ロボツトであり、先端にハンド
１ａと、ハンド１ａを動作させるアーム１ｂ、１ｃ、１
ｄ、１ｅと、ベース１ｆを有しており、ハンドｌａｓア
ーム１ｂ、１ｃ、１ｄ１１ｅがアクチュエータによって
回動して、ハンド１ａを移動するもの、２１．２２は各
アクチュエータの位置検出器であり、力と位置の検出器
の一部を構成するもの、３は信号処理回路であり、位置
検出器２２の検出位置Ｘ及び雨検出器２１．２２の検出
出力の差である検出力ｆを出力するもの、４は計算機で
あり、検出位置Ｘ及び検出力ｆとからロボットの動作状
態を監視するとともに、目標指令位置ｘｏ、目標指令力
ｆｏ及び第１、第２の重みａ、ｂを出力するもの、５は
制御装置であり、各アクチュエータの駆動ａ（モータ）
の駆動電流ｉを、指令位置ｘｏ、指令力ｆｏ、第１、第
２の重みａ、ｂ及び現在位ＷＸ、検出力ｆによって次式
のｙｅが零となる様にフィードバンク制御するものであ
る。

ｙｅ＝ａ−Δｘ＋ｂ　・△ｆ　　　　　　　−（１）但
し、 △Ｘ＝ＸＱ−Ｘ　　　　　　　　　　　　　−・・・（
２）△ｆ　＝　ｆ　ｏ　　−ｆ　　　　　　　　　　　
　　　　　　　−・（３）即ち、位置と力の線形和によ
り作られた指令値Ｏ ｙ　ｏ　＝　ａ　−ｘ　□　＋　ｂ　、ｆ　ｏ　　　　
　　　−・−（４）に一致するように、出力（現在値）
ｙ ｙ＝ａ　−ｘ十ｂ　−ｆ　　　　　　　　　　・−１５
１を制御する方法である。

係る既提案の方式では、力を指令として与えているので
、位置及び力の制御がきめ細かにでき、しかも位置偏差
と力偏差の合成（和）を制御量としているから、同一駆
動系で実現でき、好都合である。

ところで、第（１）式の条件を満たす、位１ｔｘ（角度
θ）と力（トルク）ｆは無数にあり、現実には第（１）
式の条件を満たす定常解は外的条件によって一息に決定
される。

例えば、ａ　＝　ｂ　＝Ｑ、５の場合に第７図に示すよ
うにｘ＝ｘｏ　　（＝４）の位置でｆ＝ｆｏ　　（＝１
）の力で押すように指令したとしても、例えばｘ＝ｘｓ
（＝２）の所に物体ＢＤがあれば、ロボット１はｘ＝ｘ
ｓ　　（−２）の位置でｆ＝ｆｓ（＝３）の力で押すよ
うに動作することになり、物体ＢＤという外的要因に適
応した制御が可能となり、物体ＢＤが動く場合も同様で
ある。

このような外的要因によって適合した位置と力の制御が
できるということが既提案のポイントであった。

第（１）式の偏差ｙｅは、第（４）式及び第（５）式よ
り指令位置ｘｏ、指令力ｆＯによる指令値ｙｏと、現在
位ｉｘｓ現在力ｆによる現在値ｙとの偏差である。

このような位置と力の制御を行う系においては、次数が
高いので、安定に且つ高速な制御を行うためには、状態
フィードバックを行う必要がある。

状態量がｘｌ、Ｘ　２、−・−ｘｎと表わされるとき、
状態フィードバックを行うためには各状態量の指令値ｘ
１、ｘ２、・・・ｘｎからの偏差量をフィードバックす
る量として用いる。即ち制御人力Ｕに次式％式％ ただし、ｆｌ、ｆ　２、−ｆ　ｎはフィードバックゲイ
ンである。

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

ところで、ここで取り扱っている位置−力制御系におい
て、上述の状態フィードバック制御を通用すると、次の
ような問題がある。

■位置−力制御では一般に定常値が指令値Ｘ０１ｆＯと
異なるので、指令値からの偏差量を用いてもフィードバ
ックする量が極めて広汎な値をとる。

このように広汎な値をとる量をフィードバックする量と
して用いると飽和し易くなり、取り扱いが難しくなる。

また、フィードバックする量が大きくても取り扱える構
成にすると相対的に誤差が太き（なり、発振し易くなる
のでフィードバックゲイン「を大きく出来ない。

また、フィードバックする量を定常値からの偏差量で与
えればフィードバックゲイン「を大きくできるけれど、
定常値は外的な条件により決まるため、直接知ることが
困難である。

■位置−力制御では外力が働いているとき、ｙｅを零に
できない。即ち、定常偏差が生じることになる。例えば
第７図の場合、指令した位置に物体があれば、ｘ”’ｘ
ｏとなる。このときｙ　ｅ＝Ｑであるためにはｆ＝ｆｏ
でなければならない。しかし、ｆ＝ｆｏとすると（６）
式よりＵ＝Ｏとなり、アクチュエータに電流が流れずに
、アクチュエータの出力は零となり、力が発生しないこ
とになる。

即ちｆｏ＝０以外ではｆ＝ｆｏが成り立たない事になる
。実際には指令した値とは異なる力が発生しくｆ≠ｆｏ
）、）’ｅ＝ｂ　（ｆｏ−ｆ）≠０となる。即ちｙｅは
定常偏差が生じている。

そこで、本発明は、フィードバンクする量を直接知らな
くても、安定且つ高速に制御を可能とするとともに外力
が与えられた時でも外力に抗する力を発生しながら定常
偏差を生じないように制御しうる移動体制御方式を提供
することを目的とする。

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

第１図は本発明の原理説明図であり、第１図（Ａ）はそ
の構成図、第１図（Ｂ）はその動作説明図である。

第１図（Ａ）中、６は制御対象であり、具体的には負荷
である移動体（ロボットのアーム等）を駆動するアクチ
ュエータ（駆動手段）と、アクチュエータの状態を検出
する検出器２１．２２（第６図）等を含み、アクチュエ
ータに与えられる駆動電流ｉによって動作し、現在値ｙ
を出力するもの、６ａは偏差手段であり、現在値ｙと指
令値ｙＯとの偏差ｙｅを得るもの、７は制御対象のモデ
ル（状態観測手段）であり、制御対象６のモデルを回路
的に又はソフトウェアで構成したものであり、８は合成
手段であり、偏差手段６ａの実偏差ｙｅとオブザーバ（
状態観測手段）７からの推定偏差ｙｅとを合成するもの
、９ａは第１のゲイン手段であり、合成手段の合成出力
にオブザーバゲインＧを付与してオブザーバ７へフィー
ドバックするもの、９ｂは第２のゲイン手段であり、オ
ブザーバ７の観測状態量にフィードバックゲインＦを付
与して、制御電流ｉｅを作成し、オブザーバ７の入力と
するもの、１１は積分手段であり、実偏差ｙｅを積分す
るもの、１２は合成手段であり、積分された実偏差と、
第２のゲイン手段９ｂの出力との合成により制御電流ｉ
ｅを作成するものである。

即ち、第１図（Ａ）の本発明では、オブザーバ７が設け
られ、オブザーバ７によって制御対象６と同一の動作を
偏差空間において実行して、制御対象６を観測しく制御
対象６の動作をシュミレートシ）、その状態量と実偏差
ｙｅの積分量とによって制御対象６の駆動制御を行うも
のである。

〔作用〕

第１図（Ｂ）の制御対象の解析モデル図をもとに説明す
る。

第１図（Ｂ）に示した解析モデルでは、モータ２０の出
力軸に負荷１が直結されていて、負荷１と負ｒ１２とは
捩り剛性にのばねで結合されている。

この時、運動方程式は次のようになる。

ｊ１θ１＋Ｄ１θ１＋Ｋ（θ１−０２）＝ｋＴ’ｉ−（
？） Ｊ２θ２＋Ｄ２θ２＋Ｋ（θ２−θ＋）＝ｆ’・−（８
） ただし、 Ｊｌ：負荷１の慣性能率（モータの慣性能率）Ｄｌ：負
荷１に働く減衰力 θ１：負荷１の回転角 （モータ側のエンコーダ２１で検出） Ｊ２：負荷２の慣性能率（負荷の慣性能率）Ｄ２：負荷
２に働く減衰力 θ２：負荷２の回転角（負荷側の慣性能率）ｋＴ：モー
タ２０のトルク定数 ｉ：モータ２０に流す電流 ｋＴ’ｉ：モータ２０の出力トルク ｆ′：負荷２の外力（自重、反力） ここで、指令値としてθ２０．ｆｏを与えたとすると、
指令力ｆｏは、θ２ｏとモータ２０の回転指令角θ１ｏ
とを用いて、 ｆｏ＝Ｋｉθ１ｏ−θ２０　）　　　　　　−（９）と
なり、θ１ｏは θ１ｏ　＝　ｆ　２０　＋　ｆ　ｏ　／　Ｋ　　　　　
　　−００１となる。

第＜１１式より、偏差ｙｅは、 ｙｅ＝ａ−（θ２ｏ−θ２）＋ｂ−（ｆｏ−ｆ）である
から、第（９）式より ｙｅ＝ａ・（θ２０−θ２）＋ｂ−Ｋ（（θ１ｏ　−θ
２０）−（θ１−θ２））　　　　　・−（１２）但し
、モータ２０（負荷１）から負荷２に伝えられる力ｆは
、 ｆ＝Ｋ・（θ１−θ２）　　　　　　　　−・−（１３
）この時のフィードバック量Ｕは、第（６）式よりｕ＝
ｆ４　（θ２０−θ２）＋ｆ３　（（Ｌ１２０−（１１
２）＋ｆ２　（θ１ｏ−θ１）＋ｆ１　（ω１ｏ−ωと
なる。

ここで、物体と接している時に、 θ２０＝０２ とすれば、ｆ＝ｆＱであるためには、 θ＋０＝θ１ であるべきである。この時（１４）式よりｕ−０となる
。

しかし、ｕ＝Ｑであればモータ２０には電流が流れない
から、モータ２０の伝達力ｆは零となり、力を発生せず
、外力に抗する力を発生できない。

したがって、ｆｏ＝０（θｉｏ＝θ２０）のとき以外に
はθ１＝θ１ｏは成り立たなくなる。即ちｙｅ≠０とな
り、定常偏差が生し本来の目的には果せない。なお、以
上はθ２＝θ２ｏを仮定して説明したが、θ２−θ２ｏ
の仮定を置かない場合でも一般にｙｅは零にならない。

このため、ｙ　ｅ＝Ｑでも、モータ２０に力を発生でき
るように積分器１１が設けられ、積分器１１からの出力
フィードバックによって外力に抗する（釣り合う）力が
得られ、外力があるときでも定常偏差が生じないように
している。

一方、係る位置−力制御では、前述の如く各状態量（θ
１、θ２−）の定常値（θ１ｓ、θ２ｓ−・）は外的条
件で定まり、指令する段階では、知ることができない。

定常値を知らなくても制御できるようにするには次のよ
うにすればよい。

モデル７を構成し、実際の系と同一の制御電流を流し、
モデルの出力９ｅと制御対象の出力ｙｅとの差にオブザ
ーバゲインＧをかけて、モデルにフィードバンクする。

このモデルより求まる状態量θ１ｓ、θ２ｓ−と出力の
積分量をもちいて制御電流Ｕを構成することにより安定
に定常偏差なく制御することができる。

尚、オブザーバ７は、外力がない場合は、積分量は零と
なるから、この時のオブザーバ７は偏差空間で制御対象
の状態量を再現していることになる。

このように本発明によると測定値の舎利より求まる出力
偏差ｙｅのみを用いて、フィードバックする量（積分量
とモデルから求まる量）を構成しているので、外的条件
を知ることなく広汎な範囲で定常偏差なく、しかも高速
に安定な制御が可能となる。

〔実施例〕

（ａ）一実施例の構成の説明 第２図は本発明の一実施例構成図、第３図はオブザーバ
のモデル構成図である。

図中、第１図及び第６図で示したものと同一のものは同
一の記号で示してあり、１３は極性反転部であり、第２
のゲイン手段９ｂと第３のゲイン手段１１ａからの出力
を反転して制御電流ｉｅを得るもの、２０はモータ（ア
クチュエータ）であり、アーム１ａ〜１ｅ等の負荷を駆
動するもの、３２は差回路であり、モータ２０の回転角
（位置）θ１と負荷の回転角θ２との差をとり、乗算部
３−２ａでに倍して実側力（現在力）ｆを作成するもの
、５０は差回路であり、（信号処理回路３の）差回路３
２からの検出力ｆと計算機（４の出力バッファ）からの
指令力ｆｏとの差△ｆを求めるもの（前述の第（３）式
を実行するもの）、５１は乗算回路であり、差回路５０
の差（エラー）△ｆと計算機（４の出カバソファ）から
の重みｂを乗算して出力ｂ・八ｆを発するもの、５２は
差回路であり、（信号処理回路３の第２のカウンタ３１
の計数位置）現在位置θ２と計算機（４の出カバソファ
）からの指令位置θ２ｏとの差△θを求めるもの（第（
２）式を実行するもの）、５３は乗算回路であり、差回
路５２の差（エラー）八〇と計算機（４の出力バッファ
）からの重みａとを乗算して出力ａ・△θを発するもの
、５４は和回路であり、２つの乗算回路５１．５３の出
力ｂ・△ｆ、ａ・△θを第（１）式に従って加算し、制
御量ｙｅを出力するものである。オブザーバ７において
、Ｍ１〜Ｍ　１２は各々乗算器であり、各々入力をブロ
ック内に付した係数を乗して出力するもの、Ａ１〜Ａ７
は各々演算器であり、各々２つの入力を極性に応じて加
算又は減算して出力するもの、ＩＴＩ〜ＩＴ４は各々積
分器であり、入力を積分して出力するもの、Ｄ１〜Ｄ４
は差分器であり、２つの入力の差分を出力するものであ
る。９０〜９３は乗算器であり、合成手段８の合成出力
、即ち（／？ｅ−ｙｅ）にブロック内で示されたオブザ
ーバゲイン０１〜Ｇ４を乗じて、オブザーバ７の各差分
器Ｄ１〜Ｄ４に入力するもの、９４〜９７は乗算器であ
り、オブザーバ７の各積分器ＩＴＩ〜ＩＴ４の出力にフ
ィードバックゲインＦ１〜Ｆ４を乗じるもの、９８ａ〜
９８ｃは加算器であり、各乗算器９４〜９６及び合成手
段１２の出力を加算して制御電流ｉｅを得るものである
。

１１ａは乗算器であり、偏差ｙｅの積分量（積分器１１
の出力）にフィードバックゲインＦ５を乗じて加算部（
合成手段）１２に出力するものである。

先づ、オブザーバ７の構成について、第１図（Ｂ）の制
御対象の解析モデル図及び第３図のオブザーバのモデル
構成図によって説明する。

モデル７の出力として次式で表わされる位置θ２と力ｆ
との線形和ｙをとる。

ｙ＝ａθ２　＋　ｂ　ｆ　　　　　　　　　　−−（１
５）出力ｙの目標′値ｙｏはｙと同様に、位置θ２の目
標値（指令値）θ２ｏと力ｆの目標値（指令値）ｆｏと
の線形和で表わされるものとする。

）’Ｑ　＝ａθ２０　＋　ｂ　ｆ　ｏ　　　　　　　−
−−−（１６）ｉ　（７１式、ｉ　（８）　式ヲ、ｔ＜
０でｅｌ　＝ｊＨ＝ｅ２＝θ２＝ｉ＝ｏとしてラプラス
変換すると、（Ｊ　１・ｓ２＋］）１・ｓ　＋Ｋ）　θ
１（ｓ）＝　ｋ　Ｔ−１（Ｓ）　＋　Ｋ’θ２（Ｓ）　
　　・−・（１７）（Ｊ２・ｓ２＋Ｄ２゛ｓ＋Ｋ）θ２
（ｓ）＝に一θ１（ｓ）　＋　ｆ　’　（ｓ）　　　　
・−（１８）となる。ここで、 とおくと、第（１７）式及び第（１８）式は、θ１（ｓ
）＝Ｇ＋　（ｓ）　（ｋ”ｒ’１（ｓ）＋Ｋ・θ２　（
ｓ））　−（２０）θ２　（Ｓ）＝Ｇ２　（ｓ）　（ｋ
・θ１（ｓｌ　＋　ｆ　’　（ｓｌ）　　−（２１）と
なる。

一方、第（１５）式を、ｔ＜Ｑでｙ＝ｏとして、ラプラ
ス変換すると、 ｙ（ｓ）−ａ−θ２　（３１＋　ｂ　−ｆ　（ｓｌ　　
　　　　−（２２）となる。

従って、第（２０）弐〜第（２２）式をブロック図で示
すと、第３図（Ａ）の如くなる。即ち、制御対象６は、
Ｉ　（Ｓｌを入力とし、ｙ（Ｓ）を出力とする第３図（
Ａ）のブロック図でモデル化できる。

この第３図（Ａ）のラプラス変換上のブロック図を時間
軸上のブロック図で示すと、第３図（Ｂ）の如くなり、
従って、制御対象６は、乗算器Ｍ１〜Ｍｊ２、演竺器Ａ
１〜Ａ７、積分器ＩＴＩ〜ＩＴ４で構成でき、制御対象
６における状態量（モータ２０の角速度ω１、回転角θ
１、負荷の角速度ω２、回転角θ２、力ｆ、出力ｙ）を
観測できる。

（ｂ）一実施例の動作の説明 制御対象６の出力ｙを安定且つ高速に指令値に追従させ
るには、第３図の各推定状態量（ωＩＳ％θ１Ｓ％ω２
５％　θＺＳ）をフィードバックすればよい。

しかしながら、係るフィードバック量は定常解からの偏
差であることを必要とし、本発明では２つの指令値が与
えられることから係る定常解は必ずしも固定されたもの
でなく、運動方程式と外的条件を満たす理想的な値であ
る。

このため、このままでは外力が付与されていない場合に
、オブザーバからは定常解が不明のため偏差をフィード
バックできない。

これを解決するために外力が付与されていない時には、
オブザーバ７を第２図の如く偏差空間で実際の系を再現
させるようにして、偏差状態量を得ることができるよう
にしている。即ち、定常解を知らなくてもフィードバッ
クすべき偏差量を得ることができるようにしている。

このオブザーバ７を偏差空間で再現させるために、オブ
ザーバ７に積分量零（即ち、外力零）の制御電流ｉｅを
入力し、実際の系６とオブザーバ７の偏差出力の差（９
ｅ−ｙｅ）を合成手段８でとり、それをオブザーバ７に
フィードバックすれば、オブザーバ７は外力が付与され
ていない場合には常に偏差空間で実際の系を再現するこ
とができる。

オブザーバ７では、定席値からの偏差Ｑ１　ｓ、θ１ｓ
、’？２ｓ、θ２Ｓは観測でき、この状態量を用いてフ
ィードバックを行えば、安定に高速な動作可能である。

具体的には、（↑ｅ−ｙ）にオブザーバゲインＧ１、Ｇ
２、Ｇ３、Ｇ４を乗算器９０〜９３で乗算し、オブザー
バ７の各差分器Ｄ１〜Ｄ４に入力することによって差分
をとり、偏差空間で動作させ、これによって差分器Ｄ１
〜Ｄ４の後段の積分５１Ｔ１〜ＩＴ４より偏差状態量、
即ち、偏差角速度δ１Ｓ−９２３、偏差回転角θ１Ｓ、
θ２Ｓを観測させることができる。

この観測された偏差状態量♂Ｉｓ、Ｑ２ｓ、ｎ１Ｓ、θ
２Ｓを第２のゲイン手段９ｂの乗算器９４〜９７でフィ
ードバックゲインＦ１〜Ｆ４を乗じ、加算器９８ａ〜９
８ｃで各乗算器９４〜９７の出力を加算することによっ
て制御電流ｉｅが得られる。この場合、積分量は零なの
で、オブザーバ７からの偏差状態量のみがフィードバッ
クされ、制御電流ｉｅに寄与する。これを反転部１３で
極性反転し、オブザーバ７の入力とするとともにモータ
２０の駆動電流ｉを得てモータ２０へ与える。

前述の第３図では、外力ｆ′がオブザーバ７に入力され
ているが、偏差空間ではｆ　’　ｅ＝Ｑとみなすことが
できるから、第２図の偏差空間で動作するオブザーバ７
においては、考慮しなくてよい。

このようにして、オブザーバ７は制御対象６の偏差空間
での動作を再現しているから、定常解が不明であっても
、定常解からの状態偏差をフィードバックでき、従って
現代制御理論に従う多変数制御が可能となり、高速動作
しても振動が生じることもなく且つ円滑に定常解に向っ
て動作が可能となり、高速追従性の実現及び外的振動の
影響を小とし、振動を抑制し、滑らかな動作を実現でき
る。

一方、外力がある場合は積分量は零でない、従って、こ
のときモデル（オブザーバ）７は必ずしも偏差空間で制
御対象の状態量を再現するものとはいえない。しかし、
次のように考えることはできる。充分に時間がたっんと
きモデル７の各状態量と積分量にフィードバックゲイン
をかけて構成した制御電流は定常的な力を出すための電
流になっている。即ちモデル７の状態量は有限な値（オ
フセット量）をとる。したがって、モデル７の状態量は
オフセット量と制御対象の定常値からの偏差量との和に
なっている。オフセット量は外力により異なり、外力が
働かない場合は零となる。したがって、外力が働かない
ときはオフセソ＋−ｉが零なので、モデルは制御対象を
偏差空間で再現するものとなる。

即ち、外力がある場合には、積分手段１１の積分量がフ
ィードバックされ、制御電流ｉｅに寄与する。

従って、偏差ｙｅの積分量をフィードバックし、オブザ
ーバ７の状態量（即ち第２のゲイン手段９ｂの出力）に
加算して制御電流ｉｅを得て、外力に釣り合う力をモー
タ２０より発生させることができる。

このため、偏差ｙｅが零でも、モータ２０は外力に釣り
合う力を発生できるとともに、モータ２０等に摩擦があ
る場合やアーム１ａ−１ｅの重力変化による外乱がある
時には定常偏差が生じないようにしている。

これによって特に摩擦の大きいモータを使用しても安定
に制御でき、また多関節ロボットの如きアームの位置に
よって重力変化が生じても安定に制御できるようにした
ものである。

＜Ｃ１本発明の詳細な説明 第４図は本発明の適用例構成図であり、第６図の多関節
ロボットの制御装置５に本発明を適用した例を示してい
る。

図中、第６図、第１図及び第３図で説明したものと同一
のものは同一の記号で示してあり、２はアクチュエータ
であり、モータ２０と非接触ロークリエンコーダから成
る位置検出器２２．２１とが一体で構成されており、位
置検出器２１はモータ２０の軸に直結され、モータ２０
の回転角θ１を検出するためのものであり、位置検出器
２２はモータ２０の伝達力がトーションバー（柔性部材
）２４を介して与えられる出力軸２ａに直結され、負荷
、即ち、ロボットのアーム１ａ〜１ｅの回転角θ２を検
出するためのものである。なお、出力軸２ａと負荷、即
ちロボットのアーム１ａ〜１ｅとの間に減速機があって
もよい。

３０は信号処理回路３の第１のカウンタであり、位置検
出器２１の出力である位置パルスを係数するもの、３１
は信号処理回路３の第２のカウンタであり、位置検出器
２２の出力である位置パルスを計数するもの、３２は差
回路であり、第１のカウンタ３０の計数位置と第２のカ
ウンタ３１の計数位置との差を取り、アクチュエータ２
に付与されている力ｆを検出するものであり、これらで
信号処理回路３を構成する。４０．４１．４２．４３は
各々計算機４の出カバソファであり、各々指令力ｒ　ｏ
　、　ｔｈ令位置θｚｏ、重みす、ａの出力のだめのも
の、４４は計算機４のプロセフ号であり、モード変換時
に各出カバソファ４０〜４３に指令力ｆｏ、指令位置θ
２ｏ、重みす、ａをセントするものであり、所定のプロ
グラムによって動作するものである。

先づ、アクチュエータ２の動作について説明すると、ア
クチュエータ２は図で１軸分しか示してないが、各アー
ム１ａ〜１ｆに対応して設けられており、モータ２０の
回転力は、トーションバー２４を介し出力軸２ａより出
力され、アームを駆動する。

モータ２０の回転位置は位置検出器２１により検出され
、出力軸２ａの回転位置は位置検出器２２によって検出
される。従って、出力軸２ａの位置を検出する位置キ灸
出５２２の出力でモータ２０を位置制御すれば、正確な
位置決めができる。一方、力（トルク）は開位置検出器
２１．２゛２の出力差とトーションバー２４の剛性によ
って定まる。

従って、出力軸２ａに何等かの力が付与されるとトーシ
ョンバー２４の柔性によってトーションバーがたわみ、
両検出器２１．２２の出力に位相差が生じるから、これ
によって外力及びその大きさを検知することができる。

即ち、両検出器２１．２２の位置パルスを計数するカウ
ンタ３０，３１の計数値の差によって力検出が可能とな
る。

このようなアクチュエータ２を用いることによって、モ
ータと一体化した小型な力、位置検出器の実現が可能と
なり、またロータリエンコーダは高精度な位置検出が可
能であり、ノイズに強く信頼性も高い。柔性部材として
は、トーションバーの他に渦をばねを用いてもよい。

先づ、計算機４のプロセラ号４４は出力バンファ４０．
４１．４２．４３に指令力ｆＯ１指令位置θ２０．重み
す、ａをセットする。これによって制御装置５は、信号
処理回路３からの現在位置θ２及び検出力ｆを得て、差
回路５０．５２、乗算回路５１．５３及び和回路５４よ
り実偏差ｙｅを得、オブザーバ７の推定偏差９ｅとの差
を合成手段８でとり、これを第１のゲイン手段９ａを介
しオブザーバ７にフィードバックし、一方、オブザーバ
７の状態量より第２のゲイン手段９と第３のゲイン手段
１１ａとの金相より制御電流ｉｅを作成し、オブザーバ
７に入力するとともに、モータ２０に与えてモータ２０
を駆動する。

多関節ロボットにおいては、モータ２０は軸分要し、各
軸分第４図の制御装置５が設けられる。

重みａ≠０、ｂ≠０であれば指定された力で指定された
位置にモータ２０を介しロボット１のアーム１ｂ〜１ｅ
を制御する。

このように、制御量が第（１１式の如く位置エラー△θ
とカエラー△ｆに各々重み付けａ、ｂを付した和の形式
を採っているので、位置制御のみを行う場合には、重み
ａ≠０、ｂ＝ｏとすることによって第（１１式は１ 、＝８．△θ　　　　　　　　　　　−・（２３）とな
るから、位置制御のみを行いうる。

同様に、力制御のみを行うには、重みａ＝Ｏ１ｂ＋（ｌ
とすることによって第（１）式は、ｙ＝ｂ・八ｆ　　　
　　　　　　　　　・・−（２４）となるから、力制御
のみを行いうる。

また、ａ≠Ｏ，ｂ≠０とすれば、第（１）式のままであ
るから、位置−力併用制御が可能となり、重みａ、ｂを
変更することによって容易に制御態様を変更することが
できる。

同様にして、重みａ、ｂの値自体を変えることによって
、更に決め細かな制御態様を与えることができ、例えば
、位置−力併用制御においても、泣置楕度に重きをおく
場合には、重みａを大、重みｂを小とし、力制御に重き
をお（場合には、重みａを小、重みｂを大とすることに
より、容易にきめ細かな制御を実現できる。これは、計
算機４が現在位置θ２や検出力ｆによってロボットの動
作状態を把握しながら、適時重みａ、ｂを変更して実行
する。

この重みａ、ｂの変更は、制御対象を変えることになる
から、重みに応じて、フィードバックゲインＦ、Ｆ５及
びオブザーバゲインＧを変更する。

（ｄ）他の実施例の説明 第５図は本発明の別の実施例構成図であり、図中、第６
図で示したものと同一のものには同一の記号を付してあ
り、３′は検出信号処理回路であり、モータ２０の位置
検出器２２の出力パルスを計数するカウンタ（第９図の
３１に相等）と検出電流ｉをデジタル値ｉに変換するＡ
−Ｄ　（アナログ・デジタル）コンバータとを有するも
の、５′は制御装置であり、各アクチュエータの駆動源
（モータ）の駆動電圧Ｖを、指令位置ｘｏ　　（θ２゜
）、指令力ｆｏ　　（ｉｏ）　、第１、第２の重みａｌ
ｂ及び現在位置ｘ（θ２）、検出力ｆ　（ｉ）によって
次式のｙが零となる様に制御するものである。

ｙ＝ａ・△θ＋ｂ△１−（２５） 但し △θ＝θ２ｏ　−θ２　　　　　　　　　　　　・−＝
　（２６）△ｉ−（ｆｏ−ｆ）／ｋＴ　　　　　　　　
−（２７）尚、ｋＴはモータ２０のトルク定数である。

２６は電流検出器であり、モータ２０の電流値ｉを検出
し、信号処理回路３′へ入力するものである。

制御装置５′は第４図の制御装置５と同一の構成を有し
、差回路５０は信号処理回路３′の検出電流ｉと指令電
流ｔｏとの差をとるように構成される。

従って和回路５４からは第（２５）式の偏差ｙｅが得ら
れる。又、アクチュエータは、位置検出器２２、モータ
２０、減速機２３とによって構成される。

この実施例では、モータ２０に流れる電流ｉと出力トル
クＴとの間には比例関係が成立し、比例定数（トルク定
数）をｋＴとすると、 Ｔ千ｋＴ−ｉ　　　　　　　　　　　　　・−・（２８
）となる。

従ってモータ２０に流れる電流ｉを検知すれば、モータ
の出力トルクＴＸＩＩち力を検出することができ、前述
の実施例と同様の制御が可能であり、この場合も、重み
ａ、ｂを変えることによって制御態様を同様に変更しう
る。

又、この他の実施例では、従来位置制御系で用いられた
アクチュエータの構成そのままでこれらの位置−力制御
ができ、構成が安価、コンパクトとなり、又従来から存
在するロボットを容易に高機能化できる。

又、前述の各実施例においては、位置と力の制御を例に
説明したが、速度と力の制御であってもよく、この場合
、角速度ω０を位置指令の代りに与えればよい。

同様に、オブザーバ７をハード回路で説明したがマイク
ロプロセッサ等によるソフトウェアの実行によって行っ
てもよく、更に制御装置５．５′をマイクロプロセッサ
で構成し、制御装置５．５′の機能をソフトウェアの実
行によって行うこともできる。

しかも、対象も多関節型ロボットに限られず、直交座標
型ロボット等の他の移動体であってもよい。

更に、上述の実施例においては、検出器としてエンコー
ダ形式のものを用いたが、歪ゲージ等の他の力センサに
よってもよい。

以上本発明を実施例により説明したが、本発明は本発明
の主旨に従い種々の変形が可能であり、本発明からこれ
らを排除するものではない。

〔発明の効果〕

以上説明した様に、本発明によれは、外力が付与されな
い状態では、制御対象のモデルによって状態量を再現で
き安定且つ高速なフィードハックが実現できるという効
果を奏し、一方、外力が付与されると積分フィードバッ
クによって外力に釣り合う力を発生できるという効果を
奏するから、定常偏差を生ぜず移動−力制御が実現でき
、高速且つ安定な動作を保証するものである。

【図面の簡単な説明】

第１図は本発明の原理説明図、 第２図は本発明の一実施例構成図、 第３図は第２図構成におけるオブザーバのモデル構成図
、 第４図は本発明の適用例の構成図、 第５図は本発明の別の実施例の構成図、第６図は既提案
の制御方式の説明図、 第７図は既提案の制御方式の動作説明図である。 図中、２−アクチュエータ、 ２０・・・モータ、 ２１．２２−・位置検出器、 ４−計算機、 ５．５′・・−制御装置、 ６−・制御対象、 ６ａ−偏差手段、 ７−状態観測手段（オブザーバ）、 ８−合成手段、 １１・−積分手段。 （Ａ） （Ｂつ ２セ、苑θ月の原理３免８月図 第１図 イ也の宋がシ倚１］ｆ）ネ鼻成図 第５図 既提案の制御方式説明図 第６図

Claims (1)

1. 【特許請求の範囲】 移動体を駆動する駆動手段を移動指令と力指令に基づい
   て制御する移動体制御方式において、該移動指令と該力
   指令とが与えられ、該移動指令と該力指令と該駆動手段
   の駆動による偏差を出力する制御対象と、 該制御対象のモデルと、 該制御対象の偏差を積分する積分手段とを有し、該制御
   対象の偏差と該モデルから得られた偏差との合成出力を
   該モデルにフィードバックし、該モデルの状態量と該積
   分手段の積分量との合成を該モデルに入力するとともに
   該駆動手段に与えることを特徴とする移動体制御方式。