[go: up one dir, main page]

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

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

Info

Publication number
WO2021261539A1
WO2021261539A1 PCT/JP2021/023884 JP2021023884W WO2021261539A1 WO 2021261539 A1 WO2021261539 A1 WO 2021261539A1 JP 2021023884 W JP2021023884 W JP 2021023884W WO 2021261539 A1 WO2021261539 A1 WO 2021261539A1
Authority
WO
WIPO (PCT)
Prior art keywords
target
angle
bending
distal
turning angle
Prior art date
Legal status (The legal status 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 status listed.)
Ceased
Application number
PCT/JP2021/023884
Other languages
English (en)
French (fr)
Inventor
清志 ▲高▼木
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Canon Inc
Original Assignee
Canon Inc
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
Priority claimed from JP2021100189A external-priority patent/JP7614954B2/ja
Application filed by Canon Inc filed Critical Canon Inc
Publication of WO2021261539A1 publication Critical patent/WO2021261539A1/ja
Priority to US18/069,127 priority Critical patent/US12318928B2/en
Anticipated expiration legal-status Critical
Priority to US19/196,492 priority patent/US20250262758A1/en
Ceased legal-status Critical Current

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/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
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J18/00—Arms
    • B25J18/06—Arms flexible
    • 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
    • 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
    • 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 and a control method for a continuous robot having a bendable portion provided with a plurality of curved sections that are curved by being driven by a wire, and a program for operating a computer as the control system. Is.
  • a continuous robot also called a continuous robot, is configured to have a bendable portion provided with a plurality of curved sections having a flexible structure, and the shape is controlled by deforming the curved sections. Will be done.
  • This continuum robot has two main advantages over the rigid link robot configured by the rigid link. First, the continuous robot can move along a curve in a narrow space where a rigid link robot fits in or in an environment with scattered objects. Second, the continuous robot has intrinsic softness and can be operated without damaging fragile objects. There, it is not always necessary to detect the external force required by the rigid link robot. Taking advantage of this feature, continuous 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.
  • Patent Document 1 describes an operation of moving the position of the tip of a continuous robot having a camera at the tip of a bendable portion while keeping the direction of the line of sight of the camera constant, and the line of sight of the camera is always far away.
  • a control method is described in which a characteristic motion (hereinafter referred to as "observation motion control") such as moving the position and angle of the tip so as to pass through a point can be realized by a simple operation by the operator. ..
  • Patent Document 1 in order to shift from the initial bending posture of the bendable portion to the observation motion control, an additional bending amount for all the bending sections of the bendable portion is algebraically calculated from the operation amount of the operation portion by the operator. Seeking. Therefore, in the control described in Patent Document 1, depending on the initial bending posture of the bendable portion, the additional bending amount for each bending section is largely controlled even if the operation amount of the operation portion by the operator is small. It is possible.
  • the continuous robot generally uses a control (hereinafter referred to as "head follow control”) in which the curved posture of the most distal curved section at the head is continuously propagated to the subsequent curved section with the length of the curved section.
  • the present invention has been made in view of such problems, and an object of the present invention is to provide a mechanism for safely operating a continuous robot.
  • the control system of the continuous robot of the present invention includes a bendable portion provided with a plurality of curved sections that are curved by being driven by a wire, a base portion that supports the bendable portion, and a drive portion that drives the wire.
  • a control system for a continuum robot comprising the Based on the displacement of the base portion in the traveling direction of the above, the target bending angle and the target of the subsequent curved section located between the most distal curved section and the base portion among the plurality of curved sections. Subsequent calculation means for calculating the turning angle, subsequent operating means for inputting the target bending angle and target turning angle of the succeeding bending section by being operated, and the target bending angle of the succeeding bending section obtained from the succeeding calculation means.
  • the subsequent switching means for performing switching processing for selecting one of the target turning angle and the target bending angle and the target turning angle of the succeeding bending section obtained from the succeeding operating means, and the succeeding switching means.
  • the driving unit includes a calculation means for calculating the drive displacement when driving the wire of the succeeding curved section.
  • the present invention also includes a method for controlling a continuous robot by the above-mentioned control system for the continuous robot, and a program for operating a computer as the above-mentioned control system for the continuous robot.
  • the continuous robot can be operated safely.
  • FIG. 5 is a diagram showing a fifth embodiment of the present invention and showing an example of a schematic configuration of an operating device for operating three curved sections of the continuous robot shown in FIG. 1. It is a figure which shows an example of the schematic structure of the control system of the continuous robot which concerns on 6th Embodiment of this invention.
  • FIG. 1 is a diagram showing an example of a schematic configuration of a continuous robot 100 according to the first embodiment of the present invention.
  • FIG. 1 shows a base portion 140 and a bendable portion 170 as one configuration of the continuous robot 100.
  • the bendable portion 170 is a constituent portion (constituent portion provided with a plurality of curved sections) provided with a plurality of curved sections 171 to 173 that are curved by being driven by a wire.
  • FIG. 1 illustrates n (three) curved sections of the curved section 171 according to (n-2), the curved section 172 according to (n-1), and the curved section 173 according to n. ing. Further, FIG. 1 shows the bending angle ⁇ n and the turning angle ⁇ n of the bending section 173, and also shows the bending angle ⁇ n-1 and the turning angle ⁇ n-1 of the bending section 172, and the bending angle ⁇ of the bending section 171 is shown.
  • the base portion 140 is a constituent portion that supports the bendable portion 170, and an actuator (not shown in FIG. 1) that is a drive portion for driving each wire of the curved sections 171 to 173 is provided inside the base portion 140.
  • an actuator (not shown in FIG. 1) that is a drive portion for driving each wire of the curved sections 171 to 173 is provided inside the base portion 140.
  • the origin O is set at a predetermined position (for example, the center position) of the upper surface 141 of the base portion 140, and the traveling direction (for example, the traveling direction) of the continuous robot 100 is defined as the z direction.
  • the xyz coordinate system is shown in which the x-direction and the y-direction are orthogonal directions and are orthogonal to each other.
  • FIG. 1 shows the displacement z b of the base portion 140 as an indicator of the amount of movement of the z-direction of the continuum robot 100 (forward amount).
  • the curved section 173 is the most distal curved section located at the farthest position from the base portion 140 among the plurality of curved sections 171 to 173 constituting the bendable portion 170.
  • the curved section 172 (the same applies to the curved section 171) is located between the curved section 173, which is the most distal curved section, and the base portion 140, and is the most distal curved section when the continuous robot 100 advances. It is a subsequent curved section following the curved section 173.
  • the curved section 173, which is the most distal curved section becomes the leading curved section.
  • FIG. 2 is a diagram showing an example of a detailed schematic configuration of one curved section 171 among the schematic configurations of the continuous robot 100 shown in FIG. That is, FIG. 2 shows a detailed schematic configuration of a curved section 171 at the proximal end located closest to the base 140.
  • the same components as those shown in FIG. 1 are designated by the same reference numerals, and detailed description thereof will be omitted.
  • the curvature angle of the curved section 171 is indicated by ⁇ 1
  • the turning angle of the curved section 171 is indicated by ⁇ 1
  • the radius of curvature of the curved section 171 (point O and point w1 in FIG. 2). (Corresponding to the connecting line segment) is indicated by ⁇ 1.
  • the wires 111 to 113 are connected to the connecting portions 121 to 123 of the distal end 160 of the curved section 171, and the wires 111 to 113 are installed inside the base portion 140, respectively.
  • the posture (curved shape) is controlled by being pushed and pulled by ⁇ 133.
  • the actuator 131 is a drive unit for driving the wire 111
  • the actuator 132 is a drive unit for driving the wire 112
  • the actuator 133 is for driving the wire 113. It is a drive unit.
  • the continuous robot 100 has wire guides 161 to 164 which are members for guiding the wires 111 to 113 in the curved section 171.
  • wire guides 161 to 164 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 111 at the fixing portions 150 to 153. Further, in FIG. 2, the central axis of the continuous robot 100 is shown by a broken line.
  • the wires 111 to 113 are referred to as a wire, b wire, and c wire in a counterclockwise direction in the xy plane.
  • the wire 111 corresponds to the a wire
  • the drive displacement of the wire 111 due to the push and pull of the actuator 131 in the curved section 171 is shown as l p1a.
  • the wire 112 corresponds to the b wire
  • the drive displacement of the wire 112 due to the push and pull of the actuator 132 in the curved section 171 is shown as l p1b.
  • the wire 113 corresponds to the c wire, and the drive displacement of the wire 113 by pushing and pulling the actuator 133 in the curved section 171 is shown as l p1c.
  • the wires corresponding to the wires 111 to 113 and the actuators 131 to 133, respectively, are similar to the detailed schematic configuration regarding the curved section 171 shown in FIG. It is configured to have an actuator corresponding to the above, a distal end corresponding to the distal end 160, and a wire guide corresponding to the wire guides 161 to 164.
  • the drive displacements of the a to c wires driving the nth curved section are defined as l pna, l pnb , and l pnc .
  • FIG. 3 shows a first embodiment of the present invention, and is a diagram showing an example of a schematic configuration of an operating device 200 for operating three curved sections 171 to 173 of the continuous robot 100 shown in FIG. 1.
  • the operation device 200 according to the first embodiment shown in FIG. 3 will be described as "operation device 200-1".
  • the operating device 200-1 has a first operating unit 210 for operating the first curved section 171 and a second operating unit 220 for operating the second curved section 172. It is configured to have a third operation unit 230 for operating the third curved section 173.
  • the constituent units 231 to 233 are constituent units related to the bending angle ⁇ n of the third curved section 173, which is the most distal curved section.
  • the operating lever 231 is an operating lever with respect to the bending angle ⁇ n of the third bending section 173, and is driven by the motor 232. Further, the rotation angle is detected by the angle sensor 233. At this time, the motor 232 and the angle sensor 233 are fixed to the housing (not shown) of the operation lever 231.
  • the constituent units 234 to 236 are constituent units related to the turning angle ⁇ n of the third curved section 173, which is the most distal curved section.
  • the operating lever 234 is an operating lever for the turning angle ⁇ n of the third curved section 173, and is driven by the motor 235. Further, the rotation angle is detected by the angle sensor 236. At this time, the motor 235 and the angle sensor 236 are fixed to the housing (not shown) of the operating lever 234. In this way, the third operation unit 230 inputs the target bending angle ⁇ n and the target turning angle ⁇ n of the third curved section 173, which is the most distal curved section, by being operated by the operator, for example. It is a configuration corresponding to the most distal operating means for the purpose.
  • the constituent units 221 to 223 are constituent units related to the bending angle ⁇ n-1 of the second bending section 172 which is the succeeding bending section.
  • the operating lever 221 is an operating lever for the bending angle ⁇ n-1 of the second bending section 172, and is driven by the motor 222. Further, the rotation angle is detected by the angle sensor 223. At this time, the motor 222 and the angle sensor 223 are fixed to the housing (not shown) of the operating lever 221.
  • the constituent units 224 to 226 are constituent units related to the turning angle ⁇ n-1 of the second curved section 172 which is the succeeding curved section.
  • the operating lever 224 is an operating lever for the turning angle ⁇ n-1 of the second curved section 172, and is driven by the motor 225. Further, the rotation angle is detected by the angle sensor 226. At this time, the motor 225 and the angle sensor 226 are fixed to the housing (not shown) of the operating lever 224.
  • the second operation unit 220 is operated by the operator, for example, to have the target bending angle ⁇ n-1 and the target bending angle ⁇ n-1 of the second bending section 172 (second succeeding bending section) which is the succeeding bending section. It is a configuration corresponding to the second subsequent operation means for inputting the target turning angle ⁇ n-1.
  • the constituent units 211 to 213 are constituent units related to the bending angle ⁇ n-2 of the first bending section 171 which is the succeeding bending section.
  • the operating lever 211 is an operating lever for the bending angle ⁇ n-2 of the first bending section 171 and is driven by the motor 212. Further, the rotation angle is detected by the angle sensor 213. At this time, the motor 212 and the angle sensor 213 are fixed to the housing (not shown) of the operating lever 211.
  • the constituent units 214 to 216 are constituent units related to the turning angle ⁇ n-2 of the first curved section 171 which is the subsequent curved section.
  • the operating lever 214 is an operating lever for the turning angle ⁇ n-2 of the first curved section 171 and is driven by the motor 215. Further, the rotation angle is detected by the angle sensor 216. At this time, the motor 215 and the angle sensor 216 are fixed to the housing (not shown) of the operating lever 214.
  • the first operation unit 210 is operated by, for example, the operator, so that the target bending angle ⁇ n-2 and the target bending angle ⁇ n-2 of the first bending section 171 (first succeeding bending section) which is the succeeding bending section. It is a configuration corresponding to the first subsequent operating means for inputting the target turning angle ⁇ n-2.
  • the operating device 200-1 is provided with operating levers 211 to 231 and 214 to 234 for each of the bending angle ⁇ and the turning angle ⁇ for each of the bending sections 171 to 173. Further, in the present embodiment, when the control system 300 of the continuous robot described later in FIG. 4 controls the bending angle ⁇ or the turning angle ⁇ of the continuous robot 100 to an arbitrary target angle, the operating levers 211 to 231 And 214 to 234 are controlled to be similar to the angle.
  • the operating levers 211 to 231 and 214 to 234 are provided with sensors (not shown) for detecting the contact of the operator.
  • the sensors provided on the operating levers 211 to 231 and 214 to 234 may be equipped with a push button switch or a sensor for detecting the capacitance.
  • an operation button (not shown) for operating the displacement of the base portion 140z b may be provided, or a plurality of switches (not shown) for changing the operation mode may be provided.
  • FIG. 4 is a diagram showing an example of a schematic configuration of a continuous robot control system 300 according to the first embodiment of the present invention.
  • the continuous robot control system 300 according to the first embodiment shown in FIG. 4 will be described as a “continuous robot control system 300-1”.
  • FIG. 4 shows a block diagram of a switching control system between head tracking control and observation operation control.
  • the control system 300-1 continuum robot shown in FIG. 4 block P s is the continuum robot 100 (more specifically, the control target, equivalent to the actuator 131-133 is a drive unit shown in FIG. 2 Actuator) is shown. Then, in FIG. 4, the l p is the output from block P s, a vector indicating the driving displacement of the wire.
  • Block K s is a position control system for causing the settling on the target driving displacement l pref wire.
  • block P l is an operation system for operating the subsequent curved section having a motor (subsequent operation means).
  • This block Pl is, for example, in the example shown in FIG. 3, a second operating unit 220 and a first operating unit for operating the second curved section 172 and the first curved section 171 which are subsequent curved sections. It is an operation system corresponding to 210.
  • d ⁇ lf and d ⁇ lf represent the operating torque applied by the operator to the operating lever of the bending angle and the operating lever of the turning angle in the subsequent bending section, respectively. Then, in FIG.
  • the target bending angle theta lf and target turning angle zeta lf subsequent curved section which is the output of block P l, respectively, a second operation for operating the bending angle and the turning angle of the subsequent curved section It is an angle by the operation lever in the part 220 and the first operation part 210.
  • the block K l is a position control system for setting it to a target value, and the blocks G in and G out connected to the input end and the output end of the block K l , respectively, are , A gain that takes a value from 0 to 1.
  • the input device 310 has a target bending angle ⁇ lt and a target turning angle of the most distal bending section (the third bending section 173 in the example shown in FIG. 1). It is a device that inputs ⁇ lt to the block FTL and the kinematics calculation unit 340.
  • the target bending angle ⁇ lt and the target turning angle ⁇ lt of the most distal bending section, which are the outputs of the input device 310, are the third operations for manipulating the bending angle and the turning angle of the most distal bending section, respectively. It is an angle by the operation levers 231 and 234 in the part 230.
  • the operation levers 231 and 234 included in the third operation unit 230 shown in FIG. 3 for operating the third curved section 173, which is the most distal curved section, are not driven by the motor.
  • the block FTL is a configuration example of the head-following control algorithm.
  • This block FTL is the basis of the target bending angle theta lt and target turning angle zeta lt distal-most curved section that is input from the input device 310, and a displacement z b of the base portion 140 that is input from the input device 320, the Therefore , it is a subsequent calculation means for calculating the target bending angle ⁇ fFT and the target turning angle ⁇ fFTL of the succeeding curved section. More specifically, the target bending angle ⁇ fFT and the target turning angle ⁇ fFTL represent the target value vector by the head follow control system for the succeeding bending section.
  • the changeover switch unit 330 includes a first switch 331 and a second switch 332.
  • the second switch 332 is a switch to which a signal of 0 or 1 is input, and switches according to the input signal.
  • the changeover switch unit 330 is a two-pole double-throw type shown in FIG.
  • the first switch 331 is a switching process for selecting the target bending angle ⁇ fFT and the target turning angle ⁇ fFTL of the subsequent bending section obtained from the block FTL. I do.
  • the changeover switch unit 330 detects the operator's contact with the operation lever in the subsequent curved section (specifically, when a 0 signal is input to the second switch 332), the present embodiment The control system of is switched in the direction opposite to the state of the two-pole double-throw switch shown in FIG.
  • a first switch 331 the target bending angle theta lf and target turning angle of the subsequent curved section obtained from the block P l Performs switching processing to select ⁇ lf. This allows the operator to seamlessly add additional operations to the continuous robot 100 from the posture (curved shape) by the head follow control.
  • the kinematics calculation unit (Kinematics) 340 is based on the target bending angle ⁇ and the target turning angle ⁇ of the subsequent bending section selected by the first switch 331. This is a calculation means for calculating the target drive displacement lpref when the drive unit of the continuum robot 100 drives the wire in the subsequent curved section. Further, in the present embodiment, the kinematics calculation unit 340 is a driving unit of the continuous robot 100 based on the target bending angle ⁇ lt and the target turning angle ⁇ lt of the most distal bending section input from the input device 310. The target drive displacement lpref when driving the wire in the most distal curved section is also calculated.
  • kinematics is derived in order to obtain the target drive displacement of the wire by the actuator for controlling the bending angle ⁇ and the turning angle ⁇ of the continuous robot 100.
  • the definitions of the symbols used in this chapter are shown below.
  • l d Length of the central axis of the curved section
  • ⁇ n Curvature angle at the distal end
  • ⁇ n Turning angle at the distal end
  • ⁇ n Radius of curvature of the curved section
  • the three wires in each curved section corresponding to the wires 111 to 113 shown in FIG. 2 are referred to as a wire, b wire, and c wire counterclockwise in the xy plane.
  • the drive displacements of the a to c wires driving the nth curved section are defined as l pna, l pnb , and l pnc .
  • FIG. 5 is a diagram showing an arrangement example of three wires (a wire to c wire) in the nth curved section corresponding to the three wires 111 to 113 shown in FIG. 2 in the xy plane.
  • a wire ⁇ c wire length of the piece are arranged at the vertices of an equilateral triangle with r s, also the phase angle xi] n shown in FIG. 5, curved section of the n It is an angle that determines the arrangement of the wires that drive the.
  • the phase angle ⁇ 1 0.
  • the kinematics of the continuum robot 100 is derived by making the following assumptions. 1. 1. In each curved section, the wire is deformed to a constant curvature. 2. 2. The torsional deformation of the wire is not considered. 3. 3. The wire does not deform in the longitudinal direction. 4. Friction between the wire guide and the wire is not considered.
  • J ⁇ n be all moments of inertia including the motor of the operating lever that commands the bending angle ⁇ n of the nth bending section, and all the moments of inertia including the motor of the operating lever that commands the turning angle ⁇ n of the nth bending section.
  • the moment be J ⁇ n.
  • the operator's operating torque given to the operating lever commanding the bending angle ⁇ n of the nth bending section is d ⁇ n
  • the operator's operation given to the operating lever commanding the turning angle ⁇ n of the nth bending section Let the torque be d ⁇ n.
  • FIG. 6 is a diagram showing an example of head-following control of the continuous robot 100 according to the first embodiment of the present invention.
  • the same reference numerals are given to the configurations similar to those shown in FIG.
  • the z direction shown in FIG. 1 is taken from the lower side of the paper surface toward the upper side of the paper surface.
  • FIG. 6 shows a dotted line showing a target path 610 in which the continuous robot 100 including the base portion 140 and the bendable portion 170 travels.
  • the head follow control is a method of controlling so that the subsequent curved section passes through the same path as the path (target path 610) through which the most distal curved section of the bendable portion 170 passes, as shown in FIG. ..
  • the first curve angle with respect to the curve angle of the follow-up curve section corresponding to the forward movement of the continuous robot which is set according to the first target curve angle of the input distal curve section.
  • the bending angle of the following bending section is configured to reach the first target bending angle.
  • the time point 601 indicates an initial state in which the bendable portion 170 extending in the z direction from the upper surface of the base portion 140 is not curved.
  • FIG. 6 shows how the base portion 140 moves in the z direction and the bendable portion 170 bends as time passes from time to time 602, time point 603, time point 604, and time point 605. There is.
  • the continuous robot 100 can move through the space.
  • the target path 610 is determined in advance.
  • the bending angle of the most distal curved section is continuous with the bending angle of the succeeding curved section with the length of the curved section. May be propagated to.
  • the operator gives a command to the bending angle of the most distal curved section and the displacement (movement amount (advance amount)) of the base portion 140 with a joystick or the like, so that the continuum robot
  • the head tracking control of 100 can be performed in real time.
  • FIG. 7 shows a first embodiment of the present invention, and shows an example of calculation processing of a target bending angle ⁇ fFT and a target turning angle ⁇ fFTL of a subsequent bending section by the block FTL of FIG. It is a figure.
  • FIG. 7 an example assuming a case where the curved section 173 of FIG. 1 is applied as the most distal curved section and the curved section 172 of FIG. 1 is applied as the subsequent curved section will be described.
  • the coordinates on the graph indicate the pair of the position of the base and the bending angle of the curved section.
  • the bending angle corresponding to the coordinate a may be referred to as "angle a”
  • the position (displacement) of the base portion corresponding to the coordinate a may be referred to as position a (displacement a).
  • the horizontal axis represents the displacement z b of the base portion 140
  • the vertical axis shows the bending angle ⁇ of the distal-most curved section and the subsequent curved section.
  • the broken line “Leader” is a profile of the curvature angle of the most distal curved section corresponding to the target bending angle ⁇ lt of the most distal curved section input by the operator or the like via the input device 310. Shows.
  • the thick broken line “Follower” indicates a profile of the bending angle of the following bending section corresponding to, for example, the target bending angle ⁇ lf of the succeeding bending section.
  • the horizontal axis represents the displacement z b of the base portion 140
  • the vertical axis indicates the target turning angle ⁇ of the distal-most curved section and the subsequent curved section.
  • the broken line “Leader” indicates the profile of the turning angle related to the target turning angle ⁇ lt of the most distal curved section input by the operator or the like via the input device 310.
  • the thick broken line “Follower” indicates, for example, the profile of the turning angle related to the target turning angle ⁇ lf of the succeeding curved section.
  • the displacement z b of the base portion 140 is the displacement a and the target turning angle ⁇ lt of the most distal curved section is changed from the most distal turning angle a to the most distal turning angle b
  • the target turning angle ⁇ lf of the trailing curved section indicated by the thick broken line “Follower” is automatically changed so that the displacement z b of the base portion 140 is changed from the trailing turning angle c to the trailing turning angle d at the displacement c.
  • the displacement c is the length between the displacement a and the displacement c is determined to be the length l d of the subsequent curved section.
  • the block FTL of FIG. 4 calculates the target bending angle ⁇ fFTL and the target turning angle ⁇ fFTL of the subsequent bending section as follows.
  • the block FTL of FIG. 4 interpolates between the displacement a and the displacement c with a line connecting the target bending angle a at the displacement a and the target bending angle D at the displacement c. , This is calculated as the target bending angle ⁇ fFT of the subsequent bending section. In FIG. 7A, this interpolated portion is shown by a solid line.
  • the block FTL of FIG. 4 interpolates between the displacement a and the displacement c with a line connecting the target turning angle a at the displacement a and the target turning angle d at the displacement c.
  • this is calculated as the target turning angle ⁇ fFTL of the subsequent curved section.
  • this interpolated portion is shown by a solid line.
  • the block FTL of FIG. 4 stores the calculated profile of the bending angle related to the target bending angle ⁇ fFTL and the profile of the turning angle related to the target turning angle ⁇ fFTL in the internal storage unit, for example.
  • FIG. 7 an example assuming a case where the curved section 173 of FIG. 1 is applied as the most distal curved section and the curved section 172 of FIG. 1 is applied as the subsequent curved section has been described.
  • the number of bending sections of the bendable portion 170 of the continuous robot 100 is three or more, for example, the number of bending sections of the bendable portion 170 shown in FIG. 1 is three, the curvature of FIG. 1 is defined as the subsequent bending section described above.
  • the section 171 it can be realized, for example, by applying the curved section 172 of FIG. 1 immediately before as the distalmost curved section described above.
  • This operation can change the position of the most distal end of the bendable portion 170 while keeping the direction of the tip of the third curved section 173 constant, for example, the continuous robot 100 such as the stomach wall and the intestinal wall. It is suitable for observing along the wall surface running diagonally with respect to the traveling direction of the robot (hereinafter referred to as "strabismus motion").
  • the command value of the operator is added to the bending posture by the tip tracking control system. For example, if the operation system that gives the additional command is a midpoint return type lever, the bending shape by the leading tracking control is added. It is difficult to grasp, and there is a risk of damaging the continuous robot 100 itself and surrounding objects due to erroneous operation.
  • the additional operation amount is reset. Further, for example, if the operation system that gives an additional command gives the bending angular velocity, the additional operation amount is not reset, but it is not direct and difficult to grasp the bending angle given as a result. In order to solve this, it is desirable that the angle of the operating lever is synchronized with the bending angle and the turning angle during the head follow control. Therefore, in the present embodiment, an operation system having an angle sensor such as a motor and an encoder is used as the operation device 200-1. As a result, the operator can grasp the posture of the continuous robot 100 by the head follow control, and when adding the command value, the operation torque may be applied to the operation lever as it is.
  • control system 300-1 of the continuous robot shown in FIG. 4 the model shown in “1) Modeling” above, and the head tracking control system described in the chapter “2.1) Head tracking control” above.
  • the control system 300-1 of the continuous robot shown in FIG. 4 the model shown in “1) Modeling” above, and the head tracking control system described in the chapter “2.1) Head tracking control” above.
  • the block P s is the continuum robot 100 to be controlled
  • l p which is the output from the block P s is a vector indicating the drive displacement of the wire
  • the block K s is the wire. It is a position control system for setting the target drive displacement lpref of.
  • the block Pl is an operation system including the motor shown in the equation (4)
  • the block K l is a position control system for setting it to the target value
  • the input end of the position control system and the block Pl are The blocks G in and G out connected to the output end are gains that take a value from 0 to 1.
  • the operation lever included in the operation unit for operating the most distal curved section is not driven by the motor.
  • the target bending angle ⁇ lt and the target turning angle ⁇ lt of the most distal curved section are, for example, the angles of the operating levers of the bending angle and the turning angle of the most distal curved section, and ⁇ ln and ⁇ ln in the equation (4). Equal to ⁇ ln.
  • the target bending angle ⁇ lf and the target turning angle ⁇ lf of the succeeding curved section are, for example, the angles of the operating levers of the bending angle and the turning angle of the succeeding curved section, and [ ⁇ l1 ,. .. . , ⁇ ln-1 , ⁇ l1 , ... .. ..
  • the operating torques d ⁇ lf and d ⁇ lf represent the operating torque applied to the operating levers of the bending angle and the turning angle of the subsequent bending section by the operator, and [d ⁇ 1 ,. .. .. , D ⁇ n-1 , d ⁇ 1 ,. .. .. , D ⁇ n-1 ].
  • the changeover switch unit 330 is shown in FIG. It is in the state of a polar double throw type switch. Then, at this time, the target bending angle ⁇ fTTL and the target turning angle ⁇ fFTL of the succeeding bending section for the leading follow-up control system output from the FTL block are input to the kinematics calculation unit 340. At the same time, the target bending angle ⁇ fFT and the target turning angle ⁇ fFTL of the succeeding bending section for the head following control system are the target angles of the operating lever, and 1 is input to the values of the block G in and the block G out.
  • the angle of the operating lever is feedback-controlled by the block K l and follows the target bending angle ⁇ fFT and the target turning angle ⁇ fFTL of the subsequent bending section.
  • the operation torque d Shitalf and d Zetalf applied to the operation lever because it is suppressed as a disturbance, the angle theta lf and zeta lf the operating lever, little effect.
  • the changeover switch unit 330 is shown in FIG. Switch in the direction opposite to the state of the 2-pole double-throw switch shown in.
  • the control system of the operation lever is blocked, is possible to change the angle theta lf and zeta lf the operating lever by operating torque d Shitalf and d Zetalf can.
  • the operator seamlessly adds an additional operation to the continuous robot 100 from the posture by the head follow control. It becomes possible.
  • the changeover switch unit 330 is shown in FIG. Switch to the state of the 2-pole double-throw switch shown in 4.
  • the angles of the continuous robot 100 and the operating lever return to the posture by the head follow control.
  • the posture angle of the continuous robot 100 and the angle of the operating lever are matched, but the present invention is not limited to this embodiment, and for example, a magnification may be given.
  • FIG. 8 is a diagram showing a first example of a simulation result in a control method of the continuous robot 100 by the continuous robot control system 300-1 according to the first embodiment of the present invention.
  • the horizontal direction is the x direction of FIG. 1 and the vertical direction is the z direction of FIG. 1, and the motion control of the curved sections 171 to 173 in the bendable portion 170 of the continuous robot 100.
  • FIGS. 8A to 8F perform control by head tracking control and additional operation, and show the posture of the bendable portion 170 step by step until the base portion 140 advances to 0.02 m by a stick diagram. It is a thing.
  • the solid line indicates the shape of the bendable portion 170 of the continuous robot 100
  • the round dots indicate the tips of the curved sections
  • the thin lines indicate the loci of the tips of the curved sections.
  • FIG. 8A shows a posture in which the third curved section 173, which is the most distal curved section, is curved by the bending operation of the operator, and the base portion 140 is starting to advance.
  • FIG. 8B when the operator further advances the base portion 140, it can be seen that the second curved section 172 follows the third curved section 173 by the head tracking control system.
  • FIG. 8C shows the operation control based on the operation of the operation lever by the operator at the bending angle ⁇ of the second bending section 172.
  • the changeover switch unit 330 switches in the direction opposite to the state of the two-pole double-throw switch shown in FIG. 4, indicating a posture in which an additional operation is performed with respect to the posture of the head follow-up control.
  • FIG. 8D shows a posture in which the additional operation is performed in the direction in which the bending angle ⁇ of the second bending section 172 becomes shallow.
  • FIG. 8E shows a state in which the operation of the operation lever at the bending angle ⁇ of the second bending section 172 by the operator is completed, and the posture before the additional operation is restored.
  • FIG. 8F the base portion 140 further advances, and the second curved section 172 and the first curved section 171 follow the third curved section 173 and the second curved section 172, respectively. It can be seen that it is possible to continue the head tracking control.
  • FIG. 9 is a diagram showing a second example of simulation results in the control method of the continuous robot 100 by the continuous robot control system 300-1 according to the first embodiment of the present invention.
  • the horizontal direction is the y direction of FIG. 1
  • the vertical direction is the z direction of FIG. 1
  • the depth direction is the x direction of FIG.
  • the operation control of the curved sections 171 to 173 in 170 is shown in the order of passage of time. More specifically, FIGS. 9A-9F show a simulation response in which an additional operation is added to the turning angle ⁇ .
  • FIGS. 9A and 9B the head tracking control system is performed in the same manner as in FIGS. 8A and 8B described above.
  • FIG. 9C shows a case where the operator operates the operation lever in the direction in which the turning angle ⁇ of the second curved section 172 becomes deeper.
  • the changeover switch unit 330 switches in the direction opposite to the state of the two-pole double-throw switch shown in FIG. 4, and an additional operation is performed with respect to the posture of the head follow control. Shows posture.
  • FIG. 9D shows a posture in which the additional operation is performed in the direction in which the turning angle ⁇ of the curved section 172 of 2 becomes shallower.
  • the base portion 140 further advances, and the second curved section 172 and the first curved section 171 follow the third curved section 173 and the second curved section 172, respectively. It can be seen that it is possible to continue the head tracking control.
  • FIG. 10 is a diagram showing a third example of simulation results in a method of controlling a continuous robot 100 by a continuous robot control system 300 according to the first embodiment of the present invention.
  • the horizontal direction is the y direction of FIG. 1
  • the vertical direction is the z direction of FIG. 1
  • the depth direction is the x direction of FIG.
  • the operation control of the curved sections 171 to 173 in 170 is shown in the order of passage of time. More specifically, in FIGS. 10A to 10F, by combining the operation of FIG. 8 and the operation of FIG. 9, the bendable portion 170 keeps the direction of the tip of the third curved section 173 constant in the three-dimensional space. Manipulating the position of the most distal end of.
  • the target bending angle ⁇ lt and the target turning angle ⁇ lt of the most distal curved section, and the base portion 140 The target bending angle ⁇ fFT and the target turning angle ⁇ fFTL of the subsequent bending section are calculated based on the displacement. Then, the switching unit 330, a target bending angle theta FFTL and target turning angle zeta FFTL subsequent curved section obtained from block FTL, the target bending angle of the subsequent curved section obtained from the block P l which corresponds to the subsequent operating unit theta and to perform the switching process for selecting either one of the lf and target turning angle zeta lf.
  • the drive unit of the continuum robot 100 drives the wire of the subsequent bending section based on the target bending angle and the target turning angle of the succeeding bending section selected by the changeover switch unit 330.
  • the drive displacement is calculated.
  • the schematic configuration of the continuous robot according to the second embodiment is the same as the schematic configuration of the continuous robot 100 according to the first embodiment shown in FIGS. 1 and 2. Further, the schematic configuration of the operating device according to the second embodiment is the same as the schematic configuration of the operating device 200-1 according to the first embodiment shown in FIG.
  • the most distal curved section is the subsequent curved section.
  • the form of increasing the types of observation movements is shown by linking with the additional operation of. For example, in the case where the number of curved sections is 3 as in the bendable portion 170 of the continuous robot 100 shown in FIG. 1, if the second curved section 172 and the third curved section 173 are driven in the same direction, the continuous robot The tip position of the bendable portion 170 of 100 can be greatly changed. This action is suitable, for example, when observing a wide area of the body, or when the distal end of the continuum robot 100 needs to be moved significantly to avoid contact with organs (hereinafter). , Called "large bending motion").
  • FIG. 11 is a diagram showing an example of a schematic configuration of a continuous robot control system 300 according to a second embodiment of the present invention.
  • the continuous robot control system 300 according to the second embodiment shown in FIG. 11 will be described as a “continuous robot control system 300-2”.
  • FIG. 11 shows a block diagram of the control system in which the most distal curved section is interlocked with the additional operation of the subsequent curved section.
  • the input device 310 is deleted from the continuous robot control system 300-1 of FIG. 4, and the block FTL is changed to the block FTL + Memory (storage unit).
  • the block K l is changed to the block K lf
  • block P l is changed to block P lf
  • the third switch 333 is changed to be added to the switching unit 330.
  • a NOT gate is added to the subsequent stage of the second switch 332 with respect to the continuous robot control system 300-1 of FIG.
  • the block Plt and the block Plf are an operation system including a motor in the most distal curved section (the most distal operating means) and an operating system including a motor in the succeeding curved section (subsequent operating means), respectively. ..
  • the blocks K lt and K lf are an operation system position control system in the most distal curved section and an operation system position control system in the subsequent curved section, respectively.
  • the target bending angle ⁇ lt and the target turning angle ⁇ lt of the most distal bending section are the angle by the operating lever of the bending angle of the most distal section and the operating lever of the turning angle of the most distal section, respectively.
  • the operating torques d ⁇ lt and d ⁇ lt represent the operating torques applied to the operating levers of the bending angle and the turning angle of the most distal curved section by the operator, and correspond to d ⁇ n and d ⁇ n in the equation (4).
  • the block FTL + Memory storage unit is provided with a storage unit that stores the target bending angle ⁇ lt and the target turning angle ⁇ lt of the most distal curved section in addition to the head tracking control in the first embodiment. There is.
  • Block Kr is an algorithm that links the postures of the succeeding curved section and the most distal curved section, and the type of motion can be changed by switching this.
  • the block K r is a block FTL + Memory target bending angle theta lt and target turning angle zeta lt distal-most curved section stored in (the storage unit), an input from the block P lf corresponding to the subsequent operating means It is the most distal calculation means for calculating the target bending angle and the target turning angle of the most distal bending section based on the target bending angle ⁇ lf and the target turning angle ⁇ lf of the succeeding bending section.
  • the third switch 333 of the switching unit 330 a target bending angle and the target turning angle of the distal-most curved section obtained from the block K r, farthest obtained from block P lt is the most distal operating means It is a switch (most distal switching means) that performs a switching process for selecting either the target bending angle ⁇ lt or the target turning angle ⁇ lt in the position bending section.
  • the kinematics calculation unit 340 is the farthest in the drive unit of the continuum robot 100 based on the target bending angle and the target turning angle of the most distal bending section selected by the third switch 333. It takes the form of calculating the drive displacement when driving the wire in the curved section.
  • the changeover switch unit 330 is shown in FIG. 11 when head tracking control is being performed (specifically, when a signal of 1 is input to the second switch 332). It is in the state of a polar double throw type switch. Then, in this case, the NOT gate provided in the rear stage of the second switch 332 is used to block G in and G out in the feedback loop of the operation system of the most distal curved section shown in the upper part of FIG. 0 is input. Thus, the operation system of the distal-most curved section is cut off, it is possible to vary the angle theta lt and zeta lt of the operating lever by operating torque d Shitalt and d ⁇ lt.
  • the third switch 333 selects the target bending angle ⁇ lt and the target turning angle ⁇ lt of the most distal bending section obtained from the block Plt. Is switched.
  • the kinematics calculation unit 340 a target bending angle theta lt and target turning angle zeta lt distal-most curved section obtained from the block P lt, target bending angle of the subsequent curved section obtained from block FTL ⁇ fTTL and target turning angle ⁇ fFTL are input.
  • the changeover switch unit 330 is shown in FIG. Switch in the direction opposite to the state of the 3-pole double-throw switch shown in.
  • 0 is input to the blocks G in and G out in the feedback loop of the operation system of the subsequent curved section shown in the lower part of FIG.
  • the operation system in the subsequent curved section is cut off, and the angles ⁇ lf and ⁇ lf of the operation lever can be changed by the operation torques d ⁇ lf and d ⁇ lf.
  • the target bending angle ⁇ lf and the target turning angle ⁇ lf of the succeeding curved section are input to the kinematics calculation unit 340 via the first switch 331, so that the operator follows the continuous robot 100 at the head. It is possible to seamlessly add additional operations from the controlled posture.
  • 1 is input to the blocks G in and G out in the feedback loop of the operation system of the most distal curved section shown in the upper part of FIG. 11 by the NOT gate provided in the rear stage of the second switch 332. Will be done.
  • the positioning control by the block K lt becomes effective, and the operator's operation torques d ⁇ lt and d ⁇ lt are suppressed as disturbances.
  • the third switch 333 performs the switching process for selecting a target bending angle and the target turning angle of the distal-most curved section obtained from the block K r.
  • both the continuous robot 100 and the operating lever are linked to the additional manipulated variable, and the angle related to the most distal curved section is linked to the additional manipulated variable in the subsequent curved section.
  • FIG. 12 is a diagram showing a first example of a simulation result in a control method of the continuous robot 100 by the continuous robot control system 300-2 according to the second embodiment of the present invention.
  • the horizontal direction is the x direction of FIG. 1 and the vertical direction is the z direction of FIG. 1, and the operation control of the bending sections 171 to 173 in the bendable portion 170 of the continuous robot 100 Are shown in order of passage of time.
  • FIGS. 12A-12F show the simulation response to which additional operations are added.
  • the solid line indicates the shape of the bendable portion 170 of the continuous robot 100
  • the round dots indicate the tips of the curved sections
  • the thin lines indicate the loci of the tips of the curved sections.
  • FIGS. 12A and 12B the same head-following control system as in FIGS. 9A and 9B described above is performed.
  • the operator shows the operation control based on the operation of the operation lever at the bending angle ⁇ of the second bending section 172.
  • the changeover switch unit 330 switches in the direction opposite to the state of the three-pole double-throw switch shown in FIG. 11, indicating a posture in which an additional operation is performed with respect to the posture of the head follow-up control.
  • the bending angle ⁇ 3ref of the most distal curved section is the following (8) with respect to the difference ⁇ l2- ⁇ 2FTL between the command angle of the head follow control of the second curved section 172 and the angle due to the additional operation. ) It is linked so that it becomes an expression.
  • ⁇ 3 Mem is the bending angle of the most distal curved section immediately before the 3-pole double-throw type switch of the changeover switch unit 330 is switched, which is stored in the block FTL + Memory (storage unit).
  • FIG. 12D shows the posture in which the additional operation is performed in the direction in which the bending angle ⁇ of the second bending section 172 becomes shallow.
  • the base portion 140 further advances, and the second curved section 172 and the first curved section 171 become the third curved section 173 and the second curved section 172, respectively. It can be seen that it is possible to continue the follow-up control.
  • FIG. 13 is a diagram showing a second example of simulation results in the control method of the continuous robot 100 by the continuous robot control system 300-2 according to the second embodiment of the present invention.
  • the horizontal direction is the x direction of FIG. 1 and the vertical direction is the z direction of FIG. 1, and the operation control of the bending sections 171 to 173 in the bendable portion 170 of the continuous robot 100.
  • the horizontal direction is the x direction of FIG. 1 and the vertical direction is the z direction of FIG. 1
  • the operation control of the bending sections 171 to 173 in the bendable portion 170 of the continuous robot 100 are shown in order of passage of time. More specifically, FIGS. 13A to 13F show a large bending operation due to an additional operation.
  • the bending angle ⁇ 3ref at the most distal end is set to the following equation (9) with respect to the difference between the command angle of the head tracking control of the second bending section 172 and the angle due to the additional operation. It is linked so that it becomes.
  • the second curved section 172 and the third curved section 173 are interlocked to realize a large bending operation.
  • the base portion 140 further advances, and the second curved section 172 and the first curved section 171 follow the third curved section 173 and the second curved section 172, respectively. It can be seen that the follow-up control can be continued.
  • the second embodiment it is possible to prevent the continuous robot 100 from unintentionally coming into contact with the object, as in the first embodiment. As a result, it is possible to prevent the object from being damaged or the continuous robot itself from being damaged, and it is possible to safely operate the continuous robot.
  • the schematic configuration of the continuous robot according to the third embodiment is the same as the schematic configuration of the continuous robot 100 according to the first embodiment shown in FIGS. 1 and 2. Further, the schematic configuration of the operating device according to the third embodiment is the same as the schematic configuration of the operating device 200-1 according to the first embodiment shown in FIG.
  • the most distal curved section is interlocked with the additional operation of the succeeding curved section, but in the third embodiment, the succeeding curved sections are interlocked with each other. This shows a form of increasing the types of observation movements.
  • FIG. 14 is a diagram showing an example of a schematic configuration of a continuous robot control system 300 according to a third embodiment of the present invention.
  • the continuous robot control system 300 according to the third embodiment shown in FIG. 14 will be described as “continuous robot control system 300-3”.
  • FIG. 14 shows a block diagram of a control system in which subsequent curved sections are interlocked with each other by an additional operation.
  • control system 300-3 of the continuum robot the control system of the succeeding curved section is divided into two control systems with respect to the control system 300-1 of the continuum robot of FIG.
  • the control system of the subsequent curved section including the blocks G in and G out and the block P l connected to the input end and the output end of the block K l and the block K l shown in FIG. 4 is the second. It has been changed as a control system for the curved section 172 (second subsequent curved section). More specifically, the control system of the second curved section 172 includes the blocks G in and G out and the block P l 2 connected to the input end and the output end of the block K l2 and the block K l2 shown in FIG. 14, respectively.
  • the control system of the first curved section 171 (first subsequent curved section) is added to the control system 300-1 of the continuous robot of FIG. There is. More specifically, the control system of the first curved section 171 is the control system of the subsequent curved section including the block K l1 and the block Pl 1 shown in FIG. Further, in the control system 300-3 of the continuous robot, a control gain block G w is added to the control system 300-1 of the continuous robot of FIG. Further, in the continuous robot control system 300-3, with respect to the continuous robot control system 300-1 of FIG. 4, the changeover switch unit 330 replaces the first switch 331 with the fourth switch 334 and the fifth switch. Switch 335 is provided.
  • Block P l1 is the first curved section 171 (first subsequent curved section) target bending angle theta LF1 and operation system having a motor for inputting target turning angle zeta LF1 (first subsequent operation means).
  • This block Pl1 is, for example, in the example shown in FIG. 3, an operation system corresponding to the first operation unit 210 for operating the first curved section 171 which is the subsequent curved section.
  • the target bending angle ⁇ lf1 and the target turning angle ⁇ lf1 of the first bending section 171 are angles by the operating lever of the first bending section 171.
  • the operating torques d ⁇ lf1 and d ⁇ lf1 represent the operating torques applied by the operator to the operating lever at the bending angle of the first bending section 171 and the operating lever at the turning angle of the first bending section 171, respectively. There is.
  • the block P l2 is an operation system (second subsequent operating means) having a motor for inputting the target bending angle ⁇ lf2 and the target turning angle ⁇ lf2 of the second bending section 172 (second succeeding bending section).
  • This block Pl2 is, for example, in the example shown in FIG. 3, an operation system corresponding to the second operation unit 220 for operating the second curved section 172 which is the subsequent curved section.
  • the target bending angle ⁇ lf2 and the target turning angle ⁇ lf2 of the second bending section 172 are angles by the operating lever of the second bending section 172.
  • the operating torques d ⁇ lf2 and d ⁇ lf2 represent the operating torques applied by the operator to the operating lever at the bending angle of the second bending section 172 and the operating lever at the turning angle of the second bending section 172, respectively. There is.
  • the block K l1 is an operation system position control system of the first curved section 171 (first succeeding curved section), and the block K l2 is a second curved section 172 (second succeeding curved section). Operation system Position control system.
  • the block FTL calculates the target bending angle ⁇ f2FTL and the target turning angle ⁇ f2FTL of the second bending section 172, and also calculates the target bending angle ⁇ f1FT and the target turning angle ⁇ of the first bending section 171. Calculate f1FTL .
  • the control gain block G w is a matrix that specifies the ratio of the postures of the succeeding curved section and the most distal curved section, and the type of operation is changed by switching this. Can be changed.
  • the control gain block G w, the target bending angle theta LF2 and target turning angle ⁇ of the second curved section 172 which is obtained from the block P l2 is a second subsequent operating means LF2 It is a control gain means that multiplies the control gain.
  • the changeover switch is used.
  • the unit 330 switches in the direction opposite to the state of the three-pole double-throw switch shown in FIG.
  • the changeover switch unit 330 has the second curvature obtained from the block Pl2 which is the second subsequent operating means as the target bending angle and the target turning angle of the second bending section 172.
  • a switching process for selecting the target bending angle ⁇ lf2 and the target turning angle ⁇ lf2 of the section 172 is performed, and the control gain block is used as the target bending angle and the target turning angle of the first bending section 171 in the fifth switch 335.
  • Switching processing is performed to select the target bending angle ⁇ lf2 and the target turning angle ⁇ lf2 of the second bending section 172 multiplied by the control gain by G w.
  • FIG. 15 is a diagram showing an example of a simulation result in a control method of the continuous robot 100 by the continuous robot control system 300-3 according to the third embodiment of the present invention.
  • the horizontal direction is the x direction of FIG. 1 and the vertical direction is the z direction of FIG. 1, and the operation control of the bending sections 171 to 173 in the bendable portion 170 of the continuous robot 100.
  • FIGS. 15A-15F show the simulation response to which additional operations are added.
  • the solid line shows the shape of the bendable portion 170 of the continuous robot 100
  • the round dots indicate the tips of the curved sections
  • the thin lines show the loci of the tips of the curved sections.
  • FIGS. 15A and 15B the same head-following control system as in FIGS. 9A and 9B described above is performed.
  • FIGS. 15C and 15D show a case where the operator operates the operating lever of the bending angle of the second bending section 172.
  • the changeover switch unit 330 switches in the direction opposite to the state of the 3-pole double-throw switch shown in FIG. 14, and an additional operation is performed with respect to the posture of the head follow control. It shows the posture that is being broken.
  • the bending angle ⁇ 1ref of the first bending section 171 is the following (10) with respect to the difference ⁇ l2- ⁇ 2FTL between the command angle of the head tracking control of the second bending section 172 and the angle due to the additional operation. Link so that it becomes an expression.
  • the third embodiment it is possible to prevent the continuous robot 100 from unintentionally coming into contact with the object, as in the first embodiment. As a result, it is possible to prevent the object from being damaged or the continuous robot itself from being damaged, and it is possible to safely operate the continuous robot.
  • the schematic configuration of the continuous robot according to the fourth embodiment is the same as the schematic configuration of the continuous robot 100 according to the first embodiment shown in FIGS. 1 and 2. Further, the schematic configuration of the operating device according to the fourth embodiment is the same as the schematic configuration of the operating device 200-1 according to the first embodiment shown in FIG.
  • the first to third embodiments described above are modes assuming an additional bending operation at the time of forward movement accompanied by head-following control.
  • an embodiment assuming an additional bending operation at the time of retreat will be described.
  • the curvature angle ⁇ and the turning angle ⁇ of the most distal curved section may be operated by the operator in the same manner as at the time of advancing, but the operation at the time of advancing is recorded and based on the operation. it may be reproduced in accordance with the displacement z b of the base portion 140.
  • FIG. 16 is a diagram showing an example of a schematic configuration of a continuous robot control system 300 according to a fourth embodiment of the present invention.
  • the continuous robot control system 300 according to the fourth embodiment shown in FIG. 16 will be described as a “continuous robot control system 300-4”.
  • the same reference numerals are given to the configurations similar to those shown in FIG. 11, and detailed description thereof will be omitted.
  • the most distal switching means that performs switching processing to select either the target bending angle ⁇ lt or the target turning angle ⁇ lt of the most distal bending section obtained from the block plt, which is the position operating means. be.
  • the positioning control of the operation lever in the most distal curved section is controlled. Is invalidated.
  • the operation angle of the operation lever by the operator is input to the kinematics calculation unit 340 via the one-pole double-throw type switch of the changeover switch unit 360, so that the head tracking control is performed.
  • the changeover switch unit 360 switches in the direction opposite to the state of the one-pole double-throw type switch shown in FIG.
  • the target value, the most distal curvature corresponding to the displacement z b of the base portion 140 that is recorded during forward from FTL + Memory (storage unit)
  • the target bending angle and target turning angle of the section are output. Since the target bending angle and the target turning angle of the most distal curved section are simultaneously input to the kinematics calculation unit 340, the continuum robot 100 reproduces the shape recorded during forward movement while moving backward (backward). ..
  • the changeover switch unit 330 is in the direction opposite to the state of the three-pole double-throw switch shown in FIG. Switch to.
  • the operation of the subsequent curved section at this time is the same as that of the first embodiment described above.
  • the control of the distal-most curved section, displacement z b of the base portion 140 even during reverse travel even during forward, positioning control of the operating lever of the distal-most curved section is invalidated, pilot
  • the operation angle of the operation lever is input to the kinematics calculation unit 340 via the changeover switch unit 330 without going through the changeover switch unit 360.
  • the schematic configuration of the continuous robot according to the fifth embodiment is the same as the schematic configuration of the continuous robot 100 according to the first embodiment shown in FIGS. 1 and 2. Further, the schematic configuration of the continuous robot control system according to the fifth embodiment of the present invention is a schematic configuration of the continuous robot control systems 300-1 to 300-4 according to the first to fourth embodiments described above. Any of the configurations are applicable.
  • a fifth embodiment shows a mode in which a two-axis motorized joystick is used as the operating device 200.
  • FIG. 17 shows a fifth embodiment of the present invention, and is a diagram showing an example of a schematic configuration of an operating device 200 for operating three curved sections 171 to 173 of the continuous robot 100 shown in FIG. 1.
  • the operation device 200 according to the fifth embodiment shown in FIG. 17 will be described as "operation device 200-5".
  • the operating device 200-5 has a first operating unit 240 for operating the first curved section 171 and a second operating unit 250 for operating the second curved section 172. It is configured to have a third operation unit 260 for operating the third curved section 173.
  • the first operation unit 240 includes an operation lever 241, motors 242 and 244, and angle sensors 243 and 245.
  • the operating lever 241 is an operating lever of the first curved section 171.
  • the angle sensors 243 and 245 detect the operating angles ⁇ x 1 and ⁇ y 1 with respect to the first curved section 171, respectively.
  • the motors 242 and 244 are motors for applying control torque to the operating angles ⁇ x 1 and ⁇ y 1, respectively.
  • the second operation unit 250 includes an operation lever 251, motors 252 and 254, and angle sensors 253 and 255.
  • the operation lever 251 is an operation lever of the second curved section 172.
  • the angle sensors 253 and 255 detect the operating angles ⁇ x 2 and ⁇ y 2 with respect to the second curved section 172, respectively.
  • the motors 252 and 254 are motors for applying control torque to the operating angles ⁇ x 2 and ⁇ y 2, respectively.
  • the third operation unit 260 includes an operation lever 261, motors 262 and 264, and angle sensors 263 and 265.
  • the operation lever 261 is an operation lever of the third curved section 173.
  • the angle sensors 263 and 265 detect the operating angles ⁇ x 3 and ⁇ y 3 with respect to the third curved section 173, respectively.
  • the motors 262 and 264 are motors for applying control torque to the operating angles ⁇ x 3 and ⁇ y 3, respectively.
  • the schematic configuration of the continuous robot according to the sixth embodiment is the same as the schematic configuration of the continuous robot 100 according to the first embodiment shown in FIGS. 1 and 2. Further, the schematic configuration of the operating device according to the sixth embodiment is the same as the schematic configuration of the operating device 200-1 according to the first embodiment shown in FIG.
  • a value of 0 or a value of 1 is used for the blocks G in and G out in order to switch between the head tracking control and the additional operation.
  • a value of 0 or a value of 1 is used for the blocks G in and G out in order to switch between the head tracking control and the additional operation.
  • FIG. 18 is a diagram showing an example of a schematic configuration of a continuous robot control system 300 according to a sixth embodiment of the present invention.
  • the continuous robot control system 300 according to the sixth embodiment shown in FIG. 18 will be described as “continuous robot control system 300-6”.
  • FIG. 18 the same reference numerals are given to the configurations similar to those shown in FIG. 4, and detailed description thereof will be omitted.
  • a low-pass filter F lpf is added to the subsequent stage of the second switch 332 of the changeover switch unit 330 with respect to the continuous robot control system 300-1 of FIG.
  • a signal via the low-pass filter F lpf is input to the blocks G in and G out.
  • the folding point frequency of the low-pass filter F lpf may be about 1 Hz.
  • 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.
  • a circuit for example, ASIC
  • This program and a computer-readable storage medium that stores the program are included in the present invention.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Endoscopes (AREA)
  • Manipulator (AREA)

Abstract

連続体ロボットの操作を安全に行える仕組みを提供する。ブロックＦＴＬは、最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔと、基台部の変位とに基づいて、後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬを算出し、切替スイッチ部３３０は、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬと、後続操作手段に相当するブロックＰｌから得られる後続湾曲区間の目標湾曲角度θｌｆ及び目標旋回角度ζｌｆとのうちのいずれか一方を選択するための切替処理を行い、運動学演算部３４０は、切替スイッチ部３３０で選択された後続湾曲区間の目標湾曲角度及び目標旋回角度に基づいて、連続体ロボットの駆動部で後続湾曲区間のワイヤを駆動させる際の駆動変位を演算する。

Description

連続体ロボットの制御システム及びその制御方法、並びに、プログラム
　本発明は、ワイヤが駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部を備える連続体ロボットの制御システム及びその制御方法、並びに、当該制御システムとしてコンピュータを機能させるためのプログラムに関するものである。
　連続体ロボットは、コンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ複数の湾曲区間が設けられた湾曲可能部を備えて構成されており、その湾曲区間を変形させることによって形状が制御される。この連続体ロボットは、剛体リンクによって構成される剛体リンクロボットに対して、主に２つの優位性を持つ。１つ目は、剛体リンクロボットがはまり込んでしまうような狭い空間や散乱物のある環境の中で、連続体ロボットは、曲線に沿って移動可能である。２つ目は、連続体ロボットは、本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく操作することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。この特徴を生かし、連続体ロボットは、内視鏡のシースやカテーテルなどの医療の分野や、レスキューロボットなどの極限作業ロボットへの応用が期待されている。
　特許文献１には、湾曲可能部の先端にカメラを備える連続体ロボットに対して、カメラの視線の方向を一定に保ったまま先端の位置を移動させる動作や、カメラの視線が常に遠方の１点を通るように先端の位置と角度を移動させるといった特徴的な動作（以下、「観察動作制御」と呼ぶ）を、操縦者が簡単な操作を行うだけで実現できる制御方法が記載されている。
国際公開第２０１７／００３４６８号
　具体的に、特許文献１では、湾曲可能部の初期の湾曲姿勢から観察動作制御に移行するため、湾曲可能部の全ての湾曲区間に対する追加の湾曲量を操縦者による操作部の操作量から代数的に求めている。そのため、特許文献１に記載の制御では、湾曲可能部の初期の湾曲姿勢によっては、操縦者による操作部の操作量が軽微であっても、各湾曲区間に対する追加の湾曲量が大きく制御されることがありうる。連続体ロボットは、一般的に、先頭の最遠位湾曲区間の湾曲姿勢を後続湾曲区間に湾曲区間の長さをもって連続的に伝播させる制御（以下、「先頭追従制御」と呼ぶ）を用いることによって、狭小な空間の対象物における狭い経路に沿って動作させることができる。そして、このような狭小な空間において、特許文献１に記載の制御が行われれば、意図せずに連続体ロボットが対象物と接触してしまい、その結果、対象物が傷ついたり連続体ロボット自体が破損したりして、連続体ロボットの操作を安全に行えない問題が生じうる。
　本発明は、このような問題点に鑑みてなされたものであり、連続体ロボットの操作を安全に行える仕組みを提供することを目的とする。
　本発明の連続体ロボットの制御システムは、ワイヤが駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部と、前記湾曲可能部を支える基台部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、複数の前記湾曲区間のうち、前記基台部から最も遠い位置にある最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記連続体ロボットの進行方向における前記基台部の変位と、に基づいて、前記複数の湾曲区間のうち、前記最遠位湾曲区間と前記基台部との間に位置する後続湾曲区間の目標湾曲角度および目標旋回角度を算出する後続算出手段と、操作されることによって前記後続湾曲区間の目標湾曲角度および目標旋回角度を入力する後続操作手段と、前記後続算出手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、前記後続操作手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う後続切替手段と、前記後続切替手段で選択された前記後続湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記後続湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算する演算手段と、を有する。
　また、本発明は、上述した連続体ロボットの制御システムによる連続体ロボットの制御方法、及び、上述した連続体ロボットの制御システムとしてコンピュータを機能させるためのプログラムを含む。
　本発明によれば、連続体ロボットの操作を安全に行うことができる。
本発明の第１の実施形態に係る連続体ロボットの概略構成の一例を示す図である。 図１に示す連続体ロボットの概略構成のうち、１つの湾曲区間に関する詳細な概略構成の一例を示す図である。 本発明の第１の実施形態を示し、図１に示す連続体ロボットの３つの湾曲区間を操作するための操作装置の概略構成の一例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 図２に示す３つのワイヤに相当する第ｎの湾曲区間の３つのワイヤ（ａワイヤ～ｃワイヤ）のｘｙ平面における配置例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの先頭追従制御の一例を示す図である。 本発明の第１の実施形態を示し、図４のブロックＦＴＬによる後続湾曲区間の目標湾曲角度及び目標旋回角度の算出処理の一例を示す図である。 本発明の第１の実施形態を示し、図４のブロックＦＴＬによる後続湾曲区間の目標湾曲角度及び目標旋回角度の算出処理の一例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第３例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第３例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第３例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第３例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第３例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第３例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の一例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の一例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の一例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の一例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の一例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の一例を示す図である。 本発明の第４の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第５の実施形態を示し、図１に示す連続体ロボットの３つの湾曲区間を操作するための操作装置の概略構成の一例を示す図である。 本発明の第６の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。
　以下に、図面を参照しながら、本発明を実施するための形態（実施形態）について説明する。
　（第１の実施形態）
　まず、本発明の第１の実施形態について説明する。
　図１は、本発明の第１の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。図１には、連続体ロボット１００の一構成として、基台部１４０及び湾曲可能部１７０が図示されている。
　湾曲可能部１７０は、ワイヤが駆動することによって湾曲する複数の湾曲区間１７１～１７３が設けられた構成部（湾曲区間が複数設けられた構成部）である。具体的に、図１には、（ｎ－２）に係る湾曲区間１７１、（ｎ－１）に係る湾曲区間１７２、ｎに係る湾曲区間１７３のｎ個（３個）の湾曲区間が図示されている。また、図１では、湾曲区間１７３の湾曲角度θｎ及び旋回角度ζｎを示すとともに、湾曲区間１７２の湾曲角度θｎ－１及び旋回角度ζｎ－１を示し、湾曲区間１７１の湾曲角度θｎ－２及び旋回角度ζｎ－２を示している。基台部１４０は、湾曲可能部１７０を支える構成部であり、その内部には、湾曲区間１７１～１７３のそれぞれのワイヤを駆動させるための駆動部であるアクチュエータ（図１では不図示）が設けられている。また、図１では、基台部１４０の上面１４１の所定位置（例えば中心の位置）に原点Ｏをとり、連続体ロボット１００の進行方向（例えば前進する方向）をｚ方向とし、このｚ方向と直交する方向であって相互に直交する方向をｘ方向及びｙ方向とした、ｘｙｚ座標系を図示している。このように、連続体ロボット１００は、湾曲可能部１７０を構成する複数の湾曲区間１７１～１７３による湾曲動作に加えて、ｚ方向に移動が可能となっている。この際、図１では、連続体ロボット１００のｚ方向の移動量（前進量）を示す指標として基台部１４０の変位ｚｂを図示している。
　図１において、湾曲区間１７３は、湾曲可能部１７０を構成する複数の湾曲区間１７１～１７３のうち、基台部１４０から最も遠い位置にある最遠位湾曲区間である。湾曲区間１７２（湾曲区間１７１も同様）は、最遠位湾曲区間である湾曲区間１７３と基台部１４０との間に位置し、連続体ロボット１００が前進する際に最遠位湾曲区間である湾曲区間１７３の後に続く後続湾曲区間である。なお、連続体ロボット１００が前進する際に、最遠位湾曲区間である湾曲区間１７３は、先頭の湾曲区間となる。
　図２は、図１に示す連続体ロボット１００の概略構成のうち、１つの湾曲区間１７１に関する詳細な概略構成の一例を示す図である。即ち、図２は、基台部１４０から最も近い位置にある近位端の湾曲区間１７１に関する詳細な概略構成を示している。この図２において、図１に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。なお、図２では、湾曲区間１７１の湾曲角度をθ１で示すとともに湾曲区間１７１の旋回角度をζ１で示し、また、湾曲区間１７１の曲率半径（図２では、点Ｏと点ｗ１とを結ぶ線分に相当する）をρ１で示している。
　連続体ロボット１００は、湾曲区間１７１の遠位端１６０の接続部１２１～１２３にワイヤ１１１～１１３が接続され、このワイヤ１１１～１１３が基台部１４０の内部に設置されているそれぞれのアクチュエータ１３１～１３３で押し引きされることによって、姿勢（湾曲形状）が制御される。ここで、アクチュエータ１３１は、ワイヤ１１１を駆動させるための駆動部であり、また、アクチュエータ１３２は、ワイヤ１１２を駆動させるための駆動部であり、さらに、アクチュエータ１３３は、ワイヤ１１３を駆動させるための駆動部である。
　また、連続体ロボット１００は、湾曲区間１７１において、ワイヤ１１１～１１３を案内するための部材であるワイヤガイド１６１～１６４を有する。ワイヤガイド１６１～１６４は、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤガイド１６１～１６４は、ワイヤ１１１に、固定部１５０～１５３において固定される。また、図２において、連続体ロボット１００の中心軸を破線で示している。
　また、本実施形態では、ワイヤ１１１～１１３を、ｘｙ平面内において反時計回りに、ａワイヤ、ｂワイヤ及びｃワイヤと呼ぶ。具体的に、図２に示す例では、ワイヤ１１１がａワイヤに相当し、湾曲区間１７１のアクチュエータ１３１の押し引きによるワイヤ１１１の駆動変位をｌｐ１ａとして図示している。また、図２に示す例では、ワイヤ１１２がｂワイヤに相当し、湾曲区間１７１のアクチュエータ１３２の押し引きによるワイヤ１１２の駆動変位をｌｐ１ｂとして図示している。さらに、図２に示す例では、ワイヤ１１３がｃワイヤに相当し、湾曲区間１７１のアクチュエータ１３３の押し引きによるワイヤ１１３の駆動変位をｌｐ１ｃとして図示している。
　なお、図２に示す例では、湾曲区間１７１に関する詳細な概略構成のみを図示してその説明を行った。この点、図１に示す湾曲区間１７２及び湾曲区間１７３についても、図２に示す湾曲区間１７１に関する詳細な概略構成と同様に、それぞれが、ワイヤ１１１～１１３に相当するワイヤや、アクチュエータ１３１～１３３に相当するアクチュエータ、遠位端１６０に相当する遠位端、及び、ワイヤガイド１６１～１６４に相当するワイヤガイドを有して構成されている。ここで、一般化して、第ｎの湾曲区間を駆動するａ～ｃワイヤの駆動変位をｌｐｎａ，ｌｐｎｂ，ｌｐｎｃとする。
　図３は、本発明の第１の実施形態を示し、図１に示す連続体ロボット１００の３つの湾曲区間１７１～１７３を操作するための操作装置２００の概略構成の一例を示す図である。以下の説明では、図３に示す第１の実施形態に係る操作装置２００を「操作装置２００－１」として説明を行う。
　操作装置２００－１は、図３に示すように、第１の湾曲区間１７１を操作するための第１の操作部２１０、第２の湾曲区間１７２を操作するための第２の操作部２２０、及び、第３の湾曲区間１７３を操作するための第３の操作部２３０を有して構成されている。
　第３の操作部２３０において、構成部２３１～２３３は、最遠位湾曲区間である第３の湾曲区間１７３の湾曲角度θｎに関する構成部である。具体的に、操作レバー２３１は、第３の湾曲区間１７３の湾曲角度θｎに対する操作レバーであり、モータ２３２により駆動される。また、その回転角度は、角度センサ２３３により検出される。この際、モータ２３２及び角度センサ２３３は、操作レバー２３１の筐体（不図示）に固定されている。また、第３の操作部２３０において、構成部２３４～２３６は、最遠位湾曲区間である第３の湾曲区間１７３の旋回角度ζｎに関する構成部である。具体的に、操作レバー２３４は、第３の湾曲区間１７３の旋回角度ζｎに対する操作レバーであり、モータ２３５により駆動される。また、その回転角度は、角度センサ２３６により検出される。この際、モータ２３５及び角度センサ２３６は、操作レバー２３４の筐体（不図示）に固定されている。このように、第３の操作部２３０は、例えば操縦者に操作されることによって、最遠位湾曲区間である第３の湾曲区間１７３の目標湾曲角度θｎ及び目標旋回角度ζｎを入力するための最遠位操作手段に相当する構成である。
　第２の操作部２２０において、構成部２２１～２２３は、後続湾曲区間である第２の湾曲区間１７２の湾曲角度θｎ－１に関する構成部である。具体的に、操作レバー２２１は、第２の湾曲区間１７２の湾曲角度θｎ－１に対する操作レバーであり、モータ２２２により駆動される。また、その回転角度は、角度センサ２２３により検出される。この際、モータ２２２及び角度センサ２２３は、操作レバー２２１の筐体（不図示）に固定されている。また、第２の操作部２２０において、構成部２２４～２２６は、後続湾曲区間である第２の湾曲区間１７２の旋回角度ζｎ－１に関する構成部である。具体的に、操作レバー２２４は、第２の湾曲区間１７２の旋回角度ζｎ－１に対する操作レバーであり、モータ２２５により駆動される。また、その回転角度は、角度センサ２２６により検出される。この際、モータ２２５及び角度センサ２２６は、操作レバー２２４の筐体（不図示）に固定されている。このように、第２の操作部２２０は、例えば操縦者に操作されることによって、後続湾曲区間である第２の湾曲区間１７２（第２の後続湾曲区間）の目標湾曲角度θｎ－１及び目標旋回角度ζｎ－１を入力するための第２の後続操作手段に相当する構成である。
　第１の操作部２１０において、構成部２１１～２１３は、後続湾曲区間である第１の湾曲区間１７１の湾曲角度θｎ－２に関する構成部である。具体的に、操作レバー２１１は、第１の湾曲区間１７１の湾曲角度θｎ－２に対する操作レバーであり、モータ２１２により駆動される。また、その回転角度は、角度センサ２１３により検出される。この際、モータ２１２及び角度センサ２１３は、操作レバー２１１の筐体（不図示）に固定されている。また、第１の操作部２１０において、構成部２１４～２１６は、後続湾曲区間である第１の湾曲区間１７１の旋回角度ζｎ－２に関する構成部である。具体的に、操作レバー２１４は、第１の湾曲区間１７１の旋回角度ζｎ－２に対する操作レバーであり、モータ２１５により駆動される。また、その回転角度は、角度センサ２１６により検出される。この際、モータ２１５及び角度センサ２１６は、操作レバー２１４の筐体（不図示）に固定されている。このように、第１の操作部２１０は、例えば操縦者に操作されることによって、後続湾曲区間である第１の湾曲区間１７１（第１の後続湾曲区間）の目標湾曲角度θｎ－２及び目標旋回角度ζｎ－２を入力するための第１の後続操作手段に相当する構成である。
　以上説明したように、操作装置２００－１は、各湾曲区間１７１～１７３に対して、湾曲角度θ及び旋回角度ζのそれぞれについて操作レバー２１１～２３１及び２１４～２３４を備えている。また、本実施形態では、図４で後述する連続体ロボットの制御システム３００が、連続体ロボット１００の湾曲角度θもしくは旋回角度ζを任意の目標角度に制御しているとき、操作レバー２１１～２３１及び２１４～２３４をその角度と同様になるように制御する。
　さらに、操作レバー２１１～２３１及び２１４～２３４には、操縦者の接触を検出するセンサ（不図示）が設けられている。この操作レバー２１１～２３１及び２１４～２３４に設けるセンサとしては、押しボタンスイッチが装備されていたり静電容量を検出するセンサが装備されていたりしてもよい。さらに、基台部１４０ｚｂの変位を操作するための操作ボタン（不図示）を設けてもよく、また、動作モードを変更するためのスイッチ（不図示）を複数設けてもよい。
　図４は、本発明の第１の実施形態に係る連続体ロボットの制御システム３００の概略構成の一例を示す図である。以下の説明では、図４に示す第１の実施形態に係る連続体ロボットの制御システム３００を「連続体ロボットの制御システム３００－１」として説明を行う。具体的に、図４には、先頭追従制御と観察動作制御との切り替え制御系のブロック線図が示されている。
　図４に示す連続体ロボットの制御システム３００－１において、ブロックＰｓは、制御対象である連続体ロボット１００（より具体的には、図２に示す駆動部であるアクチュエータ１３１～１３３に相当するアクチュエータ）を示している。そして、図４において、ブロックＰｓからの出力であるｌｐは、ワイヤの駆動変位を示すベクトルである。また、図４に示す連続体ロボットの制御システム３００－１において、ブロックＫｓは、ワイヤの目標駆動変位ｌｐｒｅｆに整定させるための位置制御系である。
　また、図４に示す連続体ロボットの制御システム３００－１において、ブロックＰｌは、モータを有する後続湾曲区間を操作するための操作系（後続操作手段）である。このブロックＰｌは、例えば、図３に示す例では、後続湾曲区間である第２の湾曲区間１７２及び第１の湾曲区間１７１を操作するための第２の操作部２２０及び第１の操作部２１０に相当する操作系である。また、図４において、ｄθｌｆ及びｄζｌｆは、それぞれ、操縦者が後続湾曲区間の湾曲角度の操作レバー及び旋回角度の操作レバーに印加する操作トルクを表している。そして、図４において、ブロックＰｌの出力である後続湾曲区間の目標湾曲角度θｌｆ及び目標旋回角度ζｌｆは、それぞれ、後続湾曲区間の湾曲角度及び旋回角度を操作するための第２の操作部２２０及び第１の操作部２１０における操作レバーによる角度である。また、図４において、ブロックＫｌは、それを目標値に整定させるための位置制御系であり、また、このブロックＫｌの入力端及び出力端にそれぞれ接続されるブロックＧｉｎ及びＧｏｕｔは、０から１の値をとるゲインである。
　図４に示す連続体ロボットの制御システム３００－１において、入力装置３１０は、最遠位湾曲区間（図１に示す例では、第３の湾曲区間１７３）の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔを、ブロックＦＴＬ及び運動学演算部３４０に入力する装置である。そして、入力装置３１０の出力である最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔは、それぞれ、最遠位湾曲区間の湾曲角度及び旋回角度を操作するための第３の操作部２３０における操作レバー２３１及び２３４による角度である。なお、本実施形態では、最遠位湾曲区間である第３の湾曲区間１７３を操作するための図３に示す第３の操作部２３０に含まれる操作レバー２３１及び２３４は、モータ駆動されない。
　図４に示す連続体ロボットの制御システム３００－１において、入力装置３２０は、連続体ロボット１００の進行方向（例えば前進する方向）における基台部１４０の変位ｚｂを、ブロックＦＴＬに入力する装置である。
　図４に示す連続体ロボットの制御システム３００－１において、ブロックＦＴＬは、先頭追従制御アルゴリズムの構成例である。このブロックＦＴＬは、入力装置３１０から入力された最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔと、入力装置３２０から入力された基台部１４０の変位ｚｂと、に基づいて、後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬを算出する後続算出手段である。より具体的に、目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬは、後続湾曲区間に対する先頭追従制御系による目標値ベクトルを表している。
　図４に示す連続体ロボットの制御システム３００－１において、切替スイッチ部３３０は、第１のスイッチ３３１及び第２のスイッチ３３２を備えて構成されている。第１のスイッチ３３１は、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬと、ブロックＰｌから得られる後続湾曲区間の目標湾曲角度θｌｆ及び目標旋回角度ζｌｆと、のうちのいずれか一方を選択するための切替処理を行うスイッチ（後続切替手段）である。第２のスイッチ３３２は、０または１の信号が入力されるスイッチであり、入力信号に応じて切替を行う。この切替スイッチ部３３０は、先頭追従制御が行われているときには（具体的には、第２のスイッチ３３２に１の信号が入力されているときには）、図４に示している２極双投形スイッチの状態となっている。即ち、ここで説明した先頭追従制御が行われているときには、第１のスイッチ３３１は、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬを選択するための切替処理を行う。そして、切替スイッチ部３３０は、後続湾曲区間の操作レバーへの操縦者の接触が検知されると（具体的には、第２のスイッチ３３２に０の信号が入力されると）、本実施形態の制御系は、図４に示している２極双投形スイッチの状態とは逆の方向に切り替える。即ち、ここで説明した後続湾曲区間の操作レバーへの操縦者の接触が検知されると、第１のスイッチ３３１は、ブロックＰｌから得られる後続湾曲区間の目標湾曲角度θｌｆ及び目標旋回角度ζｌｆを選択するための切替処理を行う。これにより、操縦者は、連続体ロボット１００を先頭追従制御による姿勢（湾曲形状）から継ぎ目なく追加の操作を加えることが可能となる。
　図４に示す連続体ロボットの制御システム３００－１において、運動学演算部（Ｋｉｎｅｍａｔｉｃｓ）３４０は、第１のスイッチ３３１で選択された後続湾曲区間の目標湾曲角度θ及び目標旋回角度ζに基づいて、連続体ロボット１００の駆動部で後続湾曲区間のワイヤを駆動させる際の目標駆動変位ｌｐｒｅｆを演算する演算手段である。さらに、本実施形態では、運動学演算部３４０は、入力装置３１０から入力された最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔに基づいて、連続体ロボット１００の駆動部で最遠位湾曲区間のワイヤを駆動させる際の目標駆動変位ｌｐｒｅｆも演算する。
　１）モデリング
　本章では、連続体ロボット１００の湾曲角度θ及び旋回角度ζを制御するためのアクチュエータによるワイヤの目標駆動変位を求めるため、運動学の導出を行う。
　本章で用いる記号の定義を以下に示す。
ｌｄ：湾曲区間の中心軸の長さ
θｎ：遠位端の湾曲角度
ζｎ：遠位端の旋回角度
ρｎ：湾曲区間の曲率半径
　上述したように、本実施形態では、図２に示すワイヤ１１１～１１３に相当する各湾曲区間の３つのワイヤを、ｘｙ面内において反時計回りにａワイヤ、ｂワイヤ及びｃワイヤと呼ぶ。また、第ｎの湾曲区間を駆動するａ～ｃワイヤの駆動変位をｌｐｎａ，ｌｐｎｂ，ｌｐｎｃとする。
　図５は、図２に示す３つのワイヤ１１１～１１３に相当する第ｎの湾曲区間の３つのワイヤ（ａワイヤ～ｃワイヤ）のｘｙ平面における配置例を示す図である。図５に示すように、ａワイヤ～ｃワイヤは、一片の長さをｒｓとする正三角形の頂点に配置されており、また、図５に示す位相角ξｎは、第ｎの湾曲区間を駆動するワイヤの配置を決定する角度である。本実施形態では、位相角ξ１＝０とする。
　ここでは、以下の仮定をおき、連続体ロボット１００の運動学を導出する。
１．各湾曲区間において、ワイヤは曲率一定に変形する。
２．ワイヤのねじり変形は考慮しない。
３．ワイヤは、長手方向に変形しない。
４．ワイヤガイドとワイヤ間の摩擦は考慮しない。
　まず、第１の湾曲区間（図１及び図２の湾曲区間１７１に相当）におけるａ～ｃワイヤの駆動変位ｌｐ１ａ，ｌｐ１ｂ，ｌｐ１ｃと、第１の湾曲区間の遠位端の湾曲角度θ１及び旋回角度ζ１との関係は、以下の（１）式となる。
Figure JPOXMLDOC01-appb-M000001
　次に、複数の湾曲区間を有する連続体ロボット１００の第ｎの湾曲区間におけるａ～ｃワイヤの駆動変位ｌｐｎａ，ｌｐｎｂ，ｌｐｎｃと、第ｎの湾曲区間の遠位端の湾曲角度θｎ及び旋回角度ζｎとの関係を求める。ここで、湾曲区間数をｅとし、第ｎの湾曲区間を駆動するワイヤの位相角ξｎを以下の（２）式とする。
Figure JPOXMLDOC01-appb-M000002
　これより、第ｎの湾曲区間におけるａ～ｃワイヤの駆動変位ｌｐｎａ，ｌｐｎｂ，ｌｐｎｃは、以下の（３）式となる。
Figure JPOXMLDOC01-appb-M000003
　次に、図３に示す操作装置２００－１のモデリングを行う。
　第ｎの湾曲区間の湾曲角度θｎを指令する操作レバーのモータを含む全ての慣性モーメントをＪθｎとし、第ｎの湾曲区間の旋回角度ζｎを指令する操作レバーのモータを含む全ての慣性モーメントをＪζｎとする。また、第ｎの湾曲区間の湾曲角度θｎを指令する操作レバーに与える操縦者の操作トルクをｄθｎとし、第ｎの湾曲区間の旋回角度ζｎを指令する操作レバーに与える操縦者の操作トルクをｄζｎとする。そして、第ｎの湾曲区間の湾曲角度θｎを指令する操作レバーに与える制御トルクをｕθｎとし、第ｎの湾曲区間の旋回角度ζｎを指令する操作レバーに与える制御トルクをｕζｎとすると、運動方程式は、以下の（４）式のようになる。
Figure JPOXMLDOC01-appb-M000004
Figure JPOXMLDOC01-appb-M000005
　２）制御系設計
　本章では、先頭追従制御系と、その途中で観察動作制御を行うための制御系の設計を行う。
　図６は、本発明の第１の実施形態に係る連続体ロボット１００の先頭追従制御の一例を示す図である。この図６において、図１に示す構成と同様の構成については同じ符号を付している。また、図６では、図１に示すｚ方向を紙面下側から紙面上側に向かう方向にとっている。また、図６には、基台部１４０及び湾曲可能部１７０を含み構成された連続体ロボット１００が進む目標経路６１０を点線で図示している。
　ここで、先頭追従制御とは、図６に示すように、湾曲可能部１７０の最遠位湾曲区間が通る経路（目標経路６１０）と同じ経路を後続湾曲区間が通るように制御する方法である。
　先頭追従制御の一例としては、入力された遠位湾曲区間の第１の目標湾曲角度に応じて設定される、連続体ロボットの前進移動に対応した追従湾曲区間の湾曲角度に関する第１の湾曲角度のプロファイルに従って、追従湾曲区間の湾曲角度が前記第１の目標湾曲角度に至るよう構成されている制御がある。
　図６において、時点６０１は、基台部１４０の上面からｚ方向に延びる湾曲可能部１７０が湾曲していない初期状態を示している。その後、図６においては、時点６０２，時点６０３，時点６０４，時点６０５と時間が経過するのにしたがって、基台部１４０がｚ方向に移動して湾曲可能部１７０が湾曲する様子が示されている。
　このような先頭追従制御により、連続体ロボット１００は、空間をすり抜けるように進むことができる。なお、先頭追従制御は、予め目標経路６１０が定められていることは必須ではなく、例えば、最遠位湾曲区間の湾曲角度を、後続湾曲区間の湾曲角度に当該湾曲区間の長さをもって連続的に伝播させてもよい。この方法を用いると、例えば、操縦者は、ジョイスティックなどで最遠位湾曲区間の湾曲角度と基台部１４０の変位（移動量（前進量））に対して指令を与えることで、連続体ロボット１００を実時間で先頭追従制御することができる。
　２．１）先頭追従制御
　図７は、本発明の第１の実施形態を示し、図４のブロックＦＴＬによる後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬの算出処理の一例を示す図である。なお、以下の図７の説明では、最遠位湾曲区間として図１の湾曲区間１７３を適用し、後続湾曲区間として図１の湾曲区間１７２を適用した場合を想定した例について説明する。
　グラフ上の座標が基台部の位置と湾曲セクションの湾曲角度の対を示している。説明のため以下では、座標ａに対応する湾曲角度のことを“角度ａ”、そして座標ａに対応する基台部の位置（変位）を位置ａ（変位ａ）と呼ぶことがある。
　図７Ａにおいて、横軸は、基台部１４０の変位ｚｂを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の湾曲角度θを示している。図７Ａにおいて、破線の「Ｌｅａｄｅｒ」は、操縦者等により入力装置３１０を介して入力された最遠位湾曲区間の目標湾曲角度θｌｔに対応する、最遠位湾曲区間の湾曲角度のプロファイルを示している。また、図７Ａにおいて、太破線の「Ｆｏｌｌｏｗｅｒ」は、例えば後続湾曲区間の目標湾曲角度θｌｆに対応する、追従湾曲区間の湾曲角度のプロファイルを示している。
　図７Ｂにおいて、横軸は、基台部１４０の変位ｚｂを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標旋回角度ζを示している。図７Ｂにおいて、破線の「Ｌｅａｄｅｒ」は、操縦者等により入力装置３１０を介して入力された最遠位湾曲区間の目標旋回角度ζｌｔに係る旋回角度のプロファイルを示している。また、図７Ｂにおいて、太破線の「Ｆｏｌｌｏｗｅｒ」は、例えば後続湾曲区間の目標旋回角度ζｌｆに係る旋回角度のプロファイルを示している。
　図７Ａに関し、基台部１４０の変位ｚｂが変位ａにおいて、最遠位湾曲区間の目標湾曲角度θｌｔが最遠位湾曲角度ａから最遠位湾曲角度Ｂに変更されると、例えば、太破線の「Ｆｏｌｌｏｗｅｒ」で示す後続湾曲区間の目標湾曲角度θｌｆは、基台部１４０の変位ｚｂが変位ｃにおいて、後続湾曲角度ｃから後続湾曲角度Ｄに変更されるように自動更新される。また、図７Ｂに関し、基台部１４０の変位ｚｂが変位ａにおいて、最遠位湾曲区間の目標旋回角度ζｌｔが最遠位旋回角度ａから最遠位旋回角度ｂに変更されると、例えば、太破線の「Ｆｏｌｌｏｗｅｒ」で示す後続湾曲区間の目標旋回角度ζｌｆは、基台部１４０の変位ｚｂが変位ｃにおいて、後続旋回角度ｃから後続旋回角度ｄに変更されるように自動更新される。ここで、変位ｃは、変位ａと変位ｃの間の長さが後続湾曲区間の長さｌｄとなるように決定される。
　しかしながら、図７Ａ及び図７Ｂにおいて、基台部１４０の変位ｚｂが変位ａと変位ｃとの間にあるときには、太破線の「Ｆｏｌｌｏｗｅｒ」で示す後続湾曲区間の目標湾曲角度θｌｆ及び目標旋回角度ζｌｆは変化しない。そして、図７Ａにおいて太破線の「Ｆｏｌｌｏｗｅｒ」で示す後続湾曲区間の目標湾曲角度θｌｆでは、変位ｃにおいて後続湾曲角度ｃから後続湾曲角度Ｄとなり、図７Ｂにおいて太破線の「Ｆｏｌｌｏｗｅｒ」で示す後続湾曲区間の目標旋回角度ζｌｆでは、変位ｃにおいて後続旋回角度ｃから後続旋回角度ｄとなるため、連続体ロボット１００は急峻な挙動を示し、連続体ロボット１００の操作性が損なわれる。
　そこで、本実施形態では、図４のブロックＦＴＬは、以下のようにして、後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬを算出する。
　まず、図７Ａの目標湾曲角度θについては、図４のブロックＦＴＬは、変位ａと変位ｃとの間では変位ａにおける目標湾曲角度ａと変位ｃにおける目標湾曲角度Ｄとを結ぶ線で補間し、これを後続湾曲区間の目標湾曲角度θｆＦＴＬとして算出する。図７Ａでは、この補間した部分を実線で示している。同様に、図７Ｂの目標旋回角度ζについては、図４のブロックＦＴＬは、変位ａと変位ｃとの間では変位ａにおける目標旋回角度ａと変位ｃにおける目標旋回角度ｄとを結ぶ線で補間し、これを後続湾曲区間の目標旋回角度ζｆＦＴＬとして算出する。図７Ｂでは、この補間した部分を実線で示している。そして、図４のブロックＦＴＬは、算出した後続湾曲区間の目標湾曲角度θｆＦＴＬに係る湾曲角度のプロファイル及び目標旋回角度ζｆＦＴＬに係る旋回角度のプロファイルを、例えば内部の記憶部に記憶する。
　上述した図７の説明では、最遠位湾曲区間として図１の湾曲区間１７３を適用し、後続湾曲区間として図１の湾曲区間１７２を適用した場合を想定した例について説明した。そして、連続体ロボット１００の湾曲可能部１７０の湾曲区間数が３つ以上、例えば、図１に示す湾曲可能部１７０の湾曲区間数が３つの場合において、上述した後続湾曲区間として図１の湾曲区間１７１を適用する場合には、例えば、上述した最遠位湾曲区間として１つ前の図１の湾曲区間１７２を適用することで実現可能である。
　２．２）観察動作制御との切り替え制御
　上記「２．１）先頭追従制御」の章で説明した先頭追従制御系を用いることで、操縦者が最遠位湾曲区間に角度指令を与えるのみで、後続の全ての湾曲区間の姿勢が制御される。しかしながら、目標経路６１０の状況により、操縦者が途中で観察動作制御を行うことが必要な場合が考えられる。これは、後続湾曲区間の姿勢に追加指令を加えることで可能となる。例えば、第２の湾曲区間１７２のアクチュエータのみを駆動すると、第３の湾曲区間１７３の姿勢は変化しない。この動作は、第３の湾曲区間１７３の先端の方向を一定に保ったまま湾曲可能部１７０の最遠位端の位置を変化させることができ、例えば胃壁や腸壁などの、連続体ロボット１００の進行方向に対して斜めに走る壁面に沿って、観察する際に適している（以下、「斜視動作」と呼ぶ）。このとき、先端追従制御系による湾曲姿勢に対して操縦者の指令値が追加されるが、例えば、追加指令を与える操作系が中点復帰式のレバーであると、先頭追従制御による湾曲形状が把握しづらく、誤操作により連続体ロボット１００自身やその周囲の対象物に損傷を及ぼす恐れがある。また、操作レバーを離すと、追加の操作量がリセットされてしまう。また、例えば、追加指令を与える操作系が湾曲角速度を与えるものであるとすると、追加の操作量はリセットされないが、結果として与えられる湾曲角度を把握することが直接的ではなく、難しい。これを解決するには、先頭追従制御中には、操作レバーの角度が湾曲角度や旋回角度と同期していることが望ましい。そこで、本実施形態では、操作装置２００－１として、モータ及びエンコーダ等の角度センサを有している操作系を用いる。これにより、操縦者は、先頭追従制御による連続体ロボット１００の姿勢を把握することができ、指令値を追加する場合には、そのまま操作レバーに操作トルクを与えればよい。
　ここで再び、図４の説明を行う。
　以下に、図４に示す連続体ロボットの制御システム３００－１と、上記「１）モデリング」で示したモデル及び上記「２．１）先頭追従制御」の章で説明した先頭追従制御系との関係を説明する。
　図４を用いて上述したように、ブロックＰｓは、制御対象である連続体ロボット１００、ブロックＰｓからの出力であるｌｐは、ワイヤの駆動変位を示すベクトル、ブロックＫｓは、ワイヤの目標駆動変位ｌｐｒｅｆに整定させるための位置制御系である。また、ブロックＰｌは、（４）式に示したモータを含む操作系であり、ブロックＫｌは、それを目標値に整定させるための位置制御系であり、この位置制御系の入力端及び出力端に接続されるブロックＧｉｎ及びＧｏｕｔは、０から１の値をとるゲインである。なお、上述したように、本実施形態では、最遠位湾曲区間を操作するための操作部に含まれる操作レバーは、モータ駆動されない。
　また、最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔは、例えば、最遠位湾曲区間の湾曲角度及び旋回角度の操作レバーの角度であり、（４）式におけるθｌｎ及びζｌｎと等しい。また、後続湾曲区間の目標湾曲角度θｌｆ及び目標旋回角度ζｌｆは、例えば、後続湾曲区間の湾曲角度及び旋回角度の操作レバーの角度であり、（４）式における［θｌ１，．．．，θｌｎ－１，ζｌ１，．．．，ζｌｎ－１］からなるベクトルである。また、上述したように、操作トルクｄθｌｆ及びｄζｌｆは、操縦者が後続湾曲区間の湾曲角度及び旋回角度の操作レバーに印加する操作トルクを表し、（４）式における［ｄθ１，．．．，ｄθｎ－１，ｄζ１，．．．，ｄζｎ－１］からなるベクトルである。
　上述したように、先頭追従制御が行われているときには（具体的には、第２のスイッチ３３２に１の信号が入力されているときには）、切替スイッチ部３３０は、図４に示している２極双投形スイッチの状態となっている。そして、このときには、運動学演算部３４０には、ＦＴＬブロックから出力される先頭追従制御系のための後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬが入力される。同時に、先頭追従制御系のための後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬは、操作レバーの目標角度となり、また、ブロックＧｉｎ及びブロックＧｏｕｔの値には１が入力されるため、操作レバーの角度は、ブロックＫｌによってフィードバック制御されて後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬに追従する。このとき、操作レバーに印加される操作トルクｄθｌｆ及びｄζｌｆは、外乱として抑圧されるため、操作レバーの角度θｌｆ及びζｌｆには、ほとんど影響しない。
　ここで、後続湾曲区間の操作レバーへの操縦者の接触が検知されると（具体的には、第２のスイッチ３３２に０の信号が入力されると）、切替スイッチ部３３０は、図４に示している２極双投形スイッチの状態とは逆の方向に切り替える。このとき、ブロックＧｉｎ及びブロックＧｏｕｔの値には０が入るため、操作レバーの制御系は遮断され、操作トルクｄθｌｆ及びｄζｌｆにより操作レバーの角度θｌｆ及びζｌｆを変動させることができる。そして、このときには、運動学演算部３４０には、操作レバーの角度θｌｆ及びζｌｆが入力されるため、操縦者は、連続体ロボット１００を先頭追従制御による姿勢から継ぎ目なく追加の操作を加えることが可能となる。その後、再び、後続湾曲区間の操作レバーへの操縦者の接触が未検知となると（具体的には、第２のスイッチ３３２に１の信号が入力されると）、切替スイッチ部３３０は、図４に示している２極双投形スイッチの状態に切り替える。これにより、連続体ロボット１００及び操作レバーの角度は、先頭追従制御による姿勢へと復帰する。なお、本実施形態では、連続体ロボット１００の姿勢角度と操作レバーの角度とを一致させているが、この態様に限定されるものではなく、例えば倍率を与えてもよい。
　３）シミュレーション
　本章では、前章「２）制御系設計」で説明した先頭追従制御系を用いてシミュレーションを行う。シミュレーションでは、図１に示す連続体ロボット１００の湾曲可能部１７０のように湾曲区間数が３であり、湾曲区間の長さを０．０１ｍとした連続体ロボット１００を対象とする。
　図８は、本発明の第１の実施形態に係る連続体ロボットの制御システム３００－１による連続体ロボット１００の制御方法におけるシミュレーション結果の第１例を示す図である。具体的に、図８Ａ～図８Ｆは、横方向を図１のｘ方向とし、縦方向を図１のｚ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図８Ａ～図８Ｆは、先頭追従制御と追加操作による制御を行い、基台部１４０が０．０２ｍに進行するまでの湾曲可能部１７０の姿勢を段階的にスティック線図により示したものである。また、図８Ａ～図８Ｆでは、実線で連続体ロボット１００の湾曲可能部１７０の形状を示し、丸点で各湾曲区間の先端を示し、細線で各湾曲区間の先端の軌跡を示している。
　まず、図８Ａでは、操縦者の湾曲操作により、最遠位湾曲区間である第３の湾曲区間１７３が湾曲し、基台部１４０の進行を開始している姿勢を示している。続いて、図８Ｂでは、操縦者が基台部１４０をさらに進行させると、第２の湾曲区間１７２は、先頭追従制御系により、第３の湾曲区間１７３に追従していることがわかる。
　続いて、図８Ｃでは、操縦者が第２の湾曲区間１７２の湾曲角度θにおける操作レバーの操作に基づく動作制御を示している。これにより、切替スイッチ部３３０が図４に示している２極双投形スイッチの状態とは逆の方向に切り替わり、先頭追従制御の姿勢に対して追加の操作を行った姿勢を示している。さらに、図８Ｄでは、第２の湾曲区間１７２の湾曲角度θが浅くなる方向に追加操作が行われている姿勢を示している。これにより、第３の湾曲区間１７３の先端の方向を一定に保ったまま湾曲可能部１７０の最遠位端の位置を変化させる斜視動作ができることがわかる。
　続いて、図８Ｅでは、操縦者による第２の湾曲区間１７２の湾曲角度θにおける操作レバーの操作が終了した状態を示し、追加操作の前の姿勢に復帰している。続いて、図８Ｆでは、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従し、先頭追従制御を継続することが可能であることがわかる。
　図９は、本発明の第１の実施形態に係る連続体ロボットの制御システム３００－１による連続体ロボット１００の制御方法におけるシミュレーション結果の第２例を示す図である。具体的に、図９Ａ～図９Ｆは、横方向を図１のｙ方向とし、縦方向を図１のｚ方向とし、奥行き方向を図１のｘ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図９Ａ～図９Ｆでは、旋回角度ζに追加操作が加わるシミュレーション応答を示している。
　まず、図９Ａ及び図９Ｂでは、上述した図８Ａ及び図８Ｂと同様に、先頭追従制御系が行われている。
　続いて、図９Ｃでは、操縦者が第２の湾曲区間１７２の旋回角度ζが深くなる方向に操作レバーを操作した場合を示している。これにより、図９Ｃでは、切替スイッチ部３３０が図４に示している２極双投形スイッチの状態とは逆の方向に切り替わり、先頭追従制御の姿勢に対して追加の操作が行われている姿勢を示している。さらに、図９Ｄでは、２の湾曲区間１７２の旋回角度ζが浅くなる方向に追加操作が行われている姿勢を示している。これにより、第３の湾曲区間１７３の先端の方向を一定に保ったまま湾曲可能部１７０の最遠位端の位置を湾曲形状の面外方向に変化させることができることがわかる。
　図９Ｅ及び図９Ｆでは、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従し、先頭追従制御を継続することが可能であることがわかる。
　図１０は、本発明の第１の実施形態に係る連続体ロボットの制御システム３００による連続体ロボット１００の制御方法におけるシミュレーション結果の第３例を示す図である。具体的に、図１０Ａ～図１０Ｆは、横方向を図１のｙ方向とし、縦方向を図１のｚ方向とし、奥行き方向を図１のｘ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図１０Ａ～図１０Ｆでは、図８の操作と図９の操作を組み合わせることで、３次元空間内で第３の湾曲区間１７３の先端の方向を一定に保ったまま湾曲可能部１７０の最遠位端の位置を操作している。
　以上説明した第１の実施形態に係る連続体ロボットの制御システム３００－１においては、ブロックＦＴＬでは、最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔと、基台部１４０の変位とに基づいて、後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬを算出するようにしている。そして、切替スイッチ部３３０では、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬと、後続操作手段に相当するブロックＰｌから得られる後続湾曲区間の目標湾曲角度θｌｆ及び目標旋回角度ζｌｆとのうちのいずれか一方を選択するための切替処理を行うようにしている。そして、運動学演算部３４０では、切替スイッチ部３３０で選択された後続湾曲区間の目標湾曲角度及び目標旋回角度に基づいて、連続体ロボット１００の駆動部で後続湾曲区間のワイヤを駆動させる際の駆動変位を演算するようにしている。
　かかる構成によれば、先頭追従制御に基づく後続湾曲区間の動作制御と、後続操作手段からの操作に基づく後続湾曲区間の動作制御とを切り替えることができるため、意図せずに連続体ロボット１００が対象物と接触してしまうことを回避することができる。これにより、対象物が傷ついたり連続体ロボット自体が破損したりすることを防止することができ、連続体ロボットの操作を安全に行うことができる。具体的に、本実施形態では、先頭追従制御系から継ぎ目なく観察動作制御に移行し、追加操作による斜視動作を行い、さらに、追加操作を終了すると再び先頭追従制御に復帰可能である。
　（第２の実施形態）
　次に、本発明の第２の実施形態について説明する。なお、以下に記載する第２の実施形態の説明においては、上述した第１の実施形態と共通する事項については説明を省略し、上述した第１の実施形態と異なる事項について説明を行う。
　第２の実施形態に係る連続体ロボットの概略構成は、図１及び図２に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、第２の実施形態に係る操作装置の概略構成は、図３に示す第１の実施形態に係る操作装置２００－１の概略構成と同様である。
　上述した第１の実施形態で説明した制御系は、最遠位湾曲区間については、操縦者の操作指令のみを与えていたが、第２の実施形態では、最遠位湾曲区間を後続湾曲区間の追加操作と連動させることにより、観察動作の種類を増やす形態を示す。例えば、図１に示す連続体ロボット１００の湾曲可能部１７０のように湾曲区間数が３の場合では、第２の湾曲区間１７２と第３の湾曲区間１７３を同方向に駆動すると、連続体ロボット１００の湾曲可能部１７０の先端位置を大きく変化させることができる。この動作は、例えば、体内を広範囲に渡って観察する際に、或いは臓器との接触を避けるために連続体ロボット１００の最遠位端を大きく移動させる必要がある際に、適している（以下、「大湾曲動作」と呼ぶ）。
　図１１は、本発明の第２の実施形態に係る連続体ロボットの制御システム３００の概略構成の一例を示す図である。以下の説明では、図１１に示す第２の実施形態に係る連続体ロボットの制御システム３００を「連続体ロボットの制御システム３００－２」として説明を行う。また、この図１１において、図４に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。具体的に、図１１には、最遠位湾曲区間を後続湾曲区間の追加操作と連動させる制御系のブロック線図を示している。
　図１１に示す第２の実施形態に係る連続体ロボットの制御システム３００－２において、図４に示す第１の実施形態に係る連続体ロボットの制御システム３００－１に対して、追加・変更された構成部は、以下のとおりである。
　連続体ロボットの制御システム３００－２では、図４の連続体ロボットの制御システム３００－１に対して、ブロックＫｌｔ、ブロックＫｌｔの入力端及び出力端にそれぞれ接続されるブロックＧｉｎ及びＧｏｕｔ、ブロックＰｌｆ、ブロックＫｒが追加されている。また、連続体ロボットの制御システム３００－２では、図４の連続体ロボットの制御システム３００－１に対して、入力装置３１０が削除される変更がされ、ブロックＦＴＬがブロックＦＴＬ＋Ｍｅｍｏｒｙ（記憶部）に変更され、ブロックＫｌがブロックＫｌｆに変更され、ブロックＰｌがブロックＰｌｆに変更され、切替スイッチ部３３０に第３のスイッチ３３３が追加される変更がされている。さらに、連続体ロボットの制御システム３００－２では、図４の連続体ロボットの制御システム３００－１に対して、第２のスイッチ３３２の後段にＮＯＴゲートが追加されている。
　図１１において、ブロックＰｌｔ及びブロックＰｌｆは、それぞれ、最遠位湾曲区間のモータを含む操作系（最遠位操作手段）及び後続湾曲区間のモータを含む操作系（後続操作手段）である。また、図１１において、ブロックＫｌｔ及びＫｌｆは、それぞれ、最遠位湾曲区間の操作系位置制御系及び後続湾曲区間の操作系位置制御系である。また、図１１において、最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔは、それぞれ、最遠位区間の湾曲角度の操作レバーによる角度及び最遠位区間の旋回角度の操作レバーによる角度であり、（４）式におけるθｌｎ及びζｌｎに相当する。操作トルクｄθｌｔ及びｄζｌｔは、操縦者が最遠位湾曲区間の湾曲角度及び旋回角度の操作レバーに印加する操作トルクを表し、（４）式におけるｄθｎ及びｄζｎに相当する。また、ブロックＦＴＬ＋Ｍｅｍｏｒｙ（記憶部）には、第１の実施形態における先頭追従制御に加えて、最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔを記憶する記憶部が備えられている。
　ブロックＫｒは、後続湾曲区間と最遠位湾曲区間の姿勢を連動するアルゴリズムであり、これを切り替えることにより動作の種類を変更することができる。具体的に、ブロックＫｒは、ブロックＦＴＬ＋Ｍｅｍｏｒｙ（記憶部）に記憶されている最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔと、後続操作手段に相当するブロックＰｌｆから入力された後続湾曲区間の目標湾曲角度θｌｆ及び目標旋回角度ζｌｆとに基づいて、最遠位湾曲区間の目標湾曲角度及び目標旋回角度を算出する最遠位算出手段である。
　また、切替スイッチ部３３０の第３のスイッチ３３３は、ブロックＫｒから得られる最遠位湾曲区間の目標湾曲角度及び目標旋回角度と、最遠位操作手段であるブロックＰｌｔから得られる最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔとのうちのいずれか一方を選択するための切替処理を行うスイッチ（最遠位切替手段）である。
　そして、本実施形態では、運動学演算部３４０は、第３のスイッチ３３３で選択された最遠位湾曲区間の目標湾曲角度及び目標旋回角度に基づいて、連続体ロボット１００の駆動部で最遠位湾曲区間のワイヤを駆動させる際の駆動変位を演算する形態を採る。
　本実施形態では、切替スイッチ部３３０は、先頭追従制御が行われているときには（具体的には、第２のスイッチ３３２に１の信号が入力されているときには）、図１１に示している３極双投形スイッチの状態となっている。そして、この場合、第２のスイッチ３３２の後段に設けられているＮＯＴゲートによって、図１１の上方に記載されている最遠位湾曲区間の操作系のフィードバックループにおけるブロックＧｉｎ及びＧｏｕｔには０が入力される。これにより、最遠位湾曲区間の操作系は遮断され、操作トルクｄθｌｔ及びｄζｌｔによって操作レバーの角度θｌｔ及びζｌｔを変動させることができる。即ち、ここで説明した先頭追従制御が行われているときには、第３のスイッチ３３３は、ブロックＰｌｔから得られる最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔを選択するための切替処理を行う。そして、この場合、運動学演算部３４０には、ブロックＰｌｔから得られる最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔと、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θｆＦＴＬ及び目標旋回角度ζｆＦＴＬが入力される。
　ここで、後続湾曲区間の操作レバーへの操縦者の接触が検知されると（具体的には、第２のスイッチ３３２に０の信号が入力されると）、切替スイッチ部３３０は、図１１に示している３極双投形スイッチの状態とは逆の方向に切り替える。この場合、図１１の下方に記載されている後続湾曲区間の操作系のフィードバックループにおけるブロックＧｉｎ及びＧｏｕｔには０が入力される。これにより、後続湾曲区間の操作系は遮断され、操作トルクｄθｌｆ及びｄζｌｆによって操作レバーの角度θｌｆ及びζｌｆを変動させることができる。同時に、運動学演算部３４０には、第１のスイッチ３３１を介して後続湾曲区間の目標湾曲角度θｌｆ及び目標旋回角度ζｌｆが入力されるため、操縦者は、連続体ロボット１００を先頭追従制御による姿勢から継ぎ目なく追加の操作を加えることが可能となる。一方、第２のスイッチ３３２の後段に設けられているＮＯＴゲートによって、図１１の上方に記載されている最遠位湾曲区間の操作系のフィードバックループにおけるブロックＧｉｎ及びＧｏｕｔには１が入力される。これにより、ブロックＫｌｔによる位置決め制御が有効となり、操縦者の操作トルクｄθｌｔ及びｄζｌｔは外乱として抑圧される。そして、この場合、第３のスイッチ３３３は、ブロックＫｒから得られる最遠位湾曲区間の目標湾曲角度及び目標旋回角度を選択するための切替処理を行う。これにより、連続体ロボット１００と操作レバーの双方ともに追加の操作量に連動し、最遠位湾曲区間に係る角度は、後続湾曲区間の追加操作量に連動することになる。
　図１２は、本発明の第２の実施形態に係る連続体ロボットの制御システム３００－２による連続体ロボット１００の制御方法におけるシミュレーション結果の第１例を示す図である。具体的に、図１２Ａ～図１２Ｆは、横方向を図１のｘ方向とし、縦方向を図１のｚ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図１２Ａ～図１２Ｆは、追加操作が加わるシミュレーション応答を示している。また、図１２Ａ～図１２Ｆでは、実線で連続体ロボット１００の湾曲可能部１７０の形状を示し、丸点で各湾曲区間の先端を示し、細線で各湾曲区間の先端の軌跡を示している。
　図１２Ａ及び図１２Ｂでは、上述した図９Ａ及び図９Ｂと同様の先頭追従制御系が行われている。
　続いて、図１２Ｃでは、操縦者が第２の湾曲区間１７２の湾曲角度θにおける操作レバーの操作に基づく動作制御を示している。これにより、切替スイッチ部３３０が図１１に示している３極双投形スイッチの状態とは逆の方向に切り替わり、先頭追従制御の姿勢に対して追加の操作を行った姿勢を示している。このシミュレーションでは、第２の湾曲区間１７２の先頭追従制御の指令角度と追加操作による角度との差分θｌ２－θ２ＦＴＬに対して、最遠位湾曲区間の湾曲角度θ３ｒｅｆが、以下の（８）式となるように連動させる。
Figure JPOXMLDOC01-appb-M000006
　ここで、θ３Ｍｅｍは、ブロックＦＴＬ＋Ｍｅｍｏｒｙ（記憶部）に記憶されている、切替スイッチ部３３０の３極双投形スイッチが切り替わる直前の最遠位湾曲区間の湾曲角度である。
　さらに、図１２Ｄでは、第２の湾曲区間１７２の湾曲角度θが浅くなる方向に追加操作が行われている姿勢を示している。これにより、第３の湾曲区間１７３は、常に前方の［ｘ，ｙ］＝［０．０１５，０．４］付近の狭い領域を向きながら、観察方向が追加操作により変更されていることがわかる。
　続いて、図１２Ｅ及び図１２Ｆでは、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従する先頭追従制御を継続することが可能であることがわかる。
　図１３は、本発明の第２の実施形態に係る連続体ロボットの制御システム３００－２による連続体ロボット１００の制御方法におけるシミュレーション結果の第２例を示す図である。具体的に、図１３Ａ～図１３Ｆは、横方向を図１のｘ方向とし、縦方向を図１のｚ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図１３Ａ～図１３Ｆは、追加操作による大湾曲動作を示している。
　この図１３に示すシミュレーションでは、第２の湾曲区間１７２の先頭追従制御の指令角度と追加操作による角度の差分に対して、最遠位端の湾曲角度θ３ｒｅｆが、以下の（９）式になるように連動させる。
Figure JPOXMLDOC01-appb-M000007
　これにより、第２の湾曲区間１７２と第３の湾曲区間１７３が連動し、大きな湾曲動作を実現していることわかる。図１３Ｅ及び図１３Ｆでは、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従する先頭追従制御を継続することが可能であることがわかる。
　なお、図１２及び図１３に示すシミュレーションでは、ｘｚ平面における湾曲可能部１７０の動作制御を想定したため、湾曲角度θを考慮した説明を行ったが、ｘｙｚの３次元空間の場合には、更に旋回角度ζも考慮した態様を採りうる。
　第２の実施形態によれば、第１の実施形態と同様に、意図せずに連続体ロボット１００が対象物と接触してしまうことを回避することができる。これにより、対象物が傷ついたり連続体ロボット自体が破損したりすることを防止することができ、連続体ロボットの操作を安全に行うことができる。
　（第３の実施形態）
　次に、本発明の第３の実施形態について説明する。なお、以下に記載する第３の実施形態の説明においては、上述した第１及び第２の実施形態と共通する事項については説明を省略し、上述した第１及び第２の実施形態と異なる事項について説明を行う。
　第３の実施形態に係る連続体ロボットの概略構成は、図１及び図２に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、第３の実施形態に係る操作装置の概略構成は、図３に示す第１の実施形態に係る操作装置２００－１の概略構成と同様である。
　上述した第２の実施形態で説明した制御系は、最遠位湾曲区間を後続湾曲区間の追加操作と連動させた形態であったが、第３の実施形態では、後続湾曲区間同士を連動させることにより、観察動作の種類を増やす形態を示す。
　図１４は、本発明の第３の実施形態に係る連続体ロボットの制御システム３００の概略構成の一例を示す図である。以下の説明では、図１４に示す第３の実施形態に係る連続体ロボットの制御システム３００を「連続体ロボットの制御システム３００－３」として説明を行う。また、この図１４において、図４に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。具体的に、図１４には、後続湾曲区間同士を追加操作により連動させる制御系のブロック線図を示している。
　図１４に示す第３の実施形態に係る連続体ロボットの制御システム３００－３において、図４に示す第１の実施形態に係る連続体ロボットの制御システム３００－１に対して、追加・変更された構成部は、以下のとおりである。
　連続体ロボットの制御システム３００－３では、図４の連続体ロボットの制御システム３００－１に対して、後続湾曲区間の制御系が２つの制御系に分割されて構成されたものである。具体的には、図４に示すブロックＫｌ、ブロックＫｌの入力端及び出力端にそれぞれ接続されるブロックＧｉｎ及びＧｏｕｔ及びブロックＰｌを含む後続湾曲区間の制御系が、第２の湾曲区間１７２（第２の後続湾曲区間）の制御系として変更されている。より詳細に、第２の湾曲区間１７２の制御系は、図１４に示すブロックＫｌ２、ブロックＫｌ２の入力端及び出力端にそれぞれ接続されるブロックＧｉｎ及びＧｏｕｔ及びブロックＰｌ２を含む後続湾曲区間の制御系である。また、連続体ロボットの制御システム３００－３では、図４の連続体ロボットの制御システム３００－１に対して、第１の湾曲区間１７１（第１の後続湾曲区間）の制御系が追加されている。より詳細に、第１の湾曲区間１７１の制御系は、図１４に示すブロックＫｌ１及びブロックＰｌ１を含む後続湾曲区間の制御系である。さらに、連続体ロボットの制御システム３００－３では、図４の連続体ロボットの制御システム３００－１に対して、制御ゲインブロックＧｗが追加されている。さらに、連続体ロボットの制御システム３００－３では、図４の連続体ロボットの制御システム３００－１に対して、切替スイッチ部３３０において第１のスイッチ３３１に替えて第４のスイッチ３３４及び第５のスイッチ３３５が設けられている。
　ブロックＰｌ１は、第１の湾曲区間１７１（第１の後続湾曲区間）の目標湾曲角度θｌｆ１及び目標旋回角度ζｌｆ１を入力するモータを有する操作系（第１の後続操作手段）である。このブロックＰｌ１は、例えば、図３に示す例では、後続湾曲区間である第１の湾曲区間１７１を操作するための第１の操作部２１０に相当する操作系である。具体的に、第１の湾曲区間１７１の目標湾曲角度θｌｆ１及び目標旋回角度ζｌｆ１は、第１の湾曲区間１７１の操作レバーによる角度である。また、操作トルクｄθｌｆ１及びｄζｌｆ１は、それぞれ、操縦者が、第１の湾曲区間１７１の湾曲角度における操作レバー及び第１の湾曲区間１７１の旋回角度における操作レバーに印加する操作トルクを表している。
　ブロックＰｌ２は、第２の湾曲区間１７２（第２の後続湾曲区間）の目標湾曲角度θｌｆ２及び目標旋回角度ζｌｆ２を入力するモータを有する操作系（第２の後続操作手段）である。このブロックＰｌ２は、例えば、図３に示す例では、後続湾曲区間である第２の湾曲区間１７２を操作するための第２の操作部２２０に相当する操作系である。具体的に、第２の湾曲区間１７２の目標湾曲角度θｌｆ２及び目標旋回角度ζｌｆ２は、第２の湾曲区間１７２の操作レバーによる角度である。また、操作トルクｄθｌｆ２及びｄζｌｆ２は、それぞれ、操縦者が、第２の湾曲区間１７２の湾曲角度における操作レバー及び第２の湾曲区間１７２の旋回角度における操作レバーに印加する操作トルクを表している。
　ブロックＫｌ１は、第１の湾曲区間１７１（第１の後続湾曲区間）の操作系位置制御系であり、また、ブロックＫｌ２は、第２の湾曲区間１７２（第２の後続湾曲区間）の操作系位置制御系である。
　本実施形態においては、ブロックＦＴＬは、第２の湾曲区間１７２の目標湾曲角度θｆ２ＦＴＬ及び目標旋回角度ζｆ２ＦＴＬを算出するとともに、第１の湾曲区間１７１の目標湾曲角度θｆ１ＦＴＬ及び目標旋回角度ζｆ１ＦＴＬを算出する。
　図１４に示す連続体ロボットの制御システム３００－３において、制御ゲインブロックＧｗは、後続湾曲区間と最遠位湾曲区間の姿勢の比率を指定する行列であり、これを切り替えることにより動作の種類を変更できる。具体的に、図１４に示す例では、制御ゲインブロックＧｗは、第２の後続操作手段であるブロックＰｌ２から得られる第２の湾曲区間１７２の目標湾曲角度θｌｆ２及び目標旋回角度ζｌｆ２に対して制御ゲインを乗ずる制御ゲイン手段である。
　本実施形態では、例えば第２の湾曲区間１７２の操作レバーへの操縦者の接触が検知されると（具体的には、第２のスイッチ３３２に０の信号が入力されると）、切替スイッチ部３３０は、図１４に示している３極双投形スイッチの状態とは逆の方向に切り替える。これにより、切替スイッチ部３３０は、第４のスイッチ３３４において、第２の湾曲区間１７２の目標湾曲角度及び目標旋回角度として、第２の後続操作手段であるブロックＰｌ２から得られる第２の湾曲区間１７２の目標湾曲角度θｌｆ２及び目標旋回角度ζｌｆ２を選択するための切替処理を行うとともに、第５のスイッチ３３５において、第１の湾曲区間１７１の目標湾曲角度及び目標旋回角度として制御ゲインブロックＧｗによって制御ゲインを乗じた第２の湾曲区間１７２の目標湾曲角度θｌｆ２及び目標旋回角度ζｌｆ２を選択するための切替処理を行う。
　図１５は、本発明の第３の実施形態に係る連続体ロボットの制御システム３００－３による連続体ロボット１００の制御方法におけるシミュレーション結果の一例を示す図である。具体的に、図１５Ａ～図１５Ｆは、横方向を図１のｘ方向とし、縦方向を図１のｚ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図１５Ａ～図１５Ｆは、追加操作が加わるシミュレーション応答を示している。また、図１５Ａ～図１５Ｆでは、実線で連続体ロボット１００の湾曲可能部１７０の形状を示し、丸点で各湾曲区間の先端を示し、細線で各湾曲区間の先端の軌跡を示している。
　まず、図１５Ａ及び図１５Ｂでは、上述した図９Ａ及び図９Ｂと同様の先頭追従制御系が行われている。
　続いて、図１５Ｃ及び図１５Ｄでは、操縦者が第２の湾曲区間１７２の湾曲角度の操作レバーを操作した場合を示している。これにより、図１５Ｃ及び図１５Ｄでは、切替スイッチ部３３０が図１４に示している３極双投形スイッチの状態とは逆の方向に切り替わり、先頭追従制御の姿勢に対して追加の操作が行われている姿勢を示している。このシミュレーションでは、第２の湾曲区間１７２の先頭追従制御の指令角度と追加操作による角度の差分θｌ２－θ２ＦＴＬに対して、第１の湾曲区間１７１の湾曲角度θ１ｒｅｆが以下の（１０）式となるように連動させる。
Figure JPOXMLDOC01-appb-M000008
　これにより、第３の湾曲区間１７３は、先端の方向は一定のままで、操縦者は、進行方向の前後方向に最遠位端の変位を変更できることがわかる。
　続いて、図１５Ｅ及び図１５Ｆでは、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従する先頭追従制御を継続することが可能であることがわかる。
　なお、図１５に示すシミュレーションでは、ｘｚ平面における湾曲可能部１７０の動作制御を想定したため、湾曲角度θを考慮した説明を行ったが、ｘｙｚの３次元空間の場合には、更に旋回角度ζも考慮した態様を採りうる。
　第３の実施形態によれば、第１の実施形態と同様に、意図せずに連続体ロボット１００が対象物と接触してしまうことを回避することができる。これにより、対象物が傷ついたり連続体ロボット自体が破損したりすることを防止することができ、連続体ロボットの操作を安全に行うことができる。
　（第４の実施形態）
　次に、本発明の第４の実施形態について説明する。なお、以下に記載する第４の実施形態の説明においては、上述した第１～第３の実施形態と共通する事項については説明を省略し、上述した第１～第３の実施形態と異なる事項について説明を行う。
　第４の実施形態に係る連続体ロボットの概略構成は、図１及び図２に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、第４の実施形態に係る操作装置の概略構成は、図３に示す第１の実施形態に係る操作装置２００－１の概略構成と同様である。
　上述した第１～第３の実施形態は、先頭追従制御を伴う前進時における追加湾曲操作を想定した形態であった。第４の実施形態では、後退時の追加湾曲操作を想定した形態について説明する。ここで、後退時は、最遠位湾曲区間の湾曲角度θ及び旋回角度ζは、前進時と同様に操縦者が操作してもよいが、前進時の操作を記録しておき、それを基台部１４０の変位ｚｂに応じて再現してもよい。
　図１６は、本発明の第４の実施形態に係る連続体ロボットの制御システム３００の概略構成の一例を示す図である。以下の説明では、図１６に示す第４の実施形態に係る連続体ロボットの制御システム３００を「連続体ロボットの制御システム３００－４」として説明を行う。また、この図１６において、図１１に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。
　図１６に示す第４の実施形態に係る連続体ロボットの制御システム３００－４において、図１１に示す第２の実施形態に係る連続体ロボットの制御システム３００－２に対して、追加・変更された構成部は、以下のとおりである。
　連続体ロボットの制御システム３００－４では、図１１の連続体ロボットの制御システム３００－２に対して、「Ｂｋ＝１　Ｆｗ＝０」ブロック３５０及び切替スイッチ部３６０が追加されている。また、連続体ロボットの制御システム３００－４では、図１１の連続体ロボットの制御システム３００－２に対して、ブロックＫｒが削除される変更がされている。また、連続体ロボットの制御システム３００－４では、図１１の連続体ロボットの制御システム３００－２に対して、切替スイッチ部３３０の第３のスイッチ３３３が第６のスイッチに変更され、さらに、第２のスイッチ３３２の後段のＮＯＴゲートが乗算ブロック３７０に変更されている。
　「Ｂｋ＝１　Ｆｗ＝０」ブロック３５０は、基台部１４０の変位ｚｂに基づいて、基台部１４０が前進しているか後進しているかを判定する基台部判定手段であり、判定の結果、前進時は０の信号を出力し、後進時は１の信号を出力する。
　切替スイッチ部３６０は、「Ｂｋ＝１　Ｆｗ＝０」ブロック３５０の判定結果に基づき、ブロックＦＴＬ＋Ｍｅｍｏｒｙ（記憶部）に記憶されている最遠位湾曲区間の目標湾曲角度及び目標旋回角度と、最遠位操作手段であるブロックＰｌｔから得られる最遠位湾曲区間の目標湾曲角度θｌｔ及び目標旋回角度ζｌｔとのうちのいずれか一方を選択するための切替処理を行う最遠位切替手段である。具体的に、切替スイッチ部３６０は、「Ｂｋ＝１　Ｆｗ＝０」ブロック３５０の出力信号と連動している。例えば、切替スイッチ部３６０は、「Ｂｋ＝１　Ｆｗ＝０」ブロック３５０によって基台部１４０が後進していると判定された場合（１の信号が出力された場合）、ブロックＦＴＬ＋Ｍｅｍｏｒｙ（記憶部）に記憶されている最遠位湾曲区間の目標湾曲角度及び目標旋回角度を選択するための切替処理を行う。そして、運動学演算部３４０は、この切替スイッチ部３６０で選択された最遠位湾曲区間の目標湾曲角度及び目標旋回角度に基づいて、連続体ロボット１００の駆動部で最遠位湾曲区間のワイヤを駆動させる際の駆動変位を演算する。
　具体的に、図１６に示している切替スイッチ部３３０の３極双投形スイッチの状態及び切替スイッチ部３６０の１極双投形スイッチの状態では、最遠位湾曲区間の操作レバーの位置決め制御は無効化される。そして、この場合、操縦者による操作レバーの操作角度が、切替スイッチ部３６０の１極双投形スイッチを介して運動学演算部３４０に入力されるため、先頭追従制御が行われる。ここで、基台部１４０が後進（後退）すると、切替スイッチ部３６０が図１６に示している１極双投形スイッチの状態とは逆の方向に切り替わる。この場合、最遠位湾曲区間の操作レバーの位置決め制御が有効化され、その目標値は、ＦＴＬ＋Ｍｅｍｏｒｙ（記憶部）から前進時に記録された基台部１４０の変位ｚｂに応じた最遠位湾曲区間の目標湾曲角度及び目標旋回角度が出力される。この最遠位湾曲区間の目標湾曲角度及び目標旋回角度は、同時に、運動学演算部３４０に入力されるため、連続体ロボット１００は、前進時に記録された形状を後進（後退）しながら再現する。
　後続湾曲区間の操作レバーへの操縦者の接触が検知されると、本実施形態の制御系は、切替スイッチ部３３０が図１６に示している３極双投形スイッチの状態とは逆の方向に切り替える。この時の後続湾曲区間の動作は、上述した第１の実施形態と同様である。最遠位湾曲区間の制御については、基台部１４０の変位ｚｂが前進時であっても後進時であっても、最遠位湾曲区間の操作レバーの位置決め制御は無効化され、操縦者による操作レバーの操作角度が、切替スイッチ部３６０を介さずに切替スイッチ部３３０を介して、運動学演算部３４０に入力される。
　第３の実施形態によれば、後進時に最遠位湾曲区間に対して前進時の操作記録を再現するように制御しながら、第１の実施形態と同様に、操縦者の追加操作へと継ぎ目なく遷移が可能である。
　（第５の実施形態）
　次に、本発明の第５の実施形態について説明する。なお、以下に記載する第５の実施形態の説明においては、上述した第１～第４の実施形態と共通する事項については説明を省略し、上述した第１～第４の実施形態と異なる事項について説明を行う。
　第５の実施形態に係る連続体ロボットの概略構成は、図１及び図２に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、本発明の第５の実施形態に係る連続体ロボットの制御システムの概略構成は、上述した第１～第４の実施形態に係る連続体ロボットの制御システム３００－１～３００－４の概略構成のいずれも適用可能である。
　上述した第１～第４の実施形態では、図３に示す、各湾曲区間に対する湾曲角度及び旋回角度に１対１で対応する操作レバーを有する操作装置２００－１を用いることを想定した形態であった。第５の実施形態では、操作装置２００として２軸のモータ付きジョイスティックを用いる形態を示す。
　図１７は、本発明の第５の実施形態を示し、図１に示す連続体ロボット１００の３つの湾曲区間１７１～１７３を操作するための操作装置２００の概略構成の一例を示す図である。以下の説明では、図１７に示す第５の実施形態に係る操作装置２００を「操作装置２００－５」として説明を行う。
　操作装置２００－５は、図１７に示すように、第１の湾曲区間１７１を操作するための第１の操作部２４０、第２の湾曲区間１７２を操作するための第２の操作部２５０、及び、第３の湾曲区間１７３を操作するための第３の操作部２６０を有して構成されている。
　第１の操作部２４０は、操作レバー２４１、モータ２４２及び２４４、並びに、角度センサ２４３及び２４５を含み構成されている。操作レバー２４１は、第１の湾曲区間１７１の操作レバーである。角度センサ２４３及び２４５は、それぞれ、第１の湾曲区間１７１に対する操作角度φｘ１及びφｙ１を検出する。モータ２４２及び２４４は、それぞれ、操作角度φｘ１及びφｙ１に制御トルクを与えるためのモータである。
　第２の操作部２５０は、操作レバー２５１、モータ２５２及び２５４、並びに、角度センサ２５３及び２５５を含み構成されている。操作レバー２５１は、第２の湾曲区間１７２の操作レバーである。角度センサ２５３及び２５５は、それぞれ、第２の湾曲区間１７２に対する操作角度φｘ２及びφｙ２を検出する。モータ２５２及び２５４は、それぞれ、操作角度φｘ２及びφｙ２に制御トルクを与えるためのモータである。
　第３の操作部２６０は、操作レバー２６１、モータ２６２及び２６４、並びに、角度センサ２６３及び２６５を含み構成されている。操作レバー２６１は、第３の湾曲区間１７３の操作レバーである。角度センサ２６３及び２６５は、それぞれ、第３の湾曲区間１７３に対する操作角度φｘ３及びφｙ３を検出する。モータ２６２及び２６４は、それぞれ、操作角度φｘ３及びφｙ３に制御トルクを与えるためのモータである。
　上述した第１～第４の実施形態で示した制御系で用いるためには、第ｎの湾曲区間に対する操作角度をφｘｎ及びφｙｎとすると、以下の（１１）式に示すように座標変換すればよい。
Figure JPOXMLDOC01-appb-M000009
　（第６の実施形態）
　次に、本発明の第６の実施形態について説明する。なお、以下に記載する第６の実施形態の説明においては、上述した第１～第５の実施形態と共通する事項については説明を省略し、上述した第１～第５の実施形態と異なる事項について説明を行う。
　第６の実施形態に係る連続体ロボットの概略構成は、図１及び図２に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、第６の実施形態に係る操作装置の概略構成は、図３に示す第１の実施形態に係る操作装置２００－１の概略構成と同様である。
　上述した第１～第５の実施形態で示した制御系は、先頭追従制御と追加操作の切り替えをするために、ブロックＧｉｎ及びＧｏｕｔに０の値または１の値を用いていた。しかしながら、このような２値による切り替えは、操作レバーの急峻な挙動を招くことが懸念されるため、以下の図１８に示す制御系を適用することも可能である。
　図１８は、本発明の第６の実施形態に係る連続体ロボットの制御システム３００の概略構成の一例を示す図である。以下の説明では、図１８に示す第６の実施形態に係る連続体ロボットの制御システム３００を「連続体ロボットの制御システム３００－６」として説明を行う。また、この図１８において、図４に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。
　図１８に示す第６の実施形態に係る連続体ロボットの制御システム３００－６において、図４に示す第１の実施形態に係る連続体ロボットの制御システム３００－１に対して、追加・変更された構成部は、以下のとおりである。
　連続体ロボットの制御システム３００－６では、図４の連続体ロボットの制御システム３００－１に対して、切替スイッチ部３３０の第２のスイッチ３３２における後段にローパスフィルタＦｌｐｆを追加している。この図１８に示す第６の実施形態に係る連続体ロボットの制御システム３００－６では、ローパスフィルタＦｌｐｆを介した信号がブロックＧｉｎ及びＧｏｕｔに入力されることになる。この際、ローパスフィルタＦｌｐｆの折点周波数は、１Ｈｚ程度とすればよい。図１８では、第１の実施形態の制御系に対する変更点を説明したが、第２～第４の実施形態の制御系についても同様に変更してもよい。
　（その他の実施形態）
　本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
　このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。
　なお、上述した本発明の実施形態は、いずれも本発明を実施するにあたっての具体化の例を示したものに過ぎず、これらによって本発明の技術的範囲が限定的に解釈されてはならないものである。即ち、本発明はその技術思想、又はその主要な特徴から逸脱することなく、様々な形で実施することができる。
　本発明は上記実施の形態に制限されるものではなく、本発明の精神及び範囲から離脱することなく、様々な変更及び変形が可能である。従って、本発明の範囲を公にするために以下の請求項を添付する。
　本願は、２０２０年６月２５日提出の日本国特許出願特願２０２０－１０９８５０と２０２１年６月１６日提出の日本国特許出願特願２０２１－１００１８９を基礎として優先権を主張するものであり、その記載内容の全てをここに援用する。

Claims (9)

  1. 　ワイヤが駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部と、前記湾曲可能部を支える基台部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、
    　複数の前記湾曲区間のうち、前記基台部から最も遠い位置にある最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記連続体ロボットの進行方向における前記基台部の変位と、に基づいて、前記複数の湾曲区間のうち、前記最遠位湾曲区間と前記基台部との間に位置する後続湾曲区間の目標湾曲角度および目標旋回角度を算出する後続算出手段と、操作されることによって前記後続湾曲区間の目標湾曲角度および目標旋回角度を入力する後続操作手段と、
    　前記後続算出手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、前記後続操作手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う後続切替手段と、
    　前記後続切替手段で選択された前記後続湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記後続湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算する演算手段と、
    　を有することを特徴とする連続体ロボットの制御システム。
  2. 　前記後続切替手段は、前記後続操作手段への操縦者の接触が検知された場合、前記後続操作手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行うことを特徴とする請求項１に記載の連続体ロボットの制御システム。
  3. 　前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を記憶する記憶手段と、
    　前記記憶手段に記憶されている前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記後続操作手段から入力された前記後続湾曲区間の目標湾曲角度および目標旋回角度と、に基づいて、前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を算出する最遠位算出手段と、
    　操作されることによって前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を入力する最遠位操作手段と、
    　前記最遠位算出手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記最遠位操作手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う最遠位切替手段と、
    　を更に有し、
    　前記演算手段は、更に、前記最遠位切替手段で選択された前記最遠位湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記最遠位湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算し、
    　前記最遠位切替手段は、前記後続操作手段への操縦者の接触が検知された場合、前記最遠位算出手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行う
    　ことを特徴とする請求項１または２に記載の連続体ロボットの制御システム。
  4. 　前記後続湾曲区間は、少なくとも２つの後続湾曲区間を備えており、
    　前記第２の後続操作手段から得られる前記第２の後続湾曲区間の目標湾曲角度および目標旋回角度に対して制御ゲインを乗ずる制御ゲイン手段を更に有し、
    　前記後続切替手段は、前記第２の後続操作手段への操縦者の接触が検知された場合、前記第２の後続湾曲区間の目標湾曲角度および目標旋回角度として前記後続操作手段から得られる前記第２の後続湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行うとともに、前記第１の後続湾曲区間の目標湾曲角度および目標旋回角度として前記制御ゲイン手段によって前記制御ゲインを乗じた前記第２の後続湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行う
    　ことを特徴とする請求項１または２に記載の連続体ロボットの制御システム。
  5. 　前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を記憶する記憶手段と、
    　前記基台部の変位に基づいて、前記基台部が前進しているか後進しているかを判定する基台部判定手段と、
    　操作されることによって前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を入力する最遠位操作手段と、
    　前記記憶手段に記憶されている前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記最遠位操作手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う最遠位切替手段と、
    　を更に有し、
    　前記演算手段は、更に、前記最遠位切替手段で選択された前記最遠位湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記最遠位湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算し、
    　前記最遠位切替手段は、前記基台部判定手段によって前記基台部が後進していると判定された場合、前記記憶手段に記憶されている前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行う
    　ことを特徴とする請求項１または２に記載の連続体ロボットの制御システム。
  6. 　操作されることによって前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を入力する最遠位操作手段を更に有し、
    　前記後続算出手段は、前記最遠位操作手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を用いて、前記後続湾曲区間の目標湾曲角度および目標旋回角度を算出することを特徴とする請求項１乃至５のいずれか１項に記載の連続体ロボットの制御システム。
  7. 　前記後続操作手段および前記最遠位操作手段は、モータと、前記モータにより駆動される操作レバーと、角度センサとを含み構成されていることを特徴とする請求項３、５、６のいずれか１項に記載の連続体ロボットの制御システム。
  8. 　ワイヤが駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部と、前記湾曲可能部を支える基台部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御方法であって、
    　複数の前記湾曲区間のうち、前記基台部から最も遠い位置にある最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記連続体ロボットの進行方向における前記基台部の変位と、に基づいて、前記複数の湾曲区間のうち、前記最遠位湾曲区間と前記基台部との間に位置する後続湾曲区間の目標湾曲角度および目標旋回角度を算出する後続算出ステップと、
    　操作されることによって前記後続湾曲区間の目標湾曲角度および目標旋回角度を入力する後続操作ステップと、
    　前記後続算出ステップで得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、前記後続操作ステップで得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う後続切替ステップと、
    　前記後続切替ステップで選択された前記後続湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記後続湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算する演算ステップと、
    　を有することを特徴とする連続体ロボットの制御方法。
  9. 　コンピュータを、請求項１乃至７のいずれか１項に記載の連続体ロボットの制御システムにおける各手段として機能させるためのプログラム。
PCT/JP2021/023884 2020-06-25 2021-06-24 連続体ロボットの制御システム及びその制御方法、並びに、プログラム Ceased WO2021261539A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
US18/069,127 US12318928B2 (en) 2020-06-25 2022-12-20 Continuum robot control system and control method, and program
US19/196,492 US20250262758A1 (en) 2020-06-25 2025-05-01 Continuum robot control system and control method, and program

Applications Claiming Priority (4)

Application Number Priority Date Filing Date Title
JP2020109850 2020-06-25
JP2020-109850 2020-06-25
JP2021100189A JP7614954B2 (ja) 2020-06-25 2021-06-16 連続体ロボットの制御システム及びその制御方法、並びに、プログラム
JP2021-100189 2021-06-16

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US18/069,127 Continuation US12318928B2 (en) 2020-06-25 2022-12-20 Continuum robot control system and control method, and program

Publications (1)

Publication Number Publication Date
WO2021261539A1 true WO2021261539A1 (ja) 2021-12-30

Family

ID=79281327

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2021/023884 Ceased WO2021261539A1 (ja) 2020-06-25 2021-06-24 連続体ロボットの制御システム及びその制御方法、並びに、プログラム

Country Status (2)

Country Link
US (2) US12318928B2 (ja)
WO (1) WO2021261539A1 (ja)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116968038B (zh) * 2023-09-22 2024-01-02 中国人民解放军军事科学院国防科技创新研究院 一种基于多电机编码的柔性操控系统与控制方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060178562A1 (en) * 2005-02-10 2006-08-10 Usgi Medical Inc. Apparatus and methods for obtaining endoluminal access with a steerable guide having a variable pivot
WO2017033356A1 (ja) * 2015-08-25 2017-03-02 川崎重工業株式会社 ロボットシステム
JP2018523508A (ja) * 2015-06-30 2018-08-23 キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc マニピュレータを制御する方法および装置
JP2018175602A (ja) * 2017-04-18 2018-11-15 キヤノン株式会社 連続体ロボットシステムの制御装置、連続体ロボットシステムの制御方法、プログラム
JP2018176406A (ja) * 2017-04-21 2018-11-15 キヤノン株式会社 連続体ロボットの制御システム及びその制御方法、並びに、プログラム
JP2019058648A (ja) * 2017-07-17 2019-04-18 キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc 連続体ロボット制御方法および装置
JP2019166307A (ja) * 2018-03-23 2019-10-03 キヤノン株式会社 連続体ロボット制御装置、連続体ロボット制御方法及びプログラム
US20200179652A1 (en) * 2017-08-14 2020-06-11 Project Moray, Inc. Alternative Fluid-Driven Articulation Architecture for Catheters and Other Uses

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012015659A2 (en) * 2010-07-28 2012-02-02 Arnold Oyola Surgical positioning and support system
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
WO2015168177A1 (en) * 2014-04-28 2015-11-05 Massachusetts Institute Of Technology Multi-link modular continuum robotic endoscope system
JP6632487B2 (ja) * 2016-07-13 2020-01-22 キヤノン株式会社 連続体ロボット、その運動学の補正方法、および連続体ロボットの制御方法
US11103992B2 (en) * 2017-02-28 2021-08-31 Canon Kabushiki Kaisha Apparatus of continuum robot
WO2019055701A1 (en) * 2017-09-13 2019-03-21 Vanderbilt University MULTI-SCALE CONTINUUM MOVEMENT ROBOTS BY BALANCING MODULATION
WO2021196067A1 (zh) * 2020-04-01 2021-10-07 大连理工大学 一种具有灵活抓取目标功能的多自由度连续型机器人

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060178562A1 (en) * 2005-02-10 2006-08-10 Usgi Medical Inc. Apparatus and methods for obtaining endoluminal access with a steerable guide having a variable pivot
JP2018523508A (ja) * 2015-06-30 2018-08-23 キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc マニピュレータを制御する方法および装置
WO2017033356A1 (ja) * 2015-08-25 2017-03-02 川崎重工業株式会社 ロボットシステム
JP2018175602A (ja) * 2017-04-18 2018-11-15 キヤノン株式会社 連続体ロボットシステムの制御装置、連続体ロボットシステムの制御方法、プログラム
JP2018176406A (ja) * 2017-04-21 2018-11-15 キヤノン株式会社 連続体ロボットの制御システム及びその制御方法、並びに、プログラム
JP2019058648A (ja) * 2017-07-17 2019-04-18 キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc 連続体ロボット制御方法および装置
US20200179652A1 (en) * 2017-08-14 2020-06-11 Project Moray, Inc. Alternative Fluid-Driven Articulation Architecture for Catheters and Other Uses
JP2019166307A (ja) * 2018-03-23 2019-10-03 キヤノン株式会社 連続体ロボット制御装置、連続体ロボット制御方法及びプログラム

Also Published As

Publication number Publication date
US20230131269A1 (en) 2023-04-27
US12318928B2 (en) 2025-06-03
US20250262758A1 (en) 2025-08-21

Similar Documents

Publication Publication Date Title
US11685046B2 (en) Continuum robot control methods and apparatus
JP7250859B2 (ja) 連続体ロボット及びその制御方法、並びに、プログラム
Moore et al. Cobot implementation of virtual paths and 3D virtual surfaces
US20240181630A1 (en) Control system for continuum robot and control method for same
KR20110041950A (ko) 여유자유도 제어를 이용한 로봇의 교시 및 재현 방법
CN114770459B (zh) 一种主控制臂运动控制方法
JP7614954B2 (ja) 連続体ロボットの制御システム及びその制御方法、並びに、プログラム
EP4322884A2 (en) An apparatus, computer-implemented method and computer program for enabling the tracking of a robotic instrument to a passive controller
WO2021166995A1 (ja) 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット
US20250262758A1 (en) Continuum robot control system and control method, and program
JP2009134352A (ja) ロボットの動作経路作成装置及びロボットの動作経路作成方法
WO2018105045A1 (ja) 医療システムとその制御方法
CN119112372A (zh) 手术机器人的主从运动控制方法及相关设备
Moore et al. Cobot implementation of 3D virtual surfaces
JP7646468B2 (ja) 連続体ロボット及びその制御方法、並びに、プログラム
US20240399563A1 (en) Continuum robot control system and continuum robot control method
JP7285872B2 (ja) 連続体ロボットを制御するための方法及び装置
JP3206765B2 (ja) マスタスレーブマニピュレータの制御方法
Yamada et al. Steering of pedal wave of a snake-like robot by superposition of curvatures
Alambeigi et al. A versatile data-driven framework for model-independent control of continuum manipulators interacting with obstructed environments with unknown geometry and stiffness
US20250162134A1 (en) Method and apparatus for controlling continuum robot
WO2023112620A1 (ja) 連続体ロボット制御システム、連続体ロボット制御方法及びプログラム
WO2023171232A1 (ja) 連続体ロボット及びその制御方法、並びに、プログラム
CN120228713A (zh) 操作臂运动控制方法、装置及系统、存储介质
Prasanga et al. Achievement of real haptic sensation with tendon driven segregated jaws for laparoscopic forceps

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: 21830285

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21830285

Country of ref document: EP

Kind code of ref document: A1