Exponenta Banner

stateSpaceSE3

SE (3) пространство состояний

расширьте все на странице

    Описание

    stateSpaceSE3 параметры объектно-ориентированной памяти и состояния в SE (3) пространство состояний, которое состоит из векторов состояния, представленных [x, y, z, qw, qx, qy, qz]. x, y и z являются Декартовы координаты. qw, qx, qy и qz представляют ориентацию в кватернионе. Объект использует Евклидов расчет расстояния и линейную интерполяцию для компонента перевода состояния. Объект использует расчет расстояния кватерниона и сферическую линейную интерполяцию для компонента вращения состояния.

    Создание

    Синтаксис

    space = stateSpaceSE3
    space = stateSpaceSE3(bounds)

    Описание

    space = stateSpaceSE3 создает SE (3) объект пространства состояний с границами состояния по умолчанию для x, y и z. Переменные состояния qw, qx, qy и qz, соответствующий ориентации, не ограничены.

    пример

    space = stateSpaceSE3(bounds) создает SE (3) объект пространства состояний с границами состояния, заданными как 7 2 матрица. Каждая строка задает минимальное и максимальное значение для размерности состояния в порядке x, y, z, qw, qx, qy и qz. Вход bounds устанавливает StateBounds свойство.

    Свойства

    развернуть все

    Это свойство доступно только для чтения.

    Имя пространства состояний в виде вектора символов.

    Типы данных: char

    Это свойство доступно только для чтения.

    Количество размерностей пространства состояний, заданных измерений, возвращенных как положительное целое число.

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

    Границы переменных состояния в виде 7 2 матрицы действительных значений.

    • Первая строка задает нижние и верхние границы состояния x в метрах.

    • Вторая строка задает нижние и верхние границы состояния y в метрах.

    • Третья строка задает нижние и верхние границы состояния z в метрах.

    • Четвертые через седьмые строки задают нижние и верхние границы переменных состояния qw, qx, qy и qz соответственно, соответствуя ориентации как кватерниону.

    Примечание

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

    Пример: stateSpaceSE3([-10 10; -10 10; -10 10; Inf Inf; Inf Inf; Inf Inf; Inf Inf])

    Пример: space.StateBounds = [-10 10; -10 10; -10 10; Inf Inf; Inf Inf; Inf Inf; Inf Inf]

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

    Вес применился к x, y и расчету расстояния z в виде положительного действительного скаляра. По умолчанию вес для перевода выбран, чтобы быть больше веса для вращения.

    Объект вычисляет расстояние как:

    d=wxyz(dx2+dy2+dz2)+wqdq2

    ,

    где w, xyz является весом, применился к x, y и координатам z, и w, q является весом, применился к ориентации в кватернионе. d x, d y и d z является расстояниями в x, y и направлениях z, соответственно. d q является расстоянием кватерниона.

    Пример: space.WeightXYZ = 2

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

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

    Объект вычисляет расстояние как:

    d=wxyz(dx2+dy2+dz2)+wqdq2

    ,

    где w, xyz является весом, применился к x, y и координатам z, и w, q является весом, применился к ориентации в кватернионе. d x, d y и d z является расстояниями в x, y и направлении z, соответственно. d q является расстоянием кватерниона.

    Пример: space.WeightQuaternion = 0.5

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

    Функции объекта

    copyСоздайте глубокую копию объекта пространства состояний
    distanceРасстояние между двумя состояниями
    enforceStateBoundsУменьшайте состояние, чтобы утвердить границы
    interpolateИнтерполируйте между состояниями
    sampleUniformДемонстрационное состояние с помощью равномерного распределения

    Примеры

    свернуть все

    Скрипт Open Live Script

    В этом примере показано, как подтвердить пути через 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

    Расширенные возможности

    Генерация кода C/C++
    Генерация кода C и C++ с помощью MATLAB® Coder™.

    Смотрите также

    |

    Введенный в R2020b

    Документация Navigation Toolbox

    Поддержка