Outdoor robot path planning method based on improved bidirectional A* and DWA algorithms

Abstract

The invention discloses an outdoor robot path planning method based on improved bidirectional A * and DWA algorithms. The method comprises the following steps: step 1, A * algorithm improvement; step 2, DWA algorithm improvement; step 3, algorithm fusion; step 4, path planning; the dynamic weight DOMW strategy is introduced into the A * algorithm to optimize a heuristic function, multiple factors are comprehensively considered, path planning is more reasonable, the algorithm calculation amount and search time are reduced by removing redundant nodes, the algorithm efficiency is improved, a bidirectional search mechanism is adopted, and the algorithm efficiency is improved. The search efficiency is improved, the search space is reduced, a local optimal solution is avoided, the robustness of the algorithm is enhanced, the path is smoothed by adopting a Bezier curve optimization strategy, so that the path meets the actual motion requirement of the robot, the damage to hardware is reduced, and the attractiveness and readability of the path are improved; by optimizing a DWA algorithm trajectory evaluation function and speed space constraints, the collision risk is reduced, and local obstacle avoidance and operation stability are improved.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to an outdoor robot path planning method based on improved bidirectional A* and DWA algorithms. Background Art

[0002] Path planning is a core technology for autonomous navigation in mobile robots. Effective path planning is crucial for robots operating in complex traffic environments. In existing technologies, path planning algorithms encompass both global and local aspects. The A* algorithm, a classic global planning algorithm, can quickly find the optimal path in a known environment, but suffers from issues such as an uneven path, numerous turns, low motion efficiency, and an inability to adjust the path in real time in the face of dynamic obstacles. The DWA algorithm, a commonly used local planning algorithm, can dynamically avoid obstacles based on real-time environmental information. However, relying solely on local information for decision-making can easily lead to local optimal solutions and prevent the target from being reached. Existing related technologies, such as patent application number 202411166466.9, propose a path planning method for unmanned logistics vehicles based on the fusion of an improved A* and an improved DWA. This patent optimizes the search and heuristic functions to reduce computational complexity and redundancy. The improved DWA algorithm enhances the unmanned logistics vehicle's obstacle avoidance capabilities in complex environments by adding a dynamic obstacle assessment submodule. However, the algorithm still suffers from long search times and poor obstacle avoidance effectiveness on complex and narrow roads. Existing related technologies, such as the patent with application number 202411092855.1, propose a path planning method for improving the bidirectional A* algorithm. This patent focuses on improving the efficiency of path search through bidirectional A* search, reducing the inflection points of the generated path, and shortening the length of the generated path. However, it only optimizes the bidirectional A*, so there are problems such as lack of local obstacle avoidance capability, poor adaptability to complex environments, and lack of algorithm fusion, which limits its application effect in complex scenarios. Summary of the Invention

[0003] The object of the present invention is to provide an outdoor robot path planning method based on improved bidirectional A* and DWA algorithms to solve the problems raised in the above background technology.

[0004] To achieve the above objectives, the present invention provides the following technical solution: an outdoor robot path planning method based on improved bidirectional A* and DWA algorithms, comprising the following steps: step 1, improving the A* algorithm; step 2, improving the DWA algorithm; step 3, algorithm fusion; step 4, path planning;

[0005] The above step 1 specifically includes the following steps:

[0006] 1.1 Heuristic function optimization: Adopting the dynamic weight DOMW strategy, the heuristic function of the A* algorithm is optimized by comprehensively considering distance, obstacles and map factors;

[0007] 1.2 Neighborhood Search Optimization: Based on the A* algorithm, a search strategy of four neighborhoods plus a bilaterally symmetric convex neighborhood is adopted. When expanding the child nodes of the current node n, the four nodes in the four neighborhood expansion directions of the current node are first judged. If any of the nodes has an obstacle or is a path node, the four-neighborhood expansion strategy is adopted. If there is no obstacle or the target node of the path, the bilaterally symmetric convex neighborhood expansion strategy is adopted.

[0008] 1.3 Remove redundant nodes: Extract path nodes from the initial path. Starting from the starting point, determine whether there are obstacles between adjacent nodes. If not, delete the node and update the path node. If so, keep the previous node, continue to search for the next node and update the path until the initial path node sequence is traversed.

[0009] 1.4 Bidirectional Search: Forward search starts from the starting point S, and reverse search starts from the target point T. During the search process, the forward search and reverse search respectively maintain their own open lists. Each time, the node with the smallest evaluation function value is selected from the open list for expansion. The obstacle information in the map is taken into account when calculating the actual cost and the heuristic estimated cost. When there is a common node or the open list in a certain direction is empty, the search terminates and the forward and reverse paths are connected to obtain the initial global path.

[0010] 1.5 Path smoothing: Using the Bezier curve optimization strategy, the path node sequence obtained by bidirectional A* is optimized;

[0011] The above step 2 specifically includes the following steps:

[0012] 2.1 Establishing a mathematical model of motion: Based on the motion characteristics of the robot's differential structure, a mathematical model is established to describe the motion trajectory of the robot at adjacent moments, including changes in position, speed, and angle;

[0013] 2.2 Optimize trajectory evaluation function and speed space constraints: Design a comprehensive trajectory evaluation function for the DWA algorithm. For the azimuth evaluation function, analyze the target motion trend and calculate the dynamic azimuth deviation weight. For the distance evaluation function, comprehensively consider the distance between the car and the nearest obstacle, the obstacle shape, size, and distribution density. For the speed evaluation function, consider the speed corresponding to the trajectory and combine it with the robot's power system efficiency curve and the remaining battery power. For the weight coefficient, use an adaptive dynamic adjustment strategy to adjust the weight coefficient, and update it in real time according to environmental characteristics and robot performance indicators. For the speed space constraint, in addition to considering the robot's own speed and angular velocity limitations, also introduce stability constraints based on the robot's physical parameters and dynamic model;

[0014] In the above step 3, the path nodes planned by the improved A* algorithm in step 2 are used as the local target points of the improved DWA algorithm in step 3, thereby realizing the fusion of the improved A* algorithm and the improved DWA algorithm;

[0015] In the above step 4, the initialization operation is first performed to input map information, starting point and target point, and set the parameters of the improved A* algorithm and improved DWA algorithm. Then, the improved A* algorithm is used to search for the optimal path from the starting point to the target point on the map. The improved A* algorithm determines the optimal path node sequence by evaluating the node cost and heuristic function. The path nodes planned by the improved A* algorithm are used as local target points of the DWA algorithm in sequence. The improved DWA algorithm calculates the speed and steering instructions at the next moment based on the current position, speed and surrounding environment information of the robot, so that the robot can move towards the local target point and avoid obstacles in real time. If an obstacle is detected or the path is infeasible, the improved DWA algorithm feeds the result back to the improved A* algorithm, and the improved A* algorithm replans the local path. The improved DWA algorithm then adjusts the robot's motion trajectory accordingly, and repeats this feedback and adjustment process until the robot reaches the target point.

[0016] Preferably, in step 1.1, the specific algorithm formula is as follows:

[0017] f(n)=g(n)+DOMW*h(n)

[0018]

[0019] D r,c =log r (r*c)

[0020]

[0021] Among them, f(n) is the comprehensive cost of node n, g(n) is the actual cost from the starting point to the current node, h(n) is the estimated cost from the current node to the target point, DOMW is the dynamic weight, P now,around is an obstacle, P now The number of obstacles in the search direction of the current node 24 neighborhood, Q now,g is the Euclidean distance from the current node to the target point, D r,c is the map weight; when the node is far from the target point, the effect of the heuristic function is increased to speed up the node search; when the node is close to the target point, the effect of the heuristic function is reduced to expand the range of the search node.

[0022] Preferably, in step 1.2, the specific algorithm formula is as follows:

[0023] For the grid map established by the Karto algorithm, assume that a position (x, y) in the map has four neighbors: for node n (x n ,y n ), its four neighboring nodes are n up (x n ,y n +1), n down (x n ,y n -1), n left (x n -1,y n ), n right (x n +1,y n ); bilaterally symmetric convex neighborhood: in addition to the four neighboring nodes, it also includes n leftup (x n -1,y n +1), n leftdown (x n -1,y n -1), n rightup (x n +1,y n +1), n rightdown (x n +1,y n -1), n right2 (x n +2,y n ), n left2 (x n -2,y n ); The inaccessible area in the map is represented by set O. If a node (x, y)∈O, it means that the node is an obstacle node. The path node is represented by set P, and the target node is represented by G(x g ,y g ); If the target node is in the fourth quadrant, which is the lower right corner of the current node, {n down (x n ,y n -1), n right (x n +1,y n ), n rightdown (x n +1,y n -1), n right2 (x n +2,y n )}, if the target node is in the upper right corner of the current node, that is, in the first quadrant, {n up (x n ,y n +1), n right (xn +1,y n ), n rightdown (x n +1,y n +1), n right2 (x n +2,y n )}, if the target node is in the upper left of the current node, that is, in the second quadrant, {n up (x n ,y n +1), n left (x n -1,y n ), n leftup (x n -1,y n +1), n left2 (x n -2,y n )}, if the target node is in the lower left of the current node, that is, in the third quadrant, {n down (x n ,y n -1), n left (x n -1,y n ), n leftdown (x n -1,y n -1), n left2 (x n -2,y n )}.

[0024] Preferably, in step 1.3, the specific algorithm formula is as follows:

[0025] Assume that in the two-dimensional grid map coordinate system established by the Karto algorithm, nodes n1(x1,y1) and n2(x2,y2) are adjacent nodes, and the four boundary points of the obstacle are For nodes n1 and n2, construct the vector For each obstacle boundary point p i , construct the vector and Calculate the vector cross product and the calculation formula is:

[0026]

[0027] If for all i, and If the signs of are the same, that is, both positive or negative, it means that the four boundary points of the obstacle are on the same side of n1 and n2, that is, the obstacle is not crossed; otherwise, it is considered that the obstacle is crossed; in the process of path node update, the initial path node sequence is {N1, N2, ..., N m}, starting from the starting point N1, determine the adjacent nodes N in turn i and N i+1 Is there an obstacle? If there is no obstacle, that is, the above same-side judgment condition is met, then delete N i+1 , and update the path node sequence to {N1,N2,…N i ,N i+2 ,...,N m}; If there is an obstacle, keep N i , continue to search for the next node N i+2 And update the path node sequence until the entire initial path node sequence is traversed.

[0028] Preferably, in step 1.4, the specific algorithm formula is as follows:

[0029] In the forward search, the evaluation function of node n is:

[0030] f forward (n) = g forward (n)+DOWN*h forward (n)

[0031] where g forward (n) is the actual cost from the starting point S to the node n. Assume that the edge cost from the adjacent node m to the node n is c(m,n). If the node n is the direct successor node of the starting point S, then g forward (n) = c(S,n); if node n is expanded from node m, then g forward (n) = g forward (m)+c(m,n),h forward (n) is the heuristic estimated cost from node n to target point T. The robot uses Euclidean distance as the heuristic function, that is, Where (x n ,y n ) is the coordinate of node n, (x T ,y T ) is the coordinate of the target point T;

[0032] In the reverse search, the evaluation function for node n is:

[0033] f backward (n) = g backward (n)+DOMW*h backward (n)

[0034] where g backward (n) is the actual cost from the target point T to the node n. The calculation method is similar to the forward direction, but the direction is opposite. If the node n is the direct predecessor node of the target point T, then g backward (n) = c(T,n); if node n is extended from node m, then g backward (n) = g backward (m)+c(m,n);h backward (n) is the heuristic estimated cost from node n to the starting point S, also using Euclidean distance;

[0035] For the grid map established by the Karto algorithm, when calculating the edge cost c(m,n), it is necessary to consider the obstacle information in the map. If there is an obstacle on the path from node m to node n, then c(m,n) = ∞, indicating that this path is impassable; if there is no obstacle, the cost is determined based on the resolution of the map and the robot's motion model. If the robot moves in an unobstructed straight line, the cost is unity, and the cost of diagonal movement is zero. The value of c(m,n) is determined according to the relative positions of nodes m and n. During the search process, the forward open list OpenList is maintained at the same time. forward and reverse open list OpenList backward , every time from OpenList forward Select f forward (n) The smallest node is expanded and the node is added from OpenList forward Select f backward (n) The smallest node is expanded; the termination condition is: when there is a node n in the OpenList forward and OpenList backward or when OpenList forward or OpenList backward When it is empty, the search terminates. If the search terminates successfully, that is, there is a common node n, then the forward path from the starting point to n and the reverse path from the target point to n are connected to form the final path.

[0036] Preferably, in step 1.5, the specific algorithm formula is as follows:

[0037] The cubic Bezier curve is:

[0038]

[0039] Where t∈[0,1], j represents the starting index of the node segment currently being processed, In order to better adapt to the environment and ensure the path quality, the weight factor w is introduced. i, which is related to the environment information around the node, that is, if the node w i If there are many obstacles around, w i Smaller to reduce the influence of the node on the curve shape, the improved Bezier curve formula is:

[0040]

[0041] Preferably, in step 2.1, the specific algorithm formula is as follows: Due to the limitation of the differential structure, the robot moves forward and rotates intelligently, the forward movement is the x-axis velocity v, and the rotation is the z-axis angular velocity w. Since the distance moved by the robot is short in adjacent moments, the motion trajectory between two adjacent points can be regarded as a straight line. The mathematical model of the robot is as follows:

[0042]

[0043] Among them: Δs represents how far the car has traveled, Δx and Δy represent the x and y coordinates of the car respectively, x, y, θ t Indicates the position of the car in the next state.

[0044] Preferably, in step 2.2, the DWA trajectory evaluation function is as follows:

[0045] G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω))

[0046] Among them, σ is the scaling factor, heading(v,ω) is the azimuth evaluation function, α is its weight coefficient, dist(v,ω) is the distance evaluation function, β is its weight coefficient, velocity(v,ω) is the speed evaluation function, and γ is its weight coefficient.

[0047] Preferably, in step 2.2, for the heading evaluation function heading(v,ω), not only the simple angle difference between the car and the target is considered, but also the movement trend of the target is further analyzed. By predicting the position change of the target in several future time steps, combined with the current position and speed of the robot, a dynamic heading deviation weight is calculated. If the target is moving rapidly away from the robot, then in the heading evaluation, the trajectory close to the current movement direction of the target will obtain a higher weight. The calculation formula is:

[0048]

[0049] where d i is the distance from the robot to the predicted target position, θ t is the robot's current azimuth, is the azimuth of the target at position wi It is a weight coefficient that is dynamically adjusted according to the target speed and distance. In the distance evaluation function, in addition to considering the distance between the car and the nearest obstacle, the shape, size and distribution density of the obstacle are also considered. For irregularly shaped obstacles, a polygonal approximation method is used to calculate their equivalent influence range. At the same time, an obstacle congestion factor is introduced according to the distribution density of the surrounding obstacles. When the obstacles around the robot are densely distributed, it is more inclined to choose a trajectory away from the highly congested area. The distance evaluation function dist(v,ω) is:

[0050]

[0051] where d min is the distance to the nearest obstacle, S j is the projection area of the j-th obstacle in the direction of robot movement, S total is the sum of the projected areas of all obstacles, and m is the number of obstacles within the robot's perception range. For the velocity evaluation function velocity(v,ω), not only the speed corresponding to the trajectory is considered, but also the robot's power system efficiency curve and the remaining battery power are combined. According to the energy consumption model of the power system under different speed and angular velocity combinations, the energy consumption cost E(v,ω) of each trajectory is calculated, and the remaining battery power B is also considered. remain Regarding the speed selection restriction, when the battery power is low, it is more inclined to choose a speed trajectory with lower energy consumption, so the speed evaluation function is:

[0052]

[0053] Among them E max is the maximum energy consumption when the battery is fully charged, B total is the total capacity of the battery;

[0054] Preferably, in step 2.2, for the speed space constraint, in addition to considering the robot's own maximum linear velocity v MAX , maximum angular velocity ω MAX , minimum linear velocity v MIN and the minimum angular velocity ω MIN Constraint, that is, the basic speed constraint V A , motion continuity constraint V B and obstacle avoidance related constraints V C , we also consider the speed limit of the robot's mechanical structure stability, that is, the stability constraint V D , the specific formula is as follows:

[0055] V A ={(v,ω)|v∈[v MIN ,v MAX ]∩ω∈[ω MIN,ω MAX ]}

[0056] V B ={(v,ω)|v∈[v1+v2·Δt,v1-v3·Δt]∩ω∈[ω1+ω2·Δt,ω1-ω3·Δt]

[0057]

[0058] V D ={(v,ω)|v≤f stability (v,ω)}

[0059] Among them, f stability (v,ω) is a stability function calculated based on parameters such as the robot's center of gravity, wheelbase, and friction coefficient, and is used to limit the speed range under different motion states.

[0060] Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention introduces a dynamic weight DOMW strategy into the A* algorithm to optimize the heuristic function, comprehensively considers multiple factors, makes path planning more reasonable, reduces the algorithm calculation amount and search time by removing redundant nodes, improves the efficiency of the algorithm, adopts a two-way search mechanism, improves search efficiency, reduces the search space, avoids local optimal solutions, and enhances the robustness of the algorithm, adopts a Bezier curve optimization strategy to smooth the path, makes the path meet the actual movement requirements of the robot, reduces damage to the hardware, and improves the aesthetics and readability of the path; by optimizing the DWA algorithm trajectory evaluation function and speed space constraints, the collision risk is reduced, and local obstacle avoidance and operation stability are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 This is a flowchart of the improved A* algorithm of the present invention;

[0062] Figure 2 A* algorithm comparison chart; Figure 2 (a) is the traditional eight-neighborhood search graph, Figure 2 (b) is the search graph of the search strategy using ODMW strategy + direction judgment + four neighborhoods + left-right symmetric convex neighborhood. Figure 2 (c) for Figure 2 (b) Based on the search graph with the bidirectional strategy, Figure 2 (d) is the improved A* algorithm search graph of the present invention;

[0063] Figure 3 To improve the A* algorithm and integrate it with the DWA algorithm simulation diagram;

[0064] Figure 4 To improve the A* algorithm and integrate it with the DWA algorithm, simulate the data curve; Figure 4 (a) is the simulation angle, Figure 4 (b) is the simulated angular velocity, Figure 4 (c) is the simulation speed;

[0065] Figure 5 This is the ROS experiment verification diagram; Figure 5 (a) is the initial state, Figure 5 (b) is the initial path, Figure 5 (c) is the path from bend 2 to the end point, and 5(d) is the path from bend 3 to the end point;

[0066] Figure 6 Flow chart of the method of the present invention. DETAILED DESCRIPTION

[0067] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0068] Please see the attached Figure 1 -Attached Figure 6 The present invention provides an embodiment of an outdoor robot path planning method based on improved bidirectional A* and DWA algorithms, comprising the following steps: step 1, A* algorithm improvement; step 2, DWA algorithm improvement; step 3, algorithm fusion; step 4, path planning;

[0069] The above step 1 specifically includes the following steps:

[0070] 1.1 Heuristic function optimization: Using the dynamic weight DOMW strategy, the heuristic function of the A* algorithm is optimized by comprehensively considering distance, obstacles, and map factors. The specific algorithm formula is as follows:

[0071] f(n)=g(n)+DOMW*h(n)

[0072]

[0073] D r,c =log r (r*c)

[0074]

[0075] Among them, f(n) is the comprehensive cost of node n, g(n) is the actual cost from the starting point to the current node, h(n) is the estimated cost from the current node to the target point, DOMW is the dynamic weight, P now,around is an obstacle, P nowThe number of obstacles in the search direction of the current node 24 neighborhood, Q now,g is the Euclidean distance from the current node to the target point, D r,c is the map weight; when the node is far from the target point, the effect of the heuristic function is increased to speed up the node search; when the node is close to the target point, the effect of the heuristic function is reduced to expand the range of the search node;

[0076] 1.2 Neighborhood search optimization: Based on the A* algorithm, a search strategy of four neighborhoods plus a left-right symmetrical convex neighborhood is adopted. When expanding the child nodes of the current node n, the four nodes in the four neighborhood expansion directions of the current node are first judged. If any of the nodes has an obstacle or is a path node, the four-neighborhood expansion strategy is adopted. If there is no obstacle or the target node of the path, the left-right symmetrical convex neighborhood expansion strategy is adopted. The specific algorithm formula is as follows:

[0077] For the grid map established by the Karto algorithm, assume that a position (x, y) in the map has four neighbors: for node n (x n ,y n ), its four neighboring nodes are n up (x n ,y n +1), n down (x n ,y n -1), n left (x n -1,y n ), n right (x n +1,y n ); bilaterally symmetric convex neighborhood: in addition to the four neighboring nodes, it also includes n leftup (x n -1,y n +1), n leftdown (x n -1,y n -1), n rightup (x n +1,y n +1), n rightdown (x n +1,y n -1), n right2 (x n +2,y n ), n left2 (x n -2,y n ); The inaccessible area in the map is represented by set O. If a node (x, y)∈O, it means that the node is an obstacle node. The path node is represented by set P, and the target node is represented by G(x g,y g ); If the target node is in the fourth quadrant, which is the lower right corner of the current node, {n down (x n ,y n -1), n right (x n +1,y n ), n rightdown (x n +1,y n -1), n right2 (x n +2,y n )}, if the target node is in the upper right corner of the current node, that is, in the first quadrant, {n up (x n ,y n +1), n right (x n +1,y n ), n rightdown (x n +1,y n +1), n right2 (x n +2,y n )}, if the target node is in the upper left of the current node, that is, in the second quadrant, {n up (x n ,y n +1), n left (x n -1,y n ), n leftup (x n -1,y n +1), n left2 (x n -2,y n )}, if the target node is in the lower left of the current node, that is, in the third quadrant, {n down (x n ,y n -1), n left (x n -1,y n ), n leftdown (x n -1,y n -1), n left2 (x n -2,y n )};

[0078] 1.3 Remove redundant nodes: Extract path nodes from the initial path. Starting from the starting point, determine whether there is an obstacle between adjacent nodes. If not, delete the node and update the path node. If so, retain the previous node, continue to search for the next node and update the path until the initial path node sequence is traversed. The specific algorithm formula is as follows:

[0079] Assume that in the two-dimensional grid map coordinate system established by the Karto algorithm, nodes n1(x1,y1) and n2(x2,y2) are adjacent nodes, and the four boundary points of the obstacle are For nodes n1 and n2, construct the vector For each obstacle boundary point p i , construct the vector and Calculate the vector cross product and the calculation formula is:

[0080]

[0081] If for all i, and If the signs of are the same, that is, both positive or negative, it means that the four boundary points of the obstacle are on the same side of n1 and n2, that is, the obstacle is not crossed; otherwise, it is considered that the obstacle is crossed; in the process of path node update, the initial path node sequence is {N1, N2, ..., N m}, starting from the starting point N1, determine the adjacent nodes N in turn i and N i+1 Is there an obstacle? If there is no obstacle, that is, the above same-side judgment condition is met, then delete N i+1 , and update the path node sequence to {N1,N2,…N i ,N i+2 ,...,N m}; If there is an obstacle, keep N i , continue to search for the next node N i+2 And update the path node sequence until the entire initial path node sequence is traversed;

[0082] 1.4 Bidirectional Search: Forward search starts from the starting point S, and reverse search starts from the target point T. During the search process, the forward search and reverse search respectively maintain their own open lists. Each time, the node with the smallest evaluation function value is selected from the open list for expansion. The obstacle information in the map is considered when calculating the actual cost and the heuristic estimated cost. When there is a common node or the open list in a certain direction is empty, the search terminates and the forward and reverse paths are connected to obtain the initial global path. The specific algorithm formula is as follows:

[0083] In the forward search, the evaluation function of node n is:

[0084] f forward (n) = g forward (n)+DOMW*h forward (n)

[0085] where g forward (n) is the actual cost from the starting point S to the node n. Assume that the edge cost from the adjacent node m to the node n is c(m,n). If the node n is the direct successor node of the starting point S, then g forward (n) = c(S,n); if node n is expanded from node m, then g forward (n) = g forward (m)+c(m,n),h forward (n) is the heuristic estimated cost from node n to target point T. The robot uses Euclidean distance as the heuristic function, that is, Where (x n ,y n ) is the coordinate of node n, (x T ,y T ) is the coordinate of the target point T;

[0086] In the reverse search, the evaluation function for node n is:

[0087] f backward (n) = g backward (n)+DOMW*h backward (n)

[0088] where g backward (n) is the actual cost from the target point T to the node n. The calculation method is similar to the forward direction, but the direction is opposite. If the node n is the direct predecessor node of the target point T, then g backward (n) = c(T,n); if node n is extended from node m, then g backward (n) = g backward (m)+c(m,n);h backward (n) is the heuristic estimated cost from node n to the starting point S, also using Euclidean distance;

[0089] For the grid map established by the Karto algorithm, when calculating the edge cost c(m,n), it is necessary to consider the obstacle information in the map. If there is an obstacle on the path from node m to node n, then c(m,n) = ∞, indicating that this path is impassable; if there is no obstacle, the cost is determined based on the resolution of the map and the robot's motion model. If the robot moves in an unobstructed straight line, the cost is unity, and the cost of diagonal movement is zero. The value of c(m,n) is determined according to the relative positions of nodes m and n. During the search process, the forward open list OpenList is maintained at the same time.forward and reverse open list OpenList backward , every time from OpenList forward Select f forward (n) The smallest node is expanded and the node is added from OpenList forward Select f backward (n) The smallest node is expanded; the termination condition is: when there is a node n in the OpenList forward and OpenList backward or when OpenList forward or OpenList backward When it is empty, the search terminates. If the search terminates successfully, that is, there is a common node n, then the forward path from the starting point to n and the reverse path from the target point to n are connected to form the final path;

[0090] 1.5 Path smoothing: The Bezier curve optimization strategy is used to optimize the path node sequence obtained by bidirectional A*. The specific algorithm formula is as follows:

[0091] The cubic Bezier curve is:

[0092]

[0093] Where t∈[0,1], j represents the starting index of the node segment currently being processed, In order to better adapt to the environment and ensure the path quality, the weight factor w is introduced. i , which is related to the environment information around the node, that is, if the node w i If there are many obstacles around, w i Smaller to reduce the influence of the node on the curve shape, the improved Bezier curve formula is:

[0094]

[0095] The above step 2 specifically includes the following steps:

[0096] 2.1 Establishing a mathematical model of motion: Based on the motion characteristics of the robot's differential structure, a mathematical model is established to describe the motion trajectory of the robot in adjacent moments, including changes in position, speed, and angle. The specific algorithm formula is as follows: Due to the limitations of the differential structure, the robot intelligently moves forward and rotates. The forward movement is the x-axis speed v, and the rotation is the z-axis angular velocity w. Since the distance traveled by the robot in adjacent moments is short, the motion trajectory between two adjacent points can be regarded as a straight line. The mathematical model of the robot is as follows:

[0097]

[0098] Among them: Δs represents how far the car has traveled, Δx and Δy represent the x and y coordinates of the car respectively, x, y, θ t Indicates the position of the car in the next state;

[0099] 2.2 Optimize trajectory evaluation function and speed space constraints: Design a comprehensive trajectory evaluation function for the DWA algorithm. For the azimuth evaluation function, analyze the target motion trend and calculate the dynamic azimuth deviation weight. For the distance evaluation function, comprehensively consider the distance between the car and the nearest obstacle, the obstacle shape, size, and distribution density. For the speed evaluation function, consider the speed corresponding to the trajectory and combine it with the robot's power system efficiency curve and the remaining battery power. For the weight coefficient, an adaptive dynamic adjustment strategy is used to adjust the weight coefficient, and it is updated in real time according to environmental characteristics and robot performance indicators. For the speed space constraint, in addition to considering the robot's own speed and angular velocity limitations, stability constraints based on the robot's physical parameters and dynamic model are also introduced. The DWA trajectory evaluation function is shown below:

[0100] G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω))

[0101] Among them, σ is the scaling factor, heading(v,ω) is the azimuth evaluation function, α is its weight coefficient, dist(v,ω) is the distance evaluation function, α is its weight coefficient, velocity(v,ω) is the speed evaluation function, and γ is its weight coefficient. For the azimuth evaluation function heading(v,ω), not only the simple angle difference between the car and the target is considered, but also the movement trend of the target is further analyzed. By predicting the position change of the target in several future time steps and combining the current position and speed of the robot, a dynamic azimuth deviation weight is calculated. If the target is moving rapidly away from the robot, then in the azimuth evaluation, the trajectory close to the current movement direction of the target will obtain a higher weight. Its calculation formula is:

[0102]

[0103] where d i is the distance from the robot to the predicted target position, θ t is the robot's current azimuth, is the azimuth of the target at position w iIt is a weight coefficient that is dynamically adjusted according to the target speed and distance. In the distance evaluation function, in addition to considering the distance between the car and the nearest obstacle, the shape, size and distribution density of the obstacle are also considered. For irregularly shaped obstacles, a polygonal approximation method is used to calculate their equivalent influence range. At the same time, an obstacle congestion factor is introduced according to the distribution density of the surrounding obstacles. When the obstacles around the robot are densely distributed, it is more inclined to choose a trajectory away from the highly congested area. The distance evaluation function dist(v,ω) is:

[0104]

[0105] where d min is the distance to the nearest obstacle, S j is the projection area of the j-th obstacle in the direction of robot movement, S total is the sum of the projected areas of all obstacles, and m is the number of obstacles within the robot's perception range. For the velocity evaluation function velocity(v,ω), not only the speed corresponding to the trajectory is considered, but also the robot's power system efficiency curve and the remaining battery power are combined. According to the energy consumption model of the power system under different speed and angular velocity combinations, the energy consumption cost E(v,w) of each trajectory is calculated, while the remaining battery power B is also considered. remain Regarding the speed selection restriction, when the battery power is low, it is more inclined to choose a speed trajectory with lower energy consumption, so the speed evaluation function is:

[0106]

[0107] Among them E max is the maximum energy consumption when the battery is fully charged, B total is the total capacity of the battery; for the speed space constraint, in addition to considering the robot's own maximum linear velocity v MAX , maximum angular velocity ω MAX , minimum linear velocity v MIN and the minimum angular velocity ω MIN Constraint, that is, the basic speed constraint V A , motion continuity constraint V B and obstacle avoidance related constraints V C , we also consider the speed limit of the robot's mechanical structure stability, that is, the stability constraint V D , the specific formula is as follows:

[0108] V A ={(v,ω)|v∈[v MIN ,v MAX ]∩ω∈[ω MIN ,ω MAX ]}

[0109] VB ={(v,ω)|v∈[v1+v2·Δt,v1-v3·Δt]∩ω∈[ω1+ω2·Δt,ω1-ω3·Δt]

[0110]

[0111] V D ={(v,ω)|v≤f stability (v,ω)}

[0112] Among them, f stability (v,ω) is a stability function calculated based on the robot's center of gravity, wheelbase, friction coefficient and other parameters, which is used to limit the speed range under different motion states;

[0113] In the above step 3, the path nodes planned by the improved A* algorithm in step 2 are used as the local target points of the improved DWA algorithm in step 3, thereby realizing the fusion of the improved A* algorithm and the improved DWA algorithm;

[0114] In the above step 4, the initialization operation is first performed to input map information, starting point and target point, and set the parameters of the improved A* algorithm and improved DWA algorithm. Then, the improved A* algorithm is used to search for the optimal path from the starting point to the target point on the map. The improved A* algorithm determines the optimal path node sequence by evaluating the node cost and heuristic function. The path nodes planned by the improved A* algorithm are used as local target points of the DWA algorithm in sequence. The improved DWA algorithm calculates the speed and steering instructions at the next moment based on the current position, speed and surrounding environment information of the robot, so that the robot can move towards the local target point and avoid obstacles in real time. If an obstacle is detected or the path is infeasible, the improved DWA algorithm feeds the result back to the improved A* algorithm, and the improved A* algorithm replans the local path. The improved DWA algorithm then adjusts the robot's motion trajectory accordingly, and repeats this feedback and adjustment process until the robot reaches the target point.

[0115] Experimental Example 1:

[0116] In order to verify the rationality of the improved A* algorithm, the improved A* algorithm is simulated in MATLAB and the environment is meshed in MATLAB, as shown in the following example: Figure 2 As shown, the environment is divided into a 20x20 grid map, where black represents obstacles. Figure 2 (a) is the traditional eight-neighborhood search graph, Figure 2 (b) is the search graph of the search strategy using ODMW strategy + direction judgment + four neighborhoods + left-right symmetric convex neighborhood. Figure 2 (c) for Figure 2 (b) Based on the search graph with the bidirectional strategy, Figure 2(d) is the search graph of the improved A* algorithm of the present invention. The search effect is shown in Table 1. From Table 1, it can be found that the A* algorithm that incorporates direction judgment and ODMW strategy has a 66.27% reduction in expanded nodes and a 48.15% reduction in search time compared to the traditional algorithm. After incorporating the bidirectional strategy, the search time is further reduced by 74.08%. After using the improved A* algorithm of the present invention, the path nodes are further reduced by 35%, and the search time is further optimized.

[0117] Experimental Example 2:

[0118] In order to verify the rationality of the improved A* algorithm, the improved A* algorithm integrated with the DWA algorithm is simulated in Matlab. Figure 3 As shown in the figure, black represents known obstacles, gray represents unknown static obstacles, and the snowflake area represents moving obstacles. The path nodes obtained by the improved A* algorithm are loaded into the local path DWA algorithm. When the robot encounters unknown static obstacles and moving obstacles, it will perform real-time speed adjustment and path correction according to the DWA algorithm, as shown in the figure. Figure 3 Simulation diagram and Figure 4 As shown in the data, Figure 4 (a) is the simulation angle, Figure 4 (b) is the simulated angular velocity, Figure 4 (c) is the simulation speed; the robot is speed-limited, with a maximum angular velocity of 0.4 rad / s and a maximum linear velocity of 0.5 m / s. When approaching a gray obstacle, the robot reduces its speed and circumvents the obstacle by changing its driving direction to avoid collision with it, thereby reducing the risk of collision. The real-time obstacle avoidance of the DWA algorithm enables the robot to flexibly respond to various obstacles in complex environments, ensuring the safe operation of the robot and the successful completion of the task.

[0119] Experimental Example 3:

[0120] In order to verify the rationality of the improved A* algorithm and the improved DWA algorithm, the ROS master was used to simulate the narrow traffic environment isolation experiment. First, when the traffic isolation position coordinates were not released, the karto mapping algorithm was used to build a real-time map of the narrow traffic environment isolation, such as Figure 5 As shown in (a), in this map, the snowflake area represents the expansion layer, which is to ensure that the robot will not collide with the edge of the obstacle during driving. The dark gray outside the snowflake area represents the radar data collected by the radar sensor, reflecting the distance and position information of objects in the surrounding environment. The white area represents the area that has been fully explored and determined to be passable. When the isolated target coordinates are determined on the ROS master, the robot will start global path planning. At this time, the improved A* algorithm is used to plan a global path from the current position to the target position, as shown in Figure 1. Figure 5As shown in the middle line of (b), when planning a path, the improved A* algorithm calculates the cost of each node to the starting point and the end point based on the map information (including the expansion layer of the snowflake area and the environmental information reflected by the dark gray radar data). Through continuous search and comparison, it finds a path with the minimum comprehensive cost as the global path. At this time, the robot starts to drive according to the established global planning path. During the driving process, the robot will continuously load the surrounding environment data from the sensor. Among them, the radar sensor continuously provides dark gray radar data to help the robot monitor the changes of surrounding obstacles in real time. When the robot encounters Figure 5 When an unknown obstacle is encountered as shown in (c), the improved DWA algorithm generates a series of possible speed combinations in the speed space based on the robot’s current speed, position, and surrounding environment information. Then, by evaluating factors such as whether the robot’s trajectory under each speed combination will collide with the obstacle and whether it can get closer to the target, the optimal speed combination is selected to replan the original path. The planned path is shown in the figure below. Figure 5 The paths near bends 2 and 3 in the middle are shown, showing that the robot successfully avoided unknown obstacles. During this process, the robot needs to subscribe to relevant topics to obtain necessary information. For example, it needs to subscribe to the " / map" topic to obtain real-time map information (including the map content composed of the snowflake area expansion layer and dark gray radar data) to understand the current environment and obstacle distribution; subscribe to the " / odometry" topic to obtain its own pose information (position and attitude) to determine its exact position in the map; and subscribe to the " / sensor_data" topic to obtain environmental data collected by various sensors (such as radar data) for obstacle detection and map update. When local path planning is completed and unknown obstacles are avoided, the map is refreshed in a timely manner. Newly detected unknown obstacles are loaded into the real-time map. The updated map provides more accurate environmental information for subsequent path planning and robot navigation. Map refresh may involve updating and redrawing the map data structure to ensure that the map reflects the latest environmental status, including adjusting the snowflake area expansion layer and updating the dark gray radar data. When the robot approaches the target point, it performs pose calibration to complete the isolation task.

[0121] Table 1 Search results

[0122]

[0123] Based on the above, the advantages of the present invention are that, when used, the invention adopts a fusion of the improved A* algorithm and the DWA algorithm to realize path planning for outdoor robots. In the improved A* algorithm, the heuristic function is optimized by introducing the dynamic weight DOMW strategy. This strategy comprehensively considers distance, obstacles and map factors, thereby making path planning more reasonable. Secondly, by removing redundant nodes, the algorithm's computational complexity and search time are effectively reduced, further improving the efficiency of the algorithm. In addition, a bidirectional search mechanism is introduced, that is, searching from the starting point and the target point at the same time. This method not only improves the search efficiency, but also reduces the search space, helps to avoid the occurrence of local optimal solutions, and enhances the robustness of the algorithm. Finally, the path is smoothed by using the Bezier curve optimization strategy to make it more in line with actual motion requirements, which not only reduces damage to the robot hardware, but also improves the aesthetics and readability of the path. In the improved DWA algorithm, a smooth trajectory is selected by optimizing the trajectory evaluation function and the velocity space constraint, thereby reducing mechanical wear, improving execution efficiency and stability, and reducing collision risks.

[0124] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above and that the invention can be embodied in other specific forms without departing from the spirit or essential characteristics of the invention. Therefore, the embodiments should be considered in all respects as illustrative and non-restrictive, and the scope of the invention is defined by the appended claims, not the foregoing description, and all variations within the meaning and range of equivalents of the claims are intended to be included therein. Any reference sign in a claim should not be construed as limiting the claim to which it relates.

Claims

1. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithm includes the following steps: Step 1: A* algorithm improvement; Step 2: DWA algorithm improvement; Step 3: algorithm fusion; Step 4: path planning; the characteristics are: The above step 1 specifically includes the following steps: 1.1 Heuristic function optimization: Adopting the dynamic weight DOMW strategy, the heuristic function of the A* algorithm is optimized by comprehensively considering distance, obstacles and map factors; 1.2 Neighborhood Search Optimization: Based on the A* algorithm, a search strategy of four neighborhoods plus a bilaterally symmetric convex neighborhood is adopted. When expanding the child nodes of the current node n, the four nodes in the four neighborhood expansion directions of the current node are first judged. If any of the nodes has an obstacle or is a path node, the four-neighborhood expansion strategy is adopted. If there is no obstacle or the target node of the path, the bilaterally symmetric convex neighborhood expansion strategy is adopted. 1.3 Remove redundant nodes: Extract path nodes from the initial path. Starting from the starting point, determine whether there are obstacles between adjacent nodes. If not, delete the node and update the path node. If so, keep the previous node, continue to search for the next node and update the path until the initial path node sequence is traversed. 1.4 Bidirectional Search: Forward search starts from the starting point S, and reverse search starts from the target point T. During the search process, the forward search and reverse search respectively maintain their own open lists. Each time, the node with the smallest evaluation function value is selected from the open list for expansion. The obstacle information in the map is taken into account when calculating the actual cost and the heuristic estimated cost. When there is a common node or the open list in a certain direction is empty, the search terminates and the forward and reverse paths are connected to obtain the initial global path. 1.5 Path smoothing: Using the Bezier curve optimization strategy, the path node sequence obtained by bidirectional A* is optimized; The above step 2 specifically includes the following steps: 2.1 Establishing a mathematical model of motion: Based on the motion characteristics of the robot's differential structure, a mathematical model is established to describe the motion trajectory of the robot at adjacent moments, including changes in position, speed, and angle; 2.2 Optimize trajectory evaluation function and speed space constraints: Design a comprehensive trajectory evaluation function for the DWA algorithm. For the azimuth evaluation function, analyze the target motion trend and calculate the dynamic azimuth deviation weight. For the distance evaluation function, comprehensively consider the distance between the car and the nearest obstacle, the obstacle shape, size, and distribution density. For the speed evaluation function, consider the speed corresponding to the trajectory and combine it with the robot's power system efficiency curve and the remaining battery power. For the weight coefficient, use an adaptive dynamic adjustment strategy to adjust the weight coefficient, and update it in real time according to environmental characteristics and robot performance indicators. For the speed space constraint, in addition to considering the robot's own speed and angular velocity limitations, also introduce stability constraints based on the robot's physical parameters and dynamic model; In the above step 3, the path nodes planned by the improved A* algorithm in step 2 are used as the local target points of the improved DWA algorithm in step 3, thereby realizing the fusion of the improved A* algorithm and the improved DWA algorithm; In the above step 4, the initialization operation is first performed to input map information, starting point and target point, and set the parameters of the improved A* algorithm and improved DWA algorithm. Then, the improved A* algorithm is used to search for the optimal path from the starting point to the target point on the map. The improved A* algorithm determines the optimal path node sequence by evaluating the node cost and heuristic function. The path nodes planned by the improved A* algorithm are used as local target points of the DWA algorithm in sequence. The improved DWA algorithm calculates the speed and steering instructions at the next moment based on the current position, speed and surrounding environment information of the robot, so that the robot can move towards the local target point and avoid obstacles in real time. If an obstacle is detected or the path is infeasible, the improved DWA algorithm feeds the result back to the improved A* algorithm, and the improved A* algorithm replans the local path. The improved DWA algorithm then adjusts the robot's motion trajectory accordingly, and repeats this feedback and adjustment process until the robot reaches the target point.

2. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithm according to claim 1 is characterized in that: In step 1.1, the specific algorithm formula is as follows: f(n)=g(n)+DOMW*h(n) D r,c =log r (r*c) Among them, f(n) is the comprehensive cost of node n, g(n) is the actual cost from the starting point to the current node, h(n) is the estimated cost from the current node to the target point, DOMW is the dynamic weight, P now ,a round is an obstacle, P now The number of obstacles in the search direction of the current node 24 neighborhood, Q now,g is the Euclidean distance from the current node to the target point, D r,c is the map weight; when the node is far from the target point, the effect of the heuristic function is increased to speed up the node search; when the node is close to the target point, the effect of the heuristic function is reduced to expand the range of the search node.

3. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithm according to claim 1 is characterized in that: In step 1.2, the specific algorithm formula is as follows: For the grid map established by the Karto algorithm, assume that a position (x, y) in the map has four neighbors: for node n (x n ,y n ), its four neighboring nodes are n up (x n ,y n +1), n down (x n ,y n -1), n left (x n -1,y n ), n right (x n +1,y n ); bilaterally symmetric convex neighborhood: in addition to the four neighboring nodes, it also includes n leftup (x n -1,y n +1), n leftdown (x n -1,y n -1), n rightup (x n +1,y n +1), n rightdown (x n +1,y n -1), n right2 (x n +2,y n ), n left2 (x n -2,y n ); The inaccessible area in the map is represented by set O. If a node (x, y)∈O, it means that the node is an obstacle node. The path node is represented by set P, and the target node is represented by G(x g ,y g ); If the target node is in the fourth quadrant, which is the lower right corner of the current node, {n down (x n ,y n -1), n right (x n +1,y n ), n rightdown (x n +1,y n -1), n right2 (x n +2,y n )}, if the target node is in the upper right corner of the current node, that is, in the first quadrant, {n up (x n ,y n +1), n right (x n +1,y n ), n rightdown (x n +1,y n +1), n right2 (x n +2,y n )}, if the target node is in the upper left of the current node, that is, in the second quadrant, {n up (x n ,y n +1), n left (x n -1,y n ), n leftup (x n -1,y n +1), n left2 (x n -2,y n )}, if the target node is in the lower left of the current node, that is, in the third quadrant, {n down (x n ,y n -1), n left (x n -1,y n ), n leftdown (x n -1,y n -1), n left2 (x n -2,y n )}.

4. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithms according to claim 1, characterized in that: In step 1.3, the specific algorithm formula is as follows: Assume that in the two-dimensional grid map coordinate system established by the Karto algorithm, nodes n1(x1,y1) and n2(x2,y2) are adjacent nodes, and the four boundary points of the obstacle are For nodes n1 and n2, construct the vector For each obstacle boundary point p i , construct the vector and Calculate the vector cross product and its calculation formula is: If for all i, and If the signs of are the same, that is, both positive or negative, it means that the four boundary points of the obstacle are on the same side of n1 and n2, that is, the obstacle is not crossed; otherwise, it is considered that the obstacle is crossed; in the process of path node update, the initial path node sequence is {N1, N2, ..., N m }, starting from the starting point N1, determine the adjacent nodes N in turn i and N i+1 Is there an obstacle? If there is no obstacle, that is, the above same-side judgment condition is met, then delete N i+1 , and update the path node sequence to {N1,N2,…N i ,N i+2 ,...,N m }; If there is an obstacle, keep N i , continue to search for the next node N i+2 And update the path node sequence until the entire initial path node sequence is traversed.

5. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithm according to claim 1 is characterized in that: In step 1.4, the specific algorithm formula is as follows: In the forward search, the evaluation function of node n is: f forward (n)=g forward (n)+DOMW*h forward (n) where g forward (n) is the actual cost from the starting point S to the node n. Assume that the edge cost from the adjacent node m to the node n is c(m,n). If the node n is the direct successor node of the starting point S, then g forward (n) = c(S,n); if node n is expanded from node m, then g forward (n) = g forward (m)+c(m,n),h forward (n) is the heuristic estimated cost from node n to target point T. The robot uses Euclidean distance as the heuristic function, that is, Where (x n ,y n ) is the coordinate of node n, (x T ,y T ) is the coordinate of the target point T; In the reverse search, the evaluation function for node n is: f backward (n)=g backward (n)+DOMW*h backward (n) where g backward (n) is the actual cost from the target point T to the node n. The calculation method is similar to the forward direction, but the direction is opposite. If the node n is the direct predecessor node of the target point T, then g backward (n) = c(T,n); if node n is extended from node m, then g backward (n) = g backward (m)+c(m,n);h backward (n) is the heuristic estimated cost from node n to the starting point S, also using Euclidean distance; For the grid map established by the Karto algorithm, when calculating the edge cost c(m,n), it is necessary to consider the obstacle information in the map. If there is an obstacle on the path from node m to node n, then c(m,n) = ∞, indicating that this path is impassable. If there are no obstacles, the cost is determined based on the map resolution and the robot's motion model. If the robot moves in an unobstructed straight line, the cost is 1, and the cost of diagonal movement is The value of c(m,n) is determined according to the relative positions of nodes m and n. During the search process, the forward open list OpenList is maintained at the same time. forward and reverse open list OpenList backward , every time from OpenList forward Select f forward (n) The smallest node is expanded, and at the same time from f backward Select f backward (n) The smallest node is expanded; the termination condition is: when there is a node n in the OpenList forward and OpenList backward or when OpenList forward or OpenList backward When it is empty, the search terminates. If the search terminates successfully, that is, there is a common node n, then the forward path from the starting point to n and the reverse path from the target point to n are connected to form the final path.

6. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithm according to claim 1, characterized in that: In step 1.5, the specific algorithm formula is as follows: The cubic Bezier curve is: Where t∈[0,1], j represents the starting index of the node segment currently being processed, In order to better adapt to the environment and ensure the path quality, the weight factor w is introduced. i , which is related to the environment information around the node, that is, if the node w i If there are many obstacles around, w i Smaller to reduce the influence of the node on the curve shape, the improved Bezier curve formula is:

7. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithm according to claim 1, characterized in that: In step 2.1, the specific algorithm formula is as follows: Due to the limitation of the differential structure, the robot moves forward and rotates intelligently. The forward movement is the x-axis velocity v, and the rotation is the z-axis angular velocity w. Since the distance traveled by the robot in adjacent moments is short, the motion trajectory between two adjacent points can be regarded as a straight line. The mathematical model of the robot is as follows: Among them: Δs represents how far the car has traveled, Δx and Δy represent the x and y coordinates of the car respectively, x, y, θ t Indicates the position of the car in the next state.

8. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithm according to claim 1 is characterized in that: In step 2.2, the DWA trajectory evaluation function is as follows: G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω))where σ is the scaling factor, heading(v,ω) is the azimuth evaluation function, α is its weight coefficient, dist(v,ω) is the distance evaluation function, β is its weight coefficient, velocity(v,ω) is the speed evaluation function, and γ is its weight coefficient.

9. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithm according to claim 1, characterized in that: In step 2.2, the heading evaluation function heading(v,ω) not only considers the simple angle difference between the robot and the target, but also further analyzes the target's motion trend. By predicting the target's position change in the next few time steps and combining the robot's current position and speed, a dynamic heading deviation weight is calculated. If the target is moving rapidly away from the robot, then in the heading evaluation, the trajectory close to the target's current motion direction will receive a higher weight. The calculation formula is: where d i is the distance from the robot to the predicted target position, θ t is the robot's current azimuth, is the azimuth of the target at position w i It is a weight coefficient that is dynamically adjusted according to the target speed and distance. In the distance evaluation function, in addition to considering the distance between the car and the nearest obstacle, the shape, size and distribution density of the obstacle are also considered. For irregularly shaped obstacles, a polygonal approximation method is used to calculate their equivalent influence range. At the same time, an obstacle congestion factor is introduced according to the distribution density of the surrounding obstacles. When the obstacles around the robot are densely distributed, it is more inclined to choose a trajectory away from the highly congested area. The distance evaluation function dist(v,ω) is: where d min is the distance to the nearest obstacle, S j is the projection area of the j-th obstacle in the direction of robot movement, S total is the sum of the projected areas of all obstacles, and m is the number of obstacles within the robot's perception range. For the velocity evaluation function velocity(v,ω), not only the speed corresponding to the trajectory is considered, but also the robot's power system efficiency curve and the remaining battery power are combined. According to the energy consumption model of the power system under different speed and angular velocity combinations, the energy consumption cost E(v,ω) of each trajectory is calculated, and the remaining battery power B is also considered. remain Regarding the speed selection restriction, when the battery power is low, it is more inclined to choose a speed trajectory with lower energy consumption, so the speed evaluation function is: Among them E max is the maximum energy consumption when the battery is fully charged, B total is the total capacity of the battery.

10. The outdoor robot path planning method based on the improved bidirectional A* and DWA algorithm according to claim 1, characterized in that: In step 2.2, for the speed space constraint, in addition to considering the robot's own maximum linear velocity v MAX , maximum angular velocity ω MAX , minimum linear velocity v MIN and the minimum angular velocity ω MIN Constraints, namely the basic speed constraints V A , motion continuity constraint V B and obstacle avoidance related constraints V C , we also consider the speed limit of the robot's mechanical structure stability, that is, the stability constraint V D , the specific formula is as follows: V A ={(v,ω)|v∈[v MIN ,v MAX ]∩ω∈[ω MIN ,ω MAX ]} V B ={(v,ω)|v∈[v1+v2·Δt,v1-v3·Δt]∩ω∈[ω1+ω2·Δt,ω1-ω3·Δt] V D ={(v,ω)|v≤f stability (v,ω)} Among them, f stability (v,ω) is a stability function calculated based on parameters such as the robot's center of gravity, wheelbase, and friction coefficient, and is used to limit the speed range under different motion states.

Images