Tractor-Trailer Vehicle Trajectory Planning in Narrow Environments With a Progressively Constrained Optimal Control Approach

Trajectory planning for a tractor-trailer vehicle is challenging due to the non-convex collision avoidance constraints and the unstable kinematics. This article aims to plan precise and optimal trajectory for a car-like tractor that tows standard or general N-trailers in an environment with static obstacles. This trajectory planning scheme can be formulated as a unified optimal control problem, but numerically solving that problem is difficult. Two efforts are made to address the difficulty. First, an extended hybrid A* search algorithm is proposed to provide an initial guess for the numerical optimization. Second, the numerical optimization process is eased through adopting a progressively constrained optimization strategy, which is about sequentially handling the kinematics, collision avoidance constraints, and boundary conditions. Particularly in dealing with the collision avoidance constraints, an adaptively homotopic warm-starting algorithm is proposed, which defines a sequence of subproblems with the obstacle areas adaptively increase towards their nominal scales. Through solving these subproblems in a sequence, the whole collision avoidance difficulties are dispersed and gradually tackled. Comparative simulations are conducted to validate the efficacy of the proposed trajectory planner.