JP2017104914A - 外力追従制御システム - Google Patents

Abstract

【課題】無負荷時は指令通りに正確に動作し、かつ外力により移動している状態から除荷されても、アームの位置を保持することが出来る外力追従制御システムを提供する。【解決手段】位置指令値生成部７２において、外力により生じた伸縮アーム部の差分、あるいは外力信号が取得される。制御対象に事前に設定した値を超える力が負荷として加えられた場合、コントローラ８０による位置目標値を力が逃げる（除荷される）方向に修正する。そして、外力を受け流して目標位置を上書き更新することにより、外力がなくなったときに実測値を目標値に収束させることができる。【選択図】図３

Description

本発明は、外力追従制御システムに関する。

従来、ロボットのアームに対して接触時に回避行動を行えるようにした制御装置が知られている（例えば、特許文献１等参照）。

このようなものでは、ロボットアームに複数装着されている外力計測用センサの情報に基づいて各関節が制御される。
制御部では、外力計測用センサの情報から各関節に加わるトルクを分離し、設定された関節部仮想インピーダンスからアーム関節部の逃げ量を算出する。
また、設定された手先部仮想インピーダンスからアーム手先の逃げ量を算出する。
そして、制御部では、手先部の逃げ量に、アーム関節部の逃げ量を加算して駆動指令をアームに与える。これにより、アームのどの部位に外力が加わっても適切な回避行動が行われる。

特開２００１−３８６６４号公報

このようなアーム制御を行なうロボットとして、手術用のロボット、たとえば、内視鏡を操作するスコピストによる補助に代えて、所望の位置に光学視管を保持する内視鏡ホルダロボットなどが知られている。

内視鏡ホルダロボットは、手術中にカメラケーブルが引っ張られたり、ホルダに術者が接触したり等、術者の気がつかないうちに、アームに対して外力が加えられていることがある。
このような内視鏡ホルダロボットでは、アームが外力による負荷から開放（除荷）されると、外力が加わる前の姿勢にアームを復帰させようとする。
このため、アームに装着されている光学視管の位置が急激に移動して、内視鏡によって見えていた体内の被観察部位が観察できなくなってしまう。

そこで、本発明は、無負荷時は指令通りに正確に動作し、かつ外力により移動している状態から除荷されても、アームの位置を保持することが出来る外力追従制御システムを提供することを課題としている。

本発明に係る外力追従制御システムは、ロボットアームと、ロボットアームの位置を位置目標値を用いて指示する入力部と、入力部から入力された位置目標値に応じて、ロボットアームを移動させるアクチュエータと、ロボットアームの位置を検出する位置検出部と、位置検出手段で検出されたロボットアームの位置を示す位置応答値と、位置目標値との差分に応じて、アクチュエータを駆動する制御部とを備える。そして、制御部では、ロボットアームに加えられている外力に応じて位置目標値を修正することを特徴としている。

このような構成によれば、アクチュエータに出力された位置目標値に応じて、アクチュエータは、ロボットアームを移動させる。
外力が加わると、位置目標値が外力により移動したロボットアームの位置に合わせて修正される。
このため、位置目標値に応じて、移動されたロボットアームは、ロボットアームに加わった外力がなくなっても、外力が加わる以前のもとの位置まで戻ることはない。

本発明によれば、無負荷時は指令通りに正確に動作し、かつ外力により移動している状態から除荷されても、アームの位置を保持することが出来る外力追従制御システムが提供される。

実施形態の外力追従制御システムで、内視鏡ホルダロボットの構成を説明する斜視図である。 実施形態の外力追従制御システムで、各部の構成を説明する構成図である。 実施形態の外力追従制御システムで、機能ブロック図である。 実施形態の外力追従制御システムの動作を説明するフローチャートである。 実施形態の外力追従制御システムで、検証結果を示すグラフ図である。 実施形態の外力追従制御システムで、（ａ）は、アームに外力が加わった状態を示す模式的な平面図、（ｂ）は、実施形態の外力追従制御システムで、アームに外力が加わり続けている様子を示す模式的な平面図、（ｃ）は、実施形態の外力追従制御システムで、アームに外力が加わらなくなった状態を示す模式的な平面図である。 実施例１の外力追従制御システムで、各部の構成を説明する構成図である。 実施例１の外力追従制御システムで、機能ブロック図である。 実施例１の外力追従制御システムの動作を説明するフローチャートである。 実施例２の外力追従制御システムで、各部の構成を説明する構成図である。 実施例２の外力追従制御システムで、機能ブロック図である。 実施例３の外力追従制御システムで、各部の構成を説明する構成図である。 実施例３の外力追従制御システムで、機能ブロック図である。 変形例１の一外力追従制御システムで、外力を推定する機能ブロック図である。 変形例２の外力追従制御システムで、外力を推定する機能ブロック図である。

本発明の実施形態について、図１乃至図６を参照して詳細に説明する。説明において、同一の要素には同一の番号を付し、重複する説明は省略する。

図１は、実施形態の外力追従制御システムが適用される内視鏡ホルダロボットの斜視図である。
この実施形態の制御対象である内視鏡ホルダロボット１０は、主に、ロボット本体１１と、ロボット本体１１に対して、鉛直方向の軸Ｙを回転中心として回転可能に設けられた伸縮アーム部３０と、この伸縮アーム部３０に設けられて、硬性内視鏡２０を保持する内視鏡ホルダ部４０とを備えている。

また、手術を行う者の頭部または腕部などには、入力部としてのコントローラ８０が装着されている。そして、コントローラ８０の動きに合わせて出力された指令信号に応じて、伸縮アーム部３０を、軸Ｙを回転中心として回転移動させたり、あるいは、駆動により、前後方向に、硬性内視鏡２０をズーム移動させることが内蔵されたアクチュエータを用いてできるように構成されている。この実施形態では、主に回転系のアクチュエータの制御について記載する。

この実施形態の硬性内視鏡２０は、先端が体内に挿入され、体内の被観察部の像を導光する光学視管２２と、光学視管２２を回転自在に接続して、導光された像を受光することにより画像信号を出力するカメラヘッド部２４とを含んでいる。

カメラヘッド部２４には、受光平面を有するＣＣＤイメージセンサが設けられている。ＣＣＤイメージセンサは、レンズなどを介して受光平面に結像させた像の光による明暗を電荷の量に光電変換し、それを順次読み出して電気的な画像信号に変換する。
そして、カメラヘッド部２４のＣＣＤイメージセンサから出力された画像信号は、画像生成装置により出力画像信号として生成されて、出力されることにより図示しないモニタに体内の被観察部位を映し出すように構成されている。

図２は、実施形態の外力追従制御システムで、各部の構成を説明する構成図である。
内視鏡ホルダロボット１０は、入出力部６０を介して、制御部７０に接続されている。
この実施形態の内視鏡ホルダロボット１０は、ロボット本体１１の鉛直方向の軸Ｙ（図１参照）を回転中心として伸縮アーム部３０を回転駆動するアクチュエータとしての空気圧アクチュエータ１２と、この空気圧アクチュエータ１２によって回転した伸縮アーム部３０の回転角度を検出する位置検出手段としての光学式ロータリーエンコーダ１４を含む。

このうち、空気圧アクチュエータ１２は、空気圧送ポンプなどの動力源１６から圧送される空気を駆動力としている。そして、この空気圧アクチュエータ１２は、駆動力制御装置１８に接続されている。この駆動力制御装置１８は、入出力部６０からの駆動力制御信号に応じて、空気圧アクチュエータ１２を駆動する。

光学式ロータリーエンコーダ１４は、軸Ｙ（図１参照）を回転中心とする伸縮アーム部３０の回転位置を検出する。そして、回転位置を電気的信号に変換して位置応答値として、入出力部６０に出力するように構成されている。

また、この実施形態の内視鏡ホルダロボット１０には、力検出部４２が設けられている。そして、力検出部４２は、力応答値としての力信号および外力信号を入出力部６０に出力するように構成されている。

このように構成された実施形態の内視鏡ホルダロボット１０は、制御部７０からの制御指令を、入出力部６０によって駆動力信号に変換する際、入出力部６０に入力する位置信号、力信号、および外力信号などの状態量に応じて、フィードバックすることにより所望の位置および回転角度に伸縮アーム部３０を移動させるように構成されている。
図３は、実施形態の外力追従制御システムで、機能ブロック図である。この実施形態の内視鏡ホルダロボット１０の位置制御系は、主に、制御部７０内に設けられていて、位置指令値生成部７２、位置制御器７４、駆動力制御器７６、自重補償制御器７８を含んでいる。

このうち、位置指令値生成部７２は、コントローラ８０と接続されていて、コントローラ８０のボタン操作などによる外部入力が位置指令値生成部７２に入力されるように構成されている。
この位置指令値生成部７２には、さらに、制御対象である内視鏡ホルダロボット１０の伸縮アーム部３０の状態（回転角度）および推定もしくは計測した外力の値を状態量として位置目標値を生成し、この位置目標値を出力する。
また、前記光学式ロータリーエンコーダ１４で検出された実際の伸縮アーム部３０の回転位置を示す位置応答値は、入出力部６０を介して制御部７０に入力されている。位置応答値は、位置応答値としては、このような位置検出部からの位置信号に限らず、この位置信号が生成した信号（速度推定値など）を位置応答値として用いてもよい。

位置制御器７４には、位置目標値と制御対象からの位置応答値との差分が入力される。
位置制御器７４は、位置目標値と制御対象からの位置応答値との差分を０にするような位置制御のために演算を実行し、その演算結果として駆動力参照値を生成している。駆動力参照値は、位置制御器７４から出力されることによりアクチュエータを駆動してロボットアームの位置を指令する。

駆動力制御器７６には、駆動力参照値と制御対象からの力応答値との差分が入力される。
駆動力制御器７６は、駆動力参照値と制御対象からの力応答値（＝力信号）との差分を０にするような駆動力信号を、制御対象である内視鏡ホルダロボット１０に入力として与えて駆動力制御を実行している。この実施形態の入力は、駆動力制御装置１８として実装されているサーボ弁１１８等への印加電圧（Ｖ）である。

なお、この実施形態では、制御特性の改善を図るため、自重補償制御器７８などを用いて、位置応答値（＝位置信号）に基づいた自重補償力などのフィードフォワード補償を行なってもよい。
自重補償制御器７８は、予め想定される内視鏡ホルダロボット１０の自重の影響を取り除くためのものである。
すなわち、伸縮アーム部３０は、内視鏡の光学視管２２を装着した状態で、動作位置（回転角度）によって、自重による影響を相違させる。このため、自重補償制御器７８を用いて、このような内視鏡ホルダロボット１０の自重の影響をキャンセルさせる自重補償力をフィードフォワード補償として行なうことにより、さらに制御特性を向上させることができる。

力応答値としては、力検出部４２からの力信号と、加えられている外力を推定値として出力する外力信号との２種類が考えられる。力信号または外力信号は、入出力部６０を介して制御部７０に入力される。
力検出部４２は、たとえば推定値を算出するアルゴリズムであってもよい。また、内視鏡ホルダロボット１０内に設けられた力センサなどの手段を用いて力信号を得るように構成してもよい。

駆動力参照値と力応答値との差分は、駆動力制御器７６に入力される。駆動力制御器７６は、駆動力参照値と力応答値との差分を０とするように制御を行なう。そして制御結果として駆動力信号を出力する（図２参照）。駆動力信号は、駆動力制御装置１８に入力されて、空気圧アクチュエータ１２を駆動して伸縮アーム部３０を移動させる。

位置応答値は、光学式ロータリーエンコーダ１４で計測した現在位置を示す状態量である。駆動力は圧力センサ等の力検出部４２にて検出された値にサーボ弁のシリンダの受圧面積を乗算して求められる。そして、各センサで検出された状態量は、力信号または外力信号として入出力部６０に内視鏡ホルダロボット１０から入力される。

図４は、実施形態の外力追従制御システムの動作を説明するフローチャートである。
この実施形態では、位置指令値生成部７２において、外力により生じた伸縮アーム部３０の位置偏差、あるいは外力信号が取得される。
そして、制御対象に事前に設定した値を超える力が負荷として加えられた場合、コントローラ８０による位置目標値を力が逃げる（除荷される）方向に修正するように構成されている。

一方、一般的なコンプライアンス制御・インピーダンス制御では、位置目標値を外力によって修正する。このため、負荷が開放（除荷）されたあとは、修正前の位置目標値に戻る動作が発生する。
これに対して本実施形態の外力追従制御システムでは、位置目標値を外力に応じて更新しながら上書きする。このため、負荷の開放による伸縮アーム部３０の戻り動作が発生しない。
すなわち、図４にて、処理が開始されると、ステップＳ１では、位置偏差、外力信号が取得される。

ステップＳ２では、状態量が閾値を上回っているか否かが判定される。状態量が閾値を上回っている場合（ステップＳ２にてＹＥＳ）には、次のステップＳ３に処理が進み、状態量が閾値を上回っていない場合（ステップＳ２にてＮＯ）には、処理を終了する。
ここで、偏差が予め設定された閾値ｅＬｉｍｉｔを超えていた場合、ステップＳ２において下記の数１の式（１）にて、位置目標値ｑ^ｒｅｆ（例えば３０度）を修正する。

ステップＳ３では、外力、内部状態から位置修正量を計算する。この位置修正には、修正アルゴリズムが操作する時間Ｔの設定が含まれる。
そして、修正処理が開始されてからの時間がカウントされて、アルゴリズムの動作時間内である場合は、ステップＳ３において、下記の数２の差分方程式（式２）にて位置目標値を修正する。

ここで、Ｔは収束させるための時間、ｓｔは制御周期、Ｑは修正速度を調整するパラメータである。
ステップＳ４では、位置目標値を更新しながら上書きして処理を終了する。

この実施形態の外力追従制御システムでは、位置目標値の修正に係る時間の上限と速さを制御周期ｓｔおよび修正速度Ｑの設定により指定することができる。
また、この実施形態の外力追従制御システムでは、光学式ロータリーエンコーダ１４によって検出された状態量として位置偏差のみを用いることで、外力信号および外力の検出手段もしくは外力の推定機構を別途設ける必要がない。

また、現在状態量であるｑを位置目標値ｑ^ｒｅｆと比較して（ステップＳ２）、ステップＳ３にて位置修正量を計算している。このため、外力を受け流して目標位置をステップＳ４にて上書き更新することにより、外力がなくなったときに実測値を目標値に収束させることができる。
したがって、たとえば空気圧アクチュエータなどバックドライバビリティが存在する医療用ロボットなどに用いて好適である。

図５は、実施形態の外力追従制御システムで、検証結果を示すグラフ図である。また、図６は、実施形態の外力追従制御システムで、（ａ）は、アームに外力が加わった状態を示す模式的な平面図である。（ｂ）は、実施形態の外力追従制御システムで、アームに外力が加わり続けている様子を示す模式的な平面図である。（ｃ）は、実施形態の外力追従制御システムで、アームに外力が加わらなくなった状態を示す模式的な平面図である。

ここでは、状態量として内視鏡ホルダロボット１０の伸縮アーム部３０の角度を、外力が加わっていない状態を０度、閾値を３０度として説明する。
図５中、測定開始から４秒が経過した時点ａにて、図６中矢印で示すように周方向に伸縮アーム部３０を回転させる外力が加わると、伸縮アーム部３０の回転角度が増大する。そして、位置指令値と位置応答値との間の乖離が３０度をこえた時点ｂから位置指令値の追従が開始される。

時点ｃにて、伸縮アーム部３０の位置が図６（ｂ）に示すように約８０度に到達する。時点ｃから時点ｄまでは、伸縮アーム部３０に対して、継続して外力が加わっている状態であり、伸縮アーム部３０の位置は、約８０度にて保持される。

時点ｄにて、修正アルゴリズムが開始された時点ｂから予め設定された一定の時間Ｔを経過すると修正アルゴリズムが終了する。
そして、時点ｅにて、伸縮アーム部３０に加わっていた外力が除荷される。
時点ｅから、時点ｆまでの時間にて伸縮アーム部３０は、位置制御の働きによって、位置指令値によって指定された角度（ここでは約７２度）に戻る（図６（ｃ）参照）。

しかしながら、戻り角度は伸縮アーム部３０がもとの図６（ａ）に示す位置に急激に戻るような動作と比較して大きくなく、回動速度も速くならないように抑制されている。
このため、急激に元の外力が加わる前の姿勢となることなく、この例では、約８０度の位置に伸縮アーム部３０は停止し続ける。したがって、伸縮アーム部３０に装着されている光学視管２２の位置が移動しないため、継続して体内の被観察部位を安全に観察できる。

そして、時点ｆ以降は、位置指令値と位置応答値とが一致し続ける。このように、この実施形態の内視鏡ホルダロボットでは、外力を受け流して、目標位置を現在位置にて上書きして更新することにより、外力がなくなったときに実測値を目標値に収束させることができる。

また、術者の手による硬性内視鏡２０の位置決めを行なうことにより、初期に与えられる位置目標値が実測値により上書きされる。このため、続いて行なわれる伸縮アーム部３０の自立制御への移行を、切り替えスイッチなどの操作を用いずにシームレスに行なえる。よって、円滑な手術の進行を妨げることがない。
（位置を基準とする方式）

図７は、実施例１の外力追従制御システムで、各部の構成を説明する構成図である。
なお、前記実施形態の図２と同一部分には、同一符号を付して説明を省略する。
まず構成から説明すると、この実施例１の外力追従制御システムでは、内視鏡ホルダロボット１１０の駆動力制御装置としてサーボ弁１１８が設けられている。

サーボ弁１１８は、動力源としての圧縮空気源１１６から空気圧アクチュエータ１２の制御に用いる圧縮空気の供給を受けるように構成されている。このサーボ弁１１８は、入出力部６０からの駆動力信号を受けて圧縮空気を調整する。力検出部としての圧力センサ１２０は、サーボ弁１１８の圧力を検出して、力信号として入出力部６０に出力する。
そして、制御部７０では、この力信号を力応答値として用いて駆動力信号を生成することにより、内視鏡ホルダロボット１１０の伸縮アーム部３０を空気圧アクチュエータ１２によって駆動するように構成されている。

図８は、実施例１の外力追従制御システムで、機能ブロック図である。
この実施例１の内視鏡ホルダロボット１１０の位置制御系は、主に、制御部７０内に設けられていて、位置指令値生成部７２、位置制御器７４、駆動力制御器７６、自重補償制御器７８を含んでいる。
そして、状態量として伸縮アーム部３０の回転位置を用いて偏差から、外力を推定して、位置目標値を修正するように、駆動力制御器７６は、入力ｕを制御対象である内視鏡ホルダロボット１０に与えるように構成されている。

図９は、実施例１の外力追従制御システムの動作を説明するフローチャートである。
すなわち、図９にて、処理が開始されると、ステップＳ１１では、位置偏差が取得される。

ステップＳ１２では、位置偏差が閾値を上回っているか否かが判定される。位置偏差が閾値を上回っている場合（ステップＳ１２にてＹＥＳ）には、次のステップＳ１３に処理を進めて、位置目標値を修正するとともに、修正アルゴリズムが操作する時間Ｔの設定である修正時間をリセットする。
また、位置偏差が閾値を上回っていない場合（ステップＳ１２にてＮＯ）には、処理をステップＳ１４に進める。

ステップＳ１４では、修正時間内であるか否かが判定される。修正時間内である場合（ステップＳ１４にてＹＥＳ）、ステップＳ１５に処理を進め、修正時間内でない場合（ステップＳ１４にてＮＯ）、処理を終了する。

ステップＳ１５では、位置修正量の計算が行われる。そして、ステップＳ１６にて修正時間のカウントアップが行われて、ステップＳ１７にて位置目標値を出力するとともに、処理を終了する。
これにより、実施例１の外力追従制御システムでは、無負荷時は指令通りに正確に動作し、かつ外力により移動している状態から除荷されても、アームの位置を保持することが出来る。

このように、実施例１では、実施形態の外力追従制御システムの作用効果に加えてさらに、フィードバック制御に用いる光学式ロータリーエンコーダ１４のような既設の位置センサを用いて、簡便に、異常外力に対する追従および戻り動作の抑制制御を行なうことができる。

（力を基準とする方式）
図１０は、実施例２の外力追従制御システムで、各部の構成を説明する構成図である。
なお、前記実施形態および実施例１と同一部分には、同一符号を付して説明を省略し、各部の構成について相違点を中心に説明する。

この実施例２の外力追従制御システムでは、位置修正を行なうか否かの判定基準とする状態量として外力を用いている。そして、外力を検出する力検出部１２２として、空気圧アクチュエータ１２の制御用の圧力センサ１２０とは、別体で伸縮アーム部３０に加わる外力を検出する力センサ１２４を設けている。

そして、図示しない動力源としての圧縮空気源から空気圧アクチュエータの制御に用いる圧縮空気の供給を受けたサーボ弁１１８は、力検出部としての圧力センサ１２０によって圧力が検出されて、力信号として入出力部６０に出力する。

実施例２では、この力信号とは独立して、力検出部１２２には、外力信号を出力する力センサ１２４が設けられている。
この力センサ１２４は、内視鏡ホルダロボット２１０の可動部分である伸縮アーム部３０などに設けられて、伸縮アーム部３０に加えられる外力を検出可能としている。力センサ１２４によって検出された外力は外力信号として入出力部６０に送られるように構成されている。

図１１は、実施例２の外力追従制御システムで、機能ブロック図である。
このように構成された実施例２の外力追従制御システムでは、圧力センサ１２０で検出された外力Ｆ^ｅｘｔが、閾値Ｆ^ｅｘｔ _{Ｌｉｍｉｔ}を超えた場合、図４のステップＳ３にて、以下の数３の式（３）を用いて位置目標値ｑｒｅｆが修正される。

ここで、Ｋは外力と位置修正値とを関係づける仮想コンプライアンスである。Ｋは、たとえばバネのような概念である。また、Ｔは収束させるための時間、ｓｔは制御周期、Ｑは修正速度を調整するパラメータである。なお、後述するばねの概念に加えてダンパ成分や質量も加えた概念（仮想インピーダンス）を用いてもよい。

次にこの実施例２の外力追従制御システムの作用効果について説明する。実施例２の外力追従制御システムでは、空気圧アクチュエータ１２を駆動する際に用いる圧力センサ１２０とは、独立して、力検出部１２２に力センサ１２４が設けられている。

このため、独立して、伸縮アーム部３０に加えられる外力を検出可能である。このため、状態量として外力を用いることにより、位置制御に用いるサーボの機械的な剛性と外力への応答とを独立させて設計することが可能である。したがって例えば、減速機を用いた電動モータなどの構成を有する外力追従制御システムに用いて好適である。
他の構成および作用効果については、前記実施形態および実施例１と同様であるので説明を省略する。
（外力を推定する方式）

図１２は、実施例３の外力追従制御システムで、各部の構成を説明する構成図である。
なお、前記実施形態および実施例１,２と同一部分には、同一符号を付して説明を省略する。
まず構成上の相違点から説明すると、実施例３の外力追従制御システムに用いられる制御部１７０には、外力推定器１７１が設けられている。
外力推定器１７１は、伸縮アーム部３０に加わる外力を、空気圧アクチュエータ１２の駆動状況から推定して算出することができる。

図１３は、実施例３の外力追従制御システムで、機能ブロック図である。この実施例３の外力追従制御システムでは、外力推定器１７１に、駆動力参照値ｆ_ｋ ^ｒｅｆと、位置応答値ｑ^ｒｅｓとが入力される。
そして、伸縮アーム部３０に加えられた力から、伸縮アーム部３０の駆動のために内視鏡ホルダロボット１０が使用した力を引くことにより、伸縮アーム部３０にかかっている外力が推定されるように構成されている。

推定された外力は、位置指令値生成部７２に推定外力として入力されて、前記実施例１,２と同様に、位置指令値生成部７２にて位置目標値の生成に用いられる。なお、光学式ロータリーエンコーダ１４で計測した伸縮アーム部３０の角度が位置目標値と一定の閾値以上の偏差を発生させた場合に、外力の推定を開始するようにしてもよい。

この実施例３の外力追従制御システムでは、前記実施例２の外力追従制御システムと比較して、力検出部１２２としての力センサ１２４が不要となる。このため、別途、力センサ１２４を装着する際のコスト等を削減することができる。他の構成および作用効果については、前記実施例２の外力追従制御システムの作用効果と同様であるので説明を省略する。

図１４は、実施例３の変形例１の外力追従制御システムで、外力を推定する機能ブロック図である。この変形例１の外力推定器２７１は、伸縮アーム部３０の質量をＭｎ、駆動力参照値を力応答値とみなせる場合、次の数４の式（４）にて、推定外力の推定が行なわれる。

他の構成および作用効果については、前記実施例３の外力追従制御システムの作用効果と同様であるので説明を省略する。

図１５は、実施例３の変形例２の外力追従制御システムで、外力を推定する機能ブロック図である。ここで、ｓはラプラス演算子で、ｓ²は、二階微分を示している。この変形例２の外力推定器３７１は、伸縮アーム部３０の質量をＭｎ、伸縮アーム部３０の関節の粘性摩擦力をＢｎとした場合、次の数５の式（５）にて、推定外力の推定が行なわれる。

このように、内視鏡ホルダロボット１０の伸縮アーム部３０の駆動のために使用した力を求める際、必要に応じて、重力やあるいは静止摩擦などの内部動力学補償項を加えることにより、さらに、外力を推定する際の精度を向上させることができる。他の構成および作用効果については、前記実施例３の外力追従制御システムの作用効果と同様であるので説明を省略する。

上述してきたように、この実施形態の外力追従制御システムが適用される内視鏡ホルダロボット１０は、腹腔鏡手術において使用される内視鏡を固定もしくは操作者による外部入力によって内視鏡を正確に移動させることができる。

また、手術中の術者との接触などにより外力が加わっても、実施形態の外力追従制御システムでは、外力が除荷された際に、伸縮アーム部３０が急激な移動を起こすことがない。したがって、従来の内視鏡ホルダロボット１０に増してさらに、カメラケーブルが引っ張られたり、ホルダに接触したり等、アームに対して外力が加わった場合でも、外力が加わる以前のもとの位置まで戻ることはない。

このため、この外力追従制御システムでは、無負荷時は指令通りに正確に動作し、かつ外力により移動している状態から除荷されても、硬性内視鏡２０を保持する伸縮アーム部３０の位置を保持することが出来る。
よって、伸縮アーム部３０に装着されている光学視管２２の位置が移動しないため、継続して体内の被観察部位を安全に観察できる。

以上、本実施形態に係る外力追従制御システムについて詳述してきたが、本発明はこれらの実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは言うまでもない。

例えば、本実施形態では、空気圧アクチュエータ１２によって回転した伸縮アーム部３０の回転角度を検出する位置検出手段として、光学式ロータリーエンコーダ１４を用いたものを示して説明してきたが、他の位置エンコーダやポテンショメータを用いて、ロボットアームの位置を検出するようにしてもよい。
また、入力部として、コントローラ８０を示して説明してきたがジャイロセンサやパネル入力など、他の入力装置を用いて、入力部が構成されていてもよい。

そして、アクチュエータとして空気圧アクチュエータ１２を用いたものを示して説明してきたが特にこれに限らず、たとえば、電動モータ（ＡＣ,ＤＣ,ステッピングモータなど）、油圧モータ、空気圧モータのようなアクチュエータであってもよく、アクチュエータの形状、数量および種類が特に限定されるものではない。

また、実施形態では、空気圧送ポンプなどの動力源１６を用いているが特にこれに限らず、電源を動力源として用いてもよく、動力制御装置としてサーボ弁の他、他の形式のバルブやモータドライバを用いてもよい。
さらに、力検出部としての圧力センサ１２０などを用いるものを示して説明してきたが、特にこれに限らず、たとえば、力センサ、電流検出回路などでもよく、その他、力を推定する力推定手段などを用いてもよい。
さらに、実施例３にて設明したように外力と位置修正値とを関係づけるために例えば、仮想の粘性、質量を考慮した仮想インピーダンスなどを用いるとなおよい。

そして、この実施形態では、外力追従制御システムを内視鏡ホルダロボット１０に用いたものを示して説明してきたが特にこれに限らず、他の医療用ロボットや、産業用ロボットなど、外部環境との物理的な接触が想定される機械構造物全般の制御に用いるように構成してもよい。

１０ 内視鏡ホルダロボット
１１ ロボット本体
１２ 空気圧アクチュエータ
１４ 光学式ロータリーエンコーダ
１６ 動力源
１８ 動力制御装置
２０ 硬性内視鏡
２２ 光学視管
２４ カメラヘッド部
３０ 伸縮アーム部
４０ 内視鏡ホルダ部
４２ 力検出部
６０ 入出力部
７０ 制御部
７２ 位置指令値生成部
７４ 位置制御器
７６ 駆動力制御器
７８ 自重補償制御器
８０ コントローラ（入力部）
１１０ 内視鏡ホルダロボット
１１６ 圧縮空気源
１１８ サーボ弁
１２０ 圧力センサ
１２２ 力検出部
１２４ 力センサ
１７０ 制御部
１７１,２７１，３７１ 外力推定器
２１０ 内視鏡ホルダロボット

Claims (11)

1. ロボットアームと、
   前記ロボットアームの位置を位置目標値を用いて指示する入力部と、
   前記入力部から入力された位置目標値に応じて、前記ロボットアームを移動させるアクチュエータと、
   前記ロボットアームの位置を検出する位置検出部と、
   前記位置検出手段で検出されたロボットアームの位置を示す位置応答値と、前記位置目標値との差分に応じて、前記アクチュエータを駆動する制御部と、を備え、
   前記制御部は、前記ロボットアームの状態量に応じて前記位置目標値を修正することを特徴とする、外力追従制御システム。
2. 前記制御部は、前記ロボットアームに加えられている外力に応じて前記位置目標値を修正することを特徴とする、請求項１に記載の外力追従制御システム。
   外力追従制御システム。
3. 前記制御部は、前記外力によって移動したロボットアームの位置を、外力が除荷されても、前記外力によって移動する前の位置に戻さないことを特徴とする請求項１または２に記載の外力追従制御システム。
4. 前記制御部は、前記位置目標値と前記ロボットアームに外力が加えられて移動した位置との間の該ロボットアームの移動量が予め設定された閾値を超えた場合に、前記位置目標値を上書きすることを特徴とする、請求項１ないし３のうち一項に記載の外力追従制御システム。
5. 前記アクチュエータに加えられた外力を検出して外力信号として出力する力センサをさらに備え、前記制御部は、外力信号に応じて、前記位置目標値を上書きすることを特徴とする、請求項１ないし４のうち一項に記載の外力追従制御システム。
6. 前記制御部は、前記ロボットアームの移動量が予め設定された閾値を超えた場合に、外力信号に応じて、前記位置目標値を上書きすることを特徴とする、請求項１ないし５のうち一項に記載の外力追従制御システム。
7. 前記制御部は、制御用センサにて検出された圧力値から得られる力信号を外力信号として用いることを特徴とする、請求項５または６に記載の外力追従制御システム。
8. 前記アクチュエータに設けられた制御用の圧力センサとは、別体で前記ロボットアームに加わる外力を検出する力センサを設けて、前記制御部は、前記力センサにて検出された外力に基づく外力信号を用いて、位置目標値を修正することを特徴とする、請求項１ないし７のうち一項に記載の外力追従制御システム。
9. 前記制御部は、前記ロボットアームに加わる外力を、前記アクチュエータの駆動状況から算出する外力推定部を含むことを特徴とする請求項１ないし８うち一項に記載の外力追従制御システム。
10. 前記ロボットアームは、空気圧アクチュエータによって駆動される医療用ロボットのアームであることを特徴とする請求項１ないし９のうち一項に記載の外力追従制御システム。
11. 前記ロボットアームは、電動モータによって駆動されるロボットのアームであることを特徴とする請求項１ないし９のうち一項に記載の外力追従制御システム。

Images