Rozszerzony filtr Kalmana ze skanowaniem laserowym + znana mapa

10

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.

— en4bz
źródło

3

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.

— Sebsch
źródło

^^ „Używanie EKF do lokalizacji na podstawie skanów laserowych i znanej mapy to zły pomysł”, kto to powiedział? Proszę podać referencje. EKF jest najbardziej udanym estymatorem i wiele artykułów sugeruje użycie EKF do problemu lokalizacji. Tak naprawdę jestem zaskoczony, że tak mówisz. Głównymi założeniami EKF są modele ruchu i obserwacji są liniowe, a hałas jest gaussowski. Nie wiem, co masz na myśli przez „Model pomiaru nie jest różnicowalny”.

— CroCo

W oryginalnym plakacie wspomniano o raytracingu, co oznacza, że zamierza on zastosować model pomiarowy podobny do „modelu wiązki dalmierzy” w robotyce probabilistycznej. Ten model pomiarowy pokazuje skoki (wyobraź sobie, że promień lasera ledwo uderza w przeszkodę i omija ją: potrzeba tylko niewielkiego obrotu do skoku, którego nie można rozróżnić.)

— Sebsch

0

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.

— CroCo
źródło