Mobile robot path planning method and system based on global guidance MPPI

Abstract

The application provides a mobile robot path planning method and system based on global guidance MPPI, relates to the technical field of mobile robot autonomous navigation, and aims at the problem that the existing path planning technology has weak guidance, poor trajectory, and slow response, which leads to path deviation, motion oscillation, and low dynamic obstacle avoidance efficiency. The method generates a global reference path from the starting point to the ending point based on a global path planning algorithm; the global reference path is pretreated to obtain an indexed global reference path, a global path lookahead point guidance mechanism is designed, and a path tracking cost is calculated; a path progress reward mechanism is designed, a path progress reward cost is calculated; a multi-dimensional constraint cost function is constructed, a local trajectory integrated by a model prediction path is optimized to obtain an optimal control sequence, and dynamic path planning is performed. The application can solve the problems in the prior art and achieve global optimization and local real-time dynamic balance.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation technology for mobile robots, and particularly relates to a method and system for path planning of mobile robots based on global guidance MPPI. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] With the rapid development of artificial intelligence and sensor technology, mobile robots are increasingly being used in industrial manufacturing, warehousing and distribution, and intelligent services. Their autonomous navigation capability has become a key factor determining their task execution efficiency and system operation safety. As the core module of autonomous navigation, path planning not only needs to achieve collision-free optimal path planning from the starting point to the destination for safe passage, but also needs to achieve synergistic optimization between path safety, operational efficiency, and motion smoothness.

[0004] Currently, in complex environment navigation scenarios, path planning algorithms are mainly divided into two categories based on their planning field of view and information dependence: global path planning and local path planning. Global path planning algorithms, such as Dijkstra's algorithm and A... Algorithms such as the RRT algorithm can generate macroscopically feasible paths based on known environments, but they lack flexibility in adapting to dynamic obstacles. Local path planning algorithms, such as the Dynamic Window Method (DWA), the artificial potential field method, and the model prediction path integral algorithm, can achieve dynamic obstacle avoidance through real-time environmental perception, but they are prone to getting stuck in local optima due to short-sighted local decision-making, leading to problems such as "circling and stagnation" and "snake-like movement".

[0005] Among them, the Model Predictive Path Integral (MPPI) algorithm, as an efficient local optimization method, achieves optimal control through random sampling, cost evaluation, and trajectory fusion. It can handle nonlinear systems without derivative calculations and possesses good real-time performance and parallel computing characteristics, making it a research hotspot for local path planning in mobile robots. However, traditional MPPI algorithms still have significant drawbacks: first, they rely solely on random sampling to optimize the trajectory, lacking global path guidance, and are prone to deviating from the target direction and getting trapped in local optima in complex obstacle environments; second, the cost function design is simplistic, making it difficult to simultaneously consider trajectory smoothness, global consistency, and obstacle avoidance safety; and third, the response speed and collision avoidance capabilities in dynamic environments need improvement. Clearly, a single path planning algorithm cannot balance global optimality and local real-time performance.

[0006] However, while existing global-local fusion planning schemes attempt to combine global paths with MPPI, they still have certain shortcomings: existing technologies typically only guide local sampling through dynamic adjustment of target points, without effectively constraining the consistency between the trajectory and the global path. The guidance mechanism is simple, resulting in large tracking deviations and poor global consistency. The cost function design of existing technologies is imperfect, lacking penalty or reward mechanisms for typical problems, and cannot solve the problems of "snake-like movement" and excessive trajectory deviation. Furthermore, they lack foresight in dynamic environments, and lack fine-grained optimization in handling dynamic obstacles and balancing computational efficiency with planning quality. Summary of the Invention

[0007] To overcome the shortcomings of the prior art, this invention provides a mobile robot path planning method and system based on global path guidance (MPPI). It deeply integrates global path guidance with local MPPI optimization, achieving a dynamic balance between global optimality and local real-time performance through global path preprocessing, guidance mechanism design, and multi-dimensional cost function construction.

[0008] To achieve the above objectives, one or more embodiments of the present invention provide the following technical solutions:

[0009] The first aspect of the present invention provides a mobile robot path planning method based on globally guided MPPI, comprising:

[0010] A global reference path from the starting point to the ending point is generated based on a global path planning algorithm, and the global reference path is preprocessed to obtain an indexed global reference path.

[0011] For indexed global reference paths, a global path look-ahead point guidance mechanism is designed to determine the nearest point and the look-ahead target point of the robot's current position, and to calculate the path tracking cost.

[0012] Based on the robot's current location and the nearest point, design a path progress reward mechanism and calculate the path progress reward cost.

[0013] Calculate the corridor constraint cost based on the nearest point to the robot's current location;

[0014] Calculate the orientation error cost based on the forward target point;

[0015] A cost function with multi-dimensional constraints is constructed based on path tracking cost, path progress reward cost, corridor constraint cost, and orientation error cost.

[0016] Based on a cost function with multi-dimensional constraints, the local trajectory of the model's predicted path integral is optimized to obtain the optimal control sequence, and dynamic path planning is then performed.

[0017] As one implementation method, it also includes optimizing the motion prediction and obstacle avoidance costs of moving obstacles, the specific process of which is as follows:

[0018] Calculate and predict collision time based on the speed and direction of dynamic obstacles;

[0019] Adjust the weight of obstacle avoidance costs based on the predicted collision time;

[0020] The prediction time domain length of the model's predicted path integral is dynamically adjusted based on the number and speed of dynamic obstacles.

[0021] As one implementation method, a global reference path from the starting point to the destination is generated based on a global path planning algorithm. The specific process is as follows:

[0022] Construct the cost function for each node of the raster map;

[0023] The search begins at the node with the minimum cost function and continues until the target node is found.

[0024] Based on the node with the minimum cost function and the target node, a global reference path from the starting point to the ending point is generated;

[0025] The global reference path is discretized into a sequence of path points, resulting in a discrete sequence of path points.

[0026] As one implementation method, the global reference path is preprocessed to obtain an indexed global reference path. The specific process is as follows:

[0027] A smooth path is obtained by interpolating the discrete path point sequence using B-spline curves.

[0028] Based on the path curvature of the smooth path and the set distance threshold between adjacent points, the key waypoints of the global reference path are extracted, and a sequential index sequence is constructed.

[0029] Record the index of the nearest point on the global path at the previous moment, and use it as the starting point for the index search at the current moment to obtain the indexed global reference path.

[0030] As one implementation method, for an indexed global reference path, a global path look-ahead point guidance mechanism is designed to determine the nearest point to the robot's current position and the look-ahead target point. The specific process is as follows:

[0031] Get the index of the path point closest to the current position on the global reference path;

[0032] Based on the nearest path point index at the current location and the preset look-ahead step size, calculate the look-ahead target point index and determine the look-ahead target point.

[0033] As one implementation method, a path progress reward mechanism is designed based on the nearest point to the robot's current position, and the path progress reward cost is calculated. Specifically, the difference between the index of the nearest path point at the current time and the index at the previous time is calculated and used as the path progress gain; the negative weighted value of the path progress gain is used as the path progress reward cost.

[0034] As one implementation method, the corridor constraint cost is calculated based on the nearest point to the robot's current location. The specific process is as follows:

[0035] The deviation distance is calculated as the Euclidean distance from the robot's current position to the nearest point on the global reference path.

[0036] The positive value of the deviation distance minus the preset corridor width is taken as the corridor violation amount;

[0037] A two-stage penalty approach is adopted, using the squared weighted value of the corridor violation as the cost of the corridor constraint.

[0038] As one implementation method, the multi-dimensional constraint cost function includes at least: path tracking cost, path progress reward cost, corridor constraint cost, orientation error cost, obstacle avoidance cost, terminal cost, velocity cost, control velocity cost, curvature cost, and angular velocity cost.

[0039] As one implementation method, a cost function based on multi-dimensional constraints is used to optimize the local trajectory of the model's predicted path integral to obtain the optimal control sequence, and then dynamic path planning is performed. The specific process is as follows:

[0040] Multiple candidate control sequences are generated by superimposing Gaussian noise with time autocorrelation onto the historical best control sequence.

[0041] Trajectory prediction is performed using candidate control sequences based on a mobile robot dynamics model.

[0042] The cumulative cost of each predicted trajectory is calculated based on the cost function with multi-dimensional constraints.

[0043] Each candidate trajectory is assigned a weight based on the cumulative cost, and the optimal control sequence is updated by weighted average.

[0044] Only the first control variable in the optimal control sequence is executed, and the remaining sequence is used as the initial control sequence for the next time step. The above steps are repeated to perform dynamic path planning.

[0045] A second aspect of the present invention provides a mobile robot path planning system based on globally guided MPPI, comprising:

[0046] The global path planning module is used to generate a global reference path from the starting point to the ending point based on the global path planning algorithm, and to preprocess the global reference path to obtain an indexed global reference path.

[0047] The look-ahead guidance module is used to design a global path look-ahead guidance mechanism for indexed global reference paths, determine the nearest point and the look-ahead target point of the robot's current position, and calculate the path tracking cost.

[0048] The path progress evaluation module is used to design a path progress reward mechanism and calculate the path progress reward cost based on the nearest point to the robot's current position.

[0049] The multi-dimensional cost function construction module is used to calculate the corridor constraint cost based on the nearest point of the robot's current position; calculate the orientation error cost based on the forward target point; and construct the multi-dimensional constraint cost function based on the path tracking cost, path progress reward cost, corridor constraint cost, and orientation error cost.

[0050] The MPPI local trajectory optimization module is used to optimize the local trajectory of the model's predicted path integral based on a cost function with multi-dimensional constraints, to obtain the optimal control sequence and perform dynamic path planning.

[0051] The above one or more technical solutions have the following beneficial effects:

[0052] In this embodiment, by using A The deep integration of global path planning and MPPI local trajectory optimization constructs a two-layer planning framework of global guidance and local constraints, achieving a dynamic balance between global path consistency and local obstacle avoidance flexibility. By designing a global path guidance mechanism that includes look-ahead point guidance and path progress rewards, the global consistency of the MPPI trajectory is improved, avoiding local optima and "circling stagnation" phenomena. A multi-dimensional cost function system including corridor constraints and orientation error penalties is designed, while retaining obstacle avoidance safety and speed smoothness constraints, thus balancing path smoothness, obstacle avoidance safety, and motion efficiency.

[0053] In this embodiment, for dynamic environments, by dynamically adjusting the distance weighting of obstacles and the prediction time domain, the planning success rate and collision-free rate can be improved in both static and dynamic environments, the path length and dynamic obstacle avoidance response time can be shortened, and the turning angle and number of turns can be reduced. It can effectively solve the problem of balancing global optimality and local real-time performance in mobile robot navigation in complex environments, and provide an efficient and reliable autonomous navigation solution for scenarios such as industrial inspection and warehousing logistics.

[0054] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0055] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0056] Figure 1 This is a schematic diagram of the mobile robot path planning method based on global guidance MPPI in this embodiment.

[0057] Figure 2 This is a simulation diagram of the mobile robot path planning method based on global guidance MPPI in this embodiment.

[0058] Figure 3 This is a comparison chart of the trajectories with and without global path guidance in this embodiment 1;

[0059] Figure (a) shows the trajectory without a global path guidance mechanism; Figure (b) shows the trajectory with a global path guidance mechanism.

[0060] Figure 4 This is a comparison diagram of trajectories with and without corridor constraint mechanism in this embodiment 1;

[0061] Figure (a) shows the trajectory without corridor constraints; Figure (b) shows the trajectory with corridor constraints.

[0062] Figure 5 This is a trajectory comparison diagram with and without the orientation error mechanism in this embodiment 1;

[0063] Figure (a) shows the trajectory without an orientation error mechanism; Figure (b) shows the trajectory with an orientation error mechanism.

[0064] Figure 6 This is the path planning diagram for dynamic obstacles using the GMPPI algorithm in this embodiment.

[0065] Figure (a) shows the dynamic obstacle detection and pre-avoidance stage; Figure (b) shows the dynamic obstacle avoidance execution and path recovery stage.

[0066] Figure 7 This is a comparison diagram of the GMPPI algorithm and multiple algorithm path planning in this embodiment. Detailed Implementation

[0067] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0068] It should be noted that the terminology used herein is for the purpose of describing particular implementations only and is not intended to limit the exemplary implementations of the present invention.

[0069] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0070] Terminology Explanation:

[0071] MPPI algorithm: Model Predictive Path Integral (MPPI) algorithm.

[0072] A Algorithm: Global Path Planning Algorithm;

[0073] GMPPI algorithm: Globally-guided ModelPredictive Path Integral (GMPPI) algorithm, which is a method for predicting path integrals in model prediction. A mobile robot path planning framework that deeply integrates global path planning with model prediction path integral (MPPI) local trajectory optimization.

[0074] Example 1

[0075] This embodiment discloses a mobile robot path planning method based on global guidance MPPI.

[0076] To more clearly illustrate this embodiment, the implementation process of mobile robot path planning based on globally guided MPPI can be specifically described as follows:

[0077] Mobile robot path planning methods based on globally guided MPPI include:

[0078] S1. Generate a global reference path from the starting point to the ending point based on the global path planning algorithm, and preprocess the global reference path to obtain an indexed global reference path;

[0079] S2. For indexed global reference paths, design a global path look-ahead point guidance mechanism to determine the nearest point and the look-ahead target point of the robot's current position, and calculate the path tracking cost.

[0080] S3. Based on the robot's current location and the nearest point, design a path progress reward mechanism and calculate the path progress reward cost.

[0081] S4. Calculate the corridor constraint cost based on the nearest point to the robot's current position;

[0082] Calculate the orientation error cost based on the forward target point;

[0083] A cost function with multi-dimensional constraints is constructed based on path tracking cost, path progress reward cost, corridor constraint cost, and orientation error cost.

[0084] S5. Based on the cost function with multi-dimensional constraints, optimize the local trajectory of the model's predicted path integral to obtain the optimal control sequence for dynamic path planning.

[0085] S6. Optimize the motion prediction and obstacle avoidance costs for moving obstacles.

[0086] In this embodiment, the Global Guided MPPI algorithm, also known as the GMPPI algorithm, is based on three core components: global path generation, local trajectory optimization, and control output.

[0087] like Figure 1 As shown, in step S1, a global reference path from the starting point to the ending point is generated based on the global path planning algorithm, and the global reference path is preprocessed to obtain an indexed global reference path.

[0088] S101. Generate a global reference path from the starting point to the ending point based on the global path planning algorithm.

[0089] In a known environment map, via A The heuristic search algorithm generates the globally optimal path from the starting point to the end point, and discretizes it into a sequence of path points as a guiding benchmark for local planning.

[0090] A global reference path from the starting point to the destination is generated based on a global path planning algorithm. The specific process is as follows:

[0091] (1) Construct the cost function for each node of the raster map.

[0092] Using A The algorithm performs a global path search, defining the cost function for each node in the raster map as follows:

[0093] ;

[0094] in, This represents the distance from the starting point to the current node. The actual cost; Indicates starting from the current node The heuristic cost to reach the target node.

[0095] Using Manhattan distance, calculate the distance from the current node. The heuristic cost to the target node is calculated as follows:

[0096] ;

[0097] in, Indicates the current node World coordinates; Represents the target node The world coordinates.

[0098] (2) Start the search from the node with the minimum cost function until the target node is found; generate a global reference path from the starting point to the ending point based on the node with the minimum cost function and the target node.

[0099] By prioritizing the search cost function The smallest node generates the shortest collision-free path from the start point to the end point, which is the global reference path from the start point to the end point.

[0100] (3) Discretize the global reference path into a path point sequence to obtain a discrete path point sequence.

[0101] Discretize the global reference path into a sequence of path points. The formula is:

[0102] ;

[0103] in, Here are the world coordinates of the path points, and N is the total number of path points.

[0104] S102. Preprocess the global reference path to obtain the indexed global reference path.

[0105] For A The generated discrete paths are smoothed to remove redundant inflection points, and a fast search mechanism for path point indexes is built.

[0106] The global reference path is preprocessed to obtain an indexed global reference path. The specific process is as follows:

[0107] (1) Use B-spline curves to interpolate the discrete path point sequence to obtain a smooth path;

[0108] B-spline curves are used to interpolate the discrete path point sequence to reduce path inflection points and improve the smoothness of the global path.

[0109] (2) Based on the path curvature of the smooth path and the set distance threshold between adjacent points, extract the key waypoints of the global reference path and construct a sequential index sequence;

[0110] Based on the path curvature of the smooth path and a set distance threshold between adjacent points, key waypoints of the global reference path are extracted, and a sequential index sequence is constructed, specifically including:

[0111] 1) Discretely sample the path after it has been smoothed by B-spline curves, and calculate the curvature information of the path at each sampling point based on the geometric relationship between adjacent sampling points.

[0112] 2) When the path curvature is greater than the preset curvature threshold, the corresponding path point is marked as a critical waypoint to preserve the main turning features of the path.

[0113] The preset curvature threshold is set based on the curvature change characteristics after path smoothing, and is used to identify significant turning points in the path.

[0114] 3) Set a distance threshold based on the Euclidean distance between adjacent path points. When the distance between the current path point and the previous selected critical path point exceeds the distance threshold, select the path point as the new critical path point to avoid the path guidance accuracy being affected by the excessive spacing between critical path points.

[0115] By using the joint screening method based on curvature features and distance constraints, the number of redundant path points is reduced while ensuring the integrity of global path geometric features. This results in the construction of a sequence of key waypoints for subsequent path index search and look-ahead guidance.

[0116] (3) Record the index of the nearest point of the robot on the global path at the previous moment, and use it as the starting point for the index search at the current moment to obtain the global reference path with index.

[0117] Record the index of the robot's nearest point on the global path at the previous moment, and use it as the starting point for the index search at the current moment to improve search efficiency.

[0118] After the above steps, for A The generated discrete paths are smoothed, key waypoints are extracted to construct a look-ahead guidance sequence, and the efficiency of waypoint index search is optimized.

[0119] like Figure 1 As shown, in step S2, a global path look-ahead point guidance mechanism is designed for the indexed global reference path to determine the nearest point and the look-ahead target point of the robot's current position, and to calculate the path tracking cost.

[0120] S201. For the indexed global reference path, design a global path look-ahead point guidance mechanism to determine the nearest point of the robot's current position and the look-ahead target point.

[0121] The design incorporates global path lookahead points to guide the tracking cost by calculating the geometric relationship between the current position and lookahead points on the global path.

[0122] Let the robot's current position be A The discrete point sequence of the global path generated by the algorithm is ,in, This represents the total number of path points.

[0123] For indexed global reference paths, a global path look-ahead point guidance mechanism is designed to determine the nearest point to the robot's current position and the look-ahead target point. The specific process is as follows:

[0124] (1) Get the index of the path point closest to the current position on the global reference path.

[0125] Find the index of the point on the global path that is closest to the current position. The formula is:

[0126] ;

[0127] in, This indicates the nearest point index hint from the previous time step, used to speed up the search process; This represents the path point on the global reference path that is closest to the current position, i.e., the point closest to the current position; Indicates the first global reference path There are path points.

[0128] (2) Calculate the look-ahead target point index based on the nearest path point index of the current location and the preset look-ahead step size, and determine the look-ahead target point.

[0129] The preset look-ahead step size is This controls the degree of foresight in path tracking.

[0130] Calculate the lookahead target point index based on the index of the current nearest point and the preset lookahead step size. The formula is:

[0131] .

[0132] Determine the forward target points based on the forward target point index. for:

[0133] .

[0134] S202, Calculate the path tracing cost.

[0135] The path tracking cost function is defined as the squared Euclidean distance from the current position to the look-ahead point, and the formula for calculating the path tracking cost is:

[0136] .

[0137] After the above steps, using squared distance instead of Euclidean distance to calculate path tracing cost can avoid square root operations, improve computational efficiency, and maintain the convexity of the cost function.

[0138] like Figure 1 As shown, in step S3, a path progress reward mechanism is designed based on the nearest point to the robot's current position, and the path progress reward cost is calculated.

[0139] To address the "circling stagnation" problem that may occur in local trajectory optimization, a reward mechanism based on path progress is designed. This mechanism quantifies the vehicle's progress along the global path and incentivizes the algorithm to select trajectories that can effectively advance path tracking.

[0140] Specifically, (1) calculate the difference between the index of the nearest path point at the current time and the index at the previous time, and use it as the path progress gain.

[0141] In continuous time steps and Between these points, the path progress gain is defined as:

[0142] ;

[0143] in, and These represent the indices of the nearest path point at the current and previous time steps, respectively. The `max` function ensures that positive gain is only generated when moving forward, i.e., when the index increases, thus avoiding misjudgments caused by positioning fluctuations.

[0144] (2) Use the negative weighting of the path progress gain as the path progress reward cost.

[0145] The path progress reward function is expressed as:

[0146] ;

[0147] in, Indicates path progress gain; This represents the reward weight coefficient; the negative sign indicates that the reward is actually treated as a negative cost, i.e., a reduction in penalty. Integrating into the overall cost function, the incentive algorithm selects the trajectory that can be followed along the path.

[0148] By following the steps above, a global path guidance mechanism that includes look-ahead guidance and path tracking costs can be designed to effectively guide local MPPI sampling, thereby improving the global consistency of the MPPI trajectory and avoiding local optima and "circling stagnation" phenomena.

[0149] like Figure 1 As shown, in step S4, the corridor constraint cost is calculated based on the nearest point to the robot's current position; the orientation error cost is calculated based on the forward target point; and a multi-dimensional constraint cost function is constructed based on the path tracking cost, path progress reward cost, corridor constraint cost, and orientation error cost.

[0150] S401. Calculate the corridor constraint cost based on the nearest point to the robot's current position.

[0151] The cost of corridor constraints is to construct a virtual and adjustable safe corridor around the global path, limiting the sampled trajectory of the model predicted path integral (MPPI) to this reasonable range.

[0152] The specific process for calculating the corridor constraint cost is as follows:

[0153] (1) Calculate the Euclidean distance from the robot’s current position to the nearest point on the global reference path as the deviation distance.

[0154] Let the robot's current position be A The point on the path closest to the current position is The deviation distance is defined using the Euclidean norm. The formula is:

[0155] .

[0156] (2) The positive value of the deviation distance minus the preset corridor width is taken as the corridor violation amount.

[0157] Corridor violation The formula is:

[0158] ;

[0159] in, This indicates the width of the corridor, which represents the maximum allowable deviation distance.

[0160] A positive penalty is only applied when the deviation exceeds the width of the corridor; deviations within the corridor width are not penalized.

[0161] (3) A two-fold penalty is adopted, and the quadratic weighted value of the corridor violation is used as the corridor constraint cost.

[0162] The corridor constraint cost adopts a double penalty form, and the formula is as follows:

[0163] .

[0164] After the above steps, a quadratic penalty is adopted to make the cost function continuously differentiable, which is beneficial to the convergence of the optimization algorithm. The larger the deviation, the faster the penalty increases, effectively constraining the trajectory behavior. At the same time, there is no penalty within the corridor width, giving the algorithm sufficient optimization freedom.

[0165] S402. Calculate the orientation error cost based on the forward target point.

[0166] To ensure the robot's movement direction remains consistent with the path's forward direction, suppress the robot's "snake-like" movement, and achieve natural and smooth path tracking, an orientation error cost is designed. The error between the robot's current heading and the expected heading towards the target point is calculated, and the orientation error cost is generated. The specific process is as follows:

[0167] (1) Obtain the desired motion direction vector.

[0168] Let the robot's current position be Forward target points are Then the desired motion direction vector is:

[0169] .

[0170] (2) Calculate the desired heading angle based on the vector direction.

[0171] Desired heading angle formula for:

[0172] ;

[0173] in, It is a four-quadrant arctangent function, ensuring that the angle calculation is correct.

[0174] (3) Calculate the heading error based on the robot’s expected heading angle and normalize it.

[0175] By acquiring the robot's current pose state information, we can obtain the robot's current actual heading angle. Based on the robot's expected heading angle and expected heading angle The formula for calculating the heading error is:

[0176] .

[0177] To ensure that the angle difference is within a reasonable range, the heading error is normalized to obtain the normalized heading error. The formula is:

[0178] .

[0179] Normalization operations ensure that the heading error always falls within a certain range. Within the range, the angle wrapping problem is avoided.

[0180] (4) A quadratic penalty is adopted, and the square weighted value of the normalized orientation error is used as the orientation error penalty cost.

[0181] Orientation error cost adopts a quadratic penalty form, as shown in the formula:

[0182] .

[0183] After the above steps, a quadratic penalty is adopted to ensure that positive and negative deviations are penalized equally, achieving symmetrical penalty. This results in slight penalties for small errors and strong penalties for large errors, while also making the cost function continuously differentiable, which is beneficial to the numerical stability of the optimization algorithm.

[0184] S403. Based on path tracking cost, path progress reward cost, corridor constraint cost, and orientation error cost, a cost function with multi-dimensional constraints is constructed.

[0185] The cost function of multi-dimensional constraints includes path tracking cost, path progress reward cost, corridor constraint cost, and orientation error cost.

[0186] The cost function of multi-dimensional constraints also includes obstacle avoidance cost, velocity cost, acceleration cost, control input cost, curvature cost, angular velocity cost, and terminal state cost.

[0187] The cost function for constructing multi-dimensional constraints is as follows:

[0188] ;

[0189] in, Indicates the cost of path tracing; This indicates the cost of advancing rewards, i.e., the cost of rewarding progress along the path; Indicates the cost of corridor constraints; Indicates the cost of orientation error; Indicates the cost of obstacle avoidance; Indicates the cost of speed; Indicates the cost of acceleration; Indicates the cost of controlling input; Indicates curvature cost; Indicates the cost of angular velocity; This represents the terminal cost. The cost functions for each term are shown in Table 1.

[0190] Table 1 Multi-objective cost function system

[0191]

[0192] The terminal cost is calculated based on the deviation between the predicted trajectory's terminal state and the target state. It is used to guide the path planning result to approach the target position at the end of the prediction time domain. Specifically:

[0193] Let the position of the end of the trajectory be The target location is The terminal error formula is:

[0194] .

[0195] A quadratic penalty approach is adopted, using the squared weighted value of the terminal error as the terminal cost, as shown in the formula:

[0196] .

[0197] In addition, the obstacle avoidance cost is calculated based on the distance relationship between the robot's predicted trajectory and obstacles in the environment. When the predicted trajectory is close to an obstacle or there is a potential collision risk, the corresponding cost increases to guide the trajectory away from the obstacle.

[0198] Velocity cost, acceleration cost, curvature cost, and angular velocity cost are calculated based on the robot's motion state and control input in the predicted trajectory. They are used to constrain the robot's motion within its kinematic and dynamic limits and improve the smoothness and stability of the trajectory.

[0199] The control input cost is calculated based on the amplitude and changes of each control variable in the predictive control sequence. It is used to suppress excessive control input and drastic changes, and to improve the continuity of the control process and energy utilization efficiency.

[0200] Through the above steps, the various weight coefficients constitute a precise balance system. The safety weight is relatively high to ensure basic safety; the tracking weight is moderate to guarantee task execution quality; the smoothness weight is relatively low to optimize motion quality; and the terminal weight is independently adjustable to control convergence characteristics. The various cost functions form an organic whole, with different costs focusing on different aspects of optimization, collectively guiding the trajectory towards the global optimum. The weights can be adjusted for different scenarios. This approach balances path smoothness, obstacle avoidance safety, and motion efficiency, suppressing "snake-like" motion.

[0201] like Figure 1 As shown, in step S5, the local trajectory of the model's predicted path integral is optimized based on the cost function with multi-dimensional constraints to obtain the optimal control sequence, and dynamic path planning is performed.

[0202] With A Guided by the global path, candidate control sequences are generated in the MPPI framework. The quality of the trajectory is evaluated based on a multi-dimensional cost function, and the optimal control sequence is obtained through weight fusion, thus achieving the purpose of local trajectory sampling and optimization.

[0203] Candidate control sequences are generated based on the MPPI framework. Trajectories are predicted using a mobile robot dynamics model. The cumulative cost of each trajectory is calculated, and the optimal control sequence is obtained through weighted fusion. The specific process is as follows:

[0204] (1) Gaussian noise with time autocorrelation is superimposed on the historical best control sequence to generate multiple candidate control sequences.

[0205] Historical optimal control sequence Based on this, where T is the prediction time domain length, Gaussian noise is added to generate... There are 10 candidate control sequences, and the formula is:

[0206] ;

[0207] in, The noise is zero-mean Gaussian noise, and the diagonal elements of the noise covariance matrix correspond to the noise intensity of acceleration and angular velocity, respectively.

[0208] Among them, the historical optimal control sequence is the optimal control sequence obtained by weighting and fusing multiple candidate control sequences through the model prediction path integral algorithm in the previous planning period, and is updated according to the rolling time domain principle.

[0209] The smoothness of the sampling trajectory is controlled by the noise autocorrelation coefficient, generating a smooth candidate control sequence, as shown in the formula:

[0210] ;

[0211] in, It is independent Gaussian noise. To predict the time step in the time domain, This is the noise autocorrelation coefficient.

[0212] After the above steps, control sequence sampling is achieved, providing data support for subsequent functional operations.

[0213] (2) Based on the mobile robot dynamics model, trajectory prediction is performed using candidate control sequences.

[0214] The discrete-time equations of the mobile robot's dynamics model are:

[0215] ;

[0216] ;

[0217] ;

[0218] ;

[0219] in, Indicates the robot's wheelbase. Indicates the discrete time step. , These represent the minimum and maximum speeds, respectively.

[0220] Based on the mobile robot dynamics model, candidate control sequences are used The formula for predicting the state trajectory in the next T steps is:

[0221] ;

[0222] in, This represents the robot's position, heading angle, and velocity at time step t.

[0223] (3) Calculate the cumulative cost of each predicted trajectory based on the cost function of the multidimensional constraints.

[0224] The cumulative cost for each predicted trajectory is calculated using the following formula:

[0225]

[0226] in, The stage cost at time step t includes path tracking cost, path progress reward cost, corridor constraint cost, orientation error cost, obstacle avoidance cost, velocity cost, acceleration cost, control input cost, curvature cost, angular velocity cost, and terminal state cost. This is the cost to the end user.

[0227] (4) Assign weights to each candidate trajectory based on the cumulative cost, and update the optimal control sequence by weighted average.

[0228] 1) Assign weights to each candidate trajectory based on the cumulative cost.

[0229] The formula is:

[0230] ;

[0231] The lower the cumulative cost, the greater the weight assigned to the candidate trajectory.

[0232] The candidate trajectory weights are calculated and normalized to obtain the normalized weights corresponding to the candidate control sequences, as shown in the formula:

[0233] ;

[0234] in, The minimum cumulative cost for all candidate trajectories; The temperature parameter indicates the degree of control weight concentration. To avoid the minimum value when divided by zero.

[0235] 2) Update the optimal control sequence by weighted average.

[0236] The formula for updating the optimal control sequence is:

[0237] ;

[0238] in, The normalized weights are those corresponding to the k-th candidate control sequence. The control perturbation introduced for the k-th candidate control sequence in the prediction time domain.

[0239] The weights are calculated based on the cumulative cost of the predicted trajectory corresponding to each candidate control sequence. The smaller the cumulative cost of the candidate control sequence, the greater the weight. The weights of all candidate control sequences are then normalized.

[0240] (5) Only execute the first control variable in the optimal control sequence, and use the remaining sequence as the initial control sequence for the next time step. Repeat the above steps to perform dynamic path planning.

[0241] Specifically, only the first control variable of the optimal control sequence is executed. , the remaining control sequence As the initial control sequence for the next moment Repeat steps (1) to (4) of the sampling-prediction-evaluation-fusion process to achieve real-time dynamic planning.

[0242] Through the above steps, the sampling-prediction-evaluation-fusion process is realized, which enables real-time dynamic planning.

[0243] like Figure 1 As shown, in step S6, the motion prediction and obstacle avoidance costs of moving obstacles are optimized.

[0244] The optimization process for motion prediction and obstacle avoidance costs for moving obstacles is as follows:

[0245] (1) Calculate the predicted collision time based on the speed and direction of the dynamic obstacle.

[0246] To improve the obstacle avoidance performance of the algorithm in dynamic environments, the obstacle avoidance cost term is optimized.

[0247] Based on the speed and direction of the dynamic obstacle, the predicted collision time (TTC) is calculated using the following formula:

[0248] ;

[0249] in, For dynamic obstacle speed; The angle between the robot's direction of motion and the obstacle's direction of motion; This predicts the relative distance between the robot's current position on its trajectory and the corresponding dynamic obstacle.

[0250] (2) Adjust the weight of obstacle avoidance cost based on the predicted collision time.

[0251] Obstacles with shorter predicted collision times (TTCs) are assigned higher obstacle avoidance weights, as shown in the formula:

[0252] ;

[0253] in, This represents the obstacle avoidance weight for static obstacles.

[0254] (3) Adjust the prediction time domain length of the model's predicted path integral dynamically according to the number and speed of the dynamic obstacles.

[0255] The robot's environmental perception module acquires real-time dynamic obstacle status information, including the number and speed of movement, and dynamically adjusts the prediction time domain length T of MPPI. When obstacles are dense or moving at high speeds, T is increased to predict collision risks in advance; when the environment is open, T is decreased to reduce computational load.

[0256] Through the above steps, for dynamic environments, obstacle avoidance response speed and collision-free rate are improved by weighting dynamic obstacle distance and dynamically adjusting the prediction time domain, thereby enhancing adaptability in both static and dynamic environments.

[0257] like Figure 2 As shown, the mobile robot path planning method based on global guidance MPPI is simulated according to steps S1-S6.

[0258] First, A Based on environmental map and obstacle information, the algorithm uses a global path planning algorithm to generate a global reference path from the starting point to the ending point, and transforms the global reference path into the world coordinate system as guiding information for local trajectory planning.

[0259] Subsequently, combining the robot's initial state, kinematic model, and control constraints, and based on a global guidance mechanism and a multi-dimensional constraint cost function, rolling optimization is performed on the local trajectory of the model's predicted path integral to generate candidate control sequences and calculate the cumulative cost of the corresponding predicted trajectories. Weights are assigned to each candidate trajectory according to the cumulative cost, and the optimal control sequence is updated through weighted fusion. The first control variable in the optimal control sequence is then executed to update the robot's state and record the trajectory. If the target position is not reached, the control sequence is updated according to the rolling time-domain principle, and the above process is repeated until the target position is reached.

[0260] like Figure 3 As shown, this illustrates a comparison between two strategies for mobile robot path planning: local obstacle avoidance and global guidance. Figure 3 (a) is local obstacle avoidance, where the dashed line marks the wall stop. The robot starts from the starting point and plans the path only based on the target direction and the local distance (d1 / d2 / d3) of the obstacle. When it encounters an obstacle, it can only go around in a small circle and stagnates locally, and cannot move towards the target point efficiently. Figure 3(b) Global path guidance has been added, where the routes connected by green nodes constitute the global path. The robot moves along the globally planned route, and by combining obstacle distance information, it can bypass local obstacles and efficiently move towards the target point, avoiding local stagnation. It is evident that relying solely on local obstacle avoidance can easily lead to difficulties, while adding global path guidance can improve the efficiency and rationality of robot navigation.

[0261] like Figure 4 The image shows a comparison of trajectory optimization for a mobile robot with and without corridor constraints, based on a global path. Among these... Figure 4 (a) Unconstrained motion trajectory: The robot starts from the beginning and moves along the global path. The red / black dashed lines are unconstrained. As a result, the trajectory deviates excessively from the global path and even approaches obstacles, posing a collision risk and not meeting the actual motion requirements. Figure 4 (b) The constrained motion trajectory incorporates start and end constraints. The trajectory is represented by a solid colored line and is constrained within a reasonable range, conforming to the global path while avoiding obstacles, and simultaneously satisfying the vehicle's motion characteristics (such as steering and speed limits). It is evident that adding vehicle-specific constraints to the robot's motion trajectory makes it safer and more aligned with actual needs.

[0262] like Figure 5 As shown, a comparison of mobile robot trajectories with and without orientation error mechanisms is presented. Figure 5 (a) When the robot moves along the global path, the actual trajectory is a colored line, while the desired trajectory is a dashed line. There is a deviation between the two, with a tendency to deviate clockwise, resulting in a serpentine trajectory. An orientation error mechanism is added, i.e. Figure 5 (a) The orange correction arrow indicates that the trajectory needs to be adjusted to reduce deviation. Figure 5 (b) shows the trajectory after error correction. After real-time feedback correction, the robot's actual trajectory (green line) has aligned with the global path, and the connection with the target point is more precise. It can be seen that by introducing an orientation error mechanism and dynamically correcting it, the trajectory has returned from the deviation state to the expected planned route.

[0263] like Figure 6 As shown, where, Figure 6 (a) indicates that when the robot detects a dynamic obstacle during its movement, it adjusts the local path in advance based on the predicted trajectory, and the planned trajectory deviates from the original global reference path in order to avoid the dynamic obstacle in advance. Figure 6(b) indicates that during the avoidance process, the robot continues to move according to the updated local optimized trajectory, successfully bypassing the dynamic obstacle and moving towards the target point. The figure shows that in complex environments with dynamic obstacles, the method in this embodiment can adjust the robot's motion path in real time based on the predicted trajectory, achieving smooth avoidance of dynamic obstacles and ensuring the robot's stable progress along the global reference path.

[0264] like Figure 7 As shown, in this example, the GMPPI algorithm is compared with the multi-algorithm path planning. Although the trajectories of the algorithms all successfully avoid obstacles and reach the destination smoothly from the starting point, the GMPPI algorithm in this example performs better in terms of following the optimal path and path smoothness.

[0265] Example 2

[0266] The purpose of this embodiment is to provide a mobile robot path planning system based on Global Guided Pivot Instructions (MPPI), including:

[0267] The global path planning module is used to generate a global reference path from the starting point to the ending point based on the global path planning algorithm, and to preprocess the global reference path to obtain an indexed global reference path.

[0268] The look-ahead guidance module is used to design a global path look-ahead guidance mechanism for indexed global reference paths, determine the nearest point and the look-ahead target point of the robot's current position, and calculate the path tracking cost.

[0269] The path progress evaluation module is used to design a path progress reward mechanism and calculate the path progress reward cost based on the nearest point to the robot's current position.

[0270] The multi-dimensional cost function construction module is used to calculate the corridor constraint cost based on the nearest point of the robot's current position; calculate the orientation error cost based on the forward target point; and construct the multi-dimensional constraint cost function based on the path tracking cost, path progress reward cost, corridor constraint cost, and orientation error cost.

[0271] The MPPI local trajectory optimization module is used to optimize the local trajectory of the model's predicted path integral based on a cost function with multi-dimensional constraints, to obtain the optimal control sequence and perform dynamic path planning.

[0272] It also includes a dynamic environment optimization module, which is used to optimize the motion prediction and obstacle avoidance costs of moving obstacles.

[0273] Based on the provided mobile robot path planning system based on global guidance MPPI, the method steps in Embodiment 1 are implemented.

[0274] Example 3

[0275] This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the steps of the method in Embodiment 1.

[0276] Example 4

[0277] This embodiment provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, performs the steps of the method in Embodiment 1.

[0278] The steps and methods involved in the systems of the above embodiments correspond to those in Embodiment 1. For specific implementation details, please refer to the relevant description section of Embodiment 1. The term "computer-readable storage medium" should be understood as a single medium or multiple media including one or more instruction sets; it should also be understood as including any medium capable of storing, encoding, or carrying an instruction set for execution by a processor and enabling the processor to perform any of the methods in this invention.

[0279] Those skilled in the art will understand that the modules or steps of the present invention described above can be implemented using general-purpose computer devices. Optionally, they can be implemented using computer-executable program code, thereby allowing them to be stored in a storage device for execution by a computer device, or they can be fabricated as separate integrated circuit modules, or multiple modules or steps can be fabricated as a single integrated circuit module. The present invention is not limited to any particular combination of hardware and software.

[0280] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A mobile robot path planning method based on globally guided MPPI, characterized in that, include: A global reference path from the starting point to the ending point is generated based on a global path planning algorithm, and the global reference path is preprocessed to obtain an indexed global reference path. For indexed global reference paths, a global path look-ahead point guidance mechanism is designed to determine the nearest point to the robot's current position and the look-ahead target point, and to calculate the path tracking cost. The specific process is as follows: Get the index of the path point closest to the current position on the global reference path; Based on the nearest path point index at the current location and the preset look-ahead step size, calculate the look-ahead target point index and determine the look-ahead target point; Based on the robot's current location and the nearest point, design a path progress reward mechanism and calculate the path progress reward cost. Calculate the corridor constraint cost based on the nearest point to the robot's current location; the specific process is as follows: The deviation distance is calculated as the Euclidean distance from the robot's current position to the nearest point on the global reference path. The positive value of the deviation distance minus the preset corridor width is taken as the corridor violation amount; A two-stage penalty approach is adopted, using the squared weighted value of the corridor violation as the cost of the corridor constraint; Calculate the orientation error cost based on the forward target point; A cost function with multi-dimensional constraints is constructed based on path tracking cost, path progress reward cost, corridor constraint cost, and orientation error cost. Based on a cost function with multi-dimensional constraints, the local trajectory of the model's predicted path integral is optimized to obtain the optimal control sequence. Dynamic path planning is performed based on the optimal control sequence.

2. The mobile robot path planning method based on global guidance MPPI as described in claim 1, characterized in that, It also includes optimizing the motion prediction and obstacle avoidance costs of moving obstacles, the specific process of which is as follows: Calculate and predict collision time based on the speed and direction of dynamic obstacles; Adjust the weight of obstacle avoidance costs based on the predicted collision time; The prediction time domain length of the model's predicted path integral is dynamically adjusted based on the number and speed of dynamic obstacles.

3. The mobile robot path planning method based on global guidance MPPI as described in claim 1, characterized in that, A global reference path from the starting point to the destination is generated based on a global path planning algorithm. The specific process is as follows: Construct the cost function for each node of the raster map; The search begins at the node with the minimum cost function and continues until the target node is found. Based on the node with the minimum cost function and the target node, a global reference path from the starting point to the ending point is generated; The global reference path is discretized into a sequence of path points, resulting in a discrete sequence of path points.

4. The mobile robot path planning method based on global guidance MPPI as described in claim 1, characterized in that, The global reference path is preprocessed to obtain an indexed global reference path. The specific process is as follows: A smooth path is obtained by interpolating the discrete path point sequence using B-spline curves. Based on the path curvature of the smooth path and the set distance threshold between adjacent points, the key waypoints of the global reference path are extracted, and a sequential index sequence is constructed. Record the index of the nearest point on the global path at the previous moment, and use it as the starting point for the index search at the current moment to obtain the indexed global reference path.

5. The mobile robot path planning method based on global guidance MPPI as described in claim 1, characterized in that, Based on the robot's current position and the nearest point, a path progress reward mechanism is designed to calculate the path progress reward cost. Specifically, the difference between the index of the nearest path point at the current time and the index at the previous time is calculated and used as the path progress gain; the negative weighted value of the path progress gain is used as the path progress reward cost.

6. The mobile robot path planning method based on global guidance MPPI as described in claim 1, characterized in that, The multi-dimensional constraint cost function includes at least: path tracking cost, path progress reward cost, corridor constraint cost, orientation error cost, obstacle avoidance cost, terminal cost, velocity cost, control velocity cost, curvature cost, and angular velocity cost.

7. The mobile robot path planning method based on global guidance MPPI as described in claim 1, characterized in that, include: Based on a cost function with multi-dimensional constraints, the local trajectory of the model's predicted path integral is optimized to obtain the optimal control sequence, and dynamic path planning is then performed. The specific process is as follows: Multiple candidate control sequences are generated by superimposing Gaussian noise with time autocorrelation onto the historical best control sequence. Trajectory prediction is performed using candidate control sequences based on a mobile robot dynamics model. The cumulative cost of each predicted trajectory is calculated based on the cost function with multi-dimensional constraints. Each candidate trajectory is assigned a weight based on the cumulative cost, and the optimal control sequence is updated by weighted average. Only the first control variable in the optimal control sequence is executed, and the remaining sequence is used as the initial control sequence for the next time step. The above process is repeated to perform dynamic path planning.

8. A mobile robot path planning system based on globally guided MPPI, characterized in that, include: The global path planning module is used to generate a global reference path from the starting point to the ending point based on the global path planning algorithm, and to preprocess the global reference path to obtain an indexed global reference path. The look-ahead guidance module is used to design a global path look-ahead guidance mechanism for indexed global reference paths, determine the nearest point to the robot's current position and the look-ahead target point, and calculate the path tracking cost. The specific process is as follows: Get the index of the path point closest to the current position on the global reference path; Based on the nearest path point index at the current location and the preset look-ahead step size, calculate the look-ahead target point index and determine the look-ahead target point; The path progress evaluation module is used to design a path progress reward mechanism and calculate the path progress reward cost based on the nearest point to the robot's current position. The multi-dimensional cost function construction module is used to calculate the corridor constraint cost based on the nearest point to the robot's current position, and to calculate the orientation error cost based on the forward target point. A cost function for multi-dimensional constraints is constructed based on path tracking cost, path progress reward cost, corridor constraint cost, and orientation error cost. The corridor constraint cost is calculated based on the robot's current closest point. The specific process is as follows: The deviation distance is calculated as the Euclidean distance from the robot's current position to the nearest point on the global reference path. The positive value of the deviation distance minus the preset corridor width is taken as the corridor violation amount; A two-stage penalty approach is adopted, using the squared weighted value of the corridor violation as the cost of the corridor constraint; The MPPI local trajectory optimization module is used to optimize the local trajectory of the model's predicted path integral based on a cost function with multi-dimensional constraints, to obtain the optimal control sequence and perform dynamic path planning.

Images