Method for path planning of ackerman mobile robot by improved a* algorithm and control method

Abstract

The application discloses a method for path planning of Ackerman mobile robots by using an improved A* algorithm and a control method of the Ackerman mobile robots. The method comprises the following steps: when the Ackerman mobile robot is moving, a heuristic function between a current node and a target node is used for path planning, and the Ackerman mobile robot moves according to the planned path; and the determination mode of the heuristic function comprises the following steps: a straight line distance L1 from the current node to the target node is calculated, and a straight line distance L2 from a starting point to the target node is calculated; if the L1 is greater than half of the L2, a circular circumference is used as the heuristic function, otherwise, an elliptical circumference is used as the heuristic function.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of path planning, and particularly relates to a method for path planning of Ackerman mobile robot by improved A* algorithm and a control method of Ackerman mobile robot. BACKGROUND

[0002] An efficient path planning algorithm helps mobile robots to quickly realize various task scheduling. For example, A*(A-Star) algorithm is a direct search method for solving the shortest path in static road network. However, in a complex known space, the traditional A* algorithm has low efficiency in path planning, and the generated route has many turning points and the path is not smooth. SUMMARY

[0003] Therefore, it is necessary to provide a path planning method for Ackerman mobile robot based on improved A* algorithm in view of the problems of the traditional path planning algorithm.

[0004] The present application relates to a method for path planning of Ackerman mobile robot by improved A* algorithm. When the Ackerman mobile robot is working, the heuristic function between the current node and the target node is used for path planning, and the Ackerman mobile robot walks according to the planned path.

[0005] The determination method of the heuristic function includes:

[0006] The straight line distance L1 from the current node to the target node is calculated, and the straight line distance L2 from the starting point to the target node is calculated.

[0007] If L1 is greater than half of L2, the circular circumference is used as the heuristic function, otherwise the elliptical circumference is used as the heuristic function.

[0008] Optionally, the path planning is based on the total walking cost between the current node and the target node, wherein the total walking cost includes an actual cost value and an estimated cost value, and the estimated cost value is obtained according to the heuristic function.

[0009] Optionally, after obtaining the planned path, a Savitzky-Golay filter is used for smoothing operation.

[0010] Optionally, the long axis of the ellipse is a and equal to L1, and the short axis is b, and:

[0011]

[0012]

[0013] Wherein cur_dis is the straight line distance L1 from the current node to the target node; goal_dis is the straight line distance L2 from the starting point to the target node.

[0014] The application also provides a control method of the Ackermann mobile robot, comprising:

[0015] An environment map is established for a working scenario.

[0016] A global path is obtained by using the method for path planning of the Ackermann mobile robot using the improved A* algorithm.

[0017] The Ackermann mobile robot is driven to move along the global path to perform work.

[0018] The path planning method of the Ackermann mobile robot based on the improved A* algorithm is applied to the Ackermann mobile robot by constructing a segmented geometric heuristic function and introducing a path smoothing filter in a path finding algorithm module; the improved A* path planning method is built based on the path finding algorithm module; the path planning method is applied to the Ackermann mobile robot to complete the path finding function of the robot; the Ackermann mobile robot with the improved A* algorithm is placed in an unknown environment, first, environment mapping is performed, then a mobile instruction is sent, then a feasible path from the starting point to the end point is planned according to the instruction, and finally the robot moves along the path. BRIEF DESCRIPTION OF DRAWINGS

[0019] Figure 1 FIG. 1 is a flowchart of the path planning method of the Ackermann mobile robot based on the improved A* algorithm.

[0020] Figure 2 FIG. 3 is a schematic diagram of the construction of the segmented geometric heuristic function.

[0021] Figure 3 FIG. 4 is a schematic diagram of the improved heuristic function pseudo code.

[0022] Figure 4 FIG. 5 is a schematic diagram of the improved A* algorithm pseudo code.

[0023] Figure 5 FIG. 6 is a simulation experiment result graph on five two-dimensional plane maps (the traditional A* algorithm and the improved A* algorithm are compared in the graph).

[0024] Figure 6 FIG. 7 is a schematic diagram of the Ackermann mobile robot used in the application.

[0025] Figure 7 FIG. 8 is a contrast schematic diagram of the real experimental environment and mapping used in the application.

[0026] Figure 8This is a comparison diagram of the movement of the Ackerman mobile robot in the real environment and its virtual display in this application;

[0027] Figure 9 This is a schematic diagram of a two-dimensional rasterized map. Detailed Implementation

[0028] To make the objectives, technical solutions, and advantages of this application clearer, the following description, in conjunction with the accompanying drawings and embodiments, provides further similar information. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0029] The path from the starting point to the ending point consists of a series of connected points. The A* algorithm is based on raster maps, which are created by rasterizing a two-dimensional planar map (e.g., ...). Figure 9 As shown, the black squares on the left represent the starting point, the black squares on the right represent the ending point, the black blocks in the middle represent obstacles, and the blank squares represent feasible areas. The map is divided into a series of square grids, each called a node. The selection of the next node is determined by a priority function value (F), with smaller values ​​indicating higher priority. The priority function consists of the actual cost value (G) and the estimated cost value (H). The actual cost value represents the true distance from the starting point to the current node, calculated using the Euclidean algorithm. The estimated cost value represents the predicted distance from the current node to the ending point, calculated using the improved heuristic function. Its purpose is to guide nodes closer to the ending point, reducing the number of nodes searched. The original A* algorithm mostly uses Euclidean distance or Manhattan distance as heuristic functions, which calculate an idealized total distance from the node to the ending point, assuming no obstacles in between.

[0030] This application uses an improved heuristic function to calculate the total length and predict the distance to bypass the obstacle.

[0031] Please see Figure 1 , Figure 1 This is a flowchart illustrating a path planning method for an Ackerman mobile robot based on an improved A* algorithm, as described in one embodiment of this application.

[0032] In this example, the path planning method based on the improved A* algorithm applied to the Ackerman mobile robot includes:

[0033] A pathfinding algorithm module is constructed by using a piecewise geometric heuristic function and introducing a path smoothing filter.

[0034] Understandably, piecewise geometric heuristics are used to find the next search point under different circumstances. They improve path search efficiency and reduce the expansion of search points. For cases where the generated path has many inflection points, path smoothing filters can optimize the final generated path.

[0035] An improved A* path planning method is built based on the pathfinding algorithm module.

[0036] It can be understood that in the improved A* algorithm, an improved heuristic function is first proposed. In known environment information, the estimated distance from the current node to the end node is calculated using segmented geometric figures by comparing the distance between the start node and the end node with the distance between the current node and the end node. Then, a filter is used to complete the smoothing processing of the generated global path.

[0037] When applied to Ackerman mobile robots, the improved A* algorithm can be transplanted into the global path planner in the ROS navigation package. In an unknown environment, the Ackerman mobile robot first establishes an environment map and then sends a movement instruction. After receiving the instruction, the robot calls the improved A* algorithm to plan a global path. Finally, the robot moves along the feasible path to complete the relevant work.

[0038] The segmented geometric heuristic function is constructed as shown in Figure 2 .

[0039] Figure 2 In part (a) of the figure, it is assumed that the distance from the current node to the target node is greater than half the distance from the start node to the target node. At this time, the heuristic distance is the circumference of the circle with the distance from the current node to the target node as the diameter.

[0040] Figure 2 In part (b) of the figure, it is assumed that the distance from the current node to the target node is less than or equal to half the distance from the start node to the target node. At this time, the heuristic distance is the circumference of the ellipse with the distance from the current node to the target node as the length of the major axis.

[0041] In the figure, start node is the start node; cur node is the current node; end node is the target node; cur_dis is the straight-line distance L1 from the current node to the target node; goal_dis is the straight-line distance L2 from the start node to the target node; the black solid bar is the obstacle; the black solid dot is the node; the black hollow circle is the circle with a diameter of L1; and the black hollow ellipse is the ellipse with a major axis length of L1.

[0042] The length of the minor axis of the ellipse is determined by the length of the major axis, and they have a certain probability multiple relationship. In order to ensure that the minor axis does not suddenly become the major axis in special cases, the sigmoid function is used to constrain the length of the minor axis. The sigmoid function is often used as the activation function of the neural network, which can map the variable to between 0 and 1. At this time, the length of the minor axis is equal to the sigmoid multiple of the length of the major axis. As shown in Equation 1.

[0043]

[0044]

[0045]

[0046]

[0047]

[0048] where cur_dis represents the straight-line distance L1 from the current node to the goal node. goal_dis represents the straight-line distance L2 from the start node to the goal node. r is the radius of the circle. a is the length of the major axis of the ellipse. b is the length of the minor axis of the ellipse. β is the variable of the sigmoid function.

[0049] The improved heuristic function is a geometric distance function that measures the distance of the obstacle. Compared with the Euclidean distance, it takes the extra distance as the distance of bypassing the obstacle. Although this distance is heuristic, it is more like the exact distance from the current node to the goal node in the actual environment containing obstacles. Figure 3 The improved heuristic function pseudo code is described.

[0050] The A* algorithm will maintain two tables in the pathfinding process, namely the OPEN table (storing nodes to be examined, i.e., the node has already calculated the F value, but whether it is optimal is uncertain) and the CLOSE table (storing nodes that have been examined, i.e., the node has determined that its F value is the minimum). The specific path planning is as follows: first, add the start node to the OPEN table and the CLOSE table. The start node is taken as the current node, and the adjacent 8 child nodes are traversed from it, ignoring the obstacle nodes, and stored in the OPEN table. The child node with the minimum F value is selected as the next node, and it is moved from the OPEN table to the CLOSE table. Then, the next node is taken as the current node, and its adjacent 8 child nodes are traversed, ignoring the nodes already existing in the CLOSE table. If the child node is not in the OPEN table, it is added to the OPEN table. The node with the minimum F value among them is selected as the next node, and it is moved from the OPEN table to the CLOSE table. In this way, the process is repeated, and once the end point is searched, the nodes in the CLOSE table are traced back from the end point to the start point. Then a route from the start point to the end point is found, the path search is completed, and the algorithm ends.

[0051] In other embodiments, the algorithm first initializes map information, recording the starting point, ending point, and obstacle positions, and then rasterizes the map. Next, the starting point is used as the parent node, and the algorithm traverses its surrounding child nodes, selecting the child node with the smallest priority function value as the next node. Then, the next node becomes the current node, and the algorithm traverses its surrounding child nodes, repeating the above steps until the ending point. The recorded nodes are then connected to form a feasible path from the starting point to the ending point. Finally, because there is a distance span between adjacent nodes, the route becomes sharp at turns after connection, which is detrimental to the robot's movement. Therefore, a Savitzky-Golay filter is used. It has three data quantities: the set of data points, the sliding window size, and the order of the polynomial. Specifically, the data points in the sliding window are fitted using a polynomial. After fitting, the number of points does not change, but the coordinates of the points are transformed, making the route smoother and less abrupt when connecting these points.

[0052] like Figure 4 As shown in lines 8 to 10 of the pseudocode for the improved A* algorithm in this application, the program traverses the OPEN table, passes the information of the starting point, the current node, and the target node into the piecewise geometric heuristic function to calculate H, then obtains the F value, selects the node with the smallest F value and adds it to the CLOSE table for subsequent operations.

[0053] In line 19 of the pseudocode for the improved A* algorithm in this application, the algorithm obtains the initial planned path. Since the points on this path are located at the center of the grid map and have many inflection points, sharp curves will occur when bypassing obstacles. Therefore, to make the path smoother and more consistent with the walking characteristics of a mobile robot, a Savitzky-Golay filter is used to fit the original path points, as shown in line 20 of the pseudocode for the improved A* algorithm in this application. The Savitzky-Golay filter fits a continuous subset of adjacent data points to a polynomial using the linear least squares method.

[0054] In this embodiment, to verify the effectiveness of the path planning method based on the improved A* algorithm in the Ackerman mobile robot, a simulation experiment was first conducted. Five different two-dimensional plane maps were selected for the simulation experiment. The performance of the traditional A* algorithm and the improved A* algorithm in terms of path length, pathfinding time, and number of path points searched was compared. Finally, the improved A* algorithm was ported to the Ackerman mobile robot to complete real-world scenario testing.

[0055] Specifically, for simulation experiment testing, such as Figure 5 In the land Figure 1 As shown, it is clear that the improved A* algorithm has a much smaller search range than the traditional A* algorithm. Furthermore, the improved A* algorithm performs more smoothly in handling curves. Table 1 records the algorithm's performance on various terrains.Figure 1 from Table 1, the traditional A* algorithm generated a path with total length 175.5391, path planning time 22.584 and total number of path search points 1611. The improved A* algorithm generated a path with total length 173.9582, path planning time 1.294 and total number of path search points 222. The path generated on the map in Figure 1 Figure 1 Figure 2 The path generated on the map in Figure 5 Figure 2 As shown in the map in Figure 3 Figure 5 Figure 3 As shown in the map in Figure 3 Figure 4 Figure 5 Figure 4 As shown in the map in Figure 5 Figure 5 Figure 5 ​​​​​​​​​​The improved A* algorithm was found to be trapped in a local optimum between the horizontal range of 85 to 130 and the vertical range of 0 to 140 during the experiment. It took a small amount of time to get out of the range and finally expand to the target point. The traditional A* algorithm and the previous algorithm performed similarly and took a large amount of time and searched a large number of points to find a path from the start point to the target point. Table 5 records the experimental results of the two algorithms on the map of Figure 5 . The total length of the path generated by the traditional A* algorithm was 304.7351, the pathfinding time was 111.921, and the total number of searched points was 3909. The total length of the path generated by the improved A* algorithm was 299.1676, the pathfinding time was 9.241, and the total number of searched points was 931.

[0056] Based on the experimental study, compared with the traditional A* algorithm, on the map of Figure 1 , the improved A* algorithm shortened the path length by 1.58 m, reduced the pathfinding time by 21.29 s, and reduced the number of searched points by 1389. On the map of Figure 2 , the improved A* algorithm shortened the path length by 4.82 m, reduced the pathfinding time by 1.76 s, and reduced the number of searched points by 757. On the map of Figure 3 , the improved A* algorithm shortened the path length by 4.00 m, reduced the pathfinding time by 9.08 s, and reduced the number of searched points by 901. On the map of Figure 4 , the improved A* algorithm shortened the path length by 1.47 m, reduced the pathfinding time by 11.05 s, and reduced the number of searched points by 932. On the map of Figure 5 , the improved A* algorithm shortened the path length by 5.57 m, reduced the pathfinding time by 102.68 s, and reduced the number of searched points by 2978. According to the above detailed experimental results, it can be concluded that the improved A* algorithm is superior to the traditional A* algorithm in terms of the number of inflection points, path smoothness, path total length, pathfinding time, and the number of searched points.

[0057] Table 1 Experimental results of the algorithm on the map of Figure 1

[0058] Algorithm Number of experiments Path length Pathfinding time Number of search points A* 10 175.5391 22.584 1611 Improved A* 10 173.9582(↓1.58) 1.294(↓21.29) 222(↓1389)

[0059] Table 2 Experimental results of the algorithm on the map of Figure 2

[0060] Algorithm Number of experiments Path length Pathfinding time Number of search points A* 10 171.4214 7.522 822 Improved A* 10 166.5967(↓4.82) 0.366(↓7.16) 65(↓757)

[0061] Table 3 Experimental results of the algorithm on the map of Figure 3

[0062] Algorithm Number of experiments Path length Pathfinding time Number of search points A* 10 147.1960 9.491 972 Improved A* 10 143.1950(↓4.00) 0.410(↓9.08) 71(↓901)

[0063] Table 4 Experimental results of the algorithm on the map of​​​Figure 4 Experimental results on

[0064] Algorithm Number of experiments Path length Pathfinding time Number of search points A* 10 153.1960 12.008 1091 Improved A* 10 151.7241(↓1.47) 0.957(↓11.05) 159(↓932)

[0065] Experimental results on Figure 5 the algorithm in Table 5

[0066] Algorithm Number of experiments Path length Pathfinding time Number of search points A* 10 304.7351 111.921 3909 Improved A* 10 299.1676(↓5.57) 9.241(↓102.68) 931(↓2978)

[0067] Specifically, for real experiments, the present application uses Ackerman mobile robots, as shown in Figure 6 The improved A* algorithm is transplanted into the global path planner in the ROS navigation package. As shown in Figure 7 In an unknown environment, the Ackerman mobile robot first maps according to the gmapping algorithm, and stores the map into the map package. Then a target point is marked using the 2D Nav Goal button, and a moving instruction is sent, as shown in Figure 8 After the robot receives the instruction, it calls the improved A* algorithm to plan a global path, and the robot moves along the feasible path. When encountering unknown obstacles, the Monte Carlo localization method is used to locate the current position information, and the TebLocalPlannerROS local planner plug-in is used to complete dynamic obstacle avoidance.

[0068] The technical features of the above-described embodiments can be combined in any manner. To make the description concise, not all possible combinations of the technical features in the above-described embodiments are described, but as long as the combinations of the technical features do not contradict, they should be considered within the scope of the present disclosure. When technical features in different embodiments are embodied in the same figure, it can be considered that the figure also discloses the combination of the embodiments involved.

[0069] The above-described embodiments only express several implementation manners of the present application, and the description is specific and detailed, but it should not be understood as a limitation on the patentable scope of the application. It should be pointed out that for ordinary skilled in the art, without departing from the concept of the present application, a number of modifications and improvements can be made, which are within the scope of the present application.

Claims

1. A method for path planning of an Ackerman mobile robot using an improved A* algorithm, characterized in that, When the Ackerman mobile robot is working, it uses a heuristic function between the current node and the target node to plan the path and moves accordingly according to the planned path. The methods for determining the heuristic function include: Calculate the straight-line distance L1 from the current node to the target node, and calculate the straight-line distance L2 from the starting point to the target node; If L1 is greater than half of L2, the circumference of the circle is used as the heuristic function; otherwise, the circumference of the ellipse is used as the heuristic function. Path planning is based on the total cost of walking between the current node and the target node. The total cost of walking includes the actual cost and the estimated cost. The actual cost represents the real distance from the starting point to the current node, and the estimated cost is obtained based on the heuristic function. The circumference of the circle is the circumference of the circle with the distance L1 from the current node to the target node as its diameter; The circumference of the ellipse is the circumference of the ellipse with major axis a and minor axis b, where major axis a equals L1, and: Where cur_dis is the straight-line distance L1 from the current node to the target node; and goal_dis is the straight-line distance L2 from the starting point to the target node.

2. The method for path planning of an Ackerman mobile robot using the improved A* algorithm as described in claim 1, characterized in that, After obtaining the planned path, a smoothing operation is also performed using a Savitzky-Golay filter.

3. A control method for an Ackerman mobile robot, characterized in that, include: Create an environmental map for the work scenario; A global path is obtained using the improved A* algorithm for path planning of an Ackerman mobile robot as described in any one of claims 1 to 2; The Ackerman mobile robot is driven to move along the global path to perform the task.

Images