Como fundir dados lineares e angulares de sensores

Minha equipe e eu estamos montando um robô externo que possui codificadores, IMU de nível comercial e sensor de GPS . O robô possui uma unidade básica de tanque, de modo que os codificadores fornecem carrapatos suficientemente pelas rodas esquerda e direita. A IMU fornece acelerações de rotação, inclinação, guinada e linear em x, ye z. Posteriormente, poderíamos adicionar outras IMUs, o que daria redundância, mas também poderia fornecer taxas angulares de rotação, inclinação e guinada. O GPS publica coordenadas globais x, ye z.

Conhecer a posição xy e a direção do robô será útil para o robô localizar e mapear seu ambiente de navegação. A velocidade do robô também pode ser útil para tomar decisões de movimentos suaves. É um robô terrestre, por isso não nos importamos muito com o eixo z. O robô também possui um sensor LID e uma câmera - portanto, o movimento de inclinação será útil para transformar os dados do LID e da câmera para melhor orientação.

Estou tentando descobrir como fundir todos esses números de uma maneira que otimamente aproveite a precisão de todos os sensores. No momento, estamos usando um filtro Kalman para gerar uma estimativa [x, x-vel, x-accel, y, y-vel, y-accel]com a matriz de transição simples:

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

O filtro estima o estado exclusivamente com base nas acelerações fornecidas pela IMU. (A IMU não é da melhor qualidade; em cerca de 30 segundos ele mostrará o robô (em repouso) à deriva a uns bons 20 metros de sua localização inicial.) Quero saber como usar giro, inclinação e guinada do IMU e taxas de rotação, inclinação e guinada potencialmente, dados do codificador das rodas e dados do GPS para melhorar a estimativa do estado.

Usando um pouco de matemática, podemos usar os dois codificadores para gerar x, y e informações de rumo no robô, bem como velocidades lineares e angulares. Os codificadores são muito precisos, mas podem ser suscetíveis a derrapagens em um campo externo.

Parece-me que existem dois conjuntos de dados separados aqui, difíceis de fundir:

1. Estimativas de x, x-vel, x-accel, y, y-vel, y-accel
2. Estimativas de rotação, inclinação, guinada e taxas de rotação, inclinação e guinada

Mesmo havendo cruzamento entre esses dois conjuntos, estou tendo problemas para pensar em como montá-los. Por exemplo, se o robô estiver a uma velocidade constante, a direção do robô, determinada por seu x-vel e y-vel, será a mesma que sua guinada. Embora, se o robô estiver em repouso, a guinada não pode ser determinada com precisão pelas velocidades x e y. Além disso, os dados fornecidos pelos codificadores, traduzidos para a velocidade angular, podem ser uma atualização na taxa de guinada ... mas como uma atualização na taxa de guinada pode resultar em melhores estimativas posicionais?

Faz sentido colocar todos os 12 números no mesmo filtro ou eles são normalmente mantidos separados? Já existe uma maneira bem desenvolvida de lidar com esse tipo de problema?

Duas coisas.

1. Se você planeja fazer o mapeamento, precisa de um algoritmo de Localização e Mapeamento Simultâneo (SLAM) completo. Veja: Localização e Mapeamento Simultâneos (SLAM): Parte I Os Algoritmos Essenciais . No SLAM, estimar o estado do robô é apenas metade do problema. Como fazer isso é uma pergunta maior do que pode ser respondida aqui.

2. Em relação à localização (estimativa do estado do robô), este não é um trabalho para um filtro Kalman. A transição de para x ( t + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$não é uma função linear devido às acelerações e velocidades angulares. Portanto, você precisa considerar estimadores não lineares para esta tarefa. Sim, existem maneiras padrão de fazer isso. Sim, eles estão disponíveis na literatura. Sim, normalmente todas as entradas são colocadas no mesmo filtro. Posição, velocidade, orientação e velocidade angular do robô são usadas como saídas. E sim, apresentarei uma breve introdução aos temas comuns aqui. As principais opções de take-away são

   1. inclua o viés Gyro e IMU no seu estado ou suas estimativas divergirão
   2. Um filtro Kalman estendido (EKF) é comumente usado para esse problema
   3. As implementações podem ser derivadas do zero e geralmente não precisam ser "pesquisadas".
   4. Existem implementações para a maioria dos problemas de localização e SLAM, portanto, não faça mais trabalho do que o necessário. Consulte: ROS do sistema operacional do robô

Agora, para explicar o EKF no contexto do seu sistema. Temos um IMU + Gyro, GPS e odometria. O robô em questão é uma unidade diferencial, como mencionado. A tarefa de filtragem é para levar a estimativa atual pose do robô x t , as entradas de controlo de u t , e as medições a partir de cada sensor, z t , e produzir a estimativa na etapa seguinte tempo x t + 1 . Vamos chamar as medições IMU de I t , o GPS é G t e a odometria, O t .${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Presumo que estamos interessados em estimar a pose robô como . O problema com IMU e giroscópios é drift. Existe um viés não estacionário nas acelerações que você deve considerar no EKF. Isso é feito (geralmente) colocando o viés no estado estimado. Isso permite estimar diretamente o viés a cada etapa do 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}$, para um vetor de vieses .$b$

Estou assumindo:

1. = duas medições de distância representando a distância que os degraus percorreram em um pequeno incremento de tempo$O_t$
2. = três medidas de orientação α , β , θ e três medidas de aceleração ¨ x , ¨ y , ¨ z .$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. = a posição do robô noquadroglobal, G x t , G y t .$G_t$$^Gx_t,^Gy_t$

Normalmente, é difícil mapear o resultado das entradas de controle (velocidades desejadas para cada banda de rodagem) para as saídas (a mudança na posição do robô). No lugar de , é comum (consulte Thrun , Odometry Question ) usar a odometria como o "resultado" do controle. Essa suposição funciona bem quando você não está em uma superfície quase sem atrito. O IMU e o GPS podem ajudar a corrigir o deslizamento, como veremos.$u$

Assim, a primeira tarefa é a de prever o próximo estado a partir do estado . No caso de um robô com acionamento diferencial, essa previsão pode ser obtida diretamente da literatura (consulte Sobre a cinemática dos robôs móveis com rodas ou o tratamento mais conciso em qualquer livro de robótica moderno) ou derivada do zero, como mostrado aqui: Pergunta de odometria .$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

Assim, agora podemos prever x t + 1 = f ( x t , O t ) . Esta é a etapa de propagação ou previsão. Você pode operar um robô simplesmente propagando. Se os valores de S t são completamente precisos, você nunca terá uma estimativa x , que não é igual exatamente o seu verdadeiro estado. Isso nunca acontece na prática.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

Isso fornece apenas um valor previsto da estimativa anterior e não nos diz como a precisão da estimativa diminui com o tempo. Portanto, para propagar a incerteza, você deve usar as equações EKF (que propagam a incerteza na forma fechada sob as premissas de ruído gaussianas), um filtro de partículas (que usa uma abordagem baseada em amostragem) *, o UKF (que usa uma abordagem pontual aproximação da incerteza) ou uma de muitas outras variantes.

$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ $F_x=\frac{\partial f}{\partial x}$ and $F_u=\frac{\partial f}{\partial u}$, then propagate the uncertainty as,

Now we can propagate the estimate and the uncertainty. Note the uncertainty will monotonically increase with time. This is expected. To fix this, what is typically done, is to use the $I_t$ and $G_t$ to update the predicted state. This is called the measurement step of the filtering process, as the sensors provide an indirect measurement of the state of the robot.

First, use each sensor to estimate some part of the robot state as some function $h_g()$ and $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.