Come fondere i dati lineari e angolari dai sensori

Il mio team e io stiamo installando un robot esterno con encoder, un IMU di livello commerciale e un sensore GPS . Il robot ha un azionamento di base del serbatoio, quindi gli encoder forniscono sufficientemente zecche dalle ruote sinistra e destra. L'IMU fornisce roll, pitch, yaw e accelerazioni lineari in x, ye z. In seguito potremmo aggiungere altre IMU, il che darebbe ridondanza, ma potrebbe anche fornire tassi angolari di rollio, beccheggio e imbardata. Il GPS pubblica le coordinate globali x, ye z.

Conoscere la posizione e la direzione xy del robot sarà utile per il robot per localizzare e mappare l'ambiente di navigazione. La velocità del robot potrebbe anche essere utile per prendere decisioni di movimento fluide. È un robot a terra, quindi non ci importa troppo dell'asse z. Il robot ha anche un sensore lidar e una telecamera, quindi roll and pitch saranno utili per trasformare il lidar e i dati della telecamera per un migliore orientamento.

Sto cercando di capire come fondere tutti questi numeri in un modo che sfrutti in modo ottimale l'accuratezza di tutti i sensori. In questo momento stiamo usando un filtro Kalman per generare una stima [x, x-vel, x-accel, y, y-vel, y-accel]con la semplice matrice di transizione:

[[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]]

Le stime del filtro si basano esclusivamente sulle accelerazioni fornite dall'IMU. (L'IMU non è della migliore qualità; entro circa 30 secondi mostrerà il robot (a riposo) alla deriva a una buona distanza di 20 metri dalla sua posizione iniziale.) Voglio sapere come usare rollio, beccheggio e imbardata dal IMU e potenzialmente velocità di rollio, beccheggio e imbardata, dati dell'encoder dalle ruote e dati GPS per migliorare la stima dello stato.

Usando un po 'di matematica, possiamo usare i due encoder per generare informazioni di x, y e di direzione sul robot, nonché velocità lineari e angolari. Gli encoder sono molto precisi, ma possono essere soggetti a scivolamento in un campo esterno.

Mi sembra che qui ci siano due serie separate di dati, che sono difficili da fondere:

1. Stime di x, x-vel, x-accel, y, y-vel, y-accel
2. Stime di rollio, beccheggio, imbardata e velocità di rollio, beccheggio e imbardata

Anche se c'è un crossover tra questi due set, ho difficoltà a ragionare su come metterli insieme. Ad esempio, se il robot procede a velocità costante, la direzione del robot, determinata dai suoi x-vel e y-vel, sarà la stessa della sua imbardata. Sebbene, se il robot è a riposo, l'imbardata non può essere determinata con precisione dalle velocità xey. Inoltre, i dati forniti dagli encoder, tradotti in velocità angolare, potrebbero essere un aggiornamento della velocità di imbardata ... ma come potrebbe un aggiornamento della velocità di imbardata finire per fornire migliori stime posizionali?

Ha senso mettere tutti i 12 numeri nello stesso filtro o sono normalmente tenuti separati? Esiste già un modo ben sviluppato di affrontare questo tipo di problema?

Due cose.

1. Se si prevede di eseguire la mappatura, è necessario un algoritmo di localizzazione e mappatura simultanea (SLAM) completo. Vedere: Localizzazione e mappatura simultanea (SLAM): Parte I Gli algoritmi essenziali . In SLAM, stimare lo stato del robot è solo metà del problema. Come farlo è una domanda più grande di quella a cui si può rispondere qui.

2. Per quanto riguarda la localizzazione (stima dello stato del robot), questo non è un lavoro per un filtro Kalman. La transizione da a x ( t + $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$non è una funzione lineare a causa delle accelerazioni e velocità angolari. Pertanto, è necessario considerare gli stimatori non lineari per questa attività. Sì, ci sono modi standard per farlo. Sì, sono disponibili in letteratura. Sì, in genere tutti gli input vengono inseriti nello stesso filtro. Posizione, velocità, orientamento e velocità angolare del robot vengono utilizzati come uscite. E sì, presenterò una breve introduzione ai loro temi comuni qui. I principali take-away sono

   1. includi il pregiudizio giroscopio e IMU nel tuo stato o le tue stime divergeranno
   2. Un filtro Kalman esteso (EKF) viene comunemente utilizzato per questo problema
   3. Le implementazioni possono essere derivate da zero e in genere non è necessario "cercare".
   4. Le implementazioni esistono per la maggior parte dei problemi di localizzazione e SLAM, quindi non fare più lavoro del necessario. Vedere: Sistema operativo robot ROS

Ora, per spiegare l'EKF nel contesto del tuo sistema. Abbiamo un IMU + Gyro, GPS e odometria. Il robot in questione è un azionamento differenziale come menzionato. Il compito filtraggio è prendere la stima posa corrente del robot x t , gli ingressi di controllo u t , e le misurazioni da ogni sensore, z t , e produrre la stima al passo temporale successivo x t + 1 . Chiameremo le misurazioni IMU I t , il GPS è G t e l'odometria, O t .${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Suppongo che siamo interessati a stimare la posa del robot come . Il problema con IMU e Gyros è la deriva. Vi è una propensione non stazionaria nelle accelerazioni di cui è necessario tenere conto nell'EKF. Questo viene fatto (di solito) ponendo la distorsione nello stato stimato. Ciò consente di stimare direttamente il bias in ogni fase del tempo. 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}$, per un vettore di pregiudizi .$b$

Presumo:

1. = due misurazioni della distanza che rappresentano la distanza percorsa dai gradini con un piccolo incremento di tempo$O_t$
2. = tre misure di orientamento α , β , θ e tre misure di accelerazione ¨ x , ¨ y , ¨ z .$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. = la posizione del robot nelframeglobale, G x t , G y t .$G_t$$^Gx_t,^Gy_t$

In genere, il risultato degli ingressi di controllo (velocità desiderate per ciascun battistrada) è difficile da mappare alle uscite (il cambiamento nella posa del robot). Al posto di , è comune (vedi Thrun , Domanda sull'odometria ) usare l'odometria come "risultato" del controllo. Questa ipotesi funziona bene quando non ci si trova su una superficie quasi senza attrito. L'IMU e il GPS possono aiutare a correggere lo slittamento, come vedremo.$u$

Quindi il primo compito è quello di prevedere il prossimo stato dallo stato . Nel caso di un robot con azionamento differenziale, questa previsione può essere ottenuta direttamente dalla letteratura (vedere Sulla cinematica dei robot mobili su ruote o il trattamento più conciso in qualsiasi manuale di robotica moderno), oppure derivata da zero come mostrato qui: Domanda sull'odometria .$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

Quindi, possiamo ora prevedere x t + 1 = f ( x t , O t ) . Questa è la fase di propagazione o previsione. È possibile utilizzare un robot con la semplice moltiplicazione. Se i valori O t sono completamente accurato, non avrete mai una stima x che non esattamente uguale il tuo vero stato. Questo non succede mai in pratica.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

Ciò fornisce solo un valore previsto dalla stima precedente e non ci dice in che modo l'accuratezza della stima si riduce con il tempo. Quindi, per propagare l'incertezza, è necessario utilizzare le equazioni EKF (che propagano l'incertezza in forma chiusa sotto ipotesi di rumore gaussiano), un filtro antiparticolato (che utilizza un approccio basato sul campionamento) *, UKF (che utilizza un punto di vista approssimazione dell'incertezza) o una delle tante altre varianti.

Nel caso dell'EKF, procediamo come segue. Sia la matrice di covarianza dello stato del robot. Linearizziamo la funzione f usando l'espansione della serie Taylor per ottenere un sistema lineare. Un sistema lineare può essere facilmente risolto utilizzando il filtro Kalman. Supponiamo che la covarianza della stima al momento t sia P t , e la presunta covarianza del rumore nell'odometria è data come matrice U t (di solito una matrice diagonale 2 × 2 , come .1 × I 2 × 2 ). Nel caso della funzione f , otteniamo il giacobino$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ $F_x=\frac{\partial f}{\partial x}$$F_u=\frac{\partial f}{\partial u}$

$I_t$$G_t$

$h_g()$$h_i()$ for GPS, IMU. Form the residual or innovation which is the difference of the predicted and measured values. Then, estimate the accuracy for each sensor estimate in the form of a covariance matrix $R$ for all sensors ($R_g$, $R_i$ in this case). Finally, find the Jacobians of $h$ and update the state estimate as follows:

For each sensor $s$ with state estimate $z_s$ (Following wikipedia's entry)

In the case of GPS, the measurement $z_g=h_g()$ it is probably just a transformation from latitude and longitude to the local frame of the robot, so the Jacobian $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

This field is mature enough that google (scholar) could probably find you a working implementation. If you are going to be doing a lot of work in this area, I recommend you pick up a solid textbook. Maybe something like Probablistic Robotics by S. Thrun of Google Car fame. (I've found it a useful reference for those late-night implementations).

*There are several PF-based estimators available in the Robot Operating System (ROS). However, these have been optimized for indoor use. Particle filters deal with the multi-modal PDFs which can result from map-based localization (am I near this door or that door). I believe most outdoor implementations (especially those that can use GPS, at least intermittently) use the Extended Kalman Filter (EKF). I've successfully used the Extended Kalman Filter for an outdoor, ground rover with differential drive.

You can greatly simplify the problem in most common cases:

• A lot of "commercial grade" IMus (e.g. Xsens) have very noisy accelerometers. Don't even bother fusing them to get speed, the odometry is already order of magnitudes better. The only usable data the IMU is going to provide is the pitch and roll, and to some extent the heading (see next point)
• heading from IMUs is not that trustworthy. It uses magetometers, and will show huge drifts (up to 25 degrees over 2m in our case) near ferromagnetic masses, such as the one you can find in building walls. What we did to solve this is to use the IMU heading, but estimate a heading bias.
• If you are outdoors, don't forget that travelling 10m on a 10 degree incline does not lead to the same change in X and Y than travelling 10m on a flat terrain. This is usually accounted for by estimating Z, but I guess it can be estimated differently.
• GPS is also a lying bitch, typically in high-multipath environments. Plus low-grade (and even in some conditions high-grade) GPSes have a tendency to report very wrong standard deviations. We used some simple chi-square tests to check whether a particular GPS measurement should be integrated (i.e. checking that it matches the current filter estimate up to a certain point), which gave us decent results.

The "typical" solution for us is to use odometry + IMU to get an ego-motion estimate and then use GPS to correct X,Y,Z and heading bias.

Here is an EKF implementation that we extensively used. If you need to estimate the IMU's orientation (i.e. if it does not already have a built-in filter), you can also use on of these two filter: UKF and EKF.