stateSpaceSE3

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

    Описание

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

    Создание

    Описание

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

    пример

    space = stateSpaceSE3(bounds) создает объект пространства состояний SE (3) с границами состояний, заданными как матрица 7 на 2. Каждая строка задает минимальное и максимальное значение для размерности состояния в порядок <reservedrangesplaceholder7> , <reservedrangesplaceholder6> , <reservedrangesplaceholder5> , <reservedrangesplaceholder4> , <reservedrangesplaceholder3> , <reservedrangesplaceholder2> и qz. Область входа bounds устанавливает StateBounds свойство.

    Свойства

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

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

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

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

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

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

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

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

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

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

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

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

    Примечание

    The 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

    ,

    то, где <reservedrangesplaceholder17> <reservedrangesplaceholder16> - вес, относилось x, y, и z координаты, и <reservedrangesplaceholder12> <reservedrangesplaceholder11> - вес, относился к ориентации в кватернионе. <reservedrangesplaceholder10> <reservedrangesplaceholder9>, <reservedrangesplaceholder8> <reservedrangesplaceholder7> и <reservedrangesplaceholder6> <reservedrangesplaceholder5> - расстояния в x, y, и z направления, соответственно. d q является расстоянием кватерниона.

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

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

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

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

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

    ,

    то, где <reservedrangesplaceholder17> <reservedrangesplaceholder16> - вес, относилось x, y, и z координаты, и <reservedrangesplaceholder12> <reservedrangesplaceholder11> - вес, относился к ориентации в кватернионе. <reservedrangesplaceholder10> <reservedrangesplaceholder9>, <reservedrangesplaceholder8> <reservedrangesplaceholder7> и <reservedrangesplaceholder6> <reservedrangesplaceholder5> - расстояния в x, y, и z направление, соответственно. d q является расстоянием кватерниона.

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

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

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

    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

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

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

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

    .
    Введенный в R2020b