Пространство состояния SE (3)
stateSpaceSE3 объект сохраняет параметры и состояния в пространстве состояний SE (3), которое состоит из векторов состояний, представленных [x, y, z, qw, qx, qy, qz]. x, y и z - декартовы координаты. qw, qx, qy и qz представляют ориентацию в кватернионе. Объект использует евклидово вычисление расстояния и линейную интерполяцию для компонента перевода состояния. Объект использует вычисление расстояния кватерниона и сферическую линейную интерполяцию для компонента вращения состояния.
space = stateSpaceSE3
space = stateSpaceSE3(bounds)bounds устанавливает StateBounds собственность.
copy | Создание глубокой копии государственного космического объекта |
distance | Расстояние между двумя состояниями |
enforceStateBounds | Уменьшить состояние до границ состояний |
interpolate | Интерполяция между состояниями |
sampleUniform | Образец состояния с использованием равномерного распределения |
Создайте карту занятости 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
