Estimateurs de pose WPILib

WPILib comprend des estimateurs de pose et d’état pour les transmissions différentielles, swerve et mecanum. Ces estimateurs sont conçus pour remplacer les classes existantes odometry, avec des fonctionnalités supplémentaires qui utilisent un Kalman Filter pour fusionner les estimations de pose de robot compensées par latence avec des mesures d’encodeur et de gyroscope. Ces estimateurs peuvent tenir compte de la dérive des encodeurs et des données de vision avec bruit. Ces estimateurs peuvent se comporter de manière identique à leurs classes d’odométrie correspondantes si seulement la commande update est appelée sur ces estimateurs.

Les estimateurs de pose estiment la position du robot en utilisant un système d’espace d’états avec les états \(\begin{bmatrix}x & y & \theta \end{bmatrix}^T\), qui peut représenter la position du robot sous forme de Pose2d. WPILib inclut DifferentialDrivePoseEstimator, SwerveDrivePoseEstimator et MecanumDrivePoseEstimator pour estimer la position du robot. Dans ceux-ci, les utilisateurs appellent périodiquement update avec les mesures de l’encodeur et du gyroscope (identiques aux classes d’odométrie) pour mettre à jour la position estimée du robot. Lorsque le robot reçoit des mesures de sa position relative au champ (codée en tant que Pose2d) de capteurs tels que la vision par ordinateur ou V-SLAM, l’estimateur de pose compense la latence de la mesure pour estimer avec précision la position du robot.

Les estimateurs de pose effectuent une compensation de latence en stockant une liste des états d’observateurs passés, y compris l’état estimé \(\mathbf{\hat{x}}\), la covariance d’erreur \(\mathbf{P}\), les entrées et les mesures locales . Lorsque de nouvelles mesures sont appliquées, l’état de l’estimateur est d’abord restauré à l’horodatage de la mesure. Ensuite, le filtre corrige son estimation d’état avec la nouvelle mesure et applique les entrées entre l’horodatage de la mesure et l’instant présent pour incorporer la nouvelle mesure. Cela permet des solutions de vision avec des fréquences d’images qui pourraient autrement les rendre inutilisables, ce qui constitue une solution viable pour la localisation de robots.

L’exemple suivant montre l’utilisation du 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.

Réglage des estimateurs de pose

Tous les estimateurs de pose offrent des écarts-types personnalisables par l’utilisateur pour le modèle et les mesures. Ces écarts types déterminent dans quelle mesure le filtre « se conforme » à chacun de ces états. Par exemple, augmenter l’écart type pour les mesures (comme on pourrait le faire pour un signal bruyant) conduirait l’estimateur à faire davantage confiance à son estimation d’état qu’aux mesures entrantes. Sur le terrain, cela pourrait signifier que le filtre peut bien rejeter les données de vision bruyantes, au prix d’une lenteur à corriger les écarts du modèle. Bien que ces valeurs puissent être estimées à l’avance, elles dépendent beaucoup de la configuration unique de chaque robot et de la méthode de mesure globale.