Precise trajectory optimization for articulated wheeled vehicles in cluttered environments

Abstract

Trajectory planning refers to planning a time-dependent path connecting the initial and final configurations with some special constraints simultaneously considered. It is a critical aspect in autonomously driving an articulated vehicle. In this paper, trajectory planning is formulated as a dynamic optimization problem that contains kinematic differential equations, mechanical/environmental constraints, boundary conditions and an optimization objective. The prevailing numerical methods for solving the formulated dynamic optimization problem commonly disregard the constraint satisfactions between every two adjacent discretized mesh points, thus resulting in failure when the planned motions are actually implemented. As a remedy for this limitation, the concept of minute mesh grid is proposed, which improves the constraint satisfactions between adjacent rough mesh points. On the basis of accurate penalty functions, large-scale constraints are successfully incorporated into the optimization criterion, thus transforming the dynamic optimization problem into a static one with simple bounds on the decision variables. Simulation results verify that our proposed methodology can provide accurate results and can deal with various optimization objectives uniformly.