Optimal Geometric Control Theory and Pontryagin's Maximum Principle (PMP)

Per [1], let \( Q\) be a closed manifold (say, the configuration space [c-space] of a robot or a machine [see, for instance, this webpage or this webage]), \( TQ\) be the tangent bundle of \( Q\), \(TTQ\) the double tangent bundle of \(Q\), \(\mathcal{L}\) a Lagrangian on \(TQ\), \(g\) the inertial Riemannian metric on \(Q\) induced by \(\mathcal{L}\), \(X \in \Gamma(TQ)\) the Lagrangian vector field corrsponding to \(\displaystyle \mathcal{L}, X = \frac{\partial}{\partial t}\left\{\frac{\partial \mathcal{L}}{\partial \dot{q}}\right\} - \frac{\partial L}{\partial q}\), \(R\) be a Raleigh dissipative term, \(F\) an external force, \(\mathcal{D}\) a control distribution that is a sub-vector bundle of \(TTQ\), \(B: U \to \mathcal{D}\) a parameterization of \(\mathcal{D}\) (where \(U \subseteq \mathbb{R}^m)\), so that \(\displaystyle X(\dot{\gamma}(t)) + \frac{\partial R}{\partial \dot{q}}\left(\dot{\gamma}(t)\right) - B(u(t)) - F(t) = 0\) is the equation of motion (EoM), \(M\) a cost quadratic on \(TQ\), \(N\) a cost quadratic on \(U\), and \(P\) a cost quadratic on \(TQ \times U\). Let \((q_0, \dot{q}_0)\) in \(TQ\) be given, and consider an inital cost function \(L_0((q_0, \dot{q}_0), u_0)\), a running cost function \(L( u) =M(q, \dot{q}) + N(u) + P((q,\dot{q}), u)\) , a final cost function \(L_F((q_F, \dot{q}_F), u_F)\) , and a (total) cost funtion \(\displaystyle J(T) = L_0(\dot{\gamma}(0), u(0)) + \int_0^T L(\dot{\gamma}(t), u(t)) - \lambda\left(X(\dot{\gamma}(t)) + \frac{\partial R}{\partial \dot{q}}(\dot{\gamma}(t)) -B(u(t)) - F(t)\right)\ dt + L_F(\dot{\gamma}(T), u(T))\), where \(\gamma: [0, T] \to Q\) with \(\dot{\gamma}(0) = (q_0, \dot{q}_0)\), \(u: [0, T] \to U\) with \(\pi_{TQ}(B(u(t))) = \dot{\gamma}(t)\), and \(\lambda: [0, T] \to T^*(TTQ)\) with \(\pi_{TQ}(\lambda(t)) = \dot{\gamma}(t)\) (the co-state variable, encoding the dynamics of the system as a Lagrange multipliers problem of sorts) are to be determined, and typically either \(T \in \mathbb{R}\) (the final time) or \((q_F, \dot{q}_F)\) (the final point of \(\dot{\gamma}\)) are specified.

Then we call \((Q, \mathcal{L},R, F, U, \mathcal{D}, B, M, N, P, L_0, L_F)\) a nonlinear quadratic regulator (NQR) problem. It is typically desired to find a deterministic function \(K: TQ \to U\) with \(u = K(q,\dot{q})\) "automatically" producing the minimum of \(J\) (for a then uniquely-determined \(\lambda\)). We call \(H: [0, T] \times (TTQ \oplus \mathcal{D} \oplus T^*(TTQ)) \to \mathbb{R}\) given by \(\displaystyle H(t, q, \dot{q}, \ddot{q}, u, \lambda) = L((q, \dot{q}), u) - \lambda\left(X(q, \dot{q}) + \frac{\partial R}{\partial \dot{q}} - B(u) - F\right)\) the Hamiltonian of the NQR problem.

Per [4], the NQR problem is solved in a coordinate patch by solving the following set of simultaneous equations:

\(\begin{cases}\displaystyle \frac{\partial H}{\partial (q, \dot{q})} &\displaystyle = \frac{\partial}{\partial t}\left\{\frac{\partial H}{\partial (\dot{q}, \ddot{q})}\right\} \\ \displaystyle \frac{\partial H}{\partial u} & = \displaystyle 0 \\ \displaystyle \frac{\partial H}{\partial \lambda} & = \displaystyle 0 \end{cases}\)

Recall that we have \(\displaystyle X(\dot{\gamma}(t)) + \frac{\partial R}{\partial \dot{q}}\left(\dot{\gamma}(t)\right) - B(u(t)) - F(t) = 0\) as the EoM. Per [3], this may be rewitten \(\displaystyle B(u(t)) + F(t) = g(\ddot{\gamma}(t)) + \Gamma(\dot{q}(t), \dot{q}(t)) + \frac{\partial R}{\partial \dot{q}}\left(\dot{\gamma}(t)\right) - \frac{\partial \mathcal{L}}{\partial q}\left(\dot{\gamma}(t)\right)\), where \(\Gamma\) is the Cristoffel symbol of the Riemannian metric. The EoM then becomes \(\displaystyle \ddot{\gamma}(t) = g^{-1}( \Gamma(\dot{q}(t), \dot{q}(t)) + \frac{\partial R}{\partial \dot{q}}\left(\dot{\gamma}(t)\right) - \frac{\partial \mathcal{L}}{\partial q}\left(\dot{\gamma}(t)\right) - B(u(t)) - F(t))\). Write \(\displaystyle f(t, \dot{\gamma}(t), u(t)) = g^{-1}( \Gamma(\dot{q}(t), \dot{q}(t)) + \frac{\partial R}{\partial \dot{q}}\left(\dot{\gamma}(t)\right) - \frac{\partial \mathcal{L}}{\partial q}\left(\dot{\gamma}(t)\right) - B(u(t)) - F(t))\). The Hamiltonian then becomes \(\displaystyle H(t, q, \dot{q}, \ddot{q}, u, \lambda) = L((q, \dot{q}), u) - \lambda\left((\dot{q}, \ddot{q}) - f(t, (q, \dot{q}), u)\right)\) and the equations to be solved then become

\(\begin{cases}\displaystyle \frac{\partial L}{\partial (q, \dot{q})} - \left(\frac{\partial f}{\partial (q, \dot{q})}\right)^T\lambda & \displaystyle = \dot{\lambda} \\ \displaystyle \frac{\partial L}{\partial u} - \left(\frac{\partial f}{\partial u}\right)^T\lambda & = \displaystyle 0 \\ \displaystyle (\dot{q}, \ddot{q}) - f(t, (q, \dot{q}), u) & = 0 \end{cases}\)

The type of solution to the above system of equations may be checked by applying Sylvester's criterion for The Second Derivative Test to the symmetric matrix \(\begin{bmatrix} M & P \\ P^T & N \end{bmatrix}\), which states that a symmetric matrix is (a) positive definite if and only if the determinants of all the leading diagonal submatrices are positive, meaning all the eigenvalues are positive and the solution is a local minimum, (b) negative definite if and only if the determinant of the first leading "diagonal submatrix" (entry) is negative, then the determinants of the remaining leading diagonal submatrices strictly alternate in sign, meaning all the eigenvalues are negative and the solution is a local maximum, (c) indefinite if any other pattern for the determinants of the leading diagonal submatrices holds but the determinant of the overall matrix is non-zero, meaning some eigenvalues are positive and some are negative but none are 0, and the solution is a saddle point, or (d) the determinant of the overall matrix is zero, meaning at least one eigenvalue is 0 and The Second Derivative Test fails for this solution.

Per [2], if we call \(\tilde{Q} = [0, T] \times (TTQ \oplus \mathcal{D} \oplus T^*(TTQ)) \times \mathbb{R}\) with points given by \((t, (q(t),\dot{q}(t)), (\dot{q}(t),\ddot{q}(t)), u(t), \lambda(t), H(t, (q(t),\dot{q}(t)), (\dot{q}(t),\ddot{q}(t)), u(t), \lambda(t)))\), then by Pontryagin's Maximum Principle (PMP), for any optimal (cost minimizing) solution \((\gamma(t), u(t), \lambda(t))\) to the NQR problem, the variations of \((t, \dot{\gamma}(t), \ddot{\gamma}(t), u(t), \lambda(t), H(t, \dot{\gamma}(t), \ddot{\gamma}(t), u(t), \lambda(t)))\) in \(\tilde{Q}\) form an upwards-pointing cone at each point. Hence, for any optimal solution, there is a "path" of hyperplanes (contact structure) \((\mathcal{H}_t)\) along \((t, \dot{\gamma}(t), \ddot{\gamma}(t), u(t), \lambda(t), H(t, \dot{\gamma}(t), \ddot{\gamma}(t), u(t), \lambda(t))))\) orthogonal to the symmetry line of the cone and passing through the cone point, and hence a 1-form \(\eta\) along \((t, \dot{\gamma}(t), \ddot{\gamma}(t), u(t), \lambda(t), H(t, \dot{\gamma}(t), \ddot{\gamma}(t), u(t), \lambda(t)))\) on \(\tilde{Q}\), called the Hamiltonian evolution of the NQR problem, with \(\mathcal{H}_t = \ker(\eta(t))\). If \(\displaystyle \eta(t)\left(-\frac{\partial}{\partial H}\right) \neq 0\) for all \(t \in [0, T]\) (so that \(\displaystyle -\frac{\partial}{\partial H}\) does not lie in any of the hyperplanes \(\mathcal{H}_t \)), we call \(\eta\) normal; otherwise, we call \(\eta\) abnormal.

Bibliography:
[1] Agrachev, A. A. & Sachkov, Y. L. (2004). Control Theory from the Geometric Viewpoint. Springer. https://doi.org/10.1007/978-3-662-06404-7
[2] Jóźwikowski, M. & Respondek, W. A contact covariant approach to optimal control with applications to sub-Riemannian geometry. Math. Control Signals Syst. 28, 27 (2016) https://doi.org/10.1007/s00498-016-0176-3
[3] Lynch, K. M., & Park, F. C. (2017). Modern Robotics: Mechanics, Planning, and Control. Cambridge: Cambridge University Press.https://doi.org/10.1017/9781316661239
[4] Vrabie, D.L., Vamvoudakis, K.G., & Lewis, F.L. (2012). Optimal Adaptive Control and Differential Games by Reinforcement Learning Principles. https://doi.org/10.1049/PBCE081