Inspection robot path planning method based on improved A-star fusion DWA optimization algorithm

By improving the integration of A-star algorithm and DWA algorithm, combining greedy optimization and adaptive weight coefficients, the smoothness and efficiency of path planning in the existing technology are solved, and the global optimal path of greenhouse inspection robots in greenhouse environment is realized. Planning and dynamic obstacle avoidance improve work safety.

CN115079705BActive Publication Date: 2024-11-08SHANXI JINGHONGRUI ELECTRIC POWER TECHNOLOGY CO LTD +1
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210945247.5
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-08-08
Publication Date
2024-11-08
Estimated Expiration
2042-08-08

AI Technical Summary

Technical Problem

The existing A-star algorithm and DWA algorithm have problems such as poor smoothness, long planning time, many inflection points, discontinuous path curvature and inability to achieve global optimality in path planning, especially in greenhouse environments that cannot avoid unknown obstacles in real time. , which leads to safety hazards.

Method used

Using the improved A-star fusion DWA optimization algorithm, the redundant nodes are eliminated by initializing parameters and raster maps, combined with greedy optimization methods, the search efficiency is improved by using the minimum heap fusion two-dimensional array structure, and the heuristic function is improved through adaptive weight coefficients, and the heuristic function is fused to the DWA Optimize algorithms to achieve global path planning and dynamic obstacle avoidance.

Benefits of technology

It improves the smoothness and efficiency of path planning, realizes global optimal paths and real-time obstacle avoidance capabilities, and enhances the work safety and reliability of greenhouse inspection robots.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115079705B_ABST
    Figure CN115079705B_ABST
Patent Text Reader

Abstract

The present invention is specifically a path planning method for an inspection robot based on an improved A-star fusion DWA optimization algorithm, which solves the problems of poor smoothness, long planning time, many turning points, discontinuous path curvature and inability to achieve global optimization when the existing A-star algorithm and DWA algorithm are used alone. The method is implemented by the following steps: S1: initializing parameters and grid maps; S2: using the improved A-star algorithm to expand subnodes to form a preliminary global path; S3: obtaining the optimized global path through a greedy optimization method; S4: extracting local key nodes; S5: performing speed sampling and trajectory prediction based on kinematic models, speed constraints and local target points, and selecting the optimal predicted trajectory and speed space through an improved evaluation function; S6: extracting and outputting the optimal path. The present invention provides an efficient path planning algorithm that has both global path optimization and real-time obstacle avoidance capabilities, which improves path planning efficiency, obstacle avoidance sensitivity and safety.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to a path planning method for a robot, in particular to a path planning method for an inspection robot based on an improved A-star fusion DWA optimization algorithm. Background Art

[0002] The rapid development of various intelligent robots has an increasingly profound impact on human work and life. Among them, greenhouse technology has become more and more intelligent and efficient. In a greenhouse environment with high density of plants, high temperature, high humidity and even toxicity, it is not conducive to long-term manual work. Coupled with the impact of labor shortage and cost increase, intelligent equipment such as greenhouse robots have been derived and are being adopted more and more. Using greenhouse mobile inspection robots to complete autonomous inspections of greenhouse environments meets the needs of precision agriculture. Path planning is an extremely important part of the core functions of greenhouse inspection robots, which is both a hot spot and a difficult point.

[0003] In short, path planning is to plan a collision-free and safe path connecting the starting and ending points through a certain algorithm and evaluation method, under the premise of meeting optimization conditions such as planning distance, planning time, energy consumption and smoothness.

[0004] The A-star algorithm is a breadth-first heuristic path search algorithm. Although it can plan the global optimal path, the traditional A-star algorithm has limitations such as poor smoothness, long planning time, many turning points, and discontinuous path curvature. Unknown obstacles often appear on the planned path, and the global path planning cannot "recognize" unknown obstacles, which leads to safety accidents. As a result, it is only applicable to static known environments and cannot achieve real-time obstacle avoidance of unknown obstacles.

[0005] The DWA algorithm is a widely used local obstacle avoidance algorithm. However, although the original DWA algorithm can accurately "recognize" unknown obstacles and achieve local real-time obstacle avoidance, it also has shortcomings such as being easily trapped in local optimality and unable to achieve global optimality. Summary of the invention

[0006] In order to solve the problems that the existing A-star algorithm and DWA algorithm have poor smoothness, long planning time, many turning points, discontinuous path curvature and cannot achieve global optimization when used alone, the present invention provides an inspection robot path planning method based on an improved A-star fusion DWA optimization algorithm.

[0007] The present invention is achieved by adopting the following technical solutions:

[0008] A path planning method for an inspection robot based on an improved A-star fusion DWA optimization algorithm is implemented using the following steps:

[0009] S1: Initialize parameters and grid map;

[0010] S2: Set the starting point and end point of the path planning, add the starting point to the Open list as the parent node, and use the improved A-star algorithm to expand the child nodes to form a preliminary global path;

[0011] S3: The preliminary global path is subjected to redundant node elimination, key node retention, and smoothing through the greedy optimization method to obtain the optimized global path;

[0012] S4: extract the local key nodes of the optimized global path;

[0013] S5: The extracted local key nodes are used as local target points of the DWA optimization algorithm to guide local path planning and real-time obstacle avoidance. Speed ​​sampling and trajectory prediction are performed based on the kinematic model, speed constraints and local target points. The optimal predicted trajectory and speed space are selected through the improved evaluation function in the DWA optimization algorithm to achieve the fusion of the improved A-star algorithm and the DWA optimization algorithm.

[0014] S6: After reaching the end point, backtrack all nodes, extract and output the optimal path, and end the improved A-star fusion DWA optimization algorithm, completing the path planning and dynamic obstacle avoidance based on the algorithm.

[0015] Furthermore, the parameters of the improved A-star fusion DWA optimization algorithm include the kinematic parameters of the inspection robot and the improved DWA algorithm parameters; wherein the kinematic parameters of the inspection robot include the maximum speed (m / s), the maximum rotation speed (rad / s), the maximum acceleration (m / s 2 ), maximum rotational acceleration (rad / s 2 ), speed resolution (m / s), rotation speed resolution (rad / s), time resolution (s), robot initial direction angle (rad); improved DWA algorithm parameters include the azimuth function coefficient between the end point and the local target point, the distance function coefficient between the end point and the local target point, the distance function coefficient between the end point and the known static obstacle, the distance function coefficient between the end point and the unknown static obstacle, the distance function coefficient between the end point and the unknown dynamic obstacle, the current speed function coefficient, simulation time (s), obstacle conflict radius (m); each algorithm running cycle within the simulation time needs to reinitialize the grid map used by the algorithm.

[0016] Furthermore, step S2 is implemented by the following steps:

[0017] S2.1: Set the Open list to store the information of nodes that have not been visited; set the Close list to store the information of nodes that have been visited; during the calculation process, both the Open list and the Close list use a composite data structure of a minimum heap fused with a two-dimensional array to store and process the two-dimensional search node information of the improved A-star algorithm, which will effectively improve the search efficiency;

[0018] S2.2: First, set the starting point and end point of the path planning, add the starting point of the path planning to the Open list as the parent node, and the Close list is empty at this time;

[0019] S2.3: Then start the node search. After visiting the starting point, delete it from the Open list and add it to the Close list. Use the evaluation function of the improved A-star algorithm to select the best node in the search cycle as the next parent node, and continue searching and updating. In each search cycle, search the eight adjacent nodes of the current node, use the evaluation function of the improved A-star algorithm to select the best node, and continue searching for the target point.

[0020] Among them, the evaluation function of the improved A-star algorithm is:

[0021]

[0022] In the formula, S sc is the distance from the starting point to the current node, S st is the distance from the starting point to the end point, H(n) is the heuristic function, is the adaptive weight coefficient. When the length of the environment map is C and the width is K, At the beginning of the algorithm, the current point is far away from the end point, the heuristic function estimate is much smaller than the true value, and the search efficiency is low. By increasing the weight coefficient to increase the heuristic function value, the search speed is accelerated and the search efficiency is improved. When the current point is close to the end point, in order to avoid the algorithm being unable to find the best path, the heuristic function estimate should be close to the true value. It is necessary to reduce the weight coefficient to reduce the heuristic function estimate, slow down the search speed, and increase the search nodes. Although the search efficiency will be relatively reduced, the overall efficiency will still be improved compared to the original algorithm, and the best path can still be found.

[0023] S2.4: Before reaching the target point, continue to search the adjacent nodes of the starting point to determine whether the new node is in the Open list: if the new node is not in the Open list, add the node to the Open list, use the evaluation function of the improved A-star algorithm to calculate the cost value F of the node, set the node as the parent node of the current search point, and add it to the Close list; if the new node is in the Open list, use the evaluation function of the improved A-star algorithm to calculate the cost value F of the node, and compare it with the cost value F of the previous node, select the optimal node with the smallest cost value F as the parent node of the next search point, and add the node with the smallest cost value F to the Close list, then continue to search for nodes in the target direction, add the searched nodes to the Open list, and update the node information;

[0024] S2.5: Loop through steps S2.3-S2.4 to expand the child nodes until the target position is reached; when the target point is found, the target point is added to the Close list and the loop ends; if the Open list is empty at this time, it means there is no path node;

[0025] S2.6: Backtrack and extract the optimal node in the Close list to form a preliminary global path of the improved A-star algorithm.

[0026] In the original A-star algorithm, the detection node information is stored in a two-dimensional array of nodes, where the Open list stores the candidate nodes to be detected in the current search field, and finds the optimal node with the smallest F(n) value after evaluation by the evaluation function, and the Close list stores the optimal node found. The Open list requires frequent reading, deletion, search, and update of node information, while the Close list simply does not have the deletion operation, which makes the calculation pressure greater and time-consuming when processing the candidate node data.

[0027] The present invention selects a minimum heap fused with a two-dimensional array structure to store and process the A-star algorithm search node information. Its essence is to store the A-star algorithm two-dimensional search node in a data structure such as a minimum heap, wherein the minimum heap is a sorted complete binary tree, and the data value of any non-terminal node is not greater than the value of its left child node and right child node, then the top of the heap is the node element with the smallest cost value in the entire A-star algorithm two-dimensional search node. The present invention uses a two-dimensional array as a storage method of the minimum heap, which can quickly locate the parent node and the left and right child nodes, that is, the node element with the smallest cost value in the entire sequence can be found more quickly. The present invention encapsulates this composite data structure into a priority queue as a storage container for the Open list and the Close list.

[0028] This composite data structure of the minimum heap fused with a two-dimensional array will be able to find the search node with the minimum cost in the A-star algorithm more quickly. Specifically, the composite data structure of the present invention reduces the time complexity of the original Open list node information reading, deleting, searching and updating operations from O(N) to O(1). Even if the time complexity of constructing a complete binary tree to search for the minimum cost candidate node increases to O(logN), the overall time complexity is still reduced from O(N) to O(logN). At the same time, the Closelist node information reading, searching and updating operations also reduce the time complexity from O(N) to O(1) by virtue of the composite data structure.

[0029] In summary, the advantage of the composite data structure proposed in the present invention is that the time complexity of inserting and processing nodes in the Open list and the Close list is reduced to varying degrees, and reducing the time complexity means reducing the algorithm search time and improving the search efficiency.

[0030] The present invention adds an adaptive weight coefficient to improve the traditional A-star algorithm heuristic function, including:

[0031] The search efficiency of the A-star algorithm mainly depends on the heuristic function H(n). If the estimated cost value H(n) of the heuristic function is greater than the actual cost value H(n) of the path, t (n), then fewer nodes are searched, which will reduce the node evaluation time and improve the search efficiency, but the best path cannot be found; on the contrary, the estimated cost value H(n) of the heuristic function is less than the true cost value H of the path. t (n), the path search nodes and node evaluation time will increase, resulting in reduced path search efficiency, but the best path can be searched; if the two values ​​are equal, the efficiency is the highest.

[0032] The Euclidean distance calculation result used in the present invention is generally smaller than the true cost value, so a weight coefficient greater than 1 is required to keep the heuristic cost value as close to the true cost value as possible, but the weight coefficient value must be approximately equal to 1 when close to the target. Therefore, the present invention proposes an adaptive weight coefficient that gradually decreases as the algorithm runs. To improve the heuristic function.

[0033] Furthermore, step S3 is implemented by adopting the following steps, the purpose of which is to remove redundant nodes of each segment of the original path;

[0034] S3.1: Extract the preliminary global path obtained in step S2;

[0035] S3.2: Taking the starting point as the current point, extract the coordinates (x0, y0), (x1, y1) and (x2, y2) of the current point S0, child node S1 and grandchild node S2 in turn. The value range of the path node is also related to the map size, and its value range is:

[0036]

[0037] Where C is the length of the environment map; K is the width of the environment map;

[0038] S3.3: Calculate the vectors between the current point and the child node, and between the child node and the grandchild node coordinates And determine the angle θ1 between the two vectors:

[0039]

[0040]

[0041] S3.4: When the angle θ1 is 0, it means that the current point, the child node and the grandchild node are on the same straight line, and the redundant node S1 is deleted; when the angle θ1 is not 0, the straight line equation of the line connecting the current node and the grandchild node S0S2 is calculated, as shown in formula (5):

[0042]

[0043] S3.5: Determine whether there is one or more of all obstacle nodes in the path area between the current node and the grandchild node S0S2, traverse all obstacle nodes, and set the coordinates of the obstacle node being screened to (x obc ,y obc ), the value range is Determine whether inequality (6) holds;

[0044] [x obc ≥min(x0,x2)∧x obc ≤max(x0,x2)∧y obc ≥min(y0,y2)∧y obc ≤max(y0,y2)] (6)

[0045] S3.6: Set the safety distance D between the obstacle node position and the line S0S2 between the current node and the grandchild node. The range of D is According to the equation of the straight line y line =kx obc +b calculates the vertical distance d between the obstacle node position and the line S0S2 between the current node and the grandchild node:

[0046] d=||(y obc -y line )cos(arctan(k))|| (7);

[0047] S3.7: When the included angle θ1 is not 0, the inequality (6) holds, and the perpendicular distance d < D, it can be determined that the obstacle node is within the path area of the line segment S0S2 connecting the current node and the grandchild node. The child node needs to be retained, and the grandchild node is used as the current node to repeat steps S3.2 - S3.6; when the included angle θ1 is not 0 and the inequality (6) does not hold, or when the included angle θ1 is not 0, the inequality (6) holds and the perpendicular distance d ≥ D, it is proved that the obstacle node is not within the path area of the line segment S0S2 connecting the current node and the grandchild node. The child node S1 is deleted, and the line segment S0S2 connecting the current node and the grandchild node is used to replace the original broken line path. Then, the current node remains unchanged, the child node is modified to S2, and steps S3.2 - S3.6 are repeated to obtain the optimized path in the first stage;

[0048] S3.8: The secondary optimization eliminates the redundant nodes in the line segments of the optimized path in the first stage and retains the key nodes. The specific method is as follows: Extract the optimized path in the first stage and rearrange the path nodes. Taking the starting point S0(x0, y0) of the optimized path in the first stage as the current point, obtain all the node sets {g 1i |i = 0, 1…n} on the line segment S0G1 connecting the current point and the optimized path child node G1(x4, y4); Determine whether the obstacle node is within the path area of the line segment S0G1 connecting the optimized path child node G1(x4, y4) according to the methods in steps S3.5 - S3.7;

[0049] S3.9: When there are no obstacle nodes in the connection area between all the nodes g 1i on S0G1 and the starting point S0, then delete all the nodes g 1i on S0G1 and retain the key node G1; After retaining the key node G1, then taking G1(x4, y4) as the current node and G2(x5, y5) as the child node, all the node sets on the line segment G1G2 are {g 2i |i = 0, 1…n}, repeat steps S3.8 - S3.9 until the child node is the target node to complete the secondary optimization and obtain the optimized global path; When there are obstacle nodes in the connection area between all the nodes g 1i on S0G1 and the starting point S0, the path optimization fails, and the path in the first stage needs to be re - optimized.

[0050] The so - called greedy optimization is to not consider the overall optimality first, but to perform multiple repeated local optimizations on each path segment until there are no redundant nodes in each path segment, and finally only retain the key nodes of each path segment for the local guidance of the DWA optimization algorithm in the subsequent fusion algorithm.

[0051] Furthermore, step S5 is implemented by the following steps:

[0052] S5.1: Set unknown static and unknown dynamic obstacles on the grid map;

[0053] S5.2: Based on the kinematic model and speed constraints of the inspection robot, the inspection robot moves in the speed space set V a (v,ω) is sampled to simulate an infinite number of feasible motion trajectories within a certain time interval;

[0054] S5.3: Using the local key nodes retained in step S4 as the local target points of the DWA optimization algorithm, providing local direction guidance, using the evaluation function of the DWA optimization algorithm to evaluate the trajectory, and selecting the optimal trajectory from the infinite number of feasible motion trajectories generated in step S5.2;

[0055] Among them, the evaluation function of the DWA optimization algorithm is shown in formula (8):

[0056] P(v,w)=σ(α1·Gheading(v,w)+α2·Gdist(v,w)+β1·S_Kdist(v,w)+β2·S_Udist(v,w)+β s ·D_Udist(v,w)+γ·velocity(v,w)) (8)

[0057] In the formula, Gheading(v,w) represents the angle difference between the end point of the sampling trajectory and the local target direction, and Gdist(v,w) represents the distance between the end point of the sampling trajectory and the local target point. Together, they make the predicted trajectory close to the global path. In order to further improve the safety and obstacle avoidance sensitivity of the algorithm, the minimum distance between the end point of the sampling trajectory and the nearest obstacle in the original DWA algorithm is subdivided into three parts: S_Kdist(v,w), S_Udist(v,w) and D_Udist(v,w), which represent the minimum distances between the end point of the sampling trajectory and known static obstacles, unknown static obstacles and unknown dynamic obstacles, respectively. velocity(v,w) represents the current speed. α1, α2, β1, β2, β S , γ are the weight coefficients corresponding to each sub-function of the evaluation function; σ represents the normalization process; because one item in the evaluation function may be too prominent in the scoring, it is necessary to divide each item of the evaluation function by the sum of the corresponding items, that is, normalization;

[0058] According to the danger level of known static obstacles, unknown static obstacles and unknown dynamic obstacles to the inspection robot, the weight coefficients of each item corresponding to the minimum distance between the end point of the sampling trajectory and the three obstacles satisfy β1<β2<β S ; In addition, dynamic obstacles usually appear randomly for a short time. If β SIf it is set to a constant β3 (0<β3<β2<β1<1), it will occupy a larger weight most of the time, affecting the efficiency of the algorithm, so the adaptive weight coefficient β is proposed s , set the collision radius D between the inspection robot and obstacles s , D s If it is greater than the robot turning radius R, then the collision radius is set to R. <D s <2R, when the distance between the robot and the dynamic obstacle is less than or equal to the robot's turning radius R, a collision is very likely to occur. In order to maintain safety, β s must be infinite, then β s The value can be expressed as:

[0059]

[0060] The optimized DWA algorithm enables the inspection robot to have better obstacle avoidance sensitivity while maintaining operational safety.

[0061] S5.4: retain the speed space corresponding to the optimal trajectory and perform speed smoothing;

[0062] S5.5: Sending a speed command to the chassis controller of the inspection robot, and the inspection robot moves along the optimal trajectory obtained in step S5.3;

[0063] S5.6: Determine whether the inspection robot has moved to the end point. If it has, the process ends. If it has not, repeat steps S5.2 to S5.4.

[0064] The present invention extracts the local key points of the improved A-star algorithm to achieve direction guidance, integrates the DWA optimization algorithm for path planning, and combines the laser radar to detect the surrounding real-time environment to achieve real-time dynamic obstacle avoidance. The DWA optimization algorithm performs local path planning between every two adjacent local key points, and the optimized DWA evaluation function makes the local planning as close to the global optimal path as possible, and has the ability to automatically switch to the next local target point to be guided by it when the end of the predicted trajectory is separated from the nearest local target point by a conflict radius.

[0065] Furthermore, the kinematic model in step S5.2 assumes that the inspection robot is t The kinematic model of uniform linear motion is as follows:

[0066]

[0067] Furthermore, in step S5.2, the velocity space set V a(ν,ω) is the speed space after speed constraint. Since the speed space of the inspection robot in the forward simulation time obtained according to the kinematic model is an ideal speed space without considering its own and environmental factors, in actual situations, it is necessary to consider the impact of constraints on speed sampling and constrain the speed space within a certain range. Speed ​​sampling will be subject to the following constraints:

[0068] a. Constrained by the robot body, the velocity space set V is obtained b :

[0069] V b ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]} (11)

[0070] In the formula, v min ,v max is the minimum and maximum value of the linear velocity, ω min ,ω max are the minimum and maximum values ​​of angular velocity;

[0071] b. Subject to the safety distance constraint, the velocity space set V is obtained d :

[0072]

[0073] Where Dist(v,ω) represents the minimum distance between the end of the predicted trajectory and the obstacle. Indicates the maximum linear and angular deceleration;

[0074] c. Subject to motor performance constraints, the speed space set V is obtained c :

[0075]

[0076] In the formula, v e ,ω e is the current linear and angular velocity of the robot, is the maximum linear and angular acceleration, is the maximum linear and angular deceleration;

[0077] The final feasible speed space set is the intersection of the three V a :

[0078] V a =V b ∩V d ∩V c (14).

[0079] The present invention has the following beneficial effects:

[0080] (1) The present invention provides a patrol robot path planning method based on an improved A-star fusion DWA optimization algorithm, which solves the problems of poor smoothness, long planning time, multiple turning points, discontinuous path curvature and inability to achieve global optimization when the existing A-star algorithm and DWA algorithm are used alone.

[0081] (2) Regarding the improvement of the traditional A-star algorithm, first, the present invention adopts a composite data structure to process nodes, reduces the algorithm time complexity, and reduces the path planning time; then, an adaptive weight coefficient is added to the heuristic function of the traditional A-star algorithm evaluation function to improve the path search efficiency; finally, a greedy optimization strategy is proposed to eliminate redundant nodes and extract key nodes, shorten the path length, and improve the path planning efficiency and path smoothness.

[0082] (3) Optimize the sub-functions of the trajectory sampling evaluation function of the original DWA algorithm to help it approach the global optimal path of the improved A-star algorithm, subdivide the obstacle distance sub-function, and improve the local path search efficiency and obstacle avoidance sensitivity.

[0083] (4) The A-star algorithm and the DWA algorithm each have limitations when used alone. The present invention combines the traditional A-star algorithm and the DWA algorithm to achieve global optimization and dynamic obstacle avoidance. However, the algorithm's path planning efficiency and obstacle avoidance sensitivity are still insufficient. The present invention provides an efficient path planning algorithm that has both global path optimization and real-time obstacle avoidance capabilities. It uses the local key target points extracted from the improved A-star algorithm to achieve local direction guidance, and combines the DWA optimization algorithm for local path planning to achieve dynamic obstacle avoidance. It not only overcomes the limitations of the two algorithms, but also improves the path planning efficiency, obstacle avoidance sensitivity and safety. At the same time, it improves the work efficiency and safety reliability of the greenhouse inspection robot in the greenhouse environment.

[0084] (5) A simulation comparison experiment based on Matlab verified the effectiveness and superiority of the improved A-star algorithm and the fusion path planning algorithm proposed in the present invention, and applied them to a self-built greenhouse inspection test vehicle. The experiment proved its feasibility in the actual path planning scenario of the simulated greenhouse. The fusion path planning algorithm also has global optimality and real-time obstacle avoidance capabilities. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] Figure 1 It is an example diagram of the path of the original A-star algorithm improved by the greedy optimization strategy in the present invention;

[0086] Figure 2 It is a schematic diagram of the flow of the fusion path planning algorithm of the present invention that integrates the improved A-star and the optimized DWA algorithm;

[0087] Figure 3 a), Figure 3 b) Figure 3 c) Figure 3 d) are comparative simulation results of global path planning in a 30*30 grid environment using an ant colony algorithm based on safety considerations, an ant colony algorithm with accelerated convergence, a traditional A-star algorithm, and an improved A-star algorithm of the present invention;

[0088] Figure 4 a), Figure 4 b) Figure 4 c) Figure 4 d) are comparative simulation results of global path planning in a 50*50 grid environment using an ant colony algorithm based on safety considerations, an ant colony algorithm with accelerated convergence, a traditional A-star algorithm, and an improved A-star algorithm of the present invention;

[0089] Figure 5 a), Figure 5 b) Figure 5 c) Figure 6 d) are comparative simulation results of the original DWA algorithm, the ant colony fusion DWA algorithm based on security considerations, the traditional A-star fusion DWA algorithm and the improved A-star fusion DWA optimization algorithm of the present invention in a 30*30 grid environment;

[0090] Figure 6 a), Figure 6 b) Figure 6 c) Figure 6 d) are comparative simulation results of the original DWA algorithm, the ant colony fusion DWA algorithm based on security considerations, the traditional A-star fusion DWA algorithm and the improved A-star fusion DWA optimization algorithm of the present invention in a 50*50 grid environment;

[0091] Figure 7 a), Figure 7 b) Figure 7 c) Figure 7 d) are real-time linear velocity diagrams of the comparative simulation experiments of the original DWA algorithm, the ant colony fusion DWA algorithm based on safety considerations, the traditional A-star fusion DWA algorithm and the improved A-star fusion DWA optimization algorithm of the present invention in a 30*30 grid environment;

[0092] Figure 8 a), Figure 8 b) Figure 8 c) Figure 8 d) are real-time linear velocity diagrams of the comparative simulation experiments of the original DWA algorithm, the ant colony fusion DWA algorithm based on safety considerations, the traditional A-star fusion DWA algorithm and the improved A-star fusion DWA optimization algorithm of the present invention in a 50*50 grid environment;

[0093] Fig. 9It is a simulated greenhouse real scene verification site for improving the A-star algorithm and the fusion path planning algorithm in the present invention and a corresponding two-dimensional grid map constructed;

[0094] Fig.10 It is the starting and ending point positions of the physical verification of the algorithm proposed in the present invention and the global optimal path diagram of the improved A-star algorithm planning;

[0095] Fig.11 ①-⑤ are the specific process diagrams of the physical verification of the improved A-star algorithm in the present invention;

[0096] Fig.12 ①-⑤ are specific process diagrams of the physical verification of the improved A-star fusion DWA optimization algorithm in the present invention. DETAILED DESCRIPTION

[0097] In order to make the above-mentioned objectives, contents and advantages of the present invention more clearly understood, the following specifically cites algorithm improvement examples, simulation experiments and real-life scene verification, and provides detailed explanations in conjunction with the attached drawings.

[0098] The technical solution of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0099] Example 1

[0100] As attached Figure 1 As shown, a greedy optimization strategy in the present invention improves the traditional A-star algorithm path example, the grid map is composed of a square unit grid with a side length of 1, and its main steps are as follows:

[0101] Step 1: Eliminate redundant nodes from each segment of the original path.

[0102] Step 1.1: Extract the preliminary global path of the improved A-star algorithm after optimizing the efficiency in the present invention, that is, Figure 1 The black dotted line in the figure has the original path nodes (T1, T2, T3, G1, G2, T4, T5). Taking the starting point as the current point, the coordinates of the current point S, child node T1 and grandchild node T2 are extracted in turn, which are (0.5, 0.5), (0.5, 1.5) and (1.5, 2.5) respectively.

[0103] Step 1.2: Calculate the vector And calculate the angle θ1 between the two vectors:

[0104]

[0105]

[0106] Step 1.3: If the included angle θ1 is not 0, then first calculate the linear equation of the node connection line ST2:

[0107]

[0108] Step 1.4: Then determine whether one or more of all obstacles are within the path area of ST2. Traverse all obstacle nodes. Let the coordinates of the obstacle node being screened be (x obc , y obc ), where Determine whether the following inequality holds:

[0109] [x obc ≥0.5 & x obc ≤1.5 & y obc ≥0.5 & y obc ≤2.5] (18)

[0110] Step 1.5: Since the side length of the grid cell is 1m, a safety distance D = 2m needs to be set between the obstacle node position and the node connection line ST2. Therefore, it is also necessary to calculate the perpendicular distance d between the obstacle node position and the node connection line ST2 according to the linear equation y line = 2x obc - 0.5:

[0111]

[0112] Step 1.6: When the included angle θ1 is not 0, if the inequality (18) holds and the perpendicular distance d < D, it can be determined that the obstacle node is within the path area of the node connection line ST2. It is necessary to retain the node T1, and use T2(1.5, 2.5) as the current node, T3(2.5, 3.5) as the child node, and G1(3.5, 4.5) as the grandchild node. Repeat the above steps until the final target point is the grandchild node;

[0113] Step 1.7: If the inequality (18) does not hold or the inequality (18) holds and the perpendicular distance d ≥ D, it proves that the obstacle node is not within the path area of the node connection line ST2. Delete the redundant node T1, replace the original broken line path ST1T2 with the node connection line ST2, then keep the current node unchanged, modify the child node to T2(1.5, 2.5), and the grandchild node to T3(2.5, 3.5). Repeat the above steps until the final target point is the grandchild node.

[0114] Step 1.8: Finally, after obtaining the attached Figure 1 In the example, the blue line segment is the optimized path in the first stage. This path reduces the redundant turning points and improves the path smoothness.

[0115] Step 2: Secondary optimization removes redundant nodes of the above optimization path segments and retains key nodes;

[0116] Step 2.1: Extract the first stage optimization path and rearrange the path nodes, with S(0.5, 0.5) as the current point, G1(3.5, 4.5) as the child node, and the set of all nodes on the line segment SG1 as {g 1i |i=0,1…n}, according to the method of proving whether the obstacle node is within the node connection path area in the first stage optimization above:

[0117] Step 2.2, if all nodes g on this line segment 1i If there are no obstacle nodes in the area connected to the starting point S, then all nodes g on SG1 can be deleted. 1i , retain the key node G1;

[0118] Step 2.3, if there is an obstacle node, the path optimization fails and the path is optimized again. Otherwise, G1(3.5, 4.5) is used as the current node and G2(4.5, 5.5) as the child node. The set of all nodes on the line segment G1G2 is {g 2i |i=0,1…n}, repeat the above steps until the child node is the target node, completing the secondary optimization.

[0119] In the example in this article, after two phases of path optimization, the key nodes G1 (3.5, 4.5) and G2 (4.5, 5.5) are extracted, completing the greedy optimization of the initial global path.

[0120] Example 2

[0121] The improved A-star algorithm in the present invention plans a global optimal path including the starting and ending points and local target points, and cannot avoid unknown obstacles; although the DWA algorithm can achieve local obstacle avoidance, it always takes the final target as the direction guidance, which is prone to "detour" behavior. Therefore, on the basis of extracting the local key points of the improved A-star algorithm to achieve direction guidance, the present invention integrates the DWA optimization algorithm for path planning and combines the laser radar to detect the surrounding real-time environment to achieve dynamic obstacle avoidance.

[0122] The DWA optimization algorithm performs local path planning between every two adjacent local key points, and the optimized DWA evaluation function makes the local planning as close to the global optimal path as possible, and has the ability to automatically switch to the next local target point to be guided by it when the end of the predicted trajectory is separated from the nearest local target point by a conflict radius. The specific process is shown in the attached figure. Figure 2 shown.

[0123] Steps S2-S4 in the present invention can be summarized as follows: using the method of improving the A-star algorithm evaluation function and optimizing the node array data structure to expand the nodes and eliminating redundant nodes through a greedy optimization strategy to obtain a global optimal path and extract necessary key nodes.

[0124] Example 3

[0125] In order to verify the effectiveness of the improved A-star algorithm and fusion path planning algorithm proposed in this invention, simulation experiments were carried out using Matlab. The simulation environment is a grid map simulating a complex greenhouse environment, where white grids, black grids, gray grids, and yellow grids represent idle channels, known static obstacles, unknown static obstacles, and unknown dynamic obstacles, respectively, and the blue △ in the lower left corner and the green ○ in the upper right corner represent the starting point and end point of the path planning, respectively.

[0126] According to the complexity of the working environment of the greenhouse inspection robot, two grid maps of different sizes, 30*30 and 50*50, were designed, in which the obstacle rates were 0.130 and 0.150 respectively. Simulation comparison experiments of the improved A-star algorithm and the improved A-star fusion DWA optimization algorithm of the present invention were carried out respectively.

[0127] The improved A-star algorithm in the present invention is compared with the ant colony algorithm based on safety considerations, the ant colony algorithm with accelerated convergence, and the traditional A-star algorithm in two groups of simulation experiments in two grid environments. The results are shown in the attached figure. Figure 3 , Attachment Figure 4 shown.

[0128] Attached Figure 3 , Attachment Figure 4 They are simulation result diagrams under 30*30 and 50*50 grid environments, respectively. (a), (b), (c), and (d) represent the path planning simulation results of the ant colony algorithm based on safety considerations, the ant colony algorithm with accelerated convergence, the traditional A-star algorithm, and the improved A-star algorithm of the present invention, respectively. The red line is the global path planned by each algorithm. It can be seen from the figure that the above four algorithms can all plan a complete global path, but the path performance is different. The comparison of simulation experiment parameters is shown in Table 1.

[0129] Table 1 Comparison of global path planning experimental parameters of each algorithm under two grid maps

[0130]

[0131] It can be seen from Table 1 that since the improved A-star algorithm not only eliminates redundant nodes and shortens the path length through the greedy optimization strategy, but also improves the path search efficiency by improving the evaluation function and optimizing the node array, the path length, planning time, number of turning points and number of key nodes of the improved A-star algorithm of the present invention are reduced to varying degrees compared with other comparative algorithms. In addition, due to the significant reduction in turning points, the turning angle of the path turning point of the improved A-star algorithm of the present invention is also reduced, indicating that the smoothness of the path is further improved. Although the path of the improved A-star algorithm of the present invention is closer to the obstacle, the robot does not move completely according to this global optimal path, but moves according to the path planned by the fusion path planning algorithm.

[0132] Based on the global optimal path and local target point of the improved A-star algorithm, a simulation comparison experiment of the fusion path planning algorithm of the present invention is carried out to verify the path planning and real-time obstacle avoidance capabilities of the fusion path planning algorithm. The map environment still uses the above two grid maps, and the start and end points of the fusion algorithm simulation remain unchanged. In addition, 3 and 5 unknown static obstacles and 1 dynamic obstacle are set respectively, where the dynamic obstacle running trajectory is a black dotted line trajectory and crosses the fusion path planning algorithm trajectory. The present invention sets the obstacle conflict radius D s =0.6m, keeping a safe distance between the path and various types of obstacles. The robot kinematics and DWA optimization algorithm parameters in the fusion algorithm simulation are shown in Tables 2 and 3.

[0133] Table 2: Robot kinematic parameters

[0134]

[0135] Table 3: Optimized DWA algorithm parameters

[0136]

[0137] The simulation results of the fusion path planning algorithm comparison under two grid environments are shown in the attached figure. Figure 5 , Attachment Figure 6 shown.

[0138] Attached Figure 5 , Attachment Figure 6 (a), (b), (c), and (d) represent the simulation results of the original DWA algorithm, the ant colony fusion DWA algorithm based on safety considerations, the traditional A-star fusion DWA algorithm, and the improved A-star fusion DWA optimization algorithm in this paper under two grid environments. The blue solid line in the figure is the robot trajectory planned by each algorithm, the black dotted line is the corresponding global guidance path, and the red * on it is the local target point.

[0139] Since the above four comparison algorithms all include the DWA algorithm, which makes them all have dynamic obstacle avoidance capabilities, the original DWA algorithm and the three fusion path planning algorithms can all plan a complete path and achieve dynamic obstacle avoidance. First, compared with the ant colony fusion DWA algorithm based on safety considerations and the traditional A-star fusion DWA algorithm, after the improved A-star fusion DWA optimization algorithm of the present invention eliminates redundant nodes, the number of local target points is greatly reduced compared with the traditional A-star algorithm and the ant colony algorithm based on safety considerations, so that the improved fusion algorithm of the present invention does not need to follow the guidance of unnecessary local target points, greatly reducing the planning time, thereby improving the planning efficiency. Secondly, it can be seen from the simulation results that the total turning angle of the fusion algorithm proposed in the present invention is obviously smaller, and the trajectory is more suitable for the movement of greenhouse inspection robots in complex greenhouse environments.

[0140] Attached Figure 7 , Attachment Figure 8 Yes Figure 5 , Attachment Figure 6 From the linear speed diagram of the corresponding algorithm path planning simulation, it can be seen that the speed fluctuation of the fusion algorithm proposed in the present invention is weak, and it maintains a relatively high speed for a long time. However, due to the real-time obstacle avoidance, a small speed fluctuation will occur, and the number of control nodes of the fusion algorithm of the present invention is less than that of other algorithms. Therefore, from the linear speed diagram, it can be seen macroscopically that the planning efficiency of the fusion algorithm of the present invention is high, the running speed is relatively stable, and it is suitable for the greenhouse inspection robot to work in the greenhouse environment. The comparison of the experimental parameters of the simulation path of each algorithm is shown in Table 4.

[0141] Table 4: Comparison of path planning simulation experiment parameters between original DWA and three fusion algorithms in two grid map environments

[0142]

[0143] Table 4 shows that under two grid map sizes, the path length and planning time of the improved A-star fusion DWA optimization algorithm of the present invention are optimized compared with the other three algorithms, because the fusion algorithm of the present invention strictly follows the global optimal path of the improved A-star algorithm and the local target points after eliminating redundant nodes to search for trajectories, and selects the optimal predicted trajectory through the improved evaluation function of the DWA algorithm. Under the premise of ensuring path safety and smoothness, the path planning efficiency of the improved A-star fusion DWA optimization algorithm of the present invention is higher and more practical.

[0144] Example 4

[0145] In order to verify the effectiveness, safety and reliability of the improved algorithm proposed in the present invention in the real environment, and due to the particularity of the greenhouse environment, the present invention simplifies the environmental conditions for the physical scene verification, using gray cartons to replace the vegetation in the greenhouse, red cartons (darker cartons) for unknown vegetation, and pedestrians as workers who often appear in the greenhouse environment are dynamic obstacles. The improved A-star algorithm and the improved A-star fusion DWA optimization algorithm of the present invention are applied to the self-built greenhouse inspection test car, and the physical scene verification is carried out in the simulated greenhouse environment scene built in the laboratory.

[0146] The test vehicle, the physical scene verification site and the constructed two-dimensional grid map are shown in the attached Fig. 9 ,The black areas of the grid map are static obstacles, and the white areas are channels.

[0147] Set the starting and ending points to start the physical scene verification of the improved A-star algorithm in this paper. The experimental starting point and the global path planned by the improved A-star algorithm are shown in the attached figure. Fig.10 shown.

[0148] Attached Fig.10 The light blue area around the obstacle is the obstacle expansion area to prevent the car from colliding, and the purple area near the car model is the local cost map, which is used to perceive the surrounding environment in real time to help the car avoid obstacles dynamically. Obviously, the global optimal path of the improved A-star algorithm in this paper can perfectly avoid static obstacles, with basically no redundant turning points, which is conducive to the movement of the car.

[0149] Attached Fig.11 Figures ①-⑤ show the specific process of the car strictly following the global optimal path of the improved A-star algorithm in this paper, achieving autonomous navigation and avoidance of static known obstacles. The car moves smoothly during the experiment. Since the improved A-star algorithm of this invention cannot avoid unknown obstacles, it is only applicable to known environments.

[0150] The experimental environment of the improved A-star fusion DWA optimization algorithm of the present invention is consistent with that of the improved A-star algorithm. The written fusion path planning algorithm navigation function package is also started, the same start and end points are set, and unknown static and dynamic obstacles are added to verify the path planning and real-time obstacle avoidance capabilities of the fusion algorithm. The results are shown in the attached figure. Fig.12 As shown, the fusion algorithm of the present invention strictly follows the guidance of the local target points in the global optimal path planned by the improved A-star algorithm, completes the path planning of the greenhouse inspection test vehicle in a simulated simplified greenhouse environment, and has dynamic obstacle avoidance capabilities.

[0151] Attached Fig.12The green line in ① is the global path of the improved A-star algorithm. The red line in front of the car model in the red circle is the optimal predicted trajectory after speed sampling by the DWA optimization algorithm of the present invention. The car moves strictly according to this trajectory guided by the global optimal path. When the laser radar scans an unknown obstacle, it will be presented in the local cost map. Then, the DWA optimization algorithm obtains the predicted trajectory and issues the speed command. The car starts to avoid the unknown static obstacle 1 (red box, i.e., the darker cardboard box) as shown in the attached figure. Fig.12 ②, after the end, the guidance returns to the global path. At this time, the pedestrian walks into the experimental environment as an unknown dynamic obstacle and happens to pass through the global path, as shown in the attached figure. Fig.12 As shown in ③ and ④, after the fusion algorithm of the present invention, the car perfectly avoids pedestrians, then senses the unknown static obstacle 2 and starts to avoid it as shown in Figure 12⑤, and finally successfully avoids three unknown obstacles, moves smoothly without pause, and follows the guidance of the global optimal path to reach the end point, verifying its feasibility in the simulated simplified greenhouse environment. The fusion path planning algorithm of the present invention not only achieves global optimization and real-time obstacle avoidance, but also improves the work efficiency and safety of the greenhouse inspection robot.

[0152] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A path planning method for an inspection robot based on an improved A-star fusion DWA optimization algorithm, characterized in that: This method is implemented by the following steps: S1: Initialize parameters and grid map; S2: Set the starting point and end point of the path planning, add the starting point to the Openlist as the parent node, and use the improved A-star algorithm to expand the child nodes to form a preliminary global path; S3: The preliminary global path is subjected to redundant node elimination, key node retention, and smoothing through the greedy optimization method to obtain the optimized global path; S4: extract the local key nodes of the optimized global path; S5: The extracted local key nodes are used as local target points of the DWA optimization algorithm to guide local path planning and real-time obstacle avoidance. Speed ​​sampling and trajectory prediction are performed based on the kinematic model, speed constraints and local target points. The optimal predicted trajectory and speed space are selected through the improved evaluation function in the DWA optimization algorithm to achieve the fusion of the improved A-star algorithm and the DWA optimization algorithm. S6: After reaching the end point, backtrack all nodes, extract and output the optimal path, improve the A-star fusion DWA optimization algorithm, and complete the path planning and dynamic obstacle avoidance based on the algorithm; Step S2 is implemented by the following steps: S2.1: Set the Openlist to store the information of nodes that have not been visited; set the Close list to store the information of nodes that have been visited; during the calculation process, both the Openlist and the Close list use a composite data structure of a minimum heap fused with a two-dimensional array to store and process the two-dimensional search node information of the improved A-star algorithm; S2.2: First, set the starting point and end point of the path planning, add the starting point of the path planning to the Open list as the parent node, and the Close list is empty at this time; S2.3: Then start the node search. After visiting the starting point, delete it from the Open list and add it to the Closelist. Use the evaluation function of the improved A-star algorithm to select the best node in the search cycle as the next parent node, and continue searching and updating. In each search cycle, search multiple adjacent nodes of the current node, use the evaluation function of the improved A-star algorithm to select the best node, and continue searching for the target point. Among them, the evaluation function of the improved A-star algorithm is: In the formula, G(n) is the actual cost from the search starting point to the current point, S sc is the distance from the starting point to the current node, S st is the distance from the starting point to the end point, H(n) is the heuristic function, is the adaptive weight coefficient. When the length of the environment map is C and the width is K, S2.4: Before reaching the target point, continue searching the adjacent nodes of the starting point to determine whether the new node is in the Openlist: If the new node is not in the Open list, add the node to the Open list, use the evaluation function of the improved A-star algorithm to calculate the cost value F of the node, set the node as the parent node of the current search point, and add it to the Closelist; If the new node is in the Openlist, use the evaluation function of the improved A-star algorithm to calculate the cost value F of the node, and compare it with the cost value F of the previous node, select the optimal node with the smallest cost value F as the parent node of the next search point, and add the node with the smallest cost value F to the Close list, then continue searching for nodes in the target direction, add the searched nodes to the Open list, and update the node information; S2.5: Loop and execute steps S2.3 - S2.4 to expand child nodes until the target position is reached; when the target point is searched, add the target point to the Close list and end the loop; S2.6: Backtrack to extract the optimal node in the Close list to form a preliminary global path; Step S3 is implemented by the following steps: S3.1: Extract the preliminary global path obtained in step S2; S3.2: Take the starting point as the current point, and successively extract the coordinates (x0, y0), (x1, y1), and (x2, y2) of the current point S0, child node S1, and grandchild node S2. Their value ranges are: In the formula, C is the length of the environmental map; K is the width of the environmental map; S3.3: Calculate the vectors between the current point and the child node, and between the child node and the grandchild node coordinates And determine the angle θ1 between the two vectors: S3.4: When the included angle θ1 is 0, it means that the current point, child node, and grandchild node are on the same straight line, then delete the redundant node S1; when the included angle θ1 is not 0, then calculate the straight-line equation of the connection line S0S2 between the current node and the grandchild node, as shown in formula (5): S3.5: Determine whether there is one or more of all obstacle nodes in the path area between the current node and the grandchild node S0S2, traverse all obstacle nodes, and set the coordinates of the obstacle node being screened to (x obc ,y obc ), the value range is 0 <x obc , Determine whether inequality (6) holds; [x obc ≥min(x0,x2)∧x obc ≤max(x0,x2)∧y obc ≥min(y0,y2)∧y obc ≤max(y0,y2)](6) S3.6: Set the safety distance D between the obstacle node position and the line S0S2 between the current node and the grandchild node. The range of D is According to the equation of the straight line y line =kx obc +b calculates the vertical distance d between the obstacle node position and the line S0S2 between the current node and the grandchild node: d=∥(and obc -and line )cos (arctan(k))∥ (7); S3.7: Traverse all obstacle nodes. When the included angle θ1 is not 0, the inequality (6) holds, and the vertical distance d < D, it can be judged that the obstacle node is within the path area of the connection line S0S2 between the current node and the grandchild node, and the child node needs to be retained. Then, take the grandchild node as the current node and repeat steps S3.2 - S3.6; when the included angle θ1 is not 0 and the inequality (6) does not hold, or when the included angle θ1 is not 0, the inequality (6) holds and the vertical distance d ≥ D, it proves that the obstacle node is not within the path area of the connection line S0S2 between the current node and the grandchild node. Delete the child node S1, replace the original broken-line path with the connection line S0S2 between the current node and the grandchild node, then keep the current node unchanged, modify the child node to S2, and repeat steps S3.2 - S3.6 to obtain the first-stage optimized path; S3.8: Extract the first-stage optimization path and rearrange the path nodes. Take the starting point S0 (x0, y0) of the first-stage optimization path as the current point, and obtain the set of all nodes on the line segment S0G1 connecting the current point and the optimization path child node G1 (x4, y4) {g 1i |i=0,1…n}; Determine whether the obstacle node is within the path area of ​​the line segment S0G1 of the optimized path subnode G1(x4,y4) according to the method of steps S3.5-S3.7; S3.9: When all nodes g on S0G1 1i If there are no obstacle nodes in the area connected to the starting point S0, delete all nodes g on S0G1. 1i , retain the key node G1; after retaining the key node G1, G1(x4,y4) is taken as the current node, G2(x5,y5) is taken as the child node, and the set of all nodes on the line segment G1G2 is {g 2i |i=0,1…n}, repeat steps S3.8-S3.9 until the child node is the target node, complete the secondary optimization, and obtain the optimized global path; when all nodes g on S0G1 1i If there is an obstacle node in the area connected to the starting point S0, the path optimization fails, and the first stage path is optimized again; Step S5 is implemented by the following steps: S5.1: Set unknown static and unknown dynamic obstacles on the grid map; S5.2: Based on the kinematic model and speed constraints of the inspection robot, the inspection robot moves in the speed space set V a (v,ω) is sampled to simulate an infinite number of feasible motion trajectories within a certain time interval; S5.3: Take the local key nodes retained in step S4 as the local target points of the DWA optimization algorithm to provide local direction guidance, evaluate the trajectory using the evaluation function of the DWA optimization algorithm, and select the optimal trajectory from the infinite number of feasible motion trajectories generated in step S5.2; Among them, the evaluation function of the DWA optimization algorithm is as shown in formula (8): P(v,w)=σ(α1·AGheading(v,w)+α2·Gdist(v,w)+β1·S_Kdist(v,w)+β2·S_Udist(v,w)+β S ·D_Udist(v,w)+γ·velocity(v,w)) (8) In the formula, Gheading(v,w) represents the angle difference between the end point of the sampling trajectory and the local target direction, and Gdist(v,w) represents the distance between the end point of the sampling trajectory and the local target point. Together, they make the predicted trajectory close to the global path. In order to further improve the safety and obstacle avoidance sensitivity of the algorithm, the minimum distance between the end point of the sampling trajectory and the nearest obstacle in the original DWA algorithm is subdivided into three parts: S_Kdist(v,w), S_Udist(v,w) and D_Udist(v,w), which represent the minimum distances between the end point of the sampling trajectory and known static obstacles, unknown static obstacles and unknown dynamic obstacles, respectively. velocity(v,w) represents the current speed. α1, α2, β1, β2, β S , γ are the weight coefficients corresponding to each sub-function of the evaluation function; σ represents the normalization process; According to the danger level of known static obstacles, unknown static obstacles and unknown dynamic obstacles to the inspection robot, the weight coefficients of each item corresponding to the minimum distance between the end point of the sampling trajectory and the three obstacles satisfy β1<β2<β S ; Propose adaptive weight coefficient β s , set the collision radius D between the inspection robot and obstacles s , D s If it is greater than the robot turning radius R, then the collision radius is set to R. <D s <2R, when the distance between the robot and the dynamic obstacle is less than or equal to the robot's turning radius R, a collision is very likely to occur. In order to maintain safety, β s must be infinite, then β s The value is expressed as: S5.4: Retain the speed space corresponding to the optimal trajectory and perform speed smoothing; S5.5: Send the speed command to the chassis controller of the inspection robot, and the inspection robot moves along the optimal trajectory obtained in step S5.3; S5.6: Judge whether the inspection robot has moved to the end point. If it has moved to the end point, end. If it is not the end point, repeat steps S5.2 to S5.

4.

2. The inspection robot path planning method based on the improved A-star fusion DWA optimization algorithm according to claim 1 is characterized in that: The parameters of the improved A-star fusion DWA optimization algorithm include the kinematic parameters of the inspection robot and the improved DWA algorithm parameters; among them, the kinematic parameters of the inspection robot include the maximum speed, the maximum rotation speed, the maximum acceleration, the maximum rotation acceleration, the speed resolution, the rotation speed resolution, the time resolution, and the initial direction angle of the robot; the improved DWA algorithm parameters include the azimuth function coefficient between the end point and the local target point, the distance function coefficient between the end point and the local target point, the distance function coefficient between the end point and the known static obstacle, the distance function coefficient between the end point and the unknown static obstacle, the distance function coefficient between the end point and the unknown dynamic obstacle, the current speed function coefficient, the simulation time, and the obstacle conflict radius; each algorithm running cycle within the simulation time needs to reinitialize the grid map used by the algorithm.

3. The inspection robot path planning method based on the improved A-star fusion DWA optimization algorithm according to claim 1 is characterized in that: The kinematic model in step S5.2 assumes that the inspection robot is in a certain period of time Δ t The kinematic model of uniform linear motion is as follows: Where, X t and Y t is the horizontal and vertical (X, Y) coordinates of the robot in the two-dimensional world coordinate system at time t, θ t is the heading angle of the inspection robot at time t, ν(t) is the speed of the inspection robot at time t, and ω(t) is the angular velocity of the inspection robot at time t.

4. The inspection robot path planning method based on the improved A-star fusion DWA optimization algorithm according to claim 1 is characterized in that: The velocity space set V in step S5.2 a (ν,ω) is the speed space after speed constraint. Since the speed space of the inspection robot in the forward simulation time obtained according to the kinematic model is an ideal speed space without considering its own and environmental factors, in actual situations, it is necessary to consider the impact of constraints on speed sampling and constrain the speed space within a certain range. Speed ​​sampling will be subject to the following constraints: a. Constrained by the robot body, the velocity space set V is obtained b : V b ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]} (11) In the formula, v min ,v max is the minimum and maximum value of the linear velocity, ω min ,ω max are the minimum and maximum values ​​of angular velocity; b. Subject to the safety distance constraint, the velocity space set V is obtained d : Where Dist(v,ω) represents the minimum distance between the end of the predicted trajectory and the obstacle. Indicates the maximum linear and angular deceleration; c. Subject to motor performance constraints, the speed space set V is obtained c : In the formula, v e ,ω e is the current linear and angular velocity of the robot, is the maximum linear and angular acceleration, is the maximum linear and angular deceleration; The final feasible speed space set is the intersection of the three V a : V a =V b ∩V d ∩V c (14)。