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 свойство.

Свойства

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

`Name` — Имя пространства состояний
`'SE3'` (значение по умолчанию) | вектор символов

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

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

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

`NumStateVariables` — Количество размерностей пространства состояний
7 (значение по умолчанию) | положительное целое число

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

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

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

`StateBounds` — Границы переменных состояния
`[-100 100; -100 100; -100 100; Inf Inf; Inf Inf; Inf Inf; Inf Inf]` (значение по умолчанию) | 7 2 матрица действительных значений

Границы переменных состояния в виде 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

`WeightXYZ` — Вес применился к x, y и расчету расстояния z
1 (значение по умолчанию) | положительный действительный скаляр

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

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

$d = \sqrt{w_{x y z} (d_{x}^{2} + d_{y}^{2} + d_{z}^{2}) + w_{q} d_{q}^{2}}$

где w_{, xyz} является весом, применился к x, y и координатам z, и w_{, q} является весом, применился к ориентации в кватернионе. d _x, d _y и d _z является расстояниями в x, y и направлениях z, соответственно. d _q является расстоянием кватерниона.

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

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

`WeightQuaternion` — Вес применился к расчету расстояния кватерниона
0.1 (значение по умолчанию) | положительный действительный скаляр

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

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

$d = \sqrt{w_{x y z} (d_{x}^{2} + d_{y}^{2} + d_{z}^{2}) + w_{q} d_{q}^{2}}$

где 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 среду карты заполнения

Скрипт Open Live Script

Создайте 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

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

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

Введенный в R2020b

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

stateSpaceSE3

Описание

Создание

Синтаксис

Описание

Свойства

`Name` — Имя пространства состояний
`'SE3'` (значение по умолчанию) | вектор символов

`NumStateVariables` — Количество размерностей пространства состояний
7 (значение по умолчанию) | положительное целое число

`StateBounds` — Границы переменных состояния
`[-100 100; -100 100; -100 100; Inf Inf; Inf Inf; Inf Inf; Inf Inf]` (значение по умолчанию) | 7 2 матрица действительных значений

`WeightXYZ` — Вес применился к x, y и расчету расстояния z
1 (значение по умолчанию) | положительный действительный скаляр

`WeightQuaternion` — Вес применился к расчету расстояния кватерниона
0.1 (значение по умолчанию) | положительный действительный скаляр

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

Примеры

Подтвердите путь через 3-D среду карты заполнения

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

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

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

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

Поддержка

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

stateSpaceSE3

Описание

Создание

Синтаксис

Описание

Свойства

Name — Имя пространства состояний 'SE3' (значение по умолчанию) | вектор символов

NumStateVariables — Количество размерностей пространства состояний7 (значение по умолчанию) | положительное целое число

StateBounds — Границы переменных состояния [-100 100; -100 100; -100 100; Inf Inf; Inf Inf; Inf Inf; Inf Inf] (значение по умолчанию) | 7 2 матрица действительных значений

WeightXYZ — Вес применился к x, y и расчету расстояния z1 (значение по умолчанию) | положительный действительный скаляр

WeightQuaternion — Вес применился к расчету расстояния кватерниона0.1 (значение по умолчанию) | положительный действительный скаляр

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

Примеры

Подтвердите путь через 3-D среду карты заполнения

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

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

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

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

Поддержка

`Name` — Имя пространства состояний
`'SE3'` (значение по умолчанию) | вектор символов

`NumStateVariables` — Количество размерностей пространства состояний
7 (значение по умолчанию) | положительное целое число

`StateBounds` — Границы переменных состояния
`[-100 100; -100 100; -100 100; Inf Inf; Inf Inf; Inf Inf; Inf Inf]` (значение по умолчанию) | 7 2 матрица действительных значений

`WeightXYZ` — Вес применился к x, y и расчету расстояния z
1 (значение по умолчанию) | положительный действительный скаляр

`WeightQuaternion` — Вес применился к расчету расстояния кватерниона
0.1 (значение по умолчанию) | положительный действительный скаляр

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