Гараж

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

Задайте параметры автомобиля, оборудованного датчиком.

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 для visualizer.

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

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

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

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

где $\left(\mathit{x},\mathit{y}\right)$ обозначает положение транспортного средства и $$\psi$$ обозначает угол рыскания транспортного средства. Параметр $$b$$ представляет колесную базу транспортного средства. $$(x,y,\psi )$$ переменные состояния функций состояния транспортного средства. Скорость $\mathit{v}$ и регулирование угла $$\delta$$ контрольные переменные функций состояния транспортного средства.

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

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

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.1184 (Valid when greater than safety distance 0.1000)
3) Optimization exit flag = 1 (Successful when positive)
4) Elapsed time (s) for nlmpcmove = 38.0032
5) Final states error in x (m), y (m) and theta (deg):  0.0018, 0.0023, 0.0050
6) Final control inputs speed (m/s) and steering angle (deg): 0.0212, -0.3379

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

plotTrajectoryParkingValet(xRef,uRef)

Отследите ссылочную траекторию в модели 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)

ans = 
  Simulink.SimulationOutput:

                   tout: [125x1 double] 

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

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

Заключение

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

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