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.

Sintaxis

orientation = ecompass(accelerometerReading,magnetometerReading)

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat)

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat,'ReferenceFrame',RF)

Descripción

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

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

Determinar la declinación de Boston

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

Matriz de rotación de retorno

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

Determinar el vector de gravedad

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

Plataforma de giro de pista

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')

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')

Argumentos de entrada

accelerometerReading — Lecturas del acelerómetro en el sistema de coordenadas del cuerpo del sensor (m/s²)

N-por-3 matriz

Lecturas del acelerómetro en el sistema de coordenadas del cuerpo del sensor en m/s², 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

magnetometerReading — Lecturas del magnetómetro en el sistema de coordenadas del cuerpo del sensor (μT)

N-por-3 matriz

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

orientationFormat — Formato utilizado para describir la orientación.

'quaternion' (predeterminado) | 'rotmat'

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

Tipos de datos: char | string

Argumentos de salida

orientation — Orientación que rota cantidades desde el sistema de coordenadas global al sistema de coordenadas del cuerpo del sensor.

N-por-1 vector de cuaterniones (predeterminado) | arreglo de 3 por 3 por N

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=\left[\begin{array}{ccc}& & \\ (a\times m)\times a& a\times m& a\\ & & \end{array}\right]$$

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.

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):

$$\left[\begin{array}{ccc}& & \\ & R& \\ & & \end{array}\right]\left[\begin{array}{c}\\ p_{\text{parent}}\\ \end{array}\right]=\left[\begin{array}{c}\\ p_{\text{child}}\\ \end{array}\right]$$

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.

$$\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$$

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

$$\left[\begin{array}{c}{r}_{31}\\ r_{32}\\ r_{33}\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$$

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.

Por definición, el eje E es perpendicular al plano N- D , por lo tanto D ⨯ N = 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 , a ⨯ m = y, nuevamente con cierta escala de amplitud.

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

$$\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]\times \left[\begin{array}{c}{m}_1\\ m_2\\ m_3\end{array}\right]$$

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:

$$\left[\begin{array}{c}{r}_{21}\\ r_{22}\\ r_{23}\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]\times \left[\begin{array}{c}{m}_1\\ m_2\\ m_3\end{array}\right]$$

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

$$\begin{array}{l}\left[\begin{array}{c}{r}_{11}\\ r_{12}\\ r_{13}\end{array}\right]=\left[\begin{array}{c}{r}_{21}\\ r_{22}\\ r_{23}\end{array}\right]\times \left[\begin{array}{c}{r}_{31}\\ r_{32}\\ r_{33}\end{array}\right]\\ =\left(a\times m\right)\times a\end{array}$$

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

$$R_{ij}=\frac{R_{ij}}{\sqrt{{\displaystyle \sum _{i=1}^3{R}_{ij}^2}}},\forall j$$

Nota

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

Referencias

[1] Open Source Sensor Fusion. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

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

ahrsfilter | imufilter