Найдите оптимальную траекторию для ссылочного пути
trajectoryOptimalFrenet объект генерирует оптимальную, выполнимую, и траекторию без коллизий для ссылочного пути.
plan функция вычисляет оптимальную траекторию между start и терминальные состояния. Функциональные выборки несколько траекторий для каждой пары состояний и выбирают выполнимую траекторию с наименьшим количеством стоимостью.
planner = trajectoryOptimalFrenet(refPath,validator)trajectoryOptimalFrenet объект со ссылочным путем, refPath, в форме n-by-2 массив [x y]
waypoints и блок проверки допустимости состояния, validator, заданный как validatorOccupancyMap объект.
cart2frenet | Преобразуйте Декартовы состояния в состояния Frenet |
frenet2cart | Преобразуйте состояния Frenet в Декартовы состояния |
plan | Запланируйте оптимальную траекторию |
show | Визуализация траекторий |
В этом примере показано, как запланировать оптимальную траекторию с помощью 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
Самопересечения в ссылочном пути могут привести к неожиданному поведению.
Планировщик не поддерживает противоположное управление.
Начальная ориентация для планирования должна быть в -pi/2 и pi/2 к ссылочному пути.
Контролируйте количество TerminalStates для приложений реального времени, поскольку вычислительная сложность растет с ним.
[1] Werling, Мориц, Юлиус Циглер, Серен Каммель и Себастиан Трун. "Оптимальная генерация траектории для динамических уличных сценариев в системе координат сети с бесплатным доступом". 2 010 Международных конференций IEEE по вопросам Робототехники и Автоматизации. 2010, стр 987–993.