Iterated LQR smoothing for locally-optimal feedback control of systems with non-linear dynamics and non-quadratic cost

Abstract

This paper introduces the novel concept of LQR smoothing, which analogous to Kalman smoothing consists of both a backward pass and a forward pass. In the backward pass the cost-to-go function is computed using the standard LQR Riccati equation that runs backward in time, and in the forward pass the cost-to-come function is computed using a Riccati equation that runs forward in time. The sum of the cost-to-go and the cost-to-come function gives the total-cost function, and we will show that the states for which the total-cost function is minimal constitute the minimum-cost trajectory for the linear-quadratic optimal control problem. This insight is used to construct a fast-converging iterative procedure to compute a locally-optimal feedback control policy for systems with non-linear dynamics and non-quadratic cost, where in each iteration the current minimal-total-cost states provide natural points about which the dynamics can be linearized and the cost quadratized. We demonstrate the potential of our approach on two illustrative non-linear control problems involving physical differential-drive robots and simulated quadrotor helicopters in environments with obstacles, and show that our approach converges in only about a third of the number of iterations required by existing approaches such as Iterative LQR.