A Convex Programming Approach to Multipoint Optimal Motion Planning for Unicycle Robots

An important extension of point-to-point motion planning deals with navigating a robot optimally through a sequence of waypoints. The resulting multipoint problem is typically nonconvex due to the requirement to optimize the waypoint passage time, which has hindered the determination of globally optimal solutions. In this letter, a new motion planning technique is presented for multipoint problems involving unicycle robots. Our main finding is that, by adopting a cost function that weights a combination of path length and maneuver time, together with a suitable tightening of the control input constraints, such type of problems can be cast as a convex program. This can be solved in polynomial time to obtain a global optimum. The proposed motion planner is compared to time-optimal trajectory generation scheme based on nonlinear programming, and it can find better trajectories than this method while being computationally faster.