Блок проверки допустимости состояния на основе 3-D карты сетки
validatorOccupancyMap3D объект подтверждает состояния и дискретизированные движения на основе значений заполнения в 3-D карте заполнения. Объект интерпретирует местоположения карты без препятствий как допустимые состояния. Объект интерпретирует занятые и неизвестные местоположения карты как недопустимые состояния.
validator = validatorOccupancyMap3D
validator = validatorOccupancyMap3D(stateSpace)stateSpace введите устанавливает значение свойства StateSpace.
validator = validatorOccupancyMap3D(stateSpace,Name,Value)
Например, validatorOccupancyMap3D('ValidationDistance',0.1) создает 3-D блок проверки допустимости карты заполнения с интервалом выборки 0,1.
copy | Создайте глубокую копию объекта блока проверки допустимости состояния |
isMotionValid | Проверяйте, допустим ли путь между состояниями |
isStateValid | Проверяйте, допустимо ли состояние |
В этом примере показано, как подтвердить пути через 3-D evironment карты заполнения.
Загрузите и присвойте карту блоку проверки допустимости состояния
Загрузите 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]);
Создайте occupancyMap3D основанный блок проверки допустимости состояния, использующий созданное пространство состояний.
sv = validatorOccupancyMap3D(ss);
Присвойте карту объекту блока проверки допустимости состояния. Задайте ValidationDistance свойство.
sv.Map = omap; sv.ValidationDistance = 0.1;
Запланируйте и визуализируйте путь
Создайте планировщика пути и увеличьте максимальное расстояние связи. Сократите максимальное количество итераций.
planner = plannerRRT(ss,sv); planner.MaxConnectionDistance = 50; planner.MaxIterations = 1000;
Создайте достигнутую функцию оценки пользовательской цели. Задайте GoalBias свойство.
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 = 6×1 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 = 5×1 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
