Continuation model predictive control on smooth manifolds

# Continuation model predictive control on smooth manifolds

## Abstract

Model predictive control (MPC) anticipates future events to take appropriate control actions. Nonlinear MPC (NMPC) describes systems with nonlinear models and/or constraints. Continuation MPC, suggested by T. Ohtsuka in 2004, uses Krylov-Newton iterations. Continuation MPC is suitable for nonlinear problems and has been recently adopted for minimum time problems. We extend the continuation MPC approach to a case where the state is implicitly constrained to a smooth manifold. We propose an algorithm for on-line controller implementation and demonstrate its numerical effectiveness for a test problem on a hemisphere.

n
1

footnoteinfo]Accepted to the 16th IFAC Workshop on Control Applications of Optimization (CAO’2015), Garmisch-Partenkirchen, Germany, October 6–9, 2015.

First]Andrew Knyazev Second]Alexander Malyshev

onlinear model predictive control, Newton-Krylov method, geometric integration, structure-preserving algorithm.

## 1 Introduction

Model predictive control (MPC) is exposed, e.g., in Camacho et al. (2004); Grüne et al. (2011). MPC is efficient in industrial applications; see Qin et al. (2003). Numerical aspects of MPC are discussed in Diehl et al. (2009). Our contribution to numerical methods for MPC is further development of the approach by Ohtsuka (2004) and efficient use of the Newton-Krylov approximation for numerical solving the nonlinear MPC problems. Knyazev et al. (2015a) solves a minimum-time problem and investigates preconditioning for the Newton-Krylov method.

The MPC method owes its success to efficient treatment of constraints on control and state variables. The goal of the present note is to draw attention to the important fact that, in addition to the efficient treatment of explicit constraints, MPC can easily incorporate the structure-preserving geometric integration of ordinary differential equations modeling the system.

The idea of structure-preserving numerical methods for differential equations has been employed in numerical analysis since 1950s, when the first computers began to be used in engineering computations. However, its systematic study under the name of geometric integration of differential equations has been accomplished within the last 40 years. An accessible introduction to this active research domain is found in Hairer (2011). A more advanced source is Hairer et al. (2006).

Variational formulations of many models described by differential equations lead to the Hamiltonian system dynamics; see Leimkuhler et al. (2004). Such dynamic systems conserve the energy and possess a special geometric structure called the symplectic structure. Much effort has been spent by numerical analysts to develop the called symplectic numerical methods, which are especially efficient for long-time integration, because they produce qualitatively correct computed solutions. The symplectic methods are superior even in the short-time simulations. Leimkuhler et al. (2004) presents the state-of-the-art symplectic numerical algorithms and supporting theory.

Description of the system dynamics in the form of ordinary differential equations appears twice in MPC: first, within the finite-horizon prediction problem and, secondly, when advancing the current state with the input control computed by the finite-horizon prediction. The latter can be omitted if the states of the system are measured directly by the controller sensors. Furthermore, the application of the geometric structure-preserving integration algorithms from Hairer et al. (2006) or other sources for advancing the current state is straightforward.

The use of the geometric integration during the finite-horizon prediction is less obvious. The main difference between the method from Knyazev et al. (2015a) and the variant of the present paper is in that the latter applies a structure-preserving geometric integration inside the forward recursion.

The rest of the note is organized as follows. Section 2 presents a framework of the finite-horizon prediction problem and expresses its solution in the form of a nonlinear algebraic equation. Geometric integration is incorporated during the elimination of state variables from the KKT conditions. Section 3 discusses how classical integration methods can be used for integration on manifolds. The content of Section 3 is mainly adopted from Hairer (2001) and discusses the local coordinates approach and projection methods. Section 4 describes a simple test example and necessary formulas for computer implementation. Section 5 shows numerical results and plots.

## 2 Finite-horizon prediction

Our model finite-horizon control problem, see Knyazev et al. (2015a), along a fictitious time consists in choosing the control and parameter vector , which minimize the performance index as follows:

 minu,pJ,

where

 J=ϕ(x(t+T),p)+∫t+TtL(τ,x(τ),u(τ),p)dτ

subject to the equation for the state dynamics

 dxdτ=f(τ,x(τ),u(τ),p), (1)

and the equality constraints for the state and the control

 C(τ,x(τ),u(τ),p)=0, (2)
 ψ(x(t+T),p)=0. (3)

The initial value condition for (1) is the state vector of the dynamic system. The control vector is used afterwards as an input to control the system at time . The components of the vector are parameters of the dynamic system. The horizon time length may depend on .

The continuous formulation of the finite-horizon prediction problem stated above is discretized on a time grid , , through the horizon partitioned into time steps of size , and the time-continuous vector functions and are replaced by their indexed values and at the grid points . The integral of the performance cost over the horizon is approximated by the rectangular quadrature rule. Equation (1) is approximated by a one-step integration formula, for example, a Runge-Kutta method; cf. Hairer (2001). The discretized optimal control problem is as follows:

 minui,p[ϕ(xN,p)+N−1∑i=0L(τi,xi,ui,p)Δτi],

subject to

 xi+1=xi+Φi(τi,xi,ui,p)Δτi,i=0,1,…,N−1, (4)
 C(τi,xi,ui,p)=0,i=0,1,…,N−1, (5)
 ψ(xN,p)=0. (6)

The function is implicitly determined by the structure-preserving method used for numerical integration of (1). We note that , where .

The necessary optimality conditions for the discretized finite horizon problem are obtained by means of the discrete Lagrangian function

 L(X,U) = ϕ(xN,p)+N−1∑i=0L(τi,xi,ui,p)Δτi +λT0[x(t)−x0] +N−1∑i=0λTi+1[xi−xi+1+Φi(τi,xi,ui,p)Δτi] +N−1∑i=0μTiC(τi,xi,ui,p)Δτi+νTψ(xN,p),

where , , and , . Here, is the costate vector and is the Lagrange multiplier vector associated with constraint (5). The terminal constraint (6) is relaxed by the aid of the Lagrange multiplier .

The Hamiltonian function is denoted by

 H(t,x,λ,u,μ,p)=L(t,x,u,p) +λTf(t,x,u,p)+μTC(t,x,u,p).

The necessary optimality conditions are the KKT stationarity conditions: , , , , , , , .

Since is not always available, we suggest to use instead. This modification is applied in the backward recursion used below.

The KKT conditions are reformulated in terms of a mapping , where the vector combines the control input , the Lagrange multiplier , the Lagrange multiplier , and the parameter , all in one vector:

 U(t)=[uT0,…,uTN−1,μT0,…,μTN−1,νT,pT]T.

The vector argument in denotes the current measured or estimated state vector, which serves as the initial vector in the following procedure.

1. Starting from the current measured or estimated state , compute , , by the forward recursion

 xi+1=xi+Φi(τi,xi,ui,p)Δτi.

Then starting from

 λN=∂ϕT∂x(xN,p)+∂ψT∂x(xN,p)ν

compute the costates , , by the backward recursion

 λi=λi+1+∂HT∂x(τi,xi,λi+1,ui,μi,p)Δτi.
2. Calculate , using just obtained and , as

 F[U,x,t] =⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣∂HT∂u(τ0,x0,λ1,u0,μ0,p)Δτ0⋮∂HT∂u(τi,xi,λi+1,ui,μi,p)Δτi⋮∂HT∂u(τN−1,xN−1,λN,uN−1,μN−1,p)ΔτN−1C(τ0,x0,u0,p)Δτ0⋮C(τi,xi,ui,p)Δτi⋮C(τN−1,xN−1,uN−1,p)ΔτN−1ψ(xN,p)∂ϕT∂p(xN,p)+∂ψT∂p(xN,p)ν+∑N−1i=0∂HT∂p(τi,xi,λi+1,ui,μi,p)Δτi⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦.

The equation with respect to the unknown vector

 F[U(t),x(t),t]=0 (7)

gives the required necessary optimality conditions that are solved on the controller in real time. All details about solving (7) by the Newton-Krylov method are found in Knyazev et al. (2015a). We only recall that our method uses GMRES iterations for solving linear systems with the Jacobian matrix . To accelerate the convergence of GMRES iterations, the matrix is computed exactly at some time instances and then used as a preconditioner between these time instances.

## 3 Geometric integration

This section gives a short explanation of geometric integration used in our illustrative example. We consider the ordinary differential equations (1), which describe the system dynamics. Suppose that a property is fulfilled on each solution of (1), where the constant depends on the solution. If the numerical method satisfies the same property , we refer to it as a structure preserving method. A notorious example is given by the spheres .

The article by Hairer (2001) “illustrates how classical integration methods for differential equations on manifolds can be modified in order to preserve certain geometric properties of the exact flow. Projection methods as well as integrators based on local coordinates are considered.”

We adopt the simplest method from Hairer (2001), which is called the method of local coordinates there. When it is feasible, it is the most accurate of all structure preserving methods. Let a manifold contain the solution of , which we want to compute. Assume that is a local parametrization of near the state . The coordinate change transforms the differential equation into

 α′(z)˙z=f(α(z)). (8)

This is an over-determined system of differential equations because the dimension of is less than . However, is tangent to by assumption, therefore, (8) is equivalent to a system

 ˙z=β(z),z(τi)=zi. (9)

If the transfer from (8) to (9) is easy for implementation, then the method of local coordinates is feasible.

The principal idea of the method of local coordinates is to perform one step of a numerical method applied to (9) and to map the result via the transformation back to the manifold. One step of the resulting algorithm is implemented as follows.

Algorithm 1 (Local coordinates approach)

• choose a local parametrization and compute from ;

• compute , the result of the one-step method applied to (9);

• define the numerical solution by .

### 3.1 Symmetric integration methods

Mechanical systems often obey the Hamiltonian structure, and numerical methods for such system must be symmetric or time-reversible in order to preserve geometric properties of the Hamiltonian systems. Assume that a one-step numerical method applied to a system over the interval produces the state from the state . The time-reversibility of the numerical method means that its application to the time-reversed system over the interval produces from . This is a case for the trapezoidal rule, but not for the explicit Euler method.

Hairer (2001) shows that symmetric methods perform qualitatively better for integration over long time intervals. But most of the commonly used techniques for solving differential equations on manifolds destroy the symmetry of underlying method. To restore the symmetry, additional modifications of numerical methods are necessary.

We illustrate restoration of the symmetry for another standard technique of geometric integration on manifolds called the projection methods. For an ordinary differential equation , the one step integration proceeds as follows.

Algorithm 2 (Standard projection method)

• compute by any numerical integrator applied to , e.g. by a Runge-Kutta method;

• project orthogonally onto the manifold to obtain .

The standard projection method is illustrated in Figure 1. The projection destroys the symmetry of if it is available. A symmetry-restoration algorithm is developed in Hairer (2000). The idea is to perturb the vector before applying a symmetric one-step method such that the final projection is of the same size as the perturbation.

Algorithm 3 (Symmetric projection method)

• , where ;

• (symmetric one-step method for equation );

• with vector such that .

Here, denotes the Jacobian of , if the manifold is given by the condition . It is important to choose the same vector in the perturbation an in the projection.

## 4 Test problem

We consider the minimum-time motion over the unit upper hemisphere from a state to a state with inequality constraints.

The system dynamics is governed by the system of differential equations

 ddt⎡⎢⎣xyz⎤⎥⎦=⎡⎢⎣zcosuzsinu−xcosu−ysinu⎤⎥⎦. (10)

The control variable is subject to an inequality constraint. Namely, the control always stays within the band . Following Ohtsuka (2004) we introduce a slack variable and replace the inequality constraint by the equality constraint

 C(u,ud)=(u−cu)2+u2s−r2u=0.

In order to guarantee that the state passes through the point at time , we impose three terminal constraints of the form

 ψ(x,y,z,p)=⎡⎢ ⎢⎣x−xfy−yfz−zf⎤⎥ ⎥⎦=0.

The objective is to minimize the cost function

 J=ϕ(p)+∫tft0L(x,y,z,u,us,p)dt′,

where

 ϕ(p)=p=tf−t0,L(x,y,z,u,us,p)=−wsus.

The term is responsible for the shortest time to destination, and the function serves for stabilization of the slack variable .

System (10) possesses the first integral

 x2+y2+z2=const.

Our initial condition lies on the unit sphere . Therefore, the manifold is the unit sphere with the center at the origin. To simplify the implementation, we choose all parameters such that the trajectories lie on the upper hemisphere . The natural local coordinates in this case are and , and the local parametrization is given by . The system (9) for (10) will be

 ddt[xy]=[√1−x2−y2cosu√1−x2−y2sinu]. (11)

For this particular example and choice of local coordinates the structure preserving geometric integration solver consists of the explicit Euler method for the components and and the formula for the component .

For convenience, we change the time variable within the horizon by the new time , which runs over the interval .

The corresponding discretized finite-horizon problem on a uniform grid in the local coordinates , comprises the following data structures and computations:

• , where , and ;

• the participating variables are the state , the costate , the control , the Lagrange multipliers and ;

• the state is governed by the model equation

 Unknown environment '%

where ;

• the costate is computed by the backward recursion (, )

 Unknown environment '%'

where ;

• the nonlinear equation , where

 U=[u0,…,uN−1,us,0,…,us,N−1, μ0,…,μN−1,ν1,ν2,p],

has the following rows from the top to bottom:

 ⎧⎨⎩Δτp[√1−x2i−y2i(−sinuiλ1,i+1+cosuiλ2,i+1)+2(ui−cu)μi]=0
 {Δτp[2μiusi−ws]=0
 {Δτp[(ui−cu)2+u2si−r2u]=0
 {xN−xf=0yN−yf=0
 ⎧⎪ ⎪⎨⎪ ⎪⎩Δτ{N−1∑i=0√1−x2i−y2i(cosuiλ1,i+1+sinuiλ2,i+1)+μi[(ui−cu)2+u2si−r2u]−wsusi}+1=0.

## 5 Numerical results

In our numerical experiments, we use the method of the local coordinates for geometric integration on the unit upper hemisphere.

The number of grid points on the horizon is , the time step of the dynamic system is , the end points of the computed trajectory are and . The constants of the inequality constraint for the control are and . Other parameters are and .

We use the GMRES method without restarts. The number of GMRES iterations does not exceed , and the absolute tolerance of the GMRES iterations is .

Preconditioning of GMRES accelerates its convergence as demonstrated on Figures 6 and 7. We used a simple preconditioning strategy as follows. The exact Jacobian is computed periodically at time instances with the period seconds. Then the LU factorization of the Jacobian is used as the preconditioner until the next time when it is recalculated.

The value of at is approximated by the Matlab function fsolve with a special initial guess.

## 6 Conclusion

Model predictive control is efficient in dealing with the constraints on the control and state variables. When the state space of a system lies on a manifold, it may be profitable to use numerical methods which inherit this property. For example, numerical algorithms preserving the symplectic structure of Hamiltonian dynamical systems are much superior in accuracy compared to the conventional algorithms. In this note, we show how to incorporate simple modifications of classical integration methods into our numerical approach to MPC obtaining an efficient structure-preserving NMPC method. Other structure-preserving algorithms can be used similarly.

1. thanks: [

### References

1. E. F. Camacho and C. Bordons. Model Predictive Control, 2nd ed. Springer, Heidelberg, 2004.
2. M. Diehl, H. J. Ferreau, and N. Haverbeke. Efficient numerical methods for nonlinear MPC and moving horizon estimation. L. Magni et al. (Eds.): Nonlinear Model Predictive Control, LNCIS 384, pp. 391–417, Springer, Heidelberg, 2009.
3. L. Grüne and J. Pannek. Nonlinear Model Predictive Control. Theory and Algorithms. Springer, London, 2011.
4. E. Hairer. Symmetric projection methods for differential equations on manifolds. BIT, 40:726–734, 2000.
5. E. Hairer. Geometric integration of ordinary differential equations on manifolds. BIT, 41:996–1007, 2001.
6. E. Hairer, G. Wanner, and Ch. Lubich. Geometric Numerical Integration. Structure-Preserving Algorithms for Ordinary Differential Equations. Springer, Berlin Heidelberg, 2006.
7. E. Hairer. Solving Differential Equations on Manifolds. Université de Genève, Genève, 2011. http:// www.unige.ch/~hairer/poly-sde-mani.pdf
8. A. Knyazev, Y. Fujii, and A. Malyshev. Preconditioned Continuation Model Predictive Control. SIAM Conf. Control. Appl., July 8–10, 2015, Paris, France, pp. 1–8, 2015.
9. B. Leimkuhler and S. Reich. Simulating Hamiltonian Dynamics. Cambridge University Press, 2004.
10. T. Ohtsuka. A Continuation/GMRES method for fast computation of nonlinear receding horizon control. Automatica, 40:563–574, 2004.
11. S. J. Qin and T. A. Badgwell. A survey of industrial model predictive control technology. Control Eng. Practice, 11:733–764, 2003.
72340