Окружение парковки

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

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

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

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

egoInitialPose = [7,3.1,0];

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

egoTargetPose = [-distToCenter,0,0];

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

Tv = 0.1;
helperSLVisualizeParking(egoInitialPose,0);

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

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

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

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

Проектируйте нелинейный прогнозирующий контроллер модели

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

Задайте шаг расчета (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 ®, можно использовать одну из следующих опций:

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

useMex = 0; 
runParkingAndPlot

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.

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 ®, используйте блок Nonlinear MPC Controller. В данном примере для симуляции автомобиля , оборудованного датчиком используйте блок Транспортного средства Body 3DOF Lateral, который является блоком Bicycle Модели (Automated Driving Toolbox).

Задайте длительность симуляции и откройте модель 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)

ans = 
  Simulink.SimulationOutput:

                   tout: [2142x1 double] 

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

Исследуйте Автомобиль , оборудованный датчиком Положения и Controls возможностей.

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] Шульман, Джон, Ян Дуан, Джонатан Хо, Алекс Ли, Ибрагим Аввал, Генри Брэдлоу, Джиа Панорамирование, Сачин Патил, Кен Голдберг и Питер Аббил. "Планирование движения с последовательной оптимизацией выпуклых и выпуклой Международный журнал исследований робототехники 33, № 9 (август 2014): 1251-70. https://doi.org/10.1177/0278364914528132.