Проверьте, является ли состояние допустимым
В этом примере показано, как проверить пути через среду видимости.
Загрузите карты примера. Используйте простую карту, чтобы создать двоичную карту заполнения.
load exampleMaps.mat
map = occupancyMap(simpleMap);
show(map)
Задайте грубый путь через карту.
path = [2 2 pi/2; 10 15 0; 17 8 -pi/2]; hold on plot(path(:,1),path(:,2),"--o")
Создайте валидатор состояния, используя stateSpaceSE2 определение. Укажите карту и расстояние для интерполяции и проверки сегментов контура.
validator = validatorOccupancyMap(stateSpaceSE2); validator.Map = map; validator.ValidationDistance = 0.1;
Проверяйте, что точки пути являются допустимыми состояниями. Все три точки находятся в свободном пространстве, поэтому считаются действительными.
isValid = isStateValid(validator,path)
isValid = 3x1 logical array
1
1
1
Проверяйте движение между каждыми последовательными состояниями пути. The isMotionValid функция интерполируется вдоль пути между состояниями. Если сегмент контура недопустим, постройте график последней допустимой точки вдоль пути.
startStates = [path(1,:);path(2,:)];
endStates = [path(2,:);path(3,:)];
for i = 1:2
[isPathValid, lastValid] = isMotionValid(validator,startStates(i,:),endStates(i,:));
if ~isPathValid
plot(lastValid(1),lastValid(2),'or')
end
end
hold off
В этом примере показано, как проверить пути через среду видимости.
Загрузите карты примера. Используйте простую карту, чтобы создать карту стоимости транспортного средства. Укажите инфляционный рейд на 1 метр.
load exampleMaps.mat map = vehicleCostmap(double(simpleMap)); map.CollisionChecker = inflationCollisionChecker("InflationRadius",1); plot(map)
Задайте грубый путь через карту.
path = [3 3 pi/2; 8 15 0; 17 8 -pi/2]; hold on plot(path(:,1),path(:,2),"--o")
Создайте валидатор состояния, используя stateSpaceSE2 определение. Укажите карту и расстояние для интерполяции и проверки сегментов контура.
validator = validatorVehicleCostmap(stateSpaceSE2); validator.Map = map; validator.ValidationDistance = 0.1;
Проверяйте, что точки пути являются допустимыми состояниями. Все три точки находятся в свободном пространстве, поэтому считаются действительными.
isValid = isStateValid(validator,path)
isValid = 3x1 logical array
1
1
1
Проверяйте движение между каждыми последовательными состояниями пути. The isMotionValid функция интерполируется вдоль пути между состояниями. Если сегмент контура недопустим, постройте график последней допустимой точки вдоль пути.
startStates = [path(1,:);path(2,:)];
endStates = [path(2,:);path(3,:)];
for i = 1:2
[isPathValid, lastValid] = isMotionValid(validator,startStates(i,:),endStates(i,:));
if ~isPathValid
plot(lastValid(1),lastValid(2),'or')
end
end
hold off
Создайте 3-D карту заполнения и связанный с ней валидатор состояния. Планируйте, проверяйте и визуализируйте путь через карту заполнения.
Загрузка и присвоение карты для валидатора состояний
Загрузите 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
isMotionValid | nav.StateSpace | nav.StateValidator | stateSpaceSE2