A method for end-following motion control of a snake-like redundant manipulator

Abstract

The present invention discloses a method for controlling the end-following motion of a serpentine redundant manipulator, comprising the following steps: inputting a following mode and a following distance; the following modes include: forward following, backward following, and translational following; recording the current path of the serpentine redundant manipulator as the original path, and obtaining the current path data; calculating the coordinates of each joint of the current manipulator based on the rotation angle data of each joint; sequentially calculating the distance that each new joint point on the new path slides on the original path based on the triangles formed by the new paths obtained under different following modes and the original paths; then calculating the coordinates of each new joint point on the new path, and then calculating the pitch angle and yaw angle of each joint to achieve following motion. The method for controlling the end-following motion used in the present invention is simple to operate, highly efficient, and has a fast calculation speed.

Description

Technical Field

[0001] The present invention relates to the field of robotic arm control, and in particular to an end-following motion control method for a serpentine redundant robotic arm. Background Art

[0002] The degrees of freedom of a spatial object are 6. If you want to reach any position in space, the degrees of freedom of the robot arm must be greater than or equal to 6. However, the robot arm must also meet the need to avoid obstacles while reaching the working point. A robot arm with fewer degrees of freedom cannot effectively control its own posture during motion planning. The serpentine redundant robot arm has more operational freedom and can enter a small space and operate flexibly, thereby meeting the needs of actual operations. Therefore, it is of great significance to study the motion control method of the serpentine redundant robot arm.

[0003] The control methods of snake-like redundant robotic arms can generally be divided into two categories: path planning under known environmental models and sensor-based motion control under unknown environments.

[0004] Snake-shaped redundant robotic arms generally have more than ten degrees of freedom, which makes them suitable for use in confined environments. Compared with traditional articulated industrial robots, they can perform operations such as flexible obstacle avoidance and hole crossing. However, because snake-shaped redundant robotic arms have too many degrees of freedom, their control process is relatively cumbersome, with high computational requirements and slow speed. Especially when performing manual control in unknown environments, simple teaching control methods are no longer applicable. Summary of the Invention

[0005] In view of the shortcomings of the existing technology, the present invention proposes an end-following motion control method for a serpentine redundant robotic arm, which can ensure the realization of motion control such as spatial obstacle avoidance and visual navigation in a complex workspace with minimal operation.

[0006] The specific technical solutions are as follows:

[0007] A method for controlling the end-following motion of a serpentine redundant robotic arm includes three following modes: forward following, backward following, and translational following. In the forward following mode, the end joint of the robotic arm is controlled to move forward; in the backward following mode, the robotic arm is controlled to move backward; in the translational following mode, the end joint is controlled so that the end link maintains its forward direction and position in the forward direction unchanged, and moves perpendicular to the forward direction. The method for controlling the end-following motion of a serpentine redundant robotic arm includes the following steps:

[0008] S1: Enter the selected following mode and following distance; the following distance is the single travel length of the end of the robot arm or the base, which is less than the connecting rod length L of the robot arm;

[0009] S2: Record the current path of the serpentine redundant manipulator as the original path, and obtain the current path data of the serpentine redundant manipulator, the path data including: the angle data of each joint, the offset distance of the current base relative to the initial origin of the horizontal guide rail; calculate the coordinate position of each joint of the current serpentine redundant manipulator according to the angle data of each joint; the original path of the serpentine redundant manipulator adopts a0-a1-a2…a n-1 -a n Indicates that a0 represents the base joint, a1 represents the first joint, a2 represents the second joint, and a n-1 represents the n-1th joint, i.e. the terminal joint, a n Indicates the end of the robotic arm; a0a1 is the first link, a1a2 is the second link, a n-1 a n is the nth connecting rod, i.e. the end connecting rod;

[0010] S3: In the forward following mode, the end joint moves forward to follow the distance, thereby generating a new path, which adopts b0-b1-b2…b n-1 -b n Indicates that b0 represents the base joint under the new path, b1 represents the first joint under the new path, b2 represents the second joint under the new path, and b n-1 represents the n-1th joint in the new path, b n Represents the end of the robot under the new path; Δb is formed between the new path and the original path at the end joint n-2 a n-1 b n-1 Start solving triangles one by one to get the sliding distance of each new joint on the new path on the original path;

[0011] In the retract follow mode, the base retracts the follow distance to generate a new path, and the Δb formed between the new path and the original path at the first joint n-1 a n-1 b n Start solving triangles one by one to get the sliding distance of each new joint point on the new path on the original path;

[0012] In the translation follow mode, the Δb formed by the new path at the end joint and the original path n-2 a n-1 b n-1 Start solving triangles one by one to get the sliding distance of each new joint point on the new path on the original path;

[0013] S4: Calculate the coordinates of each new joint point on the new path based on the sliding distance of the new joint point on the original path;

[0014] S5: Based on the coordinates of each new joint point, calculate the pitch angle and yaw angle of each joint to achieve following motion.

[0015] Furthermore, the forward following includes following forward along the direction of the end link and following forward deviating from the direction of the end link; in the following mode of following forward along the direction of the end link, the end joint of the serpentine redundant robotic arm is controlled by the control end to make the end of the robotic arm move forward along the direction of the end link; in the following mode of following forward deviating from the direction of the end link, the end joint is controlled by the control end to make the movement direction of the end of the robotic arm deviate from the direction of the end link and move forward, that is, the end of the robotic arm does not move on the straight line where the end link is located.

[0016] Furthermore, in step S3, in the forward following mode along the end link direction, the sliding distance of each new joint point on the new path on the original path is expressed as follows:

[0017]

[0018] Where, l i The new joint point b i Sliding distance on the original path, i=1,2,……,n-1,l i-1 The new joint point b i-1 Sliding distance on the original path; γ i represents the angle between the i-1th connecting rod and the i-th connecting rod;

[0019] In the forward following mode that deviates from the end link direction, first calculate the sliding distance l of the end joint on the original path. n-1 , the expression is as follows:

[0020]

[0021] Where Δtip is the single travel length of the end of the robotic arm, γ n is the angle between the position of the end link on the original path and the target motion direction of the end of the manipulator;

[0022] Then from Δb n-1 a n b n Start solving triangles one by one. The expression for the distance each new joint point on the new path slides on the original path is the same as that calculated in the forward follow mode along the end link direction.

[0023] In the retract follow mode, the sliding distance of each new joint point on the new path on the original path is expressed as follows:

[0024]

[0025] In the translational follow mode, the expression for the distance each new joint point on the new path slides on the original path is calculated in the same way as in the forward follow mode along the end link direction.

[0026] Furthermore, in step S4, the coordinates of each joint point on the new path are calculated by homogeneous coordinate transformation according to the sliding distance of the new joint point on the original path. The expression is as follows:

[0027]

[0028] Where, is the i-th joint point a on the original path i coordinates; is the i-th joint point a fixed on the original path i The component of the x-axis of the relative coordinate system on the base coordinate system, is the component of the y-axis of the relative coordinate system in the base coordinate system, is the component of the z-axis of the relative coordinate system in the base coordinate system; is the i-th joint point b on the new path i Coordinates of l i The new joint point b i The sliding distance on the original path.

[0029] Furthermore, in step S5, the pitch angle θ and yaw angle α of each joint are obtained according to the coordinates of the new joint point, and the expressions are as follows:

[0030]

[0031]

[0032]

[0033]

[0034]

[0035] Where, is the i-th joint point b on the new path i coordinates; is the i-1th joint point b fixed on the new path i-1 The component of the x-axis of the relative coordinate system on the base coordinate system, is the component of the y-axis of the relative coordinate system in the base coordinate system, is the component of the z-axis of the relative coordinate system in the base coordinate system; is the pitch angle of the i-th joint on the new path, is the yaw angle of the i-th joint on the new path.

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

[0037] The present invention adopts three following modes: forward following, backward following and translation following, so that the multi-joint serpentine redundant robotic arm can automatically avoid obstacles as long as the end joint does not hit the obstacle during operation. It has the characteristics of simple operation, high efficiency and fast calculation speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 Schematic diagram of the serpentine redundant robotic arm device used in the present invention.

[0039] Figure 2 It is a schematic diagram of the robot arm following the end-following motion control method in the present invention.

[0040] Figure 3 It is a flow chart of the terminal following motion control method of the present invention.

[0041] Figure 4 It is a schematic diagram of the robot arm following in the forward following mode along the direction of the end link of the present invention.

[0042] Figure 5 It is a schematic diagram of the robot arm following in the forward following mode deviating from the direction of the end link of the present invention.

[0043] Figure 6 Schematic diagram of the robot arm following in the retreat following mode of the present invention.

[0044] Figure 7 Schematic diagram of the robot arm following in the translation following mode of the present invention.

[0045] Figure 8 It is a joint schematic diagram when solving the joint angle based on the joint point coordinates, where (a) is a schematic diagram of the yaw angle of the robotic arm joint, and (b) is a schematic diagram of the pitch angle of the robotic arm joint.

[0046] Figure 9 This is a typical working process of the serpentine redundant robotic arm in an embodiment of the present invention. DETAILED DESCRIPTION

[0047] The present invention will be described in detail below based on the accompanying drawings and preferred embodiments. The purpose and effects of the present invention will become more apparent. 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 for explaining the present invention and are not intended to limit the present invention.

[0048] The essence of the end-following motion control method for a serpentine redundant robot arm is to make the new path fit the original path as closely as possible. However, due to its structural characteristics, a multi-joint redundant robot arm cannot completely fit the original path.

[0049] The present invention will be as follows Figure 1 The path of the serpentine redundant manipulator is shown as Figure 2 The broken line shown indicates that the path fitting is achieved by sliding the new path on the original path. The serpentine redundant manipulator includes: a base, connecting rods, joints, and guide rails. The base can slide on the guide rails and can only move in a straight line; the adjacent connecting rods are connected by joints, and the first connecting rod is connected to the base by the base joint. The shape and path of the manipulator are controlled by controlling the changes in the joint angle. An angle sensor is installed on each joint of the serpentine redundant manipulator to monitor the joint angle in real time; a visual feedback device (such as a camera) is installed on the end joint to observe the situation in front of the end joint through the camera, and the operation control end performs corresponding control.

[0050] The following modes of the serpentine end-of-arm motion include forward following, retracting following, and translational following. Forward following includes both forward following in the direction of the end-of-arm link and forward following away from the end-of-arm link. The initial position of the serpentine redundant manipulator base is defined as the origin of the base coordinate system. Starting from this origin, the vertical upward direction is the Z axis, and the horizontal direction along the base (i.e., the direction of the guide rails) is the Y direction. The direction of the X axis is determined according to the right-hand rule of the three-dimensional coordinate system.

[0051] In the forward following mode, the end joint of the robot arm is controlled to move forward, and the other joints of the robot arm will automatically move forward along the path that the end joint of the robot arm has already traveled. In the "Follow forward along the end link direction" following mode, the end joint of the serpentine redundant robot arm is controlled by the control end to make the end of the robot arm move forward along the end link direction; Figure 4 For example, the direction of the end link a2a3 is the positive direction of the Y axis, and the end of the robot a3 moves forward along the positive direction of the Y axis. In the "Follow forward with deviation from the end link direction" following mode, the control end controls the end joint of the robot arm so that the movement direction of the end arm deviates from the direction of the end link and moves forward (that is, the end arm does not move on the straight line where the end link is located); Figure 5 For example, the end a2 of the robot arm does not move on the straight line where the end link a1a2 is located. The end joint a1 moves forward, pushing the end of the robot arm to move along the a2G direction to point b2.

[0052] In the "return follow" following mode, the base joint of the serpentine redundant robot arm is controlled by the control end to translate backward (that is, move along the negative direction of the Y axis. This is because the layout direction of the guide rail is fixed and there is no other movement direction), so that the robot arm moves backward along the path it has traveled during forward following or translational following.

[0053] In the "translation follow" follow mode, control the end link of the robot arm to maintain the forward direction (with Figure 7 For example, the end links a2a3 are always parallel to the Y axis) and the position in the forward direction remains unchanged (with Figure 7 For example, keep the coordinates of the connecting rods a2a3 on the Y axis unchanged), and move perpendicular to the forward direction in the set direction (with Figure 7 For example, the end a3 of the robot arm can move in the XOZ plane, and the end link a2a3 in the figure moves upward by a distance Δtip).

[0054] like Figure 3 As shown, the end-following motion control method for a snake-like redundant robotic arm includes the following steps:

[0055] S1: Enter the selected following mode and following distance Δtip, where the following distance Δtip is the single travel length of the end of the robot arm or the base, and Δtip is less than the connecting rod length of either robot arm.

[0056] S2: Record the current position of the serpentine redundant manipulator as the original position, and obtain the current path of the serpentine redundant manipulator as the original path, where the path data includes: the angle data of each joint, the offset distance of the current base relative to the initial origin of the horizontal guide rail. According to the current angle of each joint, calculate the coordinate position of each joint of the current serpentine redundant manipulator. When the serpentine redundant manipulator has n joints, the path of each joint adopts the broken line a0-a1-a2…a n-1 -a n Indicates that a0 represents the base joint, a1 represents the first joint, a2 represents the second joint, and a n-1 represents the n-1th joint, i.e. the terminal joint, a n Indicates the end of the robotic arm. a0a1 is the first link, a1a2 is the second link, a n-1 a n is the nth connecting rod, that is, the end connecting rod, and the length of each connecting rod is L.

[0057] S3: Solve the corresponding triangles according to the corresponding follow mode and follow distance Δtip, and obtain the sliding distance of the new joint point on the original path and the displacement Δbase of the base. The specific operations are performed according to the follow mode:

[0058] (1) When the following mode is to follow the end link forward, if the end joint moves forward along the end link by a distance of Δtip, a new path is generated. When generating a new path, the path of each joint adopts b0-b1-b2…b n-1 -b n To describe, b0 represents the base joint under the new path, b1 represents the first joint under the new path, b2 represents the second joint under the new path, and b n-1 represents the n-1th joint in the new path, b nIndicates the end of the robot arm under the new path. The new path will be in the original path a0-a1-a2…a n Sliding upward, several triangles are generated between the new path and the original path. The sliding distance of the new joint point on the original path is obtained according to the triangle solution, and then the displacement Δbase of the base is obtained.

[0059] like Figure 4 As shown, the new path and the original path form Δb1a2b2 and Δb0a1b1, where the side length corresponding to Δb1a2b2 is represented by l2 and l a2 、l t2 Indicates that l2 is the sliding distance of the new joint point b2 on the original path, and it is easy to know that l2 = Δtip; l a2 is the length of b1b2, that is, the connecting rod length L. Since the data of the original path is known, that is, the coordinates of points a0, a1, a2, and a3 are known, and the data of each joint angle are known, according to the sine theorem and cosine theorem of the triangle, the expression for the length of the side length l1 in Δb1a2b2 is obtained as follows:

[0060]

[0061] Where l1 is the sliding distance of the new joint point b1 on the original path, and γ2 is the angle between the second link and the third link (i.e., the end link).

[0062] According to the above special case, when several triangles are generated between the new path and the original path, the Δb formed by the end joint (i.e. the n-1th joint) n-2 a n-1 b n-1 Start solving each triangle one by one. The sliding distance expression of each new joint point on the original path is as follows:

[0063]

[0064] Where, l i The new joint point b i Sliding distance on the original path, i=1,2,……,n-1,l i-1 The new joint point b i-1 Sliding distance on the original path, γ i represents the angle between the i-1th link and the i-th link.

[0065] (2) Figure 5As shown in the figure, when the following mode is to follow forward while deviating from the direction of the end link, the target motion direction of the end of the manipulator can be set to the virtual path (i.e., the line connecting the target point G and the original position of the end of the manipulator), and the end of the manipulator moves along the virtual path. While the end joint bends, the end link follows forward as a whole. By changing the direction of the virtual path, the forward direction of the end link is changed to obtain a new path. The new path forms Δb0a1b1, Δb1a2b2…, Δb1a2b2 between the new path and the original path. n-2 a n-1 b n-1 , Δb n-1 a n b n , Figure 5 Only n=2, Δb n-1 a n b n That is Δb1a2b2, and the sliding distance of the end of the robotic arm on the original path is l2 = Δtip.

[0066] according to Figure 5 The special case shown is extended to obtain the sliding distance l of the end joint on the original path n-1 The calculation expression is:

[0067]

[0068] Where, γ n is the angle between the position of the end link on the original path and the virtual path.

[0069] The expressions of the sliding distances of the remaining new joints on the original path are the same as those in formula (2). This following mode is mainly used in scenarios where the robot arm needs to quickly approach the target point.

[0070] (3) Figure 6 As shown in the figure, when the follow mode is retract follow, the base retreats along the guide rail by a distance of Δbase to generate a new path. When the robot arm retreats a distance of Δbase, the calculation method starts from the first joint, and the new path forms Δb1a1b2, Δb2a2b3…, Δb with the original path. n-1 a n-1 b n , the sliding distance l of each new joint point on the original path i The expression is:

[0071]

[0072] Where, γ i-1 represents the angle between the i-2th connecting rod and the i-1th connecting rod.

[0073] (4) Figure 7As shown, when the following mode is translation following, the end link of the robot arm remains unchanged in the direction of the end forward, and the movement direction of the end link is perpendicular to the forward direction Y. If the translation distance of the end link is Δtip, the calculation method starts from the end joint, connects a2b2, and the new path forms Δb0a1b1, Δb1a2b2…, Δb n-2 a n-1 b n-1 At this time, the triangles can be solved in sequence according to the method in (1) to obtain the sliding distance of each new joint point on the original path.

[0074] S4: According to the sliding distance of the new joint point on the original path, the new path b0-b1-b2…b is calculated by homogeneous coordinate transformation. n The coordinates of each joint point are expressed as follows:

[0075]

[0076] Where, is the i-th joint point a on the original path i coordinates; is the i-th joint point a fixed on the original path i The component of the x-axis of the relative coordinate system on the base coordinate system, is the component of the y-axis of the relative coordinate system in the base coordinate system, is the component of the z-axis of the relative coordinate system in the base coordinate system; is the i-th joint point b on the new path i 's coordinates.

[0077] S5: If Figure 8 As shown, according to the coordinates of the new joint points b0, b1, b2..., the pitch angle θ and yaw angle α of each joint are obtained, and the expressions are as follows:

[0078]

[0079]

[0080]

[0081]

[0082]

[0083] in, is the pitch angle of the i-th joint on the new path, is the yaw angle of the i-th joint on the new path.

[0084] The pitch angle θ1 and yaw angle α2 of the first joint can be calculated using equations (6.1) to (6.2), and the pitch angles and yaw angles of the remaining joints can be calculated in sequence using equations (7.1) to (7.3).

[0085] S6: The control end sends the joint angle instructions calculated in step S5 to the serpentine redundant robotic arm, and the serpentine redundant robotic arm changes the joint angles according to the instructions, thereby achieving follow-up motion.

[0086] like Figure 9 As shown in the figure, the redundant serpentine robot arm needs to perform a variety of following modes in a typical work process with the goal of entering a pipeline:

[0087] like Figure 9 As shown in ①~④, the serpentine redundant manipulator executes the instruction to follow the direction of the end link forward, adjusts the direction of the end link upward and moves forward, quickly approaching the target point; after reaching a certain height, as shown in ⑤~⑥, the serpentine redundant manipulator continues to execute the instruction to follow the direction of the end link forward, adjusts the direction of the end link downward and moves forward, reaching the target point. Figure 9 When the end joint is in position ⑥, it is close to horizontal but there is a certain deviation from the target position.

[0088] At this time, as shown in ⑥ to ⑦, the serpentine redundant robotic arm executes the terminal translation follow instruction, and the terminal moves downward to align with the target pipe opening. Finally, as shown in ⑦ to ⑧, the serpentine redundant robotic arm executes the forward follow instruction along the terminal link direction until the serpentine redundant robotic arm reaches the target position. After completing the action, the serpentine redundant robotic arm can execute the retreat follow instruction and return along a safe path or along the original path.

[0089] The present invention proposes a "snake-like" control method of end-following. When the snake moves, each part of its body will follow the route of the head. Applying this movement method to the serpentine redundant robotic arm results in a new type of motion control method: only the direction of the end and the overall forward and backward movement are controlled, and the remaining joints follow the path traveled by the end. The advantage of this control is that as long as the end does not hit an obstacle, the remaining joints can automatically avoid obstacles. It is particularly suitable for serpentine redundant robotic arms with machine vision at the end. Motion control is performed based on sensors in unknown environments. Under visual feedback, the operating control end controls the end joints or the base to enter the unknown area to achieve obstacle avoidance and path planning. The control of the serpentine redundant robotic arm is achieved by controlling the direction of the end joints of the robotic arm and the forward movement of the end joints or the backward movement of the base.

[0090] Those skilled in the art will understand that the foregoing descriptions are merely preferred embodiments of the invention and are not intended to limit the invention. Although the invention has been described in detail with reference to the foregoing examples, those skilled in the art will still be able to modify the technical solutions described in the foregoing examples or substitute equivalents for some of the technical features therein. Any modifications, equivalent substitutions, etc. made within the spirit and principles of the invention shall be included within the scope of protection of the invention.

Claims

1. A method for controlling the end-following motion of a serpentine redundant manipulator, characterized in that: The invention comprises three following modes: forward following, backward following, and translation following. In the forward following mode, the end joint of the robot arm is controlled to move forward. In the backward following mode, the robot arm is controlled to move backward. In the translation following mode, the end joint is controlled so that the end link maintains the forward direction and the position in the forward direction unchanged, and moves perpendicular to the forward direction. The end following motion control method for the serpentine redundant robot arm comprises the following steps: S1: Enter the selected following mode and following distance; the following distance is the single travel length of the end of the robot arm or the base, which is less than the connecting rod length L of the robot arm; S2: Record the current path of the serpentine redundant manipulator as the original path, and obtain the current path data of the serpentine redundant manipulator, the path data including: the angle data of each joint, the offset distance of the current base relative to the initial origin of the horizontal guide rail; calculate the coordinate position of each joint of the current serpentine redundant manipulator according to the angle data of each joint; the original path of the serpentine redundant manipulator adopts a0-a1-a2…a n-1 -a n Indicates that a0 represents the base joint, a1 represents the first joint, a2 represents the second joint, and a n-1 represents the n-1th joint, i.e. the terminal joint, a n Indicates the end of the robotic arm; a0a1 is the first link, a1a2 is the second link, a n-1 a n is the nth connecting rod, i.e. the end connecting rod; S3: In the forward following mode, the end joint moves forward to follow the distance, thereby generating a new path, which adopts b0-b1-b2…b n-1 -b n Indicates that b0 represents the base joint under the new path, b1 represents the first joint under the new path, b2 represents the second joint under the new path, and b n-1 represents the n-1th joint in the new path, b n Represents the end of the robot under the new path; Δb is formed between the new path and the original path at the end joint n-2 a n-1 b n-1 Start solving triangles one by one to get the sliding distance of each new joint on the new path on the original path; In the retract follow mode, the base retracts the follow distance to generate a new path, and the Δb formed between the new path and the original path at the first joint n-1 a n-1 b n Start solving triangles one by one to get the sliding distance of each new joint point on the new path on the original path; In the translation follow mode, the Δb formed by the new path at the end joint and the original path n-2 a n-1 b n-1 Start solving triangles one by one to get the sliding distance of each new joint point on the new path on the original path; S4: Calculate the coordinates of each new joint point on the new path based on the sliding distance of the new joint point on the original path; S5: Based on the coordinates of each new joint point, calculate the pitch angle and yaw angle of each joint to achieve following motion.

2. The end-following motion control method for a serpentine redundant manipulator according to claim 1, characterized in that: The forward following includes following forward along the direction of the end link and following forward deviating from the direction of the end link; in the following mode of following forward along the direction of the end link, the end joint of the serpentine redundant robotic arm is controlled by the control end to make the end of the robotic arm move forward along the direction of the end link; in the following mode of following forward deviating from the direction of the end link, the end joint is controlled by the control end to make the movement direction of the end of the robotic arm deviate from the direction of the end link and move forward, that is, the end of the robotic arm does not move on the straight line where the end link is located.

3. The end-following motion control method for a serpentine redundant manipulator according to claim 2, characterized in that: In step S3, in the forward following mode along the end link direction, the sliding distance of each new joint point on the new path on the original path is expressed as follows: Where, l i The new joint point b i Sliding distance on the original path, i=1,2,……,n-1,l i-1 The new joint point b i-1 Sliding distance on the original path; γ i represents the angle between the i-1th connecting rod and the i-th connecting rod; In the forward following mode that deviates from the end link direction, first calculate the sliding distance l of the end joint on the original path. n-1 , the expression is as follows: Where Δtip is the single travel length of the end of the robotic arm, γ n is the angle between the position of the end link on the original path and the target motion direction of the end of the manipulator; Then from Δb n-1 a n b n Start solving triangles one by one. The expression for the distance each new joint point on the new path slides on the original path is the same as that calculated in the forward follow mode along the end link direction. In the retract follow mode, the sliding distance of each new joint point on the new path on the original path is expressed as follows: In the translational follow mode, the expression for the distance each new joint point on the new path slides on the original path is calculated in the same way as in the forward follow mode along the end link direction.

4. The end-following motion control method for a serpentine redundant manipulator according to claim 1, characterized in that: In step S4, the coordinates of each joint point on the new path are calculated by homogeneous coordinate transformation according to the sliding distance of the new joint point on the original path. The expression is as follows: Where, is the i-th joint point a on the original path i coordinates; is the i-th joint point a fixed on the original path i The component of the x-axis of the relative coordinate system on the base coordinate system, is the component of the y-axis of the relative coordinate system in the base coordinate system, is the component of the z-axis of the relative coordinate system in the base coordinate system; is the i-th joint point b on the new path i Coordinates of l i The new joint point b i The sliding distance on the original path.

5. The end-following motion control method for a serpentine redundant manipulator according to claim 1, characterized in that: In step S5, the pitch angle θ and yaw angle α of each joint are obtained according to the coordinates of the new joint point, and the expressions are as follows: Where, is the i-th joint point b on the new path i coordinates; is the i-1th joint point b fixed on the new path i-1 The component of the x-axis of the relative coordinate system on the base coordinate system, is the component of the y-axis of the relative coordinate system in the base coordinate system, is the component of the z-axis of the relative coordinate system in the base coordinate system; is the pitch angle of the i-th joint on the new path, is the yaw angle of the i-th joint on the new path.

Images