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

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

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

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

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

egoInitialPose = [7,3.1,0];

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

egoTargetPose = [-distToCenter,0,0];

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

Tv = 0.1;
helperSLVisualizeParking(egoInitialPose,0);

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

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

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

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

Планирование пути из RRT Star

Сконфигурируйте пространство состояния для плановика. В этом примере состояние эго-транспортного средства представляет собой трехэлементный вектор [x y theta] с координатами xy в метрах и углом поворота в радианах.

xlim = [-10 10];   
ylim = [-2 6];     
yawlim = [-3.1416 3.1416]; 
bounds = [xlim;ylim;yawlim];
stateSpace = stateSpaceReedsShepp(bounds);
stateSpace.MinTurningRadius = 7;

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

stateValidator = parkingStateValidator(stateSpace);

Сконфигурируйте планировщик путей. Использовать plannerRRTStar в качестве планировщика и укажите пространство состояния и средство проверки состояния. Укажите дополнительные параметры для плановика.

planner = plannerRRTStar(stateSpace,stateValidator);
planner.MaxConnectionDistance = 4;
planner.ContinueAfterGoalReached = true;
planner.MaxIterations = 2000;

Запланируйте путь от начальной позы к целевой позе с помощью настроенного планировщика пути. Задайте начальное число случайных чисел для повторяемости.

rng(9, 'twister');
[pathObj,solnInfo] = plan(planner,egoInitialPose,egoTargetPose);

Постройте график расширения дерева в среде парковки.

f = findobj('Name','Automated Parallel Parking');
ax = gca(f);
hold(ax, 'on');
plot(ax,solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'y.-'); % tree expansion

Создание траектории из pathObj путем интерполяции с соответствующим количеством точек.

p = 100;
pathObj.interpolate(p+1);
xRef = pathObj.States;

Нарисуйте траекторию в среде.

plot(ax,xRef(:,1), xRef(:,2),'b-','LineWidth',2)

Разработка нелинейного контроллера отслеживания MPC

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

mpcverbosity('off');

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

nlobjTracking = nlmpc(3,3,2);

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

Ts = 0.1;
pTracking = 10;
nlobjTracking.Ts = Ts;
nlobjTracking.PredictionHorizon = pTracking;
nlobjTracking.ControlHorizon = pTracking;

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

nlobjTracking.MV(1).Min = -2;
nlobjTracking.MV(1).Max = 2;
nlobjTracking.MV(2).Min = -pi/6;
nlobjTracking.MV(2).Max = pi/6;

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

nlobjTracking.Weights.OutputVariables = [1,1,3]; 
nlobjTracking.Weights.ManipulatedVariablesRate = [0.1,0.2];

Движение эго-транспортного средства регулируется кинематической моделью велосипеда. Укажите функцию состояния контроллера и функцию состояния Jacobian.

nlobjTracking.Model.StateFcn = "parkingVehicleStateFcnRRT";
nlobjTracking.Jacobian.StateFcn = "parkingVehicleStateJacobianFcnRRT";

Укажите ограничения клемм на управляющих входах. Ожидается, что и скорость, и угол поворота будут равны нулю в конце.

nlobjTracking.Optimization.CustomEqConFcn = "parkingTerminalConFcn";

Проверьте конструкцию контроллера.

validateFcns(nlobjTracking,randn(3,1),randn(2,1));

Model.StateFcn is OK.
Jacobian.StateFcn is OK.
No output function specified. Assuming "y = x" in the prediction model.
Optimization.CustomEqConFcn is OK.
Analysis of user-provided model, cost, and constraint functions complete.

Выполнение моделирования с замкнутым контуром в MATLAB

Для ускорения моделирования сначала создайте функцию MEX для контроллера NLMPC.

Укажите начальное состояние эго-транспортного средства.

x = egoInitialPose';

Укажите начальные управляющие входные данные.

u = [0;0];

Получение данных генерации кода для контроллера NLMPC.

[coredata,onlinedata] = getCodeGenerationData(nlobjTracking,x,u);

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

mexfcn = buildMEX(nlobjTracking,'parkingRRTMex',coredata,onlinedata);

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

MEX function "parkingRRTMex" successfully generated.

Инициализируйте данные перед запуском моделирования.

xTrackHistory = x;
uTrackHistory = u;
mv = u;
Duration = 14;
Tsteps = Duration/Ts;
Xref = [xRef(2:p+1,:);repmat(xRef(end,:),Tsteps-p,1)];

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

for ct = 1:Tsteps
    % States
    xk = x;
    % Compute optimal control moves with MEX function
    onlinedata.ref = Xref(ct:min(ct+pTracking-1,Tsteps),:);
    [mv,onlinedata,info] = mexfcn(xk,mv,onlinedata);
    % Implement first optimal control move and update plant states.
    ODEFUN = @(t,xk) parkingVehicleStateFcnRRT(xk,mv);
    [TOUT,YOUT] = ode45(ODEFUN,[0 Ts], xk);
    x = YOUT(end,:)';
    % Save plant states for display.
    xTrackHistory = [xTrackHistory x]; %#ok<*AGROW>
    uTrackHistory = [uTrackHistory mv];
end

Печать и анимация результатов моделирования при использовании контроллера NLMPC. Результаты отслеживания соответствуют ссылочной траектории из планировщика траектории.

plotAndAnimateParkingRRT(p,xRef,xTrackHistory,uTrackHistory);

Tracking error infinity norm in x (m), y (m) and theta (deg): 0.0425, 0.0425, 1.6509
Final control inputs of speed (m/s) and steering angle (deg): 0.0000, -0.2823

Запуск моделирования с замкнутым контуром в Simulink

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

mdl = 'mpcVDAutoParkingRRT';
open_system(mdl)

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

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

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

sim(mdl)

ans = 
  Simulink.SimulationOutput:

                   tout: [7349x1 double] 

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

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

Заключение

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

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