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

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

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

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$$ являются управляющими переменными для функций состояния транспортного средства. Функции транспортного средства состояния реализованы в parkingVehicleStateFcnRRT.

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

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

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] 

Исследуйте Автомобиль , оборудованный датчиком Положения и Controls возможностей. Результаты симуляции аналогичны симуляции 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)