Lazer Taramalı Genişletilmiş Kalman Filtresi + Bilinen Harita

Şu anda lazer tarayıcılı bir nokta robotu için genişletilmiş bir Kalman Filtresi uygulamam gereken okul projesi üzerinde çalışıyorum. Robot 0 derece dönüş yarıçapı ile dönebilir ve ileri doğru sürülebilir. Tüm hareketler parçalı doğrusaldır (sürücü, döndürme, sürücü).

Kullandığımız simülatör hızlanmayı desteklemiyor, tüm hareketler anlık.

Ayrıca, yerelleştirmemiz gereken bilinen bir haritaya (png resmi) sahibiz. Lazer taramalarını simüle etmek için görüntüde iz bırakabiliriz.

Ortağım ve ben, kullanmamız gereken hareket ve sensör modelleri konusunda biraz kafamız karıştı.

Şimdiye kadar devleti bir vektör olarak modelliyoruz $(x,y,\theta)$.

Güncelleme denklemlerini aşağıdaki gibi kullanıyoruz

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;
}

Başlatmayı unuttuğumuzu Pve sıfır olduğunu fark edene kadar her şeyin çalıştığını düşündük , yani hiçbir düzeltme olmadı. Görünüşe göre yayılımımız çok hassastı çünkü henüz sisteme gürültü getirmedik.

Hareket modeli için F için aşağıdaki matrisi kullanıyoruz:

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

Bizim güncelleme formülleri Jacobian olarak. Bu doğru mu?

Sensör modeli için, robotların sonlu farklarını alarak Jacobian'a (H) yaklaşıyoruz $x$, $y$ ve $\theta$haritadaki konumlar ve ışın izleme. Bunun işe yarayacağını söyleyen TA ile konuştuk, ancak yine de olacağından emin değilim. Profesörümüz uzakta, maalesef ona soramayız. Düzeltme adımı başına 3 lazer ölçümü kullanıyoruz, böylece H 3x3'tür.

P'nin nasıl başlatılacağına dair diğer bir sorun 1,10,100'ü denedik ve hepsi sadece harita 50x50 olduğunda robotu haritanın dışına (-90, -70) yerleştirdiler.

Projemizin kodu şu adreste bulunabilir: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

Herhangi bir tavsiye büyük beğeni topluyor.

DÜZENLE:

Bu noktada filtreyi temel hareket gürültüsü ile stabilize olacak şekilde aldım, ancak gerçek hareket yok. Robot hareket etmeye başlar başlamaz filtre oldukça hızlı bir şekilde ayrılır ve haritadan çıkar.

• Lazer taramalara ve bilinen bir haritaya dayalı yerelleştirme için EKF'yi kullanmak kötü bir fikirdir ve EKF'nin ana varsayımlarından biri geçerli olmadığı için çalışmaz: Ölçüm modeli farklı değildir.

Monte Carlo Lokalizasyonuna (Parçacık Filtreleri) bakmanızı öneririm. Bu sadece ölçüm modelinizle ilgili sorunu çözmekle kalmayacak, aynı zamanda küresel lokalizasyona da izin verecektir (haritadaki ilk poz tamamen bilinmemektedir).

Edit: Üzgünüm başka bir önemli noktaya söz unuttum:

• EKF uygulamak istiyorsanız, ölçüm modelinizin yanı sıra hareket modeliniz yalnızca Gauss gürültüsünü içerebilir . Bu, ölçüm modelinizi şu şekilde yazabilmeniz gerektiği anlamına gelir:$z_t = h(x_t) + N(0, Q_t)$. Bu ciddi bir sınırlamadır, çünkü Probabilistic Robotics'deki beam_range_finder_model gibi biraz daha karmaşık bir modele izin vermez, bu da robotun önündeki dinamik nesneleri, geçersiz (maks.) Ölçümleri ve tamamen rastgele okumaları da dikkate alır. İçin döküm ışınları ile sıkışmış olurdu$h(x_t)$ ve Gauss gürültüsü ekleyerek.

Her şeyden önce, ne tür bir yerelleştirme kullandığınızdan bahsetmediniz. Bilinen veya bilinmeyen yazışmalara sahip mi?

Şimdi Matlab'da Jacobian'ı edinmek için aşağıdakileri yapabilirsiniz

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

Sonuç

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

Genişletilmiş Kalman Filtresi, sistemin doğrusal ve gürültünün Gauss olmasını gerektirir. Buradaki sistem, hareket ve gözlem modellerinin doğrusal olması gerektiği anlamına gelir. Lazer sensörler robotun çerçevesinden bir dönüm noktasına doğru menzili ve açıyı verir, böylece ölçüm modeli doğrusal değildir. Hakkında P, büyük olduğunu varsayarsanız, o zaman EKF tahminciniz başlangıçta zayıf olacaktır ve önceki durum vektörü mevcut olmadığından ölçümlere dayanacaktır. Bununla birlikte, robotunuz hareket etmeye ve algılamaya başladığında, EKF daha iyi olacak çünkü robotun pozunu tahmin etmek için hareket ve ölçüm modellerini kullanıyor. Robotunuz herhangi bir yer işareti algılamıyorsa, belirsizlik büyük ölçüde artar. Ayrıca, açı konusunda dikkatli olmanız gerekir. C ++ 'da,cos and sin, açı radyan olmalıdır. Kodunuzda bu sorunla ilgili hiçbir şey yok. Radyan cinsinden hesaplama yaparken açının gürültüsünü derece olarak kabul ediyorsanız, garip sonuçlar elde edebilirsiniz, çünkü radyandaki hesaplamalar büyük olarak kabul edilirken bir derece bir gürültü olarak.