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