A Robot Path Planning Method Based on A-Star Penalty Control Optimization Algorithm

Abstract

The present invention provides a robot path planning method based on the A-star penalty control optimization algorithm. The present invention adopts a current node neighborhood expansion method, takes into account the differences between different types of robots, and differentially expands the current node neighborhood according to the kinematic characteristics and dynamic characteristics of different types of robots, thereby reducing the search for useless nodes and improving the universality of the algorithm. In addition, the present invention introduces a penalty algorithm to optimize the cost function, so that the cost function is more accurate in estimating the cost from the current node to the target node, and the inspiration is more purposeful, which is conducive to reducing the number of explored nodes and improving the path smoothness.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to a robot path planning method based on an A-star penalty control optimization algorithm. Background Art

[0002] With the development of modern manufacturing technology, artificial intelligence technology is more widely used in robots. Robots have become a complex that integrates environmental perception, path planning, and motion control. Path planning, as a key technology in the field of mobile robots, aims to find a collision-free optimal path connecting the starting point and the end point. It is the key to realizing the autonomous navigation of intelligent robots in complex environments. Representative solutions to the problem of robot path planning include the A-star algorithm, the D-star algorithm, the rapidly expanding random tree method, the artificial potential field method, the neural network method, etc.

[0003] As the most widely used path planning method, the A-star algorithm has the advantages of high computational efficiency and short planning path length. The A-star algorithm is a heuristic search algorithm, that is, it seeks the target end point heuristically and minimizes the cost, and on this basis finds the most suitable and shortest path to the target point. However, when facing more complex obstacles, the traditional A-star algorithm will have many turning points in the path it plans, and its smoothness will be greatly reduced. It will also search for many useless nodes, resulting in a decrease in computational efficiency.

[0004] Many scholars at home and abroad have improved the traditional A-star algorithm. For example, the Chinese invention patent application number is 202010182339.3, and the patent name is “A pathfinding method based on A-star optimization algorithm”. The algorithm is optimized by preprocessing obstacles to reduce the search time and computing memory of nodes, but the problem of uneven paths in complex environments has not been deeply explored; the Chinese invention patent application number is 201711374451.1, and the patent name is “A mobile robot path planning method based on improved A-star algorithm”. The planning results are secondary smoothed to make the curve smoother, but a lot of searches are still required around obstacles. Summary of the invention

[0005] Purpose of the invention: The technical problem to be solved by the present invention is to provide a robot path planning method based on the A-star penalty control optimization algorithm in view of the shortcomings of the prior art. The purpose is that the proposed method will be able to overcome the shortcomings of the traditional A-star algorithm to reduce the number of turning points in the planned path, improve the path smoothness and reduce the number of exploration nodes, thereby shortening the search time.

[0006] The method of the present invention comprises the following steps:

[0007] Step 1: Use sensors to collect surrounding environment map information to build a map of the robot's environment;

[0008] Step 2, rasterize the map, determine the state of each grid as occupied or idle, and determine the location coordinates of obstacles;

[0009] Step 3: Determine the coordinates of the path starting point (x start ,y start ,z start ) and the target point coordinates (x goal ,y goal ,z goal ); x start ,y start 、z start are the x, y, and z axis coordinates of the starting point respectively; goal ,y goal 、z goal The x, y, and z coordinate values ​​of the target point;

[0010] Step 4: Initialize the open list and the close list, and set the coordinates of the path starting point (x start ,y start ,z start ) is put into the open list, and the closed list is empty;

[0011] Step 5, check whether the open list is empty. If it is empty, it means that the path planning fails. If it is not empty, go to step 6;

[0012] Step 6, ignore the obstacle nodes. If the close list is not empty, ignore the nodes in the close list, expand the current node, and obtain the neighboring nodes of the current node.

[0013] Step 7: Determine whether the neighboring nodes of the current node are in the open list:

[0014] If the neighboring nodes of the current node are not in the open list, then the neighboring nodes of the current node are expanded and added to the open list, and the current node is used as the parent node of the neighboring nodes, and the cost function of the neighboring nodes of the current node is calculated, that is, step 8 is executed;

[0015] If the neighboring nodes of the current node are in the open list, and if the g(n) of the neighboring nodes of the current node is lower than the original one with the current node as the parent node, then the current node is taken as the parent node, and the cost function of the neighboring nodes of the current node is recalculated, that is, step 8 is executed, otherwise step 9 is executed;

[0016] Step 8, calculate the cost function f(n) of the neighborhood node, where n represents the nth node;

[0017] Step 9, select the point with the smallest cost function f(n) value in the open list as the current node to be expanded, delete the current node from the open list, and add it to the closed list;

[0018] Step 10: Determine whether the target point is added to the open list:

[0019] If the target point has been added to the open list, the path planning is completed, and the reverse solution from the target point to the starting point is the final path obtained by this robot path planning;

[0020] If the target point is not in the open list, go to step 5.

[0021] The sensors described in step 1 include millimeter wave radar, laser radar, camera, inertial measurement unit IMU, and global positioning system GPS.

[0022] In step 6, the type of robot uses different expansion methods to expand the neighborhood of the current node, including:

[0023] Step 6-1: When the robot type is an unmanned vehicle, first calculate the minimum turning radius R at the current speed. min , and then through Calculate the steering angle δ corresponding to the minimum turning radius at the current vehicle speed, and evenly divide the [-δ,δ] angle interval into i Δδ intervals in the current driving direction. Δδ is usually set to one degree, uniformly expanding the neighborhood of the current node;

[0024] Step 6-2: When the robot type is an AGV, the neighborhood of the current node is uniformly expanded within the 360° direction of the two-dimensional plane;

[0025] Step 6-3, when the type of the robot is a drone, the neighborhood of the current node is expanded in the three-dimensional space.

[0026] The calculation steps of calculating the cost function f(n) in step 8 include:

[0027] Step 8-1, calculate the actual cost function g(n), which represents the actual moving cost from the starting point of the path to the current node, along the path generated to reach the current node;

[0028] g(n)=g(n-1)+g'(n)

[0029] g(n-1) is the actual cost from the starting node to the parent node of the current node, and g'(n) is the cost from the parent node of the previous node to the current node.

[0030] Step 8-2, calculate the heuristic function h(n), and use the Euclidean distance to represent the estimated cost from the current node to the path target point, (x n ,y n ,z n ) is the coordinate of the current node in the grid map. The Euclidean distance represents the straight-line distance between two points. The formula is as follows:

[0031]

[0032] Step 8-3, calculate the penalty function ξ(n).

[0033] Step 8-3 includes:

[0034] Step 8-3-1, introduce two vectors n1, n2:

[0035] n1=(x n-1 -x parent ,y n-1 -y parent ,z n-1 -z parent ),n2=(x parent -x n ,y parent -y n ,z parent -z n )

[0036] Among them, (x n ,y n ,z n ) is the coordinate of the current node in the grid map, (x parent ,y parent ,z parent ) is the coordinate of the parent node of the current node in the grid map, (x n-1 ,y n-1 ,z n-1 ) is the coordinate of the parent node of the parent node in the grid map; n1 represents the vector from the parent node of the parent node to the parent node of the current node, and n2 represents the vector from the parent node to the current node;

[0037] Step 8-3-2, calculate the cosine value of the angle θ between the two vectors n1 and n2:

[0038]

[0039] Step 8-3-3, calculate the value of the penalty function ξ(n) based on the cosine value result:

[0040]

[0041] a1 and a2 are adjustment thresholds, which need to be debugged according to different grid maps, and the values ​​suitable for the local map should be selected according to the size and accuracy of the grid map;

[0042] When cosθ=1, the current node, the parent node, and the parent node of the parent node are on the same straight line, indicating that the planned path does not turn, no penalty is imposed, ξ(n)=0;

[0043] When cosθ≠1, and x n =x goal ,y n =y goal ,z n =z goal , indicating that the current node is the target node, and the path from the parent node of the current node to the target node has turned relative to the planned path, that is, the turning point turns to the end point. At this time, the penalty value is ξ(n) = a1;

[0044] When -1≤cosθ<1, and the current node is not the target point, it means that a turn occurs in the path planned to reach the target point, that is, a common turning point occurs. At this time, the penalty value is ξ(n)=a2, where a2>a1.

[0045] The beneficial effects of the present invention are:

[0046] A robot path planning method based on the A-star penalty control optimization algorithm proposed in the present invention improves the problems of the traditional A-star algorithm in that the path planned has many turning points, low path smoothness, and a large number of useless nodes when facing more complex obstacles. It improves the smoothness of the planned path while effectively reducing the number of exploration nodes and search time.

[0047] The current node neighborhood expansion method adopted by the present invention takes into account the differences between different types of robots, and expands the current node neighborhood differently according to the kinematic and dynamic characteristics of different types of robots, thereby reducing the search for useless nodes and improving the universality of the algorithm. In addition, the present invention introduces a penalty algorithm to optimize the cost function, so that the cost function makes the cost estimate from the current node to the target node more accurate, and the inspiration is more purposeful, which is conducive to reducing the number of explored nodes and improving the path smoothness.

[0048] In summary, the method proposed in the present invention is highly practical and is conducive to promoting the development of robot motion planning, autonomous driving and other fields. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments, and the above and / or other advantages of the present invention will become more clear.

[0050] Figure 1 The figure is a flow chart of the method of the present invention.

[0051] Figure 2 This is a flow chart of the penalty control algorithm used in the method of the present invention.

[0052] Figure 3 This is a diagram of the simulation results of the robot path planning method of the A-star penalty control optimization algorithm proposed in the present invention under the Rviz software in the Ubuntu environment, wherein the white square pointed to by 1 is an obstacle, 2 is the starting point of the path planning generated by the Monte Carlo method, 3 is the coordinate system, 4 is the end point of the path planning generated by the Monte Carlo method, 5 is the path planned by the traditional A-star algorithm, and 6 is the path planned by the A-star algorithm of the present invention.

[0053] Figure 4 This is a data comparison table of the time required to plan a path to the same target point, path length, and number of explored nodes when the penalty function is introduced and when the penalty function is not introduced. DETAILED DESCRIPTION

[0054] The method of the present invention introduces penalty function control on the basis of the traditional A-star algorithm. When calculating the cost of a node, it is judged whether it turns. If so, a certain penalty value is given, thereby reducing the number of turning points in the planned path, achieving the purpose of smoothing the path and reducing the number of explored nodes. Figure 1 The following steps are mainly included:

[0055] Step 1: Use sensors such as millimeter-wave radar, lidar, camera, IMU inertial measurement unit (Inertial Measurement Unit), GPS global positioning system (GPS) to collect information required for path planning and draw a map of the robot's environment;

[0056] Step 2, rasterize the drawn map, determine the state of each grid as occupied or idle, and determine the location coordinates of the corresponding obstacles;

[0057] Step 3: Determine the coordinates of the path starting point (x start ,y start ,z start ) and the target point coordinates (x goal ,y goal ,z goal );

[0058] Step 4: Initialize the open list and the close list, and set the coordinates of the path starting point (x start ,y start ,z start ) is put into the open list, and the closed list is empty;

[0059] Step 5, check whether the open list is empty. If it is empty, it means that the path planning fails. If it is not empty, go to step 6;

[0060] Step 6, ignoring the obstacle nodes and the nodes in the close list, expanding the current node, and obtaining the neighboring nodes of the current node;

[0061] Step 7: Determine whether the neighboring nodes of the current node are in the open list:

[0062] If the neighboring nodes of the current node are not in the open list, then the neighboring nodes of the current node are expanded and added to the open list, and the current node is used as the parent node of the neighboring nodes, the cost function of the neighboring nodes of the current node is calculated, and step 7 is executed;

[0063] If the neighboring nodes of the current node are in the open list, and if the g(n) of the neighboring nodes of the current node is lower than the original one with the current node as the parent node, then take the current node as the parent node, recalculate the cost function of the neighboring nodes of the current node, and execute step 7, otherwise execute step 8;

[0064] Step 8, calculate the cost value of the cost function f(n) of the neighborhood node, where n represents the nth node;

[0065] Step 9: Select the point with the smallest cost function f(n) in the open list as the current node to be expanded (x n ,y n ,z n ), and delete the current node from the open list, and add it to the closed list, closelist;

[0066] Step 10: Determine whether the target point is added to the open list:

[0067] If the target point has been added to the open list, the path planning is completed, and the reverse solution from the target point to the starting point is the final path obtained by this robot path planning;

[0068] If the target point is not in the open list, go to step 5;

[0069] Furthermore, the robot in step 1 may be an unmanned car, an AGV, a drone, an unmanned boat, or any other robot with perception and decision-making capabilities;

[0070] Furthermore, in step 6, the expansion of the neighborhood nodes of the current node needs to be expanded according to the type of robot in step 1. The common A-star algorithm uses eight neighborhood node expansion for the expansion of the current node. Due to the different kinematic and dynamic characteristics of different types of robots, different expansion methods are used to expand the neighborhood of the current node:

[0071] Step 6-1: When the robot type is an unmanned vehicle, first calculate the minimum turning radius R at the current speed. min , and then through Calculate the steering angle δ corresponding to the minimum turning radius at the current vehicle speed, divide the [-δ,δ] angle interval into i Δδ intervals in the current driving direction, and evenly expand the neighborhood of the current node. It should be noted that the minimum turning radius R min Determined by the characteristics of the driverless car itself, such as the small turning radius of a family car and the large turning radius of commercial vehicles such as buses and trucks;

[0072] Step 6-2: When the robot is an AGV, it usually supports omnidirectional movement. In this scenario, the neighborhood of the current node can be evenly expanded in the 360° direction of the two-dimensional plane.

[0073] Step 6-3, when the type of the robot is a drone, the drone can move in a three-dimensional space, and the neighborhood of the current node can be expanded in the three-dimensional space;

[0074] Furthermore, the step of calculating the cost function f(n) in step 8 includes:

[0075] Step 8-1, calculate the actual cost function g(n), which represents the actual cost value from the starting point of the path to the current node;

[0076] Step 8-2, calculate the heuristic function h(n), and use the Euclidean distance to represent the estimated cost from the current node to the path target point;

[0077] Step 8-3, such as Figure 2 The penalty function ξ(n) is calculated as shown, which represents the penalty value from the current node to the path target point;

[0078] Among them, (x n ,y n ,z n) is the coordinate of the current node in the grid map, (x parent ,y parent ,z parent ) is the coordinate of the parent node of the current node in the grid map, (x n-1 ,y n-1 ,z n-1 ) is the coordinate of the parent node of the parent node in the grid map;

[0079] Furthermore, the specific calculation steps of calculating the penalty value of the penalty function in step 8-3 include:

[0080] Step 8-3-1, introduce two vectors n1, n2:

[0081] n1=(x n-1 -x parent ,y n-1 -y parent ,z n-1 -z parent ),n2=(x parent -x n ,y parent -y n ,z parent -z n )

[0082] Where n1 represents the vector from the parent node of the parent node to the parent node of the current node, and n2 represents the vector from the parent node to the current node;

[0083] Step 8-3-2, calculate the cosine value of the angle between the two vectors in step 8-3-1:

[0084]

[0085] Step 8-3-3, calculate the penalty value of the penalty function based on the cosine value result:

[0086]

[0087] When cosθ=1, the current node, the parent node, and the parent node of the parent node are on the same straight line, indicating that the planned path does not turn, no penalty is imposed, ξ(n)=0;

[0088] When cosθ≠1, and x n =x goal ,y n =y goal ,z n =z goal, indicating that the current node is the target node, and the path from its parent node to the target node has turned relative to the planned path, that is, the turning point turns to the end point. At this time, the penalty value is ξ(n) = a1;

[0089] When -1≤cosθ<1, and the current node is not the target point, it means that a turn occurs in the path planned to reach the target point, that is, a common turning point occurs. At this time, the penalty value is ξ(n)=a2. It should be noted that a2>a1 in this method.

[0090] Figure 3 In the Rviz software simulation running result diagram of this method in the Ubuntu environment, for this grid map, different values ​​of a1 and a2 are selected for debugging, and finally a1=0.2, a2=1 are determined. The results show that when the end point, starting point, and obstacle positions are the same, the path 6 planned by the A-star algorithm of the present invention has significantly fewer turning points than the path 5 planned by the traditional A-star algorithm, and the path is smoother.

[0091] In order to verify the effectiveness of this method, this embodiment selects different target points and uses the A-star algorithm of the present invention and the traditional A-star algorithm to perform path planning, and compares the planning time, the planned path length and the number of nodes explored in the planning. The data results are as follows: Figure 4 As shown in the figure, this method has obvious advantages over traditional planning methods in terms of the above evaluation indicators.

[0092] In a specific implementation, the present application provides a computer storage medium and a corresponding data processing unit, wherein the computer storage medium can store a computer program, and the computer program can run the invention content of a robot path planning method based on an A-star penalty control optimization algorithm provided by the present invention and some or all of the steps in each embodiment when executed by the data processing unit. The storage medium may be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc. Those skilled in the art can clearly understand that the technical solution in the embodiment of the present invention can be implemented by means of a computer program and its corresponding general hardware platform. Based on such an understanding, the technical solution in the embodiment of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a computer program, i.e., a software product, which can be stored in a storage medium, including several instructions to enable a device including a data processing unit (which can be a personal computer, a server, a single-chip microcomputer. MUU or a network device, etc.) to execute the methods described in each embodiment of the present invention or some parts of the embodiments.

[0093] The present invention provides a robot path planning method based on the A-star penalty control optimization algorithm. There are many methods and ways to implement the technical solution. The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the protection scope of the present invention. All components not specified in this embodiment can be implemented by existing technologies.

Claims

1. A robot path planning method based on A-star penalty control optimization algorithm, characterized in that: The following steps are involved: Step 1: Use sensors to collect surrounding environment map information to build a map of the robot's environment; Step 2, rasterize the map, determine the state of each grid as occupied or idle, and determine the location coordinates of obstacles; Step 3: Determine the coordinates of the path starting point (x start ,y start ,z start ) and the target point coordinates (x goal ,y goal ,z goal ); x start ,y start 、z start are the x, y, and z axis coordinates of the starting point respectively; goal ,y goal 、z goal The x, y, and z coordinate values ​​of the target point; Step 4: Initialize the open list and the close list, and set the coordinates of the path starting point (x start ,y start ,z start ) is put into the open list, and the closed list is empty; Step 5, check whether the open list is empty. If it is empty, it means that the path planning fails. If it is not empty, go to step 6; Step 6, ignore the obstacle nodes. If the close list is not empty, ignore the nodes in the close list, expand the current node, and obtain the neighboring nodes of the current node. Step 7: Determine whether the neighboring nodes of the current node are in the open list: If the neighboring nodes of the current node are not in the open list, then the neighboring nodes of the current node are expanded and added to the open list, and the current node is used as the parent node of the neighboring nodes, and the cost function of the neighboring nodes of the current node is calculated, that is, step 8 is executed; If the neighboring nodes of the current node are in the open list, and if the g(n) of the neighboring nodes of the current node is lower than the original one with the current node as the parent node, then the current node is taken as the parent node, and the cost function of the neighboring nodes of the current node is recalculated, that is, step 8 is executed, otherwise step 9 is executed; Step 8, calculate the cost function f(n) of the neighborhood node, where n represents the nth node; Step 9, select the point with the smallest cost function f(n) value in the open list as the current node to be expanded, delete the current node from the open list, and add it to the closed list; Step 10: Determine whether the target point is added to the open list: If the target point has been added to the open list, the path planning is completed, and the reverse solution from the target point to the starting point is the final path obtained by this robot path planning; If the target point is not in the open list, go to step 5; The calculation steps of calculating the cost function f(n) in step 8 include: Step 8-1, calculate the actual cost function g(n), which represents the actual moving cost from the starting point of the path to the current node, along the path generated to reach the current node; Step 8-2, calculate the heuristic function h(n); Step 8-3, calculating the penalty function ξ(n); Step 8-2 includes: using the Euclidean distance to represent the estimated cost value from the current node to the path target point, (x n ,y n ,z n ) is the coordinate of the current node in the grid map. The Euclidean distance represents the straight-line distance between two points. The formula is as follows: Step 8-3 includes: Step 8-3-1, introduce two vectors n1, n2: n1=(x n-1 -x parent ,y n-1 -y parent ,z n-1 -z parent ),n2=(x parent -x n ,y parent -y n ,z parent -z n ) Among them, (x n ,y n ,z n ) is the coordinate of the current node in the grid map, (x parent ,y parent ,z parent ) is the coordinate of the parent node of the current node in the grid map, (x n-1 ,y n-1 ,z n-1 ) is the coordinate of the parent node of the parent node in the grid map; n1 represents the vector from the parent node of the parent node to the parent node of the current node, and n2 represents the vector from the parent node to the current node; Step 8-3-2, calculate the cosine value of the angle θ between the two vectors n1 and n2: Step 8-3-3, calculate the value of the penalty function ξ(n) based on the cosine value result: a1 and a2 are adjustment thresholds; In step 8-3-3, when cosθ=1, the current node, the parent node, and the parent node of the parent node are on the same straight line, indicating that the planned path does not turn, no penalty is imposed, and ξ(n)=0; When cosθ≠1, and x n =x goal ,y n =y goal ,z n =z goal , indicating that the current node is the target node, and the path from the parent node of the current node to the target node has turned relative to the planned path, that is, the turning point turns to the end point. At this time, the penalty value is ξ(n) = a1; When -1≤cosθ<1, and the current node is not the target point, it means that a turn occurs in the path planned to reach the target point, that is, a common turning point occurs. At this time, the penalty value is ξ(n)=a2, where a2>a1.

2. The method according to claim 1, characterized in that The sensors described in step 1 include millimeter wave radar, laser radar, camera, inertial measurement unit IMU, and global positioning system GPS.

3. The method according to claim 2, characterized in that In step 6, the type of robot uses different expansion methods to expand the neighborhood of the current node, including: Step 6-1: When the robot type is an unmanned vehicle, first calculate the minimum turning radius R at the current speed. min , and then through Calculate the steering angle δ corresponding to the minimum turning radius at the current vehicle speed, divide the [-δ,δ] angle interval into i Δδ intervals in the current driving direction, and evenly expand the neighborhood of the current node. Step 6-2: When the robot type is an AGV, the neighborhood of the current node is uniformly expanded within the 360° direction of the two-dimensional plane; Step 6-3, when the type of the robot is a drone, the neighborhood of the current node is expanded in the three-dimensional space.

Images