WO2021166995A1 - 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット - Google Patents

連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット Download PDF

Info

Publication number
WO2021166995A1
WO2021166995A1 PCT/JP2021/006085 JP2021006085W WO2021166995A1 WO 2021166995 A1 WO2021166995 A1 WO 2021166995A1 JP 2021006085 W JP2021006085 W JP 2021006085W WO 2021166995 A1 WO2021166995 A1 WO 2021166995A1
Authority
WO
WIPO (PCT)
Prior art keywords
control system
wire
control unit
robot
force
Prior art date
Application number
PCT/JP2021/006085
Other languages
English (en)
French (fr)
Inventor
清志 ▲高▼木
Original Assignee
キヤノン株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by キヤノン株式会社 filed Critical キヤノン株式会社
Priority to EP21756932.6A priority Critical patent/EP4101605A4/en
Priority to CN202180015981.5A priority patent/CN115135465A/zh
Publication of WO2021166995A1 publication Critical patent/WO2021166995A1/ja
Priority to US17/890,104 priority patent/US20220402130A1/en

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
    • B25J9/1625—Truss-manipulator for snake-like motion
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/005—Flexible endoscopes
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J17/00—Joints
    • B25J17/02—Wrist joints
    • B25J17/0241—One-dimensional joints
    • B25J17/025—One-dimensional joints mounted in series
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • B25J9/104—Programme-controlled manipulators characterised by positioning means for manipulator elements with cables, chains or ribbons
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40234—Snake arm, flexi-digit robotic manipulator, a hand at each end

Definitions

  • the present invention relates to a control system for a continuum robot, a control method thereof, and a continuum robot.
  • the continuum robot also called a continuous robot, has a bendable portion having a flexible structure, and the shape is controlled by deforming the bendable portion.
  • This continuous robot has two main advantages over a robot composed of a rigid body link (hereinafter, referred to as a "rigid body link robot").
  • the first advantage is that the continuous robot can move along the curve of the object only by operating its tip in a narrow space where a rigid link robot fits or in an environment with scattered objects. Is the point.
  • the second advantage is that the continuum robot has an intrinsic softness, so that it can be operated without damaging a fragile object, especially in an open space. There, the detection of external force at the end effector, which is required for rigid link robots, is not always required.
  • continuum robots are expected to be applied to the medical field such as endoscope sheaths and catheters, and to extreme work robots such as rescue robots.
  • Examples of the driving method of this continuous robot include a tendon driving method, a push-pullable wire method, and an air actuator method.
  • Patent Document 1 describes a method for controlling a manipulator that detects a load amount generated on a drive wire and controls a drive unit so that the load amount falls within a preset range.
  • the manipulator when the manipulator is inserted into a narrow space, the manipulator can be inserted without performing a bending operation to follow the path, and the insertion operation is facilitated.
  • the fact that the drive unit operates passively according to the load amount is said to have "back drivability”, but in Patent Document 1, the load amount generated in the wire is detected and controlled instead of the mechanism such as a clutch.
  • An algorithm is used to prevent the drive unit from becoming large.
  • the manipulator described in Patent Document 1 is a motor composed of a continuum called a tubular portion and a plurality of joint structures at the tips thereof, and a wire connected to the joint is provided in a base end housing. It operates by being pulled by a wire, and can be applied to a continuum connected by a plurality of nodal rings and rotating joints.
  • the present invention has been made in view of such problems, and an object of the present invention is to provide a mechanism capable of realizing high positioning performance of a bendable portion at a target position without performing complicated operations by an operator. And.
  • the control system for a continuous robot of the present invention is a control system for a continuous robot including at least one bendable portion driven by a wire and configured to be bendable, and a drive portion for driving the wire. Control is performed to compensate for an error between the target displacement of the push-pull drive of the wire by the drive unit and the displacement of the wire gripping mechanism for gripping the wire obtained from the continuous robot, and the target of the wire.
  • a first loop control system including the force control unit and a second loop control system including the force control unit and the position control unit are configured.
  • the present invention also includes a method for controlling a continuum robot by the above-mentioned continuum robot and the above-mentioned control system for the continuum robot.
  • Bode diagram which shows the dynamic model of the continuum robot in 1st Embodiment of this invention. It is a Bode diagram which shows the force control part of the continuum robot in 1st Embodiment of this invention. It is a Bode diagram which shows the open-loop transfer function of the continuous robot in 1st Embodiment of this invention. It is a Bode diagram which shows the position control part of the continuum robot in 1st Embodiment of this invention. It is a Bode diagram which shows the open loop system transfer function of the continuous robot in 1st Embodiment of this invention. It is a flowchart which shows the design procedure of the control system which concerns on the control device of the continuum robot in 1st Embodiment of this invention.
  • the first embodiment of the present invention is shown, and two are an inner loop control system (first loop control system) using a force control unit and an outer loop control system (second loop control system) further including a position control unit. It is a figure which shows the positioning response with the disturbance about the rotation angle of a motor by a heavy loop control system.
  • the first embodiment of the present invention is shown, and two are an inner loop control system (first loop control system) using a force control unit and an outer loop control system (second loop control system) further including a position control unit. It is a figure which shows the positioning response with the disturbance about the displacement of the wire gripping mechanism by a heavy loop control system.
  • the first embodiment of the present invention is shown, and two are an inner loop control system (first loop control system) using a force control unit and an outer loop control system (second loop control system) further including a position control unit. It is a figure which shows the positioning response with the disturbance about the generated force by a heavy loop control system.
  • the first embodiment of the present invention is shown, and two are an inner loop control system (first loop control system) using a force control unit and an outer loop control system (second loop control system) further including a position control unit. It is a figure which shows the positioning response with the disturbance about the control input by a heavy loop control system.
  • the first embodiment of the present invention is shown, and two are an inner loop control system (first loop control system) using a force control unit and an outer loop control system (second loop control system) further including a position control unit. It is a figure which shows the positioning response with the disturbance about the bending angle by a heavy loop control system.
  • the first embodiment of the present invention is shown, and two are an inner loop control system (first loop control system) using a force control unit and an outer loop control system (second loop control system) further including a position control unit. It is a figure which shows the positioning response with the disturbance about the disturbance torque by a heavy loop control system. It is a figure which shows the parameter search of the control system which concerns on the control device of the continuum robot in the 2nd Embodiment of this invention.
  • Bode diagram which shows the force control part which concerns on the control device of the continuum robot in 2nd Embodiment of this invention. It is a Bode diagram which shows the open loop transfer function which concerns on the control device of a continuous robot in 2nd Embodiment of this invention. It is a flowchart which shows the design procedure of the control system which concerns on the control device of the continuum robot in 2nd Embodiment of this invention. It is a figure which shows the parameter search of the control system which concerns on the control device of the continuum robot in the 3rd Embodiment of this invention. It is a Bode diagram which shows the force control part which concerns on the control device of the continuum robot in 3rd Embodiment of this invention.
  • a third embodiment of the present invention which 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. It is a figure which shows the positioning response with the disturbance about the rotation angle of a motor by a heavy loop control system.
  • a third embodiment of the present invention 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. It is a figure which shows the positioning response with the disturbance about the displacement of the wire gripping mechanism by a heavy loop control system.
  • a third embodiment of the present invention is shown, which 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. It is a figure which shows the positioning response which accompanies the disturbance about the generated force by a heavy loop control system.
  • a third embodiment of the present invention is shown, which 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. It is a figure which shows the positioning response with the disturbance about the control input by a heavy loop control system.
  • a third embodiment of the present invention is shown, which 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. It is a figure which shows the positioning response with the disturbance about the bending angle by a heavy loop control system.
  • a third embodiment of the present invention is shown, which 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. It is a figure which shows the positioning response with the disturbance about the disturbance torque by a heavy loop control system. It is a figure which shows an example of the schematic structure of the control system of the continuum robot which concerns on 4th Embodiment of this invention. It is a figure which shows an example of the schematic structure of the control system of the continuum robot which concerns on 4th Embodiment of this invention. It is a figure which shows an example of the schematic structure of the control system of the continuum robot which concerns on 4th Embodiment of this invention.
  • FIG. 1 is a diagram showing an example of a schematic configuration of a control system 10 for a continuous robot according to the first embodiment of the present invention.
  • control system 10-1 of the continuous robot the control system 10 of the continuous robot shown in FIG. 1 will be described as "control system 10-1 of the continuous robot”.
  • the control system 10-1 of the continuum robot includes a mechanism unit of the continuum robot (hereinafter, may be simply referred to as “continuum robot”) 100, a control device 200 of the continuum robot, and the control device 200 of the continuum robot. It also has an input device 300. Further, as shown in FIG. 1, the control device 200 of the continuum robot includes a kinematics calculation unit 210, a position control unit K SV , and a force control unit K F.
  • the force control for the continuous robot 100 is performed by the inner loop control system (first loop control system) including the force control unit K F shown in FIG. Further, the outer loop control system (second loop control system) including the force control unit K F and the position control unit K SV shown in FIG. 1 controls the position of the continuous robot 100.
  • the continuous robot control system 10-1 according to the present embodiment, the double loop control system of the inner loop control system (first loop control system) and the outer loop control system (second loop control system) is used. The back drivability is high with respect to the continuous robot 100, and at the same time, high positioning performance of the bendable portion of the continuous robot 100 with respect to the target position is realized.
  • a dynamic model P n of a continuum robot 100 including a bendable portion driven by a wire and configured to be bendable and a drive portion for driving the wire is derived.
  • the drive unit can detect the tension of the wire, and may include, for example, a rotary motor and a rotary linear motion conversion mechanism.
  • the kinematics calculation unit 210 When the target bending angle ref ⁇ in the bendable part of the continuum robot 100 (the bendable part 110 in FIG. 2 described later) is input from the input device 300, the kinematics calculation unit 210 performs the kinematics calculation and performs the continuum.
  • the target displacement ref z of the push-pull drive of the wire by the drive unit of the robot 100 is output.
  • the position control unit K SV controls to compensate for the error between the target displacement ref z output from the kinematics calculation unit 210 and the displacement z t2 of the wire gripping mechanism that grips the wire obtained from the displacement sensor of the continuum robot 100.
  • the target generating force ref F corresponding to the target tension of the wire is output.
  • the target generated force ref F represents a target value of the generated force F, which is a positive / negative reversal value of the wire tension sensor defined in the present embodiment.
  • the force control unit K F controls to compensate for the error between the target generated force ref F output from the position control unit K SV and the generated force F corresponding to the tension of the wire obtained from the continuum robot 100. Therefore, the motor torque T m , which is the target torque of the drive unit, is output. Then, a control input is given to the rotary motor or the like of the continuous robot 100.
  • the inner loop control system including the force control unit K F
  • the difference between the target generated force ref F and the generated force F obtained from the wire tension sensor of the continuous robot 100 is calculated.
  • the error is calculated by taking the error, and the force control unit K F outputs the motor torque T m as a control input for compensating for the error.
  • This feedback loop control system is equivalent to compensating for the equivalent inertia of the rotational linear motion conversion mechanism, which makes it possible to improve the back drivability of the continuum robot 100.
  • the closed loop control system by the feedback of the inner loop control system (first loop control system) is represented by G cl.
  • the target displacement ref z and the displacement z t2 obtained from the displacement sensor of the continuous robot 100
  • the error is calculated by taking the difference, and the position control unit K SV outputs the target generating force ref F as a control input for compensating for the error.
  • the stability of the feedback system is guaranteed by using the transfer function obtained from the dynamic model P n of the continuum robot 100 in the design of the force control unit K F and the position control unit K SV. As a result, it is possible to realize high positioning performance to the target position while having high backdability against disturbance of the bendable portion in the continuous robot 100.
  • FIG. 2 is a diagram showing an example of a schematic configuration of the continuum robot 100 according to the first embodiment of the present invention.
  • the continuum robot 100 shown in FIG. 2 will be described as "continuum robot 100-1".
  • the wire 111 and the wire 112 are connected to the fixing portion 121 and the fixing portion 122 at the distal end 160 of the bendable portion 110, respectively. Further, the wires 111 and the proximal ends of the wires 112 are connected to the wire gripping pipe 131 and the wire gripping pipe 132, respectively, in the robot base portion 140.
  • the robot base portion 140 is provided with a wire gripping mechanism 171 that supports the wire gripping pipe 131, a wire gripping mechanism 172 that supports the wire gripping pipe 132, and an actuator 180 that corresponds to the drive unit. At this time, the wire gripping mechanism 172 is fixed to the robot base portion 140.
  • the wire gripping mechanism 171 is connected to the actuator 180, which is a drive unit, via a wire gripping mechanism base (not shown), and can move up and down.
  • the wire gripping pipe 131 is supportably connected to the wire gripping mechanism 171 and the posture of the bendable portion 110 is controlled by pushing and pulling with the actuator 180.
  • the continuous robot 100-1 includes wire guides 161 to 164 which are members for guiding the wire 111 and the wire 112. In this wire guide, in addition to the method of arranging a plurality of members discretely, a continuous member such as a bellows shape or a mesh shape may be used.
  • the wire guides 161 to 164 are fixed to the wire 112 at the fixing portions 150 to 153. Further, the distance between the wire 111 and the wire 112 may be different from the distance between the wire gripping mechanism 171 and the wire gripping mechanism 172.
  • the diameter conversion unit 190 is connected to the robot base unit 140.
  • the mechanism including the wire 111 and the wire 112 and the wire guides 161 to 164 is referred to as a bendable portion 110 which is a continuum portion.
  • the actuator 180 includes an actuator unit including a rotary motor and a rotary linear motion conversion mechanism.
  • the wire gripping mechanism 171 (or the actuator 180) has a function of detecting the tension of the wire 111.
  • the above-mentioned wire gripping mechanism base (not shown) is provided between the wire gripping mechanism 171 and the actuator 180, and the wire gripping mechanism base is connected to the actuator 180 to connect the wire gripping mechanism base and the wire. It is connected to the gripping mechanism 171 by a spring.
  • the tension of the wire 111 can be detected by measuring the displacement of the spring.
  • FIG. 3A to 3C are diagrams showing an example of the dynamic model P n of the continuum robot 100 according to the first embodiment of the present invention.
  • FIG. 3A shows an example of a dynamic model of the bendable portion 110 which is a continuum portion
  • FIG. 3B shows a rotary motor, a rotary linear motion conversion mechanism, and a wire gripping mechanism 171 included in the actuator 180.
  • An example of a dynamic model consisting of the above is shown.
  • the equation of motion of the continuum robot 100-1 is derived.
  • the following assumptions are made. [1] Only the motion in the two-dimensional plane is considered. [2] It is assumed that the curvature of the bendable portion 110 is constant and the spring coefficient is uniform. [3] The wire is approximated as a concentrated mass system, and a reaction force due to deformation in the longitudinal direction acts on the tip of the bendable portion 110. Lateral vibration and lateral deformation of the wire are not considered. [4] The friction between the wire and the wire guide and between the wire and the diameter conversion portion is pushed into the damping coefficient c w2 as viscous damping including non-linear friction such as Coulomb friction.
  • the kinetic energy of the bendable portion 110 is obtained.
  • the displacements x g and z g are taken on the central axis of the bendable portion 110, the following equations (1) and (2) are obtained, respectively.
  • the displacements x g and z g are the following equations (4) and (5), respectively.
  • the kinetic energy T a bendable portion 110 is a following formula (6).
  • the kinetic energy T w and the potential energy U w of the wire and the wire gripping pipe are the following equations (8) and (9), respectively.
  • the equation of motion is the following equation (10) showing Lagrange's equation.
  • the nonlinear differential equation shown in the following equation (11) can be obtained.
  • [Phi is the nonlinear terms that can not be put into M c, C c, K c .
  • Mc22 and ⁇ 3 of the equation (11) are the following equations (12) and (13).
  • FIG. 3B is a diagram showing an example of a dynamic model consisting of a rotary motor, a rotary linear motion conversion mechanism and a wire gripping mechanism.
  • 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 180
  • the wire gripping mechanism base is connected to the actuator 180
  • the wire gripping mechanism base is connected.
  • the wire gripping mechanism are connected by a spring. Tension is detected by detecting the displacement of the spring.
  • the definition of the code in the dynamic model shown in FIG. 3B is described below.
  • the generated force F is given by the following equation (26).
  • the equivalent inertia of the rotary linear motion conversion mechanism is changed by changing the equivalent acceleration ratio R of the rotary linear motion conversion mechanism from 1 time to 10000 times the design parameter, and the fluctuation of the pole and the zero point is investigated. Further, in this analysis, the attenuation coefficient of each part of the bendable part 110 is set to almost 0 in order to make it easy to understand the fluctuation of the pole and the zero point.
  • FIG. 4 is a diagram showing an example of a pole-zero point response (pole-zero point arrangement) of the dynamic model P n of the continuum robot 100 according to the first embodiment of the present invention.
  • the pole of the bendable portion 110 alone is indicated by an asterisk
  • the pole and the zero point at which the equivalent acceleration ratio R is 10000 times are indicated by asterisks and circles, respectively, and the equivalent acceleration ratio R is 1 times.
  • the poles and zeros to be used are indicated by X marks and triangle marks, respectively. From this, it can be seen that as the equivalent inertia on the motor side increases, the natural frequency of the bendable portion 110 decreases and moves to the overdamped pole and the origin pole. It should be noted here that the zeros indicated by the circles and triangles have hardly moved.
  • the frequency of the bendable portion 110 is over-attenuated, but it is necessary to consider the phase characteristic within the control band due to this zero point in the control system design.
  • FIG. 5 is a Bode diagram showing a dynamic model P n of the continuum robot 100 according to the first embodiment of the present invention.
  • FIG. 5 shows a Bode diagram of an enlarged system in which the equivalent acceleration ratio R is varied.
  • the expansion system linearizes the equations (24) and (27) with the bending angle ⁇ as 0.05 degrees.
  • FIG. 5 shows the transfer function from the motor target torque T m, which is the control input, to the generated force F, and the responses for setting the equivalent acceleration ratio R to 10 times, 50 times, and 500 times are solid lines, broken lines, and lines, respectively. It is shown by a alternate long and short dash line.
  • FIG. 6 is a Bode diagram showing a dynamic model P n of the continuum robot 100 according to the first embodiment of the present invention. Specifically, FIG. 6 shows a Bode diagram of an enlarged system in which the attenuation coefficient c w2 is varied. In FIG. 6, the transfer function from the motor torque T m, which is the control input, to the generated force F is shown, and the responses with the damping coefficients c w2 set to 0.01, 1, 10, and 150 are the solid line, the dotted line, and the broken line, respectively.
  • FIG. 7 is a Bode diagram showing a dynamic model P n of the continuum robot 100 according to the first embodiment of the present invention. Specifically, FIG. 7 shows an enlarged Bode diagram. The transfer function from the motor torque T m to the generated force F is shown by a solid line, the transfer function from the motor torque T m to the displacement z t 2 of the wire gripping mechanism is shown by a broken line, and the bending angle from the motor torque T m to the bendable portion 110. The transfer function up to ⁇ is shown by the alternate long and short dash line. In the high frequency range, high-order vibrations due to the spring of the wire tension detection mechanism, the wire, and the rotational linear motion conversion mechanism appear.
  • Control Design performs force control with the inner-loop control system comprising a force controller K F (first loop control system), the force control unit K F and position control
  • the double loop control system which controls the position by the outer loop control system (second loop control system) including the part K SV, has high back drivability to the continuous robot 100-1, and at the same time, the continuous body.
  • the dynamic model P n represents the expansion system shown in Eqs. (24) and (27).
  • the amount of expansion and contraction of the wire is considered to be minute, and is not considered for the derivation of kinematics by the kinematics calculation unit 210. Therefore, the target displacement ref z is given by the following equation (29).
  • the target generated force ref F and the generated force F which are the target values of the generated force F, are generated.
  • the error is calculated by taking the difference from and, and the force control unit K F outputs the motor torque T m as a control input for compensating for the error.
  • This feedback loop is equivalent to compensating for the equivalent inertia of the rotational linear motion conversion mechanism, which makes it possible to improve the back drivability of the continuum robot 100-1.
  • the closed loop system by this feedback is represented by G cl.
  • the difference between the target displacement ref z and the displacement z t2 obtained from the continuous robot 100 is taken.
  • the error is calculated, and the position control unit K SV outputs the target generating force ref F as a control input for compensating for the error.
  • the PI control system shown in the following equation (30) is used as the inner loop control system (first loop control system) that performs force control.
  • F zi is the zero crossing frequency of the integral controller.
  • a second-order low-pass filter with a break point frequency of 200 Hz is coupled to the PI control system for stabilization with respect to the higher-order mode shown in the previous chapter.
  • FIG. 8A and 8B are Bode plots showing the control device 200 of the continuum robot according to the first embodiment of the present invention. Specifically, FIG. 8A and 8B, (30) of K p, the F zi, respectively, 2.2-10 -3, 1.0, and the control bandwidth is about 30Hz force control unit K F
  • the Bode diagram of the above and the Bode diagram of the open-loop transfer function P n K F with the expansion system P n are shown.
  • the PID control system shown in the following equation (31) is used as the outer loop control system (second loop control system) that performs position control.
  • F zi and F zd are the zero intersection frequencies of the integral controller and the differential controller, respectively.
  • the open-loop transfer function G cl K SV is derived using the closed-loop transfer function G cl between the dynamic model P n of the expansion system and the force control unit K F, and the gain margin and phase margin are sufficient from the response.
  • a first-order low-pass filter having a break point frequency of 50 Hz is coupled to the PID control system.
  • FIGS. 9A and 9B are Bode plots showing the control device 200 of the continuum robot according to the first embodiment of the present invention. Specifically, in FIGS. 9A and 9B, K p , F zi , and F zd in Eq. (31) are set to 5-10 3 , 0.5, and 100, respectively, and the control band is set to about 3.5 Hz.
  • the board diagram of the control unit K SV and the board diagram of the open loop transfer function G cl K SV of the closed loop control system G cl and the position control unit K SV are shown.
  • FIG. 10 is a flowchart showing a design procedure of a control system according to the control device 200 of the continuous robot according to the first embodiment of the present invention.
  • the following design procedure is described in FIG. 10.
  • ⁇ 1> Derivation of dynamic model P n of expansion system
  • ⁇ 2> Set coefficients K p and K i , combine low-pass filters, and tentatively design force control unit K F
  • Completion of design of force control unit K F ⁇ 6> Derivation of closed-loop system G cl ⁇ 7> Set coefficients K p , K i , K d and, combining the low-pass filter, tentatively designed ⁇ 8> derive the open-loop transfer function G cl K SV position control unit K SV ⁇ 9> gain margin, phase margin enough determination of whether ⁇ 10> position controller K SV Design completed
  • Simulation 1.3) The simulation was performed using the control system designed in the control system design and the expansion system shown in Eqs. (24) and (27).
  • FIGS. 11A to 11F show the first embodiment of the present invention and show the disturbance response of only the inner loop control system (first loop control system) using the force control unit K F.
  • FIG. 11A shows the rotation angle ⁇ m of the motor
  • FIG. 11B shows the displacement z t2 of the wire gripping mechanism
  • FIG. 11C shows the generated force F
  • FIG. 11D shows the control input T m
  • FIG. 11E shows.
  • the bending angle ⁇ of the tip of the bendable portion 110 is shown in FIG. 11F
  • the disturbance torque applied to the tip of the bendable portion 110 is shown in FIG. 11F.
  • FIGS. 11A to 11F the control response is shown by a solid line
  • the response during non-control is shown by a broken line for comparison
  • the target generating force ref F is shown by a dotted line.
  • a disturbance torque is applied to the tip of the bendable portion 110 as shown in FIG. 11F.
  • a force generated in the negative direction of the z-axis acts on the wire gripping mechanism by the spring of the tension detecting mechanism, but the target value of the generated force F is 0. Therefore, as shown in FIG. 11D, the force control unit K F gives a positive control input to the motor, and the motor rotates in the positive direction as shown in FIG. 11A, so that the wire gripping mechanism becomes as shown in FIG. 11B. , Z-axis is displaced in the positive direction. As a result, as shown in FIG.
  • the generated force F is compensated to the target value of 0, and as shown in FIG. 11E, the tip of the bendable portion 110 is curved in the same direction as the disturbance torque, resulting in disturbance.
  • the control that the motor backdrives according to the torque is realized.
  • the disturbance torque shown in FIG. 11F is similarly applied, but since the equivalent inertia of the rotational linear motion conversion mechanism is large, as shown by the broken line in FIGS. 11A and 11E, the motor is used. It hardly rotates, and the tip of the bendable portion 110 hardly bends. From this, it can be seen that the inner loop control system that performs force control compensates for the equivalent inertia of the rotational linear motion conversion mechanism and enables back drive against the disturbance torque at the tip of the continuum.
  • FIGS. 12A to 12F show the first embodiment of the present invention, and shows an inner loop control system (first loop control system) using the force control unit K F and an outer loop including a position control unit K SV. It is a figure which shows the positioning response which accompanies the disturbance by the double loop control system with the loop control system (the second loop control system). Specifically, FIGS. 12A to 12F show each response in the same arrangement as in FIGS. 11A to 11F. In this simulation, a comparison with semi-closed control, which controls the position only at the rotation angle ⁇ m of the motor, is shown. The servo bands of the position control system of the double loop control system and the semi-closed control system are designed to be the same. In FIGS.
  • FIG. 12A shows the control response of the double loop control system with a solid line
  • the response of the semi-closed control system is shown by a broken line
  • FIG. 12A shows the target angle ref m of the motor given to the semi-closed control system with a dotted line
  • FIG. 12B shows the target displacement ref z of the wire gripping mechanism given to the outer loop with a dotted line
  • FIG. 12C shows the position control system K SV.
  • the target generated force ref F which is the target value of the generated force F, which is the control input output by, is shown by the dotted line.
  • both the double loop control system and the semi-closed control system follow the target trajectory and settle to the target displacement in about 1 second. Then, in 1.5 seconds, a disturbance torque is applied to the end of the bendable portion 110 as shown in FIG. 12F.
  • the position control unit K SV of the outer loop control system (second loop control system) has the z axis in order to compensate for the displacement error generated in the displacement z t2 of the wire gripping mechanism.
  • the target value of the generated force in the negative direction is output as the control input.
  • the force control unit K F of the inner loop control system follows the target generated force ref F , which is the target value of the generated force F.
  • the double loop control system has the same setting performance to the target position as the semi-closed control system that controls the position only at the rotation angle ⁇ m of the motor, and at the same time, backs up against the disturbance torque at the tip of the bendable portion 110. It can be seen that the drive is possible.
  • the inner loop control system comprising a force controller K F (first loop control system), the force control unit K F and the position
  • the outer loop control system (second loop control system) including the control unit K SV is configured. According to such a configuration, it is possible to realize high positioning performance of the bendable portion to the target position without performing complicated operations by the operator. As a result, it is possible to have high back-dryability against disturbance of the tip of the bendable portion of the continuous robot 100.
  • the vibration characteristic of the bendable portion 110 which is a continuum portion, is coupled with the rotation linear motion conversion mechanism to move to a low frequency region, and further, the wire, the diameter conversion portion, and the wire guide. It was shown that the phase characteristics fluctuate due to friction with. Further, in the above-described first embodiment, the PI control system is used for the force control unit K F of the inner loop control system (first loop control system) that performs force control.
  • the second embodiment is a mode in which the force control unit K F is considered to compensate the phase characteristics by using the PID control system.
  • the PID control system shown in the following equation (32) is used.
  • F zi and F zd are the zero intersection frequencies of the integral controller and the differential controller, respectively.
  • a second-order low-pass filter having a break point frequency of 200 Hz is coupled (for example, serially coupled) to a PID control system (PID controller).
  • PID controller PID control system
  • the open-loop transfer function P n K F is derived and designed so that the gain margin and the phase margin are sufficient from the response, but in the second embodiment, the gain K d is set. Shows how to design.
  • the differential controller can advance the phase at a frequency lower than the zero intersection frequency F zd with almost no increase in gain (differential gain). However, at frequencies higher than F zd , the gain is increased. Therefore, by changing the zero intersection frequency F zd and calculating the gain margin (calculating the stability margin) using the open-loop transfer function P n K F each time, the relationship between the zero intersection frequency F zd and the gain margin can be obtained. Acquire and search for the zero intersection frequency F zd that maximizes the gain margin.
  • FIG. 13 is a diagram showing a parameter search of the control system according to the control device 200 of the continuous robot according to the second embodiment of the present invention.
  • the minimum frequency is set to 1 Hz, which is sufficiently lower than the zero point of the bendable portion 110, which is the continuum portion shown in the first embodiment, and the maximum frequency is set to, for example, the Nyquist when the control system is digitally mounted.
  • the response of the search for the gain margin with the frequency of 1 kHz is shown. In FIG. 13, it can be seen that the gain margin is maximized near 95 Hz of the zero intersection frequency F zd.
  • FIGS. 14A and 14B are Bode plots and open-loop responses of the control system according to the control device 200 of the continuum robot according to the second embodiment of the present invention.
  • FIG. 14A shows a Bode diagram of the PID control system of the force control unit K F
  • FIG. 14B shows a Bode diagram of the open loop transfer function P n K.
  • the solid line shows the case of the PID control system of the second embodiment in which the zero intersection frequency F zd is 95 Hz
  • the broken line shows the case of the PI control system of the first embodiment. From the open-loop response, it can be seen that the gain margin is improved by about 7 dB by compensating for the phase lag with the differential characteristics.
  • FIG. 15 is a flowchart showing a design procedure of a control system according to the control device 200 of the continuous robot according to the second embodiment of the present invention.
  • FIG. 15 describes the following design procedure.
  • ⁇ 1> Derivation of dynamic model P n of expansion system
  • ⁇ 2> Set coefficients K p and K i , combine low-pass filters, and tentatively design force control unit K F
  • phase margin searches the determined ⁇ 6> zero-crossing frequency F zd a search range of sufficient or determination ⁇ 5> zero-crossing frequency F zd, the K d to maximize the gain margin
  • Decision ⁇ 7> Completion of design of force control unit K F ⁇ 8> Derivation of closed loop system G cl ⁇ 9> Set coefficients K p , Ki , K d , combine low-pass filter, and temporarily set position control unit K SV
  • Design ⁇ 10> Derivation of open-
  • the phase lag characteristic is compensated by the differential element of the PID control system.
  • the order of the control system is increased to further compensate for the phase lag.
  • the gain of the inner loop control system (first loop control system) that controls the force is increased so as to correspond to the improved gain margin, and the control band of the inner loop control system is expanded. Then, the effectiveness is verified by simulation.
  • a phase lead filter represented by the following equation (33) is coupled (for example, serially coupled) to the force control unit K F designed in the second embodiment described above.
  • it is referred to as a PID-Lead control system.
  • F c1 is the zero crossing frequency
  • F c2 (> F c1 ) is the pole for making the filter proper.
  • F c2 is set to 1 kHz and F c1 is searched.
  • the zero intersection frequency F c1 is changed and the gain margin is calculated each time to acquire the relationship between the zero intersection frequency F c1 and the gain margin, and the zero intersection that maximizes the gain margin is obtained. Search for frequency F c1.
  • FIG. 16 is a diagram showing a parameter search of the control system according to the control device 200 of the continuous robot according to the third embodiment of the present invention. Specifically, FIG. 16 shows a search response in which the minimum frequency is 1 Hz, which is sufficiently lower than the zero point of the bendable portion 110, which is a continuum portion, and 900 Hz, which is the maximum frequency F c2 or less.
  • the gain margin is maximized near 540 Hz at the zero intersection frequency F c1. Since the response is steep, if the zero intersection frequency F c1 is set to 545 Hz with some margin, the gain margin is improved by about 6 dB.
  • FIGS. 17A and 17B are a Bode diagram and an open loop response of the control system according to the control device 200 of the continuous robot according to the third embodiment of the present invention.
  • FIG. 17A shows a Bode diagram of the PID-Lead control system of the force control unit K F
  • FIG. 17B shows a Bode diagram of the open loop transfer function P n K.
  • the solid line shows the force control system using the PID-Lead control system
  • the broken line shows the control system of the first embodiment.
  • the PID-Lead control system raises a gain of 13 dB, which corresponds to an improved gain margin from 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 margins are the same.
  • FIG. 18 is a flowchart showing a design procedure of a control system according to the control device 200 of the continuous robot according to the third embodiment of the present invention.
  • FIG. 18 describes the following design procedure.
  • ⁇ 1> Derivation of dynamic model P n of expansion system ⁇ 2> Set coefficients K p and K i , combine low-pass filter, and tentatively design force control unit K F ⁇ 3>
  • phase margin explore the adequacy of determination ⁇ 5> store the gain margin GM1 ⁇ 6> zero-crossing frequency F determined search range of zd ⁇ 7> zero-crossing frequency F zd, gain Determine K d that maximizes the margin
  • ⁇ 8> Determine the search range of the frequency F c1 of the phase lead filter and frequency F c2 ⁇ 9> Search F c1 and determine the maximum phase advance filter with the gain margin ⁇ 10>
  • FIGS. 19A to 19F show a third embodiment of the present invention, in which an inner loop control system (first loop control system) using the force control unit K F and a position control unit K SV are further described. It is a figure which shows the positioning response with the disturbance by the double loop control system with the outer loop control system (the second loop control system) including. Specifically, FIGS. 19A to 19F show each response in the same arrangement as in FIGS. 11A to 11F. In this simulation, a comparison with the PI control system of the first embodiment is shown. The servo band of the outer loop control system (second loop control system) of the double loop control system using the PID-Lead control system is designed to be the same as that of the first embodiment. In FIGS.
  • FIG. 19A to 19F the response of the PI-Lead control system is shown by a solid line, and the response of the PI control system is shown by a broken line.
  • FIG. 19B shows the target displacement ref z of the wire gripping mechanism applied to the outer loop with a dotted line
  • FIG. 19C shows the target generating force ref F , which is a control input output by the position control unit K SV, with a dotted line.
  • both the PI-Lead control system and the PI control system follow the target trajectory and set the target displacement in about 1 second. Then, in 1.5 seconds, as shown in FIG. 19F, a disturbance torque is applied to the tip of the bendable portion 110.
  • the position control unit K SV of the outer loop control system (second loop control system) is a target value which is a target value of the generated force F in the negative direction of the z-axis.
  • the generated force ref F is output as a control input, and the force control unit K F of the inner loop control system (first loop control system) follows the target value of the generated force F.
  • the motor backdrives in the positive direction as shown in FIG. 19A, and as shown in FIG. 19B.
  • the wire gripping mechanism is extended in the positive z-axis direction.
  • the PID-Lead control system has a higher gain than the PI control system, and therefore has a large back drive.
  • the PID-Lead control system has a larger control input for reducing the equivalent inertia than the PI control system.
  • the PID-Lead control system realizes control having a large back drivability with respect to the disturbance torque to the tip of the bendable portion 110. Further, the PID-Lead control system is not impaired in stability with respect to a higher-order vibration mode.
  • the PID-Lead control system can design the force control unit K F of the inner loop control system (first loop control system) to a high gain by compensating for the phase lag characteristic at a high order. It is possible to compensate the equivalent inertia of the mechanism to high gain and increase the back drivability.
  • the continuum robot 100 can be provided with back drivability by the double loop control system. Further, in the third embodiment, it was shown that the back drivability can be increased by designing the force control unit K F of the inner loop control system (first loop control system) to a high gain. .. In this embodiment, a control system capable of adjusting the magnitude of back drivability without changing the positioning performance is shown.
  • the double loop control system is designed. Then, the force control unit K F is multiplied by a coefficient (first coefficient) ⁇ of 0 or more and 1 or less. As a result, the gain of the force control unit K F is lowered, and the magnitude of back drivability can be arbitrarily reduced. However, at the same time, the gain of the open-loop transfer function G cl K SV is lowered, so that the servo band of the positioning control is lowered. Therefore, the gain of the position control unit K SV is increased, and the servo band is redesigned so as to have the same servo band as when the coefficient ⁇ is 1. As a result, it is possible to change the back drivability without changing the position control performance.
  • a coefficient (first coefficient) ⁇ of 0 or more and 1 or less.
  • FIGS. 20A to 20C are diagrams showing an example of a schematic configuration of a control system 10 for a continuous robot according to a fourth embodiment of the present invention.
  • the control system 10 of the continuous robot shown in FIG. 20A will be described as “control system 10-2 of the continuous robot”.
  • the control system 10 of the continuous robot shown in FIG. 20B is described as “control system 10-3 of the continuous robot”.
  • the control system 10 of the continuum robot shown in FIG. 20C is described as "control system 10-4 of the continuum robot".
  • FIGS. 20A to 20C the same components as those shown in FIG. 1 are designated by the same reference numerals, and detailed description thereof will be omitted.
  • FIG. 20A shows how the gain of the force control unit K F is multiplied by the above-mentioned coefficient (first coefficient) ⁇ to obtain the first gain.
  • the coefficient (first coefficient) ⁇ is input from the input device 400 to the force control unit K F.
  • the coefficient ⁇ is 1, the gain (second gain) of the position control unit K SV is increased in small steps and the response of the open-loop transfer function G cl K SV is calculated.
  • the procedure of confirming that the servo band is the same as that of the above may be repeated.
  • the coefficient ⁇ is too small, the position control unit K SV has to have a high gain, which excites the higher-order mode and makes the control system unstable. Therefore, the lower limit of the coefficient ⁇ may be set in advance.
  • a coefficient (second coefficient) to be multiplied by the gain of the position control unit K SV is calculated in advance and saved in the memory so that the servo band of the position control unit K SV is always equal according to the coefficient ⁇ . .. In this case, for example, as in the control system 10-3 of the continuum robot shown in FIG.
  • the coefficient (second coefficient) to be multiplied by the gain of the position control unit K SV is read from the memory 220 according to the coefficient ⁇ . Just do it.
  • the back driveability can be changed in real time by adding a knob or the like that allows the operator to arbitrarily command the coefficient ⁇ to the operation system of the continuous robot 100.
  • FIG. 20C shows the block diagram.
  • the coefficient calculation unit f ⁇ is a means for calculating and changing the coefficient (first coefficient) ⁇ according to the target bending angle ref ⁇ in the bendable unit 110. Represents.
  • FIG. 21 is a diagram showing a simulation response by the control system according to the control system 10 of the continuous robot according to the fourth embodiment of the present invention. Similar to the first embodiment, for example, the disturbance torque shown in FIG. 12F is applied to the tip of the bendable portion 110 in 1.5 seconds after the positioning is completed.
  • the bending angle of the tip portion of the bendable portion 110 is shown, and the responses with the coefficients ⁇ of 1, 0.6, 0.3, and 0.1 are shown by dotted lines, solid lines, broken lines, and alternate long and short dash lines, respectively. ing. It can be seen that the back drivability decreases as the coefficient ⁇ decreases, although the positioning performance does not change with the change in the coefficient ⁇ . From this, it can be seen that a control system capable of adjusting the magnitude of back drivability can be realized without changing the positioning performance.
  • the displacements z t1 and z t 2 on the wire gripping mechanism base and the tip side are used as the observed quantities.
  • a displacement sensor that can realize it. Therefore, in this embodiment, a control system using another observable will be described.
  • the generated force F may be used as the observable amount.
  • the displacement z t2 of the wire gripping mechanism is obtained by the following equation (36).
  • the control amount of the outer loop control system (second loop control system) is changed to the displacement z of the wire gripping mechanism base. It may be t1.
  • the rotation angle ⁇ of the motor is ⁇ .
  • control system was designed for the in-plane driven continuous robot 100.
  • a control system is designed for a continuous robot that can be driven three-dimensionally.
  • FIG. 22 is a diagram showing a first example of a schematic configuration of the continuum robot 100 according to the sixth embodiment of the present invention.
  • the continuum robot 100 shown in FIG. 22 will be described as “continuum robot 100-2”.
  • wires 1011 to 1013 are connected to fixed portions 1021 to 1023 at the distal end 1060 of the bendable portion 1100. Further, wire gripping pipes 1031 to 1033 are connected to the proximal ends of the wires 1011 to 1013. Similar to the first embodiment, the robot base portion 1040 is provided with a wire gripping mechanism (not shown in FIG. 22) and an actuator (not shown in FIG. 22) for each wire in the wires 1011 to 1013. There is.
  • the wire gripping mechanism is connected to the actuator via a wire gripping mechanism base (not shown in FIG. 22) and can move up and down.
  • the wire gripping pipes 1031 to 1033 are connected to the wire gripping mechanism, and the posture is controlled by pushing and pulling with the actuator.
  • the continuous robot 100-2 has wire guides 1061 to 1064 which are members for guiding the wires 1011 to 1013.
  • a continuous member such as a bellows shape or a mesh shape may be used.
  • the wire guides 1061 to 1064 are fixed to the wire 1011 at the fixing portions 1050 to 1053. Further, the distance between the wires 1011 to 1013 and the distance between the wire gripping pipes 1031 to 1033 may be different.
  • the diameter conversion unit 1190 may be connected to the robot base unit 1040. Further, in FIG. 22, the central axis of the continuum robot 100-2 is shown by a broken line.
  • the mechanism including the wires 1011 to 1013 and the wire guides 1061 to 1064 is referred to as a bendable portion 1100 which is a continuum portion.
  • an actuator (not shown) an actuator unit including a rotary motor and a rotary linear motion conversion mechanism shall be used.
  • the wire gripping mechanism shall have a function of detecting the tension of the wire.
  • the above-mentioned 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. ..
  • l d Length of the central axis of the bendable part 1100 ⁇ n : Curvature angle of the distal end of the bendable part 1100 ⁇ n : Turning angle of the distal end of the bendable part 1100 ⁇ n : Radius of curvature of the bendable part 1100
  • the wires 1011 to 1013 are referred to as a wire, b wire, and c wire counterclockwise in the xy plane, and the driving displacements of the a to c wires 1011 to 1013 are l p1a , l p1b , l. Let it be p1c.
  • FIG. 23 is a diagram showing an example of wire arrangement of the continuum robot 100 according to the sixth embodiment of the present invention.
  • a ⁇ c wires 1011-1013 are the length of the piece are arranged at the vertices of an equilateral triangle with r s, the phase angle xi] n, an angle that determines the placement of the wire be.
  • ⁇ 1 0.
  • the kinematics of the continuum robot 100-2 is derived by making the following assumptions. [1] At each bendable portion 110, the wire is deformed to have a constant curvature. [2] Twisting deformation of the wire is not considered. [3] The wire does not deform in the longitudinal direction. [4] Friction between the wire guide and the wire is not considered.
  • distributed control is performed independently by applying the double loop control of the first to fifth embodiments to each of the wires 1011 to 1013 of the a to c wires.
  • FIG. 24 is a diagram showing an example of a schematic configuration of a control system 10 of a continuum robot including the continuum robot 100-2 shown in FIG. 22 according to the sixth embodiment of the present invention.
  • control system 10 of the continuous robot shown in FIG. 24 will be described as "control system 10-5 of the continuous robot".
  • FIG. 24 the same components as those shown in FIG. 1 are designated by the same reference numerals, and detailed description thereof will be omitted.
  • the target bending angle Refshita 1 and target turning angle Refzeta 1 of the distal end of the bendable portion 110 is inputted from the input device 510 to the kinematics calculation unit 210.
  • the target displacement ref za of each wire gripping mechanism, ref zb, ref zc is (38) bending angle theta 1 of formula, the turning angle zeta 1, substituting each target bending angle Refshita 1, the target turning angle Refzeta 1 Obtained by doing.
  • the dynamic model P na is a model obtained by fixing the wire gripping pipes of the b wire and the c wire to the actuator base and driving the a wire as equivalent to the dynamic model P n of the first embodiment. be.
  • the dynamic model P nb is obtained as equivalent to the dynamic model P n of the first embodiment by fixing the wire gripping pipes of the a wire and the c wire to the actuator base and driving the b wire. It is a model that can be used.
  • the dynamic model P nc is obtained as equivalent to the dynamic model P n of the first embodiment by fixing the wire gripping pipes of the a wire and the b wire to the actuator base and driving the c wire. It is a model that can be used.
  • K Fa , K Fb , and K Fc are force control units for the a wire, b wire, and c wire.
  • This force control unit may be designed individually, or may be the same control system as long as there is no difference in the dynamic models P na , P nb , and P nc.
  • K SVa , K SVb , and K SVc are position control units for the a wire, b wire, and c wire.
  • This position control unit may be designed individually, or may be the same control system as long as there is no difference in the dynamic models P na , P nb , and P nc.
  • the coefficients ⁇ a , ⁇ b , and ⁇ c are the coefficients for making the back drivability variable, which are shown in the fourth embodiment.
  • the coefficient ⁇ a is input from the input device 520 to the force control unit K Fa or the like
  • the coefficient ⁇ b is input from the input device 530 to the force control unit K F b or the like
  • the coefficient ⁇ c is , Is input from the input device 540 to the force control unit K Fc or the like.
  • the coefficients ⁇ a , ⁇ b , and ⁇ c are set to different values, the back drivability can be made variable according to the turning direction.
  • the coefficients ⁇ a , ⁇ b , and ⁇ c are all set to the same value, uniform back drivability can be obtained regardless of the turning direction.
  • the back drivability of one of the three wires is significantly reduced, only the back drivability of the continuum robot 100-2 against disturbance in the central axis direction is lowered, and the disturbance torque in the bending direction is reduced. It can have back drivability. This prevents the bendable portion 1100 from colliding with the base portion, and conversely, the bendable portion 1100 is extended in the positive z-axis direction and the wire gripping mechanism collides with the base portion. It can be avoided.
  • one wire or a wire gripping pipe is used as the actuator base. It should be fixed to.
  • FIG. 25 is a diagram showing a second example of the schematic configuration of the continuum robot 100 according to the sixth embodiment of the present invention.
  • the continuum robot 100 shown in FIG. 25 will be described as "continuum robot 100-3".
  • FIG. 25 shows an example of fixing the a wire 1011 to the fixing portion 1191.
  • b wire of the driving displacement l p1b, driving displacement l p1c of c wire respectively, the following equation (39) And (40).
  • FIG. 26 is a diagram showing an example of a schematic configuration of a control system 10 of a continuum robot including the continuum robot 100-3 shown in FIG. 25 according to the sixth embodiment of the present invention.
  • the target displacements ref zb and ref zc of each wire gripping mechanism are the bending angle ⁇ 1 and the turning angle ⁇ 1 of the equations (39) and (40), the target bending angle ref ⁇ 1 and the target turning angle ref ⁇ 1. It is obtained by substituting.
  • FIG. 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.
  • the continuum robot 100 shown in FIG. 27 will be described as “continuum robot 100-4”.
  • three bendable portions 1100-1 to 1100-3 are shown.
  • the drive displacements of the wires driving the nth bendable portion are l pna , l pnb , l pnc, and the a wire, b wire, and c of the continuum robot 100-4 having a plurality of bendable portions 1100.
  • each wire is driven by a double loop control system as in the sixth embodiment.
  • a wire of each bendable portion is fixed in the same manner as in the sixth embodiment, and the a wire of each bendable portion is fixed in the direction of the central axis of the continuous robot 100-4.
  • the back drivability may be set to 0.
  • the drive displacement l pnb of the b wire and the drive displacement l pnc of the c wire at this time are the following equations (43) and (44), respectively.
  • the present invention supplies a program that realizes one or more functions of the above-described embodiment to a system or device via a network or storage medium, and one or more processors in the computer of the system or device reads and executes the program. It can also be realized by the processing to be performed. It can also be realized by a circuit (for example, ASIC) that realizes one or more functions.
  • This program and a computer-readable storage medium that stores the program are included in the present invention.
  • a form in which the functional component of the control device 200 of the continuous robot is incorporated into the continuous robot 100 is also applicable to the present invention.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Surgery (AREA)
  • Optics & Photonics (AREA)
  • Biomedical Technology (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Physics & Mathematics (AREA)
  • Pathology (AREA)
  • Radiology & Medical Imaging (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Biophysics (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Medical Informatics (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Manipulator (AREA)
  • Feedback Control In General (AREA)

Abstract

操作者が煩雑な操作を行わずに、湾曲可能部の目標位置への高い位置決め性能を実現できる仕組みを提供する。ワイヤで駆動され、湾曲可能に構成された少なくとも１つの湾曲可能部と、ワイヤを駆動する駆動部とを備える連続体ロボットの制御システム１０において、駆動部によるワイヤの押し引き駆動の目標変位ｒｅｆzと、連続体ロボット１００から得られるワイヤを把持するワイヤ把持機構の変位ｚt2との誤差を補償する制御を行う位置制御部ＫSVと、位置制御部ＫSVから出力されるワイヤの目標張力に相当する目標発生力ｒｅｆFと、連続体ロボット１００から得られるワイヤの張力に対応する発生力Ｆとの誤差を補償する制御を行う力制御部ＫFと、を備え、力制御部ＫFを含む第１のループ制御系と、力制御部ＫF及び位置制御部ＫSVを含む第２のループ制御系とが構成されている。

Description

連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット
本発明は、連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボットに関するものである。
連続体ロボットは、コンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ湾曲可能部を有し、その湾曲可能部を変形させることにより形状が制御される。この連続体ロボットは、剛体リンクによって構成されるロボット（以下、「剛体リンクロボット」と記載する）に対して、主に２つの優位性を持つ。１つ目の優位性は、連続体ロボットは、剛体リンクロボットがはまり込んでしまうような狭い空間や散乱物のある環境の中で、その先端の操作のみで対象物の曲線に沿って移動可能である点である。２つ目の優位性は、連続体ロボットは、本質的な柔らかさを有するため、特に開空間においては脆弱な対象物に損傷を与えることなく操作することができる点である。そこでは、剛体リンクロボットで必要とされるエンドエフェクタでの外力検出は、必ずしも必要とされない。
この特徴を生かし、連続体ロボットは、内視鏡のシースやカテーテルなどの医療の分野や、レスキューロボットなどの極限作業ロボットへの応用が期待されている。この連続体ロボットの駆動方法としては、腱駆動によるもの、押し引き可能なワイヤによるもの、空気アクチュエータによるものなどがある。
特許文献１には、駆動用のワイヤに発生する負荷量を検出し、負荷量が予め設定された範囲となるように、駆動部を制御するマニピュレータの制御方法が記載されている。これにより、特許文献１では、マニピュレータを狭小空間に挿通する際に、その経路に沿うための湾曲操作をすることなく挿入が可能とし、挿通動作を容易にしている。ここで、駆動部が負荷量に応じて受動的に動作することを「バックドライバビリティがある」というが、特許文献１では、クラッチなどの機構ではなく、ワイヤに発生する負荷量の検出と制御アルゴリズムを用いており、駆動部の大型化を防いでいる。具体的に、特許文献１に記載のマニピュレータは、筒状部とよばれる連続体と、その先端の複数の関節構造からなり、関節に連結されるワイヤを基端部筐体に設けられたモータにより牽引することで動作し、また、複数の節輪と回動関節で連結される連続体に対して適用可能としている。
特許第６１６９０４９号公報
しかしながら、特許文献１に記載のマニピュレータでは、上述した挿通動作を行う「挿通動作モード」と、操作者が湾曲操作ボタンを用いて先端湾曲部の操作を行う「動作制御モード」との切り替えが必要であり、また、駆動部がバックドライバビリティを持つのは「挿通動作モード」時のみである。そして、特許文献１に記載のマニピュレータにおいて、「挿通動作モード」時には、湾曲操作ボタンが無効化され、先端湾曲部の操作を行うことはできない。これにより、特許文献１に記載のマニピュレータでは、分岐がある狭小な経路の選択をするときや、緩やかな経路と急峻な経路とが繰り返されるときには、上述した切り替え操作が煩雑となるとともに、例えば大湾曲動作時にバックドライバビリティが低く湾曲可能部の目標位置への位置決め性能が低いと、誤った操作により脆弱な対象物やマニピュレータ自体が損傷するという問題がある。
本発明は、このような問題点に鑑みてなされたものであり、操作者が煩雑な操作を行わずに、湾曲可能部の目標位置への高い位置決め性能を実現できる仕組みを提供することを目的とする。
本発明の連続体ロボットの制御システムは、ワイヤで駆動され、湾曲可能に構成された少なくとも１つの湾曲可能部と、前記ワイヤを駆動する駆動部とを備える連続体ロボットの制御システムであって、前記駆動部による前記ワイヤの押し引き駆動の目標変位と、前記連続体ロボットから得られる前記ワイヤを把持するワイヤ把持機構の変位との誤差を補償する制御を行うものであって、前記ワイヤの目標張力を出力する位置制御部と、前記位置制御部から出力される前記ワイヤの目標張力と、前記連続体ロボットから得られる前記ワイヤの張力との誤差を補償する制御を行う力制御部と、を有し、前記力制御部を含む第１のループ制御系と、前記力制御部および前記位置制御部を含む第２のループ制御系とが構成されている。また、本発明は、上述した連続体ロボット及び上述した連続体ロボットの制御システムによる連続体ロボットの制御方法を含む。
本発明によれば、操作者が煩雑な操作を行わずに、湾曲可能部の目標位置への高い位置決め性能を実現することができる。
本発明の第１の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの概略構成の一例を示す図である。 本発明の第１の実施形態における連続体ロボットの動的モデルの一例を示す図である。 本発明の第１の実施形態におけるアクチュエータに含まれる回転モータ及び回転直動変換機構とワイヤ把持機構等からなる動的モデルの一例を示す図である。 本発明の第１の実施形態における回転モータ及び回転直動変換機構とワイヤ把持機構等からなる動的モデルと連続体ロボットの動的モデルとの結合の一例を示す図である。 本発明の第１の実施形態における連続体ロボットの動的モデルの極－零点応答（極－零点配置）の一例を示す図である。 本発明の第１の実施形態における連続体ロボットの動的モデルを示すボード線図である。 本発明の第１の実施形態における連続体ロボットの動的モデルを示すボード線図である。 本発明の第１の実施形態における連続体ロボットの動的モデルを示すボード線図である。 本発明の第１の実施形態における連続体ロボットの力制御部を示すボード線図である。 本発明の第１の実施形態における連続体ロボットの開ループ伝達関数を示すボード線図である。 本発明の第１の実施形態における連続体ロボットの位置制御部を示すボード線図である。 本発明の第１の実施形態における連続体ロボットの開ループ系伝達関数を示すボード線図である。 本発明の第１の実施形態における連続体ロボットの制御装置に係る制御系の設計手順を示すフローチャートである。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）のみのモータの回転角度についての外乱応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）のみのワイヤ把持機構の変位についての外乱応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）のみの発生力についての外乱応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）のみの制御入力についての外乱応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）のみの湾曲角度についての外乱応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）のみの外乱トルクについての外乱応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系によるモータの回転角度についての外乱を伴う位置決め応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系によるワイヤ把持機構の変位についての外乱を伴う位置決め応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による発生力についての外乱を伴う位置決め応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による制御入力についての外乱を伴う位置決め応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による湾曲角度についての外乱を伴う位置決め応答を示す図である。 本発明の第１の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による外乱トルクについての外乱を伴う位置決め応答を示す図である。 本発明の第２の実施形態における連続体ロボットの制御装置に係る制御系のパラメータ探索を示す図である。 本発明の第２の実施形態における連続体ロボットの制御装置に係る力制御部を示すボード線図である。 本発明の第２の実施形態における連続体ロボットの制御装置に係る開ループ伝達関数を示すボード線図である。 本発明の第２の実施形態における連続体ロボットの制御装置に係る制御系の設計手順を示すフローチャートである。 本発明の第３の実施形態における連続体ロボットの制御装置に係る制御系のパラメータ探索を示す図である。 本発明の第３の実施形態における連続体ロボットの制御装置に係る力制御部を示すボード線図である。 本発明の第３の実施形態における連続体ロボットの制御装置に係る開ループ伝達関数を示すボード線図を示す図である。 本発明の第３の実施形態における連続体ロボットの制御装置に係る制御系の設計手順を示すフローチャートである。 本発明の第３の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系によるモータの回転角度についての外乱を伴う位置決め応答を示す図である。 本発明の第３の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系によるワイヤ把持機構の変位についての外乱を伴う位置決め応答を示す図である。 本発明の第３の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による発生力についいての外乱を伴う位置決め応答を示す図である。 本発明の第３の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による制御入力についての外乱を伴う位置決め応答を示す図である。 本発明の第３の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による湾曲角度についての外乱を伴う位置決め応答を示す図である。 本発明の第３の実施形態を示し、力制御部を用いるインナーループ制御系（第１のループ制御系）と更に位置制御部を含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による外乱トルクについての外乱を伴う位置決め応答を示す図である。 本発明の第４の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第４の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第４の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第４の実施形態に係る連続体ロボットの制御システムに係る制御系によるシミュレーション応答を示す図である。 本発明の第６の実施形態に係る連続体ロボットの概略構成の第１例を示す図である。 本発明の第６の実施形態に係る連続体ロボットのワイヤの配置の一例を示す図である。 本発明の第６の実施形態に係る図２２に示す連続体ロボットを含む連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第６の実施形態に係る連続体ロボットの概略構成の第２例を示す図である。 本発明の第６の実施形態に係る図２５に示す連続体ロボットを含む連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第７の実施形態に係る連続体ロボット１００の概略構成の第２例を示す図である。
以下に、図面を参照しながら、本発明を実施するための形態（実施形態）について説明する。
（第１の実施形態）
まず、本発明の第１の実施形態について説明する。
図１は、本発明の第１の実施形態に係る連続体ロボットの制御システム１０の概略構成の一例を示す図である。以下、図１に示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－１」として記載する。
連続体ロボットの制御システム１０－１は、図１に示すように、連続体ロボットの機構部（以下、単に「連続体ロボット」と記載することもある）１００、連続体ロボットの制御装置２００、及び、入力装置３００を有して構成されている。また、連続体ロボットの制御装置２００は、図１に示すように、運動学演算部２１０、位置制御部ＫSV、力制御部ＫFを有して構成されている。
本実施形態では、図１に示す力制御部ＫFを含むインナーループ制御系（第１のループ制御系）で連続体ロボット１００に対する力制御を行う。また、図１に示す力制御部ＫF及び位置制御部ＫSVを含むアウターループ制御系（第２のループ制御系）で連続体ロボット１００に対する位置制御を行う。本実施形態に係る連続体ロボットの制御システム１０－１では、このインナーループ制御系（第１のループ制御系）及びアウターループ制御系（第２のループ制御系）の二重ループ制御系により、連続体ロボット１００に対して、バックドライバビリティが高く、同時に、連続体ロボット１００における湾曲可能部の目標位置への高い位置決め性能を実現する。
第１の実施形態では、ワイヤで駆動され、湾曲可能に構成された湾曲可能部と、ワイヤを駆動する駆動部とを備えた連続体ロボット１００の動的モデルＰnを導出する。この際、駆動部は、ワイヤの張力の検出が可能であり、例えば、回転モータと回転直動変換機構を含み構成されうる。
運動学演算部２１０は、入力装置３００から、連続体ロボット１００の湾曲可能部（後述する図２の湾曲可能部１１０）における目標湾曲角度ｒｅｆθが入力されると、運動学演算を行い、連続体ロボット１００の駆動部によるワイヤの押し引き駆動の目標変位ｒｅｆzを出力する。位置制御部ＫSVは、運動学演算部２１０から出力された目標変位ｒｅｆzと、連続体ロボット１００の変位センサから得られるワイヤを把持するワイヤ把持機構の変位ｚt2との誤差を補償する制御を行うものであって、ワイヤの目標張力に対応する目標発生力ｒｅｆFを出力する。ここで、目標発生力ｒｅｆFは、本実施形態の中で定義するワイヤ張力センサの正負反転値である発生力Ｆの目標値を表す。力制御部ＫFは、位置制御部ＫSVから出力された目標発生力ｒｅｆFと、連続体ロボット１００から得られるワイヤの張力に対応する発生力Ｆとの誤差を補償する制御を行うものであって、駆動部の目標トルクであるモータトルクＴmを出力する。そして、連続体ロボット１００の回転モータ等に制御入力を与える。
上述したように、力制御部ＫFを含むインナーループ制御系（第１のループ制御系）では、目標発生力ｒｅｆFと連続体ロボット１００のワイヤ張力センサから得られる発生力Ｆとの差分を取ることで誤差を演算し、力制御部ＫFは、その誤差を補償する制御入力としてモータトルクＴmを出力する。このフィードバックループ制御系は、回転直動変換機構の等価慣性を補償することと等価であり、これにより連続体ロボット１００のバックドライバビリティを向上することを可能とする。また、本実施形態では、このインナーループ制御系（第１のループ制御系）のフィードバックによる閉ループ制御系をＧclと表す。また、この閉ループ制御系Ｇclと位置制御部ＫSVを含むアウターループ制御系（第２のループ制御系）では、目標変位ｒｅｆzと連続体ロボット１００の変位センサから得られる変位ｚt2との差分を取ることで誤差を演算し、位置制御部ＫSVは、その誤差を補償する制御入力として目標発生力ｒｅｆFを出力する。ここで、力制御部ＫF及び位置制御部ＫSVの設計には、連続体ロボット１００の動的モデルＰnから得られる伝達関数を用いることでフィードバック系の安定性を保証する。これにより、連続体ロボット１００における湾曲可能部の外乱に対する高いバックドライビリティを有しつつ、目標位置への高い位置決め性能を実現することができる。
以下に、連続体ロボット１００の動的モデルＰnの導出と制御アルゴリズムについて詳細に記述する。
１．１）モデリング
図２は、本発明の第１の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。以下、図２に示す連続体ロボット１００を「連続体ロボット１００－１」として記載する。
連続体ロボット１００－１は、湾曲可能部１１０の遠位端１６０における固定部１２１及び固定部１２２に、それぞれ、ワイヤ１１１及びワイヤ１１２が接続されている。また、ワイヤ１１１及びワイヤ１１２の近位端は、ロボット基台部１４０の中で、それぞれ、ワイヤ把持パイプ１３１及びワイヤ把持パイプ１３２に接続されている。ロボット基台部１４０には、ワイヤ把持パイプ１３１を支持するワイヤ把持機構１７１、ワイヤ把持パイプ１３２を支持するワイヤ把持機構１７２、及び、駆動部に相当するアクチュエータ１８０が配置されている。この際、ワイヤ把持機構１７２は、ロボット基台部１４０に固定されている。また、ワイヤ把持機構１７１は、駆動部であるアクチュエータ１８０にワイヤ把持機構基台（不図示）を介して接続され、上下に移動可能である。ワイヤ把持パイプ１３１は、ワイヤ把持機構１７１に支持可能に接続され、アクチュエータ１８０で押引きすることにより湾曲可能部１１０の姿勢が制御される。また、連続体ロボット１００－１は、ワイヤ１１１及びワイヤ１１２を案内するための部材であるワイヤガイド１６１～１６４を有して構成されている。このワイヤガイドは、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤガイド１６１～１６４は、ワイヤ１１２に、固定部１５０～１５３において固定される。さらに、ワイヤ１１１及びワイヤ１１２の間隔と、ワイヤ把持機構１７１及びワイヤ把持機構１７２との間隔が異なってもよい。そのときは、径変換部１９０をロボット基台部１４０に接続する。
本実施形態では、ワイヤ１１１及びワイヤ１１２と、ワイヤガイド１６１～１６４からなる機構を連続体部分である湾曲可能部１１０とよぶ。また、アクチュエータ１８０には、回転モータと回転直動変換機構からなるアクチュエータユニットを用いるものとする。さらに、ワイヤ把持機構１７１（或いはアクチュエータ１８０）は、ワイヤ１１１の張力を検出する機能を持つものとする。そのためには、ワイヤ把持機構１７１とアクチュエータ１８０との間に、上述したワイヤ把持機構基台（不図示）を設け、このワイヤ把持機構基台をアクチュエータ１８０に接続し、ワイヤ把持機構基台とワイヤ把持機構１７１との間をばねで接続する。このとき、ワイヤ把持機構１７１がｚ軸方向にのみ変位するように、リニアガイドを設けることや、ばねに平行ばねを用いることが好ましい。そして、ばねの変位を計測することでワイヤ１１１の張力が検出可能となる。
図３Ａ～図３Ｃは、本発明の第１の実施形態における連続体ロボット１００の動的モデルＰnの一例を示す図である。具体的に、図３Ａは、連続体部分である湾曲可能部１１０の動的モデルの一例を示し、また、図３Ｂは、アクチュエータ１８０に含まれる回転モータ及び回転直動変換機構とワイヤ把持機構１７１等からなる動的モデルの一例を示す。
１．１．１）連続体部分である湾曲可能部１１０の動的モデル
図３Ａに示す連続体部分である湾曲可能部１１０の動的モデルにおける符号の定義を以下に記載する。
θ：連続体ロボットの湾曲可能部１１０の湾曲角度
ρ：曲率半径
ｌ：湾曲可能部１１０の長さ
ｒ1：湾曲可能部１１０の中心軸からワイヤまでの距離
ｌp：ワイヤの駆動量
ｍs：湾曲可能部１１０の質量
ｋb：湾曲可能部１１０の湾曲角度に対するばね係数
ｚw：ワイヤ等価質量の変位
ｚp：ワイヤ把持パイプの変位
ｍw：ワイヤの質量
ｍp：ワイヤ把持パイプの質量
ｋw，ｋw3：ワイヤのばね係数
ｃw，ｃw3：ワイヤの減衰係数
ｃw2：ワイヤと径変換部及びワイヤガイドとの摩擦による減衰係数
続いて、連続体ロボット１００－１の運動方程式を導出する。本実施形態では、以下の仮定をおく。
［１］２次元平面内での運動のみを考慮する。
［２］湾曲可能部１１０の曲率は一定とし、ばね係数は均一であるものとする。
［３］ワイヤは集中質点系として近似し、長手方向の変形による反力が湾曲可能部１１０の先端に作用するものとする。ワイヤの横振動及び横方向の変形は考慮しない。
［４］ワイヤとワイヤガイドとの間、及び、ワイヤと径変換部との間の摩擦は、クーロン摩擦等の非線形性摩擦を含めて粘性減衰として減衰係数ｃw2に押し込める。 
まず、湾曲可能部１１０の運動エネルギーを求める。湾曲可能部１１０の中心軸上に変位ｘg及びｚgをとると、それぞれ、以下の（１）式及び（２）式となる。 
Figure JPOXMLDOC01-appb-M000001
また、以下の（３）式
Figure JPOXMLDOC01-appb-M000002
 であるから、変位ｘg及びｚgは、それぞれ、以下の（４）式及び（５）式となる。 
Figure JPOXMLDOC01-appb-M000003
 これを用いて、湾曲可能部１１０の運動エネルギーＴaは、以下の（６）式となる。 
Figure JPOXMLDOC01-appb-M000004
また、湾曲可能部１１０の位置エネルギーＵaは、以下の（７）式となる。 
Figure JPOXMLDOC01-appb-M000005
次に、ワイヤ及びワイヤ把持パイプの運動エネルギーＴwと位置エネルギーＵwは、ワイヤ駆動量がｌp＝ｒ1θであるから、それぞれ、以下の（８）式及び（９）式となる。 
Figure JPOXMLDOC01-appb-M000006
 そして、運動方程式をラグランジュの方程式を示す以下の（１０）式
Figure JPOXMLDOC01-appb-M000007
 より求めると、以下の（１１）式に示す非線形微分方程式が得られる。以下の（１１）式において、Φは、Ｍc，Ｃc，Ｋcに入れることのできない非線形項とする。 
Figure JPOXMLDOC01-appb-M000008
 また、（１１）式のＭc22及びΦ3は、以下の（１２）式及び（１３）式となる。 
Figure JPOXMLDOC01-appb-M000009
しかしながら、湾曲可能部１１０の湾曲角度が０度の近傍では、（１２）式及び（１３）式は不定形となり、値を求めることができない。そこで、大変形を考慮しない０度近傍線形化モデルを求める。（４）式及び（５）式より、以下の（１４）式となる。
Figure JPOXMLDOC01-appb-M000010
 そして、湾曲可能部１１０の０度近傍での運動エネルギーＴa0は、以下の（１５）式となる。
Figure JPOXMLDOC01-appb-M000011
 そして、Ｔ＝Ｔa0＋Ｔwとおいて、ラグランジュの方程式より運動方程式を求めると、以下の（１６）式となる。
Figure JPOXMLDOC01-appb-M000012
１．１．２）回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデル
図３Ｂは、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルの一例を示す図である。ワイヤ把持機構は、ワイヤの張力を検出する機能を持ち、ワイヤ把持機構とアクチュエータ１８０との間にワイヤ把持機構基台を設け、ワイヤ把持機構基台をアクチュエータ１８０に接続し、ワイヤ把持機構基台とワイヤ把持機構との間をばねで接続する。そのばねの変位を検出することにより張力検出を行う。図３Ｂに示す動的モデルにおける符号の定義を以下に記載する。
Ｊm：モータのイナーシャ
θm：モータ回転角度
Ｔm：モータ目標トルク（指令値）
ｃm：モータ軸の減衰係数
ｋg，ｃg：カップリングのばね係数，減衰係数
Ｊn：駆動軸のイナーシャ
θn：駆動軸の回転角度
ｃn：駆動軸の減衰係数
ｐ：駆動軸のねじピッチ
Ｒ：直動回転変換機構の変換係数であって等価的増速比
ｍt1：ワイヤ把持機構基台の質量
ｋt1：駆動軸のｚ方向のばね係数
ｃt1：ワイヤ把持機構基台のリニアスライダの減衰係数
ｚt1：ワイヤ把持機構基台の変位
ｍt2：ワイヤ把持機構の質量
ｋt2，ｃt2：張力検出機構のばね係数
ｚt2：ワイヤ把持機構の変位
そして、運動方程式は、以下の（１７）式～（２０）式のようになる。
Figure JPOXMLDOC01-appb-M000013
 ここでは、Ｒ＝p／２πとおいている。ｑl＝［θm，θn，ｚt1，ｚt2］Tとして行列表記すると、以下の（２１）式となる。
Figure JPOXMLDOC01-appb-M000014
１．１．３）拡大系の構成
図３Ｃに示すように、湾曲可能部１１０の動的モデルと、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルとを結合する。ワイヤ把持機構の変位ｚt2とワイヤ把持パイプの変位ｚpを同一とすることで、（１１）式と（２１）式によって拡大系を構成すると、運動方程式は、以下の（２２）式のようになる。ここで、ｑg＝［ｑl T，ｚw，θ］Tとしている。
Figure JPOXMLDOC01-appb-M000015
次に、状態方程式を求める。湾曲可能部１１０の湾曲角度が０度の近傍にないときは、非線形項Φgを行列Ｋgに押し込めて線形化を行う、以下の（２３）式の拡張線形化法を適用する。
Figure JPOXMLDOC01-appb-M000016
 これにより、Φ3は、Ｋg（６，６）の要素となる。そして、状態方程式は、以下の（２４）式及び（２５）式となる。
Figure JPOXMLDOC01-appb-M000017
本実施形態では、ワイヤ把持機構基台の変位ｚt1及びワイヤ把持機構の変位ｚt2が測定可能とし、ばね変位ｋt2の発生する力がワイヤ把持機構に及ぼす力（以下、発生力）Ｆ、及びワイヤ把持機構の変位ｚt2を観測量とする。ここで、発生力Ｆは、以下の（２６）式となる。
Figure JPOXMLDOC01-appb-M000018
このとき、観測量ygは、以下の（２７）式に示す出力方程式で表される。 
Figure JPOXMLDOC01-appb-M000019
湾曲可能部１１０の湾曲角度が０度の近傍にあるときは、湾曲可能部１１０の０度近傍での運動方程式を示す（１６）式と、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルの運動方程式を示す（２１）式により、同様に拡大系を構成する。これは線形モデルであるので、そのまま状態方程式に変換すればよい。
１．２）モデル解析
１．２．１）等価的増速比Ｒに対する変動
本章では、モデル解析を行い、回転直動変換機構による等価的増速比Ｒによる連続体部分である湾曲可能部１１０の極と零点の変動について検討する。本実施形態では、さらに、ワイヤと径変換部及びワイヤガイドとの減衰係数ｃw2による位相の変動について説明する。ここで、湾曲可能部１１０の最低次の固有振動数は６２Ｈｚであものとする。このとき、回転直動変換機構の等価的増速比Ｒを設計パラメータの１倍から１００００倍まで変化させることで回転直動変換機構の等価慣性を変化させ、極と零点の変動を調べる。また、この解析では、極と零点の変動を分かりやすくするために、湾曲可能部１１０の各部の減衰係数をほぼ０としている。
図４は、本発明の第１の実施形態における連続体ロボット１００の動的モデルＰnの極－零点応答（極－零点配置）の一例を示す図である。この図４において、湾曲可能部１１０単体の極を星印で示し、等価的増速比Ｒを１００００倍とする極と零点をそれぞれアスタリスクと丸印で示し、等価的増速比Ｒを１倍とする極と零点をそれぞれＸ印と三角印で示す。これより、モータ側の等価慣性が大きくなるにつれて湾曲可能部１１０の固有振動数は低下し、過減衰の極と、原点極に移動することがわかる。ここで、丸印及び三角印で示す零点は、ほぼ移動していないことに注目されたい。湾曲可能部１１０の振動数は過減衰となるが、この零点による制御帯域内での位相特性を制御系設計に考慮する必要がある。
図５は、本発明の第１の実施形態における連続体ロボット１００の動的モデルＰnを示すボード線図である。具体的には、図５は、等価的増速比Ｒを変動させる拡大系のボード線図を示す。ここで、拡大系は、（２４）式、（２７）式を、湾曲角度θを０．０５度として線形化している。図５では、制御入力であるモータ目標トルクＴmから発生力Ｆまでの伝達関数を示し、等価的増速比Ｒを１０倍、５０倍、５００倍とする応答を、それぞれ、実線、破線、一点鎖線で示している。等価的増速比Ｒが小さくなり、モータ側の等価慣性が大きくなるにつれて、連続体のピークが減少していき、また、反共振周波数は変わらないことがわかる。実線で示す等価的増速比Ｒを１０倍する応答では、約１８Ｈｚにある反共振の要因がわかりづらいが、破線で示す等価的増速比Ｒを５００倍とする応答では、反共振は湾曲可能部１１０と張力検出機構を結合した拡大系における連続体部分の共振・反共振特性によるものであることがわかる。そして、この反共振周波数において位相が１８０度進むことがわかる。
１．２．２）ワイヤと径変換部及びワイヤガイドとの摩擦による減衰係数ｃw2に対する変動
次に、減衰係数ｃw2によるモデルの変動について検討する。図６は、本発明の第１の実施形態における連続体ロボット１００の動的モデルＰnを示すボード線図である。具体的には、図６は、減衰係数ｃw2を変動させる拡大系のボード線図を示す。図６では、制御入力であるモータトルクＴmから発生力Ｆまでの伝達関数を示し、減衰係数ｃw2を０．０１，１，１０，１５０とする応答を、それぞれ、実線、点線、破線、一点鎖線で示している。減衰係数ｃw2が大きくなるに従って、低周波数域が積分特性から１次遅れ特性へと変化する。これに伴い、約１８Ｈｚの零点周波数付近を境に位相が１８０度反転してゆく。これが目標とするサーボ帯域付近であれば、制御系の設計に大きく影響する。回転直動変換機構を有する連続体ロボット１００－１の制御系設計においては、湾曲可能部１１０の固有振動数に加え、回転直動変換機構及び径変換部のモデル同定が重要となることがわかる。
図７は、本発明の第１の実施形態における連続体ロボット１００の動的モデルＰnを示すボード線図である。具体的には、図７は、拡大系のボード線図を示す。モータトルクＴmから発生力Ｆまでの伝達関数を実線で示し、モータトルクＴmからワイヤ把持機構の変位ｚt2までの伝達関数を破線で示し、モータトルクＴmから湾曲可能部１１０の湾曲角度θまでの伝達関数を一点鎖線で示している。高周波数域には、ワイヤ張力検出機構のばね、ワイヤ、回転直動変換機構による高次振動が現れている。
１．３）制御系設計
本章では、図１に示すように、力制御部ＫFを含むインナーループ制御系（第１のループ制御系）で力制御を行い、力制御部ＫF及び位置制御部ＫSVを含むアウターループ制御系（第２のループ制御系）で位置制御を行う、二重ループ制御系によって、連続体ロボット１００－１に対して、バックドライバビリティが高く、同時に、連続体ロボット１００における湾曲可能部の目標位置への高い位置決め性能を実現する制御系の設計を行う。ここで、動的モデルＰnは、（２４）式，（２７）式に示した拡大系を表す。
ワイヤの駆動量ｌpと湾曲可能部１１０の湾曲角度θとの関係は、以下の（２８）式で与えられる。
Figure JPOXMLDOC01-appb-M000020
本実施形態では、ワイヤの伸縮量は微小と考え、運動学演算部２１０による運動学の導出に考慮しない。そのため、目標変位ｒｅｆzは、以下の（２９）式で与えられる。 
Figure JPOXMLDOC01-appb-M000021
ここで、拡大系の動的モデルＰnと力制御部ＫFを含むインナーループ制御系（第１のループ制御系）では、発生力Ｆの目標値である目標発生力ｒｅｆFと発生力Ｆとの差分を取ることで誤差を演算し、力制御部ＫFは、その誤差を補償する制御入力としてモータトルクＴmを出力する。このフィードバックループは、回転直動変換機構の等価慣性を補償することと等価であり、これにより、連続体ロボット１００－１のバックドライバビリティを向上することを可能とする。また、本実施形態では、上述したように、このフィードバックによる閉ループ系をＧclと表す。また、この閉ループ制御系Ｇclと位置制御部ＫSVを含むアウターループ制御系（第２のループ制御系）では、目標変位ｒｅｆzと連続体ロボット１００から得られる変位ｚt2との差分を取ることで誤差を演算し、位置制御部ＫSVは、その誤差を補償する制御入力として目標発生力ｒｅｆFを出力する。 
本実施形態では、力制御を行うインナーループ制御系（第１のループ制御系）には、例えば、以下の（３０）式に示すＰＩ制御系を用いる。
Figure JPOXMLDOC01-appb-M000022
 ここで、Ｆziは、積分制御器の零交差周波数である。また、前章で示した高次モードに対する安定化のために折点周波数２００Ｈｚの２次のローパスフィルタをＰＩ制御系に結合する。回転直動変換機構を用いることによる湾曲可能部１１０の極、零点の変動と、その零点付近で減衰係数ｃw2により変動する位相特性に対して、安定な力制御部ＫFを設計するためには、開ループ伝達関数ＰnＫFを導出し、その応答からゲイン余裕、位相余裕が十分であるように設計する（安定余裕の演算をする）。
図８Ａ及び図８Ｂは、本発明の第１の実施形態における連続体ロボットの制御装置２００を示すボード線図である。具体的に、図８Ａ及び図８Ｂは、（３０）式のＫp，Ｆziを、それぞれ、２．２‐１０-3，１．０とし、制御帯域を約３０Ｈｚとする力制御部ＫFのボード線図、及び、拡大系Ｐnとの開ループ伝達関数ＰnＫFのボード線図を示す。 
また、本実施形態では、位置制御を行うアウターループ制御系（第２のループ制御系）には、例えば、以下の（３１）式に示すＰＩＤ制御系を用いる。
Figure JPOXMLDOC01-appb-M000023
 ここで、Ｆzi及びＦzdは、それぞれ、積分制御器及び微分制御器の零交差周波数である。拡大系の動的モデルＰnと力制御部ＫFとの閉ループ伝達関数Ｇclを用いて、開ループ伝達関数ＧclＫSVを導出し、その応答からゲイン余裕、位相余裕が十分であるように設計する。また、折点周波数５０Ｈｚの１次のローパスフィルタをＰＩＤ制御系に結合する。 
図９Ａ及び図９Ｂは、本発明の第１の実施形態における連続体ロボットの制御装置２００を示すボード線図である。具体的に、図９Ａ及び図９Ｂは、（３１）式のＫp，Ｆzi，Ｆzdを、それぞれ、５‐１０3，０．５，１００とし、制御帯域を約３．５Ｈｚとする位置制御部ＫSVのボード線図、及び、閉ループ制御系Ｇclと位置制御部ＫSVの開ループ伝達関数ＧclＫSVのボード線図を示す。
また、図１０は、本発明の第１の実施形態における連続体ロボットの制御装置２００に係る制御系の設計手順を示すフローチャートである。この図１０には、以下の設計手順が記載されている。
＜１＞拡大系の動的モデルＰnの導出
＜２＞係数Ｋp，Ｋiを設定し、ローパスフィルタを結合し、力制御部ＫFを仮設計＜３＞開ループ伝達関数ＰnＫFを導出
＜４＞ゲイン余裕、位相余裕は十分かの判断
＜５＞力制御部ＫFの設計完了
＜６＞閉ループ系Ｇclを導出
＜７＞係数Ｋp，Ｋi，Ｋdを設定し、ローパスフィルタを結合し、位置制御部ＫSVを仮設計
＜８＞開ループ伝達関数ＧclＫSVを導出
＜９＞ゲイン余裕、位相余裕は十分かの判断
＜１０＞位置制御部ＫSVの設計完了
１．４）シミュレーション
１．３）制御系設計で設計した制御系と、（２４）式，（２７）式に示した拡大系を用いてシミュレーションを行った。
図１１Ａ～図１１Ｆは、本発明の第１の実施形態を示し、力制御部ＫFを用いるインナーループ制御系（第１のループ制御系）のみの外乱応答を示す図である。具体的に、図１１Ａにモータの回転角度θmを示し、図１１Ｂにワイヤ把持機構の変位ｚt2を示し、図１１Ｃに発生力Ｆを示し、図１１Ｄに制御入力Ｔmを示し、図１１Ｅに湾曲可能部１１０の先端の湾曲角度θを示し、図１１Ｆに湾曲可能部１１０の先端に与える外乱トルクを示す。また、図１１Ａ～図１１Ｆでは、制御応答を実線で、比較として非制御時の応答を破線で示し、目標発生力ｒｅｆFを点線で示す。
シミュレーションの開始後、５秒で図１１Ｆに示すように湾曲可能部１１０の先端部に外乱トルクを与える。このとき、図１１Ａ～図１１Ｆに示すように、ワイヤ把持機構には、張力検出機構のばねによりｚ軸負方向の発生力が作用するが、発生力Ｆの目標値は０である。そのため、図１１Ｄに示すように力制御部ＫFは、正の制御入力をモータに与え、図１１Ａに示すようにモータが正方向に回転することで、図１１Ｂに示すようにワイヤ把持機構は、ｚ軸正方向に変位する。これにより、図１１Ｃに示すように発生力Ｆが目標値である０に補償されることで、図１１Ｅに示すように、湾曲可能部１１０の先端は、外乱トルクと同方向に湾曲し、外乱トルクに応じてモータがバックドライブする制御が実現している。破線で示す非制御時の応答では、図１１Ｆの外乱トルクを同様に与えているが、回転直動変換機構の等価慣性が大きいため、図１１Ａ及び図１１Ｅに破線で示すように、モータは、ほとんど回転せず、湾曲可能部１１０の先端はほとんど湾曲しない。これより、力制御を行うインナーループ制御系は、回転直動変換機構の等価慣性を補償し、連続体先端の外乱トルクに対するバックドライブを可能としていることがわかる。
次に、図１２Ａ～図１２Ｆは、本発明の第１の実施形態を示し、力制御部ＫFを用いるインナーループ制御系（第１のループ制御系）と更に位置制御部ＫSVを含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による外乱を伴う位置決め応答を示す図である。具体的に、図１２Ａ～図１２Ｆに、図１１Ａ～図１１Ｆと同様の配置で各応答を示す。このシミュレーションでは、モータの回転角度θmのみ位置制御を行うセミクローズド制御との比較を示す。二重ループ制御系とセミクローズド制御系の位置制御系のサーボ帯域は、同様になるように設計している。図１２Ａ～図１２Ｆでは、二重ループ制御系の制御応答を実線で示し、セミクローズド制御系の応答を破線で示す。また、図１２Ａにセミクローズド制御系に与えるモータの目標角度ｒｅｆmを点線で示し、図１２Ｂにアウターループに与えるワイヤ把持機構の目標変位ｒｅｆzを点線で示し、図１２Ｃに位置制御系ＫSVが出力する制御入力である発生力Ｆの目標値である目標発生力ｒｅｆFを点線で示す。
シミュレーションの開始後、二重ループ制御系とセミクローズド制御系は、ともに目標軌道に追従し、約１秒で目標変位に整定する。その後、１．５秒で図１２Ｆに示すように湾曲可能部１１０の端部に外乱トルクを与える。このとき、図１２Ｃに示すように、アウターループ制御系（第２のループ制御系）の位置制御部ＫSVは、ワイヤ把持機構の変位ｚt2に発生した変位誤差を補償するために、ｚ軸負方向の発生力目標値を制御入力として出力する。図１２Ｃに示すように、インナーループ制御系（第１のループ制御系）の力制御部ＫFは、発生力Ｆの目標値である目標発生力ｒｅｆFに追従する。しかしながら、同時に回転直動変換機構の等価慣性を補償し低減しているため、図１２Ａに示すように、モータは、正方向にバックドライブし、図１２Ｂに示すように、ワイヤ把持機構は、ｚ軸正方向に繰り出される。図１２Ｄの１．５秒での正方向のモータへの制御入力は、等価慣性を低減する制御が行われていることを表している。これにより、図１２Ｅに示すように、湾曲可能部１１０の先端は、外乱トルクと同方向に湾曲し、外乱トルクに対してバックドライブする制御系が実現していることがわかる。破線で示すセミクローズド制御系は、図１２Ｆの外乱トルクが加わると、回転直動変換機構の等価慣性が大きいため、図１２Ａ及び図１２Ｅに破線で示すように、モータは、ほとんど回転せず、湾曲可能部１１０の先端もほとんど湾曲しない。図１２Ｆの発生力Ｆが二重ループ制御系とほぼ等しいのは、位置決め制御系のサーボ帯域がほぼ等しいであるためである。これより、二重ループ制御系は、モータの回転角度θmのみ位置制御を行うセミクローズド制御系と同様の目標位置への整定性能を持ち、同時に、湾曲可能部１１０の先端の外乱トルクに対するバックドライブを可能としていることがわかる。
以上説明したように、第１の実施形態に係る連続体ロボットの制御システム１０では、力制御部ＫFを含むインナーループ制御系（第１のループ制御系）と、力制御部ＫF及び位置制御部ＫSVを含むアウターループ制御系（第２のループ制御系）とを構成するようにしている。かかる構成によれば、操作者が煩雑な操作を行わずに、湾曲可能部の目標位置への高い位置決め性能を実現することができる。その結果、連続体ロボット１００の湾曲可能部先端の外乱に対する高いバックドライビリティを有することができる。
（第２の実施形態）
次に、本発明の第２の実施形態について説明する。なお、以下に記載する第２の実施形態の説明では、上述した第１の実施形態と共通する事項については説明を省略し、上述した第１の実施形態と異なる事項について説明を行う。
上述した第１の実施形態では、連続体部分である湾曲可能部１１０の振動特性が回転直動変換機構と連成することにより、低周波数域に移動し、さらにワイヤと径変換部及びワイヤガイドとの摩擦により位相特性が変動することを示した。また、上述した第１の実施形態では、力制御を行うインナーループ制御系（第１のループ制御系）の力制御部ＫFにＰＩ制御系を用いた。
これに対して、第２の実施形態は、力制御部ＫFにＰＩＤ制御系を用いて位相特性を補償することを考える形態である。ここでは、例えば、以下の（３２）式に示すＰＩＤ制御系を用いる。
Figure JPOXMLDOC01-appb-M000024
 ここで、Ｆzi及びＦzdは、それぞれ、積分制御器及び微分制御器の零交差周波数である。また、折点周波数２００Ｈｚの２次のローパスフィルタをＰＩＤ制御系（ＰＩＤ制御器）に結合（例えば直列結合）する。第１の実施形態と同様に、開ループ伝達関数ＰnＫFを導出し、その応答からゲイン余裕、位相余裕が十分であるように設計するが、第２の実施形態では、ゲインＫdを設計する方法を示す。ＰＩＤ制御系において、微分制御器は、零交差周波数Ｆzdよりも低い周波数ではゲイン（微分ゲイン）をほとんど上げずに、位相を進めることができる。しかしながら、Ｆzdよりも高い周波数では、ゲインを持ち上げてしまう。そのため、零交差周波数Ｆzdを変化させ、その都度開ループ伝達関数ＰnＫFを用いてゲイン余裕を演算する（安定余裕の演算をする）ことで零交差周波数Ｆzdとゲイン余裕の関係を取得し、ゲイン余裕を最大とする零交差周波数Ｆzdを探索する。 
図１３は、本発明の第２の実施形態における連続体ロボットの制御装置２００に係る制御系のパラメータ探索を示す図である。具体的に、図１３は、最小周波数を第１の実施形態で示した連続体部分である湾曲可能部１１０の零点よりも十分低い１Ｈｚとし、最大周波数を例えば制御系をデジタル実装する際のナイキスト周波数である１ｋＨｚとするゲイン余裕の探索の応答を示す。図１３では、零交差周波数Ｆzdの９５Ｈｚ付近で、ゲイン余裕が最大となることがわかる。
図１４Ａ及び図１４Ｂは、本発明の第２の実施形態における連続体ロボットの制御装置２００に係る制御系のボード線図及び開ループ応答を示す図である。具体的に、図１４Ａに力制御部ＫFのＰＩＤ制御系のボード線図を示し、図１４Ｂに開ループ伝達関数ＰnＫのボード線図を示す。図１４Ａ及び図１４Ｂにおいて、実線で零交差周波数Ｆzdを９５Ｈｚとする第２の実施形態のＰＩＤ制御系の場合を示し、破線で第１の実施形態のＰＩ制御系の場合を示している。開ループ応答より、微分特性で位相遅れを補償することで、ゲイン余裕が約７dB改善していることがわかる。
また、図１５は、本発明の第２の実施形態における連続体ロボットの制御装置２００に係る制御系の設計手順を示すフローチャートである。この図１５には、以下の設計手順が記載されている。
＜１＞拡大系の動的モデルＰnの導出
＜２＞係数Ｋp，Ｋiを設定し、ローパスフィルタを結合し、力制御部ＫFを仮設計＜３＞開ループ伝達関数ＰnＫFを導出
＜４＞ゲイン余裕、位相余裕は十分かの判断
＜５＞零交差周波数Ｆzdの探索範囲を決定
＜６＞零交差周波数Ｆzdを探索し、ゲイン余裕を最大とするＫdを決定＜７＞力制御部ＫFの設計完了
＜８＞閉ループ系Ｇclを導出
＜９＞係数Ｋp，Ｋi，Ｋdを設定し、ローパスフィルタを結合し、位置制御部ＫSVを仮設計
＜１０＞開ループ伝達関数ＧclＫSVを導出
＜１１＞ゲイン余裕、位相余裕は十分かの判断
＜１２＞位置制御部ＫSVの設計完了
（第３の実施形態）
次に、本発明の第３の実施形態について説明する。なお、以下に記載する第３の実施形態の説明では、上述した第１及び第２の実施形態と共通する事項については説明を省略し、上述した第１及び第２の実施形態と異なる事項について説明を行う。
上述した第２の実施形態では、ＰＩＤ制御系の微分要素により位相遅れ特性を補償した。第３の実施形態では、制御系の次数を増やし、更に位相遅れを補償する。また、改善されたゲイン余裕に相当するように、力制御を行うインナーループ制御系（第１のループ制御系）のゲインを上げ、インナーループ制御系の制御帯域を拡大する。そして、シミュレーションにより有効性を検証する。
３．１）制御系設計
上述した第２の実施形態で設計した力制御部ＫFに、以下の（３３）式で示す位相進みフィルタを結合（例えば、直列結合）する。本実施形態では、ＰＩＤ－Ｌｅａｄ制御系と表記する。
Figure JPOXMLDOC01-appb-M000025
 ここで、Ｆc1は零交差周波数であり、Ｆc2（＞Ｆc1）は、フィルタをプロパーとするための極である。本実施形態では、Ｆc2を１ｋＨｚとし、Ｆc1を探索する。第２の実施形態と同様に、零交差周波数Ｆc1を変化させ、その都度ゲイン余裕を演算することで零交差周波数Ｆc1とゲイン余裕との関係を取得し、ゲイン余裕を最大とする零交差周波数Ｆc1を探索する。
図１６は、本発明の第３の実施形態における連続体ロボットの制御装置２００に係る制御系のパラメータ探索を示す図である。具体的に、図１６は、最小周波数を連続体部分である湾曲可能部１１０の零点よりも十分低い１Ｈｚとし、最大周波数Ｆc2以下となる９００Ｈｚとする探索応答を示す。図１６では、零交差周波数Ｆc1の５４０Ｈｚ付近で、ゲイン余裕が最大となることがわかる。応答が急峻であるので、若干の余裕をみて、零交差周波数Ｆc1を５４５Ｈｚとすると、ゲイン余裕は約６ｄＢ改善する。
図１７Ａ及び図１７Ｂは、本発明の第３の実施形態における連続体ロボットの制御装置２００に係る制御系のボード線図及び開ループ応答を示す図である。具体的に、図１７Ａに力制御部ＫFのＰＩＤ－Ｌｅａｄ制御系のボード線図を示し、図１７Ｂに開ループ伝達関数ＰnＫのボード線図を示す。図１７Ａ及び図１７Ｂにおいて、実線でＰＩＤ－Ｌｅａｄ制御系を用いる力制御系を示し、破線で第１の実施形態の制御系を示している。ここで、ＰＩＤ－Ｌｅａｄ制御系は、第１の実施形態のＰＩ制御系から改善されたゲイン余裕に相当する１３ｄＢのゲインを上げている。そのため、第１の実施形態の制御系に比べハイゲインとなるが、ゲイン余裕は等しい。
また、図１８は、本発明の第３の実施形態における連続体ロボットの制御装置２００に係る制御系の設計手順を示すフローチャートである。この図１８には、以下の設計手順が記載されている。
＜１＞拡大系の動的モデルＰnの導出
＜２＞係数Ｋp，Ｋiを設定し、ローパスフィルタを結合し、力制御部ＫFを仮設計＜３＞開ループ伝達関数ＰnＫFを導出
＜４＞ゲイン余裕、位相余裕は十分かの判断
＜５＞ゲイン余裕ＧＭ１を記憶
＜６＞零交差周波数Ｆzdの探索範囲を決定
＜７＞零交差周波数Ｆzdを探索し、ゲイン余裕を最大とするＫdを決定
＜８＞位相進みフィルタの周波数Ｆc1の探索範囲と、周波数Ｆc2を決定
＜９＞Ｆc1を探索し、ゲイン余裕を最大位相進みフィルタを決定
＜１０＞ゲイン余裕ＧＭ２を記憶
＜１１＞仮設計の力制御部ＫFにＧＭ２－ＧＭ１相当のゲインを乗じる
＜１２＞力制御部ＫFの設計完了
＜１３＞閉ループ系Ｇclを導出
＜１４＞係数Ｋp，Ｋi，Ｋdを設定し、ローパスフィルタを結合し、位置制御部ＫSVを仮設計
＜１５＞開ループ伝達関数ＧclＫSVを導出
＜１６＞ゲイン余裕、位相余裕は十分かの判断
＜１７＞位置制御部ＫSVの設計完了
３．２）シミュレーション
図１９Ａ～図１９Ｆは、本発明の第３の実施形態を示し、力制御部ＫFを用いるインナーループ制御系（第１のループ制御系）と更に位置制御部ＫSVを含むアウターループ制御系（第２のループ制御系）との二重ループ制御系による外乱を伴う位置決め応答を示す図である。具体的に、図１９Ａ～図１９Ｆに、図１１Ａ～図１１Ｆと同様の配置で各応答を示す。このシミュレーションでは、第１の実施形態のＰＩ制御系との比較を示す。ＰＩＤ－Ｌｅａｄ制御系を用いる二重ループ制御系のアウターループ制御系（第２のループ制御系）のサーボ帯域は、第１の実施形態と同様になるように設計している。図１９Ａ～図１９Ｆでは、ＰＩ－Ｌｅａｄ制御系の応答を実線で示し、ＰＩ制御系の応答を破線で示す。また、図１９Ｂにアウターループに与えるワイヤ把持機構の目標変位ｒｅｆzを点線で示し、図１９Ｃに位置制御部ＫSVが出力する制御入力である目標発生力ｒｅｆFを点線で示す。
シミュレーションの開始後、ＰＩ－Ｌｅａｄ制御系とＰＩ制御系は、ともに目標軌道に追従し、約１秒で目標変位に整定する。その後、１．５秒で図１９Ｆに示すように、湾曲可能部１１０の先端部に外乱トルクを与える。第１の実施形態と同様に、図１９Ｃに示すように、アウターループ制御系（第２のループ制御系）の位置制御部ＫSVは、ｚ軸負方向の発生力Ｆの目標値である目標発生力ｒｅｆFを制御入力として出力し、インナーループ制御系（第１のループ制御系）の力制御部ＫFは、発生力Ｆの目標値に追従する。そして、力制御部ＫFは、同時に回転直動変換系の等価慣性を補償し低減しているため、図１９Ａに示すように、モータは、正方向にバックドライブし、図１９Ｂに示すように、ワイヤ把持機構は、ｚ軸正方向に繰り出される。ここで、ＰＩＤ－Ｌｅａｄ制御系は、ＰＩ制御系に比べてハイゲインであるため大きくバックドライブしていることがわかる。図１９Ｄのモータへの制御入力より、ＰＩＤ－Ｌｅａｄ制御系は、ＰＩ制御系に比べて等価慣性を低減する制御入力が大きいことがわかる。これにより、図１９Ｅに示すように、ＰＩＤ－Ｌｅａｄ制御系は、湾曲可能部１１０の先端への外乱トルクに対して大きなバックドライバビリティをもつ制御を実現している。また、ＰＩＤ－Ｌｅａｄ制御系は、高次の振動モードに対する安定性も損なわれていない。
そして、ＰＩＤ－Ｌｅａｄ制御系は、位相遅れ特性を高次で補償することにより、インナーループ制御系（第１のループ制御系）の力制御部ＫFをハイゲインに設計できることから、回転直動変換機構の等価慣性をハイゲインに補償し、バックドライバビリティを増加させることが可能である。
（第４の実施形態）
次に、本発明の第４の実施形態について説明する。なお、以下に記載する第４の実施形態の説明では、上述した第１～第３の実施形態と共通する事項については説明を省略し、上述した第１～第３の実施形態と異なる事項について説明を行う。
上述した第１及び第２の実施形態では、二重ループ制御系により、連続体ロボット１００にバックドライバビリティを与えることができることを示した。さらに、第３の実施形態では、インナーループ制御系（第１のループ制御系）の力制御部ＫFをハイゲインに設計することにより、バックドライバビリティを増加させることが可能であることを示した。本実施形態では、位置決め性能を変えることなく、バックドライバビリティの大きさを調整可能とする制御系を示す。
まず、第１～第３の実施形態に示したように、二重ループ制御系を設計する。そして、力制御部ＫFに０以上１以下の係数（第１の係数）αを乗ずる。これにより、力制御部ＫFのゲインが下がり、バックドライバビリティの大きさを任意に減少できる。しかしながら、同時に開ループ伝達関数ＧclＫSVのゲインが下がるため、位置決め制御のサーボ帯域が下がってしまう。そこで、位置制御部ＫSVのゲインを上げ、係数αが１であるときと同様のサーボ帯域となるように再設計する。これにより、位置制御の性能は変えずに、バックドライバビリティを変動させることが可能である。
図２０Ａ～図２０Ｃは、本発明の第４の実施形態に係る連続体ロボットの制御システム１０の概略構成の一例を示す図である。以下、図２０Ａに示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－２」として記載する。また、図２０Ｂに示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－３」として記載する。図２０Ｃに示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－４」として記載する。この図２０Ａ～図２０Ｃにおいて、図１に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。
図２０Ａに示す連続体ロボットの制御システム１０－２において、破線は、力制御部ＫFの変動に応じて位置制御部ＫSVを再設計すること示している。具体的に、図２０Ａでは、力制御部ＫFのゲインに上述した係数（第１の係数）αを乗じで第１のゲインとする様子が示されている。この際、図２０Ａでは、係数（第１の係数）αは、入力装置４００から力制御部ＫFに入力されるようになっている。そして、計算機上では、位置制御部ＫSVのゲイン（第２のゲイン）を細かなステップで増加させ、開ループ伝達関数ＧclＫSVの応答を算出することにより、係数αが１であるときと同様のサーボ帯域となること確認する、という手順を繰り返して反復すればよい。ただし、係数αがあまりに小さいと、位置制御部ＫSVがハイゲインにならざるを得ず、高次モードを励起し制御系が不安定になってしまう。そのため、あらかじめ係数αの下限を設定しておけばよい。もしくは、係数αに応じて位置制御部ＫSVのサーボ帯域が常に等しくなるように、位置制御部ＫSVのゲインに乗ずる係数（第２の係数）を予め算出して、メモリに保存しておく。この場合、例えば、図２０Ｂに示す連続体ロボットの制御システム１０－３のように、係数αに応じてメモリ２２０から、位置制御部ＫSVのゲインに乗ずる係数（第２の係数）を読みだせばよい。これにより、操作者が係数αを任意に指令できるつまみなどを連続体ロボット１００の操作系に付加することで、実時間にバックドライビリティを変更できる。
次に、湾曲可能部１１０の湾曲角度に連動してバックドライバビリティを変動する制御系を示す。例えば、大きく湾曲するにつれてバックドライバビリティを低くし、外乱等に対する変形を防ぐには、以下の（３４）式とすることが考えられる。
Figure JPOXMLDOC01-appb-M000026
 また、逆に、大きく湾曲するにつれてバックドライバビリティを高くし、安全性を高めるためるには、以下の（３５）式とすることが考えられる。
Figure JPOXMLDOC01-appb-M000027
 ここで、β，γは係数であり、予め湾曲の範囲を設定し、係数αが０＜α＜１の範囲に収まるように設定する必要がある。図２０Ｃに、そのブロック線図を示す。ここで、係数演算部ｆαは、（３４）式，（３５）式に示すように、湾曲可能部１１０における目標湾曲角度ｒｅｆθに応じて係数（第１の係数）αを演算して変化させる手段を表している。
図２１は、本発明の第４の実施形態に係る連続体ロボットの制御システム１０に係る制御系によるシミュレーション応答を示す図である。第１の実施形態と同様に、位置決め終了後の１．５秒において、例えば図１２Ｆに示す外乱トルクを湾曲可能部１１０の先端に与えている。図２１では、湾曲可能部１１０の先端部分の湾曲角度を示し、係数αを１、０．６、０．３、０．１とする応答を、それぞれ、点線、実線、破線、一点鎖線で示している。係数αの変化に伴う位置決め性能の変化はないが、係数αが小さくなるにつれて、バックドライバビリティが低くなっていることがわかる。これより、位置決め性能を変えることなく、バックドライバビリティの大きさを調整可能とする制御系が実現できることがわかる。
（第５の実施形態）
次に、本発明の第５の実施形態について説明する。なお、以下に記載する第５の実施形態の説明では、上述した第１～第４の実施形態と共通する事項については説明を省略し、上述した第１～第４の実施形態と異なる事項について説明を行う。
上述した第１～第４の実施形態では、ワイヤ把持機構基台及び先端側の変位ｚt1，ｚt2を観測量としていた。しかしながら、機構の制約上それを実現できる変位センサを配置できないことがある。そこで、本実施形態では、他の観測量を用いる制御系について説明する。
ワイヤ把持機構の変位ｚt2が観測困難であり、張力検出機構のばね変位ｋt2がひずみゲージなどにより観測可能であれば、その発生力Ｆを観測量とすればよい。そして、この場合、ワイヤ把持機構の変位ｚt2は、以下の（３６）式により求められる。
Figure JPOXMLDOC01-appb-M000028
 また、ワイヤ把持機構の移動範囲に比べて力検出部のばね変位が実用上十分に小さいときは、アウターループ制御系（第２のループ制御系）の制御量をワイヤ把持機構基台の変位ｚt1としてもよい。
さらに、ワイヤ把持機構基台の変位ｚt1の観測が困難であり、カップリングのばね係数及び駆動軸のｚ方向のばね係数ｋg，ｋt1が十分に大きい場合には、モータの回転角度θmを観測し、以下の（３７）式と近似し、アウターループ制御系（第２のループ制御系）の位置制御量として用いてもよい。
Figure JPOXMLDOC01-appb-M000029
（第６の実施形態）
次に、本発明の第６の実施形態について説明する。なお、以下に記載する第６の実施形態の説明では、上述した第１～第５の実施形態と共通する事項については説明を省略し、上述した第１～第５の実施形態と異なる事項について説明を行う。
上述した第１～第５の実施形態では、平面内駆動の連続体ロボット１００に対して制御系設計を行った。本実施形態では、立体駆動可能な連続体ロボットに対して制御系設計を行う。
図２２は、本発明の第６の実施形態に係る連続体ロボット１００の概略構成の第１例を示す図である。以下、図２２に示す連続体ロボット１００を「連続体ロボット１００－２」として記載する。
図２２に示す連続体ロボット１００－２は、湾曲可能部１１００の遠位端１０６０における固定部１０２１～１０２３にワイヤ１０１１～１０１３が接続されている。また、ワイヤ１０１１～１０１３の近位端は、ワイヤ把持パイプ１０３１～１０３３が接続されている。ロボット基台部１０４０には、第１の実施形態と同様に、ワイヤ１０１１～１０１３における各ワイヤごとに、ワイヤ把持機構（図２２では不図示）及びアクチュエータ（図２２では不図示）が設置されている。ここで、ワイヤ把持機構は、アクチュエータにワイヤ把持機構基台（図２２では不図示）を介して接続され、上下に移動可能である。ワイヤ把持パイプ１０３１～１０３３は、ワイヤ把持機構に接続され、アクチュエータで押引きすることにより姿勢が制御される。また、連続体ロボット１００－２は、ワイヤ１０１１～１０１３を案内するための部材であるワイヤガイド１０６１～１０６４を有する。このワイヤガイドは、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤガイド１０６１～１０６４は、ワイヤ１０１１に、固定部１０５０～１０５３において固定される。さらに、ワイヤ１０１１～１０１３の間隔と、ワイヤ把持パイプ１０３１～１０３３との間隔が異なってもよい。そのときには、径変換部１１９０をロボット基台部１０４０に接続すればよい。また、図２２において、連続体ロボット１００－２の中心軸を破線で示す。
本実施形態では、ワイヤ１０１１～１０１３と、ワイヤガイド１０６１～１０６４からなる機構を連続体部分である湾曲可能部１１００とよぶ。また、不図示のアクチュエータには、回転モータと回転直動変換機構からなるアクチュエータユニットを用いるものとする。さらに、ワイヤ把持機構は、ワイヤの張力を検出する機能を持つものとする。そのためには、ワイヤ把持機構とアクチュエータとの間に上述したワイヤ把持機構基台を設け、ワイヤ把持機構基台をアクチュエータに接続し、ワイヤ把持機構基台とワイヤ把持機構の間をばねで接続する。このとき、ワイヤ把持機構がｚ軸方向にのみ変位するように、リニアガイドを設けることや、ばねに平行ばねを用いることが好ましい。そして、ばねの変位を計測することでワイヤの張力が検出可能となる。以下に、図２２に示す符号の定義を記載する。
ｌd：湾曲可能部１１００の中心軸の長さ
θn：湾曲可能部１１００の遠位端の湾曲角度
ζn：湾曲可能部１１００遠位端の旋回角度
ρn：湾曲可能部１１００の曲率半径
また、本実施形態では、ワイヤ１０１１～１０１３をｘｙ面内において反時計回りにａワイヤ、ｂワイヤ、ｃワイヤと呼び、そのａ～ｃワイヤ１０１１～１０１３の駆動変位をｌp1a，ｌp1b，ｌp1cとする。
図２３は、本発明の第６の実施形態に係る連続体ロボット１００のワイヤの配置の一例を示す図である。この図２３に示すように、ａ～ｃワイヤ１０１１～１０１３は、一片の長さをｒsとする正三角形の頂点に配置されており、位相角ξnは、ワイヤの配置を決定する角度である。本実施形態では、ξ1＝０とする。
そして、本実施形態では、以下の仮定をおき、連続体ロボット１００－２の運動学を導出する。
［１］各湾曲可能部１１０において、ワイヤは曲率一定に変形する。
［２］ワイヤのねじり変形は考慮しない。
［３］ワイヤは長手方向に変形しない。
［４］ワイヤガイドとワイヤとの間の摩擦は考慮しない。
これにより、湾曲可能部１１０の遠位端の湾曲角度θ1及び旋回角度ζ1とするための、ａワイヤの駆動変位ｌp1a，ｂワイヤの駆動変位ｌp1b，ｃワイヤの駆動変位ｌp1cは、以下の（３８）式となる。
Figure JPOXMLDOC01-appb-M000030
本実施形態では、ａ～ｃワイヤ１０１１～１０１３における各ワイヤに独立して、第１～第５の実施形態の二重ループ制御を適用する分散制御を行う。
図２４は、本発明の第６の実施形態に係る図２２に示す連続体ロボット１００－２を含む連続体ロボットの制御システム１０の概略構成の一例を示す図である。以下、図２４に示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－５」として記載する。この図２４において、図１に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。
この図２４において、湾曲可能部１１０の遠位端の目標湾曲角度ｒｅｆθ1及び目標旋回角度ｒｅｆζ1が入力装置５１０から運動学演算部２１０に入力されている。また、各ワイヤ把持機構の目標変位ｒｅｆza，ｒｅｆzb，ｒｅｆzcは、（３８）式の湾曲角度θ1、旋回角度ζ1に、それぞれ、目標湾曲角度ｒｅｆθ1、目標旋回角度ｒｅｆζ1を代入することで得られる。動的モデルＰnaは、ｂワイヤ，ｃワイヤのワイヤ把持パイプをアクチュエータ基台部に固定し、ａワイヤを駆動するものとして第１の実施形態の動的モデルＰnと等価として得られるモデルである。同様に、動的モデルＰnbは、ａワイヤ，ｃワイヤのワイヤ把持パイプをアクチュエータ基台部に固定し、ｂワイヤを駆動するものとして第１の実施形態の動的モデルＰnと等価として得られるモデルである。同様に、動的モデルＰncは、ａワイヤ，ｂワイヤのワイヤ把持パイプをアクチュエータ基台部に固定し、ｃワイヤを駆動するものとして第１の実施形態の動的モデルＰnと等価として得られるモデルである。また、ＫFa，ＫFb，ＫFcは、それそれ、ａワイヤ，ｂワイヤ，ｃワイヤに対する力制御部である。この力制御部は、個別に設計してもよいし、特に、動的モデルＰna，Ｐnb，Ｐncに差がなければ、同じ制御系でもよい。また、ＫSVa，ＫSVb，ＫSVcは、それそれ、ａワイヤ，ｂワイヤ，ｃワイヤに対する位置制御部である。この位置制御部は、個別に設計してもよいし、特に、動的モデルＰna，Ｐnb，Ｐncに差がなければ、同一の制御系でもよい。
また、係数αa，αb，αcは、第４の実施形態で示した、バックドライバビリティを可変とするための係数である。具体的に、図２４では、係数αaは、入力装置５２０から力制御部ＫFa等に入力され、係数αbは、入力装置５３０から力制御部ＫFb等に入力され、係数αcは、入力装置５４０から力制御部ＫFc等に入力されるようになっている。この際、係数αa，αb，αcのそれぞれを異なる値に設定すれば、旋回方向に応じてバックドライバビリティを可変にすることが可能となる。また、係数αa，αb，αcのそれぞれを全て同じ値にすれば、旋回方向に関わりなく、均一なバックドライバビリティを得ることができる。
ここで、３本のワイヤのうち、１本のワイヤのバックドライバビリティを大きく下げれば、連続体ロボット１００－２の中心軸方向の外乱に対するバックドライバビリティのみを下げ、湾曲方向の外乱トルクにはバックドライバビリティを持たせることができる。これは、湾曲可能部１１００が基台部に衝突することを回避したり、逆に、湾曲可能部１１００がｚ軸正方向に繰り出されてワイヤ把持機構が基台部に衝突したりすることを回避することができる。
さらに、連続体ロボット１００－２の中心軸方向の外乱に対するバックドライバビリティを０とし、湾曲方向の外乱にはバックドライバビリティを持たせるには、１本のワイヤもしくはワイヤ把持パイプをアクチュエータ基台部に固定すればよい。
図２５は、本発明の第６の実施形態に係る連続体ロボット１００の概略構成の第２例を示す図である。以下、図２５に示す連続体ロボット１００を「連続体ロボット１００－３」として記載する。この図２５において、図２２に示す構成と同様の構成については同じ符号を付している。具体的に、図２５は、ａワイヤ１０１１を固定部１１９１に固定する例を示す。このとき、湾曲可能部１１００遠位端の湾曲角度θ1及び旋回角度ζ1とするための、ｂワイヤの駆動変位ｌp1b，ｃワイヤの駆動変位ｌp1cは、それぞれ、以下の（３９）式及び（４０）式となる。
Figure JPOXMLDOC01-appb-M000031
図２６は、本発明の第６の実施形態に係る図２５に示す連続体ロボット１００－３を含む連続体ロボットの制御システム１０の概略構成の一例を示す図である。図２６では、各ワイヤ把持機構の目標変位ｒｅｆzb，ｒｅｆzcは、（３９）式，（４０）式の湾曲角度θ1、旋回角度ζ1に、目標湾曲角度ｒｅｆθ1、目標旋回角度ｒｅｆζ1を代入することで得られる。
（第７の実施形態）
次に、本発明の第７の実施形態について説明する。なお、以下に記載する第７の実施形態の説明では、上述した第１～第６の実施形態と共通する事項については説明を省略し、上述した第１～第６の実施形態と異なる事項について説明を行う。
上述した第１～第６の実施形態では、単一の湾曲可能部を有する連続体ロボット１００のバックドライバビリティを向上する制御について説明した。本実施形態では、複数の湾曲可能部を有する連続体ロボットの制御系設計方法を説明する。
図２７は、本発明の第７の実施形態に係る連続体ロボット１００の概略構成の第２例を示す図である。以下、図２７に示す連続体ロボット１００を「連続体ロボット１００－４」として記載する。この図２７では、３つの湾曲可能部１１００－１～１１００－３が示されている。
ここでは、第ｎの湾曲可能部を駆動するワイヤの駆動変位をｌpna，ｌpnb，ｌpncとし、複数の湾曲可能部１１００を有するの連続体ロボット１００－４のａワイヤ，ｂワイヤ，ｃワイヤの駆動変位ｌpna，ｌpnb，ｌpncと第ｎの湾曲可能部の遠位端の湾曲角度θn及び旋回角度ζnとの関係を求める。湾曲可能部の数をｅとし、第ｎの湾曲可能部を駆動するワイヤの位相角を以下の（４１）式とする。
Figure JPOXMLDOC01-appb-M000032
これより、第ｎの湾曲可能部のワイヤの駆動変位ｌpna，ｌpnb，ｌpncは、以下の（４２）式となる。
Figure JPOXMLDOC01-appb-M000033
これを用いて、第６の実施形態と同様に、各ワイヤを二重ループ制御系により駆動する。また、各湾曲可能部の間のバックドライブによる衝突を防ぐには、第６の実施形態と同様に、例えば各湾曲可能部のａワイヤを固定し、連続体ロボット１００－４の中心軸方向のバックドライバビリティを０にすればよい。このときのｂワイヤの駆動変位ｌpnb，ｃワイヤの駆動変位ｌpncは、それぞれ、以下の（４３）式及び（４４）式となる。 
Figure JPOXMLDOC01-appb-M000034
そして、第７の実施形態では、上述した第６の実施形態と同様に、各湾曲可能部１１００のｂワイヤ，ｃワイヤを二重ループ制御系により駆動すれば、湾曲方向のみにバックドライバビリティが得られる。
（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。また、本発明のその他の実施形態としては、連続体ロボットの制御装置２００の機能構成部を、連続体ロボット１００に組み込んだ形態も、本発明に適用可能である。
なお、上述した本発明の実施形態は、いずれも本発明を実施するにあたっての具体化の例を示したものに過ぎず、これらによって本発明の技術的範囲が限定的に解釈されてはならないものである。即ち、本発明はその技術思想、又はその主要な特徴から逸脱することなく、様々な形で実施することができる。
本発明は上記実施の形態に制限されるものではなく、本発明の精神及び範囲から離脱することなく、様々な変更及び変形が可能である。従って、本発明の範囲を公にするために以下の請求項を添付する。
本願は、２０２０年２月２１日提出の日本国特許出願特願２０２０－０２８８１３を基礎として優先権を主張するものであり、その記載内容の全てをここに援用する。
１０：連続体ロボットの制御システム、１００：連続体ロボット、２００：連続体ロボットの制御装置、２１０：運動学演算部、ＫSV：位置制御部、ＫF：力制御部、Ｐn：連続体ロボットの動的モデル、３００：入力装置

Claims (14)

  1. ワイヤで駆動され、湾曲可能に構成された少なくとも１つの湾曲可能部と、前記ワイヤを駆動する駆動部とを備える連続体ロボットの制御システムであって、
    前記駆動部による前記ワイヤの押し引き駆動の目標変位と、前記連続体ロボットから得られる前記ワイヤを把持するワイヤ把持機構の変位との誤差を補償する制御を行うものであって、前記ワイヤの目標張力を出力する位置制御部と、
    前記位置制御部から出力される前記ワイヤの目標張力と、前記連続体ロボットから得られる前記ワイヤの張力との誤差を補償する制御を行う力制御部と、
    を有し、
    前記力制御部を含む第１のループ制御系と、前記力制御部および前記位置制御部を含む第２のループ制御系とが構成されていることを特徴とする連続体ロボットの制御システム。
  2. 前記湾曲可能部における目標湾曲角度の入力に基づいて運動学演算を行い、前記目標変位を出力する運動学演算部を更に有することを特徴とする請求項１に記載の連続体ロボットの制御システム。
  3. 前記力制御部のゲインに第１の係数を乗じで第１のゲインとするとともに、前記位置制御部のゲインに第２の係数を乗じで第２のゲインとし、
    前記第２のゲインは、前記第１のゲインに応じて定められることを特徴とする請求項１または２に記載の連続体ロボットの制御システム。
  4. 前記湾曲可能部における前記目標湾曲角度に応じて前記第１の係数を変化させることを特徴とする請求項３に記載の連続体ロボットの制御システム。
  5. 前記連続体ロボットは、１つの前記湾曲可能部に複数の前記ワイヤが構成され、前記複数のワイヤのそれぞれを駆動する複数の前記駆動部が備えられており、
    前記複数の駆動部における各駆動部に対応して、前記第１のループ制御系および前記第２のループ制御系が構成されていることを特徴とする請求項１乃至４のいずれか１項に記載の連続体ロボットの制御システム。
  6. 前記力制御部のゲインに第１の係数を乗じで第１のゲインとする際に、前記各駆動部ごとに前記第１の係数が異なることを特徴とする請求項５に記載の連続体ロボットの制御システム。
  7. 前記連続体ロボットは、前記１つの湾曲可能部に構成された前記複数のワイヤのうちの１つのワイヤが、基台部に固定されていることを特徴とする請求項５または６に記載の連続体ロボットの制御システム。
  8. 前記力制御部は、前記連続体ロボットの運動方程式に基づく伝達関数および当該力制御部の開ループ伝達関数を演算する処理と、前記開ループ伝達関数から安定余裕の演算をする処理とを反復し、当該力制御部のゲインを定めることを特徴とする請求項１乃至７のいずれか１項に記載の連続体ロボットの制御システム。
  9. 前記力制御部は、ＰＩＤ制御器とローパスフィルタとの直列結合から構成されていることを特徴とする請求項１乃至８のいずれか１項に記載の連続体ロボットの制御システム。 
  10. 前記ＰＩＤ制御器の微分ゲインは、当該微分ゲインを設定する処理と、前記力制御部および前記連続体ロボットの運動方程式に基づく伝達関数を用いて前記開ループ伝達関数を演算する処理とを反復することによって、定められることを特徴とする請求項９に記載の連続体ロボットの制御システム。
  11. 前記力制御部は、ＰＩＤ制御器と位相進みフィルタとローパスフィルタとの直列結合から構成されていることを特徴とする請求項１乃至８のいずれか１項に記載の連続体ロボットの制御システム。
  12. 前記位相進みフィルタは、折点周波数を設定する処理と、前記力制御部および前記連続体ロボットの運動方程式に基づく伝達関数を用いて前記開ループ伝達関数を演算する処理とを反復することによって、定められることを特徴とする請求項１１に記載の連続体ロボットの制御システム。
  13. ワイヤと、
    湾曲可能に構成された少なくとも１つの湾曲可能部と、
    前記ワイヤを駆動する駆動部と、
    を備え、
    請求項１乃至１２のいずれか１項に記載の連続体ロボットの制御システムを備えた連続体ロボット。
  14. ワイヤで駆動され、湾曲可能に構成された少なくとも１つの湾曲可能部と、前記ワイヤを駆動する駆動部とを備える連続体ロボットの制御方法であって、
    位置制御部によって、前記駆動部による前記ワイヤの押し引き駆動の目標変位と、前記連続体ロボットから得られる前記ワイヤを把持するワイヤ把持機構の変位との誤差を補償する制御を行い、前記ワイヤの目標張力を出力する位置制御ステップと、
    力制御部によって、前記位置制御部から出力される前記ワイヤの目標張力と、前記連続体ロボットから得られる前記ワイヤの張力との誤差を補償する制御を行う力制御ステップと、
    を有し、
    前記力制御部を含む第１のループ制御系と、前記力制御部および前記位置制御部を含む第２のループ制御系とが構成されていることを特徴とする連続体ロボットの制御方法。
PCT/JP2021/006085 2020-02-21 2021-02-18 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット WO2021166995A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
EP21756932.6A EP4101605A4 (en) 2020-02-21 2021-02-18 CONTROL SYSTEM FOR ROBOT CONTINUUM AND CONTROL METHOD THEREFOR AND ROBOT CONTINUUM
CN202180015981.5A CN115135465A (zh) 2020-02-21 2021-02-18 用于控制连续体机器人的系统和方法以及连续体机器人
US17/890,104 US20220402130A1 (en) 2020-02-21 2022-08-17 System and method for controlling continuum robot, and continuum robot

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2020028813A JP7379208B2 (ja) 2020-02-21 2020-02-21 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット
JP2020-028813 2020-02-21

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US17/890,104 Continuation US20220402130A1 (en) 2020-02-21 2022-08-17 System and method for controlling continuum robot, and continuum robot

Publications (1)

Publication Number Publication Date
WO2021166995A1 true WO2021166995A1 (ja) 2021-08-26

Family

ID=77392178

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2021/006085 WO2021166995A1 (ja) 2020-02-21 2021-02-18 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット

Country Status (5)

Country Link
US (1) US20220402130A1 (ja)
EP (1) EP4101605A4 (ja)
JP (1) JP7379208B2 (ja)
CN (1) CN115135465A (ja)
WO (1) WO2021166995A1 (ja)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2023122886A (ja) * 2022-02-24 2023-09-05 キヤノン株式会社 操作システムおよび操作システムの制御方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH02310609A (ja) * 1989-05-25 1990-12-26 Toyota Central Res & Dev Lab Inc マニピユレータの位置と力の協調制御装置
JPH0631662A (ja) * 1992-07-14 1994-02-08 Kobe Steel Ltd マニピュレータの制御方法とその装置
JPH0720943A (ja) * 1993-06-30 1995-01-24 Mitsubishi Electric Corp 力制御機構
JP2011031354A (ja) * 2009-08-04 2011-02-17 Olympus Corp アクチュエータ装置
JP2016002414A (ja) * 2014-06-19 2016-01-12 オリンパス株式会社 マニピュレータの制御方法、マニピュレータ、およびマニピュレータシステム
JP2018523508A (ja) * 2015-06-30 2018-08-23 キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc マニピュレータを制御する方法および装置
JP2019058648A (ja) * 2017-07-17 2019-04-18 キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc 連続体ロボット制御方法および装置
JP2019202137A (ja) * 2018-05-17 2019-11-28 キヤノン株式会社 連続体ロボットおよび連続体ロボットの制御システム
JP2020028813A (ja) 2014-06-24 2020-02-27 三菱ケミカル株式会社 菌体低付着性物品、および菌体付着対策方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9539726B2 (en) * 2012-04-20 2017-01-10 Vanderbilt University Systems and methods for safe compliant insertion and hybrid force/motion telemanipulation of continuum robots
US10434644B2 (en) * 2014-11-03 2019-10-08 The Board Of Trustees Of The Leland Stanford Junior University Position/force control of a flexible manipulator under model-less control
JP7183062B2 (ja) 2018-03-23 2022-12-05 キヤノン株式会社 連続体ロボット制御装置、連続体ロボット制御方法及びプログラム

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH02310609A (ja) * 1989-05-25 1990-12-26 Toyota Central Res & Dev Lab Inc マニピユレータの位置と力の協調制御装置
JPH0631662A (ja) * 1992-07-14 1994-02-08 Kobe Steel Ltd マニピュレータの制御方法とその装置
JPH0720943A (ja) * 1993-06-30 1995-01-24 Mitsubishi Electric Corp 力制御機構
JP2011031354A (ja) * 2009-08-04 2011-02-17 Olympus Corp アクチュエータ装置
JP2016002414A (ja) * 2014-06-19 2016-01-12 オリンパス株式会社 マニピュレータの制御方法、マニピュレータ、およびマニピュレータシステム
JP6169049B2 (ja) 2014-06-19 2017-07-26 オリンパス株式会社 マニピュレータの制御方法、マニピュレータ、およびマニピュレータシステム
JP2020028813A (ja) 2014-06-24 2020-02-27 三菱ケミカル株式会社 菌体低付着性物品、および菌体付着対策方法
JP2018523508A (ja) * 2015-06-30 2018-08-23 キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc マニピュレータを制御する方法および装置
JP2019058648A (ja) * 2017-07-17 2019-04-18 キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc 連続体ロボット制御方法および装置
JP2019202137A (ja) * 2018-05-17 2019-11-28 キヤノン株式会社 連続体ロボットおよび連続体ロボットの制御システム

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP4101605A4

Also Published As

Publication number Publication date
CN115135465A (zh) 2022-09-30
JP2021133429A (ja) 2021-09-13
EP4101605A4 (en) 2024-03-13
US20220402130A1 (en) 2022-12-22
JP7379208B2 (ja) 2023-11-14
EP4101605A1 (en) 2022-12-14

Similar Documents

Publication Publication Date Title
US11877814B2 (en) Tension control in actuation of multi-joint medical instruments
Zhang et al. Optimization and implementation of a high-speed 3-DOFs translational cable-driven parallel robot
US7407208B2 (en) Joint drive mechanism and robot hand
WO2019221036A1 (ja) 連続体ロボットおよび連続体ロボットの制御システム
JP2018008335A (ja) 連続体ロボット、その運動学の補正方法、および連続体ロボットの制御方法
JPH02503404A (ja) エンドポイントコンプライアンスが調節可能な連接構造体
Conrad et al. Interleaved continuum-rigid manipulation: An augmented approach for robotic minimally-invasive flexible catheter-based procedures
JP2019166307A (ja) 連続体ロボット制御装置、連続体ロボット制御方法及びプログラム
WO2021166995A1 (ja) 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット
Suh et al. Harmonious cable actuation mechanism for soft robot joints using a pair of noncircular pulleys
JP4442464B2 (ja) 多関節アーム機構
US20150080908A1 (en) Energy balance mechanism for flexure joint
WO2019181694A1 (en) Continuum robot control device, continuum robot control method, and program
Carusone et al. Tracking control for end‐effector position and orientation of structurally flexible manipulators
Fujioka et al. Proposal of tendon-driven elastic telescopic arm and initial bending experiment
WO1994017461A1 (en) Apparatus for controlling weaving of robot
WO2023022056A1 (ja) 連続体ロボットの制御システム及びその制御方法
US20240181630A1 (en) Control system for continuum robot and control method for same
WO1984004722A1 (en) Articulated robot
Lin et al. Design and Implementation of a Cable-Driven Dexterous Continuum Manipulators in Confined Space Usage
JP2014104517A (ja) ロボットシステム
Prasanga et al. Evaluation of a backlash compensation method using two parallel thrust wires
Incerti An optimization method for reducing residual vibrations of compliant manipulators
Soetebier et al. Spread-band: a smart machine element enables compact parallel robot design

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21756932

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2021756932

Country of ref document: EP

Effective date: 20220908

NENP Non-entry into the national phase

Ref country code: DE