Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

stateSpaceSE3

Espacio de estados SE(3)

Descripción

El objeto stateSpaceSE3 almacena parámetros y estados en el espacio de estados SE(3), que está compuesto de vectores de estados representados por [x, y, z, qw, qx, qy, qz]. x, y y z son coordenadas cartesianas. qw, qx, qy y qz representan la orientación en un cuaternión. El objeto utiliza cálculo de distancia euclidiana e interpolación lineal para el componente de traducción del estado. El objeto utiliza el cálculo de la distancia de los cuaterniones y la interpolación lineal esférica para el componente de rotación del estado.

Creación

Sintaxis

space = stateSpaceSE3

space = stateSpaceSE3(bounds)

Descripción

space = stateSpaceSE3 crea un objeto de espacio de estado SE(3) con límites de estado predeterminados para x, y y z. Las variables de estado qw, qx, qy y qz correspondientes a la orientación no están acotadas.

space = stateSpaceSE3(bounds) crea un objeto de espacio de estados SE(3) con límites de estado especificados como una matriz de 7 por 2. Cada fila especifica el valor mínimo y máximo de una dimensión del estado en el orden x, y, z, qw, qx, qy y qz. La entrada bounds establece la propiedad StateBounds.

Propiedades

Name — Nombre del espacio de estados

Solo lectura: 'SE3' (predeterminado) | vector de caracteres

Esta propiedad o parámetro es de solo lectura.

Nombre del espacio de estados, especificado como vector de caracteres.

Tipos de datos: char

NumStateVariables — Número de dimensiones del espacio de estados

Solo lectura: 7 (predeterminado) | número entero positivo

Esta propiedad o parámetro es de solo lectura.

Número de dimensiones del espacio de estados, dimensiones especificadas, devueltas como un entero positivo.

Tipos de datos: double

StateBounds — Límites de variables de estado

[-100 100; -100 100; -100 100; Inf Inf; Inf Inf; Inf Inf; Inf Inf] (predeterminado) | Matriz de valores reales de 7 por 2

Límites de variables de estado, especificadas como una matriz de valores reales de 7 por 2.

• La primera fila especifica los límites inferior y superior del estado x en metros.

• La segunda fila especifica los límites inferior y superior del estado y en metros.

• La tercera fila especifica los límites inferior y superior del estado z en metros.

• Las filas cuarta a séptima especifican los límites inferior y superior de las variables de estado qw, qx, qy y qz respectivamente, correspondientes a la orientación como un cuaternión.

Nota

La propiedad StateBounds solo afecta a los componentes cartesianos del estado. Las variables de estado correspondientes a la orientación no están acotadas.

Ejemplo: stateSpaceSE3([-10 10; -10 10; -10 10; Inf Inf; Inf Inf; Inf Inf; Inf Inf])

Ejemplo: space.StateBounds = [-10 10; -10 10; -10 10; Inf Inf; Inf Inf; Inf Inf; Inf Inf]

Tipos de datos: double

WeightXYZ — Peso aplicado al cálculo de distancias x, y y z

1 (predeterminado) | escalar real positivo

Peso aplicado al cálculo de distancia x, y y z, especificado como un escalar real positivo. De forma predeterminada, el peso de la traducción se elige para que sea mayor que el peso de la rotación.

El objeto calcula la distancia como:

$$d=\sqrt{w_{xyz}(d_x^2+d_y^2+d_z^2)+w_q{d}_q^2}$$

,

donde w_xyz es el peso aplicado a las coordenadas x, y y z, y w_q es el peso aplicado a la orientación en cuaternión. d_x, d_y y d_z son las distancias en las direcciones x, y y z, respectivamente. d_q es la distancia del cuaternión.

Ejemplo: space.WeightXYZ = 2

Tipos de datos: double

WeightQuaternion — Peso aplicado al cálculo de la distancia del cuaternión

0.1 (predeterminado) | escalar real positivo

Peso aplicado al cálculo de la distancia del cuaternión, especificado como un escalar real positivo. De forma predeterminada, el peso de la rotación se elige para que sea menor que el peso de la traducción.

El objeto calcula la distancia como:

$$d=\sqrt{w_{xyz}(d_x^2+d_y^2+d_z^2)+w_q{d}_q^2}$$

,

donde w_xyz es el peso aplicado a las coordenadas x, y y z, y w_q es el peso aplicado a la orientación en cuaternión. d_x, d_y y d_z son las distancias en la dirección x, y y z, respectivamente. d_q es la distancia del cuaternión.

Ejemplo: space.WeightQuaternion = 0.5

Tipos de datos: double

Funciones del objeto

copy	Crear una copia profunda del objeto del espacio de estados
distance	Distancia entre dos estados
enforceStateBounds	Reducir los límites de estado a estado
interpolate	Interpolar entre estados
sampleUniform	Estado de la muestra usando distribución uniforme.

Ejemplos

Validar ruta a través del entorno del mapa de ocupación 3D

Cree un mapa de ocupación tridimensional y un validador de estado asociado. Planifique, valide y visualice una ruta a través del mapa de ocupación.

Cargar y asignar mapa al validador de estado

Cargue un mapa de ocupación tridimensional de una manzana de la ciudad en el espacio de trabajo. Especifique un umbral para qué celdas se considerarán libres de obstáculos.

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

Infle el mapa de ocupación para agregar una zona de amortiguamiento para una operación segura alrededor de los obstáculos.

inflate(omap,1)

Cree un objeto de espacio de estados SE(3) con límites para las variables de estado.

ss = stateSpaceSE3([-20 220;
    -20 220;
    -10 100;
    inf inf;
    inf inf;
    inf inf;
    inf inf]);

Cree un validador de estado de mapa de ocupación 3D utilizando el espacio de estado creado.

sv = validatorOccupancyMap3D(ss);

Asigne el mapa de ocupación al objeto validador de estado. Especifique el intervalo de distancia de muestreo.

sv.Map = omap;
sv.ValidationDistance = 0.1;

Planificar y visualizar la ruta

Cree un planificador de rutas con una distancia máxima de conexión aumentada. Reducir el número máximo de iteraciones.

planner = plannerRRT(ss,sv);
planner.MaxConnectionDistance = 50;
planner.MaxIterations = 1000;

Cree una función de evaluación definida por el usuario para determinar si la ruta alcanza la meta. Especifique la probabilidad de elegir el estado objetivo durante el muestreo.

planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3))<5);
planner.GoalBias = 0.1;

Establezca los estados de inicio y objetivo.

start = [40 180 25 0.7 0.2 0 0.1];
goal = [150 33 35 0.3 0 0.1 0.6];

Planifique una ruta utilizando el inicio, el objetivo y el planificador especificados.

[pthObj,solnInfo] = plan(planner,start,goal);

Compruebe que los puntos de la ruta sean estados válidos.

isValid = isStateValid(sv,pthObj.States)

isValid = 7×1 logical array

   1
   1
   1
   1
   1
   1
   1

Compruebe que el movimiento entre cada estado de ruta secuencial sea válido.

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 = 6×1 logical array

   1
   1
   1
   1
   1
   1

Visualizar los resultados

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

Capacidades ampliadas

Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.

Historial de versiones

Introducido en R2020b

Consulte también

stateSpaceSE2 | validatorOccupancyMap3D