Tutorial Accelerometer & Gyro: 3 Langkah

2024 Pengarang: John Day | [email protected]. Diubah suai terakhir: 2024-01-30 11:14

Pengenalan

Panduan ini ditujukan kepada semua orang yang berminat menggunakan Accelerometer dan Gyroscopes serta gabungan peranti IMU (Inertial Measurement Unit) dalam projek elektronik mereka

Kami akan merangkumi:

• Apakah ukuran pecutan?
• Apa yang diukur oleh giroskop (aka gyro)?
• Cara menukar bacaan analog-ke-digital (ADC) yang anda dapat dari sensor ini ke unit fizikal (yang akan menjadi g untuk pecutan, deg / s untuk giroskop)
• Cara menggabungkan bacaan akselerometer dan giroskop untuk mendapatkan maklumat yang tepat mengenai kecenderungan peranti anda berbanding dengan permukaan tanah

Sepanjang artikel saya akan berusaha memastikan matematik minimum. Sekiranya anda mengetahui apa itu Sine / Cosine / Tangent, anda seharusnya dapat memahami dan menggunakan idea-idea ini dalam projek anda tidak kira platform apa yang anda gunakan: Arduino, Propeller, Basic Stamp, Atmel chip, Microchip PIC, dll.

Ada orang di luar sana yang percaya bahawa anda memerlukan matematik yang rumit untuk memanfaatkan unit IMU (penapis FIR atau IIR yang kompleks seperti penapis Kalman, penapis Parks-McClellan, dll). Anda boleh meneliti semua itu dan memperoleh hasil yang luar biasa tetapi kompleks. Cara saya menerangkan sesuatu hanya memerlukan asas matematik. Saya sangat mempercayai kesederhanaan. Saya rasa sistem yang mudah lebih senang dikawal dan dipantau, selain itu banyak peranti yang tidak tertanam tidak mempunyai kekuatan dan sumber daya untuk melaksanakan algoritma kompleks yang memerlukan pengiraan matriks.

Saya akan gunakan sebagai contoh unit IMU baru, Acc_Gyro Accelerometer + Gyro IMU. Kami akan menggunakan parameter peranti ini dalam contoh kami di bawah. Unit ini adalah alat yang baik untuk bermula kerana terdiri daripada 2 peranti:

- LIS331AL (lembaran data) - pecutan 2G triaksial - LPR550AL (lembaran data) - nada dan gulungan paksi dua, giroskop 500 deg / saat

Bersama-sama mereka mewakili Unit Pengukuran Ketidakseimbangan 5 darjah. Sekarang itulah nama yang menarik! Walaupun begitu, di sebalik nama mewah terdapat peranti gabungan yang sangat berguna yang akan kami bahas dan terangkan secara terperinci dalam panduan ini.

Langkah 1: Accelerometer

Untuk memahami unit ini kita akan mulakan dengan pecutan. Semasa memikirkan pecutan, seringkali berguna untuk membayangkan kotak berbentuk kubus dengan bola di dalamnya. Anda mungkin membayangkan sesuatu yang lain seperti kuki atau donat, tetapi saya akan membayangkan bola:

Sekiranya kita mengambil kotak ini di tempat yang tidak mempunyai medan graviti atau untuk perkara itu tanpa bidang lain yang mungkin mempengaruhi kedudukan bola - bola hanya akan melayang di tengah kotak. Anda dapat membayangkan kotak itu berada di angkasa lepas yang jauh dari badan kosmik, atau jika tempat seperti itu sukar dijumpai, bayangkan sekurang-kurangnya kapal angkasa yang mengorbit planet ini di mana semuanya berada dalam keadaan tanpa berat. Dari gambar di atas, anda dapat melihat bahawa kami memberikan pada setiap sumbu sepasang dinding (kami mengeluarkan dinding Y + sehingga kami dapat melihat ke dalam kotak). Bayangkan bahawa setiap dinding sensitif terhadap tekanan. Sekiranya kita bergerak tiba-tiba kotak ke kiri (kita mempercepatnya dengan pecutan 1g = 9.8m / s ^ 2), bola akan memukul dinding X-. Kami kemudian mengukur kekuatan tekanan yang dikenakan bola ke dinding dan mengeluarkan nilai -1g pada sumbu X.

Harap diperhatikan bahawa akselerometer sebenarnya akan mengesan daya yang diarahkan ke arah yang berlawanan dari vektor pecutan. Daya ini sering disebut Inertial Force atau Fictitious Force. Satu perkara yang harus anda pelajari dari ini adalah bahawa accelerometer mengukur pecutan secara tidak langsung melalui daya yang dikenakan pada salah satu dindingnya (mengikut model kami, ia mungkin merupakan spring atau sesuatu yang lain dalam accelerometer kehidupan sebenar). Gaya ini boleh disebabkan oleh pecutan, tetapi seperti yang akan kita lihat dalam contoh berikutnya, ia tidak selalu disebabkan oleh pecutan.

Sekiranya kita mengambil model kita dan meletakkannya di Bumi bola akan jatuh di dinding Z dan akan memberikan kekuatan 1g di dinding bawah, seperti yang ditunjukkan pada gambar di bawah:

Dalam kes ini kotak tidak bergerak tetapi kita masih mendapat bacaan -1g pada paksi Z. Tekanan yang dikenakan bola di dinding disebabkan oleh gaya gravitasi. Secara teori, ini mungkin jenis kekuatan yang berbeza - misalnya, jika anda membayangkan bahawa bola kami adalah logam, meletakkan magnet di sebelah kotak dapat menggerakkan bola sehingga menyentuh dinding lain. Ini dikatakan hanya untuk membuktikan bahawa pada hakikatnya langkah-langkah akselerometer memaksa bukan pecutan. Kebetulan pecutan menyebabkan daya inersia yang ditangkap oleh mekanisme pengesanan daya pecutan.

Walaupun model ini tidak betul bagaimana sensor MEMS dibina, ia sering berguna dalam menyelesaikan masalah yang berkaitan dengan accelerometer. Sebenarnya ada sensor serupa yang mempunyai bola logam di dalamnya, ia dipanggil suis kecondongan, namun ia lebih primitif dan biasanya mereka hanya dapat mengetahui apakah peranti cenderung dalam jarak tertentu atau tidak, bukan sejauh mana kecenderungan.

Sejauh ini kami telah menganalisis output pecutan pada paksi tunggal dan hanya ini yang anda dapat dengan akselerometer paksi tunggal. Nilai sebenar pecutan triaksial berasal dari fakta bahawa mereka dapat mengesan daya inersia pada ketiga-tiga paksi. Mari kembali ke model kotak kami, dan mari putar kotak 45 darjah ke kanan. Bola akan menyentuh 2 dinding sekarang: Z- dan X- seperti yang ditunjukkan dalam gambar di bawah:

Nilai 0,71 tidak sewenang-wenang, sebenarnya adalah penghampiran untuk SQRT (1/2). Ini akan menjadi lebih jelas ketika kami memperkenalkan model seterusnya untuk accelerometer.

Pada model sebelumnya, kita telah menetapkan daya graviti dan memutar kotak khayalan kita. Dalam 2 contoh terakhir, kami telah menganalisis output dalam 2 kedudukan kotak yang berbeza, sementara vektor daya tetap tidak berubah. Walaupun ini berguna untuk memahami bagaimana akselerometer berinteraksi dengan daya luar, adalah lebih praktikal untuk melakukan pengiraan jika kita membetulkan sistem koordinat ke paksi pecutan dan membayangkan bahawa vektor daya berputar di sekitar kita.

Lihat model di atas, saya mengekalkan warna sumbu supaya anda dapat melakukan peralihan mental dari model sebelumnya ke model baru. Cuba bayangkan bahawa setiap paksi dalam model baru adalah tegak lurus dengan muka kotak masing-masing pada model sebelumnya. Vektor R adalah vektor daya yang diukur oleh akselerometer (boleh jadi gaya gravitasi atau daya inersia dari contoh di atas atau gabungan kedua-duanya). Rx, Ry, Rz adalah unjuran vektor R pada paksi X, Y, Z. Perhatikan hubungan berikut:

R ^ 2 = Rx ^ 2 + Ry ^ 2 + Rz ^ 2 (Persamaan 1)

yang pada dasarnya setara dengan teorem Pythagoras dalam 3D.

Ingat bahawa sedikit lebih awal saya memberitahu anda bahawa nilai SQRT (1/2) ~ 0.71 tidak rawak. Sekiranya anda memasukkannya dalam formula di atas, setelah mengingatkan bahawa daya graviti kami adalah 1 g, kami dapat mengesahkan bahawa:

1 ^ 2 = (-SQRT (1/2)) ^ 2 + 0 ^ 2 + (-SQRT (1/2)) ^ 2

hanya dengan menggantikan R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) dalam Persamaan.1

Setelah pembukaan teori yang panjang, kita semakin hampir dengan pecutan kehidupan sebenar. Nilai Rx, Ry, Rz sebenarnya berkaitan secara linear dengan nilai-nilai yang akan dihasilkan oleh accelerometer kehidupan nyata anda dan yang dapat anda gunakan untuk melakukan pelbagai pengiraan.

Sebelum kita sampai di sana, mari kita bincangkan sedikit tentang cara percepatan untuk menyampaikan maklumat ini kepada kami. Kebanyakan pecutan pecah dalam dua kategori: digital dan analog. Accelerometer digital akan memberi anda maklumat menggunakan protokol bersiri seperti I2C, SPI atau USART, sementara accelerometer analog akan menghasilkan tahap voltan dalam julat yang telah ditentukan yang harus anda ubah ke nilai digital menggunakan modul ADC (analog ke digital converter). Saya tidak akan membahas dengan terperinci bagaimana ADC berfungsi, sebahagiannya kerana topik ini sangat luas dan sebahagiannya kerana ia berbeza dari satu platform ke platform yang lain. Sebilangan mikrokontroler akan mempunyai modul ADC terbina dalam, sebahagiannya memerlukan komponen luaran untuk melakukan penukaran ADC. Tidak kira apa jenis modul ADC yang anda gunakan, anda akan mendapat nilai dalam julat tertentu. Sebagai contoh modul ADC 10-bit akan mengeluarkan nilai dalam julat 0..1023, perhatikan bahawa 1023 = 2 ^ 10 -1. Modul ADC 12-bit akan mengeluarkan nilai dalam lingkungan 0..4095, perhatikan bahawa 4095 = 2 ^ 12-1.

Mari kita teruskan dengan mempertimbangkan contoh mudah, andaikan modul ADC 10bit kami memberi kami nilai berikut untuk tiga saluran pecutan (paksi):

AdcRx = 586 AdcRy = 630 AdcRz = 561

Setiap modul ADC akan mempunyai voltan rujukan, mari kita anggap dalam contohnya ia adalah 3.3V. Untuk menukar nilai adc 10bit ke voltan kami menggunakan formula berikut:

VoltsRx = AdcRx * Vref / 1023

Catatan ringkas di sini: bahawa untuk ADC 8bit pembahagi terakhir adalah 255 = 2 ^ 8 -1, dan untuk pembahagi terakhir ADC 12bit adalah 4095 = 2 ^ 12 -1.

Menerapkan formula ini ke semua 3 saluran yang kami dapat:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (kita membundarkan semua hasil hingga 2 titik perpuluhan) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Setiap pecutan mempunyai tahap voltan sifar-g, anda dapat menemuinya dalam spesifikasi, ini adalah voltan yang sepadan dengan 0g. Untuk mendapatkan nilai voltan yang ditandatangani, kita perlu mengira peralihan dari tahap ini. Katakan tahap voltan 0g kami ialah VzeroG = 1.65V. Kami mengira peralihan voltan dari voltan sifar-g seperti berikut:

DeltaVoltsRx = 1.89V - 1.65V = 0.24V DeltaVoltsRy = 2.03V - 1.65V = 0.38V DeltaVoltsRz = 1.81V - 1.65V = 0.16V

Kami sekarang mempunyai pembacaan pecutan dalam Volt, masih belum dalam g (9,8 m / s ^ 2), untuk melakukan penukaran terakhir, kami menerapkan kepekaan akselerometer, biasanya dinyatakan dalam mV / g. Katakan Sensitiviti kita = 478.5mV / g = 0.4785V / g. Nilai kepekaan dapat dilihat dalam spesifikasi pecutan. Untuk mendapatkan nilai daya akhir yang dinyatakan dalam g, kami menggunakan formula berikut:

Rx = DeltaVoltsRx / Sensitiviti

Rx = 0.24V / 0.4785V / g = ~ 0.5g Ry = 0.38V / 0.4785V / g = ~ 0.79g Rz = 0.16V / 0.4785V / g = ~ 0.33g

Kami tentu saja dapat menggabungkan semua langkah dalam satu formula, tetapi saya melalui semua langkah untuk menjelaskan bagaimana anda pergi dari pembacaan ADC ke komponen vektor kekuatan yang dinyatakan dalam g.

Rx = (AdcRx * Vref / 1023 - VzeroG) / Sensitiviti (Persamaan.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Sensitiviti Rz = (AdcRz * Vref / 1023 - VzeroG) / Sensitiviti

Kita sekarang mempunyai ketiga-tiga komponen yang menentukan vektor daya inersia kita, jika peranti tidak dikenakan daya lain selain graviti, kita dapat menganggap ini adalah arah vektor daya gravitasi kita. Sekiranya anda ingin mengira kecenderungan peranti berbanding dengan tanah, anda boleh mengira sudut antara vektor ini dan paksi Z. Sekiranya anda juga berminat dengan arah kecenderungan per-paksi, anda boleh membahagikan hasil ini kepada 2 komponen: kecenderungan pada paksi X dan Y yang boleh dikira sebagai sudut antara vektor graviti dan paksi X / Y. Mengira sudut ini lebih mudah daripada yang anda fikirkan, sekarang kita telah mengira nilai untuk Rx, Ry dan Rz. Mari kembali ke model pecutan terakhir kami dan buat beberapa notasi tambahan:

Sudut yang kami minati adalah sudut antara paksi X, Y, Z dan vektor daya R. Kami akan menentukan sudut ini sebagai Axr, Ayr, Azr. Anda dapat melihat dari segitiga sudut kanan yang dibentuk oleh R dan Rx bahawa:

cos (Axr) = Rx / R, dan serupa: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Kita dapat menolak dari Persamaan 1 bahawa R = SQRT (Rx ^ 2 + Ry ^ 2 + Rz ^ 2).

Kita dapat mengetahui sudut kita sekarang dengan menggunakan fungsi arccos () (fungsi cos terbalik):

Axr = arccos (Rx / R) Ayr = arccos (Ry / R) Azr = arccos (Rz / R)

Kami telah pergi jauh untuk menjelaskan model accelerometer, hanya untuk membuat rumusan ini. Bergantung pada aplikasi anda, anda mungkin ingin menggunakan formula perantaraan yang telah kami hasilkan. Kami juga akan memperkenalkan model giroskop tidak lama lagi, dan kami akan melihat bagaimana data akselerometer dan giroskop dapat digabungkan untuk memberikan anggaran kecenderungan yang lebih tepat.

Tetapi sebelum kita melakukannya, mari kita buat notasi yang lebih berguna:

cosX = cos (Axr) = Rx / R cosy = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

Kembar tiga ini sering disebut Direction Cosine, dan pada dasarnya mewakili vektor unit (vektor dengan panjang 1) yang mempunyai arah yang sama dengan vektor R kita. Anda boleh mengesahkan bahawa:

SQRT (cosX ^ 2 + selesa ^ 2 + cosZ ^ 2) = 1

Ini adalah harta yang bagus kerana ia membebaskan kita dari pemantauan modulus (panjang) vektor R. Sering kali jika kita hanya berminat ke arah vektor inersia kita, masuk akal untuk menormalkan modulus untuk mempermudah pengiraan lain.

Langkah 2: Giroskop

Kami tidak akan memperkenalkan model kotak setara untuk giroskop seperti yang kami lakukan untuk accelerometer, sebaliknya kami akan melompat terus ke model accelerometer kedua dan kami akan menunjukkan apa yang diukur oleh giroskop mengikut model ini.

Setiap saluran giroskop mengukur putaran di salah satu paksi. Sebagai contoh giroskop 2 paksi akan mengukur putaran di sekitar (atau ada yang mungkin mengatakan "tentang") paksi X dan Y. Untuk menyatakan putaran ini dalam angka mari lakukan beberapa notasi. Pertama mari kita tentukan:

Rxz - adalah unjuran vektor daya inersia pada satah XZ Ryz - adalah unjuran vektor daya inersia R pada satah YZ

Dari segitiga sudut kanan yang dibentuk oleh Rxz dan Rz, menggunakan teorema Pythagoras kita dapat:

Rxz ^ 2 = Rx ^ 2 + Rz ^ 2, dan serupa: Ryz ^ 2 = Ry ^ 2 + Rz ^ 2

perhatikan juga bahawa:

R ^ 2 = Rxz ^ 2 + Ry ^ 2, ini dapat diturunkan dari Persamaan Persamaan 1 dan ke atas, atau dapat berasal dari segitiga sudut kanan yang dibentuk oleh R dan Ryz R ^ 2 = Ryz ^ 2 + Rx ^ 2

Kami tidak akan menggunakan formula ini dalam artikel ini tetapi berguna untuk memperhatikan hubungan antara semua nilai dalam model kami.

Sebaliknya kita akan menentukan sudut antara paksi Z dan vektor Rxz, Ryz seperti berikut:

Axz - adalah sudut antara Rxz (unjuran R pada satah XZ) dan paksi Z Ayz - adalah sudut antara Ryz (unjuran R pada satah YZ) dan paksi Z

Sekarang kita semakin hampir dengan ukuran giroskop. Giroskop mengukur kadar perubahan sudut yang ditentukan di atas. Dengan kata lain ia akan menghasilkan nilai yang berkaitan secara linear dengan kadar perubahan sudut-sudut ini. Untuk menjelaskan ini mari kita anggap bahawa kita telah mengukur sudut putaran di sekitar paksi Y (yang akan menjadi sudut Axz) pada waktu t0, dan kita mendefinisikannya sebagai Axz0, seterusnya kita mengukur sudut ini pada waktu kemudian t1 dan itu adalah Axz1. Kadar perubahan akan dikira seperti berikut:

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

Sekiranya kita menyatakan Axz dalam darjah, dan masa dalam beberapa saat, maka nilai ini akan dinyatakan dalam deg / s. Inilah yang diukur oleh giroskop.

Dalam praktiknya giroskop (melainkan giroskop digital khas) jarang memberi anda nilai yang dinyatakan dalam deg / s. Sama seperti accelerometer, anda akan mendapat nilai ADC yang anda perlukan untuk menukar menjadi deg / s menggunakan formula yang serupa dengan Persamaan. 2 yang telah kita tetapkan untuk pecutan. Mari memperkenalkan formula penukaran ADC ke deg / s untuk giroskop (kami menganggap kami menggunakan modul ADC 10bit, untuk ADC 8bit menggantikan 1023 dengan 255, untuk 12bit ADC menggantikan 1023 dengan 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensitivity Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / Sensitiviti

AdcGyroXZ, AdcGyroYZ - diperoleh dari modul adc kami dan mereka mewakili saluran yang mengukur putaran unjuran vektor R di XZ masing-masing dalam satah YZ, yang setara dengan mengatakan putaran dilakukan di sekitar paksi Y dan X masing-masing.

Vref - adalah voltan rujukan ADC yang akan kita gunakan 3.3V dalam contoh di bawah VzeroRate - adalah voltan kadar sifar, dengan kata lain voltan yang dihasilkan oleh giroskop apabila tidak dikenakan putaran, untuk papan Acc_Gyro contohnya 1.23V (anda dapat mengetahui nilai ini dalam spesifikasi) Sensitiviti - adalah kepekaan giroskop anda dinyatakan dalam mV / (deg / s) yang sering ditulis sebagai mV / deg / s, pada dasarnya ia memberitahu anda berapa mV yang akan output giroskop meningkat, jika anda meningkatkan kelajuan putaran sebanyak satu deg / s. Kepekaan papan Acc_Gyro misalnya 2mV / deg / s atau 0.002V / deg / s

Mari kita ambil contoh, andaikan modul ADC kita dikembalikan berikut nilai:

AdcGyroXZ = 571 AdcGyroXZ = 323

Dengan menggunakan formula di atas, dan menggunakan parameter spesifikasi papan Acc_Gyro, kami akan mendapat:

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 peranti berputar di sekitar paksi Y (atau kita boleh mengatakan ia berputar di satah XZ) dengan kelajuan 306 deg / s dan di sekitar paksi X (atau kita boleh mengatakan ia berputar di satah YZ) dengan kelajuan - 94 darjah / saat. Harap maklum bahawa tanda negatif bermaksud bahawa peranti berputar ke arah yang berlawanan dari arah positif konvensional. Secara konvensional, satu arah putaran adalah positif. Lembaran spesifikasi giroskop yang baik akan menunjukkan arah mana yang positif, jika tidak, anda harus mencarinya dengan bereksperimen dengan peranti dan mencatat arah putaran mana yang menghasilkan peningkatan voltan pada pin output. Ini dilakukan dengan menggunakan osiloskop kerana sebaik sahaja anda menghentikan putaran voltan akan turun kembali ke tahap kadar sifar. Sekiranya anda menggunakan multimeter, anda mesti mengekalkan kadar putaran tetap selama sekurang-kurangnya beberapa saat dan perhatikan voltan semasa putaran ini, kemudian bandingkan dengan voltan kadar sifar. Sekiranya lebih besar daripada voltan kadar sifar, ini bermaksud arah putaran positif.

Langkah 3: Menggabungkan Accelerometer dan Gyro

Menggabungkan semuanya - Menggabungkan data pecutan dan giroskop

Sekiranya anda membaca artikel ini, anda mungkin memperoleh atau merancang untuk memperoleh peranti IMU, atau mungkin anda merancang untuk membuatnya dari peranti pecutan dan giroskop yang berasingan.

Langkah pertama dalam menggunakan gabungan peranti IMU yang menggabungkan akselerometer dan giroskop adalah menyelaraskan sistem koordinatnya. Cara termudah untuk melakukannya adalah memilih sistem koordinat akselerometer sebagai sistem koordinat rujukan anda. Sebilangan besar lembaran data pecutan akan menunjukkan arah paksi X, Y, Z berbanding dengan imej cip fizikal atau peranti. Contohnya berikut adalah arah paksi X, Y, Z seperti yang ditunjukkan dalam spesifikasi untuk papan Acc_Gyro:

Langkah seterusnya adalah:

Kenal pasti output giroskop yang sesuai dengan nilai RateAxz, RateAyz yang dibincangkan di atas. Tentukan apakah output ini perlu dibalikkan kerana kedudukan giroskop fizikal berbanding dengan pecutan

Jangan menganggap bahawa jika giroskop mempunyai output yang bertanda X atau Y, ia akan sesuai dengan paksi mana pun dalam sistem koordinat pecutan, walaupun output ini adalah sebahagian daripada unit IMU. Kaedah terbaik adalah dengan mengujinya. Dengan andaian anda telah menetapkan kedudukan giroskop berbanding dengan pecutan. Diandaikan bahawa sempadan giro dan akselerometer saling sejajar antara satu sama lain, iaitu anda meletakkan giro pada sudut gandaan 90deg berbanding dengan chip pecutan. Sekiranya anda memperoleh papan IMU kemungkinan mereka sudah selaras dengan cara ini. Kami tidak akan membincangkan model artikel ini di mana giroskop diletakkan pada sudut yang tidak teratur berbanding dengan pecutan (katakanlah 45 atau 30 darjah), walaupun ini mungkin berguna dalam beberapa aplikasi.

Berikut adalah urutan sampel untuk menentukan output giroskop yang sesuai dengan nilai RateAxz yang dibincangkan di atas.

- mulakan dengan meletakkan peranti dalam kedudukan mendatar. Kedua-dua output akselerometer X dan Y akan menghasilkan voltan sifar-g (contohnya untuk papan Acc_Gyro ini adalah 1.65V)

- seterusnya mulakan memutar peranti di sekitar paksi Y, cara lain untuk mengatakan bahawa anda memutar peranti di satah XZ, sehingga output akselerometer X dan Z berubah dan output Y tetap berterusan. - semasa memutar peranti dengan catatan kelajuan tetap yang mana output giroskop berubah, output giroskop yang lain harus tetap tetap - output giroskop yang berubah semasa putaran di sekitar paksi Y (putaran dalam satah XZ) akan memberikan nilai input untuk AdcGyroXZ, dari mana kami mengira RateAxz - langkah terakhir adalah memastikan arah putaran sesuai dengan model kami, dalam beberapa kes, anda mungkin harus membalikkan nilai RateAxz kerana kedudukan fizikal giroskop berbanding dengan pecutan - lakukan lagi ujian di atas, memutar peranti di sekitar paksi Y, kali ini memantau output X akselerometer (AdcRx dalam model kami). Sekiranya AdcRx tumbuh (putaran 90 darjah pertama dari kedudukan mendatar), maka AdcGyroXZ juga harus tumbuh. Jika tidak, anda perlu membalikkan RateAxz, anda boleh mencapainya dengan memperkenalkan faktor tanda dalam Persamaan.3, seperti berikut:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensitiviti, di mana InvertAxz adalah 1 atau -1

tebu ujian yang sama dapat dilakukan untuk RateAyz, dengan memutar peranti di sekitar paksi X, dan anda dapat mengenal pasti output giroskop mana yang sesuai dengan RateAyz, dan jika perlu dibalikkan. Sebaik sahaja anda mempunyai nilai untuk InvertAyz, anda harus menggunakan formula berikut untuk mengira RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Sensitiviti

Sekiranya anda melakukan ujian ini di papan Acc_Gyro, anda akan mendapat keputusan berikut:

- pin output untuk RateAxz adalah GX4 dan InvertAxz = -1. - pin output untuk RateAyz adalah GY4 dan InvertAyz = -1

Mulai saat ini kami akan mempertimbangkan bahawa anda telah menyediakan IMU anda sedemikian rupa sehingga anda dapat mengira nilai yang betul untuk Axr, Ayr, Azr (seperti yang ditentukan Bahagian 1. Accelerometer) dan RateAxz, RateAyz (seperti yang ditentukan dalam Bahagian 2. Giroskop). Seterusnya kita akan menganalisis hubungan antara nilai-nilai ini yang ternyata berguna dalam memperoleh anggaran yang lebih tepat mengenai kecenderungan peranti berbanding dengan permukaan tanah.

Anda mungkin bertanya pada diri sendiri sejauh ini, jika model akselerometer sudah memberi kita sudut kecenderungan Axr, Ayr, Azr mengapa kita ingin bersusah payah dengan data giroskop? Jawapannya mudah: data pecutan tidak boleh dipercayai 100%. Terdapat beberapa sebab, ingat bahawa akselerometer mengukur daya inersia, daya seperti itu dapat disebabkan oleh gravitasi (dan idealnya hanya dengan gravitasi), tetapi mungkin juga disebabkan oleh percepatan (pergerakan) peranti. Akibatnya walaupun akselerometer berada dalam keadaan yang relatif stabil, ia masih sangat sensitif terhadap getaran dan bunyi mekanikal secara umum. Inilah sebab utama mengapa kebanyakan sistem IMU menggunakan giroskop untuk menyelesaikan sebarang kesalahan pecutan. Tetapi bagaimana ini dilakukan? Dan adakah giroskop bebas dari bunyi bising?

Giroskop tidak bebas dari kebisingan namun kerana mengukur putaran ia kurang sensitif terhadap pergerakan mekanikal linier, jenis kebisingan yang dialami oleh akselerometer, namun giroskop mempunyai jenis masalah lain seperti misalnya melayang (tidak kembali ke nilai kadar sifar apabila putaran berhenti). Walaupun demikian dengan rata-rata data yang berasal dari akselerometer dan giroskop, kita dapat memperoleh perkiraan kecenderungan peranti saat ini yang lebih baik daripada yang akan kita perolehi dengan menggunakan data akselerometer saja.

Pada langkah seterusnya saya akan memperkenalkan algoritma yang diilhamkan oleh beberapa idea yang digunakan dalam saringan Kalman, namun jauh lebih mudah dan lebih mudah dilaksanakan pada peranti tertanam. Sebelum itu mari kita lihat terlebih dahulu apa yang ingin dikira oleh algoritma kita. Nah, ini adalah arah vektor daya graviti R = [Rx, Ry, Rz] dari mana kita dapat memperoleh nilai lain seperti Axr, Ayr, Azr atau cosX, cozy, cosZ yang akan memberi kita idea tentang kecenderungan peranti kita relatif dengan satah darat, kita membincangkan hubungan antara nilai-nilai ini di Bahagian 1. Seseorang mungkin mengatakan - tidakkah kita sudah mempunyai nilai-nilai ini Rx, Ry, Rz dari Persamaan.2 di Bahagian 1? Baiklah, tetapi ingat bahawa nilai-nilai ini berasal dari data akselerometer saja, jadi jika anda menggunakannya secara langsung dalam aplikasi anda, anda mungkin mendapat lebih banyak suara daripada yang dapat ditoleransi oleh aplikasi anda. Untuk mengelakkan kekeliruan lebih lanjut mari kita tentukan semula ukuran pecutan seperti berikut:

Racc - adalah vektor daya inersia yang diukur oleh pecutan, yang terdiri daripada komponen berikut (unjuran pada paksi X, Y, Z):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / Sensitiviti RyAcc = (AdcRy * Vref / 1023 - VzeroG) / Sensitiviti RzAcc = (AdcRz * Vref / 1023 - VzeroG) / Sensitiviti

Sejauh ini kita mempunyai sekumpulan nilai yang diukur yang dapat kita perolehi semata-mata dari nilai ADC akselerometer. Kami akan memanggil kumpulan data ini sebagai "vektor" dan kami akan menggunakan notasi berikut.

Racc = [RxAcc, RyAcc, RzAcc]

Kerana komponen Racc ini dapat diperoleh dari data accelerometer, kita dapat menganggapnya sebagai input untuk algoritma kita.

Harap maklum bahawa kerana Racc mengukur daya tarikan graviti, anda akan betul jika anda menganggap bahawa panjang vektor ini ditakrifkan sebagai berikut adalah sama atau hampir dengan 1g.

| Racc | = SQRT (RxAcc ^ 2 + RyAcc ^ 2 + RzAcc ^ 2), Namun untuk memastikan masuk akal untuk mengemas kini vektor ini seperti berikut:

Racc (dinormalisasi) = [RxAcc / | Racc |, RyAcc / | Racc |, RzAcc / | Racc |].

Ini akan memastikan panjang vektor Racc yang dinormalisasi anda sentiasa 1.

Seterusnya kami akan memperkenalkan vektor baru dan kami akan memanggilnya

Rehat = [RxEst, RyEst, RzEst]

Ini akan menjadi output algoritma kami, ini adalah nilai yang diperbetulkan berdasarkan data giroskop dan berdasarkan data anggaran yang lalu.

Inilah yang akan dilakukan oleh algoritma kami: - accelerometer memberitahu kami: "Anda sekarang berada di posisi Racc" - kami mengucapkan "Terima kasih, tapi izinkan saya memeriksa", - kemudian betulkan maklumat ini dengan data giroskop serta dengan data Rehat yang lalu dan kami mengeluarkan sisa vektor anggaran. - kami menganggap Rehat sebagai "pertaruhan terbaik" kami mengenai kedudukan peranti semasa.

Mari lihat bagaimana kita dapat menjadikannya berfungsi.

Kami akan memulakan urutan kami dengan mempercayai accelerometer kami dan menetapkan:

Rehat (0) = Racc (0)

Ingatlah Rest dan Racc adalah vektor, jadi persamaan di atas hanyalah cara mudah untuk menulis 3 set persamaan, dan mengelakkan pengulangan:

RxEst (0) = RxAcc (0) RyEst (0) = RyAcc (0) RzEst (0) = RzAcc (0)

Seterusnya kami akan melakukan pengukuran biasa pada selang waktu yang sama T saat, dan kami akan memperoleh pengukuran baru yang akan kami tetapkan sebagai Racc (1), Racc (2), Racc (3) dan sebagainya. Kami juga akan mengeluarkan anggaran baru pada setiap selang waktu Rehat (1), Rehat (2), Rehat (3) dan sebagainya.

Katakan kita berada di langkah n. Kami mempunyai dua set nilai yang diketahui yang ingin kami gunakan:

Rehat (n-1) - anggaran kami sebelumnya, dengan Rehat (0) = Racc (0) Racc (n) - pengukuran pecutan semasa kami

Sebelum kita dapat mengira Rest (n), mari kita memperkenalkan nilai yang baru diukur, yang dapat kita peroleh dari giroskop kita dan anggaran sebelumnya.

Kami akan memanggilnya Rgyro, dan ia juga vektor yang terdiri daripada 3 komponen:

Rgyro = [RxGyro, RyGyro, RzGyro]

Kami akan mengira vektor ini satu komponen pada satu masa. Kami akan mulakan dengan RxGyro.

Mari mulakan dengan memerhatikan hubungan berikut dalam model giroskop kami, dari segitiga sudut kanan yang dibentuk oleh Rz dan Rxz kita dapat memperoleh bahawa:

tan (Axz) = Rx / Rz => Axz = atan2 (Rx, Rz)

Atan2 mungkin merupakan fungsi yang tidak pernah Anda gunakan sebelumnya, mirip dengan atan, kecuali ia mengembalikan nilai dalam julat (-PI, PI) yang bertentangan dengan (-PI / 2, PI / 2) seperti yang dikembalikan oleh atan, dan diperlukan 2 hujah dan bukannya satu. Ini membolehkan kita menukar dua nilai Rx, Rz ke sudut dalam julat penuh 360 darjah (-PI hingga PI). Anda boleh membaca lebih lanjut mengenai atan2 di sini.

Oleh itu, dengan mengetahui RxEst (n-1), dan RzEst (n-1) kita dapat menjumpai:

Axz (n-1) = atan2 (RxEst (n-1), RzEst (n-1)).

Ingat bahawa giroskop mengukur kadar perubahan sudut Axz. Oleh itu, kita dapat menganggarkan sudut baru Axz (n) seperti berikut:

Axz (n) = Axz (n-1) + RateAxz (n) * T

Ingat bahawa RateAxz boleh didapati dari bacaan ADC giroskop kami. Formula yang lebih tepat dapat menggunakan kadar putaran rata-rata yang dikira seperti berikut:

RateAxzAvg = (RateAxz (n) + RateAxz (n-1)) / 2 Axz (n) = Axz (n-1) + RateAxzAvg * T

Dengan cara yang sama kita dapati:

Ayz (n) = Ayz (n-1) + RateAyz (n) * T

Ok jadi sekarang kita mempunyai Axz (n) dan Ayz (n). Dari mana kita pergi dari sini untuk memotong RxGyro / RyGyro? Dari Persamaan. 1 kita boleh menulis panjang vektor Rgyro seperti berikut:

| Rgyro | = SQRT (RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2)

Juga kerana kami menormalkan vektor Racc kami, kami mungkin menganggap bahawa panjangnya adalah 1 dan ia tidak berubah setelah putaran, jadi agak selamat untuk menulis:

| Rgyro | = 1

Mari kita gunakan notasi yang lebih pendek sementara untuk pengiraan di bawah:

x = RxGyro, y = RyGyro, z = RzGyro

Dengan menggunakan hubungan di atas, kita dapat menulis:

x = x / 1 = x / SQRT (x ^ 2 + y ^ 2 + z ^ 2)

Mari bahagikan pembilang dan penyebut pecahan dengan SQRT (x ^ 2 + z ^ 2)

x = (x / SQRT (x ^ 2 + z ^ 2)) / SQRT ((x ^ 2 + y ^ 2 + z ^ 2) / (x ^ 2 + z ^ 2))

Perhatikan bahawa x / SQRT (x ^ 2 + z ^ 2) = sin (Axz), jadi:

x = sin (Axz) / SQRT (1 + y ^ 2 / (x ^ 2 + z ^ 2))

Sekarang kalikan pembilang dan penyebut pecahan di dalam SQRT dengan z ^ 2

x = sin (Axz) / SQRT (1 + y ^ 2 * z ^ 2 / (z ^ 2 * (x ^ 2 + z ^ 2)))

Perhatikan bahawa z / SQRT (x ^ 2 + z ^ 2) = cos (Axz) dan y / z = tan (Ayz), akhirnya:

x = sin (Axz) / SQRT (1 + cos (Axz) ^ 2 * tan (Ayz) ^ 2)

Kembali ke notasi kami, kami mendapat:

RxGyro = sin (Axz (n)) / SQRT (1 + cos (Axz (n)) ^ 2 * tan (Ayz (n)) ^ 2)

dengan cara yang sama kita dapati

RyGyro = sin (Ayz (n)) / SQRT (1 + cos (Ayz (n)) ^ 2 * tan (Axz (n)) ^ 2)

Sekarang, akhirnya kita dapat menemui:

RzGyro = Tanda (RzGyro) * SQRT (1 - RxGyro ^ 2 - RyGyro ^ 2).

Di mana Tanda (RzGyro) = 1 ketika RzGyro> = 0, dan Tanda (RzGyro) = -1 ketika RzGyro <0.

Satu kaedah mudah untuk mengira ini adalah dengan mengambil:

Tanda (RzGyro) = Tanda (RzEst (n-1))

Dalam praktiknya berhati-hati ketika RzEst (n-1) mendekati 0. Anda boleh melewati fasa giro sama sekali dalam kes ini dan menetapkan: Rgyro = Rehat (n-1). Rz digunakan sebagai rujukan untuk mengira sudut Axz dan Ayz dan ketika mendekati 0, nilai dapat mengalir dan memicu hasil buruk. Anda akan berada dalam domain bilangan titik terapung yang besar di mana pelaksanaan fungsi tan () / atan () mungkin kurang tepat.

Oleh itu, mari kita susun semula apa yang kita ada setakat ini, kita berada di tahap n algoritma kita dan kita telah mengira nilai berikut:

Racc - bacaan semasa dari accelerometer Rgyro kami - diperoleh dari Rest (n-1) dan bacaan giroskop semasa

Nilai mana yang kita gunakan untuk mengira anggaran yang dikemas kini Selebihnya (n)? Anda mungkin menduga bahawa kami akan menggunakan kedua-duanya. Kami akan menggunakan purata berwajaran, sehingga:

Rehat (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Kita boleh mempermudah formula ini dengan membahagi pengangka dan penyebut pecahan dengan w1.

Rehat (n) = (Racc * w1 / w1 + Rgyro * w2 / w1) / (w1 / w1 + w2 / w1)

dan setelah menggantikan w2 / w1 = wGyro kita mendapat:

Rehat (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

Dalam forumula di atas, wGyro memberitahu kita betapa kita mempercayai gyro kita berbanding dengan accelerometer kita. Nilai ini dapat dipilih secara eksperimen biasanya nilai antara 5..20 akan mencetuskan hasil yang baik.

Perbezaan utama algoritma ini dari penapis Kalman adalah bahawa berat ini relatif tetap, sedangkan dalam penapis Kalman bobot diperbaharui secara kekal berdasarkan kebisingan yang diukur dari pembacaan pecutan. Penapis Kalman difokuskan untuk memberi anda hasil teoritis "terbaik", sedangkan algoritma ini dapat memberikan hasil "cukup baik" untuk aplikasi praktikal anda. Anda boleh menerapkan algoritma yang menyesuaikan wGyro bergantung pada beberapa faktor kebisingan yang anda ukur, tetapi nilai tetap akan berfungsi dengan baik untuk kebanyakan aplikasi.

Kami selangkah lagi untuk mendapatkan anggaran nilai yang dikemas kini:

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 semula vektor ini:

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

Dan kami bersedia untuk mengulangi gelung kami sekali lagi.

Panduan ini mula-mula muncul di starlino.com, saya telah membuat beberapa suntingan ringan dan menyiarkannya semula dengan izin. Terima kasih Starlino!