Formally Robust and Safe Trajectory Planning and Tracking for Autonomous Vehicles

In this paper, a safe trajectory planning and tracking algorithm for autonomous vehicles is proposed. Specially, the safety problem considers the geometric constraints including the obstacle avoiding and the road side constraints, and the non-convex input constraints defined from the sideslip angle of the wheels and input boundedness. Control barrier function (CBF) is adopted to deal with the state and input constraints and generate nominal trajectory. For this purpose, the nominal dynamics of the autonomous vehicle is defined as the virtual dynamics, from which the CBF safety certificates are derived. By constructing appropriate feedback control, the tracking error of the actual trajectory can be bounded into a tube, which guarantees the geometric safety of the actual vehicle. Two safety certificates, the bearing and the distance safety certificates are derived for multiple-obstacle avoidance. In order to deal with the non-convex input constraints, a safe braking maneuver is carefully considered. The feasible initial velocity set for safe braking is proposed as a part of state constraints. The feasibility and the safety of the overall system is proved. The algorithm to synthesis the CBF certificates for multiple-obstacle avoidance, and the input constraints is proposed. Simulation results from an autonomous vehicle including disturbances demonstrate the feasibility of the algorithm. The software implementation of the proposed algorithm was developed in C++ intended for real-world testing