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

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

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

Модель транспортного средства

Автомобиль эго имеет прямоугольную форму с длиной 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 (м/с^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 и theta) обнулять. Выполнение так позволяет значениям этих других выходных параметров плавание.

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 вектор выходной переменной. Можно обновить ограничительные матрицы EF, и 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)