Парковка среды

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

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

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

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

egoInitialPose = [7,3.1,0];

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

egoTargetPose = [-distToCenter,0,0];

Визуализируйте среду парковки. Задайте visualizer шаг расчета 0.1 s.

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) скорость автомобиля, оборудованного датчиком в m/s и MV(2) держащийся угол в радианах.

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

Задайте функцию состояния контроллера и якобиан состояния функциональный.

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

Задайте функцию стоимости контроллера и якобиан функции стоимости.

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 (установите useMEX к 0).

useMex = 0; 
runParkingAndPlot

Summary of results:
1) Valid results. No collisions.
2) Minimum distance to obstacles = 0.1782 (Valid when greater than safety distance 0.1000)
3) Optimization exit flag = 1 (Successful when positive)
4) Elapsed time (s) for nlmpcmove = 17.4020
5) Final states error in x (m), y (m) and theta (deg):  -0.0087, 0.0294, 0.1698
6) Final control inputs speed (m/s) and steering angle (deg): -0.0006, -0.0180

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

Создайте файл 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.1274 (Valid when greater than safety distance 0.1000)
3) Optimization exit flag = 0 (Successful when positive)
4) Elapsed time (s) for nlmpcmove = 3.6830
5) Final states error in x (m), y (m) and theta (deg):  -0.0042, 0.0195, 0.2166
6) Final control inputs speed (m/s) and steering angle (deg): -0.0284, -1.8098

Симуляция с помощью файла MEX приводит к подобным результатам и значительно быстрее, чем симуляция с помощью nlmpcmove.

Симулируйте контроллер в Simulink

Чтобы симулировать контроллер NLMPC в Simulink ®, используйте блок Nonlinear MPC Controller. В данном примере, чтобы симулировать автомобиль, оборудованный датчиком, используйте Кузов 3DOF Боковой блок, который является блоком Bicycle Model (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: [2164x1 double] 

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

Исследуйте Положение Автомобиля, оборудованного датчиком и осциллографы Средств управления.

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.