Flexible continuum mechanical arm and damping Jacobian pseudo-inverse control method of mechanical arm

Abstract

The invention discloses a flexible continuum mechanical arm and a damping Jacobian pseudo-inverse control method thereof, and the method comprises the following steps: S1, building a mapping relation from a joint space to an operation space, and obtaining a homogeneous transformation matrix of a tail end relative to a reference coordinate system; s2, establishing a mapping relation from a joint space to a driving space of the flexible continuum mechanical arm; s3, establishing a forward kinematics relation from the driving space to the joint space; s4, establishing a Jacobian matrix from the driving space to the operation space based on a chain rule; s5, setting initial values of a step length factor and a damping factor, and establishing an improved damping least square Jacobian pseudo-inverse control model; s6, interpolating and differentiating initial and final positions, and iteratively solving a driving space increment; and S7, two flexible joints on the flexible continuum mechanical arm are controlled to reach the target position according to the solved driving space increment. According to the invention, on the basis of realizing high-precision control, huge change of the driving quantity of the continuum mechanical arm at the singular point is avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arm control, and particularly relates to a flexible continuum robotic arm and a damping Jacobian pseudo-inverse control method for the robotic arm. Background Art

[0002] Due to its unique structural characteristics, the continuum robotic arm has extremely high flexibility and environmental adaptability. It can adapt to complex, narrow and irregular working environments; it can achieve complex motions and posture adjustments to meet the requirements of complex path planning. Its ultra-redundant structural characteristics endow it with almost infinite continuous deformation ability. When subjected to large external forces, it can deform along the direction of the external force and quickly return after the external force disappears, realizing flexible and safe operation. Its driving method is generally cable-driven, and the driving motors can be centrally arranged on a fixed base, enabling it to achieve extremely lightweight design with a simple mechanical structure while improving the load capacity of the continuum robotic arm.

[0003] However, the continuous deformation characteristics of the continuum robotic arm also increase the difficulty of modeling and control. At present, researchers have proposed many methods for modeling the continuum robotic arm. Among them, the most widely used is the piecewise constant curvature hypothesis model, but no analysis, comparison and adaptive adjustment have been carried out on the actual system. Due to the ultra-redundant structural characteristics of the continuum robotic arm, in the case of multiple joints, the mapping relationship between the operation space and the driving space cannot be linearly described. Therefore, its control requires the design of corresponding optimized control models. Commonly used inverse kinematics control models include the particle swarm optimization algorithm, the Jacobian pseudo-inverse method, and the neural network-based control method. However, the particle swarm optimization algorithm is prone to falling into local optima, and its convergence speed is affected by the control accuracy. The Jacobian pseudo-inverse method is prone to severe jitter at singular points, and the neural network requires a large amount of data training and has poor generalization ability. Summary of the Invention

[0004] The present invention provides a flexible continuum robotic arm and a damping Jacobian pseudo-inverse control method for the robotic arm to solve the technical problems mentioned in the background art.

[0005] To achieve the above object, the technical solution of the present invention is realized as follows:

[0006] The present invention provides a damping Jacobian pseudo-inverse control method for a flexible continuum robotic arm, including the following steps:

[0007] S1. Establish the mapping relationship from the joint space to the operation space under the constant curvature model, and obtain the homogeneous transformation matrix of the end of the continuum robotic arm relative to the reference coordinate system;

[0008] S2. Based on the bending posture of the flexible continuum robotic arm, establish the mapping relationship from the joint space of the flexible continuum robotic arm to the driving space;

[0009] S3. Establish the forward kinematic relationship from the driving space to the joint space based on the mapping relationship from the joint space to the driving space of the flexible continuum manipulator;

[0010] S4. Establish the Jacobian matrix from the driving space to the operating space based on the chain rule;

[0011] S5. Set the initial values of the step factor and the damping factor, and establish an improved damping least squares Jacobian pseudo-inverse control model based on the Jacobian matrix from the driving space to the operating space;

[0012] S6. Interpolate and differentiate the initial and final positions, and iteratively solve for the driving space increment based on the improved damping least squares Jacobian pseudo-inverse control model, the homogeneous transformation matrix of the end of the continuum manipulator relative to the reference coordinate system, and the forward kinematic relationship from the driving space to the joint space;

[0013] S7. Control the two flexible joints on the flexible continuum manipulator to reach the target position according to the obtained driving space increment.

[0014] On the other hand, the present invention also provides a flexible continuum manipulator, which is controlled by the above damping Jacobian pseudo-inverse control method, including:

[0015] A control mechanism, including a control box, two groups of first control units and second control units installed in the control box;

[0016] A visual perception module, installed on the end face of the control box;

[0017] A flexible continuum arm body, including two flexible joints, namely a first flexible joint installed on the end face of the control box and a second flexible joint installed on the first flexible joint. The first flexible joint and the second flexible joint are respectively driven by two groups of first control units to realize the rotation and bending of the first flexible joint and the second flexible joint; each group of first control units includes two first control units;

[0018] An end gripper, installed on the second flexible joint and driven by the second control unit to realize the grasping of the target object.

[0019] Advantages of the present invention:

[0020] 1. The present invention discloses a damping Jacobian pseudo-inverse control method for a flexible continuum manipulator, in which the Jacobian matrix from the driving space to the operating space of the multi-joint continuum manipulator is established by using the chain rule, which simplifies the model complexity and the solution difficulty.

[0021] In addition, the present invention sets a single-step amplitude, interpolates and differentiates the initial and final positions according to the single-step amplitude, splits the initial and final positions into multiple process points, and dynamically adjusts the step factor and damping factor in the improved damped least-squares Jacobian pseudoinverse control model according to the error fluctuation between each process point. During the iteration process, while meeting the control accuracy, a smooth motion that avoids singularity is achieved.

[0022] 2. On the other hand, the present invention also discloses a flexible continuum manipulator, which includes two flexible joints, namely the first flexible joint and the second flexible joint. The flexible joints adopt a structure with double springs embedded, and the bending posture conforms to the constant curvature model. The rigid material has high force conduction efficiency and rapid response; it has a large linear range of force characteristics, high motion accuracy, and large load capacity. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 is a flowchart of the damped Jacobian pseudoinverse control method in the present invention;

[0024] Figure 2 is a layout diagram of each coordinate system on the second flexible joint in the present invention;

[0025] Figure 3 is a layout diagram of each coordinate system of the two flexible joints in the present invention;

[0026] Figure 4 is a planar error result diagram of a circular trajectory in the accuracy verification experiment of the embodiment of the present invention;

[0027] Figure 5 is an axial error result diagram of a circular trajectory in the accuracy verification experiment of the embodiment of the present invention;

[0028] Figure 6 is a planar error result diagram of a square trajectory in the accuracy verification experiment of the embodiment of the present invention;

[0029] Figure 7 is an axial error result diagram of a square trajectory in the accuracy verification experiment of the embodiment of the present invention;

[0030] Figure 8 is a planar error result diagram of a triangular trajectory in the accuracy verification experiment of the embodiment of the present invention;

[0031] Figure 9 is an axial error result diagram of a triangular trajectory in the accuracy verification experiment of the embodiment of the present invention

[0032] Figure 10 is a structural schematic diagram of the flexible continuum manipulator in the present invention.

[0033] DESCRIPTION OF THE REFERENCE NUMERALS:

[0034] 1. Tensile spring; 2. Compression spring; 3. Driving wheel; 4. Pulley block; 5. Driving rope; 6. First disk; 7. Second disk; 8. Third disk; 9. Threading hole; 10. End jaw. Specific embodiments

[0035] For ease of understanding of the present invention, the present invention will be described more comprehensively below with reference to the relevant drawings. Preferred embodiments of the present invention are shown in the drawings. However, the present invention can be implemented in many other different forms and is not limited to the embodiments described herein. On the contrary, the purpose of providing these embodiments is to make the understanding of the disclosure of the present invention more thorough and comprehensive.

[0036] It should be noted that when an element is referred to as being "fixed to" or "disposed on" another element, it can be directly on the other element or indirectly on the other element. When an element is referred to as being "connected to" another element, it can be directly connected to the other element or indirectly connected to the other element.

[0037] It should be understood that the orientation or positional relationship indicated by terms such as "length", "width", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation to the present invention.

[0038] In addition, the terms "first" and "second" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include one or more of such features. In the description of the present invention, "a plurality" means two or more unless otherwise specifically defined.

[0039] It should also be noted that in the embodiments of the present application, the same reference numerals are used to represent the same components or the same parts. For the same parts in the embodiments of the present application, only one of the parts or components may be marked with a reference numeral in the drawings. It should be understood that for other identical parts or components, the reference numerals are equally applicable.

[0040] Refer to Figure 1, embodiments of the present application provide a damping Jacobian pseudo-inverse control method for a flexible continuum manipulator. This method is specifically a control method for a flexible continuum manipulator. There are two sequentially connected flexible joints provided on the flexible continuum manipulator. The flexible joint mainly includes a tension spring 1 and a compression spring 2. Among them, the tension spring 1 is embedded in the center of the compression spring 2. One end of the tension spring 1 and the compression spring 2 is fixed to the second disk 7, and the other end is fixed to the first disk 6 and the third disk 8, forming an embedded double-spring structure. Compared with traditional flexible materials, the selected materials of the tension spring 1 and the compression spring 2 are 45 steel, which has a very high elastic modulus, very low energy loss after deformation, and a faster response speed. At the same time, the regular helical structure of the spring can disperse external forces through uniform deformation, making the deformation of each micro-segment consistent when the spring bends. The above properties ensure that when the flexible joint of the embedded double-spring structure bends and deforms, the deformation characteristics satisfy the constant curvature model. The constant curvature model is a mathematical model in which the shape is circular arc when bending and deforming, and the deformation angles of each micro-segment of the circular arc are the same;

[0041] The damping Jacobian pseudo-inverse control method includes the following steps:

[0042] S1. Establish the mapping relationship from the joint space to the operation space under the constant curvature model, and obtain the homogeneous transformation matrix of the end of the continuum manipulator relative to the reference coordinate system;

[0043] S2. Based on the bending posture of the flexible continuum manipulator, establish the mapping relationship from the joint space of the flexible continuum manipulator to the drive space;

[0044] S3. Establish the forward kinematic relationship from the drive space to the joint space according to the mapping relationship from the joint space of the flexible continuum manipulator to the drive space;

[0045] S4. Establish the Jacobian matrix from the drive space to the operation space based on the chain rule;

[0046] S5. Set the initial values of the step factor and the damping factor, and establish an improved damping least squares Jacobian pseudo-inverse control model according to the Jacobian matrix from the drive space to the operation space;

[0047] S6. Interpolate and differentiate the initial and final positions, and iteratively solve the drive space increment according to the improved damping least squares Jacobian pseudo-inverse control model, the homogeneous transformation matrix of the end of the continuum manipulator relative to the reference coordinate system, and the forward kinematic relationship from the drive space to the joint space;

[0048] S7. Control the two flexible joints on the flexible continuum manipulator to reach the target position according to the obtained drive space increment.

[0049] The present invention designs an improved damping least - squares Jacobian pseudo - inverse control model. The flexible joint with an embedded double - spring structure has a more uniform force distribution, making the deformation amounts of each micro - segment of the flexible joint tend to be consistent. In terms of control, a damping factor and a step - size factor that are adaptively adjusted with error fluctuations are designed. While achieving high - precision control, it avoids the huge change in the driving amount of the continuum manipulator at the singularity point, which may cause control instability.

[0050] In some embodiments, referring to Figure 2 and Figure 3 , the step S1 specifically includes the following steps:

[0051] S11. Establish the mapping relationship from the joint space to the operation space under the constant - curvature model; the constant - curvature model is a mathematical model in which the shape is circular arc during bending deformation, and the deformation angles of each micro - segment of the circular arc are the same;

[0052] S12. Establish the homogeneous transformation matrix \(T\) of the end of the \(i\) - th flexible joint relative to its own fixed coordinate system according to the mapping relationship from the joint space to the operation space i ; referring to Figure 3 , if it is the first flexible joint, its own fixed coordinate system is the coordinate system \(O1X1Y1Z1\) established with the center of the second disk 7; if it is the second flexible joint, its own fixed coordinate system is the coordinate system \(O2X2Y2Z2\) established with the center of the third disk 8;

[0053] S13. Then, according to the homogeneous transformation matrix \(T\) of the end of the \(i\) - th flexible joint relative to its own fixed coordinate system i , obtain the homogeneous transformation matrix \(T\) of the end of the continuum manipulator relative to the reference coordinate system q , the reference coordinate system is the coordinate system \(O0X0Y0Z0\) established with the center of the first disk 6; specifically as follows:

[0054]

[0055] Among them, \(n\) represents the total number of flexible joints in the flexible continuum manipulator. Since \(n = 2\), the homogeneous transformation matrix \(T\) of the end of the continuum manipulator relative to the reference coordinate system q = T1@T2, where @ represents matrix multiplication.

[0056] In some embodiments, referring to Figure 2 , the mapping relationship from the joint space to the operation space under the constant - curvature model in step S11 is specifically as follows:

[0057]

[0058] Among them, \(x\) i , \(y\) i , \(z\) irespectively represent the position changes of the end coordinate system of the i-th flexible joint relative to the x, y, and z axes of its own fixed coordinate system OXYZ, θ x , θ y , θ z are respectively the pose changes of the end coordinate system of the i-th flexible joint relative to the x, y, and z axes of its own fixed coordinate system OXYZ; ρ represents the bending radius of the tension spring 1 in the i-th flexible joint; θ i represents the bending angle of the tension spring 1 in the i-th flexible joint; represents the torsional angle of the i-th flexible joint relative to the x-axis of its own fixed coordinate system; · represents the dot product.

[0059] In some embodiments, the homogeneous transformation matrix T of the end of the i-th flexible joint relative to its own fixed coordinate system in S12 i is as follows:

[0060]

[0061] where c represents the cosine function cos(.), and s represents the sine function sin(.).

[0062] In some embodiments, in the constant curvature model, the bending posture of the flexible joint is arc-shaped, and the bending curvature of each micro-segment is the same. At the same time, due to the double-spring structure embedded in the flexible joint, the tension spring 2 restricts the axial change of the flexible joint. Therefore, when bending, the arc length formed by the spring contact points in the bending direction remains unchanged. Based on the assumption of continuous material deformation, the length change of the driving rope 5 corresponding to the wire holes in four directions can be obtained. Since the length change of the wire holes symmetric with respect to the center is the same in magnitude and opposite in direction, they can be driven by the same motor. Therefore, each flexible joint is driven by two motors, and the continuum manipulator is driven by four motors. The flexible joint closest to the reference coordinate system is the first flexible joint, and its movement is controlled by the driving variables l1 and l2. Subsequently, the second flexible joint connected in sequence is controlled by the driving variables l3 and l4. However, since the driving rope 5 controlling the movement of the second joint passes through the first flexible joint, the change of the first flexible joint will affect the changes of l3 and l4, and decoupling processing is required. Finally, the change of the driving variable relative to the initial pose when no bending deformation occurs is obtained, that is, the mapping relationship from the joint space to the driving space of the flexible continuum manipulator, which is as follows:

[0063]

[0064] where l1 and l2 are respectively the driving variables of the two motors on the first flexible joint; l3 and l4 are respectively the driving variables of the two motors on the second flexible joint; θ1, are respectively the bending angle and torsional angle of the first flexible joint, θ2, They are respectively the bending angle and the torsion angle of the second flexible joint, r1 is the radius of the compression spring 2, and r2 is the radius of the tension spring 1.

[0065] In some embodiments, the forward kinematic relationship from the driving space to the joint space is specifically as follows:

[0066]

[0067] In some embodiments, S4 specifically includes the following steps:

[0068] S41. Establish a mapping relationship from the driving space to the operating space through the chain rule. The mapping relationship from the driving space to the operating space is specifically as follows:

[0069]

[0070] Among them, They are respectively the velocity change amounts of the operating space, the joint space, and the driving space; J qx is the Jacobian matrix from the joint space to the operating space; J lq is the Jacobian matrix from the driving space to the joint space; is the Jacobian pseudoinverse matrix from the joint space to the driving space;

[0071] S42. Use the mapping relationship from the driving space to the operating space to solve the Jacobian matrix J qx from the joint space to the operating space and the Jacobian pseudoinverse matrix

[0072] S43. Dot-multiply the Jacobian matrix J qx from the joint space to the operating space and the Jacobian pseudoinverse matrix to obtain the Jacobian matrix J lx from the driving space to the operating space. It is specifically expressed by the formula as follows:

[0073]

[0074] In some embodiments, S5 specifically includes the following steps:

[0075] S51. Set the initial values of the step factor and the damping factor;

[0076] S52. Based on the Jacobian matrix J lx from the driving space to the operating space, establish an improved damping least-squares Jacobian pseudoinverse control model to solve the driving space increment corresponding to each process point. The improved damping least-squares Jacobian pseudoinverse control model is specifically as follows:

[0077]

[0078] Wherein, α is the step factor, λ is the damping factor, I is the identity matrix, Δx is the operation space increment, i.e., the motion increment of the end gripper 10 of the flexible continuum manipulator; Δl is the driving space increment, i.e., the change in the rotation angles of the four motors on the two flexible joints; T is the transpose of the matrix.

[0079] In some embodiments, S6 specifically includes the following steps:

[0080] S61. Introduce the ClampMag method and set the single-step amplitude; then perform interpolation differentiation on the initial and final positions of the flexible continuum manipulator according to the single-step amplitude to obtain multiple process points;

[0081] S62. The two flexible joints on the flexible continuum manipulator solve for the driving space increment required to move to the next process point through an improved damped least-squares Jacobian pseudoinverse control model; then, based on the driving space increment, the homogeneous transformation matrix of the end of the continuum manipulator relative to the reference coordinate system, and the forward kinematic relationship from the driving space to the joint space, solve for the position of the end gripper 10 on the flexible continuum manipulator after the movement;

[0082] S63. Calculate the error Xd between the position of the end gripper 10 on the flexible continuum manipulator after the movement and the target position, and calculate the error difference between the error between the position of the end gripper 10 on the flexible continuum manipulator after the movement and the target position and the set error Xc ε ;

[0083] S64. Dynamically adjust the step factor and the damping factor according to the error difference between the position of the end gripper 10 on the flexible continuum manipulator after the movement and the target position and the set error ε ;

[0084] S65. Loop S62 to S64 to iteratively solve for the driving space increment of each process point.

[0085] In the improved damping least squares Jacobian pseudo-inverse control model, the role of the damping factor is to improve numerical stability. Especially when the Jacobian matrix is close to the singular point, the damping term enables the continuum manipulator to smoothly pass through the singular point. However, an overly small damping factor λ cannot effectively suppress the singularity, and it may not be possible to achieve a stable and smooth transition at the singular point. On the other hand, an overly large damping factor λ will reduce the convergence speed, resulting in slow response of the end effector. The step size factor is mainly used to control the magnitude of the update amount. The step size factor multiplies the motion increment obtained by solving the pseudo-inverse control model, that is, only the part of the motion increment at this moment is intercepted as the actual motion step size to improve the accuracy of the motion increment. A larger step size factor will result in a larger update amount, which can accelerate the convergence speed, but may cause system instability or oscillation; a smaller step size factor can increase stability and improve the convergence accuracy, but may slow down the convergence speed.

[0086] To balance the relationship between control stability and accuracy, in the designed improved damping least squares Jacobian pseudo-inverse control model, the step size factor and the damping factor are adaptively adjusted based on the error fluctuation to improve the motion stability while meeting the control accuracy. Specifically, two adjustment states of the step size factor and the damping factor are set according to the error accuracy of moving to each interpolation position (i.e., the process point). Before each iteration, first judge whether the current error is less than the set error value. When the distance from the interpolation position is far, that is, the error is greater than the set error value, a larger step size factor and damping factor are set to achieve rapid approximation. After the current error is less than the set error value, the step size factor and the damping factor are adjusted to smaller values, and at the same time, dynamic adjustment is carried out. According to the fluctuation amplitude and direction of the error, the step size factor and the damping factor are adjusted according to a certain mapping relationship. Specifically, when the error is in the increasing direction, the step size factor and the damping factor change proportionally. When the error is in the decreasing direction and the decrease amplitude exceeds half, the step size factor and the damping factor are adjusted to 0.8 times of the previous value.

[0087] The damping factor and the step size factor are adaptively adjusted according to the error value, and the end motion increment is obtained by iterative solution. Finally, the continuum robot is controlled to reach the target position based on the obtained driving increment. If there is no stable solution after reaching the maximum number of iterations, the initial values of the step size factor and the damping factor need to be reset. Finally, under the condition of meeting the control accuracy, the motion stability of the continuum manipulator is achieved.

[0088] Next, the present invention will detail the technical results of the above scheme based on a continuum manipulator with two flexible joints. The main geometric parameters of the continuum manipulator are as follows: l = 80 mm; r1 = 16 mm; r2 = 4 mm; Three trajectory tracking analyses of the present invention's scheme are carried out as shown in the following table;

[0089] Table 1: Set data table of three trajectories in the accuracy verification experiment;

[0090] Trajectory Circular trajectory Square trajectory Triangular trajectory Trajectory parameter Radius r = 100 mm Side length a = 70 mm Side length b = 170 mm RMSE error 0.07 mm 0.015 mm 0.036 mm Maximum error 2.1 mm 1.75 mm 1.85 mm Accuracy 0.088％ 0.019％ 0.045％

[0091] In Table 1, the trajectory parameters, experimental root mean square error (in the Z-axis direction), maximum error, and accuracy set in the accuracy verification experiment are shown. Among them, the radius of the circular trajectory is 100 mm, the root mean square error is 0.07 mm, the maximum error is 2.1 mm, and the accuracy (the ratio of RMSE to the length of the continuum manipulator itself) is 0.088%; the side length of the square trajectory is 70 mm, the root mean square error is 0.015 mm, the maximum error is 1.75 mm, and the accuracy (the ratio of RMSE to the length of the continuum manipulator itself) is 0.019%; the side length of the triangular trajectory is 170 mm, the root mean square error is 0.036 mm, the maximum error is 1.85 mm, and the accuracy (the ratio of RMSE to the length of the continuum manipulator itself) is 0.045%. Among them, RMSE represents the root mean square error;

[0092] The experimental results are as Figures 4 to 9 shown. The dashed line represents the desired trajectory, and the solid line represents the actual trajectory. By comparing the tracking errors under different trajectories, it can be seen that the maximum error appears in the Z-axis direction and the maximum value is less than 2.5 mm. It can be seen that the experimental results well meet the control requirements of various trajectories, proving that the proposed control method can effectively improve the control accuracy.

[0093] Referring to Figure 10 , on the other hand, the present invention also provides a flexible continuum manipulator, which is controlled by the above damping Jacobian pseudo-inverse control method, including:

[0094] A control mechanism, including a control box, two groups of first control units and a second control unit installed in the control box;

[0095] A visual perception module, installed on the end face of the control box;

[0096] A flexible continuum arm body, including two flexible joints, namely a first flexible joint installed on the end face of the control box and a second flexible joint installed on the first flexible joint. The first flexible joint and the second flexible joint are respectively driven by two groups of first control units to realize the rotation and bending of the first flexible joint and the second flexible joint; each group of first control units includes two first control units;

[0097] An end gripper 10, installed on the second flexible joint, and driven by the second control unit to realize the grasping of the target object.

[0098] The flexible continuum manipulator disclosed in the present invention is similar to the underactuated flexible continuum manipulator in CN118769296A (hereinafter referred to as the reference document). The only difference is that the inventor has optimized the driving methods of the first flexible joint and the second flexible joint and the structure of the first control module in the reference document. Specifically, the difference in the driving methods is that in the reference document, the first flexible joint and the second flexible joint are driven by three first control modules in the reference document, and each first control module in the reference document includes a motor, that is, the first flexible joint and the second flexible joint in the reference document are driven by three motors; while in the present application, the first flexible joint and the second flexible joint are driven by four first control units in the present application, and the four first control units are grouped in pairs to drive the first flexible joint and the second flexible joint respectively, that is, both the first flexible joint and the second flexible joint are driven by two first control units; the structure of one of the first control units on the first flexible joint will be described below;

[0099] Specifically, the first control unit includes a motor, a driving wheel 3, three pulley groups 4 and two driving ropes 5: the three pulley groups 4 are the first pulley group to the third pulley group respectively; the second pulley group is arranged between the first pulley group and the third pulley group;

[0100] The motor is installed in the control box through a motor support frame; the driving wheel 3 is fixedly installed on the output shaft of the motor; four threading holes 9 are opened in a square shape on each of the first disk 6, the second disk 7 and the third disk 8, and two pulley groups 4 are symmetrically installed in the control box respectively; one end of one driving rope 5 is fixed in a threading hole 9 on the second disk 7, and the other end passes through the threading holes 9 on the first disk 6 respectively, bypasses the first pulley group and the second pulley group and is fixed on one side of the driving wheel 3, and one end of the other driving rope 5 is fixed in another threading hole 9 on the second disk 7, and the other end passes through the threading holes 9 on the first disk 6 respectively, bypasses the third disk 8 and the second pulley group and is fixed on the other side of the driving wheel 3; and the two driving ropes 5 are respectively fixedly connected to the two farthest (on the diagonal of the square) threading holes 9 on the second disk 7.

[0101] The structure of the other first control unit on the first flexible joint is the same as the above-mentioned first control unit structure. The only difference is that the two driving ropes 5 are fixedly connected to the other two threading holes 9 on the second disk 7, and the two driving ropes 5 also pass through the other two threading holes 9 on the first disk 6 and are finally connected to the driving wheel 3.

[0102] As described above, it is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of changes or substitutions, which should all be covered within the protection scope of the present invention. Moreover, the technical solutions between various embodiments of the present invention can be combined with each other, but it must be based on the fact that those skilled in the art can implement it. When the combination of technical solutions appears to be contradictory or unable to be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the protection scope required by the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims described above.

Claims

1. A damped Jacobian pseudo-inverse control method for a flexible continuum manipulator, characterized in that: The steps include: S1. Establish the mapping relationship from the joint space to the operation space under the constant curvature model, and obtain the homogeneous transformation matrix of the end of the continuum manipulator relative to the reference coordinate system; S2. Based on the bending posture of the flexible continuum robot arm, a mapping relationship from the joint space of the flexible continuum robot arm to the drive space is established; S3, establishing a positive kinematic relationship from the drive space to the joint space based on the mapping relationship from the joint space to the drive space of the flexible continuum manipulator; S4. Establish the Jacobian matrix from the driving space to the operating space based on the chain rule; S5, setting the initial values ​​of the step size factor and the damping factor, and establishing an improved damped least squares Jacobian pseudo-inverse control model based on the Jacobian matrix from the drive space to the operation space; S6, interpolate and differentiate the initial and final positions, iteratively solve the drive space increment based on the improved damped least squares Jacobian pseudo-inverse control model, the homogeneous transformation matrix of the end of the continuum manipulator relative to the reference coordinate system, and the positive kinematic relationship from the drive space to the joint space; S7. Control the two flexible joints on the flexible continuum robot arm to reach the target position according to the solved driving space increment.

2. The damped Jacobian pseudo-inverse control method of the flexible continuum manipulator according to claim 1, characterized in that: The S1 specifically includes the following steps: S11. Establishing a mapping relationship between the joint space and the operation space under a constant curvature model; the constant curvature model is a mathematical model in which the shape is an arc when bent and deformed, and the deformation angles of each micro-segment of the arc are the same; S12. Based on the mapping relationship from the joint space to the operation space, establish the homogeneous transformation matrix T of the end of the i-th flexible joint relative to its own fixed coordinate system i ; If it is the first flexible joint, the self-fixed coordinate system is the coordinate system established with the center of the second disk; if it is the second flexible joint, the self-fixed coordinate system is the coordinate system established with the center of the third disk; S13, then according to the homogeneous transformation matrix T of the end of the i-th flexible joint relative to its own fixed coordinate system i , obtain the homogeneous transformation matrix T of the end of the continuum manipulator relative to the reference coordinate system q , the reference coordinate system is the coordinate system established with the center of the first disk; the details are as follows: Where n represents the total number of flexible joints in the flexible continuum manipulator.

3. The damped Jacobian pseudo-inverse control method of the flexible continuum manipulator according to claim 2, characterized in that: The mapping relationship between the joint space and the operation space under the constant curvature model in S11 is as follows: Among them, x i ,y i , z i They represent the position change of the coordinate system of the end of the i-th flexible joint relative to the x, y, and z axes of its own fixed coordinate system, θ x , θx, θ z are the position changes of the coordinate system of the end of the i-th flexible joint relative to the x, y, and z axes of its own fixed coordinate system; ρ represents the bending radius of the tension spring in the i-th flexible joint; θ i represents the bending angle of the tension spring in the i-th flexible joint; represents the torsion angle of the i-th flexible joint relative to the x-axis of its own fixed coordinate system; · represents the dot product.

4. The damped Jacobian pseudo-inverse control method of the flexible continuum manipulator according to claim 3, characterized in that: The homogeneous transformation matrix T of the i-th flexible joint end relative to its own fixed coordinate system in S12 i , as follows: Among them, c represents the cosine function cos(.), and s represents the sine function sin(.).

5. The damped Jacobian pseudo-inverse control method of the flexible continuum manipulator according to claim 4, characterized in that: The mapping relationship between the flexible continuum robot arm joint space and the drive space is as follows: Among them, l1 and l2 are the driving variables of the two motors on the first flexible joint; l3 and l4 are the driving variables of the two motors on the second flexible joint; are the bending angle and torsion angle of the first flexible joint, are the bending angle and torsion angle of the second flexible joint respectively, r1 is the radius of the compression spring, and r2 is the radius of the extension spring.

6. The damped Jacobian pseudo-inverse control method of the flexible continuum manipulator according to claim 5, characterized in that: The positive kinematic relationship from the drive space to the joint space is as follows:

7. The damped Jacobian pseudo-inverse control method of the flexible continuum manipulator according to claim 6, characterized in that: The S4 specifically includes the following steps: S41. A mapping relationship from the driving space to the operating space is established by using the chain rule. The mapping relationship from the driving space to the operating space is as follows: in, are the velocity changes in the operation space, joint space, and drive space respectively; J qx is the Jacobian matrix from joint space to operation space; J lq is the Jacobian matrix from the driving space to the joint space; is the Jacobian pseudo-inverse matrix from joint space to driving space; S42. Use the mapping relationship from the drive space to the operation space to solve the Jacobian matrix J from the joint space to the operation space qx And the Jacobian pseudo-inverse matrix from joint space to drive space S43, convert the Jacobian matrix J from joint space to operation space qx And the Jacobian pseudo-inverse matrix from joint space to drive space Perform point multiplication to obtain the Jacobian matrix J from the driving space to the operation space lx , the formula is as follows:

8. The damped Jacobian pseudo-inverse control method of the flexible continuum manipulator according to claim 7, characterized in that: The S5 specifically includes the following steps: S51, setting the initial value of the step size factor and the damping factor; S52, based on the Jacobian matrix J from the driving space to the operating space lx , an improved damped least squares Jacobian pseudo-inverse control model is established to solve the driving space increment corresponding to each process point; the improved damped least squares Jacobian pseudo-inverse control model is as follows: Among them, α is the step size factor, λ is the damping factor, I is the unit matrix, Δx is the operation space increment, that is, the motion increment of the end gripper on the flexible continuum robot arm; Δl is the drive space increment, that is, the change in the rotation angles of the four motors on the two flexible joints; T is the transpose of the matrix.

9. The damped Jacobian pseudo-inverse control method of the flexible continuum manipulator according to claim 8, characterized in that: The S6 specifically includes the following steps: S61, introduce the ClampMag method, set the single-step amplitude; then interpolate and differentiate the initial and final positions of the flexible continuum manipulator according to the single-step amplitude to obtain multiple process points; S62, the two flexible joints on the flexible continuum manipulator are solved by improving the damped least squares Jacobian pseudo-inverse control model to obtain the required driving space increment to move to the next process point; Then, the position of the end gripper on the flexible continuum robot arm after the movement is solved according to the drive space increment, the homogeneous transformation matrix of the end of the continuum robot arm relative to the reference coordinate system, and the positive kinematic relationship from the drive space to the joint space; S63, calculating the error Xd between the position of the end gripper on the flexible continuum robot after the movement and the target position, and calculating the error difference ε between the error between the position of the end gripper on the flexible continuum robot after the movement and the target position and the set error Xc; S64, dynamically adjusting the step size factor and the damping factor according to the error difference ε between the position of the end gripper on the flexible continuum robot arm after the movement and the target position and the set error; S65, loop S62 to S64, iteratively solving the driving space increment of each process point.

10. A flexible continuum robotic arm, characterized in that: The control is performed using the damped Jacobian pseudo-inverse control method according to any one of claims 1 to 9, comprising: The control mechanism comprises a control box, and two sets of first control units and second control units installed in the control box; A visual perception module is installed on the end surface of the control box; The flexible continuum arm comprises two flexible joints, namely a first flexible joint mounted on the end surface of the control box and a second flexible joint mounted on the first flexible joint. The first flexible joint and the second flexible joint are driven respectively by two groups of first control units to realize the rotation and bending of the first flexible joint and the second flexible joint; each group of first control units comprises two first control units; The end gripper is mounted on the second flexible joint and driven by the second control unit to grasp the target object.

Images