JPH01136216A - 多関節ロボットのサーボ制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は多関節ロボットのサーボ制御方法に係り、特に
、リレー制御系にみられる滑り現象を活用して、アーム
手先を目標軌跡通りに制御するようにしたものの改良に
関する。

（従来の技術） 従来より、多関節型ロボットの動作制御系は非線形、可
変パラメータ、干渉特性を含むため、制御は一般に簡単
でない。このため、各種の補償方法が考えられているが
、フィードフォワード制御、非干渉、線形化などの手法
はダイナミックス・パラメータの正確な把握を必要とす
る。また、モデル模範型などの適応制御はかなり複雑な
アルゴリズムが必要であり、学習による方法は望む出力
が得られるまでに数回の試行が必要で、リアルタイム制
御が困難である。

そこで、近年、歯車のないダイレクトドライブ方式の実
用化に伴い、リレー制御系の滑り現象を積極的に活用し
て、アームの手先位置を目標位置にすべり状態制御（Ｓ
　１１ｄ１ｎｇ　　ｍｏｄｅ制御）する方法が提案され
ている。このすべり状態制御は、制御対象が状態空間内
に設定された超平面に拘束されたとき、超平面（切換え
面）の両側で制御装置パラメータが切換えられることに
より、制御対象が超平面の近傍で微少振動を繰返し、拘
束されることになる制御であり、制御対象パラメータ変
動、負荷の乱れ、外乱等に対して不感な安定性（ロバス
ト性）を特徴とするものである。

そして、多関節ロボットの動作制御にすべり状態制御を
適用する場合、サーボモータの出力トルクを切換関数で
決定する方法が一般的に採用されている。

（発明が解決しようとする問題点） ところで、上記すべり状態制御を採用した場合、各関節
のモータにおいて出力トルクそのものは電流フィードバ
ック等により正確に制御され得る。

しかし、他軸モータが発生するトルク、遠心力、コリオ
リカなどの干渉、外乱、負荷変動等の影響があり、この
影響は電流フィードバックで除去することができない したがって、上記従来のようなすべり状態制御により出
力トルクを制御するだけでは、ロボットを高精度で軌跡
制御するのが困難であるという問題がある。

本発明は、斯かる点に鑑みてなされたものであり、その
目的は、すべり状態制御をフィードバック制御系に適用
することにより、他軸モータのからの作用、外乱、負荷
変動等の影響に対してロバストな制御機能を得るととも
に、すべり状態制御における切換信号の切換幅の増加を
抑制して、軌跡制御の安定性および精度の向上を図るこ
とにある。

（問題点を解決するための手段） 上記目的を達成するため、本発明の解決手段は、多関節
ロボットのサーボ制御方法として、アーム手先が移動す
べき軌跡に基づく各関節の目標角度信号、目標角速度信
号および目標角加速度信号に応じて、各関節の目標角度
と実関節角度との角度偏差および各関節の目標角速度と
実関節角速度との角速度偏差を零に収束させるような切
換関数により各関節の角度をすべり状態制御するための
切換信号を求めるとともに、実関節加速度信号をフィー
ドバックして上記目標角加速度との角加速度偏差信号を
求め、次いで、上記切換信号と角加速度偏差信号とを関
節駆動機構を駆動するための角加速度指令信号に加え合
せた後、該加算値に応じて各関節の駆動機構を駆動して
アーム手先が目標軌跡に一致するように制御する多関節
ロボットのサーボ制御方法であって、上記各関節の切換
信号の切換幅を他の関節の駆動機構からの干渉、外乱、
角加速度検出誤差等により定まる所定値よりも大きく設
定することにある。

（作用） 以上の方法により、本発明では、アーム手先の移動すべ
き軌跡に基づき求められた各関節の目標角度信号、目標
角速度信号および目標角加速度信号に応じて、各関節の
目標角度信号と実関節角度との角度偏差および各関節の
目標角速度と実関節角速度との角速度偏差を零に収束さ
せるような切換関数により、各関節の角度をすべり状態
制御するための切換信号が求められる。次に、フィード
バックされた実関節角速度信号と上記目標角加速度との
角加速度偏差信号と、上記切換信号とが関節駆動機構を
駆動するための角加速度指令信号に加え合され、該加算
値に応じて各関節の駆動機構が駆動されてアーム手先が
目標軌跡に一致するように制御される。

その場合、上記各関節の切換信号の切換幅が他の関節の
駆動機構からの干渉、外乱、角加速度検出誤差等により
定まる所定値よりも大きく設定されているので、それら
制御信号の集積誤差によってアーム手先が目標軌跡から
逸脱するのが有効に防止される。

また、実関節角加速度のフィードバック補償を行ってい
るので、他の関節の駆動機構で発生するトルク、遠心力
、コリオリカなどの干渉、外乱、負荷変動などの影響が
上記角加速度偏差信号により打ち消され、それらの影響
が低減されることになる。よって、上記切換信号の切換
幅の増大が可及的に低減されることになり、軌跡制御の
総合的な安定性および制御精度の向上を図ることができ
る。

（実施例） 以下、本発明の実施例について、第１図〜第４図に基づ
き説明する。

第１図は多関節ロボットに本発明を適用した場合の一関
節におけるブロック線図を示し、ｏｒ。

θｒ、υ「は、それぞれロボットのアーム手先が移動す
べき目標軌跡信号に基づいてプランナ（図示せず）で立
案された速度計画に基づき逐次出力される関節の目標角
度信号、目標角速度信号、目標角加速度信号である。

そして、（３）は関節側からフィードバックされる実関
節角度信号θと上記目標角度信号θ「との代数差を演算
して角度偏差Ｘ（−〇−θｒ）を算出する第１加え合せ
点、（４）は該角度偏差Ｘに角度ループゲインＣを乗じ
る角度ループゲイン要素、（５）は上記目標角速度信号
θｒとフィードバックされる実関節角速度信号θとの代
数差を演算して角速度偏差−文（−ｏｒ−〇）を算出す
る第２加え合わせ点、（７）は上記角度ループゲイン要
素（４）の乗算結果Ｃ−ｘと上記角速度偏差−文との代
数差（Ｃｏｘ十文）を演算する第３加え合せ点、（８）
は該第３加え合せ点（７）の出力信号（Ｃ−ｘ十文）を
すべり状態で切換えるためのリレー回路である。ここに
、該リレー回路（８）において、その入力信号ＺＷを切
換関数としてすべり状態制御するようになされており、
切換関数ｚｗは ｚｗ　＝Ｃ−ｘ十文　　　　　　　　　　　（１）で表
わされる。

ここで、上記切換関数ＺＷは下記のように設定されてい
る。

ｚ、ｗ＞０　　ならば　２ｗくＯ ｚｗ＜Ｏならば　之ｗ　＞　ｏ　　　　　　ｆ２）ある
いは、 ＺＷ＞０　　ならば　父＜−Ｃ−文 ＺＷ＜０　　ならば　父＞−Ｃ−文　　　（３）を満足
するように、つまり、 ２ｚｗ　・２ｗ＜Ｏ［４） になるように設計されている。すなわち、第２図に示す
ように、状態点が切換線に収束していわゆるスライディ
ングモードに入るように設計されていて、上記リレー回
路（８）により、角度偏差Ｘおよび角加度偏差文を零に
収束させるような切換関数ｚｗによるすべり状態制御を
行って、出力信号としての切換信号ｕ（ｕは切換関数Ｚ
Ｗの値によって変化する量である）を求めるようにして
いる。

次に、（９）は上記角速度偏差−文に角速度ループゲイ
ンＫｖを乗算して、角加速度指令信号υｒ’（−−Ｋｖ
−文）を出力する角速度ループゲイン要素である。

また、（１０）は上記目標角加速度信号υｒと実関節角
加速度信号υの代数差を演算して角加速度偏差−父（−
υｒ−υ）を算出する第４加え合わせ点であって、該角
加速度偏差信号−父により、角加速度フィードバック補
償をするようにしている。なお、このとき、関節側から
フィードバックされる実関節角加速度信号υは、加速度
計を用いる方法、角速度を微分する方法、オブザーバを
用いる方法等により検出され、実関節角加速度信号υに
は、所定の検出誤差ｎが加重されている。したがって、
上記第４加え合せ点（１０）から出力される信号値は−
（父＋ｎ）となる。

そして、（１１）は、上記リレー回路（１１）からのす
べり状態制御による切換信号Ｕと、角速度ループゲイン
要素（９）の出力信号υ「′と、上記第４加え合せ点（
１０）から出力される角加速度偏差信号−（父＋ｎ）と
の代数和を演算する第５加え合せ点であって、下記出力
信号ｚｏｗ＝−Ｋｖ　−文一父−ｎ　＋　ｕ　　　　　
　（５）を出力するものである。

さらに、（１３）は上記第５加え合せ点（１１）からの
出力信号ＺＱに角加速度ループゲインＫａを乗じてトル
ク信号を算出する角加速度ループゲイン要素、（１２）
は該トルク信号に外部からの影響で加えられる外乱ｄを
加算する仮想的な加え合せ点としての第６加え合せ点で
あって、この外乱ｄの原因としては、他軸モータの発生
するトルク、遠心力、コリオリカなどの干渉、電気的ノ
イズ等がある。次に、（１４）は該第６加え合せ点（１
２）から出力される外乱の加わったトルク信号に応じて
関節を回転駆動する駆動機構としてのモータ、（１５）
は該モータ（１４）の実関節角加速度信号υを積分して
関節の角速度θを得る第１積分要素、（１６）は該第１
積分要素（１５）の実関節角速度信号θを積分して実関
節角度θを得る第２積分要素であり、該第２積分要素（
１６）の出力値である関節角度θがアームに入力され三
次元座標に変換されるようになされている。

ここに、本発明の特徴として、上記リレー回路（８）に
おけるすべり状態制御における切換信号Ｕの切換幅は以
下のように設定されている。

すなわち、上記第６加え合せ点（１２）で加え合わされ
る外乱ｄの変化範囲をＤ、角加速度指令信号の変化範囲
をＢ、関節の慣性モーメントをＩとすると、フィードバ
ックでは補償できない定常的な制御誤差ｘｏは ｘｏ　＜　（Ｄ＋Ｉ−Ｂ）　／Ｋａ　　　　　　　（６
）という条件を満足している。さらに、実関節加速度υ
の検出誤差ｎの最大値Ｎを考慮してＵＬ　−（Ｄ＋　Ｉ
−Ｂ）　／Ｋａ　＋Ｎ　　　　　（７）（ただし、Ｕ〉
０）とすると、このＵＬは第５加え合せ点（１１）から
出力される制御信号ｚＯに換算した最大誤差と考えられ
る。そこで、このＵＬを用いて、上記切換関数ｚｗの出
力値Ｕの切換えを ＺＷ＞０　　ならば　ｕ　＜　７　Ｕ　Ｌｚｗ＜Ｑ　　
ならば　ｕ　＞　Ｕ　Ｌ　　　　　　（８）となるよう
に設定する。つまり、切換信号の切換幅２Ｕを他の関節
の駆動機構からの干渉、外乱、角加速度検出誤差等によ
り定まる所定値２ＵＬよりも大きく設定するようにして
いる。

図において、ロボットのアーム手先の速度計画に基づき
目標角速度信号θｒが出力されると、第２加え合せ点（
５）において、該目標角速度信号θ「と実関節角速度信
号θとの角速度偏差−文が演算され、さらに、角速度ル
ープゲイン要素（１９）により、上記角速度偏差信号−
文に角速度ループゲインＫｖが乗じられて、角加速度指
令信号υｒ’（−−Ｋｖ　−文）が出力される。

一方、第１加え合せ点（３）において、実関節角度信号
θと目標角度信号θ「との角度偏差Ｘが演算され、角度
ループゲイン要素（４）により、該角度偏差信号Ｘに角
度ループゲインＣが乗じられて信号ＣＯＸが出力された
後、第３加え合せ点（７）で、上記第２加え合せ点（５
）からの角加度偏差信号文が加算され、その加算結果ｚ
ｗ　（＝Ｃ−ｘ＋Ｘ）を切換関数としてリレー回路（８
）ですべり状態制御が行われ、上記（８）式に表わされ
る切換信号Ｕが出力される。

また、第４加え合せ点（１０）で目標角速度信号υ「と
実関節角加速度信号りとの偏差−父が演算され、実関節
加速度υの検出誤差ｎを含んだ角加速度偏差信号−（父
°＋ｎ）が出力される。

そして、第５加え合せ点（１１）において、上記角加速
度指令信号υｒ’（−−Ｋｖ　・文）と、切換関数ｚｗ
によるすべり状態制御の切換信号Ｕと角加速度偏差信号
−（父子〇）の代数和が演算されて、最終的に上記（５
）式で示される制御信号２０が出力され、さらに、第６
加え合せ点（１２）で外乱ｄが加えられて、角加速度ル
ープゲイン要素（１３）でトルク信号に変換された後、
該トルク信号に応じてモータ（１４）が回転駆動されて
、アーム手先が目標軌跡に一致するように多関節ロボッ
トのサーボ制御が行われる。

したがって、上記実施例においては、角度偏差Ｘおよび
角速度偏差文を零に収束させるような切換関数ｚｗ（＝
Ｃａｘ＋文）によるすべり状態制御を行い、その出力信
号Ｕを角加速度指令信号υ「に加算するようにしている
ので、従来の方法のように、角速度偏差文のみを零に収
束させるようなすべり状態制御の出力をモータ（１４）
へのトルク出力として決定するのに比べて、角度誤差つ
まり角度偏差Ｘがゲインの高いすべり状態制御で補正さ
れることになり、他軸モータで発生するトルク、遠心力
、コリオリカの干渉、外乱、負荷変動等の影響に対して
、安定なしかも強い拘束力を持ったロバストな制御がで
きる。

しかも、上記すべり状態制御における切換信号Ｕの切換
幅２Ｕを他の関節の駆動機構からの干渉、外乱、角加速
度検出誤差等により定まる所定値２ＵＬよりも大きく設
定しているので、それら制御信号の集積誤差によってア
ーム手先が目標軌跡から逸脱するのを有効に防止するこ
とができる。

加えて、モータ（１４）の実関節角加速度υ「のフィー
ドバック補償を行っているので、上記他軸モータからの
干渉、外乱、負荷変動などの影響に対して、それらを打
ち消すような角加速度偏差信号−父「がフィードバック
されることになり、それらの影響が低減されることにな
る。

よって、上記切換信号の切換幅２Ｕの増大を可及的に低
減することができ、軌跡制御の総合的な安定性と制御精
度の向上とを図ることができるのである。

以上を定；的に説明するに、第３図は関節の角速度−角
加速度線図上における総合的な制御誤差の分布特性を示
し、直線ａ、ｂはそれぞれリレー回路（８）の切換によ
る正負の基準線父−−Ｃ・文＋Ｕ、父−−Ｃ・文−Ｕ１
直線Ｃは出力値Ｕが零の基準直線父−−Ｃ−文である。

また、直線ａ１＋ａｌ！はそれぞれ該基準直線ａから最
大誤差Ｕの範囲を示す上限および下限直線、同様にｂＩ
＋ｂ２は基準直線すに対する誤差範囲の上限および下限
直線である。ここに、上述のごとくリレー回路（８）に
おけるすべり制御の切換信号の切換幅２Ｕを上記（８）
式のように設定しているので、図に示されるように、正
側の基準直線ａの下限ａ１は零の基準直線Ｃよりも常に
上側に、負側の基僧直線すの上限ｂ２は零の基準直線Ｃ
よりも常に下側にあって、両者ａ１．ｂ２が交わること
がない。

すなわち、外乱等の誤差により、すべり状態制御に誤差
が生じてアーム手先が軌跡から逸脱することがないので
ある。このように、切換信号の切換幅２Ｕを制御系の外
乱等による最大誤差の幅２ＵＬよりも大きく設定するこ
とにより、軌跡制御の安定性が確保されるとともに、正
負の基準直線ａ。

ｂの位置が定量的に定められ、上記アーム手先の軌跡上
からの逸脱を防止するためにリレー回路（８）の切換信
号Ｕの切換幅２Ｕの値を必要以上に増大するのが防止さ
れるのである。

次に、第４図および第５図は２関節スカラ型マニュピレ
ータでのシミュレーション例であって、第４図は本発明
のすべり制御方法により、ロボット中心から半径１０牙
の円を描かせたときのアーム手先の半径軌道を示す実験
結果、第５図は同じ条件下における従来のすべり制御方
法による実験結果である。両図を比較すれば、従来の方
法ではアーム手先が描く軌跡の理想円からのバラツキか
大きいのに対して、本発明の方法では理想円からほぼ一
定の範囲に収束していて、本発明の効果が示されている
。

（発明の効果） 以上に説明したように、本発明の多関節ロボットのサー
ボ制御方法によれば、各関節の角度偏差および角速度偏
差を零に収束させるようなすべり状態制御による切換信
号を駆動機構の角加速度指令信号に加算し、さらに実関
節角加速度のフィードバック補償を行うとともに、各関
節の上記すべり状態制御における切換信号の切換幅を他
軸モータで発生するトルク、遠心力、コリオリカ等の干
渉、外乱、負荷変動等の集積した総合誤差値により定ま
る所定値よりも大きく設定するようにしたので、それら
の影響などに対して安定なロバスト性を維持しながら切
換信号の切換幅の増大を可及的に抑制することができ、
総合的な安定性および制御精度の向上を図ることができ
る。

【図面の簡単な説明】

第１図〜第４図は本発明の実施例を示し、第１図は多関
節マニピュレータのうちの一関節の制御系を示すブロッ
ク線図、第２図はリレー回路におけるすべり状態制御の
切換特性図、第３図は切換信号の誤差分布を示す角速度
−角加速度特性図、第４図は２関節マニュピレータによ
る軌跡制御精度を示す実験結果図である。第５図は従来
のすべり状態制御方法による第４図相当の実験結果図で
ある。 （８）・・・リレー回路、（１４）・・・モータ（駆動
機構） Ｃ＋１）”（Ｊ）　　　＝（） 第４図 第５図 第３図 東

Claims (1)

【特許請求の範囲】
1. （１）アーム手先が移動すべき軌跡に基づく各関節の目
   標角度信号、目標角速度信号および目標角加速度信号に
   応じて、各関節の目標角度と実関節角度との角度偏差お
   よび各関節の目標角速度と実関節角速度との角速度偏差
   を零に収束させるような切換関数により各関節の角度を
   すべり状態制御するための切換信号を求めるとともに、
   実関節加速度信号をフィードバックして上記目標角加速
   度との角加速度偏差信号を求め、次いで、上記切換信号
   と角加速度偏差信号とを関節駆動機構を駆動するための
   角加速度指令信号に加え合せた後、該加算値に応じて各
   関節の駆動機構を駆動してアーム手先が目標軌跡に一致
   するように制御する多関節ロボットのサーボ制御方法で
   あって、上記各関節の切換信号の切換幅を他の関節の駆
   動機構からの干渉、外乱、角加速度検出誤差等により定
   まる所定値よりも大きく設定することを特徴とする多関
   節ロボットのサーボ制御方法。