Controlling method of continuum robot end position and posture, computer device and readable storage medium

Abstract

The application discloses a kind of continuum manipulator end pose control methods, comprising: the pose error between the current pose matrix and target pose matrix of continuum manipulator end effector is calculated;Determine whether the pose error satisfies preset condition, if not, according to the pose error, the pose correction parameter of end effector is obtained, and according to pose correction parameter, pre-set target control function and the velocity Jacobian matrix of end effector, the updated control law function is obtained, the pose error is input into the updated control law function to obtain the joint drive increment of continuum manipulator end effector, and the joint drive increment is used to drive continuum manipulator to displace;After that, the current joint amount and velocity Jacobian matrix are updated by joint drive increment, and then the current pose matrix of continuum manipulator end effector is obtained, and the step of calculating pose error is returned;The method realizes the accurate control of continuum manipulator end pose.

Description

Technical Field

[0001] This invention relates to the field of medical surgical robot technology, and more specifically, to a method for end-effector pose control of a continuous robotic arm, a computer device, and a readable storage medium. Background Technology

[0002] In recent years, robots have impacted human life in many ways. Beyond manufacturing, robots have entered fields such as agriculture and aerospace. Over the past decade, robots have gradually entered the medical field, frequently appearing in operating rooms around the world to assist in or complete various minimally invasive surgical procedures. This can reduce patient discomfort during and after surgery, as well as hospital stays. Robotics technology brings precision, speed, and stability to surgical procedures.

[0003] To improve the flexibility of robotic arms in minimally invasive surgery, researchers have proposed a novel medical robotic arm: the continuum surgical arm. Compared to traditional robotic arms composed of discrete rigid links connected by joints, the continuum robotic arm has a fundamentally different structure. It consists of a continuous torso, and its degrees of freedom increase with the number of its continuous segments. Therefore, the continuum robotic arm can possess a high degree of freedom, and each segment can be driven by actuators to produce bending changes, thereby altering the position and orientation of the end effector.

[0004] To meet the requirements of minimally invasive surgery, the pose control of the end effector of a continuous robotic arm must meet precision requirements. Currently, most continuous robotic arms employ a constant curvature kinematics model, assuming that the bending change of each segment of the continuous arm is a standard curved arc. For the control of a single segment of the continuous arm, we can directly control its end effector position by finding the analytical solution of its inverse kinematics.

[0005] However, for controlling a two-segment continuous arm with multiple degrees of freedom (e.g., 6 degrees of freedom), the inverse kinematics solution cannot be directly derived due to the nonlinearity of the forward kinematics. The original Jacobi iteration method and the Newton-Raphson iteration method can yield analytical solutions to the inverse kinematics, but these methods involve a large number of mathematical calculations and suffer from drawbacks such as high iteration counts, difficulty in convergence, and ultimately, no solution. Furthermore, to address this problem, some researchers have optimized the structure of the multi-segment continuous arm by adding intermediate continuous segments to achieve end-effector pose separation. Quaternion interpolation methods have also been used to solve the inverse kinematics solution for the end-effector pose of multi-segment continuous arms, but these methods have not achieved satisfactory control results. Summary of the Invention

[0006] To address the problem of motion control of the end effector position of a continuous manipulator, this invention discloses a method for end effector position control of a continuous manipulator. This method solves the problem that in existing two-segment continuous manipulators with multiple degrees of freedom, the nonlinear factors in control often prevent the obtaining of analytical solutions for the position motion through inverse kinematics, thus failing to achieve the required control accuracy.

[0007] A method for end-effector pose control of a continuous robotic arm, the continuous robotic arm comprising two continuous arm segments and a rigid straight rod, the control method comprising:

[0008] Step S1: Calculate the pose error between the current pose matrix of the end effector of the continuous robot arm and the target pose matrix of the input continuous robot arm end effector;

[0009] Step S2: Determine whether the pose error meets the preset conditions. If the preset conditions are met, input the target pose matrix of the target pose control point of the next continuous robot arm end effector as the target pose matrix of the continuous robot arm end effector, and return to step S1. If the preset conditions are not met, continue to step S3.

[0010] Step S3: Obtain the pose correction parameters of the end effector of the continuous robot arm according to the pose error, and obtain the updated control law function according to the pose correction parameters, the preset target control function and the velocity Jacobian matrix of the end effector of the continuous robot arm. The target control function is used to constrain the difference between the current pose and the target pose of the end effector to be minimized.

[0011] Step S4: Input the pose error into the updated control law function to obtain the joint drive increment of the end effector of the continuous robot arm, and use the joint drive increment to drive the continuous robot arm to perform displacement.

[0012] Step S5: Update the current joint quantity and velocity Jacobian matrix of the end effector of the continuous robot arm by incrementally updating the joint drive, thereby obtaining the current pose matrix of the end effector of the continuous robot arm, and return to step S1.

[0013] In some embodiments, prior to step S1, the control method further includes:

[0014] Establish a base coordinate system and a local coordinate system on the continuous manipulator to obtain the end effector pose matrix when the continuous manipulator bends.

[0015] In some embodiments, the steps of establishing a base coordinate system and a local coordinate system on the continuum robot arm are as follows:

[0016] A base coordinate system is established at the base of the continuous manipulator, and local coordinate systems are established at the end of the rigid straight rod and at the end face of each segment of the continuous manipulator.

[0017] In some embodiments, the velocity Jacobian matrix of the end effector of the continuous manipulator is derived from the pose matrix of the end effector when the continuous manipulator bends, so as to obtain the mapping relationship between the end effector pose change rate and the joint change rate of the continuous manipulator.

[0018] In some embodiments, determining whether the pose error meets a preset condition specifically includes:

[0019] Determine whether the second norm of the position error vector of the end effector of the continuous robot arm is less than a first preset value;

[0020] Determine whether the second norm of the attitude error vector of the end effector of the continuous robot arm is less than a second preset value.

[0021] In some of these embodiments, the continuous robotic arm is a two-segment continuous robotic arm with six degrees of freedom.

[0022] In some embodiments, the method is further included as a step of performing simulation experiments to verify the control method.

[0023] In some of these embodiments, the control method is verified through simulation experiments in the software MATLAB.

[0024] On the other hand, the present invention also discloses a computer device, the computer device comprising:

[0025] At least one processor; and,

[0026] A memory communicatively connected to the at least one processor; wherein,

[0027] The memory stores computer-readable instructions, and when the processor executes the computer-readable instructions, it implements the above-described continuous robotic arm end-effector pose control method.

[0028] In another aspect, the present invention also discloses a computer-readable storage medium storing computer-readable instructions, which, when executed by a processor, implement the above-described end-effector pose control method for a continuous body robotic arm.

[0029] Compared with the prior art, the present invention has at least one of the following advantages or beneficial effects:

[0030] This invention provides a method for end-effector pose control of a continuous manipulator that can obtain analytical motion solutions more quickly and achieve end-effector pose control and trajectory tracking tasks. By setting adaptive pose correction parameters, this control method can significantly reduce the actual number of iterations while keeping the number of iterations uniform, resulting in faster convergence of the end-effector pose and more stable parameter calculation. Thus, it can effectively solve the end-effector pose control problem of multi-degree-of-freedom continuous manipulators and provide a good control strategy for multi-segment continuous manipulators in the field of control. Attached Figure Description

[0031] The invention, its features, shape, and advantages will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings. Like reference numerals denote like parts throughout the drawings. The drawings are not drawn to scale; their focus is on illustrating the gist of the invention.

[0032] Figure 1 This is a flowchart of the end-effector pose control method of a continuum robot arm in an embodiment of the present invention;

[0033] Figure 2 This is an overall configuration diagram of the continuous robotic arm in an embodiment of the present invention;

[0034] Figure 3 A schematic diagram illustrating motion analysis of a single-segment continuous body arm in an embodiment of the present invention;

[0035] Figure 4 This is a simulation result verification diagram of the position tracking control error in the simulation experiment of this invention;

[0036] Figure 5 This is a simulation result verification diagram of the attitude tracking control error in the simulation experiment of this invention. Detailed Implementation

[0037] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but these are not intended to limit the scope of the invention.

[0038] like Figure 1 As shown, this invention discloses a method for end-effector pose control of a continuous robotic arm. The main body of the continuous robotic arm consists of two continuous arm segments and an inflexible rigid rod, possessing a total of 6 degrees of freedom, including linear propulsion, overall torsion, proximal bending of the continuous robotic arm, and distal bending. Specifically, the control method includes:

[0039] Step S1: Calculate the pose error between the current pose matrix of the end effector of the continuous robot arm and the target pose matrix of the input continuous robot arm end effector.

[0040] In an embodiment of the present invention, before performing the above step S1, the control method further includes: establishing a base coordinate system at the base of the continuous manipulator, and establishing local coordinate systems at the end of the rigid straight rod and the end face of each segment of the continuous manipulator to obtain the end effector pose matrix when the continuous manipulator bends, and then deriving the velocity Jacobian matrix of the end effector of the continuous manipulator based on the end effector pose matrix when the continuous manipulator bends, so as to obtain the mapping relationship between the end pose change rate and the joint change rate of the continuous manipulator.

[0041] Specifically, firstly, by Figure 2 It can be seen that the basic structure and basic motion mode of the continuous robot arm in this method are known. Figure 3 The joint configuration of a single-segment continuous body arm during bending is described. Assume the length of the single-segment continuous body arm is L, and it has 2 degrees of freedom. Its coordinate system and joint variables are defined in [the following text is missing from the original] Figure 3 This is represented in the figure. A base coordinate system O is established at the base of the continuous robotic arm, where Z... o The direction is the axial direction of the continuum arm, X o To point in the direction of the first drive line, Y o The direction follows the right-hand rule. Establish a coordinate system e at the end face of the single-segment continuous robot arm. When no bending occurs, coordinate systems O and e are parallel. When the continuous robot arm bends, we can assume that its bending is an ideal circular arc, i.e., its bending plane is parallel to the X-axis. o Z o Plane The angle, θ, represents the bending angle. During bending, the relationship between the base coordinate system O and the end face coordinate system l can be transformed as follows:

[0042] Base coordinate system O rotates about the Z-axis Angle, obtain coordinate system m;

[0043] The coordinate system m is translated along the z-axis and x-axis by Rsinθ and R(1-cosθ) respectively, and then rotated around its own y-axis by an angle θ to obtain the coordinate system n;

[0044] Coordinate system n rotates about its own z-axis The angle is used to obtain the coordinate system l of the end face.

[0045] From this, the pose transformation matrix of the end face coordinate system l relative to the base coordinate system O can be obtained. The linear velocity and angular velocity of the end face coordinate system l relative to the base coordinate system O are respectively , The Jacobian matrix of the end face coordinate system l relative to the base coordinate system O is: .

[0046]

[0047] in

[0048]

[0049]

[0050] Kinematic modeling based on a single-segment continuous robotic arm can be applied to multi-segment continuous robotic arms. For example... Figure 2 As shown, the first degree of freedom of the two-segment continuous manipulator is linear motion d, the second degree of freedom is the overall torsional angle α, and the third and fourth degrees of freedom are the proximal torsional angles of the continuous manipulator, respectively. The bending angle θ1, the fifth degree of freedom and the sixth degree of freedom are distributed as the distal torsional angle of the continuum manipulator. And the bending angle θ2. Where the proximal length and the distal length are both L.

[0051] The kinematic relationship of the overall continuum robot arm is shown below:

[0052]

[0053]

[0054]

[0055]

[0056]

[0057]

[0058] In this method, a control method based on the Jacobian matrix is ​​adopted. The derivation formula of the speed Jacobian matrix J of the overall end effector of the continuum robot is as follows:

[0059] J w1 =[0,0,0] T J w2 =[0,0,1] T

[0060]

[0061]

[0062]

[0063] J = [J v J w ];

[0064] This allows us to derive the velocity Jacobian matrix (including the linear velocity and angular velocity Jacobian matrix of the end effector) of the continuous manipulator based on the pose matrix of the end effector when the continuous manipulator bends, so as to obtain the mapping relationship between the end pose change rate and the joint change rate of the continuous manipulator.

[0065] Specifically, in actual control, the initial current pose matrix of the end effector of the continuous body robotic arm is set as follows: The input target pose matrix is ​​T d .

[0066] Furthermore T diff =T init -1 T d -I 4×4 , T diff It is obtained by synthesizing differential translation and differential rotation. It describes the pose error relationship between the current pose and the target pose, and the pose error e = [d] is obtained. x ,d y ,d z ,δ x ,δ y ,δ z ] T , where d x ,d y ,d z For position error, δ x ,δ y ,δ z This represents the attitude error.

[0067] Step S2: Determine whether the pose error meets the preset conditions. If the preset conditions are met, input the target pose matrix of the target pose control point of the next continuous robot arm end effector as the target pose matrix of the continuous robot arm end effector, and return to step S1 to re-determine the pose error. If the preset conditions are not met, continue to step S3.

[0068] Specifically, determining whether the pose error meets the preset conditions includes: determining whether the second norm value of the position error vector of the end effector of the continuous robot arm is less than the first preset value Lpmm; determining whether the second norm value of the posture error vector of the end effector of the continuous robot arm is less than the second preset value La radians. If both conditions are met, the target pose matrix of the next pose target point (i.e., the target pose matrix of the target pose control point of the next continuous robot arm end effector) is substituted into the matrix, and the pose error is re-determined.

[0069] Step S3: Obtain the pose correction parameters of the end effector of the continuous robot arm based on the pose error (i.e., determine the position correction parameters based on the position error value and the attitude correction parameters based on the attitude error value), and obtain the updated control law function based on the pose correction parameters, the pre-set target control function, and the velocity Jacobian matrix of the end effector of the continuous robot arm. The target control function is used to constrain the difference between the current pose and the target pose of the end effector to be minimized (the purpose of this target control function is to make the control error tend to 0 each time, the joint drive increment as small as possible, so that the motion is smoother).

[0070] Specifically, set the target control function as min||J(q)dq-Ke|| 2 +μ 2 ||dq|| 2 , where e is the pose error, dq is the joint drive increment, μ1,μ2 are the position correction parameters, and k1,k2 are the attitude correction parameters.

[0071]

[0072]

[0073] Set the Jacobian correction matrix M and the error correction matrix K.

[0074]

[0075] The target control function is then transformed, and the pose correction parameters are input into the target control function. Combined with the velocity Jacobian matrix of the end effector of the continuum robot, the updated control law function (i.e., a function of the joint drive law dq) is obtained:

[0076]

[0077]

[0078] dq = J T (q)[J(q)J T (q)+M] -1 Ke.

[0079] Step S4: Input the pose error into the updated control law function to obtain the joint drive increment dq of the end effector of the continuous manipulator, and use the joint drive increment dq to drive the continuous manipulator to move (i.e. drive the end effector of the continuous manipulator to move in the direction of reducing the pose error).

[0080] Step S5: Incrementally update the current joint quantities (which are the original joint quantities plus the joint drive quantities) and the velocity Jacobian matrix of the end effector of the continuous robot arm by incrementally updating the joint drive (i.e., update both the current joint quantities and the velocity Jacobian matrix of the end effector of the continuous robot arm), thereby obtaining the current pose matrix T of the end effector of the continuous robot arm. unit (i.e., through the positive kinematics model T4) 0 The current pose matrix T of the end effector unit (Update), and return to step S1.

[0081] Finally, the tracking task ends when the tracking of all target points on the target trajectory reaches a convergence state.

[0082] In a preferred embodiment of the present invention, the method further includes a step of performing simulation experiments to verify the control method in the software MATLAB.

[0083] Specifically, simulation was used to verify the method of this invention. The simulation experiment used MATLAB software, and the simulation data input was a trajectory consisting of a set of 40 target pose control points. In the simulation, the first preset value Lp was set to 0.3 mm, and the second preset value La (radians) was set to 0.05 rad. The simulation results are as follows: Figure 4 , 5 As shown, a comparison was made between the actual number of iterations with and without pose correction parameters. It can be seen that after adaptive pose error compensation, the actual number of iterations required for each target point to reach convergence is less, and the iterations are more uniform. Furthermore, under the same error settings, the number of iterations in this invention is significantly less than that of the Newton-Raphson iteration method. Therefore, the control method of this invention has faster calculation speed and better convergence in actual control.

[0084] In summary, the end-effector pose control method disclosed in this invention can obtain the analytical solution of motion more quickly than the previous control method based on the iterative solution of the Jacobian matrix to obtain the inverse kinematic solution. It realizes the pose control and trajectory tracking tasks of the continuous manipulator, and the feasibility of the method is verified by actual simulation.

[0085] On the other hand, the present invention also discloses a computer device comprising:

[0086] At least one processor; and a memory communicatively connected to the at least one processor; wherein,

[0087] The memory stores computer-readable instructions, and when the processor executes the computer-readable instructions, it implements the above-described continuous manipulator end-effector pose control method.

[0088] In another aspect, the present invention also discloses a computer-readable storage medium storing computer-readable instructions, which, when executed by a processor, implement the above-described end-effector pose control method for a continuous robotic arm.

[0089] Those skilled in the art should understand that variations can be implemented by combining existing technology with the above embodiments, which will not be elaborated here. Such variations do not affect the essence of the present invention, and will not be elaborated here either.

[0090] The preferred embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and the devices and structures not described in detail should be understood as being implemented in a conventional manner in the art. Any person skilled in the art can make many possible variations and modifications to the technical solutions of the present invention using the methods and techniques disclosed above, or modify them into equivalent embodiments with equivalent changes, without departing from the scope of the present invention. This does not affect the essential content of the present invention. Therefore, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the present invention's technical solutions still fall within the protection scope of the present invention.

Claims

1. A method for end-effector pose control of a continuous robotic arm, comprising two continuous arm segments and a rigid straight rod, characterized in that, The control method includes: Step S1: Calculate the pose error between the current pose matrix of the end effector of the continuous robot arm and the target pose matrix of the input continuous robot arm end effector; The pose error is obtained by synthesizing the current pose matrix with the differential translation and differential rotation of the target pose moment; Step S2: Determine whether the pose error meets the preset conditions. If the preset conditions are met, input the target pose matrix of the target pose control point of the next continuous robot arm end effector as the target pose matrix of the continuous robot arm end effector, and return to step S1. If the preset conditions are not met, continue to step S3. Step S3: Obtain the pose correction parameters of the end effector of the continuous robot arm based on the pose error, and obtain the updated control law function based on the pose correction parameters, the preset target control function and the velocity Jacobian matrix of the end effector of the continuous robot arm. The target control function is used to constrain the difference between the current pose and the target pose of the end effector of the continuous robot arm to be minimized. The pose correction parameters include position correction parameters and attitude correction parameters. The position correction parameters are determined based on the position error value, and the attitude correction parameters are determined based on the attitude error value. Specifically, the target control function is set as follows: ; in, The velocity Jacobian matrix; This refers to the pose error; is the joint drive increment; K is the error correction matrix; e is the pose error; μ is the attitude correction parameter; Set the Jacobian correction matrix Error correction matrix The target control function is transformed, and the pose correction parameters are input into the target control function. Combined with the velocity Jacobian matrix of the end effector of the continuum robot, the updated control law function is obtained. The updated control law function is: , Step S4: Input the pose error into the updated control law function to obtain the joint drive increment of the end effector of the continuous robot arm, and use the joint drive increment to drive the continuous robot arm to perform displacement. Step S5: Update the current joint parameters and velocity Jacobian matrix of the end effector of the continuous robot arm by incrementally updating the joint drive, and then obtain the current pose matrix of the end effector of the continuous robot arm through the forward kinematics model, and return to step S1.

2. The method for end-effector pose control of a continuous robotic arm as described in claim 1, characterized in that, Before performing step S1, the control method further includes: Establish a base coordinate system and a local coordinate system on the continuous manipulator to obtain the end effector pose matrix when the continuous manipulator bends.

3. The method for end-effector pose control of a continuous robotic arm as described in claim 2, characterized in that, The specific steps for establishing a base coordinate system and a local coordinate system on a continuum robot arm are as follows: A base coordinate system is established at the base of the continuous manipulator, and local coordinate systems are established at the end of the rigid straight rod and at the end face of each segment of the continuous manipulator.

4. The method for end-effector pose control of a continuous robotic arm as described in claim 2, characterized in that, Based on the pose matrix of the end effector during bending of the continuous manipulator, the velocity Jacobian matrix of the end effector of the continuous manipulator is derived to obtain the mapping relationship between the end pose change rate and the joint change rate of the continuous manipulator.

5. The method for end-effector pose control of a continuous robotic arm as described in claim 1, characterized in that, Determining whether the pose error meets the preset conditions specifically includes: Determine whether the second norm of the position error vector of the end effector of the continuous robot arm is less than a first preset value; Determine whether the second norm of the attitude error vector of the end effector of the continuous robot arm is less than a second preset value.

6. The method for end-effector pose control of a continuous robotic arm as described in claim 1, characterized in that, The continuous robotic arm is a two-segment continuous robotic arm with 6 degrees of freedom.

7. The method for end-effector pose control of a continuous robotic arm as described in claim 1, characterized in that, It also includes the step of verifying the control method through simulation experiments.

8. The method for end-effector pose control of a continuous robotic arm as described in claim 1, characterized in that, The control method was verified through simulation experiments in the software MATLAB.

9. A computer device, characterized in that: The computer device includes: At least one processor; and, A memory communicatively connected to the at least one processor; wherein, The memory stores computer-readable instructions, and when the processor executes the computer-readable instructions, it implements the end-effector pose control method of the continuous body robotic arm as described in any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-readable instructions, which, when executed by a processor, implement the end-effector pose control method of any one of claims 1 to 8.

Images