Estimadores de posición WPILib

WPILib incluye estimadores de posición para transmisiones diferenciales, swerve y mecanum. Estos estimadores han sido diseñados para sustituir a las clases existentes de odometría, con características añadidas que utilizan un Filtro Kalman para fusionar las estimaciones de pose del robot compensadas por la latencia con las mediciones del codificador y el giroscopio. Estos estimadores pueden tener en cuenta la deriva del codificador y los datos de visión inexactos. Estos estimadores pueden comportarse de forma idéntica a sus correspondientes clases de odometría si sólo se llama a actualización de estos estimadores.

Los estimadores de pose estiman la posición del robot usando un sistema de espacio de estados con los estados \(\begin{bmatrix}x & y & \theta \end{bmatrix}^T\), que puede representar la posición del robot como una Pose2d . WPILib incluye DifferentialDrivePoseEstimator, SwerveDrivePoseEstimator y MecanumDrivePoseEstimator para estimar la posición del robot. En estos, los usuarios llaman «actualizar» periódicamente con las mediciones del codificador y del giroscopio (igual que las clases de odometría) para actualizar la posición estimada del robot. Cuando el robot recibe mediciones de su posición relativa al campo (codificada como Pose2d) de sensores como la visión por computadora o V-SLAM, el estimador de pose compensa la latencia de la medición para estimar con precisión la posición del robot.

Los estimadores de pose realizan una compensación de latencia almacenando una lista de estados de observadores pasados, incluido el estado estimado \(\mathbf{\hat{x}}\), covarianza de error \(\mathbf{P}\), entradas y medidas locales . Cuando se aplican nuevas mediciones, el estado del estimador primero se retrae a la marca de tiempo de la medición. Luego, el filtro corrige su estimación de estado con la nueva medición y aplica las entradas entre la marca de tiempo de la medición y el tiempo actual para incorporar la nueva medición. Esto permite que las soluciones de visión con velocidades de fotogramas que de otro modo las inutilicen sean una solución viable para la localización de robots.

El siguiente ejemplo muestra el uso del DifferentialDrivePoseEstimator:

var estimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(),
        new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02), // State measurement standard deviations. X, Y, theta.
        new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.02, 0.02, 0.01), // Local measurement standard deviations. Left encoder, right encoder, gyro.
        new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01)); // Global measurement standard deviations. X, Y, and theta.

Estimadores de pose de afinación

Todos los estimadores de pose ofrecen desviaciones estándar personalizables por el usuario para el modelo y las medidas. Estas desviaciones estándar determinan cuánto «confía» el filtro en cada uno de estos estados. Por ejemplo, aumentar la desviación estándar para las mediciones (como se podría hacer para una señal ruidosa) haría que el estimador confiara en su estimación de estado más que en las mediciones entrantes. En el campo, esto podría significar que el filtro puede rechazar bien los datos de visión ruidosos, a costa de ser lento para corregir las desviaciones del modelo. Si bien estos valores se pueden estimar de antemano, dependen en gran medida de la configuración única de cada robot y del método de medición global.