A Quadratic-Cost Dual Control-Based Approach for Optimal Trajectory Planning under Uncertainty

Abstract

This paper presents a dual control-based approach for optimal trajectory planning under uncertainty. The method approximately converts a nonlinear stochastic optimal control problem whose objective function is a combination of quadratic stage and/or terminal costs, with additive Gaussian process and measurement noises, into a deterministic optimal control problem by augmenting the uncertainty state defined by the square-root of the estimation error covariance matrix. The open-loop solution to the resulting deterministic optimal control reformulation is obtained using an existing pseudo-spectral method. The effectiveness of the proposed dual control-based approach is verified with two numerical examples of trajectory planning for two-dimensional robot motion with lack of observability for localization, which highlights the impact of the dual effect on the shape of designed paths.