CN103707298B - The evaluation method of a kind of continuous type robot space path tracking effect - Google Patents

Abstract

A method for evaluating the effect of continuous robot space path tracking. It includes the stage of establishing evaluation indicators and the stage of tracking calculation; the stage of establishing evaluation indicators is to first establish the evaluation indicators of path tracking performance: maximum error, average error, response time and control accuracy; the tracking calculation stage includes establishing points of snake arm end joints, scoring Point path coordinates, create an array of path points, track a path joint segment, judge whether it is the first joint segment, find the distance between the first joint segment point, find the distance from the point to the path, calculate the maximum distance value, record the response time, Judging whether the path tracking is completed, determining the maximum error value, determining the average error, determining the control accuracy and other stages: the continuous robot path tracking effect evaluation method provided by the present invention can objectively describe the tracking situation, has low time complexity and is easy to implement by software features.

Description

An Evaluation Method for Space Path Tracking Effect of Continuous Robot

technical field

The invention belongs to the technical field of automatic control, in particular to a method for evaluating the space path tracking effect of a continuous robot.

Background technique

The continuous robot is a new type of bionic robot with a "spineless" flexible structure. It has good bending performance and can change its shape softly and flexibly. comparable to biological organs. Since the shape of the continuous robot can be changed flexibly, it has the ability to change its own shape according to the conditions of environmental obstacles, and has a unique ability to adapt to the environment with limited working space. It has broad application prospects and can be used in aircraft fuel tank inspections, operations in industrial environments with multiple obstacles, detection and search and rescue in curved pipes and collapsed buildings, maintenance of internal pipelines in nuclear power plants, diagnosis and treatment of human diseases, etc.

The continuous robot is a serial robot composed of multiple joint segments, whose joint segments have structural constraints and can perform spatial bending and rotating motions. In an environment with strong spatial structure constraints, in order to avoid damage or potential danger caused by robot contact, it is necessary to plan the travel path of the robot to reach the target area. The path is composed of several curves similar to continuous joints, satisfying its structural constraints, and is the final pose of the continuous robot to reach the target area. When designing a tracking algorithm, in order to analyze and evaluate the tracking effect, it is necessary to propose an evaluation method. However, there is still a lack of basis for evaluating the tracking effect of continuous robots.

Under this technical background, there is no evaluation method for the tracking effect of continuous robots in space.

Contents of the invention

In order to solve the above problems, the object of the present invention is to provide a method for evaluating the space path tracking effect of a continuous robot.

In order to achieve the above object, the evaluation method of the continuous robot space path tracking effect provided by the present invention includes the stage of establishing evaluation index and the stage of tracking calculation;

(1) The stage of establishing evaluation indicators

Firstly, establish path tracking performance evaluation indicators: maximum error, average error, response time and control accuracy;

(2) Tracking calculation stage

In each step of the stepping process of the snake arm tracking path, calculate and record the various parameters required by the evaluation index, and determine the various values of the evaluation index after the tracking is completed.

In the stage of establishing evaluation indicators, the evaluation indicators include:

1.1) Maximum error

The maximum error refers to the maximum deviation distance between the snake arm joint segment and the corresponding path segment when the continuous robot snake arm tracks the path; since only the joint segment variables of the snake arm end joint segment are calculated during the path tracking process, the remaining joint segments repeat the snake arm The movement of the end joint segment, so after each step of the snake arm base, only the maximum distance between the end joint segment and the path needs to be solved. The maximum value of these maximum distances is the maximum error; the number of path joint segments is n, and the maximum distance is set is d _maxt (t=1,2,3,...,nw), the maximum error is e _max ;

1.2) Average error

The average error refers to the average value of all the maximum distances between the end joint segment of the snake arm and the path joint segment, which is used to measure the average deviation in the tracking process; the average error is set as but

$\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; nw</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; max</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}{\mathrm{<span\; class="notranslate">\; nw</span>}}$

1.3) Response time

Response time refers to the time required to calculate the variable of the snake arm joint segment each time, which is used to measure the rapidity of the control algorithm;

1.4) Control accuracy

Control accuracy refers to the distance between the end point of the snake arm and the end point of the path after the path tracking is completed; the path is generated according to the path planning algorithm after the target point to be detected is given, and the distance to the end point of the path is the distance to the target point ; Control accuracy is used to measure the accuracy of reaching the target point.

The specific operation process of the tracking calculation phase includes the following phases executed in sequence:

Step 1. The S01 stage of establishing the end joint segment of the snake arm: divide the end joint segment of the snake arm into w parts;

Step 2. The S02 stage of calculating the path coordinates {B ₀ } of the branch point: Find the coordinates of the branch point in the path coordinate system {B ₀ }, and record it as an array U(q)(q=1,2,...,w );

Step 3. The S03 stage of establishing the path point array {V _p,q }: Divide each joint segment of the path into w parts, and the points form an array V(p,q)(p=1,2,...,n ;q=1,2,...,w);

Step 4: Stage S04 of tracking a joint segment of a path: the snake arm moves forward one step, and completes the action of tracking the path; that is, the joint segment at the end of the snake arm tracks a joint segment of the path, and the jth (0≤j≤w) step of the base Enter;

Step 5. Stage S05 of judging whether it is the first joint segment: judge whether the tracking object of the current joint segment at the end of the snake arm is the first joint segment of the path. One step into the S07 stage;

Step 6. The S06 stage of calculating the distance of the first joint segment: determine the distance of the first joint segment of the tracking path, and calculate the distance from the last j points in the array U(q) to all the points in the array V(1,q) respectively. The distance of the point, take the minimum value as the distance from the point on the snake arm to the path, and the maximum value of these distance values is the maximum distance d _maxt to the path this time;

Step 7. The S07 stage of calculating the distance from the subpoint to the path: when tracking the mth joint segment of the path (1<m≤n), it is necessary to calculate each point in the array U(q) to the array V(m-1,q) and the distance of all points in V(m,q), the minimum value is taken as the approximate value of the distance from the point to the path, and the minimum value is approximately considered to be the distance from the point to the path;

Step 8. The S08 stage of calculating the maximum distance value: After each step of the base, the above distance is calculated, and the maximum distance is approximately considered to be the maximum distance d _maxt between the terminal joint segment and the path,

Step 9, the S09 stage of recording the response time: the response time refers to the time required to calculate the variables of the snake arm joint segment each time, and is used to measure the rapidity of the calculation process;

Step 10. Determine whether the path tracking is completed in the S10 stage: judge whether the current end of the snake arm reaches the end of the path, if the judgment result is "yes", then enter the next step S11 stage, otherwise the next step re-enter the S04 stage and continue step tracking ;

Step eleven, the S11 stage of determining the maximum error value: the maximum error is the maximum value in the maximum distance;

The maximum error e _max is:

e _max =max{d _max1 ,d _max2 ,...,d _maxt }

Step 12, the S12 stage of determining the average error: the average error refers to the average value of all the maximum distances between the joint segment at the end of the snake arm and the joint segment of the path, and is used to measure the average deviation in the tracking process; the average error is set as but

$\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; nw</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; max</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}{\mathrm{<span\; class="notranslate">\; nw</span>}}$

Step 13, the S12 stage of determining the control accuracy: the control accuracy refers to the distance between the end point of the snake arm and the end point of the path after the path tracking is completed; the path is generated according to the path planning method after the target point to be detected is given, and the path The distance from the end point is the distance from the target point; the control accuracy is used to measure the accuracy of reaching the target point; this process ends here.

The continuous robot path tracking effect evaluation method provided by the invention can objectively describe the tracking situation, and has the characteristics of low time complexity and easy software implementation.

Description of drawings

Figure 1: Schematic diagram of the continuous robot structure.

Fig. 2: Flowchart of the evaluation method for the space path tracking effect of the continuous robot provided by the present invention.

Figure 3: Schematic diagram of solving the maximum distance.

Figure 4: Continuous robot tracking coplanar path of three-joint segments - simulation experiment diagram.

Figure 5: Curves of the coplanar path error of the three-joint segment tracked by the continuous robot.

Figure 6: Schematic diagram of continuous robot tracking three-dimensional path in three-joint segment space.

Detailed ways

The method for evaluating the space path tracking effect of a continuous robot provided by the present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments.

As shown in Figure 1, the continuous robot is a new type of bionic robot with high flexibility and compliance. It is generally composed of a flexible mechanism 1, a telescopic mechanism 2, a control system 3 and a mobile platform 4. , which is called the snake arm here; the snake arm is usually a series mechanism composed of multiple structural units, and each structural unit is called a joint segment, which has a rotation and bending θ two degrees of freedom, called joint segment variables. The telescopic mechanism 2 has one degree of freedom, which can realize precise one-dimensional movement. The lifting platform on the top of the telescopic mechanism 2 is called the base, which provides the basis for the snake arm to travel. The control system 3 is the core of the control, which realizes the movement path planning, path tracking control, detection signal processing and remote transmission of the snake arm.

As shown in Figure 2, the evaluation method of the continuous robot space path tracking effect provided by the present invention includes the stage of establishing evaluation indicators and the stage of tracking calculation;

(1) The stage of establishing evaluation indicators

In order to better evaluate the effect of tracking, first establish path tracking performance evaluation indicators: maximum error, average error, response time and control accuracy;

1.1) Maximum error

The maximum error refers to the maximum deviation distance between the snake arm joint segment and the corresponding path segment when the continuous robot snake arm tracks the path; since only the joint segment variables of the snake arm end joint segment are calculated during the path tracking process, the remaining joint segments repeat the snake arm The movement of the end joint segment, so after each step of the snake arm base, only the maximum distance between the end joint segment and the path needs to be solved. The maximum value of these maximum distances is the maximum error; the number of path joint segments is n, and the maximum distance is set is d _maxt (t=1,2,3,...,nw), the maximum error is e _max ;

1.2) Average error

The average error refers to the average value of all the maximum distances between the end joint segment of the snake arm and the path joint segment, which is used to measure the average deviation in the tracking process; the average error is set as but

$\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; nw</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; max</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}{\mathrm{<span\; class="notranslate">\; nw</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

1.3) Response time

Response time refers to the time required to calculate the variable of the snake arm joint segment each time, which is used to measure the rapidity of the control algorithm;

1.4) Control accuracy

Control accuracy refers to the distance between the end point of the snake arm and the end point of the path after the path tracking is completed; the path is generated according to the path planning algorithm after the target point to be detected is given, and the distance to the end point of the path is the distance to the target point ;Control accuracy is used to measure the accuracy of reaching the target point;

(2) Tracking calculation stage

In each step of the stepping process of the snake arm tracking path, calculate and record the parameters required for the evaluation index, and after the tracking is completed, determine the values of the evaluation index; the process includes the following stages:

Step 1. The S01 stage of establishing the end joint segment of the snake arm: divide the end joint segment of the snake arm into w parts;

Step 2. The S02 stage of calculating the path coordinates {B ₀ } of the branch point: Find the coordinates of the branch point in the path coordinate system {B ₀ }, and record it as an array U(q)(q=1,2,...,w );

Step 3. The S03 stage of establishing the path point array {V _p,q }: Divide each joint segment of the path into w parts, and the points form an array V(p,q)(p=1,2,...,n ;q=1,2,...,w);

Step 4: Stage S04 of tracking a joint segment of a path: the snake arm moves forward one step, and completes the action of tracking the path; that is, the joint segment at the end of the snake arm tracks a joint segment of the path, and the jth (0≤j≤w) step of the base Enter;

Step 5. Stage S05 of judging whether it is the first joint segment: judge whether the tracking object of the current joint segment at the end of the snake arm is the first joint segment of the path. One step into the S07 stage;

Step 6. The S06 stage of calculating the distance of the first joint segment: determine the distance of the first joint segment of the tracking path, and calculate the distance from the last j points in the array U(q) to all the points in the array V(1,q) respectively. The distance of the point, take the minimum value as the distance from the point on the snake arm to the path, and the maximum value of these distance values is the maximum distance d _maxt to the path this time;

Step 7. The S07 stage of calculating the distance from the subpoint to the path: when tracking the mth joint segment of the path (1<m≤n), it is necessary to calculate each point in the array U(q) to the array V(m-1,q) and the distances of all points in V(m,q); Fig. 3 is a schematic diagram of solving the maximum distance, and Fig. 3(a) is the distance from a point in the joint segment at the end of the snake arm to each subpoint of the first joint segment of the path, and the minimum value is taken as the The approximate value of the distance from the point to the path. Figure 3(b) shows the distance from the point on the joint segment at the end of the snake arm to each point on the two joint segments of the path when tracking the second joint segment of the path. It is approximately considered that the minimum value is from the point to the path distance;

Step 8. The S08 stage of calculating the maximum distance value: After each step of the base, the above distance is calculated, and the maximum distance is approximately considered to be the maximum distance d _maxt between the terminal joint segment and the path,

Step 9, the S09 stage of recording the response time: the response time refers to the time required to calculate the variables of the snake arm joint segment each time, and is used to measure the rapidity of the calculation process;

Step 10. Determine whether the path tracking is completed in the S10 stage: judge whether the current end of the snake arm reaches the end of the path, if the judgment result is "yes", then enter the next step S11 stage, otherwise the next step re-enter the S04 stage and continue step tracking ;

Step eleven, the S11 stage of determining the maximum error value: the maximum error is the maximum value in the maximum distance;

The maximum error e _max is:

e _max = max{d _max1 ,d _max2 ,...,d _maxt }(2);

Step 12, the S12 stage of determining the average error: the average error refers to the average value of all the maximum distances between the joint segment at the end of the snake arm and the joint segment of the path, and is used to measure the average deviation in the tracking process; the average error is set as but

$\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; nw</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; max</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}{\mathrm{<span\; class="notranslate">\; nw</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ;</span>$

Step 13, the S12 stage of determining the control accuracy: the control accuracy refers to the distance between the end point of the snake arm and the end point of the path after the path tracking is completed; the path is generated according to the path planning method after the target point to be detected is given, and the path The distance from the end point is the distance from the target point; the control accuracy is used to measure the accuracy of reaching the target point; this process ends here.

The operation effect of the evaluation method of the continuous robot space path tracking effect provided by the present invention:

Use MATLAB to carry out the simulation experiment. In the experiment, the number of joint segments after path planning and the variables of each joint segment are given; the length of a single joint segment is L=50cm, and the step value of the base is s=1cm; Table 1 shows the path tracking experiment of a single joint segment Data, as the bending angle of the joint segment increases, the maximum error and the average error gradually increase; Table 2 shows the tracking experimental data of the path of the three joint segments when the rotation angle is 0°, at this time, the three joint segments are coplanar; Table 3 It is the experimental data of three-joint segment space path tracking. When the rotation angles are different, the two joint segments have different planes.

Table 1 Path tracking experiment of single joint segment

Table 2 Three-joint segment coplanar path tracking experiment

Table 3 Three-joint space path tracking experiment

It can be obtained from the experimental data that for different paths, the maximum error and average error indicators can indicate the degree of deviation from the path during the snake arm tracking process; the response time indicates the rapidity of the tracking method; the control accuracy indicates the deviation from the target point, indicating the tracking method. Accuracy.

In order to verify the effect of the evaluation method provided by the present invention, Fig. 4 shows a simulation diagram of tracking the coplanar path of three joint segments, and the variable of the joint segment of the path is the sixth group of data in Table 2; Fig. 4 (a), Fig. 4 (b) and Fig. 4(c) are respectively the first, second and third joint segments of the tracking path. During the tracking process, the end point of the snake arm joint segment corresponding to the path joint segment approaches the path, and the rotation of the snake arm end joint segment in Fig. 4(c) The angle is 180°. Figure 4(d) shows that the snake arm coincides with the path after the tracking is completed; Figure 5 shows the coplanar path error curve of the tracking three-joint segment, and the circle represents the maximum distance d _maxt of the snake arm to the path each time , the straight line is the average error ; Fig. 6 shows the simulation diagram of tracking the three-dimensional path of the three-joint segment space, and the variable of the path joint segment is the second group of data in Table 3; Fig. 6 (a), Fig. 6 (b) and Fig. 6 (c) are the tracking The first, second and third joint segments of the path. Figure 6(d) shows the completed path tracking. The snake arm coincides with the path. Figure 6(e) shows the space area swept by the snake arm during the tracking process. It can be seen that the snake arm Always stay close to the path during tracking.

Claims (3)

1. An evaluation method of continuous robot space path tracking effect, characterized in that: it includes the establishment of evaluation index stage and tracking calculation stage; (1) The stage of establishing evaluation indicators First, establish path tracking performance evaluation indicators: maximum error, average error, response time and control precision; the maximum error refers to the maximum deviation distance between the snake arm joint segment and the corresponding path segment when the continuous robot snake arm tracks the path; the average error is Refers to the average value of all the maximum distances between the joint segment at the end of the snake arm and the joint segment of the path; the response time refers to the time required to calculate the variable of the joint segment of the snake arm each time; the control accuracy refers to the distance between the end point of the snake arm and the path after path tracking distance from end point; (2) Tracking calculation stage In each step of the stepping process of the snake arm tracking path, calculate and record the various parameters required by the evaluation index, and determine the various values of the evaluation index after the tracking is completed. 2. the evaluation method of continuous robot space path tracking effect according to claim 1, is characterized in that: in establishing evaluation index stage, described evaluation index comprises: 1.1) Maximum error The maximum error refers to the maximum deviation distance between the snake arm joint segment and the corresponding path segment when the continuous robot snake arm tracks the path; since only the joint segment variables of the snake arm end joint segment are calculated during the path tracking process, the remaining joint segments repeat the snake arm The movement of the end joint segment, so after each step of the snake arm base, only the maximum distance between the end joint segment and the path needs to be solved. The maximum value of these maximum distances is the maximum error; the number of path joint segments is n, and the maximum distance is set is d _maxt , where t=1,2,3,…,nw, the maximum error is e _max ; 1.2) Average error The average error refers to the average value of all the maximum distances between the end joint segment of the snake arm and the path joint segment, which is used to measure the average deviation in the tracking process; the average error is set as but $\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}\mathrm{<span\; class="notranslate">\; w</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; max</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}{\mathrm{<span\; class="notranslate">\; no</span>}\mathrm{<span\; class="notranslate">\; w</span>}}$ Among them, w represents the number of shares equally divided by the joint segment at the end of the snake arm; 1.3) Response time Response time refers to the time required to calculate the variable of the snake arm joint segment each time, which is used to measure the rapidity of the control algorithm; 1.4) Control accuracy Control accuracy refers to the distance between the end point of the snake arm and the end point of the path after the path tracking is completed; the path is generated according to the path planning algorithm after the target point to be detected is given, and the distance to the end point of the path is the distance to the target point ; Control accuracy is used to measure the accuracy of reaching the target point. 3. The evaluation method of the continuous robot space path tracking effect according to claim 1, characterized in that: the specific operation process of the tracking calculation stage includes the following stages executed in order: Step 1. The S01 stage of establishing the end joint segment of the snake arm: divide the end joint segment of the snake arm into w parts; Step 2. The S02 stage of finding the path coordinates {B ₀ } of the branch point: finding the coordinates of the branch point in the path coordinate system {B ₀ }, which is recorded as an array U(q), where q=1,2,...,w; Step 3, the S03 stage of establishing the path subpoint array {V _p,q }: Divide each joint segment of the path into w parts, and the subpoints form an array V(p,q), where p=1,2,...,n; q=1,2,...,w; Step 4: Stage S04 of tracking a joint segment of a path: the snake arm moves forward one step, and completes the action of tracking the path; that is, the joint segment at the end of the snake arm tracks a joint segment of the path, and the j-th step of the base, where 0≤j≤ w; Step 5. Stage S05 of judging whether it is the first joint segment: judge whether the tracking object of the current joint segment at the end of the snake arm is the first joint segment of the path. One step into the S07 stage; Step 6. The S06 stage of calculating the distance of the first joint segment: determine the distance of the first joint segment of the tracking path, and calculate the distance from the last j points in the array U(q) to all the points in the array V(1,q) respectively. The distance of the point, take the minimum value as the distance from the point on the snake arm to the path, and the maximum value of these distance values is the maximum distance d _maxt to the path this time; Step 7. The S07 stage of calculating the distance from the subpoint to the path: when tracking the mth joint segment of the path, where 1<m≤n, it is necessary to calculate each point in the array U(q) to the array V(m-1,q) and the distance of all points in V(m,q), the minimum value is taken as the approximate value of the distance from the point to the path, and the minimum value is approximately considered to be the distance from the point to the path; Step 8. The S08 stage of calculating the maximum distance value: After each step of the base, the above distance is calculated, and the maximum distance is approximately considered to be the maximum distance d _maxt between the terminal joint segment and the path, Step 9, the S09 stage of recording the response time: the response time refers to the time required to calculate the variables of the snake arm joint segment each time, and is used to measure the rapidity of the calculation process; Step 10. Determine whether the path tracking is completed in the S10 stage: judge whether the current end of the snake arm reaches the end of the path, if the judgment result is "yes", then enter the next step S11 stage, otherwise the next step re-enter the S04 stage and continue step tracking ; Step eleven, the S11 stage of determining the maximum error value: the maximum error is the maximum value in the maximum distance; The maximum error e _max is: e _max ＝max{d _max1 ,d _max2 ,…,d _maxt } Step 12, the S12 stage of determining the average error: the average error refers to the average value of all the maximum distances between the joint segment at the end of the snake arm and the joint segment of the path, and is used to measure the average deviation in the tracking process; the average error is set as but $\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}\mathrm{<span\; class="notranslate">\; w</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; max</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}{\mathrm{<span\; class="notranslate">\; no</span>}\mathrm{<span\; class="notranslate">\; w</span>}}$ Step 13, the S13 stage of determining the control accuracy: the control accuracy refers to the distance between the end point of the snake arm and the end point of the path after the path tracking is completed; the path is generated according to the path planning method after the target point to be detected is given, and the path The distance from the end point is the distance from the target point; the control accuracy is used to measure the accuracy of reaching the target point; this process ends here.