A real-time motion planner with trajectory optimization for autonomous vehicles

In this paper, an efficient real-time autonomous driving motion planner with trajectory optimization is proposed. The planner first discretizes the plan space and searches for the best trajectory based on a set of cost functions. Then an iterative optimization is applied to both the path and speed of the resultant trajectory. The post-optimization is of low computational complexity and is able to converge to a higher-quality solution within a few iterations. Compared with the planner without optimization, this framework can reduce the planning time by 52% and improve the trajectory quality. The proposed motion planner is implemented and tested both in simulation and on a real autonomous vehicle in three different scenarios. Experiments show that the planner outputs high-quality trajectories and performs intelligent driving behaviors.

[1]  John A. Nelder,et al.  A Simplex Method for Function Minimization , 1965, Comput. J..

[2]  A. Kelly,et al.  TRAJECTORY GENERATION FOR CAR-LIKE ROBOTS USING CUBIC CURVATURE POLYNOMIALS , 2001 .

[3]  S. LaValle,et al.  Randomized Kinodynamic Planning , 2001 .

[4]  Mark H. Overmars,et al.  Roadmap-based motion planning in dynamic environments , 2005, IEEE Trans. Robotics.

[5]  William Whittaker,et al.  Autonomous driving in urban environments: Boss and the Urban Challenge , 2008, J. Field Robotics.

[6]  Alonzo Kelly,et al.  Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control , 2003, Int. J. Robotics Res..

[7]  Thierry Fraichard,et al.  Safe motion planning in dynamic environments , 2005, 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[8]  D. Dolgov.  Practical Search Techniques in Path Planning for Autonomous Driving , 2008 .

[9]  Julius Ziegler,et al.  Fast collision checking for intelligent vehicle motion planning , 2010, 2010 IEEE Intelligent Vehicles Symposium.

[10]  Julius Ziegler,et al.  Optimal trajectory generation for dynamic street scenarios in a Frenét Frame , 2010, 2010 IEEE International Conference on Robotics and Automation.

[11]  Sven Koenig,et al.  Improved fast replanning for robot navigation in unknown terrain , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[12]  Christopher G. Atkeson,et al.  An optimization approach to rough terrain locomotion , 2010, 2010 IEEE International Conference on Robotics and Automation.

[13]  Julius Ziegler,et al.  Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios , 2009, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[14]  Matthew McNaughton,et al.  Parallel Algorithms for Real-time Motion Planning , 2011 .

[15]  Jin-Woo Lee,et al.  Motion planning for autonomous driving with a conformal spatiotemporal lattice , 2011, 2011 IEEE International Conference on Robotics and Automation.

[16]  Sebastian Thrun,et al.  ARA*: Anytime A* with Provable Bounds on Sub-Optimality , 2003, NIPS.

[17]  Dean Pomerleau,et al.  ALVINN, an autonomous land vehicle in a neural network , 2015 .