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.

ecompass

Orientación a partir de lecturas de magnetómetro y acelerómetro.

Descripción

ejemplo

orientation = ecompass(accelerometerReading,magnetometerReading) devuelve un quaternion que puede rotar cantidades de un marco principal (NED) a un marco secundario (sensor).

ejemplo

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat) especifica el formato de orientación como quaternion o matriz de rotación.

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat,'ReferenceFrame',RF) también le permite especificar el marco de referencia RF de la salida orientation . Especifique RF como 'NED' (Noreste-Abajo) o 'ENU' (Este-Norte-Arriba). El valor predeterminado es 'NED'.

Ejemplos

contraer todo

Utilice la intensidad del campo magnético conocida y la aceleración adecuada de un dispositivo apuntado al norte verdadero en Boston para determinar la declinación magnética de Boston.

Defina la aceleración conocida y la intensidad del campo magnético en Boston.

magneticFieldStrength = [19.535 -5.109 47.930];
properAcceleration = [0 0 9.8];

Pase la intensidad del campo magnético y la aceleración a la función ecompass . La función ecompass devuelve un operador de rotación de cuaternión. Convierte el cuaternión a ángulos de Euler en grados.

q = ecompass(properAcceleration,magneticFieldStrength);
e = eulerd(q,'ZYX','frame');

El ángulo, e, representa el ángulo entre el norte verdadero y el norte magnético en Boston. Por convención, la declinación magnética es negativa cuando el norte magnético está al oeste del norte verdadero. Nega el ángulo para determinar la declinación magnética.

magneticDeclinationOfBoston = -e(1)
magneticDeclinationOfBoston = -14.6563

La función ecompass fusiona datos del magnetómetro y del acelerómetro para devolver un cuaternión que, cuando se usa dentro de un operador de rotación de cuaterniones, puede rotar cantidades de un marco principal (NED) a un marco secundario. La función ecompass también puede devolver matrices de rotación que realizan rotaciones equivalentes como el operador cuaternión.

Defina una rotación que pueda llevar un marco principal que apunta al norte magnético a un marco secundario que apunta al norte geográfico. Defina la rotación como un cuaternión y una matriz de rotación. Luego, convierta el cuaternión y la matriz de rotación a ángulos de Euler en grados para comparar.

Defina la intensidad del campo magnético en microteslas en Boston, MA, cuando apunta al norte verdadero.

m = [19.535 -5.109 47.930];
a = [0 0 9.8];

Determine el cuaternión y la matriz de rotación que es capaz de rotar un marco desde el norte magnético al norte verdadero. Muestre los resultados para comparar.

q = ecompass(a,m);
quaterionEulerAngles = eulerd(q,'ZYX','frame')
quaterionEulerAngles = 1×3

   14.6563         0         0

r = ecompass(a,m,'rotmat');
theta = -asin(r(1,3));
psi = atan2(r(2,3)/cos(theta),r(3,3)/cos(theta));
rho = atan2(r(1,2)/cos(theta),r(1,1)/cos(theta));
rotmatEulerAngles = rad2deg([rho,theta,psi])
rotmatEulerAngles = 1×3

   14.6563         0         0

Utilice ecompass para determinar el vector de gravedad basándose en los datos de una IMU giratoria.

Cargue los datos de la unidad de medida inercial (IMU).

load 'rpy_9axis.mat' sensorData Fs

Determine la orientación del cuerpo del sensor en relación con el marco NED local a lo largo del tiempo.

orientation = ecompass(sensorData.Acceleration,sensorData.MagneticField);

Para estimar el vector de gravedad, primero gire las lecturas del acelerómetro desde el marco del cuerpo del sensor al marco NED usando el vector cuaternión orientation .

gravityVectors = rotatepoint(orientation,sensorData.Acceleration);

Determine el vector de gravedad como un promedio de los vectores de gravedad recuperados a lo largo del tiempo.

gravityVectorEstimate = mean(gravityVectors,1)
gravityVectorEstimate = 1×3

    0.0000   -0.0000   10.2102

Fusione datos modelados de acelerómetro y giroscopio para rastrear una plataforma giratoria utilizando datos tanto idealizados como realistas.

Generar una trayectoria de ground-truth

Describa la orientación real de la plataforma a lo largo del tiempo. Utilice el kinematicTrajectory System object™ para crear una trayectoria para una plataforma que no tiene traslación y gira alrededor de su eje z.

duration = 12;
fs = 100;
numSamples = fs*duration;

accelerationBody = zeros(numSamples,3);

angularVelocityBody = zeros(numSamples,3);
zAxisAngularVelocity = [linspace(0,4*pi,4*fs),4*pi*ones(1,4*fs),linspace(4*pi,0,4*fs)]';
angularVelocityBody(:,3) = zAxisAngularVelocity;

trajectory = kinematicTrajectory('SampleRate',fs);

[~,orientationNED,~,accelerationNED,angularVelocityNED] = trajectory(accelerationBody,angularVelocityBody);

Modelo que recibe datos de IMU

Utilice un System object imuSensor para imitar los datos recibidos de una IMU que contiene un magnetómetro ideal y un acelerómetro ideal.

IMU = imuSensor('accel-mag','SampleRate',fs);
[accelerometerData,magnetometerData] = IMU(accelerationNED, ...
                                           angularVelocityNED, ...
                                           orientationNED);

Fusionar datos de IMU para estimar la orientación

Pase los datos del acelerómetro y del magnetómetro a la función ecompass para estimar la orientación a lo largo del tiempo. Convierta la orientación a ángulos de Euler en grados y trace el resultado.

orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');

timeVector = (0:numSamples-1).'/fs;

figure(1)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Ideal IMU')

Figure contains an axes object. The axes object with title Orientation from Ideal IMU, xlabel Time (s), ylabel Rotation (degrees) contains 3 objects of type line. These objects represent z-axis, y-axis, x-axis.

Repita el experimento con un modelo de sensor IMU realista

Modifique los parámetros del System object IMU para aproximarse a los datos realistas del sensor IMU. Restablezca el IMU y luego llámelo con la misma aceleración, velocidad angular y orientación reales. Utilice ecompass para fusionar los datos de la IMU y trazar los resultados.

IMU.Accelerometer = accelparams( ...
    'MeasurementRange',20, ...
    'Resolution',0.0006, ...
    'ConstantBias',0.5, ...
    'AxesMisalignment',2, ...
    'NoiseDensity',0.004, ...
    'BiasInstability',0.5);
IMU.Magnetometer = magparams( ...
    'MeasurementRange',200, ...
    'Resolution',0.01);
reset(IMU)

[accelerometerData,magnetometerData] = IMU(accelerationNED,angularVelocityNED,orientationNED);

orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');

figure(2)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Realistic IMU')

Figure contains an axes object. The axes object with title Orientation from Realistic IMU, xlabel Time (s), ylabel Rotation (degrees) contains 3 objects of type line. These objects represent z-axis, y-axis, x-axis.

Argumentos de entrada

contraer todo

Lecturas del acelerómetro en el sistema de coordenadas del cuerpo del sensor en m/s2, especificadas como una matriz N por 3. Las columnas de la matriz corresponden a los ejes x-, y- y z-del cuerpo del sensor. Las filas de la matriz, N, corresponden a muestras individuales. Las lecturas del acelerómetro se normalizan antes de su uso en la función.

Tipos de datos: single | double

Lecturas del magnetómetro en el sistema de coordenadas del cuerpo del sensor en µT, especificadas como una matriz N-por-3. Las columnas de la matriz corresponden a los ejes x-, y- y z-del cuerpo del sensor. Las filas de la matriz, N, corresponden a muestras individuales. Las lecturas del magnetómetro se normalizan antes de su uso en la función.

Tipos de datos: single | double

Formato utilizado para describir la orientación, especificado como 'quaternion' o 'rotmat'.

Tipos de datos: char | string

Argumentos de salida

contraer todo

Orientación que puede rotar cantidades desde un sistema de coordenadas global a un sistema de coordenadas corporales, devuelta como un vector de cuaterniones o un arreglo. El tamaño y tipo de orientation depende del formato utilizado para describir la orientación:

  • 'quaternion' –– N-por-1 vector de cuaterniones con el mismo tipo de datos subyacente que la entrada

  • 'rotmat' –– 3 por 3 por N arreglo del mismo tipo de datos que la entrada

Tipos de datos: quaternion | single | double

Algoritmos

La función ecompass devuelve un cuaternión o matriz de rotación que puede rotar cantidades de un marco principal (NED, por ejemplo) a un marco secundario (sensor). Para ambos formatos de orientación de salida, el operador de rotación se determina calculando la matriz de rotación.

Primero se calcula la matriz de rotación con un intermediario:

R=[(a×m)×aa×ma]

y luego normalizado en columnas. a y m son la entrada accelerometerReading y la entrada magnetometerReading , respectivamente.

Para comprender el cálculo de la matriz de rotación, considere un punto arbitrario de la Tierra y su correspondiente marco NED local. Supongamos un marco de cuerpo de sensor, [x, y, z], con el mismo origen.

NED Frame

Recuerde que la orientación de un cuerpo de sensor se define como el operador de rotación (matriz de rotación o cuaternión) necesario para rotar una cantidad desde un marco principal (NED) a un marco secundario (cuerpo de sensor):

[R][pparent]=[pchild]

dónde

  • R es una matriz de rotación de 3 por 3, que puede interpretarse como la orientación del marco secundario.

  • p principal es un vector de 3 por 1 en el marco principal.

  • p secundario es un vector de 3 por 1 en el marco secundario.

Para un cuerpo sensor estable, un acelerómetro devuelve la aceleración debida a la gravedad. Si el cuerpo del sensor está perfectamente alineado con el sistema de coordenadas NED, toda la aceleración debida a la gravedad se produce a lo largo del eje z y el acelerómetro indica [0 0 1]. Considere la matriz de rotación requerida para rotar una cantidad del sistema de coordenadas NED a una cantidad indicada por el acelerómetro.

Express Gravitational Vector in Different Frames

[r11r21r31r12r22r32r13r23r33][001]=[a1a2a3]

La tercera columna de la matriz de rotación corresponde a la lectura del acelerómetro:

[r31r32r33]=[a1a2a3]

La lectura de un magnetómetro apunta hacia el norte magnético y está en el plano N- D . Nuevamente, considere el marco del cuerpo de un sensor alineado con el sistema de coordenadas NED.

Magnetometer Direction

Por definición, el eje E es perpendicular al plano N- D , por lo tanto DN = E, dentro de algún escalado de amplitud. Si el marco del cuerpo del sensor está alineado con el NED, tanto el vector de aceleración del acelerómetro como el vector de campo magnético del magnetómetro se encuentran en el plano N- D . Por lo tanto , am = y, nuevamente con cierta escala de amplitud.

Considere la matriz de rotación requerida para rotar NED al marco secundario, [x y z].

Express a crosses m

[r11r21r31r12r22r32r13r23r33][010]=[a1a2a3]×[m1m2m3]

La segunda columna de la matriz de rotación corresponde al producto cruzado de la lectura del acelerómetro y la lectura del magnetómetro:

[r21r22r23]=[a1a2a3]×[m1m2m3]

Por definición de matriz de rotación, la columna 1 es el producto cruzado de las columnas 2 y 3:

[r11r12r13]=[r21r22r23]×[r31r32r33]=(a×m)×a

Finalmente, la matriz de rotación se normaliza en columnas:

Rij=Riji=13Rij2,j

Nota

El algoritmo ecompass utiliza el norte magnético, no el norte verdadero, para el sistema de coordenadas NED.

Capacidades ampliadas

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

Historial de versiones

Introducido en R2018b

Consulte también

|