System and method for controlling a continuum robot and continuum robot

Abstract

The present invention relates to a system and method for controlling a continuum robot and a continuum robot. A control system 10 for a continuum robot includes at least one bendable unit driven by a wire and configured to be bendable, and a drive unit for driving the wire. The control system 10 includes: a position control unit K SV , which is controlled so that the target displacement ref of the push-pull drive of the compensation drive unit to the line z The displacement z of the wire holding mechanism obtained from the continuum robot 100 and holding the wire t2 The error between the force control unit K F , which is controlled so that the compensation is from the position control unit K SV The target generated force ref that corresponds to the target tension of the line F The error between the generated force F corresponding to the tension of the wire obtained from the continuum robot 100. The control system 10 is constructed with a force control unit K F The first ring control system includes a position control unit K SV The second ring control system.

Description

Technical Field

[0001] The present invention relates to a control system and a control method for a continuum robot and the continuum robot. Background Art

[0002] A continuum robot, also known as a continuum-type robot, has a bendable unit with a flexible structure, and the shape of the bendable unit is controlled by deforming the bendable unit. Compared with a robot composed of rigid links (hereinafter referred to as a "rigid-link robot"), this continuum robot has two main advantages. The first advantage is that the continuum robot can move along the curve of an object simply by manipulating the top end of the continuum robot in a narrow space where the rigid-link robot would get stuck or in an environment with scattered objects. The second advantage is that the continuum robot has inherent flexibility that enables it to operate without damaging fragile objects, especially in open spaces. In this case, external force detection at the end effector required in the rigid-link robot is not necessarily required.

[0003] Utilizing this feature, continuum robots are expected to be applied in medical fields such as endoscope sheaths and catheters, as well as extreme work robots such as rescue robots. Continuum robots can be driven by tendon drives, push-pull wires, air actuators, etc.

[0004] Patent document 1 discloses a control method for a manipulator, which detects the amount of load generated in a drive wire and controls a drive unit so that the load is within a predetermined range. As a result, Patent document 1 allows the manipulator to be inserted into a narrow space without performing a bending operation along a path, thereby facilitating the insertion operation. Here, the passive operation of the drive unit according to the load is referred to as "reverse driving capability". In Patent document 1, a detection and control algorithm for the amount of load generated in the wire is used instead of a mechanism such as a clutch to prevent the drive unit from increasing in size. Specifically, the manipulator described in Patent document 1 is composed of a continuum called a cylindrical unit and a plurality of joint structures at its top end, is operated by pulling a wire connected to the joint by a motor provided in a base end housing, and is suitable for a continuum connected to a plurality of node rings through a rotating joint.

[0005] [Citation List]

[0006] [Patent Document]

[0007] Patent Document 1: Japanese Patent No. 6169049 Summary of the Invention

[0008] [Technical Issues]

[0009] However, in the manipulator described in Patent Document 1, it is necessary to switch between an "insertion operation mode" in which an insertion operation is performed and an "operation control mode" in which the operator operates the top bending unit by using a bending operation button, and the drive unit has a reverse drive capability only in the "insertion operation mode." In the manipulator described in Patent Document 1, the bending operation button is invalid in the "insertion operation mode," and the top bending unit cannot be operated. As a result, in the manipulator described in Patent Document 1, when a narrow path with branches is selected, or when a flat path and a steep path are repeated, the above-mentioned switching operation becomes complicated, and, for example, in a large curvature operation, when the reverse drive capability is low and the positioning performance of the bendable unit to the target position is low, fragile objects or the manipulator itself may be damaged due to misoperation.

[0010] An object of the present invention is to provide a mechanism capable of achieving high positioning performance of a bendable unit to a target position without requiring an operator to perform complicated operations.

[0011] [Solution to the problem]

[0012] A control system for a continuum robot is a control system for the following continuum robot, the continuum robot including at least one bendable unit driven by a wire and configured to be bendable, and a drive unit that drives the wire, the control system including: a position control unit configured to output a target tension of the wire, wherein the position control unit controls so as to compensate for an error between a target displacement of the push-pull drive of the wire by the drive unit and a displacement of a wire holding mechanism obtained from the continuum robot that holds the wire; and a force control unit configured to control so as to compensate for an error between the target tension of the wire output from the position control unit and the tension of the wire obtained from the continuum robot, wherein a first loop control system including the force control unit and a second loop control unit including the force control unit and the position control unit are constructed. The present invention also includes the above-mentioned continuum robot and a method for controlling the continuum robot performed by the control system of the continuum robot.

[0013] [Beneficial effects of the present invention]

[0014] According to the present invention, high positioning performance of the bendable unit to the target position can be achieved without requiring the operator to perform complicated operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] [ Figure 1 ] is a diagram showing an example of a schematic configuration of a control system of a continuum robot according to the first embodiment of the present invention.

[0016] [ Figure 2 ] is a diagram showing an example of a schematic configuration of a continuum robot according to the first embodiment of the present invention.

[0017] [ Figure 3A ] is a diagram showing an example of a dynamic model of a continuum robot according to the first embodiment of the present invention.

[0018] [ Figure 3B ] is a diagram showing an example of a dynamic model including a rotary motor, a rotation-linear motion conversion mechanism, a wire holding mechanism, etc. included in an actuator according to the first embodiment of the present invention.

[0019] [ Figure 3C ] is a diagram showing an example of coupling a dynamic model including a rotary motor, a rotary-linear motion conversion mechanism, a wire holding mechanism, etc. with the dynamic model of the continuum robot according to the first embodiment of the present invention.

[0020] [ Figure 4 ] is a diagram showing an example of a zero-pole response (zero-pole arrangement) of a dynamic model of a continuum robot according to the first embodiment of the present invention.

[0021] [ Figure 5 ] is a Bode plot illustrating a dynamic model of a continuum robot according to the first embodiment of the present invention.

[0022] [ Figure 6 ] is a Bode diagram illustrating a dynamic model of a continuum robot according to the first embodiment of the present invention.

[0023] [ Figure 7 ] is a Bode diagram illustrating a dynamic model of a continuum robot according to the first embodiment of the present invention.

[0024] [ Figure 8A ] is a Bode diagram illustrating a force control unit of a continuum robot according to the first embodiment of the present invention.

[0025] [ Figure 8B ] is a Bode diagram illustrating the open-loop transfer function of the continuum robot according to the first embodiment of the present invention.

[0026] [ Figure 9A ] is a Bode diagram illustrating the position control unit of the continuum robot according to the first embodiment of the present invention.

[0027] [ Figure 9B ] is a Bode diagram illustrating the open-loop system transfer function of the continuum robot according to the first embodiment of the present invention.

[0028] [ Figure 10 ] is a flowchart showing the design process of the control system of the control device of the continuum robot according to the first embodiment of the present invention.

[0029] [ Figure 11A] is a diagram showing the disturbance response of only the inner loop control system (first loop control system) using the force control unit according to the first embodiment of the present invention to the rotation angle of the motor.

[0030] [ Figure 11B ] is a diagram showing the disturbance response of only the inner loop control system (first loop control system) using the force control unit to the displacement of the wire holding mechanism according to the first embodiment of the present invention.

[0031] [ Figure 11C ] is a diagram showing the disturbance response of only the inner loop control system (first loop control system) using the force control unit according to the first embodiment of the present invention to the generated force.

[0032] [ Figure 11D ] is a diagram showing the disturbance response of only the inner loop control system (first loop control system) using the force control unit according to the first embodiment of the present invention to the control input.

[0033] [ Figure 11E ] is a diagram showing the disturbance response of only the inner loop control system (first loop control system) using the force control unit according to the first embodiment of the present invention to the bending angle.

[0034] [ Figure 11F ] is a diagram showing the disturbance response of only the inner loop control system (first loop control system) using the force control unit according to the first embodiment of the present invention to the disturbance torque.

[0035] [ Figure 12A ] is a diagram showing the positioning response of a dual-loop control system according to a first embodiment of the present invention to the accompanying disturbance of the motor rotation angle, wherein the dual-loop control system includes an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0036] [ Figure 12B ] is a diagram showing the positioning response of a dual-loop control system to the accompanying disturbance of the displacement of the line holding mechanism according to the first embodiment of the present invention, the dual-loop control system including an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0037] [ Figure 12C ] is a diagram showing the positioning response of a dual-loop control system according to a first embodiment of the present invention to the accompanying disturbance of the generated force, the dual-loop control system including an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0038] [ Figure 12D] is a diagram showing the positioning response of a dual-loop control system to an accompanying disturbance of a control input according to a first embodiment of the present invention, wherein the dual-loop control system includes an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0039] [ Figure 12E ] is a diagram showing the positioning response of a dual-loop control system to accompanying disturbances of a bending angle according to a first embodiment of the present invention, wherein the dual-loop control system includes an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0040] [ Figure 12F ] is a diagram showing the positioning response of a dual-loop control system to a disturbance accompanying a disturbance torque according to a first embodiment of the present invention, wherein the dual-loop control system includes an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0041] [ Figure 13 ] is a diagram showing a parameter search of a control system of a control device for a continuum robot according to a second embodiment of the present invention.

[0042] [ Figure 14A ] is a Bode diagram showing a force control unit of a control device for a continuum robot according to a second embodiment of the present invention.

[0043] [ Figure 14B ] is a Bode diagram showing an open-loop transfer function of a control device for a continuum robot according to a second embodiment of the present invention.

[0044] [ Figure 15 ] is a flowchart showing the design process of the control system of the control device of the continuum robot according to the second embodiment of the present invention.

[0045] [ Figure 16 ] is a diagram showing a parameter search of a control system of a control device for a continuum robot according to a third embodiment of the present invention.

[0046] [ Figure 17A ] is a Bode diagram showing a force control unit of a control device for a continuum robot according to a third embodiment of the present invention.

[0047] [ Figure 17B ] is a diagram showing a Bode diagram of an open-loop transfer function of a control device for a continuum robot according to a third embodiment of the present invention.

[0048] [ Figure 18] is a flowchart showing the design process of a control system of a control device for a continuum robot according to a third embodiment of the present invention.

[0049] [ Figure 19A ] is a diagram showing the positioning response of a dual-loop control system according to a third embodiment of the present invention to the accompanying disturbance of the motor rotation angle, wherein the dual-loop control system includes an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0050] [ Figure 19B ] is a diagram showing the positioning response of a dual-loop control system to the accompanying disturbance of the displacement of the line holding mechanism according to the third embodiment of the present invention, the dual-loop control system including an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0051] [ Figure 19C ] is a diagram showing the positioning response of a dual-loop control system according to a third embodiment of the present invention to the accompanying disturbance of the generated force, the dual-loop control system including an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0052] [ Figure 19D ] is a diagram showing the positioning response of a dual-loop control system to an accompanying disturbance of a control input according to a third embodiment of the present invention, the dual-loop control system including an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0053] [ Figure 19E ] is a diagram showing the positioning response of a dual-loop control system to accompanying disturbances of a bending angle according to a third embodiment of the present invention, the dual-loop control system including an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0054] [ Figure 19F ] is a diagram showing the positioning response of a dual-loop control system to an accompanying disturbance of a disturbance torque according to a third embodiment of the present invention, the dual-loop control system including an inner-loop control system (first-loop control system) using a force control unit and an outer-loop control system (second-loop control system) including a position control unit.

[0055] [ Figure 20A ] is a diagram showing an example of a schematic configuration of a control system of a continuum robot according to a fourth embodiment of the present invention.

[0056] [ Figure 20B] is a diagram showing an example of a schematic configuration of a control system of a continuum robot according to a fourth embodiment of the present invention.

[0057] [ Figure 20C ] is a diagram showing an example of a schematic configuration of a control system of a continuum robot according to a fourth embodiment of the present invention.

[0058] [ Figure 21 ] is a diagram showing a simulated response of a control system of a continuum robot according to a fourth embodiment of the present invention.

[0059] [ Figure 22 ] is a diagram showing a first example of a schematic configuration of a continuum robot according to a sixth embodiment of the present invention.

[0060] [ Figure 23 ] is a diagram showing an example of line arrangement of a continuum robot according to a sixth embodiment of the present invention.

[0061] [ Figure 24 ] is a diagram showing a method including a method according to a sixth embodiment of the present invention. Figure 22 FIG. 4 is a diagram showing an example of a schematic configuration of a control system for a continuum robot.

[0062] [ Figure 25 ] is a diagram showing a second example of the schematic configuration of the continuum robot according to the sixth embodiment of the present invention.

[0063] [ Figure 26 ] is a diagram showing a method including a method according to a sixth embodiment of the present invention. Figure 25 FIG. 4 is a diagram showing an example of a schematic configuration of a control system for a continuum robot.

[0064] [ Figure 27 ] is a diagram showing a second example of the schematic configuration of the continuum robot 100 according to the seventh embodiment of the present invention. DETAILED DESCRIPTION

[0065] Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings.

[0066] (First embodiment)

[0067] First, a first embodiment of the present invention will be described.

[0068] Figure 1 1 is a diagram showing an example of a schematic configuration of a control system 10 for a continuum robot according to a first embodiment of the present invention. Figure 1 The control system 10 of the continuum robot shown is described as “control system 10 - 1 of the continuum robot”.

[0069] like Figure 1 As shown, the control system 10-1 of the continuum robot includes a mechanism unit of the continuum robot (hereinafter referred to as "continuum robot") 100, a control device 200 of the continuum robot, and an input device 300. Figure 1 As shown, the control device 200 of the continuum robot includes a kinematics calculation unit 210, a position control unit K SV and force control unit K F .

[0070] In this embodiment, the Figure 1 The force control unit K shown F The inner loop control system (first loop control system) of the continuum robot 100 is used to control the force of the continuum robot 100. Figure 1 The force control unit K shown F and position control unit K SV The outer loop control system (second loop control system) is used to control the position of the continuum robot 100. In the control system 10-1 of the continuum robot according to the present embodiment, the dual-loop control system including the inner loop control system (first loop control system) and the outer loop control system (second loop control system) realizes a high reverse driving capability for the continuum robot 100 and, at the same time, realizes a high positioning performance of the bendable unit of the continuum robot 100 to the target position.

[0071] In the first embodiment, the dynamic model P of the continuum robot 100 is derived. n The continuum robot is driven by a wire and includes a bendable unit configured to be bendable and a drive unit that drives the wire. At this time, the drive unit can detect the tension of the wire and can be configured to include, for example, a rotary motor and a rotary-linear motion conversion mechanism.

[0072] When the target bending angle refθ (described later) of the bendable unit of the continuum robot 100 is input from the input device 300, Figure 2 The kinematics calculation unit 210 performs kinematics calculation and outputs the target displacement ref of the push-pull drive of the driving unit of the continuum robot 100. z . Position control unit K SV Control is performed to compensate for the target displacement ref output from the kinematics calculation unit 210 z The displacement z of the wire holding mechanism for holding the wire obtained from the displacement sensor of the continuum robot 100 is t2 The error between the target tension and the target generated force ref is outputted. F Here, the target generates a force ref FThe target value of the generated force F is represented by the positive and negative reversal value of the wire tension sensor defined in this embodiment. F Control is performed to compensate for the position control unit K SV Output target generated force ref F The error between the generated force F corresponding to the tension of the wire obtained from the continuum robot 100 and the motor torque T is output as the target torque of the driving unit. m Perform control input to the rotary motor and the like of the continuum robot 100 .

[0073] As mentioned above, in the case where the force control unit K F In the inner loop control system (first loop control system), the target force ref is obtained F The error is calculated by the difference between the generated force F obtained from the wire tension sensor of the continuum robot 100, and the force control unit K F Output motor torque T m As a control input for compensating for errors. This feedback loop control system is equivalent to compensating for the equivalent inertia of the rotation-linear motion conversion mechanism, thereby enabling the back-driving capability of the continuum robot 100 to be improved. In this embodiment, the closed-loop control system based on feedback from the inner loop control system (first loop control system) is referred to as G cl Including closed loop control system G cl and position control unit K SV In the outer loop control system (second loop control system), by taking the target displacement ref z and the displacement z obtained from the displacement sensor of the continuum robot 100 t2 The error is calculated by the difference between the two, and the position control unit K SV Output target generated force ref F As the control input for compensating the error. F and position control unit K SV When using the dynamic model P of the continuum robot 100 n The obtained transfer function is used to ensure the stability of the feedback system. As a result, it is possible to achieve high positioning performance to the target position while having high back-driving capability of the continuum robot 100 against disturbances of the flexible unit.

[0074] Hereinafter, the control algorithm and the dynamic model P of the continuum robot 100 will be described in detail. n Derivation of .

[0075] 1.1) Modeling

[0076] Figure 21 is a diagram showing an example of a schematic configuration of a continuum robot 100 according to a first embodiment of the present invention. Figure 2 The illustrated continuum robot 100 is referred to as “continuum robot 100 - 1 ”.

[0077] In the continuum robot 100-1, the wires 111 and 112 are connected to the fixing unit 121 and the fixing unit 122 at the distal end 160 of the bendable unit 110, respectively. The proximal ends of the wires 111 and 112 are connected to the wire holding tube 131 and the wire holding tube 132 in the robot base unit 140, respectively. A wire holding mechanism 171 for supporting the wire holding tube 131, a wire holding mechanism 172 for supporting the wire holding tube 132, and an actuator 180 corresponding to the drive unit are arranged on the robot base unit 140. At this time, the wire holding mechanism 172 is fixed to the robot base unit 140. The wire holding mechanism 171 is connected to the actuator 180 as the drive unit through the wire holding mechanism base unit (not shown) and can move up and down. The wire holding tube 131 is connected to the wire holding mechanism 171 to be supported, and the posture of the bendable unit 110 is controlled by pushing and pulling by the actuator 180. The continuum robot 100-1 includes a wire 111 and wire guides 161-164 serving as guides for the wire 112. In addition to discretely arranging multiple components, the wire guides may be continuous components such as bellows or mesh components. The wire guides 161-164 are fixed to the wire 112 at fixing units 150-153. Furthermore, the distance between the wires 111 and 112 may differ from the distance between the wire holding mechanisms 171 and 172. In this case, the diameter conversion unit 190 is connected to the robot base unit 140.

[0078] In the present embodiment, the mechanism including the wires 111 and 112 and the wire guides 161-164 is referred to as the bendable unit 110, which is a continuum portion. As the actuator 180, an actuator unit including a rotary motor and a rotary-linear motion conversion mechanism is used. In addition, the wire holding mechanism 171 (or the actuator 180) has the function of detecting the tension of the wire 111. For this purpose, the above-mentioned wire holding mechanism base unit (not shown) is provided between the wire holding mechanism 171 and the actuator 180, the wire holding mechanism base unit is connected to the actuator 180, and the wire holding mechanism base unit and the wire holding mechanism 171 are connected by a spring. At this time, it is preferred that a linear guide is provided so that the wire holding mechanism 171 is displaced only in the z-axis direction, or a parallel spring is used as the spring. The tension of the wire 111 can be detected by measuring the displacement of the spring.

[0079] Figures 3A to 3C The dynamic model P of the continuum robot 100 according to the first embodiment of the present invention is shown. nSpecifically, Figure 3A An example of a dynamic model of the bendable unit 110 as part of a continuum is shown, and Figure 3B An example of a dynamic model including the rotation-linear motion conversion mechanism, the wire holding mechanism 171 , the rotary motor included in the actuator 180 , and the like is shown.

[0080] 1.1.1) Dynamic Model of the Bendable Unit 110 as a Continuum Part

[0081] The following describes Figure 3A Definition of symbols in the dynamic model of the bendable element 110 of the continuum portion is shown.

[0082] θ: bending angle of the bendable unit 110 of the continuum robot.

[0083] ρ: radius of curvature.

[0084] l: length of the bendable unit 110 .

[0085] r l : The distance from the central axis of the bendable unit 110 to the line.

[0086] l p : Driving amount of the line.

[0087] m s : The mass of the bendable unit 110.

[0088] k b : Spring constant of the bending angle of the bendable unit 110.

[0089] z w : Displacement of the line equivalent mass.

[0090] z p : Displacement of the wire holding tube.

[0091] m w : The quality of the line.

[0092] m p : The line maintains the quality of the tube.

[0093] k w 、k w3 : Spring constant of the wire.

[0094] c w 、c w3 : Damping coefficient of the line.

[0095] c w2 : Damping coefficient due to friction between the wire and the diameter conversion unit and the wire guide.

[0096] Then, the motion equation of the continuum robot 100 - 1 is derived. In this embodiment, the following assumptions are made.

[0097] [1] Only motion in a two-dimensional plane is considered.

[0098] [2] The curvature of the bendable unit 110 is constant, and the spring constant is uniform.

[0099] [3] The wire is approximated as a lumped mass system, and the reaction force due to the longitudinal deformation acts on the tip of the bendable element 110. The lateral vibration and lateral deformation of the wire are not considered.

[0100] [4] The friction between the wire and the wire guide and the friction between the wire and the diameter conversion unit (including nonlinear friction such as Coulomb friction) are included in the damping coefficient c w2 , as viscous damping.

[0101] First, the kinetic energy of the bendable unit 110 is obtained. When the displacement x is taken on the central axis of the bendable unit 110 g and z g When , the following equations (1) and (2) are obtained respectively.

[0102] [Mathematical formula 1]

[0103]

[0104]

[0105] Based on the following equation (3),

[0106] [Mathematical formula 2]

[0107]

[0108] Displacement x g and z g They are the following equations (4) and (5), respectively.

[0109] [Mathematical formula 3]

[0110]

[0111]

[0112] Using them, the kinetic energy T of the bendable unit 110 a It is expressed by the following equation (6).

[0113] [Formula 4]

[0114]

[0115] The potential energy U of the bendable unit 110 a It is expressed by the following equation (7).

[0116] [Formula 5]

[0117]

[0118] Next, since the line drive amount is l p = r1θ, so the kinetic energy T of the wire and the wire holding tube w and potential energy U w They are expressed by the following equations (8) and (9), respectively.

[0119] [Formula 6]

[0120]

[0121]

[0122] Then, when the equation of motion is obtained from the following equation (10) showing the Lagrange equation,

[0123] [Formula 7]

[0124]

[0125] T=T a +T w , U=U a +U w

[0126] q=[q1,q2,q3] T =[z p， z w ,θ] T

[0127] Q = [Q1, Q2, Q3] T =[0,0,0] T

[0128] The nonlinear differential equation shown in the following equation (11) is obtained. In the following equation (11), the term Φ is a nonlinear term that cannot be included in M c 、C c and K c middle.

[0129] [Formula 8]

[0130]

[0131] In addition, M in equation (11) c22 and Φ3 are the following equations (12) and (13).

[0132] [Formula 9]

[0133]

[0134]

[0135] However, when the bending angle of the bendable unit 110 is near 0 degrees, equations (12) and (13) become uncertain and cannot obtain values. Therefore, a linearized model of the 0-degree neighborhood is obtained that does not consider large deformation. Based on equations (4) and (5), the following equation (14) is obtained.

[0136] [Formula 10]

[0137]

[0138] The kinetic energy T of the bendable unit 110 near 0 degrees a0 It is expressed by the following equation (15).

[0139] [Mathematical formula 11]

[0140]

[0141] Then, the equation of motion is obtained from the Lagrange equations to obtain the following equation (16), where T = T a0 +T w .

[0142] [Mathematical formula 12]

[0143]

[0144] 1.1.2) Dynamic model including rotary motor, rotary-linear motion conversion mechanism and wire holding mechanism

[0145] Figure 3B This is a diagram showing an example of a dynamic model including a rotary motor, a rotary-linear motion conversion mechanism, and a wire holding mechanism. The wire holding mechanism has a function of detecting the tension of the wire. The wire holding mechanism base is provided between the wire holding mechanism and the actuator 180. The wire holding mechanism base is connected to the actuator 180, and the wire holding mechanism base and the wire holding mechanism are connected by a spring. The tension is detected by detecting the displacement of the spring. Figure 3B Definition of symbols in the dynamic model shown.

[0146] J m : Motor inertia.

[0147] θ m : The rotation angle of the motor.

[0148] T m : Target torque of the motor (command value).

[0149] c m : Damping coefficient of the motor shaft.

[0150] k g 、c g : Spring coefficient and damping coefficient of the coupling.

[0151] J n : Inertia of the drive shaft.

[0152] θ n : The rotation angle of the drive shaft.

[0153] c n : Damping coefficient of the drive shaft.

[0154] p: Pitch of the drive shaft.

[0155] R: Conversion coefficient of the linear-rotation conversion mechanism, that is, equivalent speed increase ratio.

[0156] m t1 : The quality of the base unit of the line holding mechanism.

[0157] k t1 : Spring constant of the drive shaft in the z direction.

[0158] c t1 : Damping coefficient of the linear slider of the wire holding mechanism base unit.

[0159] z t1 : Displacement of the wire holding mechanism base unit.

[0160] m t2 : The quality of the line maintenance mechanism.

[0161] k t2 、c t2 : Spring coefficient and damping coefficient of the tension detection mechanism.

[0162] z t2 : Displacement of the wire holding mechanism.

[0163] The equations of motion are expressed by the following equations (17) to (20).

[0164] [Mathematical formula 13]

[0165]

[0166]

[0167]

[0168]

[0169] Here, we assume that R = p / 2π. The matrix represents q l =[θ m ,θ n ,z t1 ,z t2 ] T The following equation (21) is given.

[0170] [Formula 14]

[0171]

[0172] 1.1.3) Construction of the Augmented System

[0173] like Figure 3C As shown, the dynamic model of the bendable unit 110 is coupled to the dynamic model including the rotary motor, the rotary-linear motion conversion mechanism and the wire holding mechanism. t2 Displacement z of the wire holding tube p When the equations (11) and (21) are the same, the equation of motion is expressed by the following equation (22). g =[q l T ,z w ,θ] T .

[0174] [Mathematical formula 15]

[0175]

[0176] Next, the state equation is obtained. When the bending angle of the bendable unit 110 is not near 0 degrees, the extended linearization method of equation (23) below is applied, where the nonlinear term Φ g Contained in the matrix K g for linearization.

[0177] [Mathematical formula 16]

[0178]

[0179] Therefore, Φ3 becomes K g The state equation is expressed by the following equations (24) and (25).

[0180] [Mathematical formula 17]

[0181]

[0182]

[0183] In this embodiment, the displacement z of the wire holding mechanism base unit can be measured. t1 and the displacement z of the wire holding mechanism t2 , and is displaced by the spring k t2 The generated force (hereinafter referred to as the generated force) F is applied to the thread holding mechanism, and the displacement z of the thread holding mechanism is taken t2 Here, the generated force F is expressed by the following equation (26).

[0184] [Mathematical formula 18]

[0185] F=-k t2 (z t2 -z t1 ) (26)

[0186] In this case, the observation y g It is expressed by the output equation shown in the following equation (27).

[0187] [Mathematical formula 19]

[0188]

[0189] When the bending angle of the bendable unit 110 is near 0 degrees, the augmented system is similarly constituted by Equation (16) and Equation (21). Equation (16) shows the motion equation of the bendable unit 110 near 0 degrees, and Equation (21) shows the motion equation of the dynamic model composed of the rotary motor, the rotary-linear motion conversion mechanism, and the wire holding mechanism. Since this is a linear model, it can be directly converted into a state equation.

[0190] 1.2) Model Analysis

[0191] 1.2.1) Changes relative to the equivalent speed-increasing ratio R

[0192] In this chapter, a model analysis is performed based on the equivalent speed increase ratio R of the rotation-linear motion conversion mechanism to examine the changes in the poles and zeros of the bendable unit 110. In this embodiment, the damping coefficient c between the wire and the diameter conversion unit and the wire guide will be described. w2 The phase change caused by this is assumed. Here, the lowest natural frequency of the flexible unit 110 is 62 Hz. The equivalent inertia of the rotation-linear motion conversion mechanism is varied by changing its equivalent speed-increasing ratio R from 1 to 10,000 times the design parameter, and the changes in the poles and zeros are examined. Furthermore, in this analysis, the damping coefficients of various parts of the flexible unit 110 are set to approximately 0 to facilitate understanding of the changes between the poles and zeros.

[0193] Figure 4is a diagram showing a dynamic model P of the continuum robot 100 according to the first embodiment of the present invention. n A graph showing an example of a pole-zero response (pole-zero placement). Figure 4 In the figure, the poles of the individual flexible element 110 are indicated by stars, the poles and zeros for an equivalent speed-increasing ratio R of 10,000 are indicated by asterisks and circles, respectively, and the poles and zeros for an equivalent speed-increasing ratio R of 1 are indicated by Xs and triangles, respectively. As can be seen, the natural frequency of the flexible element 110 decreases as the equivalent inertia on the motor side increases, and the poles of the flexible element 110 shift toward the overdamped poles and the original poles. Note that the zeros indicated by circles and triangles remain essentially unchanged. Although the frequency of the flexible element 110 is overdamped, the phase characteristics within the control band must be considered due to the zeros in the control system design.

[0194] Figure 5 is a diagram showing a dynamic model P of the continuum robot 100 according to the first embodiment of the present invention. n Specifically, Figure 5 The Bode diagram of the augmented system for changing the equivalent speed-increasing ratio R is shown. Here, in the augmented system, equations (24) and (27) are linearized with a bending angle θ of 0.05 degrees. Figure 5 The motor target torque T is shown as the control input. m The transfer function to the generated force F is shown in Figure 1, with the responses for equivalent speed-up ratios R of 10, 50, and 500 times shown as solid, dashed, and dot-dash lines, respectively. It can be seen that as the equivalent speed-up ratio R decreases and the motor-side equivalent inertia increases, the peak value of the continuum decreases, while the anti-resonance frequency remains unchanged. In the response with an equivalent speed-up ratio R multiplied by 10, shown by the solid line, the anti-resonance coefficient at approximately 18 Hz is difficult to understand. However, in the response with an equivalent speed-up ratio R multiplied by 500, shown by the dot-dash line, it is clear that the anti-resonance is caused by the resonant and anti-resonant characteristics of the continuum portion of the augmented system, in which the flexible unit 110 and the tension detection mechanism are combined. It can be seen that at this anti-resonance frequency, the phase advances by 180 degrees.

[0195] 1.2.2) Damping coefficient c due to friction between the wire and the diameter conversion unit and the wire guide w2 Changes

[0196] Next, check the damping coefficient c w2 Caused by the changes in the model. Figure 6 is a diagram showing a dynamic model P of the continuum robot 100 according to the first embodiment of the present invention. n Specifically, Figure 6 The damping coefficient c is shown w2 Bode plot of the augmented system with changes. Figure 6 The motor torque T is shown as the control input. m The transfer function to generate force F, and the damping coefficient c w2 The responses for settings of 0.01, 1, 10, and 150 are shown by the solid line, dotted line, dashed line, and dash-dot line, respectively. w2 As the frequency increases, the low-frequency region changes from an integral characteristic to a first-order delay characteristic. Consequently, the phase reverses 180 degrees at zero, around a frequency of approximately 18 Hz. If this occurs near the target servo band, it significantly impacts control system design. In designing a control system for a continuum robot 100-1 having a rotation-to-linear motion conversion mechanism, it is understood that, in addition to the natural frequency of the flexible unit 110, model identification of the rotation-to-linear motion conversion mechanism and the diameter conversion unit is also crucial.

[0197] Figure 7 is a diagram showing a dynamic model P of the continuum robot 100 according to the first embodiment of the present invention. n Specifically, Figure 7 The Bode diagram of the augmented system is shown. From the motor torque T m The transfer function to the generated force F is represented by the solid line, from the motor torque T m Displacement z of the line holding mechanism t2 The transfer function is represented by the dotted line and is derived from the motor torque T m The transfer function of the bending angle θ to the bendable unit 110 is indicated by a dot-dash line. In the high frequency region, high-order vibrations due to the springs and wires of the wire tension detection mechanism and the rotation-linear motion conversion mechanism occur.

[0198] 1.3) Control system design

[0199] In this chapter, if Figure 1 As shown, the control system is designed by using a double-loop control system, which has a force control unit K F The inner loop control system (first loop control system) and the force control unit K F and position control unit K SV The outer loop control system (second loop control system) of the continuous robot 100-1 has a high reverse driving capability and achieves high positioning performance of the flexible unit of the continuous robot 100 to the target position. Here, the dynamic model P n Denote the augmented system shown in Equation (24) and Equation (27).

[0200] Line drive amount l p The relationship with the bending angle θ of the bendable unit 110 is given by the following equation (28).

[0201] [Mathematical formula 20]

[0202] l p =r1θ (28)

[0203] In this embodiment, since the amount of expansion and contraction of the line is considered small, the amount of expansion and contraction of the line is not considered in the kinematic derivation by the kinematics calculation unit 210. Therefore, the target displacement ref z It is given by the following equation (29).

[0204] [Mathematical formula 21]

[0205] ref z =r1·ref θ (29)

[0206] In the dynamic model P including the augmented system n and force control unit K F In the inner loop control system (first loop control system), the target force ref is taken as the target value of the generated force F. F The error is calculated by the difference between the generated force F and the force control unit K F Output motor torque T m As a control input for compensating errors. This feedback loop is equivalent to compensating the equivalent inertia of the rotation-linear motion conversion mechanism, thereby enabling the reverse driving capability of the continuum robot 100-1 to be improved. In this embodiment, as described above, the closed-loop system generated by this feedback is called G cl Including closed loop control system G cl and position control unit K SV In the outer loop control system (second loop control system), by taking the target displacement ref z and the displacement z obtained from the continuum robot 100 t2 The error is calculated by the difference between the two, and the position control unit K SV Output target generated force ref F As a control input for compensating errors.

[0207] In this embodiment, as an inner loop control system (first loop control system) for performing force control, for example, a PI control system represented by the following equation (30) is used.

[0208] [Mathematical formula 22]

[0209]

[0210] Among them F ziis the zero-crossing frequency of the integral controller. In addition, in order to stabilize the high-order modes shown in the previous chapter, a second-order low-pass filter with a breakpoint frequency of 200 Hz is coupled to the PI control system. In order to achieve the desired linear motion by using a rotary-to-linear motion conversion mechanism and due to the damping coefficient c near the zero point, the linear motion is converted to zero. w2 The phase characteristics of the bendable unit 110 are varied to design a stable force control unit K for the pole and zero point changes. F , derive and design the open-loop transfer function P n K F , so that according to the response, the gain margin and phase margin are sufficient (calculation of stability margin is performed).

[0211] Figure 8A and Figure 8B : is a Bode diagram showing the control device 200 of the continuum robot according to the first embodiment of the present invention. Specifically, Figure 8A and Figure 8B shows the force control unit K F The Bode diagram of , and the augmented system P n Bode plot of the open-loop transfer function of the force control unit K F In equation (30), K p and F zi 2.2-10 respectively -3 and 1.0 and the control band is approximately 30 Hz.

[0212] In this embodiment, as an outer loop control system (second loop control system) that performs position control, for example, a PID control system represented by the following equation (31) is used.

[0213] [Mathematical formula 23]

[0214]

[0215] Here, F zi and F zd are the zero-crossing frequencies of the integral controller and the differential controller, respectively. By using the dynamic model of the augmented system P n With force control unit K F The closed-loop transfer function G between cl , derive the open-loop transfer function GclK SV , and the open-loop transfer function GclK SV It is designed so that the gain margin and the phase margin are sufficient according to the response. A first-order low-pass filter having a breakpoint frequency of 50 Hz is coupled to the PID control system.

[0216] Figure 9A and Figure 9B: is a Bode diagram showing the control device 200 of the continuum robot according to the first embodiment of the present invention. Specifically, Figure 9A and Figure 9B The position control unit K is shown SV Bode diagram of the closed-loop control system G cl and position control unit K SV The open-loop transfer function G cl K SV Bode diagram of the position control unit K SV In equation (31), K p 、F zi and F zd 5-10 respectively 3 , 0.5 and 100, and the control band is about 3.5Hz.

[0217] Figure 10 1 is a flowchart illustrating a design process of a control system related to the control device 200 for a continuum robot according to the first embodiment of the present invention. Figure 10 The following design process is shown.

[0218] <1> Derive the dynamic model P of the augmented system n .

[0219] <2> Setting coefficient K p and K i , coupled with a low-pass filter, and a preliminary design of the force control unit K F .

[0220] <3> Derive the open-loop transfer function P n K F .

[0221] <4> Determine whether the gain margin and phase margin are sufficient.

[0222] <5> Force control unit K F The design is completed.

[0223] <6> Derive the closed-loop system G cl .

[0224] <7> Setting coefficient K p , K i and K d , coupled with a low-pass filter, and preliminarily designed the position control unit K SV .

[0225] <8> Derive the open-loop transfer function G cl K SV .

[0226] <9> Determine whether the gain margin and phase margin are sufficient.

[0227] <10> Position control unit K SV The design is completed.

[0228] 1.4) Simulation

[0229] The simulation is performed using the control system designed in (1.3) Design of the control system and the augmented system shown in equations (24) and (27).

[0230] Figures 11A to 11F The first embodiment of the present invention is shown and the force control unit K is used. F The disturbance response of the inner-loop control system (first-loop control system) is shown in Figure 2. Figure 11A shows the rotation angle θ of the motor m , Figure 11B shows the displacement z of the wire holding mechanism t2 , Figure 11C shows the generated force F, Figure 11D shows the control input T m , Figure 11E shows the bending angle θ of the top end of the bendable unit 110, and Figure 11F The disturbance torque applied to the top end of the bendable unit 110 is shown. Figures 11A to 11F In the figure, the controlled response is represented by a solid line, and for comparison, the uncontrolled response is represented by a dashed line. The target generated force ref F Indicated by dotted lines.

[0231] 5 seconds after the simulation starts, if Figure 11F As shown in FIG, a disturbance torque is applied to the top of the bendable unit 110. Figures 11A to 11F As shown in FIG, the generated force in the negative z-axis direction acts on the wire holding mechanism through the spring of the tension detection mechanism, but the target value of the generated force F is 0. Therefore, as Figure 11D As shown, the force control unit K F A positive control input is given to the motor and when Figure 11A When the motor rotates in the positive direction, the wire holding mechanism shifts in the positive direction of the z-axis, as shown in FIG. Figure 11B As shown. The result is Figure 11C As shown in FIG. , the generated force F is compensated to 0 as a target value, and as Figure 11E As shown in FIG, the top end of the bendable unit 110 bends in the same direction as the disturbance torque, and the motor is driven in the reverse direction according to the disturbance torque. Figure 11F The disturbance torque shown in FIG. 1 is small, but since the equivalent inertia of the rotation-linear motion conversion mechanism is large, the motor hardly rotates, and the top end of the bendable unit 110 hardly bends, as shown in FIG. Figure 11A and Figure 11EAs shown by the dotted line in . As a result, it can be seen that the inner loop control system for force control compensates for the equivalent inertia of the rotary-linear motion conversion mechanism and enables reverse driving against the disturbance torque at the top end of the continuum.

[0232] 12A to 12F FIG1 shows a first embodiment of the present invention and is a diagram showing the positioning response to disturbances of a dual-loop control system including a force control unit K using F The inner loop control system (first loop control system) and also includes a position control unit K SV The outer loop control system (second loop control system) is as follows: Figures 12A to 12F Shown with Figures 11A to 11F The response of the same arrangement. This simulation shows a comparison with semi-closed control, in which the motor is controlled only by the rotation angle θ m To control the position. The servo bands of the position control systems of the dual-loop control system and the semi-closed control system are designed to be similar. 12A to 12F In , the control response of the dual-loop control system is represented by a solid line, and the response of the semi-closed control system is represented by a dashed line. Figure 12A The target angle ref of the motor applied to the semi-closed control system is shown by a dotted line m , Figure 12B The target displacement of the wire holding mechanism applied to the outer ring is shown by a dotted line ref z ,and Figure 12C The target generative force ref, which is a target value of the generative force F, is shown by a dotted line. F , the force generated by the position control unit K SV Output control input.

[0233] After the simulation starts, both the dual-loop control system and the semi-closed control system follow the target trajectory and are set to the target displacement within about one second. Figure 12F As shown in FIG. 1 , a disturbance torque is applied to the top of the bendable unit 110 within 1.5 seconds. Figure 12C As shown, the position control unit K of the outer loop control system (second loop control system) SV Output the target value of the generated force in the negative z-axis direction as the control input to compensate for the displacement z of the wire holding mechanism t2 The displacement error generated in Figure 12C As shown, the force control unit K of the inner loop control system (first loop control system) F Follow the target to generate force ref F , which is the target value of the generated force F. However, since the equivalent inertia of the rotation-linear motion conversion mechanism is compensated and reduced at the same time, as Figure 12A As shown, the motor is driven in the forward direction and, as Figure 12BAs shown, the wire holding mechanism is displaced in the positive z-axis direction. Figure 12D The control input to the motor in the positive direction at 1.5 seconds in the figure indicates that the control is performed to reduce the equivalent inertia. Figure 12E As shown in FIG. 1 , it can be seen that the top of the bendable unit 110 bends in the same direction as the disturbance torque, thereby realizing a control system for reverse driving against the disturbance torque. In the semi-closed control system shown by the dotted line, when the Figure 12F When the disturbance torque is 0.05, the equivalent inertia of the rotation-linear motion conversion mechanism is large, so that the motor hardly rotates and the top of the bendable unit 110 hardly bends, as shown in FIG. Figure 12A and Figure 12E As shown by the dotted line in the figure. Since the servo bands of the semi-closed control system and the dual-loop control system are basically equal, the servo bands of the semi-closed control system are basically equal to those of the dual-loop control system. Figure 12F The generated force F in the motor. From this, it can be seen that the dual-loop control system has the same effect as that of only controlling the rotation angle θ of the motor. m The semi-closed control system for controlling the position has the same setting performance of the target position and simultaneously enables reverse driving of the disturbance torque at the tip of the bendable unit 110.

[0234] As described above, in the control system 10 of the continuum robot according to the first embodiment, a system including the force control unit K is constructed. F The inner loop control system (first loop control system) and the force control unit K F and position control unit K SV The outer loop control system (second loop control system) is configured to achieve high positioning performance of the bendable unit to the target position without requiring the operator to perform complex operations. Consequently, the continuum robot 100 can achieve high reverse driving capability against disturbances at the tip of the bendable unit.

[0235] (Second embodiment)

[0236] Next, a second embodiment of the present invention will be described. In the following description of the second embodiment, matters common to the above-described first embodiment will be omitted, and matters different from the above-described first embodiment will be described.

[0237] In the first embodiment described above, the bendable unit 110 as a continuum portion is coupled to the rotation-linear motion conversion mechanism, causing the vibration characteristics to shift to the low-frequency range, and showing that the phase characteristics change due to the friction between the wire, the diameter conversion unit, and the wire guide. In the first embodiment described above, the PI control system is used as the force control unit K of the inner loop control system (first loop control system) that performs force control. F .

[0238] On the other hand, in the second embodiment, the phase characteristic is obtained by using the force control unit K F Here, for example, the PID control system shown in the following equation (32) is used.

[0239] [Mathematical formula 24]

[0240]

[0241] Here, F zi and F zd are the zero-crossing frequencies of the integral controller and the differential controller, respectively. A second-order low-pass filter with a breakpoint frequency of 200 Hz is coupled (e.g., coupled in series) to the PID control system (PID control unit). As in the first embodiment, the open-loop transfer function P is derived and designed. n K F , so that the gain margin and phase margin are sufficient according to its response. In the second embodiment, the gain K is designed. d In the PID control system, the differential controller can be used at a frequency lower than the zero-crossing frequency F. zd The gain advances the phase at frequencies above F without increasing the gain (differential gain). However, the gain zd Therefore, changing the zero-crossing frequency F zd , and by using the open-loop transfer function P n K F To calculate the gain margin (calculate the stability margin), so that the zero-crossing frequency F zd and the gain margin, and searches for the zero-crossing frequency F that maximizes the gain margin. zd .

[0242] Figure 13 : is a diagram showing a parameter search of a control system related to the control device 200 of a continuum robot according to the second embodiment of the present invention. Specifically, Figure 13 The response of the gain margin search is shown, where the minimum frequency is set to 1 Hz, which is sufficiently low compared to the zero point of the flexible unit 110 as a continuum portion shown in the first embodiment, and the maximum frequency is set to, for example, 1 kHz, which is the Nyquist frequency when the control system is implemented digitally. Figure 13 It can be seen that the gain margin is around 95Hz at the zero-crossing frequency F zd Becomes the largest.

[0243] Figure 14A and Figure 14B : is a diagram showing a Bode diagram and an open-loop response of a control system of a control device 200 for a continuum robot according to a second embodiment of the present invention. Specifically, Figure 14Ashows the force control unit K F Bode diagram of the PID control system, and Figure 14B shows the open loop transfer function P n K F The Bode diagram of . Figure 14A and Figure 14B The solid line shows the zero-crossing frequency F zd The PID control system of the second embodiment is shown as 95 Hz, and the PI control system of the first embodiment is shown by the dotted line. The open loop response shows that the gain margin is improved by about 7 dB by compensating for the phase delay using the differential characteristic.

[0244] Figure 15 1 is a flowchart illustrating a design process of a control system related to the control device 200 for a continuum robot according to the second embodiment of the present invention. Figure 15 The following design process is shown.

[0245] <1> Derive the dynamic model P of the augmented system n .

[0246] <2> Setting coefficient K p and K i , coupled with a low-pass filter, and a preliminary design of the force control unit K F .

[0247] <3> Derive the open-loop transfer function P n K F .

[0248] <4> Determine whether the gain margin and phase margin are sufficient.

[0249] <5> Determine the zero-crossing frequency F zd Search scope.

[0250] <6> Search for zero-crossing frequency F zd To determine the K that maximizes the gain margin d .

[0251] <7> Force control unit K F The design is completed.

[0252] <8> Derive the closed-loop system G cl .

[0253] <9> Setting coefficient K p , K i and K d , coupled with a low-pass filter, and a preliminary design of the position control unit K SV .

[0254] <10> Derive the open-loop transfer function G cl KSV .

[0255] <11> Determine whether the gain margin and phase margin are sufficient.

[0256] <12> Position control unit K SV The design is completed.

[0257] (Third embodiment)

[0258] Next, a third embodiment of the present invention will be described. In the description of the third embodiment below, matters common to the first and second embodiments described above will be omitted, and matters different from the first and second embodiments described above will be described.

[0259] In the second embodiment described above, the phase lag characteristic was compensated for by the differential element of the PID control system. In the third embodiment, the control system order was increased to compensate for the phase lag. The gain of the inner loop control system (first loop control system) for force control was increased to widen the control band of the inner loop control system, thereby achieving an improved gain margin. The effectiveness of this approach was then verified through simulation.

[0260] 3.1) Control system design

[0261] The phase advance filter represented by the following equation (33) is coupled (for example, coupled in series) to the force control unit K designed in the above-mentioned second embodiment. F In this embodiment, it is called a PID-advance control system.

[0262] [Mathematical formula 25]

[0263]

[0264] Here, F c1 is the zero-crossing frequency, and F c2 (>F c1 ) is a pole for making the filter suitable. In this embodiment, F c2 Set to 1kHz and search for F c1 Similar to the second embodiment, the zero-crossing frequency F is changed c1 And obtain the zero-crossing frequency F by calculating the gain margin each time c1 The relationship between the gain margin and the zero-crossing frequency F with the maximum gain margin is searched. c1 .

[0265] Figure 16 : is a diagram showing parameter retrieval of a control system related to the control device 200 of a continuum robot according to the third embodiment of the present invention. Specifically, Figure 16The search response is shown, where the minimum frequency is set to 1 Hz, which is sufficiently low compared to the zero point of the bendable unit 110 as a continuum part, and the maximum frequency is set to 900 Hz, which is equal to or lower than the maximum frequency F c2 .exist Figure 16 It can be seen that the gain margin is around 540Hz at the zero-crossing frequency F c1 Because the response is steep, when the zero-crossing frequency F c1 When set to 545Hz with some headroom, the gain margin is improved by about 6dB.

[0266] Figure 17A and Figure 17B : is a diagram showing a Bode diagram and an open-loop response of a control system of a control device 200 for a continuum robot according to a third embodiment of the present invention. Specifically, Figure 17A It is the force control unit K F Bode diagram of the PID-lead control system, and Figure 17B is the open-loop transfer function P n K F The Bode diagram of . Figure 17A and Figure 17B In FIG. 1 , the force control system using the PID-lead control system is shown by the solid line, and the control system of the first embodiment is shown by the dashed line. Here, the PID-lead control system increases the gain by 13 dB, which corresponds to an improved gain margin compared to the PI control system of the first embodiment. Therefore, the gain is higher than that of the control system of the first embodiment, but the gain margin is equal.

[0267] Figure 18 1 is a flowchart illustrating a design process of a control system related to the control device 200 for a continuum robot according to the third embodiment of the present invention. Figure 18 The following design process is shown.

[0268] <1> Derive the dynamic model P of the augmented system n .

[0269] <2> Setting coefficient K p and K i , coupled with a low-pass filter, and a preliminary design of the force control unit K F .

[0270] <3> Derive the open-loop transfer function P n K F .

[0271] <4> Determine whether the gain margin and phase margin are sufficient.

[0272] <5> The gain margin GM1 is stored.

[0273] <6> Determine the zero-crossing frequency F zd Search scope.

[0274] <7> Search for zero-crossing frequency F zd To determine the K that maximizes the gain margin d .

[0275] <8> Determine the frequency F of the phase advance filter c1 Search range and frequency F c2 .

[0276] <9> Search frequency F c1 To determine the phase-lead filter with the largest gain margin.

[0277] <10> The gain margin GM2 is stored.

[0278] <11> The force control system K F Multiply by a gain equivalent to GM2-GM1.

[0279] <12> Force control unit K F The design is completed.

[0280] <13> Derive the closed-loop system G cl .

[0281] <14> Setting coefficient K p , K i and K d , coupled with a low-pass filter, and preliminarily designed the position control unit K SV .

[0282] <15> Derive the open-loop transfer function G cl K SV .

[0283] <16> Determine whether the gain margin and phase margin are sufficient.

[0284] <17> Position control unit K SV The design is completed.

[0285] 3.2) Simulation

[0286] Figures 19A to 19F FIG. 3 shows a third embodiment of the present invention and is a diagram showing the positioning response to disturbances of a dual-loop control system including a force control unit K using F The inner loop control system (first loop control system) and also includes a position control unit K SV The outer loop control system (second loop control system) is as follows: Figures 19A to 19F Shown with Figures 11A to 11FThe response of the same arrangement. This simulation shows a comparison with the PI control system of the first embodiment. The servo band of the outer loop control system (second loop control system) of the dual loop control system using the PID-advance control system is designed to be similar to the servo band of the first embodiment. Figures 19A to 19F In FIG, the response of the PID-lead control system is represented by a solid line, and the response of the PI control system is represented by a dashed line. Figure 19B The target displacement ref applied to the outer ring of the wire holding mechanism is shown as a dotted line z ,and Figure 19C The dotted line shows the position control unit K as a slave SV Output control input target generated force ref F .

[0287] After the simulation starts, both the PID-lead control system and the PI control system follow the target trajectory and set to the target displacement in about one second. Figure 19F As shown in FIG. 1 , a disturbance torque is applied to the top of the bendable unit 110 within 1.5 seconds. As in the first embodiment, Figure 19C As shown, the position control unit K of the outer loop control system (second loop control system) SV Output target generated force ref F As control input, the target generates a force ref F is the target value of the force F in the negative z-axis direction, and the force control unit K of the inner loop control system (first loop control system) F Follow the target value of the generated force F. Due to the force control unit K F At the same time, it compensates and reduces the equivalent inertia of the rotary linear motion conversion system, so Figure 19A As shown, the motor is driven in the forward direction and Figure 19B As shown, the wire holding mechanism is extended in the positive z-axis direction. Here, it can be seen that the PID-lead control system has a higher gain than the PI control system and is therefore largely driven inversely. Figure 19D As shown in the control input to the motor, it can be seen that the PID-advance control system has a larger control input for reducing the equivalent inertia than the PI control system. Figure 19E As shown, the PID-lead control system realizes control with a large reverse driving capability against the disturbance torque on the tip of the bendable unit 110. The PID-lead control system is also stable to higher order vibration modes.

[0288] Since the PID-advance control system can compensate the phase delay characteristics at a higher order, the force control unit K of the inner loop control system (first loop control system) can be FDesigned for high gain, the equivalent inertia of the rotary-linear motion conversion mechanism can be compensated for with high gain, and the reverse driving capability can be increased.

[0289] (Fourth embodiment)

[0290] Next, a fourth embodiment of the present invention will be described. In the following description of the fourth embodiment, matters common to the above-described first to third embodiments will be omitted, and matters different from the above-described first to third embodiments will be described.

[0291] In the first and second embodiments described above, it is shown that the dual-loop control system can provide a reverse driving capability for the continuum robot 100. In addition, in the third embodiment, it is shown that the force control unit K of the inner loop control system (first loop control system) is connected to the inner loop control system (first loop control system). F Designing for high gain can increase the reverse driving capability. This embodiment shows a control system that can adjust the magnitude of the reverse driving capability without changing the positioning performance.

[0292] First, as shown in the first to third embodiments, a dual-loop control system is designed. F Multiply by a coefficient (first coefficient) α between 0 and 1. Therefore, the force control unit K F The gain is reduced and the reverse driving capability can be arbitrarily reduced. However, due to the open-loop transfer function G cl K SV The gain of the position control unit K is also reduced, so the servo band of the positioning control is reduced. SV The gain of α is adjusted and the servo band is redesigned to be the same as when the coefficient α is 1. Therefore, the backdrive capability can be changed without changing the performance of the position control.

[0293] 20A to 20C FIG. 1 shows an example of a schematic configuration of a control system 10 for a continuum robot according to a fourth embodiment of the present invention. Figure 20A The control system 10 of the continuum robot shown is described as "control system 10-2 of the continuum robot". Figure 20B The control system 10 of the continuum robot shown is described as "control system 10-3 of the continuum robot". Figure 20C The control system 10 of the continuum robot is described as "control system 10-4 of the continuum robot". 20A to 20C In, with Figure 1 Components similar to those shown in FIG. 1 are denoted by the same reference numerals, and detailed descriptions thereof are omitted.

[0294] exist Figure 20AIn the control system 10-2 of the continuum robot shown in FIG. 1 , the dotted line represents the position control unit K. SV According to the force control unit K F Specifically, Figure 20A It shows that by changing the force control unit K F The first gain is obtained by multiplying the gain of by the coefficient (first coefficient) α. Figure 20A In the example, the input device 400 is directed to the force control unit K F Input coefficient (first coefficient) α. Then, on the computer, the following process can be repeated: increase the position control unit K in small steps SV The gain (second gain) and calculate the open-loop transfer function G cl K SV The response of the position control unit K is the same as that when the coefficient α is 1. However, if the coefficient α is too small, the position control unit K SV It is necessary to have a high gain, which will excite high-order modes and cause the control system to become unstable. Therefore, the lower limit of the coefficient α can be set in advance. Alternatively, the position control unit K is calculated in advance. SV The gain multiplied by the coefficient (second coefficient) and stored in the memory makes the position control unit K SV The servo bands of are always equal according to the coefficient α. In this case, for example, Figure 20B The control system 10-3 of the continuum robot shown in FIG. 1 can read the position control unit K from the memory 220 according to the coefficient α. SV As a result, by adding a knob or the like that enables the operator to arbitrarily command the coefficient α to the operating system of the continuum robot 100, the reverse driving capability can be changed in real time.

[0295] Next, a control system will be described that varies the reverse driving capability in conjunction with the bending angle of the bendable unit 110. For example, to reduce the reverse driving capability as the bending becomes greater and prevent deformation due to disturbances, etc., the following equation (34) may be used.

[0296] [Mathematical formula 26]

[0297] α=β|ref θ | (34)

[0298] On the other hand, in order to increase the reverse driving capability and safety as the bend becomes larger, the following equation (35) can be used.

[0299] [Mathematical formula 27]

[0300] α=-β|ref θ |+γ (35)

[0301] Here, β and γ are coefficients, and the range of curvature must be set in advance so that the coefficient α falls within the range of 0<α<1. Figure 20C Here, the coefficient calculation part fα represents a component that calculates and changes the coefficient (first coefficient) α according to the target bending angle refθ of the bendable unit 110, as shown in equations (34) and (35).

[0302] Figure 21 : is a diagram showing a simulated response of the control system of the continuum robot control system 10 according to the fourth embodiment of the present invention. In the same manner as the first embodiment, after 1.5 seconds of positioning completion, for example, Figure 12F The disturbance torque shown is applied to the top end of the bendable element 110. Figure 21 , the bending angle of the tip of the bendable unit 110 is shown, and the responses for coefficients α of 1, 0.6, 0.3, and 0.1 are represented by the dotted line, solid line, dashed line, and dash-dotted line, respectively. While positioning performance does not change with changes in coefficient α, it can be seen that the reverse drive capability decreases as the coefficient α decreases. Consequently, it can be seen that a control system capable of adjusting the magnitude of the reverse drive capability can be implemented without changing positioning performance.

[0303] (Fifth embodiment)

[0304] Next, a fifth embodiment of the present invention will be described. In the following description of the fifth embodiment, matters common to the first to fourth embodiments described above are omitted, and matters different from the first to fourth embodiments described above are described.

[0305] In the first to fourth embodiments described above, the observed values ​​are the displacements z of the base and tip sides of the thread holding mechanism. t1 and z t2 However, due to limitations of the mechanism, a displacement sensor capable of realizing the displacement sensor may not be arranged. Therefore, in this embodiment, a control system using other observed quantities is described.

[0306] If the displacement z of the line holding mechanism t2 It is difficult to observe, and the spring displacement k of the tension detection mechanism t2 If it can be observed by a strain gauge, the generated force F can be taken as the observed quantity. In this case, the displacement z of the wire holding mechanism is t2 It is determined by the following equation (36).

[0307] [Mathematical formula 28]

[0308] z t2 =z t1 -F / k t2 (36)

[0309] When the spring displacement of the force detection unit is sufficiently small compared to the moving range of the wire holding mechanism in actual use, the control amount of the outer ring control system (second ring control system) can be set to the displacement z of the base unit of the wire holding mechanism. t1 .

[0310] In addition, if it is difficult to observe the displacement z of the base unit of the line holding mechanism t1 , and the spring coefficient of the coupling and the spring coefficient of the drive shaft in the z direction k g 、k t1 is large enough, the motor's rotation angle θ m can be observed, approximated by the following equation (37), and used as the position control quantity of the outer loop control system (second loop control system).

[0311] [Mathematical formula 29]

[0312]

[0313] (Sixth embodiment)

[0314] Next, a sixth embodiment of the present invention will be described. In the following description of the sixth embodiment, matters common to the first to fifth embodiments described above are omitted, and matters different from the first to fifth embodiments described above are described.

[0315] In the first to fifth embodiments described above, the control system design was performed for the continuum robot 100 driven in a plane. In this embodiment, the control system design is performed for a continuum robot capable of three-dimensional driving.

[0316] Figure 22 1 is a diagram showing a first example of a schematic configuration of a continuum robot 100 according to a sixth embodiment of the present invention. Figure 22 The shown continuum robot 100 is referred to as “continuum robot 100 - 2 ”.

[0317] exist Figure 22 In the continuum robot 100-2 shown, wires 1011 to 1013 are connected to fixed portions 1021 to 1023 at the distal end 1060 of the bendable unit 1100. Wire holding tubes 1031 to 1033 are connected to the proximal ends of the wires 1011 to 1013. Similar to the first embodiment, the robot base unit 1040 is equipped with a wire holding mechanism ( Figure 22 Not shown) and actuator ( Figure 22 The wire holding mechanism is connected to the wire holding mechanism base unit ( Figure 22) are connected to the actuator and are movable up and down. The wire holding tubes 1031 to 1033 are connected to the wire holding mechanism, and the postures are controlled by pushing and pulling the actuator. The continuum robot 100-2 has wire guides 1061 to 1064 as components for guiding the wires 1011 to 1013. In addition to the method of discretely arranging a plurality of components, the wire guide may also be a continuum component such as a bellows component or a mesh component. The wire guides 1061 to 1064 are fixed to the wire 1011 at the fixing units 1050 to 1053. In addition, the distance between the wires 1011 to 1013 and the wire holding tubes 1031 to 1033 may be different. In this case, the diameter conversion unit 1190 may be connected to the robot base unit 1040. In Figure 22 In FIG, the central axis of the continuum robot 100 - 2 is indicated by a dotted line.

[0318] In this embodiment, the mechanism including wires 1011 to 1013 and wire guides 1061 to 1064 is referred to as a bendable unit 1100 as a continuum portion. An actuator unit including a rotary motor and a rotary-linear motion conversion mechanism is used for an actuator (not shown). In addition, the wire holding mechanism has a function of detecting the tension of the wire. For this purpose, the above-mentioned wire holding mechanism base unit is provided between the wire holding mechanism and the actuator, the wire holding mechanism base unit is connected to the actuator, and the wire holding mechanism base unit and the wire holding mechanism are connected by a spring. At this time, it is preferred to provide a linear guide so that the wire holding mechanism is displaced only in the z-axis direction or it is preferred to use a parallel spring as the spring. The tension of the wire can be detected by measuring the displacement of the spring. Figure 22 The definitions of the symbols shown in are as follows.

[0319] l d : The length of the central axis of the bendable unit 1100.

[0320] θ n : The bending angle of the distal end of the bendable unit 1100.

[0321] ζ n : The rotation angle of the distal end of the bendable unit 1100.

[0322] ρ n : The curvature radius of the bendable unit 1100.

[0323] In this embodiment, the lines 1011 to 1013 are referred to as line “a”, line “b”, and line “c” in the counterclockwise direction in the xy plane, and the driving displacement of the line “a” 1011 to the line “c” 1013 is referred to as “l p1a ", "l p1b ” and “lp1c ”.

[0324] Figure 23 : is a diagram showing an example of arrangement of lines of the continuum robot 100 according to the sixth embodiment of the present invention. Figure 23 As shown, lines "a" 1011 to "c" 1013 are arranged at a length of r s The vertices of the equilateral triangle, and the phase angle ξ n is an angle used to determine the arrangement of the lines. In this embodiment, ξ1=0.

[0325] In this embodiment, the kinematics of the continuum robot 100 - 2 is derived by the following assumptions.

[0326] [1] In each bendable unit 110, the wire is deformed into a constant curvature.

[0327] [2] Torsional deformation of the wire is not considered.

[0328] [3] The wire has no deformation in the longitudinal direction.

[0329] [4] The friction between the wire guide and the wire is not considered.

[0330] As a result, the driving displacement l of the line “a” for setting the bending angle θ1 and the rotation angle ζ1 of the distal end of the bendable unit 110 is p1a , the driving displacement l of line "b" p1b and the driving displacement l of line "c" p1c It is expressed by the following equation (38).

[0331] [Mathematical formula 30]

[0332]

[0333] In this embodiment, distributed control to which the dual-loop control of the first to fifth embodiments is applied is independently performed for each of the lines “a” 1011 to “c” 1013 .

[0334] Figure 24 The sixth embodiment of the present invention includes Figure 22 FIG. 1 is a diagram showing an example of a schematic configuration of a control system 10 for a continuum robot 100-2. Figure 24 The control system 10 of the continuum robot is described as "control system 10-5 of the continuum robot". Figure 24 In, with Figure 1 Components similar to those shown in FIG. 1 are denoted by the same reference numerals, and detailed descriptions thereof will be omitted.

[0335] exist Figure 24In the embodiment, the target bending angle refθ1 and the target rotation angle refζ1 at the distal end of the bendable unit 110 are input from the input device 510 to the kinematics calculation unit 210. The target displacement refθ1 of each wire holding mechanism is obtained by substituting the target bending angle refθ1 and the target rotation angle refζ1 into the bending angle θ1 and the rotation angle ζ1 of equation (38), respectively. za ,ref zb and ref zc Dynamic Model P na Equivalent to the dynamic model P of the first embodiment n The point is that the wire "a" is driven by fixing the wire holding tubes of the wires "b" and "c" to the actuator base unit. Similarly, the dynamic model P nb Equivalent to the dynamic model P of the first embodiment n The wire holding tubes of wires "a" and "c" are fixed to the actuator base unit and wire "b" is driven. Similarly, the dynamic model P of the first embodiment is obtained by fixing the wire holding tubes of wires "a" and "b" to the actuator base unit and driving wire "c". n Equivalent dynamic model P nc . K Fa , K Fb and K Fc The force control units can be designed individually or in particular in the dynamic model P na 、P nb and P nc The control system can be the same without any difference. SVa , K SVb and K SVc The position control units can be designed individually or in particular in the case of dynamic models P na 、P nb and P nc The control systems can be identical without any differences.

[0336] Coefficient α a , α b and α c is a coefficient for changing the reverse driving capability shown in the fourth embodiment. Figure 24 In the equation, the coefficient α a Input from input device 520 to force control unit K Fa etc., the coefficient α b Input from input device 530 to force control unit K Fb etc., and the coefficient α c Input from input device 540 to force control unit KFc In this case, if the coefficient α a , α b and α c If the coefficient α is set to different values, the reverse driving ability can be changed according to the direction of rotation. a , α b and α c If both are set to the same value, uniform reverse drive capability can be obtained regardless of the rotation direction.

[0337] If the reverse driving capability of one of the three wires is significantly reduced, the reverse driving capability is reduced only for disturbances in the central axis direction of the continuum robot 100-2, and reverse driving capability can be provided for disturbance torques in the bending direction. This can prevent the bendable unit 1100 from colliding with the base unit, and conversely, can prevent the bendable unit 1100 from expanding in the positive z-axis direction and causing the wire holding mechanism to collide with the base unit.

[0338] Furthermore, in order to set the back driving capability to 0 for disturbance in the center axis direction of the continuum robot 100 - 2 and provide the back driving capability to disturbance in the bending direction, a wire or a wire holding tube may be fixed to the actuator base unit.

[0339] Figure 25 1 is a diagram showing a second example of a schematic configuration of a continuum robot 100 according to a sixth embodiment of the present invention. Figure 25 The continuum robot 100 shown is described as "continuum robot 100-3". Figure 25 In, with Figure 22 Components similar to those shown are denoted by the same reference numerals. Figure 25 An example of fixing the wire "a" 1011 to the fixing unit 1191 is shown. At this time, the driving displacement l of the wire "b" for setting the bending angle θ1 and the rotation angle ζ1 of the distal end of the bendable unit 1100 is p1b and the driving displacement l of line "c" p1c They are expressed by the following equations (39) and (40), respectively.

[0340] [Mathematical formula 31]

[0341]

[0342]

[0343] Figure 26 The sixth embodiment of the present invention includes Figure 25 FIG. 1 is a diagram showing an example of a schematic configuration of a control system 10 for a continuum robot 100 - 3 . Figure 26 The target displacement ref of each wire holding mechanism is obtained by substituting the target bending angle refθ1 and the target rotation angle refζ1 into the bending angle θ1 and the rotation angle ζ1 in equations (39) and (40). zb and ref zc .

[0344] (Seventh embodiment)

[0345] Next, a seventh embodiment of the present invention will be described. In the following description of the seventh embodiment, matters common to the first to sixth embodiments described above are omitted, and matters different from the first to sixth embodiments described above are described.

[0346] In the first to sixth embodiments described above, control for improving the back-driving capability of a continuum robot 100 having a single bendable unit is described. In this embodiment, a control system design method for a continuum robot having multiple bendable units is described.

[0347] Figure 27 1 is a diagram showing a second example of a schematic configuration of a continuum robot 100 according to a seventh embodiment of the present invention. Figure 27 The continuum robot 100 shown is described as "continuum robot 100-4". Figure 27 , three bendable units 1100 - 1 to 1100 - 3 are shown.

[0348] Here, the driving displacement of the line driving the nth bendable unit is set to l pna 、l pnb and l pnc , and determining the driving displacement l of the line “a”, the line “b” and the line “c” of the continuum robot 100 - 4 having a plurality of bendable units 1100 pna 、l pnb and l pnc The bending angle θ with the distal end of the nth bendable unit n and the rotation angle ζ n The number of bendable elements is represented by "e", and the phase angle of the line driving the n-th bendable element is expressed by the following equation (41).

[0349] [Mathematical formula 32]

[0350]

[0351] As a result, the driven displacement l of the line of the nth bendable unit pna 、l pnb and l pnc It is expressed by the following equation (42).

[0352] [Mathematical formula 33]

[0353]

[0354] As in the sixth embodiment, each line is driven by a dual-loop control system. In addition, in order to prevent collisions caused by reverse driving between the bendable units, as in the sixth embodiment, for example, the line "a" of each bendable unit is fixed and the reverse driving capability in the central axis direction of the continuum robot 100-4 is set to 0. At this time, the driving displacement l of the line "b" is pnb and the driving displacement l of line "c" pnc They are expressed by the following equations (43) and (44), respectively.

[0355] [Mathematical formula 34]

[0356]

[0357]

[0358] In the seventh embodiment, as in the sixth embodiment described above, the wires "b" and "c" of each bendable unit 1100 are driven by a dual-loop control system, obtaining reverse driving capability only in the bending direction.

[0359] [Other embodiments]

[0360] The present invention can also be implemented by providing a program that implements one or more functions of the above-mentioned embodiments to a system or device via a network or storage medium, wherein one or more processors in a computer of the system or device read and execute the program. The present invention can also be implemented by a circuit (such as an ASIC) that implements one or more functions. The program and the computer-readable storage medium storing the program are included in the present invention. As another embodiment of the present invention, a construction in which the functional components of the control device 200 of the continuum robot are incorporated into the continuum robot 100 is also applicable to the present invention.

[0361] It should be noted that the above-mentioned embodiments of the present invention are merely examples for implementing the present invention and should not be interpreted in a limiting manner as to the technical scope of the present invention. That is, the present invention can be implemented in various ways without departing from its technical concept or key features.

[0362] The present invention is not limited to the above embodiments, and various modifications and changes can be made without departing from the spirit and scope of the present invention. Therefore, in order to disclose the scope of the present invention, the following claims are attached.

[0363] This application claims priority based on Japanese Patent Application No. 2020-028813, filed on February 21, 2020, the entire contents of which are incorporated herein by reference.

[0364] [Reference Signs List]

[0365] 10: Control system for continuum robot; 100: Continuum robot; 200: Control device for continuum robot; 210: Kinematics calculation unit; K SV : Position control unit; K F : force control unit; P n :Dynamic model of continuum robot; 300:Input device.

Claims

1. A control system for controlling a continuum robot, the continuum robot comprising at least one bendable unit driven by a wire and configured to be bendable, a drive unit for driving the wire, and a wire holding mechanism, the control system comprising: a position control unit configured to perform control so as to compensate for an error between a target displacement of the wire by the push-pull drive of the drive unit and a displacement of the wire holding mechanism obtained from the continuum robot; a force control unit configured to perform control so as to compensate for an error between a target tension of the wire and a tension of the wire obtained from the continuum robot; a first ring control system comprising a force control unit; as well as The second ring control system includes a force control unit and a position control unit, wherein, in the second loop control system, based on an error between the target displacement and the displacement of the wire holding mechanism, the position control unit outputs a target tension of the wire to the force control unit, and In the first loop control system, the force control unit outputs a target torque of the driving unit based on an error between the target tension of the wire and the tension of the wire.

2. The control system for controlling a continuum robot according to claim 1, further comprising a kinematics calculation unit configured to perform kinematics calculation based on an input of a target bending angle of the bendable unit to output a target displacement.

3. The control system for controlling a continuum robot according to claim 1 or 2, in, A first gain is calculated by multiplying a gain of the force control unit by a first coefficient, and a second gain is calculated by multiplying a gain of the position control unit by a second coefficient, and The second gain is calculated based on the first gain.

4. The control system for controlling a continuum robot according to claim 3, in, The first coefficient varies according to a target bending angle of the bendable unit.

5. The control system for controlling a continuum robot according to claim 1 or 2, in, The continuum robot includes a plurality of wires in one of the bendable units and a plurality of driving units driving each of the plurality of wires, and The first ring control system and the second ring control system are configured to correspond to each of the plurality of drive units.

6. The control system for controlling a continuum robot according to claim 5, in, When the first gain is obtained by multiplying the gain of the force control unit by the first coefficient, the first coefficient is different for each driving unit.

7. The control system for controlling a continuum robot according to claim 5, in, One of the plurality of wires in one of the bendable units is fixed to a base unit on the continuum robot.

8. The control system for controlling a continuum robot according to claim 1 or 2, in, The force control unit repeatedly calculates a transfer function based on the motion equation of the continuum robot and the open-loop transfer function of the force control unit and calculates a stability margin from the open-loop transfer function to determine the gain of the force control unit.

9. The control system for controlling a continuum robot according to claim 1 or 2, in, The force control unit includes a PID controller and a low-pass filter coupled in series.

10. The control system for controlling a continuum robot according to claim 9, in, The differential gain of the PID controller is determined by repeatedly performing a process of setting the differential gain and a process of calculating an open-loop transfer function using a transfer function based on the motion equations of the force control unit and the continuum robot.

11. The control system for controlling a continuum robot according to claim 1 or 2, in, The force control unit includes a PID controller, a phase advance filter and a low-pass filter coupled in series.

12. The control system for controlling a continuum robot according to claim 11, in, The phase advance filter is determined by repeatedly performing a process of setting an interruption frequency and a process of calculating an open-loop transfer function using a transfer function based on a motion equation of the force control unit and the continuum robot.

13. A continuum robot comprising: Wire; at least one bendable unit configured to be bendable; as well as The driving unit drives the line. Wherein, the continuum robot comprises the control system according to claim 1 or 2.

14. A method for controlling a continuum robot, the continuum robot comprising at least one bendable unit driven by a wire and configured to be bendable, a driving unit driving the wire, and a wire holding mechanism, the method comprising: a first loop control step, which includes a force control step; as well as A second loop control step, which includes the force control step and the position control step, The position control step is used to compensate for the error between the target displacement of the push-pull drive of the drive unit on the wire and the displacement of the wire holding mechanism obtained from the continuum robot, so as to output the target tension of the wire. wherein the force control step is used to compensate for an error between the target tension of the wire output in the position control step and the tension of the wire obtained from the continuum robot, so as to output a target torque of the driving unit, wherein, in the first loop control step, the force control step outputs a target torque of the driving unit based on an error between the target tension of the wire and the tension of the wire, and Here, in the second loop control step, the position control step outputs a target tension of the wire to the force control step based on an error between a target displacement and a displacement of the wire holding mechanism.

Images