stateSpaceSE3

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

    Описание

    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 матрица. Каждая строка задает минимальное и максимальное значение для размерности состояния в порядке 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Демонстрационное состояние с помощью равномерного распределения

    Примеры

    свернуть все

    Создайте 3-D карту заполнения и блок проверки допустимости ассоциированной страны. Запланируйте, подтвердите и визуализируйте путь через карту заполнения.

    Загрузите и присвойте карту блоку проверки допустимости состояния

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

    mapData = load('dMapCityBlock.mat');
    omap = mapData.omap;
    omap.FreeThreshold = 0.5;

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

    inflate(omap,1)

    Создайте 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 = 7x1 logical array
    
       1
       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 = 6x1 logical array
    
       1
       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 object. The axes object with title Occupancy Map contains 4 objects of type patch, scatter, line.

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

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

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

    |

    Введенный в R2020b