В этом примере показано, как спроектировать параллельный контроллер парковки, использующий нелинейное прогнозирующее управление модели (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);
В визуализации четыре припаркованных транспортных средства являются оранжевыми полями в середине. Нижняя часть оранжевый контур является дорожной обочиной и главным оранжевым контуром, является желтой линией на дороге.
Для проблем с парковкой транспортное средство перемещается в низких скоростях. Этот пример использует кинематическую модель велосипеда с передним руководящим углом для проблемы с парковкой транспортного средства. Движение автомобиля, оборудованного датчиком может быть описано с помощью следующих уравнений.
Здесь, обозначает положение транспортного средства и обозначает угол отклонения от курса транспортного средства. Параметр представляет колесную базу транспортного средства. переменные состояния для функций состояния транспортного средства. Скорость и регулирование угла контрольные переменные для функций состояния транспортного средства. Функции состояния транспортного средства реализованы в parkingVehicleStateFcn
.
Нелинейный прогнозирующий контроллер модели для парковки спроектирован на основе следующего анализа.
Выход функции состояния транспортного средства совпадает с состоянием транспортного средства . Поэтому объект контроллера NLMPC создается с тремя состояниями, тремя выходными параметрами и двумя переменными, которыми управляют.
Скорость автомобиля, оборудованного датчиком ограничивается быть между-2 и 2 м/с, и держащийся угол автомобиля, оборудованного датчиком ограничивается быть между-45 и 45 градусами.
Диспетчер NLMPC использует пользовательскую функцию стоимости, которая задана способом, похожим на квадратичные издержки плюс отслеживания терминальная стоимость. В следующей пользовательской функции стоимости, обозначает состояния автомобиля, оборудованного датчиком во время , представляет длительность симуляции, и целевое положение автомобиля, оборудованного датчиком. Матрицы веса , , , и являются постоянными.
Чтобы избежать столкновений с препятствиями, контроллер NLMPC должен удовлетворить следующим ограничениям неравенства где минимальное расстояние до всех препятствий должен быть больше безопасного расстояния . В этом примере автомобиль, оборудованный датчиком и препятствия моделируются как collisionBox
объекты и расстояние от автомобиля, оборудованного датчиком до препятствий вычисляются с помощью checkCollision
.
Чтобы повысить эффективность симуляции, Якобианы функции состояния, функции стоимости и ограничений неравенства все предоставляются контроллеру 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;
Чтобы симулировать контроллер 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 = 33.7795 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. MEX function "parkingMex" successfully generated.
Summary of results: 1) Valid results. No collisions. 2) Minimum distance to obstacles = 0.1811 (Valid when greater than safety distance 0.1000) 3) Optimization exit flag = 1 (Successful when positive) 4) Elapsed time (s) for nlmpcmove = 3.8468 5) Final states error in x (m), y (m) and theta (deg): -0.0052, 0.0317, 0.1662 6) Final control inputs speed (m/s) and steering angle (deg): -0.0101, 1.2793
Симуляция с помощью файла MEX приводит к подобным результатам и значительно быстрее, чем симуляция с помощью nlmpcmove
.
Чтобы симулировать контроллер NLMPC в Simulink ®, используйте блок Nonlinear MPC Controller. В данном примере, чтобы симулировать автомобиль, оборудованный датчиком, используйте Кузов 3DOF Боковой блок, который является Велосипедным блоком Model.
Задайте длительность симуляции и откройте модель 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: [2478x1 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.