Автоматизированный камердинер парковки

В этом примере показано, как создать автоматизированную систему камердинера парковки. В этом примере вы узнаете об инструментах и методах в поддержку планирования пути, генерации траектории и управления транспортным средством. В то время как этот пример фокусируется на рабочем процессе MATLAB®-oriented, эти инструменты также доступны в Simulink®. Для версии Simulink® этого примера смотрите Автоматизированного Камердинера Парковки в Simulink.

Обзор

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

  • Передняя сторона и камеры стороны для обнаружения маркировок маршрута, дорожные знаки (знаки Стоп, выходные маркировки, и т.д.), другие транспортные средства и пешеходы

  • Лидар и датчики ультразвука для обнаружения препятствий и вычисления точных измерений расстояния

  • Датчики ультразвука для обнаружения препятствия

  • IMU и энкодеры колеса для точного расчета

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

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

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

Модель среды

Модель среды представляет карту среды. Для системы камердинера парковки эта карта включает доступные и занятые места для парковки, дорожные разметки и препятствия, такие как пешеходы или другие транспортные средства. Карты заполнения являются общим представлением для этой формы модели среды. Такая карта обычно создается с помощью Одновременной локализации и картографии (SLAM) путем интеграции наблюдений от датчиков лидара и камеры. Этот пример концентрируется на более простом сценарии, где карта уже предоставлена, например, транспортным средством к инфраструктуре (V2X) систему или камеру, выходящую на целое парковочное место. Это использует статическую карту парковки и принимает, что самолокализация транспортного средства точна.

Пример парковки, используемый в этом примере, состоит из трех слоев сетки заполнения.

  • Стационарные препятствия: Этот слой содержит стационарные препятствия как стенки, барьеры и границы парковки.

  • Дорожные разметки: Этот слой содержит информацию о заполнении, имеющую отношение к дорожным разметкам, включая дорожные разметки для парковочных мест.

  • Припаркованные автомобили: Этот слой содержит информацию, о которой уже заняты места для парковки.

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

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

mapLayers = loadParkingLotMapLayers;
plotMapLayers(mapLayers)

Для простоты, объединение эти три слоя в один costmap.

costmap = combineMapLayers(mapLayers);

figure
plot(costmap, 'Inflation', 'off')
legend off

Объединенный costmap vehicleCostmap объект, который представляет среду транспортного средства как 2D сетку заполнения. Каждая сетка в ячейке имеет значения между 0 и 1, представляя стоимость навигации через ячейку. Препятствия имеют более высокую стоимость, в то время как свободное пространство имеет более низкую цену. Ячейка рассматривается препятствием, если его стоимость выше, чем OccupiedThreshold свойство, и свободный, если его стоимость ниже, чем FreeThreshold свойство.

costmap покрывает целую 75m-by-50m область парковки, разделенную на 0.5m-by-0.5m квадратные ячейки.

costmap.MapExtent % [x, width, y, height] in meters

costmap.CellSize  % cell size in meters
ans =

     0    75     0    50


ans =

    0.5000

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

vehicleDims      = vehicleDimensions;
maxSteeringAngle = 35; % in degrees

Обновите VehicleDimensions свойство costmap средства проверки столкновения с размерностями транспортного средства, чтобы припарковаться. Эта установка настраивает степень инфляции в карте вокруг препятствий, чтобы соответствовать размеру припаркованного транспортного средства, гарантируя, что пути без коллизий могут быть найдены через парковку.

costmap.CollisionChecker.VehicleDimensions = vehicleDims;

Задайте стартовое положение транспортного средства. Положение получено посредством локализации, которая упущена из этого примера для простоты. Положение транспортного средства задано как $[x,y,\theta]$в мировых координатах.$(x,y)$ представляет позицию центра задней оси транспортного средства в системе мировой координаты.$\theta$ представляет ориентацию транспортного средства относительно мировой оси X. Для получения дополнительной информации смотрите Системы координат в Automated Driving Toolbox.

currentPose = [4 12 0]; % [x, y, theta]

Поведенческий слой

Планирование включает организацию всей уместной информации на иерархические слои. Каждый последовательный слой ответственен за более мелкомодульную задачу. Поведенческий слой [1] находится наверху этого стека. Это ответственно за активацию и управление различные части миссии путем предоставления последовательности задач навигации. Поведенческий слой собирает информацию от всех соответствующих частей системы, включая:

  • Локализация: поведенческий слой смотрит модуль локализации для оценки текущего местоположения транспортного средства.

  • Модель Environment: Восприятие и системы сплава датчика сообщают о карте среды вокруг транспортного средства.

  • Определение места для парковки: поведенческий слой анализирует карту, чтобы определить самое близкое доступное место для парковки.

  • Нахождение глобального маршрута: модуль маршрутизации вычисляет глобальный маршрут через дорожную сеть, полученную или от сервиса отображения или от инфраструктуры V2X. При разложении глобального маршрута, когда серия дорожных ссылок позволяет траектории для каждой ссылки быть запланированной по-другому. Например, итоговый маневр парковки требует различного профиля скорости, чем подход к месту для парковки. В более общей установке это становится крайне важным для навигации по улицам, которые включают различные ограничения скорости, количества маршрутов и дорожные знаки.

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

HelperBehavioralPlanner класс подражает интерфейсу поведенческого слоя планирования. HelperBehavioralPlanner создается с помощью карты и глобального плана маршрута. Этот пример использует статический глобальный план маршрута, сохраненный в таблице MATLAB, но обычно алгоритм маршрутизации, предоставленный локальной инфраструктурой парковки или сервисом отображения, определяет этот план. Глобальный план маршрута описан как последовательность сегментов маршрута, чтобы пересечь, чтобы достигнуть места для парковки.

Загрузите MAT-файл, содержащий план маршрута, который хранится в таблице. Таблица имеет три переменные: StartPose, EndPose, и Attributes. StartPose и EndPose задайте положения начала и конца сегмента, описанного как$[x,y,\theta]$. Attributes задает свойства сегмента, такие как ограничение скорости.

data = load('routePlan.mat');
routePlan = data.routePlan %#ok<NOPTS>
routePlan =

  4×3 table

      StartPose           EndPose          Attributes 
    ______________    ________________    ____________

     4    12     0    56     11      0    [1×1 struct]
    56    11     0    70     19     90    [1×1 struct]
    70    19    90    70     32     90    [1×1 struct]
    70    32    90    53     39    180    [1×1 struct]

Постройте транспортное средство в текущем положении, и вдоль каждой цели в плане маршрута.

% Plot vehicle at current pose
hold on
helperPlotVehicle(currentPose, vehicleDims, 'DisplayName', 'Current Pose')
legend

for n = 1 : height(routePlan)
    % Extract the goal waypoint
    vehiclePose = routePlan{n, 'EndPose'};

    % Plot the pose
    legendEntry = sprintf('Goal %i', n);
    helperPlotVehicle(vehiclePose, vehicleDims, 'DisplayName', legendEntry);
end
hold off

Создайте поведенческий объект помощника планировщика. requestManeuver метод запрашивает поток задач навигации от поведенческого планировщика, пока место назначения не достигнуто.

behavioralPlanner = HelperBehavioralPlanner(routePlan, maxSteeringAngle);

Транспортное средство перемещается по каждому сегменту пути с помощью этих шагов:

  1. Планирование движения: Запланируйте выполнимый путь через карту среды с помощью оптимального быстро исследующего случайного дерева (RRT*) алгоритм (pathPlannerRRT).

  2. Сглаживание пути: Сглаживайте ссылочный путь путем подбора кривой сплайнам к нему с помощью smoothPathSpline.

  3. Генерация траектории: Преобразуйте сглаживавший путь в траекторию путем генерации профиля скорости с помощью helperGenerateVelocityProfile.

  4. Управление транспортным средством: Учитывая сглаживавший ссылочный путь, HelperPathAnalyzer вычисляет ссылочное положение и скорость на основе текущего положения и скорость транспортного средства. Предоставленный ссылочные значения, lateralControllerStanley вычисляет держащийся угол, чтобы управлять заголовком транспортного средства. HelperLongitudinalController вычисляет ускорение и команды замедления, чтобы обеспечить желаемую скорость транспортного средства.

  5. Целевая Проверка: Проверяйте, достигло ли транспортное средство итогового положения сегмента с помощью helperGoalChecker.

Остальная часть этого примера описывает эти шаги подробно, прежде, чем собрать их в полное решение.

Планирование движения

Учитывая глобальный маршрут, планирование движения может использоваться, чтобы запланировать путь через среду, чтобы достигнуть каждого промежуточного звена waypoint, пока транспортное средство не достигает конечного пункта назначения. Запланированный путь для каждой ссылки должен быть выполнимым и без коллизий. Выполнимый путь является тем, который может быть понят транспортным средством, учитывая движение и динамические ограничения, наложенные на него. Система камердинера парковки включает низкие скорости и низкие ускорения. Это позволяет нам безопасно игнорировать динамические ограничения, являющиеся результатом инерционных эффектов.

Создайте pathPlannerRRT объект сконфигурировать планировщика пути, использующего оптимальное быстро исследующее случайное дерево (RRT*) подход. Семейство RRT планирования алгоритмов находит путь путем построения дерева связанных, положений транспортного средства без коллизий. Положения соединяются с помощью регулирования Dubins или Reeds-Shepp, гарантируя, что сгенерированный путь кинематическим образом выполним.

motionPlanner = pathPlannerRRT(costmap, 'MinIterations', 1000, ...
    'ConnectionDistance', 10, 'MinTurningRadius', 20);

Запланируйте путь от текущего положения до первой цели при помощи plan функция. Возвращенный driving.Path объект, refPath, выполнимый и ссылочный путь без коллизий.

goalPose = routePlan{1, 'EndPose'};
refPath = plan(motionPlanner, currentPose, goalPose);

Ссылочный путь состоит из последовательности сегментов пути. Каждый сегмент пути описывает набор маневров Dubins или Reeds-Shepp, используемых, чтобы соединиться со следующим сегментом. Смотрите сегменты пути.

refPath.PathSegments
ans = 

  1×6 DubinsPathSegment array with properties:

    StartPose
    GoalPose
    MinTurningRadius
    MotionLengths
    MotionTypes
    Length

Ссылочный путь содержит положения перехода по пути, представляя точки вдоль пути, соответствующего переходу от одного маневра до следующего. Они могут также представлять изменения направления, например, от форварда, чтобы инвертировать движение вдоль пути Тростников-Shepp.

Получите положения перехода и направления от запланированного пути.

[transitionPoses, directions] = interpolate(refPath);

% Visualize the planned path
plot(motionPlanner)

В дополнение к запланированному ссылочному пути заметьте красные области на графике. Эти области представляют области costmap, где источник транспортного средства (центр задней оси) не должен пересекаться во избежание удара никаких препятствий. pathPlannerRRT находит пути, которые избегают препятствий путем проверки, чтобы гарантировать, что сгенерированные положения транспортного средства не лежат на этих областях.

Сглаживание пути и генерация траектории

Ссылочный путь, сгенерированный планировщиком пути, составлен любой из сегментов Dubins или Reeds-Shepp. Искривление на перекрестках двух таких сегментов не непрерывно и может привести к резким изменениям в держащемся углу. Чтобы избежать такого неестественного движения и гарантировать пассажирский комфорт, путь должен быть непрерывно дифференцируемым и поэтому сглаживать [2]. Один подход к сглаживанию пути включает подбор кривой параметрическому кубическому сплайну. Подбор кривой сплайна позволяет вам сгенерировать сглаженный путь, который может выполнить контроллер.

Используйте smoothPathSpline соответствовать параметрическому кубическому сплайну, который проходит через все точки перехода в ссылочном пути. Сплайн приблизительно совпадает с запуском и конечными направлениями с запуском и конечным углом рыскания транспортного средства.

% Specify number of poses to return using a separation of approximately 0.1 m
approxSeparation = 0.1; % meters
numSmoothPoses   = round(refPath.Length / approxSeparation);

% Return discretized poses along the smooth path
[refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses);

% Plot the smoothed path
hold on
hSmoothPath = plot(refPoses(:, 1), refPoses(:, 2), 'r', 'LineWidth', 2, ...
    'DisplayName', 'Smoothed Path');
hold off

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

Задайте начальную букву, максимум и терминальные скорости так, чтобы транспортное средство запустилось стационарный, ускоряются к скорости 5 метров/секунда и останавливаются.

maxSpeed   = 5; % in meters/second
startSpeed = 0; % in meters/second
endSpeed   = 0; % in meters/second

Сгенерируйте скоростной профиль

refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, startSpeed, endSpeed, maxSpeed);

refVelocities содержит ссылочные скорости для каждой точки вдоль сглаживавшего пути. Постройте сгенерированный скоростной профиль.

plotVelocityProfile(cumLengths, refVelocities, maxSpeed)

Управление транспортным средством и симуляция

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

  • Боковое управление: Настройте держащийся угол, таким образом, что транспортное средство следует за ссылочным путем.

  • Продольное управление: При следовании за ссылочным путем обеспечьте желаемую скорость путем управления дросселем и тормозом.

Поскольку этот сценарий включает низкие скорости, можно упростить контроллер, чтобы учесть только кинематическую модель. В этом примере боковое управление понято lateralControllerStanley функция. Продольное управление понято Системой помощника object™ HelperLongitudinalController, это вычисляет ускорение и команды замедления на основе Пропорционально-интегрального закона.

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

$$\dot x_r = v_r*\cos(\theta) $$

$$\dot y_r = v_r*\sin(\theta) $$

$$\dot \theta = \frac{v_r}{l}*\tan(\delta) $$

$$\dot v_r = a_r $$

В вышеупомянутых уравнениях,$(x_r,y_r,\theta)$ представляет положение транспортного средства в мировых координатах.$v_r$$a_r$, $l$и$\delta$ представляйте скорость заднего колеса, ускорение заднего колеса, колесную базу и держащийся угол, соответственно. Положение и скорость переднего колеса могут быть получены:

$$ x_f = x_r + l cos(\theta)$$

$$ y_f = y_r + l sin(\theta)$$

$$ v_f = \frac{v_r} {cos(\delta)}$$

% Close all the figures
closeFigures;

% Create the vehicle simulator
vehicleSim = HelperVehicleSimulator(costmap, vehicleDims);

% Set the vehicle pose and velocity
vehicleSim.setVehiclePose(currentPose);
currentVel = 0;
vehicleSim.setVehicleVelocity(currentVel);

% Configure the simulator to show the trajectory
vehicleSim.showTrajectory(true);

% Hide vehicle simulation figure
hideFigure(vehicleSim);

Создайте HelperPathAnalyzer объект вычислить ссылочное положение, ссылочную скорость и направление движения для контроллера.

pathAnalyzer = HelperPathAnalyzer(refPoses, refVelocities, directions, ...
    'Wheelbase', vehicleDims.Wheelbase);

Создайте HelperLongitudinalController возразите, чтобы управлять скоростью транспортного средства и задать шаг расчета.

sampleTime = 0.05;
lonController = HelperLongitudinalController('SampleTime', sampleTime);

Используйте HelperFixedRate объект гарантировать выполнение с фиксированной процентной ставкой контроллера обратной связи. Используйте уровень управления, чтобы быть сопоставимыми с продольным контроллером.

controlRate = HelperFixedRate(1/sampleTime); % in Hertz

Пока цель не достигнута, сделайте следующее:

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

  • Накормите командами управления средство моделирования.

  • Запишите возвращенное положение транспортного средства и скорость, чтобы питаться в контроллер в следующей итерации.

reachGoal = false;

while ~reachGoal
    % Find the reference pose on the path and the corresponding velocity
    [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);

    % Update driving direction for the simulator
    updateDrivingDirection(vehicleSim, direction);

    % Compute steering command
    steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
        'Direction', direction, 'Wheelbase', vehicleDims.Wheelbase);

    % Compute acceleration and deceleration commands
    lonController.Direction = direction;
    [accelCmd, decelCmd] = lonController(refVel, currentVel);

    % Simulate the vehicle using the controller outputs
    drive(vehicleSim, accelCmd, decelCmd, steeringAngle);

    % Check if the vehicle reaches the goal
    reachGoal = helperGoalChecker(goalPose, currentPose, currentVel, endSpeed, direction);

    % Wait for fixed-rate execution
    waitfor(controlRate);

    % Get current pose and velocity of the vehicle
    currentPose  = getVehiclePose(vehicleSim);
    currentVel   = getVehicleVelocity(vehicleSim);
end

% Show vehicle simulation figure
showFigure(vehicleSim);

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

Выполните комплексный план

Теперь объедините все предыдущие шаги в процессе планирования и запустите симуляцию для полного плана маршрута. Этот процесс включает слияние поведенческого планировщика.

% Set the vehicle pose back to the initial starting point
currentPose = [4 12 0]; % [x, y, theta]
vehicleSim.setVehiclePose(currentPose);

% Reset velocity
currentVel  = 0; % meters/second
vehicleSim.setVehicleVelocity(currentVel);

while ~reachedDestination(behavioralPlanner)

    % Request next maneuver from behavioral layer
    [nextGoal, plannerConfig, speedConfig] = requestManeuver(behavioralPlanner, ...
        currentPose, currentVel);

    % Configure the motion planner
    configurePlanner(motionPlanner, plannerConfig);

    % Plan a reference path using RRT* planner to the next goal pose
    refPath = plan(motionPlanner, currentPose, nextGoal);

    % Check if the path is valid. If the planner fails to compute a path,
    % or the path is not collision-free because of updates to the map, the
    % system needs to re-plan. This scenario uses a static map, so the path
    % will always be collision-free.
    isReplanNeeded = ~checkPathValidity(refPath, costmap);
    if isReplanNeeded
        warning('Unable to find a valid path. Attempting to re-plan.')

        % Request behavioral planner to re-plan
        replanNeeded(behavioralPlanner);
        continue;
    end

    % Retrieve transition poses and directions from the planned path
    [transitionPoses, directions] = interpolate(refPath);

    % Smooth the path
    numSmoothPoses   = round(refPath.Length / approxSeparation);
    [refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses);

    % Generate a velocity profile
    refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, startSpeed, endSpeed, maxSpeed);

    % Configure path analyzer
    pathAnalyzer.RefPoses     = refPoses;
    pathAnalyzer.Directions   = directions;
    pathAnalyzer.VelocityProfile = refVelocities;

    % Reset longitudinal controller
    reset(lonController);

    reachGoal = false;

    % Execute control loop
    while ~reachGoal
        % Find the reference pose on the path and the corresponding velocity
        [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);

        % Update driving direction for the simulator
        updateDrivingDirection(vehicleSim, direction);

        % Compute steering command
        steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
            'Direction', direction, 'Wheelbase', vehicleDims.Wheelbase);

        % Compute acceleration and deceleration commands
        lonController.Direction = direction;
        [accelCmd, decelCmd] = lonController(refVel, currentVel);

        % Simulate the vehicle using the controller outputs
        drive(vehicleSim, accelCmd, decelCmd, steeringAngle);

        % Check if the vehicle reaches the goal
        reachGoal = helperGoalChecker(nextGoal, currentPose, currentVel, speedConfig.EndSpeed, direction);

        % Wait for fixed-rate execution
        waitfor(controlRate);

        % Get current pose and velocity of the vehicle
        currentPose  = getVehiclePose(vehicleSim);
        currentVel   = getVehicleVelocity(vehicleSim);
    end
end

% Show vehicle simulation figure
showFigure(vehicleSim);

Парковка маневра

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

% Hide vehicle simulation figure
hideFigure(vehicleSim);

vehicleCostmap использует основанную на инфляции проверку столкновения. Во-первых, визуально смотрите текущее средство проверки столкновения в использовании.

ccConfig = costmap.CollisionChecker;

figure
plot(ccConfig)
title('Current Collision Checker')

Проверка столкновения выполняется путем расширения препятствий в costmap радиусом инфляции и проверки ли центр круга, показанного выше лжи на расширенной ячейке сетки. Итоговый маневр парковки требует более точного, менее консервативного проверяющего столкновение механизма. Это обычно решается путем представления формы транспортного средства с помощью (приблизительно 3-5) перекрывающиеся круги вместо одного круга.

Используйте большее число кругов в средстве проверки столкновения и визуально смотрите средство проверки столкновения. Это позволяет планировать через узкие проходы.

ccConfig.NumCircles = 4;

figure
plot(ccConfig)
title('New Collision Checker')

Обновите costmap, чтобы использовать это средство проверки столкновения.

costmap.CollisionChecker = ccConfig;

Заметьте, что радиус инфляции уменьшал, позволяя планировщику найти свободный путь к месту для парковки.

figure
plot(costmap)
title('Costmap with updated collision checker')

% Set up the pathPlannerRRT to use the updated costmap
parkMotionPlanner = pathPlannerRRT(costmap, 'MinIterations', 1000);

% Define desired pose for the parking spot, returned by the V2X system
parkPose = [36 44 90];
preParkPose = currentPose;

% Compute the required parking maneuver
refPath = plan(parkMotionPlanner, preParkPose, parkPose);

% Plot the resulting parking maneuver
figure
plotParkingManeuver(costmap, refPath, preParkPose, parkPose)

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

% Retrieve transition poses and directions from the planned path
[transitionPoses, directions] = interpolate(refPath);

% Smooth the path
numSmoothPoses   = round(refPath.Length / approxSeparation);
[refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses);

% Set up the velocity profile generator to stop at the end of the trajectory,
% with a speed limit of 5 mph
refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, currentVel, 0, 2.2352);

pathAnalyzer.RefPoses     = refPoses;
pathAnalyzer.Directions   = directions;
pathAnalyzer.VelocityProfile = refVelocities;

% Reset longitudinal controller
reset(lonController);

reachGoal = false;

while ~reachGoal
    % Find the reference pose on the path and the corresponding velocity
    [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);

    % Update driving direction for the simulator
    updateDrivingDirection(vehicleSim, direction);

    % Compute steering command
    steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
        'Direction', direction, 'Wheelbase', vehicleDims.Wheelbase);

    % Compute acceleration and deceleration commands
    lonController.Direction = direction;
    [accelCmd, decelCmd] = lonController(refVel, currentVel);

    % Simulate the vehicle using the controller outputs
    drive(vehicleSim, accelCmd, decelCmd, steeringAngle);

    % Check if the vehicle reaches the goal
    reachGoal = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction);

    % Wait for fixed-rate execution
    waitfor(controlRate);

    % Get current pose and velocity of the vehicle
    currentPose  = getVehiclePose(vehicleSim);
    currentVel   = getVehicleVelocity(vehicleSim);
end

% Show vehicle simulation figure
closeFigures;
showFigure(vehicleSim);

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

% Specify a parking pose corresponding to a back-in parking maneuver
parkPose = [49 47.2 -90];

% Change the connection method to allow for reverse motions
parkMotionPlanner.ConnectionMethod = 'Reeds-Shepp';

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

parkMotionPlanner.MinTurningRadius   = 10; % meters
parkMotionPlanner.ConnectionDistance = 15;

% Reset vehicle pose and velocity
currentVel = 0;
vehicleSim.setVehiclePose(preParkPose);
vehicleSim.setVehicleVelocity(currentVel);

% Compute the parking maneuver
replan = true;
while replan
    refPath = plan(parkMotionPlanner, preParkPose, parkPose);

    % The path corresponding to the parking maneuver is small and requires
    % precise maneuvering. Instead of interpolating only at transition poses,
    % interpolate more finely along the length of the path.

    numSamples = 10;
    stepSize   = refPath.Length / numSamples;
    lengths    = 0 : stepSize : refPath.Length;

    [transitionPoses, directions] = interpolate(refPath, lengths);

    % Replan if the path contains more than one direction switching poses
    % or if the path is too long
    replan = sum(abs(diff(directions)))~=2 || refPath.Length > 20;
end

% Visualize the parking maneuver
figure
plotParkingManeuver(costmap, refPath, preParkPose, parkPose)

% Smooth the path
numSmoothPoses   = round(refPath.Length / approxSeparation);
[refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses, 0.5);

% Generate velocity profile
refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, currentVel, 0, 1);

pathAnalyzer.RefPoses     = refPoses;
pathAnalyzer.Directions   = directions;
pathAnalyzer.VelocityProfile = refVelocities;

% Reset longitudinal controller
reset(lonController);

reachGoal = false;

while ~reachGoal
    % Get current driving direction
    currentDir = getDrivingDirection(vehicleSim);

    % Find the reference pose on the path and the corresponding velocity.
    [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);

    % If the vehicle changes driving direction, reset vehicle velocity in
    % the simulator and reset longitudinal controller
    if currentDir ~= direction
        currentVel = 0;
        setVehicleVelocity(vehicleSim, currentVel);
        reset(lonController);
    end

    % Update driving direction for the simulator. If the vehicle changes
    % driving direction, reset and return the current vehicle velocity as zero.
    currentVel = updateDrivingDirection(vehicleSim, direction, currentDir);

    % Compute steering command
    steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
        'Direction', direction, 'Wheelbase', vehicleDims.Wheelbase);

    % Compute acceleration and deceleration commands
    lonController.Direction = direction;
    [accelCmd, decelCmd] = lonController(refVel, currentVel);

    % Simulate the vehicle using the controller outputs
    drive(vehicleSim, accelCmd, decelCmd, steeringAngle);

    % Check if the vehicle reaches the goal
    reachGoal = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction);

    % Wait for fixed-rate execution
    waitfor(controlRate);

    % Get current pose and velocity of the vehicle
    currentPose  = getVehiclePose(vehicleSim);
    currentVel   = getVehicleVelocity(vehicleSim);
end

% Take a snapshot for the example
closeFigures;
snapnow;

% Delete the simulator
delete(vehicleSim);

Заключение

Этот пример показал как:

  1. Запланируйте выполнимый путь в полуструктурированной среде, такой как парковка, с помощью RRT* алгоритм планирования пути.

  2. Сглаживайте путь с помощью сплайнов и сгенерируйте профиль скорости вдоль сглаживавшего пути.

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

  4. Поймите различные поведения парковки при помощи различных настроек планировщика движения.

Ссылки

[1] Buehler, Мартин, Карл Иэгнемма и Сэнджив Сингх. DARPA Городская проблема: Автономные Транспортные средства в Городском движении (1-й редактор). Springer Publishing Company, Incorporated, 2009.

[2] Lepetic, Марко, Грегор Клэнкэр, Игорь Скрянк, Драго Матко, Bostjan Potocnik, "Время Оптимальное Планирование пути, Рассматривая Ускоряющие Пределы". Робототехника и Автономные системы. Объем 45, Выпуски 3-4, 2003, стр 199-210.

Вспомогательные Функции

среда Загрузки loadParkingLotMapLayers сопоставляет слои для парковки

function mapLayers = loadParkingLotMapLayers()
%loadParkingLotMapLayers
%   Load occupancy maps corresponding to 3 layers - obstacles, road
%   markings, and used spots.

mapLayers.StationaryObstacles = imread('stationary.bmp');
mapLayers.RoadMarkings        = imread('road_markings.bmp');
mapLayers.ParkedCars          = imread('parked_cars.bmp');
end

struct Графика plotMapLayers, содержащий слои карты

function plotMapLayers(mapLayers)
%plotMapLayers
%   Plot the multiple map layers on a figure window.

figure
cellOfMaps = cellfun(@imcomplement, struct2cell(mapLayers), 'UniformOutput', false);
montage( cellOfMaps, 'Size', [1 numel(cellOfMaps)], 'Border', [5 5], 'ThumbnailSize', [300 NaN] )
title('Map Layers - Stationary Obstacles, Road markings, and Parked Cars')
end

Объединение combineMapLayers сопоставляет слои в один costmap

function costmap = combineMapLayers(mapLayers)
%combineMapLayers
%   Combine map layers struct into a single vehicleCostmap.

combinedMap = mapLayers.StationaryObstacles + mapLayers.RoadMarkings + ...
    mapLayers.ParkedCars;
combinedMap = im2single(combinedMap);

res = 0.5; % meters
costmap = vehicleCostmap(combinedMap, 'CellSize', res);
end

configurePlanner Конфигурируют планировщика пути с заданными настройками

function configurePlanner(pathPlanner, config)
%configurePlanner
% Configure the path planner object, pathPlanner, with settings specified
% in struct config.

fieldNames = fields(config);
for n = 1 : numel(fieldNames)
    if ~strcmpi(fieldNames{n}, 'IsParkManeuver')
        pathPlanner.(fieldNames{n}) = config.(fieldNames{n});
    end
end
end

профиль скорости Графика plotVelocityProfile

function plotVelocityProfile(cumPathLength, refVelocities, maxSpeed)
%plotVelocityProfile
% Plot the generated velocity profile

% Plot reference velocity along length of the path
plot(cumPathLength, refVelocities, 'LineWidth', 2);

% Plot a line to display maximum speed
hold on
line([0;cumPathLength(end)], [maxSpeed;maxSpeed], 'Color', 'r')
hold off

% Set axes limits
buffer = 2;
xlim([0 cumPathLength(end)]);
ylim([0 maxSpeed + buffer])

% Add labels
xlabel('Cumulative Path Length (m)');
ylabel('Velocity (m/s)');

% Add legend and title
legend('Velocity Profile', 'Max Speed')
title('Generated velocity profile')
end

closeFigures

function closeFigures()
% Close all the figures except the simulator visualization

% Find all the figure objects
figHandles = findobj('Type', 'figure');
for i = 1: length(figHandles)
    if ~strcmp(figHandles(i).Name, 'Automated Valet Parking')
        close(figHandles(i));
    end
end
end

Отображение plotParkingManeuver сгенерированный маневр парковки на costmap

function plotParkingManeuver(costmap, refPath, currentPose, parkPose)
%plotParkingManeuver
% Plot the generated parking maneuver on a costmap.

% Plot the costmap, without inflated areas
plot(costmap, 'Inflation', 'off')

% Plot reference parking maneuver on the costmap
hold on
plot(refPath, 'DisplayName', 'Parking Maneuver')

title('Parking Maneuver')

% Zoom into parking maneuver by setting axes limits
lo = min([currentPose(1:2); parkPose(1:2)]);
hi = max([currentPose(1:2); parkPose(1:2)]);

buffer = 6; % meters

xlim([lo(1)-buffer hi(1)+buffer])
ylim([lo(2)-buffer hi(2)+buffer])
end

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

Функции

Объекты

Похожие темы

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