(57)【特許請求の範囲】
【請求項１】
力検出手段と、各関節の位置検出手段を備えたマニピュレータの柔軟制御装置において、
前記関節の目標位置と前記位置検出手段によって検出された位置との偏差を入力とし、前記偏差に対して第１のゲイン値を乗じる演算と前記偏差を積分して第２のゲイン値を乗じる演算とを並列して行い、それらの演算結果を加算する演算部を備え、
前記演算部の出力を前記関節へのトルク指令に変換した値を予め設定された閾値と比較し、前記トルク指令が前記閾値以上となった場合に、前記第２のゲイン値を減少させることを特徴とするマニピュレータの柔軟制御装置。
【請求項２】
力検出手段と、各関節の位置検出手段を備えたマニピュレータの柔軟制御方法において、
前記関節の目標位置と前記位置検出手段によって検出された位置との偏差に対し、第１のゲイン値を乗じる演算と積分して第２のゲイン値を乗じる演算とを並列して行い、
それらの演算結果を加算し、
前記演算部の出力を前記関節へのトルク指令に変換し、
前記トルク指令を予め設定された閾値と比較し、
前記トルク指令が前記閾値以上となった場合に、前記第２のゲイン値を減少させることを特徴とするマニピュレータの柔軟制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マニピュレータの柔軟制御装置および柔軟制御方法に関する。
【背景技術】
【０００２】
マニピュレータの柔軟制御を行う従来の装置として、先端部の力センサを用いて、マニピュレータ先端を柔軟制御するものがあった（例えば、特許文献１参照）。
また、柔軟制御の制御ゲインを、物体とマニピュレータとの接触状況に応じて位置制御モードと力制御モードの度合いを可変にし、制御の安定化を図る手法も提案されている（特許文献２参照）。
図４に特許文献１の制御装置のブロック図を示す。１０１は位置制御演算を、１０２はコンプライアンス制御演算を行う。１０４は力検出手段、１０５はサーボアンプ、１０６はマニピュレータである。力検出手段１０４はマニピュレータ１０７の先端部の接触力を検出し、コンプライアンス制御演算１０２に送る。１０２はコンプライアンスモデルに基づき、接触力に応じた位置偏差を演算し、位置制御演算１０１により演算された目標位置に加算する。位置制御演算１０１は、この変更された目標値からマニピュレータ１０６の各関節の駆動量を計算し、これをサーボアンプ１０５に送る。マニピュレータ１０６は、サーボアンプ１０５により各関節の駆動源を駆動され、その結果、力検出手段１０４により検出された接触力に応じて柔軟な動作を行う。
【０００３】
また、図５に特許文献２の制御装置のブロック図を示す。図において、マニピュレータ２０１には力検出手段２０２と位置検出手段２０６が設けられている。可変ゲイン２０３は、位置と力の協調の程度に応じてゲインを変化することができるようになっている。トルク変換２０４は力検出手段２０２の出力に可変ゲイン２０３を乗じた信号を関節トルクに変換する。位置偏差検出手段２０５は位置検出手段２０６から出力される位置信号と位置指令手段２０７から出力される指令位置との差を位置偏差として検出する。
位置偏差検出手段２０５の出力に仮想ばね要素２０８を乗じてトルクに変換したものとトルク変換２０４の出力を加減算演算２０９にて加減算演算し、その出力に比例積分演算手段２１０にて処理を施し、マニピュレータ各関節の駆動指令値とすれば、フィードバック補償演算が行える。
ここで、可変ゲイン２０３は、位置制御を行うモードではゲインを下げ、力制御を行うモードではゲインを上げて、位置制御モードと力制御モードの度合いを可変にして動作を安定化させている。
このように従来のマニピュレータの柔軟制御では、力センサを設置して柔軟制御し、さらに、柔軟制御手段のゲインを調整して位置制御モードと力制御モードの度合いを可変にして動作を安定化する手法が提案されていた。
【０００４】
【特許文献１】特開平５−１８９００８号公報（第４頁、図１）
【特許文献２】特開平２−３１０６０９号公報（第１０頁、図１）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
ところで、マニピュレータの実作業では、マニピュレータと作業対象物との接触状態が非接触から接触へ、接触から非接触へと頻繁に切り替わる。
柔軟制御によってこの状態の切り替わりを滑らかに制御することができるが、接触力が想定より大きかったり急激に変化したりすると、フィードバック補償演算の特に積分要素の累積分が接触状態の切り替わり時に不安定になり、急激な動作を引き起こすという問題があった。
また、フィードバック補償演算に積分要素がある場合にマニピュレータと作業対象物体とが接触して力が加わると、目標位置とフィードバック位置に偏差が生じる。
柔軟制御は目標位置を変化させて偏差量を小さくするように働き、フィードバック補償は偏差を小さくする方向に駆動指令を変化させるように働くので、通常は積分要素が過大に蓄積されずに動作するが、力が過大で柔軟制御やフィードバック補償が対応しきれない状態が長く続くと、積分要素が過大に蓄積される。この状態で接触状態から非接触状態に移行すると蓄積された積分要素が一気に放出される方向に補償が働くことになるため、累積量が大きいと補償が効きすぎて急激な動作を生じることになる。特許文献２の制御方法で柔軟制御ゲインを可変にしたとしても、この問題は回避することができない。
本発明はこのような問題点に鑑みてなされたものであり、マニピュレータの柔軟制御において、フィードバック補償演算の積分ゲインを状況に応じて減少させ、マニピュレータの動作を安定化させることを目的とする。
【課題を解決するための手段】
【０００６】
上記問題を解決するため、本発明は、次のようにしたのである。
請求項１に記載の発明は、力検出手段と、各関節の位置検出手段を備えたマニピュレータの柔軟制御装置において、前記関節の目標位置と前記位置検出手段によって検出された位置との偏差を入力とし、前記偏差に対して第１のゲイン値を乗じる演算と前記偏差を積分して第２のゲイン値を乗じる演算とを並列して行い、それらの演算結果を加算する演算部を備え、前記演算部の出力を前記関節へのトルク指令に変換した値を予め設定された閾値と比較し、前記トルク指令が前記閾値以上となった場合に、前記第２のゲイン値を減少させることを特徴とする。
請求項２に記載の発明は、力検出手段と、各関節の位置検出手段を備えたマニピュレータの柔軟制御方法において、前記関節の目標位置と前記位置検出手段によって検出された位置との偏差に対し、第１のゲイン値を乗じる演算と積分して第２のゲイン値を乗じる演算とを並列して行い、それらの演算結果を加算し、前記演算部の出力を前記関節へのトルク指令に変換し、前記トルク指令を予め設定された閾値と比較し、前記トルク指令が前記閾値以上となった場合に、前記第２のゲイン値を減少させることを特徴とする。
【発明の効果】
【０００７】
本発明によれば、マニピュレータと作業対象物との接触・非接触の切り替わりの際にフィードバック補償の積分要素の悪影響で急激な動作が生じることを防ぎ、安定した動作を行わせることができる。
【発明を実施するための最良の形態】
【０００８】
以下、本発明の具体的実施例について、図に基づいて説明する。
【実施例１】
【０００９】
図１は、本発明を適用する制御装置およびマニピュレータの一例を模式的に示す図である。
図１において、マニピュレータ１は作業対象部物２０を把持する多指ハンドのうちの一本を抜粋して描いたものであり、各関節に位置検出手段３を備え、各肢の腹部には力検出手段２を備えている。作業対象物２０から受ける力を力検出手段２にて検出し、柔軟制御を行うことで作業対象物２０を適切な力で把持することができる。
図２は、マニピュレータ１の１つの関節について柔軟制御の方法を示す制御ブロック図である。力検出手段２によって検出された力をもとに、柔軟制御モデル４によってマニピュレータ１の１つの関節の変位量が計算される。柔軟制御モデルとは例えば剛性、粘性、慣性の物理モデルであり、以下に示す式（１）のように表せる。
【００１０】
【数１】
0004618433.tif 000002
【００１１】
ここで、K、D、Mは、実数であって、それぞれ剛性、粘性、慣性の定数である。また、Δxは変位量、Fは力を表す。式（１）のFに力検出手段２の出力値を代入し、Δxについて式（１）を解くことで、柔軟制御モデル４の出力としてモデル変位量Δxを求める。モデル変位量は位置指令手段５の出力と加減算演算された後、フィードバック補償演算手段７の目標値として入力される。
フィードバック補償演算手段７内において、位置偏差検出手段８は入力された目標値と関節の位置検出手段３の出力との差を位置偏差として検出し、比例・積分演算手段９へ入力する。
比例・積分演算手段９内においては、入力された位置偏差に対して比例ゲイン１０を乗じたものと、積分演算手段１１にて積分演算した後に可変ゲイン１２を乗じたものとを加算した値を出力する。この値がフィードバック演算手段７の出力となり、トルク変換１３においてトルクに変換されてマニピュレータ１の関節を駆動するトルク指令となる。また、このトルク指令は閾値判定１４において予め設定した閾値と比較され、閾値よりトルク指令が大きくなった場合は、可変ゲイン１２を減少させる。閾値としては、柔軟制御モデル４で想定した最大力に相当するトルク値や、フィードバック補償演算手段７で想定する外乱としての最大トルク値を設定する。
【００１２】
トルク指令の閾値判定に基づいて積分ゲイン（可変ゲイン１２）を減少させることによって、柔軟制御モデルやフィードバック補償演算手段が対応または補償することができないトルクを要求される状況となっても、フィードバック補償の積分要素に誤差が累積されるのを防ぐことができる。
なお、図１に示したマニピュレータ１の形態や軸構成はあくまで一例に過ぎず、本発明はマニピュレータ一般に広く適用できる。例えば、図３に示すように先端に力検出手段２を介して治具２１が取り付けられたマニピュレータが、治具２１によって作業台上の作業対象物２０に対する倣い作業や磨き作業を行う場合である。
図３の場合、例えば柔軟制御モデル４にて、力検出手段２にて検出した力をマニピュレータ１の各関節軸に相当する値へと変換することで上述の制御が適用できる。
さらに、力検出手段は必ずしも力センサには限定されず、力を推定するためのオブザーバなどを構成して力センサレス柔軟制御を施す場合においても本制御方法を適用することができる。また、閾値は固定値とは限らず、作業状況や目的に応じて変化させることもできる。
【産業上の利用可能性】
【００１３】
本発明は、柔軟制御を行うマニピュレータに広く適用でき、特に動作環境や作業対象物によって、接触動作時にマニピュレータに想定以上の力が働くことがありうる場合に有用である。
【図面の簡単な説明】
【００１４】
【図１】本発明を適用する制御装置およびマニピュレータの例を示す図
【図２】本発明のマニピュレータの制御方法を示す制御ブロック図
【図３】本発明を適用する制御装置およびマニピュレータの別の例を示す図
【図４】特許文献１の制御装置を示す制御ブロック図
【図５】特許文献２の制御装置を示す制御ブロック図
【符号の説明】
【００１５】
１  マニピュレータ
２  力検出手段
３  位置検出手段
４  柔軟制御モデル
５  位置指令手段
６  加減算演算
７  フィードバック補償演算手段
８  位置偏差検出手段
９  比例・積分演算手段
１０  比例ゲイン
１１  積分演算手段
１２  可変ゲイン
１３  トルク変換
１４  閾値判定
２０  作業対象物
２１  治具
【図１】
0004618433.tif 000003
【図２】
0004618433.tif 000004
【図３】
0004618433.tif 000005
【図４】
0004618433.tif 000006
【図５】
0004618433.tif 000007