Menghitung pitch, yaw, and roll dari data mag, acc, dan gyro


19

Saya memiliki papan Arduino dengan sensor kebebasan 9 derajat, dari mana saya harus menentukan pitch, yaw, dan roll board.

Berikut adalah contoh satu set data dari sensor 9-DOF:

Akselerometer (m / s)

  • AccX = -5,85
  • AccY = 1,46
  • AccZ = 17,98

Giroskop (RPM)

  • GyrX = 35,14
  • GyrY = -40,22
  • GyrZ = -9,86

Magnetometer (Gauss)

  • MagX = 0,18
  • MagY = -0,04
  • MagZ = -0,15

Bagaimana saya bisa menghitung pitch, yaw, and roll dari data ini?


1
Prinsip dasar: dari deteksi gravitasi di accelerometer Anda, Anda tahu jalan mana yang turun; dari deteksi medan magnet bumi di magnetometer Anda, Anda tahu arah mana yang berada di Utara. Berdasarkan ini dan dengan asumsi tidak ada akselerasi signifikan atau medan magnet yang kuat Anda dapat menentukan sikap Anda sendiri.
bangun

1
Data giroskop memberikan laju rotasi, tetapi bukan posisi absolut. Ini dapat diintegrasikan untuk memperkirakan perubahan dari sikap yang diketahui, tetapi ini biasanya berisik dan cenderung melayang jika tidak digunakan bersama dengan sensor lainnya.
bangun

1
lihat juga filter kalman, karena angka statis perlu diproses cukup banyak, untuk memberikan estimasi andalan roll-pitch dan yaw. Perhatikan juga, posisi sensor itu penting (Anda harus memperhitungkannya).
Gürkan Çetin

Jawaban:


15

Pitch, roll dan yaw didefinisikan sebagai rotasi di sekitar sumbu X, Y dan Z. Di bawah ini sebagai gambar untuk menggambarkan definisi.

Gulung Pitch dan Yaw

Dalam proyek sebelumnya saya menggunakan Accelerometer ADXL345 dari Perangkat Analog untuk menghitung roll and pitch. Di bawah ini adalah persamaan yang digunakan untuk menghitung roll and pitch. Saya telah membuat beberapa kode sumber tersedia untuk penggunaan umum.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Kode sumber lengkap dapat ditemukan di sini .

Berdasarkan definisi di atas

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Catatan: M_PI = 3.14159265358979323846 itu adalah konstan didefinisikan dalam math.h

Di bawah ini adalah beberapa referensi termasuk kode sumber dasar Arduino yang mungkin membantu Anda.


Referensi:


2
Jawaban yang bagus, perlu menambahkan bahwa posisi dan orientasi sensor dalam kendaraan akan menjadi penting, dan bahwa data harus diproses lebih lanjut, untuk memberikan hasil yang dapat diandalkan. (difilter atau digabungkan dengan data frekuensi rendah yang lebih andal, seperti GPS)
Gürkan Çetin

(@Zubair) "yaw = 180 * atan (accelerationZ / sqrt (accelerationXaccelerationX + accelerationZaccelerationZ)) / M_PI;" apa itu 'M_PI' ??
Wasabi

@Wasabi M_PI = 3.14159265358979323846. Ini konstan didefinisikan dalam perpustakaan math.h.
Mahendra Gunawardena

8

Jadi jawaban saya yang lebih lama di bawah ini mengasumsikan bahwa papan akan mengalami akselerasi dan selama waktu ini Anda masih harus dapat mengukur nada, putar, dan menguap dalam waktu singkat. Jika papan akan diam untuk semua pengukuran maka jawaban Mahendra Gunawardena akan bekerja dengan baik untuk Anda. Jika ini masuk ke perangkat seperti segway atau pesawat model atau multirotor atau apa pun yang bergerak, Anda mungkin ingin terus membaca. Posting ini adalah tentang bagaimana menggunakan ketiga sensor melalui metode yang disebut sensor fusion. Sensor fusion memungkinkan Anda untuk mendapatkan kekuatan dari setiap sensor dan meminimalkan efek dari kelemahan masing-masing sensor.

Karakteristik dan latar belakang sensor

Pertama-tama pahami bahwa accelerometer mengukur semua gaya yang diterapkan padanya, bukan hanya gaya gravitasi. Jadi di dunia yang sempurna dengan akselerometer dalam posisi diam tanpa getaran apa pun, Anda dapat dengan sempurna menentukan jalan mana yang menggunakan beberapa trigonometri dasar seperti ditunjukkan oleh jawaban Mahendra Gunawardena. Namun karena accelerometer akan menangkap semua kekuatan, getaran apa pun akan menghasilkan kebisingan. Perlu juga dicatat bahwa jika papan berakselerasi Anda tidak bisa hanya menggunakan trigonometri sederhana karena gaya yang dilaporkan accelerometer tidak hanya gaya gravitasi bumi tetapi juga gaya yang menyebabkan Anda berakselerasi.

Sebuah magnetometer lebih mudah daripada accelerometer. Gerakan tidak akan menyebabkan masalah dengan itu tetapi hal-hal seperti besi dan magnet lainnya pada akhirnya akan mempengaruhi hasil Anda. Jika sumber-sumber yang menyebabkan gangguan ini konstan, maka tidak sulit untuk ditangani, tetapi jika sumber-sumber ini tidak konstan akan menghasilkan banyak kebisingan yang sulit untuk dihilangkan.

Dari ketiga sensor tersebut, giroskop bisa dibilang yang paling andal dan mereka biasanya sangat sangat baik dalam mengukur kecepatan rotasi. Itu tidak terpengaruh oleh hal-hal seperti sumber besi dan akselerasi pada dasarnya tidak berdampak pada kemampuan mereka untuk mengukur kecepatan rotasi. Mereka melakukan pekerjaan yang sangat baik untuk melaporkan kecepatan di mana perangkat berputar, namun karena Anda mencari sudut absolut Anda harus mengintegrasikan kecepatan untuk mendapatkan posisi. Melakukan ini akan menambah kesalahan pengukuran terakhir ke kesalahan pengukuran baru karena integrasi pada dasarnya adalah jumlah nilai pada suatu rentang, bahkan jika kesalahan untuk satu pengukuran hanya 0,01 derajat per detik, dalam 100 pengukuran, posisi Anda bisa dimatikan dengan 1 derajat, dengan 1000 pengukuran, Anda bisa mati dengan 10 derajat. Jika Anda melakukan ratusan pengukuran per detik, Anda dapat melihat ini menyebabkan masalah. Ini biasa disebut gyro drift.

Sensor fusi

Sekarang keindahan dari semua sensor ini bekerja bersama adalah bahwa Anda dapat menggunakan informasi dari accelerometer dan magnetometer untuk membatalkan penyimpangan gyro. Ini akhirnya memungkinkan Anda untuk memberi Anda keakuratan dan kecepatan gyro tanpa cacat fatal gyro drift.

Menggabungkan data dari ketiga sensor ini dapat dilakukan dalam lebih dari satu cara, saya akan berbicara tentang menggunakan filter komplementer karena jauh lebih sederhana maka filter kalman dan filter kalman akan menghabiskan lebih banyak sumber daya pada sistem tertanam. Sering kali filter komplementer cukup bagus, lebih mudah diimplementasikan (dengan asumsi Anda tidak menggunakan pustaka yang dibuat sebelumnya) dan memungkinkan Anda memproses data lebih cepat.

Sekarang ke proses. Langkah pertama yang perlu Anda lakukan adalah mengintegrasikan output giroskop untuk mengubah kecepatan sudut menjadi posisi sudut. Anda juga kemungkinan besar harus menerapkan filter low pass pada accelerometer dan magnetometer untuk menangani kebisingan di output. Filter FIR sederhana seperti yang ditunjukkan di bawah ini berfungsi di sini. Dengan beberapa trigonometri Anda dapat menemukan pitch and roll dengan accelerometer dan yaw dengan magnetometer.

filteredData = (1-weight)*filteredData + weight*newData

Berat hanya konstan yang dapat disesuaikan tergantung pada seberapa banyak kebisingan Anda harus berurusan dengan, semakin tinggi kebisingan semakin kecil nilai bobotnya. Sekarang menggabungkan data dari sensor dapat dilakukan dengan baris kode berikut.

fusedData = (1-weight)*gyroData + weight*accelMagData

Perlu dicatat bahwa data adalah vektor dari pitch, roll, dan yaw. Anda bisa menggunakan tiga variabel untuk melakukan ini juga, bukan array jika Anda mau. Untuk perhitungan ini, gyro memberikan posisi dalam derajat dalam pitch, roll, dan yaw, magnetometer memberikan sudut untuk yaw, sementara accelerometer menyediakan angka sendiri untuk pitch dan roll.

Jika Anda masih menginginkan informasi lebih lanjut, Anda dapat menggunakan "sensor fusion with filter komplementer" dari google, ada banyak artikel tentang ini.


3

Dari data sensor akselerator, Anda hanya dapat menghitung pitch and roll. Dokumen di bawah ini dari Freescale menjelaskan dengan banyak informasi apa yang Anda butuhkan:

AN3461 - Sensasi Kemiringan Menggunakan Accelerometer Tiga Sumbu

Berdasarkan ucapan dokumen tersebut,

tanϕxyz=GpyGpz

tanθxyz=GpxGpysinϕ+Gpzcosϕ=GpxGpy2+Gpz2

yang setara dengan:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

Tentu saja, hasilnya adalah ini hanya ketika rotasi terjadi pada urutan tertentu (Rxyz):

  1. Gulung sumbu x demi sudutϕ
  2. Lapangan di sekitar sumbu y dengan sudutθ
  3. Menguap di sekitar sumbu z dengan sudutψ

Bergantung pada urutan rotasi, Anda mendapatkan persamaan yang berbeda. Untuk urutan rotasi , Anda tidak dapat menemukan sudut untuk Yaw di sekitar sumbu z. ψRxyzψ


1
Dia, selamat datang di Teknik SE! Situs ini mendukung Lateks, lihat betapa indahnya jawaban Anda sekarang. :-)
peterh
Dengan menggunakan situs kami, Anda mengakui telah membaca dan memahami Kebijakan Cookie dan Kebijakan Privasi kami.
Licensed under cc by-sa 3.0 with attribution required.