smoothPathSpline

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

Описание

пример

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses) генерирует сглаженный путь к транспортному средству, состоя из numSmoothPoses дискретизированные положения, путем подбора кривой входному пути к ссылке позируют к кубическому сплайну. Учитывая входные направления контура ссылки, smoothPathSpline также возвращает направления, которые соответствуют каждому положению.

Используйте эту функцию, чтобы преобразовать путь к транспортному средству C1-continuous к пути C2-continuous. Пути C1-continuous включают driving.DubinsPathSegment или driving.ReedsSheppPathSegment пути, что можно запланировать использование pathPlannerRRT объект. Для получения дополнительной информации об этих типах пути см. C1-Continuous и Пути C2-Continuous.

Можно использовать возвращенные положения и направления с контроллером транспортного средства, такой как lateralControllerStanley функция.

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses,minSeparation) задает минимальный разделительный порог между положениями. Если расстояние между двумя положениями меньше, чем minSeparation, функция использует только одно из положений для интерполяции.

[___,cumLengths,curvatures] = smoothPathSpline(___) также возвращает совокупную длину пути и подписанное искривление пути в каждом возвращенном положении, с помощью любого из предыдущих синтаксисов. Используйте эти значения, чтобы сгенерировать скоростной профиль вдоль пути.

Примеры

свернуть все

Сглаживайте путь, который был запланирован RRT* планировщик пути.

Загрузите и постройте costmap парковки.

data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;
plot(costmap)

Задайте запускаются и целевые положения для транспортного средства как [x, y, Θ] векторы. Мировые единицы измерения для (x, y) местоположения исчисляются в метрах. Мировые единицы измерения для Θ углов ориентации в градусах.

startPose = [4,4,90]; % [meters, meters, degrees]
goalPose = [30,13,0];

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

planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);

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

hold on
plot(refPath,'Vehicle','off','DisplayName','Reference path')
xlim([3 31])
ylim([3 18])

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

[refPoses,refDirections] = interpolate(refPath);

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

approxSeparation = 0.1; % meters
numSmoothPoses = round(refPath.Length / approxSeparation);

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

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses);

Постройте сглаженный путь. Более резкие изменения в искривлении, которые присутствовали в ссылочном пути, теперь сглаживаются.

plot(poses(:,1),poses(:,2),'LineWidth',2,'DisplayName','Smooth path')
hold off

Входные параметры

свернуть все

Ссылочные положения транспортного средства вдоль пути в виде M-by-3 матрица [x, y, Θ] векторы, где M является количеством положений.

x и y задают местоположение транспортного средства в метрах. Θ задает угол ориентации транспортного средства в градусах.

Типы данных: single | double

Ссылочные направления транспортного средства вдоль пути в виде M-by-1 вектор-столбец 1 с (движение вперед) и –1s (инвертируют движение). M является количеством ссылочных направлений. Каждый элемент refDirections соответствует положению в refPoses входной параметр.

Типы данных: single | double

Количество сглаженных положений, чтобы возвратиться в poses выходной аргумент в виде положительного целого числа. Чтобы увеличить гранулярность возвращенных положений, увеличьте numSmoothPoses.

Минимальное разделение между положениями, в метрах в виде положительного действительного скаляра. Если Евклидово (x, y) расстояние между двумя положениями меньше этого значения, то функция использует только одно из этих положений для интерполяции.

Выходные аргументы

свернуть все

Дискретизированные положения сглаживавшего пути, возвращенного как numSmoothPoses- 3 матрицы [x, y, Θ] векторы.

x и y задают местоположение транспортного средства в метрах. Θ задает угол ориентации транспортного средства в градусах.

Значения в poses имеют совпадающий тип данных как значения в refPoses входной параметр.

Направления движения при каждом выходе позируют в poses, возвращенный как numSmoothPoses- 1 вектор-столбец 1 с (движение вперед) и –1s (инвертируют движение).

Значения в directions имеют совпадающий тип данных как значения в refDirections входной параметр.

Совокупная длина пути при каждом выходе позирует в poses, возвращенный как numSmoothPoses- 1 вектор-столбец с действительным знаком. Модули исчисляются в метрах.

Можно использовать cumLengths и curvatures выходные параметры, чтобы сгенерировать скоростной профиль транспортного средства вдоль сглаженного пути. Для получения дополнительной информации смотрите, что Автоматизированный Паркует пример Камердинера.

Искривления пути со знаком при каждом выходе позируют в poses, возвращенный как numSmoothPoses- 1 вектор-столбец с действительным знаком. Модули исчисляются в радианах на метр.

Можно использовать curvatures и cumLengths выходные параметры, чтобы сгенерировать скоростной профиль транспортного средства вдоль сглаженного пути. Для получения дополнительной информации смотрите, что Автоматизированный Паркует пример Камердинера.

Больше о

свернуть все

C1-Continuous и пути C2-Continuous

Путь является C1-continuous, если его производная существует и непрерывна. Пути, которые являются только C1-continuous, имеют разрывы в своем искривлении. Например, путь, состоявший из сегментов пути Dubins или Reeds-Sheep, имеет разрывы в искривлении в точках, где сегменты присоединяются. Эти разрывы приводят к изменениям направления, которые не являются гладкими достаточно для управления с пассажирами.

Путем является также C2-continuous, если его вторая производная существует и непрерывна. Пути C2-continuous имеют непрерывное искривление и являются гладкими достаточно для управления с пассажирами.

Советы

  • Чтобы проверять, без коллизий ли сглаженный путь, укажите, что сглаженное изображает из себя вход к checkPathValidity функция.

Алгоритмы

  • Алгоритм сглаживания пути интерполирует параметрический кубический сплайн, который проходит через все входные точки положения ссылки. Параметр сплайна является совокупной длиной хорды в этих точках. [1]

  • Направление касательной сглаживавшего выхода path приблизительно совпадает с углом ориентации транспортного средства при запуске и целевых положениях.

Ссылки

[1] Плавающий предмет, Майкл С. "На Отклонении Параметрического Кубического Сплайна Interpolant от Его Многоугольника Данных". Компьютер помог Геометрическому Проекту. Издание 25, Номер 3, 2008, стр 148–156.

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

Расширенные возможности

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

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