Parallel Parking of Truck-Trailer Using Multistage Nonlinear MPC

Parking Environment

The parking environment contains a truck-trailer system (ego vehicle) and static obstacles. The goal of the ego vehicle is to park at a target pose without colliding with the obstacles. The reference point of the ego vehicle pose is located at the center of the rear axle. The ego vehicle dynamics and parameters match the parameters in the Truck and Trailer Automatic Parking Using Multistage Nonlinear MPC example.

Load the parameters of the ego vehicle.

load truckDimensions.mat

Specify the initial ego vehicle pose and target parking pose.

% Ego initial pose: x(m), y(m), trailer yaw angle (rad), 
% and yaw angle error between truck and trailer
initialPose1 = [-15,-30,pi/2,0]';
targetPose2 = [-8,-8,pi/2,0]';

Visualize the parking environment with steering angle equal to zero using the helperSLVisualizeParkingTruck helper function.

helperSLVisualizeParkingTruck(initialPose1,0,truckDimensions);

Simulink Model

Open the Simulink® model.

mdl = 'truckParallelParking';
open_system(mdl)

The model contains four major components.

Open the path planner system.

open_system([mdl '/Planning/Path Planner'])

The path planner is designed using multistage NLMPC. The path planner requires the following information.

The path generated from the path planner is transformed to a trajectory for the parking controller. The parking controller is designed using NLMPC.

Path Planner and Trajectory-Tracking Controller

In this example, the obstacle information is represented by two parameters: wall length and wall width.

wallLength = 20;
wallWidth = 10;

The path planner takes obstacle information into account and generates a collision-free path for the ego vehicle. For more details on how to use multistage NLMPC for planning, see Truck and Trailer Automatic Parking Using Multistage Nonlinear MPC.

To create the path-planning NLMPC controller, use the mpcGeneratePlanner helper function.

planner = mpcGeneratePlanner(truckDimensions,wallLength,wallWidth);

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] 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] 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] are OK.
Analysis of user-provided model, cost, and constraint functions complete.

planningObj = planner.mpcobj; % MPC object for planning
pPlanning = planner.horizon;  % Prediction horizon
Tsplan = planner.sampleTime;  % Sample time

Trajectory-Tracking Controller

The goal for the trajectory-tracking controller is to closely track the trajectory from the path planner and guide the ego vehicle to the target parking location. For more details on how to use NLMPC for parking of a sedan vehicle, see Parallel Parking Using Nonlinear Model Predictive Control.

To create the trajectory-tracking controller, use the mpcGenerateTracker helper function.

controller = mpcGenerateTracker(truckDimensions);

Zero weights are applied to one or more OVs because there are fewer MVs than OVs.
Model.StateFcn is OK.
Jacobian.StateFcn is OK.
No output function specified. Assuming "y = x" in the prediction model.
Analysis of user-provided model, cost, and constraint functions complete.

trackingObj = controller.mpcobj; % MPC object for tracking
pTracking = controller.horizon;  % Prediction horizon
Ts = controller.sampleTime;      % Sample time

Simulate Model

To run the Simulink model, configure the following parameters.

tmin = 2.5;           % Start time for planning
tmax = tmin + Tsplan; % End time for planning (at least one sample time)
Duration = 30;        % Simulation time
mdlParas = [truckDimensions.M1;truckDimensions.L1;truckDimensions.L2;...
            truckDimensions.W1;truckDimensions.W2]; % Truck information
obsParas = [wallLength;wallWidth];                  % Obstacle information
numParas = numel(mdlParas) + numel(obsParas);

To pass the ego vehicle parameters to the tracking controller, you must create a parameter bus object.

parasTracking = {mdlParas(1:3)};
blk = [mdl '/Parking/tracking/Nonlinear MPC Controller'];
createParameterBus(trackingObj,blk,'parasBusObject',parasTracking);

Simulink Bus object "parasBusObject" created in the MATLAB Workspace.
Bus Creator block "truckParallelParking/Parking/tracking/Nonlinear MPC Controller" is configured to use it.

Simulate the model.

sim(mdl);

The animation shows that the ego vehicle parks at the target pose successfully without any obstacle collisions. You can also view the ego vehicle and pose trajectories using the scopes in the Parking Visualizer subsystem.