Optimal Path Planning and Speed Control Integration Strategy for UGVs in Static and Dynamic Environments

In this paper, we present an optimal path planning and speed control strategy for motion planning of unmanned ground vehicles to follow the desired path in various driving environments. The novelty of this work is that it integrates the longitudinal speed and lateral motion control by maximizing the longitudinal progression and minimizing the lateral path tracking errors and formulates a standard quadratic optimization problem. Firstly, a dynamic programming approach is introduced to convexify the corridor constraints in path pre-selection and replanned for obstacle avoidance. The idea is to find a convex feasible set for the optimization problem using the convex constraints. And then a standard quadratic convex problem is formulated to optimize the longitudinal motion and steering angle control in prediction and control horizon. The longitudinal progression and lateral tracking errors are decomposed in orthogonal curvilinear coordinates. The centerline as well as road borders are also described by using cubic spline curves. Some challenge scenarios including static and moving participating obstacle vehicles are tested and validated for the proposed motion planning strategy. The feasibility and performance of the proposed strategy are demonstrated by numerical experiments in terms of the stability and maneuverability of unmanned ground vehicles in different driving conditions.