Cara memadukan data linear dan sudut dari sensor

Saya dan tim saya sedang menyiapkan robot luar ruang yang memiliki encoders, IMU kelas komersial , dan sensor GPS . Robot memiliki penggerak tangki dasar, sehingga pembuat enkode cukup memberikan kutu dari roda kiri dan kanan. IMU menghasilkan percepatan roll, pitch, yaw, dan linear dalam x, y, dan z. Kami kemudian dapat menambahkan IMU lain, yang akan memberikan redundansi, tetapi juga bisa memberikan tingkat sudut roll, pitch, dan yaw. GPS menerbitkan koordinat global x, y, dan z.

Mengetahui posisi dan pos robot akan berguna bagi robot untuk melokalkan dan memetakan lingkungan yang akan dinavigasi. Kecepatan robot juga bisa berguna untuk membuat keputusan pergerakan yang lancar. Ini adalah robot berbasis darat, jadi kami tidak terlalu peduli tentang sumbu z. Robot juga memiliki sensor lidar dan kamera - jadi roll and pitch akan berguna untuk mengubah data lidar dan kamera untuk orientasi yang lebih baik.

Saya mencoba mencari cara untuk menggabungkan semua angka-angka ini dengan cara yang secara optimal memanfaatkan keakuratan semua sensor. Saat ini kami sedang menggunakan filter Kalman untuk menghasilkan estimasi [x, x-vel, x-accel, y, y-vel, y-accel]dengan matriks transisi sederhana:

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

Filter memperkirakan keadaan secara eksklusif berdasarkan akselerasi yang disediakan oleh IMU. (IMU bukan kualitas terbaik; dalam waktu sekitar 30 detik akan menunjukkan robot (diam) melayang sejauh 20 meter dari lokasi awalnya.) Saya ingin tahu cara menggunakan roll, pitch, dan yaw dari IMU, dan berpotensi roll, pitch, dan yaw rate, data enkoder dari roda, dan data GPS untuk meningkatkan perkiraan keadaan.

Dengan menggunakan sedikit matematika, kita dapat menggunakan dua penyandi untuk menghasilkan x, y, dan informasi pos pada robot, serta kecepatan linier dan sudut. Penyandi sangat akurat, tetapi mereka rentan terhadap selip di bidang luar.

Menurut saya ada dua set data terpisah di sini, yang sulit untuk dipadukan:

1. Perkiraan x, x-vel, x-accel, y, y-vel, y-accel
2. Perkiraan roll, pitch, yaw, dan laju roll, pitch, dan yaw

Meskipun ada persilangan di antara dua set ini, saya mengalami kesulitan untuk berpikir tentang bagaimana menyatukannya. Misalnya, jika robot bergerak dengan kecepatan konstan, arah robot, ditentukan oleh x-vel dan y-vel, akan sama dengan yaw-nya. Meskipun, jika robot dalam keadaan diam, yaw tidak dapat ditentukan secara akurat oleh kecepatan x dan y. Selain itu, data yang disediakan oleh pembuat enkode, yang diterjemahkan ke kecepatan sudut, dapat menjadi pembaruan ke tingkat yaw ... tetapi bagaimana pembaruan ke tingkat yaw dapat memberikan estimasi posisi yang lebih baik?

Apakah masuk akal untuk menempatkan semua 12 angka ke dalam filter yang sama, atau apakah mereka biasanya disimpan terpisah? Apakah sudah ada cara yang dikembangkan dengan baik untuk menangani masalah jenis ini?

Dua hal.

1. Jika Anda berencana melakukan pemetaan, Anda perlu Algoritma Lokalisasi dan Pemetaan Simultan yang lengkap. Lihat: Lokalisasi dan Pemetaan Simultan (SLAM): Bagian I Algoritma Esensial . Dalam SLAM, memperkirakan status robot hanya setengah dari masalah. Bagaimana melakukannya adalah pertanyaan yang lebih besar daripada yang bisa dijawab di sini.

2. Mengenai lokalisasi (memperkirakan keadaan robot), ini bukan pekerjaan untuk Filter Kalman. Transisi dari ke x ( t $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$bukan fungsi linear karena percepatan dan kecepatan sudut. Karenanya Anda perlu mempertimbangkan penduga non-linear untuk tugas ini. Ya, ada cara standar untuk melakukan ini. Ya, mereka tersedia dalam literatur. Ya, biasanya semua input dimasukkan ke filter yang sama. Posisi, kecepatan, orientasi, dan kecepatan sudut robot digunakan sebagai output. Dan Ya, saya akan menyajikan pengantar singkat untuk tema umum mereka di sini. Kesimpulan utama adalah

   1. sertakan bias Gyro dan IMU di negara Anda atau perkiraan Anda akan berbeda
   2. Sebuah Filter Kalman Extended (EKF) umumnya digunakan untuk masalah ini
   3. Implementasi dapat diturunkan dari awal, dan umumnya tidak perlu "dilihat".
   4. Implementaitons ada untuk sebagian besar masalah lokalisasi dan SLAM, jadi jangan melakukan lebih banyak pekerjaan dari yang seharusnya. Lihat: Sistem Operasi Robot ROS

Sekarang, untuk menjelaskan EKF dalam konteks sistem Anda. Kami memiliki IMU + Gyro, GPS, dan odometri. Robot yang dimaksud adalah drive diferensial seperti yang disebutkan. Tugas penyaringan adalah untuk mengambil estimasi berpose saat robot x t , input kontrol u t , dan pengukuran dari masing-masing sensor, z t , dan menghasilkan perkiraan pada saat langkah berikutnya x t + 1 . Kami akan memanggil IMU pengukuran I t , GPS G t , dan odometry, O t .${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Saya berasumsi kita tertarik dalam memperkirakan pose robot sebagai . Masalah dengan IMU dan Gyros adalah drift. Ada bias non-stasioner dalam akselerasi yang harus Anda perhitungkan dalam EKF. Hal ini dilakukan (biasanya) dengan memasukkan bias ke dalam perkiraan keadaan. Ini memungkinkan Anda untuk secara langsung memperkirakan bias pada setiap langkah waktu. x t = x , y , ˙ x , ˙ y , θ , ˙ θ , $x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$, untuk vektor bias $b$ .

Saya berasumsi:

1. = dua pengukuran jarak mewakili jarak tapak telah melakukan perjalanan di beberapa selisih waktu kecil$O_t$
2. = tiga pengukuran orientasi α , β , θ , dan tiga pengukuran akselerasi ¨ x , ¨ y , ¨ z .$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. = posisi robot diduniabingkai, G x t , G y t .$G_t$$^Gx_t,^Gy_t$

Biasanya, hasil dari input kontrol (kecepatan yang diinginkan untuk setiap tapak) sulit untuk dipetakan ke output (perubahan pose robot). Sebagai ganti , itu biasa (lihat Thrun , Pertanyaan Odometry ) untuk menggunakan odometry sebagai "hasil" dari kontrol. Asumsi ini bekerja dengan baik ketika Anda tidak berada di permukaan tanpa gesekan. IMU dan GPS dapat membantu memperbaiki selip, seperti yang akan kita lihat.$u$

Jadi tugas pertama adalah untuk memprediksi negara berikutnya dari kondisi saat . Dalam kasus robot penggerak diferensial, prediksi ini dapat diperoleh langsung dari literatur (lihat Di Kinematika Robot Beroda Bergerak atau perlakuan yang lebih ringkas dalam buku teks robotika modern mana pun), atau diperoleh dari awal seperti ditunjukkan di sini:$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$ Pertanyaan Odometri .

Jadi, kita sekarang dapat memprediksi x t + 1 = f ( x t , O t ) . Ini adalah langkah propagasi atau prediksi. Anda dapat mengoperasikan robot hanya dengan menyebarkan. Jika nilai-nilai O t benar-benar akurat, Anda tidak akan pernah memiliki perkiraan x yang tidak tepat sama keadaan sebenarnya Anda. Ini tidak pernah terjadi dalam praktek.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

Ini hanya memberikan nilai prediksi dari estimasi sebelumnya, dan tidak memberi tahu kami bagaimana keakuratan estimasi menurun seiring waktu. Jadi, untuk menyebarkan ketidakpastian, Anda harus menggunakan persamaan EKF (yang menyebarkan ketidakpastian dalam bentuk tertutup berdasarkan asumsi kebisingan Gaussian), filter partikel (yang menggunakan pendekatan berbasis pengambilan sampel) *, UKF (yang menggunakan titik-bijaksana) perkiraan ketidakpastian), atau salah satu dari banyak varian lainnya.

Dalam hal EKF, kami melanjutkan sebagai berikut. Mari menjadi matriks kovarians dari negara robot. Kami linearkan fungsi f menggunakan ekspansi Taylor-series untuk mendapatkan sistem linier. Sistem linier dapat dengan mudah dipecahkan menggunakan Filter Kalman. Asumsikan kovarians dari estimasi pada waktu t adalah P t , dan kovarians diasumsikan dari kebisingan di odometry diberikan sebagai matriks U t (biasanya diagonal 2 × 2 matriks, seperti 0,1 × I 2 × 2 ). Dalam kasus fungsi f , kita mendapatkan Jacobian$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ danFu=∂$F_x=\frac{\partial f}{\partial x}$ , lalu sebarluaskan ketidakpastian itu sebagai,$F_u=\frac{\partial f}{\partial u}$

Sekarang kita bisa menyebarkan estimasi dan ketidakpastian. Perhatikan bahwa ketidakpastian akan meningkat seiring waktu. Ini diharapkan. Untuk memperbaiki ini, apa yang biasanya dilakukan, adalah dengan menggunakan dan G t untuk memperbarui negara diprediksi. Ini disebut langkah pengukuran dari proses penyaringan, karena sensor memberikan pengukuran tidak langsung dari kondisi robot.$I_t$$G_t$

$h_g()$$h_i()$$R$$R_g$$R_i$$h$

$s$$z_s$

In the case of GPS, the measurement $z_g=h_g()$ it is probably just a transformation from latitude and longitude to the local frame of the robot, so the Jacobian $H_g$ will be nearly Identity. $R_g$ is reported directly by the GPS unit in most cases.

In the case of the IMU+Gyro, the function $z_i=h_i()$ is an integration of accelerations, and an additive bias term. One way to handle the IMU is to numerically integrate the accelerations to find a position and velocity estimate at the desired time. If your IMU has a small additive noise term $p_i$ for each acceleration estimate, then you must integrate this noise to find the accuracy of the position estimate. Then the covariance $R_i$ is the integration of all the small additive noise terms, $p_i$. Incorporating the update for the bias is more difficult, and out of my expertise. However, since you are interested in planar motion, you can probably simplify the problem. You'll have to look in literature for this.

Some off-the-top-of-my-head references:

1. Improving the Accuracy of EKF-Based Visual-Inertial Odometry

2. Observability-based Consistent EKF Estimators for Multi-robot Cooperative Localization

3. Adaptive two-stage EKF for INS-GPS loosely coupled system with unknown fault bias

This field is mature enough that google (scholar) could probably find you a working implementation. If you are going to be doing a lot of work in this area, I recommend you pick up a solid textbook. Maybe something like Probablistic Robotics by S. Thrun of Google Car fame. (I've found it a useful reference for those late-night implementations).

*There are several PF-based estimators available in the Robot Operating System (ROS). However, these have been optimized for indoor use. Particle filters deal with the multi-modal PDFs which can result from map-based localization (am I near this door or that door). I believe most outdoor implementations (especially those that can use GPS, at least intermittently) use the Extended Kalman Filter (EKF). I've successfully used the Extended Kalman Filter for an outdoor, ground rover with differential drive.

You can greatly simplify the problem in most common cases:

• A lot of "commercial grade" IMus (e.g. Xsens) have very noisy accelerometers. Don't even bother fusing them to get speed, the odometry is already order of magnitudes better. The only usable data the IMU is going to provide is the pitch and roll, and to some extent the heading (see next point)
• heading from IMUs is not that trustworthy. It uses magetometers, and will show huge drifts (up to 25 degrees over 2m in our case) near ferromagnetic masses, such as the one you can find in building walls. What we did to solve this is to use the IMU heading, but estimate a heading bias.
• If you are outdoors, don't forget that travelling 10m on a 10 degree incline does not lead to the same change in X and Y than travelling 10m on a flat terrain. This is usually accounted for by estimating Z, but I guess it can be estimated differently.
• GPS is also a lying bitch, typically in high-multipath environments. Plus low-grade (and even in some conditions high-grade) GPSes have a tendency to report very wrong standard deviations. We used some simple chi-square tests to check whether a particular GPS measurement should be integrated (i.e. checking that it matches the current filter estimate up to a certain point), which gave us decent results.

The "typical" solution for us is to use odometry + IMU to get an ego-motion estimate and then use GPS to correct X,Y,Z and heading bias.

Here is an EKF implementation that we extensively used. If you need to estimate the IMU's orientation (i.e. if it does not already have a built-in filter), you can also use on of these two filter: UKF and EKF.