Преобразуйте Декартовы состояния в состояния Frenet
cart2frenet( преобразует вектор с 6 элементами Декартовых состояний planner,cartesianStates)[x, y, theta, kappa, speed, acceleration] к вектору с 6 элементами Frenet утверждает [s, ds, dds, l, dl, ddl], где s является длиной дуги от первой точки в ссылочном пути, и l является нормальным расстоянием от самой близкой точки в s на ссылочном пути.
В этом примере показано, как запланировать оптимальную траекторию с помощью trajectoryOptimalFrenet объект.
Создайте и присвойте карту блоку проверки допустимости состояния
Создайте объект блока проверки допустимости состояния для проверки столкновения.
stateValidator = validatorOccupancyMap;
Создайте карту сетки препятствия.
grid = zeros(700,700); grid(296:305,150:175) = 1; grid(286:295,300:325) = 1; grid(306:315,300:325) = 1; grid(296:305,450:475) = 1; grid(286:295,600:625) = 1; grid(306:315,600:625) = 1;
Создайте binaryOccupancyMap с картой сетки.
map = binaryOccupancyMap(grid);
Присвойте карту блоку проверки допустимости состояния.
stateValidator.Map = map;
Запланируйте и визуализируйте траекторию
Создайте ссылочный путь для планировщика, чтобы следовать.
refPath = [10,400;700,400];
Объявите, что указатель функции стоимости приоритизирует оставленный изменения маршрута.
leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10);
Инициализируйте объект планировщика со ссылочным путем, блоком проверки допустимости состояния и пользовательской функцией стоимости.
plannerObj = trajectoryOptimalFrenet(refPath,stateValidator,'CostFunction',leftLaneChangeCost);Присвойте боковые значения отклонения.
plannerObj.TerminalStates.Lateral = -10:10:10;
Генерация траектории
Инициализируйте переменные для цикла перепланирования.
previousDeviationValue = 0; deviationValue = 0; initState = zeros(1,6);
Повторно запланируйте, пока итоговый waypoint не является самым близким к терминальному состоянию.
for j = 1:150 % Deviation from the current reference path if initState(4) > 5 deviationValue = 10; elseif initState(4) < -5 deviationValue = -10; end % Move reference path to the current lane if previousDeviationValue ~= deviationValue % Shift the waypoints by the deviation value plannerObj.Waypoints = [plannerObj.Waypoints(:,1), plannerObj.Waypoints(:,2) + deviationValue]; % Shift the terminal states such that they remain fixed in world frame plannerObj.TerminalStates.Lateral = plannerObj.TerminalStates.Lateral - deviationValue; % Store the deviation value previousDeviationValue = deviationValue; % Update initState variable with the new planner object initState = cart2frenet(plannerObj,trajectory(10,1:6)); end % Generate a trajectory from initState trajectory = plan(plannerObj,initState); % Use 10th state of the current trajectory as a starting point for % replanning initState = cart2frenet(plannerObj,trajectory(10,1:6)); % Visualize every 5th iteration if mod(j,5)==0 show(map) hold on; show(plannerObj,'TrajectoryColor','none'); drawnow end end
