A snake path tracking method with corner constraints

Abstract

The present invention discloses a serpentine path tracking method with angle limit, comprising: setting a target path for a serpentine robot arm; using a sequential search method to roughly obtain the position range of each joint point in sequence; using an interpolation method to obtain the precise position of each joint point; iteratively replanning the node position when the deviation from the target path is minimized until the rotation angle of each joint is within the rotation angle limit; and setting an angle q to obtain the position of each joint point. set Iterate for the target, q set Less than the rotation angle limit q max , and the difference between the two is the iteration tolerance δ, which is set according to actual needs. The method of the present invention can efficiently enable the snake-like robot arm to track the target path with a small deviation within the rotation angle limit.

Description

Technical Field

[0001] The present invention relates to the field of path planning of redundant robotic arms, and in particular to a serpentine path tracking method with angle restriction. Background Art

[0002] When planning the motion of a serpentine manipulator with a known target, a safe path can be planned based on the relative positions of the serpentine manipulator and the target. Motion control methods are then used to ensure that the serpentine manipulator moves along the given path and ultimately reaches the target point. When the serpentine manipulator moves along the specified path, all its links are controlled to move along the given path with minimal error. This type of motion is called serpentine path tracking. However, the serpentine manipulator has physical rotational limits. When following a given path, the manipulator's joint angles may exceed these limits, causing the manipulator to become unable to move. Common end-point following algorithms cannot address this problem. Simply limiting the manipulator's angle to the maximum joint angle when the limit is exceeded can cause the manipulator to deviate significantly from the ideal path. While numerical path planning methods can address this problem, they are computationally intensive and time-consuming, failing to meet the requirements of real-time manipulator control. Therefore, an efficient path planning method is needed that allows the serpentine manipulator to track the ideal path with minimal error within the rotational limits. Summary of the Invention

[0003] In view of the shortcomings of the prior art, the present invention proposes a serpentine path tracking method with angle limitation.

[0004] The specific technical solutions are as follows:

[0005] A snake path tracking method with corner restriction includes the following steps:

[0006] S1: Set the target path of the snake-like robot arm, which includes multiple target points, where G j is the jth target point; the robotic arm includes a base, a connecting rod, and a joint. Two adjacent connecting rods are connected by joints. a1 is the initial joint of the robotic arm, connecting the base and connecting rod 1; a n is the terminal joint, a i is the i-th joint, i=1,2,…,n; a i-1 with a i The length of the connecting rod between i ;

[0007] S2: Use sequential search method, from a1 to a n Roughly obtain the position range of each joint point in turn;

[0008] S3: Use interpolation method to obtain the precise position of each joint;

[0009] S4: When the deviation from the target path is minimized, the node positions are replanned iteratively until the rotation angles of all joints are within the rotation limits. The principle of replanning the node positions is:

[0010] For three adjacent joint points a with known positions i-1 、a i 、a i+1 , calculate joint a i The corresponding joint angle, if it exceeds the angle limit, keep the joint point a i-1 Position remains unchanged, re-plan a i and a i+1 Position to a′ i and a′ i+1 ; a′ i and a′ i+1 Are located by a i-1 、a i and a i+1 On the plane formed by the three points, and a′ i+1 Located on the given target path; calculate line segment a based on geometric relationships i-1 a′ i+1 The length of a′ is calculated using the method described in S2 and S3. i+1 The position of a′ is calculated based on spatial geometry i location;

[0011] Will and The included angle is taken as the setting angle q set , set the angle q set Less than the rotation angle limit q max , and the difference between the two is the iteration tolerance δ, which is set according to actual needs; set Iterate for the goal.

[0012] Furthermore, the specific operation of S2 is: when a i-1 When it is known, start from the nearest target point and calculate a i-1 and the Euler distance between the target point and L i For comparison, if |a i-1 G j | <L i and|a i-1 G j+1 |>L i , then a i Located in G j and G j+1 between.

[0013] Furthermore, in S3, the interpolation method is trigonometric method, and the specific operation is as follows:

[0014] G j and G j+1 The path curve between them is approximately a straight line segment, G j with a i-1 The distance is p j , G j+1 with a i-1 The distance is p j+ 1,

[0015] In Δa i-1 G j G j+1 According to the cosine theorem, we get:

[0016]

[0017] In Δa i a i-1 G j+1 According to the law of cosines, we get the equation:

[0018]

[0019] Set constraint 10·|G j G j+1 |<|a i-1 a i |, |a i-1 a i |>|a i G j+1 |,p j+1 >|a i G j+1 |; get|a i G j+1 |For:

[0020]

[0021] According to the geometric relationship, we get a i The position expression is as follows:

[0022]

[0023] Furthermore, in S3, the interpolation method uses the proportional approximation method, and the specific operation is as follows:

[0024] G j and G j+1 The path curve between them is approximately a straight line segment, G j with a i-1 The distance is pj , G j+1 with a i-1 The distance is p j+1 ; Take joint point a i-1 is the center of the circle, p j Draw an arc for the arc length, which is the same as a i-1 a i The intersection of the lines is marked as point A, and i G j+1 The intersection of the lines is marked as point B; the joint point a i-1 is the center of the circle, L i Draw an arc for the arc length, which is the same as a i G j+1 The intersection of the lines is marked as point C;

[0025] According to spatial geometry, the coordinate expression of point B is as follows:

[0026] B=a i-1 +λ1(G j+1 -a i-1 )

[0027]

[0028] Assume Δa i AG j Similar to ΔG j+1 BG j , according to the triangle similarity principle, we get:

[0029]

[0030] Then get a i The coordinates of are expressed as follows:

[0031] a i =a i-1 +λ3(Aa i-1 )=a i-1 +λ3·(G j -a i-1 +λ2·(a i-1 -G j +λ1·(G j-1 -a i-1 )))

[0032] A=G j +λ2(BG j )

[0033]

[0034] Furthermore, in S4, re-planning the node positions in an iterative manner is specifically implemented by the following sub-steps:

[0035] (4.1) Calculate the joint rotation angles of each joint in the new configuration in sequence; the joint rotation angle q of the i-th joint is i+1 The expression is as follows:

[0036]

[0037] (4.2) Sequentially determine whether the rotation angle of each joint exceeds the rotation angle limit. If so, re-plan the position of the joint point; if not, proceed to determine the next joint rotation angle;

[0038] (4.3) After completing the replanning of all joint points in sequence, recalculate the equivalent rotation angle of each joint. If any joint angle exceeds the rotation limit, the current joint point and the remaining target points are used as the subsequent follow-up path and step (4.2) is repeated to set the angle q set Repeat multiple iterations for the goal until all joint equivalent angles are within the angle limit q max Within.

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

[0040] The method of the present invention uses a heuristic algorithm to quickly search for the joint point positions, and then iteratively fine-tunes the joint point positions to achieve angle limitation, so that the robotic arm can track the target path with a small deviation within the maximum angle. It has the characteristics of fast calculation speed, high path tracking accuracy, good portability and strong versatility. This method can be extended to any multi-joint serial robotic arm to achieve real-time path planning with angle limitation. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 4 is a flow chart of a serpentine path tracking method with corner restriction in an embodiment of the present invention.

[0042] Figure 2 Schematic diagram of the snake-like robotic arm and its path target points when performing path tracking in an embodiment of the present invention.

[0043] Figure 3 Schematic diagrams of the sequential and reverse order search methods for joint points in an embodiment of the present invention, wherein (a) is a schematic diagram of the sequential search method, and (b) is a schematic diagram of the reverse order search method.

[0044] Figure 4 Schematic diagram of the positions of target points and joint points in the discretized path in an embodiment of the present invention.

[0045] Figure 5 Schematic diagram of determining the position of a joint point by triangulation in an embodiment of the present invention.

[0046] Figure 6Schematic diagram of determining the position of a joint point by using a proportional approximation method in an embodiment of the present invention.

[0047] Figure 7 3 is a schematic diagram of applying the rotation angle restriction when the connecting rod lengths are equal in an embodiment of the present invention.

[0048] Figure 8 2 is a schematic diagram of applying the rotation angle restriction when the connecting rods have different lengths in an embodiment of the present invention.

[0049] Figure 9 Schematic diagram of the configuration of each joint angle restriction applied through an iterative algorithm in an embodiment of the present invention, wherein (a) is a new configuration of the serpentine robotic arm obtained by using a joint point sequential search method, (b) is the configuration of the serpentine robotic arm after replanning the first joint angle, (c) is the configuration of the serpentine robotic arm after replanning the second joint angle, and (d) is the configuration of the serpentine robotic arm after completing the iteration. DETAILED DESCRIPTION

[0050] 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.

[0051] For a multi-joint serpentine redundant manipulator with universal joints between two adjacent links, the serpentine manipulator configuration is simplified. The universal joint connecting the two links is regarded as a point, called a joint point, and the link is simplified into a line segment. The entire serpentine manipulator can be simplified into a broken line segment. Figure 1 As shown, it is assumed that the snake-like robot arm has n links and n joints, that is, from a1 to a n+1 , describes the macroscopic geometric features of the snake-like manipulator, where a1 is the initial joint of the manipulator, which connects the base and link 1, joint a2 connects link 1 and link 2, and so on. n The end joint of the robotic arm, also known as the wrist joint, n+1 is the end effector of the robotic arm, a i is the i-th joint, i=1,2,…,n; a i-1 with a i The length of the connecting rod between i . Use a1 to a n+1 The resulting broken line segments are connected sequentially to describe the macroscopic geometric features of the snake-like robot arm, which can be called the snake-like robot arm's configuration. During operation, the snake-like robot arm's joints gradually move to form a new configuration.

[0052] The present invention proposes a serpentine path tracking method with angle restriction, such as Figure 1 As shown, the specific steps include:

[0053] The first stage: adopt the joint point sequential search method to obtain a new configuration in which all joint points are located on the target path.

[0054] S1: Set the target path of the snake robot arm, such as Figure 2 As shown, the target path is discretized into multiple target points, where G j is the jth target point. The discretization of the path curve can be achieved through different methods, such as equal parameters, equal chord length, equal arc length, etc.

[0055] In each step of the serpentine robot's movement, the serpentine robot and the target path can be fitted with the minimum error to obtain a new configuration of the robot. Since the configuration of the serpentine robot is determined by the position of each joint point and the ideal path is discretized into multiple target points, the key to the serpentine path tracking method lies in how to quickly find suitable joint points so that the distance between each joint point of the robot and the target point in the target path is minimized and the rotation angle of each joint is within the limit.

[0056] S2: Based on the current configuration of the snake-like robot arm, a sequential search method is used to search for each joint point in the new configuration to obtain the rough position of each joint point.

[0057] Based on the current configuration of the robot arm, the joint points in the new configuration are searched. According to the search order of the joint points, the search method of the joint points can be divided into sequential search and reverse search. Sequential search is as follows Figure 3 As shown in (a), in each step of the path tracking method, the driving module installed on the horizontal slide drives the base to move a fixed distance s in the direction of the given path, and then searches for the joint points in the order from the initial joint to the end of the robot arm. Figure 3 As shown in (b), in each step of the path tracking method, the target point in the path is directly used as the position of the end joint. Then, the joint points are searched in the order from the end of the robot arm to the initial joint, and finally the movement amount of the horizontal slide is determined. The present invention adopts a sequential search method.

[0058] In the sequential search method, when a i-1 When it is known, a i The search method is as follows Figure 4 As shown in a i-1 Find the distance a on the target path after point i-1 For L i Specifically, from the nearest target point (G j-3 ) starts, and calculates a i-1 and the Euler distance between the target point and L i Compare until the distance is greater than L i If |ai-1 G j | <L i and|a i-1 G j+1 |>L i , then a i Must be located in G j and G j+1 between.

[0059] S3: When using target points to describe the path, G j and G j+1 The path information between the two is lost. The present invention proposes two interpolation methods, namely triangulation or proportional approximation, to determine a i location.

[0060] (1) Figure 5 As shown in FIG, the method of determining the position of the joint point by triangulation is specifically implemented by the following operations:

[0061] G j and G j+1 The path curve between can be regarded as a straight line segment, and a is determined by solving the triangle i Position. j With the previous joint point a i-1 The distance is denoted as p j , the corresponding G j+1 with a i-1 The distance is denoted as p j+1 . |a i-1 a i | is the length of the i-th connecting rod L i ,When selecting the sampling of the path target points, in order to ensure the tracking accuracy, the target point spacing |G j G j+1 |Needs to be much smaller than the connecting rod length|a i-1 a i |, such as 10·|G j G j+1 |<|a i-1 a i In addition, the desired path should have a suitable curvature so that it can be tracked by the snake robot arm. i G j+1 a i-1 Recorded as It should be a smaller angle, that is when When the desired path curvature is too large, the snake-shaped robot arm is difficult to track the curvature. This invention does not consider The situation at that time.

[0062] In Δa i-1 G j Gj+1 According to the cosine theorem, we get:

[0063]

[0064] In Δa i a i-1 G j+1 According to the law of cosines, we get the equation:

[0065]

[0066] This equation is a quadratic equation with multiple solutions. As the constraints have been given in the previous article: 10·|G j G j+1 |<|a i-1 a i |, and|a i-1 a i |>|a i G j+1 |,p j+1 >|a i G j+1 |, according to the geometric relationship, it is easy to conclude |a i G j+1 | should be the smaller solution in the equation, so we can get |a i G j+1 The expression for | is:

[0067]

[0068] Finally, through G j G j+1 The slope of G j+1 The coordinates of a i The coordinates of are expressed as follows:

[0069]

[0070] Each joint point in the new configuration of the snake-like manipulator can be calculated in sequence using S2 and triangulation. The algorithm for finding a new configuration is a process of continuous comparison, solving triangles, and finding new points.

[0071] (2) Figure 6 As shown, the proportional approximation method is used to determine the joint point a i The location method is implemented through the following operations:

[0072] Take joint point a i-1 is the center of the circle, p j Draw an arc for the arc length, which is the same as a i-1 a iThe intersection of the lines is marked as point A, and i G j+1 The intersection of the lines is marked as point B; the joint point a i-1 is the center of the circle, L i Draw an arc for the arc length, which is the same as a i G j+1 The intersection of the lines is marked as point C.

[0073] According to the principles of spatial analytic geometry, the coordinate calculation expression of point B is as follows:

[0074] B=a i-1 +λ1(G j+1 -a i-1 )

[0075]

[0076] Assume Δa i AG j Similar to ΔG j+1 BG j , according to the similarity principle of triangles, we can get:

[0077]

[0078] According to spatial analytic geometry, the coordinate calculation expression of point A is as follows:

[0079] A=G j +λ2(BG j )

[0080] After obtaining the coordinates of point A, it is known that |a i-1 a i The length of | is L i , we can calculate a i The coordinates of are expressed as follows:

[0081] a i =a i-1 +λ3(Aa i-1 )=a i-1 +λ3·(G j -a i-1 +λ2·(a i-1 -G j +λ1·(G j-1 -a i-1 )))

[0082]

[0083] The second stage: For joints that exceed the angle limit, the positions of their joint points are replanned so that they meet the angle limit with the minimum deviation from the target path.

[0084] S4: Due to the physical structure limitations of the snake-like manipulator, the rotation angle between each two links of the manipulator (i.e., the joint angle, in the broken line configuration, the joint angle q i+1 That is and The angle between the joints (the angle between the joints) must not exceed their angular limits to avoid interference. Depending on the design of the serpentine manipulator, the angular limits are generally between 20° and 35°. This angular limit affects the minimum turning radius and path deviation of the serpentine manipulator. Furthermore, increasing the joint angular limits reduces the internal space of the manipulator's linkages. To address the issue of exceeding the angular limits in serpentine manipulator motion planning, angular constraints are necessary.

[0085] The idea behind joint constraint methods is to reposition joint points to meet joint rotation constraints. After obtaining a new configuration for the snake-like manipulator using the method described above, the rotation angle of each joint is analyzed. If a joint angle exceeds the rotation constraint, the joint position is repositioned to ensure that the joint meets the rotation constraint while minimizing deviation from the target path.

[0086] First, the principle of replanning the position of joint points is given: for three adjacent joint points, the joint points are replanned by solving the triangle they constitute, thereby simplifying the complex three-dimensional problem into a two-dimensional problem.

[0087] like Figure 7 The figure shows the schematic diagram of applying the rotation angle restriction when all the links of the robot arm are of equal length (all L), where the joint point a in the new configuration is i-1 、a i 、a i+1 The method of S2 and S3 has been used to find the joint angle q of the i-th joint. i+1 The expression is as follows:

[0088]

[0089] When q i+1 Exceeding the joint angle limit q max When maintaining joint point a i-1 Position remains unchanged, re-plan a i and a i+1 Position to a′ i and a′ i+1 , to meet the joint angle limit. i-1 a′ i a′ i+1 In the i+1 Located on the given target path, and The angle between the two is called the setting angle q set , denoted as q set. Set the angle q set Slightly smaller than the rotation angle limit q max , and the difference between the two is the iteration tolerance δ (an appropriate value is set by humans). i and a′ i+1 Located by a i-1 、a i and a i+1 On the plane formed.

[0090] In Δa i-1 a′ i a′ i+1 According to the geometric relationship, we can get:

[0091] |a i-1 a i ' +1 |=2L·cos(q set / 2)

[0092] At this time, a′ i+1 The location can be accessed through |a i-1 a′ i+1 | and a i-1 The position of is calculated using the methods of S2 and S3. According to spatial analytic geometry, a′ i The position expression is as follows:

[0093]

[0094] Where k i represents Pa′ i The slope of point P is located on line segment a i-1 a′ i+1 On the line segment a′ i P is perpendicular to line segment a i-1 a′ i+1 .

[0095] like Figure 8 The figure shows the schematic diagram of applying the rotation angle limit when the lengths of the robot arm links are unequal. i-1 with a i The length of the connecting rod between i , that is |a i a i-1 |=L i . a i-1 、a i 、a i+1 The same method of S2 and S3 is used to calculate the joint angle q of the i-th joint i+1 The expression is the same as above.

[0096] When q i+1 Exceeding the corner limit q max When maintaining joint point ai-1 Position remains unchanged, re-plan a i and a i+1 Position to a′ i and a′ i+1 , to meet the joint angle limit, a′ i and a′ i+1 Located by a i-1 、a i and a i+1 Point P is located on the plane formed by line segment a. i-1 a′ i+1 On the line segment a′ i P is perpendicular to line segment a i-1 a′ i+1 .

[0097] In Δa i-1 a′ i a′ i+1 According to the geometric relationship, we can get:

[0098]

[0099] At this time, a′ i+1 The location can be accessed through |a i-1 a′ i+1 | and a i-1 The position of point P is calculated using the methods of S2 and S3. According to spatial analytic geometry, the position expression of point P is as follows:

[0100] P=(1-λ)·a i-1 +λ·a i ' +1

[0101]

[0102] Since line segment a′ i P is perpendicular to line segment a i-1 a′ i+1 , according to spatial analytic geometry, Pa′ i The slope of can be realized by calculating the vector product, and finally a′ i The position expression is as follows:

[0103]

[0104] Based on the known principle of replanning joint positions, the equivalent rotation angle of a snake-like manipulator is determined by the three adjacent joints. Therefore, replanning joint positions may cause the equivalent rotation angle of the previous joint to exceed the rotation limit. Therefore, an iterative algorithm is required to ensure that the joint angles of all joints of the snake-like manipulator are within the rotation limit during the path tracking process. This is achieved through the following sub-steps:

[0105] (4.1) Using the joint point sequential search method mentioned above, the new configuration of the snake-like manipulator is obtained as follows Figure 9 As shown in (a), the joint angles of each joint are calculated sequentially.

[0106] (4.2) Sequentially determine whether each joint angle exceeds the angle limit. If so, re-plan the position of the joint point; if not, proceed to determine the next joint angle. In this embodiment, the joint angle q2 (i.e. and The angle between them) is greater than the rotation angle limit q max Then the positions of joint points a2 and a3 are replanned to meet the rotation angle constraint, and the configuration of the snake-like manipulator is obtained as follows: Figure 9 As shown in (b); due to the joint angle q3 (i.e. and The angle between them is also greater than the rotation angle limit q max , the positions of joints a3 and a4 are also replanned. Now the configuration of the snake-like robot arm is as follows Figure 9 As shown in (c).

[0107] (4.3) Since replanning the position of the joint points will cause the joint equivalent rotation angle to exceed the rotation limit again, after completing the replanning of all joint points in sequence, it is necessary to recalculate the equivalent rotation angle of each joint. If any joint angle exceeds the rotation limit, the current joint point and the remaining target points are used as the subsequent follow-up path, and the rotation limit is repeatedly applied to set the angle q set Repeat steps (4.2) for the target until all joint equivalent rotations are within the rotation limit q. max Within, the configuration of the snake-like robot arm is as follows Figure 9 As shown in (d). After iteration, the actual maximum joint angle of the snake-like robot arm is close to the set angle q set , but greater than q set .

[0108] If the angle q is set set Set to the maximum rotation angle q max , then after infinite iterations, the joint equivalent angle will still not be equal to or less than q max Therefore, setting the setting angle q set than q max A smaller iteration tolerance δ, the smaller δ is, the closer the actual angle will be to the angle limit q max , but this requires more iterations and computing time, so δ needs to be selected according to actual needs.

[0109] The present invention proposes a serpentine path tracking method with angle limitation. The method first quickly searches for joint points on the target path through a geometric method, and then applies the angle limitation through an iterative method, so that the serpentine robot arm can track the target path with a small deviation within the angle limitation.

[0110] 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 serpentine path tracking method with angle restriction, characterized in that: The following steps are involved: S1: Set the target path of the snake-like robot arm, which includes multiple target points, where G j is the jth target point; the robotic arm includes a base, a connecting rod, and a joint. Two adjacent connecting rods are connected by joints. a1 is the initial joint of the robotic arm, connecting the base and connecting rod 1; a n is the terminal joint, a i is the i-th joint, i=1,2,…,n; a i-1 with a i The length of the connecting rod between i ; S2: Use sequential search method, from a1 to a n Roughly obtain the position range of each joint point in turn; S3: Use interpolation method to obtain the precise position of each joint; S4: When the deviation from the target path is minimized, the node positions are replanned iteratively until the rotation angles of all joints are within the rotation limits. The principle of replanning the node positions is: For three adjacent joint points a with known positions i-1 、a i 、a i+1 , calculate joint a i The corresponding joint angle, if it exceeds the angle limit, keep the joint point a i-1 Position remains unchanged, re-plan a i and a i+1 Position to a′ i and a′ i+1 ; a′ i and a′ i+1 Are located by a i-1 、a i and a i+1 On the plane formed by the three points, and a′ i+1 Located on the given target path; calculate line segment a based on geometric relationships i-1 a′ i+1 The length of a′ is calculated using the steps described in S2 and S3. i+1 The position of a′ is calculated based on spatial geometry i location; Will and The included angle is taken as the setting angle q set , set the angle q set Less than the rotation angle limit q max , and the difference between the two is the iteration tolerance δ, which is set according to actual needs; set Iterate for the goal.

2. The serpentine path tracking method with angle limitation according to claim 1, characterized in that: The specific operation of S2 is: when a i-1 When it is known, start from the nearest target point and calculate a i-1 and the Euler distance between the target point and L i For comparison, if |a i-1 G j | <L i and|a i-1 G j+1 |>L i , then a i Located in G j and G j+1 between.

3. The serpentine path tracking method with angle limitation according to claim 2, characterized in that: In S3, the interpolation method is trigonometric method, and the specific operation is as follows: G j and G j+1 The path curve between them is approximately a straight line segment, G j with a i-1 The distance is p j , G j+1 with a i-1 The distance is p j+ 1, In Δa i-1 G j G j+1 According to the cosine theorem, we get: In Δa i a i-1 G j+1 According to the law of cosines, we get the equation: Set constraint 10·|G j G j+1 |<|a i-1 a i |, |a i-1 a i |>|a i G j+1 |,p j+1 >|a i G j+1 |; get|a i G j+1 |For: According to the geometric relationship, we get a i The position expression is as follows:

4. The serpentine path tracking method with angle limitation according to claim 2, characterized in that: In S3, the interpolation method uses the proportional approximation method, and the specific operation is as follows: G j and G j+1 The path curve between them is approximately a straight line segment, G j with a i-1 The distance is p j , G j+1 with a i-1 The distance is p j+1 ; Take joint point a i-1 is the center of the circle, p j Draw an arc for the arc length, which is the same as a i-1 a i The intersection of the lines is marked as point A, and i G j+1 The intersection of the lines is marked as point B; the joint point a i-1 is the center of the circle, L i Draw an arc for the arc length, which is the same as a i G j+1 The intersection of the lines is marked as point C; According to spatial geometry, the coordinate expression of point B is as follows: B=a i-1 +λ1(G j+1 -a i-1 ) Assume Δa i AG j Similar to ΔG j+1 BG j , according to the triangle similarity principle, we get: Then get a i The coordinates of are expressed as follows: ai=ai-1+λ3(A-ai-1)=ai-1+λ3·(Gj-ai-1+λ2·(ai-1-Gj+λ1·(Gj-1-ai-1))) A=G j +λ2(BG j ) 5. The serpentine path tracking method with angle limitation according to claim 1, characterized in that: In S4, re-planning the node positions in an iterative manner is specifically achieved through the following sub-steps: (4.1) Calculate the joint rotation angles of each joint in the new configuration in sequence; the joint rotation angle q of the i-th joint is i+1 The expression is as follows: (4.2) Sequentially determine whether the rotation angle of each joint exceeds the rotation angle limit. If so, re-plan the position of the joint point; if not, proceed to determine the next joint rotation angle; (4.3) After completing the replanning of all joint points in sequence, recalculate the equivalent rotation angle of each joint. If any joint angle exceeds the rotation limit, the current joint point and the remaining target points are used as the subsequent follow-up path and step (4.2) is repeated to set the angle q set Iterate multiple times for the goal until all joint equivalent angles are within the angle limit q max Within.

Images