A TWA*-DWA global optimal path planning method based on improved A* algorithm and DWA algorithm

Abstract

The application relates to a TWA*-DWA global optimal path planning method based on improved A* algorithm and DWA algorithm. By analyzing the basic principle of the A* algorithm, firstly, an A* algorithm based on the time wheel structure and efficiency optimization is proposed; for the global path node obtained by using the TWA* algorithm, a path node selection strategy based on a connection judgment method is proposed, redundant nodes are removed, and the optimized node is used as a key guide point of the DWA algorithm, and based on the local target point rotation strategy of the dynamic window of local reconstruction, the local target point of the DWA algorithm planning process is updated. Through simulation experiments, the effectiveness of the TWA* algorithm and the global optimal path planning capability of the unmanned vehicle based on the TWA*-DWA algorithm are verified.

Description

Technical Field

[0001] This invention relates to the field of local path planning for obstacle avoidance in autonomous vehicles and mobile robots, and specifically to a TWA*-DWA global optimal path planning method based on an improved A* algorithm and a DWA algorithm. Background Technology

[0002] In recent years, autonomous driving technology has developed rapidly, becoming a hot topic in the field of driverless vehicles. The autonomous navigation capabilities of driverless vehicles are of great significance for realizing intelligent transportation, improving road safety, and enhancing traffic efficiency. Intelligent vehicle technology is a comprehensive set of technologies encompassing perception, localization, path planning, decision-making and control, and artificial intelligence, aiming to achieve autonomous navigation, environmental perception, and intelligent decision-making capabilities for vehicles. By acquiring surrounding information through sensors, determining the vehicle's position, planning the optimal route, and making intelligent driving decisions based on the decision-making system, intelligent vehicle technology is committed to improving traffic safety, efficiency, and comfort, driving the revolution of future transportation modes.

[0003] Path planning is a key technology for intelligent vehicles. Its goal is to enable an intelligent vehicle to plan an optimal path (with the least time or shortest distance) given a known starting point and destination, avoiding static and dynamic obstacles. Path planning can be divided into global and local path planning. Global path planning refers to finding the optimal path from the starting point to the destination within the entire map. This planning considers map information, road layout, and obstacles to determine long-distance navigation for the vehicle in urban or complex environments. Local path planning focuses on the environment surrounding the vehicle's current location, using real-time perception and obstacle avoidance to select a safe and feasible path, enabling precise navigation and obstacle avoidance in confined spaces. In practice, intelligent vehicles need to combine both global and local path planning algorithms; the quality of these two algorithms determines the effectiveness of path planning. Summary of the Invention

[0004] The purpose of this invention is to provide a TWA*-DWA globally optimal path planning method based on an improved A* algorithm and a DWA algorithm. The method involves the following steps: First, to address the problem of slow speed in popping the lowest value from the open list due to the geometric growth of data in the A* algorithm, an optimization based on a time-wheel method is proposed. Simulation experiments verify that this algorithm effectively improves computational speed. Second, to address the problem of autonomous vehicles easily getting trapped in local optima when using the DWA algorithm for path planning, a key point guidance strategy based on the TWA* algorithm is proposed. This strategy guides the autonomous vehicle to quickly approach the destination by continuously rotating the key points in the target point sequence. Simulation experiments verify the feasibility of this approach.

[0005] To achieve the above objectives, the technical solution of this invention is: a TWA*-DWA global optimal path planning method based on an improved A* algorithm and a DWA algorithm, comprising the following steps:

[0006] Step 1: Initialize parameters and read the current coordinates of the autonomous vehicle, the coordinates of the obstacles, and the target point;

[0007] Step 2: Use the TWA* algorithm, call the time wheel method to pop the node with the smallest cost f in the A* algorithm, and calculate iteratively to obtain the global path node;

[0008] Step 3: Eliminate redundant nodes based on the connection determination method, and leave the optimized nodes as key guiding points for DWA;

[0009] Step 4: Verify whether the obtained key nodes of the global path meet the requirements;

[0010] Step 5: Use the DWA algorithm to perform local path planning and obtain the set of motion points;

[0011] Step 6: The driverless vehicle reaches the destination, completes path planning, and obtains the planned route.

[0012] In one embodiment of the present invention, step one is specifically implemented as follows:

[0013] By reading data from onboard sensors, including the starting point, target point, and obstacle information, a simplified turning model for the intelligent vehicle is introduced. Here, L represents the front and rear wheelbase, (x... c ,y c ,α) represents the pose of the intelligent parking space, (x c ,y c Let α be the coordinates of the intelligent vehicle in two-dimensional space, and let α be the heading angle of the intelligent vehicle at the current moment; the intelligent vehicle has a minimum turning radius R when turning. min The maximum inner turning angle of the front wheels is α2, and the maximum outer turning angle of the front wheels is α1. During the turning process, the inner and outer turning angles are related to the vehicle type, its own speed, and acceleration. Let v x Let a be the longitudinal velocity during the motion. y Let y be the lateral acceleration, where the expressions for the minimum turning radius and the maximum turning angle (i.e., the maximum internal turning angle) are:

[0014]

[0015]

[0016] In one embodiment of the present invention, in step two, the TWA* algorithm improves the A* algorithm by using a time wheel method to minimize the pop-up cost f. The time wheel method is implemented using a hierarchical time wheel, that is, using a cycle of 2. 9 Round I and its cycle is 25 In the second wheel, the pointer of the first wheel rotates one cycle, and the pointer of the second wheel rotates one position. The specific implementation is as follows:

[0017] (1) Initialization: Each time slot in round I is initialized as a queue, and the time slots in round II are initialized as a dictionary;

[0018] (2) Insertion: Assume the cost of the data to be inserted is f. cur If f cur ≤2 9 If f cur >2 9 Then add this data to the fth round of round II. cur / 2 9 In the dictionary of time slots, where the key is f cur %2 9 The value is data; when the dictionary of the time slot pointed to by the pointer of round II is not empty, it means that the data in the slot has less than one cycle of round I before it needs to be executed. Therefore, it is moved to round I. The specific operation is to read the key of the corresponding dictionary, the value of the key corresponds to the position in round I, and then access the value through the key and insert it into the corresponding queue. After the data transfer is completed, the dictionary of the corresponding time slot in round II is set to empty, and the data insertion is completed.

[0019] (3) Search: The A* algorithm needs to use the grid with the lowest f value as the expansion point. Therefore, as the time wheel rotates, when the queue corresponding to the time slot pointed to by the I wheel is not empty, the data in the queue is the target expansion point of the A* algorithm. Therefore, the corresponding data is directly popped from the tail of the queue, and the deletion operation is completed at the same time.

[0020] (4) Rewind: The time slot pointed to by the first pointer is the location where the grid cell with the lowest f value is stored. However, when applied to the A* algorithm, a problem arises: what is the storage location for this lowest f value? low When accessing information about neighboring rasters using the corresponding raster as an extension point, if the f value of the accessed neighboring raster is... near If the value is less than the value currently pointed to by the pointer, then that value will be inserted at the position before the pointer. Since the pointer keeps moving forward, the next step will not be able to access that data. Therefore, during the data insertion process, f needs to be... low with f near Compare the sizes, if f near <f low Then the pointer should be turned back to the f-th cycle of round I. near A time slot is used to ensure access to f. near The corresponding grid;

[0021] After finding the node with the minimum cost, the process is repeated until the destination is found, thus obtaining a complete intelligent vehicle path node.

[0022] In one embodiment of the present invention, step three is specifically implemented as follows:

[0023] (1) Use the TWA* algorithm to obtain the global path node set K, and define the Left pointer to point to the address of the first element of K, and the Right pointer to point to the address of the second element of K;

[0024] (2) Determine whether the line connecting Left and Right passes through an obstacle. If so, Right points to the next node until the line connecting Left and Right does not pass through an obstacle. When the line passes through an obstacle, it means that Left cannot directly reach Right. At this time, the farthest node that Left can reach is Right-1. Therefore, pop the Right-1 node into the key point set κ and adjust the Left pointer to Right-1. Determine whether Right exceeds the tail address of K. If so, terminate the loop; otherwise, repeat step (2).

[0025] (3) In step (2), we obtain κ and define pointer A to point to the address of the first element of κ, B to point to the address of the second element of κ, C to point to the address of the third element of κ, and D to point to the address of the fourth element of κ.

[0026] (4) The line AB intersects the line CD at point E. Determine whether line segments AE and ED are separated from the obstacle. If so, A can directly reach D through point E without passing through points B and C. Therefore, points B and C are non-essential nodes on κ. Delete them and insert point E after point A. At this time, adjust the pointer of A to E, and the pointers of B, C, and D move forward in turn. Otherwise, consider points B and C as essential nodes on κ and keep them unchanged. At this time, the pointers of A, B, C, and D move forward one position. Determine whether D points to the tail address of κ. If so, terminate the loop; otherwise, repeat step 4. Obtain the final κ, which will be used as the candidate target point sequence of the DWA algorithm.

[0027] In one embodiment of the present invention, step four is specifically implemented as follows:

[0028] (1) Stability Analysis: In the DWA algorithm, the heading angle function heading(v,w)=π-|θ-δ| specifically refers to the heading angle of the unmanned vehicle at the simulated trajectory terminal, and δ is the tilt angle of the line connecting the simulated trajectory terminal and the endpoint. When the connecting line is collinear with the line containing the heading angle, there are two situations: 1) The simulated trajectory terminal is in front of the target point, in which case |θ-δ|=π, and the heading function reaches its maximum value; 2) The simulated trajectory terminal is behind the target point, in which case |θ-δ|=π, and the heading function reaches its minimum value. To satisfy the condition in situation 1), the simulated trajectory terminal should always remain in front of the target point, so that the unmanned vehicle can avoid stalling due to being too close to the key point. Therefore, when the unmanned vehicle is close to the key point G, the stability analysis is as follows: t If the distance is less than R, the target point should be moved to the next key point G. t+1 The parameter R is related to the parameters of the autonomous vehicle and the DWA algorithm, as shown in the following formula:

[0029] R = mΔ t v max

[0030] In the formula, m is the number of sampling steps for the simulated trajectory, Δ t v is the sampling interval. max This represents the maximum linear velocity of the driverless vehicle.

[0031] (2) Efficiency Analysis: When there are no obstacles between the autonomous vehicle and the target, the autonomous vehicle can directly reach the target along the connecting line. At this time, the simulated trajectory with the highest heading score is the optimal trajectory. When the autonomous vehicle encounters a situation during the planning process where it can directly reach the key point G, the optimal trajectory is determined by the following criteria: t+1 When the location is key point C t It will become a non-essential node and does not need to reach point G first. t Only then can we reach G t+1 In this case, the process can jump to the key point G. t+1 However, in the actual planning process of autonomous vehicles, there are situations where they deviate from the optimal route, resulting in the loss of directly reachable key points. For example, if the autonomous vehicle deviates from the guidance of key point G1 and cannot directly reach any key point, then the key points G1 to G2 that have never been rotated will be affected. n The keypoint G3 with the highest distance-to-heading index ρ is obtained. Then, the TWA* algorithm is used to locally reconstruct the path between P2 and G3, and the keypoint G is extracted from the path between P2 and G3 using a keypoint selection strategy. p Autonomous vehicles can use G p The point guides the acquisition of the directly reachable global key point G3:

[0032]

[0033] The numerator is consistent with the definition of the heading function, and the denominator is the square root of the Euclidean distance between the autonomous vehicle and the key point. The higher the orientation of the autonomous vehicle and the closer the key point is, the more likely it is to become the target point for local reconstruction.

[0034] In one embodiment of the present invention, step five is specifically implemented as follows:

[0035] Considering the constraints of various conditions on velocity and angular velocity at time t, the velocity and angular velocity window V that the unmanned vehicle can reach at time t is obtained. win Discretize it, and combine the discretized velocities and angular velocities; the unmanned vehicle traverses all combinations and simulates moving forward m Δ steps according to the given motion model. t The duration is used to obtain the simulated trajectory set τ, which is a series of point sets; the evaluation function gives the score of all simulated trajectories in the simulated trajectory set τ, and the trajectory τ with the highest score is selected. b The corresponding combination; the time Δ for driving the autonomous vehicle forward using this combination. t The process continues until time t+1 is reached; this cycle repeats until the end. At time t, the V of the autonomous vehicle... win Constrained by itself and its surrounding environment, the following three constraints need to be considered:

[0036] First, the limit velocity and angular velocity constraints.

[0037] V lim ={(v, w)|v∈[v min v max ]∧w∈[w min w max ]}

[0038] Second, acceleration-limited velocity-angular velocity constraints.

[0039]

[0040] Third, the speed and angular velocity constraints for braking distance limitations.

[0041]

[0042] Among them, v min v max For the limiting linear velocity, w min w max For the limiting angular velocity, v cu w cu The current linear velocity and angular velocity are... For the limit linear acceleration, Let denoted as the limiting angular acceleration, and dist(v,w) be the closest distance between the simulated trajectory corresponding to the velocity-angular velocity combination (v,w) and the obstacle; the first and second constraints consider the impact of the autonomous vehicle's own performance on V. winThe third constraint considers the constraint on velocity and angular velocity for driving safety; finally, at time t, V win Represented as:

[0043] V win =V lim ∩V acc ∩V dis

[0044] The evaluation function consists of three sub-functions, which comprehensively consider three factors: the autonomous vehicle's speed, obstacle collision risk, and autonomous vehicle heading, as detailed below:

[0045] G(v,w)=σ(αheading(v,w)+ηdist(v,w)+γvel(v,w))

[0046] Where heading(v t ,w t )=π-|θ t -δ t | θ represents the heading angle of the autonomous vehicle, δ is the angle between the line connecting the autonomous vehicle and the target point and the positive x-axis; dist(v,w) is the Euclidean distance from the simulated trajectory to the nearest obstacle, vel(v,w) represents the linear velocity of the autonomous vehicle, and α, η, and γ are three weighting coefficients; the evaluation function is composed of sub-functions with different dimensions. The normalization function σ() in the formula is dimensionless learning, which can unify data with different dimensions to the same reference frame for combination or comparison, thereby avoiding evaluation bias caused by different data scales, as detailed below:

[0047]

[0048] dist(v i ,w j ) and vel(v i ,w j Perform the same normalization operation;

[0049] The unmanned vehicle obtains its simulated trajectory based on a uniform motion model. Under the assumptions of this motion model, the magnitudes of the vehicle's linear velocity and angular velocity remain constant, and the change in the direction of the linear velocity is linearly related to time. To simplify the model and speed up the calculation, the velocity direction can be considered to remain constant within small time intervals. Therefore, the uniform motion model is discretized, as shown in the following equation:

[0050]

[0051] Guided by the global path key points obtained by the TWA* algorithm, the DWA algorithm is then executed repeatedly for calculation, ultimately resulting in a series of local motion point sets, i.e., local paths.

[0052] Compared with the prior art, the present invention has the following beneficial effects:

[0053] First, regarding the problem that the A* algorithm is slow in popping the lowest value from the openlist due to the geometric growth of data, this invention proposes an optimization based on the time wheel method. Simulation experiments have verified that the algorithm can effectively improve the calculation speed.

[0054] Secondly, to address the issue of autonomous vehicles easily getting trapped in local optima when using the DWA algorithm for path planning, this invention proposes a key point guidance strategy based on the TWA* algorithm. This strategy guides the autonomous vehicle to quickly approach the destination by continuously rotating the key points in the target point sequence. Simulation experiments have verified the feasibility of this approach. Attached Figure Description

[0055] Figure 1 This is a simplified turning model for an intelligent vehicle.

[0056] Figure 2 Here is the flowchart for the A* algorithm.

[0057] Figure 3 To rasterize the map.

[0058] Figure 4 The strategies are four-way expansion and eight-way expansion.

[0059] Figure 5 Search for an array.

[0060] Figure 6 This is a binary search.

[0061] Figure 7 This is a min-heap search.

[0062] Figure 8 For the time wheel.

[0063] Figure 9 A schematic diagram illustrating the process of obtaining the optimal candidate target point sequence using the connection determination method.

[0064] Figure 10 Two scenarios are analyzed for stability.

[0065] Figure 11 This is a schematic diagram for efficiency analysis.

[0066] Figure 12 For velocity angular velocity window

[0067] Figure 13 Let θ and δ be the coordinates.

[0068] Figure 14 This is for the simulation environment and the planning results of TWA*-DWA.

[0069] Figure 15The arrival rate changes with a fixed weight.

[0070] Figure 16 This section compares the path length and number of iterations between the classic DWA algorithm and the TWA*-DWA algorithm on different maps.

[0071] Figure 17 The results show the planning outcomes of the classic DWA algorithm and the TWA*-DWA algorithm on the same map.

[0072] Figure 18 This is a flowchart of the path planning process for the method of this invention. Detailed Implementation

[0073] The technical solution of the present invention will now be described in detail with reference to the accompanying drawings.

[0074] like Figure 18 As shown, this invention provides a TWA*-DWA global optimal path planning method based on an improved A* algorithm and a DWA algorithm, comprising the following steps:

[0075] Step 1: Initialize parameters and read the current coordinates of the autonomous vehicle, the coordinates of the obstacles, and the target point;

[0076] Step 2: Use the TWA* algorithm, call the time wheel method to pop the node with the smallest cost f in the A* algorithm, and calculate iteratively to obtain the global path node;

[0077] Step 3: Eliminate redundant nodes based on the connection determination method, and leave the optimized nodes as key guiding points for DWA;

[0078] Step 4: Verify whether the obtained key nodes of the global path meet the requirements;

[0079] Step 5: Use the DWA algorithm to perform local path planning and obtain the set of motion points;

[0080] Step 6: The driverless vehicle reaches the destination, completes path planning, and obtains the planned route.

[0081] Step 1: Initialize parameters, read the current coordinates of the autonomous vehicle, the coordinates of the obstacles, and the target point.

[0082] By using onboard sensors such as cameras, radar, GPS, and inertial measurement units (IMUs) to read data from the vehicle, including the starting point, target point, and obstacle information, a simplified turning model for intelligent vehicles is introduced, as shown in the attached diagram. Figure 1 As shown in the figure, L is the front and rear wheelbase, (x c ,y c ,α) represents the intelligent parking space pose, (x c ,y cLet α be the coordinates of the intelligent vehicle in two-dimensional space, and let α be the heading angle of the intelligent vehicle at the current moment. The intelligent vehicle has a minimum turning radius R when turning. min The maximum inner turning angle of the front wheels is α2, and the maximum outer turning angle of the front wheels is α1. During the turning process, the inner and outer turning angles are related to the vehicle type, its own speed, and acceleration. Let v x Let a be the longitudinal velocity during the motion. y Let y be the lateral acceleration, where the expressions for the minimum turning radius and the maximum turning angle (i.e., the maximum internal turning angle) are:

[0083]

[0084]

[0085] Step 2: Using the TWA* algorithm, the time wheel method is invoked to identify the node with the smallest cost f in the A* algorithm, thus obtaining the global path node.

[0086] The TWA* algorithm is an improvement on the A* algorithm, and its process is roughly the same as that of the A* algorithm. Figure 2 This is the process of the A* algorithm. The process of the A* algorithm can be briefly summarized as follows: Starting from the starting point, it continuously selects the node with the minimum estimated cost in the open list, considers its neighboring nodes, and estimates the shortest path based on the actual cost and heuristic function, and finally finds the target node or determines that there is no solution.

[0087] The A* algorithm is a heuristic global path planning algorithm based on raster maps. Raster maps are a fundamental form of spatial data representation in computers. The main idea is to regularly divide a map or image into extremely small raster cells. The width of a cell represents the resolution along the X-axis, and the height represents the resolution along the Y-axis. Generally, the X and Y axis resolutions are equal, so the raster is a square. Higher resolution means more cells and higher accuracy. Each cell has its own X and Y coordinates within the map. When an obstacle exists in the area of ​​a raster cell, the cell is considered an obstacle region, represented by 0; when there are no obstacles in the region, the cell is considered a feasible region, represented by the number 1. (See attached diagram.) Figure 3 As shown. Therefore, a planar raster map can be stored using a two-dimensional numerical matrix containing only 0s and 1s. During the raster environment search process, the A* algorithm mainly employs four-directional and eight-directional node expansion strategies, as shown in the attached diagram. Figure 4 As shown. To ensure that the generated path better matches the actual motion pattern of autonomous vehicles, this paper selects an eight-directional expansion strategy.

[0088] The A* algorithm continuously acquires grid information from the map during pathfinding according to a corresponding node expansion strategy. This includes information such as whether the area is feasible, whether it's the endpoint, parent nodes, and cost value. Grids for which information has been acquired are added to the Openlist. The parent node is defined as follows: if the current grid is A, and the algorithm subsequently visits grids B through I according to the node expansion strategy, then A becomes the parent node of grids B through I. The core of the algorithm is to use the grid with the lowest cost value in the Openlist as the next expansion point. Based on this expansion point, the algorithm repeatedly acquires information from the corresponding grids according to the node expansion strategy, iterating until the expansion point becomes the endpoint. Finally, the endpoint is indexed back to its parent node, and this process continues forward until the starting point, resulting in a singly linked list representing the global path. The cost value of a grid is calculated using the following formula:

[0089] f(n) = g(n) + h(n)

[0090] In the formula, g(n) is the actual cost function, specifically the actual cost of the singly linked list from grid n to the starting point, and h(n) is the estimated cost function, specifically the estimated cost from grid n to the endpoint, which embodies the heuristic search of the A* algorithm. Common distance metrics for estimating cost in grid maps include Manhattan distance, Euclidean distance, and Chebyshev distance. When h(n) and h... * (n)(h * When h(n) represents the actual cost of a single-chain table from the endpoint to grid n on the shortest path through grid n, the A* algorithm can balance search efficiency and path optimality when the distance is sufficiently close. * h(n) is large, making it more suitable for four-way expansion strategies; the Euclidean distance metric h(n) is the straight-line distance between two points, and due to the presence of obstacles, h(n) is always less than h. * The Euclidean distance metric (h(n)) is well-suited for omnidirectional movement strategies, but it has significant limitations on raster maps. However, if the raster map resolution is high enough, node expansion can be considered as omnidirectional expansion. Therefore, the applicability of the Euclidean distance metric increases with the increase in raster resolution. The Chebyshev distance metric is perfectly suited for the eight-directional node expansion strategy, ensuring that h(n) and h(n) are consistent under this metric. * The error of h(n) is minimized, therefore this paper chooses the h(n) function of the Chebyshev distance metric. The specific steps of the algorithm are as follows:

[0091] (1) Initialize the original map into a raster map according to the given resolution, and set the starting point s and the ending point g.

[0092] (2) Create empty Openlist and Closelist. Openlist is used to store the raster that has already acquired information, while Closelist is used to store the raster that has been expanded.

[0093] (3) Calculate the cost f(s) of the starting point s and push it into the Openlist.

[0094] (4) If Openlist is empty, the target is unreachable and the loop terminates; otherwise, find and pop the grid with the lowest cost f in Openlist. If the grid is the endpoint g, index its parent node forward from g to the starting point to obtain a singly linked list representing the global path and terminate the loop; otherwise, use it as an extension point to enter step (5) and push it into Closelist.

[0095] (5) The expansion point obtains information from eight nearby grid cells based on the eight-direction expansion strategy:

[0096] 1) If a grid cell belongs to an infeasible area (obstacle, boundary), push it into the Closelist;

[0097] 2) If grid C is a feasible region but is not in the Openlist, calculate the cost value f(C) of grid C and add it to the Openlist;

[0098] 3) If grid C is a feasible region but already exists in the Openlist, compare the cost f(C) of accessing grid C from the current expansion point with the cost f(C') of the existing access method. If f(C) ≤ f(C'), then update the cost of grid C to f(C) and update the parent node to the current expansion point; if f(C) > f(C'), then leave it unchanged.

[0099] (6) Repeat steps (4) and (5), as shown in the attached flowchart. Figure 2 As shown.

[0100] As the map area increases and the resolution of raster maps improves, the length of the Openlist data grows exponentially. Popping the raster with the lowest f-value from the Openlist consumes significant computational resources, severely impacting the search efficiency of the A* algorithm. Therefore, optimizing the Openlist data structure can effectively improve the running speed of the A* algorithm, thus ensuring good real-time performance. Common methods for finding the element with the smallest f-value from the Openlist include array search, ordered array search, and min-heap search. An array is a data structure used to store variables of the same type with adjacent elements in contiguous memory space. The specific operation for finding the element with the smallest f-value in an array is as follows: Define an infinitely large variable M to store the minimum value, define a pointer to the first address of the array and access the array sequentially. If a number smaller than M is encountered, assign that number to M. The entire array needs to be traversed, resulting in a time complexity (TC) of O(n). When popping M from the array, since the array needs to maintain the continuity of memory space, further adjustments to the array are required. Assuming x0 is the minimum value, the required operations include popping x0, with a TC of O(1), and then adjusting x1-x... n Moving forward one unit has a time complexity of O(n-1), and TC is O(n); assuming x n To find the minimum value, you only need to pop up x. n TC is O(1). Therefore, the average TC of popping M from this array is O(0.5n), so TC is O(n), as shown in the appendix. Figure 5 .

[0101] Searching in an ordered array achieves fast data retrieval by maintaining a non-increasing array. When searching for the element with the smallest value f in an ordered array, since the ordered array is non-increasing, the data stored at the end address of the array is the target element, and TC is O(1). Furthermore, since it is the tail element, no data needs to be moved when popping, so the TC for popping is also O(1). When inserting data x into the non-increasing array... t When it is necessary to maintain the non-increasing property of the array, binary search is required (see appendix). Figure 6 To determine the specific location for inserting data, the total time complexity (TC) of binary search is O(log₂n). The detailed process is as follows: Define a pointer Left pointing to the first address of array Arr, a pointer Right pointing to the last address of Arr, and a pointer Mid = (Left + Right) / 2. If Mid → Arr <x t If the insertion position is within the interval (Mid, Right], then assign Mid+1 to Left; if Mid→Rrr>x tIf the position to be inserted is within the interval [Left, Mid), then Mid-1 is assigned to Right; when Right-Left = 1, then x can be determined. t It should be inserted at the Right position, and the data after Right needs to be shifted one position to the right. The time complexity of shifting the data is O(n), and the total TC is O(n).

[0102] A minimum heap (MH) is a complete binary tree (CBT) that has been sorted in a specific way, where the value of any non-terminal node is no greater than the value of its child nodes, as shown in the attached figure. Figure 7 As shown. If a binary tree satisfies the requirements of a min-heap, then the root node is the smallest element in the entire sequence, and the time complexity (TC) for finding the element with the smallest f value in the min-heap is O(1). After popping 1, in order to maintain the CBT structure, the bottom element 6 is moved to the top layer, and then the order of MH is adjusted using a downward filtering operation: 6 and its left child 2 do not meet the condition, so they are swapped; 6 is less than 7 and less than 8, completing the adjustment. The TC of the downward filtering process is O(log₂n). When new data is inserted into MH, according to the characteristics of the CBT structure, an empty space is searched from right to left at the bottom layer. After the insertion is completed, an upward filtering operation is performed to adjust the order of MH: 3 is greater than 1, which does not meet the condition, so they are swapped; 2 is greater than 1, which does not meet the condition, so they are swapped; 1 is less than 6, which meets the condition, completing the upward filtering operation. The average time complexity of the upward filtering process is the same as that of the downward filtering process, both being O(log₂n).

[0103] Table 1 Comparison of Time Complexity

[0104]

[0105] As shown in Appendix 1, even the fastest min-heap search achieves a total turn rate (TC) of O(log₂n), where n is the length of the Openlist. As the grid map grows larger and the scene becomes more complex, n becomes an extremely large number. Furthermore, each iteration of the A* algorithm requires one query, one deletion operation, and a maximum of eight insertion operations (using an eight-way expansion strategy). Therefore, the quality of the search method directly impacts the computational efficiency throughout the A* algorithm's pathfinding process. The Timing Wheel (TW) algorithm originates from the concept of a clock. As time progresses, the hands rotate at regular intervals and periods. When a task is assigned to a time slot, it is executed. It's a clever algorithm that implements delay functionality (timers) and is widely used in time task scheduling in various operating systems, such as Linux's crontab, and in Java development, such as Dubbo and Netty. In the A* algorithm, if we consider the grid's value f as absolute time, then the process of popping the grid with the smallest f value from the Openlist can be seen as prioritizing tasks with earlier times. Essentially, it's a time scheduling task, and therefore, the TW method can be used to implement the Openlist. The following analysis of the A* algorithm will determine the corresponding TW-based Openlist.

[0106] In the A* algorithm, given f(n) = g(n) + h(n), by definition, the cost f reaches its minimum when the autonomous vehicle is at the starting point. min And it is related to the distance metric of h(n). The maximum value of f is f_max. max The path length is related to the actual distance traveled by the autonomous vehicle. Since each node in the A* algorithm has only one parent node, the autonomous vehicle will not traverse duplicate grid cells. Therefore, theoretically, f... max It should be the result of the autonomous vehicle traversing all the grids without repeating any, as shown in the following formula:

[0107]

[0108] In the formula, W is the lateral distance between the starting point and the target point, H is the longitudinal distance between the two points, and dpi is the resolution. However, in reality, it is impossible to achieve this value. Numerous experiments have shown that when f is taken as... max When f = 2WH, there is no numerical overflow problem, so f is finally determined. max Let f be the map perimeter. To save data storage space and improve data retrieval speed, this paper sets the time interval to an integer of 1. Considering that the integer value of f has low distinguishability, leading to an unbalanced distribution of TW data, the time range for storing TW is determined to be:

[0109] TR = int[(f max -f min)*100]

[0110] The TWA* algorithm primarily improves upon the A* algorithm by minimizing the pop-up cost f. In this paper, TR = 12929 < 16384 = 2 was determined on a 60m × 60m map. 14 To reduce space complexity, a hierarchical time wheel can be used, specifically, a time wheel with a cycle of 2. 9 Round I and its cycle is 2 5 The second wheel moves one position for every cycle the pointer of the first wheel completes, as shown in the attached diagram. Figure 8 As shown below, the initialization, insertion, search, and rollback operations of the time wheel will be analyzed in detail.

[0111] (1) Initialization: Each time slot in round I is initialized as a queue, and the time slots in round II are initialized as a dictionary.

[0112] (2) Insertion: Assume the cost of the data to be inserted is f. cur If f cur ≤2 9 If f cur >2 9 Then add this data to the fth round of round II. cur / 2 9 In the dictionary of time slots, where the key is f cur %2 9 The value is data. When the dictionary of the time slot pointed to by the pointer of round II is not empty, it means that the data in that slot has less than one cycle of round I left to be executed. Therefore, it is moved to round I. The specific operation is to read the key of the corresponding dictionary, the value of the key corresponds to the position in round I, and then access the value through the key and insert it into the corresponding queue. After the data transfer is completed, the dictionary of the corresponding time slot in round II is set to empty, and the data insertion is completed. For example, f of data M. cur =2 9 If +3, then M should be placed in the dictionary Dict1 of the first time slot of round II, with the key 3 and the value M. When round I completes one revolution, the pointer of round II points to time slot 1. At this time, the data in Dict1 is moved to round I. Since the value of the key is 3, M is inserted into the queue of the third time slot of round I and Dict1 is reset.

[0113] (3) Search: The A* algorithm needs to use the grid with the lowest f value as the expansion point. Therefore, as the time wheel rotates, when the queue corresponding to the time slot pointed to by the I wheel is not empty, the data in the queue is the target expansion point of the A* algorithm. Therefore, the corresponding data is directly popped from the tail of the queue, and the deletion operation is completed at the same time.

[0114] (4) Rewind: The time slot pointed to by the first pointer is the location where the grid cell with the lowest f value is stored. However, when applied to the A* algorithm, a problem arises: what is the storage location for this lowest f value? low When accessing information about neighboring rasters using the corresponding raster as an extension point, if the f value of the accessed neighboring raster is... near If the value is less than the value currently pointed to by the pointer, then that value will be inserted at the position before the pointer. Since the pointer keeps moving forward, the next step will not be able to access that data. Therefore, during the data insertion process, f needs to be... low with f near Compare the sizes, if f near <f low Then the pointer should be turned back to the f-th cycle of round I. near A time slot is used to ensure access to f. near The corresponding grid.

[0115] Analysis shows that the insertion operation of the time wheel data structure only requires placing the data in the corresponding position according to the index, so the TC of insertion is O(1); as the pointer rotates, the data will be directly found and deleted, so the TC of search and deletion is O(1); the rollback operation is to move the pointer to a specific position, which can be achieved by direct assignment, so the TC of rollback is also O(1). Theoretically, the proposed time wheel search has a significant improvement in the computational efficiency of the A* algorithm compared to array search, ordered array search, and min-heap search.

[0116] After finding the node with the minimum cost, the process is repeated until the destination is found, thus obtaining a complete path node for the car.

[0117] Step 3: Based on the connection determination method, identify redundant nodes and retain the optimized nodes as key guiding points for DWA;

[0118] As defined in the heading function of the DWA algorithm, the heading function reaches its maximum value when the heading angle of the autonomous vehicle equals the tilt angle of the line connecting the vehicle and the target point. However, due to the complexity of the map environment, obstacles often exist along the line connecting the vehicle and the target point, and the number of obstacles increases with the distance between the vehicle and the target point. This means that simulated trajectories with high heading scores in the early stages of the DWA algorithm often deviate significantly from the theoretically optimal path. Furthermore, each step of the DWA algorithm's deployment affects future decisions, and the cumulative propagation of errors continuously amplifies the deviation between the solved path and the optimal path, easily leading the vehicle into local optima.

[0119] The keypoint guidance strategy proposed in this paper effectively solves the above problems. The main idea is as follows: the set of keypoints on the global path nodes obtained by the TWA* algorithm is used as the candidate target point sequence for the DWA algorithm. By continuously rotating the keypoints in the target point sequence, the autonomous vehicle is guided to quickly approach the destination. Since the candidate target point sequence consists of nodes on the globally optimal path, the sequence itself possesses global optimality. Furthermore, the distance between the autonomous vehicle and the keypoints is relatively short, so the simulated trajectory with a higher heading score is usually the potentially optimal path. Therefore, the path deviation and error accumulation problems existing in the original DWA algorithm can be effectively avoided. The TWA* algorithm obtains too many nodes on the global path. Directly using these as candidate target point sequences would increase the computational cost of the DWA algorithm. If the candidate sequence is too sparse, this problem cannot be solved. This paper proposes a connection determination method to obtain the optimal candidate target point sequence. The specific steps are as follows.

[0120] (1) Use the TWA* algorithm to obtain the global path node set K, as shown in the appendix. Figure 9 As shown in (a), define the Left pointer to point to the address of the first element of K, and the Right pointer to point to the address of the second element of K.

[0121] (2) Determine if the line connecting Left and Right passes through an obstacle. If so, Right points to the next node until the line connecting Left and Right no longer passes through an obstacle. If the line passes through an obstacle, it means that Left cannot directly reach Right. At this time, the farthest node that Left can directly reach is Right-1. Therefore, pop the Right-1 node into the key point set κ and adjust the Left pointer to Right-1. Determine if Right exceeds the tail address of K. If so, terminate the loop; otherwise, repeat step (2).

[0122] (3) The κ obtained in step (2) is shown in the appendix. Figure 9 As shown in (b), pointers A, B, C, and D are defined to point to the address of the first element of κ, B to the address of the second element of κ, C to the address of the third element of κ, and D to the address of the fourth element of κ.

[0123] (4) The line AB intersects the line CD at point E, as shown in the attached diagram. Figure 9 As shown in (c). Determine if line segments AE and ED are separated from obstacles. If so, A can directly reach D via point E without passing through points B and C. Therefore, points B and C are unnecessary nodes on κ, so delete them and insert point E after point A. At this time, adjust the pointer of A to point to E, and move the pointers of B, C, and D sequentially to the right. Otherwise, consider points B and C to be necessary nodes on κ and keep them unchanged. At this time, move the pointers of A, B, C, and D one position to the right. Determine if D points to the tail address of κ. If so, terminate the loop; otherwise, repeat step 4. The final κ is shown in the attached figure. Figure 9As shown in (d), κ will be used as the candidate target point sequence for the DWA algorithm.

[0124] Step 4: Verify whether the obtained global path key nodes meet the requirements;

[0125] In the previous section, the proposed connection determination method effectively obtained the key points of the global path. These key points, as candidate target sequences, will become the target points in the DWA algorithm path search process according to certain rules. During the key point rotation process, the requirements of stable and efficient driving of the autonomous vehicle need to be met.

[0126] (1) Stability Analysis: In the DWA algorithm, the heading angle function heading(v,w)=π-|θ-δ| specifically refers to the heading angle of the unmanned vehicle at the simulated trajectory terminal, and δ is the tilt angle of the line connecting the simulated trajectory terminal and the destination. When the connecting line is collinear with the line containing the heading angle, there are two cases: 1) The simulated trajectory terminal is ahead of the target point, at which time |θ-δ|=π is satisfied, and the heading function reaches its maximum value, as shown in the appendix. Figure 10 (a) Position A is shown in the diagram; 2) The simulated trajectory terminates after the target point, at which point |θ-δ|=π is satisfied, and the heading function reaches its minimum value, as shown in the appendix. Figure 10 (a) Position B is shown. As can be seen above, when the autonomous vehicle is close enough to the target point, if the simulated trajectory is too long and exceeds the target point, its heading term will reach its minimum value. The algorithm will tend to select simulated trajectories that just fall on the endpoint. That is, as the autonomous vehicle approaches the target, the linear velocity corresponding to the highest-scoring simulated trajectory will decrease, and eventually, the autonomous vehicle will stop at the target point under the guidance of the heading function. The algorithm should avoid the autonomous vehicle stopping at key points in the middle or experiencing deceleration to ensure smooth driving. From the above analysis, to satisfy the conditions described in case 1), the simulated trajectory endpoint should always remain before the target point. This will prevent the autonomous vehicle from stalling due to being too close to the key point. Therefore, when the autonomous vehicle is close to the key point G... t If the distance is less than R, the target point should be moved to the next key point G. t+1 As attached Figure 10 As shown in (b), parameter R is related to the parameter settings of the autonomous vehicle and the DWA algorithm, as shown in the following formula:

[0127] R = mΔ t v max

[0128] In the formula, m is the number of sampling steps for the simulated trajectory, Δ t v is the sampling interval. max This represents the maximum linear velocity of the autonomous vehicle.

[0129] (2) Efficiency Analysis: When there are no obstacles between the autonomous vehicle and the target, the autonomous vehicle can directly reach the target along the connecting line. At this time, the simulated trajectory with the highest heading score is the optimal trajectory. When the autonomous vehicle encounters a situation during the planning process where it can directly reach the key point G, the optimal trajectory is determined by the following criteria: t+1 When the location is key point G t It will become a non-essential node and does not need to reach point G first. t Only then can we reach G t+1 As attached Figure 11 As shown in P1 in (a), in this case, the process can jump to the key point G. t+1 However, in the actual planning process of autonomous vehicles, there are situations where deviations from the optimal route lead to the loss of directly reachable key points, as shown in the attached figure. Figure 11 As shown at position P2 in (b), the autonomous vehicle deviates from the guidance of key point G1 and cannot reach any key point directly. At this time, key points G1 to G2 that have not yet completed rotation are still in use. n The keypoint G3 with the highest distance-to-heading index ρ is obtained. Then, the TWA* algorithm is used to locally reconstruct the path between P2 and G3, and the keypoint G is extracted from the path between P2 and G3 using a keypoint selection strategy. p Driverless cars can use G p The point guides the reacquisition of the directly reachable global keypoint G3, as shown in the attached figure. Figure 11 As shown in (b).

[0130]

[0131] The numerator in the formula is consistent with the definition of the heading function, and the denominator is the square root of the Euclidean distance between the autonomous vehicle and the key point. The higher the orientation of the autonomous vehicle and the closer the key point is, the more likely it is to become the target point for local reconstruction.

[0132] Step 5: Use the DWA algorithm to perform local path planning and obtain the set of motion points.

[0133] In the third step, we obtained the key nodes of the global path, and in the fourth step, we verified these key nodes. Therefore, they can be used for key point guidance in the DWA algorithm. The DWA algorithm is a local path planning method that provides an intuitive understanding of the map environment of the autonomous vehicle from a velocity space perspective. Its workflow is as follows: considering the constraints of various conditions on velocity and angular velocity at time t, we derive the velocity and angular velocity window V that the autonomous vehicle can reach at time t. win Discretize it, and combine the discretized velocities and angular velocities; the unmanned vehicle traverses all combinations and simulates moving forward m Δ steps according to the given motion model. t The duration is used to obtain the simulated trajectory set τ, which is a series of point sets; the evaluation function gives the score of all simulated trajectories in the simulated trajectory set τ, and the trajectory τ with the highest score is selected. bThe corresponding combination; the time Δ for driving the autonomous vehicle forward using this combination. t The process continues until time t+1 is reached; this cycle repeats until the end. At time t, the driverless car's V... win Constrained by itself and its surrounding environment, the following three constraints need to be considered:

[0134] First, the limit velocity and angular velocity constraints, i.e.

[0135] V lim ={(v,w)|v∈[v min ,v max ]∧w∈[w min ,w max ]}

[0136] Second, acceleration-limited velocity-angular velocity constraints.

[0137]

[0138] Third, the speed and angular velocity constraints for braking distance limitations.

[0139]

[0140] Above, v min v max For the limiting linear velocity, w min w max This is the limiting angular velocity. cu w cu The current linear velocity and angular velocity are... For the limit linear acceleration, Let be the limiting angular acceleration. `dist(v,w)` is the closest distance between the simulated trajectory corresponding to the velocity-angular velocity combination (v,w) and the obstacle. The first and second equations above consider the effect of the autonomous vehicle's own performance on V. win The third equation considers the constraints on velocity and angular velocity related to driving safety. Finally, at time t, V... win Represented as:

[0141] V win =V lim ∩V acc ∩V dis

[0142] Details are as attached Figure 12 As shown. The evaluation function comprises three sub-functions, which comprehensively consider three factors: the autonomous vehicle's speed, obstacle collision risk, and the autonomous vehicle's heading, as detailed below:

[0143] G(v,w)=σ(αheading(v,w)+ηdist(v,w)+γvel(v,w))

[0144] Where heading(v t ,w t )=π-|θ t -δ t |, as attached Figure 13 As shown, θ represents the heading angle of the unmanned vehicle, and δ is the angle between the line connecting the unmanned vehicle and the target point and the positive x-axis.

[0145] `dist(v,w)` represents the Euclidean distance from the simulated trajectory to the nearest obstacle, `vel(v,w)` represents the linear velocity of the autonomous vehicle, and `α`, `η`, and `γ` are three weighting coefficients. As shown above, the evaluation function is composed of sub-functions with different dimensions. The normalization function `σ()` in the formula represents dimensionless learning, which can unify data with different dimensions under the same reference frame for combination or comparison, thereby avoiding evaluation bias caused by different data scales. Specifically:

[0146]

[0147] dist(v i ,w j ) and vel(v i ,w j Perform the same normalization operation.

[0148] The unmanned vehicle obtains a simulated trajectory based on a uniform motion model. Under the assumptions of this motion model, the magnitudes of the unmanned vehicle's linear velocity and angular velocity remain constant, and the change in the direction of the linear velocity is linearly related to time. To simplify the model and speed up the calculation, it can be assumed that the velocity direction remains constant within a small time interval. Therefore, the uniform motion model can be discretized as shown in the following formula.

[0149]

[0150] Guided by the global path key points obtained by the TWA* algorithm, the DWA algorithm is then executed repeatedly for calculation, ultimately resulting in a series of local motion point sets, i.e., local paths.

[0151] Step Six: The autonomous vehicle reaches the destination, completes path planning, and obtains the planned route.

[0152] At this point, the intelligent vehicle has completed the global and local paths and can reach its destination after some calculations. To verify the performance of the autonomous vehicle's global optimal path planning based on the TWA*-DWA algorithm, a comparative simulation experiment of DWA algorithm path planning with and without the TWA* algorithm key point guidance strategy will be conducted. The simulation environment is attached. Figure 14As shown, the map size is 60m × 60m, with the bottom left corner dot as the starting point and a pentagram as the ending point. Geometric shapes represent obstacles, including regular polygons and circles. The size and number of obstacles are randomly generated within a certain range. This paper first generates the performance of 35 sets of different weight parameters on 100 random maps under the conditions of 0.1 resolution, α + η + γ = 1, and the three weight parameters not being 0. (See appendix...) Figure 15 It can be seen that the fifth set of weight parameters performs best in the random map environment, with a reach rate of 78%. Therefore, this paper will compare the path planning results of the classic DWA algorithm and the TWA*-DWA algorithm under the optimal fixed weight parameters [α,η,γ]=[0.1,0.5,0.4]. The data comparison under 100 randomly generated maps is shown in Appendix Table 2. The TWA*-DWA algorithm has a reach rate of 89%, which is 11 percentage points higher than the classic DWA algorithm; the average path length is 95.04m, and the path efficiency is improved by 2.78%; the average number of steps is 257.37, and the average step cost is reduced by 2.80%. Among the maps reached by both algorithms, there are a total of 74 identical maps. The maps reachable by the improved algorithm basically cover the maps reachable by the classic algorithm, with a coverage rate of 94.87%. The specific performance of these 74 maps is shown in Appendix Tables 3 and 4. Figure 16 As shown, the TWA*-DWA algorithm has an average path length of 94.79m on these 74 maps, which is 4.28% more efficient than the classic DWA algorithm; the average step cost is 256.70, which is 2.82% less.

[0153] Table 2 Comparison of Simulation Results

[0154]

[0155] Table 3 Comparison of Simulation Results for the Same Map

[0156]

[0157] Appendix Figure 17 The diagram shows the planning results of the classic DWA algorithm and the TWA*-DWA algorithm on the same map. It can be seen that the autonomous vehicle path planning result based on the classic DWA algorithm gets stuck in a local optimum and fails to find the target point, while the result based on the TWA*-DWA algorithm finds a globally relatively optimal path. The nodes on the path in the diagram are the candidate target point sequence.

[0158] The above is a preferred embodiment of the present invention. Any changes made to the technical solution of the present invention that do not exceed the scope of the technical solution of the present invention shall fall within the protection scope of the present invention.

Claims

1. A TWA*-DWA global optimal path planning method based on an improved A* algorithm and a DWA algorithm, characterized in that, Includes the following steps: Step 1: Initialize parameters and read the current coordinates of the autonomous vehicle, the coordinates of the obstacles, and the target point; Step 2: Use the TWA* algorithm, call the time wheel method to pop the node with the smallest cost f in the A* algorithm, and calculate iteratively to obtain the global path node; Step 3: Eliminate redundant nodes based on the connection determination method, and leave the optimized nodes as key guiding points for DWA; Step 4: Verify whether the obtained key nodes of the global path meet the requirements; Step 5: Use the DWA algorithm to perform local path planning and obtain the set of motion points; Step 6: The driverless vehicle reaches the destination, completes path planning, and obtains the planned route; In step two, the TWA* algorithm uses a time wheel method to improve upon the A* algorithm's method of finding the node with the minimum cost f. The time wheel method employs a hierarchical time wheel, meaning it uses a time wheel with a period of... Round I and its cycle are In the second wheel, the pointer of the first wheel rotates one cycle, and the pointer of the second wheel rotates one position. The specific implementation is as follows: (1) Initialization: Each time slot in round I is initialized as a queue, and the time slots in round II are initialized as a dictionary; (2) Insertion: Assume the cost of the data to be inserted is ,like If so, the data is directly inserted into the queue of the corresponding time slot in round I; if Then add this data to the second round. In the dictionary of time slots, the key is The value is data; when the dictionary of the time slot pointed to by the pointer of round II is not empty, it means that the data in the slot has less than one cycle of round I before it needs to be executed. Therefore, it is moved to round I. The specific operation is to read the key of the corresponding dictionary, the value of the key corresponds to the position in round I, and then access the value through the key and insert it into the corresponding queue. After the data transfer is completed, the dictionary of the corresponding time slot in round II is set to empty, and the data insertion is completed. (3) Search: The A* algorithm needs to use the grid with the lowest f value as the expansion point. Therefore, as the time wheel rotates, when the queue corresponding to the time slot pointed to by the I wheel is not empty, the data in the queue is the target expansion point of the A* algorithm. Therefore, the corresponding data is directly popped from the tail of the queue, and the deletion operation is completed at the same time. (4) Rewind: The time slot pointed to by the pointer in round I is the location where the grid with the lowest f value is stored. When applied to the A* algorithm, there is a problem: the lowest f value is... low When accessing information about neighboring rasters using the corresponding raster as an extension point, if the f value of the accessed neighboring raster is... near If the value is less than the value currently pointed to by the pointer, then that value will be inserted at the position before the pointer. Since the pointer keeps moving forward, the next step will not be able to access that data. Therefore, during the data insertion process, f needs to be... low with f near Compare the sizes, if f near < f low Then the pointer should be turned back to the f-th cycle of round I. near A time slot is used to ensure access to f. near The corresponding grid; After finding the node with the minimum cost, the process is repeated until the destination is found, thus obtaining a complete autonomous vehicle path node.

2. The TWA*-DWA global optimal path planning method based on the improved A* algorithm and DWA algorithm as described in claim 1, characterized in that, Step three is implemented as follows: (1) Use the TWA* algorithm to obtain the global path node set K, and define the Left pointer to point to the address of the first element of K, and the Right pointer to point to the address of the second element of K; (2) Determine whether the line connecting Left and Right passes through an obstacle. If so, Right points to the next node until the line connecting Left and Right no longer passes through an obstacle. When the line passes through an obstacle, it means that Left cannot directly reach Right. At this time, the farthest node that Left can reach is Right-1. Therefore, pop the Right-1 node into the key point set. And adjust the pointer of Left to Right-1; check if Right exceeds the tail address of K. If it does, terminate the loop; otherwise, repeat step (2). (3) Step (2) yields Define pointer A to point to Address of the first element, B points to The address of the second element, C points to The address of the third element, D points to The address of the fourth element; (4) The line AB intersects the line CD at point E. Determine whether line segments AE and ED are separate from the obstacle. If so, A can directly reach D through point E without passing through points B and C. Therefore, points B and C are... For unnecessary nodes, delete both and insert point E after point A. Then adjust A to point to E, and move pointers B, C, and D sequentially. Conversely, if points B and C are considered unnecessary, then points B and C are considered necessary nodes. The necessary nodes remain unchanged, and the pointers of A, B, C, and D are shifted one position to the right; then check if D points to... If the address of the tail is found, the loop terminates; otherwise, step 4 is repeated; the final result is obtained. , This will be used as a candidate target point sequence for the DWA algorithm.

Images