Dual objective motion planning subject to state constraints

SUMMARYThis paper presents a novel motion planning approach inspired by the Dynamic Programming (DP) applicable to multi degree of freedom robots (mobile or stationary) and autonomous vehicles. The proposed discrete–time algorithm enables a robot to reach its destination through an arbitrary obstacle field in the fewest number of time steps possible while minimizing a secondary objective function. Furthermore, the resulting optimal trajectory is guaranteed to be globally optimal while incorporating state constraints such as velocity, acceleration, and jerk limits. The optimal trajectories furnished by the algorithm may be further updated in real time to accommodate changes in the obstacle field and/or cost function. The algorithm is proven to terminate in a finite number of steps without its computational complexity increasing with the type or number of obstacles. The effectiveness of the global and replanning algorithms are demonstrated on a planar mobile robot with three degrees of freedom subject to velocity and acceleration limits. The computational complexity of the two algorithms are also compared to that of an A*–type search.