Отобразите ссылочный путь на рисунке
Сгенерируйте альтернативные траектории для ссылочного использования пути координаты Frenet. Задайте различную начальную букву и конечные состояния для ваших траекторий. Настройте свои состояния на основе сгенерированных траекторий.
Сгенерируйте ссылочный путь от набора waypoints. Создайте trajectoryGeneratorFrenet объект от ссылочного пути.
waypoints = [0 0; ... 50 20; ... 100 0; ... 150 10]; refPath = referencePathFrenet(waypoints); 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] [~,trajGlobal] = connect(connector,initState,termState,5);
Отобразите траекторию в глобальных координатах.
show(refPath); hold on axis equal plot(trajGlobal.Trajectory(:,1),trajGlobal.Trajectory(:,2),'b') legend(["Waypoints","Reference Path","Trajectory to 30m"])
Создайте матрицу конечных состояний с боковыми отклонениями между-3 м и 3 м. Сгенерируйте траектории, которые покрывают ту же длину дуги за 10 секунд, но отклоняются со стороны от ссылочного пути. Отобразите новые альтернативные пути.
termStateDeviated = termState + ([-3:3]' * [0 0 0 1 0 0]); [~,trajGlobal] = connect(connector,initState,termStateDeviated,10); clf show(refPath); hold on axis equal for i = 1:length(trajGlobal) plot(trajGlobal(i).Trajectory(:,1),trajGlobal(i).Trajectory(:,2),'g') end legend(["Waypoints","Reference Path","Alternative Trajectories"]) hold off
Задайте новое конечное состояние, чтобы сгенерировать новую траекторию. Эта траектория не желательна, потому что она требует противоположному движению достигнуть поперечной скорости 10 м/с.
newTermState = [5 10 0 5 0 0]; [~,newTrajGlobal] = connect(connector,initState,newTermState,3); clf show(refPath); hold on axis equal plot(newTrajGlobal.Trajectory(:,1),newTrajGlobal.Trajectory(:,2),'b'); legend(["Waypoint","Reference Path","New Trajectory"]) hold off
Ослабьте ограничение на продольном состоянии путем определения длины дуги NaN. Сгенерируйте и отобразите траекторию снова. Новое положение показывает хорошую альтернативную траекторию, которая отклоняется от ссылочного пути.
relaxedTermState = [NaN 10 0 5 0 0]; [~,trajGlobalRelaxed] = connect(connector,initState,relaxedTermState,3); clf show(refPath); hold on axis equal plot(trajGlobalRelaxed.Trajectory(:,1),trajGlobalRelaxed.Trajectory(:,2),'g'); hold off
