Парковка Гараж

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

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

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

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

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

Визуализация окружения парковки. Использование шага расчета 0.1 для визуализатора.

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

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

Разработайте контроллер для отслеживания эталонной траектории.

Во-первых, установите длительность симуляции и обновите траекторию ссылки на основе длительности.

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] 

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

Заключение

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

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