A method for autonomous navigation and motion control of inspection robots

Abstract

The present invention discloses an autonomous navigation motion control method for an inspection robot, including navigation motion control outside an inspection ditch and navigation motion control inside an inspection ditch; the navigation motion control outside the inspection ditch is as follows: determining the current initial posture and driving direction of the robot, rotating the inspection robot body to the average angle direction of the initial posture angle, using an improved dead reckoning algorithm to predict the trajectory, and moving to a specified position according to the calculated trajectory and speed; the navigation motion control inside the inspection ditch is as follows: obtaining the distance to the target point, precalculating the linear speed control amount by trapezoidal acceleration and deceleration, fitting the walls on both sides of the inspection ditch by laser radar, obtaining the positioning data of the movement distance in the ditch at the same time, dynamically adjusting the linear speed and angular speed of the inspection robot, and keeping it traveling in a straight line to the specified position in the inspection ditch. The present invention can realize precise control of the robot body both inside and outside the inspection ditch, thereby safely and efficiently completing the daily inspection operation of the vehicle bottom.

Description

Technical Field

[0001] The invention belongs to the field of robots, and in particular relates to an autonomous navigation motion control method for an inspection robot. Background Art

[0002] In order to ensure the safe operation of passenger and freight vehicles, trains, subway trains, urban rail trains and other vehicles, it is necessary to conduct a comprehensive inspection of the vehicle's underbody parts outside of operating hours, including whether the underbody bolts are loose, whether the wheels are worn, whether the underbody structural parts have cracks, etc., so as to reduce the potential safety hazards in operation and ensure the normal operation of the vehicle. Traditional underbody inspections all use the method of manual inspection in the maintenance trench, but due to the high labor intensity, low maintenance efficiency and easy to cause false detection and missed detection, underbody inspection robots have begun to replace manual inspectors to complete the vehicle's underbody inspection.

[0003] The workflow and working scenario of the undercar inspection robot are very special. Usually, when receiving a new inspection task, the inspection robot starts from the ground charging station, enters the inspection trench through the elevator, and automatically navigates to the fixed point for data collection as required. After the collection is completed, the robot takes the elevator back to the ground and automatically returns to the charging station to recharge and re-enters the standby state. The biggest feature of this working scenario is that the working space is very narrow, especially in the process of autonomously entering and exiting the elevator and running in the inspection trench. The inspection robot has only 3cm-5cm margin on the left and right sides. This requires the inspection robot navigation control algorithm to be able to precisely control the inspection robot body with large volume and mass at the centimeter level. In addition, in order to ensure the quality of the collected image data, the inspection robot must maintain a straight line in the trench. However, traditional inspection robots generally operate in a relatively open environment, and there is no need to achieve such precise control of the robot body.

[0004] Therefore, it is necessary to study the motion control strategy of the inspection robot inside and outside the inspection trench for this scenario to solve the above problems. Summary of the invention

[0005] According to the unique working scenes of the under-vehicle inspection robot inside and outside the inspection trench, in order to ensure the precise control of the robot body with large volume and mass inside and outside the inspection trench, and thus safely and efficiently complete the daily inspection work under the vehicle, the present invention provides an autonomous navigation motion control method for the inspection robot.

[0006] The present invention provides an autonomous navigation motion control method for an inspection robot, including a navigation motion control method outside the inspection ditch and a navigation motion control method inside the inspection ditch. The autonomous navigation space of the undercar inspection robot includes the outside of the inspection ditch and the inside of the inspection ditch; when receiving a new inspection task, the inspection robot starts from the ground charging station and autonomously navigates to the designated location to prepare for entering the ditch for inspection via the elevator; the navigation motion control method outside the inspection ditch is as follows:

[0007] Step 1: Obtain the global navigation path through the Dijkstra algorithm and extract the current farthest tracking point according to the set track calculation time, which is used as the target point of the current movement of the inspection robot. Determine the current initial posture of the robot according to the requested positioning data source, and judge whether the robot should drive forward or backward according to the initial posture orientation angle and the angle from the final target point path.

[0008] Step 2: The inspection robot body rotates to the path to infer the average angle direction of the initial posture angle of the segment distance.

[0009] Step 3: Use the improved dead reckoning algorithm to predict the sampling trajectory, and select the path with the highest evaluation as the next running state of the inspection robot through the evaluation function. Calculate the current speed command of the robot in real time, and repeat the dead reckoning and speed command calculation to ensure that the inspection robot can accurately and efficiently drive to the final target point within the translation tolerance range.

[0010] Step 4: The inspection robot body rotates to within the rotation tolerance range of the final target point, and navigation ends.

[0011] After the inspection robot enters the inspection trench through the elevator, it will autonomously navigate to a fixed position according to the instructions to perform inspection operations; the navigation motion control method in the inspection trench is as follows:

[0012] Step A: Get the distance to the target point and pre-calculate the linear speed control amount by trapezoidal acceleration and deceleration.

[0013] Step B: Use laser radar to fit the walls on both sides of the inspection trench to obtain the geometric features of the walls, and construct two straight lines representing the left and right walls based on them, and at the same time obtain the movement distance positioning data in the trench.

[0014] Step C: Dynamically adjust the linear velocity v and angular velocity w of the inspection robot to keep it moving in a straight line in the inspection trench.

[0015] Step D: Repeat step C until the inspection robot reaches the displacement tolerance and navigation ends.

[0016] Furthermore, step 2 is as follows: first, obtain the current heading angle of the inspection robot, then find the farthest point that can be precalculated within the time of dead reckoning in the global path, traverse and accumulate the straight-line angle of the inspection robot to each point, and finally calculate the average angle within this distance, which is the angle that the inspection robot needs to rotate; the calculation formula of the angle average is as follows:

[0017]

[0018] In the formula, is the angle average, p 0 is the current point of the inspection robot in the global path, p 1 The inspection robot searches for the farthest point in the global path that can be precalculated within the dead reckoning time. i For p 0 The straight-line angle from point i to point i.

[0019] Furthermore, step 3 is specifically as follows:

[0020] Step 3.1: Determine the speed sampling space range, which is expressed as:

[0021] V m ={v∈[v min ,v max ],w∈[w min ,w max ]}

[0022] Among them, v min 、v max is the minimum and maximum linear speed of the inspection robot, w min 、w max are the minimum and maximum angular velocities of the inspection robot.

[0023] Step 3.2: Enable the improved dead reckoning algorithm to perform dead reckoning and speed command calculation.

[0024] The inspection robot goes through two stages of acceleration and uniform speed in one dead reckoning. The kinematic model expression at this time is:

[0025]

[0026] Among them, x(k), y(k), x(k+1), and y(k+1) represent the position coordinate information of the robot at time k and time k+1. acc ,y acc represents the distance traveled during the acceleration phase, x const ,y const represents the distance traveled during the uniform speed stage, v begin Indicates the initial velocity of the robot's track calculation, vend Indicates the target search speed of this dead reckoning, t sum Indicates the total time of the robot's travel, t acc represents the differential time, x acc ,ang acc represents the acceleration differential, ang begin Indicates the initial angle of the inspection robot's track calculation. end represents the target search angle of the inspection robot in this dead reckoning, v offset ,ang offset Represents the difference between the initial speed and the search speed, x acc ,ang acc It represents the speed change of the inspection robot in a differential process, and n represents the number of acceleration times required to accelerate the actual speed to the search speed.

[0027] Substitute the above-calculated trajectory into the trajectory evaluation function, select an optimal path based on the evaluation, and then publish the linear velocity v and angular velocity w corresponding to the path to drive the inspection robot to move; based on the working scenario of the inspection robot, the trajectory evaluation function scores according to two indicators: the degree of fit between the deduced track and the tracking point and the degree of fit with the global path. The mathematical expression is as follows:

[0028]

[0029] Among them, score represents the score of the current deduced track, α represents the weight of the distance from the end point of the deduced track to the tracking point in the calculation score, β represents the weight of the shortest distance from the deduced track to the global path in the calculation score, and dist 0 It indicates the distance from the end point of the deduced track to the tracking point, and dist1 indicates the shortest distance from the deduced track to the global path.

[0030] Repeat steps 3.1-3.2 until the inspection robot stops after reaching the displacement tolerance.

[0031] Further, step C is specifically as follows:

[0032] According to the front and rear radars, the distance difference between the inspection robot and the walls on both sides is calculated, with the goal of keeping the geometric relationship between the two straight lines symmetrical, and the angular velocity control amount is calculated through the PID algorithm.

[0033] Based on the current mileage, the trapezoidal acceleration and deceleration algorithm is used to pre-calculate the linear speed control quantity, and the current pre-calculated linear speed v and angular speed w are released. The inspection robot body moves according to the released linear speed v and angular speed w, which can ensure that the inspection robot moves in a straight line in the inspection ditch.

[0034] The calculation method of trapezoidal acceleration and deceleration pre-linear speed is as follows:

[0035]

[0036] The constraints are as follows:

[0037] x 1 =x 3 -x 2

[0038] Where 0≤x≤x 1 Indicates that the robot is in the acceleration stage, x 1 ≤x≤x 2 Indicates that the robot is in the uniform speed stage, x 2 ≤x≤x 3 Indicates that the robot is in the deceleration stage, x 3 Indicates the distance between the robot and the target point, max(a,b) is the maximum value of a and b, vel min is the minimum linear velocity of the robot, vel max is the maximum linear velocity of the robot, x is the current mileage of the robot, acc max is a constant.

[0039] The calculation method of angular velocity control amount is as follows:

[0040]

[0041] Among them, w output (k) represents the angular velocity of the inspection robot at time k, k p , k i , k d Represents the coefficients of proportional, integral, and differential links, dis diff (k) represents the distance difference between the inspection robot and the two walls at time k, v cur represents the real line velocity of the inspection robot at time k, v max Indicates the maximum linear speed that the inspection robot can reach.

[0042] Compared with the prior art, the present invention has the following advantages:

[0043] 1) During the autonomous navigation process of the inspection robot, it can autonomously determine whether the robot should drive forward or backward based on the initial posture orientation angle and the angle of the path from the final target point, thereby improving the efficiency of autonomous navigation.

[0044] 2) During the speed space search phase of out-of-ditch navigation, the process of accelerating the actual speed to the search speed through kinematic performance is taken into account in the track calculation, which is more accurate than the traditional track calculation result.

[0045] 3) A virtual tracking method is proposed for in-ditch navigation. The walls on both sides of the inspection ditch are fitted by lidar, and two straight lines representing the left and right walls are constructed. The goal is to keep the geometric relationship of the two straight lines symmetrical. The angular velocity control amount is calculated by the PID algorithm to keep the geometric relationship of the two straight lines symmetrical. This can ensure that the inspection robot can drive quickly and accurately in a straight line in the inspection ditch. Compared with the existing AGV tracking method, it does not require the laying of tracks or electromagnetic tracking lines, reducing the cost of robot deployment. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 The present invention is a flow chart of the navigation motion control method outside the inspection trench.

[0047] Figure 2 The present invention is a flow chart of the navigation motion control method for inspecting trenches.

[0048] Figure 3 This is the principle block diagram of the PID algorithm. DETAILED DESCRIPTION

[0049] The present invention is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0050] The invention discloses an autonomous navigation motion control method for an inspection robot, comprising a navigation motion control method outside an inspection trench and a navigation motion control method inside an inspection trench.

[0051] As the system actuator, the inspection robot is equipped with a three-dimensional laser radar, an inertial measurement unit, and a motor wheel speed meter for spatial positioning and navigation. Based on the ROS system, the inspection robot can be positioned and navigated. The autonomous navigation space of the undercar inspection robot includes the inspection ditch outside and the inspection ditch inside. When receiving a new inspection task, the inspection robot starts from the ground charging station and autonomously navigates to the designated location to prepare for entering the ditch for inspection via the elevator. Figure 1 As shown, the details are as follows:

[0052] Step 1: Obtain the global navigation path through the Dijkstra algorithm and extract the current farthest tracking point according to the set track calculation time, which is used as the target point of the current movement of the inspection robot. Determine the current initial posture of the robot according to the requested positioning data source, and judge whether the robot should drive forward or backward according to the initial posture orientation angle and the angle from the final target point path.

[0053] Step 2: The inspection robot body rotates to the path to infer the average angle direction of the initial posture angle of the segment distance.

[0054] First, obtain the current heading angle of the inspection robot, then find the farthest point that can be precalculated within the time of dead reckoning in the global path, traverse and accumulate the straight-line angle of the inspection robot to each point, and finally calculate the average angle within this distance, which is the angle that the inspection robot needs to rotate; the calculation formula of the angle average is as follows:

[0055]

[0056] In the formula, is the angle average, p 0 is the current point of the inspection robot in the global path, p 1 The inspection robot searches for the farthest point in the global path that can be precalculated within the dead reckoning time. i For p 0 The straight-line angle from point i to point i.

[0057] In this embodiment, the inspection robot hardware limit maximum speed is 1.0m / s. Assuming that the robot travels at the maximum speed, the track is estimated and the traveled distance is compared with the distance from the robot to each point in the global path to obtain the farthest point that can be precalculated. The straight-line angle from the robot to each point is obtained using the method provided in the ROS built-in software package angles.

[0058] Step 3: Use the improved dead reckoning algorithm to predict the sampling trajectory, and select the path with the highest evaluation as the next running state of the inspection robot through the evaluation function. Calculate the current speed command of the robot in real time, and repeat the dead reckoning and speed command calculation to ensure that the inspection robot can accurately and efficiently drive to the final target point within the translation tolerance range.

[0059] Step 3.1: Determine the speed sampling space range based on the current posture of the inspection robot, the distance to the target point, the dynamic performance, the robot hardware performance, the inspection robot operation scenario and other factors. The speed sampling space range is expressed as:

[0060] V m ={v∈[v min ,v max ],w∈[w min ,w max ]}

[0061] Among them, v min 、v max is the minimum and maximum linear speed of the inspection robot, w min 、w max are the minimum and maximum angular velocities of the inspection robot.

[0062] Step 3.2: Enable the improved dead reckoning algorithm to perform dead reckoning and speed command calculation.

[0063] The speed search space range of the inspection robot is known from step 3.1, and this range is subdivided into multiple groups of speeds, and each group of linear speed v and angular speed w corresponds to a feasible trajectory. The present invention proposes an improved method, in which the process of accelerating from the actual speed to the search speed via kinematic performance is taken into account during track calculation, thereby achieving high-precision navigation. The specific process is: according to the dynamic performance, the concepts of differential time and acceleration differential are introduced, and the process of accelerating the robot from the current speed to the search speed is divided into multiple differential stages according to time (differential time and number of differentials depend on the working needs of the inspection robot), and the inspection robot moves at different speeds in each differential stage until the actual speed of the inspection robot is accelerated to the search speed and maintains uniform motion. Therefore, the final calculated track takes into account the distance of the two stages of acceleration and uniform speed, thereby improving the navigation accuracy.

[0064] The inspection robot goes through two stages of acceleration and uniform speed in one dead reckoning. The kinematic model expression at this time is:

[0065]

[0066] Among them, x(k), y(k), x(k+1), and y(k+1) represent the position coordinate information of the robot at time k and time k+1. acc ,y acc represents the distance traveled during the acceleration phase, x const ,y const represents the distance traveled during the uniform speed stage, v begin Indicates the initial velocity of the robot's track calculation, v end Indicates the target search speed of this dead reckoning, t sum Indicates the total time of the robot's travel, t acc represents the differential time, x acc ,ang acc represents the acceleration differential, ang begin Indicates the initial angle of the inspection robot's track calculation. end represents the target search angle of the inspection robot in this dead reckoning, v offset ,ang offset Represents the difference between the initial speed and the search speed, x acc ,ang acc It represents the speed change of the inspection robot in a differential process, and n represents the number of acceleration times required to accelerate the actual speed to the search speed.

[0067] Based on the working scenario of the robot in this embodiment, the test results show that n = 10, t sum =3, t acc =0.1 has the best effect.

[0068] Substitute the above-calculated trajectory into the trajectory evaluation function, select an optimal path based on the evaluation, and then publish the linear velocity v and angular velocity w corresponding to the path to drive the inspection robot to move; based on the working scenario of the inspection robot, the trajectory evaluation function scores according to two indicators: the degree of fit between the deduced track and the tracking point and the degree of fit with the global path. The mathematical expression is as follows:

[0069]

[0070] Among them, score represents the score of the current deduced track, α represents the weight of the distance from the end point of the deduced track to the tracking point in the calculation score, β represents the weight of the shortest distance from the deduced track to the global path in the calculation score, and dist 0 It indicates the distance from the end point of the deduced track to the tracking point, and dist1 indicates the shortest distance from the deduced track to the global path.

[0071] In this embodiment, α=1, β=1 has a better effect.

[0072] Repeat steps 3.1-3.2 until the inspection robot stops after reaching the displacement tolerance.

[0073] Step 4: The inspection robot body rotates to within the rotation tolerance range of the final target point, and the inspection ditch navigation ends.

[0074] After the inspection robot enters the inspection trench through the elevator, it will autonomously navigate to a fixed position according to the instructions to perform inspection operations; the navigation motion control method in the inspection trench is as follows: Figure 2 As shown, the details are as follows:

[0075] Step A: Get the distance to the target point and pre-calculate the linear speed control amount by trapezoidal acceleration and deceleration.

[0076] Step B: Use laser radar to fit the walls on both sides of the inspection trench to obtain the geometric features of the walls, and construct two straight lines representing the left and right walls based on them, and at the same time obtain the movement distance positioning data in the trench.

[0077] Step C: Dynamically adjust the linear velocity v and angular velocity w of the inspection robot to keep it moving in a straight line in the inspection trench.

[0078] According to the front and rear radars, the distance difference between the inspection robot and the walls on both sides is calculated, with the goal of keeping the geometric relationship between the two straight lines symmetrical, and the angular velocity control amount is calculated through the PID algorithm.

[0079] In order to take into account both its driving smoothness and efficiency, based on the current mileage, the trapezoidal acceleration and deceleration algorithm is used to pre-calculate the linear speed control quantity, and the current pre-calculated linear speed v and angular speed w are released. The inspection robot body moves according to the released linear speed v and angular speed w, which can ensure that the inspection robot travels in a straight line in the inspection ditch.

[0080] The calculation method of trapezoidal acceleration and deceleration pre-linear speed is as follows:

[0081]

[0082] The constraints are as follows:

[0083] x 1 =x 3 -x 2

[0084] Where 0≤x≤x 1 Indicates that the robot is in the acceleration stage, x 1 ≤x≤x 2 Indicates that the robot is in the uniform speed stage, x 2 ≤x≤x 3 Indicates that the robot is in the deceleration stage, x 3 Indicates the distance between the robot and the target point, max(a,b) is the maximum value of a and b, vel min is the minimum linear velocity of the robot, vel max is the maximum linear velocity of the robot, x is the current mileage of the robot, acc max is a constant.

[0085] This embodiment tests multiple groups of inspection robot minimum linear velocity, robot maximum linear velocity, and robot maximum acceleration. Taking into account the running stability and navigation efficiency of the inspection robot in this working scenario, the best combination is vel min =0.02,vel max =0.8,acc max =0.6.

[0086] The principle block diagram of the PID algorithm is as follows: Figure 3 As shown, the calculation method of angular velocity control amount is as follows:

[0087]

[0088] Among them, w output (k) represents the angular velocity of the inspection robot at time k, k p , k i , k d Represents the coefficients of proportional, integral, and differential links, dis diff (k) represents the distance difference between the inspection robot and the two walls at time k, vcur represents the real line velocity of the inspection robot at time k, v max Indicates the maximum linear speed that the inspection robot can reach.

[0089] This embodiment tests multiple groups of PID coefficients, and the best combination for the robot to operate in this working scenario is k p =0.2,k i =0,k d =0.05.

[0090] Step D: Repeat step C until the inspection robot reaches the displacement tolerance and navigation ends.

[0091] In order to verify the navigation accuracy of the motion control strategy of the undercar inspection robot, we chose to install a laser pen on the side of the vehicle body, stick tape on the ground, use the current position output by the inspection robot positioning module as the navigation target point, repeatedly send navigation instructions to the inspection robot, so that the inspection robot repeatedly goes to the target point from other points, and use a ballpoint pen on the tape to record the parking point. After recording several points, take the absolute value of the error and calculate the average value. The smaller the value, the higher the navigation accuracy. The results of navigation outside the inspection ditch and navigation inside the inspection ditch are shown in Table 1 and Table 2 respectively.

[0092] Table 1 Results of navigation test outside the inspection trench

[0093] Serial number Target pose (m) Arrival position (m) x error (m) y error (m) 1 (10.0,10.0) (9.993,9.992) -0.007 -0.008 2 (10.0,10.0) (10.008,10.007) 0.008 0.007 3 (10.0,10.0) (10.012,10.009) 0.012 0.009 4 (10.0,10.0) (10.00,10.00) 0.00 0.00 5 (10.0,10.0) (10.01,10.007) 0.01 0.007 6 (10.0,10.0) (9.989,9.99) -0.011 -0.01 7 (10.0,10.0) (10.012,10.008) 0.012 0.008 8 (10.0,10.0) (9.99,9.992) -0.01 -0.008

[0094] Table 2 Results of navigation test in inspection trench

[0095] Serial number Target pose (m) Arrival position (m) x error (m) y error (m) 1 (100.0,0) (99.993,-0.003) -0.007 -0.003 2 (100.0,0) (100.008,0.002) 0.008 0.002 3 (100.0,0) (100.001,0.002) 0.01 0.002 4 (100.0,0) (99.994,0.00) -0.006 0.00 5 (100.0,0) (99.993,0.003) -0.007 0.003 6 (100.0,0) (99.004,-0.001) 0.004 -0.001 7 (100.0,0) (99.998,0.003) -0.008 0.003 8 (100.0,0) (100.01,-0.002) 0.01 -0.002

[0096] Table 1 shows the results of the x- and y-direction accuracy tests of the positioning and navigation outside the inspection trench. The average error of the eight points was ±0.01m in the x-direction and ±0.0085m in the y-direction. In the eight navigations, the robot did not fail to reach the point, which verified the accuracy and reliability of the positioning and navigation algorithm outside the trench. Table 2 shows the results of the x- and y-direction accuracy tests of the positioning and navigation inside the inspection trench. The average error of the eight points was ±0.0075m in the x-direction and ±0.0025m in the y-direction. In the eight navigations, the robot did not fail to reach the point, which verified the accuracy and reliability of the positioning and navigation algorithm inside the trench.

Claims

1. A method for autonomous navigation motion control of an inspection robot, characterized in that: The autonomous navigation space of the undercar inspection robot includes the inspection ditch outside and the inspection ditch inside. When receiving a new inspection task, the inspection robot starts from the ground charging station and autonomously navigates to the designated location to prepare for entering the ditch for inspection through the elevator. The specific control method of navigation motion outside the inspection ditch is as follows: Step 1: Obtain the global navigation path through the Dijkstra algorithm and extract the current farthest tracking point according to the set dead reckoning duration, which is used as the target point of the current movement of the inspection robot. Determine the current initial posture of the robot according to the requested positioning data source, and determine whether the robot should drive forward or backward according to the initial posture orientation angle and the angle from the final target point path; Step 2: The inspection robot body rotates to the path where the average angle direction of the initial posture angle of the calculated segment distance can be reached; Step 3: Use the improved dead reckoning algorithm to predict the sampling trajectory, and select the path with the highest evaluation as the next running state of the inspection robot through the evaluation function, calculate the current speed command of the robot in real time, and repeat the dead reckoning and speed command calculation to ensure that the inspection robot accurately and efficiently drives to the final target point within the translation tolerance range; Step 4: The inspection robot body rotates to within the rotation tolerance range of the final target point, and navigation ends; After the inspection robot enters the inspection trench through the elevator, it will autonomously navigate to a fixed position to perform inspection operations according to the instructions; The specific method for controlling navigation motion in the inspection trench is as follows: Step A: Get the distance to the target point and pre-calculate the linear speed control amount by trapezoidal acceleration and deceleration; Step B: Fit the walls on both sides of the inspection trench through laser radar to obtain the geometric features of the walls, and construct two straight lines representing the left and right walls based on them, and obtain the movement distance positioning data in the trench at the same time; Step C: Dynamically adjust the inspection robot's linear velocity v and angular velocity w to keep it moving in a straight line in the inspection trench; Step D: Repeat step C until the inspection robot reaches the displacement tolerance and navigation ends.

2. The autonomous navigation motion control method of an inspection robot according to claim 1, characterized in that: The step 2 is specifically as follows: first, obtain the current heading angle of the inspection robot, then find the farthest point that can be precalculated within the dead reckoning time in the global path, traverse and accumulate the straight-line angle of the inspection robot to each point, and finally calculate the average angle within this distance, which is the angle that the inspection robot needs to rotate; the calculation formula of the angle average is as follows: In the formula, is the average angle, p0 is the current point of the inspection robot in the global path, p1 is the farthest point that can be precalculated within the time of dead reckoning in the global path, and x i is the straight-line angle from point p0 to point i.

3. The autonomous navigation motion control method of an inspection robot according to claim 1, characterized in that: The step 3 is specifically as follows: Step 3.1: Determine the speed sampling space range, which is expressed as: V m ={v∈[v min ,v max ],w∈[w min ,w max ]} Among them, v min 、v max is the minimum and maximum linear speed of the inspection robot, w min 、w max is the minimum and maximum angular velocity of the inspection robot; Step 3.2: Enable the improved dead reckoning algorithm to perform dead reckoning and speed command calculation; The inspection robot goes through two stages of acceleration and uniform speed in one dead reckoning. The kinematic model expression at this time is: Among them, x(k), y(k), x(k+1), and y(k+1) represent the position coordinate information of the robot at time k and time k+1. acc ,y acc represents the distance traveled during the acceleration phase, x const ,y const represents the distance traveled during the uniform speed stage, v begin Indicates the initial velocity of the robot's track calculation, v end Indicates the target search speed of this dead reckoning, t sum Indicates the total time of the robot's travel, t acc represents the differential time, x acc ,ang acc represents the acceleration differential, ang begin Indicates the initial angle of the inspection robot's track calculation. end represents the target search angle of the inspection robot in this dead reckoning, v offset ,ang offset Represents the difference between the initial speed and the search speed, x acc ,ang acc It represents the speed change of the inspection robot in a differential process, and n represents the number of acceleration times required to accelerate the actual speed to the search speed; Substitute the above-calculated trajectory into the trajectory evaluation function, select an optimal path based on the evaluation, and then publish the linear velocity v and angular velocity w corresponding to the path to drive the inspection robot to move; based on the working scenario of the inspection robot, the trajectory evaluation function scores according to two indicators: the degree of fit between the deduced track and the tracking point and the degree of fit with the global path. The mathematical expression is as follows: Among them, score represents the score of the current deduced track, α represents the weight of the distance from the end point of the deduced track to the tracking point in the calculation score, β represents the weight of the shortest distance from the deduced track to the global path in the calculation score, dist0 represents the distance from the end point of the deduced track to the tracking point, and dist1 represents the shortest distance from the deduced track to the global path; Repeat steps 3.1-3.2 until the inspection robot stops after reaching the displacement tolerance.

4. The autonomous navigation motion control method of an inspection robot according to claim 1, characterized in that: The step C is specifically as follows: According to the front and rear radars, the distance difference between the inspection robot and the walls on both sides is calculated, with the goal of keeping the geometric relationship between the two straight lines symmetrical, and the angular velocity control amount is calculated through the PID algorithm; Based on the current mileage, the linear speed control amount is pre-calculated using the trapezoidal acceleration and deceleration algorithm, and the currently pre-calculated linear speed v and angular speed w are released. The inspection robot body moves according to the released linear speed v and angular speed w, which can ensure that the inspection robot moves in a straight line in the inspection ditch. The calculation method of trapezoidal acceleration and deceleration pre-linear speed is as follows: The constraints are as follows: x1=x3-x2 Among them, 0≤x≤x1 means that the robot is in the acceleration stage, x1≤x≤x2 means that the robot is in the uniform speed stage, x2≤x≤x3 means that the robot is in the deceleration stage, x3 means the distance between the robot and the target point, max(a,b) is the maximum value of a and b, vel min is the minimum linear velocity of the robot, vel max is the maximum linear velocity of the robot, x is the current mileage of the robot, acc max is a constant; The calculation method of angular velocity control amount is as follows: Among them, w output (k) represents the angular velocity of the inspection robot at time k, k p , k i , k d Represents the coefficients of proportional, integral, and differential links, dis diff (k) represents the distance difference between the inspection robot and the two walls at time k, v cur represents the real line velocity of the inspection robot at time k, v max Indicates the maximum linear speed that the inspection robot can reach.

Images