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

Abstract

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

Description

【発明の詳細な説明】 〔目次〕 概要 産業上の利用分野 従来の技術 発明が解決しようとする問題点 問題点を解決するための手段（第１図）作用 実施例 （ａｌ第１の発明の一実施例の説明 （第２図、第３図、第４図） （ｂｌ第１の発明の他の実施例の説明 （第５図、第６図） （Ｃ１第２の発明の一実施例の説明（第７図）（ｄ）第
２の発明の他の実施例の説明（第８図）（ｅ＋本発明の
詳細な説明（第９図） ｆｆｌ別の実施例の説明（第１０図） 発明の効果 〔概要〕 移動体を駆動する駆動手段を移動指令と力指令に基いて
制御する移動体制御方式において、制御対象の偏差空間
のモデルとして構成された状態観測手段を含み、該制御
対象の偏差と該状態観測手段の偏差との合成を状態観測
手段にフィードバックし、且つ状態観測手段の状態量に
基いて駆動手段を制御することにより、高速且つ円滑な
移動・力制御を可能とするものである。

〔産業上の利用分野〕

本発明は、ロホソト等の移動体を移動指令と力指令とに
よって制御するための移動体制御方式に関し、特に系の
状態観測を行って制御する移動体制御方式に関する。

近年、人間の行っている作業を自動化するためロボット
が開発され、更にロボットを人間の手や腕と同等な働き
に制御する技術の開発努力がなされている。

ロボットに人間の手や腕と同様な働きを行なわせること
ができれば、それだり高度な作業が出来、極めて便利で
ある。

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

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

〔従来の技術〕

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

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

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

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

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

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

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

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

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

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

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

ｙｅ＝ａ・Δｘ＋ｂ・Δｆ　　　　−−−−一−−（１
）但し、 Δｘ　＝　ｘ　Ｏ−ｘ　　　　　　　　−−−−−−（
２）Δｆ　＝　ｆ　ｏ−ｆ　　　　　　　　−−−−（
３１係る既提案の方式では、力を指令として与えている
ので、位置及び力の制御がきめ細かにでき、しかも位置
偏差と力偏差の合成（和）を制御量としているから、同
一駆動系で実現でき、好都合である。

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

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

このような外的要因によって適合した位置と力の制御が
できるということが既提案のポイントであるが、制御量
ｙは実際には、第ｆｌ１式を変形して、となり、指令位
置ｘｏ、指令力ｒｏによる指令値ｙｏと、現在位置Ｘ、
現在力ｆによる現在値ｙとの偏差ｙｅである。

この偏差ｙｅを零とするのに、古典的制御理論で実現す
ると、単なる出力フィードバック７においては、外乱に
よる影響を整定するのに時間がかかり、追従周波数も低
くなり、円滑で高速な動作が困難である。（また、補償
回路を付加して特性を改善することも考えられるが、古
典制御理論に基づく設針は一般に試行錯誤的に行われる
ので、制御系の次数が３次以上になると急激に困難にな
ってくる。今の場合、制御系の次数が４次（第一の発明
）ないし５次（第二の発明）なので、制御系の特性の改
善には多大の労力を要し、大幅な特性改善は困難である
。） 従って、本発明は、現代制御理論を適用して円滑且つ高
速な移動体制御を実現しうる移動体制御方式を提供する
ことを目的とする。

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

第１図は本発明の原理説明図であり、第１図（Ａ）は第
１の発明の原理説明図、第１図（Ｂ）は第２の発明の原
理説明図である。

第１図（Ａ）中、６ば制御対象であり、具体的には負荷
である移動体（ロボットのアーム等）を駆動するアクチ
ュエータ（駆動手段）と、アクチュエータの状態を検出
する検出器２１．２２　（第１）図）等を含み、アクチ
ュエータに与えられる駆動電流ｉによって動作し、現在
値ｙを出力するもの、６ａは偏差手段であり、現在値ｙ
と指令値ｙｏとの偏差ｙｅを得るもの、７は状態観測手
段（以下オブザーバと称す）であり、制御対象６と同−
の偏差空間でのモデルを回路的に又はソフトウェアで構
成したものであり、８は合成手段であり、偏差手段６ａ
の実偏差ｙｅとオブザーバ７からの推定偏差ｙｅとを合
成するもの、９ａは第１のゲイン手段であり、合成手段
の合成出力にオブザーバゲインＧを付与してオブザーバ
７へフィードバンクするもの、９ｂは第２のゲイン手段
であり、オブザーバ７の観測状態量にフィードバンクゲ
インＦを付与して、制御電流ｉｅを作成し、オブザーバ
７の入力とするもの、１０は合成手段であり、定常電流
ｉｓと制御電流ｉｅとを合成して駆動電流ｉを作成する
ものである。尚、この場合定常電流は移動体を空間内に
移動させるための加速電流ｉｏと定常的な力を発生する
ための電流ｉｆより成り、加速電流ｉは計算機４より与
えられ、移動終了後は零となる（以下の！ｌＩ論は１０
＝０としても成り立つ）。

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

次に、第１図（Ｂ）中、第１図（Ａ）で示したものと同
一のものは同一の記号で示してあり、１）は積分手段で
あり、実偏差ｙｅを積分するもの、１２は合成手段であ
り、積分された実偏差と、第２のゲイン手段９ｂの出力
との合成により制御電流ｉｅを作成するものである。

即ち、第２の発明では、第１の発明に加えて、実偏差ｙ
ｅの積分量とオブザーバの状態量とによって制御対象６
の駆動制御を行なうものである。

〔作用〕

本発明においては、現代制御理論として知られているオ
ブザーバを用いている。この現代制御理論ではフィード
ハックすべき量が基準値からの偏差で与えられるとき、
与えられた評価関数に対して最適なフィードバンクゲイ
ンＦを一意に定めることができ、そのゲインが最適ゲイ
ンであるという保証もある。

しかし、本発明の系の如く、指令値が移動指令と力指令
との２つであり、しかも実際の指令値はｙというこれら
の合成出力であるから、位置と力の指令値と基準値は必
ずしも一致せず、基準値からの偏差量が得られないため
、フィードバックゲインを求めるのに現代制御理論が適
用できない。

特に系の次数が高次であるので、安定なゲインは求める
ことは難しく、仮にゲインが求まってもそれが最適であ
るという保証もない。

そこで本発明では、オブザーバに制御電流ｉｅを入力し
、実際の系の偏差ｙｅとオブザーバ７の推定偏差↑ｅと
をオブザーバ７にフィードバックすることによってオブ
ザーバ７が常に偏差空間で実際の系を再現できるように
している。オブザーバ７では容易に系の状態量（例えば
、アクチュエータの速度、位置、負荷の速度、位置）を
観測できるから、オブザーバ７の状態量（偏差量）を用
いてフィートハックすれば、安定に高速な動作が可能と
なる。尚、オブザーバゲインＧはフィードバンクゲイン
Ｆと同様に、与えられた評価関数に対して一意に定める
ことができる。

又、第２の発明においては、実偏差ｙｅの積分量もフィ
ードバンクして、アクチュエータの摩擦等の外乱に対し
ても安定に動作することができる。

〔実施例〕

（ａｌ第１の発明の一実施例の説明 第２図は第１の発明の一実施例構成図、第３図は制御対
象の解析モデル図、第４図はオブザーバのモデル構成図
である。

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

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

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

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

Ｊ１θ＋　＋Ｄ＋　ｆ）１＋Ｋ　（θｌ−θ２）＝ｋＴ
ｉ−・−（５） Ｊ２１）２＋Ｄ２θ２＋Ｋ（θ２−〇＋）＝ｆ’−−−
−−−−（６ま ただし、 Ｊｌ：負荷１の慣性能率（モータの慣性能率）Ｄｌ：負
荷１に働く減衰力 θ１：負荷１の回転角 （モータ例のエンコーダ２１で検出）　　　。

Ｊ２；負荷２の慣性能率（負荷の慣性能率）Ｄ２：負荷
２に働く減衰力 θ２：負ｒｉＩ２の回転角（負荷側の慣性能率）ｋＴ：
モータ２０のトルク定数 ｔ：モータ２０に流す電流 ｋＴｉ：モータ２０の出力トルク ｆ′：負荷２の外力（自重、反力） である。

負荷１から負荷２に伝えられる力ｆは次のように表わせ
る。

ｆ＝Ｋ（θ１−θ２　）　　　　　　　　　　−−−−
−（７）ここで、モデル１の出力として次式で表わされ
る位置θ２と力ｆとの線形和ｙをとる。

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

ｙｏ−ａθ２０　＋ｂ　ｆ　ｏ　　　　　　　　−−−
−−−（９１第（５）式、第（６）式を、ｔ＜Ｑでθｌ
−θ１−θ２−θｚ＝１＝０としてラプラス変換すると
、（Ｊｌ　ｓｚ＋ＤＢ　Ｓ＋Ｋ）　θ１（Ｓ）＝　ｋ　
Ｔ　Ｉ　ｆｓｌ　＋にθ２　ｆｓｌ　　−−−−−−（
１０）（Ｊ２　Ｓ２＋Ｄ２　Ｓ　＋Ｋ）　θ２（Ｓ）−
にθ＋　（ｓ）　十ｆ　’　＋５１　　−−−−　（１
））となる。ここで、 とおくと、第（１０）式及び第（１））式は、θ＋　ｆ
ｓｌ−Ｇ＋　（ｓ）（ｋＴ　Ｉｆｓｌ＋にＧ２　（ｓｌ
）　−・−（１３）Ｇ２　（Ｓｌ＝Ｇ２　ｆｓｌ　（ｋ
θ自ｓｌ　＋　ｆ　’　（ｓｌ）　−−−−−−−（１
４）となる。

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

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

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

ところで出力ｙを安定且つ高速に指令値に追従させるに
は、第４図の各状態量（Ｇ１、Ｇ１、Ｇ２、Ｇ２）をフ
ィードバックすればよい。

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

このため、このままではオブザーバからは定常解が不明
のため偏差をフィードバックできない。

これを解決するためにオブザーバ７を第２図の如く偏差
空間で実際の系を再現させるようにして、偏差状態量を
得ることができるようにしている。

即ち、定常解を知らなくてもフィードバックすべき偏差
量を得ることができるようにしている。

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

この時、モータ２０には、各状態のフィードバンクより
求まるフィードバック電流ｉｅに、定常的な力ｆｓを発
生させるための電流ｉｆを付加したものを印加するもの
とする。

ｋＴｉｆ＝ｆｓ オブザーバ７では、定常値からの偏差量１ｅ、θ１ｅ、
ω２ｅ、θ２ｅは観測でき、この状態量を用いてフィー
ドバックを行えば、安定に高速な動作可能である。

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

この観測された偏差状態量Ｑ、ｅ、ω２ｅ、θＩｅ−＝
　θ２ｅを第２のゲイン手段９ｂの乗算器９４〜９７で
フィードバンクゲインＦｌ−Ｆ４を乗じ、加算器９８ａ
〜９８ｃで各乗算器９４〜９７の出力を加算することに
よって制御電流ｉｅが得られる。

これを反転部１３で極性反転し、オブザーバ７の入力と
するとともに合成手段（加算器）１０で定常電流ｉｓ　
　口１）＋ｉｆとの金相）との差をとってモータ２０の
駆動電流ｌを得てモータ２０へ与える。

この時、定常的な力を発生させるための電流ｉｆは、第
（７）式で示した、モータ２０と負荷２の回転角θ１、
Ｇ２の差にに倍した実測力ｆと、オブザーバ７の偏差空
間での推定力ｆｅとの差を差分部１４でとり、乗算部１
５で１／ｋＴ倍して得られる。

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

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

ｆｂｌ第１の発明の他の実施例の説明 第５図は第１の発明の他の実施例の構成図、第６図は第
５図におけるオブザーバの説明図であり、図中、第２図
及び第４図と同一のものは同一の記号で示しである。

第２の実施例では、現代制御理論を適用する前に、実測
力ｆに応じて電流ｉｆをモータに流すようにしている。

このように修正を加えた制御対象６ば第６図（Ａ＞の如
（、第４図（Ｂ）と同様モデル化される。

この時、定常電流ｉｓとしては、位置を移動させるため
の加速電流ｉｏのみを考えればよく、移動完了後は１ｓ
＝ｉｏ＝０となる。即ち、移動完了後は１ｏ＝ｏとなる
ものとする。

この第６図（Ａ）の運動方程式は、第（５）式、第（６
）式と同様にして。

Ｊ　１　θ　＋＋Ｄ　　皿　θ　１　＋　Ｋ　（θ　１
−　θ　２　）＝　ｋ　Ｔ　（ｉ　ｏ　＋　ｉｆ　）　
−−−−−−−（１６）Ｊ２θ２＋Ｄ２θ２十Ｋ（θ２
−θ＋）−ｆ’−一〜−−（１７） となる。

ここで、 ｋＴｉｆ＝ｆ＝Ｋ（θ］　−θ２　）　　　−−−−−
−−（１８）であるから、第（１６）式は、 Ｊ１θｌ＋Ｄ＋θ＋　＝　ｋ　Ｔ　ｉ　ｏ　　　−−−
−（１９）となる。

従って、第６図（Ａ）のブロック図は、θ２、θ１のフ
ィードバンク、ｆのフィードバンクは必要なくなり、第
６図（Ｂ）の如く簡略化できる。

これを用いたオブザーバ７が第５図に示したものである
。

係る他の実施例においても、第２図の実施例の場合と同
一の動作が行え、同一の作用、効果を奏する。

（Ｃ）第２の発明の一実施例の説明 第７図は第２の発明の一実施例構成図である。

図中、第１図（Ｂ）及び第２図で示したものと同一のも
のは同一の記号で示してあり、ｌｌａは乗算器であり、
偏差ｙｅの積分量（積分器１）の出力）にフィードハッ
クゲインＦ５を乗じて加算部１２に出力するものである
。

第７図の実施例においては、第２図で示した第１の発明
の実施例構成に積分器１）、乗算器１）ａ、加算器１２
を付加して、偏差ｙｅの積分量をフィードハックし、オ
ブザーバ７の状態量（即ち第２のゲイン手段９ｂの出力
）に加算して制御電流ｉｅを得るようにしているもので
ある。従って、オブザーバ７等の動作は第２図のものと
同一である。

この偏差ｙｅの積分量をフィードパソゲする意味は、モ
ータ２０に摩擦がある場合やアーム１ａ〜１ｅの重力変
化による外乱がある時にも定常偏差を生じないようにし
たものである。

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

（ｄ）第２の発明の他の実施例の説明 第８図は第２の発明の他の実施例構成図であり、図中、
第１図（Ｂ）、第５図、第７図で示したものと同一のも
のは同一の記号で示しである。

第８図の他の実施例では、第５図の第１の発明の他の実
施例の構成で示したものに、第７図の第２の発明の実施
例の偏差ｙｅの積分量フィードバックを加えたものであ
り、第７図の実施例と同一の動作、同一の作用、効果を
奏する。

（ｅｉ本発明の詳細な説明 第９図は本発明の適用例構成図であり、第１］図の多関
節ロボソ１−の制御装置５に本発明を適用した例を示し
ている。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

この重みａ、ｂの変更は、制御対象を変えることになる
から、重みに応じて、フィードバンクゲインＦ及びオブ
ザーバゲインＧを変更する。上述の例は、第５図の第１
の発明の他の実施例を適用したものを示しているが、第
１の発明の一実施例の構成及び第２の発明の構成（第７
図、第８図）を適用してもよく、第２の発明の構成を適
用するには、重みａ、ｂの変更に際し、フィードバンク
ゲインＦ５も変更する。

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

ｙ＝ａ・Δθ＋ｂΔｔ　　　　　　　　−−−−（２２
）但し Δθ−θ２ｏ−θ２　　　　　　　−一−−−−−（２
３）Δｉ＝　（ｆｏ　　ｒ）／ｋＴ　　　　　−−−−
（２４）尚、ｋＴはモータ２０のトルク定数である。

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

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

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

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

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

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

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

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

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

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

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

〔発明の効果〕

以上説明した様に、本発明の第１の発明によれば、オブ
ザーバを制御対象の偏差空間で動作するようにして、オ
ブザーバの状態量で制御しているので、第１に移動−力
制御系において高速追従性を得ることができ、又外乱（
外的振動）の影響を小にでき高速動作を可能とするとい
う効果を奏し、第２に共振点での撮動を抑制し、機械的
寿命を長くするという効果も奏し、第３に、滑らかな動
作を外的条件にかかわらず実現できるという効果も奏し
、特に負荷変動の激しい多関節型のものに適用して好適
である。

又、第２の発明によれば、第１の発明の効果に加え、更
に摩擦、重力等の外乱に対し、定常偏差を生しないよう
に動作できるという効果も奏し、一層外乱及び外的条件
の変化に対しても滑らかな動作を可能とする。

【図面の簡単な説明】

第１図は本発明の原理説明図、 第２図は第１の発明の一実施例構成図、第３図は第２図
構成における制御対象の解析モデル図、 第４図は第２図構成におけるオブザーバのモデル構成図
、 第５図は第１の発明の他の実施例構成図、第６図は第５
図構成におけるオブザーバの説明図、 第７図は第２の発明の一実施例構成図、第８図は第２の
発明の他の実施例構成図、第９図は本発明の適用例の構
成図、 第１０図は本発明の別の実施例の構成図、第１）図は既
提案の制御方式の説明図、第１２図は既提案の制御方式
の動作説明図である。 図中、１−ロボット、 １ａ−ハンド、 １ｂ〜１ｅ−アーム、 ２−アクチュエータ、 ２０−モータ、 ２１．２２−位置検出器、 ４−計算機、 ５．５′−制御装置、 ２６−電流検出器、 ６−制御対象、 ６ａ−偏差手段、 ７−状態観測手段（オブザーバ）、 ８−合成手段、 １）−＝積分手段。

Claims (2)

【特許請求の範囲】
1. （１）移動体を駆動する駆動手段と、移動指令と力指令
   と該駆動手段の状態量との偏差を得る偏差手段とを含む
   制御対象と、 該制御対象の偏差空間のモデルとして構成された状態観
   測手段とを含み、 該偏差手段の偏差と該状態観測手段の偏差との合成出力
   を該状態観測手段にフィードバックし、且つ該状態観測
   手段の状態量に基いて該駆動手段を制御することを特徴
   とする移動体制御方式。
2. （２）移動体を駆動する駆動手段と、移動指令と力指令
   と該駆動手段の状態量との偏差を得る偏差手段とを含む
   制御対象と、 該制御対象の偏差空間のモデルとして構成された状態観
   測手段とを含み、 該偏差手段の偏差と該状態観測手段の偏差との合成出力
   を該状態観測手段にフィードバックし、且つ該状態観測
   手段の状態量と該偏差手段の偏差の積分量との合成に基
   いて該駆動手段を制御することを特徴とする移動体制御
   方式。