Напишите алгоритм, чтобы запланировать путь

Создайте функцию, codegenPathPlanner, это использует plannerHybridAStar возразите, чтобы запланировать путь от положения запуска до целевого положения в карте.

function path = codegenPathPlanner(map,startPose,goalPose)
% Copyright 2021 The MathWorks, Inc.
    %#codegen
    % Create a state space object
    stateSpace = stateSpaceSE2;
    
    % Create a state validator object
    validator = validatorOccupancyMap(stateSpace);

    % Create a binary occupancy map and assign the map to the state 
    % validator object.
    validator.Map = binaryOccupancyMap(map);
    
    % Set the validation distance for the validator
    validator.ValidationDistance = 0.01;
    
    % Assign the state validator object to the plannerHybridAStar object
    planner = plannerHybridAStar(validator);
    
    % Compute a path for the given start and goal poses
    pathObj = plan(planner,startPose,goalPose);
    
    % Extract the path poses from the path object
    path = pathObj.States;
end

Эта функция действует как обертка для стандартного Гибрида* вызов планирования пути. Это признает, что стандарт вводит и возвращает путь без коллизий как массив. Поскольку вы не можете использовать объект указателя в качестве ввода или вывода функции, которая поддерживается для генерации кода, создайте объект планировщика в функции. Сохраните codegenPathPlanner функционируйте в своей текущей папке.

Проверьте алгоритм планирования пути в MATLAB

Проверьте алгоритм планирования пути в MATLAB прежде, чем сгенерировать код.

Загрузите карту в рабочую область.

map = load("exampleMaps.mat").simpleMap;

Создайте объект пространства состояний.

stateSpace = stateSpaceSE2;

Создайте объект блока проверки допустимости состояния.

stateValidator = validatorOccupancyMap(stateSpace);

Создайте бинарную карту заполнения и присвойте карту объекту блока проверки допустимости состояния.

stateValidator.Map = binaryOccupancyMap(map);

Установите расстояние валидации для блока проверки допустимости.

stateValidator.ValidationDistance = 0.01;

Инициализируйте plannerHybridAStar объект с объектом блока проверки допустимости состояния.

planner = plannerHybridAStar(stateValidator);

Задайте начальное и целевое положения как векторы[x y theta]. X и Y задают положение в метрах, и theta задает угол ориентации в радианах.

startPose = [5 5 pi/2];
goalPose = [22 4 0];

Запланируйте путь от положения запуска до целевого положения.

plan(planner,startPose,goalPose);

Визуализируйте путь с помощью show функция, и скрывает дерево расширения.

show(planner,Tree="off")
hold on

Сгенерируйте код для алгоритма планирования пути

Можно использовать любого codegen (MATLAB Coder) функция или MATLAB Coder (MATLAB Coder) приложение, чтобы сгенерировать код. В данном примере сгенерируйте файл MEX путем вызова codegen в командной строке MATLAB. Задайте демонстрационные входные параметры для каждого входа к функции с помощью -args опция и func_inputs входной параметр.

Вызовите codegen функционируйте и задайте входные параметры в массиве ячеек. Эта функция создает отдельный codegenPathPlanner_mex функционируйте, чтобы использовать. Можно также произвести код С при помощи options входной параметр. Этот шаг может занять время.

codegen codegenPathPlanner -args {map,startPose,goalPose}

Code generation successful.

Проверьте результаты Используя сгенерированную MEX-функцию

Запланируйте путь путем вызова версии MEX алгоритма планирования пути для заданного положения запуска, и целевого положения и карты.

path = codegenPathPlanner_mex(map,startPose,goalPose);

Визуализируйте путь, вычисленный версией MEX алгоритма планирования пути.

scatter(path(:,1),path(:,2),...
        Marker="o",...
        MarkerFaceColor="b",...
        MarkerEdgeColor="b")
legend("MATLAB Generated Path","Start Pose","Goal Pose","MEX Generated Path")
hold off

Проверяйте эффективность сгенерированного кода

Сравните время выполнения сгенерированной MEX-функции ко времени выполнения вашей исходной функции при помощи timeit.

time = timeit(@() codegenPathPlanner(map,startPose,goalPose))

time = 
              0.1317215226

mexTime = timeit(@() codegenPathPlanner_mex(map,startPose,goalPose))

mexTime = 
              0.0208781226

time/mexTime

ans = 
          6.30906931258273

В этом примере MEX-функция запускается больше чем в пять раз быстрее. Результаты могут варьироваться для вашей системы.

Запланируйте путь в новой карте Используя сгенерированную MEX-функцию

Запланируйте путь новый запуск и целевые положения в новой карте. Размер новой карты должен быть эквивалентным, карта раньше генерировала MEX-функцию.

mapNew = load("exampleMaze.mat").simpleMaze;
startPoseNew = [10 8 pi];
goalPoseNew = [5 22 0];
pathNew = codegenPathPlanner_mex(mapNew,startPoseNew,goalPoseNew);

Визуализируйте новый путь, вычисленный MEX-функцией.

show(binaryOccupancyMap(mapNew))
hold on
scatter(pathNew(:,1),pathNew(:,2),...
        Marker="o",...
        MarkerFaceColor="b",...
        MarkerEdgeColor="b")