Запланируйте минимум привязывают траекторию для Quadrotor

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

Скрипт Open Live Script

В этом примере показано, как запланировать минимальную поспешную траекторию (минимум управляют усилием) для беспилотного воздушного транспортного средства (UAV) мультиротора от положения запуска до целевого положения на 3D карте при помощи оптимизированного быстро исследующего случайного дерева (RRT*) планировщик пути.

В этом примере вы настраиваете 3D карту, обеспечиваете положение запуска и целевое положение, планируете путь с RRT* использование 3D связей прямой линии и соответствуете минимальной поспешной траектории через полученный waypoints.

Начальная настройка

Сконфигурируйте генератор случайных чисел для повторяемого результата.

rng(100,"twister")

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

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

mapData = load("uavMapCityBlock.mat");
omap = mapData.omap;

% Consider unknown spaces to be unoccupied
omap.FreeThreshold = omap.OccupiedThreshold;
show(omap)

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

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

startPose = [12 22 25 0.7 0.2 0 0.1];  % [x y z qw qx qy qz]
goalPose = [150 180 35 0.3 0 0.1 0.6];

% Plot the start and goal poses
hold on
scatter3(startPose(1),startPose(2),startPose(3),100,".r")
scatter3(goalPose(1),goalPose(2),goalPose(3),100,".g")
view([-31 63])
legend("","Start Position","Goal Position")
hold off

Figure contains an axes object. The axes object with title Occupancy Map contains 3 objects of type patch, scatter. These objects represent Start Position, Goal Position.

Запланируйте путь с RRT* Используя SE (3) пространство состояний

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

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

stateSpaceSE3 объект задает пространство состояний как [x y z qw qx qy qz], где [x y z] задает положение UAV в метрах и [qw qx qy qz] задает ориентацию как кватернион. Задайте положение и контуры ориентации quadrotor как 7 2 матрица. Контуры ориентации являются дополнительными, и могут быть установлены в -Inf и Inf если они не применимы.

ss = stateSpaceSE3([-20 220;
                    -20 220;
                    -10 100;
                    inf inf;
                    inf inf;
                    inf inf;
                    inf inf]);

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

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

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

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

Создайте plannerRRTStar объект путем определения пространства состояний и блока проверки допустимости состояния как входные параметры. Установите MaxConnectionDistance, GoalBias'MaxIterations' , ContinueAfterGoalReached, и MaxNumTreeNodes свойства планировщика возражают, чтобы оптимизировать возвращенный waypoints.

planner = plannerRRTStar(ss,sv);
planner.MaxConnectionDistance = 50;
planner.GoalBias = 0.8;
planner.MaxIterations = 1000;
planner.ContinueAfterGoalReached = true;
planner.MaxNumTreeNodes = 10000;

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

Выполните RRT* базирующееся планирование пути на 3D пробеле, чтобы получить waypoints. Планировщик находит угол тангажа, который без коллизий и подходит для quadrotor. Информация о решении полезна для настройки планировщика.

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

Визуализируйте путь

Проверяйте, нашел ли RRT* планировщик путь. Если планировщик нашел путь, постройте waypoints. В противном случае отключите скрипт.

if (~solnInfo.IsPathFound)
    disp("No Path Found by the RRT, terminating example")
    return
end

% Plot map, start pose, and goal pose
show(omap)
hold on
scatter3(startPose(1),startPose(2),startPose(3),100,".r")
scatter3(goalPose(1),goalPose(2),goalPose(3),100,".g")

% Plot path computed by path planner
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),"-g")
view([-31 63])
legend("","Start Position","Goal Postion","Planned Path")
hold off

Figure contains an axes object. The axes object with title Occupancy Map contains 4 objects of type patch, scatter, line. These objects represent Start Position, Goal Postion, Planned Path.

Сгенерируйте траекторию UAV привязки минимума

Исходный запланированный путь имеет некоторые острые углы при навигации к цели. Сгенерируйте сглаженную траекторию путем подбора кривой полученному waypoints к минимальной поспешной траектории с помощью minsnappolytraj функция. Для вашей первоначальной оценки времени, требуемого достигнуть каждого waypoint, примите перемещения UAV на постоянной скорости.

Чтобы настроить траекторию и время полета, задайте numSamples, TimeAllocation, и TimeWeight аргументы minsnappolytraj функция.

waypoints = pthObj.States;
nWayPoints = pthObj.NumStates;

% Calculate the distance between waypoints
distance = zeros(1,nWayPoints);
for i = 2:nWayPoints
    distance(i) = norm(waypoints(i,1:3) - waypoints(i-1,1:3));
end

% Assume a UAV speed of 3 m/s and calculate time taken to reach each waypoint
UAVspeed = 3;
timepoints = cumsum(distance/UAVspeed);
nSamples = 100;

% Compute states along the trajectory
initialStates = minsnappolytraj(waypoints',timepoints,nSamples,MinSegmentTime=4,MaxSegmentTime=20,TimeAllocation=true,TimeWeight=100)';

Визуализируйте траекторию

Визуализируйте полученную минимальную поспешную траекторию.

% Plot map, start pose, and goal pose
show(omap)
hold on
scatter3(startPose(1),startPose(2),startPose(3),30,".r")
scatter3(goalPose(1),goalPose(2),goalPose(3),30,".g")

% Plot the waypoints
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),"-g")

% Plot the minimum snap trajectory
plot3(initialStates(:,1),initialStates(:,2),initialStates(:,3),"-y")
view([-31 63])
legend("","Start Position","Goal Postion","Planned Path","Initial Trajectory")
hold off

Figure contains an axes object. The axes object with title Occupancy Map contains 5 objects of type patch, scatter, line. These objects represent Start Position, Goal Postion, Planned Path, Initial Trajectory.

Сгенерируйте допустимую минимальную поспешную траекторию

Заметьте, что сгенерированная траектория рейса имеет некоторые недопустимые состояния, которые не без препятствий. Необходимо изменить waypoints так, чтобы сгенерированная траектория была без препятствий. Чтобы избежать недопустимых состояний, добавьте промежуточное звено waypoints на сегменте между парой waypoints, для которого траектория недопустима. Вставьте waypoints итеративно, пока целая траектория не будет допустима.

% Check if the trajectory is valid
states = initialStates;
valid = all(isStateValid(sv,states));

while(~valid)
    % Check the validity of the states
    validity = isStateValid(sv,states);

    % Map the states to the corresponding waypoint segments
    segmentIndices = exampleHelperMapStatesToPathSegments(waypoints,states);

    % Get the segments for the invalid states
    % Use unique, because multiple states in the same segment might be invalid
    invalidSegments = unique(segmentIndices(~validity));

    % Add intermediate waypoints on the invalid segments
    for i = 1:size(invalidSegments)
        segment = invalidSegments(i);
        
        % Take the midpoint of the position to get the intermediate position
        midpoint(1:3) = (waypoints(segment,1:3) + waypoints(segment+1,1:3))/2;
        
        % Spherically interpolate the quaternions to get the intermediate quaternion
        midpoint(4:7) = slerp(quaternion(waypoints(segment,4:7)),quaternion(waypoints(segment+1,4:7)),.5).compact;
        waypoints = [waypoints(1:segment,:); midpoint; waypoints(segment+1:end,:)];

    end

    nWayPoints = size(waypoints,1);
    distance = zeros(1,nWayPoints);
    for i = 2:nWayPoints
        distance(i) = norm(waypoints(i,1:3) - waypoints(i-1,1:3));
    end
    
    % Calculate the time taken to reach each waypoint
    timepoints = cumsum(distance/UAVspeed);
    nSamples = 100;
    states = minsnappolytraj(waypoints',timepoints,nSamples,MinSegmentTime=2,MaxSegmentTime=20,TimeAllocation=true,TimeWeight=5000)';    
    
    % Check if the new trajectory is valid
    valid = all(isStateValid(sv,states));
   
end

Визуализируйте итоговую допустимую траекторию

Визуализируйте итоговую допустимую минимальную поспешную траекторию.

% Plot map, start and goal pose
show(omap)
hold on
scatter3(startPose(1),startPose(2),startPose(3),30,".r")
scatter3(goalPose(1),goalPose(2),goalPose(3),30,".g")

% Plot the waypoints
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),"-g")

% Plot the initial trajectory
plot3(initialStates(:,1),initialStates(:,2),initialStates(:,3),"-y")

% Plot the final valid trajectory
plot3(states(:,1),states(:,2),states(:,3),"-c")
view([-31 63])
legend("","Start Position","Goal Postion","Planned Path","Initial Trajectory","Valid Trajectory")
hold off

Figure contains an axes object. The axes object with title Occupancy Map contains 6 objects of type patch, scatter, line. These objects represent Start Position, Goal Postion, Planned Path, Initial Trajectory, Valid Trajectory.

Документация UAV Toolbox

Поддержка

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