Traffic cone robot path planning method and system based on dynamic coefficient adjustment

Abstract

The invention discloses a traffic cone robot path planning method and system based on dynamic coefficient adjustment, and the method comprises the steps: building a grid map through obtaining the environment information of a traffic cone robot; realizing global path planning and local path planning from the starting point to the target point of the traffic cone robot by using an A star algorithm and a dynamic window method respectively; in the global path planning search process, a dynamic weight coefficient changing along with the distance from the current node to the starting point is introduced to serve as a coefficient for estimating the cost to calculate the comprehensive cost of each node, the problem that an existing A star algorithm is low in search efficiency is solved, and the speed of obtaining the optimal path is increased; meanwhile, the coefficient of the distance evaluation function in the evaluation function is dynamically adjusted, so that the dynamic window method can adjust the speed of the traffic cone robot according to the distance between the position of the traffic cone robot and the obstacle, and the adaptability to the moving obstacle is remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular relates to a traffic cone robot path planning method and system based on dynamic coefficient adjustment. Background Art

[0002] Traffic cones are an important tool for traffic control, effectively dividing lanes, directing traffic flow, and reducing traffic accidents. However, traditional manual placement of traffic cones is inefficient, and improper placement can easily lead to safety accidents. Therefore, existing technologies use intelligent traffic cone robots that autonomously move and position traffic cones.

[0003] Path planning is a crucial step in the autonomous movement of a mobile traffic cone robot. It ensures the robot avoids collisions with obstacles, vehicles, and pedestrians, while also planning an efficient, safe, and optimal path. Path planning is divided into global and local path planning. Global path planning algorithms include the A-star algorithm, Dijkstra's algorithm, RRT algorithm, and artificial potential field method. Local path planning algorithms include DWA (dynamic window algorithm) and TEB (time elastic band algorithm).

[0004] The traditional A-star algorithm is the most effective direct search method for finding the shortest path in static networks. However, in dynamic or complex environments, the algorithm suffers from inefficient search and may even fail to find a path. The DWA algorithm is suitable for local path planning and achieves obstacle avoidance by finding the optimal solution in velocity space. Summary of the Invention

[0005] The object of the present invention is to provide a traffic cone robot path planning method and system based on dynamic coefficient adjustment.

[0006] In a first aspect, the present invention provides a traffic cone robot path planning method based on dynamic coefficient adjustment, comprising:

[0007] Obtain environmental information for the traffic cone robot; construct a grid map based on this environmental information and set the traffic cone robot's starting and destination points in the grid map; use the A-star algorithm to perform a global path planning search to obtain the shortest path from the starting point to the destination point; use the dynamic window method to perform local path planning based on the shortest path to complete the path planning for the traffic cone robot;

[0008] In the global path planning search process, a dynamic weight coefficient that changes with the distance from the current node to the starting point is introduced as a coefficient of the estimated cost to calculate the comprehensive cost of each node.

[0009] Preferably, the dynamic weight coefficient The method to obtain is as follows:

[0010]

[0011] in, and are the maximum and minimum values ​​of the weight respectively; k is the adjustment coefficient; For nodes Distance to the starting point; is the distance threshold.

[0012] As a preference, in local path planning, the coefficient of the distance evaluation function in the evaluation function is dynamically adjusted. , and the adjustment method is as follows:

[0013]

[0014] in, Indicates the current distance between the traffic cone robot and the obstacle; Indicates the obstacle distance within the maximum planning range; is the adjustment factor.

[0015] As a preferred method, the specific process of performing global path planning search is as follows:

[0016] Construct an open list and a closed list, and add the starting point to the open list; obtain the corresponding comprehensive cost based on the actual cost and estimated cost of each node in the open list; select the node with the smallest comprehensive cost from the open list as the current node, and transfer the current node from the open list to the closed list; use the current node as the parent node and obtain the adjacent nodes of the current node as child nodes; use the five-domain search method to filter out feasible nodes from the child nodes, and traverse the feasible nodes: if the feasible node is unreachable or already in the closed list, ignore the feasible node; calculate the actual cost and estimated cost of the remaining feasible nodes, and if the feasible node is not in the open list, add it to the open list; if the feasible node is already in the open list and the actual cost of reaching the feasible node through the current node is smaller, update the actual cost and parent node of the feasible node in the open list;

[0017] Repeatedly add the nodes in the open list to the closed list until the adjacent node of the current node is the target point, and the shortest path is obtained.

[0018] As a preferred method, the specific process of performing local path planning is as follows:

[0019] Construct a speed sampling space and kinematic model for the traffic cone robot, and simulate multiple motion trajectories generated in the next moment based on the current parameter prediction of the traffic cone robot. Use an evaluation function to perform standard evaluation on the generated multiple motion trajectories and move along the optimal trajectory. Repeat the above process until the traffic cone robot reaches the target point.

[0020] As a preferred option, the kinematic model of the traffic cone robot is constructed as follows:

[0021]

[0022] in, and are the horizontal and vertical displacements of the traffic cone robot at the current time t, respectively; and are the linear velocities of the traffic cone robot in the horizontal and vertical directions at the current time t; is the angle between the traffic cone robot and the horizontal direction; is the angular velocity of the traffic cone robot at the current time t.

[0023] As a preference, the evaluation function The method to obtain is as follows:

[0024]

[0025] in, is the azimuth evaluation function; is the distance evaluation function; is the speed evaluation function; are evaluation function coefficients.

[0026] Preferably, after obtaining the shortest path, a redundant node deletion strategy is used to optimize the path.

[0027] Preferably, the environmental information includes location information of the traffic cone robot, obstacles and target points.

[0028] In the second aspect, the present invention provides a traffic cone robot path planning system based on dynamic coefficient adjustment, which is used to execute the above-mentioned traffic cone robot path planning method; the traffic cone robot path planning system includes a data acquisition module, a global path planning module and a local path planning module; the data acquisition module is used to collect environmental information of the traffic cone robot; the global path planning module is used to obtain the optimal path of the traffic cone robot from the starting point to the target point according to the environmental information; the local path planning module is used to control the traffic cone robot to perform local movement according to the optimal path obtained by the global path planning module.

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

[0030] 1. The present invention adopts the A-star algorithm to perform global path planning for the traffic cone robot. By introducing a dynamic weight coefficient in the global path planning search process as a coefficient for estimating the cost to calculate the comprehensive cost of each node, the A-star algorithm can accelerate the search speed while selecting the optimal path, solving the problem of low search efficiency of the existing A-star algorithm and improving the speed of obtaining the optimal path.

[0031] 2. The present invention adopts a dynamic window method to perform local path planning for the traffic cone robot. By dynamically adjusting the coefficient of the distance evaluation function in the evaluation function during local path planning, the dynamic window method can adjust the speed of the traffic cone robot according to the distance between the traffic cone robot's own position and the obstacle, thereby significantly improving its adaptability to moving obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 It is the overall flow chart of the present invention.

[0033] Figure 2 Schematic diagram of adjacent child nodes of the current node in the present invention.

[0034] Figure 3 The figure compares the simulation results of the present invention and the traditional A-star algorithm under a simple grid map; wherein, (a) is a schematic diagram of the simulation results of the traditional A-star algorithm; (b) is a schematic diagram of the simulation results of the present invention.

[0035] Figure 4 The figure compares the simulation results of the present invention and the traditional A-star algorithm under complex grid maps; wherein, (a) is a schematic diagram of the simulation results of the traditional A-star algorithm; (b) is a schematic diagram of the simulation results of the present invention. DETAILED DESCRIPTION

[0036] The present invention will be further described below with reference to the accompanying drawings.

[0037] like Figure 1 As shown, a traffic cone robot path planning method based on dynamic coefficient adjustment is adopted. The traffic cone robot path planning system includes a data acquisition module, a global path planning module and a local path planning module; the data acquisition module is used to collect environmental information of the traffic cone robot; the global path planning module is used to obtain the optimal path of the traffic cone robot from the starting point to the target point according to the environmental information; the local path planning module is used to control the traffic cone robot to perform local movement according to the optimal path obtained by the global path planning module.

[0038] The traffic cone robot path planning method includes the following steps:

[0039] Step 1: Use a lidar or depth camera sensor to obtain information about the traffic cone robot's surroundings, including the locations of the robot, obstacles, and the target point. Use SLAM (Simultaneous Localization and Mapping) technology to construct a grid map and set the starting and target points for the traffic cone robot's path planning. Construct an open list (openList) and a closed list (closeList) and initialize them. The open list stores nodes to be explored, while the closed list stores nodes that have already been explored. Add the starting point to the open list and set it to the highest priority.

[0040] Step 2: Get each node in the open list separately The comprehensive cost , whose expression is:

[0041]

[0042] in, From the starting point to the node the actual cost; For nodes The estimated cost to reach the destination; is the dynamic weight coefficient.

[0043] The larger the dynamic weight coefficient, the faster the search speed but the optimal path will be missed; the smaller the dynamic coefficient, the search will choose the optimal path and slow down the speed. The expression is:

[0044]

[0045] in, and are the maximum and minimum values ​​of the weight respectively; k is the adjustment coefficient; For nodes Distance to the starting point; is the preset distance threshold.

[0046] Step 3: Select the comprehensive price from the open list The smallest node is used as the current node, and the current node is transferred from the open list to the closed list. Figure 2 As shown in the figure, a five-domain search method is used to filter out feasible nodes from child nodes. The specific process is as follows:

[0047] Take the current node as the parent node and obtain its eight adjacent nodes as child nodes. Label the child node pointed to by the positive Y-axis as child node 1. Then, in a clockwise direction, label the remaining child nodes as child node 2, child node 3, child node 4, child node 5, child node 6, child node 7, and child node 8. Based on the angle α formed by the vector pointing to the target point from the current node and the positive Y-axis, select a feasible node from the child nodes, as shown in Table 1.

[0048] Table 1 Child nodes added to the open list

[0049]

[0050] The child nodes corresponding to the search direction retained in Table 1 are regarded as feasible nodes. If the feasible node is unreachable or already in the closed list, ignore it. Calculate the actual cost of all feasible nodes (excluding the ignored feasible nodes) and estimated cost , and determine whether all feasible nodes are already in the open list. If the feasible node is not in the open list, add it to the open list; if the feasible node is already in the open list, and the actual cost of reaching the feasible node through the current node is If it is smaller, the actual cost and parent node of the feasible node in the open list are updated.

[0051] Step 4: Repeat steps 2 and 3 until a feasible node in the open list is added as the target node. From the target node, trace back along the parent node until you reach the starting point. This will give you the shortest path.

[0052] Step 5: Optimize the path using a redundant node removal strategy. Starting from the starting point, select three adjacent nodes from the nodes on the shortest path. If the line connecting the first and third of the three adjacent nodes does not pass through an obstacle, remove the second node.

[0053] Step 6: Use the dynamic window algorithm (DWA) to perform local path planning. The specific process is as follows:

[0054] 6-1. Speed ​​sampling

[0055] (1) According to the hardware conditions and environmental limitations of the traffic cone robot, the speed of the traffic cone robot has boundary restrictions. At this time, the speed space that can be sampled is for:

[0056]

[0057] in, is the linear velocity of the traffic cone robot; is the angular velocity of the traffic cone robot; and are the minimum and maximum linear speeds of the traffic cone robot, respectively; and are the minimum angular velocity and maximum angular velocity of the traffic cone robot respectively.

[0058] (2) Due to the limitation of the driving motor of the traffic cone robot, the linear acceleration and angular acceleration of the traffic cone robot are both subject to boundary limits. If the maximum acceleration and deceleration are set to the same value, the speed space that can be sampled during acceleration is for:

[0059]

[0060] in, and are the linear velocity and angular velocity of the traffic cone robot at the current moment respectively; and are the maximum linear acceleration and maximum angular acceleration of the traffic cone robot, respectively; For the interval time.

[0061] (3) Local planning also requires dynamic and real-time obstacle avoidance. Considering the obstacles around the traffic cone robot, the speed space that can be sampled without colliding with the surrounding obstacles is: for:

[0062]

[0063] in, is the distance evaluation function, which represents the shortest distance between the corresponding simulation trajectory and the obstacle at the current speed.

[0064] (4) Combining the above three types of speed limits, the final traffic cone robot speed sampling space is the intersection of three velocity spaces, namely:

[0065] 6-2. Based on the current parameters of the traffic cone robot, multiple motion trajectories generated in the next moment are predicted and simulated; if the trajectory is regarded as a continuous arc or straight line, it can be used Represents the motion trajectory of the traffic cone robot. The kinematic model of the traffic cone robot is constructed as follows:

[0066]

[0067] in, and are the horizontal and vertical displacements of the traffic cone robot at the current time t, respectively; and are the linear velocities of the traffic cone robot in the horizontal and vertical directions at the current time t; is the angle between the traffic cone robot and the horizontal direction; is the angular velocity of the traffic cone robot at the current time t.

[0068] 6-3. Use the evaluation function to perform a standard evaluation on the multiple motion trajectories generated, select a set of optimal trajectories, and the robot moves along the optimal trajectory. In order to adapt to complex environments more flexibly, an adaptive coefficient is introduced into the evaluation function. By adjusting the weights of different parameters, the traffic cone robot can respond to changes more intelligently. The constructed evaluation function Expressed as:

[0069]

[0070] in, is the azimuth evaluation function, which is used to evaluate the angle error between the trajectory endpoint position direction generated at the current sampling speed and the node direction in the shortest path; is the speed evaluation function, which indicates the current speed; are evaluation function coefficients.

[0071] In order to increase the speed of the traffic cone robot when the simulated trajectory is far away from the obstacle, the evaluation function coefficient is dynamically adjusted. , whose expression is:

[0072]

[0073] in, Indicates the current distance between the traffic cone robot and the obstacle; Indicates the obstacle distance within the maximum planning range; is the adjustment factor.

[0074] 6-4. Repeat the above process until the traffic cone robot reaches the target point.

[0075] Step 7: Use MATLAB software to conduct simulation experiments on the present invention and the traditional A-star algorithm in different simulation environments to verify the feasibility of the algorithm. The simulation results under the simulation environment of a simple grid map are shown in Table 2 and Figure 3 shown.

[0076] Table 2 Simple grid map

[0077]

[0078] As can be seen from Table 2, compared with the traditional A-star algorithm, the path length of the present invention is reduced by 23.5782%, and the search time is reduced by 20.8795%.

[0079] The simulation results under the simple grid map environment are shown in Table 3 and Figure 4 shown.

[0080] Table 3 Complex grid map

[0081]

[0082] As can be seen from Table 3, compared with the traditional A-star algorithm, the path length of the present invention is reduced by 24.0556%, and the search time is reduced by 6.54691%.

Claims

1. A traffic cone robot path planning method based on dynamic coefficient adjustment, characterized in that: The method includes: Obtain environmental information for the traffic cone robot; construct a grid map based on this environmental information and set the traffic cone robot's starting and destination points in the grid map; use the A-star algorithm to perform a global path planning search to obtain the shortest path from the starting point to the destination point; use the dynamic window method to perform local path planning based on the shortest path to complete the path planning for the traffic cone robot; In the global path planning search process, a dynamic weight coefficient that decreases with the distance from the current node to the starting point is introduced as a coefficient of the estimated cost to calculate the comprehensive cost of each node.

2. The traffic cone robot path planning method based on dynamic coefficient adjustment according to claim 1, characterized in that: The dynamic weight coefficient The method to obtain is as follows: in, and are the maximum and minimum values ​​of the weight respectively; k is the adjustment factor; For nodes Distance to the starting point; is the distance threshold.

3. The traffic cone robot path planning method based on dynamic coefficient adjustment according to claim 1, characterized in that: In local path planning, dynamically adjust the coefficient of the distance evaluation function in the evaluation function , and the adjustment method is as follows: in, Indicates the current distance between the traffic cone robot and the obstacle; Indicates the obstacle distance within the maximum planning range; is the adjustment factor.

4. The traffic cone robot path planning method based on dynamic coefficient adjustment according to claim 1, characterized in that: The specific process of global path planning search is as follows: Construct an open list and a closed list, and add the starting point to the open list; obtain the corresponding comprehensive cost based on the actual cost and estimated cost of each node in the open list; Select the node with the smallest comprehensive cost from the open list as the current node, and transfer the current node from the open list to the closed list; Take the current node as the parent node and get the adjacent nodes of the current node as child nodes; A five-domain search method is used to filter out feasible nodes from child nodes, and the feasible nodes are traversed: if the feasible node is unreachable or already in the closed list, the feasible node is ignored; Calculate the actual cost and estimated cost of the remaining feasible nodes. If the feasible node is not in the open list, add it to the open list. If the feasible node is already in the open list and the actual cost of reaching the feasible node through the current node is smaller, then update the actual cost and parent node of the feasible node in the open list; Repeatedly add the nodes in the open list to the closed list until the adjacent node of the current node is the target point, and the shortest path is obtained.

5. The traffic cone robot path planning method based on dynamic coefficient adjustment according to claim 1, characterized in that: The specific process of local path planning is as follows: Construct a speed sampling space and kinematic model for the traffic cone robot, and simulate multiple motion trajectories generated in the next moment based on the current parameter prediction of the traffic cone robot. Use an evaluation function to perform standard evaluation on the generated multiple motion trajectories and move along the optimal trajectory. Repeat the above process until the traffic cone robot reaches the target point.

6. The traffic cone robot path planning method based on dynamic coefficient adjustment according to claim 5, characterized in that: The constructed kinematic model of the traffic cone robot is as follows: in, and The current moment t The horizontal and vertical displacements of the lower traffic cone robot; and The current moment t Linear velocity of the lower traffic cone robot in the horizontal and vertical directions; is the angle between the traffic cone robot and the horizontal direction; For the current moment t Angular velocity of the traffic cone robot.

7. The traffic cone robot path planning method based on dynamic coefficient adjustment according to claim 5, characterized in that: The evaluation function The method to obtain is as follows: in, is the azimuth evaluation function; is the distance evaluation function; is the speed evaluation function; are evaluation function coefficients.

8. The traffic cone robot path planning method based on dynamic coefficient adjustment according to claim 1, characterized in that: After obtaining the shortest path, the redundant node removal strategy is used to optimize the path.

9. The traffic cone robot path planning method based on dynamic coefficient adjustment according to claim 1, characterized in that: The environmental information includes the location information of the traffic cone robot, obstacles and target points.

10. A traffic cone robot path planning system based on dynamic coefficient adjustment, characterized by: Used to execute the traffic cone robot path planning method based on dynamic coefficient adjustment as described in claim 1; the traffic cone robot path planning system includes a data acquisition module, a global path planning module and a local path planning module; the data acquisition module is used to collect environmental information of the traffic cone robot; the global path planning module is used to obtain the optimal path of the traffic cone robot from the starting point to the target point based on the environmental information; the local path planning module is used to control the traffic cone robot to perform local movement according to the optimal path obtained by the global path planning module.

Images