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

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

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

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

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

Сконфигурируйте пространство состояний для планировщика. В этом примере состояние автомобиля, оборудованного датчиком является трехэлементным вектором, [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;

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

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) скорость автомобиля, оборудованного датчиком в m/s и 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];

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

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.0538, 0.0538, 1.3432
Final control inputs of speed (m/s) and steering angle (deg): 0.0001, 0.0981

Запустите симуляцию с обратной связью в Simulink

Чтобы симулировать контроллер NLMPC в Simulink ®, используйте блок Nonlinear MPC Controller. В данном примере, чтобы симулировать автомобиль, оборудованный датчиком, используйте Кузов 3DOF Боковой блок, который является блоком Bicycle Model (Automated Driving Toolbox).

mdl = 'mpcVDAutoParkingRRT';
open_system(mdl)

Закройте график анимации прежде, чем симулировать модель.

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

Симулируйте модель.

sim(mdl)

ans = 
  Simulink.SimulationOutput:

                   tout: [7662x1 double] 

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

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

Заключение

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

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