Continuum robot, control method of stable motion thereof, and computer storage medium

Abstract

The application discloses a kind of control method of continuum robot stable motion, real-time measurement robot continuum segment multiple point pose, obtain multiple point pose information, while real-time tracking is carried out to the target trajectory of continuum segment;Whether the actual pose of continuum mechanical arm end deviates from target trajectory is judged, if deviates, pose compensation control is carried out to continuum segment using the multiple point pose information measured in real time, otherwise, keep target trajectory tracking.It realizes the real-time adjustment of continuum mechanical arm end pose trajectory, simultaneously online correction normal curvature assumption model, improves the control method of the smoothness of continuum mechanical arm motion trajectory, improves the tracking accuracy of mechanical arm end trajectory and positioning accuracy reliability.Full closed loop control is used, which can ensure the positioning accuracy and repositioning accuracy of continuum mechanical arm end.To a certain extent, the control error caused by the normal curvature assumption model is reduced, and the accuracy of compensation feedback control is ensured.

Description

Technical Field

[0001] This invention belongs to the field of continuum robot control, specifically relating to a continuum robot and its stable motion control method, and a computer storage medium. Background Technology

[0002] With the increasingly widespread application of continuum robots in the medical field, the requirements for end-effector positioning accuracy, trajectory tracking capability, and smooth motion capability are constantly increasing. Continuum robots use a constant curvature kinematic model for motion control. Due to errors between the theoretical and practical models, the actual end-effector trajectory often deviates from the target trajectory and exhibits unsmooth motion. Furthermore, traditional simple feedback closed-loop control systems are not ideal for continuum robots, exhibiting motion control instability and end-effector jitter. Summary of the Invention

[0003] The technical problem to be solved by the present invention is to provide a continuous robot and a control method for its stable motion, as well as a computer storage medium, which solves the problem of unstable motion and jitter at the end of the robotic arm caused by the traditional constant curvature control in the prior art.

[0004] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0005] A control method for stable motion of a continuous robot involves real-time measurement of the poses of multiple points on a continuous segment of the robot to obtain multi-point pose information, while simultaneously tracking the target trajectory at the end of the continuous robot arm in real time. The method determines whether the actual pose of the end of the continuous robot arm deviates from the target trajectory. If it does, the control model of the robot arm is corrected using the real-time measured multi-point pose information, and then pose compensation control is performed on the continuous segment. Otherwise, the target trajectory tracking is maintained.

[0006] Determining whether the end-effector pose of the continuous robotic arm deviates from the set target trajectory includes the following steps:

[0007] Step 1: Acquire the actual pose of the end effector of the continuous robotic arm;

[0008] Step 2: Calculate the position deviation vector and attitude deviation vector between the actual pose and the target trajectory;

[0009] Step 3: If the magnitudes of the position deviation vector and the attitude deviation vector are greater than the maximum deviation value allowed by the parameter configuration of the continuous body manipulator system, then the multi-point pose information is used to correct the manipulator control model, thereby compensating the continuous body manipulator actuator, and then the control is performed according to the original trajectory.

[0010] The actual pose deviation vector is calculated using the following formula:

[0011] Δp=[x-x',y-y',z-z']

[0012] Δs=[δx-δx', δy-δy', δz-δz']

[0013] Where Δp is the position deviation vector, Δs is the attitude deviation vector, the target pose coordinates are Ps=[x,y,z,δx,δy,δz], and the actual pose coordinates are Ps'=[x',y',z',δx',δy',δz'].

[0014] The process for determining the actual pose deviation is as follows:

[0015] Set the upper limit of position deviation to Lp and the upper limit of attitude deviation to La; determine whether the magnitude of Δp is greater than the preset system position deviation value Lp. If it is, perform position compensation; otherwise, determine whether the magnitude of Δs is greater than the upper limit of attitude deviation La. If it is, perform attitude compensation; otherwise, continue with trajectory tracking.

[0016] The pose compensation process involves real-time correction of the radius of curvature of the constant curvature segment of the continuum.

[0017] Three pose measurement points are set on the continuous segment, respectively at the end point of the continuous segment, one-third of the length of the central skeleton, and two-thirds of the length of the central skeleton. The bending angle of the continuous segment corresponding to the three points is calculated, and the bending curvature variation coefficient m of the entire continuous segment is calculated based on the three bending angles. This coefficient is sent to the constant curvature assumption model of the actuator length and joint configuration, and the curvature radius of the constant curvature is corrected in real time and the actuator is compensated. The calculation formula of the bending curvature variation coefficient is as follows:

[0018]

[0019] Wherein, θ1 is the bending angle of the first measurement point relative to the base of the bending segment, θ2 is the bending angle of the second measurement point relative to the first measurement point, and θ3 is the bending angle of the third measurement point relative to the second measurement point.

[0020] The calculation process for the bending angle of the continuous segment is as follows:

[0021] First, the position and attitude information of three measurement points are obtained. Based on the position information, the actual torsion angle of the three measurement points when the continuous segment bends is calculated. Then, based on the attitude information, the bending angle of the continuous segment between each measurement point is calculated.

[0022] The torsion angle is the angle of deflection of the point projected onto the base coordinate system relative to the X-axis of the base coordinate system.

[0023] A continuum robot includes a continuum manipulator, and the method is applied to control the continuum manipulator.

[0024] A computer storage medium includes a plurality of computer instructions, which, when invoked, execute all or part of the steps of the method.

[0025] Compared with the prior art, the present invention has the following beneficial effects:

[0026] 1. This method realizes real-time adjustment of the end-effector pose trajectory of a continuous robotic arm, while simultaneously refining the normal curvature assumption model online, thereby improving the control method for smoothness of the continuous robotic arm's motion trajectory, and also improving the tracking accuracy and positioning reliability of the robotic arm's end-effector trajectory.

[0027] 2. This invention adopts a fully closed-loop control, which can ensure the positioning accuracy and repositioning accuracy of the end effector of the continuous robotic arm.

[0028] 3. This invention uses multi-point pose information measurement, which reduces the control error caused by the constant curvature assumption model to a certain extent, while ensuring the accuracy of compensation feedback control. Attached Figure Description

[0029] Figure 1 This is a flowchart of the control method of the present invention.

[0030] Figure 2 This is a block diagram of the control method of the present invention.

[0031] Figure 3 This is a flowchart for judging the deviation of the actual pose from the target trajectory in this invention.

[0032] Figure 4 This is a schematic diagram illustrating the actual torsion of a continuous segment in the multi-point pose measurement of this invention.

[0033] Figure 5 This is a schematic diagram illustrating the actual bending of a continuous segment in the multi-point pose measurement of this invention. Detailed Implementation

[0034] The structure and working process of the present invention will be further described below with reference to the accompanying drawings.

[0035] A control method for stable motion of a continuous robot involves real-time measurement of the pose of multiple points on a continuous segment of the robot to obtain multi-point pose information, and real-time tracking of the target trajectory of the continuous segment. The method determines whether the actual pose of the end effector of the continuous robot arm deviates from the target trajectory. If it deviates, pose compensation control is performed on the continuous segment using the real-time measured multi-point pose information; otherwise, target trajectory tracking is maintained.

[0036] Specific embodiments, such as Figures 1 to 5 As shown,

[0037] Figure 1 This is a flowchart of the method of the present invention. Figure 2 This is the control block diagram for this method, which describes the entire compensated motion control process;

[0038] Set the target tracking trajectory. Determine if the actual pose deviation exceeds the system's preset value. If so, use multi-point pose information for compensation control. Otherwise, continue the tracking task according to the target trajectory.

[0039] Figure 3 This is a flowchart for judging the deviation of the actual pose from the target trajectory in this invention.

[0040] Determining whether the end-effector pose of the continuous robotic arm deviates from the set target trajectory includes the following steps:

[0041] Step 1: Acquire the actual pose of the end effector of the continuous robotic arm;

[0042] Step 2: Calculate the position deviation vector and attitude deviation vector between the actual pose and the target trajectory;

[0043] Step 3: If the magnitudes of the position deviation vector and the attitude deviation vector are greater than the maximum deviation values ​​allowed by the parameter configuration of the continuous body manipulator system, then the multi-point pose information is used to compensate the continuous body manipulator actuator, and then the control is performed according to the original trajectory.

[0044] The actual pose deviation vector is calculated using the following formula:

[0045] Δp=[x-x',y-y',z-z']

[0046] Δs=[δx-δx', δy-δy', δz-δz']

[0047] Where Δp is the position deviation vector, Δs is the attitude deviation vector, the target pose coordinates are Ps=[x,y,z,δx,δy,δz], and the actual pose coordinates are Ps'=[x',y',z',δx',δy',δz'].

[0048] The process for determining the actual pose deviation is as follows:

[0049] Set the upper limit of position deviation to Lp and the upper limit of attitude deviation to La; determine whether the magnitude of Δp is greater than the preset system position deviation value Lp. If it is, perform position compensation; otherwise, determine whether the magnitude of Δs is greater than the upper limit of attitude deviation La. If it is, perform attitude compensation; otherwise, continue with trajectory tracking.

[0050] The pose compensation process involves real-time correction of the radius of curvature of the constant curvature segment of the continuum.

[0051] Compensation control is performed using multi-point pose information. This multi-point pose information includes the pose information of three points on the continuous segment. The three pose measurement points are respectively set at the end point of the continuous segment, at 1 / 3 of the length of the central skeleton of the continuous segment, and at 2 / 3 of the length of the central skeleton of the continuous segment.

[0052] Figure 4 This is a schematic diagram of the actual torsion of the continuous segment in the multi-point pose measurement in this method. Figure 5 The schematic diagram of the actual bending of the continuous segment in the multi-point pose measurement of this method mainly includes the following points:

[0053] 1. Obtain the position information of three measurement points, i.e., [x, y, z], and calculate the actual torsion angle of the three measurement points when the continuous segment bends. The average value of the actual torsion angles at the three measurement points is taken. That is, let it be the actual torsional angle of the curved continuum segment relative to the base coordinate of the segment.

[0054] 2. Using the attitude information of three measurement points, namely [δx, δy, δz], calculate the bending angles θ1, θ2, and θ3 of the continuous segment between each measurement point, as well as the bending angle θ of the entire continuous segment. Based on the relationship between θ1, θ2, and θ3, the curvature variation coefficient m of the entire continuous segment can be obtained.

[0055]

[0056] Wherein, θ1 is the bending angle of the first measurement point relative to the base of the bending segment, θ2 is the bending angle of the second measurement point relative to the first measurement point, and θ3 is the bending angle of the third measurement point relative to the second measurement point.

[0057] 3. The coefficient m is passed to the constant curvature assumption model of actuator length and joint configuration in kinematics, thereby correcting the curvature radius of constant curvature in real time. The end pose of the entire continuum segment obtained in the previous step is used as a feedback signal to the input to obtain the pose deviation for compensation control.

[0058] The calculation process for the bending angle of the continuous segment is as follows:

[0059] First, the position and attitude information of three measurement points are obtained. Based on the position information, the actual torsion angle of the three measurement points when the continuous segment bends is calculated. Then, based on the attitude information, the bending angle of the continuous segment between each measurement point is calculated.

[0060] The torsion angle is the angle of deflection of the point projected onto the base coordinate system relative to the X-axis of the base coordinate system.

[0061] A continuum robot includes a continuum manipulator, and the method is applied to control the continuum manipulator.

[0062] A computer storage medium includes a plurality of computer instructions, which, when invoked, execute all or part of the steps of the method.

[0063] Those skilled in the art should understand that variations can be implemented by combining existing technology and the above embodiments. Such variations do not affect the substantive content of this solution and will not be elaborated here.

[0064] It should be understood that this solution is not limited to the specific embodiments described above. Devices and structures not described in detail herein should be understood as being implemented in a manner common to the art. Any person skilled in the art can make many possible variations and modifications to this solution, or modify it into equivalent embodiments, without departing from the scope of this solution, using the methods and techniques disclosed above. This does not affect the substantive content of this solution. Therefore, any simple modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of this solution, without departing from its scope, still fall within the protection scope of this solution.

Claims

1. A control method for stable motion of a continuum robot, characterized in that: The robot's multi-point pose is measured in real time to acquire multi-point pose information, and the target trajectory of the end effector of the continuous robotic arm is tracked in real time. It is determined whether the actual pose of the end effector deviates from the target trajectory. If it does, the robotic arm control model is corrected using the real-time measured multi-point pose information, and pose compensation control is then performed on the continuous segment. Otherwise, target trajectory tracking is maintained. Three pose measurement points are set on the continuous segment: the end point, one-third of the length of the central skeleton, and two-thirds of the length of the central skeleton. The bending angles of the continuous segment corresponding to these three points are calculated, and the curvature change coefficient of the entire continuous segment is calculated based on these three bending angles. The coefficient is sent to the constant curvature assumption model of the actuator length and joint configuration, and the curvature radius of the constant curvature is corrected in real time and the actuator is compensated. The calculation formula of the bending curvature change coefficient is as follows: in, The first measurement point represents the bending angle relative to the base of the curved section. The curvature angle of the second measurement point relative to the first measurement point. The curvature angle of the third measurement point relative to the second measurement point; The calculation process for the bending angle of the continuous segment is as follows: First, the position and attitude information of three measurement points are obtained. Based on the position information, the actual torsion angle of the three measurement points when the continuous segment bends is calculated. Then, based on the attitude information, the bending angle of the continuous segment between each measurement point is calculated. The torsion angle is the deflection angle of the measurement point projected onto the base coordinate system relative to the X-axis of the base coordinate system.

2. The control method for stable motion of a continuum robot according to claim 1, characterized in that: Determining whether the end-effector pose of the continuous robotic arm deviates from the set target trajectory includes the following steps: Step 1: Acquire the actual pose of the end effector of the continuous robotic arm; Step 2: Calculate the position deviation vector and attitude deviation vector between the actual pose and the target trajectory; Step 3: If the magnitudes of the position deviation vector and the attitude deviation vector are greater than the maximum deviation value allowed by the parameter configuration of the continuous body manipulator system, then the multi-point pose information is used to correct the manipulator control model, thereby compensating the continuous body manipulator actuator, and then the control is performed according to the original trajectory.

3. The control method for stable motion of a continuum robot according to claim 2, characterized in that: The actual pose deviation vector is calculated using the following formula: in, This is the position deviation vector. Let be the attitude deviation vector, and let be the target pose coordinates. The actual pose coordinates are .

4. The control method for stable motion of a continuum robot according to claim 3, characterized in that: The process for determining the actual pose deviation is as follows: Set the upper limit of position deviation to [value]. The upper limit of attitude deviation is ;judge Is the modulus greater than the preset system position deviation value? If it is greater than 0, then position compensation is performed; otherwise, a judgment is made. Is the magnitude greater than the upper limit of the attitude deviation? If the value is greater than 0, then attitude compensation is performed; otherwise, trajectory tracking is performed.

5. The control method for stable motion of a continuum robot according to claim 1, characterized in that: The pose compensation process involves real-time correction of the radius of curvature of the constant curvature segment of the continuum.

6. A continuum robot, characterized in that: The invention includes a continuous robotic arm, which is controlled by any one of the methods of claims 1 to 5.

7. A computer storage medium, characterized in that: It includes several computer instructions that, when invoked, perform the steps of the method of any one of claims 1 to 5.

Images