Main Content

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)

Desde R2020b

    Descripción

    El objeto stateSpaceSE3 almacena parámetros y estados en el espacio de estados SE(3), que se compone de vectores de estado 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

    Descripción

    space = stateSpaceSE3 crea un objeto de espacio de estados 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.

    ejemplo

    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 para una dimensión del estado en el orden x, y, z, qw, qx, qy y qz. La entrada bounds establece la propiedad StateBounds .

    Propiedades

    expandir todo

    Esta propiedad o parámetro es de solo lectura.

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

    Tipos de datos: char

    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

    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 cuaternión.

    Nota

    La propiedad StateBounds solo afecta 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

    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=wxyz(dx2+dy2+dz2)+wqdq2

    ,

    donde w xyz es el peso aplicado a x, y y z coordenadas, 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

    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=wxyz(dx2+dy2+dz2)+wqdq2

    ,

    donde w xyz es el peso aplicado a x, y y z coordenadas, 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

    copyCrear una copia profunda del objeto del espacio de estados
    distanceDistancia entre dos estados
    enforceStateBoundsReducir los límites de estado a estado
    interpolateInterpolar entre estados
    sampleUniformEstado de la muestra usando distribución uniforme.

    Ejemplos

    contraer todo

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

    Visualiza 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

    Figure contains an axes object. The axes object with title Occupancy Map, xlabel X [meters], ylabel Y [meters] contains 4 objects of type patch, scatter, line.

    Capacidades ampliadas

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

    Historial de versiones

    Introducido en R2020b