Найдите самую близкую точку на ссылочном пути к глобальной точке
pathPoints = closestPoint(refPath,points)points.
Сгенерируйте ссылочный путь от набора waypoints.
waypoints = [0 0; 50 20; 100 0; 150 10]; refPath = referencePathFrenet(waypoints);
Создайте объект trajectoryGeneratorFrenet из ссылочного пути.
connector = trajectoryGeneratorFrenet(refPath);
Сгенерируйте пять вторых траекторий между источником пути и точкой 30 m вниз путь, как Frenet утверждает.
initState = [0 0 0 0 0 0]; % [S dS ddS L dL ddL] termState = [30 0 0 0 0 0]; % [S dS ddS L dL ddL] frenetTraj = connect(connector,initState,termState,5);
Преобразуйте траекторию в глобальные состояния.
globalTraj = frenet2global(refPath,frenetTraj.Trajectory);
Отобразите ссылочный путь и траекторию.
show(refPath); axis equal hold on plot(globalTraj(:,1),globalTraj(:,2),'b')
Задайте глобальные точки и найдите самые близкие точки на ссылочном пути.
globalPoints = waypoints(2:end,:) + [20 -50]; nearestPathPoint = closestPoint(refPath,globalPoints);
Отобразите глобальные точки и самые близкие точки на ссылочном пути.
plot(globalPoints(:,1),globalPoints(:,2),'r*','MarkerSize',10) plot(nearestPathPoint(:,1),nearestPathPoint(:,2),'b*','MarkerSize',10)
Интерполируйте между длинами дуги первых двух самых близких точек вдоль ссылочного пути.
arclengths = linspace(nearestPathPoint(1,6),nearestPathPoint(2,6),10); pathStates = interpolate(refPath,arclengths);
Отобразите интерполированные точки контура.
plot(pathStates(:,1),pathStates(:,2),'g') legend(["Waypoints","Reference Path","Trajectory to 30m",... "Global Points","Closest Points","Interpolated Path Points"])
