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

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

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

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

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

Существует две манипулируемые переменные:

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

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

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

Предположим, что все состояния измеримы. В номинальной рабочей точке эго-автомобиль движется на восток с постоянной скоростью 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;

Препятствием в этом примере является неподвижный объект в середине центральной полосы с тем же размером, что и ego-автомобиль.

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);

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

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

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)