Estimación del ángulo de inclinación mediante fusión de sensores inerciales y ADIS16505
Este ejemplo muestra cómo obtener datos de un sensor IMU ADIS16505 de Analog Devices y cómo utilizar un algoritmo de fusión de sensores en los datos del sensor para calcular la inclinación del dispositivo.
El ADIS16505 es un sensor de 6 ejes con acelerómetro y giroscopio. El acelerómetro mide la aceleración y el giroscopio mide la velocidad angular en los ejes x, y y z. La siguiente animación muestra el ángulo de inclinación a medida que la IMU gira hacia adelante y hacia atrás:
Productos MathWorks necesarios
MATLAB®
Paquete de soporte MATLAB para hardware Arduino®
Navigation Toolbox™ o Sensor Fusion and Tracking Toolbox™
Hardware requerido
ArduinoUno
Dispositivos analógicos ADIS16505
Conexión de hardware
Conecte los pines SLCK, CS, DOUT, DIN, GND y VDD del sensor ADIS16505 a los pines correspondientes en el hardware Arduino. Este ejemplo utiliza la placa Arduino Uno con las siguientes conexiones entre ADIS16505 y Arduino Uno:
SCLK-D13
CS-D10
DOUT - D12
DIN-D11
Tierra - Tierra
VDD-5 V
Asegúrese de que las conexiones a los sensores estén intactas. Se recomienda utilizar un escudo prototipo y soldar el sensor a él para evitar conexiones sueltas al mover el sensor. Consulta la página Troubleshooting Sensors de sensores para depurar los problemas relacionados con los sensores.
Crear objeto sensor
Crea un objeto arduino.
a = arduino;
Cree el objeto sensor ADIS16505. Especifique el pin de selección de chip SPI como D10 para seguir la conexión de hardware anterior y el formato de salida como una matriz.
Fs = 100; % Hz imu = adis16505(a,SampleRate=Fs,SPIChipSelectPin="D10",OutputFormat="matrix");
Alineación del eje del sensor ADIS16505 con las coordenadas NED
El algoritmo de fusión de sensores utilizado en este ejemplo utiliza Noreste-Abajo (NED) como sistema de coordenadas principal fijo. En el marco de referencia NED, el eje x apunta al Norte, el eje y apunta al Este y el eje z apunta hacia abajo.
Para alinear los ejes ADIS16505 con las coordenadas NED:
1. Definir ejes del dispositivo: Defina el eje imaginario como el eje del dispositivo en el sensor de acuerdo con el sistema de coordenadas NED, que puede o no ser el mismo que los ejes del sensor.
2. Intercambie los valores x y y de las lecturas del acelerómetro y del giroscopio, de modo que el eje del acelerómetro y del giroscopio esté alineado con el eje del magnetómetro.
3. Determine los valores de polaridad para el acelerómetro y el giroscopio.
a. Acelerómetro
Coloque el sensor de manera que el eje x del dispositivo apunte hacia abajo, perpendicular a la superficie en la que se mantiene el sensor. Las lecturas del acelerómetro deben ser aproximadamente
[9.8 0.0 0.0]. En caso contrario, niega los valores x del acelerómetro.Coloque el sensor de manera que el eje y del dispositivo apunte hacia abajo, perpendicular a la superficie en la que se mantiene el sensor. Las lecturas del acelerómetro deben ser aproximadamente
[0.0 9.8 0.0]. En caso contrario, niega los valores y del acelerómetro.Coloque el sensor de manera que el eje z del dispositivo apunte hacia abajo, perpendicular a la superficie en la que se mantiene el sensor. Las lecturas del acelerómetro deben ser aproximadamente
[0.0 0.0 9.8]. En caso contrario, niega los valores z del acelerómetro.
b. Giroscopio
Gire el sensor a lo largo de cada eje y capture las lecturas. Utilice la regla del tornillo de la mano derecha para corregir la polaridad de rotación.
El método anterior se utiliza para configurar el eje del sensor en este ejemplo.
Fusión Acelerómetro-Giroscopio
imufilter System object™ fusiona datos del acelerómetro y del giroscopio utilizando un filtro Kalman de estado de error interno. El filtro es capaz de eliminar el ruido de polarización del giroscopio, que varía con el tiempo. El filtro no procesa datos del magnetómetro, por lo que no estima correctamente la dirección del norte. El algoritmo asume que la posición inicial del sensor es tal que el eje x del dispositivo apunta hacia el norte magnético, el eje y del dispositivo apunta hacia el este y el eje z del dispositivo apunta hacia abajo. El sensor debe estar estacionario antes del inicio de este ejemplo. La orientación de salida es procesada por la función tilt para obtener el ángulo de inclinación del dispositivo.
% GyroscopeNoise and AccelerometerNoise is determined from the datasheet. GyroscopeNoiseADIS16505 = 3.0462e-06; % GyroscopeNoise (variance) in units of rad/s AccelerometerNoiseADIS16505 = 0.0061; % AccelerometerNoise (variance) in units of m/s^2 fusionFilt = imufilter(SampleRate=imu.SampleRate,GyroscopeNoise=GyroscopeNoiseADIS16505,AccelerometerNoise=AccelerometerNoiseADIS16505); stopTimer = 30; % seconds
Inicializar la figura para visualizar el ángulo de inclinación.
fig = figure; ax = axes(fig); initX1 = [-5 0]; initX2 = [0 5]; initY = [0 0]; tiltLine = plot(ax,initX1,initY,"-o",initX2,initY,'-x'); yline(ax,0,Color=ax.ColorOrder(2,:)); set(tiltLine(2),Color=ax.ColorOrder(1,:)); axis(ax,[-10 10 -10 10]); xticks(ax,[]); yticks(ax,[]); tiltMatrix = @(t) [cos(t), -sin(t); sin(t), cos(t)];
Mientras se ejecuta este código, mueva lentamente el sensor para ver el cambio del ángulo de inclinación. Tenga en cuenta que el ángulo cambiará cuando cambie el signo del ángulo de inclinación.
tic; while(toc < stopTimer) [accel,gyro] = read(imu); accel = [-accel(:,1), -accel(:,2), accel(:,3)]; orient = fusionFilt(accel,gyro); for ii = numel(orient) % Update figure. tiltAngle = tilt(orient(ii)); eulAng = euler(orient(ii),"ZYX","frame"); pitch = eulAng(2); lineData1 = tiltMatrix(sign(pitch).*tiltAngle) * [initX1; initY]; set(tiltLine(1),XData=lineData1(1,:),YData=lineData1(2,:)); lineData2 = tiltMatrix(sign(pitch).*tiltAngle) * [initX2; initY]; set(tiltLine(2),XData=lineData2(1,:),YData=lineData2(2,:)); title("Pitch: " + num2str(rad2deg(pitch)) + " degrees"); drawnow limitrate end end
Limpiar
Cuando la conexión ya no sea necesaria, suelte y borre los objetos.
release(imu); clear;
