JP2010076012A - Manipulator system and control method thereof - Google Patents

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 relates to a manipulator system that transmits power of an actuator to an action part via a power transmission mechanism and a control method thereof.

  It has been a conventional problem to control the gripping force of the manipulator without providing a sensor in the gripping mechanism. For example, in the case of the medical manipulator described in Patent Document 1, since the tip of the manipulator penetrates into the body cavity, a reduction in diameter and size is required, and sufficient force transmission is required. Moreover, since it is a medical device, it is preferable that washing and sterilization processing is possible and there is no energized part. For this reason, it is desirable not to arrange a gripping force detection sensor or the like at the tip. In addition to medical manipulators, piping work robots, home robots, and the like are desired to be controllable with a small number of sensors in terms of miniaturization, weight reduction, and cost reduction.

In order to solve such a problem, Patent Document 2 describes a technique for recognizing that an object is gripped from a motor current for driving a hand and performing torque control based on the motor current value. Patent Document 3 describes a technique for constructing an observer from a motor current and an angular velocity for driving a gripping mechanism, estimating an output torque for gripping force, and controlling the output torque.
JP 2002-102248 A JP-A-11-333776 JP 2002-178281 A

  However, the motor current value and the actual gripping force have a hysteresis relationship and do not necessarily correspond one-to-one. For this reason, torque control based on the motor current value is not sufficient. Patent Document 3 described above describes that an observer is constructed to consider the influence of friction and inertia, but the estimated torque amount obtained here and the actual gripping force necessarily correspond one-to-one. It is not a thing.

  Further, in each of Patent Documents 2 and 3 described above, the gripping mechanism and the motor that drives it have a one-to-one correspondence, and it is considered that relatively good control performance is exhibited. If the manipulator has a degree of freedom, and the interference mechanism is such that the motor involved in the gripping motion also contributes to the motion of other degrees of freedom, the current value and the actual acting force will be further increased. Correlation is complicated. Therefore, if the gripping force is controlled by the torque control based on the current value, the actual gripping force is not always the desired value, and the force control behavior causes the position (angle) deviation behavior. There is.

  The present invention provides a manipulator system capable of stably and accurately controlling an acting force such as a gripping force and a control method thereof.

According to one aspect of the invention, an actuator;
An action unit that applies a force to an object based on the power of the actuator;
A power transmission mechanism having a flexible member and transmitting the power of the actuator to the action portion;
A control unit for controlling the actuator,
The controller is
An observer signal generating means for generating an observer signal for estimating an acting force start angle of the acting portion without taking into consideration a term that does not depend on a magnitude of at least one of angular velocity and acceleration but depends on a direction; ,
Action force start angle estimation means for estimating an action force start angle by comparing the signal level of the observer signal with a predetermined reference value;
Limit angle detection means for detecting a posture axis limit angle based on a posture axis angle corresponding to a preset action force of the action portion and an action force start angle estimated by the action force start angle estimation means;
There is provided a manipulator system comprising angle control means for controlling the attitude axis angle of the action part to the attitude axis limit angle.

According to one aspect of the present invention, a plurality of actuators;
An action unit having a posture axis that applies a force to an object based on power of the plurality of actuators, and a plurality of operation axes that move the object in different directions;
A power transmission mechanism having a flexible member and transmitting the power of the actuator to the action portion;
A control unit for controlling the actuator,
The plurality of actuators are interference system actuators that affect the driving of the posture axis and the plurality of operation axes,
The controller is
In order to estimate the acting force start angle of the acting portion, which is provided for each of the plurality of actuators and does not take into consideration a term that does not depend on the magnitude of at least one of angular velocity and acceleration but depends on the direction. A plurality of observer signal candidate generating means for generating the respective observer signal candidates;
Observer signal selection means for selecting one of the observer signals output from the plurality of acting force start angle estimation means according to the operation speed of each drive shaft of the plurality of actuators;
Action force start angle estimation means for estimating an action force start angle by comparing the signal level of the observer signal with a predetermined reference value;
A limit for detecting a posture axis limit angle based on a preset posture axis angle corresponding to the acting force of the acting portion and an acting force start angle corresponding to an observer signal selected by the acting force start angle estimating means An angle detection means;
There is provided a manipulator system comprising angle control means for controlling the attitude axis angle of the action part to the attitude axis limit angle.

  ADVANTAGE OF THE INVENTION According to this invention, the manipulator system which can control action force, such as a gripping force, stably accurately, and its control method can be provided.

  Hereinafter, an embodiment of the present invention will be described with reference to the drawings. Below, the example of a medical manipulator is mainly demonstrated.

  FIG. 1 is an external view of a medical manipulator system. The system of FIG. 1 includes a manipulator body 1 that operates in a master-slave manner, and a control device 2 that controls driving of the manipulator body 1. In addition, although omitted in FIG. 1, an operation state indicator for displaying the operation state of the manipulator body 1 may be provided in the system of FIG.

  The manipulator body 1 includes an operation command unit 3 in which an operator (operator) commands an operation, an action unit 4 that applies a predetermined amount of force to an object according to the operation command, an operation command unit 3 and an action unit. 4 includes a connecting portion 5 that integrally connects the driving portion 4 and a driving portion 6 that generates a driving force of the action portion 4.

  The operation command unit 3 has at least one command device (such as a dial) that indicates the amount and direction of action of the action unit 4, and a sensor that reads the command value is attached to each command device. When the operator operates the operation command unit 3, the command information is sequentially transmitted to the control device 2.

  For example, the action unit 4 performs various treatments on the affected part of the human body. The action part 4 has at least one degree of freedom for performing treatment on the affected part, and can change the posture of the tip or change the opening / closing angle of a gripper attached to the tip. The change of the acting force of the action unit 4 is performed by a command from the operation command unit 3.

  The action part 4 and the connecting part 5 of the manipulator body 1 constitute a work part 7, the operation command part 3 and the drive part 6 constitute an operation part 8, and the work part 7 and the operation part 8 are detachable from each other. It has become.

  FIG. 2 is a diagram illustrating a state where the working unit 7 and the operation unit 8 are separated. As shown in FIG. 2, a plurality of output shaft pulleys 11a, 11b, and 11c are provided on one end side of the connecting portion 5 in the working unit 7, and wires 12a, 12b, and 12c are wound around the pulleys, respectively. It is hung. Since the connection part 5 is hollow and the action part 4 side is an opening, the wires 12a, 12b, and 12c pass through the inside of the connection part 5 and extend to the action part 4 side. Note that power may be transmitted to a part of the wires 12a, 12b, and 12c using a rod link or the like.

  Although not shown in FIG. 2, the action unit 4 is provided with pulleys, gears, links, and the like around which the wires 12a, 12b, and 12c are wound. Thereby, power is transmitted to the action part 4 via the output shaft pulleys 11a, 11b, 11c and the wires 12a, 12b, 12c in the working part 7. The output shaft pulleys 11a, 11b, and 11c are connected to power receiving units 13a, 13b, and 13c for receiving power from the drive unit 6, respectively.

  The drive unit 6 includes a plurality of (three in FIG. 2) motors 14a, 14b, and 14c. The powers of the motors 14a, 14b, and 14c are transmitted to power transmission units 15a, 15b, and 15c including couplings through reduction gears (not shown), respectively. Each motor 14a, 14b, 14c is equipped with an encoder (not shown), and the rotation angle of the motors 14a, 14b, 14c can be measured. The measurement signal of each encoder is transmitted to the control device 2.

  Concave portions are formed at the end portions of the power transmission portions 15a, 15b, and 15c in the drive portion 6, and convex portions are formed at the end portions of the power receiving portions 13a, 13b, and 13c in the connecting portion 5. These concave and convex portions are engaged with each other, whereby the power of the power transmission portions 15a, 15b, and 15c is transmitted to the power receiving portions 13a, 13b, and 13c. If the phases of the power transmission parts 15a, 15b, 15c and the power receiving parts 13a, 13b, 13c do not match, the concave part and the convex part are not engaged, so that the operation part 8 and the work part 7 can be Prevents the binding.

  In FIG. 2, electronic devices such as a motor, a switch, and a sensor are arranged on the operation unit 8 side, and mechanical parts are arranged on the working unit 7 side. Thereby, the whole work part 7 can be wash | cleaned and the improvement of a washability and maintainability can be aimed at. That is, in FIG. 2, the working unit 7 and the operation unit 8 are separated in consideration of the degree of contamination and the cleaning method.

  The operation unit 8 includes a bracket 81 connected to the base end side of the coupling unit 5, a columnar operation rod 82 that is rotatably attached to the bracket 81, and an operation that is attached to the operation rod 82. With devices. The operation device of the operation rod 82 has a function of instructing the posture of the action unit 4 and the like.

  As shown in FIG. 2, the operation device has a horizontal dial 83, a vertical dial 84, a trigger 85, and an operation mode changeover switch 86. An example of operating the working unit 7 that performs the gripping operation as shown in FIG. 3 using the operation device shown in FIG. 2 will be described. In FIG. 3, the horizontal dial 83 commands the operation of the yaw axis 29, the vertical dial 84 commands the operation of the roll shaft 46, and the trigger 85 commands the operation of the gripper shaft θg. The operation mode switching switch 86 is a switch for switching operation modes such as start / end of a master / slave operation and an initial posture return operation.

  The horizontal dial 83, the vertical dial 84, and the trigger 85 each incorporate a sensor that detects the amount of movement thereof, and the sensor signal is sent to the control device 2 and processed by an internal arithmetic unit. The sensor incorporated in the horizontal dial 83 and the vertical dial 84 detects the amount of relative rotation with the operation rod 82, and for example, a potentiometer or an encoder can be applied.

  The trigger 85 can translate with respect to the operating rod 82, and commands the gripper angle of the working unit 7 based on the amount of movement. The trigger 85 is provided with a sensor for detecting a translational operation position.

  FIG. 3 is a perspective view showing the structure of the action part 4 of the manipulator body 1, and FIG. 4 is an exploded perspective view showing the detailed structure of the action part 4. As shown in FIGS. 3 and 4, the action section 4 includes a first action member 325 and a second action member 326 that have a first action member 325 and a second action member 326 at the distal end, and the length of the connection member 41. A first rotation shaft member 16 that is orthogonal to the direction along the direction, and a first rotation shaft that is rotatably supported in a direction around the rotation shaft 29 of the first rotation shaft member 16. A main shaft member 320 having a main shaft portion 320b orthogonal to the member 16, a first gear 117 that is rotatably supported in a direction around the rotation shaft 29 of the first rotation shaft member 16, and a first And a second gear 218 that is rotatably supported in a direction around the rotation shaft 29 of the rotation shaft member 16.

  A pulley 320 a is coupled to the main shaft member 320, a pulley 117 a is coupled to the first gear 117, and a pulley 218 a is coupled to the second gear 218. A first wire 50a, a second wire 50b, and a third wire 50c are wound around the pulley 320a, the pulley 117a, and the pulley 218a, respectively.

  Further, the action portion 4 meshes perpendicularly with the first gear 117 and meshes with the third gear 321a supported rotatably around the main shaft portion 320b and the second gear 218, A fourth gear 322a that is rotatably supported in a direction around the main shaft portion 320b, a second rotating shaft member 321b that rotates coaxially with the third gear 321a, and a coaxial with the fourth gear 322a. And a fifth gear 322b that rotates.

  Further, the action unit 4 rotates around the main shaft 46 along with the rotation of the second rotation shaft member 321b orthogonal to the second rotation shaft member 321b rotating around the main shaft 46, and the first rotation. A third rotation shaft member 323a is disposed so as to be rotatable from a twisted position to a position parallel to the rotation shaft 29 of the moving shaft member 16.

  The action unit 4 is supported so as to be rotatable in a direction around the third rotation shaft 323a, and a sixth gear 323 meshing perpendicularly with the fifth gear 322b and the second rotation shaft 321b. Together with the first action member 325 rotating in the direction of the third rotation shaft 323a together with the sixth gear 323, and the third gear 321a and the second rotation shaft 321b. A second action member 326 that rotates. The first action member 325 and the second action member 326 constitute a pair of action members (grippers). The first action member 325 rotates in the direction around the third rotation shaft 323a, thereby constituting a gripping mechanism that can be opened / closed relative to the second action member 326.

  As described above, the action unit 4 of the present embodiment has an opening / closing function that can change the posture with two degrees of freedom about the rotation shaft 29 (the rotation shaft member 16) and the main shaft 46 (the main shaft member 321b) as the rotation center ( It has the 1st and 2nd action members 325 and 326 which constitute a gripping mechanism: gripper).

  The cover 327 is for minimizing the exposure of the gear, and is fixed to the lower portion of the second action member 326 by a cover fixing pin 328. The cover 327 rotates together with the second action member 326 in a direction around the second rotation shaft member 321b (main shaft 46). The fixing nut 329 is fixed to a screw portion at the tip of the main shaft member 320, and 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. . The fixing nut 330 is provided to constrain the axial positions of the third rotating shaft 323a and the third rotating shaft 323a of the sixth gear 323.

  FIG. 5 is a block diagram showing an example of a detailed configuration of the control device 2 and its surrounding control system. The control device 2 controls the motors 14a, 14b, and 14c so that the action unit 4 acts on the object according to the operation command from the operation command unit 3 (first control means), Later, a control (second control) that operates the motors 14a, 14b, and 14c so that the acting portion 4 acts on the object in a reverse direction with a force smaller than a predetermined amount within a range in which the acting force by the acting portion 4 does not decrease. Means).

  More specifically, the control device 2 in FIG. 5 includes a power supply unit 21, a calculation unit 22, a motor drive circuit unit 23, a safety protection device 24, an operation state presenter 25, and a command input unit 26. Have.

  The power supply unit 21 supplies necessary power to the calculation unit 22 and the motor drive circuit unit 23 from the external power supply 27 via a transformer and a rectifier (not shown).

  The operation unit 8 in the manipulator body 1 includes motors 14a, 14b, and 14c that drive the working unit 7, an angle detector 16 that detects rotation angles of the motors 14a, 14b, and 14c, and an attitude angle ( And an angle detector 17 for detecting an operation amount).

  The arithmetic unit 22 includes a CPU (not shown), a storage device, a logic circuit, and an IO interface. Based on the operation amount of the operation command unit 3 detected by the angle detector 17, the arithmetic unit 22 operates the motors 14a, 14b, and 14c. To detect the deviation between the function for generating the control target value, the rotation angle of the motors 14a, 14b, 14c detected by the angle detector 16 and the control target value of the motors 14a, 14b, 14c, and cancel the deviation. And a function of calculating a motor command input.

  In addition, the calculation unit 22 reads a signal from a recognition unit (not shown) provided in the medical manipulator, monitors the signal input of various switches 28, and performs a control calculation according to a predetermined program.

  The motor drive circuit unit 23 is a circuit that supplies power to the motors 14a, 14b, and 14c in accordance with a command input from the calculation unit 22, and performs current command control that outputs a current proportional to the command input to the motors 14a, 14b, and 14c. Do. The motor drive circuit unit 23 measures the current values of the motors 14a, 14b, and 14c.

  The safety protection device 24 has a function of cutting off the power to the manipulator body 1 and immediately stopping its operation in preparation for abnormal situations such as a calculation cycle abnormality of the calculation unit 22, a motor drive circuit abnormality, and an emergency stop command.

  The operation state presenter 25 presents the operation state of the manipulator body 1 as character information or image information in response to an instruction from the calculation unit 22. The command input unit 26 includes operation members such as various switches, and operation information of the command input unit 26 is sent to the calculation unit 22. The command input unit 26 commands the operating state of the manipulator body 1 and the switching of the power source.

  FIG. 6 is a flowchart illustrating an example of a program executed by the calculation unit 22 of FIG. The calculation unit 22 repeatedly executes the flowchart of FIG. 6 according to a predetermined control cycle.

  First, the output of the angle detector 17 of the operation command unit 3 and the output of the angle detector 16 of the motors 14a, 14b, and 14c in the drive unit 6 are read (step S11). Further, operation information such as switches provided in the operation command unit 3 and the control device 2 is recognized by the calculation unit 22 (step S12), and the operation mode of the manipulator body 1 is determined based on the recognition result (step S13). ). Here, the operation mode refers to an automatic mode in which a predetermined operation is automatically executed, or a master-slave operation mode in which the working unit 7 is operated in accordance with an operation of the operation command unit 3. The operation method refers to an operation method such as an acceleration operation, a deceleration operation, a constant speed operation, a stop operation, etc., for safely connecting the operations in switching between the two operation modes.

  Next, according to the determined operation mode, the operation method is determined and target values for the motors 14a, 14b, and 14c are generated (step S14). A control calculation such as PID control is performed from the generated control target value and the detected value of the angle detector 16 previously read to calculate a motor output and output it to the motor drive circuit unit 23 (step S15).

  In step S14, as shown in FIG. 9 described later, the yaw axis target angle θy, the roll axis target angle θr, and the gripper axis target angle θg calculated based on the values obtained from the operation device are set to the first value. A target value is generated by performing an inverse kinematic calculation to convert the motor shaft angle θ1, the second motor shaft angle θ2, and the motor 14c shaft angle θ3.

  Next, various defined conditions are compared with the state read by a sensor or the like to determine the state (step S16). Based on the determined result, an output is made to the lamp and the operation state presenter 25 equipped in the control device 2 (step S17).

Hereinafter, the gripping operation by the gripper of the action unit 4 will be described. For simplification of explanation, assuming that the number of gear teeth at the tip portion of the manipulator body 1 in FIG. 3 are all equal, the output shaft angles θ1, θ2, θ3 of the motors 14a, 14b, 14c and the posture axis angle of the tip portion. The relationship between θy, θr, and θg can be expressed by equation (1).

The 3 × 3 matrix in the above equation (1) is called an inverse kinematic matrix K ⁻¹ .

Considering only the gripper shaft gripping operation, when no other posture axis is operated, since θy = θr = 0, θ3 = −θg. That is, by controlling the motors 14a, 14b and 14c to θd = [0, 0, −θg] ^T , the gripper shaft can be operated by θg. Regarding the gripper shaft angle, when the closed state is 0 degree, the opening direction is the positive direction, and the gripper shaft is operated in the negative direction means that the motor 14c is controlled so as to apply a force in the closing direction. .

  Now, in order to evaluate how the gripping force actually changes, a system has been constructed in which a pressure-sensitive sensor is attached to the gripping surface of the second action member 326 on one side of the gripper and its output is measured. . As described above, the motor drive circuit unit 55 employs a current control method. The angle control of the gripper shaft is constructed based on PD control. That is, the control device 2 in FIG. 5 performs control combining speed PI control, position PD control, and speed feedforward control.

  FIG. 7 is a diagram showing the relationship between the gripping angle controlled by the control device 2 and the actual gripping force. As shown in the figure, the gripping angle and the gripping force have a primary correlation. If position I control is included, the relationship between the gripping angle and the gripping force is not a linear correlation, and thus is not adopted for gripping force adjustment.

  In FIG. 7, region 1 is a region where the gripping angle is small and the amount of elongation of the wire 15c is proportional to the gripping force. In this region 1, the motor 14c is appropriately controlled to a desired target angle. Region 2 is a region where the wire 15c is fully extended and the torque of the motor 14c is proportional to the gripping force, and the motor 14c maintains a controlled state at an angle deviated from a desired target angle.

  Thus, in the region 1, the elongation of the wire 15c and the gripping force are proportional, and in the region 2, the position deviation not including the position I control is proportional to the gripping force. In either region, the grip angle and the grip force have a first-order correlation. Whether the control state is the region 1 or 2 can be confirmed by the magnitude of the control deviation of the motor 14c.

  As described above, the gripper shaft angle defines a closed angle as 0 degrees. For example, when the motor 14c is controlled so as to operate the gripper 4 from an opening angle of 45 degrees to a closing angle of 35 degrees, that is, θg from 45 degrees to -35 degrees without gripping any object, the actual gripper starts from 45 degrees. It operates to 0 degree, and then the gripper angle is geometrically constrained and actually does not move further in the closing direction. However, the motor 14c is controlled in the further closing direction by 35 degrees in terms of the gripper axis. At this time, the grip angle is 35 degrees.

  As shown in FIG. 7, when a system in which the gripping angle and the actual gripping force have a primary correlation can be constructed, the gripping force can be controlled by controlling the gripping angle as a target. It can be said.

  Here, the grip angle will be described. The gripping angle is the maximum amount of movement when operated in the gripping direction. In the above description, when the operation is performed from 45 degrees to -35 degrees, the grip angle is described as 35 degrees, but when the operation is performed from 45 degrees to -45 degrees and then returned to -35 degrees, It is not said that the grip angle is 35 degrees.

  When gripping force is controlled by gripping angle, that is, when gripping force is controlled by angle control, it is necessary to recognize the gripper angle that actually started gripping the object and control it by the gripping angle based on that angle. There is. If the gripper 4 has a force sensor, it can be recognized that the object has been gripped directly, and if there is an angle sensor, it can be estimated that the actual gripper angle has stopped or the object has been gripped due to deviation from the target value. The form is intended to perform gripping force control without a sensor, and control using a sensor is unexpected.

  Therefore, in this embodiment, the grip start angle is estimated using a grip start estimation observer. This grip start estimation observer is provided inside the control device 2 of FIG.

  FIG. 8 is a control system diagram illustrating an example of a processing operation performed by the control device 2 having the grasp start estimation observer. As shown in FIG. 8, the control device 2 includes a grip start estimation observer (observer signal generation means) 51, a grip angle determination unit 52, an inverse kinematic matrix calculation unit 53, a PID control unit 54, and a motor drive unit. 55, a target grip angle setting unit 56, a most closed angle determination unit 57, and an operation amount update unit 58.

  As will be described in detail later, the grip start estimation observer 51 generates a grip start angle estimation signal (gripping start angle estimation signal) fa for estimating the grip start angle.

  Based on the grip start angle estimation signal fa and the operation amount of the operation device, the grip angle determination unit 52 and the reference target value θa (y, r) regarding the yaw axis and the roll axis and the reference target value θa (g) regarding the gripper axis. Is generated.

The inverse kinematic matrix calculation unit 53 calculates the output shaft angle θd of the motors 14a, 14b, and 14c based on the above-described equation (1). The above formula (1) can be simply expressed by the following formula (2). θd = [θ1, θ2, θ3] ^T , θa = [θy, θr, θg] ^T.

θd = K ⁻¹ θa (2)

  The PID control unit 54 in FIG. 8 feedback-controls the deviation between the output value (target value of the motor) θd of the inverse kinematic matrix calculation unit 53 and the measured value θm of the motor. More specifically, the PID control unit 54 controls the voltage Vin supplied to the motor driving unit 55 so that the deviation becomes small.

  The target gripping angle setting unit 56 reads a value such as a volume provided separately from the operation device, for example, and obtains a desired gripping force based on the primary correlation between the gripping angle and the gripping force shown in FIG. For this purpose, a set gripping angle θh is set.

  The most closed angle determination unit 57 calculates the most closed angle θc of the gripper shaft by subtracting the set grip angle θh from the grip start angle θt based on the following equation (3).

    θc = θt−θh (3)

  The operation amount updating unit 58 sends the gripper shaft target value θg to the inverse kinematic computing unit 53 according to the determination result from the most closed angle determining unit 57 to update the gripper shaft target value θg.

  The grip start estimation observer 51 includes a differentiation circuit 61 that detects a motor angular velocity ωm, a differentiation circuit 62 that detects a motor acceleration αm, a Coulomb friction term calculation unit 63, a viscous friction term calculation unit 64, and an inertia term calculation unit 65. An adder 66 for adding the viscous friction term and the inertia term, a first switching unit SW1 for switching whether or not to take the Coulomb friction term into account, and the output of the adder 66 from the motor current and the first switching unit. A subtractor 67 that subtracts the output of SW1, a low-pass filter (LPF) 68 that removes high-frequency noise contained in the output of this subtractor 67, and one non-zero output of the LPF 68 corresponding to each motor. And a second switching unit SW2.

  Of the internal configuration of the grip start estimation observer 51, a portion excluding the second switching unit SW2 is provided for each of the motors 14a, 14b, and 14c. FIG. 9 is a block diagram illustrating an example of a more detailed internal configuration of the grip start estimation observer 51. As shown in the drawing, portions (observer signal candidate generating means) 51a, 51b, 51c from the differentiation circuit 61 to the low-pass filter 68 are provided for each of the motors 14a, 14b, 14c. Any one of them is selected by the second switching unit SW2, and is output as a grip start angle estimation signal fa. The second switching unit SW2 corresponds to an observer signal selection unit.

  In the case of a manipulator system that performs control only in one axial direction, the second switching unit SW2 is unnecessary.

  Hereinafter, the processing operation of the grip start estimation observer 51 will be described. In the case of an electric motor, since the motor current is proportional to the motor output torque, the relationship between the motor output shaft and the gripper shaft is determined based on the mechanism parameters from the motor output shaft to the gripper shaft. The torque relationship is obtained. The gripping force can be calculated from the gripper shaft torque and the distance between the gripper shaft and the gripping point.

  The gripping force can be estimated by using a disturbance observer using information such as motor current and motor angle. Here, the disturbance is a remaining value obtained by converting and subtracting the mechanism parameter from the motor current. When a gripping mechanism is provided, it is assumed that an external force generated by gripping an object is a gripping force.

  The inertia term calculation unit 65 in the grip start estimation observer 51 in FIG. 8 calculates a term obtained by multiplying the motor acceleration α by a coefficient Jn. The viscous friction term calculation unit 64 calculates a term obtained by multiplying the motor speed ω by the viscous friction coefficient fv. The Coulomb friction term calculation unit 63 calculates a term obtained by multiplying the sign function of the motor speed ω by the Coulomb friction coefficient fc. By subtracting these three calculation units 63 to 65 from the motor current i, it is possible to obtain an output converted into a gripper shaft torque, that is, a gripping force.

  For rotation, it is formal to call the rotation speed angular velocity and the rotation acceleration angular acceleration.However, in the mechanical system, it is distinguished here for the explanation in consideration of the combination of rotation and translation. Not. For the same reason, position control and angle control are synonymous.

  Inertia coefficient Jn, viscous friction coefficient fv, and Coulomb friction coefficient fc in FIG. 8 are values determined by design values or experiments. In calculating the inertial term, noise may be amplified if difference processing of discrete angle information is performed twice in order to obtain acceleration information. Therefore, the inertia term may be obtained by using an equivalent block diagram as shown in FIG.

  FIG. 10 is a diagram showing the characteristics of the grip start estimation observer 51 of FIG. FIG. 10A shows the gripper shaft angle θg, yaw axis angle θy, roll when the gripper 4 opening operation → the yaw axis operation → the roll axis operation → the gripper 4 closing operation → the gripper 4 opening operation is continuously performed. It shows how the shaft angle θr and the shaft angle θ3 of the motor 14c change.

  FIG. 10B is a diagram illustrating a change in the observer signal fe generated when the first and second switching units SW1 and SW2 are not provided in the grip start estimation observer 51 of FIG. FIG. 10C shows the output of the gripping force sensor.

  The observer signal fe is generated by subtracting the three terms of the inertia coefficient Jn, the viscous friction coefficient fv, and the Coulomb friction coefficient fc from the motor current. The gripping force estimated using the observer signal fe is actually Compared with the measured gripping force, as shown in FIG. 10 (b), the gripping force (current conversion value) estimated using the observer signal fe has a vibration characteristic, and has a spike shape near the operating speed change point. The signal also appears. This is due to the non-linearity of the Coulomb friction generated in the transmission mechanism system and the minute change in the friction coefficient depending on the operating angle and position, and the difference from the measured gripping force becomes large.

  As described above, the observer signal fe generated in consideration of Coulomb friction or the like distinguishes a noise component from a small gripping force when the working unit 7 has a fine structure and a gripping force is considerably large. Is difficult, and the grip start point cannot be accurately detected. For example, taking into consideration the magnitude of noise, it is possible to set the determination default value to the plus side (for example, f0) and estimate the point at which the estimated gripping force is greater than the determination default value as the start of gripping. In this case, a small gripping force cannot be recognized. A noise component can be removed using a filter having a lower cut-off frequency. However, a phase delay occurs in the generated signal, and a time delay occurs in recognizing the grip start time T2. As a result, a large deviation occurs between the actual grip start angle and the estimated angle.

  Under such circumstances, in the present embodiment, the first switching unit SW1 is provided in the grip start estimation observer 51 so that it can be distinguished from the noise component without causing a delay in recognition of the grip start. The first switching unit SW1 is turned off when the gripper shaft is operated, generates the grip start estimation signal fa without considering the Coulomb friction term, and is turned on except when the gripper shaft is operated to take the Coulomb friction term into consideration. To generate the same signal fa. Not taking the Coulomb friction term into account is equivalent to not taking into account a term that does not depend on the magnitude of at least one of angular velocity and acceleration but depends on the direction. Instead of cutting off the output after calculating the Coulomb friction term as in the first switching unit SW1, the Coulomb friction coefficient fc is set to zero during the gripper shaft operation so that the calculation result of the Coulomb friction term becomes zero. May be.

  In order for the grasping start estimation observer 51 of FIG. 8 to operate normally, the grasping start timing must be accurately recognized. Since the recognition of the start of gripping occurs when the gripper 4 is closed, that is, when the gripper shaft speed ωg is negative, the first switching unit SW1 switches to turn off when the gripper shaft speed ωg becomes negative. Be controlled. The gripper shaft speed is obtained by differentiating the reference target value θa (g) that is the output of the grip angle determination unit 52 in FIG.

  FIG. 10D is a waveform diagram of the grip start estimation signal fa obtained with the first switching unit SW1 turned off at the start of gripping. As can be seen from the waveform of the grip start estimation signal fa in FIG. 10D, the Coulomb friction term is not taken into consideration, and therefore the signal level of the signal fa after the time T0 when the gripper 4 starts to close is shown in FIG. It is larger than the signal level of the signal fe in (b). Then, it is determined that the grip start angle θt has been reached at time T1 when the signal fa exceeds a predetermined threshold value f1. This time T1 is before time T2 when the grip start is recognized in FIG. 10B, and it can be seen that the grip start can be estimated quickly and accurately.

  When the grip start angle θt is detected, the most closed angle θc of the gripper shaft can be obtained from the above-described equation (3).

  As described above, in this embodiment, a desired gripping force is first set, the gripper shaft is closed by angle (position) control, and the gripping start angle is recognized by the output of the gripping force start estimation observer. Is set to the most closed angle of the gripper shaft, and then the angle (position) of the gripper shaft angle is controlled to the most closed angle of the gripper shaft.

  The gripping force setting or gripping angle setting may be set using a volume attached to the control device or the operation command unit 3, or a predetermined setting value may be set according to the type of the work unit 7. It may be given. If the most closed angle of the gripper shaft is adjusted during gripping, the gripping force can be finely adjusted while gripping.

  In the grasping start estimation observer 51 of FIG. 8, the inertia term and the viscosity term are taken into consideration, but if the manipulator has a fine structure and the influence of inertia can be ignored, the inertia coefficient Jn may be set to zero. The viscous friction term may be ignored if necessary. Thus, the specific configuration of the grip start estimation observer 51 can be changed as appropriate.

  The most closed angle determination unit 57 in FIG. 8 sets the most closed angle of the gripper shaft in accordance with, for example, the processing procedure shown in the flowchart in FIG. The flowchart of FIG. 11 is performed, for example, in step S14 of FIG. 6, and corresponds to the action flag determining means.

  First, the grip start estimation signal fa output from the grip start estimation observer 51 of FIG. 8 is acquired (step S21). Next, the state of the grip flag (action flag information) is confirmed (step S22). Here, the grip flag is flag information indicating whether or not the gripper is gripping an object, and is turned on when gripped and turned off when not gripped.

  If it is determined that the grip flag is OFF, it is determined whether the target angle θg of the gripper shaft is smaller than the maximum angle θp of the gripper shaft (step S23). If θg <θp, it is determined whether or not the gripper speed ωg is negative (step S24).

  If ωg is negative, it can be determined that the gripper shaft is moving in the closing direction. Therefore, it is next determined whether or not the grip start estimation signal fa is equal to or greater than a predetermined threshold value f1 (step S25). If fa ≧ f1, it is determined that gripping has started as in the case of time t1 in FIG. 10D, and the gripping flag is turned on (step S26). Then, the most closed angle θg of the gripper shaft at this time is set to the grip start angle θt (step S27). Next, the gripper most closed angle θc is calculated by subtracting the set gripping angle θh from the gripping start angle θt according to the above-described equation (3) (step S28). The processing in steps S25 to S27 described above corresponds to the acting force start angle estimation means, and the processing in step S28 corresponds to the limit angle detection means.

  When the determination result of any of steps S23 to S25 described above is NO, it is not determined that gripping has started.

  On the other hand, if it is determined in step S22 that the grip flag is on, it is determined whether or not the target angle θg of the gripper shaft is equal to the maximum angle θp of the gripper shaft (step S29). If not equal, it is determined whether or not the target angle θg is larger than the grip start angle θt (step S30).

  If it is determined in step S29 that θg is equal to θp, or if it is determined in step S30 that θg> θt, it is determined that the gripping operation has ended, and the gripping flag is turned off (step S31). Next, the minimum angle θn of the gripper shaft is set as the grip start angle θt (step S323). Here, the reason why the grip start angle θt is set to the minimum angle of the gripper shaft is to reset the grip start angle. If the grip start angle is not reset even after the gripping operation is completed, there is a possibility that the gripper does not operate until a gripper that is thinner than the previously gripped object is gripped. At this time, the grip start angle may be set to zero, but since an initial error is considered as described later, it is set to the minimum angle in the operable range.

  If it is determined in step S29 that θg is not equal to θp, or if it is determined in step S30 that θg ≦ θt, it is determined that the gripping operation is still being performed, and the gripping flag is kept on. In this case, the angle control is performed until the target angle θg of the gripper shaft reaches the most closed angle θt. This angle control is performed by the control device 2 and corresponds to angle control means.

The drive unit 6 that drives the working unit 7 shown in FIG. 3 has an interference mechanism. The relationship between the output shaft angle θd = [θ1, θ2, θ3] ^{T of the} motors 14a, 14b, and 14c and the attitude axis θa = [θy, θr, θg] ^T of the tip of the working unit 7 has been described above (1). It is represented by the formula and the formula (2). In these equations, it is assumed that the number of teeth of the gears at the tip portion of the manipulator are all equal.

Considering only the gripper shaft gripping operation, θ3 = −θg because θy = θr = 0 when no other posture axis is operated. That is, by controlling the motor to θd = [0, 0, −θg] ^T , the gripper shaft can be operated by θg.

  Regarding the gripper shaft angle, the closed state is 0 degree and the opening direction is a positive direction. Therefore, operating the gripper shaft in the minus direction means controlling the motor so as to apply a force in the closing direction.

  When operating the gripper shaft, the grip start estimation signal fa can be generated only by the current value of the motor 14c. The waveform of the current value of the motor 14c is as shown in FIG. The operation of the gripper shaft is determined only by the current value of the motor 14c. A manipulator having a gripper having only one degree of freedom or an actuator that drives the gripper shaft even if it has two or more degrees of freedom is another posture axis. The same applies to a non-interfering structure that does not interfere with.

  However, when the gripper shaft is operated simultaneously with other posture axes such as a manipulator provided with the working unit 7 of FIG. 3, the gripping force is not always determined only by the current value of the motor 14c.

  Therefore, with the manipulator provided with the interference mechanism in mind, the relationship between the gripping force and the motor torque will be obtained in consideration of torque interference.

Due to the principle of virtual work, the relationship between the torques τ1, τ2, and τ3 of the motors 14a, 14b, and 14c and the torques τy, τr, and τg about the yaw axis, the roll axis, and the gripper axis that are the attitude axes of the working unit 7 It is expressed by the following equation (4).

The state in which the gripping force is output is a state in which the torque τg is output. If the output of the other posture axes is set to zero, the torque of each motor 14a, 14b, 14c can be calculated from the above equation (4). τ1, τ2, and τ3 are obtained by the following equation (5).

  As can be seen from the equation (5), the influence of the gripping torque also appears on the motors 14a and 14b other than the motor 14c, and it can be understood that the gripping start can be estimated from the observer signal for each motor shaft.

  As a simple example, the case where the operations of the gripper axis and the yaw axis of the manipulator are performed in synchronization will be described. When the yaw axis operating angle and the gripper axis operating angle operate synchronously by the same amount, the operating speeds of the motors 14a and 14b are equal, and the speed of the motor 14c is zero, that is, the gripping operation is performed in a stopped state.

  FIG. 12 is an operation waveform diagram in the case where the gripper shaft and the yaw axis are operated synchronously after the gripper shaft is independently operated. 12A is a waveform diagram of the motor angle, FIG. 12B is a waveform diagram of the motor current, FIG. 12C is a waveform diagram of the grip start estimation signal fa, and FIG. 12D is a waveform diagram of the grip force. In these figures, the horizontal axis is time.

  As shown in FIG. 12A, when the gripper shaft is operating alone (time t0 to t2), only the motor 14c is operating, and the gripper shaft and the yaw axis are operating in synchronization. When (time t2 to t4), the motor 14c is stopped and the motor 14b is operating even during the gripping operation.

  Thus, at time t2 to t4, the motor 14c is stopped while the gripper is gripping, so the current value of the motor 14c is substantially constant as shown in FIG. 12B. Since there is no change in angle, there is no change in the observer signal. However, actually, as shown in FIG. 12D, since a gripping force is generated, a reaction force should be applied to the shaft of the motor 14c as described above.

  Originally, even when the motor 14c is stopped, if the reaction force is applied, the current value changes. However, a hysteresis occurs between the gripping force and the motor torque due to the friction of the power transmission mechanism. The gripping force is not transmitted. In such a case, in addition to the stopped motor 14c, the gripping start estimation signal fa of the motor that is actually driven among the shafts to which the reaction force is applied from the relationship of torque interference, here, the gripping start estimation signal fa of the motor 14b is used. What is necessary is just to estimate a point.

  In FIG. 12C, the gripping start estimation signal fa corresponding to the motor 14c is used during the time t0 to t2, and the gripping start estimation signal fa corresponding to the motor 14b is used from time t2 to t4. An example of estimating the start is shown.

  During the gripping operation, the yaw axis, roll axis, and gripper axis may operate synchronously, and the speeds of the motor 14b and the motor 14c may be zero. In this case, the grip start estimation signal fa of the motor 14a The grip start point may be estimated based on this.

  Since it is impossible to perform a gripping operation in a state where the speeds of all the motors 14a, 14b, and 14c become zero at the same time, it is possible to estimate the grip start with the grip start estimation signal fa of any one of the motors.

  In this way, the motor shaft to which the gripping force is applied is identified from the relationship of torque interference, and among the motors corresponding to the identified motor shaft, the grip start estimation signal fa is switched according to the motor speed. Regardless of the movement, the grip start point can be accurately estimated. More specifically, as shown in FIG. 8, the grip start estimation signal fa corresponding to any one of the motors is selected by switching the second switching unit SW2 based on the motor shaft speed ωd.

  When it is not a special case where the motor shaft speed becomes zero, the shaft having the largest reaction force of the gripping torque is the shaft of the motor 14a from the above-described equation (5). However, a reduction gear is actually interposed between the motor and the output shaft, and when this reduction ratio is more than twice as large as that of the motors 14b and 14c (the output shaft is reduced more than twice), the motor 14a. The grip start estimation signal fa calculated from the motor current is smaller than the value calculated from the other axes and is difficult to handle in the determination.

  On the contrary, the above equation (5) shows that the grip start estimation signal fa calculated from the motor current of the motor 14a should be positively used if the reduction ratio is the same for all three axes.

  Thus, based on the value obtained by converting all the reduction ratios from the shaft (motor input shaft) corresponding to the measured motor current to the gripper 4 and the magnitude of the reaction force of the gripping torque, the second The switching unit SW2 may be switched.

  The conditions for switching the first and second switching units SW1 and SW2 in FIG. 8 are summarized as follows.

  The first switching unit SW1 is turned off when the gripper shaft target speed ωg is negative. In this case, the grip start estimation observer 51 generates the grip start estimation signal fa without taking into account the Coulomb friction term.

  The first switching unit SW1 is turned off when the gripper shaft target speed ωg is positive or zero. In this case, the grip start estimation observer 51 generates the grip start estimation signal fa taking into account the Coulomb friction term.

  The second switching unit SW2 typically selects an observer signal corresponding to the motor 14c that drives the gripper shaft as the grip start estimation signal fa. However, considering that there is torque interference, the second switching unit SW2 An observer signal corresponding to the motor 14b can be selected as the grip start estimation signal fa. More specifically, during the gripping operation (when the gripper shaft speed ωg is negative), the following three switching operations are performed.

  When the speed of the motor 14c is positive, the motor 14c is selected. When the speed of the motor 14c is not positive and the speed of the motor 14b is positive, the motor 14b is selected. When the speeds of the motors 14b and 14c are not positive, the motor 14a is selected.

  As described above, in the first embodiment, the grip start estimation observer 51 is provided, the grip start angle is estimated, the most closed angle of the gripper 4 is set based on the estimated grip start angle, and the close angle is set. Control the gripper shaft angle until it reaches. This makes it possible to control the gripping force of the gripper 4 with higher accuracy than when the gripping force is controlled by the torque control of the motor.

  In addition, since the grip start estimation observer 51 of the present embodiment estimates the grip start angle without taking the Coulomb friction term into consideration, the grip start angle can be accurately estimated.

  Further, in the case of a manipulator having a torque interference mechanism, gripping start estimation observers 51a, 51b and 51c are provided for each motor, and the output of any one gripping start estimation observer is finally determined according to the motor speed of each motor. Therefore, even when the torque interference mechanism is used, the grip start angle can be estimated quickly and accurately.

(Second Embodiment)
In the second embodiment, a gripping operation by a master-slave operation like a medical manipulator is performed using the grip start estimation observer 51 described above.

  13 (a) and 13 (b) are diagrams showing an example of the internal structure of the operating rod 82, FIG. 13 (a) is a return state of the trigger 85, and FIG. 13 (b) is a state where the trigger 85 is pulled. Is shown. As shown in these drawings, the operating rod 82 is formed on a lock member 88 that holds a lock rod 87 attached to an end of the trigger 85, a sensor 89 that detects the pulling amount of the trigger 85, and the trigger 85. The guide rod 91 is inserted / removed through the guide hole 90 and the spring 92 is mounted around the guide rod 91.

  The sensor 89 detects the translational operation position of the trigger 85, and can be specifically configured by a slide potentiometer, a linear encoder, or the like. For example, when the sensor 89 is configured by a slide potentiometer, the concave portion of the projection member 93 attached to the end of the trigger 85 is engaged with the convex portion of the knob 94 of the slide potentiometer. These concave portions and convex portions move together in accordance with the translation operation of the trigger 85. The sensor 89 detects the pulling amount of the trigger 85 based on the position of the convex portion.

  The state where the trigger 85 is pulled most is the state where the gripper of the action unit 4 is most closed (FIG. 13A), that is, the state where the opening / closing angle is the maximum closing angle, and the state where the trigger 85 is returned most is the most opened state. The gripper opening / closing target value can be obtained by converting the value detected by the sensor 89 so that the gripper is in the most open state.

  Maintaining the pulling amount of the trigger 85 for a long time places a heavy burden on the operator and may affect detailed operation instructions on the action unit 4. For this reason, a lock mechanism may be provided on the operating rod 82 so that the pulling amount is maintained even when the trigger 85 is released, and the locking mechanism is also provided in the operating rod 82 of the present embodiment. .

  When the trigger 85 is pulled to the maximum extent, the lock rod 87 attached to the end of the trigger 85 is accommodated in the lock member 88 of the operation rod 82. Thereafter, even when the operator releases his hand from the trigger 85, the lock member 88 fixes the lock rod 87, and the pulling amount of the trigger 85 is maintained constant. The lock member 88 has a play range in which the lock rod 87 slightly translates according to the pulling amount of the trigger 85. Thus, the lock mechanism is realized by the lock rod 87 and the lock member 88.

  When the trigger 85 is further pulled while the trigger 85 is locked, the lock member 88 opens the lock rod 87. The trigger 85 is provided with a guide hole 90 for inserting and removing the guide rod 91 of the operating rod 82. In a state where the trigger 85 is locked, the guide rod 91 is inserted into the guide hole 90, and the spring 92 is contracted by the end of the trigger 85. When the trigger 85 is further pulled in this locked state and the lock member 88 opens the lock rod 87, the trigger 85 is pushed out by the restoring force of the spring 92 and returns to the original position. At this time, the guide rod 91 is extracted from the guide hole 90 as shown in FIG.

  As shown in FIGS. 13 (a) and 13 (b), considering the case of adjusting the gripping force by operating the gripper shaft by the master / slave operation, the operator performs the operation while observing the gripping state. It is desirable that the gripper 4 is closed and the gripping force is increased according to the extent to which the trigger 85 is pulled so that the set maximum gripping force can be obtained when the trigger 85 is pulled to the maximum. That is, the gripping force adjustment function by the master / slave operation can be restated as a function for setting the maximum gripping force.

  Here, it is assumed that the voltage range detected by the sensor 89 is V0 when the trigger is pulled to the maximum, and V2 when the trigger is at its maximum. It is assumed that the gripper shaft angle is set to θp (> 0) when the trigger 85 is at the maximum.

FIG. 14 is a diagram showing the relationship between the output voltage of the sensor 89 and the gripper axis target value. The sensor output voltage Vt is a sensor output voltage corresponding to the grip start angle detected and updated by the grip start estimation observer 51, and is expressed by the following equation (6).

  Here, the sensor voltage V1 is a predetermined value that determines the relationship between the sensor output and the gripper shaft target value when the gripper 4 is not gripping an object, and is set between V0 and V2. More specifically, V1 is a voltage value that sets the gripper axis target value to 0 degree when the gripper 4 is closed without gripping an object.

  In order to improve the sensitivity with which the gripping force of the gripper 4 changes in accordance with the pulling amount of the trigger 85, the interval between V0 and V1 should be widened. Further, if it is assumed that a hard and small object is grasped with a set maximum force, it is preferable to keep the distance between V0 and V1 narrow.

When the object is not gripped, that is, when the sensor output voltage Vg is equal to or higher than Vt, the relationship between the gripper shaft target angle θg1 and the voltage Vg is expressed by the following equation (7).

When the object is gripped, that is, when Vg is Vt or less, the relationship between the gripper shaft target angle θg2 and the voltage Vg can be expressed by the following equation (8).

  When the grip start angle is estimated based on the grip start estimation signal fa generated by the grip start estimation observer 51 of FIG. 8 described above, the voltage of the sensor 89 at the estimated grip start angle is detected and detected. Using the sensor voltage as a base point, the relationship between the sensor output voltage corresponding to the pulling amount of the trigger 85 and the gripper shaft angle is obtained, and the target angle value of the gripper shaft is set. Accordingly, a gripping force adjusting function that can be obtained when the set maximum gripping force pulls the trigger 85 to the maximum can be realized by position (angle) control of the gripper shaft.

  As described above, in the second embodiment, in the master-slave operation manipulator capable of detecting the pulling amount of the trigger 85 with the sensor voltage of the sensor 89, the sensor output voltage and the gripper are based on the point when the grip start angle is estimated. Since the gripper shaft angle is controlled according to the pulling amount of the trigger 85 from the relationship of the shaft angle, the gripping force can be controlled with high accuracy.

(Third embodiment)
In the first embodiment, it has been described that the present invention can also be applied to a manipulator having a torque interference mechanism. In a third embodiment described below, torque interference correction in this type of torque interference mechanism will be described.

  As described above, when a gripping operation is performed, a reaction force is applied to each motor shaft due to torque interference. More specifically, when a torque acts around the posture axis of the action unit 4, a torque acting as a reaction force acts on the motor output shaft that drives each posture axis. The torque of the motor output shaft is expressed by the above-described equation (4).

  In the case of a manipulator, wires (flexible members) 12a, 12b, and 12c that constitute a part of the power transmission mechanism exist between each motor output shaft and the action portion 4 at the tip. Since these wires extend in proportion to the force, even if each motor output shaft is angle-controlled so as to satisfy the above-mentioned equation (2), the actual dependent pulley at the front end is displaced from the motor shaft side. Change posture.

Therefore, the amount of deviation is obtained. Now, assuming that the rigidity of the wire is w, the following equation (9) is established.

  Since the gripping torque τg and the gripper shaft angle are proportional, the relationship of the following equation (10) is established when the proportional constant α is used.

  τg = αΔθg (10)

Substituting the formulas (9) and (10) for both sides of the above formula (5) representing the relationship between the motor output shaft and the attitude axis, the following formula (11) is obtained.

At this time, by using Δθd = KΔθa, the posture axis deviation angle Δθd is expressed by the following equation (12).

Here, in equation (12), if the equation of the term of Δθg holds, Δθg = 6 (α / w) Δθg and α / w = 1/6. As a result, the relationship between the gripper shaft deviation Δθg (deviation between the gripper shaft target value and the actual gripper shaft angle) and the posture deviations Δθy, Δθr of the yaw axis and roll axis are expressed by the following equations (13) and (14): It is expressed by an expression.

The posture axis deviation angle Δθd used in the calculations so far is a value obtained by converting the motor output shaft angle by the above equation (2). This is a case where there is no torque loss. The posture of the part is different from this. The actual tip portion posture deviation is expressed by the following equations (15) and (16) when the torque efficiency η from the motor output shaft to the tip portion posture axis is used.

  When the gripping start point is recognized, it may be controlled to perform posture axis correction by the deviation angle obtained as described above according to the gripper shaft angle Δθg after the gripping start. Here, the torque efficiency η can be expressed as a function of the number of axes of the power transmission mechanism from the motor output shaft to the gripper shaft. For example, in the manipulator shown in FIGS. 2 to 4, since the gripper shaft operation is performed through the output shaft pulley 39c, the rotating shaft 16, the rotating shaft of the gear 322, and the four gripper shafts 323a, in a normal lubrication state. If there is, the efficiency per axis is 0.9, and the fourth power of 0.9 is the torque efficiency η.

  For example, in the case of a medical manipulator immediately after being washed, the efficiency per axis may be set to 0.8 if the lubrication is not performed after washing. Of course, it may be set appropriately because it changes depending on the material of the shaft.

  As described above, since the relationship of α / w = 1/6 is obtained from the equation (12), the deviation angle of each motor output shaft obtained by substituting this relationship into the equation (11) is used as the correction angle. The correction angle must not be subtracted from the motor output shaft target value after the inverse kinematic calculation. If this is done, a new posture angle will be defined, and the target value of the gripper shaft will also change, making it impossible to output the desired gripping force. Therefore, as shown in the above formulas (13) and (14), a correction angle based on the gripper shaft angle is obtained in terms of posture axis, and after subtracting it from each target posture angle, an inverse kinematic calculation is performed. Thus, it is desirable to calculate a target value for each motor shaft.

  Here, the default value for estimating the gripping force start point for adjusting the gripping force and the default value for estimating the gripping force start point for performing posture correction need not be the same. In the case of gripping force adjustment, the maximum gripping force is determined from the gripping start angle and the gripping angle, but the magnitude of the maximum gripping force is not absolutely related to the gripping angle. For example, in FIG. 15, a larger value f2 may be set as the prescribed value instead of the prescribed value f1 for determining the gripping force start point. In this case, the relationship between the gripping force and the gripping angle from the specified value f2 changes only in the primary relationship between the gripping force and the gripping angle only by offsetting the gripping force and the value from the specified value f1. Therefore, the gripping force can be adjusted even if the specified value changes.

  Thus, even if the specified value is changed, only the reference or scale of the magnitude of the gripping force that is desired to be set is changed. The gripping force below the specified value cannot be adjusted, and the higher the specified value, the less affected by the noise contained in the observer signal. Therefore, the level of the specified value is set by a trade-off between the two.

  On the other hand, when posture correction is performed, since a change in gripping force leads to a posture change, it is necessary to estimate the grip start point as accurately as possible. For this reason, as shown in FIG. 15, it is desirable that the prescribed value for posture correction is set to be f3 or more and f1 or less where a change appears at the start of gripping. This f3 corresponds to the motion axis correction reference value. The process of estimating the grip start point for posture correction is performed by the control device 2, and this process corresponds to the correction start angle estimating means. The control device 2 performs a process of calculating the deviation angle based on the above-described equations (15) and (16) after estimating the grip start point, and this process corresponds to the deviation angle calculation means. Thereafter, the control device 2 performs posture correction of each posture axis based on the deviation angle, and this processing corresponds to the motion axis correction means.

  Thus, in the third embodiment, in the case of a manipulator equipped with a torque interference mechanism, the control of the gripping operation affects other posture axes, so the posture is determined according to the gripper shaft angle after the gripping is started. Perform axis correction. Thereby, even if the gripping force is changed, posture correction can be performed with high accuracy.

(Fourth embodiment)
In the fourth embodiment described below, the relative flexibility of an object is measured using the grip start estimation signal fa.

  One of the reasons that control of the gripping force with the gripping angle has not been performed conventionally is that the actual gripping force becomes smaller than the originally planned gripping force in the angle control due to the deformation of the object. Because there are things. Also in the first and second embodiments, when gripping a very soft object and when gripping a hard object that does not deform, even if the same gripping angle is set, the same gripping force is not obtained.

  However, since the grip angle for determining the grip force can be arbitrarily adjusted, it is possible to take a practical measure by adjusting the setting according to the object. Since an object having an unknown size is not gripped with a gripping force larger than the set size, it can be considered equivalent to setting the maximum gripping force. A system that does not generate excessive gripping force is desirable from the viewpoint of safety. It can prevent the deterioration by suppressing the heat generation of the motor, and can construct an energy-saving and efficient system. For example, in the interference mechanism, if the motor that contributes to the gripping operation also affects other posture axis operations, but assigns an output that is not necessary for the gripping operation, it allocates sufficient output to the other posture axis operations. Can do.

  The present embodiment is characterized in that an error in gripping force caused by the hardness of the object can be corrected and the hardness of the object can be estimated.

  FIG. 16 is a graph showing the relationship between the output shaft angle θ3 of the motor 14c and the grip start estimation signal fa when the manipulator grips an object. As described above, since θ3 = −θg, the gripping force increases as θ3 increases.

  The grip angle when the grip start estimation signal fa matches the predetermined value f1 shown in FIG. 15 is set to −θt by inverting the sign in FIG. It is assumed that a function for adjusting the gripping force from the relationship between the gripping angle and the gripping force related using the object A is initially set. With this setting, when the object B softer than the object A is gripped, even if the gripper 4 is closed to the calculated most closed angle θc (−θc for the output shaft of the motor 14c), the intended gripping force is not reached. This is because the hardness coefficient ka of the object A is different from the hardness coefficient kb of the object B, and the gripping force is different even when the object is closed by the same angle θh.

  Therefore, the hardness coefficient of the object is during the gripping operation, and the gripper shaft angle θg is at an angle θu between the grip start angle θt at which the grip start is recognized and the most closed angle θc calculated at that time. The relationship between the motor shaft angle related to the gripper shaft operation, here the output shaft angle of the motor 14c, and the output of the grip start estimation observer 51 is calculated by the least square method. The calculated value is an index kb representing the hardness of the object b.

  If the hardness coefficient ka of the object a is calculated in advance by the same method, the gripping angle θb when gripping the object b with the gripping force equivalent to the gripping force set by the gripping angle θa for gripping the object a Is represented by the following equation (17).

kaθa = kbθb (17)
In this case, since kb <ka, θb> θa. Therefore, the difference Δθc between the gripping angle θb and the gripping angle θb when gripping the object b with the gripping force equivalent to the gripping force when gripping the object a with the gripping angle θa can be expressed by the following equation (18).

  It is only necessary to increase the most closed angle by the amount calculated by the above equation (18).

  FIG. 17 is a flowchart showing an example of the processing procedure of the hardness correction processing. This process is performed in the process of step S13 in the flowchart of FIG. 6, and more specifically, is performed subsequent to the process of FIG.

  First, the grip start angle θt and the gripper shaft most closed angle θc obtained by the processing of FIG. 11 are acquired. Then, the angle θu is obtained from the grip start angle θt and the gripper shaft most closed angle θc. (Step S41). Next, it is determined whether or not the gripper shaft angle θg is within a range of θt> θg> θu (step S42). 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, and the grip start estimation signal fa corresponding to the motor 14c are obtained. Obtain and save (step S43). The reason for saving θ3 and fa is to use it later when calculating the hardness coefficient kb. The process in step S43 corresponds to a recording unit.

  If the determination in step S42 is NO, the state of a correction flag indicating whether correction due to the difference in object has been performed is determined (step S44). This correction flag is reset in the initial state, indicating that it is before correction.

  When it is determined that the correction flag is in the reset state, the hardness coefficient is calculated by the least square method using the motor output shaft angle θ3 and the grip start estimation signal fa stored in step S43 (step S45), and the above-described (18 ) Updates the gripper shaft most closed angle (step S46) and sets a correction flag (step S47). The processing in step S45 corresponds to hardness estimation means, and the processing in step S46 corresponds to posture axis limit angle updating means.

  Thereafter, until the gripper shaft target value θg reaches the updated most closed angle, the correction flag is controlled at a new most closed angle in order to maintain the set state. Note that the gripping state is released and the gripping flag is reset, and the correction flag is also reset.

  In step S46 of FIG. 17, the hardness coefficient of the object is calculated. If this coefficient is presented (displayed), the operator can be informed of the hardness of the object. In reality, however, the dynamic model of the object has a damper effect that depends on the speed, and it is difficult to distinguish between the force acting on the entire manipulator and the gripping force. It is difficult to calculate and present the hardness. However, if a similar gripping operation is realized by an automatic operation or the like, it is possible to recognize a difference in relative hardness from the result of gripping each part of the object. With this function, if the hardness of a certain part is different from the surroundings, that part can be estimated and raised as a candidate.

  Thus, in the fourth embodiment, when the gripping object is hard and soft, even if the same gripping angle is set, the same gripping force is not obtained. Therefore, after the gripping start angle is estimated, Since the hardness coefficient is recalculated and the most closed angle of the gripper is updated, the gripping force can be adjusted according to the hardness of the object. Thereby, even if there is a difference in hardness of the object, the gripping force can be adjusted with high accuracy by angle control.

(Fifth embodiment)
In the fifth embodiment, the initial posture error is estimated and the initial posture is corrected.

  When the above-described grip start estimation observer 51 is not gripping an object, for example, in the case of a manipulator, the angle at which the first and second action members 325 and 326 constituting the gripper contact is estimated. For example, if the gripper is closed, the initial posture is estimated. Accordingly, even if the initial posture of the gripper shaft varies due to manufacturing and assembly errors of the manipulator, the initial posture error can be estimated and corrected based on the grip start estimation signal fa during the trial operation of the gripper shaft. .

  Of course, according to the method of the present embodiment, since the grip start point can be estimated regardless of the size of the object, even if there is an error in the initial posture, the grip force can be adjusted without being affected by the error. Needless to say.

  The correction of the initial posture is performed by the following procedure, for example. An initial posture error is estimated and recorded by causing the gripper to perform a strong gripping operation such as a preparatory operation before using the manipulator. The minimum value of the grip start point estimated during operation is the point where the first and second action members 325 and 326 of the gripper are in contact with each other without gripping the object. If it is confirmed that the minimum value is lowered during operation or when operation is resumed, it can be estimated that an abnormality has occurred in the power transmission mechanism, and an abnormality detection means can be simply realized. For example, it is conceivable that the recognized gripping start point is lowered due to elongation or fraying of the wire in the power transmission mechanism, loosening between the wire and the pulley, shaft misalignment, or the like.

  Thus, by recording the minimum value of the grip start point and observing the change, abnormality detection of the manipulator can be performed. Similarly, in a gripping operation that does not grip an object in a preparatory operation or the like, a hardness coefficient is calculated and recorded, and this value is compared in a regular equivalent operation, thereby changing the hardness of the wire, that is, the wire Can be detected. The detected result may be notified to the user with a display or the like.

  As described above, the case where the gripper grips an object has been described as the mechanism of the action unit 4. Needless to say, the present invention can be applied to the action unit 4 having a mechanism for pressing in a direction and a mechanism for cutting an object. For example, when controlling the action part 4 having a mechanism for peeling the object, the direction of the gripper shaft target value in FIG. In this way, the direction of the output changes depending on the system configuration, but accordingly, the handling of the output signal is positive on the negative side, `` more than '' in the determination process is `` less than '', `` less than '' is `` greater than '', etc. What is necessary is just to change suitably.

  In addition, the manipulator system according to the present invention is not limited to medical use, for example, a system for various uses such as a working robot that removes foreign matter in a pipe, and a robot that carries articles in a home, office, store, etc. Applicable.

  Furthermore, the actuator of the manipulator according to the present invention is not limited to an electric motor, and may be a pneumatic actuator or the like. The present invention can be widely applied to a manipulator system driven by an actuator capable of sensing an amount corresponding to an electric current of an electric motor.

  At least a part of the internal configuration of the control device 2 shown in FIG. 5 may be configured by hardware or software. When configured by software, a program for realizing at least a part of the functions of the control device 2 may be stored in a recording medium such as a flexible disk or a CD-ROM, and read and executed by a computer. The recording medium is not limited to a removable medium such as a magnetic disk or an optical disk, but may be a fixed recording medium such as a hard disk device or a memory.

  In addition, a program that realizes at least a part of the functions of the control device 2 may be distributed via a communication line (including wireless communication) such as the Internet. Further, the program may be distributed in a state where the program is encrypted, modulated or compressed, and stored in a recording medium via a wired line such as the Internet or a wireless line.

  In addition, based on the above description, those skilled in the art may arrive at additional effects of the present invention and various modifications may be made, but these are also included in the disclosure scope of the present invention. It is thought. Aspects of the invention are not limited to the individual embodiments described above. Various additions, modifications, and partial deletions can be made without departing from the technical scope defined by the content defined in the claims and equivalents thereof.

The external view of a medical manipulator system. The figure which shows the state which isolate | separated the working part and the operation part. The perspective view which shows the structure of the action part of a manipulator main body. The disassembled perspective view which shows the detailed structure of an action part. The block diagram which shows an example of a detailed structure of a control apparatus and its surrounding control system. The flowchart which shows an example of the program which the calculating part of FIG. 5 performs. The figure which shows the relationship between the gripping angle controlled by a control apparatus, and actual gripping force. The control system figure which shows an example of the processing operation which the control apparatus which has a holding | grip start estimation observer performs. FIG. 3 is a block diagram showing an example of a more detailed internal configuration of a grip start estimation observer. (A)-(d) is a figure which shows specification of the grip start estimation observer of FIG. The flowchart which shows the process sequence of the most closed angle determination part. FIG. 5 is an operation waveform diagram when the gripper shaft and the yaw axis are operated in synchronization after the gripper shaft is independently operated. (A) And (b) is a figure which shows an example of the internal structure of an operating rod. The figure which shows the relationship between the output voltage of a sensor, and a gripper axis | shaft target value. The figure explaining presumption of a grasping start point. The graph which shows the relationship between the output-shaft angle of a motor when a manipulator hold | grips an object, and a grip start estimation signal. The flowchart which shows an example of the process sequence of a hardness correction process.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Manipulator main body 2 Control apparatus 3 Operation command part 4 Action part 5 Connection part 6 Drive part 7 Working part 8 Operation part 51 Grasping start estimation observer 52 Gripping angle determination part 53 Inverse kinematic matrix calculation part 54 PID control part 55 Motor drive part 56 Target gripping angle setting unit 57 Closed angle determination unit 58 Operation amount update unit

Claims (14)

An actuator,
An action unit that applies a force to an object based on the power of the actuator;
A power transmission mechanism having a flexible member and transmitting the power of the actuator to the action portion;
A control unit for controlling the actuator,
The controller is
An observer signal generating means for generating an observer signal for estimating an acting force start angle of the acting portion without taking into consideration a term that does not depend on a magnitude of at least one of angular velocity and acceleration but depends on a direction; ,
Action force start angle estimation means for estimating an action force start angle by comparing the signal level of the observer signal with a predetermined reference value;
Limit angle detection means for detecting a posture axis limit angle based on a posture axis angle corresponding to a preset action force of the action portion and an action force start angle estimated by the action force start angle estimation means;
And an angle control means for controlling the attitude axis angle of the action part to the attitude axis limit angle. Multiple actuators;
An action unit having a posture axis that applies a force to an object based on power of the plurality of actuators, and a plurality of operation axes that move the object in different directions;
A power transmission mechanism having a flexible member and transmitting the power of the actuator to the action portion;
A control unit for controlling the actuator,
The plurality of actuators are interference system actuators that affect the driving of the posture axis and the plurality of operation axes,
The controller is
In order to estimate the acting force start angle of the acting portion, which is provided for each of the plurality of actuators and does not take into consideration a term that does not depend on the magnitude of at least one of angular velocity and acceleration but depends on the direction. A plurality of observer signal candidate generating means for generating the respective observer signal candidates,
Observer signal selection means for selecting any one of the observer signals output from the plurality of acting force start angle estimation means according to the operation speed of each drive shaft of the plurality of actuators;
Action force start angle estimation means for estimating an action force start angle by comparing the signal level of the observer signal with a predetermined reference value;
A limit for detecting a posture axis limit angle based on a preset posture axis angle corresponding to the acting force of the acting portion and an acting force start angle corresponding to an observer signal selected by the acting force start angle estimating means An angle detection means;
And an angle control means for controlling the attitude axis angle of the action part to the attitude axis limit angle.   The observer signal selection means selects an observer signal corresponding to the actuator if the operation speed of the drive shaft of the actuator directly involved in driving the posture axis is positive among the plurality of actuators. 3. An observer signal corresponding to an actuator having a positive drive shaft operating speed is selected from the other actuators if the operating speed of the drive shaft of the actuator directly involved in driving is negative. The manipulator system described in. The controller is
A correction start angle for estimating an operation axis correction start angle for correcting an operation amount of the plurality of operation axes based on an observer signal selected by the observer signal selection means and a preset operation axis correction reference value. An estimation means;
A deviation angle calculation means for calculating a deviation angle of the plurality of movement axes based on a posture axis angle of the action part after the movement axis correction start angle is estimated;
The manipulator system according to claim 2, further comprising an operation axis correction unit that corrects an operation amount of the plurality of operation axes based on the deviation angle.   The manipulator system according to claim 4, wherein the motion axis correction reference value is set to be equal to or less than a reference value for estimating an action force start angle of the action part.   The acting force start angle estimating means estimates the acting force start angle of the acting part without taking into consideration a term that does not depend on the magnitude of at least one of angular velocity and acceleration but depends on the direction, or The manipulator system according to claim 1, further comprising a first switching unit that switches whether the gripping force is estimated in consideration of a term.   The first switching means does not take into consideration a term that does not depend on the magnitude of at least one of angular velocity and acceleration but depends on the direction when the action unit moves the object in the action direction. The manipulator according to claim 6, wherein an action force start angle of the action part is estimated, and the action force is estimated in consideration of the term when the action part releases the action action of the object. system.   When the target speed of the posture axis of the object is negative, the first switching unit determines that the action unit moves the object in the action direction, and the target speed of the posture axis of the object The manipulator system according to claim 7, wherein when the value is positive or zero, the action unit determines that the action operation of the object has been released.   The control unit sets action flag information indicating an action start when the action part moves the object in the action direction and the observer signal is equal to or greater than a predetermined reference value, and the attitude of the action part When the shaft angle becomes larger than the action force start angle, the action unit has action flag determination means that considers that the action part has finished the action on the object and resets the action flag information. The manipulator system according to any one of claims 1 to 8. An operation command unit for operating and commanding the acting force of the working unit;
The control unit uses the operation amount of the operation command unit corresponding to the action force start angle estimated by the action force start angle estimation unit as a base point, the operation amount of the operation command unit, and the attitude axis angle of the action unit, The manipulator system according to claim 1, wherein an action force of the action part is adjusted on the basis of the correlation.   The manipulator system according to claim 10, wherein the control unit sets the correlation so that the gripping force becomes maximum when the operation amount of the operation command unit is maximum. The controller is
When the posture axis angle of the action portion is between the action force start angle and the posture axis limit angle, the drive shaft angle of the actuator directly related to the drive of the posture axis of the action portion and the corresponding to the actuator Recording means for recording the observer signal output from the acting force start angle estimating means;
Hardness estimation means for estimating the hardness of the object using the drive shaft angle and the observer signal recorded by the recording means when the posture axis angle of the action part reaches the posture axis limit angle; ,
2. An attitude axis limit angle updating unit that updates the attitude axis limit angle based on a preset reference hardness and the hardness estimated by the hardness estimation unit. The manipulator system according to any one of 11.   2. An abnormality detecting means for detecting an abnormality based on a deviation amount between an acting force start angle estimated by the acting force start angle estimating means and a minimum value of the acting force start angle. The manipulator system according to any one of 12. An actuator for driving the posture axis of the object;
An action unit that applies a force to the object based on the power of the actuator;
A power transmission mechanism having a flexible member and transmitting the power of the actuator to the action portion;
A control unit for controlling the actuator, and a control method of a manipulator system comprising:
The controller is
Generating an observer signal for estimating an acting force start angle of the acting portion without taking into consideration a term that does not depend on the magnitude of at least one of angular velocity and acceleration but depends on a direction;
Estimating the acting force start angle by comparing the signal level of the observer signal with a predetermined reference value;
Detecting a posture axis limit angle based on a posture axis angle corresponding to a preset acting force of the acting portion and the estimated acting force start angle;
And a step of controlling the attitude axis angle of the action part to the attitude axis limit angle.

Images