JP2010076012A - Manipulator system and control method thereof - Google Patents

Manipulator system and control method thereof Download PDF

Info

Publication number
JP2010076012A
JP2010076012A JP2008244411A JP2008244411A JP2010076012A JP 2010076012 A JP2010076012 A JP 2010076012A JP 2008244411 A JP2008244411 A JP 2008244411A JP 2008244411 A JP2008244411 A JP 2008244411A JP 2010076012 A JP2010076012 A JP 2010076012A
Authority
JP
Japan
Prior art keywords
angle
force
gripping
axis
operation
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
Application number
JP2008244411A
Other languages
Japanese (ja)
Inventor
Takamitsu Sunaoshi
押 貴 光 砂
Original Assignee
Toshiba Corp
株式会社東芝
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Toshiba Corp, 株式会社東芝 filed Critical Toshiba Corp
Priority to JP2008244411A priority Critical patent/JP2010076012A/en
Publication of JP2010076012A publication Critical patent/JP2010076012A/en
Application status is Pending legal-status Critical

Links

Images

Abstract

<P>PROBLEM TO BE SOLVED: To provide a manipulator system capable of stably and accurately controlling an acting force such as the gripping force, and a control method thereof. <P>SOLUTION: A control device 2 of the manipulator system comprises a grip-start estimation observer 51, a gripping angle determination unit 52, an inverse kinematic matrix computation unit 53, a PID control unit 54, a motor drive unit 55, a target gripping angle setting unit 56, a minimum closing angle determination unit 57, and an operation amount updating unit 58. The grip-start estimation observer 51 estimates the grip-starting angle, sets the minimum closing angle of a gripper 4 based on the estimated grip-start angle, and controls the gripper axis angle before reaching the minimum closing angle. The gripping force of the gripper 4 can be controlled more accurately than when the gripping force is controlled through the torque control of the motor. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

本発明は、アクチュエータの動力を動力伝達機構を介して作用部に伝達するマニピュレータシステムおよびその制御方法に関する。 The present invention, the power of the actuator about the manipulator system and a control method thereof is transmitted to the action portion via a power transmission mechanism.

マニピュレータの把持力を把持機構部にセンサを設けることなく制御することは従来からの課題である。 Be controlled without providing a sensor in the gripping mechanism unit gripping forces of the manipulator is subject of the prior art. 例えば特許文献1に記載されている医療用マニピュレータの場合、マニピュレータ先端部は体腔内部に侵入されるため、細径小型化が求められるとともに、十分な力の伝達が必要とされる。 For example, in the case of a medical manipulator disclosed in Patent Document 1, since the manipulator tip is entering the body cavity, with diameter miniaturization is demanded, it is required transmit sufficient force. また、医療用機器であるため、洗浄滅菌処理が可能で、通電部分がないことが好ましい。 Further, since the medical instruments, washable sterilization is preferably no live parts. このような理由から、先端部には把持力検出用のセンサ等を配置しない方が望ましい。 For this reason, it is preferable not to place sensors or the like for gripping force detected in the tip. 医療用マニピュレータに限らず、配管作業ロボット、ホームロボット等でも小型化、軽量化、低コスト化の面から、少ないセンサ数で制御可能であることが望まれている。 Is not limited to the medical manipulator, pipe work robot, miniaturization in the home robot or the like, weight reduction, from the viewpoint of cost reduction, it has been desired to be controlled with a small number of sensors.

このような課題を解決するために、特許文献2には、ハンドを駆動するモータ電流から物体を把持したことを認識し、モータ電流値によってトルク制御を行う技術が記載されている。 In order to solve such a problem, Patent Document 2, recognizes that the object is gripped from the motor current for driving the hand, it discloses a technique to perform a torque control by the motor current value. また、特許文献3には、把持機構を駆動するモータ電流と角速度からオブザーバを構築して、把持力用の出力トルクを推定し、この出力トルクを制御する技術が記載されている。 In Patent Document 3, to construct the observer from the motor current and the angular velocity of driving the gripping mechanism, to estimate the output torque of the gripping force, a technique for controlling the output torque is described.
特開2002−102248号公報 JP 2002-102248 JP 特開平11−333776号公報 JP 11-333776 discloses 特開2002−178281号公報 JP 2002-178281 JP

しかしながら、モータ電流値と実際の把持力とはヒステリシスな関係を持っており、必ずしも1対1に対応していない。 However, the actual gripping force and motor current value has a hysteresis relationship does not necessarily correspond to one to one. このため、モータ電流値によるトルク制御では十分とはいえない。 Therefore, sufficient and not be a torque control by the motor current value. 上述した特許文献3には、オブザーバを構築して摩擦や慣性の影響を考慮することが記載されているが、ここで求められたトルク推定量と実際の把持力は必ずしも1対1に対応するものではない。 Patent Document 3 described above has been described to consider the influence of the friction and inertia to build observer, the actual gripping force and torque estimator obtained here corresponds necessarily a one-to-one not.

また、上述した特許文献2、3のいずれも、把持機構とそれを駆動するモータが1対1に対応しており、比較的良好な制御性能を示すことが考えられるが、把持機構以外にも自由度を持つマニピュレータであって、把持動作に関与するモータが他の自由度の動作にも寄与しているような干渉系機構の場合であると、よりいっそう電流値と実際の作用力との相関は複雑となる。 Furthermore, none of Patent Documents 2 and 3 mentioned above, a motor for driving the gripping mechanism and it has a one-to-one correspondence, but to exhibit relatively good control performance is considered, in addition to the gripping mechanism a manipulator having a degree of freedom, the motor involved in gripping operation when it is the case of the interferometric mechanism as contribute to the operation of the other degrees of freedom, the more the current value and the actual acting force correlation is complicated. そういったことから、電流値によるトルク制御で把持力を制御すると、実際の把持力が必ずしも所望の値になるとは限らないこと、力制御の挙動が位置(角度)の偏差挙動を引き起こすことなどといった問題がある。 From such a fact, when controlling the gripping force in the torque control by the current value, the actual possible gripping force is not necessarily a desired value, a problem such as causing deviation behavior behavior position of the force control (angle) there is.

本発明は、把持力等の作用力を安定に精度よく制御可能なマニピュレータシステムおよびその制御方法を提供するものである。 The present invention is stably acting force of the gripping force or the like is to provide a precisely controllable manipulator system and its control method.

本発明の一態様によれば、アクチュエータと、 According to one aspect of the present invention, an actuator,
前記アクチュエータの動力に基づいて対象物に対して力を作用する作用部と、 A working unit which acts a force to the object based on the power of the actuator,
可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、 Flexible member, a power transmission mechanism for transmitting the power of the actuator to the working portion,
前記アクチュエータを制御する制御部と、を備え、 And a control unit for controlling the actuator,
前記制御部は、 Wherein,
角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号を生成するオブザーバ信号生成手段と、 Without taking into account the term is not dependent on at least one of the magnitude of the angular velocity and acceleration independent of the direction, the observer signal generating means for generating an observer signal for estimating the applied force starting angle of the working portion ,
前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定する作用力開始角度推定手段と、 By comparing the signal level of the observer signal with a predetermined reference value, and the acting force starting angle estimating means for estimating an action force start angle,
予め設定された前記作用部の作用力に対応する姿勢軸角度と前記作用力開始角度推定手段で推定された作用力開始角度とに基づいて、姿勢軸限界角度を検出する限界角度検出手段と、 Based on a preset estimated applied force starting angle in a posture axis angle and the acting force start angle estimating means corresponding to the acting force of the working portion, and the limit angle detecting means for detecting an attitude axis limit angle,
前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御する角度制御手段と、を有することを特徴とするマニピュレータシステムが提供される。 Manipulator system characterized by having an a angle control means for angular control the attitude axis angle of the working portion to the posture axis limit angle is provided.

また、本発明の一態様によれば、複数のアクチュエータと、 According to a further aspect of the present invention, a plurality of actuators,
前記複数のアクチュエータの動力に基づいて対象物に対して力を作用する姿勢軸と、前記対象物をそれぞれ異なる方向に動作させる複数の動作軸と、を有する作用部と、 A working portion having a plurality of operation shafts for operating in different directions and the orientation axis of a force, the object to the object on the basis of the power of the plurality of actuators,
可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、 Flexible member, a power transmission mechanism for transmitting the power of the actuator to the working portion,
前記アクチュエータを制御する制御部と、を備え、 And a control unit for controlling the actuator,
前記複数のアクチュエータは、前記姿勢軸および前記複数の動作軸の駆動に影響を与える干渉系アクチュエータであり、 Wherein the plurality of actuators are interference system actuators influencing the driving of the posture axis and the plurality of operation shafts,
前記制御部は、 Wherein,
前記複数のアクチュエータのそれぞれごとに設けられ、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号候補をそれぞれ生成する複数のオブザーバ信号候補生成手段と、 Wherein the plurality of provided for each actuator, without taking into account the terms that depend on but not dependent on at least one of the magnitude of the angular velocity and acceleration direction, to estimate the force acting start angle of the working portion a plurality of observers signal candidate generator means for generating the observer signal candidates, respectively,
前記複数のアクチュエータの各駆動軸の動作速度に応じて、前記複数の作用力開始角度推定手段から出力されたオブザーバ信号のいずれか一つを選択するオブザーバ信号選択手段と、 According to the operation speed of each drive shaft of the plurality of actuators, and the observer signal selecting means for selecting any one of the output observer signals from said plurality of applied force start angle estimating means,
前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定する作用力開始角度推定手段と、 By comparing the signal level of the observer signal with a predetermined reference value, and the acting force starting angle estimating means for estimating an action force start angle,
予め設定された前記作用部の作用力に対応する姿勢軸角度と前記作用力開始角度推定手段で選択されたオブザーバ信号に対応する作用力開始角度とに基づいて、姿勢軸限界角度を検出する限界角度検出手段と、 On the basis of the acting force starting angle corresponding to the observer signal selected by the preset position shaft angle corresponding to the acting force of the working portion and the acting force start angle estimating means, the limit for detecting the posture axis limit angle and the angle detection means,
前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御する角度制御手段と、を有することを特徴とするマニピュレータシステムが提供される。 Manipulator system characterized by having an a angle control means for angular control the attitude axis angle of the working portion to the posture axis limit angle is provided.

本発明によれば、把持力等の作用力を安定に精度よく制御可能なマニピュレータシステムおよびその制御方法を提供できる。 According to the present invention, the acting force of the gripping force or the like can provide a stable and accurately controllable manipulator system and its control method.

以下、図面を参照しながら、本発明の一実施形態について説明する。 Hereinafter, with reference to the accompanying drawings, a description will be given of an embodiment of the present invention. 以下では、主に医療用マニピュレータの例を説明する。 In the following, mainly illustrating an example of a medical manipulator.

図1は医療用マニピュレータシステムの外観図である。 Figure 1 is an external view of a medical manipulator system. 図1のシステムは、マスタスレーブ方式で動作するマニピュレータ本体1と、マニピュレータ本体1の駆動を制御する制御装置2とを備えている。 The system of Figure 1 includes a manipulator 1 that operates in the master slave system, and a control unit 2 for controlling the driving of the manipulator 1. この他、図1では省略しているが、マニピュレータ本体1の動作状態を表示する動作状態表示器を図1のシステムに設けてもよい。 In addition, although omitted in FIG. 1, the operation status indicator for indicating the operating state of the manipulator 1 may be provided in the system of FIG.

マニピュレータ本体1は、操作者(術者)が操作を指令する操作指令部3と、その操作指令に応じて対象物に所定量の力を作用する作用部4と、操作指令部3と作用部4とを一体的に接続する連結部5と、作用部4の駆動力を発生する駆動部6とを有する。 Manipulator 1, the operator and the operation command unit 3 (operator) to command the operation, the working portion 4 which acts a predetermined amount of force to the object in response to the operation command, operation command unit 3 and the working portion having a connecting portion 5 integrally connected to a drive unit 6 for generating a driving force of the action part 4 and 4.

操作指令部3は、作用部4の作用量と作用方向を指示する少なくとも一つの指令デバイス(ダイヤル等)を有し、各指令デバイスには、その指令値を読み取るセンサが取り付けられている。 Operation command unit 3 has at least one command device for instructing the direction of action and effect of the action part 4 (dial etc.), each command device, which sensor is mounted to read the command value. 操作者が操作指令部3を操作すると、その指令情報は逐次、制御装置2に送信される。 When the operator operates the operation command unit 3, the instruction information is sequentially sent to the control device 2.

作用部4は、例えば、人体の患部に種々の処置を施すものである。 Action part 4 is, for example, those subjected to various treatments of the human body affected area. 作用部4は、患部に対して処置を施すための少なくとも一つの自由度を有し、先端の姿勢を可変させたり、先端に取り付けられたグリッパの開閉角度を可変させることが可能である。 Action part 4 has at least one degree of freedom for performing a procedure on the affected area, or to vary the attitude of the tip, it is possible to vary the opening angle of the gripper attached to the distal end. 作用部4の作用力の変更は操作指令部3の指令によって行われる。 Change in the working force of the working portion 4 is performed by a command of the operation command unit 3.

マニピュレータ本体1の作用部4と連結部5は作業部7を構成し、操作指令部3と駆動部6は操作部8を構成しており、作業部7と操作部8は互いに着脱可能な構造になっている。 Connecting portion 5 and the working portion 4 of the manipulator 1 constitutes the working portion 7, the driving unit 6 and the operation command unit 3 constitutes the operation portion 8, removable and working unit 7 operating portion 8 mutually structure It has become.

図2は作業部7と操作部8を分離した状態を示す図である。 Figure 2 is a view showing a separated state of the operation unit 8 and the working portion 7. 図2に示すように、作業部7内の連結部5の一端側には、複数の出力軸プーリ11a,11b,11cが設けられており、各プーリにはそれぞれワイヤ12a,12b,12cが巻掛けられている。 As shown in FIG. 2, the one end of the connecting portion 5 in the working portion 7, a plurality of output shaft pulley 11a, 11b, 11c are provided, each respective pulley the wire 12a, 12b, 12c are wound It is over. 連結部5は中空であり、作用部4側が開口になっているため、ワイヤ12a,12b,12cは、連結部5の内部を通過して作用部4側まで延在している。 Connecting part 5 is hollow, since the working portion 4 side is turned open, the wires 12a, 12b, 12c extends to the action part 4 side through the inside of the connecting portion 5. なお、ワイヤ12a,12b,12cの一部にロッドリンク等を用いて動力を伝達してもよい。 Incidentally, the wires 12a, 12b, may transmit the power by using the rod links, etc. Some of 12c.

作用部4には、図2では省略しているが、ワイヤ12a,12b,12cを巻掛けるプーリ、歯車、リンク等が設けられている。 The action part 4, although omitted in FIG. 2, wire 12a, 12b, 12c the winding multiplying pulleys, gears, links, etc. are provided. これにより、動力が作業部7内の出力軸プーリ11a,11b,11cとワイヤ12a,12b,12cを介して作用部4まで伝達される。 Thus, the output shaft pulley 11a of the power is in the working unit 7, 11b, 11c and the wire 12a, 12b, is transmitted to the action part 4 through 12c. 各出力軸プーリ11a,11b,11cには、駆動部6からの動力を受け取るための動力受取部13a,13b,13cが接続されている。 Each output shaft pulley 11a, 11b, the 11c, the power receiving portion 13a for receiving the power from the drive unit 6, 13b, 13c are connected.

駆動部6は、複数の(図2では3つの)モータ14a,14b,14cを有する。 Driver 6 includes (in FIG. 2 3) a plurality of motors 14a, 14b, 14c. これらモータ14a,14b,14cの動力は、それぞれ不図示の減速機を介して、カップリングからなる動力伝達部15a,15b,15cに伝達される。 These motors 14a, 14b, the power of the 14c, respectively via a reduction gear (not shown), a power transmission unit 15a consisting of coupling, 15b, is transmitted to 15c. 各モータ14a,14b,14cには、不図示のエンコーダが装備されており、モータ14a,14b,14cの回転角度を計測することができる。 Each motor 14a, 14b, the 14c, an encoder (not shown) is equipped, it can be measured motor 14a, 14b, the rotation angle of 14c. 各エンコーダの計測信号は制御装置2に伝送される。 Measurement signals for each encoder is transmitted to the control device 2.

駆動部6内の動力伝達部15a,15b,15cの端部には凹部が形成され、連結部5内の動力受取部13a,13b,13cの端部には凸部が形成されている。 Power transmission portion 15a of the drive unit 6, 15b, the ends of 15c recessed portion is formed, the power receiving portion 13a of the connecting portion 5, 13b, convex portions on the end portion of the 13c are formed. これら凹部と凸部は互いに係合され、これにより動力伝達部15a,15b,15cの動力が動力受取部13a,13b,13cに伝達される。 These recesses and projections are engaged with one another, thereby the power transmission unit 15a, 15b, the power of 15c is transmitted power receiving unit 13a, 13b, to 13c. 動力伝達部15a,15b,15cと動力受取部13a,13b,13cの位相が合わないと、凹部と凸部が係合されないようにすることで、原点姿勢以外では操作部8と作業部7との結合を防止している。 Power transmission portion 15a, 15b, 15c and a power receiving portion 13a, 13b, the 13c of the phase does not match, by preventing engaged concave portion and the convex portion, an operation portion 8 at other than the origin position and the working portion 7 so as to prevent the binding of.

図2では、モータやスイッチ、センサなどの電子機器を操作部8側に配置し、作業部7側には機構部品を配置している。 In Figure 2, a motor and a switch, arranged electronic device such as the operation unit 8 side sensor are arranged mechanical components in the working portion 7 side. これにより、作業部7全体を洗浄でき、洗浄性と保守性の向上が図れる。 This enables cleaning the entire working portion 7, the improvement of cleanability and maintainability can be improved. すなわち、図2では、汚染度合いと洗浄方法の違いを考慮に入れて、作業部7と操作部8に分離している。 That is, in FIG. 2, taking into account the differences in degree of contamination and cleaning methods are divided into the working portion 7 and the operating portion 8.

操作部8は、連結部5の基端側に接続されたブラケット81と、このブラケット81に対して回動自在に取り付けられた円柱状の操作桿82と、この操作桿82に取り付けられた操作デバイスとを有する。 Operation unit 8, a bracket 81 connected to the proximal end side of the connecting portion 5, a cylindrical operating rod 82 which is mounted rotatably relative to the bracket 81, attached to the operating rod 82 Operation and a device. 操作桿82の操作デバイスは作用部4の姿勢決め等を指示する機能を有する。 An operation device operating rod 82 has a function of instructing the like determined orientation of the action part 4.

操作デバイスは、図2に示すように、横方向ダイヤル83と、縦方向ダイヤル84と、トリガ85と、操作モード切替スイッチ86とを有する。 Operation device, as shown in FIG. 2, has a lateral dial 83, the longitudinal dial 84, a trigger 85, and an operation mode selector switch 86. 図2の操作デバイスを用いて図3のような把持動作を行う作業部7を操作する例について説明する。 For example to operate the working unit 7 for gripping operation as shown in FIG. 3 will be described with reference to the operation device of FIG. 図3において、横方向ダイヤル83がヨー軸29の動作を指令し、縦方向ダイヤル84がロール軸46の動作を指令し、トリガ85がグリッパ軸θgの動作を指令する。 3, the horizontal direction dial 83 commands operation of the yaw axis 29, the vertical direction dial 84 commands operation of the roll shaft 46, trigger 85 directs the operation of the gripper shaft [theta] g. 操作モード切替スイッチ86は、マスタスレーブ動作の開始・終了、初期姿勢復帰動作等の操作モードの切り替えを行うスイッチである。 Operation mode selector switch 86, the start and end of the master slave operation, is a switch for switching the operation modes such as initial return action.

横方向ダイヤル83、縦方向ダイヤル84及びトリガ85には、その動作量を検出するセンサがそれぞれ組み込まれており、センサ信号は制御装置2に送られ、その内部の演算部で処理される。 Lateral dial 83, the vertical direction dial 84 and the trigger 85 is incorporated a sensor for detecting the operation amount of each sensor signal is sent to the control unit 2, it is processed by the internal calculation unit. 横方向ダイヤル83と縦方向ダイヤル84に組み込まれるセンサは、操作桿82との相対的な回動量を検出するものであり、例えばポテンショメータやエンコーダを適用することができる。 Sensor incorporated laterally dial 83 and the longitudinal dial 84 is adapted to detect a relative rotation amount between the operating rod 82 can be applied, for example, a potentiometer or an encoder.

トリガ85は、操作桿82に対して並進動作が可能であり、その動作量により、作業部7のグリッパ角度を指令する。 Trigger 85 is capable of translational movement relative to the operating rod 82, by the operation amount, instructs the gripper angle of the working portion 7. トリガ85には並進動作位置を検出するセンサが設けられている。 Sensor for detecting the translational operating position is provided on the trigger 85.

図3はマニピュレータ本体1の作用部4の構造を示す斜視図、図4は作用部4の詳細構造を示す分解斜視図である。 Figure 3 is a perspective view showing a structure of the action part 4 of the manipulator 1, FIG. 4 is an exploded perspective view showing the detailed structure of the action part 4. 図3および図4に示すように、作用部4は、処置対象に対して処置を行う第1の作用部材325と第2の作用部材326を先端に有する連結部材41と、連結部材41の長手方向に沿った方向に対して直交する第1の回動軸部材16と、第1の回動軸部材16の回動軸29回りの方向に回動自在に支持されて第1の回動軸部材16に対して直交する主軸部320bを有する主軸部材320と、第1の回動軸部材16の回動軸29回りの方向に回動自在に支持される第1の歯車117と、第1の回動軸部材16の回動軸29回りの方向に回動自在に支持される第2の歯車218とを有する。 As shown in FIGS. 3 and 4, the action part 4, a connecting member 41 having a first working member 325 to perform a treatment to the treatment target second working member 326 to the distal end, a longitudinal coupling member 41 first the rotation shaft member 16, the first rotating shaft is rotatably supported by the first direction of the rotation axis 29 around the rotation shaft member 16 that is orthogonal to the direction along the direction a main shaft 320 having main shaft portion 320b orthogonal to the member 16, a first gear 117 which is rotatably supported in a first direction of the rotation axis 29 around the pivot shaft member 16, the first and a second gear 218 which is rotatably supported in the direction of the rotation axis 29 around the pivot shaft member 16.

主軸部材320にはプーリ320aが結合され、第1の歯車117にはプーリ117aが結合され、第2の歯車218にはプーリ218aが結合されている。 The main shaft 320 is coupled a pulley 320a is, the first gear 117 a pulley 117a is coupled, the second gear 218 is coupled is a pulley 218a. プーリ320a、プーリ117a、プーリ218aにはそれぞれ第1ワイヤ50a、第2ワイヤ50b、第3ワイヤ50cが巻掛けられている。 Pulley 320a, pulleys 117a, each of the pulleys 218a first wire 50a, the second wire 50b, the third wire 50c is wound.

また、作用部4は、第1の歯車117と直交して噛み合い、主軸部320b回りの方向に回動自在に支持される第3の歯車321aと、第2の歯車218と直交して噛み合い、主軸部320b回りの方向に回動自在に支持される第4の歯車322aと、第3の歯車321aとともに同軸に回動する第2の回動軸部材321bと、第4の歯車322aとともに同軸に回動する第5の歯車322bとを有する。 Further, the action part 4 meshes orthogonally with the first gear 117, a third gear 321a is rotatably supported in the direction of the main shaft portion 320b around mesh perpendicular to the second gear 218, a fourth gear 322a which is rotatably supported in the direction of the main shaft portion 320b around the second rotating shaft member 321b which rotates coaxially with the third gear 321a, coaxially with the fourth gear 322a and a fifth gear 322b which rotates.

また、作用部4は、主軸46周りに回動する第2の回動軸部材321bに直交して第2の回動軸部材321bの回動とともに主軸46回りに回動し、第1の回動軸部材16の回動軸29に対して捩れの位置から平行な位置に回動自在に配置される第3の回動軸部材323aを有する。 Further, the action part 4 is pivoted together with the main shaft 46 around the rotation of the second rotating shaft member 321b perpendicular to the second rotation shaft member 321b to rotate around the main shaft 46, the first round a third rotation shaft member 323a which is rotatably disposed in a parallel position from the position of the twist relative to rotation axis 29 of the shaft member 16.

また、作用部4は、第3の回動軸323a回りの方向に回動自在に支持され、第5の歯車322bと直交して噛み合う第6の歯車323と、第2の回動軸321b 回りの方向に回動するとともに、第6の歯車323とともに第3の回動軸323a回りの方向に回動する第1の作用部材325と、第3の歯車321aおよび第2の回動軸321bとともに回動する第2の作用部材326とを有する。 Further, the action part 4 is rotatably supported in the direction of the third rotation shaft 323a around the sixth gear 323 meshing perpendicularly with the fifth gear 322b, the second rotation shaft 321b around while rotating in the direction of the first working member 325 that rotates together with the gear 323 of the sixth to the third direction of the rotation axis 323a around together with the third gear 321a and the second rotation shaft 321b and a second working member 326 to rotate. 第1の作用部材325と第2の作用部材326は、一対の作用部材(グリッパ)を構成している。 A first working member 325 the second working member 326 constitute a pair of working members (grippers). 第1の作用部材325が第3の回動軸323a回りの方向に回動することで、第2の作用部材326に対して、相対的に開閉可能な把持機構を構成している。 By first working member 325 is rotated in the direction of the third rotation shaft 323a around to the second working member 326 constitutes a relatively openable gripping mechanism.

このように、本実施形態の作用部4は、回動軸29(回動軸部材16)と主軸46(主軸部材321b)を回動中心とした2自由度で姿勢変更が可能な開閉機能(把持機構:グリッパ)を構成する第1および第2の作用部材325、326を有する。 Thus, the action part 4 of the present embodiment, the rotation shaft 29 (rotation shaft member 16) and the main shaft 46 (main shaft 321b) capable of position change in two degrees of freedom and rotation center of the opening and closing function ( gripping mechanism: has the first and second working members 325, 326 constituting the gripper).

カバー327は、歯車の露出を最小限にとどめるためのものであり、カバー固定ピン328により、第2の作用部材326の下部にて固定されている。 Cover 327 is intended to minimize the exposure of the gear, the cover fixing pin 328 is fixed at the bottom of the second working member 326. カバー327は、第2の作用部材326とともに、第2の回動軸部材321b(主軸46)回りの方向に回動する。 Cover 327, together with the second working member 326, rotates the second rotating shaft member 321b (the main shaft 46) around the direction. 固定ナット329は、主軸部材320の先端部のねじ部に固定されており、第3の歯車321aおよび第2の回動軸部材321bの主軸部320bの軸方向の位置を拘束するために設けられる。 Fixing nut 329 is fixed to the threaded portion of the distal end portion of the main shaft 320, it is provided to restrain the axial position of the main shaft portion 320b of the third gear 321a and the second rotating shaft member 321b . 固定ナット330は、第3の回動軸323aおよび第6の歯車323の第3の回動軸323aの軸方向の位置を拘束するために設けられる。 Fixing nut 330 is provided for restraining the third axial position of the rotation shaft 323a of the third rotation shaft 323a and the sixth gear 323.

図5は制御装置2とその周辺の制御系の詳細構成の一例を示すブロック図である。 Figure 5 is a block diagram showing an example of a detailed configuration of a control system of the peripheral and the control unit 2. 制御装置2は、操作指令部3による操作指令に応じた力を作用部4が対象物に作用するようにモータ14a,14b,14cを動作させる制御(第1の制御手段)と、この制御の後に、作用部4による作用力が低下しない範囲で作用部4が対象物に対して所定量よりも小さい力を逆に作用するようにモータ14a,14b,14cを動作させる制御(第2の制御手段)とを行う。 Control device 2, the motor 14a to the force corresponding to the operation command from the operation command unit 3 is the action part 4 acts on the object, 14b, the control to operate the 14c (first control means), the control later, the motor 14a so that the action part 4 to the extent that the acting force is not lowered counteracts the force smaller than the predetermined amount to the object by the action part 4, 14b, the control to operate the 14c (second control carry out the means) and.

より具体的には、図5の制御装置2は、電源部21と、演算部22と、モータ駆動回路部23と、安全保護装置24と、動作状態提示器25と、指令入力部26とを有する。 More specifically, control device 2 in FIG. 5 includes a power supply unit 21, a calculation unit 22, a motor driving circuit 23, a security device 24, the operating state presenter 25, and a command input unit 26 a.

電源部21は、外部電源27から不図示の変圧器と整流器を介して、演算部22とモータ駆動回路部23に必要な電力を供給する。 Power supply unit 21 via a rectifier transformer (not shown) from an external power source 27 supplies power required for the operation unit 22 and the motor drive circuit 23.

マニピュレータ本体1内の操作部8は、作業部7を駆動するモータ14a,14b,14cと、モータ14a,14b,14cの回転角度を検出する角度検出器16と、操作指令部3の姿勢角度(操作量)を検出する角度検出器17とを有する。 Operating portion 8 of the manipulator body 1 has a motor 14a for driving the working unit 7, 14b, 14c and the motor 14a, 14b, an angle detector 16 for detecting the rotation angle of 14c, the operation command unit 3 attitude angle ( and a angle detector 17 for detecting an operation amount).

演算部22は、不図示のCPUと、記憶装置と、論理回路と、IOインターフェイスとを有し、角度検出器17で検出した操作指令部3の操作量に基づいてモータ14a,14b,14cの制御目標値を生成する機能と、角度検出器16で検出したモータ14a,14b,14cの回転角度とモータ14a,14b,14cの制御目標値との偏差を検出して、その偏差を解消するためのモータ指令入力を算出する機能とを有する。 Calculation unit 22 includes a CPU (not shown) includes a storage device, a logic circuit, and IO interface, the angle detector 17 motor 14a based on the operation amount of the operation command unit 3 detected by, 14b, 14c of and generating a control target value, the motor 14a detected by the angle detector 16, 14b, 14c of the rotation angle and the motor 14a, 14b, detects a deviation between the control target value of 14c, to eliminate the deviation and a function of calculating a motor command input.

また、演算部22は、医療用マニピュレータに装備された不図示の認識手段の信号を読み取るとともに、各種スイッチ28の信号入力を監視して、予め決められたプログラムに従って制御演算を行う。 The arithmetic unit 22 performs with reading a signal recognition means (not shown) that is provided on the medical manipulator monitors the signal input of the various switches 28, the control operation according to a predetermined program.

モータ駆動回路部23は、演算部22からの指令入力に従ってモータ14a,14b,14cに電力を供給する回路であり、指令入力に比例した電流をモータ14a,14b,14cに出力する電流指令制御を行う。 Motor driving circuit 23, the motor 14a in accordance with the instruction input from the operation unit 22, 14b, a circuit for supplying power to 14c, a current proportional to the reference input motor 14a, 14b, a current command control output to 14c do. また、モータ駆動回路部23は、各モータ14a,14b,14cの電流値を計測する。 Further, the motor driving circuit 23 measures the motors 14a, 14b, the current value of 14c.

安全保護装置24は、演算部22の演算周期異常、モータ駆動回路異常、緊急停止指令などの異常事態に備えて、マニピュレータ本体1への電力を遮断し、その動作を直ちに停止させる機能を有する。 Security device 24, operation cycle abnormality of the operation unit 22, the motor driving circuit fault, in preparation for the abnormal situation such as an emergency stop command to cut off the power to the manipulator 1, having immediately function to stop the operation.

動作状態提示器25は、演算部22の指示に応じて、マニピュレータ本体1の動作状態を文字情報や画像情報にて提示する。 Operating status presentation unit 25 in accordance with an instruction of the operation section 22, presents the operating state of the manipulator 1 by character information and image information. 指令入力部26は、各種のスイッチ等の操作部材からなり、指令入力部26の操作情報は演算部22に送られる。 Command input unit 26 consists of operation members such as various switches, the operation information of the command input unit 26 is sent to the arithmetic unit 22. 指令入力部26は、マニピュレータ本体1の動作状態や電源の切換などを指令する。 Command input unit 26 commands the operating status, etc. and power switching of manipulator 1.

図6は図5の演算部22が実行するプログラムの一例を示すフローチャートである。 6 is a flow chart showing an example of a program by the calculation unit 22 executes in FIG. 演算部22は、図6のフローチャートを予め決められた制御周期に従って繰り返し実行する。 Calculating unit 22 repeatedly executes in accordance with a predetermined control period to the flowchart of FIG.

まず、操作指令部3の角度検出器17の出力と駆動部6内のモータ14a,14b,14cの角度検出器16の出力を読み取る(ステップS11)。 First, read the output of the operation command unit 3 of the angle detector 17 motor 14a in the driving unit 6, 14b, the output of the angle detector 16 of 14c (step S11). また、操作指令部3や制御装置2に装備されたスイッチ等の操作情報を演算部22で認識し(ステップS12)、その認識結果をもとにマニピュレータ本体1の動作モードを決定する(ステップS13)。 Further, to recognize the operation information of the switch or the like which is mounted on the operation command unit 3 and control unit 2 in the calculating portion 22 (step S12), and determines the operation mode of the manipulator 1 on the basis of the recognition result (Step S13 ). ここで、動作モードとは、予め決められた動作を自動で実行する自動モードや、操作指令部3の操作に従って作業部7を動作させるマスタスレーブ動作モードを指す。 Here, the operation mode, refer or automatic mode to perform a predetermined operation automatically, the master-slave operation mode for operating the working unit 7 in accordance with the operation of the operation command unit 3. 動作方式とは、両動作モードの切り換えにおいて安全に動作をつなげるための、加速動作、減速動作、等速動作、停止動作などの動作方式を指す。 The operation method, refer to connect the safety operation in the switching of both operating modes, acceleration operation, deceleration, constant speed operation, the operation mode such as stopping operation.

次に、判定された動作モードに従って動作方式の判別とモータ14a,14b,14cの目標値を生成する(ステップS14)。 Next, determine the motor 14a of the operating regime in accordance with the determined operation mode, 14b, generating a target value of 14c (step S14). 生成された制御目標値と先に読み取った角度検出器16の検出値とからPID制御などの制御演算を行ってモータ出力を算出し、モータ駆動回路部23に出力する(ステップS15)。 It calculates a motor output detection value of the angle detector 16 read the generated control target value and the first from the performed control operation, such as PID control, and outputs to the motor drive circuit 23 (step S15).

なお、上記ステップS14では、後述する図9に示すように、操作デバイスから得られた値をもとに算出されたヨー軸目標角度θy、ロール軸目標角度θr、グリッパ軸目標角度θgを第1モータ軸角度θ1、第2モータ軸角度θ2、モータ14c軸角度θ3に変換するための逆キネマティック演算を行って目標値を生成する。 In the step S14, as shown in FIG. 9 to be described later, the value obtained from the operation device is calculated based on the yaw axis target angle [theta] y, the roll axis target angle [theta] r, the gripper shaft target angle θg first motor shaft angle .theta.1, the second motor shaft angle .theta.2, generating a target value by performing the inverse kinematics computation for converting the motor 14c shaft angle .theta.3.

次に、定義された種々の条件とセンサなどで読み取った状態とを比較し、状態判別を行う(ステップS16)。 Next, compared with the state read in such various conditions and sensor defined, performing the state determination (step S16). 判別された結果に基づき、制御装置2に装備されたランプや動作状態提示器25に出力を行う(ステップS17)。 Based on the discriminated result to output to the lamp and the operating status presenter 25 that is provided on the control unit 2 (step S17).

以下、作用部4のグリッパによる把持動作について説明する。 The following describes the gripping operation by the gripper of the action part 4. 説明の簡略化のために、図3のマニピュレータ本体1の先端部分の歯車の歯数がすべて等しいとすると、モータ14a,14b,14cの出力軸角度θ1、θ2、θ3と先端部の姿勢軸角度θy、θr、θgとの関係は、(1)式で表すことができる。 For simplicity of explanation, if the number of teeth of the gear of the tip portion of the manipulator 1 of FIG. 3 are all equal, the output shaft angle θ1 of motor 14a, 14b, 14c, .theta.2, attitude axis angle of θ3 and a distal end [theta] y, [theta] r, the relationship between the θg can be expressed by equation (1).

上述した(1)式における3×3行列は、逆キネマティック行列K -1と呼ばれるものである。 3 × 3 matrix in the equation (1) is what is called reverse kinematic matrix K -1.

グリッパ軸の把持動作のみについて考えると、他の姿勢軸を動作させない場合は、θy=θr=0であるから、θ3=−θgとなる。 Considering only the gripping operation of the gripper shaft, if not operated other attitude axes, since it is [theta] y = [theta] r = 0, the θ3 = -θg. つまり、モータ14a、14b、14cをθd=[0、0、−θg] と制御することでグリッパ軸をθgだけ動作させることができる。 That is, the motor 14a, 14b, 14c and θd = [0,0, -θg] the gripper shaft by controlling a T can be run only [theta] g. グリッパ軸角度は、ちょうど閉じた状態を0度、開く方向をプラス方向とし、グリッパ軸をマイナス方向に動作させるということは、より閉じる方向に力をかけるようにモータ14cを制御することを意味する。 The gripper axis angle is to state 0 degrees just closed, the opening direction and the positive direction, that operates the gripper shaft in the negative direction means to control the motor 14c to exert a force more closing direction .

さて、実際に把持力がどのように変化するかを評価するために、グリッパの片側である第2の作用部材326の把持面に感圧センサを貼り付け、その出力を測定するシステムを構築した。 Now, in order to assess whether actual gripping force how changes, paste the pressure sensor on the gripping surface of the second working member 326 is one side of the gripper, to construct a system for measuring the output . 上述したように、モータ駆動回路部55は電流制御方式を採用している。 As described above, the motor driving circuit 55 employs a current control method. また、グリッパ軸の角度制御はPD制御を基本として構築している。 Further, the angle control of the gripper shaft are building the PD control basis. すなわち、図5の制御装置2は、速度のPI制御、位置のPD制御および速度フィードフォワード制御を組み合わせた制御を行う。 That is, the control device 2 in FIG. 5, the speed PI control, the control that combines PD control and speed feedforward control position performs.

図7は制御装置2によって制御される把持角度と実際の把持力との関係を示す図である。 Figure 7 is a diagram showing the relationship between the actual gripping force between the gripping angle is controlled by the control device 2. 図示のように、把持角度と把持力とは1次相関関係になる。 As shown, the primary correlation between grip angle and the gripping force. 位置のI制御が含まれると、把持角度と把持力との関係は1次相関とはならないため、把持力調整には採用しない。 The inclusion of I control of the position, for the relationship between the grip angle and the gripping force does not become a primary correlation, not adopted in the gripping force adjustment.

図7において、領域1は、把持角度が小さくて、ワイヤ15cの伸び量と把持力が比例する領域である。 7, region 1, the gripping angle is reduced, a region where the elongation amount and the gripping force of the wire 15c is proportional. この領域1では、モータ14cは所望の目標角度に適度に制御される。 In the region 1, the motor 14c is appropriately controlled to a desired target angle. 領域2は、ワイヤ15cが伸びきってモータ14cのトルクと把持力が比例する領域であり、モータ14cは所望の目標角度からずれた角度に制御された状態を維持する。 Region 2, the wire 15c is fully extended an area of ​​torque and gripping force of the motor 14c is proportional to the motor 14c maintains the state of being controlled to an angle deviating from the desired target angle.

このように、領域1ではワイヤ15cの伸びと把持力は比例し、領域2では位置のI制御を含まない位置偏差が把持力と比例する。 Thus, in the region 1 extends the gripping force of the wire 15c is proportional, the position deviation that does not include the I control of the position in the region 2 is proportional to the gripping force. どちらの領域であっても、把持角度と把持力は1次相関の関係にある。 In either of the areas, the gripping angle and gripping force in a relation of 1 order correlation. 制御状態が領域1と2のどちらであるかは、モータ14cの制御偏差の大きさにより確認することができる。 Or control state is either region 1 and 2 can be confirmed by the magnitude of the control deviation of the motor 14c.

上述したように、グリッパ軸角度は、閉じた角度を0度と定義している。 As described above, the gripper axis angle has a closed angle is defined as 0 degrees. 例えば、何も物体を把持せずにグリッパ4を開き角45度から閉じ角35度すなわちθgを45度から−35度まで動作するようにモータ14cを制御した場合、実際のグリッパは45度から0度まで動作し、その後グリッパ角度が幾何的拘束を受けて実際にはそれ以上閉じる方向には動かないが、モータ14cはよりいっそう閉じる方向にグリッパ軸換算で35度だけ制御される。 For example, if nothing to control the motor 14c to operate from 45 degrees to close angle of 35 degrees i.e. θg from open angle 45 degrees gripper 4 without grasping an object to -35 degrees, the actual gripper 45 degrees operates to 0 °, actually, but does not move more closing direction subsequent gripper angle undergoing geometric constraint, the motor 14c is controlled by 35 degrees the gripper shaft equivalent to more closing direction. このとき、把持角度は35度となる。 In this case, the gripping angle is 35 degrees.

さて、図7に示すように、把持角度と実際の把持力とが1次相関関係にあるシステムを構築できた場合には、把持角度を目標に制御すれば、把持力を制御することができるといえる。 Now, as shown in FIG. 7, when the actual gripping force between the gripping angle could construct a system in the primary correlation, by controlling the gripping angle to the target, it is possible to control the gripping force it can be said that.

ここで、把持角度について説明を加えておく。 Here, a supplementary description will grip angle. 把持角度とは把持する方向に動作させたときの最大動作量である。 A gripping angle is the maximum operation amount when operated in a direction to grip. 先の説明で、45度から−35度まで動作させた場合は、把持角度が35度であると述べたが、45度から−45度まで動作させた後に−35度まで戻した場合に、把持角度が35度であるとは言わない。 If in the previous description, when operated from 45 degrees to -35 degrees, but the gripping angle is said to be 35 degrees, returning to -35 degrees after operating from 45 degrees to -45 degrees, grip angle does not say that is 35 degrees.

把持角度で把持力を制御する場合、すなわち角度制御にて把持力を制御する場合には、実際に物体を把持し始めたグリッパ角度を認識し、その角度を基点に把持角度分だけ制御する必要がある。 When controlling the gripping force in the gripping angle, that is, when controlling the gripping force at an angle control is actually recognizing the gripper angle began to grip the object, it needs to be controlled by grasping angle amount of the angle to the base there is. グリッパ4に力センサがあれば直接物体を把持したことを認識でき、角度センサがあれば実際のグリッパ角度が停止あるいは目標値との偏差の発生で物体を把持したことを推定できるが、本実施形態は、センサレスで把持力制御を行うことを念頭に置いており、センサを用いた制御は想定外である。 The gripper 4 can recognize that grips directly an object if there is a force sensor, the actual gripper angle if the angle sensor can be estimated that the object is gripped by the occurrence of the deviation between the stop or target value, present form is in mind to make the gripping force control sensorless control using a sensor is unexpected.

そこで、本実施形態では、把持開始推定オブザーバを用いて把持開始角度を推定する。 Therefore, in this embodiment, the gripping start angle estimated using a gripping start estimation observer. この把持開始推定オブザーバは、図5の制御装置2の内部、例えば演算部22の内部に設けられる。 The gripping start estimating observer is inside the control device 2 of FIG. 5, for example, it is provided inside the operation portion 22.

図8は把持開始推定オブザーバを有する制御装置2が行う処理動作の一例を示す制御システム図である。 Figure 8 is a control system diagram showing an example of the processing operation which the control unit 2 having a gripping start estimation observer performed. 図8に示すように、制御装置2は、把持開始推定オブザーバ(オブザーバ信号生成手段)51と、把持角度決定部52と、逆キネマティック行列演算部53と、PID制御部54と、モータ駆動部55と、目標把持角度設定部56と、最閉角度決定部57と、操作量更新部58とを有する。 8, the control device 2 includes a gripping start estimation observer (observer signal generating means) 51, a gripping angle determining section 52, an inverse kinematic matrix calculator 53, a PID control unit 54, a motor drive unit with a 55, a target grip angle setting unit 56, the highest close angle determination unit 57, and an operation amount updating unit 58.

把持開始推定オブザーバ51は、後で詳述するように、把持開始角度を推定するための把持開始角度推定信号(把持開始角度推定信号)faを生成する。 Gripping start estimation observer 51, as will be described later, gripping start position estimation signal for estimating a gripping start angle (gripping start position estimation signal) to generate the fa.

把持角度決定部52は、把持開始角度推定信号faと操作デバイスの操作量に基づいて、ヨー軸およびロール軸に関する参照目標値θa(y,r)と、グリッパ軸に関する参照目標値θa(g)を生成する。 Gripping angle determining unit 52 based on the operation amount of the grip start position estimation signal fa and operation device, the reference target value θa (y, r) about the yaw axis and the roll axis, the reference target value .theta.a relates gripper shaft (g) to generate.

逆キネマティック行列演算部53は、上述した(1)式に基づいて、モータ14a,14b,14cの出力軸角度θdを算出する。 Inverse kinematics matrix calculator 53, based on the equation (1), calculates the motor 14a, 14b, the output shaft angle θd of 14c. 上述した(1)式を簡易的に表すと、以下の(2)式で表される。 Expressed described above the (1) in a simple manner, it is expressed by the following equation (2). θd=[θ1,θ2,θ3] 、θa=[θy,θr,θg] である。 θd = [θ1, θ2, θ3 ] is T, θa = [θy, θr , θg] T.

θd=K -1 θa …(2) θd = K -1 θa ... (2 )

図8のPID制御部54は、逆キネマティック行列演算部53の出力値(モータの目標値)θdとモータの実測値θmとの偏差をフィードバック制御する。 Figure PID controller 54 of the 8 feedback controls the deviation between inverse kinematic matrix output value of the arithmetic unit 53 (the target value of the motor) [theta] d and motor measured value .theta.m. より具体的には、PID制御部54は、偏差が小さくなるように、モータ駆動部55に供給する電圧Vinを制御する。 More specifically, PID control unit 54, so that the deviation becomes small, and controls the voltage Vin supplied to the motor driving section 55.

目標把持角度設定部56は、例えば操作デバイスとは別個に設けられるボリューム等の値を読み取って、図7に示した把持角度と把持力の1次相関関係に基づいて、所望の把持力を得るための設定把持角度θhを設定する。 Target grip angle setting unit 56, for example, the operation device reads the value of the volume or the like that is separately provided, based on the first order correlation of the gripping force and gripping angles shown in FIG. 7, to obtain the desired gripping force setting the set gripping angle θh for.

最閉角度決定部57は、以下の(3)式に基づいて、把持開始角度θtから設定把持角度θhを減じることにより、グリッパ軸の最閉角度θcを算出する。 Most close angle determination unit 57, based on the following equation (3), by subtracting the set gripping angle θh from the grip start angle [theta] t, to calculate the most close angle θc of the gripper shaft.

θc=θt−θh …(3) θc = θt-θh ... (3)

操作量更新部58は、最閉角度決定部57からの決定結果に応じて、グリッパ軸目標値θgを逆キネマティック演算部53に送り、グリッパ軸目標値θgを更新する。 Operation amount updating section 58, in accordance with the determination result from the most close angle determination unit 57 sends a gripper axis target value [theta] g Conversely kinematics calculation unit 53 updates the gripper axis target value [theta] g.

把持開始推定オブザーバ51は、モータ角速度ωmを検出する微分回路61と、モータ加速度αmを検出する微分回路62と、クーロン摩擦項算出部63と、粘性摩擦項算出部64と、慣性項算出部65と、粘性摩擦項と慣性項を加算する加算器66と、クーロン摩擦項を考慮に入れるか否かを切替える第1の切替部SW1と、モータ電流から加算器66の出力と第1の切替部SW1の出力とを減じる減算器67と、この減算器67の出力に含まれる高周波ノイズを除去するローパスフィルタ(LPF)68と、各モータに応じたLPF68の出力のうちゼロでないものを一つ選択する第2の切替部SW2とを有する。 Gripping start estimation observer 51, a differentiating circuit 61 which detects a motor angular velocity .omega.m, a differentiating circuit 62 for detecting a motor acceleration .alpha.m, a Coulomb friction term calculation unit 63, and the viscous friction term calculation unit 64, the inertia term calculation unit 65 When, an adder 66 for adding the viscous friction term and inertia term, a first switching unit SW1 for switching whether take into account the Coulomb friction term, the output of the adder 66 from the motor current and the first switching unit a subtracter 67 for subtracting the output of SW1, selects one low-pass filter (LPF) 68 for removing high-frequency noise contained in the output of the subtracter 67, those non-zero in the output of the LPF68 corresponding to each motor and a second switching unit SW2 to.

把持開始推定オブザーバ51の内部構成のうち、第2の切替部SW2を除く部分は、各モータ14a,14b,14cのそれぞれごとに設けられている。 Of the internal structure of the grasping start estimation observer 51, the portion excluding the second switching unit SW2, each motor 14a, 14b, are provided for each 14c. 図9は把持開始推定オブザーバ51のより詳細な内部構成の一例を示すブロック図である。 Figure 9 is a block diagram showing an example of a more detailed internal structure of the grasping start estimation observer 51. 図示のように、モータ14a,14b,14cのそれぞれごとに、微分回路61からローパスフィルタ68までの部分(オブザーバ信号候補生成手段)51a,51b,51cが設けられており、各ローパスフィルタ68の出力のうちいずれか一つが第2の切替部SW2で選択されて、把持開始角度推定信号faとして出力される。 As shown, the motor 14a, 14b, for each 14c, part of the differentiating circuit 61 to the low-pass filter 68 (the observer signal candidate generator means) 51a, 51b, and 51c are provided, the output of each low-pass filter 68 one of is selected by the second switching unit SW2, and output as a gripping start position estimation signal fa. この第2の切替部SW2はオブザーバ信号選択手段に対応する。 The second switching unit SW2 corresponds to the observer signal selecting means.

なお、1軸方向しか制御を行わないマニピュレータシステムの場合は、第2の切替部SW2は不要となる。 In the case of one axial direction only not controlled manipulator system, the second switching unit SW2 is not necessary.

以下、把持開始推定オブザーバ51の処理動作について説明する。 The following describes the processing operation of the gripping start estimation observer 51. 電動モータの場合、モータ電流はモータ出力トルクに比例するため、モータ出力軸からグリッパ軸までの機構パラメータに基づいて、モータ出力軸とグリッパ軸の軸間の関係が求まり、ひいてはモータ電流とグリッパ軸のトルクの関係が求まる。 If the electric motor, since the motor current is proportional to the motor output torque, based on the kinematic parameters of the motor output shaft to the gripper shaft, Motomari relationship between the axes of the motor output shaft and the gripper shaft, thus the motor current and the gripper shaft determined relationship of torque. 把持力は、グリッパ軸のトルクと、グリッパ軸と把持点との距離とで算出できる。 Gripping force can be calculated by the torque of the gripper shaft, the distance between the gripping point and the gripper shaft.

モータ電流やモータ角度等の情報を用いた外乱オブザーバを利用することで、把持力を推定することができる。 By using the disturbance observer using information such as a motor current or a motor angle, it is possible to estimate the clamping force. ここで、外乱とは、モータ電流から機構パラメータを換算して差し引いた残りの値である。 Here, disturbance and are the remaining minus in terms kinematic parameters from the motor current. 把持機構を備えている場合は、物体を把持することにより生じる外力が把持力であると想定している。 If you have a gripping mechanism, it is assumed that an external force caused by gripping the object is a gripping force.

図8の把持開始推定オブザーバ51内の慣性項算出部65は、モータ加速度αに係数Jnを乗じた項を算出する。 Inertia term calculation portion 65 of the grip start the estimation observer 51 in FIG. 8 calculates a term multiplied by a coefficient Jn the motor acceleration alpha. 粘性摩擦項算出部64は、モータ速度ωに粘性摩擦係数fvを乗じた項を算出する。 Viscous friction term calculating unit 64 calculates a term multiplied by a viscous friction coefficient fv in motor speed omega. クーロン摩擦項算出部63は、モータ速度ωの符号関数にクーロン摩擦係数fcを乗じた項を算出する。 Coulomb friction term calculating unit 63 calculates a term multiplied by a Coulomb friction coefficient fc in the sign function of the motor speed omega. これら3つの算出部63〜65をモータ電流iから減じることにより、グリッパ軸のトルク、すなわち把持力に変換した出力を得ることができる。 By subtracting these three calculation section 63 to 65 from the motor current i, it is possible to obtain the torque of the gripper shaft, that was converted into the gripping force output.

なお、回転については、回転速度を角速度、回転加速度を角加速度と呼ぶのが正式であるが、機構系においては、回転や並進を複合したものである場合を踏まえて、ここでは説明上区別していない。 Note that the rotation angular velocity of the rotation speed, but that the rotational acceleration is referred to as the angular acceleration is authoritative, in the mechanical system, based on the case of rotation or translation is obtained by complex, here distinguished explanation not. 同じ理由で、位置制御と角度制御は同義である。 For the same reason, the position control and the angle control are synonymous.

図8の慣性係数Jn、粘性摩擦係数fv、クーロン摩擦係数fcは設計値や実験により求められる値である。 Inertia coefficient Jn 8, the viscous friction coefficient fv, Coulomb friction coefficient fc is a value determined by the design values ​​and experiments. 慣性項を算出するにあたり、加速度情報を得るために離散的な角度情報の差分処理を二度行うとノイズが増幅されることがある。 In calculating the inertia term, sometimes it performed difference processing twice discrete angular information noise is amplified to obtain acceleration information. そこで、特許文献3の図2に示す等価的なブロック図のようなものを用いて慣性項を求めてもよい。 Accordingly, it may be calculated inertia term by using a kind of equivalent block diagram shown in FIG. 2 of Patent Document 3.

図10は図8の把持開始推定オブザーバ51の特性を示す図である。 Figure 10 is a diagram showing characteristics of a gripping start estimation observer 51 of Fig. 図10(a)はグリッパ4の開動作→ヨー軸動作→ロール軸動作→グリッパ4の閉動作→グリッパ4の開動作を連続的に行った場合のグリッパ軸角度θg、ヨー軸角度θy、ロール軸角度θr、モータ14cの軸角度θ3が変化する様子を示している。 10 (a) shows the gripper axis angle θg in the case of performing the opening operation → yaw axis operation → opening operation of the closing → grippers 4 of the roll axis motion → grippers 4 of the gripper 4 to continuously yaw axis angle [theta] y, roll axis angle [theta] r, shows how the axial angle θ3 varies the motor 14c.

図10(b)は図8の把持開始推定オブザーバ51内に第1および第2の切替部SW1,SW2を設けない場合に生成されたオブザーバ信号feの変化する様子を示す図である。 Figure 10 (b) is a diagram showing the state of change of the first and second switching unit SW1, generated when SW2 and not provided the observer signal fe to grip start the estimation observer 51 of Fig. 図10(c)は把持力センサの出力を示す図である。 10 (c) is a diagram showing the output of a gripping force sensor.

オブザーバ信号feは、モータ電流から、慣性係数Jn、粘性摩擦係数fvおよびクーロン摩擦係数fcの3つの項を減じることにより生成されるが、このオブザーバ信号feを用いて推定した把持力を、実際に測定した把持力と比較すると、図10(b)に示すように、オブザーバ信号feを用いて推定した把持力(電流換算値)は振動性の特徴を持ち、動作速度変化点付近にはスパイク状の信号も現れる。 Observer signal fe is the motor current, the inertia coefficient Jn, but is generated by subtracting the three terms of the viscous friction coefficient fv and Coulomb friction coefficient fc, the gripping force that is estimated using the observer signal fe, actually compared to the measured gripping force, as shown in FIG. 10 (b), the gripping force that is estimated using the observer signal fe (current conversion value) has the characteristics of oscillatory, in the vicinity of the operating speed changing point spiky also appear in the signal. これは、伝達機構系において発生するクーロン摩擦の非線形性や、動作角度や位置による摩擦係数の微小変化によるものであり、実測した把持力との差異が大きくなる。 This nonlinearity and Coulomb friction generated in the transmission mechanism system, is due to small changes in the friction coefficient due to the operation angle and position, the difference between the actually measured clamping force is increased.

このように、クーロン摩擦等を考慮に入れて生成したオブザーバ信号feでは、作業部7が微細な構造で、把持力が相当な大きさの場合には、ノイズ成分と小さな把持力とを区別するのは困難であり、把持開始点を正確に検出することができない。 Thus, the observer signal fe generated taking into account the Coulomb friction, working unit 7 is in a fine structure, when gripping force is sizable distinguishes a small gripping force and noise components the it is difficult, it is impossible to accurately detect the grip start point. 例えば、ノイズの大きさを考慮に入れて、判定既定値をプラス側(例えばf0)に設定しておき、判定既定値より推定把持力が大きくなった点を把持開始と推定することも可能だが、この場合、小さな把持力を認識できない。 For example, taking into account the size of the noise may be set to determine the default value on the positive side (e.g. f0), but also possible to estimate the point at which the estimated gripping force than the determination default value is larger gripping start and , in this case, it can not recognize a small gripping force. カットオフ周波数がより低いフィルタを使用してノイズ成分を除去することもできるが、生成された信号に位相遅れが生じてしまい、把持開始時点T2を認識するのに時間的な遅れが生じ、その結果、実際の把持開始角度と推定角度に大きなずれが生じてしまう。 Can also be cut-off frequency to remove noise components using a lower filter, phase delay will occur in the generated signal, occurs a time lag to recognize a gripping start time T2, the a result, a large deviation of the actual gripping start angle and the estimated angle occurs.

このような事情で、本実施形態では、把持開始の認識に遅れが生じずにノイズ成分と区別できるように、把持開始推定オブザーバ51内に第1の切替部SW1を設けている。 In such circumstances, in the present embodiment, as delayed recognition of the gripping start it can be distinguished from the noise component without causing, is provided with a first switching unit SW1 in gripping start estimation observer 51. この第1の切替部SW1は、グリッパ軸動作時にはオフして、クーロン摩擦項を考慮に入れずに把持開始推定信号faを生成し、グリッパ軸動作時以外はオンして、クーロン摩擦項を考慮に入れて同信号faを生成する。 The first switching unit SW1 is at the time of the gripper shaft operation off, generates a gripping start estimation signal fa without taking into account the Coulomb friction term, it is turned on except when the gripper shaft operation, considering the Coulomb friction term to generate the same signal fa put in. クーロン摩擦項を考慮に入れないとは、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れないことと等価である。 And does not take into account the Coulomb friction term does not depend on at least one of the magnitude of the angular velocity and acceleration is equivalent to not take into account the term depending on the direction. 第1の切替部SW1のように、クーロン摩擦項を計算した後にその出力を遮断する代わりに、グリッパ軸動作時には、クーロン摩擦係数fcをゼロとして、クーロン摩擦項の計算結果がゼロになるようにしてもよい。 As in the first switching unit SW1, instead of blocking the output after calculating the Coulomb friction term, during gripper shaft operation, as zero Coulomb friction coefficient fc, the calculation result of the Coulomb friction term is set to be zero it may be.

図8の把持開始推定オブザーバ51を正常に動作させるには、把持開始のタイミングを精度よく認識しなければならない。 To operate properly gripping start estimation observer 51 of Figure 8 must recognize accurately the timing of the gripping start. 把持開始の認識は、グリッパ4を閉じるとき、すなわち、グリッパ軸速度ωgが負の場合に起こるため、第1の切替部SW1は、グリッパ軸速度ωgが負になったときにオフするように切替制御される。 Recognition of the gripping started, when closing the gripper 4, i.e., because the gripper shaft speed ωg happens if negative, the first switch unit SW1 is switched to turn off when the gripper shaft speed ωg is negative It is controlled. なお、グリッパ軸速度は、図8の把持角度決定部52の出力である参照目標値θa(g)を微分処理することにより得られる。 Incidentally, the gripper shaft speed is obtained by differentiating the reference target value θa which is the output of the gripping angle determination unit 52 of FIG. 8 (g).

図10(d)は把持開始時に第1の切替部SW1をオフした状態で得られた把持開始推定信号faの波形図である。 Figure 10 (d) is a waveform diagram obtained gripping start estimated signal fa in the OFF state of the first switching unit SW1 at the start gripping. 図10(d)の把持開始推定信号faの波形を見ればわかるように、クーロン摩擦項を考慮に入れないことから、グリッパ4を閉じ始めた時点T0以降での信号faの信号レベルは図10(b)の信号feの信号レベルよりも大きい。 As can be seen from the waveform of the gripping start estimation signal fa in FIG. 10 (d), since it does not take into account the Coulomb friction term, the signal level of the signal fa at later time points T0 began closed gripper 4 10 (b) greater than the signal level of the signal fe of. そして、信号faが所定のしきい値f1を超えた時点T1で、把持開始角度θtに達したと判断される。 Then, at the time T1 when the signal fa exceeds a predetermined threshold value f1, is determined to have reached the gripping start angle [theta] t. この時刻T1は、図10(b)で把持開始を認識する時刻T2よりも前であり、把持開始を迅速かつ正確に推定できることがわかる。 The time T1 is before the gripping start than to recognize the time T2 in FIG. 10 (b), the it can be seen that quickly and accurately estimate the grip start.

把持開始角度θtが検出されると、上述した(3)式により、グリッパ軸の最閉角度θcを求めることができる。 When gripping start angle θt is detected by the above-described (3), can be obtained most close angle θc of the gripper shaft.

このように、本実施形態では、所望の把持力をまず設定して、角度(位置)制御でグリッパ軸を閉じる動作を行い、把持力開始推定オブザーバの出力にて把持開始角度を認識したら、次にグリッパ軸の最閉角度を設定し、次にグリッパ軸角度をグリッパ軸の最閉角度まで角度(位置)制御する。 Thus, in the present embodiment, when it first sets the desired gripping force, performs angle (position) control operation for closing the gripper axis, recognizes the gripping start angle at the output of the gripping force start estimation observer, following to set the maximum close angle of the gripper shaft, then the angle gripper axis angle to the lowest close angle of the gripper shaft (position) is controlled.

なお、把持力の設定または把持角度の設定は、制御装置や操作指令部3に付属のボリューム等を用いて設定してもよいし、作業部7の種類に応じて予め決められた設定値を付与してもよい。 The setting of the setting or gripping angle of the gripping force, to the control unit and the operation command unit 3 may be set by using a volume that came, the set value determined in advance in accordance with the type of the working unit 7 it may be applied. 把持中にグリッパ軸の最閉角度を調整すると、把持しながら把持力を微調整することが可能となる。 Adjusting the most close angle of the gripper shaft during gripping, the gripping while gripping force can be finely adjusted.

図8の把持開始推定オブザーバ51では、慣性項や粘性項を考慮に入れているが、例えばマニピュレータが微細構造で慣性の影響を無視できる場合には、慣性係数Jnをゼロにしてもよい。 In gripping start estimation observer 51 of Fig. 8, but takes into account the inertia term and viscosity term, for example, when the manipulator is negligible the effect of inertia in microstructure may inertia coefficient Jn zero. 粘性摩擦項についても必要に応じて無視してもよい。 It may be ignored if necessary also the viscous friction term. このように、把持開始推定オブザーバ51の具体的構成は、適宜変更可能である。 Thus, a specific configuration of the grasping start estimation observer 51 can be changed as appropriate.

図8の最閉角度決定部57は、例えば図11のフローチャートに示す処理手順に従ってグリッパ軸の最閉角度を設定する。 Most close angle determination unit 57 of FIG. 8 sets the highest close angle of the gripper shaft for example according to the processing procedure shown in the flowchart of FIG. 11. この図11のフローチャートは例えば図6のステップS14の中で行われるものであり、作用フラグ決定手段に対応する。 Flowchart in FIG. 11 is intended to be performed in step S14 in FIG. 6, for example, corresponds to the action flag decision means.

まず、図8の把持開始推定オブザーバ51から出力される把持開始推定信号faを取得する(ステップS21)。 First, to obtain the grasping start estimation signal fa output from the grip start estimation observer 51 of FIG. 8 (step S21). 次に、把持フラグ(作用フラグ情報)の状態を確認する(ステップS22)。 Next, to check the state of the gripping flag (action flag information) (step S22). ここで、把持フラグとは、グリッパが対象物を把持しているか否かを示すフラグ情報であり、把持している場合にオンし、把持していない場合にオフする。 Here, the gripping flag, the gripper is flag information indicating whether or not gripping an object, and turned on when you are gripped, and is turned off when not gripping.

把持フラグがオフと判断された場合は、グリッパ軸の目標角度θgがグリッパ軸の最大角度θpより小さいか否かを判定する(ステップS23)。 If the gripping flag is determined to OFF, it determines the target angle θg of the gripper shaft whether maximum angle θp is smaller than the gripper shaft (step S23). θg<θpの場合は、グリッパ速度ωgが負か否かを判定する(ステップS24)。 For [theta] g <theta] p, it determines the gripper velocity ωg is whether negative or not (step S24).

ωgが負であれば、グリッパ軸が閉じる方向に動作していると判断できる。 If ωg is negative, it can be determined to be operating in the direction in which the gripper shaft is closed. そこで、次に、把持開始推定信号faが所定のしきい値f1以上か否かを判定する(ステップS25)。 Accordingly, next, the grip start estimation signal fa is determined whether or not a predetermined threshold value f1 or more (step S25). fa≧f1であれば、図10(d)の時刻t1と同様に把持を開始したと判断して、把持フラグをオンする(ステップS26)。 If fa ≧ f1, it is determined that the time t1 shown in FIG. 10 (d) and was started gripping similarly turns on the gripping flag (step S26). そして、この時点でのグリッパ軸の最閉角度θgを把持開始角度θtに設定する(ステップS27)。 Then, it sets the highest close angle θg of the gripper shaft at this point gripping start angle [theta] t (Step S27). 次に、上述した(3)式に従って、把持開始角度θtから設定把持角度θhを減じてグリッパ最閉角度θcを算出する(ステップS28)。 Then, according to the aforementioned (3), from the grip start angle θt by subtracting the set gripping angle θh calculates the gripper top close angle .theta.c (step S28). 上述したステップS25〜S27の処理が作用力開始角度推定手段に対応し、ステップS28の処理が限界角度検出手段に対応する。 Processing in steps S25~S27 described above corresponds to the acting force start angle estimating means, the processing of step S28 corresponds to the limit angle detecting means.

上述したステップS23〜S25のいずれかの判定結果がNOの場合には、把持が開始されたとは判断されない。 If any of the decision result in the step S23~S25 described above is NO, it is not determined from the grip has been started.

一方、ステップS22で把持フラグがオンであると判定された場合は、グリッパ軸の目標角度θgがグリッパ軸の最大角度θpに等しいか否かを判定する(ステップS29)。 On the other hand, when the gripping flag is determined to be on in step S22, it determines the target angle θg of the gripper axis or not equal to the maximum angle θp of the gripper shaft (step S29). 等しくない場合には、目標角度θgが把持開始角度θtより大きいか否かを判定する(ステップS30)。 If not equal, determines the target angle θg is whether gripping start angle θt is larger than (step S30).

ステップS29でθgがθpに等しいと判定された場合、またはステップS30でθg>θtと判定された場合は、把持動作は終了したと判定して、把持フラグをオフする(ステップS31)。 If [theta] g in step S29 is determined to be equal to theta] p, or if it is determined that [theta] g> [theta] t at step S30, the grasping operation was determined to be ended, turns off the gripping flag (step S31). 次に、グリッパ軸の最小角度θnを把持開始角度θtとして設定する(ステップS323)。 Next, to set the minimum angle θn of the gripper shaft as gripping start angle [theta] t (step S323). ここで、把持開始角度θtをグリッパ軸の最小角度に設定するのは、把持開始角度をリセットするためである。 Here, to set the gripping start angle θt to the minimum angle of the gripper shaft is for resetting the gripping start angle. 把持動作が終了したのに把持開始角度をリセットしないとすると、前回把持した対象物よりも厚さが薄いものを把持するまでグリッパが動作しないといった不具合が生じうるためである。 When grasping operation is not to reset the grip start angle for finished, because the gripper may occur inconvenience not work until gripping the ones thinner than the object the previously gripped. このとき、把持開始角度をゼロに設定してもよいが、後述するように初期誤差が考えられるため、動作可能範囲での最小角度に設定している。 At this time, the grasping start angle may be set to zero, because the initial error is considered as will be described later, is set to the minimum angle of the operating range.

ステップS29でθgがθpに等しくないと判定されるか、またはステップS30でθg≦θtと判定された場合には、まだ把持動作を行っていると判断して把持フラグをオンのまま維持する。 Or at step S29 [theta] g is not equal to theta] p, or when it is determined that [theta] g ≦ [theta] t at step S30, still maintains it is determined that doing gripping operation while the gripping flag ON. この場合は、グリッパ軸の目標角度θgが最閉角度θtに達するまで角度制御が行われる。 In this case, the angle control is performed until the target angle θg of the gripper shaft reaches the highest close angle [theta] t. この角度制御は制御装置2が行うものであり、角度制御手段に対応する。 The angle control is intended to control unit 2 performs, it corresponds to the angle control means.

図3に示した作業部7を駆動する駆動部6は、干渉系の機構を有する。 Driver 6 for driving the working unit 7 shown in FIG. 3 includes an interference-based mechanisms. モータ14a,14b,14cの出力軸角度θd=[θ1、θ2、θ3] と作業部7の先端部の姿勢軸θa=[θy、θr、θg] との関係は、上述した(1)式および(2)式で表される。 Motor 14a, 14b, 14c of the output shaft angle θd = [θ1, θ2, θ3 ] T and work part 7 of the distal end portion of the posture axis θa = [θy, θr, θg ] relationship with T are described above (1) formula and (2) below. これらの式では、マニピュレータの先端部分の歯車の歯数がすべて等しいと仮定している。 In these equations, it is assumed that the number of teeth of the gear of the distal end portion of the manipulator are all equal.

グリッパ軸の把持動作のみについて考えると、他の姿勢軸を動作させない場合はθy=θr=0であるため、θ3=−θgとなる。 Considering only the gripping operation of the gripper shaft, because if not operated other orientation axis is [theta] y = [theta] r = 0, the θ3 = -θg. すなわち、モータをθd=[0、0、−θg] と制御することで、グリッパ軸をθgだけ動作させることができる。 That is, by controlling the motor θd = [0,0, -θg] T and can be operated gripper axis by [theta] g.

グリッパ軸角度は、閉じた状態を0度、開く方向をプラス方向としている。 The gripper axis angle is closed to 0 °, the opening direction as a positive direction. したがって、グリッパ軸をマイナス方向に動作させるということは、より閉じる方向に力をかけるようにモータを制御することを意味する。 Accordingly, the fact that to operate the gripper shaft in the negative direction means that controls the motor so as to apply a force to a more closing direction.

グリッパ軸を動作させる場合、把持開始推定信号faはモータ14cの電流値だけで生成できる。 When operating the gripper shaft, the grip start estimation signal fa can be produced in only the current value of the motor 14c. モータ14cの電流値の波形は図10(b)のようになる。 The waveform of the current value of the motor 14c becomes as shown in FIG. 10 (b). モータ14cの電流値だけでグリッパ軸の動作が決まるのは、1自由度しか持たないグリッパを備えたマニピュレータや、2以上の自由度を持っていてもグリッパ軸を駆動するアクチュエータが他の姿勢軸と干渉しない非干渉構造の場合でも同様である。 The operation of the gripper shaft is determined only by the current value of the motor 14c is 1 manipulators and provided with a gripper having only freedom, two or more actuators other attitude axes for driving the gripper shaft also have a degree of freedom the same applies to the case of non-interference structure which does not interfere with.

ところが、例えば図3の作業部7を備えたマニピュレータなどのように、他の姿勢軸と同時にグリッパ軸を動作させる場合は、モータ14cの電流値だけで把持力が決まるとは限らない。 However, for example, as in the manipulator with a working unit 7 of FIG. 3, when operating the gripper shaft at the same time as the other attitude axes, not necessarily gripping force is determined by only the current value of the motor 14c.

そこで、干渉系機構を備えたマニピュレータを念頭に置いて、トルク干渉を考慮に入れて、把持力とモータトルクの関係を求めてみる。 Therefore, at a manipulator provided with an interference-based mechanism in mind, taking into account the torque interference, try to seek a relation gripping force and the motor torque.

仮想仕事の原理により、モータ14a,14b,14cのトルクτ1、τ2、τ3と作業部7の姿勢軸であるヨー軸回り、ロール軸回り、グリッパ軸回りのトルクτy、τr、τgとの関係は、以下の(4)式で表される。 The principle of virtual work, the motor 14a, 14b, 14c of the torque .tau.1, .tau.2, .tau.3 and around the yaw axis is a posture axis of the working unit 7, about a roll axis, the gripper axis torque .tau.y, .tau.r, relationship with τg is is expressed by the following equation (4).

把持力が出力されている状態とは、トルクτgが出力されている状態であり、他の姿勢軸の出力をゼロにすれば、上記(4)式から、各モータ14a,14b,14cのトルクτ1、τ2、τ3は、以下の(5)式で求められる。 A state in which the gripping force is output, a state in which torque τg is output, if the output of the other orientation axis to zero, from equation (4), the motors 14a, 14b, 14c torque .tau.1, .tau.2, .tau.3 is determined by the following equation (5).

(5)式を見ればわかるように、モータ14c以外のモータ14a,14bにも把持トルクの影響が現れることになり、各モータ軸に関するオブザーバ信号により把持開始を推定できることがわかる。 (5) As can be seen from equation motor 14a other than the motor 14c, also will be the influence of the gripping torque appears to 14b, it can be seen that estimate the grip initiated by observer signals for each motor shaft.

端的な例として、マニピュレータのグリッパ軸とヨー軸の動作が同期して行われる場合を説明する。 As typical example, the case where the operation of the gripper and yaw axes of the manipulator are performed synchronously. ヨー軸動作角度とグリッパ軸動作角度が同量だけ同期して動作するとき、モータ14a、14bの動作速度は等しく、モータ14cの速度はゼロ、すなわち停止した状態で把持動作を行うことになる。 When the yaw axis operation angle and the gripper axis operation angle operated synchronously by the same amount, the motor 14a, the operation speed of 14b are equal, the speed of the motor 14c will be performed gripping operating with zero, i.e. stopped.

図12は、グリッパ軸を単独動作させた後、引き続いてグリッパ軸とヨー軸が同期して動作する場合の動作波形図である。 12, after the gripper shaft is independent operation is an operation waveform diagram in the case of operating synchronously gripper and yaw axes are subsequently. 図12(a)はモータ角度の波形図、図12(b)はモータ電流の波形図、図12(c)は把持開始推定信号faの波形図、図12(d)は把持力の波形図であり、これらの図の横軸はいずれも時間である。 12 (a) is a waveform diagram of the motor angle, and FIG. 12 (b) shows a waveform of a motor current, Fig. 12 (c) waveform diagram of gripping start estimation signal fa, FIG 12 (d) shows a waveform of the gripping force and is a both horizontal axes of these figures times.

図12(a)に示すように、グリッパ軸が単独で動作しているときは(時刻t0〜t2)、モータ14cのみが動作しており、グリッパ軸とヨー軸が同期して動作しているときは(時刻t2〜t4)は、把持動作中であってもモータ14cは停止し、モータ14bが動作していることを示している。 As shown in FIG. 12 (a), when the gripper shaft is operated alone is operating in synchronization (time t0 to t2), only the motor 14c is operating, the gripper axis and yaw axis when (time t2 to t4), the motor 14c even during gripping operation stops, indicating that the motor 14b is operated.

このように、時刻t2〜t4では、グリッパで把持動作をしている間、モータ14cが停止中であるため、図12(b)に示すようにモータ14cの電流値はほぼ一定となっており、角度の変化がないため、オブザーバ信号の変化も生じない。 Thus, at time t2 to t4, since while the gripping operation by the gripper, the motor 14c is stopped, the current value of the motor 14c, as shown in FIG. 12 (b) is almost constant , since there is no change in angle, it does not occur a change in the observer signal. しかしながら、実際には、図12(d)に示すように、把持力は生じているため、上述のようにモータ14cの軸にも反力が働いているはずである。 However, in practice, as shown in FIG. 12 (d), since the gripping force is caused, it should working reaction force to the shaft of the motor 14c, as described above.

本来ならば、モータ14cが停止中であっても、反力が働くと電流値に変化が生じるが、動力伝達機構の摩擦により把持力とモータトルクとの間にヒステリシスが生じて、モータ軸まで把持力が伝わらなくなる。 Would otherwise also the motor 14c is a stopped, a change in current value when the reaction force acts occurs, hysteresis is generated between the gripping force and the motor torque by the friction of the power transmission mechanism, to the motor shaft the gripping force is not transmitted. このようなときは、停止しているモータ14c以外で、トルク干渉の関係から反力が加わる軸のうち実際に駆動しているモータ、ここではモータ14bの把持開始推定信号faを用いて把持開始点を推定すればよい。 In such a case, other than the motor 14c is stopped, the motor is actually driven among the axial reaction force is applied from the relationship of the torque interference, gripping initiated with gripping start estimation signal fa of the motor 14b here point a may be estimated.

図12(c)は、時刻t0〜t2の間はモータ14cに対応する把持開始推定信号faを利用し、時刻t2〜t4ではモータ14bに対応する把持開始推定信号faを利用して、それぞれ把持開始を推定する例を示している。 FIG. 12 (c), during the time t0~t2 utilizes a gripping start estimation signal fa corresponding to utilize gripping start estimation signal fa corresponding to the motor 14c, the time t2~t4 the motor 14b, respectively gripping It shows an example for estimating the start.

把持動作時に、ヨー軸、ロール軸およびグリッパ軸が同期して動作し、モータ14bとモータ14cの速度がいずれもゼロになる場合もありうるが、この場合はモータ14aの把持開始推定信号faに基づいて把持開始点を推定すればよい。 During the gripping operation, the yaw axis, a roll axis and a gripper shaft operate synchronously, the speed of the motor 14b and a motor 14c may sometimes both zero, the gripping start estimation signal fa in this case the motor 14a may be estimated grip starting point based.

すべてのモータ14a,14b,14cの速度が同時にゼロになった状態で把持動作を行うことはありえないため、いずれか一つのモータの把持開始推定信号faで把持開始を推定することができる。 All motors 14a, 14b, since the speed of 14c is impossible is to perform the gripping operation in the state became zero at the same time, it is possible to estimate the grip start at either one of the grasping start estimation signal fa of the motor.

このように、トルク干渉の関係から把持力のかかるモータ軸を特定し、特定したモータ軸に対応するモータのうち、モータ速度に応じて把持開始推定信号faを切替えるため、作用部4の種々の動きによらず、把持開始点を精度よく推定することができる。 Thus, to identify the motor shaft consuming gripping force from the relationship of the torque interference, among motor corresponding to the specified motor shaft, for switching the gripping start estimation signal fa in response to the motor speed, various action part 4 regardless of the motion, the grip start point can be estimated accurately. より具体的には、図8に示すように、モータ軸速度ωdに基づいて、第2の切替部SW2を切替えることにより、いずれか一つのモータに対応する把持開始推定信号faを選択する。 More specifically, as shown in FIG. 8, based on the motor shaft speed .omega.d, by switching the second switching unit SW2, selects a gripping start estimation signal fa corresponding to any one of the motor.

モータ軸速度がゼロとなるような特殊な場合でないときは、上述した(5)式より、把持トルクの反力が最も大きくなる軸はモータ14aの軸である。 When the motor shaft speed is not the case such a special becomes zero, from the above-described (5), the axial reaction force becomes the largest of the gripping torque is an axis of the motor 14a. ところが、実際はモータと出力軸との間には減速機が介在しており、この減速比がモータ14b,14cよりも2倍以上大きい(出力軸が2倍以上に減速される)と、モータ14aのモータ電流から算出される把持開始推定信号faは、他の軸から算出される値よりも小さくなり、判定では扱いにくい。 However, in practice the speed reducer is interposed between the motor and the output shaft, and the reduction ratio motor 14b, more than two times greater than 14c (the output shaft is decelerated more than doubled), the motor 14a gripping start estimated signal fa is calculated from the motor current is smaller than the value calculated from the other axes, the decision unwieldy.

逆に、3軸とも減速比が同じであれば、モータ14aのモータ電流から算出される把持開始推定信号faを積極的に使用するとよいことを上記(5)式は示している。 Conversely, if all three axes reduction ratio is the same, and that the good used positively gripping start estimation signal fa calculated from the motor current of the motor 14a shown in equation (5).

このように、測定しているモータ電流に対応する軸(モータ入力軸)からグリッパ4に至るまでのすべての減速比を換算した値と把持トルクの反力の大きさとに基づいて、第2の切替部SW2を切替えてもよい。 Thus, based on the magnitude of the reaction force of a value obtained by converting all of the speed reduction ratio from the corresponding shaft (motor input shaft) to the gripper 4 the gripping torque to the motor current being measured, the second it may switch the switching unit SW2.

図8の第1および第2の切替部SW1,SW2の切替の条件をまとめると、以下のようになる。 To summarize the switching conditions of the first and second switching unit SW1, SW2 of FIG. 8, as follows.

第1の切替部SW1は、グリッパ軸目標速度ωgが負の場合にオフする。 The first switching unit SW1 is turned off when the gripper axis target velocity ωg is negative. この場合、把持開始推定オブザーバ51は、クーロン摩擦項を考慮に入れずに把持開始推定信号faを生成する。 In this case, the gripping start estimating observer 51 generates a gripping start estimation signal fa without taking into account the Coulomb friction term.

第1の切替部SW1は、グリッパ軸目標速度ωgが正かゼロの場合にオフする。 The first switching unit SW1 is gripper axis target velocity ωg is turned off when a positive or zero. この場合、把持開始推定オブザーバ51は、クーロン摩擦項を考慮に入れて把持開始推定信号faを生成する。 In this case, the gripping start estimating observer 51 generates a gripping start estimation signal fa into account the Coulomb friction term.

第2の切替部SW2は、代表的にはグリッパ軸を駆動するモータ14cに対応するオブザーバ信号を把持開始推定信号faとして選択するが、トルク干渉があることを考慮して、他のモータ14aまたはモータ14bに対応するオブザーバ信号を把持開始推定信号faとして選択することもできる。 The second switching unit SW2 is typically selects the observer signal corresponding to the motor 14c for driving the gripper shaft as gripping start estimation signal fa, considering that there is a torque interference, other motors 14a or it is also possible to select an observer signal corresponding to the motor 14b as a gripping start estimation signal fa. より具体的には、把持動作中(グリッパ軸速度ωgが負のとき)には、以下の3つの切替を行う。 More specifically, during the gripping operation (when the gripper shaft speed ωg is negative) performs the following three switching.

モータ14cの速度が正の場合にはモータ14cを選択する。 The speed of the motor 14c is for positive selects the motor 14c. モータ14cの速度が正でなく、かつモータ14bの速度が正の場合はモータ14bを選択する。 The speed of the motor 14c is positive in not, and if the speed of the motor 14b is positive to select the motor 14b. モータ14b、14cの速度が正でない場合は、モータ14aを選択する。 If the motor 14b, the speed of 14c not positive, selecting a motor 14a.

このように、第1の実施形態では、把持開始推定オブザーバ51を設けて、把持開始角度を推定し、推定した把持開始角度に基づいてグリッパ4の最閉角度を設定して、最閉角度に達するまでグリッパ軸角度を制御する。 Thus, in the first embodiment, provided with a gripping start estimation observer 51 estimates the grip start angle, and set the highest close angle of the gripper 4 based on gripping start angle estimated, the most close angle It controls the gripper axis angle until. これにより、モータのトルク制御で把持力を制御する場合よりも、精度よくグリッパ4の把持力制御が可能となる。 Accordingly, as compared with the case of controlling the gripping force by the torque control of the motor, it is possible to grip force control accurately the gripper 4.

また、本実施形態の把持開始推定オブザーバ51では、クーロン摩擦項を考慮に入れずに把持開始角度を推定するため、把持開始角度を精度よく推定することができる。 Further, the gripping start estimation observer 51 of the present embodiment, to estimate the grip start angle without taking into account the Coulomb friction term, a gripping start angle can be estimated accurately.

さらに、トルク干渉機構を備えたマニピュレータの場合には、各モータごとに把持開始推定オブザーバ51a,51b,51cを設けて、各モータのモータ速度によって、いずれか一つの把持開始推定オブザーバの出力を最終的な把持開始推定信号faとして選択するため、トルク干渉機構の場合であっても、迅速かつ正確に把持開始角度を推定できる。 Furthermore, in the case of a manipulator provided with a torque interference mechanism, gripping starts estimating observer 51a for each motor, 51b, 51c and is provided, the final by a motor speed of each motor, the output of one of the gripping start estimation observer to select a gripping start estimation signals fa, even when the torque interference mechanism, quickly and accurately estimate the grip start angle.

(第2の実施形態) (Second Embodiment)
第2の実施形態は、上述した把持開始推定オブザーバ51を用いて、医療用マニピュレータのようなマスタスレーブ操作による把持動作を行うものである。 The second embodiment uses a gripping start estimation observer 51 described above, it performs a gripping action by the master slave operations, such as medical manipulator.

図13(a)および図13(b)は操作桿82の内部構造の一例を示す図であり、図13(a)はトリガ85の復帰状態、図13(b)はトリガ85を引いた状態を示している。 FIG. 13 (a) and FIG. 13 (b) is a diagram showing an example of the internal structure of the operating rod 82, FIG. 13 (a) state return state of the trigger 85, FIG. 13 (b) obtained by subtracting the trigger 85 the shows. これらの図に示すように、操作桿82は、トリガ85の端部に取り付けられたロック棒87を保持するロック部材88と、トリガ85の引き量を検出するセンサ89と、トリガ85に形成されたガイド孔90を通って挿脱されるガイド棒91と、ガイド棒91の周囲に取り付けられたばね92とを有する。 As shown in these figures, the operating rod 82 is provided with a locking member 88 for holding the locking bar 87 attached to the end of the trigger 85, the sensor 89 for detecting the pulling amount of the trigger 85, is formed in the trigger 85 It has been through the guide hole 90 and the guide rod 91 to be inserted and removed, and a spring 92 mounted around the guide rod 91.

センサ89は、トリガ85の並進動作位置を検出するものであり、具体的には、スライドポテンションメータやリニアエンコーダ等で構成することができる。 Sensor 89 is for detecting a translational movement position of the trigger 85, specifically, can be composed of a slide potentiometer, a linear encoder or the like. 例えば、スライドポテンションメータでセンサ89を構成する場合、トリガ85の端部に取り付けられた突起部材93の凹部がスライドポテンションメータのつまみ94の凸部と係合される。 For example, when a sensor 89 in sliding potentiometer, the concave portion of the projecting member 93 attached to the end of the trigger 85 is engaged with the protrusion of the knob 94 of the slide potentiometer. これら凹部と凸部は、トリガ85の並進動作に合わせて一体に移動する。 These recesses and projections are moved together in accordance with the translational movement of the trigger 85. そして、センサ89は、凸部の位置によりトリガ85の引き量を検出する。 The sensor 89 detects the pulling amount of the trigger 85 by the position of the convex portion.

トリガ85を最も引いた状態が作用部4のグリッパを最も閉じた状態(図13(a))、すなわち開閉角度が最大閉角度となる状態となり、トリガ85を最も戻した状態がグリッパを最も開いた状態となり、すなわち、グリッパを最も開いた状態となるように、センサ89で検出した値を変換することで、グリッパ開閉目標値を得ることができる。 State state where most pulled the trigger 85 is closed most gripper action part 4 (FIG. 13 (a)), i.e. a state in which the opening angle is maximized close angle, most most reconstituted state gripper trigger 85 open It becomes state, i.e., as the most open state gripper, to convert the detected value by the sensor 89, it is possible to obtain the gripper target value.

トリガ85の引き量を長時間維持することは操作者にとって負担が大きく、作用部4での細かい操作指示にも影響を与える可能性がある。 For a long time maintaining the pulling amount of the trigger 85 it is burdensome for the operator, which may also affect detailed operation instruction by the operation unit 4. このため、トリガ85から手を離してもその引き量が維持されるように、操作桿82にロック機構を設けることがあり、本実施形態の操作桿82においても、ロック機構が設けられている。 Therefore, as the drawing amount even release the trigger 85 is maintained, it may provide a locking mechanism to the operating rod 82, even in the operating rod 82 of this embodiment, the locking mechanism is provided .

トリガ85を最大限引くと、トリガ85の端部に取り付けられたロック棒87が操作桿82のロック部材88に収まる。 Subtracting maximize trigger 85, the lock rod 87 attached to the end of the trigger 85 is within the locking member 88 of the operating rod 82. その後、操作者がトリガ85から手を離しても、ロック部材88はロック棒87を固定し、トリガ85の引き量が一定に維持される。 Thereafter, the operator may release the hand from the trigger 85, the locking member 88 secures the lock bar 87, the drawing amount of the trigger 85 is maintained constant. ロック部材88には、トリガ85の引き量に応じて、ロック棒87がわずかに並進動作するためのあそび範囲が設けられている。 The locking member 88, in accordance with the pulling amount of the trigger 85, is provided tolerance range for translational motion slightly lock rod 87. このように、ロック棒87とロック部材88によりロック機構が実現されている。 Thus, the locking mechanism is realized by the lock bar 87 and the locking member 88.

トリガ85がロックされた状態でさらにトリガ85を引くと、ロック部材88はロック棒87を開放する。 Further Pulling the trigger 85 in a state where the trigger 85 is locked, the locking member 88 releases the locking bar 87. トリガ85には、操作桿82のガイド棒91を挿脱するためのガイド孔90が設けられている。 The trigger 85, the guide hole 90 for inserting and removing the guide rod 91 of the operating rod 82 is provided. トリガ85をロックした状態では、ガイド棒91がガイド孔90に挿入され、ばね92がトリガ85の端部により収縮される。 In the locked state of the trigger 85, the guide rod 91 is inserted into the guide hole 90, the spring 92 is contracted by the end of the trigger 85. このロック状態でトリガ85をさらに引いてロック部材88がロック棒87を開放すると、ばね92の復元力により、トリガ85は押し出されて、元の位置に復帰する。 When the lock member 88 further pulling the trigger 85 in this locked state to open the lock bar 87, by the restoring force of the spring 92, trigger 85 is pushed out, it returns to the original position. このとき、図13(a)のように、ガイド棒91はガイド孔90から抜き出される。 At this time, as shown in FIG. 13 (a), the guide rod 91 is withdrawn from the guide hole 90.

図13(a)および図13(b)に示すように、マスタースレーブ操作によりグリッパ軸を動作させて把持力を調整する場合を考えると、操作者は把持状態を観測しながら操作を行うため、トリガ85を引く程度に応じてグリッパ4が閉じて把持力が増大し、設定した最大把持力が最大限トリガ85を引いたときに得られるようにすると、操作感と適合して望ましい。 As shown in FIG. 13 (a) and 13 (b), considering the case of adjusting the gripping force by operating the gripper shaft by master-slave operation, the operator to perform operations while observing the gripping state, gripper 4 is closed gripping force increases according to the degree of pulling the trigger 85, the maximum grip force is set to be obtained when subtracting the maximum trigger 85, preferably adapted and feel. すなわち、マスタスレーブ操作による把持力調整機能とは、最大把持力を設定する機能と言い換えることができる。 That is, the gripping force adjusting function according to a master-slave operation can be expressed as the function of setting the maximum gripping force.

ここで、センサ89が検出する電圧範囲がトリガを最大に引いたときにV0、トリガが最も出ている状態でV2であるとする。 Here, a V0, is V2 in a state in which the trigger is most out when the voltage range of the sensor 89 is detected by subtracting the maximum trigger. トリガ85が最も出ている状態では、グリッパ軸角度がθp(>0)に設定されるものとする。 In a state where the trigger 85 is best out, it is assumed that the gripper axis angle is set to θp (> 0).

図14はセンサ89の出力電圧とグリッパ軸目標値との関係を示す図である。 Figure 14 is a graph showing the relationship between the output voltage and the gripper axis target value of the sensor 89. センサ出力電圧Vtは、把持開始推定オブザーバ51によって検出更新された把持開始角度に対応するセンサ出力電圧であり、以下の(6)式で表される。 Sensor output voltage Vt is a sensor output voltage corresponding to the grip start angle detected updated by the gripping start estimation observer 51 is expressed by the following equation (6).

ここで、センサ電圧V1は、グリッパ4が物体を把持していないときの、センサ出力とグリッパ軸目標値との関係を決める既定値で、V0とV2の間に設定されるものである。 Here, the sensor voltage V1, when the gripper 4 is not gripping an object, the default value for determining the relationship between the sensor output and the gripper axis target value, and is set between V0 and V2. より具体的には、V1はグリッパ4が物体を把持せずに閉じた場合に、グリッパ軸目標値を0度とする電圧値である。 More specifically, V1 if the gripper 4 is closed without grasping an object, the gripper axis target value is a voltage value to be 0 degrees.

トリガ85の引き量に応じてグリッパ4の把持力が変化する感度をよくするためには、V0とV1の間隔を広めに取っておけばよい。 To improve the sensitivity of gripping force of the gripper 4 is changed according to the pulling amount of the trigger 85, it is sufficient to take the spread distance V0 and V1. また、硬くて小さいものを設定した最大力で掴むことを前提にしていれば、V0とV1との間隔を狭く取っておくと良い。 Furthermore, if the assumption that grab the maximum force set those hard and small, may set aside reduce the distance between V0 and V1.

物体を把持していない状態、すなわち、センサ出力電圧VgがVt以上の場合、グリッパ軸目標角度θg1と電圧Vgとの関係は以下の(7)式で表される。 State of not holding the object, i.e., the sensor output voltage Vg is equal to or greater than Vt, the relationship between the gripper shaft target angle θg1 and voltage Vg is expressed by the following equation (7).

物体を把持している状態、すなわち、VgがVt以下の場合、グリッパ軸目標角度θg2と電圧Vgとの関係は以下の(8)式で表すことができる。 Condition gripping on the object, that is, when Vg is Vt or less, the relationship between the gripper shaft target angle θg2 voltage Vg can be expressed by the following equation (8).

上述した図8の把持開始推定オブザーバ51で生成された把持開始推定信号faに基づいて把持開始角度が推定されると、推定された把持開始角度でのセンサ89の電圧を検出し、検出されたセンサ電圧を基点として、トリガ85の引き量に応じたセンサ出力電圧とグリッパ軸角度との関係を求めて、グリッパ軸の角度目標値を設定する。 When gripping start angle on the basis of the generated gripped initiated estimated signal fa by the gripping start estimation observer 51 of Figure 8 described above is estimated by detecting the voltage of the sensor 89 in the estimated gripped start angle, is detected the sensor voltage as a base point, and obtain the relation between the sensor output voltage and the gripper axis angle corresponding to the pulled amount of the trigger 85, to set the angle target value of the gripper shaft. これにより、設定した最大把持力が最大限トリガ85を引いたときに得られるような把持力調整機能をグリッパ軸の位置(角度)制御で実現することができる。 Thus, it is possible to realize a grasping force adjusting function, as obtained when the maximum gripping force set by the minus maximum trigger 85 at the position (angle) control of the gripper shaft.

このように、第2の実施形態では、トリガ85の引き量をセンサ89のセンサ電圧で検出可能なマスタスレーブ操作のマニピュレータにおいて、把持開始角度が推定された時点を基点として、センサ出力電圧とグリッパ軸角度の関係から、トリガ85の引き量に応じたグリッパ軸角度の制御を行うため、把持力の制御を精度よく行うことができる。 Thus, in the second embodiment, the manipulator detectable master slave operation the pulling amount of the trigger 85 a sensor voltage of the sensor 89, as a base point the point at which the gripping start angle is estimated, the sensor output voltage and the gripper from the relationship between the shaft angle, for controlling the gripper shaft angle corresponding to the pulled amount of the trigger 85, it is possible to control the gripping force accurately.

(第3の実施形態) (Third Embodiment)
第1の実施形態では、本発明がトルク干渉機構を備えたマニピュレータにも適用できることを説明した。 In the first embodiment, the present invention has been described can also be applied to a manipulator equipped with a torque interference mechanism. 以下に説明する第3の実施形態では、この種のトルク干渉機構におけるトルク干渉補正について説明する。 In a third embodiment described below, a description will be given torque interference correction in this type of torque interference mechanism.

上述したように、把持動作をすると、トルク干渉により各モータ軸に対して反力がかかる。 As described above, when the gripping operation, the reaction force is applied to each motor shaft by torque interference. より具体的には、作用部4の姿勢軸周りにトルクが作用すると、各姿勢軸を駆動するモータ出力軸に反力となるトルクが作用する。 More specifically, when torque acts about the attitude axis of the working portion 4, the torque working as the reactive to the motor output shaft for driving the posture axes acts. モータ出力軸のトルクは、上述した(4)式で表される。 The torque of the motor output shaft is expressed by the aforementioned equation (4).

マニピュレータの場合、各モータ出力軸と先端の作用部4との間には、動力伝達機構の一部を構成するワイヤ(可とう性部材)12a,12b,12cが存在する。 For manipulators, between the action part 4 of the motor output shaft and the tip, the wire that constitutes a part of the power transmission mechanism (flexible members) 12a, 12b, 12c are present. これらワイヤは力に比例して伸びるため、各モータ出力軸が上述した(2)式を満たすように角度制御されていても、実際の先端部の従属プーリはモータ軸側に対して角度がずれて姿勢が変化してしまう。 These wires are for extending in proportion to the force, be angularly controlled so as to satisfy the respective motor output shaft mentioned above (2), the dependent pulley actual tip angle offset relative to the motor shaft side attitude is changed Te.

そこで、そのずれ量を求めてみる。 So, we try to determine the amount of deviation. 今、ワイヤの剛性をwとすると、以下の(9)式が成り立つ。 Now, when the rigidity of the wire and w, holds the following equation (9).

把持トルクτgとグリッパ軸角度は比例することから、比例定数αを用いると、以下の(10)式の関係が成り立つ。 Since the gripping torque τg and the gripper axis angle is proportional to, the use of proportional constant alpha, below is established (10) relationship.

τg=αΔθg …(10) τg = αΔθg ... (10)

モータ出力軸と姿勢軸との関係を表した上記(5)式の両辺にそれぞれ(9)式と(10)式を代入して整理すると、以下の(11)式が得られる。 And rearranging by substituting each of both sides of equation (5) representing the relationship between the motor output shaft and the orientation axis (9) and (10), equation (11) below is obtained.

このとき、Δθd=KΔθaを用いることで、姿勢軸偏差角度Δθdは、以下の(12)式で表される。 At this time, by using the Δθd = KΔθa, attitude axis deviation angle .DELTA..theta.d it is expressed by the following equation (12).

ここで、(12)式において、Δθgの項の等式が成り立つとすると、Δθg=6(α/w)Δθgとなり、α/w=1/6となる。 Here, in equation (12), when the established equality sections Δθg, Δθg = 6 (α / w) Δθg next, the α / w = 1/6. これにより、グリッパ軸偏差Δθg(グリッパ軸目標値と実際のグリッパ軸角度との偏差)と、ヨー軸、ロール軸の姿勢偏差Δθy、Δθrとの関係は、以下の(13)式および(14)式で表される。 Thus, the gripper shaft deviation Derutashitag (deviation of the actual gripper axis angle and the gripper axis target value), the yaw axis, attitude deviation of the roll shaft [Delta] [theta] y, the relationship between the Δθr is and below (13) (14) formula.

これまでの算出で用いられている姿勢軸偏差角度Δθdは、モータ出力軸角度を上記(2)式により換算した値で、これはトルク損失がない場合であるが、実際はトルク損失が起こり、先端部の姿勢はこれと異なる。 Previously attitude axis deviation angle Δθd as used calculation of the motor output shaft angle value converted by the above equation (2), this is when there is no torque loss actually occurs torque losses, the tip attitude of the parts are different from this. 実際の先端部姿勢偏差は、モータ出力軸から先端部姿勢軸までのトルク効率ηを用いると、以下の(15)式および(16)式で表される。 The actual tip position deviation, the use of torque efficiency η from the motor output shaft to the distal end posture axis, is expressed by the following equation (15) and (16).

把持開始点を認識したら、把持開始後のグリッパ軸角度Δθgに応じて上述のように求めた偏差角度分だけ、姿勢軸補正を行うように制御すればよい。 After recognizing the grasping starting point, only the deviation angle amount determined as described above in accordance with the grip after the start of the gripper axis angle Derutashitag, it may be controlled to perform the orientation axis correction. なお、ここでトルク効率ηは、モータ出力軸からグリッパ軸に至る動力伝達機構の軸数の関数として表すことができる。 Here, torque efficiency η can be expressed as the axis of the function of the power transmission mechanism ranging from the motor output shaft to the gripper shaft. 例えば、図2〜図4に示すマニピュレータにおいては、出力軸プーリ39c、回転軸16、ギア322の回転軸、グリッパ軸323aの4軸を通じてグリッパ軸動作が行われているため、通常の潤滑状態であれば1軸あたりの効率を0.9として、0.9の4乗がトルク効率ηになる。 For example, in the manipulator shown in Figures 2-4, the output shaft pulley 39c, the rotation axis of the rotary shaft 16, the gear 322, since the gripper shaft operation is performed through the four axes of the gripper shaft 323a, in a normal lubrication state 0.9 the efficiency per axis if, the fourth power of 0.9 is torque efficiency eta.

例えば、洗浄された直後の医療用マニピュレータの場合、洗浄後に潤滑処理していない無潤滑状態であれば、1軸あたりの効率を0.8とすればよい。 For example, when the medical manipulator immediately after being cleaned, if no lubrication state after washing is not lubricated, may be the efficiency per axis 0.8. もちろん軸の材質などでも変化するので適宜設定すればよい。 Of course it may be suitably set so also changes like the axis of the material.

なお、上述したように、(12)式からα/w=1/6の関係が得られるため、この関係を(11)式に代入して求めた各モータ出力軸のずれ角度を補正角度として、逆キネマティック算出後の各モータ出力軸目標値にこの補正角度を減算してはならない。 As described above, (12) for the relationship of α / w = 1/6 is obtained from the equation, the deviation angle of each motor output shaft obtained by substituting this relation into equation (11) as a correction angle It should not subtract this correction angle to each motor output shaft target value after the inverse kinematic calculation. このようにすると、新たな姿勢角度を定義することになり、グリッパ軸の目標値までも変化してしまい、目的とした把持力を出力できなくなってしまう。 In this way, it would define a new attitude angle, even to the target value of the gripper shaft will vary, it becomes impossible outputs a gripping force for the purpose. よって、上述した(13)および(14)式に示すように、グリッパ軸角度を基準とした補正角度を姿勢軸換算で求め、それを目標の各姿勢角度から減算した後、逆キネマティック演算をして、各モータ軸の目標値を算出するのが望ましい。 Therefore, as shown in the above (13) and (14), we obtain a correction angle relative to the gripper axis angle posture shaft conversion, after subtracting it from the attitude angle target, an inverse kinematic calculation to, it is desirable to calculate the target value of the motor shaft.

ここで、把持力を調整するための把持力開始点を推定する既定値と、姿勢補正を行うための把持力開始点を推定する既定値とは同じである必要はない。 Here, the default value for estimating a gripping force starting points for adjusting the gripping force, need not be the same as the default value for estimating a gripping force starting point for performing the posture correction. 把持力調整の場合、把持開始角度と把持角度から最大把持力を決めるが、その最大把持力の大きさは把持角度と絶対的な関係にあるわけではない。 For gripping force adjustment, but determines the maximum gripping force from the grip start angle and the holding angle, the magnitude of the maximum gripping force is not necessarily located in the grip angle and absolute relationship. 例えば、図15において、把持力開始点を判定するための規定値f1の代わりに、より大きな値f2を規定値として設定してもよい。 For example, in FIG. 15, in place of the specified value f1 for determining the clamping force starting point may be set to a larger value f2 as defaults. この場合、規定値f2からの把持力と把持角度との関係は、規定値f1からの把持力と値がオフセットされるだけで、把持力と把持角度についての1次的な関係には変化がないため、規定値が変化しても把持力の調整は可能である。 In this case, the relationship between the gripping force and gripping angle from the specified value f2, only the gripping force and values ​​from the specified value f1 is offset, changes to the primary relationship for gripping force and gripping angle no order, the specified value can be the adjustment of the gripping force vary.

このように、規定値を変更しても、設定したい把持力の大きさの基準またはスケールが変化したに過ぎない。 Thus, changes to the predetermined value, only the reference or scale the size of the gripping force to be set is changed. 規定値以下の把持力を調整することはできず、規定値を高くするほどオブザーバ信号に含まれるノイズの影響を受けにくくなることから、両者のトレードオフにより規定値のレベルは設定される。 It can not be adjusted a defined value below the gripping force, since it is less affected by the noise contained in the observer signal the higher the predetermined value, the level of specified values ​​by both a trade-off is set.

一方、姿勢補正を行う場合は、把持力の変化が姿勢変化につながることから、できるかぎり把持開始点を正確に推定する必要がある。 On the other hand, when performing position correction, since a change in the gripping force will lead to a change in posture, it is necessary to accurately estimate the grip starting point as possible. このため、図15に示すように、姿勢補正を行う場合の規定値は、把持開始時に変化が現れるf3以上で、かつf1以下に設定するのが望ましい。 Therefore, as shown in FIG. 15, the specified value when performing posture correction is a gripping start to change appears f3 higher, and it is desirable to set to f1 below. このf3は動作軸補正基準値に対応する。 The f3 corresponds to the operation axis correction reference value. 姿勢補正のための把持開始点を推定する処理は制御装置2が行うものであり、この処理は補正開始角度推定手段に対応する。 Processing for estimating the grip starting point for the posture correction are those control unit 2 performs, this process corresponds to the correction start angle estimating unit. また、制御装置2は、把持開始点を推定した後に上述した(15)および(16)式に基づいて偏差角度を算出する処理を行うが、この処理は偏差角度算出手段に対応する。 The control device 2, the above-described gripping starting point after the estimated (15) and (16) performs the process of calculating the deviation angle based on the type, this process corresponds to the deviation angle calculating means. その後、制御装置2は、偏差角度に基づいて各姿勢軸の姿勢補正を行うが、この処理は動作軸補正手段に対応する。 Thereafter, the controller 2 performs the posture correction of the orientation axis based on the deviation angle, this process corresponds to the operation axis correcting means.

このように、第3の実施形態では、トルク干渉機構を備えたマニピュレータの場合、把持動作の制御により、他の姿勢軸にも影響が及ぶことから、把持開始後のグリッパ軸角度に応じて姿勢軸補正を行う。 Thus, in the third embodiment, when the manipulator provided with a torque interference mechanism, the control of the gripping operation, since the influence is exerted on other orientation axis, depending on the gripper shaft angle after gripping the start position perform the axis correction. これにより、把持力が変化しても姿勢補正を精度よく行うことができる。 This makes it possible to grip it performed accurately even posture correction changes.

(第4の実施形態) (Fourth Embodiment)
以下に説明する第4の実施形態は、把持開始推定信号faを用いて、対象物の相対的な柔軟度を測定するものである。 Fourth embodiment described below, using a gripping start estimation signals fa, which measures the relative flexibility of the object.

把持力を把持角度で制御することが従来行われていない理由の一つは、対象物が変形することにより、角度制御では実際の把持力が当初予定していた把持力よりも小さくなってしまうことがあるためである。 One reason for controlling the gripping force in the gripping angle is not performed conventionally, by the object is deformed, it becomes smaller than the gripping force actual gripping force is planned initially at an angle control it is because there is. 第1および第2の実施形態においても、非常に柔らかい物体を把持するときと、変形しない硬い物体を把持するときでは、同じ把持角度を設定しても、同じ把持力にはならない。 Also in the first and second embodiments, as when gripping a very soft objects, when grasping the hard object which is not deformed, then setting the same gripping angle, not to the same gripping force.

しかしながら、把持力を決定する把持角度は任意に調整できるため、対象物に応じて設定を調整することで実用的な対応が可能となる。 However, since the gripping angle to determine the gripping force can be arbitrarily adjusted, it is possible to practical response by adjusting the set according to the object. 未知の大きさの物体を、設定した大きさ以上の把持力で把持することはないことから、最大把持力を設定することと同等と考えることができる。 The object of unknown size, since it is not possible to grip by the gripping force of magnitude or more set may be considered as equivalent to setting the maximum gripping force. 把持力を必要以上に出さないシステムは、安全性の上でも望ましく、モータの発熱を抑えて劣化を防止し、また省エネで効率的なシステムを構築できる。 Not issue a gripping force than necessary system desirably also on the safety, preventing deterioration by suppressing the heat generation of the motor and can build an efficient system in energy saving. 例えば、干渉機構では、把持動作に寄与するモータが他の姿勢軸動作にも影響を与えるが、把持動作に必要以上の出力を割り当てないとすると、他の姿勢軸動作に十分な出力を割り当てることができる。 For example, the interference mechanism, but contributes motor to the gripping action also affects other orientation axis operation, when not assigning an output of more than necessary gripping action, to allocate sufficient output to another posture axis operation can.

本実施形態では、対象物の硬さによって生じる把持力の誤差を補正できるとともに、対象物の硬さを推定できることを特徴とする。 In the present embodiment, it is possible to correct an error of the gripping force generated by the hardness of the object, characterized in that it can estimate the hardness of the object.

図16はマニピュレータが物体を把持するときのモータ14cの出力軸角度θ3と把持開始推定信号faとの関係を示すグラフである。 Figure 16 is a graph showing the relationship between a gripping start estimation signal fa and the output shaft angle θ3 of the motor 14c when the manipulator to grip the object. 上述したように、θ3=−θgであるため、θ3が増加するほど把持力が高くなる。 As described above, since it is θ3 = -θg, the gripping force, the higher .theta.3 increases.

把持開始推定信号faが図15に示した既定値f1と一致するときの把持角度は、図16では符号を反転させて−θtとしている。 Gripping angle when the grip start estimation signal fa matches the default f1 shown in FIG. 15 is a -θt by reversing the sign in Figure 16. 仮に物体Aを用いて関係付けられた把持角度と把持力の関係から把持力を調整する機能が初期設定されていたとする。 If the ability to adjust the gripping force from the gripping force of the relationship between grip angle associated with the object A is assumed to have been initialized. この設定で、物体Aよりも柔らかい物体Bを把持すると、算出された最閉角度θc(モータ14cの出力軸では−θc)までグリッパ4を閉じても予定の把持力まで達しない。 In this configuration, when grasping a soft object B than the object A, the calculated top-close angle .theta.c (the output shaft of the motor 14c is -Shitashi) does not reach the gripping force will be closed gripper 4 to. これは物体Aの硬さ係数kaと物体Bの硬さ係数kbとが異なり、同じ角度θhだけ閉じても把持力は異なるためである。 This is different from the hardness coefficient kb hardness coefficient ka and the object B of the object A, the gripping force be closed by the same angle θh is because different.

そこで、物体の硬さ係数を、把持動作中であって、グリッパ軸角度θgが把持開始を認識した把持開始角度θtからそのとき算出された最閉角度θcまでの間の角度θuにあるときのグリッパ軸動作に関わるモータ軸角度、ここではモータ14cの出力軸角度と、把持開始推定オブザーバ51の出力との関係を最小二乗法で算出する。 Therefore, the hardness factor of the object, even during gripping operation, when an angle θu between the gripping start angle θt of the gripper axis angle θg recognizes the start gripping to the most close angle θc which is calculated at that time motor shaft angle related to the gripper shaft operation, here calculates the output shaft angle of the motor 14c, the relationship between the output of the gripping start estimation observer 51 with the least squares method. 算出された値はその物体bの硬さを表す指標kbである。 Calculated value is an indicator kb representing the hardness of the object b.

予め物体aの硬さ係数kaを同様の手法で算出しておくとすると、物体aを把持する把持角度θaで設定された把持力と同等の把持力で物体bを把持するときの把持角度θbの関係は以下の(17)式で表される。 Advance when the hardness coefficient ka of the object a to keep calculated in the same manner, the gripping angle θb when gripping an object b by the gripping force equivalent gripping force set by the gripping angle θa for gripping an object a the relationship is expressed by the following equation (17).

kaθa=kbθb …(17) kaθa = kbθb ... (17)
この場合、kb<kaであるから、θb>θaである。 <Because it is ka, θb> In this case, kb is θa. よって、物体aを把持角度θaで把持した場合の把持力と同等の把持力で物体bを把持するときの把持角度θbとの差Δθcは、以下の(18)式で表すことができる。 Therefore, the difference Δθc of the gripping angle θb when gripping an object b by the gripping force equivalent to the gripping force when gripping the object a by the gripping angle θa can be expressed by the following equation (18).

上記(18)式で算出された分だけ最閉角度を増やせばよいことになる。 So that may be increased only top close angle amount calculated in the above equation (18).

図17は硬さ補正処理の処理手順の一例を示すフローチャートである。 Figure 17 is a flow chart showing an example of a processing procedure of the hardness correction. この処理は図6のフローチャート内のステップS13の処理の中で行われ、より具体的には図11の処理に引き続いて行われる。 This process is performed in the processing in step S13 in the flowchart of FIG. 6, it is more specifically performed subsequent to the process in FIG. 11.

まず、図11の処理によって求めた把持開始角度θtとグリッパ軸最閉角度θcを取得する。 First, to obtain the grasping start angle θt and the gripper shaft lowest close angle θc obtained by the process of FIG. 11. そして把持開始角度θtとグリッパ軸最閉角度θcとから、角度θuを取得する。 And from a gripping start angle θt and the gripper shaft lowest close angle .theta.c, obtains the angle .theta.u. (ステップS41)。 (Step S41). 次に、グリッパ軸角度θgがθt>θg>θuの範囲内にあるか否かを判定する(ステップS42)。 Next, it is determined whether or not the gripper axis angle [theta] g is in the range of θt> θg> θu (step S42). ステップS42の判定がYESの場合には、その時点でのグリッパ軸動作に関係するモータ出力軸角度、具体的にはモータ14cの出力軸角度θ3と、モータ14cに対応する把持開始推定信号faを取得して保存する(ステップS43)。 If the determination in step S42 is YES, the motor output shaft angle related to the gripper shaft operation at that time, specifically the output shaft angle θ3 of the motor 14c, a gripping start estimation signal fa corresponding to the motor 14c acquires and to save (step S43). θ3とfaを保存する理由は、後に硬さ係数kbを算出する際に用いるためである。 Why save θ3 and fa is for use in calculating the later stiffness coefficient kb. このステップS43の処理は記録手段に対応する。 Processing in step S43 corresponds to the recording means.

ステップS42の判定がNOの場合には、物体の違いによる補正を行ったか否かを示す補正フラグの状態を判定する(ステップS44)。 If the determination in step S42 is NO, it determines the state of the correction flag indicating whether or not correction is performed by the object difference in (step S44). この補正フラグは初期状態ではリセットされており、補正前であることを示している。 The correction flag is in the initial state are reset, indicating that a pre-correction.

補正フラグがリセット状態と判断された場合は、ステップS43で保存したモータ出力軸角度θ3と把持開始推定信号faを用いて最小二乗法により硬さ係数を算出し(ステップS45)、上述した(18)式に基づいてグリッパ軸最閉角度を更新し(ステップS46)、補正フラグをセットする(ステップS47)。 If the correction flag is determined to reset calculates the hardness coefficient by the least square method using the gripping start estimation signal fa to the motor output shaft angle θ3 stored in step S43 (step S45), the aforementioned (18 ) update the gripper shaft lowest close angle based on the expression (step S46), and sets the correction flag (step S47). このステップS45の処理は硬さ推定手段に対応し、ステップS46の処理は姿勢軸限界角度更新手段に対応する。 Processing in step S45 corresponds to the hardness estimation means, the processing of step S46 corresponds to the orientation axis limit angle updating means.

以降、グリッパ軸目標値θgが更新された最閉角度になるまでの間、補正フラグはセット状態を維持するため、新たな最閉角度で制御される。 Thereafter, until the gripper axis target value θg is most close angle that is updated, the correction flag to maintain the set state, is controlled by a new top-close angle. なお、把持状態が解除されて把持フラグがリセットされるとともに、補正フラグもリセットされる。 Note that the grasping condition is released by grasping flag are reset and, the correction flag is reset.

図17のステップS46では、対象物の硬さ係数を算出しているが、この係数を提示(表示)するようにすれば、対象物の硬さを操作者に知らせることができる。 In step S46 in FIG. 17, calculates the stiffness coefficient of the object, if to present (display) the coefficients, it is possible to inform the hardness of the object to the operator. ただし、実際には、対象物の動力学モデルには速度に依存するダンパ効果があることや、マニピュレータ全体に作用する力と把持力との区別などが困難であることなどから、絶対的な物体の硬さを算出提示することは困難である。 However, in practice, and that the dynamic model of the object is a damper effect which depends on the speed, and the like that such distinction between the force and the gripping force acting on the entire manipulator is difficult, absolute objects it is difficult to make the hardness of the calculation presentation. しかし、同様な把持動作を自動動作などで実現すれば、対象物の部分ごとに把持した結果から、相対的な硬さの違いを認識することが可能である。 However, if implemented by an automatic operation similar gripping operation, the result of the grip for each portion of the object, it is possible to recognize the difference of the relative hardness. この機能により、周囲に対してある部分だけ硬さが違うとその部分を推定して候補として上げることができる。 This feature can be increased as a candidate to estimate the partial when only the hardness is different portions thereof with respect to ambient.

このように、第4の実施形態では、把持対象物が硬い場合と柔らかい場合では、同じ把持角度を設定しても、同じ把持力にはならないことから、把持開始角度を推定した後に対象物の硬さ係数を計算し直して、グリッパの最閉角度を更新するため、対象物の硬さに応じて把持力を調整できる。 Thus, in the fourth embodiment, in the case when the grasped object is hard and soft, setting the same gripping angle, from having the same gripping force, the object gripping start angle after the estimated recalculates the hardness factor, to update the top-close angle of the gripper can be adjusted to grip force according to the hardness of the object. これにより、対象物の硬さに差異があっても、角度制御により精度よく把持力を調整できる。 Thus, even if there is difference in hardness of the object, it can be adjusted accurately grasping force by the angle control.

(第5の実施形態) (Fifth Embodiment)
第5の実施形態は、初期姿勢誤差を推定して初期姿勢を補正するものである。 The fifth embodiment is to correct the initial attitude estimate the initial attitude error.

上述した把持開始推定オブザーバ51は、物体を把持していない場合、例えばマニピュレータの場合は、グリッパを構成する第1および第2の作用部材325,326が接触した角度を推定することになる。 Gripping start estimation observer 51 discussed above, if not grip the object, for example the case of a manipulator, so that to estimate the angle of the first and second working members 325, 326 are in contact constituting the gripper. 例えば、グリッパを閉じた状態を初期姿勢とすれば、グリッパ軸における初期姿勢を推定することになる。 For example, if the closed state of the gripper and initial posture, so that to estimate the initial attitude of the gripper shaft. したがって、マニピュレータの製造、組み立て誤差によりグリッパ軸の初期姿勢にばらつきが生じても、グリッパ軸の試行動作中の把持開始推定信号faに基づいて、初期姿勢の誤差を推定して補正することができる。 Therefore, production of the manipulator, even if variations in the initial position of the gripper shaft by an assembly error, based on the grasped start estimation signal fa during trial operation of the gripper shaft, can be corrected by estimating the error in the initial position .

もちろん、本実施形態の手法によれば、物体の大きさによらず把持開始点を推定できるため、初期姿勢に誤差があっても、その影響を受けることなく、把持力を調整することができるのは言うまでもない。 Of course, according to the method of the present embodiment, since it is possible to estimate the grip start points regardless of the size of the object, even if there is an error in the initial position, it is possible without being affected, to adjust the gripping force It goes without saying.

初期姿勢の補正は、例えば以下の手順で行われる。 Correction of the initial orientation is carried out, for example, by the following procedure. マニピュレータを使用する前の準備動作等でグリッパに強く把持する動作を行わせて、初期姿勢誤差を推定して記録しておく。 Made to perform an operation for gripping strongly gripper before the preparation operation such as the use of manipulators, it is recorded by estimating the initial attitude error. 運用中に推定した把持開始点の最小値は、物体を把持せずにグリッパの第1および第2の作用部材325,326が接触した点である。 Minimum value of the gripping starting point estimated during operation is that first and second working members 325, 326 of the gripper without gripping the object is in contact. この最小値が運用中あるいは運用再開時に低下していることが確認されたら、動力伝達機構に異常が発生していると推定でき、簡易的に異常検知手段を実現できる。 If this minimum value is confirmed that the decrease at the time during or resuming operations management, can be estimated that an abnormality in the power transmitting mechanism occurs, it can be realized simply by the abnormality detection means. 例えば、動力伝達機構におけるワイヤの伸びやほつれ、ワイヤとプーリとの締結緩み、軸のずれなどにより、認識した把持開始点の低下が考えられる。 For example, elongation and fraying of the wires in the power transmission mechanism, fastening loose wire and pulley, due to misalignment of the shaft, reduction of the recognized gripping starting point is considered.

このように、把持開始点の最小値を記録して、その変化を観測することで、マニピュレータの異常検知を行うことができる。 Thus, by recording the minimum value of the gripping starting point, by observing the change, it is possible to perform abnormality detection of the manipulator. 同様に、準備動作等における物体を把持しない把持動作において、硬さ係数を算出して記録しておき、定期的な同等の動作でこの値を比較することで、ワイヤの硬さ変化すなわち、ワイヤの劣化を検知することができる。 Similarly, in the gripping operation which does not grip the object in the preparation operation and the like, may be recorded to calculate the hardness coefficient by comparing this value with regular operation equivalent to the hardness of the wire changes i.e., wire it is possible to detect the deterioration. 検知した結果は表示器等で使用者に知らせればよい。 The detected result may be caused known to the user in the display, and the like.

以上、作用部4の機構としてグリッパが物体を把持する場合について説明してきたが、本発明に係るマニピュレータシステムの作用部4においては、例えば、物体を二つに分ける作用を行う機構、物体を一定方向に押さえつける機構、物体を切断する機構などを有する作用部4に対して適用できるのは言うまでもない。 Although the gripper as a mechanism of action unit 4 has been described the case of gripping an object, in the action part 4 of the manipulator system according to the present invention, for example, a mechanism for performing the function of dividing the object into two, the object constant mechanism to press in the direction, of course can be applied to the working portion 4 having such mechanism for cutting the object. 例えば対象物を剥離する機構を有する作用部4を制御する場合は、図17においてグリッパ軸目標値の向きを反対にすればよい。 For example, when controlling the action part 4 having a mechanism for separating the object may be the direction of the gripper axis target value in the opposite 17. このようにシステム構成によって出力の向きが変わるが、それに応じて、出力信号の扱い方をプラス側をマイナス側、判定処理における「以上」を「以下」、「未満」を「より大きい」など、適宜変更すればよい。 Although this way changes the direction of the output by the system configuration, accordingly, the negative side on the positive side how to handle the output signal, "or more" and "or less" in the determination process, the "less than" and "greater than", it may be appropriately changed.

また、本発明に係るマニピュレータシステムは、医療用に限らず、例えば、配管内で異物除去を行う作業用ロボット、家庭やオフィス、店舗等で物品を運搬するロボットなどの種々の用途のシステムにも適用可能である。 Further, the manipulator system according to the present invention is not limited to medical, for example, a working robot for performing foreign matter removed in piping, home or office, in a variety of applications systems such as a robot for transporting goods in a store or the like it is applicable.

さらに、本発明に係るマニピュレータのアクチュエータは、電機モータに限らず、空圧アクチュエータ等でもよい。 Further, the actuator of the manipulator according to the present invention is not limited to electric motors, or pneumatically actuators. 本発明は、電機モータの電流に相当する量をセンシングできるアクチュエータで駆動されるマニピュレータシステムに幅広く適用可能である。 The present invention is widely applicable to a manipulator system which is driven by an actuator capable of sensing the amount corresponding to the current of the electric motor.

図5に示した制御装置2の内部構成の少なくとも一部は、ハードウェアで構成してもよいし、ソフトウェアで構成してもよい。 At least a portion of the internal structure of the control device 2 shown in FIG. 5 may be constituted by hardware, or may be configured by software. ソフトウェアで構成する場合には、制御装置2の少なくとも一部の機能を実現するプログラムをフレキシブルディスクやCD−ROM等の記録媒体に収納し、コンピュータに読み込ませて実行させてもよい。 When implemented in software, a program for executing at least some functions of the control device 2 and stored in a recording medium such as a flexible disk, a CD-ROM, or it may be executed by loading into the computer. 記録媒体は、磁気ディスクや光ディスク等の着脱可能なものに限定されず、ハードディスク装置やメモリなどの固定型の記録媒体でもよい。 The recording medium is not limited to a removable, such as a magnetic disk or optical disk, or a fixed recording medium such as a hard disk drive or a memory.

また、制御装置2の少なくとも一部の機能を実現するプログラムを、インターネット等の通信回線(無線通信も含む)を介して頒布してもよい。 Further, the program for executing at least some of the functions of the control unit 2, may be distributed through a communication line such as the Internet (including wireless communication). さらに、同プログラムを暗号化したり、変調をかけたり、圧縮した状態で、インターネット等の有線回線や無線回線を介して、あるいは記録媒体に収納して頒布してもよい。 Further, the program which is encrypted, or by modulating, in a compressed state via a wired line or a wireless line such as the Internet, or may be distributed to the recording medium storing the program.

この他、上記の記載に基づいて、当業者であれば、本発明の追加の効果を想到できたり、種々の変形が可能であるかもしれないが、これらも本発明の開示範囲に含まれるものと思料される。 In addition, based on the above description, those one skilled in the art or can envision additional advantages of the present invention, although it may be susceptible to various modifications, which are included in the disclosures of which are also present invention It is Shiryo with. 本発明の態様は、上述した個々の実施形態には限定されない。 Aspect of the present invention is not limited to the particular embodiments described above. 特許請求の範囲に規定された内容およびその均等物から定義される技術的範囲から逸脱しない範囲で種々の追加、変更および部分的削除が可能である。 Various additional without departing from the scope as defined by Claims in defined contents and their equivalents claims, but may be modified and partial deletion.

医療用マニピュレータシステムの外観図。 Exterior view of a medical manipulator system. 作業部と操作部を分離した状態を示す図。 Figure showing a separated state of the operating unit and the working unit. マニピュレータ本体の作用部の構造を示す斜視図。 Perspective view showing the structure of a working portion of the manipulator. 作用部の詳細構造を示す分解斜視図。 It exploded perspective view showing the detailed structure of the working portion. 制御装置とその周辺の制御系の詳細構成の一例を示すブロック図。 Controller block diagram showing an example of a detailed configuration of a control system of its periphery. 図5の演算部が実行するプログラムの一例を示すフローチャート。 Flow chart showing an example of a program operation unit executes in FIG. 制御装置によって制御される把持角度と実際の把持力との関係を示す図。 Diagram showing the relationship between the actual gripping force between the gripping angle is controlled by the control device. 把持開始推定オブザーバを有する制御装置が行う処理動作の一例を示す制御システム図。 Control system diagram showing an example of the processing operation of the control device having a gripping start estimation observer is carried out. 把持開始推定オブザーバのより詳細な内部構成の一例を示すブロック図。 Block diagram illustrating an example of a more detailed internal structure of the grasping start estimation observer. (a)〜(d)は図8の把持開始推定オブザーバの特定を示す図。 (A) ~ (d) is a diagram showing a specific gripping start estimation observer in FIG. 最閉角度決定部の処理手順を示すフローチャート。 Flowchart showing a processing procedure of top close angle determination unit. グリッパ軸を単独動作させた後、引き続いてグリッパ軸とヨー軸が同期して動作する場合の動作波形図。 After the gripper shaft is independent operation, operation waveform diagram in the case of the gripper axis and yaw axis is operated synchronously subsequently. (a)および(b)は操作桿の内部構造の一例を示す図。 (A) and (b) is a diagram showing an example of the internal structure of the operating rod. センサの出力電圧とグリッパ軸目標値との関係を示す図。 It shows the relationship between the output voltage and the gripper axis target value of the sensor. 把持開始点の推定を説明する図。 Diagram for explaining the estimation of the grip start point. マニピュレータが物体を把持するときのモータの出力軸角度と把持開始推定信号との関係を示すグラフ。 Graph showing the relationship between the output shaft angle and grip the start estimation signal of the motor when the manipulator to grip the object. 硬さ補正処理の処理手順の一例を示すフローチャート。 Flow chart illustrating an example of a processing procedure of the hardness correction.

符号の説明 DESCRIPTION OF SYMBOLS

1 マニピュレータ本体 2 制御装置 3 操作指令部 4 作用部 5 連結部 6 駆動部 7 作業部 8 操作部 51 把持開始推定オブザーバ 52 把持角度決定部 53 逆キネマティック行列演算部 54 PID制御部 55 モータ駆動部 56 目標把持角度設定部 57 最閉角度決定部 58 操作量更新部 1 manipulator 2 controller 3 operation command unit 4 acting portion 5 connecting portion 6 driving unit 7 working unit 8 operating portion 51 gripped start estimation observer 52 grip angle determining unit 53 inverse kinematics matrix calculator 54 PID controller 55 the motor driving unit 56 target grip angle setting unit 57 most close angle determining section 58 operation amount updating unit

Claims (14)

  1. アクチュエータと、 And the actuator,
    前記アクチュエータの動力に基づいて対象物に対して力を作用する作用部と、 A working unit which acts a force to the object based on the power of the actuator,
    可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、 Flexible member, a power transmission mechanism for transmitting the power of the actuator to the working portion,
    前記アクチュエータを制御する制御部と、を備え、 And a control unit for controlling the actuator,
    前記制御部は、 Wherein,
    角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号を生成するオブザーバ信号生成手段と、 Without taking into account the term is not dependent on at least one of the magnitude of the angular velocity and acceleration independent of the direction, the observer signal generating means for generating an observer signal for estimating the applied force starting angle of the working portion ,
    前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定する作用力開始角度推定手段と、 By comparing the signal level of the observer signal with a predetermined reference value, and the acting force starting angle estimating means for estimating an action force start angle,
    予め設定された前記作用部の作用力に対応する姿勢軸角度と前記作用力開始角度推定手段で推定された作用力開始角度とに基づいて、姿勢軸限界角度を検出する限界角度検出手段と、 Based on a preset estimated applied force starting angle in a posture axis angle and the acting force start angle estimating means corresponding to the acting force of the working portion, and the limit angle detecting means for detecting an attitude axis limit angle,
    前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御する角度制御手段と、を有することを特徴とするマニピュレータシステム。 Manipulator system characterized by having an a angle control means for angular control the attitude axis angle of the working portion to the posture axis limit angle.
  2. 複数のアクチュエータと、 A plurality of actuators,
    前記複数のアクチュエータの動力に基づいて対象物に対して力を作用する姿勢軸と、前記対象物をそれぞれ異なる方向に動作させる複数の動作軸と、を有する作用部と、 A working portion having a plurality of operation shafts for operating in different directions and the orientation axis of a force, the object to the object on the basis of the power of the plurality of actuators,
    可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、 Flexible member, a power transmission mechanism for transmitting the power of the actuator to the working portion,
    前記アクチュエータを制御する制御部と、を備え、 And a control unit for controlling the actuator,
    前記複数のアクチュエータは、前記姿勢軸および前記複数の動作軸の駆動に影響を与える干渉系アクチュエータであり、 Wherein the plurality of actuators are interference system actuators influencing the driving of the posture axis and the plurality of operation shafts,
    前記制御部は、 Wherein,
    前記複数のアクチュエータのそれぞれごとに設けられ、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号候補をそれぞれ生成する複数のオブザーバ信号候補生成手段と、 Wherein the plurality of provided for each actuator, without taking into account the terms that depend on but not dependent on at least one of the magnitude of the angular velocity and acceleration direction, to estimate the force acting start angle of the working portion a plurality of observers signal candidate generator means for generating the observer signal candidates, respectively,
    前記複数のアクチュエータの各駆動軸の動作速度に応じて、前記複数の作用力開始角度推定手段から出力されたオブザーバ信号のいずれか一つを選択するオブザーバ信号選択手段と、 According to the operation speed of each drive shaft of the plurality of actuators, and the observer signal selecting means for selecting any one of the output observer signals from said plurality of applied force start angle estimating means,
    前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定する作用力開始角度推定手段と、 By comparing the signal level of the observer signal with a predetermined reference value, and the acting force starting angle estimating means for estimating an action force start angle,
    予め設定された前記作用部の作用力に対応する姿勢軸角度と前記作用力開始角度推定手段で選択されたオブザーバ信号に対応する作用力開始角度とに基づいて、姿勢軸限界角度を検出する限界角度検出手段と、 On the basis of the acting force starting angle corresponding to the observer signal selected by the preset position shaft angle corresponding to the acting force of the working portion and the acting force start angle estimating means, the limit for detecting the posture axis limit angle and the angle detection means,
    前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御する角度制御手段と、を有することを特徴とするマニピュレータシステム。 Manipulator system characterized by having an a angle control means for angular control the attitude axis angle of the working portion to the posture axis limit angle.
  3. 前記オブザーバ信号選択手段は、前記複数のアクチュエータのうち、前記姿勢軸の駆動に直接関与するアクチュエータの駆動軸の動作速度が正であれば、このアクチュエータに対応するオブザーバ信号を選択し、前記姿勢軸の駆動に直接関与するアクチュエータの駆動軸の動作速度が負であれば、他のアクチュエータの中で駆動軸の動作速度が正のアクチュエータに対応するオブザーバ信号を選択することを特徴とする請求項2に記載のマニピュレータシステム。 The observer signal selecting means, among the plurality of actuators, when the operating speed of the drive shaft of the actuator to be directly involved in driving of the posture axis is positive, select the observer signal corresponding to the actuator, the posture axis if negative operating speed of the drive shaft of the actuator to be directly involved in the driving of, claim and selects an observer signal operating speed of the drive shaft corresponds to a positive actuator among other actuator 2 manipulator system according to.
  4. 前記制御部は、 Wherein,
    前記オブザーバ信号選択手段で選択されたオブザーバ信号と予め設定された動作軸補正基準値とに基づいて、前記複数の動作軸の動作量を補正するための動作軸補正開始角度を推定する補正開始角度推定手段と、 On the basis of the observer signal selected by the observer signal selecting means and the preset operation axis correction reference value, the correction starting angle for estimating the motion axis correction start angle for correcting the amount of operation of said plurality of operation shafts and estimating means,
    前記動作軸補正開始角度が推定された後に、前記作用部の姿勢軸角度に基づいて前記複数の動作軸の偏差角度を算出する偏差角度算出手段と、 After the operation axis correction start angle is estimated, the deviation angle calculating means for calculating the deviation angle of the plurality of motion axes based on the orientation axis angle of the working portion,
    前記偏差角度に基づいて前記複数の動作軸の動作量を補正する動作軸補正手段と、を有することを特徴とする請求項2または3に記載のマニピュレータシステム。 Manipulator system according to claim 2 or 3, characterized in that it has a, a motion axis correcting means for correcting the amount of operation of said plurality of motion axes on the basis of the deviation angle.
  5. 前記動作軸補正基準値は、前記作用部の作用力開始角度を推定するための基準値以下に設定されることを特徴とする請求項4に記載のマニピュレータシステム。 The operation axis correction reference value, the manipulator system according to claim 4, characterized in that it is set to be equal to or less than the reference value for estimating the applied force starting angle of the working section.
  6. 前記作用力開始角度推定手段は、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに前記作用部の作用力開始角度を推定するか、または前記項を考慮に入れて把持力を推定するかを切替える第1の切替手段を有することを特徴とする請求項1乃至5のいずれかに記載のマニピュレータシステム。 The acting force start angle estimating means, or does not depend on at least one of the magnitude of the angular velocity and acceleration estimates the acting force starting angle of the acting portion without taking into account the term depending on the direction, or the manipulator system according to any one of claims 1 to 5, characterized in that it has a first switching means for switching whether to estimate the gripping force taking into account the section.
  7. 前記第1の切替手段は、前記作用部が対象物を作用方向に動作させるときに、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに前記作用部の作用力開始角度を推定し、前記作用部が対象物の作用動作を解除したときに、前記項を考慮に入れて作用力を推定することを特徴とする請求項6に記載のマニピュレータシステム。 It said first switching means, said when the working portion operates the object to the working direction, without taking into account the term is not dependent on at least one of the magnitude of the angular velocity and acceleration which depends on the direction estimates the acting force starting angle of the working portion, the manipulator according to claim 6 wherein the working portion is at releasing the action operation of the object, characterized by estimating an action force to put the term into account system.
  8. 前記第1の切替手段は、前記対象物の姿勢軸の目標速度が負の場合に、前記作用部が対象物を作用方向に動作させていると判断し、前記対象物の姿勢軸の目標速度が正かゼロの場合に、前記作用部が前記対象物の作用動作を解除したと判断することを特徴とする請求項7に記載のマニピュレータシステム。 Said first switching means, when the target speed of the posture axis of the object is negative, it is determined that the working portion is running the object to the working direction, the target speed of the posture axis of the object manipulator system according to claim 7 but that in the case of a positive or zero, the acting portion is characterized in that it is determined that releasing the action operation of the object.
  9. 前記制御部は、前記作用部が前記対象物を作用方向に動作させ、かつ前記オブザーバ信号が所定の基準値以上である場合に、作用開始を示す作用フラグ情報をセットし、前記作用部の姿勢軸角度が前記作用力開始角度よりも大きくなった場合に、前記作用部が前記対象物への作用を終了したとみなして前記作用フラグ情報をリセットする作用フラグ決定手段を有することを特徴とする請求項1乃至8のいずれかに記載のマニピュレータシステム。 Wherein, when said working unit operates the said object in the direction of action, and the observer signal is equal to or greater than a predetermined reference value, sets an action flag information indicating a start action, the attitude of the working portion If the axial angle is greater than the working force start angle, the working unit is characterized by having an effect flag decision means for resetting the working flag information is regarded to have ended the action on the object manipulator system according to any one of claims 1 to 8.
  10. 前記作用部の作用力を操作指令する操作指令部を備え、 An operation command section for operation command acting force of the working portion,
    前記制御部は、前記作用力開始角度推定手段により推定された作用力開始角度に対応する前記操作指令部の操作量を基点として、前記操作指令部の操作量と前記作用部の姿勢軸角度との相関関係に基づいて、前記作用部の作用力を調整することを特徴とする請求項1乃至8のいずれかに記載のマニピュレータシステム。 Wherein the control unit as a base point the operation amount of the operation command unit corresponding to the acting force start angle estimated by the working force start angle estimating means, the operation amount of the operation command section and the posture axis angle of the working portion manipulator system according to any one of claims 1 to 8 based on the correlation, and adjusting the working force of the working part of.
  11. 前記制御部は、前記操作指令部の操作量が最大のときに最大把持力となるように前記相関関係を設定することを特徴とする請求項10に記載のマニピュレータシステム。 Wherein the control unit, a manipulator system according to claim 10, characterized in that the operation amount of the operation command unit sets the correlation to be a maximum gripping force at the maximum.
  12. 前記制御部は、 Wherein,
    前記作用部の姿勢軸角度が作用力開始角度と姿勢軸限界角度との間にある場合に、前記作用部の姿勢軸の駆動に直接関係するアクチュエータの駆動軸角度と、該アクチュエータに対応する前記作用力開始角度推定手段から出力されたオブザーバ信号とを記録する記録手段と、 When the posture axis angles of the working portion is between the working force starting angle and the orientation axis angle limit, a drive shaft angle of the actuator that is directly related to the driving of the posture axis of said working portion, the corresponding to the actuator and recording means for recording the observer signal output from the action force start angle estimating means,
    前記作用部の姿勢軸角度が前記姿勢軸限界角度に達したときに、前記記録手段で記録された駆動軸角度とオブザーバ信号とを用いて前記対象物の硬さを推定する硬さ推定手段と、 When the posture axis angles of the working portion reaches the posture axis limit angle, and hardness estimating means for estimating the hardness of the object using the drive shaft angle and an observer signal recorded by said recording means ,
    予め設定された基準の硬さと前記硬さ推定手段で推定された硬さとに基づいて、前記姿勢軸限界角度を更新する姿勢軸限界角度更新手段と、を有することを特徴とする請求項1乃至11のいずれかに記載のマニピュレータシステム。 Based on the preset reference hardness and the and hardness estimated by the hardness estimation means, claim 1, characterized in that it has a, an attitude axis limit angle updating means for updating the attitude axis limit angle 11 manipulator system according to any one of.
  13. 前記作用力開始角度推定手段により推定された作用力開始角度とこの作用力開始角度の最小値とのずれ量に基づいて、異常検知を行う異常検知手段を備えることを特徴とする請求項1乃至12のいずれかに記載のマニピュレータシステム。 Based on the shift amount between the minimum value of the acting force starting angle and acting force start angle estimated by the working force start angle estimating means, claim 1, characterized in that it comprises an abnormality detection means for performing abnormality detection 12 manipulator system according to any one of.
  14. 対象物の姿勢軸を駆動するアクチュエータと、 An actuator for driving the posture axis of the object,
    前記アクチュエータの動力に基づいて前記対象物に対して力を作用する作用部と、 A working unit which acts a force to the object based on the power of the actuator,
    可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、 Flexible member, a power transmission mechanism for transmitting the power of the actuator to the working portion,
    前記アクチュエータを制御する制御部と、を備えたマニピュレータシステムの制御方法であって、 A control method for a manipulator system comprising a control unit for controlling the actuator,
    前記制御部は、 Wherein,
    角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号を生成するステップと、 A step is not dependent on at least one of the magnitude of the angular velocity and acceleration without considering a term depending on the direction, to generate an observer signal for estimating the applied force starting angle of the working portion,
    前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定するステップと、 By comparing the signal level of the observer signal with a predetermined reference value, estimating an action force start angle,
    予め設定された前記作用部の作用力に対応する姿勢軸角度と前記推定された作用力開始角度とに基づいて、姿勢軸限界角度を検出するステップと、 Based the preset position shaft angle corresponding to the acting force of the working portion and the estimated applied force starting angle, a step of detecting an attitude axis limit angle,
    前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御するステップと、を有することを特徴とするマニピュレータシステムの制御方法。 Control method for a manipulator system characterized by having a step of angle controls the posture axis angles of the working portion to the posture axis limit angle.
JP2008244411A 2008-09-24 2008-09-24 Manipulator system and control method thereof Pending JP2010076012A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2008244411A JP2010076012A (en) 2008-09-24 2008-09-24 Manipulator system and control method thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2008244411A JP2010076012A (en) 2008-09-24 2008-09-24 Manipulator system and control method thereof

Publications (1)

Publication Number Publication Date
JP2010076012A true JP2010076012A (en) 2010-04-08

Family

ID=42207054

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2008244411A Pending JP2010076012A (en) 2008-09-24 2008-09-24 Manipulator system and control method thereof

Country Status (1)

Country Link
JP (1) JP2010076012A (en)

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013018934A1 (en) 2011-08-04 2013-02-07 Olympus Corporation Manipulation input device and manipulator system having the same
WO2013018935A1 (en) 2011-08-04 2013-02-07 Olympus Corporation Surgical instrument and control method thereof
WO2014021433A1 (en) * 2012-08-02 2014-02-06 東芝機械株式会社 Robotic apparatus and control method therefor
CN104385279A (en) * 2014-09-26 2015-03-04 京东方科技集团股份有限公司 Manipulator demonstration method and system
US9161772B2 (en) 2011-08-04 2015-10-20 Olympus Corporation Surgical instrument and medical manipulator
US9218053B2 (en) 2011-08-04 2015-12-22 Olympus Corporation Surgical assistant system
US9244523B2 (en) 2011-08-04 2016-01-26 Olympus Corporation Manipulator system
US9423869B2 (en) 2011-08-04 2016-08-23 Olympus Corporation Operation support device
CN106029307A (en) * 2014-02-28 2016-10-12 奥林巴斯株式会社 Calibration method for manipulator, manipulator, and manipulator system
US9477301B2 (en) 2011-08-04 2016-10-25 Olympus Corporation Operation support device and assembly method thereof
US9519341B2 (en) 2011-08-04 2016-12-13 Olympus Corporation Medical manipulator and surgical support apparatus
US9524022B2 (en) 2011-08-04 2016-12-20 Olympus Corporation Medical equipment
US9568992B2 (en) 2011-08-04 2017-02-14 Olympus Corporation Medical manipulator
US9632577B2 (en) 2011-08-04 2017-04-25 Olympus Corporation Operation support device and control method thereof
US9632573B2 (en) 2011-08-04 2017-04-25 Olympus Corporation Medical manipulator and method of controlling the same
US9851782B2 (en) 2011-08-04 2017-12-26 Olympus Corporation Operation support device and attachment and detachment method thereof
WO2019039612A3 (en) * 2017-08-21 2019-04-18 地方独立行政法人神奈川県立産業技術総合研究所 Forceps system

Cited By (26)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9477301B2 (en) 2011-08-04 2016-10-25 Olympus Corporation Operation support device and assembly method thereof
WO2013018935A1 (en) 2011-08-04 2013-02-07 Olympus Corporation Surgical instrument and control method thereof
JP2013034832A (en) * 2011-08-04 2013-02-21 Olympus Corp Surgical instrument and control method thereof
US9632573B2 (en) 2011-08-04 2017-04-25 Olympus Corporation Medical manipulator and method of controlling the same
CN103717170A (en) * 2011-08-04 2014-04-09 奥林巴斯株式会社 The surgical instrument and its control method
US9851782B2 (en) 2011-08-04 2017-12-26 Olympus Corporation Operation support device and attachment and detachment method thereof
US9632577B2 (en) 2011-08-04 2017-04-25 Olympus Corporation Operation support device and control method thereof
US9568992B2 (en) 2011-08-04 2017-02-14 Olympus Corporation Medical manipulator
US9524022B2 (en) 2011-08-04 2016-12-20 Olympus Corporation Medical equipment
US9161772B2 (en) 2011-08-04 2015-10-20 Olympus Corporation Surgical instrument and medical manipulator
US9218053B2 (en) 2011-08-04 2015-12-22 Olympus Corporation Surgical assistant system
US9244524B2 (en) 2011-08-04 2016-01-26 Olympus Corporation Surgical instrument and control method thereof
US9244523B2 (en) 2011-08-04 2016-01-26 Olympus Corporation Manipulator system
WO2013018934A1 (en) 2011-08-04 2013-02-07 Olympus Corporation Manipulation input device and manipulator system having the same
US9423869B2 (en) 2011-08-04 2016-08-23 Olympus Corporation Operation support device
US9519341B2 (en) 2011-08-04 2016-12-13 Olympus Corporation Medical manipulator and surgical support apparatus
US9671860B2 (en) 2011-08-04 2017-06-06 Olympus Corporation Manipulation input device and manipulator system having the same
CN104507645B (en) * 2012-08-02 2016-04-27 东芝机械株式会社 Robot apparatus and a control method
US9434073B2 (en) 2012-08-02 2016-09-06 Toshiba Kikai Kabushiki Kaisha Robot apparatus and control method therefor
GB2518576B (en) * 2012-08-02 2015-09-02 Toshiba Machine Co Ltd Robotic apparatus and control method therefor
JP5727103B2 (en) * 2012-08-02 2015-06-03 東芝機械株式会社 Robot apparatus and control method thereof
CN104507645A (en) * 2012-08-02 2015-04-08 东芝机械株式会社 Robotic apparatus and control method therefor
WO2014021433A1 (en) * 2012-08-02 2014-02-06 東芝機械株式会社 Robotic apparatus and control method therefor
CN106029307A (en) * 2014-02-28 2016-10-12 奥林巴斯株式会社 Calibration method for manipulator, manipulator, and manipulator system
CN104385279A (en) * 2014-09-26 2015-03-04 京东方科技集团股份有限公司 Manipulator demonstration method and system
WO2019039612A3 (en) * 2017-08-21 2019-04-18 地方独立行政法人神奈川県立産業技術総合研究所 Forceps system

Similar Documents

Publication Publication Date Title
Preusche et al. Teleoperation concepts in minimal invasive surgery
JP5875973B2 (en) Medical manipulator
JP5105147B2 (en) Robot and a control method
US9402689B2 (en) Control system for reducing internally generated frictional and inertial resistance to manual positioning of a surgical manipulator
US20070120512A1 (en) Method for controlling a robot arm, and robot for implementing the method
US7741802B2 (en) Medical robotic system with programmably controlled constraints on error dynamics
JP3926119B2 (en) Medical manipulator
EP2859999A2 (en) Robot controller, robot system, robot, robot control method, and program
JP6305673B2 (en) Robot control system, robot systems and robot
US20140246258A1 (en) Electromechanically counterbalanced humanoid robotic system
EP1977713A2 (en) Manipulator and control method therefor
KR101806385B1 (en) Drive force control in medical instrument providing position measurements
JP5306313B2 (en) Robot controller
CN1291698C (en) Medical manipulator, its control device and control method
Hashtrudi‐Zaad et al. Bilateral parallel force/position teleoperation control
US9008842B2 (en) Medical robotic system with sliding mode control
US9743990B2 (en) Tension control in actuation of multi-joint medical instrument
JP5242342B2 (en) Robot controller
US20090069942A1 (en) Robot apparatus and method of controlling the same
KR101795847B1 (en) Method for programming an industrial robot and related industrial robot
MX338904B (en) Force estimation for a minimally invasive robotic surgery system.
CN101239467A (en) Robot control device
JP6087368B2 (en) Prompting the operator of the input device to the instructions that assume a suitable position in the joint device, application of a force feedback input device
WO2008012386A1 (en) Robotic system for assisting in minimally-invasive surgery, which can position a surgical instrument in response to orders from a surgeon, is not attached to the operating table and does not require pre-calibration of the insertion point
CN104440913A (en) Method for manually adjusting the pose of a manipulator arm of an industrial robot and industrial robots