Path planning method based on improved Hybrid A* algorithm and DWA algorithm

Abstract

The invention discloses a path planning method based on an improved Hybrid A * algorithm and a DWA algorithm. The path planning method specifically comprises the following steps: establishing a grid map for a working environment and setting task starting and ending points; improving a heuristic function in the A * algorithm; the cost function of the Hybrid A * algorithm is improved, the path cost in the improved A * algorithm is used as the complete heuristic constraint cost with obstacles, the extension distance of the Dubins curve is used as the incomplete constraint heuristic cost without obstacles, and a variable step size search strategy based on the obstacle distance is adopted; smoothing the path searched by the global path planning algorithm; extracting path key nodes; optimizing a DWA local path planning algorithm; and outputting the optimal motion path. The efficiency of path planning of the workshop inspection robot in the unstructured complex environment is effectively improved, the real-time performance and safety are enhanced, and the workshop inspection robot can dynamically avoid obstacles while finding the optimal path in the complex environment.

Description

Technical Field

[0001] The invention belongs to the technical field of workshop inspection robot path planning, and in particular relates to a path planning method based on improved Hybrid A* algorithm and DWA algorithm. Background Art

[0002] Path planning technology is widely used in the field of robot navigation. In the complex industrial workshop environment, inspection robots face huge challenges when performing tasks, and have high requirements for autonomous navigation functions. Path planning and dynamic obstacle avoidance are key technologies for autonomous navigation. The real-time and safety of path search have an important impact on navigation performance.

[0003] According to the scene environment, the complexity of the road conditions, and the prior environment, the inspection robot path planning can be divided into global path planning and local path planning. The global path planning Hybrid A* algorithm generates a feasible path while considering the kinematic constraints, but its computational efficiency is low when facing complex and dynamic obstacles, the planned path has many turning points, and it lacks the ability to respond quickly to environmental changes. The local path planning algorithm dynamic window method (DWA) evaluates the feasibility of the current path through a dynamic window search strategy and selects the optimal obstacle avoidance path in real time, but it often falls into the problem of local optimal solution, especially in a highly dynamic environment, and has a slow response speed and cannot quickly adjust to changes.

[0004] Therefore, in the autonomous navigation of inspection robots, improving the real-time and safety of path planning is a key issue that needs to be solved urgently. Summary of the invention

[0005] In order to overcome the above shortcomings, the present invention discloses a path planning method based on an improved Hybrid A* algorithm and a DWA algorithm. The method performs global path planning by improving the cost function and search step size of the Hybrid A* algorithm, and performs B-Spline algorithm smoothing on the searched global path; the DWA algorithm with an optimized evaluation function is combined to perform local path obstacle avoidance, thereby improving the real-time and safety of the path planning of the workshop inspection robot in an unstructured complex environment. In order to achieve the above purpose, the technical solution adopted by the present invention is as follows: The path planning method based on the improved Hybrid A* algorithm and DWA algorithm includes the following steps: Step S1: Based on the laser radar sensor data, a global grid map of the working environment is constructed by the Gmapping mapping algorithm, and a starting point and a target point are set; Step S2, introducing dynamic weight of heuristic function, using Euclidean distance as heuristic distance, and using bidirectional search strategy to improve A* algorithm; Step S3, improving the cost function of the Hybrid A* algorithm, using the path cost in the improved A* algorithm as the complete heuristic constraint cost with obstacles, and the extended distance of the Dubins curve as the non-complete constraint heuristic cost without obstacles, and adopting a variable step-size search strategy based on obstacle distance; Step S4: using Douglas-Peucker algorithm to reduce redundant points on the path, and using B-Spline algorithm to smooth the searched global path to obtain the global optimal path; Step S5, extracting key nodes of the global optimal path; Step S6: According to the dynamically changing environmental conditions, CNN is used to dynamically adjust the weight of the evaluation function of the DWA algorithm to optimize the path planning strategy in real time; Step S7: output the optimal motion path.

[0006] Further optimization, in step S2, a threshold parameter is set according to the heuristic distance. When the distance of the calculated target point is greater than the threshold, the weight of the heuristic function is large, and when the distance of the calculated target point is less than the threshold, the weight of the actual cost function is large.

[0007] Further optimization, in step S3, during the search process, the target point is connected with a Dubins curve every 15 path points, and the path search is completed if there are no obstacles.

[0008] Further optimization, in step S3, a variable step search strategy based on obstacle distance is adopted to dynamically select the step size according to the distance between the current path point position and the obstacle.

[0009] Further optimization, step S4 is specifically as follows: select the first and last points A and B of the curve, calculate and connect them with a straight line, calculate the vertical distance from each point on the curve to the straight line, and determine the point C with the largest distance; if the maximum distance is greater than a preset threshold, retain point C and divide the curve into two segments: A to C, C to B, and recursively perform the same operation on these two segments respectively; if the maximum distance is less than or equal to the threshold, it is considered that the curve segment is simple enough and can be replaced by a straight line AB, and the recursion terminates.

[0010] Further optimization, in step S5, the key nodes represent turning points or important way points of the path, which are used as process points for local path planning.

[0011] Further optimization, in step S6, CNN receives environmental features and robot states as input, outputs the optimal weight combination by learning historical data and feedback, and guides the DWA algorithm to evaluate different motion trajectories and select the optimal path.

[0012] The beneficial effects of the present invention are: The cost function of the Hybrid A* algorithm is improved, and the path cost in the improved A* algorithm is used as the complete heuristic constraint cost with obstacles, and the extended distance of the Dubins curve is used as the incomplete constrained heuristic cost without obstacles. A variable step search strategy based on obstacle distance is adopted to improve the efficiency of global path planning. By combining CNN with the DWA algorithm, the weights of the evaluation function in the DWA algorithm are adaptively adjusted, making local path obstacle avoidance more intelligent and efficient, and avoiding the problem of local optimal solution in the traditional DWA algorithm. Based on the global path algorithm combined with the local path algorithm, the present invention proposes an improved Hybrid A* algorithm and DWA algorithm, which effectively solves the problems of incoordination between global and local optimization in the path planning process of the robot in a dynamic and complex environment, improves the efficiency of path planning, and enhances real-time performance and safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 This is the structural diagram of the workshop inspection robot; Figure 2 It is a two-dimensional grid map; Figure 3 is a set of Dubins curves; Figure 4 This is a comparison chart of the Douglas-Peucker algorithm processing results; Figure 5 Optimize schematics for typical curves; Figure 6 To improve the global path planning of Hybrid A* algorithm; Figure 7 To improve the local path planning of DWA algorithm; Figure 8 Simulate the fusion algorithm in the ROS environment; Fig. 9 It is the flow chart of the global and local combination algorithm; Figure numerals: 1. Carriage, 2. Motor, 3. Reducer, 4. Battery, 5. Driving wheel, 6. Supporting wheel, 7. Track, 8. Control panel, 9. Controller. DETAILED DESCRIPTION

[0014] In order to more clearly understand the above-mentioned purposes, features and advantages of the present invention, the present invention is described in detail below in conjunction with specific embodiments. The following embodiments are implemented based on the technical solutions of the present invention, and detailed implementation methods and specific operating procedures are given. However, the present invention can also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited to the following embodiments.

[0015] Taking the path planning of the workshop inspection robot as an example, the path planning method based on the improved Hybrid A* algorithm and DWA algorithm effectively combines global path planning and local path planning. Based on the global optimal path, the DWA local path planning algorithm is integrated and combined with sensor data to achieve dynamic obstacle avoidance. In a specific embodiment, it specifically includes the following steps: Step S1: The workshop inspection robot includes a body model, a crawler walking mechanism, and a control module. The body model is composed of a car box 1, a motor 2, a reducer 3, a battery 4 and other components. The crawler walking mechanism is composed of a driving wheel 5, a supporting wheel 6, and a crawler 7. The control module is composed of a control board 8 and a controller 9, such as Figure 1 shown.

[0016] Create a grid map of the working environment and set the starting and ending points of the task. The workshop inspection robot is equipped with a laser radar sensor to scan the environment. The Gmapping mapping algorithm obtains laser radar data in real time to generate a high-precision grid map. In this step, set the starting point and target point. The starting point is usually the current position of the robot, and the target point is set according to the task requirements. The two-dimensional grid map of the environment is as follows: Figure 2 shown.

[0017] Step S2: Improve the heuristic function in the A* algorithm. This step optimizes the heuristic function of the traditional A* algorithm by introducing dynamic weights and Euclidean distance. The heuristic function coefficient in the traditional A* algorithm is a constant, while in the present invention, a heuristic function weight factor is introduced to dynamically adjust the search trend; the threshold parameter is set according to the heuristic distance; when the distance to the calculated target point is greater than the threshold, the weight of the heuristic function is large, and when the distance to the calculated target point is less than the threshold, the actual cost function weight is large. Euclidean distance, as a heuristic distance, can simplify the calculation process and improve the efficiency of path planning. The cost function formula f(n) is shown in formula (1): f(n)=g(n)+h(n) (1) Among them: f(n) is the cost function, g(n) is the actual cost function, and h(n) is the heuristic function. The formula g(n) is usually the accumulation of the single-step Euclidean distance of the determined path points, and the formula can be expressed as: Where: (x n ,y n ) is the current coordinate, (x n-1 ,y n-1 ) are the coordinates of the previous node.

[0018] Euclidean distance does not rely on the horizontal and vertical constraints of the grid and is suitable for path planning in relatively free space. Euclidean distance is the straight-line distance between two points. It is fast to calculate and easy to implement. Therefore, in this paper, h(n) is expressed as the Euclidean distance between the current point and the target point. The formula is: Where: (x goal ,y goal ) are the target point coordinates.

[0019] At the same time, the bidirectional search strategy is adopted to significantly reduce the computational complexity of the A* algorithm and improve the efficiency of path planning. Dynamic weight adjustment ensures flexibility in the path planning process and guarantees real-time optimization of path search.

[0020] Step S3: Improve the cost function and search step strategy of the Hybrid A* algorithm. Improve the cost function of the Hybrid A* algorithm, use the path cost in the improved A* algorithm as the complete heuristic constraint cost with obstacles, and use the extended distance of the Dubins curve as the incomplete constraint heuristic cost without obstacles. The Dubins curve fully considers the kinematic constraints of the inspection robot to ensure that the planned path does not exceed its maximum turning radius when the vehicle turns, avoiding infeasible or unstable paths caused by too small turning radius. During the search process, the target point is connected with the Dubins curve every 15 path points. If there are no obstacles, the path search is completed.

[0021] The Dubins curve is divided into 6 cases: {LSL, RSR, RSL, LSR, RLR, LRL}. L represents counterclockwise circular motion, R represents clockwise circular motion, and S represents straight line motion. The circle represents the starting point, and the asterisk represents the end point. The schematic diagram of the Dubins curve is as follows Figure 3 shown.

[0022] A variable step search strategy based on obstacle distance is adopted, and the step size is dynamically selected according to the distance between the current path point and the obstacle. The accuracy and speed of the path search of the Hybrid A* algorithm are improved by dynamically adjusting the search step size.

[0023] This improvement ensures the feasibility of path planning in dynamic environments and is particularly suitable for scenarios that require precise control of turning radius.

[0024] Step S4: Smooth the path searched by the global path planning algorithm. In order to improve the feasibility of path planning, the Douglas-Peucker algorithm is used to remove redundant points in the path and simplify the path structure, thereby reducing the complexity of path calculation. Select the first and last points A and B of the curve, calculate and connect them with a straight line, calculate the vertical distance from each point on the curve to the straight line, and determine the point C with the largest distance. If the maximum distance is greater than the preset threshold, then point C is retained and the curve is divided into two segments: A to C and C to B, and the same operation is recursively performed on these two segments; if the maximum distance is less than or equal to the threshold, it is considered that the curve segment is simple enough and can be replaced by straight line AB, and the recursion terminates. Repeat this process until the simplification of all segments meets the threshold condition, retain the most representative key points, and output the simplified curve. The comparison chart of the processing results of the Douglas-Peucker algorithm is shown in the figure below. Figure 4 shown.

[0025] The B-Spline algorithm is used to smooth the path, eliminate the jagged shape in the path, optimize the common U-shaped bends and S-shaped bends, and improve the robot's motion stability. The B-Spline algorithm achieves fine adjustment of a specific path segment by adjusting a certain control point. The B-Spline algorithm formula is shown in formula (4): The formula contains three major elements: basis function N i，p , control point P i , knot vector U={u0,u1,u2,……,u m}; The B-Spline algorithm uses the deBoor-Cox recursive algorithm as the basis function, and recursively calculates the points on the B-Spline algorithm under given parameters. According to the control points and node vectors, the coordinates of the curve at specific parameter positions are gradually obtained through the linear combination of the basis functions. The formula is shown in formula (5): in: is the curve point to be calculated, u is the parameter value currently to be calculated, and U i+k and U i is a node in the node vector, and k represents the order of recursion. The termination condition is that when the recursion reaches the order of 1, the point obtained is the actual point on the curve.

[0026] Typical curve optimization principle diagram is as follows Figure 5 The global path planning diagram of the improved Hybrid A* algorithm is shown in Figure 6 shown.

[0027] Step S5: Extraction of key nodes of the path. After obtaining the global optimal path, this step extracts the key nodes of the path. These key nodes represent turning points or important path points of the path. They are used as process points of local path planning to help the robot strictly follow the global path in actual movement and effectively reduce path tracking errors.

[0028] Step S6: Optimize the DWA local path planning algorithm. This step involves the optimization of the DWA algorithm. By combining the convolutional neural network (CNN) with the DWA algorithm, the weights of the evaluation function in the DWA algorithm are adaptively adjusted to optimize the path planning process. CNN receives environmental features and robot status as input, and outputs the optimal weight combination by learning historical data and feedback, guiding the DWA algorithm to evaluate different motion trajectories and select the optimal path.

[0029] The implementation process of the DWA algorithm mainly consists of three steps: (1) Gradient sampling is performed based on the robot's own linear velocity and angular velocity range to form a linear velocity and angular velocity sampling space; (2) Select linear velocity and angular velocity from the sampling space in turn, and calculate the simulated path within the prediction time based on the obstacle position; (3) Score the simulated paths at different sampling points, intercept the points per unit time on the optimal path as the robot's motion points, and select the linear velocity and angular velocity under the path as the robot's motion state at the next time.

[0030] The evaluation function formula of the DWA algorithm is as follows: G(v,ω)=σ(α·heading(v,ω))+σ(β·dist(v,ω))+σ(γ·velocity(v,ω)) (6) Wherein: heading(v, ω) is the azimuth evaluation function, which is used to evaluate the error Δθ between the trajectory endpoint position direction and the angle between the target point and the target point at the current sampling speed; heading(v, ω) = π-Δθ, the larger the evaluation function, the better the sampling speed.

[0031] dist(v, ω) is a distance evaluation function, which indicates the closest distance between the corresponding simulated trajectory and the obstacle at the current speed. If there is no obstacle or the closest distance is greater than the set threshold, its value is set to a larger constant value.

[0032] velocity(v, ω) is a velocity evaluation function, which indicates the current velocity and can be expressed by the current linear velocity. The larger its value is, the faster the speed on the planned trajectory is, and the higher the evaluation score is.

[0033] α, β and γ are all evaluation function coefficients. The evaluation scores are normalized, and the normalization formula is as follows: CNN is used to dynamically adjust the weights in the objective function according to environmental conditions, improve the robot's adaptability and path planning efficiency, avoid the limitations of manual weight adjustment, and continuously optimize through online learning. CNN can effectively process lidar data with spatial structure.

[0034] In the CNN-based weight adaptation method, it is first necessary to train the CNN through a large amount of training data and simulation environment. It is mainly divided into the following steps: (1) Definition of input data: The sensor data mounted on the robot is used as the input of CNN, such as LiDAR, IMU, and Odom. These input data include: obstacle location and density, target distance and direction, and the relative position of the robot to the target. (2) Training process: Supervised learning is used to continuously optimize the DWA weights, so as to automatically adjust the weights of the direction angle evaluation function, the distance evaluation function, and the speed evaluation function according to the environmental status. (3) Real-time reasoning and weight adjustment. The trained CNN is used to dynamically adjust the weights of the evaluation function according to the acquired environmental information to achieve real-time adjustment. The DWA algorithm evaluates different actions or trajectories in real time based on the adjusted weights and selects the optimal path to achieve safe and accurate navigation of the robot.

[0035] An online learning strategy is adopted to enhance the applicability of CNN in the face of continuously changing environments. During real-time execution, the robot can not only perform reasoning through the existing trained network, but also accumulate new data and continuously optimize the network model during interaction with the environment.

[0036] The CNN algorithm with strong adaptability and long-term optimization ability is used to dynamically adjust the weights of the DWA algorithm evaluation function to achieve flexible and efficient path planning of the robot in a complex dynamic environment. The local path planning diagram of the improved DWA algorithm is shown in the figure Figure 7 shown.

[0037] Step S7: Output the optimal motion path. The global optimal path is planned according to the improved Hybrid A* algorithm, and the local obstacle avoidance strategy is adjusted in real time in combination with the DWA algorithm to output the optimal motion path to ensure that the robot can perform tasks smoothly and efficiently in a dynamic and complex environment. The effect of path planning after simulation algorithm fusion in the ROS environment is shown in the figure below. Figure 8 shown.

[0038] The above shows and describes the main features, methods of use, basic principles and advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited by the above embodiments. The above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements according to actual conditions, and these changes and improvements fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A path planning method based on improved Hybrid A* algorithm and DWA algorithm, characterized in that: The following steps are involved: Step S1: Based on the laser radar sensor data, a global grid map of the working environment is constructed by the Gmapping mapping algorithm, and a starting point and a target point are set; Step S2, introducing dynamic weight of heuristic function, using Euclidean distance as heuristic distance, and using bidirectional search strategy to improve A* algorithm; Step S3, improving the cost function of the Hybrid A* algorithm, using the path cost in the improved A* algorithm as the complete heuristic constraint cost with obstacles, and the extended distance of the Dubins curve as the non-complete constraint heuristic cost without obstacles, and adopting a variable step-size search strategy based on obstacle distance; Step S4: using Douglas-Peucker algorithm to reduce redundant points on the path, and using B-Spline curve method to smooth the searched global path to obtain the global optimal path; Step S5, extracting key nodes of the global optimal path; Step S6: According to the dynamically changing environmental conditions, CNN is used to dynamically adjust the weight of the evaluation function of the DWA algorithm to optimize the path planning strategy in real time; Step S7: output the optimal motion path.

2. The path planning method based on the improved Hybrid A* algorithm and the optimized DWA algorithm as claimed in claim 1, characterized in that: In step S2, a threshold parameter is set according to the heuristic distance. When the distance to the target point is calculated to be greater than the threshold, the weight of the heuristic function is large. When the distance to the target point is calculated to be less than the threshold, the weight of the actual cost function is large.

3. The path planning method based on the improved Hybrid A* algorithm and the optimized DWA algorithm as claimed in claim 1, characterized in that: In step S3, during the search process, the target point is connected with a Dubins curve every 15 path points, and the path search is completed if there is no obstacle.

4. The path planning method based on the improved Hybrid A* algorithm and the optimized DWA algorithm as claimed in claim 1, characterized in that: In step S3, a variable step-size search strategy based on obstacle distance is adopted to dynamically select the step size according to the distance between the current path point position and the obstacle.

5. The path planning method based on the improved Hybrid A* algorithm and the optimized DWA algorithm as claimed in claim 1, characterized in that: The step S4 is specifically as follows: selecting the first and last points A and B of the curve, calculating and connecting them with a straight line, calculating the vertical distance from each point on the curve to the straight line, and determining the point C with the largest distance; if the maximum distance is greater than a preset threshold, retaining point C and dividing the curve into two segments: A to C and C to B, and recursively performing the same operation on the two segments; if the maximum distance is less than or equal to the threshold, it is considered that the curve segment is simple enough and can be replaced by a straight line AB, and the recursion terminates.

6. The path planning method based on the improved Hybrid A* algorithm and the optimized DWA algorithm as claimed in claim 1, characterized in that: In step S5, the key nodes represent turning points or important waypoints of the path, which are used as process points for local path planning.

7. The path planning method based on the improved Hybrid A* algorithm and the optimized DWA algorithm as claimed in claim 1, characterized in that: In step S6, CNN receives environmental features and robot status as input, outputs the optimal weight combination by learning historical data and feedback, and guides the DWA algorithm to evaluate different motion trajectories and select the optimal path.

Images