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

Этот пример использует:

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

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

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

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

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

$\begin{array}{l} x_{}^{˙} = v \cdot \cos (ψ) \\ y_{}^{˙} = v \cdot \sin (ψ) \\ ψ_{}^{˙} = \frac{v}{b} \cdot \tan (δ) \end{array}$

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

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

Скорость автомобиля, оборудованного датчиком ограничивается быть в [-6.5 6.5], m/s (приблизительно с ограничением скорости как 15 миль в час) и держащийся угол автомобиля, оборудованного датчиком ограничивается быть в [-45,45] степени.
Функция стоимости для nlmpc объект контроллера является пользовательской функцией стоимости, заданной способом, похожим на квадратичные издержки плюс отслеживания терминальная стоимость. В следующей пользовательской функции стоимости, $s (t)$ обозначает состояния автомобиля, оборудованного датчиком во время $t$ , $d$ представляет длительность симуляции. $s_{r e f}$ дан целевым положением для автомобиля, оборудованного датчиком. Матрицы $Q_{p}$ , $R_{p}$ , $Q_{t}$ , и $R_{t}$ являются постоянными.

$J = \int_{0}^{d} (s (t) - s_{r e f})^{T} Q_{p} (s (t) - s_{r e f}) + u (t)^{T} R_{p} u (t) d t + (s (d) - s_{r e f})^{T} Q_{t} (s (d) - s_{r e f}) + u (d)^{T} R_{t} u (d)$

Чтобы избежать столкновения с препятствиями, контроллер NLMPC должен удовлетворить следующим ограничениям неравенства, где минимальное расстояние до всех препятствий $d i s t_{m i n}$ должен быть больше безопасного расстояния $d i s t_{s a f e}$ . В этом примере автомобиль, оборудованный датчиком и препятствия моделируются как collisionBox объекты и расстояние от theego транспортного средства до препятствий вычисляются checkCollision функция.

$d i s t_{m i n} \geq d i s t_{s a f e}$

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

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

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

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

% 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 = 331.2143
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

Спроектируйте контроллер 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)

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

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

Гараж

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

Отследите ссылочную траекторию в модели Simulink

Заключение

Смотрите также

Функции

Блоки

Похожие темы

Документация Model Predictive Control Toolbox

Поддержка