Загрузка и присвоение карты для валидатора состояний

Загрузите 3-D карту заполнения городского блока в рабочую область. Задайте порог, для которого камеры должны считаться свободными от препятствий.

mapData = load('dMapCityBlock.mat');
omap = mapData.omap;
omap.FreeThreshold = 0.5;

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

inflate(omap,5)

Создайте объект пространства состояний SE (3) с границами для переменных состояний.

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

Создайте 3-D средство проверки состояния карты заполнения с помощью созданного пространства состояний.

sv = validatorOccupancyMap3D(ss);

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

sv.Map = omap;
sv.ValidationDistance = 0.1;

Планирование и визуализация пути

Создайте планировщик пути с увеличенным максимальным расстоянием соединения. Уменьшите максимальное количество итераций.

planner = plannerRRT(ss,sv);
planner.MaxConnectionDistance = 50;
planner.MaxIterations = 1000;

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

planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3))<5);
planner.GoalBias = 0.1;

Установите начальное и целевое состояния.

start = [40 180 25 0.7 0.2 0 0.1];
goal = [150 33 35 0.3 0 0.1 0.6];

Планируйте путь с помощью указанного начала, цели и планировщика.

[pthObj,solnInfo] = plan(planner,start,goal);

Проверяйте, что точки пути являются допустимыми состояниями.

isValid = isStateValid(sv,pthObj.States)

isValid = 6x1 logical array

   1
   1
   1
   1
   1
   1

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

isPathValid = zeros(size(pthObj.States,1)-1,1,'logical');
for i = 1:size(pthObj.States,1)-1
    [isPathValid(i),~] = isMotionValid(sv,pthObj.States(i,:),...
        pthObj.States(i+1,:));
end
isPathValid

isPathValid = 5x1 logical array

   1
   1
   1
   1
   1

Визуализация результатов.

show(omap)
hold on
scatter3(start(1,1),start(1,2),start(1,3),'g','filled') % draw start state
scatter3(goal(1,1),goal(1,2),goal(1,3),'r','filled')    % draw goal state
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),...
    'r-','LineWidth',2) % draw path