Inverse kinematics solving method for snakelike redundant mechanical arm

Abstract

The invention discloses an inverse kinematics solving method for a snakelike redundant mechanical arm, which comprises the following steps: setting m target points of a tail end joint of the mechanical arm according to a to-be-detected target, constructing a target function under the condition that the offset of each joint relative to a Y axis is minimum, and the target function is the weighted square sum of the offset of a1 to an-1 relative to the Y axis, the weights from the (an-1) to the (a1) are sequentially increased; the physical corner limit of each joint of the mechanical arm, the length of each connecting rod, an initial joint moving along the Y axis all the time, a tail end joint reaching a target point in a set posture, and a motion smoothness index are used as constraints of a target function; and solving coordinates of each joint point when the objective function is minimum by using a constrained nonlinear optimization problem solving algorithm. According to the method, the redundant freedom degree of the snakelike mechanical arm is fully utilized, and automatic optimization configuration of the mechanical arm is achieved on the premise that the tail end joint target point is known.

Description

Technical Field

[0001] The invention relates to the field of inverse kinematics solution of redundant mechanical arms, and in particular to an inverse kinematics solution method for a serpentine redundant mechanical arm. Background Art

[0002] When the serpentine redundant manipulator reaches the vicinity of the target to be measured, it relies on the actuators carried at the end, such as industrial cameras and high-pressure water nozzles, to maintain and inspect the target to be measured. In order to perform multi-directional and multi-angle maintenance and observation on the target to be measured, it is necessary to link several joints near the end of the manipulator so that the end link of the manipulator maintains a certain posture and moves near the target to be measured. Given the end path of the manipulator, the problem of finding the joint space of the manipulator belongs to the inverse kinematics problem. Considering that the length of the serpentine redundant manipulator is limited and it needs to pass through the door of the manipulator cabin during operation, if the manipulator is made to achieve the optimal configuration under multiple constraints, the redundant degrees of freedom of the serpentine manipulator can be fully utilized, so that it can reach the maximum operating range under the constraints. At present, the common inverse kinematics solution methods for serpentine redundant manipulators include the inverse solution method based on the pseudo-inverse of the Jacobian matrix and the heuristic inverse solution method. Since there is no unique solution to the inverse kinematics problem of the serpentine redundant manipulator, the inverse kinematics solution method based on the pseudo-inverse of the Jacobian matrix cannot achieve the optimal configuration of the manipulator. The common heuristic inverse solution algorithms currently calculate the new configuration based on the previous configuration. The joints move in sequence, and the already moved joints cannot return to the zero position in the subsequent movement. In other words, the connecting rod will only be lifted continuously, and the automatic optimization configuration cannot be achieved according to the given path. In this case, the manipulator may not be able to reach the target position that it should be able to reach. At present, a method for solving the inverse kinematics of a serpentine redundant manipulator is needed, so that the manipulator can achieve the optimal configuration under multiple constraints, thereby making full use of the redundant degrees of freedom of the serpentine manipulator. Summary of the invention

[0003] In view of the deficiencies in the prior art, the present invention proposes an inverse kinematics solution method for a serpentine redundant manipulator.

[0004] The specific technical solutions are as follows:

[0005] An inverse kinematics solution method for a serpentine redundant manipulator comprises the following steps:

[0006] S1: Set the target point of the end joint of the robot arm according to the target to be detected. There are m target points. The robot arm includes a base, n connecting rods, and n joints. Two adjacent connecting rods are connected by joints. a1 is the initial joint of the robot arm, connecting the base and connecting rod 1. n is the terminal joint, a n+1 is the end effector; a i is the i-th joint, i=1,2,…,n; G jis the jth target point, j = 1, 2, ..., m; the robot arm is initially arranged along the Y axis of the base coordinate system;

[0007] S2: Based on the condition that the offset of each joint relative to the Y axis is the smallest, construct the objective function, which is from a1 to a n-1 The weighted sum of squares of the offsets relative to the Y axis, and a n-1 The weights of a1 increase in order;

[0008] S3: The physical rotation angle limit of each joint of the robot arm, the length of each link, the initial joint always moves along the Y axis, and the end joint reaches the target point G with the set posture j , the motion smoothness index is used as the constraint of the objective function;

[0009] S4: Use a constrained nonlinear optimization problem solving algorithm to find the coordinates of each joint point when the objective function is minimized.

[0010] Furthermore, the objective function is expressed as:

[0011]

[0012] In the formula, is the X coordinate of the ith joint, is the Z coordinate of the ith joint, Represents the square of the vertical distance from the i-th joint point to the horizontal rail.

[0013] Furthermore, in the constraints of the objective function, the physical rotation angle limit constraint expression of each joint of the robot arm is as follows:

[0014] q i ≤q max ,i=1,...,n

[0015]

[0016] In the formula, q i represents the rotation angle of the i-th joint, q max Represents the physical rotation limit of the joint.

[0017] Furthermore, in the constraints of the objective function, the expression of the length constraint of each link of the robot arm is as follows:

[0018] |a i -a i+1 |=L i ,i=1,...,n

[0019] Where, L i represents the length of the i-th connecting rod.

[0020] Furthermore, in the constraints of the objective function, the expression of the initial joint motion constraint is as follows:

[0021]

[0022] In the formula, is the X coordinate of the initial joint, is the Z coordinate of the initial joint.

[0023] Furthermore, in the constraints of the objective function, the expression of the end joint posture constraint is as follows:

[0024] a n =G j

[0025] a n+1 =a n +k j ·L n

[0026] In the formula, k j Indicates that the end joint reaches the target point G j The direction vector of the end link, L n Represents the length of the nth connecting rod.

[0027] Furthermore, among the constraints of the objective function, the expression of the motion smoothness constraint is as follows:

[0028]

[0029] In the formula, represents the sum of the motions of the remaining n-1 joints of the robot except the end joint, a i Indicates that the end joint is at the target point G j The i-th joint point at time, a i ′ indicates that the end joint is located at the target point G j-1 The i-th joint point at time .

[0030] Furthermore, in S4, the algorithm for solving the constrained nonlinear optimization problem uses sequential quadratic programming.

[0031] The beneficial effects of the present invention are:

[0032] The optimization inverse solution algorithm proposed in the present invention takes the minimum joint deviation as the optimization goal, and considers factors such as joint limit, motion smoothness, and end position and posture at the same time. By setting the motion smoothness index, the optimization goal is converted into a constraint to achieve collaborative optimization for multiple goals. The present invention has the characteristics of high computational efficiency and strong versatility. The robot arm configuration obtained by the optimization inverse solution algorithm in the method of the present invention is better than the conventional inverse solution algorithm. In practical applications, the objective function and constraints can be configured according to requirements, and the present invention can be used to obtain the optimal inverse solution under multiple goals and constraints. The present invention can be used as a general method to solve the problem of solving the inverse kinematics of super-redundant robot arms. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 Schematic diagram of the macro-geometric features and path target points of the snake-like robotic arm in an embodiment of the present invention.

[0034] Figure 2 It is a schematic diagram of the configuration of the snake-like robotic arm in an embodiment of the present invention, in which the end is kept horizontal while performing "end translation" near the target.

[0035] Figure 3 Schematic diagram of the configuration of the snake-like robot arm under different adjacent solution steps in an embodiment of the present invention.

[0036] Figure 4 It is a schematic diagram of the configuration of the snake-shaped robotic arm in an embodiment of the present invention when all the links except the end link are located in the same straight line.

[0037] Figure 5 Schematic diagram of the joint rotation angle of the snake-like robot arm in an embodiment of the present invention.

[0038] Figure 6 Schematic diagram of the configuration of the snake-like robotic arm when the wrist joint is located at different target points in an embodiment of the present invention.

[0039] Figure 7 It is a schematic diagram of the configuration of the snake-like robotic arm in an embodiment of the present invention, after the wrist joint reaches the target point, it begins to observe the target from multiple angles. DETAILED DESCRIPTION

[0040] The present invention will be described in detail below according to the accompanying drawings and preferred embodiments, and the purpose and effect of the present invention will become more clear. The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0041] For a multi-joint serpentine redundant manipulator with universal joints between two adjacent links, in order to describe the inverse kinematics problem and simplify the configuration of the serpentine manipulator, the universal joint connecting the two links is regarded as a point, called a joint point, and the link is simplified to a line segment. The entire serpentine manipulator can be simplified to a broken line segment. Figure 1 As shown, it is assumed that the snake-like robot arm has n connecting rods and n joints, that is, from a1 to a n+1 , describes the macroscopic geometric features of the snake-like robot arm, where a1 is the initial joint of the robot arm, which connects the base and link 1, and joint a2 connects link 1 and link 2, and so on. n The end joint of the robot arm, also known as the wrist joint, n+1 is the end effector of the robot arm, a i is the i-th joint, i=1,2,…,n.

[0042] The inverse kinematics problem of the snake-like robot arm can be described as the following process: the snake-like robot arm is in the initial state (i.e. Figure 1 When the robot arm is in the state shown in Figure 1, all joints are at zero position and the connecting rods are kept horizontal. When the robot arm is working, it moves step by step from the end joint to the initial joint, and each connecting rod of the robot arm is gradually lifted. As the robot arm gradually goes deeper into the operation, when all the connecting rods leave the initial position, the robot arm will reach the limit of movement, and the end connecting rod cannot reach a farther position. The inverse kinematics problem is the process of solving the solution from the robot arm task space to the robot arm joint space. The position of the robot arm in the task space is the wrist joint a n The position of the end link (which can be recorded as a n -a n+1 ) posture, after the wrist joint reaches the target position, the end link posture is adjusted through several joints near the end to observe the target in multiple directions. The target path of the snake-like robot wrist joint in the task space is described by multiple discrete points. Assuming that the target path consists of m target points in total, the entire inverse kinematics solution process is divided into m steps. In the jth step (j = 1, 2, ..., m) of the inverse kinematics solution, the end link of the robot reaches the jth target point G with the specified posture j , and the wrist joint a n The position of the target point G j Therefore, in each step of the inverse kinematics solution process, the wrist joint a n With actuator a n+1 The positions of a1 to a are determined, and then the specified optimization goal is used to solve n-1 Position in the base coordinate system.

[0043] According to the operational requirements of the serpentine inspection robot arm to clean the inspection tool in the shield, combined with the characteristics of the small space in the shield sealed cabin and the robot arm's own rotation angle limit, the robot arm wrist joint position and the end link posture are specified under multiple constraints to obtain the robot arm joint space parameters, which is the research direction of the present invention to solve the inverse kinematics problem. Figure 2 As shown, the operator controls the serpentine robot to perform "terminal translation" operation near the target while keeping the terminal horizontal. Different from the serpentine following path planning method, the inverse kinematics of the serpentine robot only gives the position and posture of the terminal joint, and the paths of the remaining joints of the robot are generated by the inverse kinematics algorithm. There is no unique solution to the inverse kinematics problem of the redundant serpentine robot, so how to make full use of the redundant degrees of freedom of the serpentine robot is the key issue studied in this invention.

[0044] Based on the above architecture, an embodiment of the present invention proposes an inverse kinematics solution method for a serpentine redundant manipulator. The method takes into account factors such as the advantages and disadvantages of the manipulator configuration, solution efficiency, motion smoothness, joint rotation angle limit, and posture accuracy. The method specifically includes the following steps:

[0045] S1: m target points of the end joint of the robot arm are set according to the target to be detected; the robot arm is initially arranged along the Y axis of the base coordinate system.

[0046] S2: Based on the condition that the offset of each joint relative to the Y axis is the smallest, construct the objective function, which is from a1 to a n-1 The weighted sum of squares of the offsets relative to the Y axis, and a n-1 The weights of a1 to a1 increase in order.

[0047] Specifically, since the configuration of the serpentine robot is determined by the position of each joint of the robot, the joints of the serpentine robot are used as system variables in the optimization problem modeling. The inverse kinematics solution is performed based on the principle of as few joints as possible. When implementing the algorithm, this principle can be converted into the minimum deviation from each joint to the horizontal linear slide rail, so as to model the optimization problem; since the initial joint a1 must be on the Y axis, and the wrist joint a n The position is given equal to the target point G j , so except for the initial joint point a1 and wrist joint a n The sum of the vertical distances from the remaining joints to the Y axis can be used to describe the degree to which the robot as a whole leaves the initial position.

[0048] like Figure 3 As shown in the figure, the deviation of each joint of the snake-like robot arm to the horizontal linear slide rail is the vertical distance from each joint point to the Y axis. The sum of the squares of the vertical distances from all the nodes except the wrist joint to the Y axis is used as the objective function. The expression is as follows:

[0049]

[0050] In the formula, is the X coordinate of the ith joint, is the Z coordinate of the ith joint, Represents the square of the vertical distance from the i-th joint point to the horizontal rail.

[0051] Considering that when the snake-like robot arm is in motion, the joints close to the wrist joint are usually farther away from the Y-axis than the joints close to the initial joints, if the sum of the vertical distances from each joint point to the Y-axis is directly used as the objective function, the deviations of the initial few joints can only have a small impact on the objective function, which will cause the optimization results to be biased towards the joints close to the wrist joint, and will not meet the optimization goal of minimizing the number of joints in the robot arm's motion. Therefore, it is necessary to gradually increase the weight coefficient in the objective function in the order from the wrist joint to the initial joint to increase the influence of the joint deviation close to the base on the objective function.

[0052] For the convenience of calculation, the snake robot is in the following Figure 4 When the posture is shown, the weight coefficient of each joint offset is set so that the deviation from joint 2 to joint n-1 has the same effect on the objective function. At this time, according to the principle of similar triangles, we can get:

[0053]

[0054] Where l2 represents the vertical distance from the joint point a2 to the Y axis. Similarly, l i Represents the vertical distance from joint point i to the y-axis.

[0055] Then we get:

[0056]

[0057] From this, we can see that multiplying the distance from the i-th joint to the Y axis by the coefficient Then the distance from the i-th joint to the Y axis can be converted to joint a n-1 Distance to the Y axis.

[0058] Then we get the vertical distance l from the i-th joint point to the Y axis i The expression is as follows:

[0059]

[0060] The vertical distance from the n-1th joint to the Y axis is used as the measurement standard, and the vertical distances from the remaining joints to the Y axis are multiplied by the coefficient Then the influence of the vertical distance from the remaining joint points to the Y axis on the target function will be the same as that of the n-1th joint, and the objective function expression after the weight coefficient is optimized is as follows:

[0061]

[0062] S3: The snake-like robot arm mounted on a horizontal linear slide needs to meet the following conditions when solving the inverse kinematics: on the basis of as few joints as possible, ensure that the movement of two consecutive solution steps is smooth, all joint angles are within the angle limit, the length of each link is determined, the initial joint always moves along the Y axis, and the wrist joint of the robot arm reaches the target point with a specified posture. Therefore, the constraints of the objective function are constructed according to the above conditions, as follows:

[0063] (1) Physical rotation limit constraints of each joint of the robot: The serpentine redundant robot itself has physical rotation limit. Therefore, in the process of inverse kinematics solution, it is necessary to ensure that all joint rotation angles are within the physical rotation limit. According to the universal joint structure between the robot links, each joint has a yaw angle and a pitch angle corresponding to the two physical rotation angles of the universal joint. Through the verification of the yaw angle and the pitch angle, the angle formed between the two adjacent links becomes the equivalent rotation angle. The equivalent rotation angle is used to measure whether the robot joint angle is within the physical rotation limit. The equivalent rotation angle q of the i-th joint of the robot i is the angle between the i-1th link and the ith link, especially when i=1, It should be cross-multiplied with the unit vector of the Y axis. The physical rotation limit constraint expression is as follows:

[0064] q i ≤q max ,i=1,...,n

[0065]

[0066] In the formula, q i represents the rotation angle of the i-th joint, q max Represents the physical rotation limit of the joint.

[0067] In order to simplify the calculation in solving the optimization problem, the expression of the physical rotation limit constraint is simplified as follows:

[0068]

[0069] (2) Constraints on the length of each link of the robot arm: The link length condition must be met during the inverse solution, that is, the joint a i With joint a i+1 The distance between them is equal to the length L of the i-th link i , the expression is as follows:

[0070] a i -a i+1 |=L i ,i=1,...,n

[0071] (3) Initial joint motion constraint: The initial joint always moves along the Y axis, that is, and

[0072] (4) Wrist joint posture constraint: In each step of the inverse kinematics solution, the wrist joint of the robot n Reach the jth target point G with the specified posture j , the expression is as follows:

[0073] a n =G j

[0074] a n+1 =a n +k j ·L n

[0075] In the formula, k j Indicates that the wrist joint reaches the target point G j The direction vector of the end link.

[0076] (5) Motion smoothness constraint: If the inverse kinematics solution method proposed in the present invention considers both the optimal joint offset and the optimal operation smoothness at the same time, the optimal inverse solution problem will become a multi-objective optimization problem. In solving the multi-objective optimization problem, it is impossible to find a solution that can simultaneously satisfy the optimal joint offset and the optimal operation smoothness at each step. Therefore, performing multi-objective optimization will increase the complexity of the optimal inverse solution problem, resulting in lower solution efficiency. The present invention analyzes the configuration of the serpentine robot arm and adds the motion smoothness optimization goal as a constraint condition to the inverse kinematics solution problem, thereby achieving multi-objective collaborative optimization.

[0077] The specific motion smoothness index is: in each step of the inverse kinematics solution, it must be ensured that the new configuration obtained is continuous relative to the configuration in the previous step. Figure 6 As shown, assuming that the current configuration of the robot arm is a1-a2-…-a n+1 , the robot arm moves from the j-1th target point G j-1 Move to the jth target point G j , the new configuration to be solved is a1'-a2'-…-a n+1 ', since the intervals between target points under different paths are different, the intervals between target points in each step determine the offset of the robot as a whole relative to the previous step. Therefore, the amount of joint movement of the robot in each step is related to the distance between the current target point and the target point in the previous step.

[0078] In each step of the inverse solution, the movement of each joint relative to the previous step should be less than the interval |G of the target point in this step relative to the previous step j -G j-1|σ times, σ is set manually, and the motion smoothness constraint expression is as follows:

[0079]

[0080] In the formula, represents the sum of the motions of the remaining n-1 joints of the robot except the wrist joint, a i Indicates that the wrist joint is at the target point G j The i-th joint point at time, a i ′ indicates that the wrist joint is at the target point G j-1 The i-th joint point at the time; in this embodiment, the value of σ is 2.

[0081] According to the above analysis, the expression of inverse kinematics problem is:

[0082]

[0083] S4: Use a constrained nonlinear optimization problem solving algorithm to calculate the inverse kinematics problem and obtain the coordinates of each joint point when the objective function is minimized.

[0084] After the wrist joint of the snake robot reaches the target, it starts to detect the target. Some targets, such as shield cutters, require the snake robot to observe the different positions of the target from multiple angles. It is difficult to control the multi-angle precise observation of the target object under manual operation. Through the above inverse kinematics solution method, the robot can realize the multi-angle precise observation of the same target. n In the case of , it is necessary to observe the target at a specific angle in the circumferential direction, find the coordinates of the new end joint target point (that is, complete the setting of m target points in S1), and the process of how to move from the original end joint to the new target point is as follows:

[0085] like Figure 7 As shown, the wrist joint position of the robot is G j , the target to be measured is T, the end effector of the robot arm is a n+1 The distance to T is L T , the direction vector of the end link of the robot arm is k, and the line of sight of the robot arm needs to be θ with the direction of the original end link T The target T is observed from multiple angles. In this process, the wrist joint position P of the robot arm is adjusted by G j is the center of the circle, R T The position of the wrist joint in each step of the multi-angle observation process can be obtained by solving the parametric equation of the circle with radius t and normal vector k, and then the direction vector of the end link can be calculated according to the position of the observation target T.

[0086] Take a set of orthogonal unit vectors u and v in the plane where the circle is located. In order to find u and v, select any vector e that is not parallel to u, such as the basis vector i of the X axis. If u is parallel to i, then the basis vector j of the Y axis can be selected; the cross product of k and e must be perpendicular to k, and after normalizing it, we get u, and then cross product k and u, and normalize the result to get v. The expression is as follows:

[0087]

[0088] Coordinate origin O and target point G j Composition vector G j It forms a vector with a point P on the circumference The angle between the vector and the unit vector u is α, then the vector The expression is as follows:

[0089]

[0090] In the plane where the circle is located, u and v can be regarded as coordinate axes. Projecting onto the u-axis and v-axis, the expression is as follows:

[0091]

[0092] In the formula, For vector The projection on u, For vector The projection onto v.

[0093] The parametric equation of a circle can be written as:

[0094] P(α)=G j +R·cos(α)·u+R·sin(α)·v

[0095] The inverse kinematics problem proposed in the present invention is a planning problem in which the objective function and the constraints contain nonlinear functions. Sequential Quadratic Programming (hereinafter referred to as SQP) has the advantages of good convergence, high computational efficiency, and strong boundary search capability. Therefore, in this embodiment, the algorithm for solving the nonlinear optimization problem with constraints selects SQP to solve the inverse kinematics problem proposed in the present invention. In each iteration, SQP uses the current iteration point as the basis to construct an approximate quadratic programming problem, and then solves the quadratic programming problem to update the iteration point. The above process is repeated until the termination condition is met. Therefore, SQP is divided into the following three processes: (1) approximation of the object function and constraints, (2) solving the quadratic programming sub-problem, and (3) line search and iteration point update, and finally the optimal solution of inverse kinematics is obtained, that is, reaching the target point G.j The coordinates of each joint point of the robot arm at time a i .

[0096] The method of the present invention uses the minimum joint deviation as the optimization goal to solve the inverse kinematics, while considering factors such as joint limits, motion smoothness, and end position. By setting a motion smoothness index, the optimization goal is converted into a constraint to achieve collaborative optimization for multiple goals; and a numerical optimization method is used to obtain the optimal inverse solution of the serpentine robot under the above-mentioned specific optimization goals and constraints. The inverse kinematics solution method proposed in the present invention is not limited to robots of a specific configuration, but can be applied to all serial redundant robots. In practical applications, the objective function and constraints can be configured according to actual needs, and the method in the present invention can be used to convert multiple optimization goals into constraints. Finally, the solution method in the present invention is used to obtain the optimal inverse solution under multiple goals and constraints.

[0097] Those skilled in the art can understand that the above are only preferred examples of the invention and are not intended to limit the invention. Although the invention is described in detail with reference to the above examples, those skilled in the art can still modify the technical solutions recorded in the above examples or replace some of the technical features therein with equivalents. Any modification, equivalent replacement, etc. made within the spirit and principle of the invention shall be included in the protection scope of the invention.

Claims

1. An inverse kinematics solution method for a serpentine redundant manipulator, characterized in that: The following steps are involved: S1: Set the target point of the end joint of the robot arm according to the target to be detected. There are m target points. The robot arm includes a base, n connecting rods, and n joints. Two adjacent connecting rods are connected by joints. a1 is the initial joint of the robot arm, connecting the base and connecting rod 1. n is the terminal joint, a n+1 is the end effector; a i is the i-th joint, i=1,2,…,n; G j is the jth target point, j = 1, 2, ..., m; the robot arm is initially arranged along the Y axis of the base coordinate system; S2: Based on the condition that the offset of each joint relative to the Y axis is the smallest, the objective function is constructed. The objective function is from a1 to a n-1 The weighted sum of squares of the offsets relative to the Y axis, and a n-1 The weights of a1 increase in order; S3: The physical rotation angle limit of each joint of the robot arm, the length of each link, the initial joint always moves along the Y axis, and the end joint reaches the target point G with the set posture j , the motion smoothness index is used as the constraint of the objective function; S4: Use a constrained nonlinear optimization problem solving algorithm to find the coordinates of each joint point when the objective function is minimized.

2. The inverse kinematics solution method for a serpentine redundant manipulator according to claim 1, characterized in that: The expression of the objective function is: In the formula, is the X coordinate of the ith joint, is the Z coordinate of the ith joint, Represents the square of the vertical distance from the i-th joint point to the horizontal rail.

3. The inverse kinematics solution method for a serpentine redundant manipulator according to claim 1, characterized in that: Among the constraints of the objective function, the physical rotation angle limit constraint expressions of each joint of the robot arm are as follows: q i ≤q max ,i=1,...,n In the formula, q i represents the rotation angle of the i-th joint, q max Represents the physical rotation limit of the joint.

4. The inverse kinematics solution method for a serpentine redundant manipulator according to claim 1, characterized in that: Among the constraints of the objective function, the expression of the length constraint of each link of the robot arm is as follows: |a i -a i+1 |=L i ,i=1,...,n Where, L i represents the length of the i-th connecting rod.

5. The inverse kinematics solution method for a serpentine redundant manipulator according to claim 1, characterized in that: Among the constraints of the objective function, the expression of the initial joint motion constraint is as follows: In the formula, is the X coordinate of the initial joint, is the Z coordinate of the initial joint.

6. The inverse kinematics solution method for a serpentine redundant manipulator according to claim 1, characterized in that: Among the constraints of the objective function, the expression of the end joint posture constraint is as follows: a n =G j a n+1 =a n +k j ·L n In the formula, k j Indicates that the end joint reaches the target point G j The direction vector of the end link, L n Represents the length of the nth connecting rod.

7. The inverse kinematics solution method for a serpentine redundant manipulator according to claim 1, characterized in that: Among the constraints of the objective function, the expression of the motion smoothness constraint is as follows: In the formula, represents the sum of the motions of the remaining n-1 joints of the robot except the end joint, a i Indicates that the end joint is at the target point G j The i-th joint point at time, a i ′ indicates that the end joint is located at the target point G j-1 The i-th joint point at time .

8. The inverse kinematics solution method for a serpentine redundant manipulator according to claim 1, characterized in that: In S4, the algorithm for solving the nonlinear optimization problem with constraints adopts the sequential quadratic programming method.

Images