Doğrusal ve açısal verilerin sensörlerden nasıl kaynaştırılacağı

Ekibim ve ben enkoderleri, ticari bir IMU ve GPS sensörü olan bir dış mekan robotu kuruyoruz . Robotun temel bir tank tahriki vardır, bu yüzden enkoderler sol ve sağ tekerleklerden yeterince keneler sağlar. IMU, x, y ve z'de yuvarlanma, eğim, yalpalama ve doğrusal ivme verir. Daha sonra fazlalık sağlayacak başka IMU'lar ekleyebiliriz ancak ek olarak açısal yuvarlanma, zift ve yalpalama oranları sağlayabiliriz. GPS global x, y ve z koordinatlarını yayınlar.

Robotun xy konumunu ve yönünü bilmek, robotun gezineceği ortamını yerelleştirmek ve haritalandırmak için yararlı olacaktır. Robotun hızı, yumuşak hareket kararları vermek için de faydalı olabilir. Zemine dayalı bir robot, bu yüzden z ekseni hakkında çok fazla umurumuzda değil. Robotun ayrıca bir lidar sensörü ve bir kamerası var - bu yüzden rulo ve perde, daha iyi bir yönlendirme için lidar ve kamera verilerini dönüştürmek için faydalı olacaktır.

Tüm bu sayıları bir araya getirip, tüm sensörlerin hassasiyetinden en iyi şekilde faydalanabilecek bir yol bulmaya çalışıyorum. Şu anda basit geçiş matrisiyle ilgili bir tahmin oluşturmak için bir Kalman filtresi kullanıyoruz [x, x-vel, x-accel, y, y-vel, y-accel]:

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

Filtre, yalnızca IMU tarafından sağlanan ivmelere dayanarak durumu tahmin eder. (IMU en iyi kalite değil; yaklaşık 30 saniye içinde robotun (istirahatta) ilk konumundan 20 metre uzakta sürüklendiğini gösterecektir.) Rulodan, perdeden ve yalpalamanın nasıl kullanılacağını bilmek istiyorum. IMU ve potansiyel olarak yuvarlanma, adım ve yalpalama oranları, tekerleklerden gelen kodlayıcı verileri ve durum tahminini iyileştirmek için GPS verileri.

Biraz matematik kullanarak, iki kodlayıcıyı, x, y ve robot üzerinde rota bilgileri, ayrıca doğrusal ve açısal hızlar üretmek için kullanabiliriz. Enkoderler çok hassastır, ancak dış alandaki kaymaya duyarlı olabilirler.

Bana öyle geliyor ki burada iki farklı veri kümesi var ve bunlar sigortalanması zor:

1. X, x-vel, x-accel, y, y-vel, y-accel tahminleri
2. Yuvarlanma, adım, yalpa ve yuvarlanma, adım ve yalpalama oranları

Bu iki kümenin arasında geçit olmasına rağmen, onları nasıl bir araya getireceğinizi düşünmekle ilgili sorun yaşıyorum. Örneğin, robot sabit bir hızda ilerliyorsa, x-vel ve y-vel tarafından belirlenen robot yönü, yaw ile aynı olacaktır. Her ne kadar, eğer robot hareketsizse, yalpa, x ve y hızları ile doğru bir şekilde belirlenemez. Ayrıca, kodlayıcılar tarafından sağlanan ve açısal hıza çevrilen veriler, yalpa oranının bir güncellemesi olabilir ... fakat yalpa oranındaki bir güncelleme nasıl daha iyi konumsal tahminler sağlayarak sona erebilir?

12 sayının tümünü aynı filtreye koymak mantıklı mı yoksa normalde ayrı mı tutuluyor? Bu tür bir sorunla baş etmek için zaten gelişmiş bir yol var mı?

İki şey.

1. Eğer haritalama yapmayı planlıyorsanız, tam teşekküllü bir Eşzamanlı Lokalizasyon ve Haritalama (SLAM) Algoritmasına ihtiyacınız var. Bakınız: Eşzamanlı Lokalizasyon ve Haritalama (SLAM): Kısım I Temel Algoritmalar . SLAM'da robot durumunu tahmin etmek sorunun sadece yarısı kadardır. Bunun nasıl yapılacağı, burada cevaplanabileceğinden daha büyük bir sorudur.

2. Yerelleştirme ile ilgili olarak (robotun durumunu tahmin etmek), bu bir Kalman Filtresi için bir iş değildir. Geçiş için bağlı bir doğrusal fonksiyonu değildir açısal ivme ve hızlar. Bu nedenle, bu görev için doğrusal olmayan tahmin edicileri düşünmeniz gerekir. Evet, bunu yapmanın standart yolları var. Evet, literatürde var. Evet, genellikle tüm girişler aynı filtreye konur. Robotun konumu, hızı, yönü ve açısal hızı çıkış olarak kullanılır. Ve Evet, burada ortak temalarına kısa bir giriş yapacağım. Ana kalkışlar$x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$

   1. Gyro ve IMU’nun önyargısını eyaletinize dahil edin, aksi halde tahminleriniz farklılık gösterecektir.
   2. Bu sorun için bir Genişletilmiş Kalman Filtresi (EKF) yaygın olarak kullanılır.
   3. Uygulamalar sıfırdan türetilebilir ve genellikle "bakılması" gerekmez.
   4. Yerelleştirme ve SLAM sorununun çoğu için uygulamalar var, bu yüzden gerekenden daha fazla iş yapmayın. Bakınız: Robot İşletim Sistemi ROS

Şimdi EKF'yi sisteminiz bağlamında açıklamak. Bir IMU + Gyro, GPS ve kilometre sayacımız var. Söz konusu robot, belirtildiği gibi diferansiyel bir tahriktir. Filtreleme görevi, robotunun geçerli poz tahminini, kontrol girişlerini ve her bir sensörden ölçümlerini almak ve bir sonraki adımda adımında tahmini üretmek içindir. . IMU ölçümlerini , GPS ve odometri, .${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Robot tahmin etmekle ilgilendiğimizi farz ediyorum . IMU ve Gyros'taki sorun sürükleniyor. EKF'de hesaba katmanız gereken ivmelerde durağan olmayan bir önyargı var. Bu (genellikle) önyargıyı tahmin edilen duruma getirerek yapılır. Bu, her zaman adımında önyargılı doğrudan tahmin etmenizi sağlar. , önyargı vektörü için .$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$$b$

Varsayıyorum:

1. $O_t$ = basamakların seyahat ettiği mesafeyi temsil eden iki küçük mesafe ölçümü
2. $I_t$ = üç yönelim ölçümü ve üç ivme ölçümü .${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. $G_t$ = robotun global karedeki konumu, .$^Gx_t,^Gy_t$

Tipik olarak, kontrol girişlerinin (her basamak için istenen hızlar) sonucunun çıkışlarla eşleştirilmesi zordur (robotun pozundaki değişiklik). Yerine , o (bkz yaygındır Thrun , Odometri Soru kontrolünün "sonucu" olarak Odometri kullanmak için). Bu varsayım, sürtünmesiz bir yüzeyde olmadığınızda iyi çalışır. IMU ve GPS, göreceğimiz gibi kaymanın düzeltilmesine yardımcı olabilir.$u$

Dolayısıyla ilk görev bir sonraki durumu geçerli durumdan tahmin etmektir: . Diferansiyel tahrikli bir robot söz konusu olduğunda, bu öngörü doğrudan literatürden elde edilebilir (bkz . Tekerlekli Mobil Robotların Kinematiği veya herhangi bir modern robotik ders kitabındaki daha kısa süreli tedavi) veya burada gösterildiği gibi sıfırdan elde edilebilir: Kilometre Sayacı Sorusu .$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

Dolayısıyla şimdi . Bu, yayılma veya tahmin aşamasıdır. Sen edebilirsiniz basitçe çoğaltarak bir robot çalışır. değerleri tamamen doğruysa, hiçbir zaman gerçek durumunuza tam olarak eşit olmayan bir tahminine sahip olmayacaksınız. Bu asla pratikte olmaz.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

Bu sadece önceki tahminden tahmin edilen bir değer verir ve tahminin doğruluğunun zamanla nasıl azaldığını bize söylemez. Bu yüzden, belirsizliği yaymak için EKF denklemlerini (belirsizliği Gauss gürültüsü varsayımları altında kapalı formda yayan) kullanmalısınız, bir parçacık filtresi (örnekleme tabanlı bir yaklaşım kullanan) belirsizliğin yakınlaştırılması) veya başka bir çok değişkenden biri.

EKF durumunda, aşağıdaki şekilde ilerliyoruz. robot halinin kovaryans matrisi olmasına izin verin . Lineer bir sistem elde etmek için Taylor serisini genişletme işlevini kullanarak işlevini doğrusallaştırır. Kalman Filtresi kullanılarak doğrusal bir sistem kolayca çözülebilir. Süresi en tahmin kovaryansını varsayalım olan , odometri gürültü kabul kovaryans matrisi olarak verilir (genellikle bir çapraz matris gibi ) . fonksiyonu durumunda , Jacobian ve$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}$, sonra belirsizliği şu şekilde yaymak:

Şimdi tahmin ve belirsizliği artırabiliriz. Belirsizliğin zaman içinde monoton olarak artacağını unutmayın. Bu bekleniyor. Bunu düzeltmek için tipik olarak yapılan şey, öngörülen durumu güncellemek için ve kullanmaktır . Sensörler robotun durumunun dolaylı bir ölçümünü sağladığından, buna filtreleme işleminin ölçüm adımı denir.$I_t$$G_t$

İlk olarak, robot durumunun bir kısmını GPS, IMU için bir ve fonksiyonu olarak tahmin etmek için her bir sensörü kullanın . Form kalıntısını veya yenilik öngörülen ve ölçülen değerlerin farktır. Ardından, her sensör için tahminin doğruluğunu, tüm sensörler için bir kovaryans matrisi şeklinde tahmin edin ( bu durumda , ). Son olarak, bir Jacobians bulmak aşağıdaki gibi ve devlet tahminini güncellemek:$h_g()$$h_i()$$R$$R_g$$R_i$$h$

Devlet tahmini olan her bir sensör için ( wikipedia girişini takip edin )$s$$z_s$

GPS durumunda, ölçümü , muhtemelen enlem ve boylamdan robotun yerel çerçevesine bir dönüşümdür, yani Jacobian neredeyse kazanacaktır. çoğu durumda doğrudan GPS ünitesi tarafından rapor edilir.H g R $z_g=h_g()$$H_g$$R_g$

IMU + Gyro durumunda, işlevi , ivmelerin bir entegrasyonu ve bir ek önyargı terimidir. IMU'yu kullanmanın bir yolu, istenen zamanda bir konum ve hız tahmini bulmak için ivmeleri sayısal olarak entegre etmektir. her hızlanma tahmini için küçük bir ek gürültü terimi varsa, konum tahmininin doğruluğunu bulmak için bu gürültüyü birleştirmeniz gerekir. O zaman kovaryansı tüm küçük ek gürültü terimlerinin . Önyargı için güncellemeyi dahil etmek daha zor ve uzmanlık alanım dışında. Ancak, düzlemsel hareketle ilgilendiğiniz için muhtemelen problemi basitleştirebilirsiniz. Bunun için literatüre bakmanız gerekecek.p i R i p $z_i=h_i()$$p_i$$R_i$$p_i$

Bazı kafamda olmayan referanslar:

1. EKF Tabanlı Görme-Atalet Kilometre Sayacının Doğruluğunu Artırma

2. Çok Robotlu Kooperatif Lokalizasyonu için Gözlenebilirlik Temelli Tutarlı EKF Tahmin Ediciler

3. INS-GPS için uyarlanabilir iki kademeli EKF, bilinmeyen hata önyargısına sahip gevşek bağlantılı sistem

Bu alan google’ın (bilgin) muhtemelen size çalışan bir uygulama bulabilmesi için yeterince olgun. Bu alanda çok çalışacaksanız, sağlam bir ders kitabı almanızı öneririm. Belki Google Araba Şöhreti S. Thrun tarafından Probablistic Robotik gibi bir şey . (Gece ​​geç saatlerde yapılan uygulamalar için faydalı bir referans buldum).

* Robot İşletim Sisteminde (ROS) PF tabanlı tahmin ediciler vardır . Ancak, bunlar iç mekan kullanımı için optimize edilmiştir. Parçacık filtreleri, harita tabanlı yerelleştirmeden kaynaklanabilecek çok modlu PDF'lerle ilgilidir (ben bu kapının veya o kapının yanında mıyım ). Dış mekan uygulamalarının çoğunun (özellikle GPS kullanabilenlerin, en azından aralıklı olarak) Genişletilmiş Kalman Filtresi'ni (EKF) kullandığına inanıyorum . Genişletilmiş Kalman Filtresini, diferansiyel tahrikli dış mekan, topraklama rotoru için başarıyla kullandım.

Çoğu durumda sorunu büyük ölçüde basitleştirebilirsiniz:

• Birçok "ticari sınıf" IMus (örneğin, Xsens) çok gürültülü ivmeölçerlere sahiptir. Hız elde etmek için onları kaynatma zahmetine bile girmeyin, kilometre sayacı zaten büyüklüklerin sırasıdır. IMU'nun sağlayacağı tek kullanılabilir veri adım ve adım ve bir dereceye kadar başlık (sonraki noktaya bakınız).
• IMU'lardan gitmek o kadar da güvenilir değil. Magetometreler kullanır ve duvarlarda bulabileceğiniz gibi, ferromanyetik kütlelerin yanında devasa (2 m'nin üzerinde 25 dereceye kadar) sürüklenir. Bunu çözmek için yaptığımız şey IMU başlığını kullanmak, ancak bir başlık yanlılığı tahmin etmektir.
• Dışarıdaysanız, 10 derecelik bir eğimde 10 metreye seyahat etmenin X ve Y'de aynı düzeye sahip bir arazide 10 metreye seyahat etmeyle aynı şekilde yol açmayacağını unutmayın. Bu genellikle Z'nin tahmin edilmesiyle açıklanır, ancak sanırım farklı bir şekilde tahmin edilebilir.
• GPS ayrıca, çok yollu ortamlarda tipik olarak yalancı bir kaltaktır. Ayrıca düşük dereceli (ve hatta bazı durumlarda yüksek dereceli) GPS'lerin çok yanlış standart sapmalar bildirme eğilimi vardır. Belirli bir GPS ölçümünün entegre edilip edilmeyeceğini kontrol etmek için bazı basit ki-kare testleri kullandık (yani mevcut filtre tahmininin belli bir noktaya kadar uyduğunu kontrol ettik), bu da bize iyi sonuçlar verdi.

Bizim için "tipik" çözüm, ego hareketi tahminini almak için odometri + IMU kullanmak ve ardından X, Y, Z ve yön sapmasını düzeltmek için GPS kullanmaktır.

İşte yaygın olarak kullandığımız bir EKF uygulaması. IMU’nun yönünü tahmin etmeniz gerekiyorsa (yani yerleşik bir filtreye sahip değilse), şu iki filtreden de yararlanabilirsiniz: UKF ve EKF .