JP5409974B1 - マニピュレータ装置 - Google Patents

Abstract

マニピュレータ装置は、先端腕部の振動状態及び前記先端腕部の負荷状態の少なくとも一方を経時的に検出し、前記先端腕部の前記振動状態及び前記負荷状態の少なくとも一方を示す検出信号を生成する状態検出部を備える。前記マニピュレータ装置は、前記状態検出部で生成された前記検出信号に基づいてサーボ制御部での駆動指令に対する駆動電流のサーボゲインを変更し、前記サーボゲインを変更することにより、前記先端腕部の前記振動状態又は前記負荷状態に対応させて、前記先端腕部の振動に対する周波数特性をリアルタイムで変化させるサーボゲイン変更部を備える。

Description

本発明は、駆動部材を駆動させることにより動作を行う先端腕部がマニピュレータの先端部に設けられたマニピュレータ装置に関する。

特許文献１には、原子炉内の検査等に用いられるマニピュレータ装置が開示されている。このマニピュレータ装置は、原子炉内に挿入されるマニピュレータを備える。マニピュレータは、腕部（先端腕部）を備え、腕部は、３つの関節部と、それぞれが対応する関節部の先端方向側に延設される３つの棒状部（リンク部）と、を備える。それぞれの関節部の内部には、駆動部材である球面超音波モータが設けられている。原子炉外の操作指令入力部での操作指令に基づいてそれぞれの球面超音波モータの駆動指令が生成され、それぞれの駆動指令に基づいて球面超音波モータが駆動される。それぞれの球面超音波球面モータが駆動されることにより、対応する関節部が作動され、対応する関節部より先端方向側の部位が動作を行う。

また、特許文献１のマニピュレータ装置では、それぞれの球面超音波モータは、ステータと、ロータとを備える。それぞれの球面超音波モータでは、ステータからのロータへの押圧力により、駆動速度等の駆動特性が変化する。それぞれの関節部では、腕部（マニピュレータ）の姿勢の変化等に対応して作用する力が変化する。それぞれの球面超音波モータでのステータからのロータへの押圧力は、対応する関節部に作用する力に依存して変化する。

そこで、特許文献１のマニピュレータ装置では、それぞれの関節部の位置及び姿勢を検出する位置姿勢検出部が設けられている。それぞれの関節部に作用する力は、関節部の位置及び姿勢に基づいて、算出される。また、それぞれの関節部には、電磁石又は形状記憶合ばねが設けられている。それぞれの関節部では、電磁石に印加する電圧を変化させること、又は、形状記憶合金ばねに供給する電流を変化させることにより、ステータからのロータへの押圧力を調整している。電磁石に印加する電圧、及び、形状記憶合金ばねに供給する電流は、それぞれの関節部に作用する力の算出結果に基づいて制御される。以上のように、それぞれの関節部では、作用する力の算出結果に基づいて、ステータからのロータへの押圧力が調整され、剛性が調整される。

特開２０１１−１８２４８５号公報

マニピュレータ装置の使用時において、外力が作用することにより、腕部（マニピュレータ）が振動することがある。特に体内にマニピュレータが挿入される医療用のマニピュレータ装置では、処置性の低下を防止する観点から、先端腕部（マニピュレータ）において振動が発生した場合に、振動を迅速に抑制する必要がある。前記特許文献１のマニピュレータ装置では、それぞれの関節部において、作用する力に対応して剛性を調整している。しかし、それぞれの関節部の剛性を変化させるのみでは、発生した振動が迅速に抑制されるとは限らない。

本発明は前記課題に着目してなされたものであり、その目的とするところは、動作を行うマニピュレータの先端腕部において振動が発生した場合に、振動を迅速に抑制することが可能なマニピュレータ装置を提供することにある。

前記目的を達成するために、本発明のある態様のマニピュレータ装置は、長手軸に沿って延設されるマニピュレータであって、先端部に動作可能に設けられる先端腕部を備え、前記先端腕部は先端部に先端機能部を備えるマニピュレータと、前記先端機能部の目標位置及び目標姿勢を示す操作指令が入力される操作指令入力部と、駆動電流が供給されることにより駆動され、駆動されることにより前記先端腕部を動作させる駆動部材と、前記操作指令入力部での前記操作指令に基づいて前記駆動部材の駆動指令が入力され、前記駆動指令に基づいて前記駆動部材に前記駆動電流を供給するサーボ制御部と、前記先端腕部の振動状態及び前記先端腕部の負荷状態の少なくとも一方を経時的に検出し、前記先端腕部の前記振動状態及び前記負荷状態の少なくとも一方を示す検出信号を生成する状態検出部と、前記状態検出部で生成された前記検出信号に基づいて前記サーボ制御部での前記駆動指令に対する前記駆動電流のサーボゲインを変更するサーボゲイン変更部であって、前記サーボゲインを変更することにより、前記先端腕部の前記振動状態又は前記負荷状態に対応させて、前記先端腕部の振動に対する周波数特性をリアルタイムで変化させるサーボゲイン変更部と、を備える。

本発明によれば、動作を行うマニピュレータの先端腕部において振動が発生した場合に、振動を迅速に抑制することが可能なマニピュレータ装置を提供することができる。

本発明の第１の実施形態に係るマニピュレータ装置を示す概略図。 第１の実施形態に係るマニピュレータの関節部を作動する構成を概略的に示すブロック図。 第１の実施形態に係る駆動指令生成部での処理を説明する概略図。 第１の実施形態に係るマニピュレータ装置のある１つのサーボ制御部の構成を概略的に示すブロック図。 第１の実施形態に係るマニピュレータ装置のある１つのサーボ制御部、及び、そのサーボ制御部に対応するモータ及びエンコーダでの処理を説明するブロック図。 第１の実施形態に係るマニピュレータ装置のある１つのサーボ制御部でのサーボゲインと先端腕部の振動に対する周波数特性との関係を示す概略図。 マニピュレータ及び体壁を１つの振動系とした場合の、第１の実施形態に係るマニピュレータ装置のある１つのサーボ制御部でのサーボゲインと先端腕部の振動に対する周波数特性との関係を示す概略図。 第１の実施形態に係るマニピュレータ装置のある１つのサーボ制御部の振動検出部の構成を概略的に示すブロック図。 第１の実施形態に係るマニピュレータ装置のある１つのサーボ制御部の振動検出部での処理を経時的に示す概略図。 第１の実施形態に係る振動検出部の相関データ算出部での処理を説明する概略図。 第１の実施形態に係る振動検出部の相関データ算出部で算出された相関データとある１つのサーボ制御部でのサーボゲインの関係を示す概略図。 本発明の第２の実施形態に係るマニピュレータ装置の先端腕部を示す概略図。 第２の実施形態に係るマニピュレータの関節部及び把持部を作動する構成を概略的に示すブロック図。 第２の実施形態に係るマニピュレータ装置のある１つのサーボ制御部、そのサーボ制御部に対応するモータ及びエンコーダ、及び、負荷検出部での処理を説明するブロック図。

（第１の実施形態）
本発明の第１の実施形態について、図１乃至図１０を参照して説明する。図１は、マニピュレータ装置１を示す図である。マニピュレータ装置１は、医療処置等に用いられる医療用のマニピュレータ装置である。図１に示すように、マニピュレータ装置１は、マニピュレータ２と、制御ユニット３と、３Ｄデジタライザー等の操作指令入力部５と、を備える。

マニピュレータ２は、長手軸Ｃを有し、長手軸Ｃに沿って延設されている。ここで、長手軸Ｃに平行な方向の一方を先端方向（図１の矢印Ｃ１の方向）とし、長手軸Ｃに平行な方向の他方を基端方向（図１の矢印Ｃ２の方向）とする。マニピュレータ２は、長手軸Ｃに沿って延設される細長い管状部１１と、管状部１１の先端方向側に設けられる先端腕部１２と、を備える。先端腕部１２は、マニピュレータ２の先端部に位置し、動作可能に設けられている。管状部１１の基端方向側には、保持部１３が設けられている。保持部１３には、ユニバーサルコード７の一端が接続されている。ユニバーサルコード７の他端は、制御ユニット３に接続されている。また、制御ユニット３は、操作指令入力部５からの操作指令を無線通信により受信可能である。

先端腕部１２は、複数（本実施形態では３つ）の関節部１５Ａ〜１５Ｃと、複数（本実施形態では３つ）のリンク部１７Ａ〜１７Ｃと、を備える。それぞれのリンク部１７Ａ〜１７Ｃは、対応する関節部１５Ａ〜１５Ｃの先端方向側に延設されている。リンク部１７Ｃの先端方向側には、先端処置部（先端機能部）であるメス部１９が設けられている。すなわち、メス部１９は、先端腕部１２の先端部に位置し、最も先端方向側の関節部１５Ｃより先端方向側に位置している。それぞれの関節部１５Ａ〜１５Ｃが作動される（actuated）ことにより、先端腕部１２において作動された関節部（１５Ａ〜１５Ｃ）を中心として作動された関節部（１５Ａ〜１５Ｃ）より先端方向側の部位が回動する。また、メス部１９によって、生体組織等の処置対象の切開が行われ、処置対象が処置される。

図２は、関節部１５Ａ〜１５Ｃを作動する構成を示す図である。図２に示すように、マニピュレータ２の保持部１３には、駆動部材であるモータ２１Ａ〜２１Ｃと、エンコーダ２２Ａ〜２２Ｃとが設けられている。それぞれのエンコーダ２２Ａ〜２２Ｃは、対応するモータ２１Ａ〜２１Ｃの駆動状態（駆動位置）を検出する。モータ２１Ａ〜２１Ｃ及びエンコーダ２２Ａ〜２２Ｃは、先端腕部１２の基端より基端方向側に位置している。

マニピュレータ２の内部では、線状部材であるワイヤ２３Ａ〜２３Ｃが長手軸Ｃに沿って延設されている。それぞれのワイヤ２３Ａ〜２３Ｃは、対応するモータ２１Ａ〜２１Ｃと先端腕部１２との間で、延設されている。それぞれのワイヤ２３Ａ〜２３Ｃの先端は、対応する関節部１５Ａ〜１５Ｃに接続されている。それぞれのワイヤ２３Ａ〜２３Ｃは、対応するモータ２１Ａ〜２１Ｃの駆動状態に対応して、長手軸Ｃに沿って移動する。それぞれのワイヤ２３Ａ〜２３Ｃが長手軸Ｃに沿って移動することにより、対応する関節部１５Ａ〜１５Ｃが作動される。これにより、先端腕部１２が動作を行う。

図２に示すように、制御ユニット３は、操作指令入力部５での操作指令を無線通信により受信する指令受信部２５を備える。指令受信部２５は、制御ユニット３に設けられる駆動指令生成部２６に、電気的に接続されている。駆動指令生成部２６は、操作指令入力部５での操作指令に含まれるメス部１９（先端処置部）の目標位置データ及び目標姿勢データを検出する。すなわち、操作指令入力部１２では、メス部１９の目標位置及び目標姿勢を示す操作指令が入力される。そして、操作指令に含まれるメス部１９の目標位置データ及び目標姿勢データに基づいて、それぞれのモータ２１Ａ〜２１Ｃの駆動指令を生成する。

図３は、駆動指令生成部２６での処理を説明する図である。図３に示すように、駆動指令生成部２６は、メス部１９を目標位置及び目標姿勢にする関節部１５Ａ〜１５Ｃの位置及び姿勢を算出する。そして、Denavit-Hartenberg法に基づく座標変換を用いて、メス部１９を目標位置及び目標姿勢にする関節部１５Ａ〜１５Ｃの位置及び姿勢での、関節部１５Ａ〜１５Ｃの屈曲角度等の作動状態が算出される。ここで、４つの座標系Ｓ０〜Ｓ３を規定する。座標系Ｓ０は、管状部１１に原点を有し、管状部１１において長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ１は、関節部１５Ａを原点とし、リンク部１７Ａにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ２は、関節部１５Ｂを原点とし、リンク部１７Ｂにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ３は、関節部１５Ｃを原点とし、リンク部１７Ｃにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。

座標系Ｓ１から座標系Ｓ０への変換行列をＨ（０←１）とし、座標系Ｓ２から座標系Ｓ１への変換行列をＨ（１←２）とし、座標系Ｓ３から座標系Ｓ２への変換行列をＨ（２←３）とする。この場合、座標系Ｓ０でのメス部１９（マニピュレータ２の先端）の位置姿勢ベクトルＵ０は、座標系Ｓ１でのメス部１９の位置姿勢ベクトルＵ１、座標系Ｓ２でのメス部１９の位置姿勢ベクトルＵ２及び座標系Ｓ３でのメス部１９の位置姿勢ベクトルＵ３を用いて、

となる。変換行列Ｈ（０←１）に基づいて、メス部１９を目標位置及び目標姿勢にする関節部１５Ａ〜１５Ｃの位置及び姿勢での、関節部１５Ａの作動状態が算出される。変換行列Ｈ（１←２）に基づいて、メス部１９を目標位置及び目標姿勢にする関節部１５Ｂの位置及び姿勢での、関節部１５Ｂの作動状態が算出される。そして、変換行列Ｈ（２←３）に基づいて、メス部１９を目標位置及び目標姿勢にする関節部１５Ｃの位置及び姿勢での、関節部１５Ｃの作動状態が算出される。以上のように、メス部１９を目標位置及び目標姿勢にする関節部１５Ａ〜１５Ｃの位置及び姿勢での、関節部１５Ａ〜１５Ｃの作動状態が算出される。そして、算出結果に基づいて、それぞれの関節部１５Ａ〜１５Ｃに対応するモータ２１Ａ〜２１Ｃの駆動指令が生成される。

駆動指令生成部２６は、制御ユニット３に設けられるサーボ制御部２７Ａ〜２７Ｃに電気的に接続されている。それぞれのサーボ制御部２７Ａ〜２７Ｃには、対応するモータ２１Ａ〜２１Ｃの駆動指令が、駆動指令生成部２６から入力される。それぞれのサーボ制御部２７Ａ〜２７Ｃは、対応するモータ２１Ａ〜２１Ｃに電気的に接続され、駆動指令に基づいて対応するモータ２１Ａ〜２１Ｃに駆動電流を供給する。また、それぞれのサーボ制御部２７Ａ〜２７Ｃには、対応するエンコーダ２２Ａ〜２２Ｃが電気的に接続されている。これにより、それぞれのサーボ制御部２７Ａ〜２７Ｃには、対応するモータ２１Ａ〜２１Ｃの駆動状態（駆動位置）がフィードバックされる。

図４は、サーボ制御部２７Ａの構成を示す図である。図５は、サーボ制御部２７Ａ、モータ２１Ａ及びエンコーダ２２Ａでの処理を説明する図である。なお、以下の説明では、ある１つのサーボ制御部２７Ａ、及び、そのサーボ制御部２７Ａに対応するモータ２１Ａ及びエンコーダ２２Ａについてのみ説明するが、サーボ制御部２７Ｂ，２７Ｃ、モータ２１Ｂ，２１Ｃ及びエンコーダ２２Ｂ，２２Ｃについても、サーボ制御部２７Ａ、モータ２１Ａ及びエンコーダ２２Ａと構成及び処理は、同様である。

図４に示すように、サーボ制御部２７Ａは、モータ２１Ａの駆動位置を制御する駆動位置制御部３１と、モータ２１Ａの駆動速度を制御する駆動速度制御部３２と、を備える。また、サーボ制御部２７Ａは、微分実施部３３と、サーボゲイン変更部３５と、振動検出部３７と、を備える。なお、本実施形態では、サーボ制御部２７Ａにサーボゲイン変更部３５及び振動検出部３７が設けられているが、サーボゲイン変更部３５及び振動検出部３７がサーボ制御部２７Ａとは別に設けられていてもよい。

図５に示すように、駆動指令生成部２６からのモータ２１Ａの駆動指令は、駆動位置制御部３１に入力される。駆動位置制御部３１は、駆動指令に含まれるモータ２１Ａの駆動位置データに基づいて、モータ２１Ａの駆動位置の制御を行う（ステップＳ１０１）。そして、モータ２１Ａの駆動指令は、駆動速度制御部３２に入力される。駆動速度制御部３２は、駆動指令に含まれるモータ２１Ａの駆動速度データに基づいて、モータ２１Ａの駆動速度の制御を行う（ステップＳ１０２）。駆動位置制御部３１でのモータ２１Ａの駆動位置の制御、及び、駆動速度制御部３２でのモータ２１Ａの駆動速度の制御に基づいて、駆動電流がモータ２１Ａに供給される。

モータ２１Ａに駆動電流が供給されることにより、モータ２１Ａが駆動される（ステップＳ１０３）。これにより、関節部１５Ａが作動される。この際、先端腕部１２の振動状態が、モータ２１Ａの駆動に影響を及ぼす。マニピュレータ装置１の使用において、外力等によって、先端腕部１２が振動することがある。この場合、先端腕部１２の振動が外乱として、モータ２１Ａに作用する。したがって、先端腕部１２の振動状態によっては、モータ２１Ａの駆動指令と実際のモータ２１Ａの駆動状態とが、互いに対して相関性が低いものとなる。

実際のモータ２１Ａの駆動位置（駆動状態）は、エンコーダ２２Ａにより検出される（ステップＳ１０４）。検出されたモータ２１Ａの駆動位置情報は、駆動位置制御部３１でのモータ２１Ａの駆動位置制御（ステップＳ１０１）において、フィードバックされる。また、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置情報は、微分実施部３３に入力される。そして、微分実施部でモータ２１Ａの駆動位置情報が微分され（ステップＳ１０５）、実際のモータ２１Ａの駆動速度が検出される。検出されたモータ２１Ａの駆動速度情報は、駆動速度制御部３２でのモータ２１Ａの駆動速度制御（ステップＳ１０２）において、フィードバックされる。

振動検出部３７には、モータ２１Ａの駆動指令が入力される。また、振動検出部３７には、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置（駆動状態）情報が入力される。振動検出部３７は、モータ２１Ａの駆動指令とエンコーダ２２Ａで検出したモータ２１Ａの駆動状態（駆動位置）との相関関係に基づいて、先端腕部１２の振動状態を経時的に検出する（ステップＳ１０６）。この際、先端腕部１２が振動している場合は、振動の周波数（ｆ）が検出される。そして、振動検出部３７は、先端腕部１２の振動状態を示す検出信号を生成する。すなわち、振動検出部３７が、先端腕部１２の振動状態を検出する状態検出部となっている。なお、振動検出部３７での処理の詳細については、後述する。

先端腕部１２の振動状態を示す検出信号は、サーボゲイン変更部３５に入力される。サーボゲイン変更部３５は、検出信号に基づいて、サーボ制御部２７Ａでのモータ２１Ａの駆動指令に対する駆動電流のサーボゲインＧａを変更する（ステップＳ１０７）。サーボゲインＧａを変更することにより、駆動指令に対するモータ２１Ａの駆動特性が変化する。モータ２１Ａの駆動特性が変化することにより、関節部１５Ａの作動特性が変化する。関節部１５Ａの作動特性が変化することにより、例えば、同一の駆動指令がサーボ制御部２７Ａに入力された場合でも、変化前と変化後で関節部１５Ａの作動速度が異なる。また、関節部１５Ａの作動特性が変化することにより、例えば、同一の外力が関節部１５Ａに作用した場合でも、変化前と変化後で関節部１５Ａの作動速度が異なる。

関節部１５Ｂ，１５Ｃの作動特性についても、関節部１５Ａと同様である。すなわち、サーボ制御部２７Ｂでのモータ２１Ｂの駆動指令に対する駆動電流のサーボケインＧｂを変更することにより、関節部１５Ｂの作動特性が変化する。また、サーボ制御部２７Ｃでのモータ２１Ｃの駆動指令に対する駆動電流のサーボケインＧｃを変更することにより、関節部１５Ｃの作動特性が変化する。関節部１５Ａ〜１５Ｃの中の少なくとも１つの作動特性が変化することにより、先端腕部１２の振動に対する周波数特性が変化する。

図６Ａは、サーボ制御部２７ＡでのサーボゲインＧａと先端腕部１２の振動に対する周波数特性との関係を示す図である。図６Ｂは、マニピュレータ２及び体壁を１つの振動系とした場合の、サーボ制御部２７ＡでのサーボゲインＧａと先端腕部１２の振動に対する周波数特性との関係を示す図である。なお、以下の説明では、ある１つのサーボ制御部２７ＡのサーボゲインＧａと先端腕部１２の周波数特性について説明するが、サーボ制御部２７Ｂ，２７ＣのサーボゲインＧｂ，Ｇｃについても、サーボ制御部２７ＡのサーボゲインＧａと同様である。図６Ａ及び図６Ｂにおいて、縦軸が振幅（Ｖ）を示し、横軸が周波数（ｆ）を示している。また、サーボゲインＧａがＧａ１の状態での先端腕部１２の周波数特性と、サーボゲインＧａがＧａ１より小さいＧａ２の状態での先端腕部１２の周波数特性と、が示されている。

ここで、マニピュレータ２の先端腕部１２が体壁等に接触することなく空間で動作を行う場合は、先端腕部１２は図６Ａに示す振動に対する周波数特性を有する。このため、サーボゲインＧａがいずれの大きさであっても、振動を発生させることなく先端腕部１２は動作可能である。しかし、マニピュレータ２の先端腕部１２が体壁に接触したり、先端腕部１２が体壁から離れたりした場合、マニピュレータ２及び体壁を１つの振動系として捉えられる。このため、先端腕部１２は、図６Ｂに示す振動に対する周波数特性を有する。このため、サーボ制御部２７ＡでのサーボゲインＧａがＧａ１の状態では、先端腕部１２は、周波数ｆ１において大きい振幅Ｖ１で振動する。この際、振動検出部３７は、先端腕部１２の振動の周波数ｆ１を検出し、振動の周波数データを含む検出信号がサーボゲイン変更部３５に入力される。

サーボゲイン変更部３５は、検出信号に基づいて、サーボ制御部２７ＡでのサーボゲインＧａをＧａ１からＧａ２に小さくする。サーボゲインＧａが小さくなることにより、関節部１５Ａの作動特性が変化し、関節部１５Ａが柔軟になる。関節部１５Ａが柔軟になることにより、関節部１５Ａで振動が吸収される状態となる。したがって、サーボ制御部２７ＡでのサーボゲインＧａをＧａ１からＧａ２に小さくすることにより、先端腕部１２の周波数特性が変化し、先端腕部１２の振動が抑制される（図６Ｂ参照）。すなわち、サーボゲイン変更部３５は、サーボゲインＧａをＧａ２に変更することにより、振動検出部３７で検出された周波数ｆ１において振動が抑制される状態に、先端腕部１２の振動に対する周波数特性を変化させる。

以上のように、先端腕部１２の振動状態に基づいてサーボ制御部２７ＡでのサーボゲインＧａを変更することにより、先端腕部１２の周波数特性を変化させ、発生した振動を抑制している。このため、先端腕部１２の振動状態に対応して、リアルタイムで先端腕部１２の周波数特性が変化し、先端腕部１２で発生した振動が迅速に抑制される。

図７はサーボ制御部２７Ａの振動検出部３７の構成を示す図であり、図８はサーボ制御部２７Ａの振動検出部３７での処理を経時的に示す図である。なお、以下の説明では、ある１つのサーボ制御部２７Ａの振動検出部３７を例に説明するが、サーボ制御部２７Ｂ，２７Ｃの振動検出部３７についても、サーボ制御部２７Ａの振動検出部３７と同様である。

図７及び図８に示すように、振動検出部３７は、駆動指令生成部２６からモータ２１Ａの駆動指令が入力される窓関数フィルタ４１Ａを備える。窓関数フィルタ４１Ａにより、モータ２１Ａの駆動指令データが、所定の時間範囲Ｔ０ごとに区切られる。窓関数フィルタ４１Ａは、データバッファ４２Ａに電気的に接続されている。所定の時間範囲Ｔ０ごとに区切られた駆動指令データは、データバッファ４２Ａに一時的に記憶される。データバッファ４２Ａでは、駆動指令生成部２６での駆動指令の生成に対して微小時間だけ遅れて、駆動指令データが記憶される。

データバッファ４２Ａは、フーリエ変換部４３Ａに電気的に接続されている。フーリエ変換部４３Ａでは、所定の時間範囲Ｔ０ごとにモータ２１Ａの駆動指令データの高速フーリエ変換（ＦＦＴ：fast Fourier transformation）が行われ、モータ２１Ａの駆動指令のＦＦＴデータが所定の時間範囲Ｔ０ごとに区切られて生成される。フーリエ変換部４３Ａは、データバッファ４５Ａに電気的に接続されている。所定の時間範囲Ｔ０ごとに区切られたＦＦＴデータは、データバッファ４５Ａに一時的に記憶される。データバッファ４５Ａでは、データバッファ４２Ａでの駆動指令データの記憶に対して、所定の時間範囲Ｔ０に等しい時間だけ遅れて、ＦＦＴデータが記憶される。

振動検出部３７は、エンコーダ２２Ａからモータ２１Ａの駆動状態（駆動位置）が入力される窓関数フィルタ４１Ｂを備える。窓関数フィルタ４１Ｂにより、モータ２１Ａの駆動状態データが、所定の時間範囲Ｔ０ごとに区切られる。窓関数フィルタ４１Ｂは、データバッファ４２Ｂに電気的に接続されている。所定の時間範囲Ｔ０ごとに区切られた駆動状態データは、データバッファ４２Ｂに一時的に記憶される。データバッファ４２Ｂでは、エンコーダ２２Ａでの駆動状態の検出に対して微小時間だけ遅れて、駆動状態データが記憶される。

データバッファ４２Ｂは、フーリエ変換部４３Ｂに電気的に接続されている。フーリエ変換部４３Ｂでは、所定の時間範囲Ｔ０ごとにモータ２１Ａの駆動状態データの高速フーリエ変換が行われ、モータ２１Ａの駆動状態のＦＦＴデータが所定の時間範囲Ｔ０ごとに区切られて生成される。フーリエ変換部４３Ｂは、データバッファ４５Ｂに電気的に接続されている。所定の時間範囲Ｔ０ごとに区切られたＦＦＴデータは、データバッファ４５Ｂに一時的に記憶される。データバッファ４５Ｂでは、データバッファ４２Ｂでの駆動状態データの記憶に対して、所定の時間範囲Ｔ０に等しい時間だけ遅れて、ＦＦＴデータが記憶される。

データバッファ４５Ａ及びデータバッファ４５Ｂは、相関データ算出部４７に電気的に接続されている。相関データ算出部４７は、データバッファ４５ＡのＦＦＴデータ及びデータバッファ４５ＢのＦＦＴデータに基づいて、モータ２１Ａの駆動指令とエンコーダ２２Ａで検出したモータ２１Ａの駆動状態との相関関係を示す相関データを算出する。相関データは、所定の時間範囲Ｔ０ごとに、経時的に算出される。相関データ算出部４７は、データバッファ４８に電気的に接続されている。所定の時間範囲Ｔ０ごとに区切られた相関データは、データバッファ４８に一時的に記憶される。データバッファ４８では、データバッファ４５ＡでのＦＦＴデータの記憶及びデータバッファ４５ＢでのＦＦＴデータの記憶に対して微小時間だけ遅れて、相関データが記憶される。

以上のように、相関データ算出部４７では、エンコーダ２２Ａでの駆動状態の検出に対して所定の時間範囲Ｔ０と略同一の時間だけ遅れて、相関データが算出される。したがって、所定の時間範囲Ｔ０を小さくすることにより、先端腕部１２の振動状態に対応させて、モータ２１Ａの駆動指令とエンコーダ２２Ａで検出したモータ２１Ａの駆動状態との相関関係を示す相関データが、リアルタイムで算出される。

図９は、相関データ算出部４７での処理を説明する図である。ここで、相関データは、周知の交互相関関数の処理を用いて、算出される。すなわち、データバッファ４５Ａに記憶された駆動指令のＦＦＴデータのデータ列、及び、データバッファ４５Ｂに記憶された駆動状態のＦＦＴデータのデータ列に対して、畳込み積分が行われる。図９に示すように、先端腕部１２に振動が発生した状態では、モータ２１Ａの駆動指令を示す信号と比較して、モータ２１Ａの駆動状態を示す信号にノイズＮが発生する。駆動状態を示す信号にノイズＮが発生する状態では、モータ２１Ａの駆動指令とエンコーダ２２Ａで検出したモータ２１Ａの駆動状態との相関性は低くなる。一方、先端腕部１２に振動しない状態では、モータ２１Ａの駆動状態を示す信号にノイズＮが発生しない。駆動状態を示す信号にノイズＮが発生しない状態では、モータ２１Ａの駆動指令とエンコーダ２２Ａで検出したモータ２１Ａの駆動状態との相関性は高くなる。なお、先端腕部１２の振動の周波数（ｆ）は、モータ２１Ａの駆動状態を示す信号でのノイズＮの発生状態に基づいて、検出される。

モータ２１Ａの駆動指令とエンコーダ２２Ａで検出したモータ２１Ａの駆動状態との相関性が高くなるにつれて、相関データ算出部４７での相関データの相関値Ｐは１に近づく。一方、モータ２１Ａの駆動指令とエンコーダ２２Ａで検出したモータ２１Ａの駆動状態との相関性が低くなるにつれて、相関データ算出部４７での相関データの相関値Ｐは０に近づく。すなわち、先端腕部１２での振動が発生しない状態では相関データの相関値Ｐは１に近づき、先端腕部１２が大きく振動している状態では相関データの相関値Ｐは０に近づく。以上のように、相関データに基づいて、先端腕部１２の振動状態が検出される。

データバッファ４８に記憶された相関データは、サーボゲイン変更部３５に出力される。サーボゲイン変更部３５は、相関データに基づいて、サーボ制御部２７ＡでのサーボゲインＧａを変更する。図１０は、相関データ算出部４７で算出された相関データとサーボ制御部２７ＡでのサーボゲインＧａの関係を示す図である。図１０に示すように、変更前のサーボ制御部２７ＡのサーボゲインＧａをＧａ１とすると、相関データの相関値Ｐが１の場合、変更後のサーボゲインＧａはＧａ１以上となる。一方、相関データの相関値Ｐが１より小さい場合、変更後のサーボゲインＧａはＧａ１より小さくなる。そして、相関データの相関値Ｐが０に近づくにつれて、変更後のサーボゲインＧａは小さくなる。

以上のように、相関データに基づいて、サーボ制御部２７ＡでのサーボゲインＧａが変更される。ここで、所定の時間範囲Ｔ０を小さくすることにより、先端腕部１２の振動状態に対応させて、モータ２１Ａの駆動指令とエンコーダ２２Ａで検出したモータ２１Ａの駆動状態との相関関係を示す相関データが、リアルタイムで算出される。したがって、所定の時間範囲Ｔ０を小さくすることにより、先端腕部１２の振動状態に対応させて、リアルタイムでサーボ制御部２７ＡのサーボゲインＧａが変更され、先端腕部１２の振動に対する周波数特性がリアルタイムで変化する。

また、モータ２１Ａ〜２１Ｃ及びエンコーダ２２Ａ〜２２Ｃは先端腕部１２より基端方向側の保持部１３に設けられている。このため、医療処置において体内に挿入される管状部１１及び先端腕部１２が大型化することはない。

そこで、前記構成のマニピュレータ装置１では、以下の効果を奏する。すなわち、マニピュレータ装置１では、それぞれのサーボ制御部２７Ａ〜２７Ｃの振動検出部３７において、対応するモータ２１Ａ〜２１Ｃの駆動指令と対応するエンコーダ２２Ａ〜２２Ｃで検出した対応するモータ２１Ａ〜２１Ｃの駆動状態との相関関係に基づいて、先端腕部１２の振動状態を検出する。そして、それぞれのサーボ制御部２７Ａ〜２７Ｃのサーボゲイン変更部３５において、先端腕部１２の振動状態に基づいて、それぞれのサーボ制御部２７Ａ〜２７ＣでのサーボゲインＧａ〜Ｇｃを変更している。それぞれのサーボ制御部２７Ａ〜２７ＣのサーボゲインＧａ〜Ｇｃを変更することにより、対応する関節部１５Ａ〜１５Ｃの作動特性が変化する。関節部１５Ａ〜１５Ｃの中の少なくとも１つの作動特性が変化することにより、先端腕部１２の周波数特性を変化し、発生した振動が抑制される。したがって、先端腕部１２の振動状態に対応して、リアルタイムで先端腕部１２の周波数特性が変化し、先端腕部１２で発生した振動を迅速に抑制することができる。

（第２の実施形態）
次に、本発明の第２の実施形態について、図１１乃至図１３を参照して説明する。なお、第１の実施形態と同一の部分及び同一の機能を有する部分については同一の符号を付して、その説明は省略する。

図１１は、本実施形態の先端腕部１２の構成を示す図である。図１１に示すように、先端腕部１２は、第１の実施形態と同様に、３つの関節部１５Ａ〜１５Ｃと、３つのリンク部１７Ａ〜１７Ｃと、を備える。リンク部１７Ｃの先端方向側には、先端処置部である把持部５１がメス部１９の代わりに設けられている。把持部５１は、作動されることにより、生体組織等の把持対象（処置対象）を把持可能である。

図１２は、関節部１５Ａ〜１５Ｃ及び把持部５１を作動する構成を示す図である。図１２に示すように、駆動指令生成部２６は、第１の実施形態と同様に、操作指令に含まれる把持部５１（先端処置部）の目標位置データ及び目標姿勢データに基づいて、それぞれのモータ２１Ａ〜２１Ｃの駆動指令を生成する。それぞれのモータ２１Ａ〜２１Ｃの駆動指令を生成する際には、Denavit-Hartenberg法に基づく座標変換が用いられる。そして、それぞれのモータ２１Ａ〜２１Ｃの駆動状態に対応して、対応する関節部１５Ａ〜１５Ｃが作動される。これにより、先端腕部１２が動作を行う。

また、マニピュレータ２の保持部１３には、モータ２１Ａ〜２１Ｃとは別に、駆動部材であるモータ５２が設けられている。マニピュレータ２の内部では、線状部材であるワイヤ５３が長手軸Ｃに沿って延設されている。ワイヤ５３は、モータ５２と先端腕部１２との間で、延設されている。ワイヤ５３の先端は、把持部５１に接続されている。ワイヤ５３は、モータ５２の駆動状態に対応して、長手軸Ｃに沿って移動する。ワイヤ５３が長手軸Ｃに沿って移動することにより、把持部５１が作動される。

制御ユニット３には、駆動指令生成部２６とは別に、駆動指令生成部５５が設けられている。駆動指令生成部５５は、制御ユニット３の指令受信部２５に電気的に接続されている。駆動指令生成部５５は、操作指令入力部５での操作指令に含まれる把持部５１（先端処置部）の目標位置データ及び目標姿勢データを検出する。そして、操作指令に含まれる把持部５１の目標位置データ及び目標姿勢データに基づいて、モータ５２の駆動指令を生成する。駆動指令生成部５５は、制御ユニット３に設けられるサーボ制御部５７に電気的に接続されている。サーボ制御部５７には、モータ５２の駆動指令が、駆動指令生成部５５から入力される。サーボ制御部５７は、モータ５２に電気的に接続され、駆動指令に基づいてモータ５２に駆動電流を供給する。これにより、モータ５２の駆動状態が制御される。

また、本実施形態では、先端腕部１２に、負荷センサ６１が設けられている。負荷センサ６１は、制御ユニット３に設けられる負荷検出部６３に電気的に接続されている。負荷検出部６３は、把持有無検出部６５を備える。負荷検出部６３は、サーボ制御部２７Ａ〜２７Ｃ及びサーボ制御部５７に電気的に接続されている。負荷検出部６３での処理については、後述する。なお、本実施形態では、それぞれのサーボ制御部２７Ａ〜２７Ｃには、振動検出部３７が設けられていない。ただし、それぞれのサーボ制御部２７Ａ〜２７Ｃは、第１の実施形態と同様に、駆動位置制御部３１と、駆動速度制御部３２と、微分実施部３３と、サーボゲイン変更部３５と、を備える。

図１３は、サーボ制御部２７Ａ、モータ２１Ａ、エンコーダ２２Ａ及び負荷検出部６３での処理を説明する図である。なお、以下の説明では、ある１つのサーボ制御部２７Ａ、及び、そのサーボ制御部２７Ａに対応するモータ２１Ａ及びエンコーダ２２Ａについてのみ説明するが、サーボ制御部２７Ｂ，２７Ｃ、モータ２１Ｂ，２１Ｃ及びエンコーダ２２Ｂ，２２Ｃについても、サーボ制御部２７Ａ、モータ２１Ａ及びエンコーダ２２Ａと構成及び処理は、同様である。

図１３に示すように、本実施形態では第１の実施形態と同様に、駆動指令に含まれるモータ２１Ａの駆動位置データに基づいて、駆動位置制御部３１がモータ２１Ａの駆動位置の制御を行う（ステップＳ１０１）。そして、駆動指令に含まれるモータ２１Ａの駆動速度データに基づいて、駆動速度制御部３２がモータ２１Ａの駆動速度の制御を行う（ステップＳ１０２）。駆動位置制御部３１でのモータ２１Ａの駆動位置の制御、及び、駆動速度制御部３２でのモータ２１Ａの駆動速度の制御に基づいて、駆動電流がモータ２１Ａに供給される。

モータ２１Ａに駆動電流が供給されることにより、モータ２１Ａが駆動される（ステップＳ１０３）。これにより、関節部１５Ａが作動される。この際、先端腕部１２の振動状態が、モータ２１Ａの駆動に外乱として影響を及ぼす。実際のモータ２１Ａの駆動位置（駆動状態）は、エンコーダ２２Ａにより検出される（ステップＳ１０４）。検出されたモータ２１Ａの駆動位置情報は、駆動位置制御部３１でのモータ２１Ａの駆動位置制御（ステップＳ１０１）において、フィードバックされる。また、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置情報は、微分実施部３３によって微分され（ステップＳ１０５）、実際のモータ２１Ａの駆動速度が検出される。検出されたモータ２１Ａの駆動速度情報は、駆動速度制御部３２でのモータ２１Ａの駆動速度制御（ステップＳ１０２）において、フィードバックされる。

ただし、本実施形態では第１の実施形態とは異なり、振動検出部３７が設けられていない。代わりに、本実施形態では、先端腕部１２に負荷センサ６１が設けられ、負荷センサ６１からのセンサ信号が負荷検出部６３に入力される。負荷検出部６３は、センサ信号に基づいて先端腕部１２の負荷状態を検出する（ステップＳ１１１）。すなわち、負荷検出部６３が、先端腕部１２の負荷状態を検出する状態検出部となっている。そして、先端腕部１２の負荷状態に基づいて、把持部５１での把持対象の把持の有無が把持有無検出部６５により検出される（ステップＳ１１２）。例えば、把持部５１で把持対象を把持した状態では、先端腕部１２に作用する負荷は大きくなる。一方、把持部５１で把持対象が把持されない状態では、先端腕部１２に作用する負荷は小さくなる。そして、先端腕部１２の負荷状態及び把持部５１での把持対象の把持の有無を示す検出信号が生成される。

先端腕部１２の負荷状態を示す検出信号は、サーボゲイン変更部３５に入力される。サーボゲイン変更部３５は、検出信号に基づいて、サーボ制御部２７Ａでのモータ２１Ａの駆動指令に対する駆動電流のサーボゲインＧａを変更する（ステップＳ１１３）。サーボゲインＧａを変更することにより、駆動指令に対するモータ２１Ａの駆動特性が変化する。モータ２１Ａの駆動特性が変化することにより、関節部１５Ａの作動特性が変化する。 関節部１５Ｂ，１５Ｃの作動特性についても、関節部１５Ａと同様である。すなわち、サーボ制御部２７Ｂでのモータ２１Ｂの駆動指令に対する駆動電流のサンボケインＧｂを変更することにより、関節部１５Ｂの作動特性が変化する。また、サーボ制御部２７Ｃでのモータ２１Ｃの駆動指令に対する駆動電流のサーボケインＧｃを変更することにより、関節部１５Ｃの作動特性が変化する。関節部１５Ａ〜１５Ｃの中の少なくとも１つの作動特性が変化することにより、先端腕部１２の振動に対する周波数特性が変化する。それぞれのサーボ制御部２７Ａ〜２７ＣのサーボゲインＧａ〜Ｇｃと先端腕部１２の周波数特性との関係は、第１の実施形態で前述した通りである（図６Ａ、図６Ｂ参照）。

先端処置部として把持部５１を備えるマニピュレータ２では、把持対象を把持部５１に把持し、把持対象を切除する（resect）処置が行われる。このような処置では、把持対象を切除する際に、先端腕部１２で振動が発生し易い。そこで、本実施形態では、把持有無検出部６５が、先端腕部１２の負荷状態に基づいて、把持部５１での把持対象の把持の有無を検出している。把持部５１で把持対象が把持している状態では、少なくとも１つのサーボ制御部２７Ａ〜２７ＣでのサーボゲインＧａ〜Ｇｃを小さくする。これにより、先端腕部１２の振動に対する周波数特性が変化する。したがって、把持対象を把持部５１で把持した状態で把持対象を切除した際に、先端腕部１２での振動の発生が防止される。

以上のように本実施形態では、先端腕部１２の負荷状態から、把持部５１での把持対象の把持の有無を検出している。そして、把持対象が把持部５１で把持されている場合は、先端腕部１２の周波数特性を変化させている。これにより、把持対象を把持部５１で把持した状態で把持対象を切除した際に、先端腕部１２での振動の発生が防止される。すなわち、先端腕部１２の負荷状態に対応して、リアルタイムで先端腕部１２の振動に対する周波数特性が変化し、先端腕部１２での振動の発生を前もって防止している。

（変形例）
なお、前述の実施形態では、３つの関節部１５Ａ〜１５Ｃが設けられているが、これに限るものではない。例えば、関節部（１５Ａ〜１５Ｃ）の数は２つでもよく、４つ以上でもよい。また、関節部（１５Ａ〜１５Ｃ）の数が１つであってもよい。そして、それぞれの関節部（１５Ａ〜１５Ｃ）に対応して、駆動部材であるモータ（２１Ａ〜２１Ｃ）及びサーボ制御部（２７Ａ〜２７Ｃ）が設けられていればよい。

また、第１の実施形態では先端処置部としてメス部１９が設けられ、第２の実施形態では先端処置部として把持部５１が設けられているが、これに限るものではない。例えば、先端処置部（先端機能部）として処置対象を引掛けて処置を行うフック状部が設けられてもよい。また、マニピュレータ装置１は、医療用のマニピュレータ装置であるが、管路等に挿入される工業用のマニピュレータ装置であってもよい。この場合、先端腕部１２の先端部に先端機能部として撮像素子が設けられる。

また、第１の実施形態では先端腕部１２の振動状態に基づいてそれぞれのサーボ制御部（２７Ａ〜２７Ｃ）のサーボゲイン（Ｇａ〜Ｇｃ）を変更し、第２の実施形態では先端腕部１２の負荷状態に基づいてそれぞれのサーボ制御部（２７Ａ〜２７Ｃ）のサーボゲイン（Ｇａ〜Ｇｃ）を変更しているが、これに限るものではない。例えば、第１の実施形態の構成及び第２の実施形態の構成を組み合わせて、先端腕部１２の振動状態及び先端腕部１２の負荷状態に基づいて、それぞれのサーボ制御部（２７Ａ〜２７Ｃ）のサーボゲイン（Ｇａ〜Ｇｃ）を変更してもよい。すなわち、先端腕部１２の振動状態及び負荷状態の少なくとも一方を示す検出信号を生成する状態検出部（３７，６３）が設けられていればよい。また、状態検出部（３７，６３）での検出信号に基づいて、サーボゲイン変更部３５がサーボ制御部（２７Ａ〜２７Ｃ）での駆動指令に対する駆動電流のサーボゲイン（Ｇａ〜Ｇｃ）を変更すればよい。そして、サーボゲイン（Ｇａ〜Ｇｃ）を変更することにより、先端腕部１２の振動状態又は負荷状態に対応させて、先端腕部１２の振動に対する周波数特性がリアルタイムで変化すればよい。

以上、本発明の実施形態について説明したが、本発明は前記の実施形態に限定されるものではなく、本発明の要旨を逸脱しない範囲で種々の変形ができることは勿論である。

Claims (7)

1. 長手軸に沿って延設されるマニピュレータであって、先端部に動作可能に設けられる先端腕部を備え、前記先端腕部は先端部に先端機能部を備えるマニピュレータと、
   前記先端機能部の目標位置及び目標姿勢を示す操作指令が入力される操作指令入力部と、
   駆動電流が供給されることにより駆動され、駆動されることにより前記先端腕部を動作させる駆動部材と、
   前記操作指令入力部での前記操作指令に基づいて前記駆動部材の駆動指令が入力され、前記駆動指令に基づいて前記駆動部材に前記駆動電流を供給するサーボ制御部と、
   前記先端腕部の振動状態及び前記先端腕部の負荷状態の少なくとも一方を経時的に検出し、前記先端腕部の前記振動状態及び前記負荷状態の少なくとも一方を示す検出信号を生成する状態検出部と、
   前記状態検出部で生成された前記検出信号に基づいて前記サーボ制御部での前記駆動指令に対する前記駆動電流のサーボゲインを変更するサーボゲイン変更部であって、前記サーボゲインを変更することにより、前記先端腕部の前記振動状態又は前記負荷状態に対応させて、前記先端腕部の振動に対する周波数特性をリアルタイムで変化させるサーボゲイン変更部と、
   を具備するマニピュレータ装置。
2. 前記状態検出部は、前記先端腕部が振動している状態において振動の周波数を検出する振動検出部であり、
   前記サーボゲイン変更部は、前記サーボゲインを変更することにより、検出された前記周波数において前記振動が抑制される状態に、前記先端腕部の前記振動に対する前記周波数特性を変化させる、
   請求項１のマニピュレータ装置。
3. 前記先端腕部は、関節部と、前記関節部の先端方向側に延設されるリンク部と、を備え、
   前記先端機能部は、前記関節部より先端方向側に位置し、
   前記駆動部材は、駆動されることにより前記関節部を作動し、前記関節部を作動することにより前記先端腕部において前記関節部を中心として前記関節部より前記先端方向側の部位を回動させ、
   前記サーボゲイン変更部は、前記サーボゲインを変更することにより前記関節部の作動特性を変化させ、前記先端腕部の前記振動に対する前記周波数特性を変化させる、
   請求項１のマニピュレータ装置。
4. 前記関節部は、複数の前記関節部であり、
   前記リンク部は、それぞれが対応する前記関節部の前記先端方向側に延設される複数の前記リンク部であり、
   前記先端機能部は、最も前記先端方向側の前記関節部より前記先端方向側に位置し、
   前記駆動部材は、駆動されることによりそれぞれが対応する前記関節部を作動する複数の前記駆動部材であり、
   前記サーボ制御部は、それぞれに対応する前記駆動部材の前記駆動指令が入力され、それぞれが前記駆動指令に基づいて対応する前記駆動部材に前記駆動電流を供給する複数の前記サーボ制御部であり、
   前記サーボゲイン変更部は、それぞれが対応する前記サーボ制御部での前記駆動指令に対する前記駆動電流の前記サーボゲインを変更し、それぞれが対応する前記関節部の前記作動特性を変化させる複数の前記サーボゲイン変更部である、
   請求項３のマニピュレータ装置。
5. 前記駆動部材の駆動状態を検出するエンコーダをさらに具備し、
   前記状態検出部は、前記駆動部材の前記駆動指令と前記エンコーダで検出した前記駆動部材の前記駆動状態との相関関係を示す相関データを経時的に算出する相関データ算出部を備え、前記相関データに基づいて前記先端腕部の前記振動状態を検出する振動検出部であり、
   前記サーボゲイン変更部は、前記相関データに基づいて前記サーボゲインを変更する、
   請求項１のマニピュレータ装置。
6. 前記駆動部材及び前記エンコーダは、前記先端腕部の基端より基端方向側に位置し、
   前記マニピュレータは、前記駆動部材と前記先端腕部との間で前記長手軸に沿って延設され、前記駆動部材の前記駆動状態に対応して前記長手軸に沿って移動することにより、前記先端腕部を動作させる線状部材を備える、
   請求項５のマニピュレータ装置。
7. 前記状態検出部は、前記先端腕部の前記負荷状態を検出する負荷検出部であり、
   前記先端腕部は、把持対象を把持可能な把持部を備え、
   前記負荷検出部は、前記先端腕部の前記負荷状態に基づいて、前記把持部での前記把持対象の把持の有無を検出する把持有無検出部を備え、
   前記サーボゲイン変更部は、前記把持対象の前記把持の前記有無に基づいて、前記サーボゲインを変更する、
   請求項１のマニピュレータ装置。

Images