Сглаживание траектории транспортного средства с помощью интерполяции кубических сплайнов
[ создает гладкий путь транспортного средства, состоящий из 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 *.
Погрузите и постройте карту парковки.
data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;
plot(costmap)
Определите начальные и целевые позы для транспортного средства как векторы [x, y, Start]. Мировые единицы измерения для местоположений (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-continuous, если его производная существует и является непрерывной. Контуры, C1-continuous только имеют разрывы кривизны. Например, траектория, состоящая из сегментов Dubins или Reeds-Sheep, имеет разрывы кривизны в точках соединения сегментов. Эти разрывы приводят к изменениям в направлении, которые недостаточно гладки для движения с пассажирами.
Путь также C2-continuous, если его вторая производная существует и является непрерывной. C2-continuous дорожки имеют непрерывную кривизну и достаточно гладкие для движения с пассажирами.
Чтобы проверить, свободен ли плавный путь от коллизий, укажите плавные представления в качестве входных данных для checkPathValidity функция.
Алгоритм сглаживания траектории интерполирует параметрический кубический сплайн, который проходит через все входные опорные точки позы. Параметр сплайна - кумулятивная длина хорды в этих точках. [1]
Касательное направление сглаженной выходной траектории приблизительно соответствует углу ориентации транспортного средства в начальной и целевой позициях.
[1] Поплавок, Майкл С. «Об отклонении параметрического кубического сплайна от его полигона данных». Автоматизированное геометрическое проектирование. Том 25, номер 3, 2008, стр. 148-156.
[2] Лепетик, Марко, Грегор Кланкар, Игорь Скрыан, Драго Матко и Бостьян Поточник. «Планирование оптимального пути времени с учетом пределов ускорения». Робототехника и автономные системы. Том 45, номера 3-4, 2003, стр. 199-210.