(57)【要約】【課題】ある関節部の移動が規制された状態で、挿入機器に不具合が生じることなく、適切に先端腕部を動作させる挿入機器の動作制御装置を提供すること。【解決手段】挿入機器の動作制御装置は、対応する駆動部材の駆動状態に基づいて、対応する関節部が移動自在な非規制状態であるか、又は、対応する前記関節部の移動が規制される規制状態であるか、をそれぞれが判定する複数の移動規制判定部を備える。前記動作制御装置は、第１の制御モードにおいて１つ以上の前記関節部が前記規制状態と判定された場合に、前記規制状態と判定された前記関節部に対応する前記駆動部材を、駆動しない、又は、前記第1の制御モードより駆動速度が小さくする第２の制御モードで、それぞれの前記駆動部材を駆動制御する駆動制御部を備える。【選択図】図６
2015107138.tif 000002
【特許請求の範囲】
【請求項１】
長手軸に沿って延設される挿入部を備え、前記挿入部は先端部に動作可能に設けられる先端腕部を備える挿入機器であって、前記先端腕部は、複数の関節部と、最も前記先端方向側に位置する前記関節部の前記先端方向側に設けられる先端機能部と、を備え、それぞれの前記関節部が作動されることにより前記先端腕部において作動された前記関節部より前記先端方向側の部位が動作する挿入機器と、
前記先端機能部の目標位置及び目標姿勢を示す操作指令が入力される操作指令入力部と、
駆動エネルギーが供給されることにより駆動され、駆動されることによりそれぞれが対応する前記関節部を作動させる複数の駆動部材と、
対応する前記駆動部材の駆動状態に基づいて、対応する前記関節部が移動自在な非規制状態であるか、又は、対応する前記関節部の移動が規制される規制状態であるか、をそれぞれが判定する複数の移動規制判定部と、
前記操作指令に基づいて第1の制御モードでそれぞれの前記駆動部材を駆動制御する駆動制御部であって、前記第１の制御モードにおいて１つ以上の前記関節部が前記規制状態と判定された場合に、前記規制状態と判定された前記関節部に対応する前記駆動部材を、駆動しない、又は、前記第1の制御モードより駆動速度が小さくする第２の制御モードで、それぞれの前記駆動部材を駆動制御する駆動制御部と、
を具備する挿入機器の動作制御装置。
【請求項２】
前記駆動制御部は、
前記操作指令に基づいて、前記先端機能部を前記目標位置及び前記目標姿勢にするそれぞれの前記関節部の関節位置及び関節姿勢を算出する関節位置姿勢算出部と、
前記関節位置姿勢算出部で算出された前記関節位置及び前記関節姿勢にするそれぞれの前記関節部の作動状態を算出する関節作動状態算出部と、
前記関節作動状態算出部で算出されたそれぞれの前記関節部の前記作動状態に基づいて、それぞれの前記駆動部材の駆動指令を生成する駆動指令生成部と、
対応する前記駆動部材の前記駆動指令に基づいて、それぞれが対応する前記駆動部材に前記駆動エネルギーを供給する複数のサーボ制御部と、
を備える、請求項１の動作制御装置。
【請求項３】
前記関節位置姿勢算出部は、前記第２の制御モードにおいて、前記規制状態と判定された前記関節部を現在の前記関節位置及び前記関節姿勢から変化させることなく、前記先端機能部を前記目標位置及び前記目標姿勢にするそれぞれの前記関節部の前記関節位置及び前記関節姿勢を算出する、請求項２の動作制御装置。
【請求項４】
ぞれぞれの前記移動規制判定部での判定結果に基づいて、前記規制状態と判定された前記関節部の中で最も前記先端方向側に位置する最先端規制関節を特定する規制先端特定部をさらに具備し、
前記関節位置姿勢算出部は、前記第２の制御モードにおいて、前記最先端規制関節及び前記最先端規制関節より基端方向側の前記関節部を現在の前記関節位置及び前記関節姿勢から変化させることなく、前記先端機能部を前記目標位置及び前記目標姿勢にするそれぞれの前記関節部の前記関節位置及び前記関節姿勢を算出する、
請求項３の動作制御装置。
【請求項５】
前記関節位置姿勢算出部は、前記先端機能部を前記目標位置及び前記目標姿勢にするそれぞれの前記関節部の前記関節位置及び前記関節姿勢を算出不可能な場合に、前記規制状態の前記関節部を現在の前記関節位置姿勢及び前記関節姿勢から変化させることなく、かつ、前記先端機能部の先端姿勢を一意的に決定することなく、前記先端機能部を前記目標位置にするそれぞれの前記関節部の前記関節位置及び前記関節姿勢を算出する、請求項３の動作制御装置。
【請求項６】
前記先端機能部を前記目標位置及び前記目標姿勢にするそれぞれの前記関節部の前記関節位置及び前記関節姿勢を前記関節位置姿勢算出部が算出不可能な場合に、警告情報を発生する警告発生部をさらに具備する、請求項３の動作制御装置。
【請求項７】
前記駆動指令生成部は、前記先端機能部を前記目標位置及び前記目標姿勢にするそれぞれの前記関節部の前記関節位置及び前記関節姿勢を前記関節位置姿勢算出部が算出不可能な場合に、全ての前記駆動部材について前記駆動指令を生成せず、全ての前記関節部の作動を停止する、請求項３の動作制御装置。
【請求項８】
前記駆動制御部は、前記第1の制御モードにおいて対応する前記関節部が前記規制状態と判定された場合に、前記第２の制御モードにおいて、それぞれが対応する前記サーボ制御部での前記駆動指令に対する前記駆動エネルギーの前記サーボゲインを前記第1の制御モードより小さくし、それぞれが対応する前記駆動部材の前記駆動速度を前記第1の制御モードより小さくする複数のサーボゲイン変更部を備える、請求項２の動作制御装置。
【請求項９】
それぞれの前記移動規制判定部は、対応する前記関節部が前記長手軸に垂直な方向に移動自在な非規制状態であるか、又は、対応する前記関節部の前記長手軸に垂直な前記方向への移動が規制される規制状態であるか、を判定する、請求項１の動作制御装置。
【請求項１０】
前記駆動部材は、前記駆動エネルギーとして駆動電流が供給されるモータである、請求項１の動作制御装置。
【請求項１１】
前記挿入機器は、内視鏡の処置具挿通路に前記挿入部を挿通可能なマニピュレータである、請求項１の動作制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、体腔内、工業用の管路内等に挿入される挿入部を備えるマニピュレータ、内視鏡等の挿入機器の動作を制御する挿入機器の動作制御装置に関する。
【背景技術】
【０００２】
特許文献１には、内視鏡の処置具挿通路に挿通されるマニピュレータが開示されている。マニピュレータは、体腔内に挿入される挿入部を備える挿入機器である。マニピュレータの挿入部の先端部には、先端腕部が設けられている。先端腕部は、複数の関節部と、それぞれが対応する関節部の先端方向側に設けられるリンク部と、最も先端方向側のリンク部の先端方向側に位置する先端処置部（先端機能部）と、を備える。それぞれの関節部が作動されることにより、先端腕部において作動された関節部を中心として作動された関節部より先端方向側の部位が回動し、先端腕部が動作を行う。マニピュレータの先端腕部の動作を制御する動作制御装置は、それぞれの関節部の作動（駆動）を制御する作動制御部（駆動制御部）を備える。また、マニピュレータの動作制御装置は、先端処置部の目標位置及び目標姿勢を示す操作指令（軌道指令）が入力される操作指令入力部（軌道入力部）を備える。そして、軌道設定部が、先端処置部が目標位置及び目標姿勢となる状態に、作動する関節部の数及び作動する関節部の作動角度を設定する。作動制御部は、軌道設定部で設定された作動する関節部の数及び作動する関節部の作動角度に基づいて、それぞれの関節部の作動状態を制御している。
【０００３】
また、マニピュレータ装置は、それぞれの関節部の負荷量を検出する負荷検出部を備える。ここで、先端腕部を動作させることによって、ある関節部での負荷量が負荷許容量以上となることがある。そこで、軌道設定部は、負荷検出部での検出結果に基づいて、全ての関節部の負荷量が許容負荷量未満となる状態に、作動する関節部の数及び作動する関節部の作動角度を設定している。これにより、全ての関節部の負荷量が許容負荷量未満となる状態で、先端処置部を目標位置及び目標姿勢にすることが可能となる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００９−１０７０７４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
前記特許文献１のように内視鏡の処置具挿通路にマニピュレータを挿通して使用する場合、ある関節部が処置具挿通路の内部に位置することがある。この際、処置具挿通路の内部に位置する関節部は、処置具挿通路を規定する内周面によって、特に長手軸に垂直な方向について、移動が規制される。この状態で移動が規制された関節部を作動した場合、処置具挿通路の内周面と先端腕部との衝突等が原因で、挿入機器であるマニピュレータに不具合が生じる可能性がある。
【０００６】
本発明は前記課題に着目してなされたものであり、その目的とするところは、ある関節部の移動が規制された状態で、挿入機器に不具合が生じることなく、適切に先端腕部を動作させる挿入機器の動作制御装置を提供することにある。
【課題を解決するための手段】
【０００７】
前記目的を達成するために、本発明のある態様の挿入機器の動作制御装置は、長手軸に沿って延設される挿入部を備え、前記挿入部は先端部に動作可能に設けられる先端腕部を備える挿入機器であって、前記先端腕部は、複数の関節部と、最も前記先端方向側に位置する前記関節部の前記先端方向側に設けられる先端機能部と、を備え、それぞれの前記関節部が作動されることにより前記先端腕部において作動された前記関節部より前記先端方向側の部位が動作する挿入機器と、前記先端機能部の目標位置及び目標姿勢を示す操作指令が入力される操作指令入力部と、駆動エネルギーが供給されることにより駆動され、駆動されることによりそれぞれが対応する前記関節部を作動させる複数の駆動部材と、対応する前記駆動部材の駆動状態に基づいて、対応する前記関節部が移動自在な非規制状態であるか、又は、対応する前記関節部の移動が規制される規制状態であるか、をそれぞれが判定する複数の移動規制判定部と、前記操作指令に基づいて第1の制御モードでそれぞれの前記駆動部材を駆動制御する駆動制御部であって、前記第１の制御モードにおいて１つ以上の前記関節部が前記規制状態と判定された場合に、前記規制状態と判定された前記関節部に対応する前記駆動部材を、駆動しない、又は、前記第1の制御モードより駆動速度が小さくする第２の制御モードで、それぞれの前記駆動部材を駆動制御する駆動制御部と、を備える。
【発明の効果】
【０００８】
本発明によれば、ある関節部の移動が規制された状態で、挿入機器に不具合が生じることなく、適切に先端腕部を動作させる挿入機器の動作制御装置を提供することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の第１の実施形態に係るマニピュレータの動作制御装置を示す概略図。
【図２】第１の実施形態に係るマニピュレータの関節部を作動する構成を概略的に示すブロック図。
【図３】第１の実施形態に係る駆動制御部の関節位置姿勢算出部及び関節作動状態算出部の第１の制御モードでの処理を説明する概略図。
【図４】第１の実施形態に係る駆動制御部のある１つのサーボ制御部の構成を概略的に示すブロック図。
【図５】第１の実施形態に係る駆動制御部のある１つのサーボ制御部、及び、そのサーボ制御部に対応するモータ及びエンコーダでの処理を説明するブロック図。
【図６】第１の実施形態に係る駆動制御部の関節位置姿勢算出部及び関節作動状態算出部の第１の制御モードでの処理を説明する概略図。
【図７】第１の実施形態のある変形例に係る駆動制御部の関節位置姿勢算出部の処理を説明する概略図。
【図８】第２の実施形態に係るマニピュレータの関節部を作動する構成を概略的に示すブロック図。
【図９】第２の実施形態に係る駆動制御部のある１つのサーボ制御部の構成を概略的に示すブロック図。
【図１０】第２の実施形態に係る駆動制御部のある１つのサーボ制御部、及び、そのサーボ制御部に対応するモータ及びエンコーダでの処理を説明するブロック図。
【図１１】第１の実施形態及び第２の実施形態のある変形例に係る内視鏡の、管路に挿通された状態を示す概略図。
【発明を実施するための形態】
【００１０】
（第１の実施形態）  
本発明の第１の実施形態について、図１乃至図６を参照して説明する。図１は、マニピュレータ２の動作制御装置１を示す図である。図１に示すように、マニピュレータ２の動作制御装置１は、挿入機器であるマニピュレータ２と、制御ユニット３と、３Ｄデジタライザー等の操作指令入力部５と、を備える。
【００１１】
マニピュレータ２は、長手軸Ｃを有する。ここで、長手軸Ｃに平行な方向の一方を先端方向（図１の矢印Ｃ１の方向）とし、長手軸Ｃに平行な方向の他方を基端方向（図１の矢印Ｃ２の方向）とする。マニピュレータ２は、長手軸Ｃに沿って延設される細長い挿入部１０と、挿入部１０の基端方向側に設けられる保持部１３と、を備える。
【００１２】
挿入部１０は、長手軸Ｃに沿って延設される細長い管状部１１と、管状部１１の先端方向側に設けられる先端腕部１２と、を備える。先端腕部１２は、マニピュレータ２の先端部に位置し、動作可能に設けられている。保持部１３には、ユニバーサルコード７の一端が接続されている。ユニバーサルコード７の他端は、制御ユニット３に接続されている。また、制御ユニット３は、操作指令入力部５からの操作指令を例えば無線通信等により受信可能である。
【００１３】
先端腕部１２は、複数（本実施形態では５つ）の関節部１５Ａ〜１５Ｅと、複数（本実施形態では５つ）のリンク部１７Ａ〜１７Ｅと、を備える。それぞれのリンク部１７Ａ〜１７Ｅは、対応する関節部１５Ａ〜１５Ｅの先端方向側に延設されている。リンク部１７Ｅの先端方向側には、先端処置部（先端機能部）であるメス部１９が設けられている。すなわち、メス部１９は、先端腕部１２の先端部に位置し、最も先端方向側の関節部１５Ｅより先端方向側に位置している。それぞれの関節部１５Ａ〜１５Ｅが作動される（actuated）ことにより、先端腕部１２において作動された関節部（１５Ａ〜１５Ｅ）を中心として作動された関節部（１５Ａ〜１５Ｅ）より先端方向側の部位が回動する。すなわち、それぞれの関節部１５Ａ〜１５Ｅが作動されることにより、先端腕部１２において作動された関節部（１５Ａ〜１５Ｅ）より先端方向側の部位が動作する。また、メス部１９によって、生体組織等の処置対象の切開が行われ、処置対象が処置される。
【００１４】
図２は、関節部１５Ａ〜１５Ｅを作動する構成を示す図である。図２に示すように、マニピュレータ２の保持部１３には、駆動部材であるモータ２１Ａ〜２１Ｅと、エンコーダ２２Ａ〜２２Ｅとが設けられている。それぞれのエンコーダ２２Ａ〜２２Ｅは、対応するモータ２１Ａ〜２１Ｅの駆動状態（駆動位置）を検出する。モータ２１Ａ〜２１Ｅ及びエンコーダ２２Ａ〜２２Ｅは、先端腕部１２の基端より基端方向側に位置している。
【００１５】
マニピュレータ２の内部では、線状部材であるワイヤ２３Ａ〜２３Ｅが長手軸Ｃに沿って延設されている。それぞれのワイヤ２３Ａ〜２３Ｅは、対応するモータ２１Ａ〜２１Ｅと先端腕部１２との間で、延設されている。それぞれのワイヤ２３Ａ〜２３Ｅの先端は、対応する関節部１５Ａ〜１５Ｅに接続されている。それぞれのワイヤ２３Ａ〜２３Ｅは、対応するモータ２１Ａ〜２１Ｅの駆動状態に対応して、長手軸Ｃに沿って移動する。それぞれのワイヤ２３Ａ〜２３Ｅが長手軸Ｃに沿って移動することにより、対応する関節部１５Ａ〜１５Ｅが作動される。これにより、先端腕部１２が動作を行う。なお、モータ２１Ａ〜２１Ｅを保持部１３に配置せず、駆動部材であるモータ２１Ａ〜２１Ｅを対応する関節部１５Ａ〜１５Ｅに設けてもよい。この場合、ワイヤ２３Ａ〜２３Ｅは設けられない。それぞれのモータ２１Ａ〜２１Ｅを駆動することにより、対応する関節部１５Ａ〜１５Ｅが作動され、先端腕部１２が動作を行う。
【００１６】
図２に示すように、制御ユニット３は、操作指令入力部５での操作指令を無線通信により受信する指令受信部２５を備える。指令受信部２５は、制御ユニット３に設けられる駆動制御部２６に、電気的に接続されている。駆動制御部２６は、操作指令入力部５での操作指令に含まれるメス部１９（先端処置部）の目標位置データ及び目標姿勢データを検出する。すなわち、操作指令入力部５では、メス部１９の目標位置及び目標姿勢を示す操作指令が入力される。そして、操作指令に含まれるメス部１９の目標位置データ及び目標姿勢データに基づいて、それぞれのモータ２１Ａ〜２１Ｅの駆動制御を行う。
【００１７】
図２に示すように、駆動制御部２６は、操作指令に基づいて、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する関節位置姿勢算出部２８と、関節位置姿勢算出部２８で算出された関節位置及び関節姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの作動状態を算出する関節作動状態算出部２９と、を備える。駆動制御部２６は、第1の制御モード及び第２の制御モードで、それぞれのモーと２１Ａ〜２１Ｅを駆動制御することが可能である。
【００１８】
図３は、第1の制御モードでの関節位置姿勢算出部２８及び関節作動状態算出部２９の処理を説明する図である。図３に示すように、関節位置姿勢算出部２８は、第1の制御モードにおいて、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。ここで、関節位置姿勢算出部２８での算出において、メス部１９が目標位置及び目標姿勢となればよく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢は、一意的に決定される必要はない。例えば、メス部１９が目標位置及び目標姿勢となれば、それぞれの関節部１５Ａ〜１５Ｅを、図３の実線で示す関節位置及び関節姿勢に決定してもよく、図３の点線で示す関節位置及び関節姿勢に決定してもよい。
【００１９】
そして、ロボットアームの運動学においてよく知られ、一般的に用いられるDenavit-Hartenberg法に基づく座標変換を用いて、関節作動状態算出部２９が、算出された関節位置及び関節姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの屈曲角度等の作動状態を算出する。ここで、図３の実線で示す関節位置及び関節姿勢に関節位置姿勢算出部２８により決定された場合を考える。そして、座標系Ｓ０〜Ｓ５を規定する。座標系Ｓ０は、管状部１１に原点を有し、管状部１１において長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ１は、関節部１５Ａを原点とし、リンク部１７Ａにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ２は、関節部１５Ｂを原点とし、リンク部１７Ｂにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ３は、関節部１５Ｃを原点とし、リンク部１７Ｃにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ４は、関節部１５Ｄを原点とし、リンク部１７Ｄにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ５は、関節部１５Ｅを原点とし、リンク部１７Ｅにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。
【００２０】
座標系Ｓｋ（ｋ＝１，２，３，４，５）から座標系Ｓｋ−１への変換行列をＨ（ｋ−１←ｋ）とする。この場合、座標系Ｓ０でのメス部１９（マニピュレータ２の先端）の位置姿勢ベクトルＵ０は、それぞれの座標系Ｓｋでのメス部１９の位置姿勢ベクトルＵｋを用いて、  
【数１】
2015107138.tif 000003
となる。変換行列Ｈ（０←１）に基づいて、関節位置姿勢算出部２８で算出された関節部１５Ａの関節位置及び関節姿勢にする関節部１５Ａの作動状態が算出される。変換行列Ｈ（１←２）に基づいて、関節位置姿勢算出部２８で算出された関節部１５Ｂの関節位置及び関節姿勢にする関節部１５Ｂの作動状態が算出される。変換行列Ｈ（２←３）に基づいて、関節位置姿勢算出部２８で算出された関節部１５Ｃの関節位置及び関節姿勢にする関節部１５Ｃの作動状態が算出される。変換行列Ｈ（３←４）に基づいて、関節位置姿勢算出部２８で算出された関節部１５Ｄの関節位置及び関節姿勢にする関節部１５Ｄの作動状態が算出される。変換行列Ｈ（４←５）に基づいて、関節位置姿勢算出部２８で算出された関節部１５Ｅの関節位置及び関節姿勢にする関節部１５Ｅの作動状態が算出される。以上のように、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出され、算出された関節位置及び関節姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの作動状態が算出される。
【００２１】
図３の点線で示す関節位置及び関節姿勢に関節位置姿勢算出部２８により決定された場合は、座標系Ｓ１〜Ｓ５の代わりに座標系Ｓ´１〜Ｓ´５が規定される。そして、変換行列Ｈ（ｋ−１←ｋ）の代わりに変換行列Ｈ´（ｋ−１←ｋ）が用いられる。この場合、座標系Ｓ０でのメス部１９（マニピュレータ２の先端）の位置姿勢ベクトルＵ０は、それぞれの座標系Ｓ´ｋでのメス部１９の位置姿勢ベクトルＵ´ｋを用いて、  
【数２】
2015107138.tif 000004
となる。
【００２２】
駆動制御部２６は、関節作動状態算出部２９で算出されたそれぞれの関節部１５Ａ〜１５Ｅの作動状態に基づいて、それぞれのモータ２１Ａ〜２１Ｅの駆動指令を生成する駆動指令生成部３１を備える。また、駆動制御部２６は、関節部１５Ａ〜１５Ｅに対応させて複数のサーボ制御部３２Ａ〜３２Ｅを備える。駆動指令生成部３１は、サーボ制御部３２Ａ〜３２Ｅに電気的に接続されている。それぞれのサーボ制御部３２Ａ〜３２Ｅには、対応するモータ２１Ａ〜２１Ｅの駆動指令が、駆動指令生成部３１から入力される。それぞれのサーボ制御部３２Ａ〜３２Ｅは、対応するモータ２１Ａ〜２１Ｅに電気的に接続され、駆動指令に基づいて対応するモータ２１Ａ〜２１Ｅに駆動エネルギーである駆動電流を供給する。また、それぞれのサーボ制御部３２Ａ〜３２Ｅには、対応するエンコーダ２２Ａ〜２２Ｅが電気的に接続されている。これにより、それぞれのサーボ制御部３２Ａ〜３２Ｅには、対応するモータ２１Ａ〜２１Ｅの駆動状態（駆動位置）がフィードバックされる。
【００２３】
図４は、サーボ制御部３２Ａの構成を示す図である。図５は、サーボ制御部３２Ａ、モータ２１Ａ及びエンコーダ２２Ａでの処理を説明する図である。なお、以下の説明では、ある１つのサーボ制御部３２Ａ、及び、そのサーボ制御部３２Ａに対応するモータ２１Ａ及びエンコーダ２２Ａについてのみ説明するが、サーボ制御部３２Ｂ〜３２Ｅ、モータ２１Ｂ〜２１Ｅ及びエンコーダ２２Ｂ〜２２Ｅについても、サーボ制御部３２Ａ、モータ２１Ａ及びエンコーダ２２Ａと構成及び処理は、同様である。
【００２４】
図４に示すように、サーボ制御部３２Ａは、モータ２１Ａの駆動位置を制御する駆動位置制御部３５と、モータ２１Ａの駆動速度を制御する駆動速度制御部３６と、を備える。また、サーボ制御部３２Ａは、微分実施部３７と、移動規制判定部３９と、を備える。なお、本実施形態では、サーボ制御部３２Ａに移動規制判定部３９が設けられているが、移動規制判定部３９がサーボ制御部３２Ａとは別に設けられていてもよい。
【００２５】
図５に示すように、駆動指令生成部３１からのモータ２１Ａの駆動指令は、駆動位置制御部３５に入力される。駆動位置制御部３５は、駆動指令に含まれるモータ２１Ａの駆動位置データに基づいて、モータ２１Ａの駆動位置の制御を行う（ステップＳ１０１）。そして、モータ２１Ａの駆動指令は、駆動速度制御部３６に入力される。駆動速度制御部３６は、駆動指令に含まれるモータ２１Ａの駆動速度データに基づいて、モータ２１Ａの駆動速度の制御を行う（ステップＳ１０２）。駆動位置制御部３５でのモータ２１Ａの駆動位置の制御、及び、駆動速度制御部３６でのモータ２１Ａの駆動速度の制御に基づいて、駆動電流がモータ２１Ａに供給される。
【００２６】
モータ２１Ａに駆動電流が供給されることにより、モータ２１Ａが駆動される（ステップＳ１０３）。これにより、関節部１５Ａが作動される。この際、関節部１５Ａの移動が規制された場合、関節部１５Ａの負荷が大きくなり、モータ２１Ａの駆動に影響を及ぼす。マニピュレータ２の使用において、内視鏡の処置具挿通路の内壁、挿入される管状部の内壁等によって、特に長手軸Ｃに垂直な方向について関節部１５Ａの移動が規制されることがある。この場合、関節部１５Ａの負荷が大きくなり、負荷が外乱としてモータ２１Ａの駆動に作用する。負荷の外乱は、モータ２１Ａの駆動加速度に直接的に影響を及ぼす。したがって、関節部１５Ａの移動が規制されることにより、モータ２１Ａの駆動指令と実際のモータ２１Ａの駆動状態との相関性が低くなる。
【００２７】
実際のモータ２１Ａの駆動位置（駆動状態）は、エンコーダ２２Ａにより検出される（ステップＳ１０４）。検出されたモータ２１Ａの駆動位置情報は、駆動位置制御部３５でのモータ２１Ａの駆動位置制御（ステップＳ１０１）において、フィードバックされる。また、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置情報は、微分実施部３７に入力される。そして、微分実施部でモータ２１Ａの駆動位置情報が微分され（ステップＳ１０５）、実際のモータ２１Ａの駆動速度が検出される。検出されたモータ２１Ａの駆動速度情報は、駆動速度制御部３６でのモータ２１Ａの駆動速度制御（ステップＳ１０２）において、フィードバックされる。
【００２８】
移動規制判定部３９には、駆動速度制御部３６からモータ２１Ａの駆動電流が入力される。また、移動規制判定部３９には、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置（駆動状態）情報が入力される。移動規制判定部３９は、モータ２１Ａの駆動電流及びエンコーダ２２Ａで検出したモータ２１Ａの駆動状態（駆動位置）に基づいて、関節部１５Ａが移動自在な非規制状態であるか、又は、関節部１５Ａの移動が規制される規制状態であるか、を判定する（ステップＳ１０６）。非規制状態では、例えば長手軸Ｃに垂直な方向へ関節部１５Ａが移動自在である。また、規制状態では、特に長手軸Ｃに垂直な方向への関節部１５Ａの移動が規制される。移動規制判定部３９では、モータ２１Ａの駆動電流から駆動速度制御部３６での駆動速度制御に基づくモータ２１Ａの駆動加速度が算出される。また、エンコーダ２２Ａで検出したモータ２１Ａの駆動位置を２回微分することにより、モータ２１Ａの駆動状態に基づくモータ２１Ａの駆動加速度を算出する。そして、駆動速度制御に基づく駆動加速度と駆動状態に基づく駆動加速度とを比較することにより、モータ２１Ａの駆動に作用する負荷の外乱を検出する。負荷の外乱が所定の閾値以上であれば、関節部１５Ａの（特に長手軸Ｃに垂直な方向への）移動が規制される規制状態と、判定される。一方、負荷の外乱が所定の閾値未満であれば、関節部１５Ａが（長手軸Ｃに垂直な方向へ）移動自在な非規制状態と、判定される。
【００２９】
なお、その他のサーボ制御部３２Ｂ〜３２Ｅのそれぞれにおいても、サーボ制御部３２Ａと同様に、移動規制判定部３９により、対応する関節部１５Ｂ〜１５Ｅが規制状態であるか、又は、対応する関節部１５Ｂ〜１５Ｅが非規制状態であるかが、判定される。
【００３０】
図２に示すように、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９は、規制先端特定部４１に電気的に接続されている。なお、本実施形態では、駆動制御部２６に規制先端特定部４１が設けられているが、規制先端特定部４１が駆動制御部２６とは別に設けられていてもよい。
【００３１】
図６は、規制先端特定部での処理及び第２の制御モードでの関節位置姿勢算出部２８及び関節作動状態算出部２９の処理を説明する図である。図６に示すように、マニピュレータ２の挿入部１０が内視鏡５０の処置具挿通路５１に挿通された状態で、マニピュレータ２を使用することがある。この場合、関節部１５Ａ〜１５Ｅの位置によっては、１つ以上の関節部１５Ａ〜１５Ｅの特に長手軸Ｃに垂直な方向への移動が、処置具挿通路５１の内壁により規制される。例えば、最も基端方向側の関節部１５Ａ及び２番目に基端方向側の関節部１５Ｂの移動が規制される場合を、考える。
【００３２】
この場合、第1の制御モードでそれぞれのモータ２１Ａ〜２１Ｅを駆動制御すると、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９では、関節部１５Ａ，１５Ｂが規制状態と判定され、関節部１５Ｃ〜１５Ｅが非規制状態と判定される。すなわち、第1の制御モードにおいて、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９により、１つ以上の関節部１５Ａ，１５Ｂが規制状態と判定される。規制先端特定部４１は、それぞれの移動規制判定部３９での判定結果に基づいて、規制状態と判定された関節部１５Ａ，１５Ｂの中で最も先端方向側に位置する最先端規制関節を特定する。図６では、関節部１５Ｂが、最先端規制関節として特定される。
【００３３】
規制先端特定部４１は、関節位置姿勢算出部２８に電気的に接続されている。それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９で１つ以上の関節部１５Ａ，１５Ｂが規制状態と判定された場合、駆動制御部２６は第２の制御モードでモータ２１Ａ〜２１Ｅを駆動制御する。第２の制御モードにおいて、関節位置姿勢算出部２８は、それぞれの移動規制判定部３９での判定結果及び規制先端特定部４１での特定結果に基づいて、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。この際、最先端規制関節である関節部１５Ｂ及び最先端規制関節より基端方向側の関節部１５Ａを現在の関節位置及び関節姿勢から変化させることなく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。すなわち、規制状態と判定された関節部１５Ａ，１５Ｂを現在の関節位置及び関節姿勢から変化させることなく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。
【００３４】
ここで、関節位置姿勢算出部２８での算出において、第1の制御モードと同様に、メス部１９が目標位置及び目標姿勢となればよく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢は、一意的に決定される必要はない。例えば、メス部１９が目標位置及び目標姿勢となれば、それぞれの関節部１５Ａ〜１５Ｅを、図６の実線で示す関節位置及び関節姿勢に決定してもよく、図６の点線で示す関節位置及び関節姿勢に決定してもよい。
【００３５】
そして、前述のように一般的に用いられるDenavit-Hartenberg法に基づく座標変換を用いて、第１の制御モードと同様にして、関節作動状態算出部２９が、算出された関節位置及び関節姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの屈曲角度等の作動状態を算出する。ここで、図６の実線で示す関節位置及び関節姿勢に関節位置姿勢算出部２８により決定された場合を考える。そして、座標系Ｓ０，Ｔ１〜Ｔ５を規定する。座標系Ｔ１〜Ｔ５の定義については、第1の制御モードで前述した座標系Ｓ１〜Ｓ５の定義と同様である。
【００３６】
座標系Ｔｋ（ｋ＝１，２，３，４，５）から座標系Ｔｋ−１への変換行列をＪ（ｋ−１←ｋ）とし、座標系Ｔ３から座標系Ｔ０への変換行列をＪ（０←３）とする。この場合、座標系Ｓ０でのメス部１９（マニピュレータ２の先端）の位置姿勢ベクトルＵ０は、それぞれの座標系Ｔｋでのメス部１９の位置姿勢ベクトルＶｋを用いて、  
【数３】
2015107138.tif 000005
となる。変換行列Ｊ（０←３）に基づいて、関節位置姿勢算出部２８で算出された関節部１５Ｃの関節位置及び関節姿勢にする関節部１５Ｃの作動状態が算出される。変換行列Ｊ（３←４）に基づいて、関節位置姿勢算出部２８で算出された関節部１５Ｄの関節位置及び関節姿勢にする関節部１５Ｄの作動状態が算出される。変換行列Ｊ（４←５）に基づいて、関節位置姿勢算出部２８で算出された関節部１５Ｅの関節位置及び関節姿勢にする関節部１５Ｅの作動状態が算出される。また、第２の制御モードでは規制状態と判定された関節部１５Ａ，１５Ｂは現在の関節位置及び関節姿勢から変化されないため、関節部１５Ａ，１５Ｂは作動されない。関節部１５Ａ，１５Ｂが作動されないため、先端腕部１２において関節部１５Ｃより基端方向側の部位は、操作指令に基づく動作を行わない。以上のように、規制状態と判断された関節部１５Ａ，１５Ｂを現在の関節位置及び関節姿勢から変化させることなく、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出され、算出された関節位置及び関節姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの作動状態が算出される。
【００３７】
図６の点線で示す関節位置及び関節姿勢に関節位置姿勢算出部２８により決定された場合は、座標系Ｔ１〜Ｔ５の代わりに座標系Ｔ´１〜Ｔ´５が規定される。そして、変換行列Ｊ（ｋ−１←ｋ），Ｊ（０←３）の代わりに変換行列Ｊ´（ｋ−１←ｋ），Ｊ（０←３）が用いられる。この場合、座標系Ｓ０でのメス部１９（マニピュレータ２の先端）の位置姿勢ベクトルＵ０は、それぞれの座標系Ｔ´ｋでのメス部１９の位置姿勢ベクトルＶ´ｋを用いて、  
【数４】
2015107138.tif 000006
となる。
【００３８】
そして、駆動指令生成部３１が、関節作動状態算出部２９で算出されたそれぞれの関節部１５Ａ〜１５Ｅの作動状態に基づいて、それぞれのモータ２１Ａ〜２１Ｅの駆動指令を生成する。そして、それぞれのサーボ制御部３２Ａ〜３２Ｅが、対応するモータ２１Ａ〜２１Ｅの駆動指令に基づいて、対応するモータ２１Ａ〜２１Ｅに駆動電流を供給する。ここで、第２の制御モードでは、規制状態と判定された関節部１５Ａ，１５Ｂは作動されない。このため、駆動指令生成部３１では、関節部１５Ａ，１５Ｂに対応するモータ２１Ａ，２１Ｂの駆動指令は生成されない。したがって、モータ２１Ａ，２１Ｂに駆動電流は供給されず、モータ２１Ａ，２１Ｂは駆動されない。以上のようにして、第１の制御モードにおいて１つ以上の関節部１５Ａ，１５Ｂが規制状態と判定された場合に、規制状態と判定された関節部１５Ａ，１５Ｂに対応するモータ２１Ａ，２１Ｂを駆動しない第２の制御モードで、駆動制御部２６はそれぞれのモータ２１Ａ〜２１Ｅを駆動制御する。
【００３９】
図２に示すように、制御ユニット３は、警告発生部４２を備える。警告発生部４２は、関節位置姿勢算出部２８に電気的に接続されている。警告発生部４２は、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、警告情報を発生する。また、制御ユニット３には、ランプ、ディスプレイ等の警告表示部４３が設けられている。警告発生部４２で警告情報が発生することにより、警告表示部４３に警告が表示される。
【００４０】
そこで、前記構成のマニピュレータ２の動作制御装置１では、以下の効果を奏する。すなわち、動作制御装置１では、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９が、第1の制御モードでの対応するモータ２１Ａ〜２１Ｅの駆動状態に基づいて、対応する関節部１５Ａ〜１５Ｅが非規制状態であるか、又は、規制状態であるか、を判定する。それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９で１つ以上の関節部（１５Ａ，１５Ｂ）が規制状態と判定された場合、駆動制御部２６は第２の制御モードでモータ２１Ａ〜２１Ｅを駆動制御する。第２の制御モードでは、関節位置姿勢算出部２８は、規制状態と判定された関節部（１５Ａ，１５Ｂ）を現在の関節位置及び関節姿勢から変化させることなく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。このため、第２の制御モードでは、規制状態と判定された関節部（１５Ａ，１５Ｂ）は作動されず、規制状態の関節部（１５Ａ，１５Ｂ）に対応するモータ（２１Ａ，２１Ｂ）の駆動指令は生成されない。したがって、（長手軸Ｃに垂直な方向への）移動が規制された関節部（１５Ａ，１５Ｂ）の作動に起因して生じるマニピュレータ２の不具合を、有効に防止することができる。
【００４１】
また、第２の制御モードでは、関節位置姿勢算出部２８が、規制状態と判定された関節部（１５Ａ，１５Ｂ）を現在の関節位置及び関節姿勢から変化させることなく、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。そして、規制状態の関節部（１５Ａ，１５Ｂ）を作動することなく、関節作動状態算出部２９が、関節位置姿勢算出部２８での算出結果に基づいて、それぞれの関節部１５Ａ〜１５Ｅの作動状態を算出する。そして、駆動指令生成部３１が、関節作動状態算出部２９での算出結果に基づいて、それぞれのモータ２１Ａ〜２１Ｅの駆動指令を生成する。したがって、第２の制御モードでは、規制状態の関節部（１５Ａ，１５Ｂ）を作動することなく、先端機能部であるメス部１９を目標位置及び目標姿勢にすることができきる。
【００４２】
（第1の実施形態の変形例）  
なお、第1の実施形態では、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、警告発生部４２が警告情報を発生するが、これに限るものではない。例えば、ある変形例として、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、関節位置姿勢算出部２８が、先端機能部であるメス部１９の先端姿勢（姿勢）を一意的に決定することなく、メス部１９を目標位置にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出してもよい。本変形例でも、規制状態の関節部（例えば１５Ａ，１５Ｂ）を現在の関節位置及び関節姿勢から変化させることなく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出される。
【００４３】
図７は、本変形例の関節位置姿勢算出部２８での処理を説明する図である。図７に示すように、例えば、メス部１９を目標位置及び目標姿勢にすることにより、メス部１９（マニピュレータの先端）が位置姿勢ベクトルＷを有する場合を考える。ここで、関節部１５Ａ，１５Ｂが規制状態で、かつ、関節部１５Ｃ〜１５Ｅが非規制状態ならば、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出可能である。すなわち、図７の実線で示すように、関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出される。
【００４４】
しかし、関節部１５Ａ〜１５Ｃが規制状態で、かつ、関節部１５Ｄ，１５Ｅが非規制状態ならば、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出不可能となる。この場合、メス部１９の先端姿勢（姿勢）を目標姿勢に一意的に決定せず、メス部１９を目標位置にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。すなわち、メス部１９が目標位置となれば、メス部１９の先端姿勢は目標姿勢と異なってもよい。したがって、図７の点線で示すように、関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出される。メス部１９の先端姿勢が目標姿勢とは異なるため、メス部１９は、位置姿勢ベクトルＷとは異なる位置姿勢ベクトルＷ´を有する。この際、規制状態の関節部（１５Ａ〜１５Ｃ）は、現在の関節位置及び関節姿勢から変化させない。メス部１９（先端機能部）の先端姿勢（姿勢）を目標姿勢に一意的に決定せず、メス部１９（先端機能部）を目標位置にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する方法については、特開２００７−２９２８８号公報で（式４）を用いて詳細に説明されている。
【００４５】
また、本変形例では、メス部１９の先端姿勢を目標姿勢に一意的に決定しない場合においても、メス部１９を目標位置にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出不可能ならば、警告発生部４２が警告情報を発生する。
【００４６】
また、別のある変形例として、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、駆動指定生成部３２が全てのモータ２１Ａ〜２１Ｅについて駆動指令を生成しなくてもよい。本変形例では、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、全てのモータ２１Ａ〜２１Ｅに駆動電流が供給されず、全ての関節部１５Ａ〜１５Ｅの作動が停止する。
【００４７】
（第２の実施形態）  
次に、本発明の第２の実施形態について、図８乃至図１０を参照して説明する。なお、第１の実施形態と同一の部分及び同一の機能を有する部分については同一の符号を付して、その説明は省略する。
【００４８】
図８は、関節部１５Ａ〜１５Ｅを作動する構成を示す図である。図８に示すように、本実施形態のマニピュレータ２は、第1の実施形態と同様に、モータ２１Ａ〜２１Ｅ及びエンコーダ２２Ａ〜２２Ｅを備える。また、制御ユニット３の駆動制御部２６には、第1の実施形態と同様に、関節位置姿勢算出部２８、関節作動状態算出部２９、駆動指令生成部３１及びサーボ制御部３２Ａ〜３２Ｅが設けられている。そして、それぞれのサーボ制御部３２Ａ〜３２Ｅには、移動規制判定部３９が設けられている。ただし、本実施形態では第1の実施形態と異なり、規制先端特定部４１は設けられていない。
【００４９】
図９は、サーボ制御部３２Ａの構成を示す図である。図１０は、サーボ制御部３２Ａ、モータ２１Ａ及びエンコーダ２２Ａでの処理を説明する図である。なお、以下の説明では、ある１つのサーボ制御部３２Ａ、及び、そのサーボ制御部３２Ａに対応するモータ２１Ａ及びエンコーダ２２Ａについてのみ説明するが、サーボ制御部３２Ｂ〜３２Ｅ、モータ２１Ｂ〜２１Ｅ及びエンコーダ２２Ｂ〜２２Ｅについても、サーボ制御部３２Ａ、モータ２１Ａ及びエンコーダ２２Ａと構成及び処理は、同様である。
【００５０】
図９に示すように、サーボ制御部３２Ａは、第1の実施形態と同様に、駆動位置制御部３５と、駆動速度制御部３６と、微分実施部３７と、移動規制判定部３９と、を備える。また、本実施形態では、サーボ制御部３２Ａは、サーボゲイン変更部６１をさらに備える。
なお、本実施形態では、サーボ制御部３２Ａに移動規制判定部３９及びサーボゲイン変更部６１が設けられているが、移動規制判定部３９及びサーボゲイン変更部６１がサーボ制御部３２Ａとは別に設けられていてもよい。
【００５１】
図１０に示すように、駆動位置制御部３５は、第1の実施形態と同様に、駆動指令に含まれるモータ２１Ａの駆動位置データに基づいて、モータ２１Ａの駆動位置の制御を行う（ステップＳ１０１）。そして、駆動速度制御部３６は、駆動指令に含まれるモータ２１Ａの駆動速度データに基づいて、モータ２１Ａの駆動速度の制御を行う（ステップＳ１０２）。駆動位置制御部３５でのモータ２１Ａの駆動位置の制御、及び、駆動速度制御部３６でのモータ２１Ａの駆動速度の制御に基づいて、駆動エネルギーである駆動電流がモータ２１Ａに供給される。
【００５２】
モータ２１Ａに駆動電流が供給されることにより、モータ２１Ａが駆動される（ステップＳ１０３）。これにより、関節部１５Ａが作動される。この際、特に長手軸Ｃに垂直な方向への関節部１５Ａの移動が規制された場合、関節部１５Ａの負荷が大きくなり、負荷が外乱としてモータ２１Ａの駆動に作用する。
【００５３】
実際のモータ２１Ａの駆動位置（駆動状態）は、エンコーダ２２Ａにより検出される（ステップＳ１０４）。検出されたモータ２１Ａの駆動位置情報は、駆動位置制御部３５でのモータ２１Ａの駆動位置制御（ステップＳ１０１）において、フィードバックされる。また、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置情報は、微分実施部３７で微分され（ステップＳ１０５）、実際のモータ２１Ａの駆動速度が検出される。検出されたモータ２１Ａの駆動速度情報は、駆動速度制御部３６でのモータ２１Ａの駆動速度制御（ステップＳ１０２）において、フィードバックされる。
【００５４】
移動規制判定部３９には、駆動速度制御部３６からモータ２１Ａの駆動電流が入力される。また、移動規制判定部３９には、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置（駆動状態）情報が入力される。移動規制判定部３９は、モータ２１Ａの駆動電流及びエンコーダ２２Ａで検出したモータ２１Ａの駆動状態（駆動位置）に基づいて、関節部１５Ａが移動自在な非規制状態であるか、又は、関節部１５Ａの移動が規制される規制状態であるか、を判定する（ステップＳ１０６）。移動規制判定部３９は、第１の実施形態と同様にして、判定を行う。なお、その他のサーボ制御部３２Ｂ〜３２Ｅのそれぞれにおいても、サーボ制御部３２Ａと同様に、移動規制判定部３９により、対応する関節部１５Ｂ〜１５Ｅが規制状態であるか、又は、対応する関節部１５Ｂ〜１５Ｅが非規制状態であるかが、判定される。
【００５５】
ここで、第1の制御モードにおいて1つ以上の関節部（１５Ａ，１５Ｂ）が規制状態と判定された場合には、駆動制御部２６は第２の制御モードでそれぞれのモータ２１Ａ〜２１Ｅを駆動制御する。ここで、関節部１５Ａ，１５Ｂが規制状態であり、関節部１５Ｃ〜１５Ｅが非規制状態である場合を考える。この場合、第1の制御モードにおいて対応する関節部１５Ａが規制状態と判定されるため、サーボ制御部３２Ａのサーボゲイン変更部６１は、第２の制御モードでのサーボ制御部３２Ａの駆動指令に対する駆動電流のサーボゲインＧａ２を、第1の制御モードでのサーボ制御部３２ＡのサーボゲインＧａ１より小さくする。これにより、対応するモータ２１Ａの駆動速度が第1の制御モードより小さくなり、対応する関節部１５Ａの作動速度が第1の制御モードより小さくなる。また、サーボ制御部３２Ｂのサーボゲイン変更部６１も、第2の制御モードでのサーボ制御部３２ＢのサーボゲインＧｂ２を、第1の制御モードでのサーボ制御部３２ＢのサーボゲインＧｂ１より小さくする。
【００５６】
また、第1の制御モードにおいて対応する関節部１５Ｃが非規制状態と判定されるため、サーボ制御部３２Ｃのサーボゲイン変更部６１は、第1の制御モードと第２の制御モードとの間で、サーボ制御部３２Ｃの駆動指令に対する駆動電流のサーボゲインＧｃを変更しない。サーボ制御部３２Ｄ，３２ＥのサーボゲインＧｄ，Ｇｅについても、サーボゲインＧｃと同様である。
【００５７】
以上のようにして、それぞれのサーボ制御部３２Ａ〜３２Ｅのサーボゲイン変更部６１は、対応する移動規制判定部３９での判定結果に基づいて、対応するサーボ制御部３２Ａ〜３２ＥでのサーボゲインＧａ〜Ｇｅを設定する（ステップＳ１０７）。例えば、サーボ制御部３２Ａのサーボゲイン変更部６１は、判定結果に基づいて、サーボ制御部３２ＡでのサーボゲインＧａを設定する。
【００５８】
そこで、前記構成のマニピュレータ２の動作制御装置１では、以下の効果を奏する。すなわち、動作制御装置１では、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９が、第1の制御モードでの対応するモータ２１Ａ〜２１Ｅの駆動状態に基づいて、対応する関節部１５Ａ〜１５Ｅが非規制状態であるか、又は、規制状態であるか、を判定する。それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９で１つ以上の関節部（１５Ａ，１５Ｂ）が規制状態と判定された場合、駆動制御部２６は第２の制御モードでモータ２１Ａ〜２１Ｅを駆動制御する。規制状態の関節部（１５Ａ，１５Ｂ）に対応するサーボ制御部（３２Ａ，３２Ｂ）では、サーボゲイン変更部６１によって、第２の制御モードでのサーボゲイン（Ｇａ２，Ｇｂ２）が、第1の制御モードでのサーボゲイン（Ｇａ１，Ｇｂ１）に比べて小さくなる。これにより、第２の制御モードでは、対応するモータ（２１Ａ，２１Ｂ）の駆動速度が第1の制御モードより小さくなり、対応する関節部（１５Ａ，１５Ｂ）の作動速度が第1の制御モードより小さくなる。したがって、（長手軸Ｃに垂直な方向への）移動が規制された関節部（１５Ａ，１５Ｂ）の作動に起因して生じるマニピュレータ２の不具合を、有効に防止することができる。
【００５９】
ただし、本実施形態では、第２の制御モードにおいて、規制状態の関節部（１５Ａ，１５Ｂ）に対応するサーボ制御部（３２Ａ，３２Ｂ）のサーボゲイン（Ｇａ，Ｇｂ）を、第１の制御モードに比べて小さくする構成である。このため、第２の制御モードでは、メス部１９が目標位置及び目標姿勢にならない。
【００６０】
（その他の変形例）  
なお、前述の実施形態では、５つの関節部１５Ａ〜１５Ｅが設けられているが、これに限るものではない。すなわち、複数の関節部（１５Ａ〜１５Ｅ）が設けられていればよく、例えば、関節部（１５Ａ〜１５Ｅ）の数は２つ、３つ、４つでもよく、６つ以上でもよい。そして、それぞれの関節部（１５Ａ〜１５Ｃ）に対応して、駆動部材であるモータ（２１Ａ〜２１Ｅ）及びサーボ制御部（３２Ａ〜３２Ｃ）が設けられていればよい。
【００６１】
また、前述の実施形態では先端機能部としてメス部１９が設けられているが、これに限るものではない。例えば、先端処置部（先端機能部）として処置対象を把持して処置を行う把持処置部が設けられてもよい。また、前述の実施形態では、駆動エネルギーとして駆動電流が供給されるモータ２１Ａ〜２１Ｅが駆動部材として設けられているが、これに限るものではない。すなわち、駆動エネルギーが供給されることにより駆動される駆動部材が設けられていればよい。
【００６２】
また、前述の実施形態では、挿入機器として内視鏡５０の処置具挿通路５１に挿通されるマニピュレータ２が用いられているが、これに限るものではない。例えば、ある変形例として図１１に示すように、工業用の管路７１に挿通される内視鏡７０を挿入機器として用いてもよい。本変形例でも第１の実施形態と同様に、内視鏡７０の挿入部１０の先端部に先端腕部１２が設けられ、先端腕部１２は、関節部１５Ａ〜１５Ｅ及びリンク部１７Ａ〜１７Ｅを備える。そして、リンク部１７Ｅの内部に先端機能部である撮像素子７２が設けられている。以上、本変形例から、先端腕部１２では、最も先端方向側に位置する関節部（１５Ｅ）の先端方向側に先端機能部（１９，７２）が設けられていればよい。
【００６３】
また、第1の実施形態の構成及び第２の実施形態の構成を組合わせてもよい。この場合、規制先端特定部４１が設けられるとともに、それぞれのサーボ制御部３２Ａ〜３２Ｅに対応してサーボゲイン変更部６１が設けられる。
【００６４】
以上、本発明の実施形態について説明したが、本発明は前記の実施形態に限定されるものではなく、本発明の要旨を逸脱しない範囲で種々の変形ができることは勿論である。
【符号の説明】
【００６５】
１…動作制御装置、２…マニピュレータ、３…制御ユニット、５…操作指令入力部、１０…挿入部、１２…先端腕部、１５Ａ〜１５Ｅ…関節部、１９…メス部、２１Ａ〜２１Ｃ…モータ、２６…駆動制御部、３９…移動規制判定部。
【図１】
2015107138.tif 000007
【図２】
2015107138.tif 000008
【図３】
2015107138.tif 000009
【図４】
2015107138.tif 000010
【図５】
2015107138.tif 000011
【図６】
2015107138.tif 000012
【図７】
2015107138.tif 000013
【図８】
2015107138.tif 000014
【図９】
2015107138.tif 000015
【図１０】
2015107138.tif 000016
【図１１】
2015107138.tif 000017