Карта распределения памяти при загрузке

Загрузите 3-D карту заполнения uavMapCityBlock.mat, который содержит набор предсгенерированных препятствий в рабочую область. Раздуйте карту однородно на 1 м, чтобы увеличить безопасность пути и составлять размах крыла фиксированного крыла UAV. Карта заполнения находится в системе координат ENU (Восточный Север).

mapData = load("uavMapCityBlock.mat","omap");
omap = mapData.omap;
% Consider unknown spaces to be unoccupied
omap.FreeThreshold = omap.OccupiedThreshold;
inflate(omap,1)
figure("Name","CityBlock")
show(omap)

Установите запускают положение и целевое положение

Используя карту для ссылки, выберите незанятое положение запуска и целевое положение.

startPose = [12 22 25 pi/2];
goalPose = [150 180 35 pi/2];
figure("Name","StartAndGoal")
hMap = show(omap);
hold on
scatter3(hMap,startPose(1),startPose(2),startPose(3),30,"red","filled")
scatter3(hMap,goalPose(1),goalPose(2),goalPose(3),30,"green","filled")
hold off
view([-31 63])

Запланируйте путь с RRT Используя 3-D примитивы движения Dubins

RRT является основанным на дереве планировщиком движения, который создает дерево поиска инкрементно из случайных выборок данного пространства состояний. Дерево в конечном счете охватывает пространство поиска и соединяет начальное состояние и целевое состояние. Соедините два состояния с помощью uavDubinsConnection объект, который удовлетворяет аэродинамическим ограничениям. Используйте validatorOccupancyMap3D объект для проверки столкновения между фиксированным крылом UAV и средой.

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

Этот пример обеспечивает предопределенное пространство состояний, ExampleHelperUavStateSpace, для планирования пути. Пространство состояний задано как [x y z headingAngle], где [x y z] задает положение UAV и headingAngle задает угол рыскания в радианах. Пример использует uavDubinsConnection возразите как кинематическая модель для UAV, который ограничивается максимальным углом вращения, скоростью полета и углом угла тангажа. Создайте объект пространства состояний путем определения максимального угла вращения, скорости полета, и угол угла тангажа ограничивает свойства UAV как пары "имя-значение". Используйте "Bounds" аргумент пары "имя-значение", чтобы задать положение и контуры ориентации UAV как 4 2 матрица, где первые три строки представляют xY-, и z- контуры оси в 3-D карте заполнения и последней строке представляют угол рыскания в области значений [-pi, pi] радианы.

ss = ExampleHelperUAVStateSpace("MaxRollAngle",pi/6,...
                                "AirSpeed",6,...
                                "FlightPathAngleLimit",[-0.1 0.1],...
                                "Bounds",[-20 220; -20 220; 10 100; -pi pi]);

Установите пороговые границы рабочей области на основе целевого целевого положения. Этот порог диктует, насколько большой целевая область цели рабочей области вокруг целевого положения, который используется для выборки смещения подхода области цели рабочей области.

threshold = [(goalPose-0.5)' (goalPose+0.5)'; -pi pi];

Используйте setWorkspaceGoalRegion функционируйте, чтобы обновить целевое положение и область вокруг этого.

setWorkspaceGoalRegion(ss,goalPose,threshold)

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

validatorOccupancyMap3D объект решает, что состояние недопустимо, если xyz-местоположение занято на карте. Движение между двумя состояниями допустимо, только если все промежуточные состояния допустимы, что означает, что UAV не проходит ни через какое занятое местоположение на карте. Создайте validatorOccupancyMap3D объект путем определения пространства состояний возражает и расширенная карта. Затем установите расстояние валидации, в метрах, для интерполяции между состояниями.

sv = validatorOccupancyMap3D(ss,"Map",omap);
sv.ValidationDistance = 0.1;

Настройте планировщика пути RRT

Создайте plannerRRT объект путем определения пространства состояний и блока проверки допустимости состояния как входные параметры. Установите MaxConnectionDistance, GoalBias, и MaxIterations свойства объекта планировщика, и затем задают пользовательскую целевую функцию. Эта целевая функция решает, что путь достиг цели, если Евклидово расстояние до цели ниже порога 5 м.

planner = plannerRRT(ss,sv);
planner.MaxConnectionDistance = 50;
planner.GoalBias = 0.10;  
planner.MaxIterations = 400;
planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3)) < 5);

Выполните планирование пути

Выполните основанное на RRT планирование пути в трехмерном пространстве. Планировщик находит путь, который без коллизий и подходит для рейса фиксированного крыла.

[pthObj,solnInfo] = plan(planner,startPose,goalPose);

Симулируйте UAV после запланированного пути

Визуализируйте запланированный путь. Интерполируйте запланированный путь на основе связей Dubins UAV. Постройте интерполированные состояния как зеленую линию.

Симулируйте рейс UAV с помощью обеспеченной функции помощника, exampleHelperSimulateUAV, который требует, чтобы waypoints, скорость полета, и время достигли цели (на основе скорости полета и длины пути). Функция помощника использует fixedwing модель руководства, чтобы симулировать поведение UAV на основе входных параметров управления, сгенерированных от waypoints. Постройте симулированные состояния как красную линию.

Заметьте, что симулированный рейс UAV отклоняется немного от запланированного пути из-за небольших ошибок отслеживания управления. Кроме того, 3-D путь Dubins принимает мгновенные изменения в углу вращения UAV, но фактические движущие силы имеют более медленный ответ, чтобы прокрутить команды. Один способ компенсировать эту задержку состоит в том, чтобы запланировать пути с более консервативными аэродинамическими ограничениями.

if (solnInfo.IsPathFound)
    figure("Name","OriginalPath")
    % Visualize the 3-D map
    show(omap)
    hold on
    scatter3(startPose(1),startPose(2),startPose(3),30,"red","filled")
    scatter3(goalPose(1),goalPose(2),goalPose(3),30,"green","filled")
    
    interpolatedPathObj = copy(pthObj);
    interpolate(interpolatedPathObj,1000)
    
    % Plot the interpolated path based on UAV Dubins connections
    hReference = plot3(interpolatedPathObj.States(:,1), ...
        interpolatedPathObj.States(:,2), ...
        interpolatedPathObj.States(:,3), ...
        "LineWidth",2,"Color","g");
    
    % Plot simulated UAV trajectory based on fixed-wing guidance model
    % Compute total time of flight and add a buffer
    timeToReachGoal = 1.05*pathLength(pthObj)/ss.AirSpeed;
    waypoints = interpolatedPathObj.States;
    [xENU,yENU,zENU] = exampleHelperSimulateUAV(waypoints,ss.AirSpeed,timeToReachGoal);
    hSimulated = plot3(xENU,yENU,zENU,"LineWidth",2,"Color","r");
    legend([hReference,hSimulated],"Reference","Simulated","Location","best")
    hold off
    view([-31 63])
end

Сглаживайте путь Dubins и симулируйте траекторию UAV

Исходный запланированный путь делает некоторые ненужные повороты при навигации к цели. Упростите 3-D путь Dubins при помощи алгоритма сглаживания пути, которому предоставляют пример, exampleHelperUAVPathSmoothing. Эта функция удаляет промежуточные 3-D положения Дубинса на основе итеративной стратегии. Для получения дополнительной информации о стратегии сглаживания см. [1]. Функция сглаживания соединяет непоследовательные 3-D положения Дубинса друг с другом, пока выполнение так не приводит к столкновению. Сглаженные пути, сгенерированные этим процессом, улучшают характеристики отслеживания для имитационной модели фиксированного крыла. Симулируйте модель UAV фиксированного крыла с ними новый, сглаживавший waypoints.

if (solnInfo.IsPathFound)
    smoothWaypointsObj = exampleHelperUAVPathSmoothing(ss,sv,pthObj);
    
    figure("Name","SmoothedPath")
    % Plot the 3-D map
    show(omap)
    hold on
    scatter3(startPose(1),startPose(2),startPose(3),30,"red","filled")
    scatter3(goalPose(1),goalPose(2),goalPose(3),30,"green","filled")
    
    interpolatedSmoothWaypoints = copy(smoothWaypointsObj);
    interpolate(interpolatedSmoothWaypoints,1000)
    
    % Plot smoothed path based on UAV Dubins connections
    hReference = plot3(interpolatedSmoothWaypoints.States(:,1), ...
        interpolatedSmoothWaypoints.States(:,2), ...
        interpolatedSmoothWaypoints.States(:,3), ...
        "LineWidth",2,"Color","g");
    
    % Plot simulated flight path based on fixed-wing guidance model
    waypoints = interpolatedSmoothWaypoints.States;
    timeToReachGoal = 1.05*pathLength(smoothWaypointsObj)/ss.AirSpeed;
    [xENU,yENU,zENU] = exampleHelperSimulateUAV(waypoints,ss.AirSpeed,timeToReachGoal);
    hSimulated = plot3(xENU,yENU,zENU,"LineWidth",2,"Color","r");
    
    legend([hReference,hSimulated],"SmoothedReference","Simulated","Location","best")
    hold off
    view([-31 63]);
end

Сглаживавший путь намного короче и показывает улучшенное отслеживание в целом.

Ссылки

[1] Борода, Рэндал В. и Тимоти В. Маклэйн. Маленький беспилотный самолет: теория и практика. Принстон, Нью-Джерси: Издательство Принстонского университета, 2012.

[2] Hornung, Армин, Кай М. Верм, Марен Бенневитс, Кирилль Штахнисс и Вольфрам Burgard. “OctoMap: Эффективная Вероятностная 3D Среда Отображения На основе Деревьев октантов”. Автономные Роботы 34, № 3 (апрель 2013): 189–206. https://doi.org/10.1007/s10514-012-9321-0.