Real-time Trajectory Optimization: An Integrated Approach of Differential Flatness and Convex Feasible Set

Trajectory optimization is of significant importance in reducing energy consumption, avoiding obstacles, and improving the efficiency of navigation for mobile robots. However, due to the nonlinear dynamic model of the robotic system and non-convex obstacle avoidance constraints, solving the problem is still troublesome. In this paper, a real-time two-stage trajectory optimization algorithm containing trajectory generating and velocity optimization is proposed. In the first stage, leveraging the differentiable flatness property of robot trajectories, and combining the constructed convex feasible set, a dynamically feasible and obstacle-free trajectory is generated. In the second stage, a velocity optimization algorithm is proposed to optimize the velocity, limit the angular velocity, and minimize the total cost time for the trajectory. The simulation and experiment results show the optimality and efficiency of our proposed approach.