Track execution success rate-oriented TEB track online predictability evaluation and selection method

Abstract

The invention relates to the technical field of TEB trajectory execution, in particular to a trajectory execution success rate-oriented TEB trajectory online predictability evaluation and selection method, which comprises the following steps: S1, TEB candidate trajectory generation: a robot motion planning system receives a target action instruction and environment sensing data, and sends the target action instruction and the environment sensing data to the TEB candidate trajectory; generating a plurality of candidate trajectories meeting kinematics constraints and obstacle avoidance geometric requirements through a TEB trajectory planning algorithm; s2, track execution feature extraction; s3, constructing and training a lightweight trajectory quality prediction model; s4, track execution success rate online evaluation; s5, selecting an optimal track and triggering execution; s6, iteratively optimizing the model; in order to solve the problem of execution uncertainty of the same-action different-quality trajectories, by extracting the steering angle change rate of the trajectories and the obstacle minimum distance time sequence characteristics, constructing a lightweight prediction model and performing predictive evaluation on the execution success rate of TEB candidate trajectories, the high-quality trajectories and the low-quality trajectories which are both target actions such as right turning and the like are effectively distinguished; and the failure risk caused by low-quality trajectory execution is avoided.

Description

Technical Field

[0001] This invention relates to the field of TEB trajectory execution technology, and in particular to a method for online predictive evaluation and selection of TEB trajectories oriented towards trajectory execution success rate. Background Technology

[0002] The Temporal Elastic Band (TEB) trajectory planning algorithm, with its advantages of high computational efficiency and strong adaptability to dynamic obstacles, is widely used in motion planning scenarios for automated equipment such as mobile robots and industrial robotic arms. This algorithm can quickly generate trajectories that satisfy robot kinematic constraints, enabling obstacles avoidance and path tracking in complex environments.

[0003] However, in practical applications, it has been found that for the same target action, such as a robot turning right, the TEB algorithm often generates multiple candidate trajectories with similar geometric shapes but significantly different trajectory quality. While these candidate trajectories theoretically meet kinematic constraints and obstacle avoidance requirements, they fail to consider key execution characteristics such as fluctuations in the rate of change of the turning angle and the temporal stability of the minimum distance to obstacles during trajectory execution. Consequently, some trajectories are prone to robot jitter, obstacle avoidance failure, or even collisions with obstacles during actual execution, leading to unstable trajectory execution success rates and severely impacting the robot's operational reliability in complex scenarios.

[0004] In existing technologies, the evaluation of TEB trajectories mainly focuses on geometric feature analysis in the offline stage or real-time collision detection in the online stage, lacking a predictive evaluation mechanism for trajectory execution success rate. When a low-quality planned trajectory is executed, it is often necessary to wait until an anomaly occurs during execution before replanning, which not only wastes computing resources but also reduces the robot's operational efficiency and safety. To address this, a predictive online evaluation and selection method for TEB trajectories based on trajectory execution success rate is proposed. Summary of the Invention

[0005] The purpose of this invention is to address the shortcomings of existing technologies by proposing an online predictive evaluation and selection method for TEB trajectories oriented towards trajectory execution success rate.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: A method for online predictive evaluation and selection of TEB trajectories based on trajectory execution success rate includes the following steps: S1, TEB Candidate Trajectory Generation: The robot motion planning system receives the target action command and environmental perception data, and generates multiple candidate trajectories that meet the kinematic constraints and obstacle avoidance geometry requirements through the TEB trajectory planning algorithm; S2. Trajectory Execution Feature Extraction: Perform forward simulation on each candidate trajectory to extract the steering angle change rate feature parameter and the obstacle minimum distance time series feature parameter. The steering angle change rate feature parameter includes the maximum value, variance, and mean of the steering angle change rate time series sequence. The obstacle minimum distance time series feature parameter includes the minimum value, fluctuation amplitude, and duration of the minimum distance time series sequence. S3. Lightweight trajectory quality prediction model construction and training: Construct a lightweight trajectory quality prediction model with the aforementioned feature parameters as input and the actual trajectory execution success rate as output, and collect sample data to train the model; S4. Online evaluation of trajectory execution success rate: Input the feature parameters of each candidate trajectory into the trained model, output the predicted execution success rate, and select candidate trajectories with a predicted execution success rate higher than a preset threshold to form a candidate trajectory set. S5. Optimal Trajectory Selection and Execution Trigger: If the candidate trajectory set is not empty, select the trajectory with the highest prediction execution success rate as the optimal trajectory for execution; if the candidate trajectory set is empty, trigger the TEB planner to adjust constraint parameters and generate alternative trajectories. S6. Model Iteration and Optimization: Record the actual execution results of the optimal trajectory, feed them back to the training dataset, and iterate the training of the model periodically.

[0007] Preferably, the forward simulation in S2 is based on the robot's kinematics model, and the candidate trajectory is virtually executed according to a preset sampling frequency without driving the robot body.

[0008] Preferably, the lightweight trajectory quality prediction model is constructed using a gradient boosting decision tree or a lightweight neural network, and the model's single prediction time does not exceed 10ms.

[0009] Preferably, the success rate threshold in S4 is dynamically adjusted according to the robot's operating scenario, with the threshold for high-precision operating scenarios being no less than 90% and the threshold for general obstacle avoidance scenarios being no less than 80%.

[0010] Preferably, the target action command includes a robot right turn command, and the candidate trajectory is a trajectory that meets the requirements of the right turn action.

[0011] The beneficial effects of this invention are: This solution addresses the uncertainty in the execution of trajectories with different qualities for the same action. By extracting the turning angle change rate and the temporal features of the minimum distance to obstacles from the trajectory, a lightweight prediction model is constructed to achieve a predictive assessment of the success rate of TEB candidate trajectories. This effectively distinguishes between high-quality and low-quality trajectories that are both right turns and other target actions, thus avoiding the risk of failure caused by the execution of low-quality trajectories.

[0012] It achieves online real-time evaluation and rapid decision-making. The constructed prediction model is highly lightweight, and the single evaluation time is controlled in milliseconds. It can complete the evaluation and selection in a very short time before trajectory execution, meeting the real-time requirements of robot dynamic programming.

[0013] It improves the success rate of single obstacle avoidance in complex scenarios. By actively abandoning high-risk trajectories and triggering replanning, it significantly reduces the probability of collisions in complex obstacle environments and improves the success rate and reliability of single obstacle avoidance operations.

[0014] Furthermore, it possesses self-iterative optimization capabilities, iteratively training the model by providing feedback on actual execution results, enabling the model to adapt to changes in different work scenarios and continuously improve prediction accuracy. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of an online predictive evaluation and selection method for TEB trajectories based on trajectory execution success rate proposed in this invention. Detailed Implementation

[0016] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0017] Example: Refer to Figure 1 A method for online predictive evaluation and selection of TEB trajectories oriented towards trajectory execution success rate includes the following steps: S1. TEB Candidate Trajectory Generation: The robot motion planning system receives the target action command and environmental perception data, and generates multiple candidate trajectories that meet the kinematic constraints and obstacle avoidance geometry requirements through the TEB trajectory planning algorithm. Each candidate trajectory contains basic information such as position time sequence, steering angle time sequence, and velocity time sequence. S2. Trajectory Execution Feature Extraction: Perform forward simulation for each candidate trajectory to extract the steering angle change rate feature parameter and the time series feature parameter of the minimum distance to obstacles; Steering angle change rate characteristic parameter: Calculate the steering angle change rate at each sampling time in the candidate trajectory, generate a time series of steering angle change rate, and extract the maximum value, variance, and mean of the series as characteristic parameters to characterize the smoothness of the steering action; Temporal characteristics of minimum distance to obstacles: Calculate the minimum distance between the robot body and surrounding obstacles at each sampling moment in the candidate trajectory, generate a temporal sequence of minimum distance, and extract the minimum value, fluctuation amplitude, and duration of the sequence as feature parameters to characterize the obstacle avoidance safety and stability of the trajectory; S3. Lightweight trajectory quality prediction model construction and training: Construct a lightweight trajectory quality prediction model based on machine learning. The lightweight trajectory quality prediction model takes the steering angle change rate feature parameter and the obstacle minimum distance time series feature parameter extracted in S2 as input and the actual trajectory execution success rate as output. We collect feature parameters of TEB candidate trajectories and corresponding actual execution results under different scenarios, construct a training dataset, train a lightweight trajectory quality prediction model, optimize model parameters, and ensure that the model completes prediction calculation in a very short time to meet the real-time requirements of online evaluation. S4. Online evaluation of trajectory execution success rate: Input the feature parameters of each candidate trajectory extracted in S2 into the trained lightweight trajectory quality prediction model. The model outputs the prediction execution success rate of each candidate trajectory. Set an execution success rate threshold and select candidate trajectories with a prediction execution success rate higher than the threshold as the candidate trajectory set. S5. Optimal Trajectory Selection and Execution Trigger: If the candidate trajectory set is not empty, the trajectory with the highest predicted execution success rate is selected as the optimal trajectory and sent to the robot motion controller for execution; if the candidate trajectory set is empty, it is determined that all currently generated candidate trajectories are high-failure-risk trajectories, all candidate trajectories are actively abandoned, and a replanning trigger signal is sent to the TEB planner to drive the planner to adjust constraint parameters (such as increasing the trajectory smoothness coefficient and increasing the obstacle safety distance threshold) to generate a safer alternative trajectory; S6. Model Iteration and Optimization: Record the actual execution results (success / failure) of the optimal trajectory, feed the feature parameters of the trajectory and the actual execution results back to the training dataset, and periodically iterate and train the lightweight trajectory quality prediction model to improve the prediction accuracy of the model in different scenarios.

[0018] Specifically, the forward simulation process in S2 is based on the robot's kinematics model and performs virtual execution of candidate trajectories according to a preset sampling frequency. This does not require actual driving of the robot body, ensuring the efficiency and non-interference of the evaluation process.

[0019] Furthermore, the lightweight trajectory quality prediction model is constructed using gradient boosting decision trees or lightweight neural networks. The model's input dimension does not exceed 10 dimensions, and the single prediction time does not exceed 10ms, meeting the real-time requirements of robot online planning.

[0020] Furthermore, the success rate threshold in S4 is dynamically adjusted according to the robot's operational scenario requirements. In high-precision operational scenarios, the threshold is set to above 90%, and in general obstacle avoidance scenarios, the threshold is set to above 80%.

[0021] The contents not described in detail in this specification are existing technologies known to those skilled in the art.

[0022] All standard parts used in this invention can be purchased from the market, and irregular parts can be customized according to the description and drawings. The specific connection methods of each part adopt conventional methods such as bolts, rivets, and welding that are mature in the prior art. The machinery, parts and equipment adopt conventional models in the prior art, and the circuit connection adopts conventional connection methods in the prior art, which will not be described in detail here.

[0023] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A method for online predictive evaluation and selection of TEB trajectories based on trajectory execution success rate, characterized in that, Includes the following steps: S1, TEB Candidate Trajectory Generation: The robot motion planning system receives the target action command and environmental perception data, and generates multiple candidate trajectories that meet the kinematic constraints and obstacle avoidance geometry requirements through the TEB trajectory planning algorithm; S2. Trajectory Execution Feature Extraction: Perform forward simulation on each candidate trajectory to extract the steering angle change rate feature parameter and the obstacle minimum distance time series feature parameter. The steering angle change rate feature parameter includes the maximum value, variance, and mean of the steering angle change rate time series sequence. The obstacle minimum distance time series feature parameter includes the minimum value, fluctuation amplitude, and duration of the minimum distance time series sequence. S3. Lightweight trajectory quality prediction model construction and training: Construct a lightweight trajectory quality prediction model with the aforementioned feature parameters as input and the actual trajectory execution success rate as output, and collect sample data to train the model; S4. Online evaluation of trajectory execution success rate: Input the feature parameters of each candidate trajectory into the trained model, output the predicted execution success rate, and select candidate trajectories with a predicted execution success rate higher than a preset threshold to form a candidate trajectory set. S5. Optimal trajectory selection and execution triggering: If the set of candidate trajectories is not empty, select the trajectory with the highest prediction success rate as the optimal trajectory for execution. If the set of candidate trajectories is empty, the TEB planner is triggered to adjust the constraint parameters and generate alternative trajectories. S6. Model Iteration and Optimization: Record the actual execution results of the optimal trajectory, feed them back to the training dataset, and iterate the training of the model periodically.

2. The online predictive evaluation and selection method for TEB trajectories based on trajectory execution success rate as described in claim 1, characterized in that, The forward simulation in S2 is based on the robot's kinematics model and performs virtual execution of candidate trajectories according to a preset sampling frequency, without driving the robot body.

3. The online predictive evaluation and selection method for TEB trajectories based on trajectory execution success rate as described in claim 2, characterized in that, The lightweight trajectory quality prediction model is constructed using a gradient boosting decision tree or a lightweight neural network.

4. The online predictive evaluation and selection method for TEB trajectories based on trajectory execution success rate as described in claim 3, characterized in that, The success rate threshold in S4 is dynamically adjusted according to the robot's operating scenario. The threshold for high-precision operating scenarios is no less than 90%, and the threshold for general obstacle avoidance scenarios is no less than 80%.

5. The online predictive evaluation and selection method for TEB trajectories based on trajectory execution success rate as described in claim 4, characterized in that, The target action command includes a robot right turn command, and the candidate trajectory is a trajectory that meets the requirements of the right turn action.

Images