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

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

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

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

Автомобиль эго имеет прямоугольную форму длиной 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;

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

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

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

obstacle.DetectionDistance = 30;

Постройте график в номинальном состоянии:

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

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

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

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