Nonlinear Model Predictive Control
Nonlinear Model Predictive Control, NMPC, is generally formulated as online solving an optimization problem for a system with nonlinear dynamics while satisfying a set of constraints. According to the feedback strategy, based on the receding horizon, only the first control input from the optimal control policy is applied. In the next time step, the optimization problem is solved again and a new optimal control policy is obtained based on new measurements of the system. This can be summarized as the following step :
1. Measure the state of the system at the current time t0.
2. Solve the optimization problem over a time horizon T and find the op-timal control policy u( ) .
3. Apply the first control from the optimal control policy.
4. Wait for one time step and start again from step 1.
To formulate the optimization problem mathematically, consider a continuous-time system given by the nonlinear dynamics x(t) = f(x(t); u(t)) (2.1) , where x(t) 2 Rn is the state of the system, u(t) 2 Rm is the control input of the system and f : Rn Rm ! Rn is a globally Lipschitz continuous function. The nonlinear system satisfies the state and input constraints
x(t) 2 X ; 8t 0 (2.2)
u(t) 2 U; 8t 0 (2.3)
, where X Rn is connected, and U Rm is compact. Note that for sim-plicity (2.3) can be denoted as u( ) 2 U where u( ) is referred as the input trajectory. The objective of the optimization problem is to find an optimal control policy u( ) while minimizing a cost function, that describes the de-sired performance of the system, over a time horizon T J(x(t0); u( )) = L(x(t); u(t))dt (2.4)
Given equations (2.1), (2.2), (2.3), (2.4), the optimization problem that is solved online in NMPC is generally formulated as:
u( ) 0 t0+T (2.5a)
Zt0 L(x(t); u(t))dt
min J(x(t ); u( )) =
subject to x(t) = f(x(t); u(t)); 8t 2 [t0; t0 + T ] (2.5b)
x(t0) = x0 (2.5c)
u(t) 2 U; 8t 2 [t0; t0 + T ] (2.5d)
x(t) 2 X ; 8t 2 [t0; t0 + T ] (2.5e)
Most systems have nonlinear dynamics and it is desirable to use a nonlinear model of the system for the prediction in NMPC because it is more accurate than the linear model. Therefore NMPC may be appealing because it allows using a nonlinear model and also allows considering the state and input con-straints explicitly as shown in the optimization problem above. However to guarantee stability, as mentioned in , the cost function (2.4) should have an infinite time horizon, T = 1. This causes some issues since it is very diﬃcult, if not impossible, to solve such an optimization problem for nonlinear systems. To find a numerical solution for the optimization problem that is solved online in NMPC the time horizon should be finite. In  diﬀerent approaches are discussed on how to achieve closed-loop stability for NMPC while having a finite time horizon. One of the approaches, that is mostly used because of its simplicity, is the terminal equality constraint x(t0 + T ) = 0. The closed-loop stability is achieved under the assumption of a quadratic objective function. In addition to that, the system must be in the origin in finite time, which may result in a long time horizon. Another approach that was discussed is Quasi Infinite Horizon NMPC, where the idea is to split up the infinite horizon cost function into a finite horizon cost function and a terminal penalty as:
J(x(t0); u( )) = t0 L(x(t); u(t))dt 1
= Z t0+T
t0 L(x(t); u(t))dt + Zt0+T L(x(t); u(t))dt (2.6)
L(x(t); u(t))dt + Lf (x(t0 + T ))
t= R 1
For the inequality t0+T L(x(t); u(t))dt Lf (x(t0 + T )) to hold, the trajecto-ries of the closed-loop system should remain in the terminal region, , for the time interval [t0 + T; 1]. To achieve this the terminal region, , is created so that a local feedback control law u = k(x) asymptotically stabilizes the sys-tem in the terminal region for the time interval [t0 + T; 1]. To ensure that the trajectories of the closed-loop system remain in the terminal region a terminal constraint is added to the NMPC optimization problem: x(t0 + T ) 2 . How-ever finding the terminal region, , and adding the terminal constraint is not an easy task, where can be found for instance by solving an optimization prob-lem oﬄine as explained in . Nevertheless, in  it is shown that closed-loop stability is achieved without terminal constraints where the terminal region is chosen as = X . The closed-loop system for NMPC is semiglobally asymp-totically stable given that the terminal cost function Lf (x(t0 + T )) is a local control Lyapunov function of the system, see chapter 7 in  for more details.
Obstacle avoidance is fundamental for autonomous systems, where they move and navigate in a complex environment with many diﬀerent obstacles. For ex-ample, unmanned air vehicles, UAVs, or autonomous vehicles try to move in their environment while not colliding with trees, buildings, people, or other au-tonomous vehicles. Using MPC for obstacle avoidance is appealing because the obstacle avoidance can be expressed as a state constraint in for instance equation (2.2) with the system dynamics in the MPC formulation. There are many implementations for obstacle avoidance using MPC for instance  pro-poses navigation and obstacle avoidance for UAV using NMPC. In  the ob-stacles are modeled as spheres and the obstacle avoidance is included as a state constraint where the distance between the UAV’s position and the obstacle’s position should satisfy some safety distance margin. Another implementation is described in , where the authors propose obstacle avoidance based on the vehicle’s sensor data using MPC. The obstacle avoidance in  is formulated as a state constraint in which a safe area is defined based on LIDAR sensor data, where the vehicle should lie within it to avoid the obstacles. The au-thors in  present a novel method for optimization based collision avoidance. Their method will be used in this thesis because it applies to general obstacles that can be represented as the union of convex sets. Using strong duality and convex optimization, the non-diﬀerentiable obstacle avoidance constraints are reformulated into smooth nonlinear constraints
Firstly the following definitions are introduced:
For two vectors a; b 2 Rl and a proper cone K Rl then the following holds a K b () (b a) 2 K. If K = Rl+ then K is the standard element-wise inequality . For more details see section 2.4 in .
Let k k be the dual norm of k k, and K is the dual cone of K.
The Minkowski diﬀerence of two sets A and B is defined as A B = fa bja 2 A; b 2 Bg.
Consider the space occupied by an obstacle is described by the a convex set with no relative interior Z = y 2 Ri : Cy K d (2.7) , where C 2 Rj i, d 2 Rj and K Ri is a closed convex pointed cone with non-empty interior.
The space which the controlled object occupies at time t with a state x(t) 2 Rn is denoted by S(x(t)) = R(x(t))X0 + T (x(t)) (2.8) , where R(x(t)) : Rn ! Ri i is the rotation matrix for the controlled ob-ject and T (x(t) : Rn ! Ri is the translation vector. The initial set of the controlled object, X0, is given by X0 = y 2 Ri : Ay b (2.9) where A 2 R h i , b 2 R h i is a closed convex pointed cone with and K R non-empty interior. The obstacle avoidance constraint can be formulated as S(x(t)) \ Z = ;. However, it is non-convex and non-diﬀerentiable, which cannot be included directly in the NMPC formulation. This obstacle avoidance constraint can be reformulated using the notion of signed distance which is
given by (2.10) sd(S(x); Z) = dist(S(x); Z) pen(S(x); Z) , where the distance function dist( ; ) and the penetration function pen( ; ) are defined as
dist (S( ) Z) = k k : ( S (x) + ) \ Z 6 ; (2.11)
x ; min k =
pen (S( ) Z) = k : ( S (x) + ) \ Z = ; (2.12)
x ; min
If the signed distance (2.10) is positive then the two sets S(x) and Z do not intersect. On the other hand if the two sets S(x) and Z overlap with each other then the signed distance (2.10) is negative.
The definition in (2.11) can be rewritten as (S( ) Z) = s;z fk s z k : s 2 S ; z 2 Zg
dist x ; min
Using equations (2.7), (2.8) and (2.9) the distance between the controlled set S and the obstacle set Z can be found by solving the optimization problem
min R(x)s + T (x)
subject to Cz K d
As K b
The dual of this minimization problem is given by the following (see section 8.2 in  for more details)
max bT + (CT (x) d)T
subject to AT + R(x)T CT = 0
CT 1 K 0
Assuming the two sets S(x) and Z have non-empty relative interior, strong duality holds. The condition where the distance between the two sets is greater than a minimum distance margin, , can in turn be reformulated as
dist(S(x); Z) > ()
8 bT + (CT (x) d)T > (2.13)
9 K 0; 9 0 : AT + R(x)T CT = 0
K > T
If the dual norm k k is the Euclidean distance and the dual cones K , are K standard cones or second-order cones, then smoothness is ensured if (2.13) is included in the NMPC formulation. Since the distance function does not han-dle overlapping sets, the penetration function needs to be included as shown in (2.10). This is done because if collision-free trajectories are not found by the NMPC then the optimization problem is infeasible. Including the penetration function, infeasibility can be solved by adding slack variables in the NMPC formulation where trajectories with minimum penetration can be found if a collision cannot be avoided.
Reachability analysis is often used in applications to provide safety for a dy-namical system, which may be driven to unsafe states in the presence of dis-turbances or complex environments. An example of such applications is  where the author proposes an approach for ensuring safety when a vehicle of diﬀerent levels of automation is parking. The author uses Linear Temporal Logic to formulate a safe parking task and then uses Hamilton-Jacobi reach-ability analysis to compute backward reachable sets to ensure safety for a ve-hicle with nonlinear dynamics. Another application is  where the authors present an approach to verify safety for high order models of automated vehi-cles using low order models for a planned maneuver. The authors use forward reachable sets to compute the occupancy of other traﬃc participants and check whether the vehicle intersects with them, i.e. a possible collision or not. The approach also considers many types of uncertainties for instance sensor noise, uncertain friction coeﬃcient, and uncertain initial states.
One appealing way to represent sets is using zonotopes since they can han-dle high-dimensional systems very eﬃciently. Moreover, they have impor-tant properties for instance they are closed under Minkowski addition and lin-ear maps . A zonotope, Z Rn with a center c 2 Rn and generators g(i) 2 Rn, is defined as
Z = nz 2 Rn e ig(i); i 2 [ 1; 1] o (2.16)
z = c + i=1
The order of the zonotope is defined as = ne . Zonotopes have three diﬀerent interpretations as mentioned in . Before introducing the interpretations recall the following set operations:
The Minkowski sum of two sets A Rn and B Rn is defined as A B = fa + bja 2 A; b 2 Bg
The linear transformation of the set A Rn with the transformation matrix T 2 Rp n is define as T A = fT aja 2 Ag
One interpretation is that the zonotope (2.16) is a Minkowski sum of the line segments l(i) = [ 1; 1] g(i). Figure 2.1 illustrates the construction of a two-dimensional zonotope with three generators. The zonotope (2.16) can also be reformulated as Z = c G B, given that B = [ 1; 1]e and G = [g(1); ; g(e)]. The following reformulation allows the zonotope to be interpreted as the projection of the e-dimensional unit hypercube B with the transformation matrix G, translated to the center c.
The zonotope (2.16) which is in G-Representation is usually denoted for sim-plicity as Z = (c; G). However zonotopes can also be represented in half-space representation, H-Representation, as Z = fy 2 RnjCy d; C 2 R n; d 2 Rpg. To convert a zonotope from G-Representation to H-representation the n-dimensional cross product operator is needed nX().
Consider the matrix H = [h(1); ; h(n 1)] 2 Rn n 1, where h(i) 2 Rn. Let H[i] 2 Rn n be the H matrix where the ith row is removed. The n-dimensional cross product of H is defined as nX(H) = [ ; ( 1)i+1 det H[i]; ]T (2.17)
Table of contents :
1.1 Problem formulation
2.1 Model Predictive Control
2.1.1 Nonlinear Model Predictive Control
2.1.2 Obstacle Avoidance
2.2 Reachability Analysis
2.2.2 Zonotope order reduction
2.2.3 Reachable sets
3.1 Autonomous vehicle
3.1.1 Vehicle modeling
3.1.2 Obstacle avoidance NMPC
3.2 Edge server
4.1 Implementation overview
4.2 Simulation setup
5.1 Parameters evaluation
5.1.1 Time step for the reachable set computation
5.1.2 Zonotope reduction technique
7 Future Work