smoothPathSpline

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

Описание

пример

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

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

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

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

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

Примеры

свернуть все

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

Загрузка и построение косметики парковки.

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

Figure contains an axes. The axes contains 2 objects of type image, patch. This object represents Inflated Areas.

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

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

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

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

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

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

Figure contains an axes. The axes contains 4 objects of type image, patch, scatter, line. These objects represent Inflated Areas, Reference path.

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

[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

Figure contains an axes. The axes contains 5 objects of type image, patch, scatter, line. These objects represent Inflated Areas, Reference path, Smooth path.

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

свернуть все

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

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

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

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

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

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

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

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

свернуть все

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

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

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

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

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

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

Можно использовать cumLengths и curvatures формирует профиль скорости транспортного средства вдоль гладкого пути. Для получения дополнительной информации см. пример Automated Parking Valet.

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

Можно использовать curvatures и cumLengths формирует профиль скорости транспортного средства вдоль гладкого пути. Для получения дополнительной информации см. пример Automated Parking Valet.

Подробнее о

свернуть все

C1- непрерывный и C2- непрерывные пути

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

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

Совет

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

Алгоритмы

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

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

Ссылки

[1] Floater, Michael S. «On the Deviation of a Parametric Cubic Spline Interpolant from Its Data Polygon». Автоматизированный геометрический проект. Том 25, № 3, 2008, стр. 148-156.

[2] Лепетич, Марко, Грегор Кланкар, Игорь Скрянц, Драго Матко, и Бостьян Потокник. «Оптимальное по времени планирование пути с учетом пределов ускорения». Робототехника и автономные системы. Том 45, номера 3-4, 2003, стр. 199-210.

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

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

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