Robot smooth path planning method and system based on improved TEB algorithm, and storage medium

Abstract

The invention discloses a robot smooth path planning method and system based on an improved TEB algorithm and a storage medium, and the method comprises the following steps: S1, creating a robot motion environment, generating a cost map, and planning a global initial path; s2, performing adaptive dynamic sampling on path points on the global path according to the bending degree of the global path; s3, screening global path points; s4, initializing a track to be optimized; s5, updating the track to be optimized and the cost map; s6, constructing a total objective function, and creating a hypergraph; s7, solving the hypergraph to obtain an optimal track; s8, confirming whether the optimal track is feasible or not, and if not, returning to the step S4; s9, controlling the robot to move; and after the robot moves, re-screening the global path points and returning to the step S4 until the robot reaches the target point. According to the invention, frequent pose regulation and control and frequent speed fluctuation of the robot can be reduced.

Description

Technical Field

[0001] This invention relates to robot path planning, and in particular to a method, system, and storage medium for smooth robot path planning based on an improved TEB algorithm. Background Technology

[0002] With the rapid development of artificial intelligence and automation technologies, mobile robots have been widely used in many fields such as industrial production, logistics and transportation, service industries, and exploration of unknown environments. Path planning, as one of the core technologies for mobile robots to achieve autonomous navigation, aims to calculate a collision-free path from the starting point to the destination based on the robot's initial position, target position, and environmental information, thereby ensuring that the robot can complete its tasks safely and efficiently.

[0003] Based on the degree of understanding of the environment, path planning can be divided into global path planning and local path planning. Global path planning is a static planning method that cannot update environmental information in real time and is not suitable for changing environments. Local path planning, on the other hand, can dynamically adjust the path as the environment changes to cope with unknown situations and achieve real-time obstacle avoidance. Therefore, global path planning is usually used in conjunction with local path planning.

[0004] The Temporal Elastic Band (TEB) algorithm is a widely used local path planning algorithm. Its core idea is to abstract the global path as a deformable "elastic band" containing pose and time interval information. By adjusting the pose and time interval, obstacle avoidance is achieved while satisfying kinematic constraints. The TEB algorithm comprehensively considers various constraints, such as obstacle distance constraints, global path following constraints, time-optimal constraints, and nonholonomic kinematic constraints, transforming the path planning problem into a multi-objective optimization problem. Classical local planning algorithms such as the Dynamic Window (DWA) method and the Artificial Potential Field (APF) method do not consider complex kinematics and are therefore only suitable for differential robots. The TEB algorithm, however, is applicable not only to differential robots but also to robots with nonholonomic kinematics, such as Ackerman robots, thus possessing greater applicability.

[0005] However, robots often experience speed fluctuations when using the traditional TEB algorithm, causing stress on the robot and reducing the smoothness of its trajectory and speed. For example, when navigating sharp bends or other high-curvature road sections, due to kinematic limitations and the influence of surrounding obstacles, the robot frequently experiences sudden stops, reversals, and detours, resulting in significant jumps in linear and angular velocities. Furthermore, because the pose points of the TEB algorithm are obtained by sampling global pathpoints, and the number of samples is determined by static parameters, it lacks dynamic adjustment capabilities. Even on relatively straight road sections, the large number of pose points leads to numerous inflection points in the trajectory, causing the robot to frequently adjust its pose, resulting in frequent fluctuations in angular velocity and a "swaying" phenomenon. Summary of the Invention

[0006] Purpose of the invention: The purpose of this invention is to provide a robot smooth path planning method, system, and storage medium based on an improved TEB algorithm that can reduce the frequent adjustments to robot pose and speed fluctuations.

[0007] Technical Solution: To achieve the above objectives, the robot smooth path planning method based on the improved TEB algorithm described in this invention includes the following steps:

[0008] S1. Create the robot's motion environment and generate a cost map; obtain the robot's localization information and call the global planner to plan the global initial path;

[0009] S2. Based on the curvature of the global path, adaptively and dynamically sample the path points on the global path, reducing the number of sampling points at low curvature paths.

[0010] S3. Select a subset of global path points from the sampled global path points;

[0011] S4. Initialize the selected global path points to obtain pose points, and add time intervals between adjacent pose points to complete the initialization of the trajectory to be optimized.

[0012] S5. Insert or delete pose points in the trajectory to be optimized according to the time interval; update the cost map, obstacle information and waypoints according to the updated trajectory to be optimized;

[0013] S6. Construct multiple constraints and sub-objective functions, and construct the overall objective function accordingly. Create a hypergraph based on the cost map, the current trajectory to be optimized, and the overall objective function. Constraints include curvature radius constraints that use the global path curvature radius to constrain the linear velocity.

[0014] S7. Solve the hypergraph. If the solution fails, reduce the number of global path points selected and return to step S4; otherwise, proceed to step S8.

[0015] S8. Traverse the optimal trajectory and interpolate adjacent pose points where the angle difference or distance difference exceeds the preset minimum resolution to obtain the final trajectory. Then determine whether the distance between the robot and the obstacle is less than the threshold when the robot travels along the final trajectory. If it is, reduce the number of global path points selected and return to step S4. Otherwise, proceed to step S9.

[0016] S9. Calculate the robot's motion control quantity based on the final trajectory, and control the robot's movement accordingly; after the robot moves, re-select global path points, and then return to step S4 until the robot reaches the target point.

[0017] Based on the above technical solution, step S2 performs adaptive dynamic sampling according to the curvature of the global path, reducing sampling points at low curvature paths and retaining relatively more sampling points at curved (high curvature) paths to ensure tracking accuracy. This significantly reduces the number of unnecessary inflection points in the trajectory, thereby effectively reducing the robot's body swaying and frequent fluctuations in angular velocity caused by frequent small adjustments to the direction angle, and improving motion stability. Step S6 further establishes a velocity curvature radius constraint, enabling linear velocity adjustment based on the global path curvature radius. This constraint links the dynamic adjustment of the robot's linear velocity to the curvature radius of the current global path point. This forces the robot to smoothly reduce its speed in advance when entering a high-curvature section, effectively avoiding abnormal behaviors such as sudden stops, reversals, or detours caused by the traditional TEB algorithm's failure to consider path curvature, as well as the accompanying drastic jumps in linear and angular velocities. This significantly improves the smoothness of the trajectory and velocity, and reduces the mechanical shocks experienced by the robot. Finally, steps S8 (interpolation and obstacle overlap check) and S9 (reinitialization after movement) ensure the real-time feasibility and continuous optimization capability of the trajectory, enabling the robot to move stably and reliably toward the target point in a dynamic environment.

[0018] Preferably, the filtering process in step S3 is as follows: starting from the global path point closest to the robot, traverse the global path points in the directions not reached by the robot, and retain global path points whose distance from the robot does not exceed a threshold and whose cumulative distance from the nearest global path point does not exceed a preset cumulative length.

[0019] By limiting the number of path points participating in local planning through filtering, focusing only on path segments near the robot with controllable length, the scale of subsequent optimization problems is reduced, and the computational efficiency and real-time response capability of the algorithm are improved.

[0020] The robot smooth path planning system based on the improved TEB algorithm of the present invention includes:

[0021] Global path planning module: used to create the robot's motion environment and generate a cost map; obtain robot localization information and call the global planner to plan the global initial path;

[0022] Adaptive sampling module: Used to adaptively and dynamically sample path points on the global path according to the curvature of the global path, reducing the number of sampling points in low curvature paths;

[0023] Filtering module: Used to filter a subset of global path points from the sampled global path points;

[0024] Initialization module: used to initialize the selected global path points to obtain pose points, and add time intervals between adjacent pose points to complete the initialization of the trajectory to be optimized;

[0025] Update module: Used to insert or delete pose points in the trajectory to be optimized according to time intervals; update the cost map, obstacle information and waypoints according to the updated trajectory to be optimized;

[0026] Hypergraph Creation Module: Used to construct multiple constraints and sub-objective functions, and based on these, construct the overall objective function. It creates a hypergraph based on the cost map, the current trajectory to be optimized, and the overall objective function. Constraints include curvature radius constraints that use the global path curvature radius to constrain linear velocity. Optimal Trajectory Solving Module: Used to solve the hypergraph. If the solution fails, it reduces the number of global path points selected and returns to the initialization module; otherwise, it enters the trajectory feasibility determination module.

[0027] Trajectory Feasibility Determination Module: This module is used to traverse the optimal trajectory and interpolate adjacent pose points where the angle difference or distance difference exceeds the preset minimum resolution to obtain the final trajectory. Then, it determines whether the distance between the robot and the obstacle is less than the threshold when the robot travels along the final trajectory. If it is, the number of global path points selected is reduced and the module is returned to the initialization module. Otherwise, the module is entered.

[0028] Motion module: Used to calculate the robot's motion control quantities based on the final trajectory, and control the robot's movement accordingly; after the robot moves, it re-selects global path points and initializes them, and then returns to the initialization module until the robot reaches the target point.

[0029] The computer-readable storage medium for storing one or more programs according to the present invention includes one or more programs comprising instructions that, when executed by a computing device, cause the computing device to perform any of the methods described above.

[0030] Beneficial Effects: This invention has the following advantages: It adaptively adjusts the number of TEB pose points based on the curvature of the global path, ensuring global path following while reducing the number of angular velocity fluctuations and improving robot body swaying. Simultaneously, it introduces constraints on linear velocity and the radius of curvature of the global path, enabling dynamic speed adjustment based on the path's curvature radius. This avoids the robot reversing, stopping suddenly, or detouring when traversing high-curvature road sections, improving the smoothness of the path and speed, and reducing the impact on the robot. By detecting whether the robot overlaps with obstacles and re-selecting and initializing path points after movement, it ensures the real-time feasibility and continuous optimization capability of the trajectory, enabling the robot to move stably and reliably towards the target point in a dynamic environment. Attached Figure Description

[0031] Figure 1 This is a flowchart illustrating the improved TEB algorithm described in this invention.

[0032] Figure 2 This is a sequence diagram of the TEB trajectory;

[0033] Figure 3 A schematic diagram of the kinematic model of a differential robot;

[0034] Figure 4 A schematic diagram of the kinematics model of the Ackerman robot;

[0035] Figure 5 This is the hypergraph of the improved TEB algorithm described in this invention. Detailed Implementation

[0036] The technical solution of the present invention will be described in detail below with reference to the embodiments and accompanying drawings.

[0037] like Figure 1 As shown, the robot smooth path planning method based on the improved TEB algorithm of the present invention includes the following steps:

[0038] S1. Create the robot's motion environment and generate a map:

[0039] The motion environment required for the Ackerman robot is constructed, and a global static map is built using the SLAM algorithm to obtain known static obstacle information, and the environmental grid map is converted into a cost map.

[0040] Obtain location information, invoke the global planner, and plan the global initial path {z j}, as input to the TEB algorithm

[0041] Optionally, GPS, odometry, LiDAR, IMU, or UWB can be used to determine the robot's real-time position on the map. Based on ROS (Robot Operating System), the move_base framework is used for robot navigation. First, the global planner is invoked to set the starting and target points. Algorithms such as Dijkstra's algorithm, A* algorithm, or RRT are used to plan the global path and obtain the initial global path {z}. j}

[0042] S2. Based on the curvature of the global path, adaptively and dynamically sample the path points on the global path, including the following steps:

[0043] S21. Calculate the angle between the displacement vectors formed by three consecutive path points (i.e., global path points) on the global path, and use the magnitude of this angle to measure the curvature of the global path corresponding to that point.

[0044] Let the current path point (the i-th path point) be . The path points behind and in front are respectively Among them, X i Y i and These represent the x-coordinate, y-coordinate, and robot orientation angle of the i-th path point in the global coordinate system; in this invention, "forward" refers to the direction the robot has not yet reached, and "backward" refers to the direction it has already traveled.

[0045] The formula for calculating the angle between the displacement vectors corresponding to the current path point is:

[0046]

[0047] in and These are two adjacent displacement vectors. and The angle between the x-axis and the positive x-axis of the global coordinate system, where and P respectively i-1 and P i+1 With P i The displacement vector formed;

[0048] and The calculation formula is:

[0049]

[0050] S22. Based on the curvature of the global path, perform adaptive dynamic sampling of global path points.

[0051] First, retain the start and end points for path point P. i-1 P iand P i+1 Calculate P respectively i-1 and P i With P i+1 The distance, denoted as d i-1,i and d i-1,i+1 Set a maximum path distance D. p_max ,

[0052] If d is satisfied i-1,i ≤D p_max And d i-1,i+1 >D p_max Then P will be forcibly retained. i Otherwise, through and The angle between two adjacent displacement vectors Judgment, will With angle threshold Compare, if Then retain P i Otherwise, do not retain P. i Continue to determine subsequent path points until all path points have been filtered.

[0053] This step accurately identifies key areas of path shape change, ensuring that more sampling points are retained in curved areas where high precision is truly required, and that redundant points are intelligently reduced in straight areas. This effectively reduces path inflection points and thus suppresses vehicle sway.

[0054] S3. Select a subset of global pathpoints from the sampled global pathpoints. The specific process is as follows: Find the global pathpoint closest to the robot's current position. Starting from this point (the closest global pathpoint), traverse forward (in directions the robot has not yet reached). If a point satisfies the condition that its distance to the robot's current position does not exceed a threshold, and the cumulative distance from the previously determined closest global pathpoint to this point does not exceed a set cumulative length, then retain this point; otherwise, do not retain it. The cumulative distance is the cumulative value of the distances between all adjacent pathpoints from the closest global pathpoint to the current global pathpoint, and the cumulative length is the set maximum value of the cumulative distance.

[0055] S4. Initialize the selected global path points to obtain pose points, and add time intervals between adjacent pose points to complete the initialization of the trajectory to be optimized: including the following steps.

[0056] S41. Global pathpoint initialization:

[0057] The selected global path points are transformed into the coordinate system of the local planner and used as the initial pose points of the TEB. Let the i-th pose point be denoted as .

[0058] S i =(x i ,yi ,θ i ) T

[0059] Where, x i y i and θ i Let x, y, and y represent the x-coordinate, y-coordinate, and orientation angle of the i-th pose point (i.e., the i-th global path point) in the coordinate system of the local planner, respectively; the initial pose point sequence of the TEB can be represented as...

[0060] Q = {S} i} i＝1,2,…N

[0061] S i Let be the pose point corresponding to the i-th global path point, and N be the total number of global path points selected.

[0062] S42. Initialization of the trajectory to be optimized:

[0063] First, determine if the number of path points meets the requirement. If it is less than the set value, interpolation is required. Interpolation involves linearly inserting more intermediate pose points between the second-to-last pose point and the last pose point of the current trajectory until the minimum number of samples (minimum number of pose points) requirement is met.

[0064] Then add a time interval ΔT between adjacent pose points. i The time interval is calculated based on the distance between two adjacent pose points and the two parameters of maximum linear velocity and maximum angular velocity. The calculation method is existing technology and will not be elaborated here.

[0065] The subsequent optimization process will solve for each ΔT. i The optimal value; if there are n+1 pose points, there are n time intervals. The ordered sequence of all time intervals added to N pose points can be represented as

[0066] τ={ΔT i} i＝1,2,…N-1

[0067] like Figure 2 As shown, the pose point sequence and time interval sequence are merged to obtain the preliminary pose point and time interval sequence, which is the initial trajectory to be optimized in TEB. The expression for the trajectory to be optimized is:

[0068] B={S1,ΔT1,S2,ΔT2,…ΔT N-1 ,S N}

[0069] Where B is the trajectory to be optimized, ΔT N-1 The time interval between the Nth and N-1th pose points.

[0070] S5. Insert or delete pose points in the trajectory to be optimized according to the time interval; update the cost map, obstacle information, and waypoints based on the updated trajectory to be optimized; this includes the following sub-steps:

[0071] S51. Adjusting pose points, i.e., inserting / deleting pose points:

[0072] This operation can improve the smoothness and computational efficiency of path planning; the specific process of inserting or deleting pose points is as follows:

[0073] (1) If the time interval between two adjacent left and right pose points is greater than the set maximum duration and the number of path points is within the specified range, then an interpolation operation is performed: calculate the position where the new time interval is half of the current interval, insert a new pose point at this position, the position of this point is the average of the positions of two adjacent path points, that is, in the middle position of two adjacent path points, the newly inserted pose point satisfies that it is in the middle of the two original pose points, and the new time interval is half of the original, because the two adjacent points move at a constant speed, the speed value is calculated by the ratio of the distance between the two pose points to the time interval, so the movement from this pose point to the next pose point is at a constant speed, so the above limitation can be satisfied; (2) If the time interval between two adjacent left and right pose points is less than the set minimum duration, then delete the pose point on the right (that is, the pose point that is farther away from the robot).

[0074] S52. Update the cost map, obstacle information, and waypoints based on the updated trajectory to be optimized.

[0075] Cost Map: In ROS (Robot Operating System), robot navigation typically uses a cost map to aid in path planning and obstacle avoidance. A cost map is a two-dimensional grid that reflects the accessibility of an area by assigning a "cost" to each grid cell. Higher costs indicate more difficult passage or areas with obstacles.

[0076] Cost map update method: The robot uses onboard sensors (such as LiDAR, radar, depth cameras, etc.) to perceive its surroundings. Sensor data typically contains information such as the location, shape, and size of obstacles. As the robot moves forward, it detects nearby obstacles based on the sensor data and calculates the cost of each obstacle.

[0077] Cost map publishing: Whenever the cost map changes, the system publishes the updated cost map via ROS messages, so that other system components (such as the path planner) can obtain the latest environmental information.

[0078] Obstacle information update: Iterate through each cell of the cost map. For each cell, determine whether it is an obstacle based on its cost value. If it is an obstacle, calculate the distance between the obstacle and the robot. If the distance is less than a critical distance, add the obstacle to the obstacle list, indicating that the obstacle will affect the trajectory to be optimized.

[0079] Path point update: Path points are obtained from the initial trajectory obtained from S42. Steps: First, clear the path point container and record the index pre_idex of the last added path point. From this point, traverse the trajectory to be optimized in S42. If the distance between the current point and the point pre_idex is greater than the set minimum interval, add the point to the path point container and update pre_idex to the index of the current point. Continue traversing until the end of the trajectory to be optimized obtained from S42. Note that path points only extract pose information from the trajectory to be optimized, not time information.

[0080] S6. Establish the overall objective function for optimization, and based on this, create a hypergraph by combining the cost map and the current trajectory to be optimized:

[0081] The overall objective function includes multiple constraints and sub-objective functions. The constraints include:

[0082] (1) Path point constraints

[0083] The constraint function is

[0084] f path =e Γ (d min,k D pmax ,ε,S,n)

[0085] Where, d min,k Let the pose point be its nearest path point P. k The closest distance, D pmax e is the maximum allowed path deviation distance. Γ Let ε be the error function, ε be the safety margin (i.e., tolerance), n be the order, and S be the scaling factor.

[0086] Error function e Γ The general expression is

[0087]

[0088] Where x is the constrained object, which in different constraint functions is the first independent variable on the left, such as d. min,k and -d min,j etc.; x r For boundary values, in different constraint functions, it is the first independent variable on the left, such as D. pmax and -Domin wait.

[0089] Taking path point constraints as an example: Assuming the maximum path deviation is 0.3 and the safety margin is 0.1, then when the pose point and path point P... k An error occurs when the nearest distance is greater than 0.2, resulting in a penalty. The greater the deviation from the range, the greater the error and the greater the penalty.

[0090] (2) Obstacle constraints

[0091] The constraint function is

[0092] f obst =e Γ (-d min,j ,-D omin ,ε,S,n)

[0093] d min,j Let the pose point be its nearest obstacle O j The distance, D omin This represents the minimum distance between the allowed path point and the obstacle.

[0094] (3) Velocity and acceleration constraints

[0095] For the robot at two adjacent pose points S i With S i+1 The speed v between i angular velocity w i acceleration a i With angular acceleration α i Restrictions are imposed to satisfy the following inequalities.

[0096]

[0097] Among them, v min and v max These represent the minimum and maximum linear velocities, w. min and w max These are the minimum and maximum angular velocities, a. min and a max These are the minimum and maximum linear accelerations, v. min and v max These are the minimum and maximum angular accelerations, respectively.

[0098] The robot's linear velocity and angular velocity can be calculated based on the Euclidean distance between two adjacent pose points, the time interval, and the change in heading angle. Using the finite difference approximation, they can be expressed as:

[0099]

[0100] Similarly, linear acceleration and angular acceleration can be calculated from the average linear velocity and average angular velocity of two adjacent poses, and can be expressed as follows:

[0101]

[0102] (4) Nonholonomic kinematic constraints

[0103] Two consecutive pose points S i With S i+1 The constraint function for the nonholonomic kinematic constraint should be given on a common circular arc with constant curvature.

[0104]

[0105] The formula itself contains an error, used to measure the perpendicularity between the robot's displacement vector and orientation vector. The optimization algorithm will try to reduce this error, and this formula forms a nonholonomic kinematic constraint.

[0106] θ i and θ i+1 These represent the robot's poses at two adjacent points S. i With S i+1 The direction angle and the magnitude of the displacement satisfy the following relationship:

[0107]

[0108] d i,i+1 For the robot to start from pose point S i To S i+1 displacement vector

[0109]

[0110] (5) Velocity curvature radius constraint

[0111] Based on the original constraints of the TEB algorithm, constraints are established between velocity and the radius of curvature corresponding to the global path. The constraint formula is as follows:

[0112] f vel_r =e Γ (v i ,v des_lim ,ε,S,n)

[0113] Among them, v des_lim v is the limit of the desired speed. des_lim The calculation formula is:

[0114]

[0115] Among them, v max For the set maximum speed, r minR is the minimum turning radius of the mobile robot. i Point P on the global path i The corresponding radius of curvature, β is the radius of curvature influence factor used to describe the degree of influence of the radius of curvature on the desired velocity.

[0116] Point P on the global path i The corresponding radius of curvature R i The calculation can be performed using the principle of the three-point method:

[0117] For three consecutive pose points on a non-linear global path and There exists a unique circumcircle that passes through the triangle formed by these three points. The radius of this circumcircle is the global path at point P. i-1 P i and P i+1 The radius of curvature at the point.

[0118] From the formula for the radius of the circumcircle of a triangle, we can obtain...

[0119]

[0120] in, and These are the scalars corresponding to the displacement vectors, which are the Euclidean distances between the corresponding pose points, for example... Represents pose point S i To S i+1 The Euclidean distance between the three points, where A is the area of ​​the circumcircle determined by the three points.

[0121] A is calculated using the cross product method.

[0122]

[0123] For differential robots, such as Figure 3 As shown, the differential robot moves in an arc of radius R around its instantaneous rotation center ICR, where R is the instantaneous turning radius of the robot at that moment. l With v r The velocities of the left and right drive wheels are respectively, satisfying v = (v l +v r The differential robot's steering is achieved through the differential speed of its left and right wheels. Theoretically, its minimum turning radius can be 0, meaning it can rotate in place, in which case the speeds of the left and right drive wheels are equal in magnitude and opposite in direction. However, in actual navigation, the differential robot's motion is continuous, making it difficult to achieve opposite speeds for the two wheels. The extreme case is when the speed of one drive wheel is 0. In this case, the robot moves in an arc with the point of contact between one drive wheel and the ground as the center and the wheelbase D as the radius. Therefore, the effective minimum turning radius r of the differential robot in actual operation is... min_difffor

[0124]

[0125] Where D is the wheelbase between the two wheels of the differential robot.

[0126] Therefore, for differential robots, v des_lim It can also be expressed as

[0127]

[0128] For Ackerman robots, such as Figure 4 As shown, the Ackerman robot moves in an arc of radius R around its instantaneous rotation center ICR, where R is the instantaneous turning radius of the Ackerman robot at that moment, satisfying...

[0129]

[0130] Where L is the wheelbase between the front and rear axles of the Ackerman robot, and δ is the steering angle of the front wheel.

[0131] Minimum turning radius r min_car It can be achieved through the maximum front wheel steering angle δ max calculate:

[0132]

[0133] Therefore, for the Ackerman robot, v des_lim It can also be expressed as

[0134]

[0135] The velocity curvature radius constraint in the above constraints is the core innovation. As mentioned earlier, this constraint directly and effectively solves the problems of velocity jumps and abnormal behavior of robots on high-curvature road sections, significantly improving speed and trajectory smoothness. This is the key difference between it and traditional TEB and the solution to its pain points.

[0136] Simultaneously, other key constraints ensure the multifaceted performance of the planning results: waypoint constraints and obstacle constraints guarantee basic path safety (obstacle avoidance) and task objective achievement (approaching waypoints). Velocity and acceleration constraints ensure the robot's motion state remains within physically feasible limits, preventing excessive speed or acceleration / angular acceleration. Nonholonomic kinematic constraints strictly adhere to the robot's nonholonomic kinematic characteristics (movement can only be along straight lines or circular arcs), ensuring that the generated trajectory can theoretically be precisely executed by the robot chassis, improving the practicality of the planning.

[0137] Sub-objective functions include:

[0138] (1) The optimal time sub-objective function is expressed as follows:

[0139]

[0140] (2) The shortest path length sub-objective function is expressed as follows:

[0141]

[0142] The two sub-objective functions aim for the highest time efficiency and the shortest path, respectively.

[0143] Establish the overall objective function

[0144] The overall objective function of the TEB algorithm can be obtained by weighted summation of various constraint functions and sub-objective functions.

[0145]

[0146] Where f(B) is the overall objective function, γ k f is the weighting coefficient. k (B) represents the k-th constraint or sub-objective function, where K is the total number of constraints and sub-objective functions.

[0147] Building a hypergraph

[0148] A hypergraph is constructed based on cost map information, the current trajectory to be optimized, and the overall objective function. A hypergraph is an important concept in graph theory used to represent relationships between objects, extending from the basic graph. A graph consists of a set of vertices and a set of edges, with each edge connecting two vertices. Traditional graph edges can only connect two vertices, which is inflexible in some complex scenarios. Hypergraphs allow a single edge (called a hyperedge) to connect multiple vertices, thus more naturally representing complex relationships and higher-order interactions. The TEB algorithm transforms the path planning problem into a multi-objective optimization problem and represents it using a hypergraph.

[0149] Figure 5 This is the hypergraph of the improved TEB algorithm proposed in this invention. Circles represent objects to be optimized, including vertices of two types: pose points and time intervals, such as S1 in the figure. Double circles represent fixed vertices, such as obstacles O1, path points P1, and starting pose S0 in the figure; these do not change with algorithm optimization. Rectangular boxes represent various edges related to the vertices, i.e., the various constraints and optimization objectives mentioned above, including path point constraint edges f. path Obstacle constraint edge f obst Velocity constraint edge f vel Acceleration constraint edge f acc Nonholonomic kinematic constraint edge f nh The velocity curvature radius constraint edge f proposed in this invention vel_rOptimal time objective edge f time and the shortest path target edge f length The lines connecting nodes and edges correspond to the objective function and constraint functions, respectively.

[0150] S7. Solve the hypergraph to obtain the optimal trajectory. If the solution fails, reduce the number of global path points and return to step S4. Here, reduce the number of global path points to half of the total number of previously selected points and then return to step S4. The specific number of points to be reduced can be adjusted according to the actual situation; otherwise, proceed to step S8.

[0151] The hypergraph is solved using optimization algorithms within the g2o framework. g2o is a general graph optimization framework that transforms multi-objective optimization problems into nonlinear least squares problems, as follows:

[0152]

[0153] Where x is the object to be optimized, Ω k Let z be the weight matrix. jl Indicates parameter x j With x l The constraints between them, e k (x j ,x l ,z jl ) is the vector error function, that is, the error function e Γ However, the constrained object here is x. j With x l The boundary value is z jl Used to calculate parameter x j With x l Satisfying constraint z jl The degree to which this is satisfied is such that when all constraints are fully met, this value is 0. The optimal solution is...

[0154]

[0155] x in the formula * The x that corresponds to the minimum value of F(x).

[0156] When optimizing the TEB trajectory using the g2o graph optimization method, x is replaced by B, and the parameter x j With x l From the trajectory to be optimized (also known as the TEB state) (x) i ,ΔT i Given x, let x j =x i ,x l =ΔT i In other words, during the optimization process, these two parameters can be replaced by the pose point and the time interval, respectively. Assuming the error term is a scalar, then we have...

[0157] F k =Ω k e k

[0158] in

[0159]

[0160] The optimal solution for TEB can then be expressed as:

[0161]

[0162] B * The trajectory sequence corresponding to the minimum value of f(B) is given. During the optimization process, the algorithm will adjust the position and time interval of the pose points to minimize the overall objective function.

[0163] S8. Traverse the optimal trajectory and interpolate between adjacent pose points where the angle difference and displacement distance exceed a preset threshold to obtain the final trajectory; then determine whether the distance between the robot traveling along the final trajectory and the obstacle is less than the threshold. If so, reduce the number of filtered global path points and return to step S4; otherwise, proceed to step S9. Specifically, this includes the following sub-steps:

[0164] S81. Check if the pose point needs interpolation.

[0165] Traverse the pose points on the trajectory and check whether the angle difference or distance difference between two points exceeds the preset minimum resolution. If it does, interpolation is required. The interpolation process is as follows: calculate the number of extra points to be inserted between two points by dividing the angle difference or distance difference between two points by the minimum resolution, denoted as μ. Perform linear interpolation between the two points to insert μ points.

[0166] S82. If the distance between the vehicle and the obstacle is less than the threshold, reduce the number of global path points filtered and return to step S4; otherwise, proceed to step S9.

[0167] The process checks whether the distance between the robot's footprint and the obstacle at each pose point is less than a threshold. If it is less than the threshold (a distance less than a certain value may cause the robot's footprint to overlap with the obstacle), the trajectory is not feasible, and the number of filtered global path points needs to be reduced, returning to step S4. In this embodiment, the number of filtered global path points is reduced to half of the previously filtered global path points. The specific reduction number can be adjusted according to the actual situation. A footprint is the geometric representation of a robot in a two-dimensional plane (that is, it approximately corresponds to the robot's actual outline), used to describe the area occupied by the robot in space. It is usually a polygon or a circle, and the specific shape depends on the type of robot. For example, differential robots usually use circular footprints, while Ackerman robots use rectangular footprints. Calculating the distance between the footprint and the obstacle can more accurately reflect the actual situation compared to only calculating the distance between the pose point and the obstacle.

[0168] S9. Calculate the motion control quantity of the mobile robot, send the control quantity to the chassis of the mobile robot, and move according to the optimal trajectory; after the robot moves, re-select the global path points, and then return to step S4 until the robot reaches the target point.

[0169] The process of re-filtering global pathpoints is as follows:

[0170] Identify the global path point closest to the robot's current position (this point may have been visited or it may not have been visited yet), remove all global path points before this point, that is, remove all path points in the direction from this point to the starting point of the global path, and filter the remaining global path points to global path points whose distance from the robot's current position does not exceed a threshold and whose cumulative distance from the nearest global path point does not exceed a preset cumulative length.

[0171] After the robot moves, global path points that have been traversed and are no longer relevant are removed, and only path points in front that are relevant to the current and future movements are retained. This ensures that each optimization loop (starting from S4) focuses on the robot's current pose and the path ahead, continuously refreshing the optimization problem based on the latest state. This not only improves computational efficiency (keeping the problem size under control), but more importantly, it enables the planning to closely follow the robot's actual progress and environmental changes, significantly improving the coherence, real-time performance, and robustness of the overall path planning.

Claims

1. A robot smooth path planning method based on an improved TEB algorithm, characterized in that, Includes the following steps: S1. Create the robot's motion environment and generate a cost map; Obtain robot positioning information and call the global planner to plan the global initial path; S2. Based on the curvature of the global path, adaptively and dynamically sample the path points on the global path, reducing the number of sampling points at low curvature paths. S3. Select a subset of global path points from the sampled global path points; S4. Initialize the selected global path points to obtain pose points, and add time intervals between adjacent pose points to complete the initialization of the trajectory to be optimized. S5. Insert or delete pose points in the trajectory to be optimized according to the time interval; update the cost map, obstacle information and waypoints according to the updated trajectory to be optimized; S6. Construct multiple constraints and sub-objective functions, and construct the overall objective function accordingly. Create a hypergraph based on the cost map, the current trajectory to be optimized, and the overall objective function. Constraints include curvature radius constraints that use the global path curvature radius to constrain the linear velocity. S7. Solve the hypergraph. If the solution fails, reduce the number of global path points selected and return to step S4; otherwise, proceed to step S8. S8. Traverse the optimal trajectory and interpolate adjacent pose points where the angle difference or distance difference exceeds the preset minimum resolution to obtain the final trajectory. Then determine whether the distance between the robot and the obstacle is less than the threshold when the robot travels along the final trajectory. If it is, reduce the number of global path points selected and return to step S4. Otherwise, proceed to step S9. S9. Calculate the robot's motion control quantity based on the final trajectory, and control the robot's movement accordingly; after the robot moves, re-select global path points, and then return to step S4 until the robot reaches the target point.

2. The method according to claim 1, characterized in that, Step S2 includes the following sub-steps: S21. Calculate the angle between the displacement vectors formed by all three consecutive path points on the global path. The formula for calculating the angle between the displacement vectors is: in, Let be the angle between the displacement vectors formed by the i-th path point and its adjacent path points before and after it. and These are the angles between the displacement vector formed by the i-th path point and the (i+1)-th and (i-1)-th path points and the positive x-axis of the global coordinate system. S22. Adaptive sampling of path points based on the angle between displacement vectors: Retain the start and end points of the global path points. For any other (i-1)th path point, calculate its distance d to the i-th and (i+1)-th path points respectively. i-1,i and d i-1,i+1 If d i-1,i ≤D p_max And d i-1,i+1 >D p_max If the i-th path point is selected, then the i-th path point is retained; otherwise, the decision is made... If the value is greater than or equal to the angle threshold, the i-th path point is retained; otherwise, the path point is not retained. Where D... p_max This represents the maximum path point distance.

3. The method according to claim 1, characterized in that, The filtering process in step S3 is as follows: starting from the global path point closest to the robot, traverse the global path points in the directions not reached by the robot, and retain global path points whose distance from the robot does not exceed a threshold and whose cumulative distance from the nearest global path point does not exceed a preset cumulative length.

4. The method according to claim 1, characterized in that, Step S4 includes the following sub-steps: S41. Global pathpoint initialization: Transform the retained global pathpoints to the local coordinate system to obtain the TEB pose sequence Q. The expression for Q is Q = {S}. i } i＝1…N Among them, S i Let be the pose point corresponding to the i-th global path point, and N be the total number of global path points selected. S i The expression is S i =(x i ,y i ,θ i ) T Where, x i and y i θ represents the x and y coordinates of the i-th global path point in the local planner's coordinate system; i Let be the orientation angle of the robot at the i-th global path point in the coordinate system of the local planner; S42. Initialization of the trajectory to be optimized: Determine if the number of pose points is less than a set value. If it is less than the set value, interpolate between the last two pose points until the set number of pose points is reached. Then, add time intervals between all adjacent pose points and merge the pose point sequence and all time intervals to obtain the trajectory to be optimized. B={S1,ΔT1,S2,ΔT2,…ΔT N-1 ,S N } Where B is the trajectory to be optimized, ΔT N-1 The time interval between the Nth and N-1th pose points.

5. The method according to claim 1, characterized in that: In step S5, if the time interval between adjacent pose points is greater than the set maximum duration and the total number of pose points is less than the set number, a new pose point is inserted between the two pose points; if the time interval between adjacent pose points is less than the set minimum duration, the pose point that is farther away from the robot's current position is deleted.

6. The method according to claim 1, characterized in that: The multiple constraints in step S6 include (1) Path point constraint f path The expression is f path =e Γ (d min,k D pmax ,ε,S,n) Where, d min,k Let the pose point be its nearest path point P. k The closest distance, D pmax e is the maximum allowed path deviation distance. Γ Let ε be the error function, ε be the safety margin, n be the order, and S be the scaling factor. e Γ The calculation formula is: Where x is the constrained object, x r These are boundary values; (2) Obstacle constraint f obst The expression is f obst =e Γ (-d min,j ,-D omin ,ε,S,n) d min,j Let the pose point be its nearest obstacle O j The distance, D omin The minimum distance between the allowed pose point and the obstacle; (3) Velocity and acceleration constraints, the expression is: Among them, v i w i a i and α i These represent the robot's velocity, angular velocity, acceleration, and angular acceleration between the i-th and i+1-th pose points, respectively. min and v max These represent the minimum and maximum linear velocities, w. min and w max These are the minimum and maximum angular velocities, a. min and a max These are the minimum and maximum linear accelerations, v. min and v max These are the minimum and maximum angular accelerations, respectively. (4) Nonholonomic kinematic constraints f nh The expression is Where, θ i and θ i+1 Let d be the robot's orientation angles at pose points i and i+1, respectively. i,i+1 Let be the displacement vector of the robot from the i-th pose point to the (i+1)-th pose point; (5) Velocity curvature radius constraint f vel_r The expression is f vel_r =e Γ (v i ,v des_lim ,ε,S,n) Among them, v des_lim This represents the limit of the desired speed; v des_lim The calculation formula is: Among them, v max For the set maximum speed, r min R is the minimum turning radius of the mobile robot. i Point P on the global path i The corresponding radius of curvature, where β is the radius of curvature influencing factor.

7. The method according to claim 6, characterized in that: In step S6, the multiple sub-objective functions include the optimal time sub-objective function f. time The expression is Where B is the trajectory to be optimized, N is the total number of path points in the trajectory to be optimized, and ΔT i The time interval between the i-th and i+1-th pose points; Shortest path length sub-objective function f length The expression is Where, x i and x i+1 Let y and y be the x-coordinates of the i-th and i+1-th pose points, respectively. i and y i+1 These are the ordinates of the i-th and i+1-th pose points, respectively; The overall objective function is Where f(B) is the overall objective function, γ k f is the weighting coefficient. k (B) represents the k-th constraint or sub-objective function, where K is the total number of constraints and sub-objective functions.

8. The method according to claim 1, characterized in that, The process of selecting new global path points in step S9 is as follows: identify the global path point closest to the robot's current position, remove all global path points before that point, and select global path points from the remaining global path points whose distance from the robot's current position does not exceed a threshold and whose cumulative distance from the nearest global path point does not exceed a preset cumulative length.

9. A robot smooth path planning system based on an improved TEB algorithm, characterized in that, The system includes: Global path planning module: used to create the robot's motion environment and generate a cost map; obtain robot localization information and call the global planner to plan the global initial path; Adaptive sampling module: Used to adaptively and dynamically sample path points on the global path according to the curvature of the global path, reducing the number of sampling points in low curvature paths; Filtering module: Used to filter a subset of global path points from the sampled global path points; Initialization module: used to initialize the selected global path points to obtain pose points, and add time intervals between adjacent pose points to complete the initialization of the trajectory to be optimized; Update module: Used to insert or delete pose points in the trajectory to be optimized according to time intervals; update the cost map, obstacle information and waypoints according to the updated trajectory to be optimized; Hypergraph creation module: Used to construct multiple constraints and sub-objective functions, and based on these, construct the overall objective function. Create a hypergraph based on the cost map, the current trajectory to be optimized, and the overall objective function. Constraints include curvature radius constraints that use the global path curvature radius to constrain linear velocity. Optimal trajectory solution module: used to solve the hypergraph. If the solution fails, the number of global path points selected is reduced and the module returns to the initialization module; otherwise, the trajectory feasibility determination module is entered. Trajectory Feasibility Determination Module: This module is used to traverse the optimal trajectory and interpolate adjacent pose points where the angle difference or distance difference exceeds the preset minimum resolution to obtain the final trajectory. Then, it determines whether the distance between the robot and the obstacle is less than the threshold when the robot travels along the final trajectory. If it is, the number of global path points selected is reduced and the module is returned to the initialization module. Otherwise, the module is entered. Motion module: Used to calculate the robot's motion control quantities based on the final trajectory, and control the robot's movement accordingly; after the robot moves, it re-selects global path points and initializes them, and then returns to the initialization module until the robot reaches the target point.

10. A computer-readable storage medium for storing one or more programs, characterized in that: The program includes one or more instructions that, when executed by a computing device, cause the computing device to perform any of the methods according to claims 1 to 8.

Images