JP2017104914A - External force follow-up control system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide an external force follow-up control system capable of holding a position of an arm, even if a load is removed from a state of moving by external force, by accurately operating as a command in no-load time.SOLUTION: In a position command value generation part 72, a difference or an external force signal of an expansion arm part caused by external force is acquired. When force of exceeding a value preset in a control object is added as a load, a position target value by a controller 80 is corrected in the force-escaping (load-removing) direction. A target position is superscribed-updated by turning aside the external force, so that when the external force is lost, a measured value can be converged in the target value.SELECTED DRAWING: Figure 3

Description

  The present invention relates to an external force tracking control system.

  2. Description of the Related Art Conventionally, there is known a control device that can perform an avoidance action when contacting a robot arm (for example, see Patent Document 1).

In such a case, each joint is controlled based on information from a plurality of external force measurement sensors attached to the robot arm.
The control unit separates the torque applied to each joint from the information of the external force measurement sensor, and calculates the escape amount of the arm joint from the set joint virtual impedance.
Further, the escape amount of the arm hand is calculated from the set hand tip virtual impedance.
Then, the control unit adds the escape amount of the arm joint portion to the escape amount of the hand portion and gives a drive command to the arm. Thereby, an appropriate avoidance action is performed no matter which part of the arm is applied with an external force.

JP 2001-38664 A

  As a robot for performing such arm control, a surgical robot, for example, an endoscope holder robot that holds an optical tube at a desired position instead of assistance by a scopist who operates an endoscope is known. Yes.

In the endoscope holder robot, an external force may be applied to the arm before the operator notices, such as a camera cable being pulled during surgery or an operator coming into contact with the holder.
In such an endoscope holder robot, when the arm is released (unloaded) from the load due to the external force, the arm is intended to return to the posture before the external force is applied.
For this reason, the position of the optical viewing tube attached to the arm moves abruptly, making it impossible to observe the site to be observed in the body that was visible by the endoscope.

  Therefore, the present invention provides an external force follow-up control system that can accurately operate as instructed when there is no load and can maintain the position of the arm even when unloaded from a state of being moved by an external force. Is an issue.

  An external force tracking control system according to the present invention includes a robot arm, an input unit that indicates a position of the robot arm using a position target value, and an actuator that moves the robot arm according to the position target value input from the input unit A position detection unit that detects the position of the robot arm, a control unit that drives the actuator in accordance with a difference between the position response value indicating the position of the robot arm detected by the position detection unit, and the position target value. Prepare. The control unit is characterized in that the position target value is corrected according to the external force applied to the robot arm.

According to such a configuration, the actuator moves the robot arm according to the position target value output to the actuator.
When an external force is applied, the position target value is corrected according to the position of the robot arm moved by the external force.
For this reason, the moved robot arm does not return to the original position before the external force is applied even if the external force applied to the robot arm disappears according to the position target value.

  According to the present invention, there is provided an external force follow-up control system that can operate accurately as instructed when there is no load and can maintain the position of the arm even when unloaded from a state of being moved by an external force. .

It is a perspective view explaining composition of an endoscope holder robot in an external force tracking control system of an embodiment. It is a block diagram explaining the structure of each part in the external force tracking control system of embodiment. It is a functional block diagram in the external force tracking control system of the embodiment. It is a flowchart explaining operation | movement of the external force tracking control system of embodiment. It is a graph which shows a verification result in the external force tracking control system of an embodiment. In the external force tracking control system of the embodiment, (a) is a schematic plan view showing a state in which an external force is applied to the arm, and (b) is the external force tracking control system of the embodiment, in which an external force is continuously applied to the arm. (C) is a schematic plan view showing a state in which an external force is no longer applied to the arm in the external force tracking control system of the embodiment. It is a block diagram explaining the structure of each part in the external force tracking control system of Example 1. FIG. In the external force tracking control system of Example 1, it is a functional block diagram. It is a flowchart explaining operation | movement of the external force tracking control system of Example 1. FIG. In the external force tracking control system of Example 2, it is a block diagram explaining the structure of each part. In the external force follow-up control system of Example 2, it is a functional block diagram. In the external force tracking control system of Example 3, it is a block diagram explaining the structure of each part. In the external force tracking control system of Example 3, it is a functional block diagram. It is a functional block diagram which estimates an external force with the one external force tracking control system of the modification 1. FIG. It is a functional block diagram which estimates an external force in the external force tracking control system of the modification 2.

  An embodiment of the present invention will be described in detail with reference to FIGS. 1 to 6. In the description, the same elements are denoted by the same reference numerals, and redundant description is omitted.

FIG. 1 is a perspective view of an endoscope holder robot to which the external force tracking control system of the embodiment is applied.
The endoscope holder robot 10 to be controlled in this embodiment mainly includes a robot body 11 and a telescopic arm section 30 provided to be rotatable about the axis Y in the vertical direction with respect to the robot body 11. And an endoscope holder portion 40 that is provided on the telescopic arm portion 30 and holds the rigid endoscope 20.

  A controller 80 as an input unit is attached to the head or arm of the person performing the operation. Then, in response to a command signal output in accordance with the movement of the controller 80, the telescopic arm unit 30 is rotated and moved about the axis Y as a rotation center, or by driving, the rigid endoscope 20 is moved in the front-rear direction. The zoom movement can be performed by using a built-in actuator. In this embodiment, control of a rotary actuator is mainly described.

  The rigid endoscope 20 of this embodiment has a distal end inserted into the body, and an optical tube 22 that guides an image of a portion to be observed in the body, and the optical tube 22 that is rotatably connected to be guided. And a camera head unit 24 that outputs an image signal by receiving the received image.

The camera head unit 24 is provided with a CCD image sensor having a light receiving plane. The CCD image sensor photoelectrically converts light and darkness of an image formed on a light receiving plane through a lens or the like into an amount of electric charge, and sequentially reads out and converts it into an electrical image signal.
Then, the image signal output from the CCD image sensor of the camera head unit 24 is generated as an output image signal by the image generation device, and is output so as to project the observation site in the body on a monitor (not shown). ing.

FIG. 2 is a configuration diagram illustrating the configuration of each unit in the external force tracking control system of the embodiment.
The endoscope holder robot 10 is connected to the control unit 70 via the input / output unit 60.
The endoscope holder robot 10 according to this embodiment includes a pneumatic actuator 12 as an actuator that rotationally drives the telescopic arm unit 30 about a vertical axis Y (see FIG. 1) of the robot body 11, and the pneumatic actuator 12. The optical rotary encoder 14 is included as position detecting means for detecting the rotation angle of the telescopic arm portion 30 rotated by.

  Among these, the pneumatic actuator 12 uses air pumped from a power source 16 such as a pneumatic pump as a driving force. The pneumatic actuator 12 is connected to the driving force control device 18. The driving force control device 18 drives the pneumatic actuator 12 in accordance with a driving force control signal from the input / output unit 60.

  The optical rotary encoder 14 detects the rotational position of the telescopic arm unit 30 with the axis Y (see FIG. 1) as the center of rotation. The rotational position is converted into an electrical signal and output to the input / output unit 60 as a position response value.

  In addition, the endoscope holder robot 10 of this embodiment is provided with a force detection unit 42. The force detection unit 42 is configured to output a force signal and an external force signal as force response values to the input / output unit 60.

The endoscope holder robot 10 according to the embodiment configured as described above has a position signal and a force input to the input / output unit 60 when the control command from the control unit 70 is converted into a driving force signal by the input / output unit 60. The telescopic arm unit 30 is configured to move to a desired position and rotation angle by feedback according to a state quantity such as a signal and an external force signal.
FIG. 3 is a functional block diagram of the external force tracking control system according to the embodiment. The position control system of the endoscope holder robot 10 of this embodiment is mainly provided in the control unit 70, and includes a position command value generation unit 72, a position controller 74, a driving force controller 76, a self-weight compensation control. Instrument 78 is included.

Among these, the position command value generation unit 72 is connected to the controller 80, and is configured such that an external input by a button operation or the like of the controller 80 is input to the position command value generation unit 72.
The position command value generation unit 72 further generates a position target value using the state (rotation angle) of the telescopic arm unit 30 of the endoscope holder robot 10 to be controlled and the estimated or measured external force value as state quantities. Then, this position target value is output.
The position response value indicating the actual rotational position of the telescopic arm unit 30 detected by the optical rotary encoder 14 is input to the control unit 70 via the input / output unit 60. As the position response value, the position response value is not limited to the position signal from the position detection unit, but a signal (such as a speed estimation value) generated by the position signal may be used as the position response value.

The position controller 74 receives a difference between the position target value and the position response value from the control target.
The position controller 74 performs calculation for position control so that the difference between the position target value and the position response value from the controlled object is 0, and generates a driving force reference value as the calculation result. The driving force reference value is output from the position controller 74 to drive the actuator and command the position of the robot arm.

A difference between the driving force reference value and the force response value from the control target is input to the driving force controller 76.
The driving force controller 76 inputs a driving force signal that makes the difference between the driving force reference value and the force response value (= force signal) from the controlled object to zero to the endoscope holder robot 10 that is the controlled object. As shown, the driving force control is executed. The input of this embodiment is a voltage (V) applied to the servo valve 118 and the like mounted as the driving force control device 18.

In this embodiment, in order to improve the control characteristics, feedforward compensation such as self weight compensation force based on the position response value (= position signal) may be performed using the self weight compensation controller 78 or the like.
The dead weight compensation controller 78 is for removing the influence of the dead weight of the endoscope holder robot 10 assumed in advance.
In other words, the telescopic arm unit 30 varies the influence of its own weight depending on the operation position (rotation angle) in a state in which the optical tube 22 of the endoscope is mounted. For this reason, the control characteristics can be further improved by using the self-weight compensation controller 78 as a feedforward compensation for the self-weight compensation force that cancels the influence of the self-weight of the endoscope holder robot 10.

There are two types of force response values: a force signal from the force detector 42 and an external force signal that outputs the applied external force as an estimated value. The force signal or the external force signal is input to the control unit 70 via the input / output unit 60.
The force detection unit 42 may be, for example, an algorithm that calculates an estimated value. Further, a force signal may be obtained by using means such as a force sensor provided in the endoscope holder robot 10.

  The difference between the driving force reference value and the force response value is input to the driving force controller 76. The driving force controller 76 performs control so that the difference between the driving force reference value and the force response value is zero. Then, a driving force signal is output as a control result (see FIG. 2). The driving force signal is input to the driving force control device 18 to drive the pneumatic actuator 12 to move the telescopic arm unit 30.

  The position response value is a state quantity indicating the current position measured by the optical rotary encoder 14. The driving force is obtained by multiplying the value detected by the force detection unit 42 such as a pressure sensor by the pressure receiving area of the servo valve cylinder. Then, the state quantity detected by each sensor is input from the endoscope holder robot 10 to the input / output unit 60 as a force signal or an external force signal.

FIG. 4 is a flowchart for explaining the operation of the external force tracking control system of the embodiment.
In this embodiment, the position command value generation unit 72 acquires the position deviation of the extendable arm unit 30 caused by an external force or an external force signal.
When a force exceeding a preset value is applied as a load to the control target, the position target value by the controller 80 is corrected in a direction in which the force escapes (unloads).

On the other hand, in general compliance control / impedance control, the position target value is corrected by an external force. For this reason, after the load is released (unloaded), an operation of returning to the position target value before correction occurs.
In contrast, in the external force tracking control system of the present embodiment, the position target value is overwritten while being updated according to the external force. For this reason, the return operation | movement of the expansion-contraction arm part 30 by releasing of load does not generate | occur | produce.
That is, when the process is started in FIG. 4, in step S1, a position deviation and an external force signal are acquired.

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

ここで、Ｔは収束させるための時間、ｓｔは制御周期、Ｑは修正速度を調整するパラメータである。
ステップＳ４では、位置目標値を更新しながら上書きして処理を終了する。 In step S2, it is determined whether or not the state quantity exceeds a threshold value. If the state quantity exceeds the threshold value (YES in step S2), the process proceeds to the next step S3. If the state quantity does not exceed the threshold value (NO in step S2), the process ends. To do.
Here, when the deviation exceeds a preset threshold eLimit, the position target value q ^ref (for example, 30 degrees) is corrected by the following equation (1) in Step S2.

In step S3, the position correction amount is calculated from the external force and the internal state. This position correction includes setting the time T during which the correction algorithm operates.
Then, when the time since the start of the correction process is counted and within the operation time of the algorithm, the position target value is corrected by the difference equation (Equation 2) of the following formula 2 in step S3.

Here, T is a time for convergence, st is a control period, and Q is a parameter for adjusting the correction speed.
In step S4, the position target value is overwritten while being updated, and the process is terminated.

In the external force tracking control system of this embodiment, the upper limit and speed of time required for correcting the position target value can be specified by setting the control cycle st and the correction speed Q.
Further, in the external force tracking control system of this embodiment, by using only the position deviation as the state quantity detected by the optical rotary encoder 14, it is not necessary to separately provide an external force signal and an external force detection means or an external force estimation mechanism. .

Further, the current state quantity q is compared with the position target value q ^ref (step S2), and the position correction amount is calculated in step S3. For this reason, the measured value can be converged to the target value when the external force disappears by receiving the external force and overwriting and updating the target position in step S4.
Therefore, it is suitable for use in medical robots having back drivability such as pneumatic actuators.

  FIG. 5 is a graph showing a verification result in the external force tracking control system of the embodiment. FIG. 6 is an external force tracking control system according to the embodiment, and FIG. 6A is a schematic plan view showing a state in which an external force is applied to the arm. (B) is a schematic plan view showing a state in which an external force continues to be applied to an arm in the external force tracking control system of the embodiment. (C) is a schematic plan view showing a state in which no external force is applied to the arm in the external force tracking control system of the embodiment.

Here, the angle of the telescopic arm unit 30 of the endoscope holder robot 10 will be described as a state quantity, a state where no external force is applied, and a threshold value of 30 degrees.
In FIG. 5, when an external force is applied to rotate the telescopic arm portion 30 in the circumferential direction as indicated by an arrow in FIG. 6 at a time point a when 4 seconds have elapsed from the start of measurement, the rotation angle of the telescopic arm portion 30 increases. . Then, the tracking of the position command value is started from the time point b when the deviation between the position command value and the position response value exceeds 30 degrees.

  At the time point c, the position of the telescopic arm portion 30 reaches about 80 degrees as shown in FIG. From the time point c to the time point d, an external force is continuously applied to the telescopic arm unit 30, and the position of the telescopic arm unit 30 is held at about 80 degrees.

At a time point d, the correction algorithm ends when a predetermined time T elapses from a time point b at which the correction algorithm is started.
At time e, the external force applied to the telescopic arm 30 is unloaded.
The telescopic arm unit 30 returns to the angle (about 72 degrees here) specified by the position command value by the action of the position control in the time from the time point e to the time point f (see FIG. 6C).

However, the return angle is not large as compared with the operation in which the telescopic arm portion 30 suddenly returns to the original position shown in FIG. 6A, and the rotation speed is suppressed so as not to increase.
For this reason, in this example, the telescopic arm portion 30 continues to stop at a position of about 80 degrees without becoming the posture before the original external force is suddenly applied. Therefore, since the position of the optical visual tube 22 attached to the telescopic arm portion 30 does not move, it is possible to safely observe the observed site in the body continuously.

  After time point f, the position command value and the position response value continue to match. Thus, in the endoscope holder robot of this embodiment, the measured value is converged to the target value when the external force disappears by receiving the external force and overwriting and updating the target position at the current position. Can do.

Further, by positioning the rigid endoscope 20 by the operator's hand, the initial position target value is overwritten with the actually measured value. Therefore, the subsequent transition to the self-supporting control of the telescopic arm section 30 can be performed seamlessly without using an operation such as a changeover switch. Therefore, smooth progress of the operation is not hindered.
(Method based on position)

FIG. 7 is a configuration diagram illustrating the configuration of each unit in the external force tracking control system according to the first embodiment.
Note that the same parts as those in FIG.
First, the configuration will be described. In the external force tracking control system of the first embodiment, a servo valve 118 is provided as a driving force control device for the endoscope holder robot 110.

The servo valve 118 is configured to receive a supply of compressed air used for controlling the pneumatic actuator 12 from a compressed air source 116 as a power source. This servo valve 118 receives the driving force signal from the input / output unit 60 and adjusts the compressed air. The pressure sensor 120 as a force detection unit detects the pressure of the servo valve 118 and outputs it to the input / output unit 60 as a force signal.
The control unit 70 is configured to drive the telescopic arm unit 30 of the endoscope holder robot 110 by the pneumatic actuator 12 by generating a driving force signal using the force signal as a force response value. .

FIG. 8 is a functional block diagram of the external force tracking control system according to the first embodiment.
The position control system of the endoscope holder robot 110 according to the first embodiment is mainly provided in the control unit 70, and includes a position command value generation unit 72, a position controller 74, a driving force controller 76, and self-weight compensation. A controller 78 is included.
Then, the driving force controller 76 uses the input u as the control target so as to correct the position target value by estimating the external force from the deviation using the rotational position of the telescopic arm unit 30 as the state quantity. It is configured to be given to the holder robot 10.

FIG. 9 is a flowchart for explaining the operation of the external force tracking control system according to the first embodiment.
That is, when the process is started in FIG. 9, the position deviation is acquired in step S11.

In step S12, it is determined whether or not the position deviation exceeds a threshold value. If the position deviation exceeds the threshold value (YES in step S12), the process proceeds to the next step S13 to correct the position target value, and the correction time that is the time T set by the correction algorithm. To reset.
If the position deviation does not exceed the threshold value (NO in step S12), the process proceeds to step S14.

  In step S14, it is determined whether it is within the correction time. If it is within the correction time (YES in step S14), the process proceeds to step S15. If it is not within the correction time (NO in step S14), the process ends.

In step S15, a position correction amount is calculated. In step S16, the correction time is counted up. In step S17, the position target value is output, and the process is terminated.
Thus, in the external force tracking control system according to the first embodiment, the arm position can be maintained even when the load is removed from the state of being moved by the external force when it is not loaded and operates accurately as instructed.

  As described above, in Example 1, in addition to the operation and effect of the external force tracking control system of the embodiment, an existing position sensor such as the optical rotary encoder 14 used for feedback control can be easily used to cope with an abnormal external force. It is possible to perform suppression control of follow-up and return operations.

(Method based on force)
FIG. 10 is a configuration diagram illustrating the configuration of each unit in the external force tracking control system according to the second embodiment.
The same parts as those in the embodiment and the first embodiment are denoted by the same reference numerals and description thereof is omitted, and the configuration of each part will be described focusing on the differences.

  In the external force tracking control system of the second embodiment, an external force is used as a state quantity as a criterion for determining whether or not position correction is performed. As a force detector 122 for detecting an external force, a force sensor 124 for detecting an external force applied to the telescopic arm unit 30 is provided separately from the pressure sensor 120 for controlling the pneumatic actuator 12.

  The servo valve 118 that has received compressed air used for controlling the pneumatic actuator from a compressed air source as a power source (not shown) detects pressure by a pressure sensor 120 as a force detector, and inputs and outputs as a force signal. To the unit 60.

In the second embodiment, a force sensor 124 that outputs an external force signal is provided in the force detection unit 122 independently of the force signal.
The force sensor 124 is provided on the telescopic arm unit 30 which is a movable part of the endoscope holder robot 210, and can detect an external force applied to the telescopic arm unit 30. The external force detected by the force sensor 124 is configured to be sent to the input / output unit 60 as an external force signal.

ここで、Ｋは外力と位置修正値とを関係づける仮想コンプライアンスである。Ｋは、たとえばバネのような概念である。また、Ｔは収束させるための時間、ｓｔは制御周期、Ｑは修正速度を調整するパラメータである。なお、後述するばねの概念に加えてダンパ成分や質量も加えた概念（仮想インピーダンス）を用いてもよい。 FIG. 11 is a functional block diagram of the external force tracking control system according to the second embodiment.
In the external force tracking control system of the second embodiment configured as described above, when the external force F ^ext detected by the pressure sensor 120 exceeds the threshold value F ^ext _Limit , in step S3 of FIG. The position target value qref is corrected using Expression (3).

Here, K is a virtual compliance that relates the external force and the position correction value. K is a concept like a spring, for example. T is a time for convergence, st is a control period, and Q is a parameter for adjusting the correction speed. A concept (virtual impedance) in which a damper component and mass are added in addition to the concept of a spring described later may be used.

  Next, the effect of the external force tracking control system of the second embodiment will be described. In the external force tracking control system of the second embodiment, the force sensor 124 is provided in the force detection unit 122 independently of the pressure sensor 120 used when driving the pneumatic actuator 12.

For this reason, the external force applied to the telescopic arm part 30 can be detected independently. For this reason, by using an external force as the state quantity, it is possible to design the servo mechanical rigidity used for position control independently of the response to the external force. Therefore, for example, it is suitable for use in an external force tracking control system having a configuration such as an electric motor using a speed reducer.
Other configurations and functions and effects are the same as those of the above-described embodiment and Example 1, and thus description thereof is omitted.
(Method to estimate external force)

FIG. 12 is a configuration diagram illustrating the configuration of each unit in the external force tracking control system according to the third embodiment.
In addition, the same code | symbol is attached | subjected to the part same as the said embodiment and Example 1, 2, and description is abbreviate | omitted.
First, in terms of structural differences, the controller 170 used in the external force tracking control system of the third embodiment is provided with an external force estimator 171.
The external force estimator 171 can calculate the external force applied to the telescopic arm unit 30 by estimating from the driving state of the pneumatic actuator 12.

FIG. 13 is a functional block diagram of the external force tracking control system according to the third embodiment. In the external force tracking control system according to the third embodiment, the driving force reference value f _k ^ref and the position response value q ^res are input to the external force estimator 171.
The external force applied to the telescopic arm unit 30 is estimated by subtracting the force used by the endoscope holder robot 10 for driving the telescopic arm unit 30 from the force applied to the telescopic arm unit 30. It is configured as follows.

  The estimated external force is input as an estimated external force to the position command value generation unit 72 and is used by the position command value generation unit 72 to generate a position target value, as in the first and second embodiments. The estimation of the external force may be started when the angle of the telescopic arm unit 30 measured by the optical rotary encoder 14 deviates from the position target value by a certain threshold value or more.

  In the external force follow-up control system according to the third embodiment, the force sensor 124 as the force detector 122 is not required as compared with the external force follow-up control system according to the second embodiment. For this reason, the cost at the time of mounting | wearing with the force sensor 124 separately can be reduced. Other configurations and operational effects are the same as the operational effects of the external force tracking control system according to the second embodiment, and thus the description thereof is omitted.

他の構成および作用効果については、前記実施例３の外力追従制御システムの作用効果と同様であるので説明を省略する。 FIG. 14 is a functional block diagram for estimating an external force in the external force tracking control system according to the first modification of the third embodiment. In the external force estimator 271 of the first modification, when the mass of the telescopic arm unit 30 can be regarded as Mn and the driving force reference value can be regarded as a force response value, the estimated external force is estimated by the following equation (4). It is.

Other configurations and operational effects are the same as the operational effects of the external force tracking control system according to the third embodiment, and thus the description thereof is omitted.

FIG. 15 is a functional block diagram for estimating an external force in the external force tracking control system according to the second modification of the third embodiment. Here, s represents a Laplace operator, and s ² represents a second derivative. The external force estimator 371 of the second modified example uses the following equation (5) as the estimated external force when the mass of the telescopic arm unit 30 is Mn and the viscous frictional force of the joint of the telescopic arm unit 30 is Bn. Is estimated.

  In this way, when obtaining the force used for driving the telescopic arm 30 of the endoscope holder robot 10, an internal dynamic compensation term such as gravity or static friction is added as necessary. The accuracy in estimating the external force can be improved. Other configurations and operational effects are the same as the operational effects of the external force tracking control system according to the third embodiment, and thus the description thereof is omitted.

  As described above, the endoscope holder robot 10 to which the external force tracking control system of this embodiment is applied fixes the endoscope used in laparoscopic surgery or the endoscope accurately by external input by an operator. Can be moved to.

  Further, even when an external force is applied due to contact with the surgeon during the operation, in the external force tracking control system according to the embodiment, the extensible arm portion 30 does not rapidly move when the external force is unloaded. Therefore, in addition to the conventional endoscope holder robot 10, even when an external force is applied to the arm, such as when the camera cable is pulled or contacted with the holder, the original position before the external force is applied is reached. Never come back.

For this reason, in this external force tracking control system, when there is no load, the telescopic arm unit 30 that holds the rigid endoscope 20 even when unloaded operates from the commanded state and is unloaded from the moving state due to the external force. The position can be maintained.
Therefore, since the position of the optical visual tube 22 attached to the telescopic arm unit 30 does not move, it is possible to continuously observe the observed site in the body safely.

  The external force tracking control system according to the present embodiment has been described in detail above. However, the present invention is not limited to these embodiments, and it is needless to say that the external force tracking control system can be appropriately changed without departing from the gist of the present invention. Yes.

For example, in the present embodiment, the position detection unit that uses the optical rotary encoder 14 as the position detection unit that detects the rotation angle of the telescopic arm unit 30 rotated by the pneumatic actuator 12 has been described, but other position encoders or You may make it detect the position of a robot arm using a potentiometer.
Further, although the controller 80 has been described as an input unit, the input unit may be configured using another input device such as a gyro sensor or a panel input.

  In addition, the actuator using the pneumatic actuator 12 has been shown and described. However, the present invention is not limited to this. For example, an actuator such as an electric motor (AC, DC, stepping motor, etc.), a hydraulic motor, and a pneumatic motor. The shape, quantity, and type of the actuator are not particularly limited.

In the embodiment, the power source 16 such as a pneumatic pump is used. However, the present invention is not limited to this, and a power source may be used as a power source. A motor driver may be used.
Furthermore, the description has been made by using the pressure sensor 120 as the force detection unit. However, the present invention is not limited to this. For example, a force sensor, a current detection circuit, or the like may be used. May be used.
Further, as described in the third embodiment, in order to relate the external force and the position correction value, for example, it is more preferable to use virtual impedance considering virtual viscosity and mass.

  In this embodiment, the external force tracking control system is shown and described for the endoscope holder robot 10. However, the present invention is not limited to this, and the external environment such as other medical robots and industrial robots can be used. It may be configured to be used for control of the entire mechanical structure in which physical contact is assumed.

DESCRIPTION OF SYMBOLS 10 Endoscope holder robot 11 Robot main body 12 Pneumatic actuator 14 Optical rotary encoder 16 Power source 18 Power control device 20 Rigid endoscope 22 Optical endoscope 24 Camera head part 30 Telescopic arm part 40 Endoscope holder part 42 Force detection Unit 60 Input / output unit 70 Control unit 72 Position command value generation unit 74 Position controller 76 Driving force controller 78 Self-weight compensation controller 80 Controller (input unit)
DESCRIPTION OF SYMBOLS 110 Endoscope holder robot 116 Compressed air source 118 Servo valve 120 Pressure sensor 122 Force detection part 124 Force sensor 170 Control part 171,271,371 External force estimator 210 Endoscope holder robot

Claims (11)

A robot arm,
An input unit for instructing the position of the robot arm using a position target value;
An actuator for moving the robot arm according to a position target value input from the input unit;
A position detector for detecting the position of the robot arm;
A controller that drives the actuator in accordance with a difference between a position response value indicating the position of the robot arm detected by the position detecting means and the position target value;
The external force follow-up control system, wherein the control unit corrects the position target value in accordance with a state quantity of the robot arm. The external force tracking control system according to claim 1, wherein the control unit corrects the position target value according to an external force applied to the robot arm.
External force tracking control system.   3. The external force tracking according to claim 1, wherein the control unit does not return the position of the robot arm moved by the external force to the position before the movement by the external force even when the external force is unloaded. Control system.   The control unit overwrites the position target value when a movement amount of the robot arm between the position target value and a position moved by applying an external force to the robot arm exceeds a preset threshold value. The external force tracking control system according to claim 1, wherein:   2. The apparatus according to claim 1, further comprising a force sensor that detects an external force applied to the actuator and outputs an external force signal, and the control unit overwrites the position target value according to the external force signal. 5. The external force tracking control system according to one of the items 4.   The control unit overwrites the position target value according to an external force signal when the movement amount of the robot arm exceeds a preset threshold value. The external force tracking control system according to the item.   The external force tracking control system according to claim 5, wherein the control unit uses a force signal obtained from a pressure value detected by a control sensor as an external force signal.   A force sensor for detecting an external force applied to the robot arm is provided separately from the control pressure sensor provided in the actuator, and the control unit is an external force signal based on the external force detected by the force sensor. The position target value is corrected using the external force tracking control system according to claim 1, wherein the position target value is corrected.   9. The external force tracking control system according to claim 1, wherein the control unit includes an external force estimating unit that calculates an external force applied to the robot arm from a driving state of the actuator. 10.   10. The external force tracking control system according to claim 1, wherein the robot arm is a medical robot arm driven by a pneumatic actuator.   The external force follow-up control system according to claim 1, wherein the robot arm is a robot arm driven by an electric motor.

Images