Парковка с использованием системы предсказания нелинейной модели

В этом примере используются:

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

Гараж для парковки

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

Определите параметры эго-транспортного средства.

vdims = vehicleDimensions;
egoWheelbase = vdims.Wheelbase;
distToCenter = 0.5*egoWheelbase;

Укажите исходную позу эго-транспортного средства.

% Ego initial pose: x(m), y(m) and yaw angle (rad)
egoInitialPose = [4,12,0];

Определите целевую позу для эго-транспортного средства. В этом примере существует два возможных направления парковки. Чтобы припарковаться на север, установите parkNorth кому true. Для парковки, выходящей на юг, установить parkNorth кому false.

parkNorth = true;
if parkNorth
    egoTargetPose = [36,45,pi/2];
else
    egoTargetPose = [27.2,4.7,-pi/2];
end

helperSLCreateCostmap создает статическую карту стоянки, содержащую информацию о стационарных препятствиях, дорожной разметке и припаркованных автомобилях. Дополнительные сведения см. в примере Автоматизированная парковочная камера в Simulink (Automated Driving Toolbox).

costmap = helperSLCreateCostmap();
centerToFront = distToCenter;
centerToRear = distToCenter;
helperSLCreateUtilityBus;
costmapStruct = helperSLCreateUtilityStruct(costmap);

Визуализация парковочной среды. Использовать время выборки 0.1 для визуализатора.

Tv = 0.1;
helperSLVisualizeParkingValet(egoInitialPose, 0, costmapStruct);

Figure Automated Parking Valet contains an axes. The axes with title Parking garage contains 21 objects of type image, rectangle, line, patch, polygon.

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

Создание траектории с помощью контроллера прогнозирования нелинейной модели

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

$\begin{array}{l} _{}^{} x˙=v⋅cos (в \\ )_{}^{} y˙=v⋅sin \\ (_{}^{в}) \frac{}{} \end{array}$ ψ˙=vb⋅tan (δ)

где $(x, y$ ) обозначает положение транспортного средства, а Параметр b представляет колесную базу транспортного средства. $(x, y,$ start) - переменные состояния функций состояния транспортного средства. Скорость v и угол δ поворота являются управляемыми переменными функций состояния транспортного средства.

Траектория парковочной камеры от контроллера NLMPC для разработана на основе анализа, аналогичного примеру параллельной парковки с использованием нелинейной модели прогнозирующего управления. Конструкция контроллера реализована в createMPCForParkingValet сценарий.

Скорость эго-транспортного средства ограничена в пределах [-6,5,6,5] м/с (приблизительно с ограничением скорости 15 миль в час), а угол поворота эго-транспортного средства ограничен в пределах [-45,45] градусов.
Функция затрат для nlmpc объект контроллера представляет собой пользовательскую функцию затрат, определенную способом, аналогичным квадратной стоимости отслеживания плюс стоимость терминала. В следующей пользовательской функции затрат $s (t$ ) обозначает состояния эго-транспортного средства в момент времени t, d представляет длительность моделирования. $_{}$ sref задается целевой позой для эго-транспортного средства. Матрицы $_{}$ Qp, $_{}$ Rp, $_{}$ Qt и $_{}$ Rt являются постоянными.

$_{}^{J=∫0d} (s (t)_{-}^{} {sref}_{)} TQp (s_{(t}) -^{sref})_{} + u (t) TRpu (_{t)}^{dt}_{+} (s (d)_{-} sref)^{} {TQt}_{} (s ($ d) -sref) + u (d) TRtu (d)

Чтобы избежать столкновения с препятствиями, контроллер NLMPC должен удовлетворять следующим ограничениям неравенства, где минимальное расстояние до всех препятствий $_{distmin}$ должно быть больше безопасного расстояния $_{distsafe}$ . В этом примере эго-транспортное средство и препятствия моделируются как collisionBox Объекты (Robotics System Toolbox) и расстояние от эго-транспортного средства до препятствий вычисляются с помощью checkCollision(Панель инструментов системы робототехники).

$_{}_{distmin≥distsafe}$

Начальное предположение для пути решения задается двумя прямыми линиями. Первая линия находится от начальной позы эго-транспортного средства до средней точки, а вторая линия находится от средней точки до целевой позы эго-транспортного средства.

Выберите среднюю точку для начального приближения пути решения.

if parkNorth
    midPoint = [4,34,pi/2];
else
    midPoint = [27,12,0];
end

Сконфигурируйте параметры контроллера NLMPC. Чтобы спланировать оптимальную траекторию по всему горизонту прогнозирования, установите горизонт управления равным горизонту прогнозирования.

% Sample time
Ts = 0.1;
% Prediction horizon
p = 100;
% Control horizon
c = 100;
% Weight matrices for terminal cost
Qt = 0.5*diag([10 5 20]); 
Rt = 0.1*diag([1 2]);
% Weight matrices for tracking cost
if parkNorth
    Qp = 1e-6*diag([2 2 0]);
    Rp = 1e-4*diag([1 15]);
else
    Qp = 0*diag([2 2 0]);
    Rp = 1e-2*diag([1 5]);
end
% Safety distance to obstacles (m)
safetyDistance = 0.1;
% Maximum iteration number
maxIter = 70;
% Disable message display
mpcverbosity('off');

Создайте контроллер NLMPC, используя указанные параметры.

[nlobj,opt,paras] = createMPCForParkingValet(p,c,Ts,egoInitialPose,egoTargetPose,...
    maxIter,Qp,Rp,Qt,Rt,distToCenter,safetyDistance,midPoint);

Задайте начальные условия для эго-транспортного средства.

x0 = egoInitialPose';
u0 = [0;0];

Создайте опорную траекторию с помощью nlmpcmove функция.

tic;
[mv,nloptions,info] = nlmpcmove(nlobj,x0,u0,[],[],opt);
timeVal = toc;

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

xRef = info.Xopt;
uRef = info.MVopt;

Проанализируйте запланированную траекторию.

analyzeParkingValetResults(nlobj,info,egoTargetPose,Qp,Rp,Qt,Rt,...
    distToCenter,safetyDistance,timeVal)

Summary of results:
1) Valid results. No collisions.
2) Minimum distance to obstacles = 0.1200 (Valid when greater than safety distance 0.1000)
3) Optimization exit flag = 1 (Successful when positive)
4) Elapsed time (s) for nlmpcmove = 158.6983
5) Final states error in x (m), y (m) and theta (deg):  0.0013, 0.0024, -0.0980
6) Final control inputs speed (m/s) and steering angle (deg): 0.0161, -0.6212

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

plotTrajectoryParkingValet(xRef,uRef)

Figure contains 3 axes. Axes 1 contains an object of type line. Axes 2 contains an object of type line. Axes 3 contains an object of type line.

Figure contains 2 axes. Axes 1 contains an object of type stair. Axes 2 contains an object of type stair.

Отслеживание опорной траектории в модели Simulink

Спроектируйте контроллер NLMPC для отслеживания опорной траектории.

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

Duration = 12;
Tsteps = Duration/Ts;
Xref = [xRef(2:p+1,:);repmat(xRef(end,:),Tsteps-p,1)];

Создайте контроллер NLMPC с горизонтом прогнозирования отслеживания (pTrackingиз 10.

pTracking = 10;
nlobjTracking = createMPCForTrackingParkingValet(pTracking,Xref);

Откройте модель Simulink.

mdl = 'mpcAutoParkingValet';
open_system(mdl)

Перед запуском моделирования закройте графики анимации.

f = findobj('Name','Automated Parking Valet');
close(f)

Моделирование модели.

sim(mdl)

Figure Automated Parking Valet contains an axes. The axes with title Parking garage contains 141 objects of type image, rectangle, line, patch, polygon.

ans = 
  Simulink.SimulationOutput:

                   tout: [125x1 double] 

     SimulationMetadata: [1x1 Simulink.SimulationMetadata] 
           ErrorMessage: [0x0 char]

Анимация показывает, что эго транспортное средство паркуется в целевой позе успешно без каких-либо столкновений препятствий. Также можно просматривать ego-транспортное средство и траектории размещения с помощью областей «Ego Vehicle Pose» и «Control».

Заключение

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

mpcverbosity('on');
bdclose(mdl)
f = findobj('Name','Automated Parking Valet');
close(f)

См. также

Функции

nlmpc | nlmpcmove

Блоки

Нелинейный контроллер MPC

Связанные темы

Автоматизированное вождение с использованием модели прогнозирующего управления

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