A method for measuring AGV trajectory error based on laser tracker

Abstract

This invention relates to a laser tracker-based AGV trajectory error measurement method. The method uses a laser tracker to measure the coordinates of the AGV's trajectory over multiple cycles. After fitting the ideal trajectory using the least squares method, some parameters are modified to generate initial upper and lower wrapping lines to wrap around the actual trajectory. After obtaining the parameter value range, the upper and lower wrapping lines are gradually approximated to the actual trajectory using a dichotomy method. The difference between the upper and lower wrapping lines is then calculated as the trajectory error. Because the trajectory error calculated by this method is fitted using multiple cycle trajectory coordinate data, it essentially eliminates the influence of odometer cumulative error and laser radar single positioning error, improving trajectory error measurement accuracy. This method provides an important basis for comparing AGV trajectory errors under different speed and positioning algorithm conditions.

Description

Technical Field

[0001] The present invention relates to the technical field of AGV trajectory error measurement, and in particular to an AGV trajectory error measurement method based on a laser tracker. Background Art

[0002] Common AGV trajectory error measurement methods include the following two:

[0003] Odometer sensor: The AGV trajectory path is calculated by calculating the acceleration values ​​in different directions. The disadvantage is that the odometer is easily mixed with sampling noise, has low accuracy, and has cumulative errors.

[0004] LiDAR: Theoretically, the position data of the LiDAR can be directly read throughout the entire trajectory. However, in actual operation, LiDAR installation errors and steering wheel deformation may cause the laser scanning plane to be uneven, thus affecting the LiDAR positioning accuracy. Summary of the Invention

[0005] In view of the problems existing in the prior art, the purpose of the present invention is to provide an AGV trajectory error measurement method based on a laser tracker to improve the trajectory error measurement accuracy.

[0006] To achieve the above object, the technical solution adopted by the present invention is:

[0007] A method for measuring AGV trajectory error based on a laser tracker comprises the following steps:

[0008] Step 1: Use a laser tracker to measure the coordinates of the AGV's multiple cycle tracks;

[0009] Step 2: After fitting the ideal trajectory based on the actual trajectory coordinates, change the parameter values ​​of the ideal trajectory to generate the upper and lower wrapping lines so that the ordinates of the actual trajectory are within the corresponding coordinate ranges of the upper and lower wrapping lines;

[0010] Step 3: Use the dichotomy method to make the upper and lower wrapping lines gradually approach the actual trajectory, and take the vertical distance between the upper and lower wrapping lines as the AGV trajectory error;

[0011] Step 4: Set different track speeds and repeat steps 1-3 to test. Measure the track error of the AGV at different speeds. Select the maximum AGV track speed under certain error requirements based on the AGV speed-track error data to improve the AGV operation efficiency.

[0012] In step 2, the least square method is used to fit the ideal trajectory according to the actual trajectory coordinates.

[0013] The AGV runs multiple cycles along the straight line trajectory and uses the least squares method to fit the ideal straight line trajectory, as follows:

[0014] The ideal straight line trajectory expression is set as y=kx+b, where k is the slope of the line and b is the intercept;

[0015] Use the least squares method to estimate the parameters, requiring the measured value y i The weighted sum of squared deviations Minimum, respectively, to find the partial derivative of k, b can be obtained equation bN+k∑xi=∑y i ,b∑x i +k∑x i 2 =Σx i y i , the optimal values ​​of the straight line parameters k and b are obtained respectively Thus, the ideal straight line trajectory is determined.

[0016] The upper wrapping line and the lower wrapping line are determined as follows:

[0017] Maintain slope k m unchanged, set b0 as the initial value, so that Δ=[k m x i +b0]-[y i ] When all elements in are greater than or equal to 0, the straight line expression y=k m x+b0 is the initial upper wrapping line; determine b j The value range is [b m ,b0], let b j =(b0+b m ) / 2, substitute Δ for judgment. If Δ>0, it means that all positions of the upper wrapping line are above the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approximate the actual trajectory. Let b j As the upper limit of the value, we get b j+1 =(b j +b m ) / 2, continue to substitute Δ for judgment; if Δ<0, it means that part of the upper wrapping line is below the track coordinate and does not completely wrap the track coordinate. Let b j As the lower limit of the value, we get b j+1 =(b m +b j ) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the upper wrapping line expression corresponding to the intercept value;

[0018] Maintain slope k m unchanged, set b1 as the initial value, so that Δ=[k m x i +b1]-[y i ] When all elements in are less than 0, the straight line expression y=k mx+b1 is the initial lower wrapping line, determine b j The value range is [b1,b m ], let b j =(b m +b1) / 2, substitute Δ for judgment. If Δ<0, it means that all positions of the lower wrapping line are below the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approximate the actual trajectory. Let b m As the upper limit of the value, we get b j+1 =(b m +b j ) / 2, continue to substitute Δ for judgment; if Δ>0, it means that the lower wrapping line is partially above the track coordinate and does not completely wrap the track coordinate, then let b1 be the lower limit of the value, and get b j+1 =(b j +b1) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the lower wrapping line expression with the corresponding intercept value.

[0019] The AGV performs multiple cycles along the arc trajectory;

[0020] The ideal arc trajectory is simplified to x 2 +y 2 +ax+by+c=0, where a=-2x0, b=-2y0, c=x0 2 +y0 2 -r 2 ;

[0021] The least square method is used to fit the arc trajectory, requiring the sum of the squares of the distances between the ideal trajectory and the actual trajectory to be minimum, that is, f = ∑((x i -x0) 2 +(y i -y0) 2 -r 2 ) 2 =∑g(x i ,y i ) 2 The value is 0, which is the smallest.

[0022] set up in After variable substitution, we can get Σ((a i -a0) 2 +(b i -b0) 2 -r 2 )a i =0,Σ((a i -a0) 2 +(b i -b0) 2 -r 2 )bi =0, after expansion and simplification, we get Σ(a i 2 b i -2a i b i b0+b i 3 -2b i 2 b0)=0; define various parameters, G=Σa i b i , The formula is simplified to C+I-2Ea0-2Gb0=0,H+D-2Ga0-2Fb0=0, and the solution is

[0023]

[0024]

[0025] That is, the ideal trajectory arc expression obtained by fitting is (x-x0) 2 +(y-y0) 2 =r 2 , (x0,y0) is the center of the ideal trajectory, and r is the radius.

[0026] The upper wrapping line and the lower wrapping line are determined as follows:

[0027] Ensure that the center coordinates (x0, y0) remain unchanged, set r0 as the initial radius, so that Δ = r0 2 -(x i -x0) 2 -(y i -y0) 2 If all elements in the arc are greater than 0, the wrapping line expression on the arc trajectory is (x-x0) 2 +(y-y0) 2 =r0 2 , ensure that the initial upper wrapping line completely wraps the actual arc trajectory, and determine r j The value range is [r, r0], let r j =(r+r0) / 2 and bring in Δ to make a judgment. If Δ>0, it means that all positions of the upper wrapping line are above the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approach the actual trajectory. Then let r be the lower limit of the value and calculate r j+1 =(r j +r) / 2, continue to substitute Δ for judgment; if Δ<0, it means that part of the upper wrapping line is below the actual track coordinates and does not completely wrap the track coordinates, then let r0 be the upper limit of the value, and get r j+1 =(r0+rj ) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the wrapping line expression on the arc trajectory according to the corresponding intercept value;

[0028] Ensure that the center coordinates (x0, y0) remain unchanged, set r1 as the initial radius, so that Δ = r1 2 -(x i -x0) 2 -(y i -y0) 2 If all elements in the arc are less than 0, the wrapping line expression under the arc trajectory is (x-x0) 2 +(y-y0) 2 =r1 2 , ensure that the initial lower wrapping line completely wraps the actual arc trajectory, and determine r j The value range is [r1,r], let r j =(r+r1) / 2 and bring in Δ to make a judgment. If Δ<0, it means that all positions of the lower wrapping line are below the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approach the actual trajectory. Then let r be the upper limit of the value and calculate r j+1 =(r j +r) / 2, continue to substitute Δ for judgment; if Δ>0, it means that the lower wrapping line is partially above the actual track coordinates and does not completely wrap the track coordinates, then let r1 be the lower limit of the value, and get r j+1 =(r1+r j ) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the wrapping line expression under the arc trajectory corresponding to the radius value.

[0029] Using this approach, a laser tracker was used to measure the coordinates of the AGV's trajectory over multiple cycles. After fitting the ideal trajectory using the least squares method, some parameters were modified to generate initial upper and lower wrapping lines to wrap around the actual trajectory. After determining the range of these parameters, a bisection method was used to gradually approximate the upper and lower wrapping lines to the actual trajectory. The difference between the upper and lower wrapping lines was then calculated as the trajectory error. Because the trajectory error calculated by this method is fitted using multiple cycle trajectory coordinate data, it essentially eliminates the influence of the odometry's cumulative error and the lidar's single-shot positioning error, improving trajectory error measurement accuracy. This provides an important basis for comparing AGV trajectory errors under different speed and positioning algorithm conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 is a flow chart of the method of the present invention;

[0031] Figure 2 Schematic diagram of straight line trajectory error. DETAILED DESCRIPTION

[0032] like Figure 1 As shown, the present invention discloses a method for measuring AGV trajectory error based on a laser tracker, which includes the following steps:

[0033] Step 1: Use a laser tracker to measure the coordinates of the AGV's multiple cycle tracks;

[0034] The target ball is fixed on the AGV body, and the AGV is set to run multiple cycles according to the set trajectory. The target ball position is adjusted to ensure that the laser tracker can receive the laser reflected by the target ball during the complete trajectory cycle. The target ball is tracked by the laser tracker to measure the coordinate values ​​(x i ,y i ).

[0035] Step 2: Fit the ideal trajectory according to the actual trajectory coordinates, and change the parameter values ​​of the ideal trajectory to generate the upper and lower wrapping lines so that the ordinates of the actual trajectory are within the corresponding coordinate ranges of the upper and lower wrapping lines.

[0036] Step 3: Use the dichotomy method to make the upper and lower wrapping lines gradually approach the actual trajectory, and take the vertical distance between the upper and lower wrapping lines as the AGV trajectory error.

[0037] Step 4: Set different track speeds and repeat steps 1-3 to test. Measure the track error of the AGV at different speeds. Select the maximum AGV track speed under certain error requirements based on the AGV speed-track error data to improve the AGV operation efficiency.

[0038] The AGV can perform multiple cycles along a straight or circular trajectory and measure trajectory errors based on these cycles. The following sections will explain these in detail using straight and circular trajectories as examples.

[0039] Example 1

[0040] In this embodiment, the AGV performs multiple cycles along a straight line trajectory.

[0041] The ideal straight line trajectory expression is set as y=kx+b, k is the slope of the straight line, and b is the intercept; taking the straight line trajectory as an example, the least squares method is used to estimate the parameters, and the measured value y is required to be i The weighted sum of squared deviations Minimum, take partial derivatives of k and b respectively to get the equation system bN+kΣxi=∑y i ,b∑x i +k∑x i 2 =∑x i y i , the optimal values ​​of the straight line parameters k and b are obtained respectively

[0042] The slope k in the ideal trajectory expression is set as a fixed value, and the initial values ​​of other data are set, such as the intercept b of the straight line trajectory expression. The upper and lower wrapping lines are generated so that the ordinates of the actual trajectory are within the range of the ordinates corresponding to the upper and lower wrapping lines. Then, the ideal trajectory data value and the initial value are used as the boundary, and the dichotomy method is used to make the upper and lower wrapping lines gradually approach the actual trajectory. The vertical distance between the upper and lower wrapping lines is used as the AGV trajectory error.

[0043] Specifically, keep the slope k m unchanged, set b0 as the initial value, so that Δ=[k m x i +b0]-[y i ] When all elements in are greater than or equal to 0, the straight line expression y=k m x+b0 is the initial upper wrapping line; determine b j The value range is [b m ,b0], let b j =(b0+b m ) / 2, substitute Δ for judgment. If Δ>0, it means that all positions of the upper wrapping line are above the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approximate the actual trajectory. Let b j As the upper limit of the value, we get b j+1 =(b j +b m ) / 2, continue to substitute Δ for judgment; if Δ<0, it means that part of the upper wrapping line is below the track coordinate and does not completely wrap the track coordinate. Let b j As the lower limit, we get b j+1 =(b m +b j ) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the upper wrapping line expression corresponding to the intercept value;

[0044] Maintain slope k m unchanged, set b1 as the initial value, so that Δ=[k m x i +b1]-[y i ] When all elements in are less than 0, the straight line expression y=k m x+b1 is the initial lower wrapping line, determine b j The value range is [b1,b m ], let b j =(b m +b1) / 2, substitute Δ for judgment. If Δ<0, it means that all positions of the lower wrapping line are below the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approximate the actual trajectory. Let b m As the upper limit of the value, we get b j+1 =(b m+b j ) / 2, continue to substitute Δ for judgment; if Δ>0, it means that the lower wrapping line is partially above the track coordinate and does not completely wrap the track coordinate, then let b1 be the lower limit of the value, and get b j+1 =(b j +b1) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the lower wrapping line expression with the corresponding intercept value.

[0045] The vertical distance between the upper and lower wrapping lines is calculated as the trajectory error.

[0046] Set different track speeds for testing, measure the track error of the AGV at different speeds, and select the maximum AGV track speed under certain error requirements based on the AGV speed-track error data to improve the AGV operation efficiency.

[0047] Example 2

[0048] In this embodiment, the AGV performs multiple cycles along the arc trajectory.

[0049] The ideal arc trajectory is simplified to x 2 +y 2 +ax+by+c=0, where a=-2x0, b=-2y0, c=x0 2 +y0 2 -r 2 .

[0050] The least square method is used to fit the arc trajectory, requiring the sum of the squares of the distances between the ideal trajectory and the actual trajectory to be minimum, that is, f = ∑((x i -x0) 2 +(y i -y0) 2 -r 2 ) 2 =∑g(x i ,y i ) 2 The minimum value is 0

[0051] set up in After variable substitution, we can get ∑((a i -a0) 2 +(b i -b0) 2 -r 2 )a i =0,∑((a i -a0) 2 +(b i -b0) 2 -r 2 )b i=0, after expansion and simplification, we get ∑(a i 2 b i -2a i b i b0+b i 3 -2b i 2 b0)=0; define various parameters, G=∑a i b i , The formula is simplified to C+I-2Ea0-2Gb0=0,H+D-2Ga0-2Fb0=0, and the solution is

[0052]

[0053]

[0054] That is, the ideal trajectory arc expression obtained by fitting is (x-x0) 2 +(y-y0) 2 =r 2 , (x0,y0) is the center of the ideal trajectory, and r is the radius.

[0055] The center coordinates (x0, y0) in the ideal trajectory expression are used as fixed values, and the initial values ​​of other data are set, such as the radius r of the arc trajectory. The upper and lower wrapping lines are generated so that the ordinates of the actual trajectory are within the corresponding ordinate ranges of the upper and lower wrapping lines. Then, the ideal trajectory data values ​​and the initial values ​​are used as boundaries, and the dichotomy method is used to make the upper and lower wrapping lines gradually approach the actual trajectory. The vertical distance between the upper and lower wrapping lines is used as the AGV trajectory error.

[0056] Specifically, ensure that the coordinates of the center of the circle (x0, y0) remain unchanged, set r0 as the initial radius, so that Δ = r0 2 -(x i -x0) 2 -(y i -y0) 2 If all elements in the arc are greater than 0, the wrapping line expression on the arc trajectory is (x-x0) 2 +(y-y0) 2 =r0 2 , ensure that the initial upper wrapping line completely wraps the actual arc trajectory, and determine r j The value range is [r, r0], let r j =(r+r0) / 2 and bring in Δ to make a judgment. If Δ>0, it means that all positions of the upper wrapping line are above the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approach the actual trajectory. Then let r be the lower limit of the value and calculate rj+1 =(r j +r) / 2, continue to substitute Δ for judgment; if Δ<0, it means that part of the upper wrapping line is below the actual track coordinates and does not completely wrap the track coordinates, then let r0 be the upper limit of the value, and get r j+1 =(r0+r j ) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the wrapping line expression on the arc trajectory according to the corresponding intercept value;

[0057] Ensure that the center coordinates (x0, y0) remain unchanged, set r1 as the initial radius, so that Δ = r1 2 -(x i -x0) 2 -(y i -y0) 2 If all elements in the arc are less than 0, the wrapping line expression under the arc trajectory is (x-x0) 2 +(y-y0) 2 =r1 2 , ensure that the initial lower wrapping line completely wraps the actual arc trajectory, and determine r j The value range is [r1,r], let r j =(r+r1) / 2 and bring in Δ to make a judgment. If Δ<0, it means that all positions of the lower wrapping line are below the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approach the actual trajectory. Then let r be the upper limit of the value and calculate r j+1 =(r j +r) / 2, continue to substitute Δ for judgment; if Δ>0, it means that the lower wrapping line is partially above the actual track coordinates and does not completely wrap the track coordinates, then let r1 be the lower limit of the value, and get r j+1 =(r1+r j ) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the wrapping line expression under the arc trajectory corresponding to the radius value.

[0058] The radius difference between the upper and lower wrapping line expressions of the arc trajectory is calculated as the arc trajectory error.

[0059] Finally, different trajectory speeds are set for testing, and the trajectory error of the AGV at different speeds is measured. According to the AGV speed-trajectory error data, the maximum AGV trajectory speed under certain error requirements is selected to improve the AGV operation efficiency.

[0060] In summary, the present invention uses a laser tracker to measure the coordinates of the AGV's multiple-cycle trajectory, and after fitting the ideal trajectory using the least squares method, changes some parameters to generate initial upper and lower wrapping lines to wrap the actual trajectory. After obtaining the parameter value range, the dichotomy method is used to make the upper and lower wrapping lines gradually approach the actual trajectory, and then the difference between the upper and lower wrapping lines is calculated as the trajectory error. The trajectory error calculated by this method is basically eliminated by fitting the trajectory coordinate data of multiple cycles, thereby improving the measurement accuracy of the trajectory error, and thus providing an important basis for comparing the trajectory errors of AGVs under different speeds and positioning algorithm conditions. The above is only an embodiment of the present invention and does not limit the technical scope of the present invention. Therefore, any minor modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention are still within the scope of the technical solution of the present invention.

Claims

1. A method for measuring AGV trajectory error based on a laser tracker, characterized by: The method comprises the following steps: Step 1: Use a laser tracker to measure the coordinates of the AGV's multiple cycle tracks; Step 2: After fitting the ideal trajectory based on the actual trajectory coordinates, change the parameter values ​​of the ideal trajectory to generate the upper and lower wrapping lines so that the ordinates of the actual trajectory are within the corresponding coordinate ranges of the upper and lower wrapping lines; Step 3: Use the dichotomy method to make the upper and lower wrapping lines gradually approach the actual trajectory, and take the vertical distance between the upper and lower wrapping lines as the AGV trajectory error; Step 4: Set different track speeds and repeat steps 1-3 to test. Measure the track error of the AGV at different speeds. Select the maximum AGV track speed under the set error requirement based on the AGV speed-track error data to improve the AGV operation efficiency.

2. The AGV trajectory error measurement method based on a laser tracker according to claim 1, characterized in that: In step 2, the least square method is used to fit the ideal trajectory according to the actual trajectory coordinates.

3. The AGV trajectory error measurement method based on a laser tracker according to claim 1, characterized in that: The AGV runs multiple cycles along the straight line trajectory and uses the least squares method to fit the ideal straight line trajectory, as follows: The ideal straight line trajectory expression is set as y=kx+b, where k is the slope of the line and b is the intercept; Use the least squares method to estimate the parameters, requiring the measured value y i The weighted sum of squared deviations Minimum, respectively, to find the partial derivative of k, b can be obtained equation bN+k∑xi=∑y i ,b∑x i +k∑x i 2 =∑x i y i , the optimal values ​​of the straight line parameters k and b are obtained respectively Thus, the ideal straight line trajectory is determined.

4. The AGV trajectory error measurement method based on a laser tracker according to claim 3, characterized in that: The upper wrapping line and the lower wrapping line are determined as follows: Maintain slope k m unchanged, set b0 as the initial value, so that Δ=[k m x i +b0]-[y i ] When all elements in are greater than or equal to 0, the straight line expression y=k m x+b0 is the initial upper wrapping line; determine b j The value range is [b m ,b0], let b j =(b0+b m ) / 2, substitute Δ for judgment. If Δ>0, it means that all positions of the upper wrapping line are above the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approximate the actual trajectory. Let b j As the upper limit of the value, we get b j+1 =(b j +b m ) / 2, continue to substitute Δ for judgment; If Δ<0, it means that part of the upper wrapping line is below the trajectory coordinate and does not completely wrap the trajectory coordinate. Let b j As the lower limit, we get b j+1 =(b m +b j ) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the upper wrapping line expression corresponding to the intercept value; Maintain slope k m unchanged, set b1 as the initial value, so that Δ=[k m x i +b1]-[y i ] When all elements in are less than 0, the straight line expression y=k m x+b1 is the initial lower wrapping line, determine b j The value range is [b1,b m ], let b j =(b m +b1) / 2, substitute Δ for judgment. If Δ<0, it means that all positions of the lower wrapping line are below the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approximate the actual trajectory. Let b m As the upper limit of the value, we get b j+1 =(b m +b j ) / 2, continue to substitute Δ for judgment; if Δ>0, it means that the lower wrapping line is partially above the track coordinate and does not completely wrap the track coordinate, then let b1 be the lower limit of the value, and get b j+1 =(b j +b1) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the lower wrapping line expression with the corresponding intercept value.

5. The AGV trajectory error measurement method based on a laser tracker according to claim 1, characterized in that: The AGV performs multiple cycles along the arc trajectory; The ideal arc trajectory is simplified to x 2 +y 2 +ax+by+c=0, where a=-2x0, b=-2y0, c=x0 2 +y0 2 -r 2 ; The least square method is used to fit the arc trajectory, requiring the sum of the squares of the distances between the ideal trajectory and the actual trajectory to be minimum, that is, f = ∑((x i -x0) 2 +(y i -y0) 2 -r 2 ) 2 =Σg(x i ,y i ) 2 The value is 0, which is the smallest. set up in After variable substitution, we can get Σ((a i -a0) 2 +(b i -b0) 2 -r 2 )a i =0,Σ((a i -a0) 2 +(b i -b0) 2 -r 2 )b i =0, after expansion and simplification, we get ∑(a i 2 b i -2a i b i b0+b i 3 -2b i 2 b0)=0; define various parameters, The formula is simplified to C+I-2Ea0-2Gb0=0,H+D-2Ga0-2Fb0=0, and the solution is That is, the ideal trajectory arc expression obtained by fitting is (x-x0) 2 +(y-y0) 2 =r 2 , (x0,y0) is the center of the ideal trajectory, and r is the radius.

6. The AGV trajectory error measurement method based on a laser tracker according to claim 5, characterized in that: The upper wrapping line and the lower wrapping line are determined as follows: Ensure that the center coordinates (x0, y0) remain unchanged, set r0 as the initial radius, so that Δ = r0 2 -(x i -x0) 2 -(y i -y0) 2 If all elements in the arc are greater than 0, the wrapping line expression on the arc trajectory is (x-x0) 2 +(y-y0) 2 =r0 2 , ensure that the initial upper wrapping line completely wraps the actual arc trajectory, and determine r j The value range is [r, r0], let r j =(r+r0) / 2 and bring in Δ to make a judgment. If Δ>0, it means that all positions of the upper wrapping line are above the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approach the actual trajectory. Then let r be the lower limit of the value and calculate r j+1 =(r j +r) / 2, continue to substitute Δ for judgment; If Δ<0, it means that part of the upper wrapping line is below the actual trajectory coordinates and does not completely wrap the trajectory coordinates. Then let r0 be the upper limit of the value, and get r j+1 =(r0+r j ) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the wrapping line expression on the arc trajectory according to the corresponding intercept value; Ensure that the center coordinates (x0, y0) remain unchanged, set r1 as the initial radius, so that Δ = r1 2 -(x i -x0) 2 -(y i -y0) 2 If all elements in the arc are less than 0, the wrapping line expression under the arc trajectory is (x-x0) 2 +(y-y0) 2 =r1 2 , ensure that the initial lower wrapping line completely wraps the actual arc trajectory, and determine r j The value range is [r1,r], let r j =(r+r1) / 2 and bring in Δ to make a judgment. If Δ<0, it means that all positions of the lower wrapping line are below the trajectory coordinates and completely wrap the trajectory coordinates. It is necessary to continue to approach the actual trajectory. Then let r be the upper limit of the value and calculate r j+1 =(r j +r) / 2, continue to substitute Δ for judgment; if Δ>0, it means that the lower wrapping line is partially above the actual trajectory coordinates and does not completely wrap the trajectory coordinates, then let r1 be the lower limit of the value, and get r j+1 =(r1+r j ) / 2, continue to substitute Δ for judgment; stop the loop when Δ=0 or the set number of loops is reached, and confirm the wrapping line expression under the arc trajectory corresponding to the radius value.

Images