Wheeled robot path planning method fusing improved Informed-RRT* and TEB algorithms

Abstract

The invention discloses a wheeled robot path planning method fusing an improved Inform-RRT * algorithm and a TEB algorithm, and belongs to the technical field of robot path planning. According to the method, firstly, global path planning is carried out through an improved Informed-RRT * algorithm integrating a target bias strategy and a sampling area limiting strategy, an adaptive step length, node optimization screening and father node reselection mechanism is introduced, an optimized global path is rapidly generated, redundant nodes are removed through a greedy strategy, and a simplified global path is obtained. And then, discretizing the generated global path into a pose point sequence, combining the pose point sequence with a time sequence, constructing a TEB trajectory model, and solving a local optimal trajectory through a multi-target weighted optimization function under the conditions of meeting Ackerman model kinematics constraint, obstacle avoidance, path following and time optimization. According to the method, the problems that in the prior art, the path is not smooth, does not conform to the kinematics constraint of the robot and is slow in convergence are effectively solved, and a safe and smooth track capable of being efficiently executed can be planned for the wheeled robot.

Description

Technical Field

[0001] This invention relates to the field of robot path planning technology, and specifically to a path planning method for wheeled robots that integrates an improved Informed-RRT* and TEB algorithms. Background Technology

[0002] In recent years, with the rapid development of robotics technology, path planning algorithms have become one of the research hotspots in this field. Based on the degree of environmental information they possess, existing algorithms can be mainly divided into two categories: global path planning and local path planning. Global path planning algorithms, based on a known environmental model, plan a feasible path from the starting point to the destination for the robot. Their planning results typically exhibit global superiority, and the planning accuracy depends on the accuracy of the environmental information. However, these algorithms have poor adaptability when the environment changes and are sensitive to errors and noise in the environmental model. Local path planning algorithms, on the other hand, do not rely entirely on global environmental information but rather perform real-time planning based on the robot's currently perceived local environment. Therefore, they have good obstacle avoidance capabilities and strong robustness in dynamic environments. However, due to the lack of a global perspective, these algorithms are prone to getting trapped in local optima.

[0003] The Informed-RRT* algorithm is a classic global path planning algorithm, an improvement upon the RRT* algorithm. Before finding a feasible path, it operates similarly to the RRT* algorithm; however, after finding a path, it uses elliptic sampling instead of global uniform sampling, reducing redundant nodes and improving the search efficiency and convergence speed. Nevertheless, this algorithm still suffers from the problem of overly random node generation during iterative path pruning and optimization, and the resulting paths are not smooth enough, potentially leading to collision risks for wheeled robots.

[0004] The Time-Elastic-Band (TEB) algorithm is a typical local path planning algorithm. Its core idea is to decouple the path planning problem into two parts: path optimization and time optimization. This algorithm inserts trajectory points containing time information between two fixed endpoints on an elastic path, and then uses the G2o optimization algorithm to solve for the optimal path based on time parameterization. The TEB algorithm allows the robot to dynamically adjust its pose during movement, which helps reduce the posture correction time after reaching the target point and improves motion smoothness.

[0005] In complex and dynamic environments, the "real-time performance, smoothness, and safety" of wheeled robot path planning directly determine its task execution efficiency and application feasibility. Especially in core scenarios such as autonomous driving, intelligent logistics, and industrial inspection, traditional algorithms struggle to balance path optimization and motion feasibility under multiple constraints, becoming a key bottleneck restricting the large-scale application of robot technology. Therefore, there is an urgent need for an innovative path planning method that combines global planning optimization with local adjustment flexibility. Summary of the Invention

[0006] The purpose of this invention is to provide a path planning method for wheeled robots that integrates the improved Informed-RRT* and TEB algorithms. Traditional path planning algorithms struggle to balance global path optimality, local dynamic obstacle avoidance flexibility, kinematic constraint adaptability of wheeled robots, and multi-objective collaborative optimization in complex environments. This invention provides an efficient, smooth, safe, and dynamically feasible path planning scheme.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A path planning method for wheeled robots that integrates improved Informed-RRT* and TEB algorithms includes the following steps: S1. Environmental Modeling and Initialization: Environmental information is collected using sensors mounted on the wheeled robot to construct an environmental map model and determine the map boundaries and the robot's starting point x. start Target point x goal and the location of obstacles; S2. Global Path Planning: An improved Informed-RRT* algorithm is used to plan a path from the starting point x in the environment map. start to target point x goal The global path; The improved Informed-RRT* algorithm includes: (1) A sampling method that combines a restricted sampling region strategy with a target bias strategy is used to generate samples from the starting point x. start to target point x goal An initial path; (2) After obtaining the initial path, starting from point x start and target point x goal Random sampling is performed within an elliptical region with the current optimal path length as the major axis, focusing on the current optimal path length, to obtain the sampling point x. rand Find the distance x in a random tree rand The nearest node x nearst A new node x is selected along the direction of the line connecting the two nodes with an adaptive step size S. new ; (3) For each new node x generated by sampling new Collision detection and node optimization screening are performed; the node optimization screening involves calculating a new node x. new to the starting point x start The Euclidean distance to the target point x goal If the sum of the Euclidean distances is greater than the currently known optimal global path length, then the new node x is discarded. new Otherwise, it is added to the random expansion tree; (4) Execute a parent node reselection strategy during the random tree generation process, with x new Define a neighborhood range with the center of the circle and an adaptive step size S as the radius, and find the starting point x. start To x new Replace the parent node with the one with the lower path cost and perform a rewiring operation; repeat steps (2) to (3) until a new node x is reached. new Enter target point x goal The neighborhood is defined, an initial global path is generated, the elliptical sampling region is updated and iteratively optimized, and then a greedy strategy is used to remove redundant nodes in the path to obtain a simplified global path. S3. Local Trajectory Optimization: The global path obtained in step S2 is discretized into a sequence of pose points using the TEB algorithm. This sequence is then combined with a time interval sequence to construct a TEB trajectory model. By solving a multi-objective weighted optimization problem that includes obstacle avoidance constraints, path following constraints, velocity and acceleration constraints, time-optimal constraints, and nonholonomic kinematic constraints based on the Ackerman steering model, a smooth and kinematically feasible local optimal trajectory is generated.

[0008] Furthermore, in step S2, the restricted sampling area is defined as starting from point x. start With target point x goal The rectangular sampling region centered on the connecting line, and the specific strategies for limiting the sampling region include: The rectangular sampling area is divided into multiple sub-regions, and search priorities and expansion number thresholds are set. First, sampling expansion is performed in the central sub-region with the highest priority. If the initial path is not found after reaching the first expansion number threshold, the sampling range is expanded to the adjacent sub-regions. If the initial path is not found after reaching the second expansion number threshold, sampling is performed in the entire map area. The target bias strategy specifically includes: Set a target bias probability threshold P t During each sampling, a probability value is randomly generated based on the uniform sampling probability. If this probability value is greater than the target bias probability threshold P, then... t Then the target point x goal Directly used as sampling point x rand Otherwise, use the SampleFree() function to randomly generate a sampling point within the restricted sampling area.

[0009] Furthermore, in step S2, the specific strategy for limiting the sampling region is as follows: [The starting point x...] start With target point x goal Centered on the connection, the environment is... Figure 5The map is divided into four regions: D1, D2, D3, D4, and D5. Thresholds T1 and T2 are set for the number of times the random tree is expanded. The search is first performed in region D1. If the initial path is not found after the number of expansions reaches T1, the search is expanded to regions D2 and D3. If the initial path is still not found after the number of expansions reaches T2, the random sampling range is expanded to the entire map.

[0010] Furthermore, in step S2, the adaptive step size S is dynamically adjusted based on the distance between the current node and the target point, and the calculation formula is as follows: Where, d goal It is node x nearst With target point x goal The minimum Euclidean distance between them, where step is the expansion step size of the random tree, and r is the minimum distance between them. goal The ratio to step.

[0011] Furthermore, in step S2, the calculation formula for the node optimization screening is as follows: Among them: (x n ,y n ) is the new node x new Coordinates, (x s ,y s (x) is the starting point start Coordinates, (x g ,y g (x) represents the target point. goal coordinate; If D sum If the new sampling point is greater than the current optimal global path length, then discard it.

[0012] Furthermore, in step S2, the strategy for reselecting the parent node specifically includes: With new node x new A circular neighborhood is defined with the center as the center and the adaptive step size S as the radius. Iterate through the existing nodes in the random tree within the neighborhood, for each node x nearst Calculate if the new node x new Replace the parent node with x nearst parent node x parent Or grandfather node x ancestry Then, from the starting point x start To x new Path cost; If a node exists that reduces the path cost, then perform a rewiring operation and move the new node x... new Replace the parent node with this node.

[0013] Furthermore, in step S2, the greedy strategy is specifically as follows: From the starting point x start Begin by trying different points along the target point x. goal If a direct connection is made and the line connecting the two points does not collide with any obstacle, then this connection is used as a simplified global path. If a collision occurs, then from the target point x goal Start by traversing the path nodes backwards along the generated global path, searching for a node that does not collide with the current connection starting point; once the node is found, connect the two points directly, and delete all intermediate nodes between the two points in the original path. Use the newly successfully connected node as the starting point for the next iteration, and repeat the reverse traversal and connection process described above until the current connection starting point and the target point x are connected. goal A direct connection simplifies the entire global path, resulting in a simplified global path.

[0014] Furthermore, in step S3, the objective function of the multi-objective weighted optimization constructed by the TEB algorithm is: Where, γ k B represents the weighting coefficients, and B represents the TEB model. Includes the following sub-functions: obstacle avoidance function f obs (B) Path following function f path (B) Velocity constraint function f vel (B) Acceleration constraint function f acc (B) Time-optimal function f time (B) and the nonholonomic kinematic constraint function f kin (B); The nonholonomic kinematic constraint function f kin (B) Based on the Ackerman steering model, it is used to constrain adjacent pose points to be located on arcs with the same curvature, and to ensure that the turning radius corresponding to the curvature is not less than the minimum turning radius of the wheeled robot.

[0015] Furthermore, in step S2, after removing redundant nodes from the path using a greedy strategy, cubic spline interpolation is applied to the simplified global path to improve trajectory smoothness. Specifically, this includes: The simplified global path is divided into n+1 path points into n adjacent paths, and each path is assigned a cubic function. A system of coefficient equations is constructed by interpolation conditions, first derivative continuity conditions and second derivative continuity conditions of adjacent intervals. Boundary conditions are added to make the system of equations have a unique solution, and the coefficients of the cubic function in each segment are obtained by solving the system. A smooth global path is obtained by connecting the cubic functions of each segment, and this smooth global path serves as the basis for the subsequent discretization of the pose point sequence.

[0016] Furthermore, after generating the local optimal trajectory in step S3, a collision detection and processing step is also included: Collision detection is performed on the local optimal trajectory and map boundary. Multiple detection points are uniformly sampled between adjacent smooth points of the trajectory, and point-level collision detection is performed on each detection point. If a certain trajectory is determined to be unsafe, then a safe alternative point is searched for with multiple radii and multiple angles centered on the collision point. The original safe path point closest to the collision point is selected first, or a vertical offset is performed to avoid the obstacle and insert a new path point. The updated path point sequence is then smoothed and iteratively optimized again until the entire trajectory passes the safety check or reaches the maximum number of iterations, outputting a safe and smooth final planned path.

[0017] The beneficial effects of the above scheme are as follows: 1. In the global path planning phase, the improved Informed-RRT* algorithm integrates strategies such as restricted sampling region, target bias, and adaptive step size, fundamentally reducing sampling randomness and compressing the invalid search space. The restricted sampling region strategy prioritizes the core area between the starting point and the target point, avoiding blind global sampling and reducing the generation of redundant nodes; the target bias strategy uses probability threshold control to make sampling points more inclined towards the target direction, enhancing search guidance; the adaptive step size is dynamically adjusted according to obstacle density and target distance. Simultaneously, through node optimization filtering and greedy post-processing, redundant nodes are effectively eliminated, and the path structure is simplified, providing a higher quality and simpler input foundation for subsequent local optimization.

[0018] 2. In terms of trajectory generation and optimization, the TEB algorithm elevates the geometric path into a complete trajectory incorporating time, pose, and kinematic information. The nonholonomic kinematic constraints are designed based on the Ackerman steering model, resulting in a smooth final trajectory and seamless integration from path planning to motion control. Cubic spline interpolation can achieve continuity of the first and second derivatives of the path with a small number of path points, improving trajectory smoothness to adapt to robot motion characteristics and enabling collaboration with local optimization algorithms.

[0019] 3. This method also demonstrates good overall synergy and robustness. The improved Informed-RRT* algorithm is responsible for quickly generating a global reference path that is as short as possible and collision-free in the known environment; the TEB algorithm discretizes the global path into a sequence of pose points, constructs a trajectory model by combining it with time series, and adjusts the local trajectory in real time through multi-objective weighted optimization. The improved global planning provides a high-quality initial path for local trajectory optimization, significantly improving the latter's convergence efficiency and optimization starting point; local trajectory optimization ensures the smoothness, safety, and kinematic feasibility of the final trajectory. The two work together to reliably transform from coarse-grained paths to fine-grained feasible trajectories.

[0020] 4. The fusion framework proposed in this invention achieves synergistic improvement in planning efficiency, trajectory quality, motion feasibility, and system robustness. It effectively solves the problem of balancing path smoothness, motion constraint satisfaction, and real-time performance in traditional methods, and provides a reliable technical solution for the safe, smooth, and efficient movement of wheeled robots in complex scenarios. Attached Figure Description

[0021] Figure 1 This is an overall framework diagram of the present invention; Figure 2 This is a schematic diagram of the restricted sampling area in an embodiment of the present invention; Figure 3 This is a schematic diagram of the elliptical sampling region in an embodiment of the present invention; Figure 4 This is a graph of the Sigmoid function in an embodiment of the present invention; Figure 5 This is a schematic diagram of the Ackermann steering model in an embodiment of the present invention; Figure 6 This is a schematic diagram of nonholonomic kinematic constraints in an embodiment of the present invention; Figure 7 This is a schematic diagram of the robot's motion trajectory in an embodiment of the present invention; Figure 8 This is a schematic diagram of the path planning hypergraph structure in an embodiment of the present invention; Figure 9 This is a comparison chart of the path optimization effects in the embodiments of the present invention. Detailed Implementation

[0022] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0023] It should be noted that, 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 application pertains.

[0024] like Figure 1As shown, this invention proposes a path planning method for wheeled robots that integrates improved Informed-RRT* and TEB algorithms. Based on the Informed-RRT* algorithm, an improvement is proposed by adding a target bias strategy, a restricted sampling area strategy, a parent node reselection strategy, and a greedy strategy to generate a global path. The path is then smoothed using cubic spline interpolation. Local trajectory optimization is then performed based on the TEB algorithm, and finally, collision detection is performed to generate a smooth and safe final planned path.

[0025] The following describes in detail, with reference to the accompanying drawings, a path planning method for wheeled robots that integrates the improved Informed-RRT* and TEB algorithms according to the present invention.

[0026] (a) Step S1: Environment modeling and initialization.

[0027] Step A: Collect environmental information about the robot's working environment using the sensors built into the Ackerman chassis wheeled robot, and create an environmental map model. In this embodiment, the map is 100*100m, the wheeled robot radius is 1m, the starting point coordinates are (10, 10), and the target point coordinates are (90, 90).

[0028] In a wheeled robot with an Ackerman structure, the turning radius R, wheelbase L, and average steering angle δ of the front wheels are... f Satisfy the following formula: Mean steering angle of front wheels δ f The relationship between angular velocity ω and angular velocity ω is shown in the following equation: Where v is the linear velocity of the robot.

[0029] Based on the above relationships, the kinematic equations of the Ackerman structure wheeled robot are as follows: in, The rate of change of the robot's pose. , These represent the rate of change of the robot's x-coordinate and y-coordinate in the map coordinate system, respectively. The rate of change of heading angle, This is the robot's heading angle.

[0030] Step B: Initialize environmental map information, confirm map boundaries and confirm robot starting point x. start and target point x goal And the location of static obstacles in the environment. To ensure safety, static obstacles are expanded, with the obstacle radius being the radius R of the robot's circumscribed circle, so that the robot is treated as a point mass in subsequent planning.

[0031] (ii) Step S2: Global path planning based on the improved Informed-RRT* algorithm.

[0032] Step C: Utilize a target bias strategy and limit the sampling area range to fuse with the Informed-RRT* algorithm. Use the fusion algorithm to sample on the map for global path planning, generating a path from the starting point x. start to target point x goal An initial path.

[0033] Sampling region restriction strategy: The sampling region is restricted to the starting point x. start With target point x goal A rectangular sampling area centered on the line is divided into multiple sub-regions, and search priorities and expansion number thresholds are set. First, sampling expansion is performed in the central sub-region with the highest priority. If the initial path is not found after reaching the first expansion number threshold, the sampling range is expanded to the adjacent sub-regions. If the initial path is not found after reaching the second expansion number threshold, sampling is performed throughout the entire map.

[0034] In this embodiment, as Figure 2 As shown, the starting point x start With target point x goal Centered on the connection, the environment is... Figure 5 The map is divided into five equal regions: D1, D2, D3, D4, and D5. Random tree expansion thresholds T1 and T2 are set. Priority is given to searching the D1 region. If no initial path is found by expansion count T1, the search continues to regions D2 and D3. Once an initial path is found, the search ends. If no initial path is found by expansion count T2, the random sampling range is expanded to the entire map. Using a restricted sampling region strategy effectively reduces redundant sampling points generated during the Informed-RRT* initial path generation process. It should be noted that setting multiple sub-regions and dividing the map into five equal regions are both examples of the restricted sampling region strategy.

[0035] Target bias strategy: Define a target bias probability threshold P t A probability value P (0~1) is randomly obtained based on the uniform sampling probability, as shown in the following formula. If the probability value is greater than the threshold P... t Then the target point x goal As a random sampling point x rand Otherwise, use the random points generated within the restricted sampling region by the SampleFree() function as the random sampling points x. rand ; Step D: After finding a line of length C bestAfter determining the initial path, the optimization phase begins. For example... Figure 3 As shown, construct a [structure] with x start and x goal With the focus on the current optimal path length C best The elliptical sampling region is defined by its major axis. All solutions that might be shorter than the current path must lie within this ellipse. Subsequent random sampling is performed within this region to obtain a random sampling point x. rand .

[0036] Step E: For random sampling point x rand Find the node x that is closest to this sampling point in the random tree. nearst , will x rand With x nearst Connected, along from x nearst Point to x rand In the direction of the selection, a new node x is chosen on this connection line with an adaptive step size S. new The adaptive step size S is dynamically adjusted based on the distance between the current node and the target point.

[0037] Step F: For x new Perform collision detection. If a collision occurs with an obstacle, delete the sampling point and repeat the previous step to select a new node x. new Conversely, if the detected nodes are not selected, the node selection process is repeated to optimize and filter them. If the node fails the optimization, the previous step is repeated to select a new node x. new If it passes, it is added to the random expansion tree.

[0038] The formula for calculating node optimization screening is: Among them, (x n ,y n ) is the new node x new Coordinates, (x s ,y s (x) is the starting point start Coordinates, (x g ,y g (x) represents the target point. goal Coordinates. If D sum If the new sampling point is greater than the current optimal global path length, then discard it.

[0039] Step G: with x new Using the circle as the center and the adaptive step size S as the radius, define a neighborhood range, and within this range, search for whether there exists a value that makes the starting point x... start To x new A lower-cost alternative parent node x min If it exists, perform a rewiring operation and set x... new Replace the parent node with x minThe adaptive step size S is calculated using the sigmoid function for a smooth transition (e.g., ...). Figure 4 As shown in the figure, it has a large neighborhood in the early stage of exploration to enhance the global search capability, and shrinks the neighborhood when it gets close to the target to improve local fine optimization, as shown in the following figure.

[0040] Where, d goal It is node x nearst With target point x goal The minimum Euclidean distance between them, where step is the expansion step size of the random tree, and r is the minimum distance between them. goal The ratio to step.

[0041] Furthermore, the strategy for reselecting the parent node is as follows: traverse the existing nodes in the random tree within the neighborhood, and for each node x... nearst Calculate if x new Replace the parent node with x nearst parent node x parent Or grandfather node x ancestry Then, from the starting point x goal To x new The path cost; if a node exists that reduces the path cost, then a rerouting operation is performed, and x is changed. new Replace the parent node with this node.

[0042] Step H: Regenerate random sampling points x rand Repeat steps E to G until a new node x is reached. new Enter target point x goal The process ends when the neighborhood is set. Step I: Continuously trace new node x new The parent node generates a child node x in the random tree. start To x goal The path; Step J: Based on the path generated in Step I, generate another elliptical sampling region, repeat steps D~I, and continuously iterate to generate new paths until the planned global path length tends to be stable. Step K: Simplify the global path planned in Step J using a greedy strategy, remove redundant nodes, and use a traversal method starting from the starting point x. start Start, and check its distance from the target point x. goal Check if there are obstacles in the line connecting the two points; if there are no obstacles, the line connecting these two points is the simplified global path; if there are obstacles, then the path starts from the target point x. goal Begin by traversing the nodes of the global path in reverse order, sequentially checking the starting point x. start Collision detection is performed on the connection between the node and the current node being traversed, and a collision is found with the starting point x. startCollision-free nodes (denoted as node P); delete x from the original path start All intermediate nodes between node P and x are directly connected. start With node P.

[0043] Step L: Connect the node P found in step K with the target point x. goal Perform collision detection; if no collision is found, then connect node P to x. goal Connect the lines to generate the final simplified global path; if a collision occurs, continue from x. goal Start traversing the path nodes in reverse order, repeat the collision detection operation in step K, find a collision-free node (denoted as node Q), delete all intermediate nodes between node P and collision-free node Q in the original path, and connect the found node P to node Q. Step M: Repeat steps K~L until the simplification of the entire planned global path is completed, resulting in a simplified global path; Furthermore, in order to obtain a smoother local trajectory with fewer path points, this scheme introduces a cubic spline interpolation method.

[0044] Given that the simplified global path to be processed contains n+1 path points, it is now divided into n consecutive path intervals. Each path interval is assigned a cubic function, defined as follows: Among them, a i b i c i d i Let be the coefficients of the cubic function s(x) corresponding to the i-th path segment, i=0,1,…n-1.

[0045] To ensure that the interpolated trajectory meets the smoothness requirements of "continuity, continuity of the first derivative, and continuity of the second derivative", the following constraints must be satisfied: (1) Interpolation conditions: (2) Continuity condition: (3) Continuity condition of the first derivative: (4) Continuity condition of the second derivative: Combining the above four conditions, we obtain 4n-2 equations. However, solving for the coefficients of n segments of cubic functions requires 4n equations. Therefore, two additional boundary conditions are needed to form a complete system of coefficient equations. Commonly used boundary conditions include: (1) Natural boundary: The second derivative of the specified path start and end points is 0. (2) Fixed Boundaries: The first derivative of the specified path starting point is set to a preset value A, and the first derivative of the specified path ending point is set to a preset value B, i.e. (3) Non-knotted boundary: Ensures the third derivative values ​​are the same for the first two interpolation points in both ascending and descending order. That is... , After considering the boundary conditions, the final set of coefficient equations is as follows: in, ,make ,in for exist The second derivative at a given point, by a specific coefficient The coefficients of the cubic function for each segment can be obtained, as shown in the following formula: The coefficients are obtained by solving the above formula. , , , This allows us to determine the cubic function corresponding to each path segment, and by splicing the cubic functions of each path segment, we can obtain a complete cubic spline interpolation curve (i.e., a smooth global path). This interpolation curve satisfies the constraints of "interpolation conditions, continuity, and continuity of the first / second derivatives", thus achieving smooth connection of the paths.

[0046] (III) Step S3, Local Trajectory Optimization and Security Verification.

[0047] Step N: Discretize the smooth global path obtained in step M into a sequence of pose points, and import the discretized pose points into the pose coordinate array of the mobile robot, as shown in the following formula: Mobile robot pose S i The sequence is defined by Q; Among them, S i x represents the pose of the i-th control point of the mobile robot; i y i θ i These represent the horizontal coordinate, vertical coordinate, and heading angle of the mobile robot on the map, respectively, and n represents the total number of control points.

[0048] Step O: Divide the planned path into n-1 consecutive path segments, and let the time interval between two adjacent path control points be ΔT, such as... Figure 7 As shown, the time intervals of all path segments form the time step sequence set τ, which is represented as: Step P: Merge the time step sequence τ with the pose sequence Q, as shown in the following formula, and define TEB as a tuple containing pose and time information; Step Q: Set the pose information and time interval information of the mobile robot as variables to be optimized, and perform multi-objective weighted optimization. The objective function of the multi-objective weighted optimization is as follows: Where, γ k Let B represent the weight coefficients, and let f represent the TEB model. k (B) Includes the following sub-functions: obstacle avoidance function f obs (B) Path following function f path (B) Velocity constraint function f vel (B) Acceleration constraint function f acc (B) Time-optimal function f time (B) and the nonholonomic kinematic constraint function f kin (B); B* represents the optimal solution in the TEB algorithm sequence that satisfies the constraints after optimization.

[0049] Furthermore, the sub-objective functions in step Q are as follows: Obstacle avoidance and path following functions. By setting a continuously differentiable penalty function, penalties are imposed for violations of constraints. The penalty function is defined as: Among them, f path (B) is the path following function, f obs (B) is the obstacle avoidance function, x r Boundary condition values, ε is the small displacement near the boundary value, S is the deformation factor, n is the power coefficient, usually n=2, dmin is the distance between each pose point and the obstacle, r omin It is the minimum distance between a local planning point and the global path pose point; r pmax It is the maximum distance that a local planning point deviates from the global path pose point.

[0050] Velocity and acceleration constraint functions. The objective function for velocity limitation, f. vel The formula is used to limit the maximum angular velocity and linear velocity of the mobile robot, as shown below: Among them, V i Let ω represent the lateral and longitudinal velocities of the pose point (unit: m / s); i ω represents the angular velocity of the pose point (unit: rad / s).

[0051] Acceleration constraint function f acc The following formula is used to limit the robot's average linear acceleration and average angular acceleration: Among them, a i Linear acceleration at the pose point (unit: m / s²) 2 ); Angular acceleration at the pose point (unit: rad / s²) 2 ).

[0052] Time-optimal constraint. With the goal of minimizing the total trajectory time, the corresponding time-optimal constraint function ftine(B) is shown in the following equation: Nonholonomic kinematic constraints. For example... Figure 6 As shown, the wheeled robot with the Ackerman steering structure is limited by its own minimum turning radius, and its motion is a nonholonomic constraint motion (that is, adjacent poses must be on arcs of the same curvature). Figure 5 This visually demonstrates the key parameter relationships of an Ackerman structure robot. The nonholonomic kinematic constraint function f is also shown. kin (B) is shown in the following formula: in, is the displacement vector of adjacent pose points. The cost function for nonholonomic kinematic constraints is shown in the following equation: For a mobile robot with an Ackerman structure, its turning radius R must not be less than its minimum turning radius r. min That is, R≥r min The constraints are shown in the following equation. like Figure 8As shown, this step integrates constraints such as velocity, acceleration, and obstacle avoidance into a global objective function by constructing an interconnected network of "pose-time-constraint function". By minimizing the global objective function f(B), the optimal TEB sequence that satisfies all constraints is obtained, ensuring that the local trajectory simultaneously satisfies multi-dimensional constraints such as velocity, acceleration, and obstacle avoidance.

[0053] Step R: Perform collision detection on the local optimal trajectory and map boundary obtained in step Q. Select multiple detection points evenly between adjacent smooth points of the trajectory and perform point-level collision detection on each detection point. If any detection point collides with an obstacle or map boundary, the line segment between the adjacent smooth points is determined to be an unsafe line segment.

[0054] Step S: Perform collision handling on unsafe line segments: Using the collision point as the center, search for safe alternative points using a multi-radius and multi-angle approach, prioritizing the original safe path point closest to the collision point; if no suitable alternative point is found, vertically offset the unsafe line segment to avoid obstacles, insert a new path point, and re-perform smoothing; vertically offset to the extension direction relative to the unsafe line segment, and move the path segment along a direction perpendicular to the line segment (the side away from the obstacle) to maintain a safe distance between the path and the obstacle.

[0055] Repeat the above operations to smooth and iteratively optimize the updated path point sequence until all line segments pass the collision detection or the preset maximum number of iterations is reached, and output a safe and smooth final planned path.

[0056] (iv) Experimental verification To verify the effectiveness and feasibility of the wheeled robot path planning method integrating the improved Informed-RRT* and TEB algorithms of this invention, two typical path planning algorithms were selected as comparison benchmarks: (1) RRT* algorithm based on asymptotic optimization: using uniform random sampling and node reconnection mechanism. (2) Informed-RRT* algorithm based on heuristic sampling: using elliptic constraint sampling strategy.

[0057] Experimental environment and settings: Simulation was performed using Python 3.10.12 in a virtual environment of Ubuntu 22.04 (Intel Core i7-12700KF processor, 3.6GHz, 16 cores, 32GB memory).

[0058] All comparison algorithms were tested under the same complex obstacle environment, the same start / end point, and the same robot parameters. The maximum number of iterations was set to 2000 to ensure fairness in the comparison. Each algorithm underwent 10 independent experiments, and the average value was used for performance analysis.

[0059] Table 1. Comparison of Technical Features of Four Path Planning Algorithms In Table 1, the improved Informed-RRT* is an optimization of the basic Informed-RRT* algorithm, improving only the generation of the global path by employing measures such as limiting the sampling area, target bias, dynamic step size, reselecting parent nodes, and a greedy strategy. The improved Informed-RRT*+TEB is the complete technical solution of this invention (integrating global path planning and local path optimization). Based on the global path generated by the improved Informed-RRT*, it adds local trajectory optimization using the TEB algorithm and incorporates cubic spline interpolation for smoothing.

[0060] Combining the data in Table 1 and Figure 9 As can be seen, compared with the other two algorithms, this fusion algorithm has the following significant advantages: (1) Redundant turning points are significantly reduced, and path continuity is significantly improved; (2) The path smoothness is significantly improved, effectively reducing frequent turning and shaking during robot movement; (3) Enhanced trajectory feasibility enables the robot to reach the target point x more smoothly and efficiently. goal .

[0061] Improved planning performance of the Informed-RRT* algorithm: Compared to the RRT* algorithm, with essentially the same path planning length, the planning efficiency is improved by 59.85%, and the number of nodes is reduced by 11.65%. Compared to the Informed-RRT* algorithm, the planning efficiency is improved by 57.69%, the path length is reduced by 7.46%, and the number of nodes is reduced by 3.98%, showing significant improvement.

[0062] This invention effectively solves the core contradiction in the field of path planning, which has long been the difficulty in achieving both "optimality" and "real-time performance". It provides a high-performance, stable and reliable innovative technical solution for real-time motion planning in fields such as mobile robots and autonomous driving, and has outstanding theoretical innovation value and practical application prospects.

[0063] Finally, it should be noted that any parts of this invention not described in detail are prior art. Those skilled in the art will understand that the above descriptions are merely preferred embodiments of the invention and are not intended to limit the invention. Although the invention has been described in detail with reference to the foregoing examples, those skilled in the art can still modify the technical solutions described in the foregoing examples or make equivalent substitutions for some of the technical features. All modifications and equivalent substitutions made within the spirit and principles of the invention should be included within the scope of protection of the invention.

Claims

1. A path planning method for wheeled robots that integrates improved Informed-RRT* and TEB algorithms, characterized in that, Includes the following steps: S1. Environmental Modeling and Initialization: Environmental information is collected using sensors mounted on the wheeled robot to construct an environmental map model and determine the map boundaries and the robot's starting point x. start Target point x goal and the location of obstacles; S2. Global Path Planning: An improved Informed-RRT* algorithm is used to plan a path from the starting point x in the environment map. start to target point x goal The global path; The improved Informed-RRT* algorithm includes: (1) A sampling method that combines a restricted sampling region strategy with a target bias strategy is used to generate samples from the starting point x. start to target point x goal An initial path; (2) After obtaining the initial path, starting from point x start and target point x goal Random sampling is performed within an elliptical region with the current optimal path length as the major axis, focusing on the current optimal path length, to obtain the sampling point x. rand Find the distance x in a random tree rand The nearest node x nearst A new node x is selected along the direction of the line connecting the two nodes with an adaptive step size S. new ; (3) For each new node x generated by sampling new Collision detection and node optimization screening are performed; the node optimization screening involves calculating a new node x. new to the starting point x start The Euclidean distance to the target point x goal If the sum of the Euclidean distances is greater than the currently known optimal global path length, then the new node x is discarded. new Otherwise, it is added to the random expansion tree; (4) Execute a parent node reselection strategy during the random tree generation process, with x new Define a neighborhood range with the center of the circle and an adaptive step size S as the radius, and find the starting point x. start To x new Replace the parent node with the one with the lower path cost and perform a rewiring operation; repeat steps (2) to (3) until a new node x is reached. new Enter target point x goal The neighborhood is defined, an initial global path is generated, the elliptical sampling region is updated and iteratively optimized, and then a greedy strategy is used to remove redundant nodes in the path to obtain a simplified global path. S3. Local Trajectory Optimization: The global path obtained in step S2 is discretized into a sequence of pose points using the TEB algorithm. This sequence is then combined with a time interval sequence to construct a TEB trajectory model. By solving a multi-objective weighted optimization problem that includes obstacle avoidance constraints, path following constraints, velocity and acceleration constraints, time-optimal constraints, and nonholonomic kinematic constraints based on the Ackerman steering model, a smooth and kinematically feasible local optimal trajectory is generated.

2. The wheeled robot path planning method integrating the improved Informed-RRT* and TEB algorithms according to claim 1, characterized in that, In step S2, the restricted sampling area is defined as starting from point x. start With target point x goal The rectangular sampling region centered on the connecting line, and the specific strategies for limiting the sampling region include: The rectangular sampling area is divided into multiple sub-regions, and search priorities and expansion number thresholds are set. First, sampling expansion is performed in the central sub-region with the highest priority. If the initial path is not found after reaching the first expansion number threshold, the sampling range is expanded to the adjacent sub-regions. If the initial path is not found after reaching the second expansion number threshold, sampling is performed in the entire map area. The target bias strategy specifically includes: Set a target bias probability threshold P t During each sampling, a probability value is randomly generated based on the uniform sampling probability. If this probability value is greater than the target bias probability threshold P, then... t Then the target point x goal Directly used as sampling point x rand Otherwise, use the SampleFree() function to randomly generate a sampling point within the restricted sampling area.

3. The wheeled robot path planning method integrating the improved Informed-RRT* and TEB algorithms according to claim 1, characterized in that, In step S2, the specific strategy for limiting the sampling region is as follows: The starting point x... start With target point x goal Centered on the connecting lines, the environmental map is divided into five equal regions: D1, D2, D3, D4, and D5. Set thresholds T1 and T2 for the number of random tree expansions. Prioritize searching in region D1. If the initial path is not found after the number of expansions reaches T1, expand to regions D2 and D3. If the initial path is still not found after the number of expansions reaches T2, expand the random sampling range to the entire map.

4. The wheeled robot path planning method integrating the improved Informed-RRT* and TEB algorithms according to claim 1, characterized in that, In step S2, the adaptive step size S is dynamically adjusted based on the distance between the current node and the target point, and the calculation formula is as follows: Where, d goal It is node x nearst With target point x goal The minimum Euclidean distance between them, where step is the expansion step size of the random tree, and r is the minimum distance between them. goal The ratio to step.

5. A path planning method for a wheeled robot integrating the improved Informed-RRT* and TEB algorithms as described in claim 1, characterized in that, In step S2, the calculation formula for the node optimization screening is: Among them: (x n ,y n ) is the new node x new Coordinates, (x s ,y s (x) is the starting point start Coordinates, (x g ,y g (x) represents the target point. goal coordinate; If D sum If the new sampling point is greater than the current optimal global path length, then discard it.

6. The wheeled robot path planning method integrating the improved Informed-RRT* and TEB algorithms according to claim 1, characterized in that, In step S2, the specific strategy for reselecting the parent node is as follows: With new node x new A circular neighborhood is defined with the center as the center and the adaptive step size S as the radius. Iterate through the existing nodes in the random tree within the neighborhood, for each node x nearst Calculate if the new node x new Replace the parent node with x nearst parent node x parent Or grandfather node x ancestry Then, from the starting point x start To x new Path cost; If a node exists that reduces the path cost, then perform a rewiring operation and move the new node x... new Replace the parent node with this node.

7. A path planning method for a wheeled robot integrating the improved Informed-RRT* and TEB algorithms as described in claim 1, characterized in that, In step S2, the greedy strategy is specifically as follows: From the starting point x start Begin by trying different points along the target point x. goal If a direct connection is made and the line connecting the two points does not collide with any obstacle, then this connection is used as a simplified global path. If a collision occurs, then from the target point x goal Start by traversing the path nodes backwards along the generated global path, searching for a node that does not collide with the current connection starting point; once the node is found, connect the two points directly, and delete all intermediate nodes between the two points in the original path. Use the newly successfully connected node as the starting point for the next iteration, and repeat the reverse traversal and connection process described above until the current connection starting point and the target point x are connected. goal A direct connection simplifies the entire global path, resulting in a simplified global path.

8. A path planning method for a wheeled robot integrating the improved Informed-RRT* and TEB algorithms according to claim 1, characterized in that, In step S3, the objective function of the multi-objective weighted optimization constructed by the TEB algorithm is: Where, γ k B represents the weighting coefficients, and B represents the TEB model. Includes the following sub-functions: obstacle avoidance function f obs (B) Path following function f path (B) Velocity constraint function f vel (B) Acceleration constraint function f acc (B) Time-optimal function f time (B) and the nonholonomic kinematic constraint function f kin (B); The nonholonomic kinematic constraint function f kin (B) Based on the Ackerman steering model, it is used to constrain adjacent pose points to be located on arcs with the same curvature, and to ensure that the turning radius corresponding to the curvature is not less than the minimum turning radius of the wheeled robot.

9. A path planning method for a wheeled robot integrating the improved Informed-RRT* and TEB algorithms according to claim 1, characterized in that, In step S2, after removing redundant nodes from the path using a greedy strategy, cubic spline interpolation is applied to the simplified global path to improve trajectory smoothness. Specifically, this includes: The simplified global path is divided into n+1 path points into n adjacent paths, and each path is assigned a cubic function. A system of coefficient equations is constructed by interpolation conditions, first derivative continuity conditions and second derivative continuity conditions of adjacent intervals. Boundary conditions are added to make the system of equations have a unique solution, and the coefficients of the cubic function in each segment are obtained by solving the system. A smooth global path is obtained by connecting the cubic functions of each segment, and this smooth global path serves as the basis for the subsequent discretization of the pose point sequence.

10. A path planning method for a wheeled robot integrating the improved Informed-RRT* and TEB algorithms according to claim 1, characterized in that, After generating the local optimal trajectory in step S3, the process also includes collision detection and processing steps: Collision detection is performed on the local optimal trajectory and map boundary. Multiple detection points are uniformly sampled between adjacent smooth points of the trajectory, and point-level collision detection is performed on each detection point. If a certain trajectory is determined to be unsafe, then a safe alternative point is searched for with multiple radii and multiple angles centered on the collision point. The original safe path point closest to the collision point is selected first, or a vertical offset is performed to avoid the obstacle and insert a new path point. The updated path point sequence is then smoothed and iteratively optimized again until the entire trajectory passes the safety check or reaches the maximum number of iterations, outputting a safe and smooth final planned path.

Images