Orientación con IMU y brújula

01 · Punto de partida

Por qué mejorar el rumbo

La odometría de un robot diferencial (tema 27) calcula el rumbo a partir de la diferencia de recorrido de las ruedas, dividida por el ancho de vía. Cualquier error de calibración o cualquier deslizamiento se acumula en la orientación, y un error de orientación se vuelve error de posición: un solo grado de error de rumbo, mantenido durante 10 m de avance, desplaza el robot unos 17 cm hacia un costado.

Una unidad de medición inercial (IMU) y un magnetómetro atacan justo ese punto. Ya vimos en el tema 8 cómo estimar la inclinación con un acelerómetro y un giroscopio. Aquí nos concentramos en el rumbo (yaw): el ángulo en el plano horizontal, que es el que importa para navegar.

Una advertencia desde el comienzo: ninguna fuente de rumbo es buena sola. El giroscopio deriva y la brújula se perturba. Lo útil es cómo se complementan.

02 · Comparación

Cuatro fuentes de orientación

La última fila es una trampa frecuente: el acelerómetro no sirve para el rumbo. Un giro alrededor del eje vertical no cambia la dirección de la gravedad. Por eso una IMU de seis ejes (acelerómetro y giroscopio) puede estimar el cabeceo y el alabeo con un filtro complementario, pero el rumbo necesita un tercer sensor, el magnetómetro (IMU de nueve ejes), o aceptar la deriva.

03 · Integrar el giro

El giroscopio: rumbo por integración

El giroscopio entrega la velocidad angular ωz en grados por segundo. El rumbo se obtiene integrándola:

donde b es el sesgo: la lectura que da el sensor cuando está quieto. Un sesgo de 0,3 °/s parece despreciable, pero al integrarlo da 18° de error por minuto y 90° en cinco minutos. Todo sesgo no corregido se convierte en una rampa de error.

Cómo se maneja:

• Calibración en reposo al arrancar. Con el robot inmóvil, se promedian N lecturas y se resta ese promedio. La incertidumbre del sesgo estimado es σ/√N: con σ = 0,5 °/s y 500 muestras (10 s a 50 Hz), 0,022 °/s, es decir unos 1,3° de deriva por minuto. Más muestras ayudan, pero con rendimientos decrecientes.
• El sesgo cambia con la temperatura, el tiempo y las vibraciones. Una calibración inicial no es para siempre; hay que reestimarlo cuando el robot esté quieto (un robot que se detiene en una estación puede hacerlo).
• Ruido. Al integrar, el ruido blanco produce un paseo aleatorio que crece con la raíz del tiempo; es mucho menor que el efecto del sesgo.
• Escala. La sensibilidad del sensor puede tener un error de algunos puntos porcentuales: un giro real de 360° se mide como 355° o 365°. Se calibra haciendo girar el robot vueltas completas sobre una marca.
• Rango. El rango del sensor debe cubrir la máxima velocidad angular del robot (un giro a 2 rad/s son unos 115 °/s).

Lo que sí hace bien el giroscopio es seguir los giros rápidos sin ser engañado por el deslizamiento de las ruedas: mide el giro del chasis, no el de las ruedas.

04 · Una referencia absoluta

La brújula: rumbo absoluto

Un magnetómetro de tres ejes mide el campo magnético en el marco del sensor. Con el robot horizontal, las componentes mx y my en el plano dan la dirección del norte magnético visto desde el robot. El rumbo, medido en sentido antihorario como en el resto del curso, es:

El signo negativo aparece porque, cuando el robot gira a la izquierda, el norte parece girar a la derecha en su marco. El signo exacto depende de cómo esté montado el sensor y de la convención de sus ejes: se verifica girando el robot a la izquierda y comprobando que el rumbo calculado aumenta. Si no, se invierte el signo.

Detalles que importan:

• Declinación magnética. El norte magnético difiere del geográfico en un ángulo que depende del lugar. Para navegar en un plano local suele bastar un rumbo relativo a una dirección de referencia (por ejemplo, la del arranque o la de una pared), y la declinación se absorbe.
• Inclinación. El campo terrestre no es horizontal; si el robot se inclina, las componentes mx y my cambian. Con terreno plano se ignora; con inclinaciones, se compensa con la inclinación del acelerómetro.
• Magnitud. La intensidad total del campo terrestre es del orden de 25 a 65 µT según el lugar, y la componente horizontal es menor. Una lectura con magnitud muy distinta de la esperada indica una perturbación.
• Perturbaciones. Estructuras de acero, imanes de altavoces, cables con corriente, baterías y, sobre todo, los propios motores del robot, distorsionan el campo. En interiores es frecuente que el campo varíe de un lugar a otro de la misma habitación.

La brújula no deriva: su error no se acumula. Pero es ruidosa y puede ser mala en un lugar y buena en otro. Por eso es una excelente referencia lenta y una mala fuente instantánea.

05 · Antes de confiar

Calibrar el magnetómetro

Si se hace girar un magnetómetro ideal una vuelta completa en un plano, sus lecturas (mx, my) dibujan una circunferencia centrada en el origen. Dos efectos la deforman:

• Hard iron (hierro duro): imanes o corrientes fijos al robot suman un campo constante. La circunferencia se desplaza: aparece un offset (ox, oy).
• Soft iron (hierro blando): materiales ferromagnéticos que distorsionan el campo externo. La circunferencia se vuelve una elipse: los ejes tienen distinta escala (y puede estar girada).

El procedimiento básico es hacer girar el robot lentamente una o dos vueltas completas en el plano horizontal, en su lugar de trabajo y con los motores y la electrónica encendidos, registrando el mínimo y el máximo de cada eje:

Y lo mismo para y. En el cálculo del rumbo se resta el offset y se divide por la escala: x = (mx − ox)/escalax.

El efecto es grande. Con un campo horizontal de 25 µT, un offset de 10 µT en un solo eje produce errores de rumbo de hasta 23,6°. Con un offset de (10; −6) µT y una elipse con razón 1,2, el error máximo es de 31°; al restar el offset baja a 5,2°; al corregir también la escala, a 0°. El laboratorio 2 permite experimentarlo.

La calibración con mínimo y máximo no corrige un soft iron girado (una elipse inclinada); para eso hay métodos de ajuste más generales. Y vale solo para la configuración actual: si cambia la batería, un motor o un cable de lugar, hay que repetirla.

06 · Lo mejor de cada una

Fusionar giroscopio y brújula

La idea del filtro complementario del tema 8 se aplica igual al rumbo: confiar en el giroscopio a corto plazo y en la brújula a largo plazo. En cada muestra:

La constante τ fija la escala de tiempo: la brújula corrige el rumbo con una "memoria" de unos τ segundos. Dos puntos esenciales:

• La diferencia se normaliza. Si el estimado vale 175° y la brújula −175°, la diferencia correcta es +10°, no −350° (tema 20). Sin eso, el filtro se desbarranca al cruzar ±180°.
• El sesgo residual deja un error permanente. Si el giroscopio conserva un sesgo br tras la calibración, el filtro se estabiliza con un error de aproximadamente br · τ. Con 0,3 °/s y τ = 20 s, 6°. Un τ menor reduce ese error (con τ = 5 s, 1,6°) pero deja pasar más ruido de la brújula y más perturbaciones: es el compromiso del diseño.

Medidas atípicas. Si la brújula se perturba (un motor, un trozo de acero), el filtro la sigue durante un rato. Una defensa habitual es rechazar la medición cuando difiere demasiado de la estimación (por ejemplo, más de 15°). Pero esta defensa tiene su propia trampa: si la estimación ya está equivocada, el criterio rechaza la brújula buena y el filtro queda bloqueado, derivando con el giroscopio sin corrección. Es un caso en que la protección empeora las cosas.

La salida es el reenganche: si se rechazaron medidas durante cierto tiempo (por ejemplo 5 s), se vuelve a aceptar la brújula aunque difiera. Con un contador que cuente los rechazos seguidos y se descuente con cada aceptación, el filtro se recupera. El laboratorio 1 lo muestra con una perturbación de 20° durante 20 s.

Existen estimadores más sofisticados (el filtro de Kalman estima el sesgo y pondera cada fuente según su incertidumbre), que no desarrollamos aquí. El complementario con rechazo y reenganche es simple y suficiente para un robot educativo.

07 · Para qué sirve

Usar el rumbo en el robot

• En la odometría (tema 27). La posición se sigue integrando con el avance de las ruedas, Δs, pero con el rumbo fusionado en lugar del de las ruedas: x ← x + Δs·cos θf, y ← y + Δs·sen θf. El error de posición deja de crecer con el error de ancho de vía y diámetros, y queda limitado por el de distancia.
• En la navegación (tema 28). El ángulo al objetivo α y el giro final usan el rumbo fusionado. Un giro de 90° puede ordenarse hasta que el giroscopio marque 90°, sin depender del ancho de vía.
• Mantener el rumbo en recta. ω = K·normalizar(θref − θ) corrige las desviaciones de los motores con una referencia independiente de las ruedas.
• Detectar problemas. Comparar el giro de las ruedas con el del giroscopio revela deslizamiento o choques: si las ruedas dicen que giró 30° y el giroscopio 5°, el robot está patinando.

Con un rumbo mejor, las rutas pueden ser más largas antes de requerir una corrección externa, aunque el error de distancia sigue creciendo (la odometría de posición sigue siendo estimación a ciegas). El rumbo bueno resuelve la parte que más amplifica el error.

08 · Con cuidado

Limitaciones y cuidados

• Arranque. Calibrar el sesgo con el robot quieto, antes de moverlo. Si se mueve durante la calibración, el sesgo estimado es incorrecto.
• Posición de la brújula. Montarla lejos de motores, cables de potencia y baterías, y de la misma manera cada vez. Calibrar con todo encendido.
• Interiores. El campo puede estar muy distorsionado por la estructura del edificio. La brújula puede ser inútil en un laboratorio con muchas estructuras de acero; en ese caso, el giroscopio con calibración frecuente es lo mejor que se puede hacer.
• Muestreo y comunicación. Leer los sensores por I2C a una tasa estable; un Δt mal medido introduce error en la integración (tema 16). Verificar el estado de los datos y los desbordes.
• Marcos y ejes. Los ejes del sensor pueden no coincidir con los del robot; hay que transformar (tema 20) y comprobar signos girando el robot en cada sentido.
• Módulos con fusión incorporada. Existen sensores que entregan directamente el rumbo fusionado. Son cómodos, pero conviene entender qué hacen y cómo se degradan, y verificar su salida contra una medida real.

Y sobre el modelo: lo que simulamos aquí tiene un sesgo constante y un ruido blanco. En un sensor real el sesgo deriva lentamente y el ruido no es perfectamente blanco. Siempre hay que validar con un experimento: girar el robot varias vueltas, volver al punto de partida y medir el error.

09 · Código para la placa

Estimador de rumbo en Arduino

El sketch contiene el estimador completo —integración del giroscopio, cálculo del rumbo con la brújula, filtro complementario con rechazo y reenganche— y sensores simulados que reproducen un robot quieto con sesgo, ruido y una perturbación magnética de 20° entre los 40 y 60 s. Así se puede probar la lógica sin hardware y comparar los modos de rechazo cambiando la constante MODO.

// Estimación de rumbo con giroscopio + brújula (filtro complementario).
// Los sensores se SIMULAN dentro del sketch (robot quieto, rumbo real 0°) para poder probar el estimador.
// Para un robot real, reemplazá la sección "Sensores simulados" por las lecturas de tu IMU y tu magnetómetro.

const float DT = 0.02;                  // s entre muestras (50 Hz)
const float TAU = 20.0;                 // s: escala de tiempo de la corrección con la brújula
const float UMBRAL = 15.0;              // grados: diferencia a partir de la cual la brújula parece atípica
const float REENGANCHE_S = 5.0;         // s de rechazos antes de volver a aceptar la brújula
const byte MODO = 2;                    // 0 = sin rechazo, 1 = rechazo simple, 2 = rechazo con reenganche

// Calibración del magnetómetro (se obtiene girando el robot una vuelta, ver el texto).
const float OFFSET_X = 0, OFFSET_Y = 0, ESCALA_X = 1, ESCALA_Y = 1;
const float DECLINACION = 0;            // grados: depende del lugar

float normalizar(float a) {
  while (a > 180.0) a -= 360.0;
  while (a <= -180.0) a += 360.0;
  return a;
}

// ---- Parte que se conserva en un robot real ----
float rumboGiro = 0, rumboFusion = 0;   // grados
int rechazosSeguidos = 0;

float rumboBrujula(float mx, float my) {
  const float x = (mx - OFFSET_X) / ESCALA_X;
  const float y = (my - OFFSET_Y) / ESCALA_Y;
  return normalizar(-atan2(y, x) * RAD_TO_DEG + DECLINACION);   // verificá el signo girando el robot a la izquierda
}

void actualizar(float giroGradosPorS, float brujulaGrados) {
  rumboGiro += giroGradosPorS * DT;
  rumboFusion += giroGradosPorS * DT;                          // predicción con el giroscopio
  const float e = normalizar(brujulaGrados - rumboFusion);     // diferencia en el camino corto
  if (fabs(e) < UMBRAL || MODO == 0) {
    if (rechazosSeguidos > 0) rechazosSeguidos--;
    rumboFusion += DT / TAU * e;
  } else {
    rechazosSeguidos++;
    if (MODO == 2 && rechazosSeguidos * DT > REENGANCHE_S) rumboFusion += DT / TAU * e;
  }
}

// ---- Sensores simulados ----
const float SESGO = 0.3, RUIDO_GIRO = 0.5, RUIDO_BRUJULA = 3.0, PERTURBACION = 20.0;
uint32_t estadoAzar = 7;

float azar() {
  estadoAzar += 0x6D2B79F5UL;
  uint32_t t = estadoAzar;
  t = (t ^ (t >> 15)) * (t | 1);
  t ^= t + (t ^ (t >> 7)) * (t | 61);
  return (float)((t ^ (t >> 14)) / 4294967296.0);
}

float normal() {                        // suma de 12 uniformes: media 0, desvío 1
  float s = 0;
  for (byte i = 0; i < 12; i++) s += azar();
  return s - 6.0;
}

void setup() {
  Serial.begin(115200);
  Serial.println("Simulacion de 120 s: error de rumbo (grados), robot quieto");
  for (long k = 1; k <= 6000; k++) {
    const float giro = SESGO + RUIDO_GIRO * normal();
    const float brujula = ((k >= 2000 && k < 3000) ? PERTURBACION : 0.0) + RUIDO_BRUJULA * normal();
    actualizar(giro, brujula);
    if (k == 500 || k == 1500 || k == 2500 || k == 3500 || k == 6000) {
      Serial.print("t="); Serial.print(k * DT, 0);
      Serial.print(" s: solo giroscopio="); Serial.print(rumboGiro, 2);
      Serial.print(" fusion="); Serial.println(rumboFusion, 2);
    }
  }
}

void loop() {}

Con MODO = 2 imprime aproximadamente (los decimales pueden diferir ligeramente de Python por la precisión de float): t=10 s: solo giroscopio=3.09 fusion=2.31, t=30 s: 8.84 / 4.49, t=50 s: 14.41 / 12.08, t=70 s: 20.26 / 17.04 y t=120 s: 35.11 / 7.15. Con MODO = 1 (rechazo simple), a los 120 s la fusión queda en unos 34 grados, casi como el giroscopio solo. Con MODO = 0, en unos 6,4.

Para un robot real, la parte que se conserva es actualizar() y rumboBrujula(). Reemplazá los sensores simulados por las lecturas de tu IMU y de tu magnetómetro, ya convertidas a °/s y µT, con el sesgo restado y los ejes alineados con los del robot. Cargá en OFFSET_X, OFFSET_Y, ESCALA_X y ESCALA_Y los valores de tu calibración y verificá el signo con un giro a la izquierda.

Calculá Δt con el reloj en lugar de suponerlo fijo (temas 16 y 18) y publicá el rumbo normalizado a (−180°, 180°]. Mostrá siempre que es una estimación, y definí qué hace el robot si ambas fuentes dejan de responder.

10 · Exploración interactiva

Laboratorios: deriva y calibración

Con los valores iniciales, el giroscopio solo llega a 35° de error en 2 minutos, y la fusión termina en 6,4°, que es el error permanente sesgo·τ. Con rechazo simple, la fusión se bloquea y termina en 34°, casi igual que sin brújula. Con reenganche vuelve a 7,2°. Bajá τ a 5 s y sacá la perturbación: el error final cae a 1,6°. Poné la perturbación en 60°: con rechazo simple la brújula mala se descarta y la fusión apenas se mueve (máximo de unos 11°); con reenganche, tras 5 s vuelve a aceptarla y la perturbación la arrastra hasta unos 38°; sin rechazo, hasta unos 44°. El reenganche cuesta algo de protección: es un compromiso.

Con offset (10; −6) µT y razón 1,2: 31° sin calibrar, 5,2° al restar el offset y 0° al corregir también la escala. Con solo offset en x de 10 µT y razón 1: 23,6° sin calibrar y 0° al restarlo. Con offset cero y razón 1,2: 5,2° sin corregir la escala.

11 · Código ejecutable

Simular el estimador en Python

La simulación reproduce el experimento de los laboratorios con el mismo generador pseudoaleatorio: los tres modos de rechazo, la incertidumbre de la calibración del sesgo y el efecto del hard y soft iron.

import math

def generador(semilla):
    a = [semilla & 0xFFFFFFFF]
    def siguiente():
        a[0] = (a[0] + 0x6D2B79F5) & 0xFFFFFFFF
        t = a[0]
        t = ((t ^ (t >> 15)) * (t | 1)) & 0xFFFFFFFF
        t ^= (t + (((t ^ (t >> 7)) * (t | 61)) & 0xFFFFFFFF)) & 0xFFFFFFFF
        return ((t ^ (t >> 14)) & 0xFFFFFFFF) / 4294967296
    return siguiente

def normalizar(a):
    return (a + 180) % 360 - 180

def simular(sesgo, ruido_g, ruido_m, perturbacion, tau, modo, semilla=7, dt=0.02, duracion=120.0):
    azar = generador(semilla)
    normal = lambda: sum(azar() for _ in range(12)) - 6.0          # media 0, desvio 1
    th_g = th_f = 0.0
    seguidos = 0
    inst, err_g, err_f, err_m = [], [], [], []
    for k in range(1, round(duracion / dt) + 1):
        t = k * dt
        giro = sesgo + ruido_g * normal()                            # °/s medido (el real es 0)
        brujula = (perturbacion if 40 <= t < 60 else 0.0) + ruido_m * normal()
        th_g += giro * dt
        th_f += giro * dt
        e = normalizar(brujula - th_f)
        if abs(e) < 15 or modo == "sin":
            seguidos = max(0, seguidos - 1)               # contador con fuga
            th_f += dt / tau * e
        else:
            seguidos += 1
            if modo == "reenganche" and seguidos * dt > 5.0:
                th_f += dt / tau * e
        inst.append(t); err_g.append(th_g); err_f.append(th_f); err_m.append(brujula)
    return inst, err_g, err_f, err_m

def en(t, serie, inst, dt=0.02):
    return serie[round(t / dt) - 1]

# 1. Deriva del giroscopio y fusión con brújula (el robot está quieto; el rumbo real es 0°)
for modo, nombre in (("sin", "sin rechazo"), ("rechazo", "rechazo simple"), ("reenganche", "rechazo con reenganche a los 5 s")):
    inst, g, f, m = simular(0.3, 0.5, 3.0, 20.0, 20.0, modo)
    print(f"Brujula con perturbacion de 20 grados entre 40 y 60 s - {nombre}:")
    for t in (10, 30, 50, 70, 120):
        print(f"  t={t:3d} s: solo giroscopio={en(t, g, inst):6.2f} grados; fusion={en(t, f, inst):6.2f} grados")
    pico = max(abs(v) for tt, v in zip(inst, f) if 40 <= tt < 70)
    print(f"  error maximo de la fusion entre 40 y 70 s: {pico:.2f} grados")

# 2. Calibración del sesgo en reposo
sigma, n = 0.5, 500
print(f"Sesgo estimado con {n} muestras: incertidumbre {sigma / math.sqrt(n):.3f} grados/s -> deriva {sigma / math.sqrt(n) * 60:.1f} grados por minuto")

# 3. Magnetómetro: hard iron y soft iron
B = 25.0
def error_maximo(ox, oy, razon, corregir_offset, corregir_escala):
    peor = 0.0
    for paso in range(3600):
        fi = math.radians(paso / 10)
        mx, my = B * math.cos(fi) + ox, -B * math.sin(fi) * razon + oy
        if corregir_offset:
            mx, my = mx - ox, my - oy
        if corregir_escala:
            my /= razon
        estimado = -math.degrees(math.atan2(my, mx))
        peor = max(peor, abs(normalizar(estimado - math.degrees(fi))))
    return peor
print(f"Offset (10, -6) uT y elipse 1,2 - sin calibrar: {error_maximo(10, -6, 1.2, False, False):.1f} grados; "
      f"solo offset: {error_maximo(10, -6, 1.2, True, False):.1f}; offset y escala: {error_maximo(10, -6, 1.2, True, True):.1f}")

El giroscopio solo llega a 35,11° de error a los 120 s. La fusión sin rechazo termina en 6,39°; con rechazo simple se bloquea en 34,25° (casi el mismo error que el giroscopio solo); con reenganche termina en 7,15°. El error máximo de la fusión entre 40 y 70 s es de 18–20° en los tres casos: la perturbación de 20° siempre deja huella. Calibrar el sesgo con 500 muestras deja 0,022 °/s de incertidumbre (1,3° por minuto). El magnetómetro con offset (10; −6) µT y elipse 1,2: 31,0° sin calibrar, 5,2° solo con el offset y 0° con ambos.

Probá tau = 5 (menos error permanente), subí el ruido de la brújula a 10° y observá cómo cambia el compromiso; ponele sesgo = 0 y mirá que el error permanente desaparece. Agregá el reenganche con una ventana distinta y compará.

12 · Método y diagnóstico

Pruebas y diagnóstico

1. Con el robot quieto, registrá el giroscopio un minuto: el promedio es el sesgo y su desvío el ruido. Comprobá que el rumbo integrado casi no se mueve tras restar el sesgo.
2. Girá el robot a mano a la izquierda: el rumbo del giroscopio y el de la brújula deben aumentar. Si alguno disminuye, invertí su signo.
3. Hacé girar el robot 10 vueltas completas sobre una marca y comparalo con el ángulo del giroscopio: así se detecta un error de escala.
4. Calibrá el magnetómetro con los motores y la electrónica encendidos, y dibujá las lecturas: deben formar un círculo centrado después de corregir.
5. Acercá un objeto de acero a la brújula y observá cómo se perturba. Verificá la reacción del filtro.
6. Recorré un cuadrado (temas 27 y 28) con y sin el rumbo fusionado y comparalo con la posición real.
7. Repetí todo en el lugar de trabajo definitivo y con la batería usual.

13 · Práctica guiada

Predecí antes de comprobar

1. Deriva por sesgo

Un giroscopio tiene un sesgo residual de 0,3 °/s. ¿Cuánto vale el error de rumbo tras 5 minutos de integración?

2. Calibrar el sesgo

El ruido del giroscopio es σ = 0,5 °/s. Se promedian 2000 muestras en reposo (40 s a 50 Hz). ¿Qué incertidumbre queda y cuánta deriva por minuto?

3. Rumbo de la brújula

Un magnetómetro calibrado da m_x = 20 µT y m_y = −10 µT. ¿Qué rumbo resulta con θ = −atan2(m_y, m_x)?

4. Offset y escala

Al girar el robot una vuelta se registran m_x entre −15 y +35 µT y m_y entre −34 y +22 µT. Calculá los offsets, los semi-rangos y las escalas.

5. Error permanente

El sesgo residual es 0,1 °/s y τ = 30 s. ¿Qué error permanente deja el filtro complementario? ¿Y con τ = 10 s?

6. Cruce de ±180°

El rumbo estimado es 175° y la brújula marca −175°. ¿Cuánto vale el error normalizado y hacia dónde corrige el filtro?

7. Error de posición

Un error de rumbo de 2° se mantiene durante 5 m de avance. ¿Cuánto se desvía lateralmente el robot?

14 · Síntesis

Ideas para recordar

¿Listo para el tema 30?

Antes de avanzar, definí cómo calibrarías el sesgo y el magnetómetro de tu robot, qué τ elegirías y qué harías si la brújula se perturba durante una prueba. Pensá cómo cambiaría la navegación del tema 28 con el rumbo fusionado.

En el próximo tema empezamos con el seguimiento de línea: sensores y estrategias, con la disposición de sensores, los umbrales, la calibración y los casos de borde.