JPH0852675A - マニピュレータの選択的外乱補償型ハイブリッド制御装置および制御方法 - Google Patents

Abstract

(57)【要約】 【目的】本発明は、マニピュレータの精密な動特性モデ
ルが不必要で、先端と外部環境との干渉力の影響を受け
にくいマニピュレータの選択的外乱補償型ハイブリッド
制御装置を提供することを目的とする。 【構成】本発明は、多関節マニピュレータ２の関節角度
を入力とする角速度と角加速度の推定部３と、アクチュ
エータへの関節入力トルク、マニピュレータの推定角加
速度および力センサからの力信号を利用してモデル化誤
差トルクを外乱トルクとして推定する誤差トルク計算部
４と、誤差トルク計算部４の出力を入力としノイズを除
去するフィルタ５と、マニピュレータの関節角度とマニ
ピュレータの推定角速度と力信号を入力としてマニピュ
レータ先端の可動方向に関する位置と拘束方向に関する
力を目標軌道に追従させるための指令関節トルクを計算
する位置と力のハイブリッド制御部６とを具備して構成
する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ロボットマニピュレー
タの運動制御装置に関し、特にマニピュレータ先端の位
置と力を制御するマニピュレータの選択的外乱補償型ハ
イブリッド制御方法に関する。

【０００２】

【従来の技術】従来のハイブリッド制御装置では、バリ
取りなど外部環境とのダイナミックな接触を要する作業
を安定で高精度に実現するためには、マニピュレータの
動特性を求めて計算トルク法による線形化補償を基礎に
設計する必要がある。

【０００３】

【発明が解決しようとする課題】しかし、多関節マニピ
ュレータの複雑な動特性を精密に推定したり、これをリ
アルタイムに計算して線形化補償を行うことは非常に困
難であった。また、マニピュレータ先端と外部環境との
接触点における摩擦力などの干渉力が制御性能を劣化さ
せる要因となっていた。

【０００４】本発明は上記の事情に鑑みてなされたもの
で、マニピュレータの精密な動特性モデルが不必要で、
先端と外部環境との干渉力の影響を受けにくいマニピュ
レータの選択的外乱補償型ハイブリッド制御装置を提供
することを目的とする。

【０００５】

【課題を解決するための手段】上記目的を達成するため
に本発明のマニピュレータの選択的外乱補償型ハイブリ
ッド制御装置は、先端の力センサと各関節軸の角度検出
装置とアクチュエータを具備する多関節マニピュレータ
の先端の位置と力に関するハイブリッド制御装置であっ
て、前記多関節マニピュレータの関節角度を入力として
角速度と角加速度を出力する角速度と角加速度の推定部
と、前記アクチュエータへの関節入力トルク、前記角速
度と角加速度の推定部で求められたマニピュレータの推
定角加速度および前記力センサからの力信号を利用して
モデル化誤差トルクを外乱トルクとして推定する誤差ト
ルク計算部と、前記誤差トルク計算部の出力を入力とし
ノイズを除去するフィルタと、前記多関節マニピュレー
タの関節角度と前記角速度と角加速度の推定部で求めら
れたマニピュレータの推定角速度と力信号を入力として
マニピュレータ先端の可動方向に関する位置と拘束方向
に関する力を目標軌道に追従させるための指令関節トル
クを計算する位置と力のハイブリッド制御部とを具備
し、前記誤差トルク計算部が、前記マニピュレータの力
信号を入力としてマニピュレータの先端が外部環境から
拘束を受けている方向の成分を検出する拘束方向成分検
出部と、この拘束方向成分検出部からの出力を入力とし
てこの入力と等価な関節トルクを計算する等価関節トル
ク計算部と、前記角速度と角加速度の推定部からの出力
に慣性行列モデルを掛け合わせる慣性行列モデル乗算部
とを有し、この慣性行列モデル乗算部の出力から前記ア
クチュエータへの関節入力トルクと前記等価関節トルク
計算部の出力を減じて外乱トルクとして出力することを
特徴とするものである。

【０００６】又、本発明のマニピュレータの選択的外乱
補償型ハイブリッド制御方法は、先端の力センサと各関
節軸の角度検出装置とアクチュエータを具備する多関節
マニピュレータの先端の位置と力に関するハイブリッド
制御方法において、前記各関節軸の角度検出装置により
検出される関節角度から角速度と角加速度を推定する第
１の手段と、この第１の手段で推定された関節角加速度
にマニピュレータの慣性行列モデルを乗算して慣性トル
クを計算する第２の手段と、前記力センサから検出され
た外部環境との接触力のうち接触面垂直方向成分を選択
してこれにより生じる等価関節トルクを計算する第３の
手段と、前記第２の手段で計算した慣性トルクから関節
入力トルクと等価関節トルクを引き算して誤差トルクを
推定する第４の手段と、この第４の手段で推定した誤差
トルクの補償効果の周波数帯域を制限すると共にノイズ
を除去するようにフィルタ処理する第５の手段と、前記
各関節の角度と推定された角速度と力センサ情報から対
象環境の幾何学的モデルとマニピュレータの慣性行列モ
デルを用いて先端の可動方向に対する位置制御と拘束方
向に対する力制御を行うための指令関節トルクを計算す
る第６の手段と、この第６の手段で計算した指令関節ト
ルクからフィルタ処理された誤差トルクを引き算して関
節入力トルクを計算する第７の手段とを、関節角度と力
センサ情報を検出して関節入力トルクを出力する過程に
具備することを特徴とする。

【０００７】

【作用】上記手段により本発明は、マニピュレータの動
特性モデルと誤差ダイナミクスや可動方向に対する外部
環境からの干渉力を外乱トルクとして選択的に推定・補
償して、マニピュレータの動特性をモデルとマッチング
させることにより、先端位置と力のハイブリッド制御系
をモデルに基づいて容易に構成することが可能となる。

【０００８】

【実施例】以下図面を参照して本発明の実施例を詳細に
説明する。図１は本発明であるマニピュレータの選択的
外乱補償型ハイブリッド制御装置の一実施例を示すブロ
ック図である。即ち、先端の力センサと各関節軸の角度
検出装置とアクチュエータを具備する多関節マニピュレ
ータ２の先端の位置と力に関する選択的外乱補償型ハイ
ブリッド制御装置１であって、前記多関節マニピュレー
タ２の関節角度を入力として角速度と角加速度を出力す
る角速度と角加速度の推定部３と、前記アクチュエータ
２への関節入力トルク、前記角速度と角加速度の推定部
３で求められたマニピュレータ２の推定角加速度および
前記力センサからの力信号を利用してモデル化誤差トル
クを外乱トルクとして推定する誤差トルク計算部４と、
前記誤差トルク計算部４の出力を入力としノイズを除去
するフィルタ５と、前記多関節マニピュレータ２の関節
角度と前記角速度と角加速度の推定部３で求められたマ
ニピュレータ２の推定角速度と力信号を入力としてマニ
ピュレータ２の先端の可動方向に関する位置と拘束方向
に関する力を目標軌道に追従させるための指令関節トル
クを計算する位置と力のハイブリッド制御部６とを具備
する。７は関節入力トルクと外乱の加算部であり、８は
指令関節トルクからフィルタ処理された誤差トルクを引
き算して関節入力トルクを計算する減算部である。

【０００９】図２は図１の誤差トルク計算部４の一例を
示すブロック図である。即ち、前記誤差トルク計算部４
は、前記マニピュレータ２の力信号を入力としてマニピ
ュレータ２の先端が外部環境から拘束を受けている方向
の成分を検出する拘束方向成分検出部２３と、この拘束
方向成分検出部２３からの出力を入力としてこの入力と
等価な関節トルクを計算する等価関節トルク計算部２４
と、前記角速度と角加速度の推定部３からの出力に慣性
行列モデルを掛け合わせる慣性行列モデル乗算部２２と
を有し、この慣性行列モデル乗算部２２の出力から前記
アクチュエータへの関節入力トルクと前記等価関節トル
ク計算部２４の出力を減算部２５，２６により減じて外
乱トルクとして出力する。

【００１０】図３は本発明に係るマニピュレータの選択
的外乱補償型ハイブリッド制御方法の関節角度と力セン
サ情報を検出して関節入力トルクを出力するまでの過程
に含まれるフローチャートである。即ち、イにおいて、
先端の力センサと各関節軸の角度検出装置とアクチュエ
ータを具備する多関節マニピュレータの関節角度、外界
との接触力、及び関節入力トクルを検出する。次に、ロ
において、前記各関節軸の角度検出装置により検出され
る関節角度から各関節の角速度と角加速度を推定する。
次に、ハにおいて、前記関節角加速度にマニピュレータ
の慣性行列モデルを乗算して慣性トルクを計算する。次
に、ニにおいて、外部環境との接触力のうち接触面垂直
方向成分（拘束方向成分）を検出してこれにより生じる
等価関節トルクを計算する。次に、ホにおいて、前記慣
性トルクから関節入力トルクと等価関節トルクを引き算
して誤差トルクを計算する。次に、ヘにおいて、前記誤
差トルクの補償効果の周波数帯域を制限すると共にノイ
ズを除去するようにフィルタ処理する。次に、トにおい
て、前記各関節の角度と推定された角速度と力センサ情
報から対象環境の幾何学的モデルとマニピュレータの慣
性行列モデルを用いて先端の可動方向に対する位置制御
と拘束方向に対する力制御のハイブリッド制御を行うた
めの指令関節トルクを計算する。次に、チにおいて、前
記指令関節トルクからフィルタ処理された誤差トルクを
引き算して関節入力トルクを計算する。次に、次式で与
えられるｎ自由度の多関節マニピュレータ

【００１１】

【数１】 の動的ハイブリッド制御について考える。ただし、ｑは
多関節マニピュレータの関節角ベクトル、τは関節駆動
力ベクトル、Ｍは慣性行列、ｈはコリオリ力、求心力、
摩擦力及び重力項を表す。Ｆはマニピュレータ先端が対
象環境に与える力を示す。また、作業座標中のマニピュ
レータ先端の位置ベクトルをｒ、マニピュレータ先端に
関するヤコビ行列をＪとし、冗長性はないものとする。
一方、対象環境の機械的インピーダンス行列をＺ_e (s)
とおくときｓ領域で次の関係式が成り立つものとする。

【００１２】

【数２】 また、マニピュレータ先端が接触している対象環境面か
ら受ける拘束の式（拘束曲面）は既知で次のように与え
られているものとする。

【００１３】

【数３】

【００１４】ただし、ｒ_F (r) はｎ_F 次元ベクトルであ
る。ここで、｛ｒ_F (r) ，ｒ_P (r)｝の各要素が独立と
なるような（ｎ−ｎ_F ）次元ベクトルｒ_P (r) を選んで
一般化座標ｒ_c ＝［ｒ_P ^T ，ｒ_F ^T ］^T を定義する。ま
た、これに対応する一般化力

【００１５】

【数４】 を定義する。このとき、マニピュレータ先端の可動方向
に対する位置制御と拘束方向に対する力制御を同時に実
現するハイブリッド制御のための次のような制御アルゴ
リズムが計測と制御ｖｏｌ．３０，ｎｏ．５，１９９１
の吉川の文献“マニピュレータの力制御”に示されてい
る。

【００１６】

【数５】

【００１７】ただし、ｒ_Prは可動方向に対する位置の目
標軌道、ｆ_Frは拘束方向に対する力の目標軌道、Ｋ
_Pp- ，Ｋ_Pd，Ｋ_F1はフィードバックゲインである。一
方、マニピュレータが対象環境面と接触状態にあると
き、その先端に取り付けられた力センサからは拘束方向
の押しつけ力の他に可動方向の運動に伴う摩擦力が検出
される。あるいは、力センサの先に適当なワークを装備
している場合にはその可動方向の動特性に対応した反力
も加わることになる。さらに、マニピュレータ自体の動
特性モデルの誤差が無視できないときには（５），
（６）式のサーボ補償だけでは十分な安定性や制御性能
が得られない場合もある。

【００１８】そこで、このような従来技術の問題点を解
決するために、モデル化誤差や外界からの干渉の影響を
受けにくい外乱補償型のハイブリッド制御系を構築す
る。具体的には、マニピュレータの適当な動特性モデル
を考え、実際の動特性との誤差ダイナミクスを推定して
補償する。特に、マニピュレータ先端に外界から作用す
る力を可動方向と拘束方向とに分けて考え、可動方向に
加わる摩擦力等の干渉力によって発生するトルクＪ^T Ｅ
^T ［ｆ_P ^T ，０］^T をマニピュレータ本体の誤差ダイナ
ミクスと合わせて外乱トルクとして推定・ 補償する選択
的な外乱補償制御を考える。ここでは、次のような動特
性モデルを定義する。

【００１９】

【数６】 はこれに対する関節角度ベクトルと力ベクトルの理想応
答、Ｓは次のような選択行列を表す。

【００２０】

【数７】

【００２１】このとき、マニピュレータが対象環境面垂
直方向（拘束方向）から受ける力は−Ｅ^T ＳＥ^-TＦ、こ
れと等価な関節トルクは−Ｊ^T Ｅ^T ＳＥ^-TＦで表される
ことになる。従って、（７）式の動特性モデルに対する
マニピュレータ誤差ダイナミクスは次式で与えられる。

【００２２】

【数８】

【００２３】この外乱トルクを（９）式中の２番目の表
現式を用いて推定する。例えば関節角度ベクトルのサン
プリング値を用いて関節角速度と角加速度を次のように
推定する。

【００２４】

【数９】 はそれぞれ推定された関節角速度と角加速度を示し、各
変数の添字ｋは時刻ｔ＝ｋＴ_s におけるサンプル値であ
ることを表す。無論、これらは適当な状態オブザーバを
構成して推定してもよい。これに慣性行列モデルを掛け
合わせ、１サンプル前の入力トルクと対象環境面から拘
束方向に作用する反力と等価な関節トルクを引き算すれ
ば、外乱トルクを次のように算出できる。

【００２５】

【数１０】 τ_d ^* を推定された外乱トルク、φ(s) を外乱オブザー
バによって決まる外乱推定の伝達関数であるとすれば、
次式が成り立つ。

【００２６】

【数１１】 τ_d ^* を適当なフィルタｋ(s) を介して、

【００２７】

【数１２】 のように関節トルク入力にフィードバックすれば次の結
果を得る。

【００２８】

【数１３】

【００２９】また、ｈをマニピュレータの運動によって
動的に変化するものと、マニピュレータの運動に直接相
関のないものとに分けられるものとして、便宜上次のよ
うに表している。

【００３０】

【数１４】

【００３１】従って、（１５）式を考慮すれば少なくと
もｋφ＝１となるような周波数領域では（７）式の動特
性モデルが実現されることがわかる。このとき、位置と
力のハイブリッド制御のために指令関節駆動ベクトルτ
_c は次のように与えればよい。

【００３２】

【数１５】

【００３３】ただし、ｒ_Prは可動方向に対する位置の目
標軌道、ｆ_Frは拘束方向に対する力の目標軌道、Ｋ_Pp，
Ｋ_Pd，Ｋ_Fiはフィードバックゲインである。表１に示す
物理常数を持つ２自由度マニピュレータを用いてハイブ
リッド制御シミュレーションを行い、選択的外乱補償制
御によるハイブリッド制御系の制御性能を検証する。図
４に示すようにマニピュレータ先端はノミナル値５Ｋｇ
の負荷を介してバネ、質量、ダンパモデルで表される対
象環境面に接触しているものとする。表２に対象環境の
各物理定数を示す。

【００３４】

【表１】

【００３５】

【表２】

【００３６】目標とする押し付け力に対して対象環境の
ｘ方向のインピーダンスが十分に大きい場合、拘束面の
方程式はｘがほぼコンスタント（ｃｏｎｓｔａｎｔ）と
なるのでＥ＝ｄｉａｇ（１，１）となる。ｙ方向の位置
制御の目標軌道は

【００３７】

【数１６】 でｆ＝１Ｈｚとしたものを、ｘ方向の力制御の目標軌道
には次式を用いる。

【００３８】

【数１７】

【００３９】選択的外乱補償制御のための角速度と角加
速度は（１０），（１１）式を使って推定する。誤差ト
ルク（外乱トルク）はノミナル状態の負荷の影響も慣性
行列モデルに含めて次のように計算する。

【００４０】

【数１８】

【００４１】Ｔ_s はサンプリング時間でＴ_s ＝５ｍｓｅ
ｃとする。このときフィルタｋは、例えば計測自動制御
学会論文集、ｖｏｌ．３０，ｎｏ．７，１９９４の吉田
らの文献“外乱補償制御型マニピュレータシステムのμ
シンセシス”に記載されている方法で設計された。

【００４２】

【数１９】 をＴ_s ＝５ｍｓｅｃで双一次変換したものを用いる。シ
ミュレーションは（４），（５），（６）式の計算トル
ク法に基づく制御法と選択的外乱補償制御を用いた場合
を比較する。位置制御系のフィードバックゲインはｋ_Pp
＝１００ｓｅｃ^-2，ｋ_Pd＝２０ｓｅｃ^-1、力制御系のフ
ィードバックゲインはｋ_Fi＝１００ｓｅｃ^-1と選ぶ。

【００４３】図５は負荷がノミナル値のときの各制御系
のよるシミュレーション結果を示す。目標軌道を細線で
表しているが計算トルク法を用いた場合の位置と力の応
答はほとんど目標軌道と一致している。外乱補償制御系
を用いた場合、位置の応答は軌道追従制御とほぼ同等の
特性を示しているが力の応答は目標値に安定に収束する
がやや振動的になっている。

【００４４】図６及び図７はそれぞれ負荷重量が減少し
た場合と増加した場合のシミュレーション結果を示す。
計算トルク法による制御においては負荷の増減に応じて
位置と力の応答が目標軌道に対し大きな誤差を生じサー
ボ補償だけでは不十分であることを示している。外乱補
償制御による場合、位置制御においては負荷増加時に若
干の追従誤差を生じているもののほぼ良好な応答が得ら
れているが、力制御においては制御性能が劣化するかあ
るいは不安定になっている。

【００４５】

【発明の効果】以上述べたように本発明によれば、マニ
ピュレータの動特性モデルとの誤差ダイナミスクや先端
の可動方向に作用する外部環境からの干渉力を外乱トル
クとして推定・補償することにより、マニピュレータと
その外部環境との相互作用に関する正確な動特性モデル
が得られない場合でも高精度な位置と力のハイブリッド
制御が実現できるという効果がある。

【図面の簡単な説明】

【図１】本発明であるマニピュレータの選択的外乱補償
型ハイブリッド制御装置の一実施例を示すブロック図で
ある。

【図２】図１の誤差トルク計算部の一例を示すブロック
図である。

【図３】本発明に係るハイブリッド制御方法の関節角度
と力センサ情報を検出して関節入力トルクを出力するま
での過程に含まれるフローチャートの一例である。

【図４】本発明に係るハイブリッド制御シミュレーショ
ンに用いた２自由度マニピュレータと外部環境のモデル
の一例を示す説明図である。

【図５】負荷がノミナル状態であるときの従来の計算ト
ルク法に基づく制御と本発明を適用した場合の目標軌道
への追従性を比較した特性図である。

【図６】負荷が軽いときの従来の計算トルク法に基づく
制御と本発明を適用した場合の目標軌道への追従性を比
較した特性図である。

【図７】負荷が重いときの従来の計算トルク法に基づく
制御と本発明を適用した場合の目標軌道への追従性を比
較した特性図である。

【符号の説明】

１…選択的外乱補償型ハイブリッド制御装置、２…マニ
ピュレータ、３…角速度と角加速度の推定部、４…誤差
トルク計算部、５…フィルタ、６…ハイブリッド制御
部、７…加算部、８…減算部、２２…慣性行列モデル乗
算部、２３…拘束方向成分検出部、２４…等価関節トル
ク計算部、２５，２６…減算部。

Claims (2)

【特許請求の範囲】
1. 【請求項１】 先端の力センサと各関節軸の角度検出装
   置とアクチュエータを具備する多関節マニピュレータの
   先端の位置と力に関するハイブリッド制御装置であっ
   て、前記多関節マニピュレータの関節角度を入力として
   角速度と角加速度を出力する角速度と角加速度の推定部
   と、前記アクチュエータへの関節入力トルク、前記角速
   度と角加速度の推定部で求められたマニピュレータの推
   定角加速度および前記力センサからの力信号を利用して
   モデル化誤差トルクを外乱トルクとして推定する誤差ト
   ルク計算部と、前記誤差トルク計算部の出力を入力とし
   ノイズを除去するフィルタと、前記多関節マニピュレー
   タの関節角度と前記角速度と角加速度の推定部で求めら
   れたマニピュレータの推定角速度と力信号を入力として
   マニピュレータ先端の可動方向に関する位置と拘束方向
   に関する力を目標軌道に追従させるための指令関節トル
   クを計算する位置と力のハイブリッド制御部とを具備
   し、 前記誤差トルク計算部が、前記マニピュレータの力信号
   を入力としてマニピュレータの先端が外部環境から拘束
   を受けている方向の成分を検出する拘束方向成分検出部
   と、この拘束方向成分検出部からの出力を入力としてこ
   の入力と等価な関節トルクを計算する等価関節トルク計
   算部と、前記角速度と角加速度の推定部からの出力に慣
   性行列モデルを掛け合わせる慣性行列モデル乗算部とを
   有し、この慣性行列モデル乗算部の出力から前記アクチ
   ュエータへの関節入力トルクと前記等価関節トルク計算
   部の出力を減じて外乱トルクとして出力することを特徴
   とするマニピュレータの選択的外乱補償型ハイブリッド
   制御装置。
2. 【請求項２】 先端の力センサと各関節軸の角度検出装
   置とアクチュエータを具備する多関節マニピュレータの
   先端の位置と力に関するハイブリッド制御方法におい
   て、前記各関節軸の角度検出装置により検出される関節
   角度から角速度と角加速度を推定する第１の手段と、こ
   の第１の手段で推定された関節角加速度にマニピュレー
   タの慣性行列モデルを乗算して慣性トルクを計算する第
   ２の手段と、前記力センサから検出された外部環境との
   接触力のうち接触面垂直方向成分を選択してこれにより
   生じる等価関節トルクを計算する第３の手段と、前記第
   ２の手段で計算した慣性トルクから関節入力トルクと等
   価関節トルクを引き算して誤差トルクを推定する第４の
   手段と、この第４の手段で推定した誤差トルクの補償効
   果の周波数帯域を制限すると共にノイズを除去するよう
   にフィルタ処理する第５の手段と、前記各関節の角度と
   推定された角速度と力センサ情報から対象環境の幾何学
   的モデルとマニピュレータの慣性行列モデルを用いて先
   端の可動方向に対する位置制御と拘束方向に対する力制
   御を行うための指令関節トルクを計算する第６の手段
   と、この第６の手段で計算した指令関節トルクからフィ
   ルタ処理された誤差トルクを引き算して関節入力トルク
   を計算する第７の手段とを、関節角度と力センサ情報を
   検出して関節入力トルクを出力する過程に具備すること
   を特徴とするマニピュレータの選択的外乱補償型ハイブ
   リッド制御方法。