JP2007136564A - マニピュレータ制御方法および装置 - Google Patents

Abstract

【課題】マニピュレータ先端もしくは指先の柔軟性を実現すると同時に、中間リンク部にセンサを付加することなく関節部の柔軟性を実現する。
【解決手段】マニピュレータの関節角度を検出する角度検出器（２）と、マニピュレータの先端に作用する力を検出する力検出器（７）と、力とマニピュレータ先端の位置との関係を表す柔軟モデルを用いてマニピュレータ先端の柔軟特性を規定する柔軟モデル部（９）と、位置指令（１１）と位置変位量とを加算した信号を入力し関節角度指令を出力する逆運動学演算部（１２）と、柔軟モデルに従ってマニピュレータ先端を制御するマニピュレータ制御装置において、関節角度指令と関節角度との関節角度追従誤差（５）に基づき関節角度制御する関節角度追従制御部（６）と、外力トルク推定部（１５）と、関節の柔軟モデル部（１７）と、関節変位量を関節角度指令に加算し関節角度指令を減算器（２７）へ出力する加算器（２６）と、関節角度追従偏差を求める減算器（２７）とを備える。
【選択図】 図１

Description

本発明は、マニピュレータの柔軟制御方法および多関節多指型ハンドの柔軟制御方法に関する。特にマニピュレータ先端以外の関節部にも柔らかさを設定でき包み込みの握りができるマニピュレータの柔軟な制御に関する。

従来のマニピュレータの柔軟制御方法は、先端部の力センサを用いて、マニピュレータ先端を柔軟制御する。（例えば、特許文献１参照）。また、多指ハンドのリンク部に接触センサを設けているものもある（例えば、特許文献２参照）。
特許文献１の方法を示す図３において、１０１は従来の位置制御演算を、１０２はコンプライアンス制御演算をおこなう。なお、コンプライアンスとは剛性（ばね性）の逆数で，物体の柔らかさを示す。コンプライアンス制御とは例えばロボットの場合でいえば、手先にばねがついているかの如くコンプライアンスを望みの値に設定すること。これは関節を動かすモータの位置フィードバックゲインを 調整すれば可能である。またコンプライアンスを状況に応じて変更する制御方式を指すこともある。
図３において、１０４は力検出手段、１０５はサーボアンプ、１０６はマニピュレータである。力検出手段１０４はマニピュレータ１０６の先端部の接触力を検出し、コンプライアンス制御演算装置１０２に送る。コンプライアンス制御演算装置１０２はコンプライアンスモデルに基づき、接触力に応じた位置偏差を演算し、位置制御演算１０１により演算された目標位置に加算する。位置制御演算１０１は、この変更された目標値からマニピュレータ１０６の各関節の駆動量を計算し、これをサーボアンプ１０５に送る。マニピュレータ１０６は、サーボアンプ１０５により各関節を駆動する駆動モータにより駆動され、その結果、力検出手段１０４により検出された接触力に応じて柔軟な動作をおこなう。
特許文献２を示す図４においては、指機構のそれぞれのリンク機構２０１、２０２の腹側の面部に圧力センサ２０３、２０４を設け、指機構のそれぞれの先端部のリンク機構２０５に力センサ２０６を設けている。指機構で物体を把持する場合に、その指機構の力センサ２０３、２０４、２０６のいずれかが物体との接触に伴う外力を検知したとき、その力センサにより近い力センサが順次外力を検知するように指機構の関節を動作させる。
このように、従来のマニピュレータの柔軟制御方法では、柔軟な動作をさせたい部位に力センサを設置して柔軟制御する、という方法がとられていた。
特開平５−１８９００８号公報（第５頁、図１） 特開２００１−２８７１８２号公報（第１０頁、図１）

特許文献１の制御方法では、先端の力に対する柔軟性は実現できるが、関節部の柔軟性は実現できない。指の腹部には柔軟性がないので、指全体を使った包み込みの握りができなかった。
また、特許文献２の制御方法では、関節部の柔軟性を実現するために指リンクの腹部にセンサが必要で、コスト面とセンサに定格値以上の過大な過重、圧力が加わるとセンサが故障するという問題があった。また、各センサへの入力電源と出力信号配線が必要となりセンサの増加に伴ってハンドの配線処理が複雑になっていた。
本発明はこのような問題点に鑑みてなされたものであり、マニピュレータ先端もしくは指先の柔軟性を実現していると同時に、中間リンク部（指の腹部）に力、圧力センサを付加することなく関節部の柔軟性を実現できる方法と装置を提供することを目的とする。

上記問題を解決するため、本発明は、次のようにしたのである。
請求項１記載の発明は、マニピュレータの先端に作用する力を検出し、この力とマニピュレータ先端の位置との関係を表すモデルを用いてマニピュレータ先端の柔軟特性を規定し、前記モデルに従ってマニピュレータ先端を制御するマニピュレータ制御方法において、マニピュレータの関節を制御するにあたり、前記マニピュレータの関節角度の追従誤差と関節に加わるトルクを推定し、前記推定したトルクと関節角度との関係を表す柔軟モデルに基づいて前記関節の角度を制御することを特徴とするものである。
また、請求項２記載の発明は、請求項１において前記マニピュレータが多関節多指ハンドであることを特徴とするものである。
また請求項３記載の発明は、マニピュレータの関節角度を検出する角度検出器（２）と、マニピュレータの先端に作用する力を検出する力検出器（７）と、前記力とマニピュレータ先端の位置との関係を表す柔軟モデルを用いてマニピュレータ先端の柔軟特性を規定する柔軟モデル部（９）と、位置指令（１１）と位置変位量とを加算した信号を入力し関節角度指令を出力する逆運動学演算部（１２）と、前記柔軟モデルに従ってマニピュレータ先端を制御するマニピュレータ制御装置において、関節角度指令と関節角度との関節角度追従誤差（５）に基づき関節角度制御する関節角度追従制御部（６）と、前記関節角度追従誤差（５）を入力してトルク推定値を求め、関節の柔軟モデル部（１７）へ前記トルク推定値を出力する外力トルク推定部（１５）と、外力トルク推定部（１５）からのトルク推定値に基づき関節変位量を求める関節の柔軟モデル部（１７）と、関節の柔軟モデル部（１７）から出力される関節変位量を関節角度指令に加算し関節角度指令を減算器（２７）へ出力する加算器（２６）と、前記関節角度指令と関節角度を入力し前記関節角度追従偏差を求める前記減算器（２７）とを備えたことを特徴とするものである。
また請求項４記載の発明は、請求項３において前記減算器（２７）出力と前記外力トルク推定部（１５）入力との間にフィルタ（１４）を設けたことを特徴とするものである。
請求項５記載の発明は、請求項３において前記マニピュレータが多関節多指ハンドとしたことを特徴とするものである。

本発明によると、マニピュレータ先端もしくは指先の柔軟性を実現していると同時に、中間リンク部（指の腹部）に力、圧力センサを付加することなく関節部の柔軟性を実現できる。このようにマニピュレータ先端の力を検出して柔軟に制御するのに加え、マニピュレータ先端以外の部位に作用した力に対して柔軟な制御をすることができる。また、多関節多指型ハンドの各指先端の力を検出して柔軟に制御するのに加え、各指先端以外の部位に作用した力に対しても柔軟な動作をすることができ、指全体を使った包み込みの握りができる。

以下、本発明の方法の具体的実施例について、図に基づいて説明する。

図１は、本発明のマニピュレータの制御装置の構成を示す制御ブロック図である。１はモータ、２は角度検出器、３は関節角度、４は関節角度目標、５は関節角度追従誤差、６は関節角度追従制御部、７は先端力検出手段、８は先端力検出値、９は先端の柔軟モデル部、１０は位置変位量、１１は１１ 先端位置指令、１２は逆運動学演算部、１３は関節角度指令、１４はフィルタ、１５は外力トルク推定部、１６はトルク推定値、１７は関節の柔軟モデル部、１８は関節変位量である。２５、２６は加算器、２７は減算器である。
ここで、関節角度追従制御部６は、関節角度指令と関節角度との関節角度追従誤差５に基づき関節角度制御する。外力トルク推定部１５は、前記関節角度追従誤差５を入力してトルク推定値を求め、関節の柔軟モデル部１７へ前記トルク推定値を出力する。関節の柔軟モデル部１７は、外力トルク推定部１５からのトルク推定値に基づき関節変位量を求める。加算器２６は、関節の柔軟モデル部１７から出力される関節変位量１８を関節角度指令に加算し関節角度指令を減算器２７へ出力する。減算器２７は、関節角度指令と関節角度を入力し関節角度追従偏差を求める。
次に本発明の動作を説明する。図においてマニピュレータの関節を駆動するモータ１は角度検出器２を備え、検出された関節角度３と関節角度目標４との誤差５に基づき、関節角度追従制御部６によって制御される。また、マニピュレータの先端力の力検出器７によって検出された先端力検出値８から、先端の柔軟モデル部９によって位置変位量１０が演算される。先端位置指令１１にこの位置変位量１０が加算され、逆運動学演算部１２によって関節角度指令１３が演算される。
一方、関節角度追従誤差５をフィルタ演算（１４）し、これを制御モデルに基づく外力トルク推定部１５で演算することによってトルク推定値１６を推定する。推定されたトルク推定値１６から、関節の柔軟モデル部１７によって関節変位量１８が演算され、これを関節角度指令１３に加算し、これが関節角度目標４となる。

外力トルク推定部１５はたとえば、関節角度追従誤差Δθの関数から求められる。最も単純には、
τ ＝ Ｋ・Δθ ・・・(１)
という関節角度追従制御６でモータ１を制御するサーボ特性をモデル化した式を立て、これを演算する。τは外力トルク、Ｋはサーボ剛性を表す定数、Δθは関節のサーボ追従誤差で、関節角度追従誤差５をフィルタ演算した５’をΔθに当てはめることで、外力トルクτが求められる。なお、ここでフィルタ１４を挿入しているのは、サーボによる変動成分を除去するためである。（１）の代わりに、より高次の特性式を立ててもよいし、非線形要素を含んだモデルを使ってもよい。その式を解くことで、同様に外力トルクτを推定することができる。

このように、マニピュレータ先端で力を検出して先端を柔軟に制御するのに加え、関節の外力トルクを推定して関節を柔軟制御するので、マニピュレータのリンク部に加わった力に対しても柔軟動作をすることができるのである。

図２は、本発明の多関節多指型ハンドの制御装置の構成を示す制御ブロック図である。本図では３本指ハンドを想定し、各指の機構および関節制御部は２０ａ、２０ｂ、２０ｃが同じ構成で、各指の先端力を検出する力検出手段７、関節を駆動するモータ１、角度検出器２、関節角度追従制御６、フィルタ１４、外力トルク推定式１５、関節の柔軟モデル部１７をブロックとして備える。
図１と同様に、多関節多指型ハンドの関節を駆動するモータ１は角度検出器２を備え、検出された関節角度３と関節角度目標４との誤差５に基づき、関節角度追従制御部６によって制御される。また、力の検出手段７によって検出された力は、各指から各々８ａ、８ｂ、８ｃとして得られ、ハンド指先の柔軟モデル部２１によって位置変位量２２が演算され、先端位置指令２３に加算され、ハンドの逆運動学演算部２４によって各指の関節角度指令１３ａ、１３ｂ、１３ｃが演算され、各関節制御部に送られる。各軸の関節制御部では、関節角度追従誤差５をフィルタ演算（１４）し、これを制御モデルに基づく外力トルク推定式１５で演算することによって外力トルク１６を推定する。推定された外力トルクから、関節の柔軟モデル部１７によって関節変位量１８が演算され、関節角度指令１３ａ（１３ｂ、１３ｃ）に加算し、これが関節角度目標４となる。

このように、多関節多指型ハンドの各指先で力を検出して指先を柔軟に制御するのに加え、関節の外力トルクを推定して関節を柔軟制御するので、ハンドの指腹部に加わった力に対しても柔軟動作をすることができる。

多関節多指型ハンドでは、指腹部を使って物体を把持する際、関節部が柔軟に動作するため、指腹部に加わった力に対し関節が逃げるような動作を取るので、物体を包み込んで把持する用途にも適用できる。

本発明の請求項１の方法を適用するマニピュレータの柔軟制御装置の構成を示す制御ブロック図 本発明の請求項２の方法を適用する多関節多指型ハンドの柔軟制御装置の構成を示す制御ブロック図 従来の方法を適用したマニピュレータの柔軟制御装置を示す制御ブロック図 従来の方法の多関節多指型ハンドの装置構成図

符号の説明

１ モータ
２ 角度検出
３ 関節角度
４ 関節角度目標
５ 関節角度追従誤差
６ 関節角度追従制御部
７ 力検出器
８、８ａ、８ｂ、８ｃ 先端力検出値
９ 先端の柔軟モデル部
１０ 位置変位量
１１ 先端位置指令
１２ マニピュレータの逆運動学演算部
１３、１３ａ、１３ｂ、１３ｃ 関節角度指令
１４ フィルタ
１５ 外力トルク推定部
１６ トルク推定値
１７ 関節の柔軟モデル部
１８ 関節変位量
２０ａ、２０ｂ、２０ｃ 指関節制御部
２１ ハンド指先の柔軟モデル部
２２ ハンド指先の位置変位量
２３ ハンド指先位置指令
２４ ハンドの逆運動学演算部
２５、２６ 加算器
２７ 減算器
１０１ 位置制御演算装置
１０２ コンプライアンス制御演算装置
１０４ 力検出
１０５ サーボアンプ
１０６ マニピュレータ
２０１、２０２ リンク機構
２０３、２０４ 圧力センサ
２０５ 先端リンク機構
２０６ 力センサ

Claims (5)

1. マニピュレータの先端に作用する力を検出し、この力とマニピュレータ先端の位置との関係を表すモデルを用いてマニピュレータ先端の柔軟特性を規定し、前記モデルに従ってマニピュレータ先端を制御するマニピュレータ制御方法において、
   マニピュレータの関節を制御するにあたり、前記マニピュレータの関節角度の追従誤差と関節に加わるトルクを推定し、前記推定したトルクと関節角度との関係を表す柔軟モデルに基づいて前記関節の角度を制御することを特徴とするマニピュレータ制御方法。
2. 前記マニピュレータが多関節多指ハンドである請求項１記載のマニピュレータ制御方法。
3. マニピュレータの関節角度を検出する角度検出器（２）と、マニピュレータの先端に作用する力を検出する力検出器（７）と、前記力とマニピュレータ先端の位置との関係を表す柔軟モデルを用いてマニピュレータ先端の柔軟特性を規定する柔軟モデル部（９）と、位置指令（１１）と位置変位量とを加算した信号を入力し関節角度指令を出力する逆運動学演算部（１２）と、前記柔軟モデルに従ってマニピュレータ先端を制御するマニピュレータ制御装置において、
   関節角度指令と関節角度との関節角度追従誤差（５）に基づき関節角度制御する関節角度追従制御部（６）と、
   前記関節角度追従誤差（５）を入力してトルク推定値を求め、関節の柔軟モデル部（１７）へ前記トルク推定値を出力する外力トルク推定部（１５）と、
   外力トルク推定部（１５）からのトルク推定値に基づき関節変位量を求める関節の柔軟モデル部（１７）と、
   関節の柔軟モデル部（１７）から出力される関節変位量を関節角度指令に加算し関節角度指令を減算器（２７）へ出力する加算器（２６）と、
   前記関節角度指令と関節角度を入力し前記関節角度追従偏差を求める前記減算器（２７）とを備えたことを特徴とするマニピュレータ制御装置。
4. 前記減算器（２７）出力と前記外力トルク推定部（１５）入力との間にフィルタ（１４）を設けたことを特徴とする請求項３記載のマニピュレータ制御装置。
5. 前記マニピュレータが多関節多指ハンドである請求項３記載のマニピュレータ制御装置。

Images