Snake path tracking method with rotation angle limitation

Abstract

The invention discloses a snakelike path tracking method with rotation angle limitation. The snakelike path tracking method comprises the steps that a target path of a snakelike mechanical arm is set; adopting a sequential search method to roughly obtain the position range of each joint point in sequence; obtaining the accurate position of each joint point by adopting an interpolation method; under the condition that the deviation with the target path is minimum, the node positions are planned again in an iteration mode until all the joint rotation angles are within the rotation angle limit; iteration is carried out with a set angle qset as a target, qset is smaller than a corner limit qmax, the difference between qset and the corner limit qmax is an iteration tolerance delta, and delta is set according to actual requirements. According to the method, the snakelike mechanical arm can efficiently track the target path with small deviation within the rotation angle limit.

Description

Technical Field

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

[0002] When the motion planning of the serpentine robot is performed under the condition of known target, a safe path can be planned according to the relative position of the serpentine robot and the target, and the serpentine robot can be made to move along the given path and finally reach the target point through the motion control method. When the serpentine robot moves along the specified path, the serpentine robot is controlled so that all its links move on the given path with a small error. This kind of movement is called serpentine path tracking. However, the serpentine robot itself has a physical angle limit. In the process of following the given path, the robot joint angle may exceed the album limit, causing the robot to be unable to continue to move. The common end-following algorithm cannot solve the problem of angle exceeding the limit. If the angle is simply limited to the maximum angle of the joint when the robot exceeds the angle limit, the robot will seriously deviate from the ideal path. Although the use of numerical calculation method for robot path planning can solve the problem of angle exceeding the limit, it has a large amount of calculation and takes a long time to solve, which cannot meet the requirements of real-time control of the robot. Therefore, an efficient path planning method is needed to enable the serpentine robot to track the ideal path with the smallest possible error within the angle limit. Summary of the invention

[0003] In view of the deficiencies of the prior art, the present invention proposes a serpentine path tracking method with turning 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-shaped robot arm, which includes multiple target points, among which G j is the jth target point; the robot 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 robot 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 point;

[0009] S4: When the deviation from the target path is minimal, the node positions are replanned in an iterative manner until the rotation angles of each joint are within the rotation angle limit; 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 The position remains unchanged, and a new plan is made i and a i+1 Position to a′ i and a′ i+1 ; a′ i and a′ i+1 Both are located in 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 the spatial geometry i location;

[0011] Will and The angle between the two 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 known, start from the nearest target point and calculate a in sequence 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 uses trigonometric method, and the specific operation is as follows:

[0014] G j and G j+1 The path curve between 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 cosine theorem, 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 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. 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 connecting 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] Assuming Δ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, replanning the node positions in an iterative manner is specifically implemented by the following sub-steps:

[0035] (4.1) Calculate the joint rotation of each joint in the new configuration in sequence; the joint rotation of the i-th joint q 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 all joint points are replanned sequentially, the equivalent rotation angle of each joint is recalculated. If any joint rotation 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 rotations are within the rotation 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 continuously fine-tunes the joint point positions through iteration to achieve angle limitation, so that the robot arm tracks the target path with a small deviation within the maximum angle. The method has the characteristics of fast calculation speed, high path tracking accuracy, good portability and strong versatility. The method can be extended to any multi-joint serial robot arm to achieve real-time path planning with angle limitation. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 It is a flowchart 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 robot arm and its path target points when performing path tracking in an embodiment of the present invention.

[0043] Figure 3 Schematic diagrams of sequential and reverse 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 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 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 Schematic diagram of applying rotation angle restriction when the connecting rods have different lengths in an embodiment of the present invention.

[0049] Fig. 9 Schematic diagram of the configuration of applying the joint angle restrictions through an iterative algorithm in an embodiment of the present invention, wherein (a) is a new configuration of the serpentine robot arm obtained by using a joint point sequential search method, (b) is the configuration of the serpentine robot arm after re-planning the first joint angle, (c) is the configuration of the serpentine robot arm after re-planning the second joint angle, and (d) is the configuration of the serpentine robot arm after completing the iteration. DETAILED DESCRIPTION

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

[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 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; a i-1 with a i The length of the connecting rod between i . Use a1 to a n+1 The macroscopic geometric features of the snake-like robot arm are described by connecting the obtained broken line segments in sequence, which can be called the configuration of the snake-like robot arm. When operating, the joints of the snake-like robot arm gradually move to form a new configuration.

[0052] The present invention proposes a serpentine path tracking method with a turning 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 movement of the serpentine robot, 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 is 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. 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 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, and then the joint points are searched in 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 ) and calculate 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 the target point is used to describe the path, G j and G j+1 The path information between 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, 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 |Need 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. i G j+1 a i-1 Recorded as It should be a smaller angle, that is when When the curvature of the desired path is too large, it is difficult for the snake-shaped robot 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 cosine theorem, 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 of | 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 robot can be calculated in turn according to 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. 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 connecting 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 the joints that exceed the angle limit, re-plan the position of their joint points so that they meet the angle limit with the minimum deviation from the target path.

[0084] S4: Due to the physical structure of the snake-like robot arm, the rotation angle between each two links of the robot arm (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 its rotation angle limit to avoid interference. Due to different designs of snake-like manipulators, the rotation angle limit is mostly between 20° and 35°. The rotation angle limit affects the minimum turning radius and path deviation of the snake-like manipulator. In addition, increasing the joint rotation angle limit will reduce the internal space of the manipulator link. In order to solve the problem of excessive rotation angle in the motion planning of the snake-like manipulator, rotation angle limitation is required.

[0085] The idea of ​​the joint constraint method is to replan the position of the joint points to meet the joint angle limit. After the new configuration of the snake-like robot arm is obtained by the method described above, the angle of each joint is analyzed. If the joint angle exceeds the joint angle limit, the position of the joint point is replanned so that the joint point meets the angle limit with the minimum deviation from the target path.

[0086] Firstly, the principle of replanning the position of joint points is given: for three adjacent joint points, the joint points are replanned by solving the triangles 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 out, then the joint angle q of the i-th joint i+1 The expression is as follows:

[0088]

[0089] When i+1 Exceeding the joint rotation limit q max When i-1 The position remains unchanged, and a new plan is made i and a i+1 Position to a′ i and a′ i+1 , to meet the joint rotation limit. i-1 a′ i a′ i+1 In the i+1 Located on a 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 less than the rotation angle limit q max , and the difference between the two is the iteration tolerance δ (a suitable value is set by humans). i and a′ i+1 Located by a i-1 、a i and a i+1 On the composed plane.

[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 obtained by |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] In the formula, 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 i+1 Exceeding the corner limit q max Wheni-1 The position remains unchanged, and a new plan is made i and a i+1 Position to a′ i and a′ i+1 , to meet the joint rotation 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 obtained by |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 the position of joint points, from the configuration of the snake-like robot, its joint equivalent rotation angle is determined by the three adjacent joint points. Therefore, replanning the position of the joint points may cause the equivalent rotation angle of the previous joint to exceed the rotation angle limit. Therefore, an iterative algorithm is required to ensure that the joint rotation angles of all joints of the snake-like robot are within the rotation angle 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 robot arm is obtained as follows Fig. 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, 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 robot is obtained as follows: Fig. 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 joint points a3 and a4 are also replanned. At this time, the configuration of the snake-like robot arm is as follows Fig. 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 rotation 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 then repeatedly applied to set the angle q set Perform multiple iterations for the goal (i.e. repeat step (4.2)) until all joint equivalent rotations are within the rotation limit q max Within, the configuration of the snake-like robot arm is as follows Fig. 9 After iteration, the actual maximum joint rotation angle of the snake 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 rotation angle will not be equal to or less than q max Therefore, setting the setting angle q set 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 restriction, which firstly searches for joint points on a target path quickly by a geometric method, and then applies the angle restriction by an iterative method, so that a serpentine robot arm can track the target path with a small deviation within the angle restriction.

[0110] 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. A serpentine path tracking method with turning angle limitation, characterized in that: The following steps are involved: S1: Set the target path of the snake-shaped robot arm, which includes multiple target points, among which 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 point; S4: When the deviation from the target path is minimal, the node positions are replanned in an iterative manner until the rotation angles of each joint are within the rotation angle limit; 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 The position remains unchanged, and a new plan is made i and a i+1 Position to a′ i and a′ i+1 ; a′ i and a′ i+1 Both are located in 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 the spatial geometry i location; Will and The angle between the two 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 turning angle limitation according to claim 1, characterized in that: The specific operation of S2 is: when a i-1 When known, start from the nearest target point and calculate a in sequence 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 turning angle limitation according to claim 2, characterized in that: In S3, the interpolation method uses trigonometric method, and the specific operation is as follows: G j and G j+1 The path curve between 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 cosine theorem, 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 turning 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 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. 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 of arc length that is equal to a i G j+1 The intersection of the connecting 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 ) Assuming Δ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 turning angle limitation according to claim 1, characterized in that: In S4, replanning the node positions in an iterative manner is specifically implemented through the following sub-steps: (4.1) Calculate the joint rotation of each joint in the new configuration in sequence; the joint rotation of the i-th joint q 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 all joint points are replanned sequentially, the equivalent rotation angle of each joint is recalculated. If any joint rotation 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 rotations are within the rotation limit q max Within.

Images