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

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

Визуализируйте среду парковки. Задайте visualizer шаг расчета 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.

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

Модель автомобиля, оборудованного датчиком

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

x˙=vcos(ψ)y˙=vsin(ψ)ψ˙=vbtan(δ)

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

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

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

  • Выход функции состояния транспортного средства совпадает с состоянием транспортного средства (x,y,ψ). Поэтому объект контроллера 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 (Robotics System Toolbox).

distmindistsafe

  • Чтобы повысить эффективность симуляции, Якобианы функции состояния, функции стоимости и ограничений неравенства все предоставляются контроллеру 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) скорость автомобиля, оборудованного датчиком в 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®, можно использовать одну из следующих опций:

  • Симулируйте контроллер, использующий 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 ®, используйте блок 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)

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] 

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

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.

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

Функции

Блоки

Похожие темы

Для просмотра документации необходимо авторизоваться на сайте