A method for automatically solving the positioning angle of rotating tooling

Abstract

The present invention relates to the field of automated production, and in particular to a method for automatically solving the positioning angle of a rotary tool, comprising the following steps: S1: reading processing trajectory information; S2: calculating each trajectory point p i The cumulative arc length c of the machining trajectory i ; S3: Calculate the feasible solution interval of the rotary tooling at all processing trajectory points; S4: Determine the feasible solution interval of all processing trajectory points [s i,min ,s i,max ] is empty: If it is empty, it means that there is no feasible solution for the processing trajectory point, and the solution ends; if it is not empty, go to step S5; S5: In the feasible solution interval [s i,min ,s i,max ] solves the rotary tooling positioning angle within the range. The present invention automatically solves the rotary tooling positioning angle without presetting conditional parameters such as the rotary tooling initial angle and feasible solution range, thus being versatile and easy to use. The solved positioning angle is guaranteed to fall within the feasible solution range, and the angle curve is smooth, which is beneficial to improving the stability and processing quality of the robot machining process.

Description

Technical Field

[0001] The present invention relates to the field of automated production, and in particular to a method for automatically solving the positioning angle of a rotary tool. Background Art

[0002] Industrial robots, with their advantages of stable operation, flexible operation, and multiple degrees of freedom, can accomplish tasks requiring high labor intensity, harsh environments, high precision, and significant risks. Consequently, they are widely used in digital manufacturing technology. In robotic processing applications such as automated wire laying, automated welding, and automated spray painting, rotary tooling is often used to drive the workpiece, enhancing the machining system's flexibility and obstacle avoidance capabilities. However, rotary tooling introduces redundant rotational degrees of freedom into the machining system, necessitating a solution to determine the rotary tooling's positioning angle.

[0003] The methods for solving the positioning angle of rotating tooling commonly used in engineering are currently divided into two types according to their characteristics: the equal height method and the equal angle method. The equal height method first rotates the processing trajectory point to a specified height by rotating the tooling, and then calculates the corresponding end effector position and posture based on the rotated trajectory point. The equal angle method rotates the normal vector at the processing trajectory point to a fixed angle with the horizontal plane by rotating the tooling, and then calculates the corresponding end effector position and posture based on the rotated trajectory point. This method only limits the pressure angle of the end effector and is suitable for some tooling with complex shapes that can only be processed at specific angles.

[0004] Both the equal-height method and the equal-angle method use a fixed parameter to determine the rotation angle of the tooling. As the structural shapes of machined parts become increasingly complex, the end effector frequently changes speed and reverses when machining complex-shaped parts, and even causes interference between the end effector and the tooling, making it impossible to solve the problem of non-rotational tooling. Summary of the Invention

[0005] In order to solve the above-mentioned defects in the prior art, the present invention proposes a method for automatically solving the positioning angle of a rotating tool.

[0006] In order to achieve the above technical effects, the present invention is implemented through the following technical solutions:

[0007] A method for automatically calculating the positioning angle of a rotary tool comprises the following steps:

[0008] S1: Read processing trajectory information;

[0009] Read the processing trajectory point information (p0, p1, ..., p N-1 ), where N is the number of trajectory points;

[0010] S2: Calculate each trajectory point p iThe cumulative arc length c of the machining trajectory i ;

[0011] Get the processing trajectory curve length (c0, c1, ..., c N-1 ); the value of i ranges from 0 to N-1;

[0012] S3: Calculate the feasible solution interval of the rotary tooling at all machining trajectory points;

[0013] For the i-th processing trajectory point, first calculate the initial feasible solution interval [s′ i,min ,s′ i,max ], then divide the initial feasible solution interval into M parts according to the given step size, forming M+1 temporary solutions, and judge whether the solution is a feasible solution one by one. Finally, define the feasible solution interval [s i,min ,s i,max ];

[0014] S4: Determine the feasible solution interval of all processing trajectory points [s i,min ,s i,max ] Is it empty: If it is empty, it means that there is no feasible solution for the processing trajectory point and the solution ends; if it is not empty, go to step S5;

[0015] S5: In the feasible solution interval [s i,min ,s i,max ]Solve the positioning angle of the rotating tooling.

[0016] Furthermore, the specific steps of S3 are as follows:

[0017] S3-1: set i=0;

[0018] S3-2: According to the characteristics of the robot end effector and the processing technology characteristics, calculate the initial feasible solution interval [s′] of the i-th processing trajectory point i,min ,s′ i,max ];

[0019] S3-3: The initial feasible solution interval [s′ i,min ,s′ i,max ]According to the given step size s step Divide into M equal parts;

[0020] S3-4: set j=0;

[0021] S3-5: Calculate the temporary solution s i,j =s step ×j+s i,min ;

[0022] S3-6: Analysis of rotary tooling in s i,jPosition, whether there is a solution for other motion axes of the system, if there is a solution, go to S3-7, otherwise, go to S3-8;

[0023] S3-7: Temporary release i,j Mark as feasible solution;

[0024] S3-8: j=j+1;

[0025] S3-9: If j ≥ M, go to S3-10, otherwise, go to S3-5;

[0026] S3-10: According to all feasible solutions s i,j Define the feasible solution interval [s i,min ,s i,max ];

[0027] S3-11: i=i+1;

[0028] S3-12: If i ≥ N, end; otherwise, go to S3-2.

[0029] Furthermore, the specific steps of S5 are as follows:

[0030] S51: Take the current trajectory point p i The length is 2c s The adjacent trajectory length interval [c i –c s ,c i +c s ]The trajectory points p near j The feasible solution midpoint s j,m The weighted average of is the temporary solution s i,t , the specific calculation steps are as follows:

[0031] S52: Temporary solution i,t Projected to the feasible solution interval of the current trajectory point [s i,min ,s i,max ] get s i

[0032] When temporary solution i,t Located in the feasible solution interval [s i,min ,s i,max ], its projection point is s i =s i,t ; When s i,t i,min When the projection point is s i =s i,min ; When s i,t >s i,max When the projection point is s i =s i,max . ​

[0033] S53: Take s i as the final solution of the rotating tooling

[0034] Furthermore, the specific steps of S51 are as follows:

[0035] S51-1: Determine the neighborhood interval [c i at the current trajectory point p i – c s , c i + c s . The neighborhood range c s can be taken as 1.5 times the average length of a single step of the machining trajectory, that is

[0036]

[0037] S51-2: Set the weighted sum of feasible solutions to zero, s i,sum = 0;

[0038] S51-3: When the trajectory point p i is near the starting point of the machining trajectory and c i – c s < c0, then the weighted sum in the interval [c i – c s , c0] is counted as

[0039] s i,sum = s i,sum + [c0 - (c i – c s )] · s0

[0040] When the trajectory point p i is near the ending point of the machining trajectory and c i + c s > c N-1 , then the weighted sum in the interval [c N-1 , c i + c s is counted as

[0041] s i,sum = s i,sum + [(c i + c s ) - c N-1 · s N-1

[0042] S51-4: Search along the machining trajectory curve for whether the trajectory interval [c j to p j+1 of each trajectory point p j , c j+1 is located in the neighborhood [c i – c i ​s ,c i +c s ] range, if it is within the neighborhood range, then calculate the weighted sum of this interval. The specific calculation steps are as follows:

[0043] S51-5: Calculate the neighborhood interval [c i –c s ,c i +c s ]The weighted average of the internal rotation tooling positioning angles, that is,

[0044]

[0045] The result of weighted average is the current trajectory point p i Temporary solution for rotating fixture i,t .

[0046] Furthermore, the specific steps of S51-4 are as follows:

[0047] S51-4-1: Set j=0;

[0048] S51-4-2: If c j+1 <c i -c s , indicating that the trajectory point interval [c j ,c j+1 ] and [c i –c s ,c i +c s ] No intersection, go to S51-4-7, otherwise go to S51-4-3;

[0049] S51-4-3: If c j ≤c i -c s ≤c j+1 , indicating that the trajectory point interval [c j ,c j+1 ] and [c i –c s ,c i +c s ] intersection[c i –c s ,c j+1 ]The weighted sum within the range

[0050] s i,sum =s i,sum +0.5·(s i-s +s j+1 )·[c j+1 -(c i -c s )]

[0051] in Go to S51-4-7, otherwise, go to S51-4-4;

[0052] S51-4-4: If c i -c s ≤c j ≤c j+1 ≤c i +c s , indicating that the trajectory point interval [c j ,c j+1 ] and [c i –c s ,c i +c s ] intersection[c j ,c j+1 ]The weighted sum within the range

[0053] s i,sum =s i,sum +0.5·(s j +s j+1 )·(c j+1 -c j )

[0054] Go to S51-4-7, otherwise, go to S51-4-5;

[0055] S51-4-5: If c j ≤c i +c s ≤c j+1 , indicating that the trajectory point interval [c j ,c j+1 ] and [c i –c s ,c i +c s ] intersection[c j ,c i +c s ]The weighted sum within the range

[0056] s i,sum =s i,sum +0.5·(s j +s i+s )·[(c i +c s )-c j ]

[0057] in Go to S51-4-7, otherwise, go to S51-4-6;

[0058] S51-4-6: If c i +cs ≤c j , indicating that the trajectory point interval [c j , c j+1 and [c i –c s , c i +c s have no intersection, and [c j , c j+1 has exceeded the range of [c i –c s , c i +c s , stop searching;

[0059] S51-4-7: j = j + 1. If j < N, go to S51-4-2; otherwise, stop searching.

[0060] The advantages of the present invention are as follows:

[0061] This application adopts an automatic solution method. The operator does not need to preset condition parameters such as the preset angle of the rotating tooling and the feasible solution interval, which improves the versatility and usability of this method. Determining the initial feasible solution interval according to the characteristics of the robot end effector and the machining process characteristics can significantly narrow the range of the initial feasible solution interval and significantly reduce the calculation amount of the feasible solution interval. Using the neighborhood weighted average method to calculate the positioning angle of the rotating tooling at each machining trajectory point and projecting the calculated angle onto the feasible solution interval, on the one hand, improves the motion smoothness of the rotating tooling, and on the other hand, ensures that the solutions adopted are all feasible solutions, which is beneficial to improving the stability and machining quality of robot machining. The solved positioning angle can be ensured to fall within the feasible solution interval and the angle curve is smooth, which is beneficial to improving the stability and machining quality in the robot machining process. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 is the flowchart for automatically solving the positioning angle of the rotating tooling.

[0063] Figure 2 is the flowchart for calculating the feasible solution interval of the rotating tooling.

[0064] Figure 3 is the schematic diagram of the feasible solution weighted average algorithm.

[0065] Figure 4 is the flowchart of the feasible solution weighted average algorithm. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0066] Based on this method, the embodiments of the present invention are described in detail below. These embodiments are implemented on the premise of the technical solution of the present invention and give detailed implementation manners, but the protection scope of the present invention is not limited to the following embodiments.

[0067] Example 1

[0068] like Figure 1 As shown, a method for automatically solving the positioning angle of a rotary tooling includes the following steps:

[0069] S1: Read processing trajectory information;

[0070] Read the processing trajectory point information (p0, p1, ..., p N-1 ), where N is the number of trajectory points;

[0071] S2: Calculate each trajectory point p i The cumulative arc length c of the machining trajectory i ;

[0072] Get the processing trajectory curve length (c0, c1, ..., c N-1 ); the value of i ranges from 0 to N-1;

[0073] S3: Calculate the feasible solution interval of the rotary tooling at all machining trajectory points;

[0074] For the i-th processing trajectory point, first calculate the initial feasible solution interval [s′ i,min ,s′ i,max ], then divide the initial feasible solution interval into M parts according to the given step size, forming M+1 temporary solutions, and judge whether the solution is a feasible solution one by one. Finally, define the feasible solution interval [s i,min ,s i,max ];

[0075] S4: Determine the feasible solution interval of all processing trajectory points [s i,min ,s i,max ] Is it empty: If it is empty, it means that there is no feasible solution for the processing trajectory point and the solution is ended; if it is not empty, go to step S5;

[0076] S5: In the feasible solution interval [s i,min ,s i,max ]Solve the positioning angle of the rotating tooling.

[0077] Example 2

[0078] like Figure 1 As shown, a method for automatically solving the positioning angle of a rotary tooling includes the following steps:

[0079] S1: Read processing trajectory information

[0080] Read the processing trajectory point information (p0, p1, ..., p N-1 ), where N is the number of trajectory points.

[0081] S2: Calculate the cumulative arc length c of the machining trajectory at each trajectory point i

[0082] Get the processing trajectory curve length (c0, c1, ..., c N-1 ).

[0083] S3: Calculate the feasible solution interval of the rotating tooling at all processing trajectory points

[0084] For the i-th processing trajectory point, first calculate the initial feasible solution interval [s′ i,min ,s′ i,max ], then divide the initial feasible solution interval into M parts according to the given step size, forming M+1 temporary solutions, and judge whether the solution is a feasible solution one by one. Finally, define the feasible solution interval [s i,min ,s i,max ].

[0085] like Figure 2 Specifically, the calculation is performed using the following steps:

[0086] S3-1: set i=0;

[0087] S3-2: According to the characteristics of the robot end effector and the processing technology characteristics, calculate the initial feasible solution interval [s′] of the i-th processing trajectory point i,min ,s′ i,max ];

[0088] S3-3: The initial feasible solution interval [s′ i,min ,s′ i,max ]According to the given step size s step Divide into M equal parts;

[0089] S3-4: set j=0;

[0090] S3-5: Calculate the temporary solution s i,j =s step ×j+s i,min ;

[0091] S3-6: Analysis of rotary tooling in s i,j Position, whether there is a solution for other motion axes of the system, if there is a solution, go to S3-7, otherwise, go to S3-8;

[0092] S3-7: Temporary release i,j Mark as feasible solution;

[0093] S3-8: j=j+1;

[0094] S3-9: If j ≥ M, go to S3-10, otherwise, go to S3-5;

[0095] S3-10: According to all feasible solutions s i,j Define the feasible solution interval [s i,min , s i,max ;

[0096] S3-11: i = i + 1;

[0097] S3-12: If i ≥ N, end; otherwise, go to S3-2.

[0098] S4: Judge whether the feasible solution interval [s i,min , s i,max of all machining trajectory points is empty: If it is empty, it means that there is no feasible solution for this machining trajectory point, and the solution is ended; if it is not empty, go to step S5

[0099] S5: Solve the rotation tooling positioning angle within the feasible solution interval [s i,min , s i,max

[0100] S51: Take the weighted average of the feasible solution midpoints s i at the current trajectory point p s within the adjacent trajectory length interval [c i – c s , c i + c s as the temporary solution s j , and the specific calculation steps are as follows: j,m i,t i,t i,t

[0101] S51-1: Determine the neighborhood interval [c i at the current trajectory point p i – c s , c i + c s , and the neighborhood range c s can be taken as 1.5 times the average length of a single step of the machining trajectory, that is

[0102]

[0103] S51-2: Set the weighted sum of feasible solutions to zero, s i,sum = 0;

[0104] S51-3: When the trajectory point p i is near the starting point of the machining trajectory and c i – c s < c0, then the weighted sum within the interval [c i – c s , c0] is counted as

[0105] si,sum =s i,sum +[c0-(c i -c s )]·s0

[0106] When the trajectory point p i Located near the end point of the machining trajectory and c i +c s >c N-1 When [c N-1 ,c i +c s The weighted sum within the interval is calculated as

[0107] s i,sum =s i,sum +[(c i +c s )-c N-1 ]·s N-1

[0108] S51-4: Search for each trajectory point p along the machining trajectory curve j to p j+1 The trajectory interval [c j ,c j+1 ] is located in p i Neighborhood [c i –c s ,c i +c s ] range, if it is within the neighborhood range, then calculate the weighted sum of this interval. The specific calculation steps are as follows:

[0109] S51-4-1: Set j=0;

[0110] S51-4-2: If c j+1 <c i -c s , indicating that the trajectory point interval [c j ,c j+1 ] and [c i –c s ,c i +c s ] No intersection, go to S51-4-7, otherwise go to S51-4-3;

[0111] S51-4-3: If c j ≤c i -c s ≤c j+1 , indicating that the trajectory point interval [c j ,c j+1 ] and [c i –c s ,c i +cs ] intersection[c i –c s ,c j+1 ]The weighted sum within the range

[0112] s i,sum =s i,sum +0.5·(s i-s +s j+1 )·[c j+1 -(c i -c s )]

[0113] in Go to S51-4-7, otherwise, go to S51-4-4;

[0114] S51-4-4: If c i -c s ≤c j ≤c j+1 ≤c i +c s , indicating that the trajectory point interval [c j ,c j+1 ] and [c i –c s ,c i +c s ] intersection[c j ,c j+1 ]The weighted sum within the range

[0115] s i,sum =s i,sum +0.5·(s j +s j+1 )·(c j+1 -c j )

[0116] Go to S51-4-7, otherwise, go to S51-4-5;

[0117] S51-4-5: If c j ≤c i +c s ≤c j+1 , indicating that the trajectory point interval [c j ,c j+1 ] and [c i –c s ,c i +c s ] intersection[c j ,c i +c s ]The weighted sum within the range

[0118] s i,sum =si,sum +0.5·(s j +s i+s )·[(c i +c s )-c j

[0119] Where Transfer to S51-4-7; otherwise, transfer to S51-4-6;

[0120] S51-4-6: If c i +c s ≤c j , it indicates that the trajectory point interval [c j ,c j+1 and [c i –c s ,c i +c s have no intersection, and [c j ,c j+1 has exceeded the range of [c i –c s ,c i +c s . Stop the search;

[0121] S51-4-7: j = j + 1. If j < N, transfer to S51-4-2; otherwise, stop the search;

[0122] S51-5: Calculate the weighted average of the rotation tooling positioning angles within the neighborhood interval [c i –c s ,c i +c s , that is

[0123]

[0124] The result of the weighted average is the temporary solution s i of the rotation tooling at the current trajectory point p i,t .

[0125] S52: Project the temporary solution s i,t onto the feasible solution interval [s i,min ,s i,max of the current trajectory point to obtain s i

[0126] When the temporary solution s i,t is within the feasible solution interval [s i,min ,s i,max , its projection point is s i = s i,t ; when s i,t <s​i,min When the projection point is s i =s i,min ; When s i,t >s i,max When the projection point is s i =s i,max .

[0127] S53: With s i This is the final solution for the rotary tooling.

[0128] This application adopts an automatic solution method. The operator does not need to preset the conditional parameters such as the preset angle of the rotary tooling and the feasible solution interval, which improves the versatility and ease of use of this method. The initial feasible solution interval is determined according to the characteristics of the robot end effector and the processing technology characteristics, which can significantly narrow the range of the initial feasible solution interval and significantly reduce the amount of calculation of the feasible solution interval. The neighborhood weighted average method is used to calculate the positioning angle of the rotary tooling at each processing trajectory point, and the calculated angle is projected to the feasible solution interval. On the one hand, it improves the smoothness of the rotary tooling movement, and on the other hand, it ensures that the solutions adopted are feasible solutions, which is beneficial to improving the stability and processing quality of the robot processing. The solved positioning angle can be ensured to fall within the feasible solution interval, and the angle curve is smooth, such as Figure 3 As shown, it is beneficial to improve the stability and processing quality of the robot processing.

Claims

1. A method for automatically calculating the positioning angle of a rotary tool, characterized by: including the following steps, S1: Read the machining trajectory information; Read the processing trajectory point information (p0, p1, ..., p N-1 ), where N is the number of trajectory points; S2: Calculate each trajectory point p i The cumulative arc length c of the machining trajectory i ; Get the processing trajectory curve length (c0, c1, ..., c N-1 ); the value of i ranges from 0 to N-1; S3: Calculate the feasible solution intervals of the rotating tooling at all machining trajectory points; For the i-th processing trajectory point, first calculate the initial feasible solution interval [s′ i,min ,s′ i,max ], then divide the initial feasible solution interval into M parts according to the given step size, forming M+1 temporary solutions, and judge whether the solution is a feasible solution one by one. Finally, define the feasible solution interval [s i,min ,s i,max ]; S4: Determine the feasible solution interval of all processing trajectory points [s i,min ,s i,max ] Is it empty: If it is empty, it means that there is no feasible solution for the processing trajectory point and the solution is ended; if it is not empty, go to step S5; S5: In the feasible solution interval [s i,min ,s i,max ] Solve the positioning angle of the rotating tooling; The specific steps of S5 are as follows, S51: Take the current trajectory point p i The length is 2c s The adjacent trajectory length interval [c i -c s ,c i +c s ]The trajectory points p near j The feasible solution midpoint s j,m The weighted average of is the temporary solution s i,t ; The specific steps of S51 are as follows, S51-1: Determine the current trajectory point p i The neighborhood interval [c i -c s ,c i +c s ], domain range c s It can be taken as 1.5 times the average length of a single step of the machining trajectory, that is, S51-2: Feasible solution weighted sum is set to zero, s i,sum =0; S51-3: When the trajectory point p i Located near the starting point of the machining trajectory, and c i -c s <c0, then in [c i -c s ,c0] interval is calculated as s i,sum =s i,sum +[c0-(c i -c s )]·s0 When the trajectory point p i Located near the end point of the machining trajectory and c i +c s >c N-1 When [c N-1 ,c i +c s The weighted sum within the interval is calculated as s i,sum =s i,sum +[(c i +c s )-c N-1 ]·s N-1 S51-4: Search for each trajectory point p along the machining trajectory curve j to p j+1 The trajectory interval [c j ,c j+1 ] is located in p i Neighborhood [c i -c s ,c i +c s ] range, if it is within the neighborhood range, then calculate the weighted sum of this interval; S51-5: Calculate the neighborhood interval [c i -c s ,c i +c s ]The weighted average of the internal rotation tooling positioning angles, that is, The result of weighted average is the current trajectory point p i Temporary solution for rotating fixture i,t . S52: Temporary solution i,t Projected to the feasible solution interval of the current trajectory point [s i,min ,s i,max ] get s i When temporary solution i,t Located in the feasible solution interval [s i,min ,s i,max ], its projection point is s i =s i,t ; When s i,t i,min When the projection point is s i =s i,min ; When s i,t >s i,max When the projection point is s i =s i,max ;​ S53: with s i The final solution for the rotary tooling; 2. The method for automatically calculating the positioning angle of a rotary tool according to claim 1, characterized in that: The specific steps of the said S3 are as follows, S3-1: Set i = 0; S3-2: According to the characteristics of the robot end effector and the processing technology characteristics, calculate the initial feasible solution interval [s′] of the i-th processing trajectory point i,min ,s′ i,max ]; S3-3: The initial feasible solution interval [s′ i,min ,s′ i,max ]According to the given step size s step Divide into M equal parts; S3-4: Set j = 0; S3-5: Calculate the temporary solution s i,j =s step ×j+s i,min ; S3-6: Analysis of rotary tooling in s i,j Position, whether there is a solution for other motion axes of the system, if there is a solution, go to S3-7, otherwise, go to S3-8; S3-7: Temporary release i,j Mark as feasible solution; S3-8: j = j + 1; S3-9: If j ≥ M, go to S3-10, otherwise, go to S3-5; S3-10: According to all feasible solutions s i,j Define the feasible solution interval [s i,min ,s i,max ]; S3-11: i = i + 1; S3-12: If i ≥ N, end, otherwise, go to S3-2.

3. The method for automatically calculating the positioning angle of a rotary tool according to claim 1, wherein: The specific steps of S51-4 are as follows: S51-4-1: Set j = 0; S51-4-2: If c j+1 <c i -c s , indicating that the trajectory point interval [c j ,c j+1 ] and [c i -c s ,c i +c s ] No intersection, go to S51-4-7, otherwise go to S51-4-3; S51-4-3: If c j ≤c i -c s ≤c j+1 , indicating that the trajectory point interval [c j ,c j+1 ] and [c i -c s ,c i +c s ] intersection[c i -c s ,c j+1 ]The weighted sum within the range s i,sum =s i,sum +0.5·(s i-s +s j+1 )·[c j+1 -(c i -c s )] in Go to S51-4-7, otherwise, go to S51-4-4; S51-4-4: If c i -c s ≤c j ≤c j+1 ≤c i +c s , indicating that the trajectory point interval [c j ,c j+1 ] and [c i -c s ,c i +c s ] intersection[c j ,c j+1 ]The weighted sum within the range s i,sum =s i,sum +0.5·(s j +s j+1 )·(c j+1 -c j ) Go to S51-4-7, otherwise, go to S51-4-5; S51-4-5: If c j ≤c i +c s ≤c j+1 , indicating that the trajectory point interval [c j ,c j+1 ] and [c i -c s ,c i +c s ] intersection[c j ,c i +c s ]The weighted sum within the range s i,sum =s i,sum +0.5·(s j +s i+s )·[(c i +c s )-c j ] in Go to S51-4-7, otherwise, go to S51-4-6; S51-4-6: If c i +c s ≤c j , indicating that the trajectory point interval [c j ,c j+1 ] and [c i -c s ,c i +c s ] have no intersection, and [c j ,c j+1 ] has exceeded [c i -c s ,c i +c s ] range, stop searching; S51-4-7: j = j + 1, if j < N, go to S51-4-2, otherwise, stop searching.

Images