Comment fusionner les données linéaires et angulaires des capteurs

Mon équipe et moi mettons en place un robot d'extérieur doté d'encodeurs, d'une IMU de qualité commerciale et d'un capteur GPS . Le robot a un entraînement de base du réservoir, de sorte que les encodeurs fournissent suffisamment de tiques des roues gauche et droite. L'IMU donne des accélérations de roulis, de tangage, de lacet et linéaires en x, y et z. Nous pourrions plus tard ajouter d'autres IMU, ce qui donnerait une redondance, mais pourrait également fournir des taux angulaires de roulis, de tangage et de lacet. Le GPS publie les coordonnées globales x, y et z.

Connaître la position xy et le cap du robot sera utile au robot pour localiser et cartographier son environnement de navigation. La vitesse du robot pourrait également être utile pour prendre des décisions de mouvement en douceur. C'est un robot au sol, donc nous ne nous soucions pas trop de l'axe z. Le robot possède également un capteur lidar et une caméra - donc le roulis et le tangage seront utiles pour transformer les données lidar et caméra pour une meilleure orientation.

J'essaie de comprendre comment fusionner tous ces nombres de manière à tirer le meilleur parti de la précision de tous les capteurs. En ce moment, nous utilisons un filtre de Kalman pour générer une estimation de [x, x-vel, x-accel, y, y-vel, y-accel]la matrice de transition simple:

[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]

Les estimations du filtre se fondent exclusivement sur les accélérations fournies par l'IMU. (L'IMU n'est pas de la meilleure qualité; dans environ 30 secondes, il montrera le robot (au repos) à une bonne dérive de 20 mètres de son emplacement initial.) Je veux savoir comment utiliser le roulis, le tangage et le lacet depuis le IMU, et potentiellement les taux de roulis, de tangage et de lacet, les données d'encodeur des roues et les données GPS pour améliorer l'estimation de l'état.

En utilisant un peu de mathématiques, nous pouvons utiliser les deux encodeurs pour générer des informations x, y et de cap sur le robot, ainsi que des vitesses linéaires et angulaires. Les encodeurs sont très précis, mais ils peuvent être susceptibles de glisser sur un champ extérieur.

Il me semble qu'il y a ici deux ensembles de données distincts, difficiles à fusionner:

1. Estimations de x, x-vel, x-accel, y, y-vel, y-accel
2. Estimations du roulis, du tangage, du lacet et des taux de roulis, tangage et lacet

Même s'il existe un croisement entre ces deux ensembles, j'ai du mal à raisonner sur la façon de les assembler. Par exemple, si le robot se déplace à une vitesse constante, la direction du robot, déterminée par son x-vel et son y-vel, sera la même que son lacet. Bien que, si le robot est au repos, le lacet ne peut pas être déterminé avec précision par les vitesses x et y. De plus, les données fournies par les encodeurs, traduites en vitesse angulaire, pourraient être une mise à jour du taux de lacet ... mais comment une mise à jour du taux de lacet pourrait-elle finir par fournir de meilleures estimations de position?

Est-il judicieux de mettre les 12 nombres dans le même filtre, ou sont-ils normalement séparés? Existe-t-il déjà une manière bien développée de traiter ce type de problème?

Deux choses.

1. Si vous prévoyez de faire du mappage, vous avez besoin d'un algorithme SLAM (Full Simultaneous Localisation and Mapping) à part entière. Voir: Localisation et cartographie simultanées (SLAM): Partie I Les algorithmes essentiels . Dans SLAM, l'estimation de l'état du robot n'est que la moitié du problème. Comment faire est une question plus importante que celle à laquelle on peut répondre ici.

2. Concernant la localisation (estimation de l'état du robot), ce n'est pas un travail pour un filtre de Kalman. La transition de à x ( t + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$n'est pas une fonction linéaire en raison des accélérations et des vitesses angulaires. Par conséquent, vous devez considérer des estimateurs non linéaires pour cette tâche. Oui, il existe des moyens standard de procéder. Oui, ils sont disponibles dans la littérature. Oui, généralement toutes les entrées sont placées dans le même filtre. La position, la vitesse, l'orientation et la vitesse angulaire du robot sont utilisées comme sorties. Et oui, je vais présenter ici une brève introduction à leurs thèmes communs. Les principaux plats à emporter sont

   1. inclure le biais Gyro et IMU dans votre état ou vos estimations divergent
   2. Un filtre Kalman étendu (EKF) est couramment utilisé pour ce problème
   3. Les implémentations peuvent être dérivées de zéro et n'ont généralement pas besoin d'être "recherchées".
   4. Des implémentations existent pour la plupart des problèmes de localisation et de SLAM, alors ne faites pas plus de travail que vous n'en avez. Voir: Robot Operating System ROS

Maintenant, pour expliquer l'EKF dans le contexte de votre système. Nous avons un IMU + Gyro, un GPS et une odométrie. Le robot en question est un entraînement différentiel comme mentionné. La tâche de filtrage est de prendre l'estimation de pose actuelle du robot x t , les entrées de commande u t , et les mesures de chaque capteur, z t , et produire l'estimation à l'étape suivante du temps x t + 1 . Nous appellerons les mesures IMU I t , le GPS est G t et l'odométrie, O t .${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Je suppose que nous souhaitons estimer la pose du robot comme . Le problème avec IMU et Gyros est la dérive. Il y a un biais non stationnaire dans les accélérations dont vous devez tenir compte dans l'EKF. Cela se fait (généralement) en plaçant le biais dans l'état estimé. Cela vous permet d'estimer directement le biais à chaque pas de temps. x t = x , y , ˙ x , ˙ y , θ , ˙ θ , $x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$, pour un vecteur de biais .$b$

Je suppose:

1. = deux mesures de distance représentant la distance parcourue par les marches dans un petit incrément de temps$O_t$
2. = trois mesures d'orientation α , β , θ et trois mesures d'accélération ¨ x , ¨ y , ¨ z .$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. = la position du robot dans lecadreglobal, G x t , G y t .$G_t$$^Gx_t,^Gy_t$

En règle générale, le résultat des entrées de contrôle (vitesses souhaitées pour chaque bande de roulement) est difficile à associer aux sorties (le changement de pose du robot). Au lieu de , il est courant (voir Thrun , Question d'odométrie) d'utiliser l'odométrie comme "résultat" du contrôle. Cette hypothèse fonctionne bien lorsque vous n'êtes pas sur une surface presque sans frottement. L'IMU et le GPS peuvent aider à corriger le glissement, comme nous le verrons.$u$

Ainsi , la première tâche consiste à prédire l'état suivant de l'état . Dans le cas d'un robot à entraînement différentiel, cette prédiction peut être obtenue directement à partir de la littérature (voir On the Kinematics of Wheeled Mobile Robots ou le traitement plus concis dans n'importe quel manuel de robotique moderne), ou dérivée de zéro comme indiqué ici: Odometry Question .$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

Ainsi, nous pouvons maintenant prédire x t + 1 = f ( x t , O t ) . Il s'agit de l'étape de propagation ou de prédiction. Vous pouvez faire fonctionner un robot en propageant simplement. Si les valeurs O t sont tout à fait exact, vous n'avez une estimation x qui ne correspond pas exactement à votre véritable état. Cela ne se produit jamais dans la pratique.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

Cela ne donne qu'une valeur prédite à partir de l'estimation précédente et ne nous dit pas comment la précision de l'estimation se dégrade avec le temps. Donc, pour propager l'incertitude, vous devez utiliser les équations EKF (qui propagent l'incertitude sous forme fermée sous des hypothèses de bruit gaussien), un filtre à particules (qui utilise une approche basée sur l'échantillonnage) *, l'UKF (qui utilise une méthode ponctuelle approximation de l'incertitude), ou l'une des nombreuses autres variantes.

Dans le cas de l'EKF, nous procédons comme suit. Soit la matrice de covariance de l'état du robot. Nous linéarisons la fonction f en utilisant l'expansion de la série Taylor pour obtenir un système linéaire. Un système linéaire peut être facilement résolu en utilisant le filtre de Kalman. Supposons que la covariance de l'estimation au temps t est P t , et la covariance supposée du bruit dans l'odométrie est donnée comme la matrice U t (généralement une matrice diagonale 2 × 2 , comme .1 × I 2 × 2 ). Dans le cas de la fonction f , on obtient le jacobien$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ etFu=∂$F_x=\frac{\partial f}{\partial x}$ , puis propagez l'incertitude comme,$F_u=\frac{\partial f}{\partial u}$

Nous pouvons maintenant propager l'estimation et l'incertitude. Notez que l'incertitude augmentera de façon monotone avec le temps. C'est attendu. Pour résoudre ce problème, ce qui est généralement fait, est d'utiliser les et G t pour mettre à jour l'état prévu. C'est ce qu'on appelle l'étape de mesure du processus de filtrage, car les capteurs fournissent une mesure indirecte de l'état du robot.$I_t$$G_t$

$h_g()$$h_i()$$R$$R_g$$R_i$$h$

$s$$z_s$

$z_g=h_g()$$H_g$ will be nearly Identity. $R_g$ is reported directly by the GPS unit in most cases.

In the case of the IMU+Gyro, the function $z_i=h_i()$ is an integration of accelerations, and an additive bias term. One way to handle the IMU is to numerically integrate the accelerations to find a position and velocity estimate at the desired time. If your IMU has a small additive noise term $p_i$ for each acceleration estimate, then you must integrate this noise to find the accuracy of the position estimate. Then the covariance $R_i$ is the integration of all the small additive noise terms, $p_i$. Incorporating the update for the bias is more difficult, and out of my expertise. However, since you are interested in planar motion, you can probably simplify the problem. You'll have to look in literature for this.

Some off-the-top-of-my-head references:

1. Improving the Accuracy of EKF-Based Visual-Inertial Odometry

2. Observability-based Consistent EKF Estimators for Multi-robot Cooperative Localization

3. Adaptive two-stage EKF for INS-GPS loosely coupled system with unknown fault bias

Ce domaine est suffisamment mature pour que Google (érudit) puisse probablement vous trouver une implémentation fonctionnelle. Si vous allez faire beaucoup de travail dans ce domaine, je vous recommande de vous procurer un manuel solide. Peut-être quelque chose comme Probablistic Robotics par S. Thrun de la renommée de Google Car. (Je l'ai trouvé une référence utile pour ces implémentations de fin de soirée).

* Il existe plusieurs estimateurs basés sur PF disponibles dans le Robot Operating System (ROS). Cependant, ceux-ci ont été optimisés pour une utilisation en intérieur. Les filtres à particules traitent les fichiers PDF multimodaux qui peuvent résulter d'une localisation basée sur une carte (suis-je près de cette porte ou de cette porte). Je crois que la plupart des implémentations en extérieur (en particulier celles qui peuvent utiliser le GPS, au moins par intermittence) utilisent le filtre Kalman étendu (EKF). J'ai utilisé avec succès le filtre Kalman étendu pour un mobile d'extérieur au sol avec entraînement différentiel.

Vous pouvez grandement simplifier le problème dans les cas les plus courants:

• Beaucoup d'IMus de "qualité commerciale" (par exemple Xsens) ont des accéléromètres très bruyants. Ne vous embêtez même pas à les fusionner pour obtenir de la vitesse, l'odométrie est déjà mieux l'ordre de grandeur. Les seules données utilisables que l'IMU va fournir sont le tangage et le roulis, et dans une certaine mesure le cap (voir point suivant)
• la direction des IMU n'est pas si fiable. Il utilise des magétomètres et montrera d'énormes dérives (jusqu'à 25 degrés sur 2 m dans notre cas) près des masses ferromagnétiques, comme celle que vous pouvez trouver dans les murs des bâtiments. Ce que nous avons fait pour résoudre ce problème est d'utiliser la rubrique IMU, mais d'estimer un biais de cap.
• Si vous êtes à l'extérieur, n'oubliez pas que parcourir 10 m sur une pente de 10 degrés n'entraîne pas le même changement de X et Y que de parcourir 10 m sur un terrain plat. Cela s'explique généralement par l'estimation de Z, mais je suppose que cela peut être estimé différemment.
• Le GPS est également une chienne couchée, généralement dans des environnements à trajets multiples élevés. De plus, les GPS de faible qualité (et même dans certaines conditions de haute qualité) ont tendance à signaler des écarts-types très erronés. Nous avons utilisé des tests chi carré simples pour vérifier si une mesure GPS particulière devait être intégrée (c'est-à-dire vérifier qu'elle correspond à l'estimation actuelle du filtre jusqu'à un certain point), ce qui nous a donné des résultats décents.

La solution "typique" pour nous consiste à utiliser l'odométrie + IMU pour obtenir une estimation du mouvement de l'ego, puis à utiliser le GPS pour corriger les biais X, Y, Z et cap.

Voici une implémentation EKF que nous avons largement utilisée. Si vous avez besoin d'estimer l'orientation de l'IMU (c'est-à-dire s'il n'a pas déjà de filtre intégré), vous pouvez également utiliser l'un de ces deux filtres: UKF et EKF .