JP2015107138A - Operation control device for insertion instrument - Google Patents
Operation control device for insertion instrument Download PDFInfo
- Publication number
- JP2015107138A JP2015107138A JP2012057577A JP2012057577A JP2015107138A JP 2015107138 A JP2015107138 A JP 2015107138A JP 2012057577 A JP2012057577 A JP 2012057577A JP 2012057577 A JP2012057577 A JP 2012057577A JP 2015107138 A JP2015107138 A JP 2015107138A
- Authority
- JP
- Japan
- Prior art keywords
- joint
- unit
- posture
- drive
- state
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/30—Surgical robots
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/30—Surgical robots
- A61B2034/302—Surgical robots specifically adapted for manipulations within body cavities, e.g. within abdominal or thoracic cavities
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B90/00—Instruments, implements or accessories specially adapted for surgery or diagnosis and not covered by any of the groups A61B1/00 - A61B50/00, e.g. for luxation treatment or for protecting wound edges
- A61B90/50—Supports for surgical instruments, e.g. articulated arms
- A61B2090/508—Supports for surgical instruments, e.g. articulated arms with releasable brake mechanisms
Abstract
Description
本発明は、体腔内、工業用の管路内等に挿入される挿入部を備えるマニピュレータ、内視鏡等の挿入機器の動作を制御する挿入機器の動作制御装置に関する。 The present invention relates to an operation control device for an insertion device that controls the operation of an insertion device such as a manipulator or an endoscope having an insertion portion that is inserted into a body cavity, an industrial conduit, or the like.
特許文献１には、内視鏡の処置具挿通路に挿通されるマニピュレータが開示されている。マニピュレータは、体腔内に挿入される挿入部を備える挿入機器である。マニピュレータの挿入部の先端部には、先端腕部が設けられている。先端腕部は、複数の関節部と、それぞれが対応する関節部の先端方向側に設けられるリンク部と、最も先端方向側のリンク部の先端方向側に位置する先端処置部（先端機能部）と、を備える。それぞれの関節部が作動されることにより、先端腕部において作動された関節部を中心として作動された関節部より先端方向側の部位が回動し、先端腕部が動作を行う。マニピュレータの先端腕部の動作を制御する動作制御装置は、それぞれの関節部の作動（駆動）を制御する作動制御部（駆動制御部）を備える。また、マニピュレータの動作制御装置は、先端処置部の目標位置及び目標姿勢を示す操作指令（軌道指令）が入力される操作指令入力部（軌道入力部）を備える。そして、軌道設定部が、先端処置部が目標位置及び目標姿勢となる状態に、作動する関節部の数及び作動する関節部の作動角度を設定する。作動制御部は、軌道設定部で設定された作動する関節部の数及び作動する関節部の作動角度に基づいて、それぞれの関節部の作動状態を制御している。
また、マニピュレータ装置は、それぞれの関節部の負荷量を検出する負荷検出部を備える。ここで、先端腕部を動作させることによって、ある関節部での負荷量が負荷許容量以上となることがある。そこで、軌道設定部は、負荷検出部での検出結果に基づいて、全ての関節部の負荷量が許容負荷量未満となる状態に、作動する関節部の数及び作動する関節部の作動角度を設定している。これにより、全ての関節部の負荷量が許容負荷量未満となる状態で、先端処置部を目標位置及び目標姿勢にすることが可能となる。 Further, the manipulator device includes a load detection unit that detects a load amount of each joint unit. Here, by operating the distal arm portion, the load amount at a certain joint portion may be more than the allowable load amount. Therefore, the trajectory setting unit sets the number of operating joints and the operating angle of the operating joints to be in a state where the load amount of all the joints is less than the allowable load amount based on the detection result in the load detection unit. It is set. As a result, the distal treatment section can be set to the target position and the target posture in a state where the load amount of all the joint portions is less than the allowable load amount.
前記特許文献１のように内視鏡の処置具挿通路にマニピュレータを挿通して使用する場合、ある関節部が処置具挿通路の内部に位置することがある。この際、処置具挿通路の内部に位置する関節部は、処置具挿通路を規定する内周面によって、特に長手軸に垂直な方向について、移動が規制される。この状態で移動が規制された関節部を作動した場合、処置具挿通路の内周面と先端腕部との衝突等が原因で、挿入機器であるマニピュレータに不具合が生じる可能性がある。
When using the manipulator through the treatment instrument insertion passage of the endoscope as in
本発明は前記課題に着目してなされたものであり、その目的とするところは、ある関節部の移動が規制された状態で、挿入機器に不具合が生じることなく、適切に先端腕部を動作させる挿入機器の動作制御装置を提供することにある。 The present invention has been made paying attention to the above-mentioned problems, and the object of the present invention is to appropriately operate the distal arm without causing any trouble in the insertion device in a state where movement of a certain joint is restricted. An object of the present invention is to provide an operation control device for an insertion device.
前記目的を達成するために、本発明のある態様の挿入機器の動作制御装置は、長手軸に沿って延設される挿入部を備え、前記挿入部は先端部に動作可能に設けられる先端腕部を備える挿入機器であって、前記先端腕部は、複数の関節部と、最も前記先端方向側に位置する前記関節部の前記先端方向側に設けられる先端機能部と、を備え、それぞれの前記関節部が作動されることにより前記先端腕部において作動された前記関節部より前記先端方向側の部位が動作する挿入機器と、前記先端機能部の目標位置及び目標姿勢を示す操作指令が入力される操作指令入力部と、駆動エネルギーが供給されることにより駆動され、駆動されることによりそれぞれが対応する前記関節部を作動させる複数の駆動部材と、対応する前記駆動部材の駆動状態に基づいて、対応する前記関節部が移動自在な非規制状態であるか、又は、対応する前記関節部の移動が規制される規制状態であるか、をそれぞれが判定する複数の移動規制判定部と、前記操作指令に基づいて第1の制御モードでそれぞれの前記駆動部材を駆動制御する駆動制御部であって、前記第１の制御モードにおいて１つ以上の前記関節部が前記規制状態と判定された場合に、前記規制状態と判定された前記関節部に対応する前記駆動部材を、駆動しない、又は、前記第1の制御モードより駆動速度が小さくする第２の制御モードで、それぞれの前記駆動部材を駆動制御する駆動制御部と、を備える。 In order to achieve the above object, an operation control device for an insertion device according to an aspect of the present invention includes an insertion portion extending along a longitudinal axis, and the insertion portion is provided at a distal end portion so as to be operable. The distal arm portion includes a plurality of joint portions, and a distal end functional portion provided on the distal direction side of the joint portion located closest to the distal direction side. An operation command indicating the target position and target posture of the distal end functional unit and an insertion device in which the distal end side portion of the joint is operated in the distal arm by operating the joint are input. An operation command input unit, a plurality of drive members that are driven by being supplied with drive energy, and that actuate the corresponding joint units when driven, and a drive state of the corresponding drive member Therefore, a plurality of movement restriction determination units that respectively determine whether the corresponding joint part is in a non-restricted state in which the corresponding joint part is movable or in a restricted state in which movement of the corresponding joint part is restricted. A drive control unit that drives and controls each of the drive members in the first control mode based on the operation command, wherein one or more of the joint units are determined to be in the restricted state in the first control mode. In the second control mode in which the driving member corresponding to the joint portion determined to be in the restricted state is not driven or the driving speed is lower than that in the first control mode. A drive control unit that drives and controls the member.
本発明によれば、ある関節部の移動が規制された状態で、挿入機器に不具合が生じることなく、適切に先端腕部を動作させる挿入機器の動作制御装置を提供することができる。 ADVANTAGE OF THE INVENTION According to this invention, the movement control apparatus of the insertion apparatus which operates a front-end | tip arm part appropriately can be provided, without the malfunction in an insertion apparatus in the state in which the movement of a certain joint part was controlled.
（第１の実施形態）
本発明の第１の実施形態について、図１乃至図６を参照して説明する。図１は、マニピュレータ２の動作制御装置１を示す図である。図１に示すように、マニピュレータ２の動作制御装置１は、挿入機器であるマニピュレータ２と、制御ユニット３と、３Ｄデジタライザー等の操作指令入力部５と、を備える。
(First embodiment)
A first embodiment of the present invention will be described with reference to FIGS. FIG. 1 is a diagram showing an
マニピュレータ２は、長手軸Ｃを有する。ここで、長手軸Ｃに平行な方向の一方を先端方向（図１の矢印Ｃ１の方向）とし、長手軸Ｃに平行な方向の他方を基端方向（図１の矢印Ｃ２の方向）とする。マニピュレータ２は、長手軸Ｃに沿って延設される細長い挿入部１０と、挿入部１０の基端方向側に設けられる保持部１３と、を備える。
The
挿入部１０は、長手軸Ｃに沿って延設される細長い管状部１１と、管状部１１の先端方向側に設けられる先端腕部１２と、を備える。先端腕部１２は、マニピュレータ２の先端部に位置し、動作可能に設けられている。保持部１３には、ユニバーサルコード７の一端が接続されている。ユニバーサルコード７の他端は、制御ユニット３に接続されている。また、制御ユニット３は、操作指令入力部５からの操作指令を例えば無線通信等により受信可能である。
The
先端腕部１２は、複数（本実施形態では５つ）の関節部１５Ａ〜１５Ｅと、複数（本実施形態では５つ）のリンク部１７Ａ〜１７Ｅと、を備える。それぞれのリンク部１７Ａ〜１７Ｅは、対応する関節部１５Ａ〜１５Ｅの先端方向側に延設されている。リンク部１７Ｅの先端方向側には、先端処置部（先端機能部）であるメス部１９が設けられている。すなわち、メス部１９は、先端腕部１２の先端部に位置し、最も先端方向側の関節部１５Ｅより先端方向側に位置している。それぞれの関節部１５Ａ〜１５Ｅが作動される（actuated）ことにより、先端腕部１２において作動された関節部（１５Ａ〜１５Ｅ）を中心として作動された関節部（１５Ａ〜１５Ｅ）より先端方向側の部位が回動する。すなわち、それぞれの関節部１５Ａ〜１５Ｅが作動されることにより、先端腕部１２において作動された関節部（１５Ａ〜１５Ｅ）より先端方向側の部位が動作する。また、メス部１９によって、生体組織等の処置対象の切開が行われ、処置対象が処置される。
The
図２は、関節部１５Ａ〜１５Ｅを作動する構成を示す図である。図２に示すように、マニピュレータ２の保持部１３には、駆動部材であるモータ２１Ａ〜２１Ｅと、エンコーダ２２Ａ〜２２Ｅとが設けられている。それぞれのエンコーダ２２Ａ〜２２Ｅは、対応するモータ２１Ａ〜２１Ｅの駆動状態（駆動位置）を検出する。モータ２１Ａ〜２１Ｅ及びエンコーダ２２Ａ〜２２Ｅは、先端腕部１２の基端より基端方向側に位置している。
FIG. 2 is a diagram illustrating a configuration for operating the
マニピュレータ２の内部では、線状部材であるワイヤ２３Ａ〜２３Ｅが長手軸Ｃに沿って延設されている。それぞれのワイヤ２３Ａ〜２３Ｅは、対応するモータ２１Ａ〜２１Ｅと先端腕部１２との間で、延設されている。それぞれのワイヤ２３Ａ〜２３Ｅの先端は、対応する関節部１５Ａ〜１５Ｅに接続されている。それぞれのワイヤ２３Ａ〜２３Ｅは、対応するモータ２１Ａ〜２１Ｅの駆動状態に対応して、長手軸Ｃに沿って移動する。それぞれのワイヤ２３Ａ〜２３Ｅが長手軸Ｃに沿って移動することにより、対応する関節部１５Ａ〜１５Ｅが作動される。これにより、先端腕部１２が動作を行う。なお、モータ２１Ａ〜２１Ｅを保持部１３に配置せず、駆動部材であるモータ２１Ａ〜２１Ｅを対応する関節部１５Ａ〜１５Ｅに設けてもよい。この場合、ワイヤ２３Ａ〜２３Ｅは設けられない。それぞれのモータ２１Ａ〜２１Ｅを駆動することにより、対応する関節部１５Ａ〜１５Ｅが作動され、先端腕部１２が動作を行う。
Inside the
図２に示すように、制御ユニット３は、操作指令入力部５での操作指令を無線通信により受信する指令受信部２５を備える。指令受信部２５は、制御ユニット３に設けられる駆動制御部２６に、電気的に接続されている。駆動制御部２６は、操作指令入力部５での操作指令に含まれるメス部１９（先端処置部）の目標位置データ及び目標姿勢データを検出する。すなわち、操作指令入力部５では、メス部１９の目標位置及び目標姿勢を示す操作指令が入力される。そして、操作指令に含まれるメス部１９の目標位置データ及び目標姿勢データに基づいて、それぞれのモータ２１Ａ〜２１Ｅの駆動制御を行う。
As shown in FIG. 2, the
図２に示すように、駆動制御部２６は、操作指令に基づいて、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する関節位置姿勢算出部２８と、関節位置姿勢算出部２８で算出された関節位置及び関節姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの作動状態を算出する関節作動状態算出部２９と、を備える。駆動制御部２６は、第1の制御モード及び第２の制御モードで、それぞれのモーと２１Ａ〜２１Ｅを駆動制御することが可能である。
As shown in FIG. 2, the
図３は、第1の制御モードでの関節位置姿勢算出部２８及び関節作動状態算出部２９の処理を説明する図である。図３に示すように、関節位置姿勢算出部２８は、第1の制御モードにおいて、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。ここで、関節位置姿勢算出部２８での算出において、メス部１９が目標位置及び目標姿勢となればよく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢は、一意的に決定される必要はない。例えば、メス部１９が目標位置及び目標姿勢となれば、それぞれの関節部１５Ａ〜１５Ｅを、図３の実線で示す関節位置及び関節姿勢に決定してもよく、図３の点線で示す関節位置及び関節姿勢に決定してもよい。
FIG. 3 is a diagram for explaining processing of the joint position and
そして、ロボットアームの運動学においてよく知られ、一般的に用いられるDenavit-Hartenberg法に基づく座標変換を用いて、関節作動状態算出部２９が、算出された関節位置及び関節姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの屈曲角度等の作動状態を算出する。ここで、図３の実線で示す関節位置及び関節姿勢に関節位置姿勢算出部２８により決定された場合を考える。そして、座標系Ｓ０〜Ｓ５を規定する。座標系Ｓ０は、管状部１１に原点を有し、管状部１１において長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ１は、関節部１５Ａを原点とし、リンク部１７Ａにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ２は、関節部１５Ｂを原点とし、リンク部１７Ｂにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ３は、関節部１５Ｃを原点とし、リンク部１７Ｃにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ４は、関節部１５Ｄを原点とし、リンク部１７Ｄにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。座標系Ｓ５は、関節部１５Ｅを原点とし、リンク部１７Ｅにおいて長手軸Ｃに平行な方向が１つの軸の軸方向と一致する。
Each joint operating
座標系Ｓｋ（ｋ＝１，２，３，４，５）から座標系Ｓｋ−１への変換行列をＨ（ｋ−１←ｋ）とする。この場合、座標系Ｓ０でのメス部１９（マニピュレータ２の先端）の位置姿勢ベクトルＵ０は、それぞれの座標系Ｓｋでのメス部１９の位置姿勢ベクトルＵｋを用いて、
図３の点線で示す関節位置及び関節姿勢に関節位置姿勢算出部２８により決定された場合は、座標系Ｓ１〜Ｓ５の代わりに座標系Ｓ´１〜Ｓ´５が規定される。そして、変換行列Ｈ（ｋ−１←ｋ）の代わりに変換行列Ｈ´（ｋ−１←ｋ）が用いられる。この場合、座標系Ｓ０でのメス部１９（マニピュレータ２の先端）の位置姿勢ベクトルＵ０は、それぞれの座標系Ｓ´ｋでのメス部１９の位置姿勢ベクトルＵ´ｋを用いて、
駆動制御部２６は、関節作動状態算出部２９で算出されたそれぞれの関節部１５Ａ〜１５Ｅの作動状態に基づいて、それぞれのモータ２１Ａ〜２１Ｅの駆動指令を生成する駆動指令生成部３１を備える。また、駆動制御部２６は、関節部１５Ａ〜１５Ｅに対応させて複数のサーボ制御部３２Ａ〜３２Ｅを備える。駆動指令生成部３１は、サーボ制御部３２Ａ〜３２Ｅに電気的に接続されている。それぞれのサーボ制御部３２Ａ〜３２Ｅには、対応するモータ２１Ａ〜２１Ｅの駆動指令が、駆動指令生成部３１から入力される。それぞれのサーボ制御部３２Ａ〜３２Ｅは、対応するモータ２１Ａ〜２１Ｅに電気的に接続され、駆動指令に基づいて対応するモータ２１Ａ〜２１Ｅに駆動エネルギーである駆動電流を供給する。また、それぞれのサーボ制御部３２Ａ〜３２Ｅには、対応するエンコーダ２２Ａ〜２２Ｅが電気的に接続されている。これにより、それぞれのサーボ制御部３２Ａ〜３２Ｅには、対応するモータ２１Ａ〜２１Ｅの駆動状態（駆動位置）がフィードバックされる。
The
図４は、サーボ制御部３２Ａの構成を示す図である。図５は、サーボ制御部３２Ａ、モータ２１Ａ及びエンコーダ２２Ａでの処理を説明する図である。なお、以下の説明では、ある１つのサーボ制御部３２Ａ、及び、そのサーボ制御部３２Ａに対応するモータ２１Ａ及びエンコーダ２２Ａについてのみ説明するが、サーボ制御部３２Ｂ〜３２Ｅ、モータ２１Ｂ〜２１Ｅ及びエンコーダ２２Ｂ〜２２Ｅについても、サーボ制御部３２Ａ、モータ２１Ａ及びエンコーダ２２Ａと構成及び処理は、同様である。
FIG. 4 is a diagram illustrating a configuration of the
図４に示すように、サーボ制御部３２Ａは、モータ２１Ａの駆動位置を制御する駆動位置制御部３５と、モータ２１Ａの駆動速度を制御する駆動速度制御部３６と、を備える。また、サーボ制御部３２Ａは、微分実施部３７と、移動規制判定部３９と、を備える。なお、本実施形態では、サーボ制御部３２Ａに移動規制判定部３９が設けられているが、移動規制判定部３９がサーボ制御部３２Ａとは別に設けられていてもよい。
As shown in FIG. 4, the
図５に示すように、駆動指令生成部３１からのモータ２１Ａの駆動指令は、駆動位置制御部３５に入力される。駆動位置制御部３５は、駆動指令に含まれるモータ２１Ａの駆動位置データに基づいて、モータ２１Ａの駆動位置の制御を行う（ステップＳ１０１）。そして、モータ２１Ａの駆動指令は、駆動速度制御部３６に入力される。駆動速度制御部３６は、駆動指令に含まれるモータ２１Ａの駆動速度データに基づいて、モータ２１Ａの駆動速度の制御を行う（ステップＳ１０２）。駆動位置制御部３５でのモータ２１Ａの駆動位置の制御、及び、駆動速度制御部３６でのモータ２１Ａの駆動速度の制御に基づいて、駆動電流がモータ２１Ａに供給される。
As shown in FIG. 5, the drive command for the motor 21 </ b> A from the
モータ２１Ａに駆動電流が供給されることにより、モータ２１Ａが駆動される（ステップＳ１０３）。これにより、関節部１５Ａが作動される。この際、関節部１５Ａの移動が規制された場合、関節部１５Ａの負荷が大きくなり、モータ２１Ａの駆動に影響を及ぼす。マニピュレータ２の使用において、内視鏡の処置具挿通路の内壁、挿入される管状部の内壁等によって、特に長手軸Ｃに垂直な方向について関節部１５Ａの移動が規制されることがある。この場合、関節部１５Ａの負荷が大きくなり、負荷が外乱としてモータ２１Ａの駆動に作用する。負荷の外乱は、モータ２１Ａの駆動加速度に直接的に影響を及ぼす。したがって、関節部１５Ａの移動が規制されることにより、モータ２１Ａの駆動指令と実際のモータ２１Ａの駆動状態との相関性が低くなる。
By supplying a drive current to the
実際のモータ２１Ａの駆動位置（駆動状態）は、エンコーダ２２Ａにより検出される（ステップＳ１０４）。検出されたモータ２１Ａの駆動位置情報は、駆動位置制御部３５でのモータ２１Ａの駆動位置制御（ステップＳ１０１）において、フィードバックされる。また、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置情報は、微分実施部３７に入力される。そして、微分実施部でモータ２１Ａの駆動位置情報が微分され（ステップＳ１０５）、実際のモータ２１Ａの駆動速度が検出される。検出されたモータ２１Ａの駆動速度情報は、駆動速度制御部３６でのモータ２１Ａの駆動速度制御（ステップＳ１０２）において、フィードバックされる。
The actual driving position (driving state) of the
移動規制判定部３９には、駆動速度制御部３６からモータ２１Ａの駆動電流が入力される。また、移動規制判定部３９には、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置（駆動状態）情報が入力される。移動規制判定部３９は、モータ２１Ａの駆動電流及びエンコーダ２２Ａで検出したモータ２１Ａの駆動状態（駆動位置）に基づいて、関節部１５Ａが移動自在な非規制状態であるか、又は、関節部１５Ａの移動が規制される規制状態であるか、を判定する（ステップＳ１０６）。非規制状態では、例えば長手軸Ｃに垂直な方向へ関節部１５Ａが移動自在である。また、規制状態では、特に長手軸Ｃに垂直な方向への関節部１５Ａの移動が規制される。移動規制判定部３９では、モータ２１Ａの駆動電流から駆動速度制御部３６での駆動速度制御に基づくモータ２１Ａの駆動加速度が算出される。また、エンコーダ２２Ａで検出したモータ２１Ａの駆動位置を２回微分することにより、モータ２１Ａの駆動状態に基づくモータ２１Ａの駆動加速度を算出する。そして、駆動速度制御に基づく駆動加速度と駆動状態に基づく駆動加速度とを比較することにより、モータ２１Ａの駆動に作用する負荷の外乱を検出する。負荷の外乱が所定の閾値以上であれば、関節部１５Ａの（特に長手軸Ｃに垂直な方向への）移動が規制される規制状態と、判定される。一方、負荷の外乱が所定の閾値未満であれば、関節部１５Ａが（長手軸Ｃに垂直な方向へ）移動自在な非規制状態と、判定される。
The movement
なお、その他のサーボ制御部３２Ｂ〜３２Ｅのそれぞれにおいても、サーボ制御部３２Ａと同様に、移動規制判定部３９により、対応する関節部１５Ｂ〜１５Ｅが規制状態であるか、又は、対応する関節部１５Ｂ〜１５Ｅが非規制状態であるかが、判定される。
In each of the other
図２に示すように、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９は、規制先端特定部４１に電気的に接続されている。なお、本実施形態では、駆動制御部２６に規制先端特定部４１が設けられているが、規制先端特定部４１が駆動制御部２６とは別に設けられていてもよい。
As shown in FIG. 2, the movement
図６は、規制先端特定部での処理及び第２の制御モードでの関節位置姿勢算出部２８及び関節作動状態算出部２９の処理を説明する図である。図６に示すように、マニピュレータ２の挿入部１０が内視鏡５０の処置具挿通路５１に挿通された状態で、マニピュレータ２を使用することがある。この場合、関節部１５Ａ〜１５Ｅの位置によっては、１つ以上の関節部１５Ａ〜１５Ｅの特に長手軸Ｃに垂直な方向への移動が、処置具挿通路５１の内壁により規制される。例えば、最も基端方向側の関節部１５Ａ及び２番目に基端方向側の関節部１５Ｂの移動が規制される場合を、考える。
FIG. 6 is a diagram for explaining the processing in the restriction tip specifying unit and the processing of the joint position /
この場合、第1の制御モードでそれぞれのモータ２１Ａ〜２１Ｅを駆動制御すると、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９では、関節部１５Ａ，１５Ｂが規制状態と判定され、関節部１５Ｃ〜１５Ｅが非規制状態と判定される。すなわち、第1の制御モードにおいて、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９により、１つ以上の関節部１５Ａ，１５Ｂが規制状態と判定される。規制先端特定部４１は、それぞれの移動規制判定部３９での判定結果に基づいて、規制状態と判定された関節部１５Ａ，１５Ｂの中で最も先端方向側に位置する最先端規制関節を特定する。図６では、関節部１５Ｂが、最先端規制関節として特定される。
In this case, when the
規制先端特定部４１は、関節位置姿勢算出部２８に電気的に接続されている。それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９で１つ以上の関節部１５Ａ，１５Ｂが規制状態と判定された場合、駆動制御部２６は第２の制御モードでモータ２１Ａ〜２１Ｅを駆動制御する。第２の制御モードにおいて、関節位置姿勢算出部２８は、それぞれの移動規制判定部３９での判定結果及び規制先端特定部４１での特定結果に基づいて、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。この際、最先端規制関節である関節部１５Ｂ及び最先端規制関節より基端方向側の関節部１５Ａを現在の関節位置及び関節姿勢から変化させることなく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。すなわち、規制状態と判定された関節部１５Ａ，１５Ｂを現在の関節位置及び関節姿勢から変化させることなく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。
The restricting
ここで、関節位置姿勢算出部２８での算出において、第1の制御モードと同様に、メス部１９が目標位置及び目標姿勢となればよく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢は、一意的に決定される必要はない。例えば、メス部１９が目標位置及び目標姿勢となれば、それぞれの関節部１５Ａ〜１５Ｅを、図６の実線で示す関節位置及び関節姿勢に決定してもよく、図６の点線で示す関節位置及び関節姿勢に決定してもよい。
Here, in the calculation by the joint position /
そして、前述のように一般的に用いられるDenavit-Hartenberg法に基づく座標変換を用いて、第１の制御モードと同様にして、関節作動状態算出部２９が、算出された関節位置及び関節姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの屈曲角度等の作動状態を算出する。ここで、図６の実線で示す関節位置及び関節姿勢に関節位置姿勢算出部２８により決定された場合を考える。そして、座標系Ｓ０，Ｔ１〜Ｔ５を規定する。座標系Ｔ１〜Ｔ５の定義については、第1の制御モードで前述した座標系Ｓ１〜Ｓ５の定義と同様である。
Then, using the coordinate transformation based on the commonly used Denavit-Hartenberg method as described above, the joint operation
座標系Ｔｋ（ｋ＝１，２，３，４，５）から座標系Ｔｋ−１への変換行列をＪ（ｋ−１←ｋ）とし、座標系Ｔ３から座標系Ｔ０への変換行列をＪ（０←３）とする。この場合、座標系Ｓ０でのメス部１９（マニピュレータ２の先端）の位置姿勢ベクトルＵ０は、それぞれの座標系Ｔｋでのメス部１９の位置姿勢ベクトルＶｋを用いて、
図６の点線で示す関節位置及び関節姿勢に関節位置姿勢算出部２８により決定された場合は、座標系Ｔ１〜Ｔ５の代わりに座標系Ｔ´１〜Ｔ´５が規定される。そして、変換行列Ｊ（ｋ−１←ｋ），Ｊ（０←３）の代わりに変換行列Ｊ´（ｋ−１←ｋ），Ｊ（０←３）が用いられる。この場合、座標系Ｓ０でのメス部１９（マニピュレータ２の先端）の位置姿勢ベクトルＵ０は、それぞれの座標系Ｔ´ｋでのメス部１９の位置姿勢ベクトルＶ´ｋを用いて、
そして、駆動指令生成部３１が、関節作動状態算出部２９で算出されたそれぞれの関節部１５Ａ〜１５Ｅの作動状態に基づいて、それぞれのモータ２１Ａ〜２１Ｅの駆動指令を生成する。そして、それぞれのサーボ制御部３２Ａ〜３２Ｅが、対応するモータ２１Ａ〜２１Ｅの駆動指令に基づいて、対応するモータ２１Ａ〜２１Ｅに駆動電流を供給する。ここで、第２の制御モードでは、規制状態と判定された関節部１５Ａ，１５Ｂは作動されない。このため、駆動指令生成部３１では、関節部１５Ａ，１５Ｂに対応するモータ２１Ａ，２１Ｂの駆動指令は生成されない。したがって、モータ２１Ａ，２１Ｂに駆動電流は供給されず、モータ２１Ａ，２１Ｂは駆動されない。以上のようにして、第１の制御モードにおいて１つ以上の関節部１５Ａ，１５Ｂが規制状態と判定された場合に、規制状態と判定された関節部１５Ａ，１５Ｂに対応するモータ２１Ａ，２１Ｂを駆動しない第２の制御モードで、駆動制御部２６はそれぞれのモータ２１Ａ〜２１Ｅを駆動制御する。
Then, the drive
図２に示すように、制御ユニット３は、警告発生部４２を備える。警告発生部４２は、関節位置姿勢算出部２８に電気的に接続されている。警告発生部４２は、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、警告情報を発生する。また、制御ユニット３には、ランプ、ディスプレイ等の警告表示部４３が設けられている。警告発生部４２で警告情報が発生することにより、警告表示部４３に警告が表示される。
As shown in FIG. 2, the
そこで、前記構成のマニピュレータ２の動作制御装置１では、以下の効果を奏する。すなわち、動作制御装置１では、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９が、第1の制御モードでの対応するモータ２１Ａ〜２１Ｅの駆動状態に基づいて、対応する関節部１５Ａ〜１５Ｅが非規制状態であるか、又は、規制状態であるか、を判定する。それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９で１つ以上の関節部（１５Ａ，１５Ｂ）が規制状態と判定された場合、駆動制御部２６は第２の制御モードでモータ２１Ａ〜２１Ｅを駆動制御する。第２の制御モードでは、関節位置姿勢算出部２８は、規制状態と判定された関節部（１５Ａ，１５Ｂ）を現在の関節位置及び関節姿勢から変化させることなく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。このため、第２の制御モードでは、規制状態と判定された関節部（１５Ａ，１５Ｂ）は作動されず、規制状態の関節部（１５Ａ，１５Ｂ）に対応するモータ（２１Ａ，２１Ｂ）の駆動指令は生成されない。したがって、（長手軸Ｃに垂直な方向への）移動が規制された関節部（１５Ａ，１５Ｂ）の作動に起因して生じるマニピュレータ２の不具合を、有効に防止することができる。
Therefore, the
また、第２の制御モードでは、関節位置姿勢算出部２８が、規制状態と判定された関節部（１５Ａ，１５Ｂ）を現在の関節位置及び関節姿勢から変化させることなく、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。そして、規制状態の関節部（１５Ａ，１５Ｂ）を作動することなく、関節作動状態算出部２９が、関節位置姿勢算出部２８での算出結果に基づいて、それぞれの関節部１５Ａ〜１５Ｅの作動状態を算出する。そして、駆動指令生成部３１が、関節作動状態算出部２９での算出結果に基づいて、それぞれのモータ２１Ａ〜２１Ｅの駆動指令を生成する。したがって、第２の制御モードでは、規制状態の関節部（１５Ａ，１５Ｂ）を作動することなく、先端機能部であるメス部１９を目標位置及び目標姿勢にすることができきる。
In the second control mode, the joint position /
（第1の実施形態の変形例）
なお、第1の実施形態では、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、警告発生部４２が警告情報を発生するが、これに限るものではない。例えば、ある変形例として、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、関節位置姿勢算出部２８が、先端機能部であるメス部１９の先端姿勢（姿勢）を一意的に決定することなく、メス部１９を目標位置にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出してもよい。本変形例でも、規制状態の関節部（例えば１５Ａ，１５Ｂ）を現在の関節位置及び関節姿勢から変化させることなく、それぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出される。
(Modification of the first embodiment)
In the first embodiment, a warning is generated when the joint position /
図７は、本変形例の関節位置姿勢算出部２８での処理を説明する図である。図７に示すように、例えば、メス部１９を目標位置及び目標姿勢にすることにより、メス部１９（マニピュレータの先端）が位置姿勢ベクトルＷを有する場合を考える。ここで、関節部１５Ａ，１５Ｂが規制状態で、かつ、関節部１５Ｃ〜１５Ｅが非規制状態ならば、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出可能である。すなわち、図７の実線で示すように、関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出される。
FIG. 7 is a diagram for explaining processing in the joint position /
しかし、関節部１５Ａ〜１５Ｃが規制状態で、かつ、関節部１５Ｄ，１５Ｅが非規制状態ならば、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出不可能となる。この場合、メス部１９の先端姿勢（姿勢）を目標姿勢に一意的に決定せず、メス部１９を目標位置にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する。すなわち、メス部１９が目標位置となれば、メス部１９の先端姿勢は目標姿勢と異なってもよい。したがって、図７の点線で示すように、関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出される。メス部１９の先端姿勢が目標姿勢とは異なるため、メス部１９は、位置姿勢ベクトルＷとは異なる位置姿勢ベクトルＷ´を有する。この際、規制状態の関節部（１５Ａ〜１５Ｃ）は、現在の関節位置及び関節姿勢から変化させない。メス部１９（先端機能部）の先端姿勢（姿勢）を目標姿勢に一意的に決定せず、メス部１９（先端機能部）を目標位置にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を算出する方法については、特開２００７−２９２８８号公報で（式４）を用いて詳細に説明されている。
However, if the
また、本変形例では、メス部１９の先端姿勢を目標姿勢に一意的に決定しない場合においても、メス部１９を目標位置にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢が算出不可能ならば、警告発生部４２が警告情報を発生する。
Further, in this modification, even when the tip posture of the
また、別のある変形例として、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、駆動指定生成部３２が全てのモータ２１Ａ〜２１Ｅについて駆動指令を生成しなくてもよい。本変形例では、メス部１９を目標位置及び目標姿勢にするそれぞれの関節部１５Ａ〜１５Ｅの関節位置及び関節姿勢を関節位置姿勢算出部２８が算出不可能な場合に、全てのモータ２１Ａ〜２１Ｅに駆動電流が供給されず、全ての関節部１５Ａ〜１５Ｅの作動が停止する。
As another modified example, when the joint position and
（第２の実施形態）
次に、本発明の第２の実施形態について、図８乃至図１０を参照して説明する。なお、第１の実施形態と同一の部分及び同一の機能を有する部分については同一の符号を付して、その説明は省略する。
(Second Embodiment)
Next, a second embodiment of the present invention will be described with reference to FIGS. In addition, the same code | symbol is attached | subjected about the part which has the same function as 1st Embodiment, and the same function, and the description is abbreviate | omitted.
図８は、関節部１５Ａ〜１５Ｅを作動する構成を示す図である。図８に示すように、本実施形態のマニピュレータ２は、第1の実施形態と同様に、モータ２１Ａ〜２１Ｅ及びエンコーダ２２Ａ〜２２Ｅを備える。また、制御ユニット３の駆動制御部２６には、第1の実施形態と同様に、関節位置姿勢算出部２８、関節作動状態算出部２９、駆動指令生成部３１及びサーボ制御部３２Ａ〜３２Ｅが設けられている。そして、それぞれのサーボ制御部３２Ａ〜３２Ｅには、移動規制判定部３９が設けられている。ただし、本実施形態では第1の実施形態と異なり、規制先端特定部４１は設けられていない。
FIG. 8 is a diagram showing a configuration for operating the
図９は、サーボ制御部３２Ａの構成を示す図である。図１０は、サーボ制御部３２Ａ、モータ２１Ａ及びエンコーダ２２Ａでの処理を説明する図である。なお、以下の説明では、ある１つのサーボ制御部３２Ａ、及び、そのサーボ制御部３２Ａに対応するモータ２１Ａ及びエンコーダ２２Ａについてのみ説明するが、サーボ制御部３２Ｂ〜３２Ｅ、モータ２１Ｂ〜２１Ｅ及びエンコーダ２２Ｂ〜２２Ｅについても、サーボ制御部３２Ａ、モータ２１Ａ及びエンコーダ２２Ａと構成及び処理は、同様である。
FIG. 9 is a diagram illustrating a configuration of the
図９に示すように、サーボ制御部３２Ａは、第1の実施形態と同様に、駆動位置制御部３５と、駆動速度制御部３６と、微分実施部３７と、移動規制判定部３９と、を備える。また、本実施形態では、サーボ制御部３２Ａは、サーボゲイン変更部６１をさらに備える。
なお、本実施形態では、サーボ制御部３２Ａに移動規制判定部３９及びサーボゲイン変更部６１が設けられているが、移動規制判定部３９及びサーボゲイン変更部６１がサーボ制御部３２Ａとは別に設けられていてもよい。
As shown in FIG. 9, the
In this embodiment, the movement
図１０に示すように、駆動位置制御部３５は、第1の実施形態と同様に、駆動指令に含まれるモータ２１Ａの駆動位置データに基づいて、モータ２１Ａの駆動位置の制御を行う（ステップＳ１０１）。そして、駆動速度制御部３６は、駆動指令に含まれるモータ２１Ａの駆動速度データに基づいて、モータ２１Ａの駆動速度の制御を行う（ステップＳ１０２）。駆動位置制御部３５でのモータ２１Ａの駆動位置の制御、及び、駆動速度制御部３６でのモータ２１Ａの駆動速度の制御に基づいて、駆動エネルギーである駆動電流がモータ２１Ａに供給される。
As shown in FIG. 10, the drive
モータ２１Ａに駆動電流が供給されることにより、モータ２１Ａが駆動される（ステップＳ１０３）。これにより、関節部１５Ａが作動される。この際、特に長手軸Ｃに垂直な方向への関節部１５Ａの移動が規制された場合、関節部１５Ａの負荷が大きくなり、負荷が外乱としてモータ２１Ａの駆動に作用する。
By supplying a drive current to the
実際のモータ２１Ａの駆動位置（駆動状態）は、エンコーダ２２Ａにより検出される（ステップＳ１０４）。検出されたモータ２１Ａの駆動位置情報は、駆動位置制御部３５でのモータ２１Ａの駆動位置制御（ステップＳ１０１）において、フィードバックされる。また、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置情報は、微分実施部３７で微分され（ステップＳ１０５）、実際のモータ２１Ａの駆動速度が検出される。検出されたモータ２１Ａの駆動速度情報は、駆動速度制御部３６でのモータ２１Ａの駆動速度制御（ステップＳ１０２）において、フィードバックされる。
The actual driving position (driving state) of the
移動規制判定部３９には、駆動速度制御部３６からモータ２１Ａの駆動電流が入力される。また、移動規制判定部３９には、エンコーダ２２Ａにより検出されたモータ２１Ａの駆動位置（駆動状態）情報が入力される。移動規制判定部３９は、モータ２１Ａの駆動電流及びエンコーダ２２Ａで検出したモータ２１Ａの駆動状態（駆動位置）に基づいて、関節部１５Ａが移動自在な非規制状態であるか、又は、関節部１５Ａの移動が規制される規制状態であるか、を判定する（ステップＳ１０６）。移動規制判定部３９は、第１の実施形態と同様にして、判定を行う。なお、その他のサーボ制御部３２Ｂ〜３２Ｅのそれぞれにおいても、サーボ制御部３２Ａと同様に、移動規制判定部３９により、対応する関節部１５Ｂ〜１５Ｅが規制状態であるか、又は、対応する関節部１５Ｂ〜１５Ｅが非規制状態であるかが、判定される。
The movement
ここで、第1の制御モードにおいて1つ以上の関節部（１５Ａ，１５Ｂ）が規制状態と判定された場合には、駆動制御部２６は第２の制御モードでそれぞれのモータ２１Ａ〜２１Ｅを駆動制御する。ここで、関節部１５Ａ，１５Ｂが規制状態であり、関節部１５Ｃ〜１５Ｅが非規制状態である場合を考える。この場合、第1の制御モードにおいて対応する関節部１５Ａが規制状態と判定されるため、サーボ制御部３２Ａのサーボゲイン変更部６１は、第２の制御モードでのサーボ制御部３２Ａの駆動指令に対する駆動電流のサーボゲインＧａ２を、第1の制御モードでのサーボ制御部３２ＡのサーボゲインＧａ１より小さくする。これにより、対応するモータ２１Ａの駆動速度が第1の制御モードより小さくなり、対応する関節部１５Ａの作動速度が第1の制御モードより小さくなる。また、サーボ制御部３２Ｂのサーボゲイン変更部６１も、第2の制御モードでのサーボ制御部３２ＢのサーボゲインＧｂ２を、第1の制御モードでのサーボ制御部３２ＢのサーボゲインＧｂ１より小さくする。
Here, when one or more joint parts (15A, 15B) are determined to be in the restricted state in the first control mode, the
また、第1の制御モードにおいて対応する関節部１５Ｃが非規制状態と判定されるため、サーボ制御部３２Ｃのサーボゲイン変更部６１は、第1の制御モードと第２の制御モードとの間で、サーボ制御部３２Ｃの駆動指令に対する駆動電流のサーボゲインＧｃを変更しない。サーボ制御部３２Ｄ，３２ＥのサーボゲインＧｄ，Ｇｅについても、サーボゲインＧｃと同様である。
Further, since the corresponding
以上のようにして、それぞれのサーボ制御部３２Ａ〜３２Ｅのサーボゲイン変更部６１は、対応する移動規制判定部３９での判定結果に基づいて、対応するサーボ制御部３２Ａ〜３２ＥでのサーボゲインＧａ〜Ｇｅを設定する（ステップＳ１０７）。例えば、サーボ制御部３２Ａのサーボゲイン変更部６１は、判定結果に基づいて、サーボ制御部３２ＡでのサーボゲインＧａを設定する。
As described above, the servo
そこで、前記構成のマニピュレータ２の動作制御装置１では、以下の効果を奏する。すなわち、動作制御装置１では、それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９が、第1の制御モードでの対応するモータ２１Ａ〜２１Ｅの駆動状態に基づいて、対応する関節部１５Ａ〜１５Ｅが非規制状態であるか、又は、規制状態であるか、を判定する。それぞれのサーボ制御部３２Ａ〜３２Ｅの移動規制判定部３９で１つ以上の関節部（１５Ａ，１５Ｂ）が規制状態と判定された場合、駆動制御部２６は第２の制御モードでモータ２１Ａ〜２１Ｅを駆動制御する。規制状態の関節部（１５Ａ，１５Ｂ）に対応するサーボ制御部（３２Ａ，３２Ｂ）では、サーボゲイン変更部６１によって、第２の制御モードでのサーボゲイン（Ｇａ２，Ｇｂ２）が、第1の制御モードでのサーボゲイン（Ｇａ１，Ｇｂ１）に比べて小さくなる。これにより、第２の制御モードでは、対応するモータ（２１Ａ，２１Ｂ）の駆動速度が第1の制御モードより小さくなり、対応する関節部（１５Ａ，１５Ｂ）の作動速度が第1の制御モードより小さくなる。したがって、（長手軸Ｃに垂直な方向への）移動が規制された関節部（１５Ａ，１５Ｂ）の作動に起因して生じるマニピュレータ２の不具合を、有効に防止することができる。
Therefore, the
ただし、本実施形態では、第２の制御モードにおいて、規制状態の関節部（１５Ａ，１５Ｂ）に対応するサーボ制御部（３２Ａ，３２Ｂ）のサーボゲイン（Ｇａ，Ｇｂ）を、第１の制御モードに比べて小さくする構成である。このため、第２の制御モードでは、メス部１９が目標位置及び目標姿勢にならない。
However, in the present embodiment, in the second control mode, the servo gains (Ga, Gb) of the servo control units (32A, 32B) corresponding to the joint portions (15A, 15B) in the restricted state are set in the first control mode. It is the structure made small compared with. For this reason, in the second control mode, the
（その他の変形例）
なお、前述の実施形態では、５つの関節部１５Ａ〜１５Ｅが設けられているが、これに限るものではない。すなわち、複数の関節部（１５Ａ〜１５Ｅ）が設けられていればよく、例えば、関節部（１５Ａ〜１５Ｅ）の数は２つ、３つ、４つでもよく、６つ以上でもよい。そして、それぞれの関節部（１５Ａ〜１５Ｃ）に対応して、駆動部材であるモータ（２１Ａ〜２１Ｅ）及びサーボ制御部（３２Ａ〜３２Ｃ）が設けられていればよい。
(Other variations)
In the above-described embodiment, the five
また、前述の実施形態では先端機能部としてメス部１９が設けられているが、これに限るものではない。例えば、先端処置部（先端機能部）として処置対象を把持して処置を行う把持処置部が設けられてもよい。また、前述の実施形態では、駆動エネルギーとして駆動電流が供給されるモータ２１Ａ〜２１Ｅが駆動部材として設けられているが、これに限るものではない。すなわち、駆動エネルギーが供給されることにより駆動される駆動部材が設けられていればよい。
Moreover, although the
また、前述の実施形態では、挿入機器として内視鏡５０の処置具挿通路５１に挿通されるマニピュレータ２が用いられているが、これに限るものではない。例えば、ある変形例として図１１に示すように、工業用の管路７１に挿通される内視鏡７０を挿入機器として用いてもよい。本変形例でも第１の実施形態と同様に、内視鏡７０の挿入部１０の先端部に先端腕部１２が設けられ、先端腕部１２は、関節部１５Ａ〜１５Ｅ及びリンク部１７Ａ〜１７Ｅを備える。そして、リンク部１７Ｅの内部に先端機能部である撮像素子７２が設けられている。以上、本変形例から、先端腕部１２では、最も先端方向側に位置する関節部（１５Ｅ）の先端方向側に先端機能部（１９，７２）が設けられていればよい。
In the above-described embodiment, the
また、第1の実施形態の構成及び第２の実施形態の構成を組合わせてもよい。この場合、規制先端特定部４１が設けられるとともに、それぞれのサーボ制御部３２Ａ〜３２Ｅに対応してサーボゲイン変更部６１が設けられる。
Moreover, you may combine the structure of 1st Embodiment and the structure of 2nd Embodiment. In this case, a regulation
以上、本発明の実施形態について説明したが、本発明は前記の実施形態に限定されるものではなく、本発明の要旨を逸脱しない範囲で種々の変形ができることは勿論である。 Although the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments, and various modifications can be made without departing from the scope of the present invention.
１…動作制御装置、２…マニピュレータ、３…制御ユニット、５…操作指令入力部、１０…挿入部、１２…先端腕部、１５Ａ〜１５Ｅ…関節部、１９…メス部、２１Ａ〜２１Ｃ…モータ、２６…駆動制御部、３９…移動規制判定部。
DESCRIPTION OF
Claims (11)
前記先端機能部の目標位置及び目標姿勢を示す操作指令が入力される操作指令入力部と、
駆動エネルギーが供給されることにより駆動され、駆動されることによりそれぞれが対応する前記関節部を作動させる複数の駆動部材と、
対応する前記駆動部材の駆動状態に基づいて、対応する前記関節部が移動自在な非規制状態であるか、又は、対応する前記関節部の移動が規制される規制状態であるか、をそれぞれが判定する複数の移動規制判定部と、
前記操作指令に基づいて第1の制御モードでそれぞれの前記駆動部材を駆動制御する駆動制御部であって、前記第１の制御モードにおいて１つ以上の前記関節部が前記規制状態と判定された場合に、前記規制状態と判定された前記関節部に対応する前記駆動部材を、駆動しない、又は、前記第1の制御モードより駆動速度が小さくする第２の制御モードで、それぞれの前記駆動部材を駆動制御する駆動制御部と、
を具備する挿入機器の動作制御装置。 An insertion device including an insertion portion extending along a longitudinal axis, wherein the insertion portion includes a tip arm portion operably provided at a tip portion, the tip arm portion including a plurality of joint portions, A distal end functional part provided on the distal direction side of the joint part located on the distal direction side, and the joint part operated by the distal arm part by actuating each joint part, An insertion device in which the distal side portion operates,
An operation command input unit to which an operation command indicating a target position and a target posture of the tip function unit is input;
A plurality of driving members that are driven by being supplied with driving energy and that actuate each of the corresponding joints by being driven;
Based on the drive state of the corresponding drive member, whether the corresponding joint part is in a non-restricted state in which the corresponding joint part is freely movable, or whether the corresponding joint part is in a restricted state in which movement is restricted. A plurality of movement restriction determination units for determining;
A drive control unit that drives and controls each of the drive members in a first control mode based on the operation command, and in the first control mode, one or more of the joint portions are determined to be in the restricted state. The driving members corresponding to the joints determined to be in the restricted state are not driven or are driven in the second control mode in which the driving speed is lower than that in the first control mode. A drive control unit for controlling the drive,
An operation control apparatus for an insertion device comprising:
前記操作指令に基づいて、前記先端機能部を前記目標位置及び前記目標姿勢にするそれぞれの前記関節部の関節位置及び関節姿勢を算出する関節位置姿勢算出部と、
前記関節位置姿勢算出部で算出された前記関節位置及び前記関節姿勢にするそれぞれの前記関節部の作動状態を算出する関節作動状態算出部と、
前記関節作動状態算出部で算出されたそれぞれの前記関節部の前記作動状態に基づいて、それぞれの前記駆動部材の駆動指令を生成する駆動指令生成部と、
対応する前記駆動部材の前記駆動指令に基づいて、それぞれが対応する前記駆動部材に前記駆動エネルギーを供給する複数のサーボ制御部と、
を備える、請求項１の動作制御装置。 The drive control unit
Based on the operation command, a joint position and posture calculation unit that calculates a joint position and a joint posture of each joint unit that sets the distal end function unit to the target position and the target posture;
A joint operation state calculation unit that calculates an operation state of each joint unit to be the joint position and the joint posture calculated by the joint position and posture calculation unit;
A drive command generation unit that generates a drive command for each of the drive members based on the operation state of each joint unit calculated by the joint operation state calculation unit;
A plurality of servo control units for supplying the drive energy to the corresponding drive member based on the drive command of the corresponding drive member;
The operation control apparatus according to claim 1, comprising:
前記関節位置姿勢算出部は、前記第２の制御モードにおいて、前記最先端規制関節及び前記最先端規制関節より基端方向側の前記関節部を現在の前記関節位置及び前記関節姿勢から変化させることなく、前記先端機能部を前記目標位置及び前記目標姿勢にするそれぞれの前記関節部の前記関節位置及び前記関節姿勢を算出する、
請求項３の動作制御装置。 Based on the determination result in each of the movement restriction determination units, a restriction tip specifying unit that specifies the most advanced restriction joint located closest to the tip direction among the joints determined to be in the restriction state Further comprising
In the second control mode, the joint position / orientation calculation unit changes the joint part closer to the proximal direction than the most advanced restriction joint and the most advanced restriction joint from the current joint position and the joint attitude. Without calculating the joint position and the joint posture of each of the joint portions to make the tip function unit the target position and the target posture,
The operation control device according to claim 3.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2012057577A JP2015107138A (en) | 2012-03-14 | 2012-03-14 | Operation control device for insertion instrument |
| PCT/JP2012/079107 WO2013136583A1 (en) | 2012-03-14 | 2012-11-09 | Operation-control device for insertion apparatus |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2012057577A JP2015107138A (en) | 2012-03-14 | 2012-03-14 | Operation control device for insertion instrument |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2015107138A true JP2015107138A (en) | 2015-06-11 |
Family
ID=49160536
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2012057577A Pending JP2015107138A (en) | 2012-03-14 | 2012-03-14 | Operation control device for insertion instrument |
Country Status (2)
| Country | Link |
|---|---|
| JP (1) | JP2015107138A (en) |
| WO (1) | WO2013136583A1 (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10859999B2 (en) | 2017-07-10 | 2020-12-08 | Fanuc Corporation | Numerical controller |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106255472B (en) | 2014-03-17 | 2020-08-25 | 直观外科手术操作公司 | System and method for breakaway clutching in an articulated arm |
| US11311343B2 (en) | 2015-09-17 | 2022-04-26 | Endomaster Pte Ltd | Flexible robotic endoscopy system |
| US20240108422A1 (en) * | 2021-01-26 | 2024-04-04 | Versitech Limited | A steerable arm for use in endoscopic surgical procedures |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH01240280A (en) * | 1988-03-17 | 1989-09-25 | Meidensha Corp | Variable speed control method for manipulator |
| JPH02218569A (en) * | 1989-02-20 | 1990-08-31 | Mitsubishi Heavy Ind Ltd | Interpolation of singular point passing for ptp teaching of articulated robot |
| JPH08224246A (en) * | 1995-02-22 | 1996-09-03 | Olympus Optical Co Ltd | Medical manipulator |
| JP2000190262A (en) * | 1998-12-22 | 2000-07-11 | Denso Corp | Control device for robot |
| JP2007136590A (en) * | 2005-11-16 | 2007-06-07 | Kawasaki Heavy Ind Ltd | Control device and control method for redundant robot having redundant joint |
| JP5336760B2 (en) * | 2008-05-01 | 2013-11-06 | オリンパスメディカルシステムズ株式会社 | Endoscope system |
-
2012
- 2012-03-14 JP JP2012057577A patent/JP2015107138A/en active Pending
- 2012-11-09 WO PCT/JP2012/079107 patent/WO2013136583A1/en active Application Filing
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10859999B2 (en) | 2017-07-10 | 2020-12-08 | Fanuc Corporation | Numerical controller |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2013136583A1 (en) | 2013-09-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11596487B2 (en) | Robotic surgical system control scheme for manipulating robotic end effectors | |
| JP6880324B2 (en) | Systems and methods for controlling robot manipulators or related tools | |
| US10300599B2 (en) | Systems and methods for safe compliant insertion and hybrid force/motion telemanipulation of continuum robots | |
| US8909376B2 (en) | Robot hand and method of controlling the same | |
| JP5743495B2 (en) | Robot controller | |
| EP3242773B1 (en) | Autonomous correction of alignment error in a master-slave robotic system | |
| JP7250859B2 (en) | CONTINUOUS ROBOT, CONTROL METHOD THEREOF, AND PROGRAM | |
| US10035267B2 (en) | Robot system and method for controlling a robot system | |
| EP2777597A3 (en) | Robots and methods of controlling the same | |
| CN110035707B (en) | Medical system and control method thereof | |
| WO2013136583A1 (en) | Operation-control device for insertion apparatus | |
| WO2016073367A1 (en) | Position/force control of a flexible manipulator under model-less control | |
| JP5108032B2 (en) | Multi-joint structure teaching device | |
| JP5409974B1 (en) | Manipulator device | |
| Xia et al. | A constrained optimization approach to virtual fixtures for multi-robot collaborative teleoperation | |
| CN112894827B (en) | Method, system and device for controlling motion of mechanical arm and readable storage medium | |
| US20170028548A1 (en) | Robot control device and robot control method | |
| Jiao et al. | Vision based cable assembly in constrained environment | |
| Khalifa et al. | Workspace mapping and control of a teleoperated endoscopic surgical robot | |
| EP3954319A1 (en) | Surgical operation system and method for controlling surgical operation system | |
| Mak et al. | Development of Haptic Approaches for a Head-Controlled Soft Robotic Endoscope | |
| Pott et al. | BOrEScOPE–exoskeleton for active surgeon support during orthopedic surgery | |
| HA et al. | Compliant Motion Control of Robotic Catheter based on Long-Short Term Memory Network | |
| Prasanga et al. | Achievement of real haptic sensation with tendon driven segregated jaws for laparoscopic forceps | |
| WO2024086122A1 (en) | Controlling software remote centers of motion for computer-assisted systems subject to motion limits |