Загрузка карты

Загрузите 3-D карту заполнения uavMapCityBlock.mat, который содержит набор предгенерированных препятствий, в рабочую область. Равномерно надьте карту на 1 м для повышения безопасности пути и учета размаха крыльев БПЛА фиксированного типа. Карта заполнения находится в системе координат ENU (East-North-Up).

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 Motion Primitives

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

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

Этот пример предоставляет предопределенное пространство состояний, ExampleHelperUavStateSpace, для планирования пути. Пространство состояний определяется как [x y z headingAngle], где [x y z] определяет положение БПЛА и headingAngle задает угол рыскания в радианах. В примере используется uavDubinsConnection объект как кинематическая модель для БПЛА, которая ограничена максимальным углом крена, воздушной скоростью и углом угла тангажа. Создайте космический объект состояния путем определения максимальных свойств угла крена, скорости полета и пределов угла угла тангажа БПЛА как пары "имя-значение". Используйте "Bounds" аргумент пары "имя-значение" для определения контуров положения и ориентации БПЛА как матрицы 4 на 2, где первые три строки представляют x-, y-, и 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)

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

The validatorOccupancyMap3D объект определяет, что состояние недопустимо, если местоположение xyz занято на карте. Движение между двумя состояниями действительно только, если все промежуточные состояния действительны, что означает, что БПЛА не проходит через какое-либо занятое место на карте. Создайте 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);

Симулируйте БПЛА, следующий по запланированному пути

Визуализация запланированного пути. Интерполяция запланированного пути на основе соединений БПЛА Dubins. Постройте график интерполированных состояний как зеленой линии.

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

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

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

Сглаживайте путь Дубинса и моделируйте траекторию БПЛА

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

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] Борода, Рэндал У. и Тимоти У. Маклейн. Малые беспилотные самолеты: теория и практика. Princeton, N.J: Princeton University Press, 2012.

[2] Hornung, Armin, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss и Wolfram Burgard. OctoMap: эффективная вероятностная среда 3D отображения на основе октреев. Автономные роботы 34, № 3 (апрель 2013): 189-206. https://doi.org/10.1007/s10514-012-9321-0.