isMotionValid

Проверяйте, допустим ли путь между состояниями

Описание

пример

[isValid,lastValid] = isMotionValid(validator,state1,state2) проверки, если путь между двумя состояниями допустим путем интерполяции между состояниями. Функция также возвращает последнее допустимое состояние вдоль пути.

Примеры

свернуть все

В этом примере показано, как подтвердить пути через evironment.

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

load exampleMaps.mat
map = occupancyMap(simpleMap);
show(map)

Figure contains an axes. The axes with title Occupancy Grid contains an object of type image.

Задайте крупный путь через карту.

path = [2 2 pi/2; 10 15 0; 17 8 -pi/2];
hold on
plot(path(:,1),path(:,2),"--o")

Figure contains an axes. The axes with title Occupancy Grid contains 2 objects of type image, line.

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

validator = validatorOccupancyMap(stateSpaceSE2);
validator.Map = map;
validator.ValidationDistance = 0.1;

Проверяйте, что точки пути являются допустимыми состояниями. Все три точки находятся в свободном пространстве, рассмотрен допустимым - также.

isValid = isStateValid(validator,path)
isValid = 3x1 logical array

   1
   1
   1

Проверяйте движение между каждым последовательным путем состояния. 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

Figure contains an axes. The axes with title Occupancy Grid contains 3 objects of type image, line.

В этом примере показано, как подтвердить пути через evironment.

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

load exampleMaps.mat
map = vehicleCostmap(double(simpleMap));
map.CollisionChecker = inflationCollisionChecker("InflationRadius",1);
plot(map)

Figure contains an axes. The axes contains 2 objects of type image, patch. This object represents Inflated Areas.

Задайте крупный путь через карту.

path = [3 3 pi/2; 8 15 0; 17 8 -pi/2];
hold on
plot(path(:,1),path(:,2),"--o")

Figure contains an axes. The axes contains 3 objects of type image, patch, line. This object represents Inflated Areas.

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

validator = validatorVehicleCostmap(stateSpaceSE2);
validator.Map = map;
validator.ValidationDistance = 0.1;

Проверяйте, что точки пути являются допустимыми состояниями. Все три точки находятся в свободном пространстве, рассмотрен допустимым - также.

isValid = isStateValid(validator,path)
isValid = 3x1 logical array

   1
   1
   1

Проверяйте движение между каждым последовательным путем состояния. 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

Figure contains an axes. The axes contains 4 objects of type image, patch, line. This object represents Inflated Areas.

Создайте 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

Figure contains an axes. The axes with title Occupancy Map contains 4 objects of type patch, scatter, line.

Входные параметры

свернуть все

Объект блока проверки допустимости состояния в виде объекта подкласса nav.StateValidator. Это предопределенные объекты блока проверки допустимости состояния:

Положения начального состояния в виде n - вектор-строка элемента или m-by-n матрица. n является размерностью пространства состояний, заданного в validator. m является количеством состояний, чтобы подтвердить.

Типы данных: single | double

Положения конечного состояния в виде n - вектор-строка элемента или m-by-n матрица. n является размерностью пространства состояний, заданного в validator. m является количеством состояний, чтобы подтвердить.

Типы данных: single | double

Выходные аргументы

свернуть все

Допустимые состояния, возвращенные как m - элемент логический вектор-столбец.

Типы данных: логический

Итоговое допустимое состояние вдоль каждого пути, возвращенного как n - вектор-строка элемента или m-by-n матрица. n является размерностью пространства состояний, заданного в свойстве пространства состояний в validator. m является количеством подтвержденных путей. Каждая строка содержит итоговое допустимое состояние вдоль связанного пути.

Типы данных: single | double

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