CN113510706B - Trajectory following motion planning method and system for continuum robot - Google Patents

Trajectory following motion planning method and system for continuum robot Download PDF

Info

Publication number
CN113510706B
CN113510706B CN202110810418.9A CN202110810418A CN113510706B CN 113510706 B CN113510706 B CN 113510706B CN 202110810418 A CN202110810418 A CN 202110810418A CN 113510706 B CN113510706 B CN 113510706B
Authority
CN
China
Prior art keywords
joint
coordinate system
robot
axis
base
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.)
Active
Application number
CN202110810418.9A
Other languages
Chinese (zh)
Other versions
CN113510706A (en
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.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202110810418.9A priority Critical patent/CN113510706B/en
Publication of CN113510706A publication Critical patent/CN113510706A/en
Application granted granted Critical
Publication of CN113510706B publication Critical patent/CN113510706B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J17/00—Joints
    • B25J17/02—Wrist joints

Abstract

Disclosure of the inventionA trajectory following motion planning method and a system of a continuum robot belong to the field of obstacle avoidance algorithms of continuum robots, and the method comprises the following steps: defining a cavity mouth coordinate system, a tail end coordinate system and a base coordinate system; setting joint parameters of a target state of the continuum robot according to the internal environment of the cavity, and obtaining a target track; wherein the joint parameters comprise a tangent line at the end of the curved joint segment and a base coordinate system { O }2Z axis positive direction angle thetaiAnd central axis of the single joint in the base coordinate system { O }2Projection of the OXY plane onto the base coordinate system { O }2The positive direction corner of the X axis
Figure DDA0003166475730000011
The target track is divided into three arc track sections which are connected end to end, and each arc track section is divided into a plurality of sections of stepping motion to follow. The track following obstacle avoidance algorithm has good control effect and short solving time consumption, and the continuum robot adopting the algorithm can reach the designated position through the narrow cavity without collision.

Description

Trajectory following motion planning method and system for continuum robot
Technical Field
The invention belongs to the field of obstacle avoidance algorithms of continuum robots, and particularly relates to a trajectory following motion planning method and system of a continuum robot.
Background
In the field of modern industrial engineering, robots have been widely used in many working scenarios, such as: processing and manufacturing, production and manufacturing, logistics sorting, detection and maintenance and the like. However, when facing a working environment with a complicated structure and a narrow space, such as an aircraft oil tank endoscopic inspection, an engine maintenance inspection, a single-hole operation, and the like, the industrial engineering robot cannot meet the requirements of the application scenes due to the limitations of an overlarge structure size, insufficient motion flexibility and the like.
Compared with the traditional robot, the motion of the continuum robot is not in a mode of rotating or translating around the joint but generated by self bending deformation, so that the continuum robot has higher dexterity, safety and stronger environment adaptability, and the driver is arranged at the far end due to the design of the rope drive of the continuum robot, the size and the weight of the continuum robot are greatly reduced, and the continuum robot has better load capacity and controllability. The characteristics enable the continuum robot to show great advantages and play an important role in occasions such as minimally invasive surgery, micropore detection, task execution in complex limited space and the like.
Although the continuum robot has the advantages and the application value, the kinematic model of the continuum robot is greatly different from that of the traditional robot, and the problems of more complex kinematic model, low control precision and the like exist in application scenes such as narrow environments with complex structures and narrow spaces.
Disclosure of Invention
Aiming at the defects of the related art, the invention aims to provide a trajectory following motion planning method and system of a continuum robot, and aims to solve the problem that the control precision is not high when the continuum robot follows the trajectory through a narrow cavity.
To achieve the above object, an aspect of the present invention provides a trajectory following motion planning method for a continuum robot including a first joint, a second joint, and a third joint along a direction pointing from a tip to a base; the trajectory following motion planning method comprises the following steps:
defining a cavity mouth coordinate system { O }0}, end coordinate system { O1And the base coordinate system { O }2-moving the robot to bring said end coordinate system { O }1And the coordinate system of the orifice of the cavity { O }0The superposition;
setting joint parameters of a target state of the continuum robot according to the internal environment of the cavity, and obtaining a target track; wherein the joint parameters comprise a tangent line at the end of the curved joint segment and a base coordinate system { O }2Z axis positive direction angle thetaiAnd central axis of the single joint in the base coordinate system { O }2Projection of the OXY plane onto the base coordinate system { O }2The positive direction corner of the X axis
Figure BDA0003166475710000022
Divide the target track into three end to end's circular arc orbit sections, every circular arc orbit section divides into multistage step motion and follows, includes:
the continuum robot follows the lumen crossing coordinate system { O }0Forward movement of the Z axis;
solving a nonlinear equation set according to a damping Newton method to calculate joint parameters of the first joint, the second joint and the third joint;
and sequentially controlling the third joint, the second joint and the first joint to move according to the respective corresponding joint parameters.
Further, solving the nonlinear system of equations according to the damped newton method to calculate joint parameters of the first joint, the second joint, and the third joint, comprising the steps of:
defining the arc track segment corresponding to the ith joint as Pathi AFind Pathi AKey point B ofi A、Oi AIn the cavity crossing coordinate system { O0Coordinates below and radius R of the circular arc track segmenti A(ii) a Wherein, the key point Oi AIs Pathi ACenter of circle, key point Bi AAnd Oi ALine segment O formed by connectioni ABi ALength of l0And is perpendicular to Pathi AThe plane of the device;
will Pathi AKey point B ofi A、Oi ACoordinate transformation to the end coordinate system { O }1Fourthly, the step of mixing;
solving a nonlinear system of equations according to the damped newton method
Figure BDA0003166475710000021
Function G (y)m) Independent variable of (2)
Figure BDA0003166475710000031
The joint parameters obtained for the mth iteration; the iterative equations and constraint conditions are constructed as follows:
an iteration equation: y ism+1＝ym-λG′(ym+1)-1G(ym)
Constraint conditions are as follows: norm (G (y)m+1)，2)＜norm(G(ym)，2)
In the formula, λ ∈ (0,1) is a step factor, for each iteration, calculation is started from λ ═ 1, and when the constraint condition is not met, the step factor is reduced to 9/10 until the constraint condition is met; when iterating to norm (G (y)m) And when 2) is less than or equal to 0.1, finishing the iteration and outputting the joint parameter ym。
Further, sequentially controlling the third joint, the second joint and the first joint to move according to the respective corresponding joint parameters comprises:
controlling the third joint to move until the joint parameter is equal to the j section track following
Figure BDA0003166475710000032
And specifies that when j-2n is ≦ 0,
Figure BDA0003166475710000033
then controlling the second joint to move until the joint parameter is equal to
Figure BDA0003166475710000034
Then controlling the first joint to move until the tail end of the continuum robot is overlapped with the target track; finally, the joint parameters at that time are recorded
Figure BDA0003166475710000035
Furthermore, the origin of the coordinate system of the cavity channel opening is positioned at the center of the cavity channel inlet, and the Z-axis direction of the coordinate system of the cavity channel opening is inward from the plane where the vertical cavity channel inlet is positioned; the terminal coordinate system and the base coordinate system are both lagrangian coordinate systems following the continuum robot;
the end coordinate system { O1The origin of the robot is positioned at the center of the tail end of the robot, the Z-axis direction of the robot is superposed with the central axis of the robot, and the base in the initial reset state points to the tail end;
the base coordinate system { O2The origin of the point is located at the robot baseThe center of the seat disk, the Z-axis direction of the seat disk is coincident with the central axis of the robot and points to the tail end from the base in the initial reset state, the X-axis of the seat disk is equal to { O }1The X axes of the coordinate systems are parallel and the positive directions are consistent.
Another aspect of the present invention provides a trajectory following motion planning system for a continuum robot, the continuum robot comprising a first joint, a second joint, and a third joint along a direction pointing from a tip to a base; the trajectory following motion planning system includes:
an initialization unit for defining a cavity port coordinate system { O }0}, end coordinate system { O1And the base coordinate system { O }2-moving the robot to bring said end coordinate system { O }1And the coordinate system of the orifice of the cavity { O }0The superposition;
the setting unit is used for setting joint parameters of a target state of the continuum robot according to the internal environment of the cavity, so that a target track is obtained; wherein the joint parameters comprise a tangent line at the end of the curved joint segment and a base coordinate system { O }2Z axis positive direction angle thetaiAnd central axis of the single joint in the base coordinate system { O }2Projection of the OXY plane onto the base coordinate system { O }2The positive direction corner of the X axis
Figure BDA0003166475710000041
The following unit divides the target track into three end-to-end circular arc track sections, and each circular arc track section is divided into a plurality of sections of stepping motion to follow, and the following unit comprises:
the continuum robot follows the lumen crossing coordinate system { O }0Forward movement of the Z axis;
solving a nonlinear equation set according to a damping Newton method to calculate joint parameters of the first joint, the second joint and the third joint;
and sequentially controlling the third joint, the second joint and the first joint to move according to the respective corresponding joint parameters.
Further, the following unit solves the nonlinear equation system according to a damped Newton method to calculate joint parameters of the first joint, the second joint and the third joint, and comprises the following steps:
defining the arc track segment corresponding to the ith joint as Pathi AFind Pathi AKey point B ofi A、Oi AIn the cavity crossing coordinate system { O0Coordinates below and radius R of the circular arc track segmenti A(ii) a Wherein, the key point Oi AIs Pathi ACenter of circle, key point Bi AAnd Oi ALine segment O formed by connectioni ABi ALength of l0And is perpendicular to Pathi AThe plane of the device;
will Pathi AKey point B ofi A、Oi ACoordinate transformation to the end coordinate system { O }1Fourthly, the step of mixing;
solving a nonlinear system of equations according to the damped newton method
Figure BDA0003166475710000042
m is the iteration number; the iterative equations and constraint conditions are constructed as follows:
an iteration equation: y ism+1＝ym-λG′(ym+1)-1G(ym)
Constraint conditions are as follows: norm (G (y)m+1)，2)＜norm(G(ym)，2)
In the formula, λ ∈ (0,1) is a step factor, for each iteration, calculation is started from λ ═ 1, and when the constraint condition is not met, the step factor is reduced to 9/10 until the constraint condition is met; when iterating to norm (G (y)m) And when 2) is less than or equal to 0.1, finishing the iteration and outputting the joint parameter ym。
Further, the following unit sequentially controls the third joint, the second joint and the first joint to move according to the respective corresponding joint parameters, and the following unit comprises:
controlling the third joint to move until the joint parameter is equal to the j section track following
Figure BDA0003166475710000051
And specifies that when j-2n is ≦ 0,
Figure BDA0003166475710000052
then controlling the second joint to move until the joint parameter is equal to
Figure BDA0003166475710000053
Then controlling the first joint to move until the tail end of the continuum robot is overlapped with the target track; finally, the joint parameters at that time are recorded
Figure BDA0003166475710000054
Furthermore, the origin of the coordinate system of the cavity channel opening is positioned at the center of the cavity channel inlet, and the Z-axis direction of the coordinate system of the cavity channel opening is inward from the plane where the vertical cavity channel inlet is positioned; the terminal coordinate system and the base coordinate system are both lagrangian coordinate systems following the continuum robot;
the end coordinate system { O1The origin of the robot is positioned at the center of the tail end of the robot, the Z-axis direction of the robot is superposed with the central axis of the robot, and the base in the initial reset state points to the tail end;
the base coordinate system { O2The origin of the Z axis coincides with the central axis of the robot and points to the end from the base in the initial reset state, the X axis of the Z axis and the { O }1The X axes of the coordinate systems are parallel and the positive directions are consistent.
Through the technical scheme, compared with the prior art, the robot is controlled to continuously follow a target track while an external linear driver is controlled to drive the whole robot to perform stepping motion in the vertical direction based on kinematic modeling and forward and reverse kinematic analysis of a continuum robot, so that the robot is controlled to reach a specified position through a narrow cavity without collision, the track following is more accurate, the solution time is short, and the robot is suitable for endoscopic inspection of narrow-inlet internal complex environments of aircraft fuel tanks and the like.
Drawings
Fig. 1 is a flowchart of a continuum robot adopting a track following obstacle avoidance algorithm in the embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. In addition, the technical features involved in the embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.
The contents of the above embodiments will be described with reference to a preferred embodiment.
S1: defining a cavity mouth coordinate system { O }0Terminal coordinate system of continuum robot { O }1And the base coordinate system { O }2The requirements for defining the coordinate system are as follows:
the origin of the coordinate system of the cavity channel opening is positioned at the center of the cavity channel inlet, and the Z-axis direction of the coordinate system is inward of the plane where the vertical cavity channel inlet is positioned; the tail end and base coordinate system of the continuum robot is a Lagrange coordinate system which follows the continuum robot; the continuum robot is formed by connecting a first joint, a second joint and a third joint in series from top to bottom; the continuum robot end coordinate system { O1The origin point of the Z-axis direction is positioned at the center of the tail end of the robot, and the Z-axis direction of the Z-axis direction is superposed with the central axis of the robot and is vertically upward; the continuum robot base coordinate system { O2}, the origin of which is located at the center of the bottom of the robot, the Z-axis direction of which coincides with the center axis of the robot and is vertically upward, the X-axis of which coincides with { O }1The X axes of the coordinate systems are parallel and the positive directions are consistent.
In one embodiment, the continuum robot is a three joint continuum robot. The robot adopts line drive, and single joint takes place bending deformation under the line drive effect, and its central axis can be regarded as the circular arc that the curvature is invariable when taking place to warp, can change crooked direction and the size of bending curvature through control, so every joint has two degrees of freedom, and the robot has six degrees of freedom altogether.
S2: keeping the continuum robot in an initial reset state, moving the machineRobot end coordinate System { O }1And the coordinate system of the orifice of the cavity { O }0The superposition;
the initial reset state means that the central axes of the three joints of the continuum robot are all straight lines and coincide, namely parameters of each joint of the robot
Figure BDA0003166475710000071
Are all 0.
S3: appointing the final joint parameter of the robot after completely entering the cavity according to the internal environment of the cavity
Figure BDA0003166475710000072
At this time, the base coordinate system { O }2And the coordinate system of the orifice of the cavity { O }0Coincidence, wherein the joint parameters are defined as follows:
each single joint is defined with joint parameters
Figure BDA0003166475710000073
Wherein theta isiDescribing the size of the central angle of the central axis of the curved articular segment,
Figure BDA0003166475710000074
description of the center axis of the single joint in the base coordinate System { O }2Projection of the OXY plane onto the base coordinate system { O }2The positive direction turning angle of the X axis takes the clockwise direction as the positive direction.
S4: a continuous curve formed by connecting the central axes of three sections of joints after the robot completely enters a cavity is named as a target track A. The track A is formed by connecting three arcs end to end, and the arc length of each arc is equal to the length L of a single joint0And taking the center of a base plate of a third joint section of the robot as a starting point and the center of an end plate of the first joint section as an end point, wherein the center of the base plate of the third joint section of the robot is 100 mm.
And (3) track following is carried out on the track A, the whole track following process is divided into 3n sections, each section of circular arc track of the track A is divided into n sections for following, wherein n is set according to the track following precision requirement. The trajectory following process is as follows:
during the j-th track following, the whole robot is driven linearly outsideDriving lower edge z0The shaft moves forward by a distance of 300/(3n) mm; controlling the movement of the third joint segment to a joint parameter equal to
Figure BDA0003166475710000075
(it is specified that when j-2n is 0 or less,
Figure BDA0003166475710000076
) (ii) a Then controlling the second joint segment to move until the joint parameter is equal to
Figure BDA0003166475710000077
Then controlling the first joint section to move until the tail end coincides with the track A; finally recording the joint parameters at the moment
Figure BDA0003166475710000078
After 3 n-segment stepping movement, the robot designates a target task.
In the j section stepping motion, the first joint section is controlled to move to be coincident with the track A, and in order to achieve the aim, only theta is calculated1，j、
Figure BDA0003166475710000079
The two joint parameters and the remaining four joint parameters are read from the recorded data. Although only two joint parameters need to be solved, the analytic solution is still difficult to obtain, and the damping Newton method is adopted for solving the problem. Wherein the first joint section is controlled to move to the end to coincide with the track A, and the specific solving process is as follows:
defining the circular arc track segment corresponding to the ith joint segment on the track A as Pathi AThe jth segment in the whole track following process will be opposite to the jth segment
Figure BDA0003166475710000086
Arc track segment corresponding to segment
Figure BDA0003166475710000081
Follow-up is performed. Find Pathi AKey point B ofi A、Oi AIn the orifice coordinate system { O0Coordinates below, and radius R of the circular arc trajectory segmenti A. Wherein the key point Oi AIs Pathi ACenter of circle, key point Bi AAnd Oi ALine segment O formed by connectioni ABi A10mm long and perpendicular to Pathi AThe plane of the plane.
Suppose that the j (j ∈ [1, 3n ]) is proceeding]And j ∈ N*) Step by step, then
Figure BDA0003166475710000087
Will Pathi AKey point B ofi A、Oi ACoordinate transformation to the current end coordinate system O1And (6) below.
The function is defined as follows:
Figure BDA0003166475710000082
in the formula, the function G (y)m) Independent variable of (2)
Figure BDA0003166475710000083
For the joint parameters obtained for the mth iteration,
Figure BDA0003166475710000084
represents Pathi AArc radius and origin O of joint terminal coordinate system after mth iteration1To point Oi AThe difference in the distance of (a) to (b),
Figure BDA0003166475710000085
represents O1To Pathi ADistance of the plane. Equation set G (y)n) The solution of 0 is the joint parameter corresponding to the superposition of the first joint movement to the track a. Solving the solution of the nonlinear equation system according to the damping Newton method, and constructing an iterative equation and constraint conditions as follows:
an iteration equation: y ism+1＝ym-λG′(ym+1)-1G(ym)
Constraint conditions are as follows: norm (G (y)m+1)，2)＜norm(G(ym)，2)
In the formula, λ ∈ (0,1) is a step factor, and in each iteration, the calculation is started from λ ═ 1, and when the constraint condition is not satisfied, the step factor is reduced to 9/10 until the constraint condition is satisfied. When iterating to norm (G (y)m) And 2) is less than or equal to 0.1, the solution error is shown to be within an acceptable range, the iteration is finished, and the joint parameter y is outputm。
It will be understood by those skilled in the art that the foregoing is only a preferred embodiment of the present invention, and is not intended to limit the invention, and that any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the scope of the present invention.

Claims (8)

1. A trajectory following motion planning method for a continuum robot, the continuum robot comprising a first joint, a second joint and a third joint along a direction pointing from a tip to a base; the method for planning the track following motion is characterized by comprising the following steps of:
defining a cavity mouth coordinate system { O }0}, end coordinate system { O1And the base coordinate system { O }2-moving the robot to bring said end coordinate system { O }1And the coordinate system of the orifice of the cavity { O }0The superposition;
setting joint parameters of a target state of the continuum robot according to the internal environment of the cavity, and obtaining a target track; wherein the joint parameters comprise a tangent line at the end of the curved joint segment and a base coordinate system { O }2Z axis positive direction angle thetaiAnd central axis of the single joint in the base coordinate system { O }2Projection of the OXY plane onto the base coordinate system { O }2The positive direction corner of the X axis
Figure FDA0003483435240000011
Divide the target track into three end to end's circular arc orbit sections, every circular arc orbit section divides into multistage step motion and follows, includes:
the continuum robot follows the lumen crossing coordinate system { O }0Forward movement of the Z axis;
solving a nonlinear equation set according to a damping Newton method to calculate joint parameters of the first joint, the second joint and the third joint;
and sequentially controlling the third joint, the second joint and the first joint to move according to the respective corresponding joint parameters.
2. A trajectory-following motion planning method according to claim 1, wherein solving a system of nonlinear equations to calculate joint parameters of the first joint, the second joint, and the third joint according to damped newton's method comprises the steps of:
defining the arc track segment corresponding to the ith joint as Pathi AFind Pathi AKey point B ofi A、Oi AIn the cavity crossing coordinate system { O0Coordinates below and radius R of the circular arc track segmenti A(ii) a Wherein, the key point Oi AIs Pathi ACenter of circle, key point Bi AAnd Oi ALine segment O formed by connectioni ABi ALength of l0And is perpendicular to Pathi AThe plane of the device;
will Pathi AKey point B ofi A、Oi ACoordinate transformation to the end coordinate system { O }1Fourthly, the step of mixing;
solving a nonlinear system of equations according to the damped newton method
Figure FDA0003483435240000021
Function G (y)m) Independent variable of (2)
Figure FDA0003483435240000022
Is the first joint inThe j section track is followed by the joint parameters obtained by the m iteration; the iterative equations and constraint conditions are constructed as follows:
an iteration equation: y ism+1＝ym-λG′(ym+1)-1G(ym)
Constraint conditions are as follows: norm (G (y)m+1),2)＜norm(G(ym),2)
In the formula, λ ∈ (0,1) is a step factor, for each iteration, calculation is started from λ ═ 1, and when the constraint condition is not met, the step factor is reduced to 9/10 until the constraint condition is met; when iterating to norm (G (y)m) And when 2) is less than or equal to 0.1, finishing the iteration and outputting the joint parameter ym。
3. The trajectory-following motion planning method according to claim 2, wherein sequentially controlling the third joint, the second joint, and the first joint according to the respective corresponding joint parameters comprises:
controlling the third joint to move until the joint parameter is equal to the j section track following
Figure FDA0003483435240000023
And specifies that when j-2n is ≦ 0,
Figure FDA0003483435240000024
then controlling the second joint to move until the joint parameter is equal to
Figure FDA0003483435240000025
Then controlling the first joint to move until the tail end of the continuum robot is overlapped with the target track; finally, the joint parameters at that time are recorded
Figure FDA0003483435240000026
4. The trajectory following motion planning method according to any one of claims 1 to 3, wherein the origin of the coordinate system of the lumen opening is located at the center of the lumen entrance, and the Z-axis direction thereof is inward of the plane where the vertical lumen entrance is located; the terminal coordinate system and the base coordinate system are both lagrangian coordinate systems following the continuum robot;
the end coordinate system { O1The origin of the robot is positioned at the center of the tail end of the robot, the Z-axis direction of the robot is superposed with the central axis of the robot, and the base in the initial reset state points to the tail end;
the base coordinate system { O2The origin of the Z axis coincides with the central axis of the robot and points to the end from the base in the initial reset state, the X axis of the Z axis and the { O }1The X axes of the coordinate systems are parallel and the positive directions are consistent.
5. A trajectory following motion planning system for a continuum robot comprising a first joint, a second joint, and a third joint along a direction pointing from a tip to a base; characterized in that the trajectory following motion planning system comprises:
an initialization unit for defining a cavity port coordinate system { O }0}, end coordinate system { O1And the base coordinate system { O }2-moving the robot to bring said end coordinate system { O }1And the coordinate system of the orifice of the cavity { O }0The superposition;
the setting unit is used for setting joint parameters of a target state of the continuum robot according to the internal environment of the cavity, so that a target track is obtained; wherein the joint parameters comprise a tangent line at the end of the curved joint segment and a base coordinate system { O }2Z axis positive direction angle thetaiAnd central axis of the single joint in the base coordinate system { O }2Projection of the OXY plane onto the base coordinate system { O }2The positive direction corner of the X axis
Figure FDA0003483435240000031
The following unit divides the target track into three end-to-end circular arc track sections, and each circular arc track section is divided into a plurality of sections of stepping motion to follow, and the following unit comprises:
the continuum robot follows the lumen crossing coordinate system { O }0Z axis ofMoving in the forward direction;
solving a nonlinear equation set according to a damping Newton method to calculate joint parameters of the first joint, the second joint and the third joint;
and sequentially controlling the third joint, the second joint and the first joint to move according to the respective corresponding joint parameters.
6. The trajectory-following motion planning system of claim 5, wherein the following unit solves a system of nonlinear equations according to damped newton's method to calculate joint parameters for the first joint, the second joint, and the third joint, comprising:
defining the arc track segment corresponding to the ith joint as Pathi AFind Pathi AKey point B ofi A、Oi AIn the cavity crossing coordinate system { O0Coordinates below and radius R of the circular arc track segmenti A(ii) a Wherein, the key point Oi AIs Pathi ACenter of circle, key point Bi AAnd Oi ALine segment O formed by connectioni ABi ALength of l0And is perpendicular to Pathi AThe plane of the device;
will Pathi AKey point B ofi A、Oi ACoordinate transformation to the end coordinate system { O }1Fourthly, the step of mixing;
solving a nonlinear system of equations according to the damped newton method
Figure FDA0003483435240000041
m is the iteration number; the iterative equations and constraint conditions are constructed as follows:
an iteration equation: y ism+1＝ym-λG′(ym+1)-1G(ym)
Constraint conditions are as follows: norm (G (y)m+1),2)＜norm(G(ym),2)
Where λ e (0,1) is the step factor, and for each iteration, starting with λ ═ 1,when the constraint condition is not met, the step factor is reduced to 9/10 until the constraint condition is met; when iterating to norm (G (y)m) And when 2) is less than or equal to 0.1, finishing the iteration and outputting the joint parameter ym。
7. The trajectory-following motion planning system of claim 6, wherein the following unit sequentially controlling the third joint, the second joint, and the first joint according to the respective corresponding joint parameters comprises:
controlling the third joint to move until the joint parameter is equal to the j section track following
Figure FDA0003483435240000051
And specifies that when j-2n is ≦ 0,
Figure FDA0003483435240000052
then controlling the second joint to move until the joint parameter is equal to
Figure FDA0003483435240000053
Then controlling the first joint to move until the tail end of the continuum robot is overlapped with the target track; finally, the joint parameters at that time are recorded
Figure FDA0003483435240000054
8. The trajectory following motion planning system according to any one of claims 5-7, wherein the origin of the lumen-crossing coordinate system is located at the center of the entrance of the lumen, and the Z-axis direction thereof is inward of the plane of the vertical lumen entrance; the terminal coordinate system and the base coordinate system are both lagrangian coordinate systems following the continuum robot;
the end coordinate system { O1The origin of the robot is positioned at the center of the tail end of the robot, the Z-axis direction of the robot is superposed with the central axis of the robot, and the base in the initial reset state points to the tail end;
the base coordinate system { O2The origin of theAt the center of the robot base plate, the Z-axis direction coincides with the center axis of the robot and points to the end from the base in the initial reset state, the X-axis and the { O1The X axes of the coordinate systems are parallel and the positive directions are consistent.
CN202110810418.9A 2021-07-16 2021-07-16 Trajectory following motion planning method and system for continuum robot Active CN113510706B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110810418.9A CN113510706B (en) 2021-07-16 2021-07-16 Trajectory following motion planning method and system for continuum robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110810418.9A CN113510706B (en) 2021-07-16 2021-07-16 Trajectory following motion planning method and system for continuum robot

Publications (2)

Publication Number Publication Date
CN113510706A CN113510706A (en) 2021-10-19
CN113510706B true CN113510706B (en) 2022-04-01

Family

ID=78067472

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110810418.9A Active CN113510706B (en) 2021-07-16 2021-07-16 Trajectory following motion planning method and system for continuum robot

Country Status (1)

Country Link
CN (1) CN113510706B (en)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109434838A (en) * 2018-12-25 2019-03-08 北方工业大学 Coordinated motion planning method and system for endoscopic operation of line-driven continuous robot
CN109591020A (en) * 2019-01-21 2019-04-09 兰州大学 A kind of the distributed collaboration control method and device of the adaptive navigability optimization of multi-redundant mechanical arm
CN112402020A (en) * 2020-12-01 2021-02-26 北京天智航医疗科技股份有限公司 Control method and device for surgical robot mechanical arm, medium and equipment

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10737398B2 (en) * 2010-07-08 2020-08-11 Vanderbilt University Continuum devices and control methods thereof
US9364289B2 (en) * 2013-10-09 2016-06-14 Wisconsin Alumni Research Foundation Interleaved manipulator

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109434838A (en) * 2018-12-25 2019-03-08 北方工业大学 Coordinated motion planning method and system for endoscopic operation of line-driven continuous robot
CN109591020A (en) * 2019-01-21 2019-04-09 兰州大学 A kind of the distributed collaboration control method and device of the adaptive navigability optimization of multi-redundant mechanical arm
CN112402020A (en) * 2020-12-01 2021-02-26 北京天智航医疗科技股份有限公司 Control method and device for surgical robot mechanical arm, medium and equipment

Also Published As

Publication number Publication date
CN113510706A (en) 2021-10-19

Similar Documents

Publication Publication Date Title
CN111152224B (en) Dual-optimization robot motion trajectory optimization method
CN101612734B (en) Pipeline spraying robot and operation track planning method thereof
CN111300425B (en) Super-redundant mechanical arm tail end track motion planning method
CN107966907B (en) Obstacle avoidance solution applied to redundant manipulator
CN110682286B (en) Real-time obstacle avoidance method for cooperative robot
CN109397271B (en) 7-degree-of-freedom anthropomorphic mechanical arm and control method and system thereof
CN112757306A (en) Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm
CN113146600B (en) Flexible robot trajectory planning method and device based on kinematics iterative learning control
CN115605328A (en) Collision avoidance motion planning method for industrial robot
CN110561420B (en) Arm profile constraint flexible robot track planning method and device
CN112605996B (en) Model-free collision avoidance control method for redundant mechanical arm
CN106844951B (en) Method and system for solving inverse kinematics of super-redundant robot based on segmented geometric method
CN113552877A (en) Initial reference generation for robot-optimized action planning
CN107538493A (en) A kind of industrial robot method for planning track for avoiding cable interference constraint
CN110561419A (en) arm-shaped line constraint flexible robot track planning method and device
CN115416016A (en) Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method
Dong et al. An optimal curvature smoothing method and the associated real-time interpolation for the trajectory generation of flying robots
CN113510706B (en) Trajectory following motion planning method and system for continuum robot
US20170282370A1 (en) Position monitoring of a kinematic linkage
CN111123943A (en) Pseudo-inverse constraint-based super-redundant robot trajectory planning method and system
US11813756B2 (en) Disassembly based assembly planning
Su et al. Pythagorean-Hodograph curves-based trajectory planning for pick-and-place operation of Delta robot with prescribed pick and place heights
Chen et al. Optimizing the obstacle avoidance trajectory and positioning error of robotic manipulators using multigroup ant colony and quantum behaved particle swarm optimization algorithms
US20230302645A1 (en) Method of robot dynamic motion planning and control
CN116330267A (en) Control method based on industrial robot wrist singular point calculation

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant