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