Kinodynamic Motion Planning for Autonomous Vehicles

Abstract

This article proposes a computationally effective motion planning algorithm for autonomous ground vehicles operating in a semi-structured environment with a mission specified by waypoints, corridor widths and obstacles. The algorithm switches between two kinds of planners, (i) static planners and (ii) moving obstacle avoidance manoeuvre planners, depending on the mobility of any detected obstacles. While the first is broken down into a path planner and a controller, the second generates a sequence of controls without global path planning. Each subsystem is implemented as follows. The path planner produces an optimal piecewise linear path by applying a variant of cell decomposition and dynamic programming. The piecewise linear path is smoothed by Bézier curves such that the maximum curvatures of the curves are minimized. The controller calculates the highest allowable velocity profile along the path, consistent with the limits on both tangential and radial acceleration and the steering command for the vehicle to track the trajectory using a pure pursuit method. The moving obstacle avoidance manoeuvre produces a sequence of time-optimal local velocities, by minimizing the cost as determined by the safety of the current velocity against obstacles in the velocity obstacle paradigm and the deviation of the current velocity relative to the desired velocity, to satisfy the waypoint constraint. The algorithms are shown to be robust and computationally efficient, and to demonstrate a viable methodology for autonomous vehicle control in the presence of unknown obstacles.