smoothPathSpline

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

Синтаксис

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses)
[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses,minSeparation)
[___,cumLength,curvatures] = 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, функция использует только одно из положений для интерполяции.

[___,cumLength,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-by-3 матрица [x, y, Θ] векторы.

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

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

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

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

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

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

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

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

Больше о

свернуть все

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

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