www.gusucode.com > 自适应巡航控制示例，matlab代码，可运行 > AdaptiveCruiseControlExample.m
%% Adaptive Cruise Control System Using Model Predictive Control
% This example shows how to use a model predictive controller (MPC) to
% implement an adaptive cruise control (ACC) system.
% Copyright 2016 The MathWorks, Inc.
%% Adaptive Cruise Control System
% An ACC equipped vehicle (host car) has a sensor, such as radar, that
% measures the distance to the preceding vehicle in the same lane (lead
% car), as well as the relative velocity of the lead car. The ACC system
% operates in two modes: _speed control_ and _spacing control_.
%
% * In speed control, the host car travels at a driver-set speed.
% * In spacing control, the host car maintains a safe distance from the
% lead car.
%
% The ACC system decides which mode to use based on real-time radar
% measurements. For example, if the lead car is too close, the ACC system
% switches from speed control to spacing control. Similarly, if the lead
% car is further away, the ACC system switches from spacing control to
% speed control. In other words, the ACC system makes the host car travel
% at a driver-set speed as long as a safe distance is maintained.
%
% In this example, to achieve speed control or spacing control, the ACC
% system manipulates acceleration.
%% ACC System Design
% Open the Simulink model. The ACC system is modeled using the Adaptive
% Cruise Control System subsystem block in Simulink.
mdl = 'mpcACCsystem';
open_system(mdl)
%%
% The inputs to the ACC system are:
%
% * Driver-set velocity $V_{set}$
% * Velocity of the host car $V_{host}$
% * Actual distance to the lead car $D_{act}$ (from radar)
% * Velocity of the lead car $V_{lead}$ (from radar)
%
% The output for the ACC system is the acceleration of the host car.
%
% The dynamics between acceleration and velocity are modeled as:
%
% $$ \frac{1}{s(0.5s+1)} $$
%
% which approximates the dynamics of the throttle body and vehicle
% inertia. The same transfer function applies to both the host car and
% lead car.
%
% The safe distance between the lead car and the host car is a function
% of the velocity of the host car, $V_{host}$:
%
% $$ D_{safe} = 10 + 1.4\times V_{host} $$
%
% where |10| (m) is the standstill distance and |1.4| (sec) is the time
% gap.
%
% The following rules are used to determine the ACC system operating mode:
%
% * If $D_{act} \geq D_{safe}$, then speed control mode is active. The
% control goal is to track the driver-set velocity, $V_{set}$.
%
% * If $D_{act} < D_{safe}$, then spacing control mode is active. The
% control goal is to maintain the safe distance, $D_{safe}$.
%
% <<../mpcACCsystem.png>>
%
% Specify the initial conditions for the two vehicles.
x0_lead = 50; % initial position for lead car (m)
v0_lead = 25; % initial velocity for lead car (m/s)
x0_host = 10; % initial position for host car (m)
v0_host = 20; % initial velocity for host car (m/s)
a0_host = 0; % initial acceleration for host car (m/s^2)
%%
% Specify the target driver-set velocity in m/s.
v_set = 30;
%%
% Define the sample time, |Ts|, and simulation duration, |T|, in seconds.
Ts = 0.1;
T = 80;
%%
% To approximate a realistic driving environment, vary the acceleration of
% the lead car during the simulation.
a0_lead.time = (0:0.1:T)';
a0_lead.signals.values = 0.6*sin(a0_lead.time/5);
a0_lead.signals.dimensions = 1;
%% MPC Structure
% In this example, the ACC is implemented using a linear MPC.
ACC = [mdl '/Adaptive Cruise Control System'];
open_system(ACC)
%%
% The MPC controller has:
%
% * Two measured outputs (MO): Spacing error, $e = D_{act}-D_{safe}$, and
% host car velocity, $V_{host}$. The spacing error should have a lower
% bound of |0| to maintain the safe distance. The desired host car velocity
% is the driver-set velocity, $V_{set}$.
%
% * One manipulated variable (MV): Host car acceleration, $a$. Considering
% the physical limitations of the vehicle dynamics, the acceleration is
% constrained to be within $[-3,2]$ ($m/s^2$).
%
% * One measured disturbance (MD): Lead car velocity, $V_{lead}$,
% which allows MPC to predict its impact on the spacing error.
%%
% Given this MPC structure, define the linear prediction model by
% linearizing the Simulink model at the initial operating point. This step
% requires Simulink(R) Control Design(TM) software.
%
% Specify analysis points for linearization.
io(1) = linio([ACC '/MPC'],1,'openinput'); % MV: acceleration
io(2) = linio([mdl '/Lead Car/Sum1'],1,'openinput'); % MD: lead velocity
io(3) = linio([ACC '/Sum1'],1,'output'); % OV(1): spacing error
io(4) = linio([mdl '/Host Car'],2,'output'); % OV(2): host velocity
%%
% Linearize the plant and obtain a minimum realization of the resulting
% linear model.
sys = linearize(mdl,io);
sys = minreal(sys);
%%
% Specify the MPC signal types in the plant.
sys = setmpcsignals(sys,'MV',1,'MD',2);
%% MPC Design
% To design a model predictive controller for the plant model, first create
% an MPC controller using a default prediction horizon (|10| steps) and
% control horizon (|2| moves).
mpc1 = mpc(sys,Ts);
%%
% Specify the controller nominal values based on the simulation initial
% conditions.
mpc1.Model.Nominal.Y(1) = (x0_lead-x0_host) - (10+1.4*v0_host);
mpc1.Model.Nominal.Y(2) = v0_host;
mpc1.Model.Nominal.U(1) = a0_host;
mpc1.Model.Nominal.U(2) = v0_lead;
%%
% Specify scale factors based on the operating ranges of the variables.
mpc1.MV.ScaleFactor = 5; % range of acceleration is 2 - (-3)
mpc1.DV.ScaleFactor = 30; % assume same as host car speed limit
mpc1.OV(1).ScaleFactor = 10; % typical spacing error
mpc1.OV(2).ScaleFactor = 30; % host car ACC speed limit
%%
% Specify MV constraints.
mpc1.MV.Min = -3; % maximum deceleration
mpc1.MV.Max = 2; % maximum acceleration
%%
% Specify OV constraints. To achieve spacing control, set the minimum
% spacing error contraint to |0|.
mpc1.OV(1).Min = 0;
%%
% Specify weights. To achieve speed control, use a nonzero weight on the
% host car velocity.
mpc1.Weights.OV = [0 1];
%%
% If the spacing error constraint is active, the controller treats the
% constraint violation as a higher priority over reference tracking. In
% this case, the ACC system is in spacing control mode and maintains a
% safe distance.
%
% If the spacing error constraint is inactive, the controller performs
% reference tracking. In this case, the ACC system is in speed control mode
% and achieves the driver-set cruising velocity.
%%
% Write the MPC controller to the Simulink block.
set_param([ACC '/MPC'],'mpcobj','mpc1')
%% Simulation Analysis
% Run the simulation with the designed model predictive controller.
sim(mdl)
%%
% Plot the simulation result.
mpcACCplot(logsout)
%%
% In the first 3 seconds, to reach the driver-set velocity, the host car
% accelerates at full throttle.
%
% From 3 to 13 seconds, the lead car accelerates slowly. As a result, to
% maintain a safe distance to the lead car, the host car accelerates with a
% much slower rate.
%
% From 13 to 25 seconds, the host car maintains the driver-set velocity, as
% shown in the *Velocity* plot. However, as the lead car reduces speed, the
% spacing error starts approaching 0 after 20 seconds.
%
% From 25 to 45 seconds, the lead car slows down and then accelerates
% again. The host car maintains a safe distance from the lead car, as shown
% in the *Distance* and *Spacing error* plots, by adjusting its speed.
%
% From 45 to 56 seconds, the spacing error is above 0. Therefore, the host
% car achieves the driver-set velocity again.
%
% From 56 to 76 seconds, the deceleration/acceleration sequence from the 25
% to 25 second interval is repeated.
%%
% In this example, an adaptive cruise control system is designed using
% model predictive control. The design guarantees that the actual distance
% between the two vehicles is greater than a safe distance. If the actual
% distance is sufficiently large, then the design guarantees that the
% vehicle follows the driver-set velocity. This example demonstrates that
% the control objectives and constraints required by the ACC system can be
% achieved using MPC.
%%
bdclose(mdl)