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

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

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

Парковочная среда

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

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

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

Эго-транспортное средство начинается в следующей начальной позе.

Положение X 7 m
Положение Y 3.1 m
Угол рыскания 0 радиус

egoInitialPose = [7,3.1,0];

Для парковки центра эго-транспортного средства в целевом месте (X = 0, Y = 0) используйте следующую целевую позу, определяющую местоположение опорной точки заднего моста.

Положение X, равное половине длины колесной базы в отрицательном направлении X
Положение Y 0 m
Угол рыскания 0 радиус

egoTargetPose = [-distToCenter,0,0];

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

Tv = 0.1;
helperSLVisualizeParking(egoInitialPose,0);

Figure Automated Parallel Parking contains an axes. The axes with title Parallel Parking contains 20 objects of type rectangle, line, patch, polygon.

В визуализации четыре припаркованных автомобиля - оранжевые коробки посередине. Нижняя оранжевая граница - бордюр дороги, а верхняя оранжевая граница - жёлтая линия дороги.

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

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

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

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

Расчетный контроллер прогнозирования нелинейной модели

Контроллер прогнозирования нелинейной модели для парковки разработан на основе следующего анализа.

Выходной сигнал функции состояния транспортного средства совпадает с состоянием транспортного средства $(x, y,$ start). Поэтому объект контроллера NLMPC создается с тремя состояниями, тремя выходами и двумя управляемыми переменными.
Скорость эго-транспортного средства ограничена в диапазоне от -2 до 2 м/с, а угол поворота эго-транспортного средства ограничен в диапазоне от -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}$

Для повышения эффективности моделирования якобинцы функции состояния, функции затрат и ограничений неравенства предоставляются контроллеру NLMPC. Якобинцы ограничений неравенства аппроксимированы на основе [1].
Начальные догадки для решений состояний определяются прямыми линиями между начальной и целевой позициями эго-транспортного средства.

Укажите время выборки (Ts), горизонт прогнозирования (p) и горизонт управления (m) для нелинейного контроллера MPC.

Ts = 0.1;
p = 70;
c = 70;

Укажите матрицы постоянного веса для контроллера. Определите обе матрицы веса отслеживания (Qp и Rp) и матриц веса терминала (Qt и Rt).

Qp = diag([0.1 0.1 0]);
Rp = 0.01*eye(2);
Qt = diag([1 5 100]); 
Rt = 0.1*eye(2);

Укажите безопасное расстояние 0.1 m, который контроллер использует при определении своих ограничений.

safetyDistance = 0.1;

Укажите максимальное количество итераций для решателя NLMPC.

maxIter = 40;

Создайте нелинейный контроллер MPC. Для ясности сначала отключите сообщения командного окна MPC.

mpcverbosity('off');

Создать nlmpc объект контроллера с тремя состояниями, тремя выходами и двумя входами.

nx = 3;
ny = 3;
nu = 2;
nlobj = nlmpc(nx,ny,nu);

Укажите время выборки (Ts), горизонт прогнозирования (PredictionHorizon) и горизонт управления (ControlHorizon) для контроллера.

nlobj.Ts = Ts;
nlobj.PredictionHorizon = p;
nlobj.ControlHorizon = c;

Определите ограничения для управляемых переменных. Здесь, MV(1) - скорость эго-транспортного средства в м/с, и MV(2) - угол поворота в радианах.

nlobj.MV(1).Min = -2;
nlobj.MV(1).Max = 2;
nlobj.MV(2).Min = -pi/4;
nlobj.MV(2).Max = pi/4;

Укажите функцию состояния контроллера и функцию состояния Jacobian.

nlobj.Model.StateFcn = "parkingVehicleStateFcn";
nlobj.Jacobian.StateFcn = "parkingVehicleStateJacobianFcn";

Укажите функцию затрат контроллера и функцию затрат Jacobian.

nlobj.Optimization.CustomCostFcn = "parkingCostFcn";
nlobj.Optimization.ReplaceStandardCost = true;
nlobj.Jacobian.CustomCostFcn = "parkingCostJacobian";

Определите пользовательские ограничения неравенства для контроллера и ограничения Якобиана. Функция пользовательских ограничений вычисляет расстояние от эго-транспортного средства до всех препятствий в среде и сравнивает эти расстояния с безопасным расстоянием.

nlobj.Optimization.CustomIneqConFcn = "parkingIneqConFcn";
nlobj.Jacobian.CustomIneqConFcn = "parkingIneqConFcnJacobian";

Настройте решатель оптимизации контроллера.

nlobj.Optimization.SolverOptions.FunctionTolerance = 0.01;
nlobj.Optimization.SolverOptions.StepTolerance = 0.01;
nlobj.Optimization.SolverOptions.ConstraintTolerance = 0.01;
nlobj.Optimization.SolverOptions.OptimalityTolerance = 0.01;
nlobj.Optimization.SolverOptions.MaxIter = maxIter;

Определите начальное предположение для оптимального решения состояния. Эта начальная догадка - прямая линия от начальной позы до целевой позы. Также укажите значения параметров эго-транспортного средства в nlmpcmoveopt объект.

opt = nlmpcmoveopt;
opt.X0 = [linspace(egoInitialPose(1),egoTargetPose(1),p)', ...
          linspace(egoInitialPose(2),egoInitialPose(2),p)'...
          zeros(p,1)];
opt.MV0 = zeros(p,nu);

Вычисление функции затрат и ограничений неравенства, наряду с их якобинцами, требует передачи параметров пользовательским функциям. Определите вектор параметров и укажите количество параметров. Также задайте значения параметров в nlmpcmoveopt объект.

paras = {egoTargetPose,Qp,Rp,Qt,Rt,distToCenter,safetyDistance}';
nlobj.Model.NumberOfParameters = numel(paras);
opt.Parameters = paras;

Моделирование контроллера в MATLAB

Для моделирования контроллера NLMPC в MATLAB ® можно использовать одну из следующих опций:

Моделирование контроллера с помощью nlmpcmove функция.
Создайте файл MEX для контроллера с помощью buildMEX функция. Оценка этого файла MEX повышает эффективность моделирования по сравнению с nlmpcmove.

Моделирование контроллера NLMPC для парковки с помощью runParkingAndPlot сценарий. Для этого моделирования не создавайте файл MEX (набор useMEX кому 0).

useMex = 0; 
runParkingAndPlot

Figure Automated Parallel Parking contains an axes. The axes with title Parallel Parking contains 91 objects of type rectangle, line, patch, polygon.

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.

Summary of results:
1) Valid results. No collisions.
2) Minimum distance to obstacles = 0.1797 (Valid when greater than safety distance 0.1000)
3) Optimization exit flag = 1 (Successful when positive)
4) Elapsed time (s) for nlmpcmove = 19.5123
5) Final states error in x (m), y (m) and theta (deg):  -0.0031, 0.0310, 0.1601
6) Final control inputs speed (m/s) and steering angle (deg): -0.0034, -0.4517

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

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

useMex = 1;
runParkingAndPlot

Generating MEX function "parkingMex" from nonlinear MPC to speed up simulation.
Code generation successful.

MEX function "parkingMex" successfully generated.

Figure Automated Parallel Parking contains an axes. The axes with title Parallel Parking contains 162 objects of type rectangle, line, patch, polygon.

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.

Summary of results:
1) Valid results. No collisions.
2) Minimum distance to obstacles = 0.1845 (Valid when greater than safety distance 0.1000)
3) Optimization exit flag = 1 (Successful when positive)
4) Elapsed time (s) for nlmpcmove = 22.0876
5) Final states error in x (m), y (m) and theta (deg):  -0.0007, 0.0290, 0.1594
6) Final control inputs speed (m/s) and steering angle (deg): 0.0206, 1.9807

Моделирование с использованием файла MEX дает аналогичные результаты и значительно быстрее, чем моделирование с использованием nlmpcmove.

Моделирование контроллера в Simulink

Для моделирования контроллера NLMPC в Simulink ® используйте блок нелинейного контроллера MPC. Для этого примера, чтобы моделировать транспортное средство эго, используют Кузов 3DOF Боковой блок, который является Моделью Велосипеда (Автоматизированный Ведущий Комплект инструментов) блок.

Укажите продолжительность моделирования и откройте модель Simulink.

Duration = p*Ts;
mdl = 'mpcVDAutoParking';
open_system(mdl)

Чтобы передать параметры эго транспортного средства контроллеру, необходимо создать объект шины параметров.

createParameterBus(nlobj,[mdl '/Nonlinear MPC Controller'],'parasBusObject',paras);

Закройте анимационный график перед моделированием модели.

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

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

sim(mdl)

Figure Automated Parallel Parking contains an axes. The axes with title Parallel Parking contains 90 objects of type rectangle, line, patch, polygon.

ans = 
  Simulink.SimulationOutput:

                   tout: [2142x1 double] 

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

Изучите области позы и контроля Ego Vehicle.

open_system([mdl '/Ego Vehicle Model/Ego Vehicle Pose'])
open_system([mdl '/Controls'])

Результаты моделирования аналогичны моделированию MATLAB. Эго-автомобиль успешно припарковался в целевой позе без столкновений с какими-либо препятствиями.

Заключение

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

% Enable message display
mpcverbosity('on');
% Close Simulink model
bdclose(mdl)
% Close animation plots
f = findobj('Name','Automated Parallel Parking');
close(f)

Ссылки

[1] Шульман, Джон, Ян Дуан, Джонатан Хо, Алекс Ли, Ибрагим Аввал, Генри Брэдлоу, Джиа Пан, Сачин Патил, Кен Голдберг и Питер Аббил. "Планирование движения с последовательной оптимизацией выпуклостей и коллизией выпуклостей The International Journal of Robotics Research 33, no. 9 (август 2014): 1251-70. https://doi.org/10.1177/0278364914528132.

См. также

Функции

nlmpc | nlmpcmove

Блоки

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

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

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

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