plan

Запланируйте оптимальную траекторию

свернуть все на странице

Синтаксис

[traj,index,cost] = plan(planner,start)

Описание

пример

[traj,index,cost] = plan(planner,start) вычисляет выполнимую траекторию, traj, из списка траекторий кандидата, сгенерированных от trajectoryOptimalFrenet объект, plannerзапуск задан как векторный [s, ds, dds, l, dl, ddl] с 6 элементами, где s является длиной дуги от первой точки в ссылочном пути, и l является нормальным расстоянием от самой близкой точки в s на ссылочном пути. Выходная траектория, traj, также имеет связанный cost и index для TrajectoryList свойство планировщика. Чтобы улучшить результаты планирования выход, измените параметры на planner объект.

Примеры

свернуть все

Этот пример использует:

Скрипт Open Live Script

В этом примере показано, как запланировать оптимальную траекторию с помощью 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

Входные параметры

свернуть все

Оптимальный планировщик траектории на пробеле сети с бесплатным доступом, заданном как trajectoryOptimalFrenet объект.

Начальное состояние Frenet, заданное как 1 6 вектор, [s, ds, dds, l, dl, ddl]. s задает длину дуги от первой точки в ссылочном пути в метрах, ds задает первую производную длины дуги, dds задает вторую производную длины дуги, l задает нормальное расстояние от самой близкой точки в ссылочном пути, dl задает первую производную нормального расстояния, и ddl задает вторую производную нормального расстояния.

Выходные аргументы

свернуть все

Выполнимая траектория с минимальной стоимостью, возвращенной как n-by-7 матрица [x, y, theta, kappa, speed, acceleration], где n является количеством выполнимой траектории.

Индекс выполнимой траектории с наименьшим количеством стоимостью, повторно настроенной как положительный целочисленный скаляр.

Наименьшее количество стоимости выполнимой траектории, повторно настроенной как положительная скалярная величина.

Расширенные возможности

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

Смотрите также

|

Введенный в R2019b

Документация Navigation Toolbox
Поддержка
Для просмотра документации необходимо авторизоваться на сайте