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,θ).

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=[10vΔtsin(θ)01vΔtcos(θ)001]

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 θ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

Odpowiedzi:

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 jakozt=h(xt)+N(0,Qt). 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 dlah(xt) 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