【手続補正書】
【提出日】平成20年11月7日(2008.11.7)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
マニピュレータの先端に作用する力と前記マニピュレータ先端の位置との関係を表す先端柔軟モデルを用いて前記マニピュレータ先端の柔軟特性を規定し、前記マニピュレータの先端に作用する力を検出して前記先端柔軟モデルに入力し前記マニピュレータ先端位置を制御するマニピュレータ制御方法において、
前記マニピュレータの関節角度を制御するにあたり、前記マニピュレータの関節に作用するトルクと前記マニピュレータの関節角度変位との関係を表す関節柔軟モデルを用いて前記マニピュレータの関節の柔軟特性を規定し、
前記マニピュレータの関節角度の追従誤差をもとに前記関節に加わるトルクを推定し、
前記推定したトルクを前記関節柔軟モデルに入力して前記関節の角度を制御することを特徴とするマニピュレータ制御方法。
【請求項２】
前記マニピュレータが多関節多指ハンドである請求項１記載のマニピュレータ制御方法。
【請求項３】
マニピュレータの関節角度（３）を検出する角度検出器（２）と、
マニピュレータの先端に作用する力を検出する力検出器（７）と、
前記力とマニピュレータ先端の位置との関係を表す先端柔軟モデルを用いて前記マニピュレータ先端の柔軟特性を規定する先端柔軟モデル部（９）と、
前記力検出器（７）によって検出された力に対し前記先端柔軟モデル部（９）が出力した位置変位量（１０）と前記マニピュレータ先端の位置指令（１１）とを加算した信号を入力し関節角度指令（１３）を出力する逆運動学演算部（１２）とを備え、前記マニピュレータ先端位置を制御するマニピュレータ制御装置において、
前記マニピュレータの関節に作用するトルクと前記マニピュレータの関節角度変位との関係を表す関節柔軟モデル部（１７）と、
前記関節角度指令（１３）と前記関節角度（３）との差分である関節角度追従誤差（５）に基づき前記マニピュレータの関節角度を制御する関節角度追従制御部（６）と、
前記関節角度追従誤差（５）をもとに前記関節に加わるトルク推定値を求め、前記関節柔軟モデル部（１７）へ前記トルク推定値を出力する外力トルク推定部（１５）と、
前記外力トルク推定部（１５）からのトルク推定値を入力として前記関節柔軟モデル部（１７）から出力される関節変位量を前記関節角度指令に加算する加算器（２６）と、
前記加算器（２６）の出力と前記関節角度（３）との差分を前記関節角度追従偏差（５）として出力する減算器（２７）とを備えたことを特徴とするマニピュレータ制御装置。
【請求項４】
前記減算器（２７）出力と前記外力トルク推定部（１５）入力との間にフィルタ（１４）を設けたことを特徴とする請求項３記載のマニピュレータ制御装置。
【請求項５】
前記マニピュレータが多関節多指ハンドである請求項３又は４記載のマニピュレータ制御装置。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】０００４
【補正方法】変更
【補正の内容】
【０００４】
上記問題を解決するため、本発明は、次のようにしたのである。
請求項１記載の発明は、マニピュレータの先端に作用する力と前記マニピュレータ先端の位置との関係を表す先端柔軟モデルを用いて前記マニピュレータ先端の柔軟特性を規定し、前記マニピュレータの先端に作用する力を検出して前記先端柔軟モデルに入力し前記マニピュレータ先端位置を制御するマニピュレータ制御方法において、前記マニピュレータの関節角度を制御するにあたり、前記マニピュレータの関節に作用するトルクと前記マニピュレータの関節角度変位との関係を表す関節柔軟モデルを用いて前記マニピュレータの関節の柔軟特性を規定し、前記マニピュレータの関節角度の追従誤差をもとに前記関節に加わるトルクを推定し、前記推定したトルクを前記関節柔軟モデルに入力して前記関節の角度を制御することを特徴とするものである。
また、請求項２記載の発明は、請求項１において前記マニピュレータが多関節多指ハンドであることを特徴とするものである。
また請求項３記載の発明は、マニピュレータの関節角度（３）を検出する角度検出器（２）と、マニピュレータの先端に作用する力を検出する力検出器（７）と、前記力とマニピュレータ先端の位置との関係を表す先端柔軟モデルを用いて前記マニピュレータ先端の柔軟特性を規定する先端柔軟モデル部（９）と、前記力検出器（７）によって検出された力に対し前記先端柔軟モデル部（９）が出力した位置変位量（１０）と前記マニピュレータ先端の位置指令（１１）とを加算した信号を入力し関節角度指令（１３）を出力する逆運動学演算部（１２）とを備え、前記マニピュレータ先端位置を制御するマニピュレータ制御装置において、前記マニピュレータの関節に作用するトルクと前記マニピュレータの関節角度変位との関係を表す関節柔軟モデル部（１７）と、前記関節角度指令（１３）と前記関節角度（３）との差分である関節角度追従誤差（５）に基づき前記マニピュレータの関節角度を制御する関節角度追従制御部（６）と、前記関節角度追従誤差（５）をもとに前記関節に加わるトルク推定値を求め、前記関節柔軟モデル部（１７）へ前記トルク推定値を出力する外力トルク推定部（１５）と、前記外力トルク推定部（１５）からのトルク推定値を入力として前記関節柔軟モデル部（１７）から出力される関節変位量を前記関節角度指令に加算する加算器（２６）と、前記加算器（２６）の出力と前記関節角度（３）との差分を前記関節角度追従偏差（５）として出力する減算器（２７）とを備えたことを特徴とするものである。
また請求項４記載の発明は、請求項３において前記減算器（２７）出力と前記外力トルク推定部（１５）入力との間にフィルタ（１４）を設けたことを特徴とするものである。
請求項５記載の発明は、請求項３又は４において前記マニピュレータが多関節多指ハンドであることを特徴とするものである。
【手続補正３】
【補正対象書類名】明細書
【補正対象項目名】０００７
【補正方法】変更
【補正の内容】
【０００７】
図１は、本発明のマニピュレータの制御装置の構成を示す制御ブロック図である。１はモータ、２は角度検出器、３は関節角度、４は関節角度目標、５は関節角度追従誤差、６は関節角度追従制御部、７は先端力検出手段、８は先端力検出値、９は先端の柔軟モデル部、１０は位置変位量、１１は先端位置指令、１２は逆運動学演算部、１３は関節角度指令、１４はフィルタ、１５は外力トルク推定部、１６はトルク推定値、１７は関節の柔軟モデル部、１８は関節変位量である。２５、２６は加算器、２７は減算器である。
ここで、関節角度追従制御部６は、関節角度指令と関節角度との関節角度追従誤差５に基づき関節角度制御する。外力トルク推定部１５は、前記関節角度追従誤差５を入力してトルク推定値を求め、関節の柔軟モデル部１７へ前記トルク推定値を出力する。関節の柔軟モデル部１７は、外力トルク推定部１５からのトルク推定値に基づき関節変位量を求める。加算器２６は、関節の柔軟モデル部１７から出力される関節変位量１８を関節角度指令に加算し関節角度指令を減算器２７へ出力する。減算器２７は、関節角度指令と関節角度を入力し関節角度追従偏差を求める。
次に本発明の動作を説明する。図においてマニピュレータの関節を駆動するモータ１は角度検出器２を備え、検出された関節角度３と関節角度目標４との誤差５に基づき、関節角度追従制御部６によって制御される。また、マニピュレータの先端力の力検出器７によって検出された先端力検出値８から、先端の柔軟モデル部９によって位置変位量１０が演算される。先端位置指令１１にこの位置変位量１０が加算され、逆運動学演算部１２によって関節角度指令１３が演算される。
一方、関節角度追従誤差５をフィルタ演算（１４）し、これを制御モデルに基づく外力トルク推定部１５で演算することによってトルク推定値１６を推定する。推定されたトルク推定値１６から、関節の柔軟モデル部１７によって関節変位量１８が演算され、これを関節角度指令１３に加算し、これが関節角度目標４となる。
【手続補正４】
【補正対象書類名】明細書
【補正対象項目名】０００８
【補正方法】変更
【補正の内容】
【０００８】
外力トルク推定部１５はたとえば、関節角度追従誤差Δθの関数から求められる。最も単純には、
τ ＝ Ｋ・Δθ ・・・(１)
という関節角度追従制御部６でモータ１を制御するサーボ特性をモデル化した式を立て、これを演算する。τは外力トルク、Ｋはサーボ剛性を表す定数、Δθは関節のサーボ追従誤差で、関節角度追従誤差５をフィルタ演算した５’をΔθに当てはめることで、外力トルクτが求められる。なお、ここでフィルタ１４を挿入しているのは、サーボによる変動成分を除去するためである。（１）の代わりに、より高次の特性式を立ててもよいし、非線形要素を含んだモデルを使ってもよい。その式を解くことで、同様に外力トルクτを推定することができる。