A robot path planning method

Abstract

The present invention relates to the field of path planning technology and discloses a robot path planning method, comprising the following steps: constructing a hybrid cost map, the hybrid cost map comprising: a static obstacle field, a dynamic obstacle probability prediction field, and a terrain energy consumption field; constructing an anisotropic heuristic function and searching on the hybrid cost map to obtain an initial path; parameterizing the initial path into a set of piecewise polynomial curves, constructing a joint optimization objective using the spatiotemporal overlap integral of the total curvature, the piecewise polynomial curves, and the dynamic obstacle probability prediction field, performing trajectory optimization under the condition that the robot's kinematic constraints are satisfied, and obtaining a smooth spatiotemporal trajectory; while executing a task, continuously calculating information divergence, and when the information divergence exceeds a preset threshold, re-executing the complete path planning process with the robot's current state as a new starting point. The present invention proactively avoids future collision risks while taking into account the robot's energy consumption, thereby improving the overall quality of the path from the source.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a robot path planning method. Background Art

[0002] Robotic path planning is a core technology for autonomous robot navigation and operation. Its goal is to find an optimal or suboptimal path from a starting point to a destination in complex environments. Traditional path planning algorithms, such as the A* algorithm, the D* algorithm, and their variants, are well-suited for static environments. They effectively find collision-free shortest paths by searching on discretized maps (e.g., grid maps). However, these methods exhibit numerous limitations in the unstructured and dynamic environments of the real world. First, for dynamic scenes containing moving obstacles (e.g., pedestrians and other vehicles), traditional algorithms typically treat the position of dynamic obstacles at a given moment as static obstacles and avoid them accordingly. This lack of foresight can lead to frequent sudden stops or unnecessary detours, significantly compromising the efficiency and smoothness of the path. Second, grid-based search methods inherently produce "zigzag" paths composed of numerous straight line segments, which are inconsistent with the robot's kinematic characteristics and require additional smoothing. This smoothing process can cause the path to collide with obstacles again, increasing planning complexity.

[0003] To address these challenges, existing technologies have undergone a series of improvements. For dynamic obstacle avoidance, some methods incorporate the time dimension, searching within the spatiotemporal state space or employing velocity obstacle methods to predict and avoid collisions. However, these methods often make overly simplified assumptions about obstacle motion (such as uniform linear motion), making them incapable of addressing the complex and variable motion patterns of real-world obstacles. Furthermore, densely populated obstacles can significantly increase the solution space and lead to high computational costs. Regarding path quality, independent post-processing steps, such as smoothing the searched path using spline curves, are often employed. This separation of search and optimization makes it difficult to coordinate the optimization of geometric smoothness with dynamic obstacle avoidance and terrain adaptability. Furthermore, existing replanning mechanisms are often triggered by fixed safety distances or collision detection. This can lead to the robot remaining on a suboptimal path when the environment undergoes subtle but cumulative changes, and can also result in delayed response to sudden environmental changes. Therefore, how to build a comprehensive cost model that can uniformly consider multiple environmental factors such as static, dynamic, and terrain, and on this basis achieve close coupling of path search, trajectory optimization, and intelligent replanning, is a key issue that needs to be urgently addressed in the current field of robot path planning. Summary of the Invention

[0004] The present invention provides a robot path planning method, which aims to solve the problem of how to build a comprehensive cost model that can uniformly consider multiple environmental factors such as static, dynamic, and terrain, and on this basis achieve a tight coupling of path search, trajectory optimization, and intelligent replanning.

[0005] The robot path planning method of the present invention comprises the following steps:

[0006] Obtain map data and real-time sensor data of the robot's environment and construct a hybrid cost map. The hybrid cost map includes: a static obstacle field generated based on map data, a dynamic obstacle probability prediction field generated based on real-time sensor data and a moving target trajectory prediction model, and a terrain energy consumption field generated based on terrain information and the robot's energy consumption model;

[0007] Based on the hybrid cost map, construct an anisotropic heuristic function for estimating the comprehensive cost of a path from any search node to a target node, and apply a heuristic search algorithm in combination with the anisotropic heuristic function to search on the hybrid cost map to obtain an initial path;

[0008] The initial path is parameterized into a set of piecewise polynomial curves, and a joint optimization objective is constructed, wherein the total curvature of the piecewise polynomial curve and the spatiotemporal overlap integral of the piecewise polynomial curve and the dynamic obstacle probability prediction field are used. Trajectory optimization is performed under the conditions of satisfying the robot kinematic constraints and the end tool posture reachability envelope to obtain a smooth spatiotemporal trajectory;

[0009] When the robot performs the task along the smooth space-time trajectory, it continuously calculates the information divergence between the dynamic obstacle probability prediction field and the environmental state reflected by the real-time sensor data. When the information divergence exceeds a preset threshold, the complete path planning process is re-executed with the current state of the robot as a new starting point.

[0010] The robot path planning method of the present invention constructs a hybrid cost map that integrates a static obstacle field, a dynamic obstacle probability prediction field, and a terrain energy consumption field. This allows path planning to proactively avoid future collision risks within a unified framework while also taking into account the robot's energy consumption, thereby improving the overall quality of the path from the source. In addition, by jointly optimizing the geometric smoothness of the path, the risk of spatiotemporal overlap with the moving target, and the robot's kinematic constraints, a smooth trajectory that not only meets the robot's own motion characteristics but is also highly secure in the spatiotemporal dimension can be directly generated, avoiding the path conflicts and suboptimal problems that may result from the traditional "search first, smooth later" method. In addition, by deciding when to replan based on the information difference between the predicted and actual environmental states, the robot can respond promptly to significant changes in the environment while remaining stable to minor or expected disturbances, enhancing its ability to operate stably for a long time in complex and changing scenarios.

[0011] Preferably, the step of generating a dynamic obstacle probability prediction field includes: using a Kalman filter to track the motion state of the dynamic obstacle; and predicting its future trajectory based on a motion model, projecting the trajectory onto a grid map, and forming a dynamic obstacle probability prediction field that changes with time.

[0012] The Kalman filter has the advantages of noise suppression, multi-source fusion, dynamic prediction, real-time performance, and uncertainty quantification, and can more accurately track the state and predict the trajectory of dynamic obstacles.

[0013] Preferably, the step of constructing the anisotropic heuristic function includes: for any search node, its heuristic cost value is obtained by weighted summing the estimated distance from the search node to the target node, the terrain energy consumption field cost value of the search node, and the dynamic obstacle probability prediction field cost value when the robot is expected to reach the search node through preset weights.

[0014] The terrain energy consumption field proxy value and dynamic obstacle probability prediction field proxy value in the anisotropic heuristic function can upgrade the heuristic function from "single distance guidance" to "multi-constraint comprehensive guidance", achieving comprehensive optimization of path planning in energy consumption and safety.

[0015] Preferably, the step of applying the heuristic search algorithm is: using the A* algorithm, wherein the total cost function of the current node is defined as the sum of the actual cumulative cost from the starting point to the current node and the heuristic cost value of the current node, and the actual cumulative cost is the sum of the costs of the path from the starting point to the current node on the hybrid cost map.

[0016] The A* algorithm can integrate multi-dimensional costs such as "distance, terrain energy consumption, and spatiotemporal risks of dynamic obstacles" into path evaluation through an efficient search process, and flexibly adapt to the constraints of complex dynamic environments while finding the optimal solution.

[0017] Preferably, the step of parameterizing the initial path into a set of segmented polynomial curves includes: fitting the initial path with a B-spline curve, and generating a smooth curve that ensures the continuity of high-order derivatives by selecting a part of path points on the initial path as control points.

[0018] The B-spline curve is used to fit the initial path, which can ensure the kinematic feasibility of the path with high-order continuity, adapt to the real-time adjustment of the dynamic environment with local controllability, and simplify the subsequent motion planning and time allocation through parametric expression.

[0019] Preferably, the step of constructing the joint optimization objective includes: constructing an objective function, which is the weighted sum of the total curvature of the piecewise polynomial curve and the spatiotemporal overlap integral of the piecewise polynomial curve and the dynamic obstacle probability prediction field, and solving the minimum value of the objective function through a numerical optimization method.

[0020] Preferably, the information divergence is the Kullback-Leibler divergence, and the step of continuously calculating the information divergence between the dynamic obstacle probability prediction field and the environmental state reflected by the real-time sensor data includes: defining the dynamic obstacle probability prediction field generated based on the moving target trajectory prediction model as a first probability distribution, and defining the environmental state perceived based on the real-time sensor data as a second probability distribution; and periodically calculating the Kullback-Leibler divergence between the first probability distribution and the second probability distribution.

[0021] Preferably, the step of generating the static obstacle field includes: generating the static obstacle field using a pre-constructed grid map, dilating the obstacle area in the grid map to retain a safety margin, and assigning an extremely high cost value.

[0022] Preferably, the motion model is a constant velocity model.

[0023] Preferably, the step of generating the terrain energy consumption field includes: calculating the roughness of each grid based on the digital elevation map, correlating the robot energy consumption with the roughness in combination with the robot dynamics model, and assigning a corresponding energy consumption cost value to each grid.

[0024] The beneficial effects of the present invention are as follows: the robot path planning method of the present invention constructs a hybrid cost map that integrates a static obstacle field, a dynamic obstacle probability prediction field, and a terrain energy consumption field, so that path planning can proactively avoid future collision risks under a unified framework while taking into account the robot's energy consumption, thereby improving the overall quality of the path from the source. In addition, by jointly optimizing the geometric smoothness of the path, the risk of spatiotemporal overlap with the moving target, and the robot's kinematic constraints, a smooth trajectory that not only meets the robot's own motion characteristics but is also highly secure in the spatiotemporal dimension can be directly generated, avoiding the path conflicts and sub-optimization problems that may be caused by the traditional "search first, smooth later" method. In addition, the decision on when to re-plan is based on the information difference between the predicted and actual environmental states, allowing the robot to respond promptly to significant changes in the environment while remaining stable to small or expected disturbances, thereby enhancing its ability to operate stably for a long time in complex and changing scenarios. In addition, the terrain energy consumption field value and dynamic obstacle probability prediction field value in the anisotropic heuristic function can upgrade the heuristic function from "single distance guidance" to "multi-constraint comprehensive guidance", achieving comprehensive optimization of path planning in energy consumption and safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 A schematic flow chart of a robot path planning method provided in an embodiment of the present invention.

[0026] Figure 2 is a schematic diagram of a static obstacle field in an embodiment of the present invention.

[0027] Figure 3 Schematic diagram of a dynamic obstacle probability prediction field in an embodiment of the present invention.

[0028] Figure 4 Schematic diagram of the terrain energy consumption field in an embodiment of the present invention.

[0029] Figure 5 It is a schematic diagram of a hybrid cost map after the static obstacle field, dynamic obstacle probability prediction field and terrain energy consumption field are integrated in an embodiment of the present invention.

[0030] Figure 6 Schematic diagram of anisotropic heuristic function and path search in an embodiment of the present invention.

[0031] Figure 7 Schematic diagram of the initial path and the optimized smooth space-time trajectory in an embodiment of the present invention.

[0032] Figure 8 Schematic diagram of the predicted probability distribution P in the re-planning trigger mechanism based on information divergence in an embodiment of the present invention.

[0033] Figure 9 2 is a schematic diagram of the observation probability distribution Q in the replanning trigger mechanism based on information divergence in an embodiment of the present invention. DETAILED DESCRIPTION

[0034] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and intended to be used to explain the present invention, but should not be understood as limiting the present invention.

[0035] like Figures 1-9 As shown, the robot path planning method provided by the embodiment of the present invention specifically includes the following steps:

[0036] S1. Obtain the map data and real-time sensor data of the robot's environment and construct a hybrid cost map. The hybrid cost map includes: a static obstacle field generated based on map data, a dynamic obstacle probability prediction field generated based on real-time sensor data and a motion target trajectory prediction model, and a terrain energy consumption field generated based on terrain information and a robot energy consumption model.

[0037] Specifically, a static obstacle field is first generated using a pre-built grid map, and the obstacle area in the grid map is expanded to retain the safety margin and assigned a very high cost value. The steps for generating a dynamic obstacle probability prediction field include: using a Kalman filter to track the motion state of the dynamic obstacle; and predicting its future trajectory based on a motion model, projecting the trajectory onto the grid map to form a dynamic obstacle probability prediction field that changes over time. Specifically, the environmental point cloud is acquired through sensors such as lidar or depth cameras, and potential dynamic obstacles are identified using clustering algorithms such as DBSCAN, and the Kalman filter is used to continuously track and estimate the state of these dynamic obstacles. For example, when the lidar identifies a moving pedestrian, a Kalman filter is initialized to track the state of the pedestrian. The state includes the pedestrian's two-dimensional position X and Y, and the corresponding speed and For example, at time t = 0, the pedestrian's position is measured to be 10.0 meters, 5.0 meters, and its speed is 1.2 meters per second, and 0.1 meters per second. The Kalman filter will fuse these noisy measurements and output a better state estimate and its uncertainty covariance matrix. Based on this state estimate, a constant velocity motion model is used to predict the pedestrian's trajectory within the next 5 seconds. For example, the pedestrian's position is predicted to be 11.2 meters, 5.1 meters at t = 1 second, 12.4 meters, 5.2 meters at t = 2 seconds, and so on. These predicted positions and their uncertainties that increase over time are projected onto a grid map with a resolution of 0.1 meters. Each future moment corresponds to a probability map, in which the grid where the predicted position is located and its neighboring grids are assigned higher probability values, forming a Gaussian probability distribution. This series of probability maps sorted by time together constitutes a dynamic obstacle probability prediction field, providing dynamic risk assessment for subsequent path planning. For the terrain energy consumption field, the roughness of each grid is calculated based on the digital elevation map. Combined with the robot's dynamics model, energy consumption is correlated with roughness, and a corresponding energy cost value is assigned to each grid. Finally, these three fields are fused into a hybrid cost map through a weighted summation.

[0038] S2. Based on the hybrid cost map, construct an anisotropic heuristic function for estimating the comprehensive cost of the path from any search node to the target node, and apply a heuristic search algorithm in combination with the anisotropic heuristic function to search on the hybrid cost map to obtain an initial path.

[0039] The steps for applying the heuristic search algorithm are as follows: Using the A* algorithm, the total cost function for the current node is defined as the sum of the actual cumulative cost from the starting point to the current node and the heuristic cost value of the current node. The actual cumulative cost is the sum of the costs of the path from the starting point to the current node on the hybrid cost map. Specifically, the A* algorithm calculates a total cost f(n) for each node n, which is in the form of f(n) = g(n) + h(n). Here, g(n) is the actual cumulative cost from the starting point to node n. This cost is obtained by summing the cost values ​​of all grids passed on the path. These cost values ​​are derived from a static hybrid cost map and take into account the difficulty of the terrain and the distance to static obstacles. For example, if the cost values ​​of 5 grids passed from the starting point are 1, 1, 5, 5, and 2 respectively, then the g(n) value for reaching the current node is 14. h(n) is the aforementioned anisotropic heuristic function, which estimates the future cost from node n to the target node and takes into account the spatiotemporal impact of dynamic obstacles. The A* algorithm maintains a priority queue of nodes to be explored, prioritizing the node with the lowest total cost f(n) for expansion. For example, given two candidate nodes, A and B, if f(A) is 150 and f(B) is 165, the algorithm will choose to expand node A. This allows the A* algorithm to efficiently find the initial path with the lowest overall cost in complex environments, taking into account static terrain, static obstacles, and the risk of future dynamic obstacle positions.

[0040] The core of the anisotropic heuristic function designed for the A* algorithm is that when evaluating the cost, not only the geometric distance from the target node is considered, but also the influence of the terrain is taken into account. The steps of constructing the anisotropic heuristic function include: for any search node, its heuristic cost value is obtained by weighted summing the estimated distance from the search node to the target node, the terrain energy consumption field cost value of the search node, and the dynamic obstacle probability prediction field cost value when the robot is expected to arrive at the search node through preset weights. Specifically, in the path search process, for any node n to be evaluated on the grid map, the calculation of the anisotropic heuristic function h(n) includes three parts. The first part is the Euclidean distance from the node n to be evaluated to the target node, and the Euclidean distance is normalized to obtain The second part is the terrain energy consumption of the node n to be evaluated , the normalized value of the friction coefficient of the ground type where the node n is located is the terrain energy consumption Assume that the node n to be evaluated is located on the grass and its friction coefficient is ; Assume that the node n to be evaluated is located on the cement ground, and its friction coefficient is According to common sense, Greater than , the terrain energy consumption of the node n to be evaluated on the grass is greater than that of the node n to be evaluated on the cement ground. The terrain energy consumption is positively correlated with the friction coefficient of the ground type. Therefore, the normalized value of the friction coefficient of the ground type where the node n to be evaluated is the terrain energy consumption. The third part is the dynamic risk, which requires estimating the time it takes for the robot to reach the node n to be evaluated. If the estimated time is 5 seconds, then the probability value corresponding to the node n to be evaluated in the dynamic obstacle probability prediction field when t equals 5 seconds is queried. These three parts are weighted and summed using preset weights to form the final heuristic cost value. For example, the anisotropic heuristic function can be defined as:

[0041]

[0042] in: is the heuristic cost value of the node n to be evaluated; is the normalized value of the Euclidean distance from the node to be evaluated to the target node; for The weight of is the terrain energy consumption of the node n to be evaluated; for The weight of is the probability value corresponding to the node to be evaluated n in the dynamic obstacle probability prediction field when the robot reaches the node to be evaluated n; for If Setting a high weight can make the planner strongly prefer to avoid areas where there may be future collisions with dynamic obstacles, even if this means choosing a path with a longer physical distance or more complex terrain, thus reflecting the anisotropy of the heuristic function, that is, the quality of the path depends not only on the spatial position, but also strongly on the time to reach that position.

[0043] S3. Parameterize the initial path into a set of piecewise polynomial curves, construct a joint optimization objective with the total curvature of the piecewise polynomial curve, the spatiotemporal overlap integral of the piecewise polynomial curve and the dynamic obstacle probability prediction field, perform trajectory optimization under the conditions of satisfying the robot kinematic constraints and the end tool posture reachability envelope, and obtain a smooth spatiotemporal trajectory.

[0044] Specifically, the step of parameterizing the initial path into a set of piecewise polynomial curves includes fitting the initial path using a B-spline curve. By selecting a portion of path points from the initial path as control points, a smooth curve is generated that ensures continuity of higher-order derivatives. Specifically, the initial path generated by the A* algorithm is a broken line formed by connecting a series of adjacent grid center points, which has discontinuous turns and cannot be directly executed by the robot. To address this issue, a B-spline curve is used for smoothing. For example, the initial path may contain 200 path points. Ten sparse path points are obtained by selecting one point every 20 points. These points serve as control points for a cubic B-spline curve. Cubic B-spline curves have the advantageous property of continuous second-order derivatives, meaning that the curvature of any point on the curve changes continuously. This means that the generated trajectory is smooth in both velocity and acceleration, avoiding jerkiness and instability in robot motion. By adjusting the position or number of control points, the shape of the B-spline curve can be modified so that it closely follows the initial path's guidance direction while also satisfying the robot's kinematic constraints. The resulting B-spline curve is a smooth path expressed analytically, which can easily calculate the precise position, tangent direction, and curvature of any point on the path, providing a basis for subsequent optimization.

[0045] To further improve the quality of B-spline curve paths, a joint optimization objective function is constructed. This step includes constructing an objective function that is the weighted sum of the total curvature of the piecewise polynomial curve and the spatiotemporal overlap integral of the piecewise polynomial curve and the dynamic obstacle probability prediction field, and then solving for the minimum value of this objective function using a numerical optimization method. This function consists of two main components. The first component is the path smoothness cost, Jsmooth, which is quantified by integrating the square of the curvature over the path. A lower smoothness cost means a straighter path, smoother turns, and more user-friendly robot control. The second component is the safety cost, calculated by calculating the spatiotemporal overlap integral of the path with the dynamic obstacle probability prediction field: Jsafety = ∫P(r(t), t)dt, where Jsafety is the safety cost and P(r(t), t) is the probability of the robot's position on the path at time t in the dynamic obstacle probability prediction field. A higher value of this integral indicates a greater risk of collision between the path and an obstacle in spatiotemporal space. The objective function is the weighted sum of these two costs:

[0046] .in, is the weighted sum of the path smoothness cost and safety cost, for The weight of , Jsmooth is the smoothness cost, for The weight of For example, you can set the weight is 0.1, = 100. This objective function uses the control points of the B-spline curve as optimization variables. Using a nonlinear numerical optimization algorithm like Sequential Quadratic Programming (SQP), the positions of the control points are iteratively adjusted to find a solution that minimizes the objective function J, while satisfying physical constraints such as the robot's maximum velocity and maximum acceleration. The B-spline curve defined by the optimized control points is the resulting smooth and safe optimal spatiotemporal trajectory.

[0047] S4. When the robot performs the task along the smooth space-time trajectory, it continuously calculates the information divergence between the dynamic obstacle probability prediction field and the environmental state reflected by the real-time sensor data. When the information divergence exceeds a preset threshold, the complete path planning process is re-executed with the current state of the robot as a new starting point.

[0048] The information divergence is the Kullback-Leibler divergence. The step of continuously calculating the information divergence between the dynamic obstacle probability prediction field and the environmental state reflected by real-time sensor data includes: defining the dynamic obstacle probability prediction field generated based on the moving target trajectory prediction model as a first probability distribution, and defining the environmental state perceived based on real-time sensor data as a second probability distribution; and periodically calculating the Kullback-Leibler divergence between the first and second probability distributions. Specifically, to cope with sudden changes in the environment, a mechanism is required to determine whether the current plan is still valid. The dynamic obstacle probability prediction field generated within the system based on Kalman filter prediction is defined as the first probability distribution P. Simultaneously, the environmental information directly observed by real-time sensors, such as lidar, is converted into an instantaneous probability map, defined as the second probability distribution Q. In Q, the grid probability of a sensor detecting an obstacle is 1, and all other areas are 0. This process is performed periodically at a frequency of, for example, 10 Hz. During each cycle, the Kullback-Leibler divergence between the first and second probability distributions P and Q is calculated. The Kullback-Leibler divergence measures the error between the first and second probability distributions P and Q. For example, if a pedestrian predicted to be heading north suddenly turns east, then in the new observation Q, the probability of the pedestrian occupying the grid is 1, while the predicted probability values ​​of P on these grids may be very low, close to 0, which will cause the calculated Kullback-Leibler divergence to increase sharply. A threshold is preset, such as 0.8. Once the calculated Kullback-Leibler divergence exceeds this threshold, it indicates that there is a significant deviation between the prediction and reality, and the currently planned trajectory is likely no longer safe or optimal. At this point, the current trajectory execution is immediately interrupted and a new planning is triggered. The current physical position, velocity, and acceleration of the robot are used as the initial state of the planning algorithm, and a new round of map construction and path search begins.

[0049] The implementation principle of the robot path planning method of the embodiment of the present invention is as follows: by fusing the static obstacle field, the dynamic obstacle probability prediction field, and the terrain energy consumption field to form a hybrid cost map, path planning is carried out under a unified framework, proactively avoiding future collision risks while taking into account the robot's energy consumption, thus improving the overall quality of the path from the source. In addition, by jointly optimizing the geometric smoothness of the path, the spatiotemporal overlap risk of dynamic obstacles, and the robot's kinematic constraints, a smooth trajectory can be generated that not only meets the robot's own motion characteristics but also is highly secure in the spatiotemporal dimension, avoiding the path conflicts and suboptimal problems that may be caused by the traditional "search first, smooth later" method. In addition, the decision on when to replan is based on the information difference between the predicted and actual environmental states, allowing the robot to respond promptly to significant changes in the environment while remaining stable to minor or expected disturbances, enhancing the robot's ability to operate stably for a long time in complex and changing scenarios.

[0050] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

Claims

1. A robot path planning method, characterized in that: The steps include: Obtain map data and real-time sensor data of the robot's environment and construct a hybrid cost map. The hybrid cost map includes: a static obstacle field generated based on map data, a dynamic obstacle probability prediction field generated based on real-time sensor data and a moving target trajectory prediction model, and a terrain energy consumption field generated based on terrain information and the robot's energy consumption model; Based on the hybrid cost map, construct an anisotropic heuristic function for estimating the comprehensive cost of a path from any search node to a target node, and apply a heuristic search algorithm in combination with the anisotropic heuristic function to search on the hybrid cost map to obtain an initial path; The initial path is parameterized into a set of piecewise polynomial curves, and a joint optimization objective is constructed, wherein the total curvature of the piecewise polynomial curve and the spatiotemporal overlap integral of the piecewise polynomial curve and the dynamic obstacle probability prediction field are used. Trajectory optimization is performed under the conditions of satisfying the robot kinematic constraints and the end tool posture reachability envelope to obtain a smooth spatiotemporal trajectory; When the robot performs a task along the smooth spatiotemporal trajectory, the robot continuously calculates the information divergence between the dynamic obstacle probability prediction field and the environmental state reflected by the real-time sensor data. When the information divergence exceeds a preset threshold, the robot re-executes the complete path planning process with the current state of the robot as a new starting point. The information divergence is the Kullback-Leibler divergence. The step of continuously calculating the information divergence between the dynamic obstacle probability prediction field and the environmental state reflected by the real-time sensor data includes: defining the dynamic obstacle probability prediction field generated based on the moving target trajectory prediction model as a first probability distribution, and defining the environmental state perceived based on the real-time sensor data as a second probability distribution; and periodically calculating the Kullback-Leibler divergence between the first probability distribution and the second probability distribution.

2. The robot path planning method according to claim 1, characterized in that: The steps of generating a dynamic obstacle probability prediction field include: using a Kalman filter to track the motion state of the dynamic obstacle; and predicting its future trajectory based on a motion model, projecting the trajectory onto a grid map, and forming a dynamic obstacle probability prediction field that changes with time.

3. The robot path planning method according to claim 1, characterized in that: The steps of constructing the anisotropic heuristic function include: for any search node, its heuristic cost value is obtained by weighted summing the estimated distance from the search node to the target node, the terrain energy consumption field cost value of the search node, and the dynamic obstacle probability prediction field cost value when the robot is expected to reach the search node through preset weights.

4. The robot path planning method according to claim 1, characterized in that: The steps of applying the heuristic search algorithm are: using the A* algorithm, wherein the total cost function of the current node is defined as the sum of the actual cumulative cost from the starting point to the current node and the heuristic cost value of the current node, and the actual cumulative cost is the sum of the costs of the path from the starting point to the current node on the hybrid cost map.

5. The robot path planning method according to claim 1, characterized in that: The step of parameterizing the initial path into a set of piecewise polynomial curves includes: fitting the initial path with a B-spline curve, and generating a smooth curve that ensures the continuity of high-order derivatives by selecting a part of path points on the initial path as control points.

6. The robot path planning method according to claim 1, characterized in that: The step of constructing the joint optimization target includes: constructing an objective function, which is a weighted sum of the total curvature of the piecewise polynomial curve and the spatiotemporal overlap integral of the piecewise polynomial curve and the dynamic obstacle probability prediction field, and solving the minimum value of the objective function through a numerical optimization method.

7. The robot path planning method according to claim 1, characterized in that: The step of generating the static obstacle field includes: generating the static obstacle field using a pre-built grid map, dilating the obstacle area in the grid map to retain a safety margin, and assigning an extremely high cost value.

8. The robot path planning method according to claim 2, characterized in that: The motion model is a constant velocity model.

9. The robot path planning method according to claim 1, characterized in that: The steps of generating the terrain energy consumption field include: calculating the roughness of each grid according to the digital elevation map, correlating the robot energy consumption with the roughness in combination with the robot dynamics model, and assigning a corresponding energy consumption cost value to each grid.

Images