Saya akan mencoba mencoba untuk menutupi beberapa topik dasar namun penting dalam artikel ini :

- Apa tindakan accelerometer

- Apa giroskop ( giro alias ) mengukur
- Bagaimana mengkonversi analog - ke-digital ( ADC ) pembacaan yang Anda dapatkan dari sensor ini untuk unit fisik ( yang akan g
untuk accelerometer , deg / s untuk giroskop )
- Bagaimana menggabungkan accelerometer dan giroskop bacaan untuk memperoleh informasi yang akurat tentang
kecenderungan relatif perangkat Anda ke tanah pesawat
Sepanjang artikel saya akan mencoba untuk menjaga matematika untuk minimum . Jika Anda tahu apa Sine / kosinus / Tangent
kemudian Anda harus dapat memahami dan menggunakan ide-ide dalam proyek Anda tidak peduli apa platform Anda
menggunakan Arduino , Propeller , Basic Stamp , chip Atmel , Microchip PIC , dll Ada orang di luar sana yang percaya bahwa Anda
perlu matematika kompleks untuk memanfaatkan unit IMU ( kompleks FIR atau IIR filter seperti Kalman filter , filter Taman McClellan , dll) . Anda dapat penelitian semua orang dan mencapai hasil yang indah tapi kompleks . Cara saya menjelaskan hal
memerlukan matematika hanya dasar . Saya sangat percaya dalam kesederhanaan . Saya pikir sistem yang sederhana lebih
mudah untuk mengontrol dan memantau , selain banyak perangkat embedded tidak memiliki kekuasaan dan sumber daya untuk
menerapkan algoritma kompleks yang membutuhkan perhitungan matriks .
Saya akan menggunakan contoh sebuah unit baru IMU yang saya rancang - Accelerometer Acc_Gyro + Gyro IMU . Kami akan
menggunakan parameter dari perangkat ini dalam contoh-contoh di bawah ini . Unit ini merupakan perangkat yang baik untuk
memulai dengan karena terdiri dari 3 perangkat :
- LIS331AL (lembar ) - analog 3 - axis accelerometer 2G
- LPR550AL ( datasheet ) - dual -sumbu ( pitch and Roll ) , 500deg/second giroskop
- LY550ALH ( datasheet ) - sumbu tunggal ( Yaw ) giroskop ( perangkat terakhir ini tidak digunakan dalam tutorial ini tetapi menjadi
relevan ketika Anda pindah ke implementasi DCM Matrix )
Bersama-sama mereka mewakili 6 - Derajat Kebebasan Inertial Measurement Unit . Nah, itu nama mewah ! Namun , di balik nama
mewah adalah perangkat kombinasi yang sangat berguna yang kita akan membahas dan menjelaskan secara rinci di bawah .
Bagian 1 . accelerometer

Untuk memahami unit ini kita akan mulai dengan accelerometer . Ketika berpikir tentang accelerometers sering berguna untuk
gambar kotak dalam bentuk kubus dengan bola di dalamnya . Anda mungkin membayangkan sesuatu yang lain seperti kue atau
donat , tapi aku akan membayangkan bola :

Jika kita mengambil kotak ini di tempat tanpa gravitasi bidang atau dalam hal ini dengan
tidak ada bidang lain yang mungkin mempengaruhi posisi bola - bola hanya akan
mengambang di tengah kotak. Anda bisa membayangkan kotak adalah di luar angkasa
jauh-jauh dari setiap badan kosmik, atau jika tempat seperti itu sulit untuk menemukan
membayangkan setidaknya pesawat ruang angkasa yang mengorbit di sekitar planet di
mana segala sesuatu dalam keadaan tanpa bobot. Dari gambar di atas Anda dapat
melihat bahwa kita tetapkan untuk setiap sumbu sepasang dinding (kami dihapus Y dinding + sehingga kita dapat melihat ke dalam
kotak). Bayangkan bahwa setiap dinding sensitif tekanan. Jika tiba-tiba kita bergerak kotak di sebelah kiri (kita mempercepatnya
dengan akselerasi 1g = 9.8m / s ^ 2), bola akan memukul dinding X-. Kami kemudian mengukur kekuatan tekanan bahwa bola
berlaku untuk dinding dan output nilai-1g pada sumbu X.

Dalam hal ini kotak tidak bergerak tapi kami masih mendapatkan pembacaan-1g pada sumbu Z. Tekanan bahwa bola telah
diterapkan di dinding itu disebabkan oleh gaya gravitasi. Secara teori itu bisa menjadi berbagai jenis kekuatan - misalnya, jika Anda
membayangkan bahwa bola kita adalah logam, menempatkan magnet sebelah kotak bisa bergerak bola sehingga hits dinding lain.
Hal ini dikatakan hanya untuk membuktikan bahwa dalam langkah-langkah accelerometer dasarnya memaksa tidak percepatan. Itu
hanya terjadi percepatan yang menyebabkan gaya inersia yang ditangkap oleh mekanisme deteksi kekuatan accelerometer.
Meskipun model ini tidak persis bagaimana sensor MEMS dibangun seringkali berguna dalam memecahkan masalah terkait
accelerometer. Ada sensor sebenarnya mirip yang memiliki bola logam di dalam, mereka disebut switch tilt, namun mereka lebih
primitif dan biasanya mereka hanya bisa mengatakan jika perangkat cenderung dalam beberapa jangkauan atau tidak, bukan
tingkat kemiringan.
Sejauh ini kami telah menganalisis output accelerometer pada sumbu tunggal dan ini adalah semua yang Anda akan mendapatkan
dengan accelerometers sumbu tunggal. Nilai riil accelerometers triaksial berasal dari fakta bahwa mereka dapat mendeteksi gaya
inersia pada semua tiga sumbu. Mari kita kembali ke model kotak kami, dan mari kita memutar kotak 45 derajat ke kanan. Bola
akan menyentuh dinding 2 sekarang: Z-dan X-seperti yang ditunjukkan pada gambar di bawah

:
Nilai-nilai 0,71 yang tidak sembarangan, mereka adalah benar-benar sebuah pendekatan untuk sqrt (1/2). Ini akan menjadi lebih
jelas saat kita memperkenalkan model berikutnya kami untuk accelerometer.
Dalam model sebelumnya kita telah tetap gaya gravitasi dan diputar kotak imajiner kita. Dalam 2 contoh terakhir kami telah
menganalisis output dalam 2 posisi kotak yang berbeda, sedangkan vektor gaya tetap konstan. Sementara ini berguna dalam
memahami bagaimana accelerometer berinteraksi dengan kekuatan luar, itu lebih praktis untuk melakukan perhitungan jika kita
memperbaiki sistem koordinat dengan sumbu accelerometer dan membayangkan bahwa vektor gaya berputar di sekitar kita.

Silakan lihat pada model di atas, saya diawetkan warna sumbu sehingga Anda dapat membuat transisi mental dari model
sebelumnya ke yang baru. Bayangkan bahwa setiap sumbu dalam model baru tegak lurus ke wajah masing-masing kotak pada
model sebelumnya. Vektor R adalah vektor gaya yang accelerometer adalah mengukur (bisa berupa kekuatan gravitasi atau gaya
inersia dari contoh di atas atau kombinasi keduanya). Rx, Ry, Rz adalah proyeksi dari vektor R pada X, Y, Z sumbu. Mohon
perhatikan hubungan berikut:

R ^ 2 = Rx ^ 2 + Ry ^ 2 + Rz ^ 2 (Persamaan 1)which is basically the equivalent of the Pythagorean theorem in 3D.

Ingatlah bahwa sedikit lebih awal saya katakan bahwa nilai-nilai sqrt ( 1/2 ) ~ 0,71 tidak acak . Jika Anda steker mereka dalam
rumus di atas , setelah mengingat bahwa gaya gravitasi kami adalah 1 g kita dapat memverifikasi bahwa :
1 ^ 2 = ( - sqrt ( 1/2 ) ) ^ 2 + 0 ^ 2 + ( sqrt - ( 1/2 ) ) ^ 2
hanya dengan mengganti R = 1 , Rx = sqrt - ( 1/2 ) , Ry = 0 , Rz = - sqrt ( 1/2 ) dalam Eq.1
Setelah basa-basi panjang teori kita semakin dekat dengan accelerometers kehidupan nyata . Nilai-nilai Rx , Ry , Rz sebenarnya
berhubungan linier terhadap nilai-nilai yang accelerometer kehidupan nyata Anda akan output dan yang bisa Anda gunakan untuk
melakukan berbagai perhitungan .
Sebelum kita sampai di sana mari kita bicara sedikit tentang cara accelerometers akan memberikan informasi ini kepada kami .
Kebanyakan accelerometers akan jatuh dalam dua kategori : digital dan analog . Accelerometers digital akan memberikan informasi
menggunakan protokol serial seperti I2C , SPI atau USART , sementara accelerometers analog akan menampilkan tingkat
tegangan dalam kisaran standar yang Anda harus mengkonversi ke nilai digital menggunakan ADC ( analog ke digital ) modul . Aku
tidak akan pergi ke banyak detail tentang bagaimana bekerja ADC , sebagian karena itu adalah topik yang luas dan sebagian
karena berbeda dari satu platform yang lain . Beberapa mikrokontroler akan memiliki built -in modul ADC beberapa dari mereka
akan membutuhkan komponen eksternal untuk melakukan konversi ADC . Tidak peduli apa jenis modul ADC yang Anda gunakan
Anda akan berakhir dengan nilai dalam rentang tertentu . Misalnya ADC Modul 10 - bit akan menampilkan nilai dalam kisaran 0 ..
1023, diketahui bahwa 1023 = 2 ^ 10 1. Sebuah ADC modul 12 -bit akan menampilkan nilai di kisaran 0 .. 4095 , diketahui bahwa
4095 = 2 ^ 12-1 .
Mari kita lanjutkan dengan mempertimbangkan sebuah contoh sederhana , misalkan kita 10bit ADC modul memberi kami nilai
sebagai berikut untuk tiga saluran accelerometer ( sumbu ) :
AdcRx = 586
AdcRy = 630
AdcRz = 561
Setiap modul ADC akan memiliki tegangan referensi , mari kita asumsikan dalam contoh kita adalah 3.3V . Untuk mengkonversi
nilai ADC 10bit pada tegangan kami menggunakan rumus berikut :
VoltsRx = AdcRx * Vref / 1023
Sebuah catatan cepat di sini yaitu bahwa untuk ADC 8bit pembagi terakhir akan 255 = 8 2 ^ 1, dan untuk 12bit ADC pembagi
terakhir akan 4095 = 12 2 ^ 1.
Menerapkan rumus ini untuk semua 3 saluran kita mendapatkan :
VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V ( kami akan mengumpulkan semua hasil 2 poin desimal )
VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V
VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V
Tiap accelerometer memiliki tingkat tegangan nol - g , Anda dapat menemukannya dalam spesifikasi , hal ini adalah tegangan yang
sesuai dengan 0g . Untuk mendapatkan nilai tegangan ditandatangani kita perlu menghitung pergeseran dari tingkat ini . Katakanlah
tingkat tegangan 0g kami VzeroG = 1.65V . Kami menghitung pergeseran tegangan dari tegangan nol - g sebagai berikut ::
DeltaVoltsRx = 1.89V - 1.65V = 0.24V
DeltaVoltsRy = 2.03V - 1.65V = 0.38V
DeltaVoltsRz = 1.81V - 1.65V = 0.16V
Kami sekarang memiliki pembacaan accelerometer kami dalam Volt , masih tidak dalam g ( 9,8 m / s ^ 2 ) , untuk melakukan
konversi akhir kita menerapkan sensitivitas accelerometer , biasanya dinyatakan dalam mV / g . Katakanlah Sensitivitas kita =
478.5mV / g = 0.4785V / g . Nilai sensitivitas dapat ditemukan dalam spesifikasi accelerometer . Untuk mendapatkan nilai kekuatan
akhir dinyatakan dalam g kita gunakan rumus berikut :
Rx = DeltaVoltsRx / Sensitivitas
Rx = 0.24V / 0.4785V / g = ~ 0.5g

Ry = 0.38V / 0.4785V / g = ~ 0.79g

Rz = 0.16V / 0.4785V / g = ~ 0.33g
Kita tentu saja dapat menggabungkan semua langkah dalam satu rumus, tetapi saya melalui semua langkah-langkah untuk
membuat jelas bagaimana Anda pergi dari pembacaan ADC ke komponen vektor gaya dinyatakan dalam g .
Rx = ( AdcRx * Vref / 1023 - VzeroG ) / Sensitivitas ( Eq.2 )
Ry = ( AdcRy * Vref / 1023 - VzeroG ) / Sensitivitas
Rz = ( AdcRz * Vref / 1023 - VzeroG ) / Sensitivitas
Kami sekarang memiliki semua 3 komponen yang mendefinisikan vektor gaya inersia kami , jika perangkat tidak tunduk pada
kekuatan lain selain gravitasi , kita bisa berasumsi ini adalah arah kami gravitasi vektor gaya . Jika anda ingin menghitung
kecenderungan perangkat relatif terhadap tanah Anda bisa menghitung sudut antara vektor ini dan Z sumbu . Jika Anda juga tertarik
pada arah per - sumbu kecenderungan Anda dapat membagi hasil ini menjadi 2 komponen : kecenderungan pada sumbu X dan Y
yang bisa dihitung sebagai sudut antara gravitasi vektor dan X / Y sumbu . Menghitung sudut ini lebih sederhana dibandingkan
Anda mungkin berpikir , sekarang kita telah menghitung nilai untuk Rx , Ry dan Rz . Mari kita kembali ke model accelerometer
terakhir kami dan melakukan beberapa tambahan notasi :

Sudut yang kita tertarik adalah sudut antara X, Y, Z sumbu dan kekuatan vektor R. Kita akan mendefinisikan sudut ini sebagai AXR,
Ayr, AZR. Anda dapat melihat dari segitiga sudut kanan dibentuk oleh R dan Rx bahwa:
cos (AXR) = Rx / R, dan juga:
cos (Ayr) = Ry / R
cos (AZR) = Rz / R
Kita bisa memotong dari Eq.1 bahwa R = sqrt (Rx ^ 2 + Ry ^ 2 + Rz ^ 2).
Kita dapat menemukan sudut sekarang kami dengan menggunakan arccos () function (cos terbalik () function):
AXR = arccos (Rx / R)
Ayr = arccos (Ry / R)
AZR = arccos (Rz / R)
Kami telah pergi jauh untuk menjelaskan model accelerometer, hanya untuk datang ke formula ini. Tergantung pada aplikasi Anda,
Anda mungkin ingin menggunakan formula menengah yang kami telah diturunkan. Kami juga akan memperkenalkan model
giroskop segera, dan kita akan melihat bagaimana accelerometer dan giroskop data dapat dikombinasikan untuk memberikan
perkiraan kemiringan lebih akurat.
Tapi sebelum kita melakukan itu mari kita melakukan beberapa notasi yang lebih berguna:cosX = cos(Axr) = Rx / R
cosY = cos(Ayr) = Ry / R
cosZ = cos(Azr) = Rz / R

Triplet ini sering disebut Direction Cosine, dan itu pada dasarnya merupakan unit vektor (vektor dengan panjang 1) yang memiliki
arah yang sama dengan vektor R kami. Anda dapat dengan mudah memverifikasi bahwa:
Sqrt (cos ^ 2 + Cosy ^ 2 + cosZ ^ 2) = 1
Ini adalah properti yang bagus karena membebaskan kita dari pemantauan modulus (panjang) dari R vektor. Sering kali jika kita
hanya tertarik pada arah vektor inersia kami, masuk akal untuk menormalkan modulus itu untuk menyederhanakan perhitungan
lainnya.

Bagian 2. giroskop
Kami tidak akan memperkenalkan apapun model kotak setara untuk giroskop seperti yang kami lakukan untuk accelerometer,
sebaliknya kita akan melompat langsung ke model accelerometer kedua dan kami akan menunjukkan apa ukuran giroskop menurut
model ini.

Setiap saluran giroskop mengukur rotasi di sekitar salah satu sumbu . Misalnya giroskop 2 - sumbu akan mengukur rotasi di sekitar
( atau beberapa orang mungkin mengatakan "tentang " ) X dan Y sumbu . Untuk mengungkapkan rotasi ini dalam jumlah mari kita
melakukan beberapa notasi . Pertama mari kita mendefinisikan :
Rxz - adalah proyeksi dari vektor gaya inersia R pada bidang XZ
Ryz - adalah proyeksi dari vektor gaya inersia R di pesawat YZ
Dari segitiga sudut kanan dibentuk oleh rxz dan Rz , menggunakan teorema Pythagoras kita mendapatkan :
Rxz ^ 2 = Rx ^ 2 + Rz ^ 2 , dan juga :
Ryz ^ 2 = Ry ^ 2 + Rz ^ 2
juga mencatat bahwa :
R ^ 2 = rxz ^ 2 + Ry ^ 2 , ini bisa berasal dari Eq.1 dan persamaan di atas , atau dapat berasal dari segitiga sudut kanan dibentuk
oleh R dan Ryz
R ^ 2 = Ryz ^ 2 + Rx ^ 2
Kami tidak akan menggunakan formula ini dalam artikel ini tetapi berguna untuk mencatat hubungan antara semua nilai dalam
model kami .
Sebaliknya kita akan menentukan sudut antara sumbu Z dan rxz , vektor Ryz sebagai berikut :
Axz - adalah sudut antara rxz ( proyeksi R pada XZ pesawat) dan Z sumbu
AYZ - adalah sudut antara Ryz ( proyeksi R pada YZ pesawat) dan Z sumbu
Sekarang kita semakin dekat dengan apa langkah-langkah giroskop . Giroskop mengukur tingkat perubahan sudut didefinisikan di
atas . Dengan kata lain itu akan menampilkan nilai yang berhubungan linier terhadap laju perubahan sudut-sudut ini . Untuk
menjelaskan ini mari kita asumsikan bahwa kita telah mengukur sudut rotasi sekitar sumbu Y ( yang akan sudut Axz ) pada waktu t0
, dan kita mendefinisikannya sebagai Axz0 , selanjutnya kita mengukur sudut ini pada waktu t1 kemudian dan itu Axz1 . Laju
perubahan akan dihitung sebagai berikut :

RateAxz = ( Axz1 - Axz0 ) / ( t1 - t0 ) .

Jika kita mengekspresikan Axz dalam derajat, dan waktu dalam detik , maka nilai ini akan dinyatakan dalam deg / s . Inilah langkahlangkah giroskop .
Dalam prakteknya giroskop ( kecuali itu adalah digital giroskop khusus) jarang akan memberikan nilai yang dinyatakan dalam deg /
s . Sama seperti untuk accelerometer Anda akan mendapatkan nilai ADC bahwa Anda akan perlu mengkonversi ke deg / s
menggunakan rumus yang mirip dengan Persamaan. 2 bahwa kita telah ditetapkan untuk accelerometer . Mari kita
memperkenalkan ADC untuk deg / s rumus konversi untuk giroskop ( kita asumsikan kita menggunakan modul ADC 10bit , untuk
8bit ADC ganti 1023 dengan 255 , untuk 12bit ADC ganti 1023 dengan 4095 ) .
RateAxz = ( AdcGyroXZ * Vref / 1023 - VzeroRate ) / Sensitivitas Eq.3
RateAyz = ( AdcGyroYZ * Vref / 1023 - VzeroRate ) / Sensitivitas
AdcGyroXZ , AdcGyroYZ - diperoleh dari modul ADC kami dan mereka mewakili saluran yang mengukur rotasi proyeksi vektor R di
XZ masing-masing dalam pesawat YZ , yang setara dengan mengatakan rotasi dilakukan sekitar Y dan X sumbu masing-masing.
Vref - adalah tegangan referensi ADC kita akan menggunakan 3.3V dalam contoh di bawah ini
VzeroRate - adalah nol - tingkat tegangan , dengan kata lain tegangan yang giroskop output jika tidak tunduk pada rotasi , untuk
papan Acc_Gyro itu misalnya 1.23V ( Anda dapat menemukan nilai-nilai ini dalam spesifikasi - tapi don ' t kepercayaan spesifikasi
yang paling gyros akan menderita sedikit offset setelah disolder jadi ukuran VzeroRate untuk setiap output sumbu menggunakan
voltmeter , biasanya nilai ini tidak akan berubah dari waktu ke waktu setelah gyro itu disolder , jika variates - menulis rutin kalibrasi
untuk mengukur sebelum perangkat start-up , pengguna harus diinstruksikan untuk menjaga perangkat dalam posisi masih pada
start -up untuk gyros untuk mengkalibrasi ) .
Sensitivitas - adalah sensitivitas giroskop Anda itu dinyatakan dalam mV / ( deg / s ) sering ditulis sebagai mV / deg / s , pada
dasarnya memberitahu Anda berapa banyak mV akan peningkatan keluaran giroskop , jika Anda meningkatkan kecepatan putaran
per satu deg / s . Sensitivitas Acc_Gyro papan misalnya 2mV/deg/s atau 0.002V/deg/s
Mari kita mengambil contoh, misalkan modul ADC kami kembali nilai-nilai berikut :
AdcGyroXZ = 571
AdcGyroXZ = 323
Dengan menggunakan rumus di atas, dan menggunakan spesifikasi parameter Acc_Gyro papan kami akan mendapatkan :
RateAxz = ( 571 * 3.3V / 1023 - 1.23V ) / ( 0.002V/deg/s ) = ~ 306 deg / s
RateAyz = ( 323 * 3.3V / 1023 - 1.23V ) / ( 0.002V/deg/s ) = ~ -94 deg / s
Dengan kata lain perangkat berputar di sekitar sumbu Y ( atau kita dapat mengatakan itu berputar di XZ pesawat) dengan
kecepatan 306 deg / s dan di sekitar sumbu X ( atau kita dapat mengatakan itu berputar di YZ pesawat) dengan kecepatan - 94 deg
/ s . Harap dicatat bahwa tanda negatif berarti bahwa perangkat berputar dalam arah yang berlawanan dari arah positif
konvensional . Dengan konvensi satu arah rotasi adalah positif . Sebuah giroskop baik lembar spesifikasi akan menunjukkan arah
mana yang positif , jika tidak, anda harus menemukannya dengan bereksperimen dengan perangkat dan mencatat arah mana hasil
rotasi dalam meningkatkan tegangan pada output pin . Hal ini sebaiknya dilakukan dengan menggunakan osiloskop karena segera
setelah Anda menghentikan rotasi tegangan akan turun kembali ke level nol - tingkat . Jika anda menggunakan multimeter Anda
harus mempertahankan tingkat rotasi konstan untuk setidaknya beberapa detik dan perhatikan tegangan selama rotasi ini ,
kemudian membandingkannya dengan nol - tingkat tegangan . Jika lebih besar dari nol - tingkat tegangan itu berarti bahwa arah
rotasi positif .
Bagian 3 . Menyatukan semuanya . Menggabungkan accelerometer dan data giroskop .Jika Anda membaca artikel ini Anda
mungkin diperoleh atau berencana untuk memperoleh perangkat IMU , atau mungkin Anda berencana untuk membangun satu dari
accelerometer terpisah dan perangkat giroskop .
CATATAN : UNTUK IMPLEMENTASI PRAKTIS DAN PENGUJIAN ALGORITMA INI BACALAH ARTIKEL INI
:http://starlino.com/imu_kalman_arduino.html
Langkah pertama dalam menggunakan perangkat IMU kombinasi yang menggabungkan accelerometer dan giroskop adalah untuk
menyelaraskan sistem koordinat mereka. Cara termudah untuk melakukannya adalah untuk memilih sistem koordinat
accelerometer sebagai sistem koordinat referensi Anda . Kebanyakan lembar data accelerometer akan menampilkan arah X , Y , Z
sumbu relatif terhadap citra chip fisik atau perangkat . Sebagai contoh di sini adalah arah X , Y , Z sumbu seperti yang ditunjukkan
dalam spesifikasi untuk papan Acc_Gyro :

Langkah berikutnya adalah:

- Mengidentifikasi output giroskop yang sesuai dengan RateAxz , nilai RateAyz dibahas di atas .
- Menentukan apakah output ini perlu dibalik karena posisi fisik giroskop relatif terhadap accelerometer
Jangan berasumsi bahwa jika giroskop memiliki output ditandai X atau Y , maka akan sesuai dengan setiap poros dalam sistem
koordinat accelerometer , bahkan jika output ini merupakan bagian dari unit IMU . Cara terbaik adalah untuk menguji itu.
Berikut adalah urutan sampel untuk menentukan output giroskop sesuai dengan nilai RateAxz dibahas di atas .
- Mulai dari menempatkan perangkat dalam posisi horisontal . Kedua X dan Y output dari accelerometer akan output tegangan nol g ( misalnya untuk Acc_Gyro forum ini adalah 1.65V )
- Awal berikutnya memutar perangkat sekitar sumbu Y , cara lain untuk mengatakan ini adalah bahwa Anda memutar perangkat di
XZ pesawat, sehingga X dan Z accelerometer output perubahan dan output Y tetap konstan .
- Sambil memutar perangkat pada catatan laju konstan yang giroskop perubahan output, output giroskop lainnya harus tetap
konstan
- Output giroskop yang berubah selama rotasi sekitar sumbu Y ( rotasi XZ pesawat) akan memberikan nilai masukan untuk
AdcGyroXZ , dari mana kita menghitung RateAxz
- Langkah terakhir adalah untuk memastikan arah putaran sesuai dengan model kami , dalam beberapa kasus, Anda mungkin harus
membalikkan RateAxz nilai karena posisi fisik giroskop relatif terhadap accelerometer
- Tampil lagi pengujian di atas , berputar perangkat ke sumbu Y. , kali ini memonitor output X accelerometer ( AdcRx dalam model
kami) . Jika AdcRx tumbuh ( pertama dari 90 derajat rotasi dari posisi horisontal ) , maka AdcGyroXZ harus menurun . Hal ini
disebabkan fakta bahwa kami sedang memantau vektor gravitasi dan ketika perangkat berputar di satu arah vektor akan berputar
ke arah berlawanan ( relatif terhadap sistem coordonate perangkat , yang kita gunakan ) . Maka , jika Anda perlu untuk
membalikkan RateAxz , Anda bisa mencapai hal ini dengan memperkenalkan faktor masuk Eq.3 , sebagai berikut :
RateAxz = InvertAxz * ( AdcGyroXZ * Vref / 1023 - VzeroRate ) / Sensitivitas , di mana InvertAxz adalah 1 atau -1
pengujian yang sama dapat dilakukan untuk RateAyz , dengan memutar perangkat sekitar sumbu X , dan Anda dapat
mengidentifikasi keluaran gyroscope sesuai dengan RateAyz , dan jika perlu dibalikkan . Setelah Anda punya nilai untuk InvertAyz ,
Anda harus menggunakan rumus berikut untuk menghitung RateAyz :
RateAyz = InvertAyz * ( AdcGyroYZ * Vref / 1023 - VzeroRate ) / Sensitivitas
Jika Anda akan melakukan tes ini pada Acc_Gyro papan Anda akan mendapatkan hasil sebagai berikut :
- Pin output untuk RateAxz adalah GX4 dan InvertAxz = 1
- Pin output untuk RateAyz adalah GY4 dan InvertAyz = 1
Dari titik ini kita akan menganggap bahwa Anda harus men-setup IMU Anda sedemikian rupa sehingga Anda bisa menghitung nilai
yang benar untuk AXR , Ayr , AZR ( sebagaimana didefinisikan Bagian 1 . Accelerometer ) dan RateAxz , RateAyz ( sebagaimana
didefinisikan dalam Bagian 2 . Giroskop ) . Berikutnya kita akan menganalisis hubungan antara nilai-nilai yang ternyata berguna
untuk mendapatkan estimasi yang lebih akurat dari kecenderungan perangkat relatif terhadap bidang tanah .
Anda mungkin bertanya-tanya pada titik ini , jika model accelerometer sudah memberi kami sudut inklinasi AXR , Ayr , AZR kenapa
kita ingin repot-repot dengan data giroskop ? Jawabannya sederhana : Data accelerometer tidak selalu dapat dipercaya 100 % .
Ada beberapa alasan , mengingat accelerometer yang mengukur gaya inersia kekuatan, seperti dapat disebabkan oleh gravitasi
( dan idealnya hanya dengan gravitasi ) , tetapi mungkin juga disebabkan oleh percepatan ( gerakan) perangkat. Akibatnya bahkan
jika accelerometer dalam keadaan relatif stabil , masih sangat sensitif terhadap mekanik getaran dan kebisingan pada umumnya .
Ini adalah alasan utama kenapa kebanyakan sistem IMU menggunakan giroskop untuk kelancaran keluar setiap kesalahan
accelerometer . Tapi bagaimana hal ini dilakukan? Dan giroskop bebas dari kebisingan ?

Giroskop ini tidak bebas dari kebisingan namun karena mengukur rotasi itu kurang sensitif terhadap gerakan mekanis linier , jenis
suara yang accelerometer menderita , namun giroskop memiliki jenis lain dari masalah seperti misalnya melayang ( tidak kembali ke
nilai nol - tingkat ketika rotasi berhenti ) . Namun oleh rata-rata data yang berasal dari accelerometer dan giroskop kita bisa
mendapatkan perkiraan yang relatif lebih baik dari kecenderungan perangkat saat ini daripada kami akan memperoleh dengan
menggunakan data accelerometer saja .
Pada langkah selanjutnya saya akan memperkenalkan suatu algoritma yang terinspirasi oleh beberapa ide yang digunakan dalam
filter Kalman , namun adalah jauh lebih sederhana dan lebih mudah untuk menerapkan pada perangkat embedded . Sebelum itu
mari kita lihat dulu apa kita ingin algoritma untuk menghitung . Yah, itu adalah arah vektor gaya gravitasi R = [ Rx , Ry , Rz ] dari
mana kita bisa memperoleh nilai-nilai lain seperti AXR , Ayr , AZR atau cos , Nyaman, cosZ yang akan memberikan kita gambaran
tentang kecenderungan perangkat kami relatif terhadap bidang tanah , kita membahas hubungan antara nilai-nilai dalam Bagian 1 .
Orang mungkin mengatakan - jangan kita sudah memiliki nilai-nilai Rx , Ry , Rz dari Eq.2 dalam Bagian 1 ? Well ya , tapi ingat
bahwa ini nilai-nilai yang berasal dari data hanya accelerometer , jadi jika Anda akan menggunakan mereka secara langsung dalam
aplikasi Anda, Anda mungkin mendapatkan lebih banyak suara daripada aplikasi Anda bisa tolerir. Untuk menghindari kebingungan
mari kita mendefinisikan kembali ukuran accelerometer sebagai berikut :
RACC - vektor adalah gaya inersia yang diukur dengan accelerometer , yang terdiri dari komponen berikut ( proyeksi pada X , Y , Z
kapak ) :
RxAcc = ( AdcRx * Vref / 1023 - VzeroG ) / Sensitivitas
RyAcc = ( AdcRy * Vref / 1023 - VzeroG ) / Sensitivitas
RzAcc = ( AdcRz * Vref / 1023 - VzeroG ) / Sensitivitas
Sejauh ini kita memiliki seperangkat nilai-nilai diukur bahwa kita bisa memperoleh murni dari nilai ADC accelerometer . Kita akan
menyebutnya ini set data yang " vektor " dan kami akan menggunakan notasi berikut .
RACC = [ RxAcc , RyAcc , RzAcc ]
Karena komponen RACC dapat diperoleh dari data accelerometer , kita bisa menganggapnya sebagai masukan untuk algoritma
kami .
Harap dicatat bahwa karena RACC mengukur gaya gravitasi Anda akan benar jika Anda menganggap bahwa panjang vektor ini
didefinisikan sebagai berikut adalah sama atau mendekati 1 g.
| RACC | = sqrt ( RxAcc ^ 2 + RyAcc ^ 2 + RzAcc ^ 2 ) ,
Namun untuk memastikan itu masuk akal untuk memperbarui vektor ini sebagai berikut :
RACC ( normalized ) = [ RxAcc / | RACC | , RyAcc / | RACC | , RzAcc / | RACC | ] .
Ini akan memastikan panjang vektor RACC normalisasi Anda selalu 1 .
Selanjutnya kami akan memperkenalkan sebuah vektor baru dan kita akan menyebutnya
Istirahat = [ RxEst , RyEst , RzEst ]
Ini akan menjadi output dari algoritma kami , ini dikoreksi nilai berdasarkan data giroskop dan berdasarkan data estimasi masa lalu .
Di sini adalah apa algoritma kami akan melakukan :
- Accelerometer memberitahu kita : " Kamu sekarang pada posisi RACC "
- Kita mengatakan " Terima kasih tapi biarkan aku memeriksa " ,
- Kemudian perbaiki informasi ini dengan data giroskop serta dengan data sisa masa lalu dan kami output vektor diperkirakan
Istirahat baru.
- Kita menganggap Istirahat menjadi kami " harga" untuk posisi saat perangkat.
Mari kita lihat bagaimana kita dapat membuatnya bekerja .
Kita akan memulai urutan kita dengan mempercayai accelerometer kami dan menugaskan :
Istirahat ( 0 ) = RACC ( 0 )

By the way ingat Istirahat dan RACC adalah vektor , sehingga persamaan di atas adalah cara sederhana untuk menulis 3 set
persamaan , dan menghindari pengulangan :
RxEst ( 0 ) = RxAcc ( 0 )
RyEst ( 0 ) = RyAcc ( 0 )
RzEst ( 0 ) = RzAcc ( 0 )
Selanjutnya kami akan melakukan pengukuran teratur pada interval waktu yang sama detik T , dan kami akan memperoleh
pengukuran baru yang kami akan mendefinisikan sebagai RACC ( 1 ) , RACC ( 2 ) , RACC ( 3 ) dan seterusnya. Kami juga akan
menerbitkan perkiraan baru pada setiap interval waktu Istirahat ( 1 ) , Istirahat ( 2 ) , Istirahat ( 3 ) dan seterusnya.Misalkan kita
berada di langkah n . Kami memiliki dua set diketahui nilai-nilai yang kita ingin gunakan :Istirahat ( n - 1 ) - perkiraan kami
sebelumnya , dengan Istirahat ( 0 ) = RACC ( 0 )RACC ( n ) - pengukuran accelerometer kami saat iniSebelum kita bisa menghitung
Istirahat ( n ) , mari kita memperkenalkan nilai yang terukur baru , yang bisa kita peroleh dari giroskop dan estimasi sebelumnya .
Kita akan menyebutnya Rgyro , dan juga merupakan vektor yang terdiri dari 3 komponen :
Rgyro = [ RxGyro , RyGyro , RzGyro ]
Kami akan menghitung vektor ini satu komponen pada suatu waktu . Kita akan mulai dengan RxGyro .

Mari kita mulai dengan mengamati hubungan berikut dalam model giroskop kami , dari sudut kanan segitiga yang dibentuk oleh Rz
dan rxz kita dapat memperoleh bahwa:
tan ( Axz ) = Rx / RZ = > Axz = atan2 ( Rx , Rz )
Atan2 mungkin menjadi fungsi Anda tidak pernah digunakan sebelumnya, itu mirip dengan atan , kecuali mengembalikan nilai
dalam kisaran ( - PI , PI ) sebagai lawan ( PI - / 2 , PI / 2) sebagai dikembalikan oleh atan , dan dibutuhkan 2 argumen , bukan satu.
Hal ini memungkinkan kita untuk mengkonversi dua nilai Rx , Rz ke sudut dalam berbagai 360 derajat ( - PI PI ) . Anda dapat
membaca banyak aboutatan2 sini.
Jadi tahu RxEst ( n - 1 ) , dan RzEst ( n - 1 ) kita bisa menemukan :
Axz ( n - 1 ) = atan2 ( RxEst ( n - 1 ) , RzEst ( n - 1 ) ) .
Ingat bahwa giroskop mengukur tingkat perubahan dari sudut Axz . Jadi kita bisa memperkirakan sudut Axz baru ( n ) sebagai
berikut :
Axz ( n ) = Axz ( n - 1 ) + RateAxz ( n ) T *
Ingat bahwa RateAxz bisa didapat dari giroskop kami ADC pembacaan . Sebuah formula yang lebih tepat dapat menggunakan ratarata tingkat rotasi dihitung sebagai berikut :
RateAxzAvg = ( RateAxz ( n ) + RateAxz ( n - 1 ) ) / 2
Axz ( n ) = Axz ( n - 1 ) + RateAxzAvg * T
Dengan cara yang sama kita dapat menemukan :

AYZ ( n ) = AYZ ( n - 1 ) + RateAyz ( n ) * TOk

jadi sekarang kita memiliki Axz ( n ) dan AYZ ( n ) . Dimana kita pergi dari sini untuk mengurangi RxGyro / RyGyro ? Dari
Persamaan . 1We dapat menulis panjang vektor Rgyro sebagai berikut :
| Rgyro | = sqrt ( RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2 )
Juga karena kita normal vektor RACC kami , kita bisa mengasumsikan bahwa panjang itu adalah 1 dan itu tidak berubah setelah
rotasi , sehingga relatif aman untuk menulis :
| Rgyro | = 1
Mari mengadopsi notasi pendek sementara untuk perhitungan di bawah ini :
x = RxGyro , y = RyGyro , z = RzGyro
Menggunakan hubungan di atas kita dapat menulis :
x = x / 1 = x / sqrt ( x ^ 2 + y ^ 2 + z ^ 2 )
Mari kita membagi pembilang dan penyebut pecahan oleh sqrt ( x ^ 2 + z ^ 2 )
x = ( x / sqrt ( x * 2 + z ^ 2 ) ) / sqrt ( ( x * 2 + y 2 + ^ z ^ 2 ) / ( x * 2 + z ^ 2 ) )
Perhatikan bahwa x / SQRT ( 2 x ^ + z ^ 2 ) = sin ( Axz ) , sehingga :
x = sin ( Axz ) / SQRT ( 1 + y ^ 2 / ( x ^ 2 + z ^ 2 ) )
Sekarang kalikan pembilang dan penyebut dari fraksi dalam sqrt oleh z ^ 2
x = sin ( Axz ) / sqrt ( 1 + y ^ 2 * z ^ 2 / ( z ^ 2 * ( x ^ 2 + z ^ 2 ) ) )
Perhatikan bahwa z / SQRT ( 2 x ^ + ^ z 2 ) = cos ( Axz ) dan y / z = tan ( AYZ ) , jadi akhirnya :
x = sin ( Axz ) / sqrt ( 1 + cos ( Axz ) ^ 2 * tan ( AYZ ) ^ 2 )
Akan kembali ke notasi kita mendapatkan:
RxGyro = sin ( Axz ( n ) ) / SQRT ( 1 + cos ( Axz ( n ) ) ^ 2 * tan ( AYZ ( n ) ) ^ 2 )
sama kita temukan bahwa
RyGyro = sin ( AYZ ( n ) ) / SQRT ( 1 + cos ( AYZ ( n ) ) ^ 2 * tan ( Axz ( n ) ) ^ 2 )
Side Catatan : adalah mungkin untuk lebih menyederhanakan formula ini . Dengan membagi kedua bagian fraksi oleh dosa ( Axz ( n
) ) Anda mendapatkan :
RxGyro = 1 / sqrt ( 1 / dosa ( Axz ( n ) ) ^ 2 + cos ( Axz ( n ) ) ^ 2 / sin ( Axz ( n ) ) ^ 2 * tan ( AYZ ( n ) ) ^ 2 )
RxGyro = 1 / sqrt ( 1 / dosa ( Axz ( n ) ) ^ 2 + cot ( Axz ( n ) ) ^ 2 * sin ( AYZ ( n ) ) ^ 2 / cos ( AYZ ( n ) ) ^ 2 )
sekarang tambahkan dan kurangi cos ( Axz ( n ) ) ^ 2/sin ( Axz ( n ) ) ^ 2 = cot ( Axz ( n ) ) ^ 2
RxGyro = 1 / sqrt ( 1 / dosa ( Axz ( n ) ) ^ 2 - cos ( Axz ( n ) ) ^ 2/sin ( Axz ( n ) ) ^ 2 + cot ( Axz ( n ) ) ^ 2 * sin ( AYZ ( n ) ) ^ 2 / cos
( AYZ ( n ) ) ^ 2 + cot ( Axz ( n ) ) ^ 2 )
dan dengan mengelompokkan istilah 1 & 2 dan kemudian 3 & 4 kita mendapatkan
RxGyro = 1 / sqrt ( 1 + cot ( Axz ( n ) ) ^ 2 * detik ( AYZ ( n ) ) ^ 2 ) , di mana cot (x) = 1 / tan (x) dan sec (x) = 1 / cos ( X)
Rumus ini hanya menggunakan 2 fungsi trigonometri dan dapat komputasi lebih murah . Jika anda punya program Mathematica
Anda bisa memverifikasinya

dengan mengevaluasi FullSimplify [ Sin [ A ] ^ 2 / ( 1 + Cos [ A ] ^ 2 * Tan [ B ] ^ 2 ) ]

Sekarang , akhirnya kita dapat menemukan :
RzGyro = Masuk ( RzGyro ) * sqrt ( 1 - RxGyro ^ 2 - RyGyro ^ 2 ) .
Dimana Sign ( RzGyro ) = 1 ketika RzGyro > 0 = , serta Tanda ( RzGyro ) = -1 ketika RzGyro <0.
Salah satu cara sederhana memperkirakan ini untuk mengambil :
Sign ( RzGyro ) = Sign ( RzEst ( n - 1 ) )
Dalam praktek berhati-hati ketika RzEst ( n - 1 ) dekat dengan 0 . Anda bisa melewati tahap gyro sama sekali dalam kasus ini dan
menetapkan : Rgyro = Istirahat ( n - 1 ) . Rz dipakai sebagai acuan untuk menghitung Axz dan sudut AYZ dan ketika itu mendekati 0
, nilai mungkin meluap dan pemicu hasil buruk . Anda akan berada di domain bilangan floating point besar di mana tan ( ) / atan ( )
implementasi fungsi mungkin kurang presisi .
Jadi mari kita rekap yang kita miliki sejauh ini, kita berada di langkah n algoritma kami dan kami telah menghitung nilai berikut :
RACC - pembacaan arus dari accelerometer kami
Rgyro - diperoleh dari Rest ( n - 1 ) dan bacaan giroskop saat ini
Nilai-nilai mana kita menggunakan untuk menghitung perkiraan diperbarui Istirahat ( n ) ? Anda mungkin menduga bahwa kami
akan menggunakan keduanya. Kami akan menggunakan rata-rata tertimbang , sehingga :
Istirahat ( n ) = ( w1 + RACC * Rgyro * w2 ) / ( w1 + w2 )
Kita dapat menyederhanakan formula ini dengan membagi pembilang dan penyebut kedua fraksi oleh w1 .
Istirahat ( n ) = ( RACC * w1/w1 + Rgyro * w2/w1 ) / ( w1/w1 + w2/w1 )
dan setelah mengganti w2/w1 = wGyro kita mendapatkan :
Istirahat ( n ) = ( RACC + Rgyro * wGyro ) / ( + 1 wGyro )
Dalam rumus di atas wGyro memberitahu kita berapa banyak kita percaya gyro kami dibandingkan dengan accelerometer kami .
Nilai ini dapat dipilih secara eksperimental biasanya nilai antara 5 .. 20 akan memicu hasil yang baik .
Perbedaan utama dari algoritma ini dari Kalman Filter adalah bahwa berat badan ini relatif tetap , sedangkan di filter Kalman bobot
diperbarui secara permanen didasarkan pada suara diukur dari pembacaan accelerometer . Kalman Filter difokuskan pada memberi
Anda " yang terbaik " hasil teoritis , sedangkan algoritma ini dapat memberikan hasil "cukup baik " untuk aplikasi praktis . Anda bisa
menerapkan algoritma yang menyesuaikan wGyro tergantung beberapa faktor kebisingan yang Anda mengukur , tapi nilai tetap
akan bekerja dengan baik untuk sebagian besar aplikasi .
Kami adalah salah satu langkah menjauh dari mendapatkan nilai estimasi kami diperbarui :
RxEst ( n ) = ( RxAcc + RxGyro * wGyro ) / ( 1 + wGyro )
RyEst ( n ) = ( RyAcc + RyGyro * wGyro ) / ( 1 + wGyro )
RzEst ( n ) = ( RzAcc + RzGyro * wGyro ) / ( 1 + wGyro )
Sekarang mari kita menormalkan vektor ini lagi :
R = sqrt ( RxEst ( n ) ^ 2 + RyEst ( n ) ^ 2 + RzEst ( n ) ^ 2 )
RxEst ( n ) = RxEst ( n ) / R
RyEst ( n ) = RyEst ( n ) / R
RzEst ( n ) = RzEst ( n ) / R