# Truck and Trailer Automatic Parking Using Multistage Nonlinear MPC

This example shows how to use multistage nonlinear model predictive control (MPC) as a path planner to find the optimal trajectory to automatically park a truck and trailer system in the presence of static obstacles.

### Overview

An MPC controller has an internal model that can be used to predict plant behavior. Given the current states of the plant, based on the prediction model, MPC finds an optimal control sequence that minimizes cost and satisfies the constraints specified across the prediction horizon. Because MPC finds the plant future trajectory at the same time, it can work as a powerful tool to solve trajectory optimization problems, such as autonomous parking of a vehicle and motion planning of a robot arm.

In such trajectory optimization problems the plant, cost function, and constraints can often be nonlinear. Therefore, you need to use nonlinear MPC controller for problem formulation and solution. In this example, you design a nonlinear MPC controller that finds an optimal route to automatically park a truck with a single trailer from its initial position to its target position, which is between two static obstacles. The generated path can then be passed to low-level controllers as a reference signal, so that it can execute the parking maneuver in real time.

This example requires Optimization Toolbox and Robotics System Toolbox software.

### Truck and Single Trailer System

The following figure shows the truck and trailer nonlinear dynamic system. The states for this model are:

1. `x`(center of the trailer rear axle, global x position)

2. `y` (center of the trailer rear axle, global y position)

3. `theta` (trailer orientation, global angle, 0 = east)

4. `beta` (truck orientation with respect to trailer, 0 = aligned)

The inputs for this model are:

1. `alpha` (truck steering angle)

2. `v` (truck longitudinal velocity)

For this model, length and position are in meters, velocity is in m/s, and angles are in radians.

Defien the following model parameters.

• `M` (hitch length)

• `L1` (truck length)

• `W1` (truck width)

• `L2` (trailer length)

• `W2` (trailer width)

• `Lwheel` (wheel diameter)

• `Wwheel` (wheel width)

```params = struct('M', 1, ... 'L1',6,... 'W1',2.5,... 'L2',10,... 'W2',2.5,... 'Lwheel',1,... 'Wwheel',0.4); ```

The nonlinear model is implemented using the `TruckTrailerStateFcn` function and its analytical Jacobian is manually derived in the `TruckTrailerStateJacobianFcn` function to speed up optimization.

In this example, since MPC is used as a path planner instead of a low-level path-following controller, `velocity` is used as one of the manipulated variables instead of `acceleration`.

### Automatic Parking Problem

The parking lot is 100 meters wide and 60 meters long. The goal is to find a viable path that brings the truck and trailer system from any initial position to the target position (the green cross in the following figure) in 20 seconds using reverse parking. In the process, the planned path must avoid collisions with two obstacles next to the parking spot.

```initialPose = [0;0;0;0]; targetPose = [0;-25;pi/2;0]; TruckTrailerPlot(initialPose,targetPose,params); ``` Initial plant inputs (steering angle and longitudinal velocity) are 0.

```u0 = zeros(2,1); ```

The initial position must be valid. Use the inequality constraint function to check that this is so. Details about this function are discussed later in the example. Collision detection is carried out by specific Robotics System Toolbox functions.

```cineq = TruckTrailerIneqConFcn(1,initialPose,u0,... [params.M;params.L1;params.L2;params.W1;params.W2]); if any(cineq>0) fprintf('Initial pose is not valid.\n'); return end ```

### Path Planner Using Multistage Nonlinear MPC

Compared with the generic nonlinear MPC controller (`nlmpc` object), multistage nonlinear MPC provides you with a more flexible and efficient way to implement MPC with staged cost and constraints. This flexibility and efficiency is especially useful for trajectory planning.

A multistage nonlinear MPC controller with prediction horizon `p` defines `p+1` stages, representing time `k` (current time), `k+1`, ..., `k+p`. For each stage, you can specify stage-specific cost, inequality constraint, and equality constraint functions. These functions depend only on the plant state and input values at that stage. Given the current plant states `x[k]`, MPC finds the manupulated variable (MV) trajectory (from time `k` to `k+p-1` to optimize the summed stage costs (from time `k` to `k+p`), satisfying all the stage constraints (from time `k` to `k+p`).

In this example, the plant has four states and two inputs (both MVs). The prediction horizon `p` and sample time `Ts` are chosen such that p*Ts = 20.

Create the multistage nonlinear MPC controller.

```p = 40; nlobj = nlmpcMultistage(p,4,2); nlobj.Ts = 0.5; ```

Specify the prediction model and its analytical Jacobian in the controller object. Since the model requires three parameters (`M`, `L1`, and `L2`), set `Model.ParameterLength` to `3`.

```nlobj.Model.StateFcn = "TruckTrailerStateFcn"; nlobj.Model.StateJacFcn = "TruckTrailerStateJacobianFcn"; nlobj.Model.ParameterLength = 3; ```

Specify hard bounds on the two manipulated variables. The steering angle must remain in the range +/- 45 degrees. The maximum forward speed is 10 m/s and the maximum reverse speed is -10 m/s.

```nlobj.MV(1).Min = -pi/4; % Minimum steering angle nlobj.MV(1).Max = pi/4; % Maximum steering angle nlobj.MV(2).Min = -10; % Minimum velocity (reverse) nlobj.MV(2).Max = 10; % Maximum velocity (forward) ```

Specify hard bounds on the fourth state, which is the angle between truck and trailer. It cannot go beyond +/-|90| degrees due do mechanics limitations.

```nlobj.States(4).Min = -pi/2; nlobj.States(4).Max = pi/2; ```

There are different ways to define the cost terms. For example, you might want to minimize time, fuel consumption, or parking speed. For this exmaple, minimize parking speed in the quadratic format to promote safety.

Since MVs are only valid from stage 1 to stage `p`, you do notneed to define any stage cost for the last stage, `p+1`. Five model settings `M`, `L1`, `L2`, `W1`, and `W` are stage parameters for stages 1 to `p` and are used by the inequality constraint functions.

```for ct=1:p nlobj.Stages(ct).CostFcn = "TruckTrailerCost"; nlobj.Stages(ct).CostJacFcn = "TruckTrailerCostGradientFcn"; nlobj.Stages(ct).ParameterLength = 5; end ```

You can use inequality constraints to avoid collision during the parking process. In the `TruckTrailerIneqConFcn` function, check whether truck and trailer at a specific stage collide with any of the two static obstacles. When either the truck or trailer gets within the 1 m safety zone around the obstacles, a collision is detected.

In theory, you should check such collisions for all the prediction steps (from stage 2 to stage `p+1`). In this example, however, you only need to check stages from 2 to `p` because the last stage is already taken care of by the equality constraint function.

For this example, do not provide an analytical Jacobian for the inequality constraint functions because it is too complicated to derive manually. Therefore, the controller uses a finite-difference method (numerical perturbation) to estimate the Jacobian at run time.

```for ct=2:p nlobj.Stages(ct).IneqConFcn = "TruckTrailerIneqConFcn"; end ```

Use terminal state for the last stage to ensure successful parking at the target position. In this example, the target position is provided as a run-time signal. Here we use a dummy finite value to let MPC know which states will have terminal values at run time.

```nlobj.Model.TerminalState = zeros(4,1); ```

At the end of multistage nonlinear MPC design, you can use the `validateFcns` command with random initial plant states and inputs to check whether there is any problem with user-defined state, cost, and constraint functions as well as their analytical Jacobian functions.

Any state function and stage parameters, if defined, must be provided to to the controller at run time. `StageParameter` contains all the stage parameters stacked into a single vector. We also use `TerminalState` to specify terminal state at run time.

```simdata = getSimulationData(nlobj,'TerminalState'); simdata.StateFcnParameter = [params.M;params.L1;params.L2]; simdata.StageParameter = repmat([params.M;params.L1;params.L2;params.W1;params.W2],p,1); simdata.TerminalState = targetPose; validateFcns(nlobj,[2;3;0.5;0.4],[0.1;0.2],simdata); ```
```Model.StateFcn is OK. Model.StateJacFcn is OK. "CostFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40] are OK. "CostJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40] are OK. "IneqConFcn" of the following stages [2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40] are OK. Analysis of user-provided model, cost, and constraint functions complete. ```

Since the default nonlinear programming solver `fmincon` searches for a local minimum, it is critical for you to provide a good initial guess for the decision variables, especially for trajectory optimization problems that usually involve a complicated (likely nonconvex) solution space.

In this example, there are 244 decision variables: The plant states and inputs (6 in total) for each of the first p (40) stages, and plant states (4) for the last stage `p+1`. The `TruckTrailerInitialGuess` function uses simple heuristics to generate the initial guess. For example, if the truck and trailer is initially positioned above the obstacles, one waypoint is used, otherwise, two waypoints are used (a waypoint is an intermediate point on a route of travel at which course is changed). The initial guess is displayed as dots in the following animation plot.

```[simdata.InitialGuess, XY0] = TruckTrailerInitialGuess(initialPose,targetPose,u0,p); ```

### Trajectory Planning and Simulation Result

Use the `nlmpcmove` function to find the optimal parking path, which typically takes ten to twenty seconds, depending on the initial position.

```fprintf('Automated Parking Planner is runing...\n'); tic;[~,~,info] = nlmpcmove(nlobj,initialPose,u0,simdata);t=toc; fprintf('Calculation Time = %s; Objective cost = %s; ExitFlag = %s; Iterations = %s\n',... num2str(t),num2str(info.Cost),num2str(info.ExitFlag),num2str(info.Iterations)); ```
```Automated Parking Planner is runing... Calculation Time = 9.4315; Objective cost = 251.8455; ExitFlag = 1; Iterations = 47 ```

Two plots are generated. One is the animation of the parking process, where blue circles indicate the optimal path and the initial guess is shown as a dot. The other displays the optimal trajectory of plant states and control moves.

```TruckTrailerPlot(initialPose, targetPose, params, info, XY0); ```  Here are a few screenshots of the optimal trajectories that MPC found for different initial positions.      You can try other initial X-Y positions in the Automatic Parking Problem section, as long as the positions are valid.

If `ExitFlag` is negative, it means that the nonlinear MPC controller failed to find an optimal solution and you cannot trust the returned trajectory. In that case, you might need to provide a better initial guess and specify it in `simdata.InitialGuess` before calling `nlmpcmove`.