Optimization control method of continuum robot
By establishing a positive kinematic model and optimizing control parameters using particle swarm algorithms, and combining fiber grating sensors to realize closed-loop feedback control of continuum robots, the problems of low control accuracy and singularity are solved, and the stability and safety of the system are improved. Meets high flexibility and real-time requirements.
Patent Information
- Application Number
- CN202411048287.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-01
- Publication Date
- 2024-12-06
- Estimated Expiration
- 2044-08-01
AI Technical Summary
Continuous robots have low control accuracy, singularity and safety issues, making it difficult to meet the requirements of high flexibility and real-time.
The positive kinematic model is established using the exponential product formula, solve the Jacobian pseudo-inverse matrix, optimize the control parameters using the particle swarm algorithm, and combine it with the fiber grating sensor to realize closed-loop feedback control of position, overcome singularity and pathology, and ensure the stability and safety of the system. sex.
It realizes high-precision and real-time control, improves the control accuracy and security of the system, meets multiple requirements, and effectively makes up for the shortcomings of the existing technology.
Smart Images

Figure CN119077725A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of continuum robot control, and in particular to an optimization control method for a continuum robot. Background Art
[0002] A continuum robot is a snake-like robot with multiple flexible segments and a high degree of freedom. Its structural design is usually inspired by creatures in nature, such as elephant trunks and snakes. The characteristic of this type of robot is that it can achieve flexible movement in multiple directions and dimensions. Due to its unique structure and movement ability, continuum robots have shown great application potential in the fields of aircraft engine maintenance, medical surgery, pipeline inspection, disaster relief, deep-sea exploration, etc., and have become a hot spot for the development of a new generation of robots.
[0003] Compared with traditional rigid robots, continuum robots can enter narrow or hard-to-reach spaces and perform precise or complex tasks. For example, in the narrow and complex internal space of an aircraft engine, conventional detection technology is often unable to cope with it. At present, it mainly relies on technicians to manually operate the borescope for detection. This method is not only limited in efficiency, but also difficult to fully cover all key 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 flexibility, provide 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. At the same time, their good flexibility ensures the safety of work. However, it is this high degree of flexibility and flexibility that makes the continuum robot a system with significant nonlinear characteristics. In addition, the continuum robot lacks a clear joint structure, and its movement and force transmission are more complicated. These characteristics bring significant challenges to the control of continuum robots.
[0005] The above information disclosed in this Background section is only for enhancement of understanding of the background of the invention and therefore it may contain information that does not form the prior art that is already known to a person of ordinary skill in the art. Summary of the invention
[0006] The present invention provides an optimization control method for a continuum robot, which solves the problems of low control accuracy, singularity and safety in the current field of snake-like robot control. It achieves dual guarantees of real-time performance and control accuracy, can meet multiple requirements such as control accuracy, high flexibility, and real-time performance, and effectively makes up for the deficiencies and defects of the existing technology in the field of snake-like robot control.
[0007] An optimization control method for a continuum robot includes:
[0008] S1: Use the exponential product formula to establish the forward kinematic model of the continuum robot;
[0009] S2: Solve the Jacobian pseudo-inverse matrix of the continuum robot based on the forward kinematics model and the current posture;
[0010] S3: Calculate the iteration direction of the control increment in this iteration cycle using the Jacobian pseudo-inverse matrix and the error;
[0011] S4: At the same time, an optimization function is set up to establish a particle swarm algorithm PSO to optimize control parameters;
[0012] S5: Determine the optimal proportional coefficient of the control increment based on the optimization function and the direction search of the control increment;
[0013] S6: Combine the direction of the control increment and the optimal proportional coefficient to solve the control increment;
[0014] S7: Control the motion of the continuum robot according to the control increment and obtain position feedback.
[0015] In the optimization control method of a continuum robot, in step S1, firstly, coordinate systems O0X0Y0Z0, O1X1Y1Z1, O2X2Y2Z2 and O3X3Y3Z3 are established for the base of the continuum robot, the end of the first section of the continuum robot, the end of the second section of the continuum robot and the end of the third section of the continuum robot respectively; let T (n,m) represents the posture transformation matrix of an n-section m-segment continuum robot, where each continuum robot contains m-segment continuum manipulators; T(α i ,β i ) describes the posture transformation matrix of the i-th segment continuum robot; T(θ i ) describes the posture transformation matrix of the i-th bending degree of freedom of the continuum robot; based on the exponential product method, the posture transformation matrix T of the continuum robot end effector relative to the continuum robot reference posture 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 They represent the bending angles of a continuum robot in two directions, L represents the distance between the two disks, H represents the thickness of the disk, θ represents the bending angle of the continuum robot, and α i , β i where I is the identity matrix, ω is the unit vector in the direction of the screw axis, and v is the velocity vector of the terminal under unit angular velocity motion around the screw axis.
[0017] In the optimization control method of a continuum robot, in step S2, the Jacobian matrix is first solved by using the definition of differential according to the bending angle of the current state, and then the Jacobian pseudo-inverse matrix is solved by using the pseudo-inverse method; In the expression, λ is the regularization coefficient, J i is the i-th column of the Jacobian matrix J, F represents the forward kinematics model, Represents the pseudoinverse of the Jacobian matrix.
[0018] In the optimization control method of a continuum robot, in step S3, the iteration direction of the control increment in the next iteration cycle is calculated by taking the current Jacobian pseudo-inverse matrix as the slope and multiplying the difference between the desired posture and the feedback posture as the guide, and the direction of the bending angle increment is obtained as follows: In the expression, ΔP is the difference between the actual posture and the desired posture, and ΔΦ is the increment of the bending angle vector.
[0019] In the optimization control method of a continuum robot, in step S4, the error between the next theoretical posture and the expected posture is minimized as the objective function, the particle swarm algorithm is initialized as 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 speed 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 weight coefficients, r1 and r2 are random numbers between 0 and 1, i V and i k is the velocity and position of the particle in the ith period, i p b and i g b are the optimal position and global optimal position of the particle in the i-th period respectively.
[0020] In the optimization control method of a continuum robot, in step S5, the search for the optimal proportional coefficient of the control increment in the next iteration cycle is performed within the joint angle constraint range under the optimization objective function, wherein two exit conditions are set, and the maximum search iteration cycle is reached or the objective function is less than the allowable error; In the above formula, K is the proportional coefficient to be optimized, F represents the kinematic model, e is the posture error vector of the previous cycle, and K min and K max are the upper and lower limits of K value, Φ min and Φ max is the limit value of the joint angle.
[0021] In the optimization control method of a continuum robot, in step S6, the angle vector increment ΔΦ and the proportional coefficient K are multiplied to obtain the optimal angle increment: ΔΦ op =KΔΦ.
[0022] In the optimization control method of a continuum robot, in step S7, controlling the movement of the continuum robot and obtaining posture feedback is to send the control amount to the control board through serial port communication to control the motor to rotate and reach the specified position; the obtaining of posture feedback is based on the fiber grating sensor 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 TCP / IP communication, wherein the position of the current posture is obtained by the coordinates of the end point, and the posture is obtained by subtracting the two coordinates.
[0023] In the optimization control method of a continuum robot, the feedback system is composed of a host computer, a fiber grating sensor, a sensor base and a sponge hollow circular tube.
[0024] In the optimization control method of a continuum robot, a fiber grating sensor passes through a hollow circular sponge tube and is placed at the center of the continuum robot to ensure the centering of the sensor.
[0025] Compared with the prior art, the present invention has the following advantages: the present invention utilizes fiber Bragg grating sensor feedback to realize the closed-loop feedback control process of the continuum robot posture, thereby realizing high-precision real-time control; the method adopts the particle swarm algorithm in the swarm intelligence optimization algorithm to adaptively control the size of the control quantity, which can effectively overcome the large increment problem caused by singularity and pathological nature, ensure the safety and stability of the system, and also improve the system convergence speed. BRIEF DESCRIPTION OF THE DRAWINGS
[0026] By reading the detailed description of the preferred specific embodiments below, various other advantages and benefits of the present invention will become clear to those of ordinary skill in the art. The drawings in the specification are only for the purpose of illustrating the preferred embodiments and are not considered to be limitations of the present invention. Obviously, the drawings described below are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can also be obtained based on these drawings without creative work. Moreover, the same reference numerals are used to represent the same components throughout the drawings.
[0027] In the attached picture:
[0028] Figure 1 A flow chart of an intelligent optimization control method for a continuum robot provided by one embodiment of the present disclosure;
[0029] Figure 2 A schematic diagram of a process of obtaining posture feedback based on a fiber grating sensor in an intelligent optimization control method of a continuum robot provided by an embodiment of the present disclosure;
[0030] Figure 3 A schematic diagram of a structure of a multi-segment continuum robot feedback system and establishment of various coordinate systems for an intelligent optimization control method of a continuum robot provided by an embodiment of the present disclosure;
[0031] Figure 4 A schematic diagram of the bending degrees of freedom and posture acquisition points of each segment of a continuum robot according to an intelligent optimization control method of a continuum robot provided by an embodiment of the present disclosure;
[0032] Figure 5 A schematic structural diagram of a single-segment continuum robot according to an intelligent optimization control method of a continuum robot provided by an embodiment of the present disclosure;
[0033] Figure 6 A comparison diagram of a circular trajectory experiment of an intelligent optimization control method for a continuum robot provided by an embodiment of the present disclosure;
[0034] Figure 7 A comparison diagram of a square trajectory experiment of an intelligent optimization control method for a continuum robot provided by an embodiment of the present disclosure;
[0035] Figure 8 A comparison diagram of a triangle trajectory experiment of an intelligent optimization control method for a continuum robot provided by an embodiment of the present disclosure.
[0036] The present invention is further explained below in conjunction with the accompanying drawings and embodiments. DETAILED DESCRIPTION
[0037] The specific embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although the specific embodiments of the present invention are shown in the accompanying drawings, it should be understood that the present invention can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided in order to enable a more thorough understanding of the present invention and to fully convey the scope of the present invention to those skilled in the art.
[0038] It should be noted that certain words are used in the specification and claims to refer to specific components. Those skilled in the art should understand that technicians may use different nouns to refer to the same component. This specification and claims do not use the difference in nouns as a way to distinguish components, but use the difference in the functions of the components as the criterion for distinction. As mentioned throughout the specification and claims, "including" or "comprising" is an open term, so it should be interpreted as "including but not limited to". The subsequent description of the specification is a preferred embodiment of the present invention, but the description is based on the general principles of the specification and is not intended to limit the scope of the present invention. The scope of protection of the present invention shall be determined by the attached claims.
[0039] To facilitate understanding of the embodiments of the present invention, further explanation will be given below by taking specific embodiments as examples in conjunction with the accompanying drawings, and each of the accompanying drawings does not constitute a limitation on the embodiments of the present invention.
[0040] like Figures 1 to 8 As shown, the optimization control method of the continuum robot includes the following steps:
[0041] S1: Use the exponential product formula to establish the forward kinematic model of the continuum robot;
[0042] S2: Solve the Jacobian pseudo-inverse matrix of the continuum robot based on the forward kinematics model and the current posture;
[0043] S3: Calculate the iteration direction of the control increment in this iteration cycle using the Jacobian pseudo-inverse matrix and the error;
[0044] S4: At the same time, an optimization function is set up to establish a particle swarm algorithm PSO to optimize control parameters;
[0045] S5: Determine the optimal proportional coefficient of the control increment based on the optimization function and the direction search of the control increment;
[0046] S6: Combine the direction of the control increment and the optimal proportional coefficient to solve the control increment;
[0047] S7: Control the motion of the continuum robot according to the control increment and obtain position feedback.
[0048] In a preferred embodiment of the optimization control method of a continuum robot, in step S1, firstly, coordinate systems O0X0Y0Z0, O1X1Y1Z1, O2X2Y2Z2 and O3X3Y3Z3 are established for the base of the continuum robot, the end of the first section of the continuum robot, the end of the second section of the continuum robot and the end of the third section of the continuum robot respectively; let T (n,m) represents the posture transformation matrix of an n-section m-segment continuum robot, where each continuum robot contains m-segment continuum manipulators; T(αi ,β i ) describes the posture transformation matrix of the i-th segment continuum robot; T(θ i ) describes the posture transformation matrix of the i-th bending degree of freedom of the continuum robot; based on the exponential product method, the posture transformation matrix T of the continuum robot end effector relative to the continuum robot reference posture 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 They represent the bending angles of a continuum robot in two directions, L represents the distance between the two disks, H represents the thickness of the disk, θ represents the bending angle of the continuum robot, and α i , β i where I is the identity matrix, ω is the unit vector in the direction of the screw axis, and v is the velocity vector of the terminal under unit angular velocity motion around the screw axis.
[0051] In a preferred embodiment of the optimization control method of a continuum robot, in step S2, the Jacobian matrix is first solved using the definition of differential according to the bending angle of the current state, and then the Jacobian pseudo-inverse matrix is solved using the pseudo-inverse method; In the expression, λ is the regularization coefficient, J i is the i-th column of the Jacobian matrix J, F represents the forward kinematics model, Represents the pseudoinverse of the Jacobian matrix.
[0052] In a preferred embodiment of the optimization control method of a continuum robot, in step S3, the iterative direction of calculating the control increment in the next iteration cycle is to multiply the difference between the desired posture and the feedback posture with the current Jacobian pseudo-inverse matrix as the slope, and the direction of the bending angle increment is obtained as follows: In the expression, ΔP is the difference between the actual posture and the desired posture, and ΔΦ is the increment of the bending angle vector.
[0053] In a preferred embodiment of the optimization control method of a continuum robot, in step S4, the error between the next theoretical posture and the expected posture is minimized as the objective function, the particle swarm algorithm is initialized as 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 speed 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 weight coefficients, r1 and r2 are random numbers between 0 and 1, i V and i k is the velocity and position of the particle in the ith period, i p b and i g b are the optimal position and global optimal position of the particle in the i-th period respectively.
[0054] In a preferred embodiment of the optimization control method of a continuum robot, in step S5, the search for the optimal proportional coefficient of the control increment in the next iteration cycle is performed within the joint angle constraint range under the optimization objective function, wherein two exit conditions are set, and the maximum search iteration cycle is reached or the objective function is less than the allowable error; In the above formula, K is the proportional coefficient to be optimized, F represents the kinematic model, e is the posture error vector of the previous cycle, and K min and K max are the upper and lower limits of K value, Φ min and Φ max is the limit value of the joint angle.
[0055] In a preferred embodiment of the optimization control method of a continuum robot, in step S6, the angle vector increment ΔΦ and the proportional coefficient K are multiplied to obtain the optimal angle increment: ΔΦ op =KΔΦ.
[0056] In a preferred embodiment of the optimization control method for a continuum robot, in step S7, controlling the movement of the continuum robot and obtaining posture feedback is to send the control quantity to the control board through serial port communication to control the motor to rotate and reach the specified position; the obtaining of posture feedback is based on the fiber grating sensor 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 TCP / IP communication, wherein the position of the current posture is obtained by the coordinates of the end point, and the posture is obtained by subtracting the two coordinates.
[0057] In a preferred implementation of the optimization control method for a continuum robot, the feedback system is composed of a host computer, a fiber grating sensor, a sensor base and a sponge hollow circular tube.
[0058] In a preferred embodiment of the optimization control method of a continuum robot, the fiber grating sensor passes through a hollow circular sponge tube and is placed at the center of the continuum robot to ensure the centering of the sensor.
[0059] In one embodiment, the optimization control method of a continuum robot includes:
[0060] S1: Use the exponential product formula to establish the forward kinematic model of the continuum robot.
[0061] As a preferred solution of this embodiment, the continuum robot of this solution is a multi-segment continuum robot, specifically, a 3-segment 6-DOF continuum robot, with a total of 15 segments; the forward kinematic 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 regarded as bending in two directions; driving wires are respectively arranged in each section of the continuum robot, and each section is driven by four wires. The motor rotates to drive the lead screw to advance or retreat so that the driving wires are stretched or contracted.
[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 of the continuum robot, the end of the first section of the continuum robot, the end of the second section of the continuum robot and the end of the third section of the continuum robot respectively; let T (n,m) represents the posture transformation matrix of an n-section m-segment continuum robot, where each continuum robot contains m-segment continuum manipulators; T(α i ,β i ) describes the posture transformation matrix of the i-th segment continuum robot; T(θ i ) describes the posture transformation matrix of the i-th bending degree of freedom of the continuum robot; based on the exponential product method, the posture transformation matrix T of the continuum robot end effector relative to the continuum robot reference posture 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 , β iThey represent the bending angles of a continuum robot in two directions, L represents the distance between the two disks, H represents the thickness of the disk, θ represents the bending angle of the continuum robot, and α i , β i where I is the identity matrix, ω is the unit vector in the direction of the screw axis, and v is the velocity vector of the terminal under unit angular velocity motion around the screw axis.
[0065] S2: Solve the Jacobian pseudo-inverse matrix of the continuum robot based on the current posture.
[0066] As a preferred solution of this embodiment, the step S2 of solving the Jacobian pseudo-inverse matrix of the continuum robot according to the current posture is to first solve the Jacobian matrix according to the definition of differential according to the bending angle of the current state, and specifically, give each bending angle a small increment, calculate the transformation of the posture under this small increment, and then use the pseudo-inverse method to solve the Jacobian pseudo-inverse matrix to overcome the singularity problem;
[0067] In the expression, λ is the regularization coefficient, J i is the i-th column of the Jacobian matrix J, F represents the forward kinematics model, Represents the pseudoinverse of the Jacobian matrix.
[0068] S3: Calculate the iteration direction of the control increment in the next iteration cycle.
[0069] As a preferred solution of this embodiment, the iteration direction of calculating the control increment in the next iteration cycle in step S3 is to multiply the difference between the desired posture and the feedback posture with the current Jacobian pseudo-inverse matrix as the slope, and the direction of the bending angle increment is as follows: In the expression, ΔP is the difference between the actual posture and the desired posture, and ΔΦ is the increment of the bending angle vector.
[0070] S4: Set up a suitable optimization function and establish a particle swarm algorithm PSO.
[0071] As a preferred solution of this embodiment, in step S4, a suitable optimization function is set up to establish a particle swarm algorithm PSO, which minimizes the error between the next theoretical posture and the expected posture as the objective function. The particle swarm algorithm is initialized as 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 speed 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 weight coefficients, r1 and r2 are random numbers between 0 and 1, i V and i k is the velocity and position of the particle in the ith period, i p b and i g b are the optimal position and global optimal position of the particle in the i-th period respectively.
[0073] S5: Search for the optimal proportional coefficient of the control increment in the next iteration cycle.
[0074] As a preferred solution of this embodiment, the search for the optimal proportional coefficient of the control increment in the next iteration cycle in step S5 is performed within the joint angle constraint range under the optimization objective function, wherein two exit conditions are set, namely, reaching the maximum search iteration cycle or the objective function is less than the allowable error;
[0075] In the above formula, K is the proportional coefficient to be optimized, F represents the kinematic model, e is the posture error vector of the previous cycle, and K min and K max are the upper and lower limits of K value, Φ min and Φ max is the limit value of the joint angle.
[0076] S6: Combine the control direction and size to solve the control increment.
[0077] As a preferred solution of this embodiment, the control increment in step S6 is solved by combining the control direction and size, and the optimal angle increment is obtained by multiplying the angle vector increment ΔΦ obtained in step S3 and step S5 by the proportional coefficient K: ΔΦ op =KΔΦ.
[0078] S7: Control the motion of the continuum robot and obtain position feedback.
[0079] As a preferred solution of this embodiment, the feedback system consists of a host computer, a fiber grating sensor, a sensor base and a sponge hollow circular tube, wherein the fiber grating sensor passes through the sponge hollow circular tube and is placed at the center of the continuum robot to ensure the centration of the sensor.
[0080] The step S7 controls the motion of the continuum robot and obtains the posture feedback by sending the control quantity to the control board through the serial communication to control the motor to rotate and reach the specified position; Figure 2 As shown, the posture feedback is obtained based on the fiber grating sensor 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 TCP / IP communication, wherein the current posture position is obtained by the coordinates of the end point, and the posture is obtained by the difference between the two coordinates. It is worth noting that the posture representation of this scheme is different from the conventional method, but is represented by the unit vector of the end direction.
[0081] As a preferred solution of this embodiment, after obtaining the posture feedback, the L2 norm is calculated between it and the expected posture to determine whether the error is less than the allowable value.
[0082] Below, the present disclosure takes a 3-section 5-segment continuum robot as an example to explain the technical effect of the above method in detail. Among them, 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 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 the present disclosure is subjected to three trajectory tracking analyses as shown in the following table:
[0084] Table 1
[0085] Trajectory Circular trajectory Square track Triangle track average value parameter Radius r = 30 mm Side length l = 40 mm 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, experimental RMS errors and precision of three desired trajectories; the radius of the circular trajectory is 30 mm, the RMS error is 1.07 mm, and the precision (ratio of error to the total length of the robot) is 0.29%; the side length of the square trajectory is 40 mm, the RMS error is 1.18 mm, and the precision (ratio of error to the total length of the robot) is 0.32%; the side length of the triangular trajectory is 40 mm, the RMS error is 1.29 mm, and the precision (ratio of error to the total length of the robot) is 0.35%; the average error and precision are 1.18 mm and 0.33%, respectively.
[0087] In this embodiment, the tracking errors of the control methods under various trajectories are compared, where the maximum error is less than 3 mm and the minimum error is less than 0.3 mm. The experimental results are shown in the figure below. Figures 6 to 8 As shown, the dotted line represents the expected 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 the embodiments of the present invention are described above in conjunction with the accompanying drawings, the present invention is not limited to the above specific embodiments and application fields, and the above specific embodiments are only illustrative and instructive, rather than restrictive. A person of ordinary skill in the art can also make many forms under the guidance of this specification and without departing from the scope of protection of the claims of the present invention, all of which belong to the protection of the present invention.
Claims
1. An optimization control method for a continuum robot, characterized in that: The steps include: S1: Use the exponential product formula to establish the forward kinematic model of the continuum robot; S2: Solve the Jacobian pseudo-inverse matrix of the continuum robot based on the forward kinematics model and the current posture; S3: Calculate the iteration direction of the control increment in this iteration cycle using the Jacobian pseudo-inverse matrix and the error; S4: At the same time, an optimization function is set up to establish a particle swarm algorithm PSO to optimize control parameters; S5: Determine the optimal proportional coefficient of the control increment based on the optimization function and the direction search of the control increment; S6: Combine the direction of the control increment and the optimal proportional coefficient to solve the control increment; S7: Control the motion of the continuum robot according to the control increment and obtain position feedback.
2. The optimization control method of a continuum robot according to claim 1, characterized in that: Preferably, in step S1, firstly, coordinate systems O0X0Y0Z0, O1X1Y1Z1, O2X2Y2Z2 and O3X3Y3Z3 are established for the base of the continuum robot, the end of the first section of the continuum robot, the end of the second section of the continuum robot and the end of the third section of the continuum robot respectively; let T (n,m) represents the posture transformation matrix of an n-section m-segment continuum robot, where each continuum robot contains m-segment continuum manipulators; T(α i ,β i ) describes the posture transformation matrix of the i-th segment continuum robot; T(θ i ) describes the posture transformation matrix of the i-th bending degree of freedom of the continuum robot; based on the exponential product method, the posture transformation matrix T of the continuum robot end effector relative to the continuum robot reference posture is expressed as The above expression describes the forward kinematics model, where s represents the sine function, c represents the cosine function, and α i , β i They represent the bending angles of a continuum robot in two directions, L represents the distance between the two disks, H represents the thickness of the disk, θ represents the bending angle of the continuum robot, and α i , β i where I is the identity matrix, ω is the unit vector in the direction of the screw axis, and v is the velocity vector of the terminal under unit angular velocity motion around the screw axis.
3. The optimization control method of a continuum robot according to claim 1, characterized in that: In step S2, the Jacobian matrix is first solved according to the bending angle of the current state using the definition of differential, and then the Jacobian pseudo-inverse matrix is solved using the pseudo-inverse method; In the expression, λ is the regularization coefficient, J i is the i-th column of the Jacobian matrix J, F represents the forward kinematics model, Represents the pseudoinverse of the Jacobian matrix.
4. The optimization control method of a continuum robot according to claim 3, characterized in that: In step S3, the iteration direction of the control increment in the next iteration cycle is calculated by taking the current Jacobian pseudo-inverse matrix as the slope and multiplying 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 posture and the desired posture, and ΔΦ is the increment of the bending angle vector.
5. The optimization control method of a continuum robot according to claim 1, characterized in that: In step S4, the error between the next theoretical posture and the expected posture is minimized as the objective function. The particle swarm algorithm is initialized as 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 speed 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 weight coefficients, r1 and r2 are random numbers between 0 and 1, i V and i k is the velocity and position of the particle in the ith period, i p b and i g b are the optimal position and global optimal position of the particle in the i-th period respectively.
6. The optimization control method of a continuum robot according to claim 4, characterized in that: In step S5, the search for the optimal proportional coefficient of the control increment in the next iteration cycle is performed within the joint angle constraint range under the optimization objective function, wherein two exit conditions are set, namely, reaching the maximum search iteration cycle or the objective function is less than the allowable error; In the above formula, K is the proportional coefficient to be optimized, F represents the kinematic model, e is the posture error vector of the previous cycle, and K min and K max are the upper and lower limits of K value, Φ min and Φ max is the limit value of the joint angle.
7. The optimization control method of a continuum robot according to claim 1, characterized in that: In step S6, the angle vector increment ΔΦ is multiplied by the proportional coefficient K to obtain the optimal angle increment: DF op =KDF.
8. The optimization control method of a continuum robot according to claim 1, characterized in that: In step S7, controlling the movement of the continuum robot and obtaining posture feedback is to send the control amount to the control board through serial communication to control the motor to rotate and reach the specified position; the posture feedback is obtained based on the fiber grating sensor 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 TCP / IP communication, wherein the current posture position is obtained by the coordinates of the end point, and the posture is obtained by the difference between the two coordinates.
9. The optimization control method of a continuum robot according to claim 8, characterized in that: The feedback system consists of a host computer, a fiber grating sensor, a sensor base and a sponge hollow tube.
10. The optimization control method of a continuum robot according to claim 9, characterized in that: The fiber grating sensor passes through the hollow circular tube of the sponge and is placed at the center of the continuum robot to ensure the centering of the sensor.
Citation Information
Patent Citations
Optimal prediction control method of tracking errors of all shafts of redundancy rehabilitation walking training robot
CN107479381A
Industrial robot inverse kinematics solving method based on PSO-RBFNN
CN112733423A
Flexible robot trajectory planning method and device based on kinematics iterative learning control
CN113146600A
Optimization method for dexterity of continuum robot
CN115890659A
Force and position type fusion compliance control method and system for rope-driven space flexible mechanical arm
CN117245663A
