Robot path planning method based on improved Theta algorithm

Abstract

The invention discloses a robot path planning method based on an improved Theta * algorithm, and the method comprises the following steps: building a three-dimensional space-time map based on a search space of a robot target task, converting a dynamic obstacle coordinate into a static obstacle coordinate, and obtaining a three-dimensional space-time obstacle map; an improved Theta * algorithm is adopted, and path planning of the robot from a starting node to a target node is carried out on the three-dimensional space-time obstacle map; wherein the Theta * algorithm improvement comprises improvement of node properties, improvement of a node expansion mode, improvement of a cost function and improvement of an LOS judgment mechanism from two dimensions to three dimensions. According to the invention, by introducing the time dimension and improving the Theta * algorithm, path planning is expanded from a two-dimensional space to a three-dimensional time-space domain; through dynamic environment perception and path optimization, the obstacle avoidance capability and path quality of the robot in a dynamic environment are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and more particularly to a robot path planning method based on an improved Theta* algorithm. Background Art

[0002] With the rapid development of intelligent manufacturing and logistics automation, mobile robots (Automated Guided Vehicles, AGVs), as the core transportation carriers in the modern industrial system, have become a key technology for improving production efficiency and system flexibility due to their autonomous navigation capabilities in dynamic environments.

[0003] However, traditional path planning algorithms face problems such as insufficient real-time obstacle avoidance, tortuous paths, excessive energy consumption, and poor adaptability to dynamic obstacles in dynamic and complex scenarios. These problems seriously restrict the application efficiency of AGVs in highly dynamic scenarios such as warehousing and logistics and flexible production lines.

[0004] Path planning in dynamic environments has long been a major challenge for both academia and industry in AGV navigation systems. While traditional path planning algorithms (such as A* and Dijkstra) perform well in static scenarios, their limited two-dimensional search logic makes them incapable of handling the real-time changes of dynamic obstacles, leading to frequent failures of planned paths, delayed obstacle avoidance, and redundant trajectories. Existing theta* algorithms also have limitations in practical applications, particularly when demanding computational resources and path quality.

[0005] Therefore, it is an urgent problem for those skilled in the art to propose a trajectory planning method that can achieve efficient, safe and globally optimized trajectory planning in a dynamic environment with spatiotemporal coupling. Summary of the Invention

[0006] In view of this, the present invention provides a robot path planning method based on an improved Theta* algorithm, which combines the three-dimensional spatiotemporal Theta* algorithm with a dynamic obstacle avoidance mechanism to achieve AGV obstacle avoidance and path planning in complex dynamic scenes.

[0007] In order to achieve the above object, the present invention adopts the following technical solutions:

[0008] A robot path planning method based on an improved Theta* algorithm includes the following steps:

[0009] S1. Construct a 3D spatiotemporal map based on the search space of the robot's target task, convert the dynamic obstacle coordinates into static obstacle coordinates, and obtain a 3D spatiotemporal obstacle map;

[0010] S2. Using an improved Theta* algorithm, the robot plans a path from a starting node to a target node on a three-dimensional space-time obstacle map; wherein, the improvements to the Theta* algorithm include improvements to node properties, improvements to node expansion methods, improvements to cost functions, and improvements to the LOS determination mechanism from two dimensions to three dimensions.

[0011] Furthermore, step S1 specifically includes:

[0012] Obtaining a search space for the robot's target task, adding a time axis to the two-dimensional plane of the search space to obtain a three-dimensional space-time map and rasterizing it;

[0013] The predicted trajectory of dynamic obstacles changing over time is mapped into a three-dimensional space-time grid, and the two-dimensional dynamic obstacles are transformed into static obstacles in three-dimensional space-time, thus obtaining a gridded three-dimensional space-time obstacle map.

[0014] Furthermore, the improvement of the node properties specifically includes:

[0015] Map the position coordinates of the robot's current node and the current time to the three-dimensional space-time obstacle map corresponding to the robot's target task;

[0016] When a node is expanded, the current node coordinates are updated by a fixed time step.

[0017] Furthermore, the improvement of the node expansion method specifically includes:

[0018] Convert the coordinates (x±Δx, y±Δy) of the two-dimensional space Theta* algorithm to (x±Δx, y±Δy, t+Δt) in three-dimensional space; where x and y represent the current position; t represents the current time; Δx and Δy represent the spatial step, and Δt represents the time step;

[0019] When a node is expanded, the expansion domain is expanded from the eight-neighborhood domain in two-dimensional space, and the time neighborhood is added as the ninth domain, converting it into a nine-neighborhood expansion in three-dimensional space;

[0020] In dynamic neighbor selection and conflict detection, conflict detection is performed on the extended domain of each node; if the extended domain node contains obstacles at the corresponding time, the corresponding domain node is excluded; if the extended domain node does not contain obstacles at the corresponding time, the corresponding domain node is used as a feasible node for the current node's extended trajectory.

[0021] Furthermore, the improvement of the cost function specifically includes:

[0022] The improved cost function includes the cumulative cost from the starting point to the current node and the heuristic estimated cost from the current node to the end point when the robot performs the target task in three dimensions;

[0023] When performing path planning, the robot's path trajectory length, time efficiency and dynamic obstacle avoidance requirements are balanced by minimizing the improved cost function.

[0024] Furthermore, the improvement of the LOS determination mechanism from two dimensions to three dimensions specifically includes:

[0025] Introducing time dimension constraints, the spatiotemporal visibility between moving objects and obstacles in a dynamic environment is calculated in real time, ray casting is performed in the three-dimensional state space, and in the node expansion phase, the visibility of the current node's expansion node and the current node's parent node is detected;

[0026] If it is visible, it is connected through the path of the parent node, which has a lower cost. The parent node of the expanded node is updated to the parent node of the current node to avoid the situation where the driving cost is higher by passing through the current node. If it is not visible, the next node of the current node is determined by the improved cost function.

[0027] By introducing the spindle selection criterion to optimize the LOS detection direction and reduce invalid calculation steps, a dual error compensation mechanism is adopted to dynamically correct the detection results, thereby comprehensively improving the computational efficiency and robustness of the LOS algorithm.

[0028] Furthermore, the spindle selection criteria include:

[0029] When detecting path obstacles, the dimension with the largest change is used as the main axis and also as the iteration direction;

[0030] The number of steps in the main axis direction is traversed. In each iteration, the increments of the other two dimensions are dynamically calculated according to the main axis step ratio to avoid traversing the entire space and reduce the number of iterations.

[0031] The dual error compensation mechanism includes:

[0032] For the secondary axis and the third axis except the main axis, perform error term initialization operation;

[0033] According to the difference between the initialization error terms of the secondary axis and the third axis and the preset threshold, iterative compensation is performed respectively to achieve double error compensation.

[0034] Furthermore, in step S2, an improved Theta* algorithm is used to plan the path of the robot from the starting node to the target node on the three-dimensional space-time obstacle map; specifically, the steps include:

[0035] S21, taking the starting node of the robot's target task as the center, generating nine neighboring nodes in the next time plane as candidate nodes;

[0036] S22. On the three-dimensional spatiotemporal obstacle map, query whether the candidate nodes include an obstacle node; if so, remove the corresponding candidate node; if not, add the corresponding candidate node to an expansion queue;

[0037] S23. Calculating the trajectory cost from the feasible nodes in the extended queue to the current node based on the improved cost function;

[0038] S24. Use the improved LOS judgment function to check the feasibility of line of sight between nodes, optimize the node selection of the Theta* algorithm, and reduce the trajectory cost.

[0039] S25. Add feasible nodes that meet the trajectory cost calculation requirements to a priority queue; perform global trajectory cost calculation on the nodes added to the priority queue to generate feasible trajectories that meet the obstacle avoidance requirements;

[0040] Furthermore, step S24 specifically includes:

[0041] For two nodes whose visibility needs to be checked, generate several intermediate points using uniform step-size interpolation;

[0042] For each intermediate point, query its status in the three-dimensional space-time obstacle map;

[0043] If the states of all intermediate points are unoccupied, the line segment between the two nodes is considered feasible, there are no dynamic obstacles, and the two nodes can be directly connected;

[0044] If the state of any intermediate point is occupied, the line segment between the two nodes is considered infeasible and needs to be dynamically adjusted.

[0045] The above technical solution demonstrates that, compared to existing technologies, the present invention provides a robot path planning method based on an improved Theta* algorithm. This method first expands path planning from two-dimensional space to the spatiotemporal domain, introduces the time dimension to construct a three-dimensional search space, and then improves the Theta* algorithm. By reconstructing the cost function and implementing an improved node expansion strategy, the algorithm simultaneously optimizes spatial trajectory and temporal scheduling during path planning, significantly reducing the frequency of path replanning caused by sudden changes in dynamic obstacles.

[0046] The present invention deeply integrates dynamic obstacle trajectory prediction with path planning algorithms; dynamically updates the occupancy status of obstacles at future moments, providing a real-time obstacle avoidance basis for path search; further, combined with an improved path detection mechanism, it is possible to avoid the predicted trajectory of obstacles in advance during the path planning process. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0048] Figure 1 A flowchart of a robot path planning method based on an improved Theta* algorithm is provided in an embodiment of the present invention.

[0049] Figure 2 A comparison diagram of two-dimensional nodes and three-dimensional space-time nodes provided in an embodiment of the present invention.

[0050] Figure 3 A comparison diagram of two-dimensional node expansion and three-dimensional spatiotemporal node expansion provided by an embodiment of the present invention.

[0051] Figure 4 This is a visualization diagram showing the trajectory results of the simulated A* algorithm planning provided by an embodiment of the present invention.

[0052] Figure 5 This is a visualization diagram showing the trajectory planning results of the simulated Theta* algorithm provided in an embodiment of the present invention.

[0053] Figure 6 This is a visualization diagram of the trajectory results of the simulation space-time A* algorithm planning provided by an embodiment of the present invention.

[0054] Figure 7 This is a visualization diagram of the trajectory results of the simulation space-time Theta* algorithm planning provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0055] 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.

[0056] The embodiment of the present invention discloses a robot path planning method based on an improved Theta* algorithm. Figure 1 As shown, the following steps are included:

[0057] S1. Construct a 3D spatiotemporal map based on the search space of the robot's target task, convert the dynamic obstacle coordinates into static obstacle coordinates, and obtain a 3D spatiotemporal obstacle map;

[0058] S2. Using an improved Theta* algorithm, the robot plans a path from a starting node to a target node on the three-dimensional space-time obstacle map; wherein the Theta* algorithm improvement includes an improvement in node properties, an improvement in node expansion mode, an improvement in cost function, and an improvement in LOS determination function.

[0059] This embodiment is applied in a fully automated warehouse logistics factory. The robot task of this embodiment is to transport goods from warehouse area A to warehouse area B. Dynamic obstacles include forklifts and other robots. Forklifts are used to carry goods within the warehouse and may block the path of the AGV. Other robots also carry out transportation tasks within the warehouse and may collide with the robot in this embodiment. When planning the path for the robot's predetermined task of transporting goods from warehouse area A to warehouse area B, the robot first obtains a corresponding two-dimensional plane map based on the robot's task range, and adds a time axis to obtain a three-dimensional spatiotemporal map. The predicted trajectory of the dynamic obstacle is mapped to the three-dimensional coordinates of the static obstacle to obtain a three-dimensional spatiotemporal obstacle map. Secondly, by improving the Theta* algorithm, the robot's path is planned from the starting node to the target node on the obtained three-dimensional spatiotemporal obstacle map. The spatiotemporal improvements are mainly made to the node definition, expansion method, cost function, and LOS judgment function of the Theta* algorithm to achieve trajectory planning that adapts to dynamic environments.

[0060] Traditional Theta* algorithms only consider path planning on a two-dimensional plane and cannot effectively handle the impact of dynamic obstacles on the planned path. In today's environment, there are almost no purely static environments; dynamic obstacles are always present. Avoiding these dynamic obstacles poses a challenge to path planning algorithms.

[0061] The following is a detailed introduction to the content of this embodiment regarding the improved Theta* algorithm.

[0062] In this embodiment, the improvements to the Theta* algorithm include improvements to node properties, improvements to node expansion methods, improvements to cost functions, and improvements to the LOS determination mechanism from two dimensions to three dimensions.

[0063] 1. Improvements to node properties, including:

[0064] In the traditional Theta* algorithm, nodes only contain two-dimensional position coordinates (x, y). To adapt to the dynamic environment in which robots operate, a time attribute t is added, expanding the node definition to a three-dimensional form (x, y, t). Here, x and y represent the spatial coordinates of the node on the two-dimensional plane, and t represents the time at which the node is located.

[0065] Define time attributes to improve node properties. Each node records the current time t when expanding, which is used to verify the motion state of dynamic obstacles. Time attributes are cumulatively updated with a fixed time step Δt.

[0066] Through time synchronization, the time attributes of the trajectory planning process are ensured to be consistent with the movement of dynamic obstacles. For example, assuming the starting point is (1,1,0) and the target point is (5,5,T), the update time for each expansion step in the trajectory planning is Δt = 1 unit time. Figure 2 The figure shows the difference between the 2D and 3D nodes in which the robot performs its tasks. 2D nodes have only spatial attributes, while 3D nodes incorporate temporal attributes. Path planning and motion prediction for dynamic obstacles occur in the same spatiotemporal domain.

[0067] 2. Improvements to node expansion methods, including the following:

[0068] The node expansion method determines the algorithm's planning performance in dynamic environments. The traditional Theta* algorithm only considers neighbor node expansion on a plane. Spatiotemporal improvements require three-dimensional expansion, combining the time dimension. This embodiment improves the node expansion method.

[0069] First, a three-dimensional extension of the time constraint is performed.

[0070] When expanding the current node of the robot's position, the feasibility of the spatial dimension (x, y) is checked, while ensuring that the expanded node matches the motion state of the dynamic obstacle according to the time dimension t.

[0071] The extended node set is improved from the two-dimensional Theta* (x±Δx, y±Δy) to the three-dimensional space (x±Δx, y±Δy, t+Δt), where Δx, Δy are the spatial step lengths and Δt is the time step length. The extended domain is improved from the eight-neighborhood extension in the two-dimensional space to the nine-neighborhood extension in the spatiotemporal domain. As for why it is not a 26-neighborhood extension, this involves the unidirectional attribute of time, and only the nine-neighborhood extension in the future moment is extended. Therefore, after the improvement to the spatiotemporal domain, the complexity of the That* algorithm does not increase, and it still maintains the same efficiency as the traditional Theta* algorithm. Figure 3 The figure shows the difference between the two-dimensional node expansion and the three-dimensional space-time node expansion of the robot position, showing the significance of the extra ninth field. Figure 3 The node (3,3,3) of the neighborhood is at the same position at the next moment, which means that there will be no movement at the next moment.

[0072] Secondly, dynamic neighbor selection and conflict detection are performed.

[0073] The range of neighbor nodes is adjusted based on obstacle motion prediction. If an obstacle enters a neighbor node area at the predicted time position, the node is excluded. Conflict detection in the dynamic environment is performed on each expanded node to ensure that the expanded trajectory does not collide with dynamic obstacles.

[0074] 3. Regarding the improvement of the cost function, specifically including:

[0075] Cost functions are used to evaluate the quality of a robot's path planning. For the spatiotemporal Theta* algorithm, the cost function must comprehensively consider the trajectory's spatial distance, time consumption, and obstacle avoidance performance.

[0076] Improved cost function: Let the current node n e =(x e ,y e ,t e ), the parent node is n p =(x p ,y p ,t p ), the target node is n g =(x g ,y g ,t g ).

[0077] Cost function f(n i ) is expressed as:

[0078] f(n e )=g(n e )+h(n e )

[0079] g(n e ) represents the distance from the starting point to the current node n e The cumulative cost, including spatial distance and time cost, is expressed as follows:

[0080]

[0081] h(n e ) represents the heuristic estimate of the cost from the current node to the target node, considering space and time, and is expressed as:

[0082]

[0083] Among them, λ is the time cost weight, and μ is the time delay penalty coefficient.

[0084] In path planning, the total cost f(n e ) to balance the robot path trajectory length, time efficiency and dynamic obstacle avoidance requirements during robot movement as the optimization goal.

[0085] 4. Improvements to the LOS determination mechanism from 2D to 3D, specifically including:

[0086] The LOS detection mechanism, a core component of the improved Theta* algorithm, optimizes path planning by determining line visibility in three-dimensional space. The spatiotemporal LOS detection algorithm (Line of Sight 3D Time) proposed in this embodiment expands upon the traditional two-dimensional LOS algorithm by performing multi-dimensional expansion and dynamic optimization. This embodiment explains its improved mechanism from three perspectives: mathematical modeling, algorithmic flow, and spatiotemporal characteristics.

[0087] First, dimension expansion and spatiotemporal joint modeling;

[0088] The traditional two-dimensional LOS algorithm is based on the detection of straight line paths in Euclidean space, and its mathematical description is:

[0089]

[0090] in, is a set of static obstacles; Bresenham (p1, p2) is a set of discrete straight line path points from p1 to p2 generated by the Bresenham algorithm; (x, y) is the coordinate of a discrete point on the straight line path; p1 and p2 represent the coordinates of the two-dimensional starting point and end point.

[0091] This embodiment extends it to the spatiotemporal domain and defines the spatiotemporal trajectory as:

[0092]

[0093] Among them, n1 and n2 represent the space-time states of the three-dimensional starting point and end point, (x, y, t) represents the three-dimensional coordinates after the time dimension is extended, and the two-dimensional coordinates of the normal plane are represented by xy, which is equivalent to not considering the time dimension; in order to facilitate the distinction between various formulas, the xyt subscripts in different formulas are different, defined as (x1, y1, t1) and (x2, y2, t2); among them, (x1, y1, t1) represents the three-dimensional state of the starting point, that is, the position and time state, and (x2, y2, t2) represents the three-dimensional state of the end point; k represents the normalization parameter, k∈[0,1].

[0094] After introducing the time dimension constraint, the obstacle detection condition is strengthened to:

[0095]

[0096] in, represents a set of dynamic obstacles that change over time; P is the space-time trajectory.

[0097] This extension enables the algorithm to detect trajectory conflicts of moving obstacles in space and time, resolving the contradiction between trajectory continuity and safety in dynamic environments.

[0098] Secondly, three-dimensional Bresenham algorithm optimization;

[0099] The traditional Bresenham algorithm controls line generation in two-dimensional space through a single error term:

[0100] e=2Δy-Δx

[0101] Where e represents the error term; Δx and Δy represent the increments of the trajectory in the x- and y-axis directions, respectively.

[0102] The improved algorithm proposes a dual error compensation mechanism for the robot's three-dimensional space-time trajectory. These two mechanisms are mainly used to make the calculation of dynamic and static obstacles more accurate and faster. The mathematical description is as follows:

[0103] First, about the spindle selection criteria.

[0104] In the unoptimized 3D spatiotemporal path detection, the algorithm needs to traverse all 3D discrete points from the starting point to the end point, that is, check whether the coordinates (x, y, t) of each point conflict with the dynamic obstacle. If the step size of each dimension is n, there are a total of n 3 Points need to be checked.

[0105] By selecting the dimension with the largest change as the principal axis and the iteration direction, the three-dimensional problem is simplified to a one-dimensional linear traversal.

[0106] Assume that the increments of the trajectory in the x-, y-, and t-axis directions are Δx, Δy, and Δt respectively, and the principal axis α is determined by the following formula:

[0107]

[0108] This criterion reduces the number of iterations to K = max(Δx, Δy, Δt) + 1, that is, only the steps in the main axis direction need to be traversed. In each iteration, the increments of the other two dimensions are dynamically calculated according to the main axis step ratio, avoiding the full space traversal. The time complexity is reduced from O(n 3 ) is optimized to O(n).

[0109] Secondly, regarding the double error compensation equation;

[0110] For the secondary axis β and the third axis γ, the error terms are first initialized;

[0111]

[0112] When walking along the main axis, the coordinates of the secondary axes, β and γ, are updated synchronously. The following is the update rule of the other two axes when stepping along the main axis (α); among them, α (main axis), β, γ (secondary axis) are the identifiers of the main axis and secondary axis; e β ,e γ They represent the error terms of the secondary axis step; s represents the step direction, s β represents the step along β; Δ represents the absolute change in each dimension, Δ α Indicates the increment in the main axis direction.

[0113] During the iteration process, the update is performed according to the following rules:

[0114] When e β >0, β←β+s β , e β ←e β -2Δ α ; If eβ>0, the secondary axis β needs to be stepped up.

[0115] When e γ >0,γ←γ+e γ , e γ ←e γ -2Δ α ; If eγ>0, the secondary axis γ needs to be stepped forward.

[0116] Update each step; e β ←e β +2Δ β , e γ ←e γ +2Δ γ ; After each update step, update the error term.

[0117] The improved spatiotemporal Theta* algorithm performs trajectory planning in the robot's three-dimensional spacetime. This 3D spacetime is formed by upscaling a 2D plane and the spacetime axis. The trajectories of dynamic obstacles are mapped into this 3D spacetime, transforming the 2D dynamic obstacles into static obstacles in 3D spacetime. During the node expansion process of the Theta* spacetime algorithm, the spacetime occupancy grid map serves as a core tool for processing dynamic obstacle trajectory information. By mapping the predicted trajectories of dynamic obstacles into the 3D spacetime grid, the spatiotemporal characteristics of dynamic obstacles are staticized, allowing them to be processed uniformly with traditional static obstacles. This mapping not only reduces the complexity of dynamic obstacle processing but also enables the improved Theta* algorithm to efficiently complete trajectory search within the same framework.

[0118] The following details the specific steps of this embodiment when performing path planning for a given robot task:

[0119] Step 1: Perform environmental perception and map construction.

[0120] This embodiment uses sensors to perceive the surrounding environment, obtain information about static and dynamic obstacles, and construct the perceived information into a three-dimensional spatiotemporal occupancy grid map, which contains the occupancy status of static and dynamic obstacles at future moments, that is, a gridded three-dimensional spatiotemporal obstacle map.

[0121] The robot's starting point and target point are determined and represented as three-dimensional space-time nodes including position and time attributes.

[0122] Step 2: Integrate the 3D Theta* spatiotemporal algorithm node expansion mechanism with dynamic obstacles to perform path planning.

[0123] First, starting from the robot start node, nine neighborhood nodes are generated in the next time plane as candidate nodes.

[0124] Regarding, candidate node generation; this embodiment takes the current node of the robot as the center and generates nine neighborhood nodes in the next time plane.

[0125] The candidate domain node set is:

[0126]

[0127] Among them, δs is the spatial resolution and δt is the time step.

[0128] Next, perform a grid status query.

[0129] In the spatiotemporal occupancy grid map of this embodiment, query candidate node n i (x i ,y i ,t i ) state G(n i ).

[0130] If G(n i )=1, it means that the node is occupied by obstacles such as forklifts or other robots, and the node is considered infeasible; if G(n i )=0, it means idle, then the node is feasible and joins the expansion queue.

[0131] Next, perform cost calculation and node selection; specifically, the following steps:

[0132] The original Theta* algorithm cost function evaluates two-dimensional planning capabilities and does not incorporate a time-dependent cost function. Therefore, it has been improved to a three-dimensional cost function, used to evaluate the quality of paths in path planning. For the spatiotemporal Theta* algorithm, the cost function comprehensively considers the trajectory's spatial distance, time consumption, and obstacle avoidance performance.

[0133] After the trajectory of a dynamic obstacle is mapped to the spatiotemporal occupancy grid, it is processed during the expansion process in the same way as static obstacles. This process avoids the complex dynamic trajectory collision detection operation and directly determines whether a node is occupied by an obstacle in the future time step. In this way, the improved Theta* algorithm of this embodiment can naturally avoid the influence of dynamic obstacles when expanding nodes in robot path planning, thereby generating a feasible trajectory that meets the obstacle avoidance requirements.

[0134] In this embodiment, feasible nodes that meet the trajectory cost calculation requirements are added to a priority queue; a global trajectory cost calculation is performed on the nodes added to the priority queue to generate feasible trajectories that meet the obstacle avoidance requirements.

[0135] The spatiotemporal Theta* algorithm, which dynamically adjusts trajectories based on an improved LOS decision function, optimizes path planning in the node expansion phase by detecting the three-dimensional spatiotemporal visibility of the current node, the expanded node, and the current node's parent node. If spatiotemporal visibility exists, an attempt is made to connect the expanded node directly to the current node's parent node, and the parent node relationship is updated after the spatiotemporal feasibility of the path is verified by the improved three-dimensional cost function. If it is not visible, the three-dimensional trajectory cost from the current node to each feasible neighbor node is dynamically calculated using the spatiotemporal expansion cost function. Under the premise of ensuring dynamic obstacle avoidance, the node with the best spatiotemporal comprehensive cost is selected from the expanded queue to continue the search, and finally a spatiotemporal trajectory that meets the requirements of shortest space, optimal time, and dynamic obstacle avoidance is generated.

[0136] Introducing time dimension constraints to reduce the consumption of path planning by optimizing node expansion methods;

[0137] In the traditional Theta* algorithm, visibility detection is used to determine whether there is an unobstructed straight line between two nodes. In the improved Theta* algorithm, this detection mechanism is extended to three-dimensional space-time. The specific steps are as follows:

[0138] First, for the two node space-time coordinates (x a ,y a ,t a ) and (x b ,y b ,t b ), generate several intermediate point space-time coordinates (x c ,y c ,t c );

[0139] The interpolation point calculation formula is:

[0140]

[0141] In the formula, M represents the total number of interpolation points.

[0142] Secondly, query the status of the intermediate point.

[0143] For each interpolation point, query its state G(x c ,y c ,t c ).

[0144] If the state of any point is 1, it means that the point is occupied and the line segment is infeasible.

[0145] Finally, visibility determination is performed.

[0146] If the states of all interpolation points are idle G(x c ,y c ,t c )=0, it is considered that the line segment between the two nodes is feasible, there are no dynamic obstacles, and the two nodes can be directly connected.

[0147] Through the interpolation and state query process described above, the trajectory information of dynamic obstacles is naturally integrated into LOS detection. The impact of dynamic obstacles is reflected in the following: if a dynamic obstacle occupies the position of an interpolation point at a certain time step, the line segment is judged to be infeasible. The LOS detection process can dynamically adjust the trajectory to bypass areas that may be affected by dynamic obstacles. By introducing a spatiotemporal occupancy grid map in LOS detection, the improved Theta* algorithm can effectively achieve the following in trajectory planning: trajectory smoothing and reduction of unnecessary turning points. Obstacle avoidance is also achieved in advance, bypassing areas that may be affected by dynamic obstacles in advance. This combination not only optimizes trajectory quality but also enhances the dynamic adaptability of the algorithm.

[0148] Through the above method, the improved Theta* algorithm fully utilizes the characteristics of the spatiotemporal occupancy grid map in the trajectory planning node expansion and LOS detection process, effectively solving the obstacle avoidance problem of dynamic obstacles.

[0149] This embodiment also conducts simulation experiments of different Theta* algorithms.

[0150] First, the simulation platform and raster map are constructed.

[0151] This experiment uses Python to develop a 2D grid map simulation platform. NumPy (for storing and manipulating grid map data), Matplotlib (for visualizing AGV trajectories and obstacle distribution), and OpenCV are used for data processing and visualization. Matplotlib is also used for dynamic scene simulation. The experimental task is a robotic task: transporting goods from warehouse area A to area B. Forklifts and other robots serve as dynamic obstacles.

[0152] The core functions of the experimental environment include grid map construction, AGV motion modeling, obstacle dynamic simulation, and comparative testing of trajectory planning algorithms. The simulation platform uses a two-dimensional discrete grid map as the AGV motion space. Each grid cell is set to a 1×1 size, and the AGV can only move in eight directions (diagonal movement is allowed). The map data uses a dictionary to store obstacle locations. This dictionary allows the AGV to easily query the location to be explored for obstacles during trajectory planning.

[0153] Secondly, the simulation models the AGV motion model and dynamic obstacle position occupancy.

[0154] The movement mode of AGV in the simulation environment is defined as follows: for two-dimensional A* and two-dimensional Theta*, only the spatial dimension is considered, and the time dimension is not considered. The AGV is allowed to move in eight directions (including diagonal directions). For three-dimensional space-time A* and three-dimensional space-time Theta*, the time dimension is added, indicating that the AGV arrives at a certain position at a certain time. The trajectory prediction of dynamic obstacles is considered, so that the AGV can avoid the predicted position of dynamic obstacles in advance when planning the trajectory. The AGV is allowed to move in eight directions (including diagonal directions) within the spatial range. In addition, the AGV is also allowed not to move. Therefore, in fact, the AGV is allowed to move in nine directions within the spatial range. This improvement adds a speed change function to the AGV, so that the AGV can move in the trajectory search. The optimal speed is selected within the speed range. This allows the AGV to search for the optimal trajectory in three-dimensional space. For the dynamic obstacle motion prediction in the question, this experiment uses the traditional A* algorithm to randomly select two points in the grid map for trajectory planning. After the trajectory is obtained, three-dimensional space-time occupation is performed to simulate the motion prediction of dynamic obstacles, which is used to test the effectiveness of the fusion method and trajectory planning method.

[0155] The operation process of the simulation experiment environment is as follows.

[0156] 1. Map initialization: Load the raster map, generate static obstacles, and set the starting and target points.

[0157] 2. Dynamic obstacle trajectory setting: Use the A* algorithm to generate 10 dynamic obstacle trajectories to simulate the predicted dynamic obstacle trajectories.

[0158] 3. Execute the trajectory planning algorithm. Run 2D A*, 3D A*, 2D Theta*, and 3D Theta* to perform path and trajectory planning.

[0159] 4. Trajectory visualization: Plot the path trajectories of each algorithm to visually compare the planning effects of each algorithm in the same environment.

[0160] 5. Data recording and evaluation: Record the trajectory length, trajectory time, success rate and other indicators of each algorithm, and conduct experimental comparative analysis.

[0161] Reference Figure 4-Figure 7 As shown, this is the result of trajectory visualization, where Figure 4 The trajectory planned by the A* algorithm, Figure 5 The trajectory planned by the Theta* algorithm, Figure 6 The trajectory planned by the spatiotemporal A* algorithm, Figure 7 The trajectory planned for spacetime Theta*.

[0162] The experimental indicators are set as:

[0163] The track length is defined as the actual distance traveled by the AGV from its starting point to its end point. The calculation formula is as follows:

[0164]

[0165] Among them, P h represents the hth node on the trajectory, d(P h ,P h+1 ) represents the Euclidean distance between two adjacent trajectory points (for 8-direction motion, the straight distance is 1 and the diagonal motion distance is r is the number of summary points in the trajectory. This indicator indicates that the shorter the trajectory, the better the global optimality of the planning algorithm.

[0166] The trajectory planning success rate is defined as the ratio of the number of paths that the algorithm successfully finds to reach the target point without collision in a given environment. The calculation formula is as follows:

[0167]

[0168] Where: N success Indicates the number of experiments to successfully find the trajectory, N total Represents the total number of experiments. The higher the success rate, the stronger the adaptability of the algorithm in complex environments and the better its robustness.

[0169] Based on trajectory energy consumption: A trajectory consists of a series of discrete nodes, each representing the AGV's position at a specific moment. Therefore, this experiment uses physics and kinematics to calculate the total energy consumption required for the AGV to execute this trajectory. When executing trajectory planning, the AGV consumes two main sources of energy: 1. Rolling energy consumption, which depends on the trajectory length and ground friction, and 2. Acceleration and deceleration energy consumption, which depends on speed changes. For discrete trajectories, these energy consumptions can be calculated based on physical formulas to obtain the total energy consumption of the trajectory. The following is the calculation method for total energy consumption.

[0170] Driving energy consumption mainly comes from rolling friction and air resistance, and the calculation formula is as follows:

[0171]

[0172] Among them, E rolling Indicates total driving energy consumption; F rolling =μmg, indicating rolling friction; d h It represents the Euclidean distance between the hth to h+1th trajectory points. Since the same car is used and the ground is ideally the same, this term is only related to the length of the trajectory planning.

[0173] Acceleration and deceleration energy consumption calculation method: When the AGV changes speed (acceleration or deceleration) in the trajectory, it consumes additional energy. The acceleration and deceleration energy consumption can be calculated using the kinetic energy change formula as shown below:

[0174]

[0175] Among them, E acceleration represents the total acceleration and deceleration energy consumption, m represents the AGV mass, v h represents the speed of AGV at the hth trajectory point; v h+1 represents the speed of AGV at the h+1th track point; if v h+1 >v h , indicating that AGV acceleration requires additional energy; if v h+1 <v h , indicating that the AGV slows down and some of the energy may be braked or recovered.

[0176] Calculate the total energy consumption. The total energy consumption of the AGV to execute a complete trajectory is calculated as follows:

[0177] E total =E rolling +E acceleration

[0178] The formula expands to the following formula:

[0179]

[0180] Among them, driving energy consumption depends on the track length. The longer the track, the higher the energy consumption. Acceleration and deceleration energy consumption depends on the number of speed changes. The more frequent the speed changes, the higher the energy consumption.

[0181] This experiment compares the experimental results of four algorithms: two-dimensional A* (2D A*), three-dimensional spatio-temporal A* (3D Spatio-Temporal A*), two-dimensional Theta* (2D Theta*), and three-dimensional spatio-temporal Theta* (3D Spatio-Temporal Theta*), and analyzes them based on the following four core indicators.

[0182] (1) Path Length: This measures the quality of the trajectory planned by the algorithm. The shorter the trajectory, the better.

[0183] (2) Computation Time: The time required to complete the planned trajectory. The shorter the time, the higher the efficiency.

[0184] (3) Path Planning Success Rate: The probability that the algorithm successfully finds a feasible trajectory. The higher the success rate, the better.

[0185] (4) Energy Consumption: The energy consumption during trajectory execution. The lower the energy consumption, the better.

[0186] Static environment tests (without dynamic obstacles) are conducted to evaluate the trajectory quality of the algorithm in a fixed obstacle environment; dynamic environment tests (with dynamic obstacles) are conducted to examine the adaptability and energy consumption of the algorithm in a complex environment.

[0187] The experimental results show that 3D Theta* outperforms other algorithms in many aspects, especially in trajectory optimization, dynamic environment adaptability and energy consumption control.

[0188] (1) Optimal trajectory. In both dynamic and static environments, 3D Theta* plans trajectories that are 4.6-4.9% shorter than those planned by 2D A* and 3D A*. The trajectory planning success rate for 3D Theta* in dynamic obstacle environments is as high as 100%, while the success rate for 2D A* and 2D Theta* in this experiment is only 60%. This indicates that 3D Theta* is better able to handle dynamic obstacles and avoid trajectory planning failures caused by moving obstacles.

[0189] (2) Lowest energy consumption. 3D Theta* performs best in terms of trajectory energy consumption, reducing it by 29.4-32.3% compared to 2D A*. This is mainly due to the smoother trajectory planned by 3DTheta*, which avoids unnecessary sudden acceleration and stopping, thereby reducing acceleration energy consumption. In terms of acceleration energy consumption, 3D Theta* reduces it by 71% compared to 3D A*. This significantly reduces total energy consumption.

[0190] First, this embodiment proposes a three-dimensional space-time Theta* algorithm to extend path planning from two-dimensional space to the space-time domain. The traditional Theta algorithm optimizes path smoothness through line of sight (LOS), but it only considers the spatial dimension and cannot handle time-related dynamic obstacle conflicts. To this end, the present invention introduces the time dimension, defines the node as a (x, y, t) triple, and constructs a three-dimensional search space. By reconstructing the cost function, integrating path length, time efficiency and obstacle avoidance safety, and an improved node expansion strategy, that is, the space-time nine-neighborhood search, the algorithm can simultaneously optimize spatial trajectory and time scheduling during the planning process, significantly reducing the frequency of path replanning due to sudden changes in dynamic obstacles.

[0191] Secondly, to achieve active obstacle avoidance around dynamic obstacles, this paper deeply integrates dynamic obstacle trajectory prediction with the path planning algorithm. By constructing a spatio-temporal occupancy grid map (STOGM), the predicted obstacle trajectory is mapped into a static occupancy structure in the three-dimensional spatiotemporal domain. This map, indexed by (x, y, t), dynamically updates the occupancy status of obstacles at future moments, providing a real-time obstacle avoidance basis for path search. Furthermore, combined with an improved line-of-sight detection mechanism (the three-dimensional Bresenham algorithm), the algorithm can proactively avoid the predicted obstacle trajectory during the planning process, avoiding the collision risk caused by the delayed response of traditional methods.

[0192] This invention focuses on two core innovations: the three-dimensional spatiotemporal Theta* algorithm and the dynamic obstacle avoidance mechanism, aiming to break through the limitations of traditional methods and provide theoretical support and technical solutions for AGV navigation in complex dynamic scenarios.

[0193] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Reference can be made to the common and similar parts between the various embodiments. For the devices disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple, and the relevant parts can be referred to the method description.

[0194] The above description of the disclosed embodiments is intended to enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention is not limited to the embodiments shown herein but is intended to conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robot path planning method based on an improved Theta* algorithm, characterized in that: The following steps are involved: S1. Construct a 3D spatiotemporal map based on the search space of the robot's target task, convert the dynamic obstacle coordinates into static obstacle coordinates, and obtain a 3D spatiotemporal obstacle map; S2. Using an improved Theta* algorithm, the robot plans a path from a starting node to a target node on the three-dimensional space-time obstacle map; wherein the improvements to the Theta* algorithm include improvements to node properties, improvements to node expansion methods, improvements to cost functions, and improvements to the LOS determination mechanism from two dimensions to three dimensions.

2. A robot path planning method based on an improved Theta* algorithm as claimed in claim 1, characterized in that: Step S1 specifically includes: Obtaining a search space for the robot's target task, adding a time axis to the two-dimensional plane of the search space to obtain a three-dimensional space-time map and rasterizing it; The predicted trajectory of dynamic obstacles changing over time is mapped into a three-dimensional space-time grid, and the two-dimensional dynamic obstacles are transformed into static obstacles in three-dimensional space-time, thus obtaining a gridded three-dimensional space-time obstacle map.

3. A robot path planning method based on an improved Theta* algorithm as claimed in claim 1, characterized in that: The improvements to the node properties specifically include: Map the position coordinates of the robot's current node and the current time to the three-dimensional space-time obstacle map corresponding to the robot's target task; When a node is expanded, the current node coordinates are updated by a fixed time step.

4. A robot path planning method based on an improved Theta* algorithm as claimed in claim 3, characterized in that: The improvements to the node expansion method specifically include: Convert the coordinates (x±Δx, y±Δy) of the two-dimensional space Theta* algorithm to (x±Δx, y±Δy, t+Δt) in three-dimensional space; where x and y represent the current position; t represents the current time; Δx and Δy represent the spatial step, and Δt represents the time step; When a node is expanded, the expansion domain is expanded from the eight-neighborhood domain in two-dimensional space, and the time neighborhood is added as the ninth domain, converting it into a nine-neighborhood expansion in three-dimensional space; In dynamic neighbor selection and conflict detection, conflict detection is performed on the extended domain of each node; if the extended domain node contains obstacles at the corresponding time, the corresponding domain node is excluded; if the extended domain node does not contain obstacles at the corresponding time, the corresponding domain node is used as a feasible node for the current node's extended trajectory.

5. A robot path planning method based on an improved Theta* algorithm as claimed in claim 4, characterized in that: The improvement of the cost function specifically includes: The improved cost function includes the cumulative cost from the starting point to the current node and the heuristic estimated cost from the current node to the end point when the robot performs the target task in three dimensions; When performing path planning, the robot's path trajectory length, time efficiency and dynamic obstacle avoidance requirements are balanced by minimizing the improved cost function.

6. A robot path planning method based on an improved Theta* algorithm as claimed in claim 5, characterized in that: The improvement of the LOS determination mechanism from two dimensions to three dimensions specifically includes: Introducing time dimension constraints, the spatiotemporal visibility between moving objects and obstacles in a dynamic environment is calculated in real time, ray casting is performed in the three-dimensional state space, and in the node expansion phase, the visibility of the current node's expansion node and the current node's parent node is detected; If it is visible, it is connected through the path of the parent node, which has a lower cost. The parent node of the expanded node is updated to the parent node of the current node to avoid the situation where the driving cost is higher by passing through the current node. If it is not visible, the next node of the current node is determined by the improved cost function. By introducing the spindle selection criterion to optimize the LOS detection direction and reduce invalid calculation steps, a dual error compensation mechanism is adopted to dynamically correct the detection results, thereby comprehensively improving the computational efficiency and robustness of the LOS algorithm.

7. A robot path planning method based on an improved Theta* algorithm as claimed in claim 6, characterized in that: The spindle selection criteria include: When detecting path obstacles, the dimension with the largest change is used as the main axis and also as the iteration direction; The number of steps in the main axis direction is traversed. In each iteration, the increments of the other two dimensions are dynamically calculated according to the main axis step ratio to avoid traversing the entire space and reduce the number of iterations. The dual error compensation mechanism includes: For the secondary axis and the third axis except the main axis, perform error term initialization operation; According to the difference between the initialization error terms of the secondary axis and the third axis and the preset threshold, iterative compensation is performed respectively to achieve double error compensation.

8. A robot path planning method based on an improved Theta* algorithm as claimed in claim 7, characterized in that: In step S2, an improved Theta* algorithm is used to plan the path of the robot from the starting node to the target node on the three-dimensional space-time obstacle map; specifically, the steps include: S21, taking the starting node of the robot's target task as the center, generating nine neighboring nodes in the next time plane as candidate nodes; S22. On the three-dimensional spatiotemporal obstacle map, query whether the candidate nodes include an obstacle node; if so, remove the corresponding candidate node; if not, add the corresponding candidate node to an expansion queue; S23. Calculating the trajectory cost from the feasible nodes in the extended queue to the current node based on the improved cost function; S24, using the improved LOS decision function to check the feasibility of line of sight between nodes, optimize the node selection of the Theta* algorithm, and reduce the trajectory cost; S25. Add feasible nodes that meet the trajectory cost calculation requirements to a priority queue; perform global trajectory cost calculation on the nodes added to the priority queue to generate feasible trajectories that meet the obstacle avoidance requirements.

9. A robot path planning method based on an improved Theta* algorithm as claimed in claim 8, characterized in that: Step S24 specifically includes: For two nodes whose visibility needs to be checked, generate several intermediate points using uniform step-size interpolation; For each intermediate point, query its status in the three-dimensional space-time obstacle map; If the states of all intermediate points are unoccupied, the line segment between the two nodes is considered feasible, there are no dynamic obstacles, and the two nodes can be directly connected; If the state of any intermediate point is occupied, the line segment between the two nodes is considered infeasible and needs to be dynamically adjusted.

Images