Lecture 8: trajectory optimization
Goal: derive sufficient conditions for local optimality in NLP subject to constraints.
References
- Section 2.1: Stengel, R. F. Optimal Control and Estimation. (Dover, New York, 1994).
- Chapter 3: Bertsekas, D. P. Nonlinear Programming. (Athena Scientific, 1999).
Problem formulation
Consider the optimal control problem (OCP)
\[ \min_u c(x,u)\ \text{s.t.}\ x(s+1) = F(s,x(s),u(s)),\ x(s)\in\mathbb{R}^d, u(s)\in\mathbb{R}^m \]
where
\[ c(x,u) = \ell(t,x(t)) + \sum_{s = 0}^{t - 1}\mathcal{L}(s,x(s),u(s)). \]
In what follows we’ll push time dependence into the subscript, i.e. \(x_s = x(s)\), \(u_s = u(s)\), \(\ell(t,x(t)) = \ell_t(x_t)\), etc.
Bellman’s principle
The OCP can be reduced to solving a sequence of NLP as follows. Letting \(v_s^*(x_s)\) denote the optimal value of state \(x_s\), Bellman’s equation at all times \(s < t\) is
\[ v_s^*(x_s) = \min_{u_s}\left[\mathcal{L}(s,x_s,u_s) + v_{s+1}^*(x_{s+1})\right]. \]
If we solved this NLP parameterized by \(x_s\) backward in time from \(s = t-1\) to \(s = 0\) we’d obtain the optimal policy \(u^* : [0,t)\times\R^d\rightarrow\R^m\) that minimizes \(v_s^*\) at every \(s\in[0,t)\). But other than the special case of linear dynamics with a quadratic cost we studied previously, this problem is generally intractable.
Another approach
What if we tried to solve (OC) for a given initial condition \(x_0\in\R^d\) to obtain the optimal \(u_{x_0}^*:[0,t)\rightarrow\R^m\)?
Q: show that (OCP) can be rewritten as the NLP
\[ \min_{\bar{u}\in\R^{\bar{m}}} \bar{c}(\bar{x},\bar{u})\ \text{s.t.}\ \bar{f}(\bar{x},\bar{u}) = 0. \]
Note that \(u:[0,t)\rightarrow\R^m\) can be regarded as a vector \(\bar{u}\in(\R^m)^t = \R^{t m}\) where \([\bar{u}]_s = u(s)\), i.e., \(\bar{u}\) is a block-defined vector with the \(s\)-th block equal to the value of \(u\) at time \(s\). Similarly define \(\bar{x}\in\R^{t d}\) by \([\bar{x}]_s = x_u(s)\) where \(x_u:[0,t]\rightarrow\R^d\) is the trajectory generated by \(u\) from initial condition \(x_u(0) = x_0\).
Then defining \(\bar{c}(\bar{x},\bar{u}) = c(x,u)\) and \([\bar{f}(\bar{x},\bar{u})]_s = x_{s+1} - F_s(x_s,u_s)\) shows the problems are equivalent.
For \(u_0:[0,t)\rightarrow\R^m\), \(x_0:[0,t)\rightarrow\R^d\) to be optimal, they must be stationary:
\[ D\bar{c}(\bar{x},\bar{u}) + \bar{\lambda} D\bar{f}(\bar{x},\bar{u}) = 0. \]
Q: start at time \(t\) and use stationarity with respect to \(x_t\in\R^d\) to determine \(\lambda_t\in\R^{1\times d}\).
\(D_{x_t} \bar{c} = D_{x_t} \ell_t\)
\(D_{x_t} \bar{f}_t = D_{x_t}\left[ x_t - F_{t-1}(x_{t-1},u_{t-1}) \right] = I_d\)
Therefore \(D_{x_t} \bar{c} + \lambda_t D_{x_t} \bar{f}_t = D_{x_t}\ell_t + \lambda_t I_d\) so \(\boxed{\lambda_t = -D_{x_t}\ell_t(x_t)}\).
Q: now start at time \(s\) and use stationarity with respect to \(x_s\) to determine \(\lambda_{s-1}\) in terms of \(\lambda_s\).
\(D_{x_s} \bar{c} = D_{x_s} \mathcal{L}_s\)
\(D_{x_s} \bar{f}_s = D_{x_s}\left[ x_{s+1} - F_s(x_s,u_s) \right] = -D_{x_s} F_s\)
\(D_{x_s} \bar{f}_{s-1} = D_{x_s}\left[ x_{s} - F_{s-1}(x_{s-1},u_{s-1}) \right] = I_d\)
Therefore \(D_{x_s} \bar{c} + \left[ \lambda_s, \lambda_{s-1} \right] D_{x_s} \bar{f}_{s,s-1} = D_{x_s}\mathcal{L}_s + \lambda_{s-1} I_d - \lambda_s D_{x_s} F_s\) so \(\boxed{\lambda_{s-1} = -D_{x_s}\mathcal{L}_s(x_s,u_s) + \lambda_s D_{x_s} F_s(x_s,u_s)}\).
What we find is that the \(\lambda\)’s satisfy a linear time-varying (LTV) difference equation (DE) that runs backward in time from the final state. These variables are referred to as costates.
Importantly, the costates have the following relationship to the cost even when \(u\) is not optimal. If we let
\[ v_s(x_s) = \ell_t(x_t) + \sum_{\tau=s}^{t-1} \mathcal{L}_\tau(x_\tau,u_\tau) \]
denote the (not necessarily optimal) cost-to-go of state \(x_s\) with input \(u\), then \(D_{x_s} v_s(x_s) = \lambda_s\), that is, the costate computes the gradient with respect to the state of the cost that will be incurred from state \(x_s\) while applying input \(u\). This may initially seem like an unhelpful quantity since we cannot directly adjust the state, but we can use the chain rule to compute the gradient of the cost with respect to the control input:
\[ D_{u_s} c = D_{u_s} \mathcal{L}_s + D_{x_{s+1}} c \cdot D_{u_s} x_{s+1} = D_{u_s} \mathcal{L}_s(x_s,u_s) + \lambda_{s+1} D_{u_s} F_s(x_s,u_s). \]
This formula gives us a recipe to update the control input in a direction that (to first order) will decrease the cost: \(u^+ = u - \gamma D_u c\).