Control system for continuum robot and control method therefor

The control system for continuum robots with multiple curved sections uses wire driving means and coupled control actuators/dampers to stabilize the robot's operation, ensuring high stability and accurate positioning.

JP2024168760APending Publication Date: 2024-12-05CANON KK
View PDF 1 Cites 0 Cited by

Patent Information

Application Number
JP2023085690
Authority / Receiving Office
JP · JP
Patent Type
Applications
Current Assignee / Owner
Filing Date
2023-05-24
Publication Date
2024-12-05

AI Technical Summary

Technical Problem

Existing continuum robots with multiple curved sections experience instability in their control systems due to coupling between sections, which is not adequately addressed by existing models.

Method used

A control system for continuum robots with multiple curved sections that includes wire driving means for each section, coupled control actuators or dampers to suppress tension fluctuations, and a double-loop control system to maintain stability and backdrivability.

Benefits of technology

The system effectively suppresses instability and maintains high stability and positioning accuracy in continuum robots with multiple curved sections by addressing coupling issues.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure 2024168760000001_ABST
    Figure 2024168760000001_ABST
Patent Text Reader

Abstract

To provide a technique capable of suppressing deterioration in stability of a control system of a continuum robot.SOLUTION: Provided is a control system 10 of a continuum robot 100 comprising: a curvable part provided with a plurality of curved sections driven by a wire; and wire drive means for driving the wire for each curved section in the plurality of curved sections. The control system has a position control part KSV, a generated force calculation part -kt2 and a force control part KF which are control means for controlling operation of the wire drive means for each curved section in the plurality of curved sections, and a coupled control part KD which is one component of suppression means for suppressing fluctuations in the tension of the wire in other curved sections when operating the wire drive means in the plurality of curved sections.SELECTED DRAWING: Figure 2
Need to check novelty before this filing date? Find Prior Art

Description

[Technical field]

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

[0002] Continuum robots, also called continuum robots, have a bendable part consisting of multiple curved sections with a flexible structure, and their shape is controlled by deforming the bendable part. This continuum robot has two main advantages over robots composed of rigid links (hereinafter referred to as "rigid link robots"). The first advantage is that continuum robots can move along the curves of objects in narrow spaces or cluttered environments where rigid link robots can get stuck. The second advantage is that continuum robots are inherently soft, so they can operate fragile objects without damaging them. In this case, it is not necessarily necessary to detect external forces, which is required for rigid link robots.

[0003] Taking advantage of this characteristic, continuum robots are expected to be applied in the medical field, such as in endoscopic sheaths and catheters, and in robots for extreme tasks, such as rescue robots. Furthermore, when a drive unit operates passively according to the amount of load, it is said to have backdrivability, and there are continuum robots that have high backdrivability due to their mechanisms and control algorithms.

[0004] In Patent Document 1, a dynamic model of a continuum robot including a wire driving unit capable of detecting tension of a wire that bends a bendable portion by driving the wire is derived. Patent Document 1 also realizes a double-loop control system in which an inner-loop control system performs force control on the continuum robot and an outer-loop control system performs position control on the continuum robot. As a result, Patent Document 1 realizes a continuum robot that has high backdrivability against disturbances at the tip of the continuum robot and is capable of positioning to a target position. [Prior art documents] [Patent documents]

[0005] [Patent Document 1] JP 2021-133429 A Summary of the Invention [Problem to be solved by the invention]

[0006] In Patent Document 1, the stability of the feedback system is guaranteed in the force control system and position control system by using a transfer function obtained from a dynamic model of the continuum robot. In this case, Patent Document 1 models a continuum robot having a single curved section. When Patent Document 1 is applied to a continuum robot having multiple curved sections, a distributed backdrivable control system is used in which the above-mentioned control system is applied to each curved section.

[0007] However, a continuum robot having a bendable section consisting of multiple bendable sections has coupling between the bendable sections, in which the movement of one bendable section affects the movement of the other bendable sections. In Patent Document 1, the coupling between the bendable sections is not taken into consideration, and therefore there is a problem that the stability of the control system of the continuum robot decreases due to changes in the shape of the continuum robot or changes in the mechanism such as an increase or decrease in friction elements.

[0008] The present invention has been made in consideration of such problems, and has an object to provide a technique capable of suppressing a decrease in the stability of a control system of a continuum robot. [Means for solving the problem]

[0009] The control system for a continuous body robot of the present invention is a control system for a continuous body robot comprising a bendable section having a plurality of curved sections driven by a wire, and a wire driving means for driving the wire for each curved section among the plurality of curved sections, and has a control means for controlling the operation of the wire driving means for each curved section among the plurality of curved sections, and a suppression means for suppressing fluctuations in the tension of the wire in other curved sections when the wire driving means is operated in a curved section among the plurality of curved sections. Effect of the Invention

[0010] According to the present invention, it is possible to suppress a decrease in stability of a control system of a continuum robot. [Brief description of the drawings]

[0011] [Figure 1] 1 is a diagram illustrating an example of a schematic configuration of a continuum robot according to a first embodiment. [Diagram 2] 1 is a diagram illustrating an example of a schematic configuration of a control system for a continuum robot according to a first embodiment. [Diagram 3] FIG. 11 is a diagram illustrating an example of a schematic configuration of a continuum robot according to a second embodiment. [Figure 4] FIG. 11 is a diagram illustrating an example of a schematic configuration of a continuum robot according to a third embodiment. [Diagram 5] FIG. 11 is a diagram showing an example of a dynamic model of a continuum robot (continuum part) according to the third embodiment. [Figure 6-1] FIG. 11 is a diagram showing an example of a model including a rotary motor, a rotary-to-linear motion conversion mechanism, and a wire gripping mechanism in a continuum robot according to a third embodiment. [Figure 6-2] This figure shows an example configuration of a continuum robot according to the third embodiment, combining a dynamic model of the continuum part shown in Figure 5 with a model consisting of a rotary motor, a rotary-linear motion conversion mechanism, and a wire gripping mechanism shown in Figure 6-1. [Figure 7] FIG. 13 is a diagram showing an example of a frequency response when cw21=cw22=0 Ns / m in the continuum robot according to the third embodiment. [Figure 8] FIG. 13 is a diagram showing an example of a frequency response when cw21=cw22=150 Ns / m in the continuum robot according to the third embodiment. [Figure 9] FIG. 13 is a Bode plot of a force control unit when Kp is 2.2·10−3, Fzi is 1.0, and the control band is approximately 10 Hz in the control system for a continuum robot according to the fourth embodiment. [Figure 10] FIG. 13 is a plot of changes in eigenvalues ​​of a closed loop system in a control system for a continuum robot according to the fourth embodiment. [Figure 11] FIG. 13 is a diagram showing an example of the frequency response of the continuum robot when the suboptimal damping coefficient of the coupled control damper is cw21=cw22=25 Ns / m in the control system of the continuum robot according to the fourth embodiment. [Figure 12] FIG. 13 is a Bode plot of a position control unit when Kp is 5·103, Fzi is 0.5, and Fzd is 100 in the control system for a continuum robot according to the fourth embodiment. [Figure 13] FIG. 13 is a diagram showing a simulation response in a control system for a continuum robot according to a fourth embodiment, in which a suboptimal model is shown by a solid line, a low damping model is shown by a dashed line, and a high damping model is shown by a dashed and dotted line. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0012] DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Hereinafter, an embodiment of the present invention will be described with reference to the drawings.

[0013] (First embodiment) First, the first embodiment will be described.

[0014] [1] Configuration of continuum robots Fig. 1 is a diagram showing an example of a schematic configuration of a continuum robot 100 according to a first embodiment. In the following description, the continuum robot 100 according to the first embodiment shown in Fig. 1 will be referred to as "continuum robot 100-1."

[0015] As shown in FIG. 1, the continuum robot 100-1 includes a base unit 140 and a bendable unit 170. The bendable unit 170 is a component (a component provided with a plurality of bendable units) provided with a plurality of bendable sections 171-172 that are bent by driving the wires. The base unit 140 is a component that supports the bendable unit 170, and is provided therein with actuators 131 and 132 that are wire driving means for driving the wires 111-112 of the bendable units 171-172. Note that FIG. 1 shows an example in which two bendable units, a first bendable unit 171 and a second bendable unit 172, are configured as the plurality of bendable units provided in the bendable unit 170, but this is not limited to this in the present embodiment. For example, a configuration in which three or more bendable units are configured as the plurality of bendable units provided in the bendable unit 170 can also be applied to the present embodiment.

[0016] 1, an origin O is set at a predetermined position on the upper surface 141 of the base unit 140. In addition, in the continuum robot 100-1, an xz coordinate system is illustrated in which the forward direction is set as the z direction of the coordinate system and a predetermined direction perpendicular to the z direction is set as the x direction of the coordinate system.

[0017] The wire 111 is connected to a connection portion 121 at a distal end 163 of the first curved section 171. The posture (curved shape) of the continuum robot 100-1 is controlled by pushing and pulling the wire 111 with an actuator 131 installed inside the base portion 140. Here, the actuator 131 is a wire driving means for driving the wire 111.

[0018] The wire 112 is connected to a connection portion 122 at a distal end 160 of the second curved section 172. The posture (curved shape) of the continuum robot 100-1 is controlled by pushing and pulling the wire 112 with an actuator 132 installed inside the base portion 140. Here, the actuator 132 is a wire driving means for driving the wire 112.

[0019] The continuum robot 100-1 has wire guides 164 and 165 which are members for guiding the wire 111 and the wire 112 in the first curved section 171. The continuum robot 100-1 also has wire guides 161 and 162 which are members for guiding the wire 112 in the second curved section 172. The distal end and wire guides 160-165 are fixed to the spine 113. Note that the continuum robot 100-1 is configured such that a plurality of members made up of the distal end and wire guides 160-165 are discretely arranged, but other continuum members such as bellows-shaped or mesh-shaped members may also be used.

[0020] In the internal mechanism of base part 140 in first curved section 171 driven by wire 111, wire gripper 212 that grips wire 111 and spring 211 are interposed between wire 111 and actuator 131. Similarly, in the internal mechanism of base part 140 in second curved section 172 driven by wire 112, wire gripper 222 that grips wire 112 and spring 221 are interposed between wire 112 and actuator 132.

[0021] Displacement z of the actuator 131 in the first curved section 171 t11 , and the displacement z of the wire gripping portion 212 t21 can be detected by an encoder or the like. These displacements and the spring constant k t2 Based on this, the wire tension of the wire 111 that drives (bends) the first bending section 171 can be detected. In this case, the spring 211 is one configuration of a tension detection means for detecting the tension of the wire 111. Similarly, the displacement z t12 , and the displacement z of the wire gripping portion 222 t22 and the spring constant k of spring 221 t2 Based on this, it is possible to detect the wire tension of the wire 112 that drives (bends) the second bending section 172. In this case, the spring 221 is one component of the tension detection means for detecting the tension of the wire 112.

[0022] Furthermore, in this embodiment, in the internal mechanism of the base unit 140 in the first curved section 171 driven by the wire 111, a coupling control actuator 213 is connected to the wire gripper 212. In this coupling control actuator 213, the surface opposite to the wire 111 side is connected to the base unit 140. Similarly, in this embodiment, in the internal mechanism of the base unit 140 in the second curved section 172 driven by the wire 112, a coupling control actuator 223 is connected to the wire gripper 222. In this coupling control actuator 223, the surface opposite to the wire 112 side is connected to the base unit 140.

[0023] Next, the connection between the first curved section 171 and the second curved section 172 will be described. For example, as shown in FIG. 1, when the actuator 132 operates in the forward direction, the second curved section 172 is curved, but since a torque is applied to the distal end 163 of the first curved section 171 via a member such as the spine 113, the first curved section 171 is curved at the same time. If the position control is applied to each of the first curved section 171 and the second curved section 172, they can be curved into any shape, but a part of the wire tension of each curved section is superimposed on the wire tension of the wire 111 and the wire 112. That is, when the actuator 132 is operated, the tension of the wire 111 changes in addition to the tension of the wire 112. Similarly, when the actuator 131 is operated, the tension of the wire 112 changes in addition to the tension of the wire 111. In this embodiment, this is defined as coupling between the curved sections. When a control system is designed by considering the wire tension of a single curved section without considering the coupling between the curved sections, as in Patent Document 1, and this is applied to a continuum robot having multiple curved sections, there is a problem that the stability of the control system changes (decreases). In particular, when the frictional force applied to the driving wire becomes small, the coupling between the curved sections increases, and the control system becomes unstable. In this embodiment, the change in the tension of the wire 111 when the actuator 131 is driven and the change in the tension of the wire 112 when the actuator 132 is driven are described as direct characteristics.

[0024] [2] Control system configuration Fig. 2 is a diagram showing an example of a schematic configuration of a control system 10 for a continuum robot according to the first embodiment. As shown in Fig. 2(a), the control system 10 for a continuum robot includes a mechanical part P (hereinafter simply referred to as "continuum robot 100") of the continuum robot 100-1 shown in Fig. 1, a control device 200 for the continuum robot, and an input device 300.

[0025] As shown in FIG. 2(a), the control device 200 for the continuum robot includes a kinematics calculation unit (Kinematics) 210 and controllers 220-1 to 220-n. Here, the controller 220-1 is, for example, a controller corresponding to the first curved section 171 shown in FIG. 1. The controller 220-2 is, for example, a controller corresponding to the second curved section 172 shown in FIG. 1. Furthermore, when three or more curved sections are configured in the continuum robot 100, 220-n is a controller corresponding to a certain curved section among the three or more curved sections. Furthermore, for example, the controller 220-n shown in FIG. 2(a) includes a position control unit K SV , force control part K F , generated force calculation unit-k t2 , and coupling control unit K D 2(b) shows the internal configuration of the controller 220-n shown in FIG. 2(a), but the controllers 220-1 and 220-2 shown in FIG. 2(a) also have the same internal configuration as the controller 220-n shown in FIG. 2(b). In this case, in the case of the internal configurations of the controllers 220-1 and 220-2 shown in FIG. 2(a), the symbol "n" in the controller 220-n shown in FIG. 2(b) is changed to the symbol "1" and the symbol "2", respectively. Hereinafter, when the matters common to the controllers 220-1 to 220-n are described without specifying the controllers 220-1 to 220-n, they will be simply described as "controller 220".

[0026] In this embodiment, the controller 220 shown in FIG. 2B includes a force control unit K FThe force control for the continuum robot 100 is performed by an inner loop control system (first loop control system) including the force control unit K F and position control unit K SV The position of the continuum robot 100 is controlled by an outer loop control system (second loop control system) including the inner loop control system (first loop control system) and the outer loop control system (second loop control system). The control system 10 of the continuum robot according to this embodiment is a double loop control system including the inner loop control system (first loop control system) and the outer loop control system (second loop control system). Due to this double loop control system, the control system 10 of the continuum robot according to this embodiment has high backdrivability for the continuum robot 100, and at the same time, achieves high positioning performance of the bendable section 170 of the continuum robot 100 to a target position.

[0027] Furthermore, in this embodiment, in the controller 220 shown in FIG. 2B, in order to improve the stability of the control system due to the coupling between the curved sections described above (suppress the decrease in stability), a coupling control unit K D For example, the coupling control unit K D In order to suppress the deterioration of the stability of the control system due to the coupling between the curved sections, a control input is provided to the coupling control actuator 213. In addition, for example, the coupling control unit K D gives a control input to the coupling control actuator 223 in order to suppress a decrease in stability of the control system due to the coupling between the curved sections described above.

[0028] Next, a specific signal flow in the control system 10 for the continuum robot will be described. The kinematics calculation unit 210 shown in FIG. 2A receives a target bending angle ref θ1 ~ref θn When the input is received, the kinematics calculation is performed. Then, the kinematics calculation unit 210 calculates the target displacement ref z1 ~ref znto the corresponding controllers 220-1 to 220-n.

[0029] The position control unit K shown in FIG. SV is the target displacement ref output from the kinematics calculation unit 210 z and the actuator displacement z t1 The control is performed to compensate for the error between the target tension of the wire and the target generated force ref F Here, the target generated force ref F represents the target value of the generated force F, which is the positive / negative inverted value of the wire tension sensor defined in this embodiment. t2 is the displacement of the wire gripper z t2 and the actuator displacement z t1 The difference between these and the spring constant k t2 The force control unit K shown in FIG. 2(b) outputs the generated force F corresponding to the tension of the wire based on the force F. F is the position control unit K SV The target force ref output from F and the generated force calculation unit-k t2 The control is performed to compensate for the error between the generated force F output from the wire drive means and the target torque u of the actuator. c Output.

[0030] The feedback loop control system in the control device 200 of the continuum robot shown in Figure 2 (a) is equivalent to compensating for the equivalent inertia of the rotary-linear conversion mechanism, thereby making it possible to improve the backdrivability of the continuum robot 100.

[0031] Furthermore, in this embodiment, the controller 220 shown in FIG. 2B controls the displacement z of the wire gripping part independently of the above-mentioned first and second loop control systems. t2 A coupled control unit K controls the operation of the coupled control actuator based on D In the third loop control system, a coupling control unit K D is the displacement of the wire gripper z t2 is used as the input signal, and the control input ud By applying t2 For example, the third loop control system is a coupled control unit K D When direct velocity control using as a differentiator is used, a direct velocity feedback control system is configured. In this case, the coupled control actuators 213 and 223 act as dampers, suppressing the fluctuation in the tension of the coupled wire 111 when the actuator 132 is operated, and suppressing the fluctuation in the tension of the coupled wire 112 when the actuator 131 is operated. In this way, by providing a third loop control system in the controller 220, coupling between the curved sections can be suppressed, and appropriate first and second loop control systems can be designed taking into account only the direct characteristics of each curved section. Also, the coupling control unit K D may be a control system in which a low-pass filter is connected in series to a differentiator, or may be a PD control system or a PID control system. D The first and second loop control systems may be designed by evaluating the stability of the control system. For the evaluation, a numerical model of the continuum robot 100 may be used, or an experimental system may be directly measured.

[0032] The control system 10 of the continuum robot according to the first embodiment described above has the following configuration. First, the continuum robot 100 includes a bendable section 170 having a plurality of bending sections 171 and 172, and actuators 131 and 132, which are wire driving means for driving the wires 111 and 112, for each bending section of the plurality of bending sections. In addition, the control system 10 includes a position control unit K, which is a control means for controlling the operation of the actuators 131 and 132, which are wire driving means, for each bending section of the plurality of bending sections. SV , generated force calculation unit-k t2 and force control section K FFurthermore, for each of the curved sections in the plurality of curved sections, there is provided suppression means for suppressing fluctuations in tension of the wires 112 or 111 in the other curved sections when the actuator 131 or 132, which is the wire driving means in that curved section, is operated. Specifically, the suppression means in the first curved section 171 includes a coupling control actuator 213, which is the second wire driving means, and a coupling control unit K in the first curved section 171, which is the second control means for controlling the operation of the coupling control actuator 213. D In this case, the coupling control actuator 213 in the first curved section 171 is connected to a wire gripping part 212 that grips the wire 111 in order to drive the wire 111 without the intervention of a spring 211, which is a tension detection means. In addition, the coupling control part K in the first curved section 171 D is the displacement z of the wire gripping portion 212, which is the wire gripping means. t21 The operation of the coupling control actuator 213 is controlled based on the coupling control actuator 223. Similarly, for example, the suppression means in the second bending section 172 is a means for suppressing fluctuations in the tension of the wire 111 in the other bending section 171 when the actuator 132, which is the wire driving means, is operated. Specifically, the suppression means in the second bending section 172 is a coupling control actuator 223, which is the second wire driving means, and a coupling control unit K in the second bending section 172, which is the second control means for controlling the operation of the coupling control actuator 223. D In this case, the coupling control actuator 223 in the second curved section 172 is connected to a wire gripping unit 212 that grips the wire 112 so as to drive the wire 112 without the intervention of a spring 221, which is a tension detection means. D is the displacement z of the wire gripping portion 222, which is the wire gripping means. t22 Based on this, the operation of the coupling control actuator 223 is controlled. According to this configuration, since the suppression means is provided, it is possible to reduce the influence of the operation of a certain curved section on the operation of other curved sections, and to suppress coupling between the curved sections. Therefore, in the continuum robot 100 having multiple curved sections, it is possible to suppress a decrease in stability of the control means (control system) that controls the operation of each curved section in the multiple curved sections. In this embodiment, the coupling control unit K D The control of the displacement z is based t21 and displacement z t22 is defined as the displacement of wire gripping portion 212 and wire gripping portion 222. However, it is not limited to the wire gripping means as long as it is a portion between the connection portion of coupling control actuator 213 and spring 211 which is the tension detection means, and a portion between the connection portion of coupling control actuator 223 and spring 221 which is the tension detection means. Also, although an example has been given in which the coupled control actuator is connected to the wire gripping means, the object to which the coupled control actuator is connected may be replaced with the wire gripping means so long as it is a means for connecting the wire and the tension detection means.

[0033] Second embodiment Next, a second embodiment will be described. In the following description of the second embodiment, matters common to the first embodiment will be omitted, and only matters different from the first embodiment will be described.

[0034] In the control system 10 for the continuous body robot according to the first embodiment described above, as suppression means for suppressing coupling between curved sections, coupling control actuators 213 and 223 which are second wire driving means, and a coupling control unit K which is a second control means are used. D In contrast to this, in the control system 10 of the continuum robot according to the second embodiment, a coupling control damper connected to the wire gripping parts 212 and 222 is provided in the continuum robot 100 as a suppression means for suppressing the coupling between the curved sections.

[0035] Fig. 3 is a diagram showing an example of a schematic configuration of the continuum robot 100 according to the second embodiment. In the following description, the continuum robot 100 according to the second embodiment shown in Fig. 3 will be described as "continuum robot 100-2". In Fig. 3, the same components as those shown in Fig. 1 are denoted by the same reference numerals, and detailed description thereof will be omitted.

[0036] In the continuum robot 100-2 shown in FIG. 3, a coupled control damper 513 connected to the wire gripping unit 212 is provided as a suppression means in the first curved section 171. The side of the coupled control damper 513 opposite the wire gripping unit 212 is connected to the base unit 140. In addition, in the continuum robot 100-2 shown in FIG. 3, a coupled control damper 523 connected to the wire gripping unit 222 is provided as a suppression means in the second curved section 172. The side of the coupled control damper 523 opposite the wire gripping unit 222 is connected to the base unit 140. These coupled control dampers 513 and 523 are the same as those of the coupled control unit K in the first embodiment. D In this case, the damping coefficients of the coupled control dampers 513 and 523 may be set by direct measurement, or may be set by using a numerical model (dynamic model) of the continuum robot 100-2.

[0037] In the second embodiment, as in the first embodiment, in a continuum robot 100 having multiple curved sections, it is possible to suppress a decrease in the stability of the control means (control system) that controls the operation of each curved section among the multiple curved sections.

[0038] (Third embodiment) Next, a third embodiment will be described. In the following description of the third embodiment, the description of the matters common to the first and second embodiments will be omitted, and only the matters different from the first and second embodiments will be described.

[0039] In the control system 10 of the continuous body robot according to the first embodiment described above, the coupled control actuators 213 and 223 are provided in the wire gripping parts 212 and 222, respectively. In the control system 10 of the continuous body robot according to the second embodiment described above, the coupled control dampers 513 and 523 are provided in the wire gripping parts 212 and 222, respectively. In contrast to this, in the control system 10 of the continuous body robot according to the third embodiment, the coupled control actuators or coupled control dampers are provided in the wires.

[0040] Fig. 4 is a diagram showing an example of a schematic configuration of a continuum robot 100 according to a third embodiment. In the following description, the continuum robot 100 according to a first example of the third embodiment shown in Fig. 4(a) is described as a "continuum robot 100-31", and the continuum robot 100 according to a second example of the third embodiment shown in Fig. 4(b) is described as a "continuum robot 100-32". In Fig. 4, the same components as those shown in Figs. 1 and 3 are denoted by the same reference numerals, and detailed description thereof will be omitted.

[0041] In the continuum robot 100-31 shown in Fig. 4(a), a coupled control actuator 713 is provided on the wire 111 in the first curved section 171, and a coupled control actuator 723 is provided on the wire 112 in the second curved section 172. In this robot, the coupled control actuator 713 is provided instead of the coupled control actuator 213 of the continuum robot 100-1 shown in Fig. 1, and the coupled control actuator 723 is provided instead of the coupled control actuator 223 of the continuum robot 100-1 shown in Fig. 1.

[0042] In the continuum robot 100-32 shown in Fig. 4(b), a coupled control damper 813 is provided on the wire 111 in the first curved section 171, and a coupled control damper 823 is provided on the wire 112 in the second curved section 172. This is the same as the continuum robot 100-2 shown in Fig. 3, in which the coupled control damper 813 is provided instead of the coupled control damper 513, and the coupled control damper 823 is provided instead of the coupled control damper 523 of the continuum robot 100-2 shown in Fig. 3.

[0043] As in the first embodiment described above, the coupling control unit K of the coupling control actuators 713 and 723 shown in FIG. D may be designed by evaluating the stability of the first and second loop control systems. For this evaluation, a numerical model of the continuum robot 100 may be used, or an experimental system may be directly measured. In this embodiment, the damping coefficients of the coupled control dampers 813 and 823 shown in FIG. 4(b) are designed using a dynamic model of the continuum robot 100. Details will be described below. In this embodiment, the first curved section 171 and the second curved section 172 are also referred to.

[0044] [1] Modeling 100 continuum robots 5 is a diagram showing an example of a dynamic model of the continuum robot 100 (continuum part) according to the third embodiment. The definitions of symbols in the dynamic model of the continuum robot 100 (continuum part) shown in FIG. 5 are shown below. θ n : Bending angle of continuum robot m sn : Mass of the continuum part k bn : Spring constant for the curved angle of the continuum part z wn : Displacement of equivalent mass of wire z pn : Displacement of wire-held pipe m wn : Mass of wire m pn : Mass of wire-holding pipe k wn ,k w3n : Spring constant of the wire c wn ,c w3n : damping coefficient of the wire c w2n : Damping coefficient of coupled control damper

[0045] Here, the number of curved sections n=2 (the first curved section 171 and the second curved section 172) is set, and an equation of motion for the continuum robot 100 is derived. In this embodiment, the following assumptions are made. <1> Only motion in a two-dimensional plane (xz plane) is considered. <2> The curvature of the continuum portion is assumed to be constant and the spring constant is assumed to be uniform. <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 continuous body part. The lateral vibration and lateral deformation of the wire are not taken into account. <4> The friction between the wire and the wire guide, and between the wire and the diameter changer, is calculated as viscous damping, including nonlinear friction such as Coulomb friction, with a damping coefficient c w2 Can be pushed into.

[0046] First, the kinetic energy of the continuum part is calculated. The first curved section 171 is displaced x g1 , z g1 Taking the displacement x g1 and z g1 are expressed as the following equations (1) and (2), respectively.

number

[0047] Furthermore, since the following formula (3) holds, the above formulas (1) and (2) become the following formulas (4) and (5), respectively.

number

[0048] The second curved section 172 is displaced x on the central axis of the continuum portion. g2 , z g2 Taking the displacement x g2 and z g2 is expressed as the following equation (6).

number

[0049] Furthermore, since the following formula (7) holds, the above formula (6) becomes the following formula (8).

number

[0050] In this embodiment, a zero degree neighborhood linearization model that does not take into account large deformation is obtained. From the above-mentioned formulas (4) and (5), the first curved section 171 is expressed by the following formula (9).

number

[0051] As for the second curved section 172, first, the first curved section 171 is approximated near zero degree from the above-mentioned formula (8), to obtain the following formula (10).

number

[0052] Next, when the second curved section 172 is approximated near zero degree, the following formula (11) is obtained.

number

[0053] As a result, the second curved section 172 satisfies the following equations (12) and (13).

number

[0054] Here, the kinetic energy T of the continuum part near 0 degrees a0 is expressed as the following equation (14).

number

[0055] In addition, the potential energy of the continuum part U a is expressed as the following equation (15).

number

[0056] Next, the kinetic energy T of the wire and the wire-holding pipe w and potential energy U w is the wire drive amount l p1 = r1θ1, l p2 =r2θ2, the following equations (16) and (17) result, respectively.

number

[0057] Consider determining the equation of motion from the Lagrange equation shown in the following equation (18).

number

[0058] Then, the equation of motion becomes the following equation (19).

number

[0059] [2] Model consisting of a rotary motor, rotary-to-linear conversion mechanism, and wire gripping mechanism FIG. 6-1 is a diagram showing an example of a model consisting of a rotary motor, a rotary-linear conversion mechanism, and a wire gripping mechanism in the continuum robot 100 according to the third embodiment. The wire gripping mechanism has a function of detecting the tension of the wire. A wire gripping mechanism base is provided between the wire gripping mechanism and the actuator, the wire gripping mechanism base is connected to the actuator, and the wire gripping mechanism base and the wire gripping mechanism are connected by a spring. The tension of the wire is detected by detecting the displacement of the spring. The symbols are defined below. Note that the subscript n indicating the nth curved section is added to the end of all of the following symbols, but this is omitted in the text. J m : Motor inertia θ m : Motor rotation angle T m : Motor torque command c m : Damping coefficient of the motor shaft k g ,c g : Coupling spring constant, damping coefficient J n : Drive shaft inertia θ n : Rotation angle of the drive shaft c n : damping coefficient of drive shaft p: thread pitch of drive shaft R: Conversion coefficient of the linear-rotation conversion mechanism, which is the equivalent speed increase ratio m t1 : Mass of the wire gripping mechanism base k t1 : Spring constant of the drive shaft in the z direction c t1 : Damping coefficient of the linear slider of the wire gripping mechanism base z t1 : Displacement of the wire gripping mechanism base m t2 : Mass of wire gripping mechanism k t2 ,c t2 : Spring constant of tension detection mechanism z t2 : Displacement of wire gripping mechanism

[0060] The equations of motion are as follows: (20) to (23).

number

[0061] In the above equations (21) and (22), R=p / 2π. l =[θ m , θ n , z t1 , z t2 ] T In matrix notation, this gives the following equation (24).

number

[0062] Using equation (24), a model consisting of a rotary motor for driving the nth curved section, a rotary-to-linear motion conversion mechanism, and a wire gripping mechanism can be expressed as the following equation (25).

number

[0063] [3] Configuration of the magnified system FIG. 6-2 is a diagram showing an example of a configuration in which the dynamic model of the continuum part shown in FIG. 5 is combined with the model including the rotary motor, rotary-to-linear conversion mechanism, and wire gripping mechanism shown in FIG. 6-1 in the continuum robot 100 according to the third embodiment. In the first curved section 171, the displacement z t21 and the displacement of the wire-held pipe z p1 In the second curved section 172, the displacement z t22 and the displacement of the wire-held pipe z p2 As a result, when an expanded system is constructed from the above-mentioned equations (19) and (24), the equation of motion becomes the following equation (26).

number

[0064] Next, the state equations are expressed as the following equations (27) and (28).

number

[0065] In this embodiment, the displacement z of the wire gripping mechanism base t1n and the displacement z of the wire gripping mechanism t2n The spring constant k can be measured. t2n The force generated by the wire gripping mechanism (hereinafter referred to as the "generated force") F n and the displacement z of the wire gripping mechanism t2n is the observable quantity. Here, the generated force F n is expressed as the following equation (29). F n=-k t2n (z t2n -z t1n ) …(29)

[0066] In this case, the observable y g is expressed by the output equation shown in the following equation (30).

number

[0067] [4] Simulation Using the model of the continuum robot 100 shown in Equation (27) and Equation (30), the damping coefficient c w21 ,c w22 The variation of the model with respect to the variation of is simulated.

[0068] FIG. 7 shows a state where the continuum robot 100 according to the third embodiment has a w21 =c w22 7(a) is a diagram showing an example of a frequency response when the control input u c1 The transfer function from the control input u c2 The transfer function from the force F1 generated by the wire in the second curved section 172 to the force F2 generated by the wire in the second curved section 172 is shown by a solid line, and the transfer function from the force F1 generated by the wire in the first curved section 171 to the force F1 generated by the wire in the first curved section 171 is shown by a dashed line. That is, in Fig. 7(a) and Fig. 7(b), the frequency response due to coupling between the curved sections is shown by dashed lines, and in both cases, the phase exceeds 180 degrees around 400 Hz. For example, when the force control section K F When using a P controller, it can be seen that increasing the gain makes the control system unstable due to coupling between curved sections.

[0069] FIG. 8 shows a state where the continuum robot 100 according to the third embodiment has a w21 =c w228(a) is a diagram showing an example of a frequency response in which the control input u c1 The transfer function from the control input u c2 The transfer function from the first curved section 171 to the wire generated force F2 in the second curved section 172 is shown by a solid line, and the transfer function from the first curved section 171 to the wire generated force F1 in the first curved section 172 is shown by a dashed line. From Fig. 8(a) and Fig. 8(b), it can be seen that the gain characteristics due to coupling between the curved sections are sufficiently lower than the direct gain characteristics. From this, it can be seen that the control system needs to be designed taking into account only the direct frequency response. Furthermore, the direct frequency responses in Fig. 8(a) and Fig. 8(b) are similar, and it can be seen that there is no significant difference in the respective curve drive control characteristics when performing distributed control using the same controller.

[0070] (Fourth embodiment) Next, a fourth embodiment will be described. In the following description of the fourth embodiment, matters common to the first to third embodiments will be omitted, and only matters different from the first to third embodiments will be described.

[0071] In the above-mentioned third embodiment, the response is compared between when the values ​​of the coupling control dampers 813 and 823 are set to zero and when a large value is given, and it has been explained that by giving a large value, the coupling between the curved sections is reduced. As a result, it has been explained that it is possible to design a control means (control system) for a single curved section without considering the coupling, and to stably design distributed control to be placed in each curved section. In the fourth embodiment, it will be explained that it is possible to improve the control performance of the entire control system including the coupling by setting appropriate values ​​to the coupling control dampers 813 and 823.

[0072] [1] Control system design In the fourth embodiment, as shown in FIG. 2(b), a double-loop control system is designed to provide high backdrivability and positioning capability for the continuum robot 100 by performing force control in an inner loop and position control in an outer loop. n represents the expanded system shown in equations (27) and (30). Also, ref θn is the target command for the bending angle in the nth bending section, ref zn is the target displacement of the wire gripping mechanism in the nth curved section, ref F represents the target value of the generated force F. And the wire drive amount l p The relationship between the bending angle θ and the bending angle θ is given by the following equation (31). l p =r1θ …(31)

[0073] In the fourth embodiment, the amount of stretch of the wire is considered to be small and is not taken into consideration in the derivation of the kinematics. zn is given by the following equation (32). ref zn =r1·ref θn …(32)

[0074] Enlarged P n and the force control section K F In the inner loop, the target value of the generated force is ref F The error is calculated by taking the difference between the generated force F and the force control section K F is the motor torque T m This feedback loop is equivalent to compensating for the equivalent inertia of the rotary-to-linear conversion mechanism, and this makes it possible to improve the backdrivability of the continuum robot 100. In the fourth embodiment, the force control unit K F The PI control system shown in the following equation (33) is used.

number

[0075] Here, in equation (33), F ziis the zero crossing frequency of the integral controller. In addition, a second-order low-pass filter with a corner frequency of 30 Hz is connected to the PI control system for stabilization against higher modes. FIG. 9 shows the control system 10 for a continuum robot according to the fourth embodiment, p 2.2 10 -3 Then, F zi is set to 1.0, and the control bandwidth is set to approximately 10 Hz. F 1 is a Bode plot of

[0076] Next, the state space model of the continuum robot 100 shown in equations (27) and (30) is denoted by P, and the closed loop system G shown in equation (34) is cl Request.

number

[0077] And the damping coefficient c of the coupled control damper 813 w21 and the damping coefficient c of the coupled control damper 823 w22 is changed from 0 Ns / m to 150 Ns / m in increments of 1 Ns / m, and the eigenvalues ​​of the closed loop system are calculated. FIG. 10 is a diagram plotting changes in the eigenvalues ​​of the closed loop system in the control system 10 for a continuum robot according to the fourth embodiment. The eigenvalues ​​of the system become more stable as the real part of the complex number decreases, but some poles do not monotonically decrease with an increase in the damping coefficient of the coupled control damper. This shows that there is an optimal value for the damping coefficient of the coupled control damper for stabilizing the closed loop system. In the fourth embodiment, attention is paid to the locus of the roots that draws the maximum arc shown in FIG. 10, and the damping coefficient of the coupled control damper that takes the pole that minimizes the value of the real part of the locus is set as the quasi-optimal damping coefficient.

[0078] FIG. 11 shows a control system 10 for a continuum robot according to a fourth embodiment, in which the suboptimal damping coefficient of the coupled control damper is set to c w21 =c w22 11A is a diagram showing an example of the frequency response of the continuum robot 100 when the control input u c1The transfer function from the control input u c2 The transfer function from the force F2 generated by the wire in the second curved section 172 is shown by a solid line, and the transfer function from the force F1 generated by the wire in the first curved section 171 is shown by a dashed line. From Fig. 11(a) and Fig. 11(b), it can be seen that the gain characteristics due to coupling between the curved sections are sufficiently lower at 100 Hz or more where the phase approaches 180 degrees than the direct gain characteristics. Furthermore, the direct frequency responses in Fig. 11(a) and Fig. 11(b) are similar, and it can be seen that there is no significant difference in the curve drive control characteristics when performing distributed control using the same controller.

[0079] In the fourth embodiment, the position control unit K SV The PID control system shown in the following equation (35) is used.

number

[0080] Here, in equation (35), F zi is the zero crossing frequency of the integral controller, and F zd is the zero crossing frequency of the differential controller. In the fourth embodiment, the augmented system P n and the force control section K F The closed loop transfer function G cl Using the open loop transfer function G cl K SV is derived, and the gain margin and phase margin are designed to be sufficient from the response. In the fourth embodiment, a first-order low-pass filter with a corner frequency of 50 Hz is coupled to the PID control system. FIG. 12 shows the control system 10 for a continuum robot according to the fourth embodiment, in which K p 5 to 10 3 Then, F zi is set to 0.5, and F zd Position control unit K when SV 1 is a Bode plot of

[0081] [2] Simulation The damping coefficient of the coupled control damper obtained in [1] Control system design is c w21 =c w22 A simulation is performed using a model in which the damping coefficient of the coupled control damper obtained in the third embodiment is c = 25 Ns / m (hereinafter, referred to as a "quasi-optimal damping model"). w21 =c w22 = 0Ns / m, c w21 =c w22 A simulation is performed using models in which the damping force is 150 Ns / m (hereinafter, referred to as the "low damping model" and the "high damping model", respectively). In this simulation, the control system shown in FIG. 2 used in the first embodiment is used, but the coupled control unit K D = 0, and a coupling control damper is used as shown in the third embodiment. In this simulation, the dynamic model P of the continuum robot 100 includes a coupling control damper.

[0082] In both the first curved section 171 and the second curved section 172, the force control section K F The PI control system shown in equation (33) is used for the suboptimal model and the high damping model. However, since the gains are different between the suboptimal model and the high damping model, the gain K p In addition, in the low damping model, the response diverges in the same servo band as the suboptimal model, so the gain is adjusted so that the control input does not become oscillatory.

[0083] The first curved section 171 is a position control section K SV The PID control system shown in equation (35) is used. In addition, the displacement z of the wire gripping mechanism is adjusted so that the bending angle becomes 45 degrees 0.5 seconds after the start of the simulation. t21 Reference command ref z1 Gives.

[0084] The second curved section 172 has a position control gain of zero. The second curved section 172 is then coupled to the first curved section 171 to determine a control problem for positioning. This is because if a position control system is used for the second curved section 172, the effect of optimizing the designed coupled control damper will not be clear. If the damping of the coupled control damper is appropriate, the second curved section 172 will follow the angle of the first curved section 171 by quickly backdriving. This allows the performance of the coupled control damper to be evaluated over time.

[0085] FIG. 13 is a diagram showing the simulation response in the control system 10 of the continuum robot according to the fourth embodiment, with a quasi-optimal model shown in a solid line, a low damping model shown in a dashed line, and a high damping model shown in a dashed line. FIG. 13(a) shows the angular response of the distal end of the first curved section 171. The first curved section 171 is controlled by a position servo system, so there is no significant difference in the response. FIG. 13(b) shows the wire force generated in the first curved section 171 and its reference value in a thick line and a thin line. In FIG. 13(b), the high damping model generates a large tension in the wire because the damping force of the coupled control damper is strong. FIG. 13(c) shows the control input of the first curved section 171. In FIG. 13(c), it can be seen that the control input of the low damping model is somewhat oscillatory. FIG. 13(d) shows the angular response of the distal end of the second curved section 172. In FIG. 13(d), the suboptimal model is coupled to the motion of the first curved section 171 and quickly converges to 45 degrees. However, in FIG. 13(d), the low damping model becomes oscillatory, and the high damping model has a slow convergence to 45 degrees due to the strong damping of the coupled control damper. FIG. 13(e) shows the generated force of the wire in the second curved section 172. In FIG. 13(e), the second curved section 172 does not have a positioning loop enabled, so all of the force reference values ​​are zero. In addition, the response of the suboptimal model shows that the generated force quickly converges to zero, and high backdrivability is obtained. However, the low damping model becomes oscillatory, and the high damping model has a slow convergence due to the strong damping of the coupled control damper. FIG. 13(f) shows the control input of the second curved section 172. The high damping model has a slow convergence due to its large damping.

[0086] In this way, by using a dynamic model to find the quasi-optimal value of the coupling control damper, the coupling between the curved sections becomes appropriate, and it can be seen that vibration coupling and response delays due to coupling do not occur.

[0087] (Other embodiments) The present invention can also be realized by a process in which a program for implementing one or more of the functions of the above-described embodiments is supplied to a system or device via a network or a storage medium, and one or more processors in a computer of the system or device read and execute the program. The present invention can also be realized by a circuit (e.g., ASIC) that implements one or more of the functions. This program and a computer-readable storage medium storing the program are included in the present invention.

[0088] It should be noted that the above-mentioned embodiments of the present invention are merely illustrative examples of the implementation of the present invention, and the technical scope of the present invention should not be interpreted as being limited by these. In other words, the present invention can be implemented in various forms without departing from its technical concept or main features.

[0089] Disclosure of embodiments of the present invention includes the following configurations and methods. [Configuration 1] A control system for a continuum robot comprising: a bendable section having a plurality of bending sections driven by a wire; and a wire driving means for driving the wire for each of the bending sections, a control means for controlling an operation of the wire driving means for each of the plurality of bending sections; a suppression means for suppressing a fluctuation in tension of the wire in other curved sections when the wire driving means is operated in a curved section of the plurality of curved sections; A control system for a continuum robot comprising: [Configuration 2] the continuum robot further includes a tension detection means for detecting a tension of the wire, the tension detection means being interposed between the wire and the wire driving means for each of the plurality of curved sections; The suppression means is a second wire driving means for driving the wire without using the tension detection means; a second control means for controlling the operation of the second wire driving means based on a displacement of a displacement detection means provided on either the wire extending between the curved section and the tension detection means or a wire connection portion connecting the wire and the tension detection means; 2. A control system for a continuum robot according to claim 1, comprising: [Configuration 3] The second wire driving means is connected to the wire connection portion. 3. A control system for a continuum robot according to configuration 2. [Configuration 4] The second wire drive means is connected to the wire. 3. A control system for a continuum robot according to configuration 2. [Configuration 5] The second control means is configured with a control system using a differentiator. 5. A control system for a continuum robot according to any one of configurations 2 to 4. [Configuration 6] The suppressing means is a damper connected to a wire connecting portion that connects the wire and the wire driving means. 2. A control system for a continuum robot according to configuration 1. [Configuration 7] The suppression means is a damper connected to the wire. 2. A control system for a continuum robot according to configuration 1. [Configuration 8] The damping coefficient of the damper is set using a dynamic model of the continuum robot. 8. A control system for a continuum robot according to configuration 6 or 7. [Method 1] A method for controlling a continuum robot including a bendable section having a plurality of bending sections driven by wires, and a wire driving means for driving the wires for each of the bending sections, comprising: a control step of controlling the operation of the wire driving means for each of the plurality of bending sections by using a control means provided for each of the bending sections; a suppression step of suppressing a fluctuation in tension of the wire in other curved sections when the wire driving means is operated in a curved section of the plurality of curved sections by using a suppression means; A method for controlling a continuum robot comprising the steps of: [Explanation of symbols]

[0090] 10: Control system for continuum robot, 100: continuum robot, 111, 112: wire, 113: spine, 121, 122: connection part, 131, 132: actuator, 140: base part, 141: upper surface of base part 140, 160, 163: distal end, 161, 162, 164, 165: wire guide, 170: bendable part, 171, 172: bending section, 200: control device for continuum robot, 210: kinematic calculation part, 211, 221: spring, 212, 222: wire gripping part, 213, 223, 713, 723: coupled control actuator, 220-1 to 220-n: controller, 300: input device, 513, 523, 813, 823: coupled control damper, K SV : Position control section, K F :force control section, -k t2 : Generated force calculation unit, K D :Coupled control section

Claims

1. A control system for a continuum robot comprising: a bendable section having a plurality of bending sections driven by a wire; and a wire driving means for driving the wire for each of the bending sections, a control means for controlling an operation of the wire driving means for each of the plurality of bending sections; a suppression means for suppressing a fluctuation in tension of the wire in other curved sections when the wire driving means is operated in a curved section of the plurality of curved sections; A control system for a continuum robot comprising:

2. the continuum robot further includes a tension detection means for detecting a tension of the wire, the tension detection means being interposed between the wire and the wire driving means for each of the plurality of curved sections; The suppression means is a second wire driving means for driving the wire without using the tension detection means; a second control means for controlling the operation of the second wire driving means based on a displacement of a displacement detection means provided on either the wire extending between the curved section and the tension detection means or a wire connection portion connecting the wire and the tension detection means; 2. The control system for a continuum robot according to claim 1, further comprising:

3. The second wire driving means is connected to the wire connection portion.

3. The control system for a continuum robot according to claim 2.

4. The second wire driving means is connected to the wire.

3. The control system for a continuum robot according to claim 2.

5. The second control means is configured with a control system using a differentiator.

3. The control system for a continuum robot according to claim 2.

6. The suppressing means is a damper connected to a wire connecting portion that connects the wire and the wire driving means.

2. The control system for a continuum robot according to claim 1.

7. The suppression means is a damper connected to the wire.

2. The control system for a continuum robot according to claim 1.

8. The damping coefficient of the damper is set using a dynamic model of the continuum robot.

8. A control system for a continuum robot according to claim 6 or 7.

9. A method for controlling a continuum robot including: a bendable section having a plurality of bending sections driven by wires; and a wire driving means for driving the wires for each of the bending sections, the method comprising the steps of: a control step of controlling the operation of the wire driving means for each of the plurality of bending sections by using a control means provided for each of the bending sections; a suppression step of suppressing a fluctuation in tension of the wire in other curved sections when the wire driving means is operated in a curved section of the plurality of curved sections by using a suppression means; A method for controlling a continuum robot comprising the steps of:

Citation Information

Patent Citations

  • Control system of continuum robot and control method for the same as well as continuum robot

    JP2021133429A