Odometria diferencial con codificadores de rueda

Un robot de traccion diferencial tiene dos ruedas accionadas de forma independiente separadas por una anchura de via. Cuando ambas ruedas recorren la misma distancia, el robot avanza recto. Cuando una recorre mas, el chasis describe un arco. La odometria integra esos pequenos movimientos en el tiempo para estimar la pose: la posicion horizontal x, la posicion vertical y y el rumbo θ.

La estimacion es local y acumulativa. Parte de una pose elegida y no puede corregirse por si sola a partir de los codificadores de rueda. Los pequenos errores de diametro de rueda, anchura de via, temporizacion y deslizamiento se acumulan, asi que el objetivo es una estimacion util a corto plazo con limites medidos, no una verdad de referencia perfecta.

Define una unica convencion de coordenadas

Elige un marco antes de escribir ecuaciones. Una convencion bidimensional habitual es:

• x apunta hacia delante desde la pose inicial.
• y apunta hacia la izquierda.
• θ positivo gira en sentido antihorario.
• las distancias de las ruedas izquierda y derecha son positivas cuando el robot avanza.

Guarda los angulos en radianes internamente. Convierte a grados solo para mostrarlos. Normaliza el rumbo si los valores acotados te ayudan a depurar, pero el seno y el coseno funcionan igual con angulos sin acotar.

Sean dL y dR las distancias de las ruedas izquierda y derecha desde la actualizacion anterior, y b la distancia efectiva entre las lineas de contacto de las ruedas.

Convierte cuentas en metros

Todo lo anterior supone que ya tienes dL y dR en metros. Los codificadores no producen metros; producen cuentas. Entre ambos hay una conversion, y equivocarla escala todos los numeros que calcula el resto de esta pagina.

cuentasPorVueltaDeRueda = PPR x flancosDecodificados x relacionDeEngranajes
metrosPorCuenta        = (pi x diametroDeRueda) / cuentasPorVueltaDeRueda

flancosDecodificados vale 4 solo si decodificas ambos flancos de ambos canales: la cuadratura completa. Decodificar el flanco de subida de un canal da 1, y ademas descarta la informacion de direccion, asi que el robot no puede distinguir marcha atras de avance.

La ultima fila es la que sorprende a la gente. Un disco ranurado atado a la rueda con una brida es barato, pero 10 mm de cuantizacion significan que un desplazamiento de 200 mm se conoce con un 5% de error, y ninguna calibracion recupera informacion que el sensor nunca capturo. El tutorial de codificadores en cuadratura cubre la parte de decodificacion en detalle.

La resolucion es un suelo para la precision, no una promesa de exactitud. Una cuenta de 73 um no significa nada si el diametro de la rueda esta un 2% equivocado.

Calcula el movimiento lineal y angular

El centro del eje se desplaza la distancia media de las ruedas:

dCentro = (dR + dL) / 2

El rumbo cambia segun su diferencia:

dTheta = (dR - dL) / b

Distancias iguales hacen dTheta = 0. Distancias iguales y opuestas hacen dCentro = 0, asi que el robot gira sobre su sitio alrededor del punto medio del eje.

Para actualizaciones pequenas, aplicar la traslacion en el rumbo medio es exacto y evita un caso especial para la linea recta:

rumboMedio = theta + dTheta / 2
x = x + dCentro * cos(rumboMedio)
y = y + dCentro * sin(rumboMedio)
theta = theta + dTheta

Esta integracion por el punto medio es lo bastante sencilla para un controlador embebido y se comporta bien cuando el bucle se ejecuta con frecuencia. Una integracion de arco exacta es util para pasos grandes, pero no repara mediciones de rueda equivocadas.

Implementa una actualizacion repetible

Conserva los totales brutos de los codificadores y convierte sus diferencias a metros. No reinicies los contadores de hardware en cada bucle; las instantaneas hacen mas faciles el desbordamiento y el diagnostico.

struct Pose { float x, y, theta; };

void updateOdometry(Pose& pose, float dLeft, float dRight, float trackWidth) {
  const float dCenter = 0.5f * (dLeft + dRight);
  const float dTheta = (dRight - dLeft) / trackWidth;
  const float headingMid = pose.theta + 0.5f * dTheta;

  pose.x += dCenter * cosf(headingMid);
  pose.y += dCenter * sinf(headingMid);
  pose.theta += dTheta;
}

Llama a esto despues de leer ambos codificadores de forma atomica. Las instantaneas izquierda y derecha deberian representar casi el mismo instante; un hueco largo entre ellas crea una direccion aparente durante el movimiento rapido.

Si otro subsistema necesita velocidad, divide los incrementos entre el tiempo transcurrido medido. La integracion de pose usa distancia, asi que el jitter del tiempo de bucle no cambia directamente la estimacion de distancia recorrida.

Lee ambos codificadores sin perder cuentas

La funcion anterior es aritmetica pura. Alimentarla correctamente es donde la odometria embebida se rompe de verdad, y tres detalles concentran casi todo el riesgo.

Toma una instantanea de ambos contadores en el mismo instante. Los manejadores de interrupcion siguen incrementandolos mientras tu bucle los lee. Leer el izquierdo, hacer trabajo y luego leer el derecho atribuye a una sola rueda el movimiento ocurrido entretanto, lo que las ecuaciones interpretan fielmente como un giro.

No reinicies nunca los contadores de hardware. Los totales en caida libre mas un valor anterior recordado te dan el delta, y dejan la cuenta bruta disponible para diagnostico.

Calcula el delta en aritmetica sin signo. El desbordamiento con signo es comportamiento indefinido; la resta sin signo se ajusta de forma definida y la conversion de vuelta recupera el delta con signo incluso al cruzar el limite de 2^32.

volatile int32_t leftCount = 0, rightCount = 0;   // escrito solo por las ISR

void isrLeft()  { leftCount  += digitalRead(L_CHAN_B) ? +1 : -1; }
void isrRight() { rightCount += digitalRead(R_CHAN_B) ? +1 : -1; }

constexpr float METRES_PER_COUNT = (3.14159265f * 0.065f) / 2800.0f;   // 72,9 um
constexpr float TRACK_WIDTH      = 0.152f;                             // calibrada, no medida con regla

void odometryTick(Pose& pose) {
  int32_t nowLeft, nowRight;
  noInterrupts();                      // un instante, ambas ruedas
  nowLeft  = leftCount;
  nowRight = rightCount;
  interrupts();

  static int32_t prevLeft = 0, prevRight = 0;
  // La resta sin signo se ajusta de forma definida; la conversion de vuelta
  // recupera el delta con signo incluso al cruzar el limite de 2^32.
  const int32_t dLeftCounts  = (int32_t)((uint32_t)nowLeft  - (uint32_t)prevLeft);
  const int32_t dRightCounts = (int32_t)((uint32_t)nowRight - (uint32_t)prevRight);
  prevLeft  = nowLeft;
  prevRight = nowRight;

  updateOdometry(pose,
                 dLeftCounts  * METRES_PER_COUNT,
                 dRightCounts * METRES_PER_COUNT,
                 TRACK_WIDTH);
}

A 73 um por cuenta, un contador con signo de 32 bits da la vuelta tras unos 157 km, asi que en un robot de escritorio el desbordamiento es teorico. Cuesta dos conversiones acertar de todos modos, y el mismo patron es obligatorio con un contador de 16 bits, que da la vuelta tras 2,4 m.

Resuelve un giro paso a paso

Supongamos que la rueda izquierda recorre 0,20 m, la derecha 0,30 m y la anchura de via es 0,25 m:

• dCentro = 0,25 m
• dTheta = (0,30 - 0,20) / 0,25 = 0,4 rad
• partiendo de rumbo cero, el rumbo medio es 0,2 rad
• dx ≈ 0,25 cos(0,2) = 0,245 m
• dy ≈ 0,25 sin(0,2) = 0,050 m

El robot termina ligeramente a la izquierda de su eje de avance original y girado unos 22,9°. Comprobar un calculo a mano como este atrapa errores de signo y de unidades antes de que el codigo llegue al hardware.

Calibra los dos parametros dominantes

El diametro efectivo de rueda controla la escala de distancia. Recorre una linea recta larga y medida, compara la distancia reportada con la real y multiplica la escala del diametro por real / reportada. Calibra cada lado si el robot se curva a pesar de comandos iguales.

La anchura de via efectiva controla la escala de rotacion. Ordena varios giros completos sobre el sitio, mide el cambio real de rumbo con una referencia externa y ajusta la anchura de via. Varias vueltas amplifican el error y facilitan la medicion. El arrastre del neumatico hace que el valor efectivo pueda diferir de la separacion medida con regla.

Manten ambos parametros configurables. La superficie, la compresion del neumatico, la carga util y el desgaste pueden cambiarlos; los modelos de hardware necesitan mandos de calibracion.

Conoce a que ritmo crece el error

La calibracion no elimina el error, reduce su coeficiente. Conocer el tamano de lo que queda te dice cuanto tiempo puedes confiar en la estimacion antes de que algo externo tenga que corregirla.

La cifra del cuadrado sale de integrar un cuadrado de 1 m con cuatro giros de 90 grados sobre el sitio y nada mal salvo la anchura de via. Todo lo demas es exacto, y el robot aun termina a 8,7 cm de donde empezo.

Lee la ultima fila junto a las dos primeras. La matematica de integracion nunca es tu error dominante. Esta tres ordenes de magnitud por debajo de los terminos de calibracion y cuatro por debajo del deslizamiento, y por eso subir la frecuencia del bucle para perseguir precision es la optimizacion equivocada, y por eso la pagina sobre deslizamiento de ruedas importa mas para tu pose final que cualquier cosa de esta pagina.

Elige una frecuencia de actualizacion

La integracion por el punto medio es exacta para lineas rectas y ligeramente erronea en arcos, con un error aproximado de dTheta^2 / 24:

Elige 50 Hz y deja de pensar en ello. Dedica la atencion que ahorras a sincronizar las dos lecturas de los codificadores, que a cualquier frecuencia importa mas que la frecuencia misma: un desfase de 1 ms entre las instantaneas izquierda y derecha con las ruedas difiriendo en 0,5 m/s inventa 0,5 mm de diferencia de rueda de la nada, en cada vuelta del bucle.

Anade un giroscopio sin confundir los marcos

Un giroscopio a menudo proporciona un mejor cambio de rumbo a corto plazo que la diferencia entre codificadores de rueda. Puedes usar el rumbo del giroscopio para theta y seguir usando la distancia media de las ruedas para la traslacion. Aplica la traslacion en el promedio del rumbo antiguo y el nuevo.

Eso no hace que la posicion sea absoluta. Los giroscopios tienen sesgo y la distancia de rueda sigue deslizando. Hace falta un sensor global (referencias visuales de camara, localizacion por lidar u otra referencia externa) para corregir la pose acumulada.

En terminologia ROS, una estimacion local continua pertenece de forma natural al marco odom. Un sistema de localizacion puede actualizar map → odom sin forzar un salto en la transformacion localmente suave odom → base_link.

Mide la deriva en lugar de ocultarla

Ejecuta tres pruebas repetibles:

1. Avanza y retrocede hasta el punto de partida.
2. Gira varias vueltas y vuelve al rumbo original.
3. Recorre un cuadrado grande y compara la pose final con la marca inicial.

Registra los deltas de los codificadores, la pose calculada, la tension de la bateria y la superficie. Repite cada prueba en ambos sentidos. Un error de escala consistente sugiere calibracion; un error que cambia con el sentido sugiere juego mecanico o asimetria; un error que cambia con la superficie sugiere deslizamiento.

Solucion de problemas

Prueba las consecuencias sobre la direccion en el simulador de seguidor de linea y compara las estimaciones de movimiento local con los conceptos de mapeo de fundamentos de SLAM. Para la tarea que depende mas directamente de esta estimacion (recorrer filas paralelas por todo un suelo), consulta la planificacion de cobertura y el simulador de cobertura.