Документация

Предотвращение препятствия

Автомобиль с предотвращением препятствия (или передающая помощь) имеет датчик, такой как лазерный дальномер, который измеряет расстояние до препятствия перед автомобилем и в том же маршруте. Препятствие может быть статичным, таким как большая дыра горшка или перемещение, таким как медленный автомобиль. Наиболее распространенный маневр от драйвера должен временно переместиться в другой маршрут, проехать мимо препятствия и попятиться к исходному маршруту.

Как часть автономного водительского стажа, система предотвращения препятствия может выполнить маневр без человеческого вмешательства. В этом примере вы разрабатываете систему предотвращения препятствия, которая перемещает автомобиль эго вокруг статического препятствия в маршруте с помощью дросселя и регулируя угол. Эта система использует адаптивную модель прогнозирующий контроллер, который обновляет и прогнозирующую модель и смешанные ограничения ввода/вывода в каждом интервале управления.

Модель автомобиля

Автомобиль эго имеет прямоугольную форму с длиной 5 метров и шириной 2 метров. Модель имеет четыре состояния:

Существует две переменные, которыми управляют:

Используйте простую нелинейную модель, чтобы описать динамику автомобиля эго:

Аналитические Якобианы нелинейной модели в пространстве состояний используются, чтобы создать модель линейного предсказания в номинальной рабочей точке.

где автомобильная длина.

Примите, что все состояния измеримы. В номинальной рабочей точке автомобиль эго управляет востоком на постоянной скорости метров 20 в секунду.

V = 20;
x0 = [0; 0; 0; V];
u0 = [0; 0];

Дискретизируйте непрерывно-разовую модель с помощью метода держателя нулевого порядка в функции obstacleVehicleModelDT.

Ts = 0.02;
[Ad,Bd,Cd,Dd,U,Y,X,DX] = obstacleVehicleModelDT(Ts,x0,u0);
dsys = ss(Ad,Bd,Cd,Dd,'Ts',Ts);
dsys.InputName = {'Throttle','Delta'};
dsys.StateName = {'X','Y','Theta','V'};
dsys.OutputName = dsys.StateName;

Дорога и информация о препятствии

В этом примере примите что:

lanes = 3;
laneWidth = 4;

Препятствие в этом примере является недвижущимся объектом посреди центрального маршрута с тем же размером как автомобиль эго.

obstacle = struct;
obstacle.Length = 5;
obstacle.Width = 2;

Поместите препятствие метры 50 в будущем.

obstacle.X = 50;
obstacle.Y = 0;

Создайте виртуальную безопасную зону вокруг препятствия так, чтобы автомобиль эго не становился слишком близким к препятствию при передаче его. Безопасная зона сосредоточена на препятствии и имеет a:

obstacle.safeDistanceX = obstacle.Length;
obstacle.safeDistanceY = laneWidth;
obstacle = obstacleGenerateObstacleGeometryInfo(obstacle);

В этом примере примите, что устройство лазерного дальномера может обнаружить препятствие метры 30 перед автомобилем.

obstacle.DetectionDistance = 30;

Постройте следующее при номинальном условии:

f = obstaclePlotInitialCondition(x0,obstacle,laneWidth,lanes);

Проект MPC в номинальной рабочей точке

Разработайте образцовый прогнозирующий контроллер, который может заставить автомобиль эго поддержать желаемую скорость и остаться посреди центрального маршрута.

status = mpcverbosity('off');
mpcobj = mpc(dsys);

Горизонт прогноза является шагами 25, который эквивалентен 0,5 секундам.

mpcobj.PredictionHorizon = 60;%25;
mpcobj.ControlHorizon = 2;%5;

Чтобы препятствовать тому, чтобы автомобиль эго ускорился или замедлился слишком быстро, добавьте трудное ограничение 0,2 (m/s^2) на скорости изменения дросселя.

mpcobj.ManipulatedVariables(1).RateMin = -0.2*Ts;
mpcobj.ManipulatedVariables(1).RateMax = 0.2*Ts;

Точно так же добавьте трудное ограничение 6 градусов в секунду на держащейся угловой скорости изменения.

mpcobj.ManipulatedVariables(2).RateMin = -pi/30*Ts;
mpcobj.ManipulatedVariables(2).RateMax = pi/30*Ts;

Масштабируйте дроссель и держащийся угол их соответствующими рабочими диапазонами.

mpcobj.ManipulatedVariables(1).ScaleFactor = 2;
mpcobj.ManipulatedVariables(2).ScaleFactor = 0.2;

С тех пор существует только две переменные, которыми управляют, чтобы достигнуть нулевого установившегося смещения, можно выбрать только два выходных параметров для совершенного отслеживания. В этом примере выберите положение Y и скорость, установив веса других двух выходных параметров (X и тета) обнулять. Выполнение так позволяет значениям этих других выходных параметров плавание.

mpcobj.Weights.OutputVariables = [0 30 0 1];

Обновите контроллер с номинальными условиями работы. Для объекта дискретного времени:

mpcobj.Model.Nominal = struct('U',U,'Y',Y,'X',X,'DX',DX);

Задайте Смешанные Ограничения ввода-вывода для Маневра Предотвращения Препятствия

Существуют различные стратегии заставить автомобиль эго избежать препятствия на дороге. Например, планировщик пути в реальном времени может вычислить новый путь после того, как препятствие обнаруживается, и контроллер следует за этим путем.

В этом примере используйте другой подход, который использует в своих интересах способность MPC обработать ограничения явным образом. Когда препятствие обнаруживается, оно задает область на дороге (с точки зрения ограничений), который автомобиль эго не должен вводить во время горизонта прогноза. В следующем интервале управления область переопределена на основе новых положений автомобиля эго и препятствия, пока передача не завершается.

Чтобы задать область, чтобы избежать, используйте следующие смешанные ограничения ввода/вывода:

E*u + F*y <= G

где u является переменным вектором, которым управляют, и y является вектором выходной переменной. Можно обновить ограничительные матрицы E, F и G, когда контроллер запускается.

Первое ограничение является верхней границей на (на этой трехполосной дороге).

E1 = [0 0];
F1 = [0 1 0 0];
G1 = laneWidth*lanes/2;

Второе ограничение является нижней границей на (на этой трехполосной дороге).

E2 = [0 0];
F2 = [0 -1 0 0];
G2 = laneWidth*lanes/2;

Третье ограничение для предотвращения препятствия. Даже при том, что никакое препятствие не обнаруживается в номинальных условиях работы, необходимо добавить "поддельное" ограничение здесь, потому что вы не можете изменить размерности ограничительных матриц во время выполнения. Для поддельного ограничения используйте ограничение с той же формой как второе ограничение.

E3 = [0 0];
F3 = [0 -1 0 0];
G3 = laneWidth*lanes/2;

Задайте смешанные ограничения ввода/вывода в контроллере, использующем функцию setconstraint.

setconstraint(mpcobj,[E1;E2;E3],[F1;F2;F3],[G1;G2;G3],[1;1;0.1]);

Моделируйте контроллер

В этом примере вы используете адаптивный контроллер MPC, потому что он обрабатывает нелинейную динамику аппарата эффективнее, чем традиционный контроллер MPC. Традиционный диспетчер MPC использует постоянную модель объекта управления. Однако адаптивный MPC позволяет вам обеспечивать новую модель объекта управления в каждом интервале управления. Поскольку новая модель описывает динамику объекта более точно в новых условиях работы, адаптивный контроллер MPC выполняет лучше, чем традиционный контроллер MPC.

Кроме того, чтобы позволить контроллеру избежать безопасной зоны, окружающей препятствие, вы обновляете третье смешанное ограничение в каждом интервале управления. В основном автомобиль эго должен быть выше строки, сформированной от автомобиля эго до левого верхнего угла безопасной зоны. Для получения дополнительной информации, открытый obstacleComputeCustomConstraint.

Используйте постоянный ссылочный сигнал.

refSignal = [0 0 0 V];

Инициализируйте состояния контроллера и объект.

x = x0;
u = u0;
egoStates = mpcstate(mpcobj);

Время симуляции является секундами 4.

T = 0:Ts:4;

Регистрируйте данные моделирования для графического вывода.

saveSlope = zeros(length(T),1);
saveIntercept = zeros(length(T),1);
ympc = zeros(length(T),size(Cd,1));
umpc = zeros(length(T),size(Bd,2));

Запустите симуляцию.

for k = 1:length(T)
    % Obtain new plant model and output measurements for interval |k|.
    [Ad,Bd,Cd,Dd,U,Y,X,DX] = obstacleVehicleModelDT(Ts,x,u);
    measurements = Cd * x + Dd * u;
    ympc(k,:) = measurements';

    % Determine whether the vehicle sees the obstacle, and update the mixed
    % I/O constraints when obstacle is detected.
    detection = obstacleDetect(x,obstacle,laneWidth);
    [E,F,G,saveSlope(k),saveIntercept(k)] = ...
        obstacleComputeCustomConstraint(x,detection,obstacle,laneWidth,lanes);

    % Prepare new plant model and nominal conditions for adaptive MPC.
    newPlant = ss(Ad,Bd,Cd,Dd,'Ts',Ts);
    newNominal = struct('U',U,'Y',Y,'X',X,'DX',DX);

    % Prepare new mixed I/O constraints.
    options = mpcmoveopt;
    options.CustomConstraint = struct('E',E,'F',F,'G',G);

    % Compute optimal moves using the updated plant, nominal conditions,
    % and constraints.
    [u,Info] = mpcmoveAdaptive(mpcobj,egoStates,newPlant,newNominal,...
        measurements,refSignal,[],options);
    umpc(k,:) = u';

    % Update the plant state for the next iteration |k+1|.
    x = Ad * x + Bd * u;
end

mpcverbosity(status);

Анализ результатов

Постройте траекторию автомобиля эго (черная линия), и третьи смешанные ограничения ввода-вывода (подчеркнул штриховой линией зеленые строки) во время маневра предотвращения препятствия.

figure(f)
for k = 1:length(saveSlope)
    X = [0;50;100];
    Y = saveSlope(k)*X + saveIntercept(k);
    line(X,Y,'LineStyle','--','Color','g' )
end
plot(ympc(:,1),ympc(:,2),'-k');
axis([0 ympc(end,1) -laneWidth*lanes/2 laneWidth*lanes/2]) % reset axis

Контроллер MPC успешно выполняет задачу без человеческого вмешательства.

Моделируйте контроллер в Simulink

Откройте модель Simulink. Система предотвращения препятствия содержит несколько компонентов:

mdl = 'mpc_ObstacleAvoidance';
open_system(mdl)
sim(mdl)

Результат симуляции идентичен результату командной строки. Чтобы поддержать рабочий процесс быстрого прототипирования, можно сгенерировать код C/C++ для блоков в системе предотвращения препятствия.

bdclose(mdl)