Сглаживайте путь к транспортному средству с помощью кубической сплайн интерполяции
[ генерирует гладкий путь транспортного средства, состоящий из 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)
Задайте начальное и целевое положения для транспортного средства как векторы [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])
Интерполируйте переходные положения пути. Используйте эти положения как опорные положения для интерполяции сглаженного пути. Также верните направления движения в каждом положении.
[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
Путь есть 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.