Optimized control methods for continuum robots

Abstract

An optimization control method for a continuum robot is disclosed. This method involves establishing a forward kinematics model of the continuum robot using the exponential product formula; solving for the Jacobian pseudo-inverse matrix of the continuum robot based on the forward kinematics model and the current pose; calculating the iterative direction of the control increment within the iteration cycle using the Jacobian pseudo-inverse matrix and the error; simultaneously establishing an optimization function and implementing a particle swarm optimization (PSO) algorithm to optimize the control parameters; determining the optimal proportional gain of the control increment based on this optimization function and the direction of the control increment; solving for the control increment by combining the direction of the control increment and the optimal proportional gain; and controlling the motion of the continuum robot and obtaining pose feedback based on the control increment.

Description

Technical Field

[0001] This invention relates to the field of continuous robot control technology, and in particular to an optimized control method for a continuous robot. Background Technology

[0002] Continuum robots are snake-like robots with multiple flexible segments and a high degree of freedom. Their structural design is often inspired by creatures in nature, such as elephant trunks and snakes. These robots are characterized by their ability to achieve flexible movement in multiple directions and dimensions. Due to their unique structure and mobility, continuum robots have shown great application potential in fields such as aircraft engine maintenance, medical surgery, pipeline inspection, disaster relief, and deep-sea exploration, becoming a hot topic in the development of next-generation robots.

[0003] Compared to traditional rigid robots, continuum robots can enter narrow or hard-to-reach spaces to perform precise or complex tasks. For example, in the narrow and complex internal space of an aero-engine, conventional inspection techniques are often inadequate. Currently, inspections mainly rely on technicians manually operating borescopes. This method is not only inefficient, but also difficult to fully cover all critical areas inside the engine due to the limited range and accuracy of the borescope.

[0004] Continuum robots, with their high flexibility, degrees of freedom, and adaptability, offer new possibilities for solving this problem. They can actively control their shape, adapt to complex environments, and perform precise short-distance inspection and maintenance operations, while their excellent flexibility ensures operational safety. However, it is precisely this high flexibility and adaptability that makes continuum robots a system with significant nonlinear characteristics. Furthermore, continuum robots lack a defined joint structure, making their motion and force transmission more complex. These characteristics present significant challenges to the control of continuum robots.

[0005] The information disclosed in the background section is only for enhancing the understanding of the background of this invention, and therefore may contain information that does not constitute prior art known to those skilled in the art. Summary of the Invention

[0006] This invention provides an optimized control method for a continuum robot, solving problems such as low control accuracy, singularity, and safety in the current field of snake robot control. It achieves a dual guarantee of real-time performance and control accuracy, meeting multiple requirements such as control precision, high flexibility, and real-time performance, effectively overcoming the shortcomings and defects of existing technologies in the field of snake robot control.

[0007] An optimized control method for a continuum robot includes:

[0008] S1: Establish the forward kinematics model of the continuum robot using the exponential product formula;

[0009] S2: Solving the Jacobian pseudo-inverse matrix of a continuum robot based on the forward kinematics model and the current pose;

[0010] S3: Calculate the iterative direction of the control increment within this iteration cycle using the Jacobian pseudo-inverse matrix and the error;

[0011] S4: Simultaneously, an optimization function is established, and a particle swarm optimization (PSO) algorithm is built to optimize the control parameters;

[0012] S5: Based on this optimization function and the direction search of the control increment, determine the optimal proportional coefficient of the control increment;

[0013] S6: Solve for the control increment by combining the direction of the control increment and the optimal proportional coefficient;

[0014] S7: Control the motion of the continuum robot based on the control increment and obtain pose feedback.

[0015] In the aforementioned optimized control method for a continuum robot, in step S1, coordinate systems O0X0Y0Z0, O1X1Y1Z1, O2X2Y2Z2, and O3X3Y3Z3 are first established for the base, the end face of the first section, the end face of the second section, and the end face of the third section of the continuum robot, respectively; let T (n,m) T(α) represents the pose transformation matrix of an n-section, m-segment continuum robot, where each section of the continuum robot contains m segments of a continuum robotic arm; i ,β i T(θ) describes the pose transformation matrix of the i-th segment of the continuum robot; i The pose transformation matrix of the i-th bending degree of freedom of the continuum robot is described by ); then, based on the exponential product method, the pose transformation matrix T of the end effector of the continuum robot relative to the reference pose of the continuum robot is expressed as:

[0016] The above expression describes the forward kinematics model, where s represents the sine function, c represents the cosine function, and α... i β i Let α represent the bending angle of a continuous robot segment in two directions, L represent the distance between the two disks, H represent the thickness of the disks, and θ represent the bending angle of the continuous robot. i β i In this context, I is the identity matrix, ω is the unit vector along the helical axis, and v is the velocity vector at the end of the motion with a unit angular velocity around the helical axis.

[0017] In the optimization control method for a continuum robot, in step S2, the Jacobian matrix is ​​first solved using the definition of differential based on the bending angle of the current state, and then the pseudo-inverse method is used to solve the Jacobian pseudo-inverse matrix. In the expression, λ is the regularization coefficient, J i Let J be the i-th column of the Jacobian matrix, and F represent the forward kinematics model. The pseudo-inverse matrix representing the Jacobian matrix.

[0018] In the optimization control method for a continuum robot, in step S3, the iterative direction of the control increment in the next iteration cycle is calculated by multiplying the current Jacobian pseudo-inverse matrix with the slope as the guide and the difference between the desired pose and the feedback pose as the guide, to obtain the direction of the bending angle increment as follows: In the expression, ΔP is the difference between the actual pose and the desired pose, and ΔΦ is the increment of the bending angle vector.

[0019] In the optimization control method for a continuum robot, step S4 sets minimizing the error between the theoretical pose and the desired pose as the objective function. The particle swarm optimization algorithm is initialized with a random particle swarm, and then the optimal solution is found through iteration. In each iteration, the particles track two extreme values. i p b and i g b To update itself, after finding these two optimal values, the particle updates its velocity and position using the following formula: i+1 V=ω× i V+c1×r1×( i p b - i k)+c2×r2×( i g b - i k); i+1 k = i k+ i v; In the expression, ω, c1, and c2 are weighting coefficients, and r1 and r2 are random numbers between 0 and 1. i V and i k represents the velocity and pose of the particle in the i-th period. i p b and i g b These are the optimal position and the global optimal position of the particle in the i-th period, respectively.

[0020] In the optimization control method for a continuum robot, in step S5, the search for the optimal proportional coefficient of the control increment in the next iteration cycle is conducted within the joint angle constraint range under the optimization objective function. Two exit conditions are set: reaching the maximum search iteration cycle or the objective function being less than the allowable error. In the above formula, K is the scaling factor to be optimized, F represents the kinematic model, e is the pose error vector of the previous cycle, and K min and K max Φ represents the upper and lower limits of the value of K. min and Φ max It is the limit value of the joint angle.

[0021] In the optimization control method for a continuum robot, in step S6, the optimal angle increment ΔΦ is obtained by multiplying the angle vector increment ΔΦ and the proportional coefficient K: ΔΦ op =KΔΦ.

[0022] In the optimized control method for a continuum robot, in step S7, controlling the movement of the continuum robot and obtaining pose feedback involves sending the control quantity to the control board via serial communication to control the motor to rotate and reach the specified position. The obtaining of pose feedback is based on reading the coordinates of the end point of the continuum robot and the coordinates of the position 1 cm away from the end point from the port via a fiber optic grating sensor through TCP / IP communication. The current pose position is obtained from the end point coordinates, and the posture is obtained by subtracting the two coordinates.

[0023] In the aforementioned optimized control method for a continuum robot, the feedback system consists of a host computer, a fiber optic grating sensor, a sensor base, and a hollow sponge tube.

[0024] In the aforementioned optimized control method for a continuum robot, a fiber optic grating sensor passes through a hollow sponge tube and is placed at the center of the continuum robot to ensure the sensor's alignment.

[0025] Compared with the prior art, the present invention has the following advantages: The present invention utilizes fiber optic grating sensor feedback to realize the closed-loop feedback control process of the pose of the continuum robot, achieving high-precision real-time control; the method adopts the particle swarm optimization algorithm in the swarm intelligence optimization algorithm to adaptively adjust the magnitude of the control quantity, which can effectively overcome the large increment problem caused by singularity and ill-conditionedness, ensure the safety and stability of the system, and also improve the convergence speed of the system. Attached Figure Description

[0026] Various other advantages and benefits of the present invention will become apparent to those skilled in the art upon reading the detailed description of the preferred embodiments below. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. It is obvious that the drawings described below are merely some embodiments of the invention, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort. Furthermore, the same reference numerals denote the same parts throughout the drawings.

[0027] In the attached diagram:

[0028] Figure 1 A flowchart illustrating an intelligent optimization control method for a continuum robot, provided as an embodiment of this disclosure;

[0029] Figure 2 A schematic diagram of the process for obtaining pose feedback based on a fiber optic grating sensor, which is an embodiment of the present disclosure, for an intelligent optimization control method for a continuum robot.

[0030] Figure 3 A schematic diagram of the structure of a multi-segment continuous robot feedback system and the establishment of each coordinate system, which is provided as an embodiment of the intelligent optimization control method for a continuous robot according to this disclosure;

[0031] Figure 4 This is a schematic diagram of the bending degrees of freedom and pose acquisition points of each segment of a continuum robot, provided in an embodiment of the present disclosure, for an intelligent optimization control method for a continuum robot.

[0032] Figure 5 A schematic diagram of the structure of a single-segment continuous robot, which is provided as an embodiment of the present disclosure, for an intelligent optimization control method for a continuous robot.

[0033] Figure 6 A comparative diagram of circular trajectory experiments for an intelligent optimization control method for a continuum robot provided in one embodiment of this disclosure;

[0034] Figure 7 A square trajectory experimental comparison diagram of an intelligent optimization control method for a continuum robot provided in one embodiment of this disclosure;

[0035] Figure 8 A comparative diagram of triangular trajectories for an intelligent optimization control method for a continuum robot provided in one embodiment of this disclosure.

[0036] The present invention will be further explained below with reference to the accompanying drawings and embodiments. Detailed Implementation

[0037] Specific embodiments of the invention will now be described in more detail with reference to the accompanying drawings. While specific embodiments of the invention are shown in the drawings, it should be understood that the invention may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this invention will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.

[0038] It should be noted that certain terms are used in the specification and claims to refer to specific components. Those skilled in the art will understand that different terms may be used to refer to the same component. This specification and claims do not distinguish components based on differences in terminology, but rather on differences in function. The terms "comprising" or "including" used throughout the specification and claims are open-ended and should be interpreted as "comprising but not limited to." The following descriptions are preferred embodiments for carrying out the invention; however, these descriptions are for the purpose of understanding the general principles of the specification and are not intended to limit the scope of the invention. The scope of protection of this invention is determined by the appended claims.

[0039] To facilitate understanding of the embodiments of the present invention, further explanations and descriptions will be provided below with reference to the accompanying drawings and specific embodiments. The accompanying drawings do not constitute a limitation on the embodiments of the present invention.

[0040] like Figures 1 to 8 As shown, the optimized control method for a continuum robot includes the following steps:

[0041] S1: Establish the forward kinematics model of the continuum robot using the exponential product formula;

[0042] S2: Solving the Jacobian pseudo-inverse matrix of a continuum robot based on the forward kinematics model and the current pose;

[0043] S3: Calculate the iterative direction of the control increment within this iteration cycle using the Jacobian pseudo-inverse matrix and the error;

[0044] S4: Simultaneously, an optimization function is established, and a particle swarm optimization (PSO) algorithm is built to optimize the control parameters;

[0045] S5: Based on this optimization function and the direction search of the control increment, determine the optimal proportional coefficient of the control increment;

[0046] S6: Solve for the control increment by combining the direction of the control increment and the optimal proportional coefficient;

[0047] S7: Control the motion of the continuum robot based on the control increment and obtain pose feedback.

[0048] In a preferred embodiment of the optimized control method for a continuum robot, in step S1, coordinate systems O0X0Y0Z0, O1X1Y1Z1, O2X2Y2Z2, and O3X3Y3Z3 are first established for the base, the end of the first section, the end of the second section, and the end of the third section of the continuum robot, respectively; let T (n,m) T(α) represents the pose transformation matrix of an n-section, m-segment continuum robot, where each section of the continuum robot contains m segments of a continuum robotic arm;i ,β i T(θ) describes the pose transformation matrix of the i-th segment of the continuum robot; i The pose transformation matrix of the i-th bending degree of freedom of the continuum robot is described by ); then, based on the exponential product method, the pose transformation matrix T of the end effector of the continuum robot relative to the reference pose of the continuum robot is expressed as:

[0049]

[0050] The above expression describes the forward kinematics model, where s represents the sine function, c represents the cosine function, and α... i β i Let α represent the bending angle of a continuous robot segment in two directions, L represent the distance between the two disks, H represent the thickness of the disks, and θ represent the bending angle of the continuous robot. i β i In this context, I is the identity matrix, ω is the unit vector along the helical axis, and v is the velocity vector at the end of the motion with a unit angular velocity around the helical axis.

[0051] In a preferred embodiment of the optimization control method for a continuum robot, in step S2, the Jacobian matrix is ​​first solved using the definition of differential based on the bending angle of the current state, and then the pseudo-inverse method is used to solve the Jacobian pseudo-inverse matrix. In the expression, λ is the regularization coefficient, J i Let J be the i-th column of the Jacobian matrix, and F represent the forward kinematics model. The pseudo-inverse matrix representing the Jacobian matrix.

[0052] In a preferred embodiment of the optimization control method for a continuum robot, in step S3, the iterative direction of the control increment in the next iteration cycle is calculated by multiplying the current Jacobian pseudo-inverse matrix as the slope and the difference between the desired pose and the feedback pose as the guide, to obtain the direction of the bending angle increment as follows: In the expression, ΔP is the difference between the actual pose and the desired pose, and ΔΦ is the increment of the bending angle vector.

[0053] In a preferred embodiment of the optimization control method for a continuum robot, in step S4, minimizing the error between the theoretical pose and the desired pose is set as the objective function. The particle swarm optimization algorithm is initialized with a random particle swarm, and then the optimal solution is found through iteration. In each iteration, the particles track two extreme values. i p b and i g bTo update itself, after finding these two optimal values, the particle updates its velocity and position using the following formula: i+1 V=ω× i V+c1×r1×( i p b - i k)+c2×r2×( i g b - i k); i+1 k = i k+ i v; In the expression, ω, c1, and c2 are weighting coefficients, and r1 and r2 are random numbers between 0 and 1. i V and i k represents the velocity and pose of the particle in the i-th period. i p b and i g b These are the optimal position and the global optimal position of the particle in the i-th period, respectively.

[0054] In a preferred embodiment of the optimization control method for a continuum robot, in step S5, the search for the optimal proportional coefficient of the control increment in the next iteration cycle is conducted within the joint angle constraint range under the optimization objective function. Two exit conditions are set: reaching the maximum search iteration cycle or the objective function being less than the allowable error. In the above formula, K is the scaling factor to be optimized, F represents the kinematic model, e is the pose error vector of the previous cycle, and K min and K max Φ represents the upper and lower limits of the value of K. min and Φ max It is the limit value of the joint angle.

[0055] In a preferred embodiment of the optimization control method for a continuum robot, in step S6, the optimal angle increment ΔΦ is obtained by multiplying the angle vector increment ΔΦ and the proportional coefficient K: ΔΦ op =KΔΦ.

[0056] In a preferred embodiment of the optimized control method for a continuum robot, in step S7, controlling the movement of the continuum robot and obtaining pose feedback involves sending the control quantity to the control board via serial communication to control the motor to rotate and reach the specified position. The obtaining of pose feedback is based on reading the coordinates of the end point of the continuum robot and the coordinates of the position 1 cm away from the end point from the port via a fiber optic grating sensor through TCP / IP communication. The current pose position is obtained from the end point coordinates, and the posture is obtained by subtracting the two coordinates.

[0057] In a preferred embodiment of the optimized control method for a continuum robot, the feedback system consists of a host computer, a fiber optic grating sensor, a sensor base, and a hollow sponge tube.

[0058] In a preferred embodiment of the optimized control method for a continuum robot, a fiber optic grating sensor passes through a hollow sponge tube and is placed at the center of the continuum robot to ensure the centering of the sensor.

[0059] In one embodiment, the optimized control method for a continuum robot includes,

[0060] S1: Establish the forward kinematics model of the continuum robot using the exponential product formula.

[0061] As a preferred embodiment, the continuum robot in this solution is a multi-segment continuum robot, specifically a 3-segment, 6-DOF continuum robot, comprising 15 segments; the positive kinematics model of the continuum robot is established based on joint bending, such as... Figure 4 and 5 As shown, the degrees of freedom of the continuum robot are considered as bending in two directions; each section of the continuum robot is equipped with a drive wire, and each section is driven by four wires. The motor rotates to drive the screw to feed or retract, thereby stretching or contracting the drive wire.

[0062] The kinematic model established in this step is based on the exponential product formula, such as... Figure 3 As shown, coordinate systems O0X0Y0Z0, O1X1Y1Z1, O2X2Y2Z2, and O3X3Y3Z3 are established for the base, the end face of the first segment, the end face of the second segment, and the end face of the third segment of the continuum robot, respectively; let T (n,m) T(α) represents the pose transformation matrix of an n-section, m-segment continuum robot, where each section of the continuum robot contains m segments of a continuum robotic arm; i ,β i T(θ) describes the pose transformation matrix of the i-th segment of the continuum robot; i The pose transformation matrix of the i-th bending degree of freedom of the continuum robot is described by ); then, based on the exponential product method, the pose transformation matrix T of the end effector of the continuum robot relative to the reference pose of the continuum robot is expressed as:

[0063]

[0064] The above expression describes the forward kinematics model, where s represents the sine function, c represents the cosine function, and α... i β iLet α represent the bending angle of a continuous robot segment in two directions, L represent the distance between the two disks, H represent the thickness of the disks, and θ represent the bending angle of the continuous robot. i β i In this context, I is the identity matrix, ω is the unit vector along the helical axis, and v is the velocity vector at the end of the motion with a unit angular velocity around the helical axis.

[0065] S2: Solve for the Jacobian pseudo-inverse matrix of the continuum robot based on the current pose.

[0066] As a preferred embodiment, step S2, which involves solving the Jacobian pseudo-inverse matrix of the continuum robot based on the current pose, first solves the Jacobian matrix using the definition of differential based on the bending angle of the current state. Specifically, each bending angle is given a small increment, the pose transformation under this small increment is calculated, and then the pseudo-inverse method is used to solve the Jacobian pseudo-inverse matrix to overcome the singularity problem.

[0067] In the expression, λ is the regularization coefficient, J i Let J be the i-th column of the Jacobian matrix, and F represent the forward kinematics model. The pseudo-inverse matrix representing the Jacobian matrix.

[0068] S3: Calculate the iteration direction of the control increment in the next iteration cycle.

[0069] As a preferred embodiment, step S3, which calculates the iteration direction of the control increment in the next iteration cycle, multiplies the result by the difference between the desired pose and the feedback pose, with the current Jacobian pseudo-inverse matrix as the slope, to obtain the direction of the bending angle increment as follows: In the expression, ΔP is the difference between the actual pose and the desired pose, and ΔΦ is the increment of the bending angle vector.

[0070] S4: Establish a suitable optimization function and build the Particle Swarm Optimization (PSO) algorithm.

[0071] In a preferred embodiment, step S4 establishes a suitable optimization function. The particle swarm optimization (PSO) algorithm aims to minimize the error between the theoretical pose and the desired pose. The PSO algorithm is initialized with a random particle swarm and then finds the optimal solution through iteration. In each iteration, the particles track two extreme values. i p b and i g b To update itself, after finding these two optimal values, the particle updates its velocity and position using the following formula: i+1 V=ω× i V+c1×r1×( i p b- i k)+c2×r2×( i g b - i k);

[0072] i+1 k = i k+ i v; In the expression, ω, c1, and c2 are weighting coefficients, and r1 and r2 are random numbers between 0 and 1. i V and i k represents the velocity and pose of the particle in the i-th period. i p b and i g b These are the optimal position and the global optimal position of the particle in the i-th period, respectively.

[0073] S5: Search for the optimal proportional coefficient for controlling the increment in the next iteration cycle.

[0074] As a preferred embodiment, the optimal proportional coefficient of the control increment in the next iteration cycle in step S5 is searched within the joint angle constraint range under the optimization objective function. There are two exit conditions: reaching the maximum search iteration cycle or the objective function being less than the allowable error.

[0075] In the above formula, K is the scaling factor to be optimized, F represents the kinematic model, e is the pose error vector of the previous cycle, and K min and K max Φ represents the upper and lower limits of the value of K. min and Φ max It is the limit value of the joint angle.

[0076] S6: Combine the control direction and magnitude to solve for the control increment.

[0077] In a preferred embodiment, step S6, which combines the control direction and magnitude to solve for the control increment, obtains the optimal angle increment by multiplying the angle vector increment ΔΦ obtained in steps S3 and S5 with the proportional coefficient K: ΔΦ op =KΔΦ.

[0078] S7: Control the motion of the continuum robot and obtain pose feedback.

[0079] As a preferred embodiment, the feedback system consists of a host computer, a fiber optic grating sensor, a sensor base, and a hollow sponge tube. The fiber optic grating sensor passes through the hollow sponge tube and is placed at the center of the continuum robot to ensure the centering of the sensor.

[0080] Step S7, controlling the motion of the continuous robot and obtaining pose feedback, involves sending the control quantity to the control board via serial communication to control the motor rotation and reach the designated position; for example... Figure 2 As shown, the pose feedback is obtained by reading the coordinates of the end point of the continuum robot and the coordinates of the position 1 cm away from the end point from the port through the fiber optic grating sensor via TCP / IP communication. The current pose is obtained from the coordinates of the end point, and the attitude is obtained by the difference between the two coordinates. It is worth noting that the attitude representation in this scheme is different from the conventional method, and is represented by a unit vector of the end point orientation.

[0081] As a preferred embodiment, after obtaining the pose feedback, the L2 norm is calculated between it and the desired pose, and it is determined whether the error is less than the allowable value.

[0082] The following is a detailed explanation of the technical effects of the above method using a 3-section, 5-segment continuum robot as an example. The main geometric parameters of the continuum robot are as follows: L = 6mm; H = 6mm; n = 3; m = 5; total length La = 360mm; the parameters of the particle swarm optimization algorithm are as follows: Φ max =90°; Φ min = -90°; K max =2;K min =0; ω=0.6; c1=1.2; c2=1.2.

[0083] The control method proposed in this disclosure is analyzed using the following three trajectory tracking methods, as shown in the table below:

[0084] Table 1

[0085] trajectory Circular trajectory Square trajectory Triangle Locus average value parameter radius r = 30mm Side length l = 40mm Side length a = 40mm RMSE error 1.07mm 1.18mm 1.29mm 1.18mm accuracy 0.29％ 0.32％ 0.35％ 0.33％

[0086] Table 1 provides the parameters, root mean square error (RMSE), and accuracy for three desired trajectories: a circular trajectory with a radius of 30 mm, an RMSE of 1.07 mm, and an accuracy (error to total robot length) of 0.29%; a square trajectory with a side length of 40 mm, an RMSE of 1.18 mm, and an accuracy (error to total robot length) of 0.32%; and a triangular trajectory with a side length of 40 mm, an RMSE of 1.29 mm, and an accuracy (error to total robot length) of 0.35%. The average error and accuracy are 1.18 mm and 0.33%, respectively.

[0087] In this embodiment, the tracking errors of various trajectory control methods were compared. The maximum error was less than 3 mm, and the minimum error was less than 0.3 mm. The experimental results are shown in the figure below. Figures 6 to 8 As shown, the dashed line represents the desired trajectory, and the solid line represents the actual measured trajectory. It can be seen that the experimental results meet the control requirements of various trajectories very well, proving that the proposed control method can effectively improve the control accuracy.

[0088] Although embodiments of the present invention have been described above in conjunction with the accompanying drawings, the present invention is not limited to the specific embodiments and application fields described above. The specific embodiments described above are merely illustrative and instructive, and not restrictive. Those skilled in the art can make many other forms based on the guidance of this specification and without departing from the scope of protection of the claims of the present invention, and all of these are within the scope of protection of the present invention.

Claims

1. An optimized control method for a continuum robot, characterized in that, Includes the following steps: S1: Establish the forward kinematics model of the continuum robot using the exponential product formula; S2: Solving the Jacobian pseudo-inverse matrix of a continuum robot based on the forward kinematics model and the current pose; S3: Calculate the iterative direction of the control increment within this iteration cycle using the Jacobian pseudo-inverse matrix and the error; S4: Simultaneously, an optimization function is established, and a particle swarm optimization (PSO) algorithm is built to optimize the control parameters; S5: Based on this optimization function and the direction search of the control increment, determine the optimal proportional coefficient of the control increment; S6: Solve for the control increment by combining the direction of the control increment and the optimal proportional coefficient; S7: Control the motion of the continuum robot based on the control increment and obtain pose feedback.

2. The optimized control method for a continuum robot according to claim 1, characterized in that, In step S1, coordinate systems O0X0Y0Z0, O1X1Y1Z1, O2X2Y2Z2, and O3X3Y3Z3 are first established for the base, the first end segment, the second end segment, and the third end segment of the continuous robot, respectively; let... The pose transformation matrix represents an n-section m-segment continuum robot, where each section of the continuum robot contains m segments of a continuum robotic arm. Describe the pose transformation matrix of the i-th segment of the continuum robot; The pose transformation matrix describing the i-th bending degree of freedom of the continuum robot; then, based on the exponential product method, the pose transformation matrix T of the end effector of the continuum robot relative to the reference pose of the continuum robot is expressed as: ; ; ; ; ; The above expression describes the forward kinematics model, where s represents the sine function and c represents the cosine function. , Let L represent the angles at which a segment of the continuous robot bends in two directions, H represent the distance between the two disks, H represent the thickness of the disks, and θ represent the bending angle of the continuous robot. , one of the, It is the identity matrix. It is the unit vector along the spiral axis. It is the velocity vector at the end of the motion around the helical axis with a unit angular velocity.

3. The optimized control method for a continuum robot according to claim 1, characterized in that, In step S2, the Jacobian matrix is ​​first solved using the definition of differential based on the bending angle of the current state, and then the pseudo-inverse method is used to solve the Jacobian pseudo-inverse matrix. ; ; In the expression, λ is the regularization coefficient. Jacobian matrix The i-th column, F represents the forward kinematics model, The pseudo-inverse matrix representing the Jacobian matrix.

4. The optimized control method for a continuum robot according to claim 3, characterized in that, In step S3, the iterative direction of the control increment in the next iteration cycle is calculated by multiplying the current Jacobian pseudo-inverse matrix with the slope as the guide and the difference between the desired pose and the feedback pose as the guide, resulting in the following direction of the bending angle increment: In the expression The difference between the actual pose and the desired pose. This is the increment of the bending angle vector.

5. The optimized control method for a continuum robot according to claim 1, characterized in that, In step S4, the objective function is to minimize the error between the theoretical pose and the desired pose. The particle swarm optimization algorithm is initialized with a random particle swarm, and then the optimal solution is found through iteration. In each iteration, the particles track two extreme values. and To update itself, after finding these two optimal values, the particle updates its velocity and position using the following formula: ; In the expression , and These are the weighting coefficients. and A random number between 0 and 1 and Let be the velocity and pose of the particle in the i-th period. and These are the optimal position and the global optimal position of the particle in the i-th period, respectively.

6. The optimized control method for a continuum robot according to claim 4, characterized in that, In step S5, the optimal proportional coefficient for controlling the increment in the next iteration cycle is searched within the joint angle constraint range under the optimization objective function. Two exit conditions are set: reaching the maximum search iteration cycle or the objective function being less than the allowable error. In the above formula, K is the scaling factor to be optimized, F represents the kinematic model, and e is the pose error vector of the previous cycle. and These are the upper and lower limits of the K value. and It is the limit value of the joint angle.

7. The optimized control method for a continuum robot according to claim 6, characterized in that, In step S6, the increment of the bending angle vector Multiplying by the scaling factor K yields the optimal angle increment: .

8. The optimized control method for a continuum robot according to claim 1, characterized in that, In step S7, controlling the movement of the continuous robot and obtaining pose feedback involves sending the control quantity to the control board via serial communication to control the motor to rotate and reach the specified position. The obtaining of pose feedback is based on the fiber optic grating sensor reading the coordinates of the end point of the continuous robot and the coordinates of the position 1 cm away from the end point from the port via TCP / IP communication. The current pose position is obtained from the end point coordinates, and the posture is obtained by subtracting the two coordinates.

9. The optimized control method for a continuum robot according to claim 8, characterized in that, The feedback system consists of a host computer, a fiber optic grating sensor, a sensor base, and a hollow sponge tube.

10. The optimized control method for a continuum robot according to claim 9, characterized in that, The fiber optic grating sensor passes through a hollow sponge tube and is placed at the center of the continuum robot to ensure the sensor's alignment.

Images