Rozszerzony filtr Kalmana ze skanowaniem laserowym + znana mapa

Obecnie pracuję nad projektem dla szkoły, w którym muszę wdrożyć rozszerzony filtr Kalmana dla robota punktowego ze skanerem laserowym. Robot może się obracać z promieniem obrotu 0 stopni i jechać do przodu. Wszystkie ruchy są częściowo liniowe (napęd, obrót, napęd).

Symulator, którego używamy, nie obsługuje przyspieszenia, cały ruch jest natychmiastowy.

Mamy również znaną mapę (obraz PNG), w której musimy się zlokalizować. Możemy naświetlić ślad na obrazie w celu symulacji skanów laserowych.

Mój partner i ja jesteśmy trochę zdezorientowani co do modeli ruchu i czujników, których będziemy musieli użyć.

Do tej pory modelujemy stan jako wektor $(x,y,\theta)$.

Używamy równań aktualizacji w następujący sposób

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}

Myśleliśmy, że wszystko działa, dopóki nie zauważyliśmy, że zapomnieliśmy zainicjować Pi że było zero, co oznacza, że ​​nie nastąpiła korekta. Najwyraźniej nasza propagacja była bardzo dokładna, ponieważ nie wprowadziliśmy jeszcze hałasu do systemu.

Dla modelu ruchu używamy następującej macierzy dla F:

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

Jak to jest jakobian z naszych formuł aktualizacji. Czy to jest poprawne?

W przypadku modelu czujnika przybliżamy jakobian (H), biorąc skończone różnice w robotach $x$, $y$ i $\theta$pozycje i ray tracing na mapie. Rozmawialiśmy z TA, który powiedział, że to zadziała, ale nadal nie jestem pewien. Nasz prof jest nieobecny, więc nie możemy go niestety zapytać. Używamy 3 pomiarów laserowych na krok korekty, więc H to 3x3.

Innym problemem jest to, jak zainicjować P. Wypróbowaliśmy 1,10 100 i wszyscy umieszczają robota poza mapą w (-90, -70), gdy mapa ma tylko 50 x 50.

Kod naszego projektu można znaleźć tutaj: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

Wszelkie porady są mile widziane.

EDYTOWAĆ:

W tym momencie filtr ustabilizował się przy podstawowym hałasie ruchu, ale bez rzeczywistego ruchu. Gdy tylko robot zacznie się poruszać, filtr rozbiega się dość szybko i wychodzi z mapy.

• Używanie EKF do lokalizacji na podstawie skanów laserowych i znanej mapy jest złym pomysłem i nie będzie działać, ponieważ jedno z głównych założeń EKF jest nieprawidłowe: Modelu pomiaru nie można rozróżnić.

Sugerowałbym przyjrzenie się lokalizacji Monte Carlo (filtry cząstek). To nie tylko rozwiąże problem z twoim modelem pomiarowym, ale także pozwoli na globalną lokalizację (początkowa pozycja na mapie zupełnie nieznana).

Edycja: Przepraszam, zapomniałem wspomnieć o innym ważnym punkcie:

• Jeśli chcesz zastosować EKF, zarówno model pomiaru, jak i model ruchu mogą zawierać tylko szum Gaussa . Oznacza to, że musisz być w stanie zapisać swój model pomiarowy jako$z_t = h(x_t) + N(0, Q_t)$. Jest to poważne ograniczenie, ponieważ nie pozwala na nieco bardziej złożony model, taki jak model beam_range_finder_model w Probabilistic Robotics, który uwzględnia również obiekty dynamiczne przed robotem, nieprawidłowe (maksymalne) pomiary i całkowicie losowe odczyty. Utknąłbyś z promieniami rzucania dla$h(x_t)$ część i dodanie szumu gaussowskiego.

Przede wszystkim nie wspomniałeś o tym, jakiego rodzaju lokalizacji używasz. Czy to ze znanymi lub nieznanymi korespondencjami?

Teraz, aby zdobyć Jakobian w Matlabie, możesz wykonać następujące czynności

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

Wynik

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

Rozszerzony filtr Kalmana wymaga, aby system był liniowy, a szum - Gaussa. System oznacza tutaj, że modele ruchu i obserwacji muszą być liniowe. Czujniki laserowe podają zasięg i kąt w kierunku punktu orientacyjnego z ramy robota, więc model pomiarowy nie jest liniowy. O P, jeśli założymy, że jest duży, to estymator EKF będzie początkowo słaby i opiera się na pomiarach, ponieważ poprzedni wektor stanu nie jest dostępny. Jednak gdy robot zacznie się poruszać i wykrywać, EKF będzie coraz lepszy, ponieważ wykorzystuje modele ruchu i pomiarów do oszacowania pozycji robota. Jeśli twój robot nie wykrywa żadnych punktów orientacyjnych, niepewność rośnie drastycznie. Musisz także uważać na kąt. W C ++cos and sin, kąt powinien być w radianach. W kodzie nic nie ma na ten temat. Jeśli zakładasz szum kąta w stopniach podczas obliczania w radianach, możesz uzyskać dziwne wyniki, ponieważ jeden stopień jako hałas, podczas gdy obliczenia w radianach są uważane za ogromne.