Jak połączyć dane liniowe i kątowe z czujników

Mój zespół i ja przygotowujemy robota zewnętrznego, który ma enkodery, IMU klasy komercyjnej i czujnik GPS . Robot ma podstawowy napęd zbiornika, więc enkodery w wystarczającym stopniu dostarczają kleszcze z lewego i prawego koła. IMU podaje przyspieszenia przechyłu, pochylenia, odchylenia i liniowe w x, yiz. Później moglibyśmy dodać inne IMU, co zapewniłoby nadmiarowość, ale mogłoby dodatkowo zapewnić kątowe prędkości przechylenia, skoku i odchylenia. GPS publikuje globalne współrzędne x, y i z.

Znajomość pozycji xy i kierunku robota przyda się robotowi do zlokalizowania i zmapowania środowiska do nawigacji. Prędkość robota może być również przydatna do podejmowania płynnych decyzji ruchowych. Jest to robot naziemny, więc nie przejmujemy się zbytnio osią Z. Robot ma również czujnik lidar i kamerę - więc przechylenie i nachylenie będą przydatne do przekształcania danych lidar i kamery w celu lepszej orientacji.

Próbuję dowiedzieć się, jak połączyć wszystkie te liczby w taki sposób, aby optymalnie wykorzystać dokładność wszystkich czujników. Obecnie używamy filtru Kalmana do generowania oszacowania [x, x-vel, x-accel, y, y-vel, y-accel]za pomocą prostej macierzy przejścia:

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

Filtr szacuje stan wyłącznie na podstawie przyspieszeń dostarczonych przez IMU. (IMU nie jest najlepszej jakości; w ciągu około 30 sekund pokaże robota (w spoczynku) dryfującego dobre 20 metrów od swojej początkowej lokalizacji.) Chcę wiedzieć, jak korzystać z przechyłu, skoku i odchylenia od IMU i potencjalnie prędkości przechyłu, pochylenia i odchylenia, dane enkodera z kół i dane GPS w celu poprawy oceny stanu.

Używając odrobiny matematyki, możemy użyć dwóch koderów do wygenerowania x, y i informacji o kursie na robocie, a także prędkości liniowych i kątowych. Enkodery są bardzo dokładne, ale mogą być podatne na poślizg na polu zewnętrznym.

Wydaje mi się, że istnieją tutaj dwa oddzielne zestawy danych, które trudno połączyć:

1. Oszacowania x, x-vel, x-accel, y, y-vel, y-accel
2. Oszacowania przechyłu, skoku, odchylenia i prędkości przechylenia, skoku i odchylenia

Mimo że między tymi dwoma zestawami występuje krzyżowanie, mam problem z uzasadnieniem, jak je połączyć. Na przykład, jeśli robot jedzie ze stałą prędkością, kierunek robota, określony przez jego x-vel i y-vel, będzie taki sam jak jego odchylenie. Chociaż, jeśli robot jest w spoczynku, odchylenie nie może być dokładnie określone przez prędkości xiy. Również dane dostarczone przez kodery, przetłumaczone na prędkość kątową, mogą być aktualizacją prędkości odchylenia ... ale w jaki sposób aktualizacja prędkości odchylenia może zapewnić lepsze oszacowania położenia?

Czy sensowne jest umieszczenie wszystkich 12 liczb w tym samym filtrze, czy zwykle są one oddzielone? Czy istnieje już dobrze opracowany sposób radzenia sobie z tego rodzaju problemem?

Dwie rzeczy.

1. Jeśli planujesz wykonać mapowanie, potrzebujesz pełnoprawnego algorytmu jednoczesnej lokalizacji i mapowania (SLAM). Zobacz: Jednoczesna lokalizacja i mapowanie (SLAM): Część I Niezbędne algorytmy . W SLAM oszacowanie stanu robota to tylko połowa problemu. Jak to zrobić, jest większym pytaniem niż można tutaj znaleźć odpowiedź.

2. Jeśli chodzi o lokalizację (szacowanie stanu robota), nie jest to zadanie dla filtra Kalmana. Przejście od do x ( t + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$nie jest funkcją liniową z powodu przyspieszeń i prędkości kątowych. Dlatego należy rozważyć nieliniowe estymatory dla tego zadania. Tak, istnieją standardowe sposoby na zrobienie tego. Tak, są dostępne w literaturze. Tak, zwykle wszystkie dane wejściowe są umieszczane w tym samym filtrze. Pozycja, prędkość, orientacja i prędkość kątowa robota są wykorzystywane jako dane wyjściowe. I tak, przedstawię tutaj krótkie wprowadzenie do ich wspólnych tematów. Główne dania na wynos to

   1. uwzględnij odchylenie Gyro i IMU w swoim stanie, w przeciwnym razie twoje prognozy będą się różnić
   2. Rozszerzony filtr Kalmana (EKF) jest powszechnie stosowany do tego problemu
   3. Implementacje można uzyskać od zera i zazwyczaj nie trzeba ich „sprawdzać”.
   4. Implementacje istnieją dla większości problemów związanych z lokalizacją i SLAM, więc nie wykonuj więcej pracy niż musisz. Zobacz: System operacyjny robota ROS

Teraz, aby wyjaśnić EKF w kontekście twojego systemu. Mamy IMU + żyroskop, GPS i odometrię. Wspomniany robot jest napędem różnicowym, jak wspomniano. Filtrowanie zadaniem jest pobieranie aktualnego oszacowania Pozy robot x t , wejścia sterujące U t i pomiar z każdego czujnika oo t i dają oszacowanie przy następnym takcie x t + 1 . Nazwiemy pomiary IMU I t , GPS to G t , a odometria, O t .${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Zakładam, że jesteśmy zainteresowani w szacowaniu pozę robota jako . Problem z IMU i Gyros polega na znoszeniu. W przyspieszeniach występuje niestacjonarne odchylenie, które należy uwzględnić w EKF. Odbywa się to (zwykle) poprzez ustawienie błędu w szacowanym stanie. Umożliwia to bezpośrednie oszacowanie odchylenia na każdym etapie. 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}$, dla wektora uprzedzeń .$b$

Jestem zarozumiały:

1. = dwa pomiary odległości reprezentujące odległość pokonaną przez stopnie w niewielkim odstępie czasu$O_t$
2. = trzy pomiary orientacji α , β , θ i trzy pomiary przyspieszenia ¨ x , ¨ y , ¨ z .$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. = pozycja robota wglobalnejramce, G x t , G y t .$G_t$$^Gx_t,^Gy_t$

Zazwyczaj wyniki danych wejściowych sterowania (pożądane prędkości dla każdego stopnia) są trudne do odwzorowania na dane wyjściowe (zmiana pozy robota). Zamiast powszechne jest (patrz Thrun , pytanie o odometrię) stosowanie odometrii jako „wyniku” kontroli. To założenie działa dobrze, gdy nie znajdujesz się na prawie pozbawionej tarcia powierzchni. IMU i GPS mogą pomóc skorygować poślizg, jak zobaczymy.$u$

Tak więc pierwszym celem jest przewidzieć następny stan ze stanu . W przypadku robota z napędem różnicowym prognozę tę można uzyskać bezpośrednio z literatury (patrz: Kinematyka kołowych robotów mobilnych$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$ lub bardziej zwięzłe traktowanie w jakimkolwiek współczesnym podręczniku robotyki) lub wyprowadzić od zera, jak pokazano tutaj: Pytanie o odometrię .

Tak więc, można teraz przewidzieć x t + 1 = F ( x , T , O , T ) . To jest krok propagacji lub przewidywania. Państwo może działać robota po prostu rozmnożeniowego. Jeśli wartości O t są całkowicie dokładne, nigdy nie będzie miał szacunkową x , który nie dokładnie równa swój prawdziwy stan. W praktyce tak się nigdy nie dzieje.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

Daje to tylko przewidywaną wartość z poprzedniego oszacowania i nie mówi nam, jak dokładność oszacowania zmniejsza się z czasem. Aby propagować niepewność, należy zastosować równania EKF (które propagują niepewność w formie zamkniętej przy założeniach szumu Gaussa), filtr cząstek stałych (który wykorzystuje podejście oparte na próbkowaniu) *, UKF (który stosuje punktowe przybliżenie niepewności) lub jeden z wielu innych wariantów.

W przypadku EKF postępujemy w następujący sposób. Niech będzie macierzą kowariancji stanu robota. Linearyzujemy funkcję f za pomocą rozszerzenia szeregu Taylora, aby uzyskać układ liniowy. System liniowy można łatwo rozwiązać za pomocą filtra Kalmana. Załóżmy, że kowariancja oszacowania w czasie t wynosi P t , a zakładana kowariancja szumu w odometrii jest podana jako macierz U t (zwykle macierz diagonalna 2 × 2 , jak np. 1 × I 2 × 2 ). W przypadku funkcji f otrzymujemy jakobian$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}$

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.