Accelerometer və Gyro Tutorial: 3 addım

2024 Müəllif: John Day | [email protected]. Son dəyişdirildi: 2024-01-30 07:49

Giriş

Bu təlimat, elektronika layihələrində Accelerometers və Gyroscopes ilə birlikdə IMU cihazlarının (Atalet Ölçmə Birliyi) istifadəsi ilə maraqlanan hər kəs üçün nəzərdə tutulmuşdur.

Biz əhatə edəcəyik:

• Bir akselerometr nəyi ölçür?
• Bir giroskop (aka giro) nəyi ölçür?
• Bu sensordan əldə etdiyiniz analoq-rəqəmsal (ADC) oxunuşlarını fiziki vahidlərə necə çevirmək olar (bunlar akselerometr üçün g, giroskop üçün deg/s olardı)
• Cihazınızın yer səthinə nisbətən meyli haqqında dəqiq məlumat əldə etmək üçün akselerometr və giroskop oxunuşlarını necə birləşdirmək olar

Məqalə ərzində riyaziyyatı minimuma endirməyə çalışacağam. Sine/Cosine/Tangent nə olduğunu bilirsinizsə, hansı platformadan istifadə etməyinizdən asılı olmayaraq bu fikirləri başa düşüb layihənizdə istifadə etməlisiniz: Arduino, Pervane, Əsas Damğa, Atmel çipləri, Microchip PIC və s.

IMU vahidindən istifadə etmək üçün kompleks riyaziyyata ehtiyacınız olduğuna inanan insanlar var (Kalman filtrləri, Parks-McClellan filtrləri və s. Kimi kompleks FIR və ya IIR filtrləri). Bütün bunları araşdırıb gözəl, lakin kompleks nəticələr əldə edə bilərsiniz. Şeyləri izah etmək üsulum yalnız sadə riyazi tələb edir. Mən sadəliyə böyük inanıram. Düşünürəm ki, sadə bir sistemin idarə edilməsi və izlənməsi daha asandır, bir çox gömülü cihazın matris hesablamaları tələb edən kompleks alqoritmləri tətbiq etmək üçün gücü və mənbələri yoxdur.

Nümunə olaraq yeni bir IMU vahidi olan Acc_Gyro Accelerometer + Gyro IMU istifadə edəcəyəm. Aşağıdakı nümunələrdə bu cihazın parametrlərindən istifadə edəcəyik. Bu cihaz 2 cihazdan ibarət olduğu üçün başlamaq üçün yaxşı bir cihazdır:

- LIS331AL (məlumat vərəqi) - üç eksenli 2G akselerometr - LPR550AL (məlumat vərəqi) - cüt oxlu addım və yuvarlanma, 500 dərəcə/san giroskop

Birlikdə 5 dərəcə Azadlıq İnertial Ölçmə Birimini təmsil edirlər. İndi bu möhtəşəm bir ad! Buna baxmayaraq, xülya adının arxasında bu təlimatda ətraflı şəkildə izah edəcəyimiz çox faydalı bir birləşmə cihazı dayanır.

Addım 1: Akselerometr

Bu vahidi başa düşmək üçün akselerometrdən başlayaq. Akselerometrlər haqqında düşünərkən, içərisində top olan bir kub şəklində bir qutu təsvir etmək çox vaxt faydalıdır. Çerez və ya çörek kimi başqa bir şey təsəvvür edə bilərsiniz, amma bir top təsəvvür edirəm:

Bu qutunu cazibə sahələri olmayan və ya topun mövqeyini təsir edə biləcək başqa sahələri olmayan bir yerə götürsək - top sadəcə qutunun ortasında üzəcək. Qutunun hər hansı bir kosmik cisimdən çox uzaq bir kosmosda olduğunu təsəvvür edə bilərsiniz və ya belə bir yer tapmaq çətindirsə, heç olmasa hər şeyin çəkisiz vəziyyətdə olduğu planetin ətrafında fırlanan bir kosmik gəmi təsəvvür edin. Yuxarıdakı şəkildən görə bilərsiniz ki, hər oxa bir cüt divar təyin edirik (qutunun içərisinə baxmaq üçün Y+ divarını çıxardıq). Hər divarın təzyiqə həssas olduğunu düşünün. Qutunu birdən sola hərəkət etdirsək (1g = 9.8m/s^2 sürətlə sürətləndirərik), top X- divarına dəyəcək. Sonra topun divara tətbiq etdiyi təzyiq gücünü ölçürük və X oxunda -1g dəyərini çıxarırıq.

Unutmayın ki, akselerometr əslində sürət vektorundan əks istiqamətə yönəldilmiş bir qüvvəni aşkar edəcək. Bu qüvvəyə tez -tez Atalet Gücü və ya Xəyali Güc deyilir. Bundan öyrənməyiniz lazım olan bir şey, bir akselerometrin divarlarından birinə tətbiq olunan bir qüvvə ilə dolayısı ilə sürətlənməni ölçməsidir (modelimizə görə, real həyatda olan akselerometrlərdə bir yay və ya başqa bir şey ola bilər). Bu qüvvə sürətlənmədən qaynaqlana bilər, lakin növbəti nümunədə görəcəyimiz kimi, həmişə sürətlənmədən qaynaqlanmır.

Modelimizi götürüb Yerə qoyduqda, top Z-divarına düşəcək və aşağıdakı şəkildə göstərildiyi kimi alt divara 1 q qüvvə tətbiq edəcək:

Bu halda qutu yerindən tərpənmir, amma yenə də Z oxunda -1g oxuyuruq. Topun divara vurduğu təzyiq cazibə qüvvəsindən qaynaqlanır. Teorik olaraq fərqli bir qüvvə növü ola bilər - məsələn, topumuzun metal olduğunu təsəvvür edirsinizsə, qutunun yanına bir maqnit yerləşdirmək topu hərəkət etdirə bilər və başqa divara dəyəcək. Bu, mahiyyətcə, akselerometrin ölçülərin sürətlənməni deyil, güclədiyini sübut etmək üçün deyildi. Sadəcə olur ki, sürətlənmə, akselerometrin qüvvə aşkarlama mexanizmi tərəfindən tutulan bir ətalət qüvvəsinə səbəb olur.

Bu model bir MEMS sensorunun tam olaraq necə qurulmadığına baxmayaraq, tez -tez akselerometrlə bağlı problemlərin həllində faydalıdır. İçərisində metal topları olan oxşar sensorlar var, onlara əyilmə açarları deyilir, lakin daha primitivdirlər və ümumiyyətlə cihazın meyl dərəcəsini deyil, müəyyən bir məsafədə meylli olub -olmadığını anlaya bilərlər.

İndiyə qədər bir oxdakı akselerometr çıxışını təhlil etdik və tək eksenli akselerometrlərlə əldə edəcəyiniz şey budur. Üç eksenli akselerometrlərin əsl dəyəri, hər üç oxda ətalət qüvvələrini aşkar edə bilmələridir. Qutu modelimizə qayıdaq və qutunu 45 dərəcə sağa döndərək. Top indi 2 divara toxunacaq: Z- və X- aşağıdakı şəkildə göstərildiyi kimi:

0.71 dəyərləri ixtiyari deyil, əslində SQRT (1/2) üçün təxmini bir dəyərdir. Növbəti akselerometr modelimizi təqdim etdikcə bu daha aydın olacaq.

Əvvəlki modeldə cazibə qüvvəsini təyin etdik və xəyali qutumuzu döndərdik. Son 2 nümunədə, çıxışı 2 fərqli qutu mövqeyində təhlil etdik, güc vektoru sabit qaldı. Bu, akselerometrin xarici qüvvələrlə necə qarşılıqlı əlaqədə olduğunu anlamaqda faydalı olsa da, koordinat sistemini akselerometrin oxlarına bağlasaq və qüvvə vektorunun ətrafımızda fırlandığını təsəvvür etsək, hesablamalar aparmaq daha praktikdir.

Zəhmət olmasa yuxarıdakı modelə baxın, baltaların rənglərini qorudum ki, əvvəlki modeldən yenisinə zehni bir keçid edə biləsiniz. Təsəvvür edin ki, yeni modeldəki hər bir ox əvvəlki modeldəki qutunun müvafiq üzlərinə dikdir. R vektoru, akselerometrin ölçdüyü güc vektorudur (ya cazibə qüvvəsi, ya da yuxarıdakı nümunələrdən gələn ətalət qüvvəsi və ya hər ikisinin birləşməsi ola bilər). Rx, Ry, Rz, R vektorunun X, Y, Z oxlarında proyeksiyasıdır. Zəhmət olmasa aşağıdakı əlaqəyə diqqət yetirin:

R^2 = Rx^2 + Ry^2 + Rz^2 (1 -ci bərabərlik)

bu, əsasən 3D formatında Pifaqor teoreminə bərabərdir.

Unutmayın ki, bir az əvvəl sizə dedim ki, SQRT (1/2) ~ 0.71 dəyərləri təsadüfi deyil. Onları yuxarıdakı düstura bağlasanız, cazibə qüvvəmizin 1 g olduğunu xatırladıqdan sonra bunu təsdiq edə bilərik:

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

sadəcə R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) Denk.1 -də əvəz etməklə

Uzun bir nəzəriyyə girişindən sonra real həyat sürətləndiricilərinə yaxınlaşırıq. Rx, Ry, Rz dəyərləri əslində real həyat sürətləndiricinizin çıxış edəcəyi və müxtəlif hesablamalar aparmaq üçün istifadə edə biləcəyiniz dəyərlərlə doğrudur.

Oraya getməzdən əvvəl, akselerometrlərin bu məlumatları bizə necə çatdıracağı haqqında bir az danışaq. Əksər akselerometrlər iki kateqoriyaya bölünəcək: rəqəmsal və analoq. Rəqəmsal sürətölçənlər sizə I2C, SPI və ya USART kimi ardıcıl protokoldan istifadə edərək məlumat verəcək, analoq akselerometrlər isə ADC (analoqdan rəqəmsal çeviriciyə) modulundan istifadə edərək rəqəmsal dəyərə çevirmək məcburiyyətində olduğunuz əvvəlcədən təyin edilmiş diapazonda bir gərginlik səviyyəsi çıxaracaq. ADC -nin necə işlədiyini bir qədər ətraflı izah etməyəcəyəm, qismən bu qədər geniş bir mövzu olduğu üçün, bir tərəfdən də bir platformadan digərinə fərqli olduğu üçün. Bəzi mikrokontrolörlərin daxili ADC modulları olacaq, bəzilərinin ADC çevrilməsini həyata keçirmək üçün xarici komponentlərə ehtiyacı olacaq. Hansı növ ADC modulundan istifadə etməyinizdən asılı olmayaraq, müəyyən bir diapazonda dəyər əldə edəcəksiniz. Məsələn, 10 bitlik ADC modulu 0..1023 aralığında bir dəyər çıxaracaq, unutmayın ki, 1023 = 2^10 -1. 12 bitlik ADC modulu 0..4095 aralığında bir dəyər çıxaracaq, unutmayın ki, 4095 = 2^12-1.

Sadə bir nümunəni nəzərdən keçirərək davam edək, tutaq ki, 10 bitlik ADC modulumuz bizə üç akselerometr kanalı (balta) üçün aşağıdakı dəyərləri verdi:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Hər bir ADC modulunda bir referans gərginliyi olacaq, nümunəmizdə 3.3V olduğunu düşünək. 10 bitlik bir adc dəyərini gərginliyə çevirmək üçün aşağıdakı düsturdan istifadə edirik:

VoltsRx = AdcRx * Vref / 1023

Burada qısa bir qeyd: 8 bit ADC üçün son bölücünün 255 = 2 ^ 8 -1, 12 bitlik ADC üçün son bölücünün 4095 = 2 ^ 12 -1 olacağı.

Bu formulu bütün 3 kanala tətbiq edərək əldə edirik:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (bütün nəticələri 2 onluq nöqtəyə yuvarlaqlaşdırırıq) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Hər bir akselerometr sıfır-g gərginlik səviyyəsinə malikdir, onu spesifikasiyalarda tapa bilərsiniz, bu 0 q-a uyğun gələn gərginlikdir. İmzalı bir gərginlik dəyəri əldə etmək üçün bu səviyyədən sürüşməni hesablamalıyıq. Deyək ki, 0g gərginlik səviyyəmiz VzeroG = 1.65V -dir. Gərginlik sıfır-g gərginlikdən aşağıdakı kimi hesablanır:

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

İndi sürətölçən oxumalarımız Voltda, hələ də g (9.8 m/s^2) deyil, son dönüşümü etmək üçün ümumiyyətlə mV/g ilə ifadə olunan akselerometr həssaslığını tətbiq edirik. Deyək ki, Həssaslığımız = 478.5mV/g = 0.4785V/g. Həssaslıq dəyərlərini akselerometr xüsusiyyətlərində tapa bilərsiniz. G ilə ifadə olunan son qüvvə dəyərlərini əldə etmək üçün aşağıdakı düsturdan istifadə edirik:

Rx = DeltaVoltsRx / Həssaslıq

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

Əlbəttə ki, bütün addımları bir düsturda birləşdirə bilərik, amma ADC oxunuşlarından g ilə ifadə olunan güc vektoru komponentinə necə keçdiyinizi aydınlaşdırmaq üçün bütün addımları keçdim.

Rx = (AdcRx * Vref / 1023 - VzeroG) / Həssaslıq (Eq.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Həssaslıq Rz = (AdcRz * Vref / 1023 - VzeroG) / Həssaslıq

İndi inertial qüvvə vektorumuzu təyin edən bütün 3 komponentə sahibik, əgər cihaz cazibə qüvvəsindən başqa başqa qüvvələrə tabe deyilsə, bunun cazibə qüvvəsi vektorumuzun istiqamətini güman edə bilərik. Cihazın yerə nisbətən meylini hesablamaq istəyirsinizsə, bu vektor və Z oxu arasındakı bucağı hesablaya bilərsiniz. Əgər hər oxun meyl istiqaməti ilə maraqlanırsınızsa, bu nəticəni 2 komponentə ayıra bilərsiniz: X və Y oxundakı meyl, cazibə vektoru ilə X / Y oxları arasındakı bucaq olaraq hesablana bilər. Bu açıları hesablamaq, düşündüyünüzdən daha sadədir, indi Rx, Ry və Rz üçün dəyərləri hesabladıq. Son akselerometr modelimizə qayıdaq və əlavə qeydlər edək:

Maraqlandığımız açılar X, Y, Z oxları ilə R vektor qüvvələri arasındakı açılardır. Bu bucaqları Axr, Ayr, Azr olaraq təyin edəcəyik. R və Rx tərəfindən düzbucaqlı üçbucaqdan görə bilərsiniz:

cos (Axr) = Rx / R və buna bənzər: cos (Ayr) = Ry / R cos (Azr) = Rz / R

1 -dən R = SQRT (Rx^2 + Ry^2 + Rz^2) olduğunu çıxara bilərik.

İndi arccos () funksiyasından (tərs cos () funksiyasından) istifadə edərək bucaqlarımızı tapa bilərik:

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

Akselerometr modelini izah etmək üçün uzun bir yol qət etdik, yalnız bu düsturlara gəlmək üçün. Tətbiqlərinizdən asılı olaraq əldə etdiyimiz hər hansı bir aralıq düsturdan istifadə etmək istəyə bilərsiniz. Jiroskop modelini də tezliklə təqdim edəcəyik və daha dəqiq meyl təxminləri təmin etmək üçün akselerometr və giroskop məlumatlarının necə birləşdiriləcəyini görəcəyik.

Ancaq bunu etməzdən əvvəl daha faydalı qeydlər edək:

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

Bu üçlüyə tez -tez İstiqamət Kosinosu deyilir və əsasən R vektorumuzla eyni istiqamətə malik olan vahid vektoru (uzunluğu 1 olan vektor) təmsil edir. Bunu asanlıqla yoxlaya bilərsiniz:

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

Bu, R vektorunun modulunu (uzunluğunu) izləməkdən azad etdiyimiz üçün gözəl bir xüsusiyyətdir. Əksər hallarda inertial vektorumuzun istiqaməti ilə maraqlanırıqsa, digər hesablamaları asanlaşdırmaq üçün modulunu normallaşdırmağın mənası var.

Addım 2: Giroskop

Giroskop üçün akselerometrdə olduğu kimi heç bir ekvivalent qutu modeli təqdim etməyəcəyik, bunun əvəzinə düz ikinci akselerometr modelinə keçəcəyik və bu modelə görə giroskopun nə ölçdüyünü göstərəcəyik.

Hər bir giroskop kanalı, oxlardan birinin ətrafında fırlanmanı ölçür. Məsələn, 2 oxlu bir giroskop X və Y oxlarının ətrafında fırlanmanı ölçəcək (və ya bəziləri "haqqında" deyə bilər). Bu fırlanmanı rəqəmlərlə ifadə etmək üçün bəzi qeydlər edək. Əvvəlcə təyin edək:

Rxz - inertial qüvvə vektorunun R XZ müstəvisindəki proyeksiyasıdır Ryz - inertial qüvvə vektorunun YZ müstəvisində proyeksiyasıdır.

Pxagor teoremindən istifadə edərək Rxz və Rz tərəfindən düzbucaqlı üçbucaqdan əldə edirik:

Rxz^2 = Rx^2 + Rz^2 və buna bənzər: Ryz^2 = Ry^2 + Rz^2

onu da qeyd edin:

R^2 = Rxz^2 + Ry^2, bu Eq.1 və yuxarıdakı tənliklərdən və ya R və Ryz R^2 = Ryz^2 + Rx^2 ilə düzbucaqlı üçbucaqdan əldə edilə bilər.

Bu yazıda bu düsturlardan istifadə etməyəcəyik, ancaq modelimizdəki bütün dəyərlər arasındakı əlaqəni qeyd etmək faydalıdır.

Bunun əvəzinə Z oxu ilə Rxz, Ryz vektorları arasındakı bucağı aşağıdakı kimi təyin edəcəyik:

Axz - Rxz (XZ müstəvisində R proyeksiyası) və Z oxu Ayz arasındakı bucaq - Ryz (YZ müstəvisində R proyeksiyası) ilə Z oxu arasındakı bucaqdır

İndi jiroskopun ölçdüyünə yaxınlaşırıq. Giroskop yuxarıda göstərilən açıların dəyişmə sürətini ölçür. Başqa sözlə, bu açıların dəyişmə sürəti ilə xətti əlaqəli bir dəyər çıxaracaq. Bunu izah etmək üçün t oxunda Y oxu ətrafında (bu Axz bucağı olardı) fırlanma bucağını ölçdüyümüzü və Axz0 olaraq təyin etdiyimizi, daha sonra t1 zamanında bu açını ölçdüyümüzü və Axz1 olduğunu fərz edək. Dəyişiklik dərəcəsi aşağıdakı kimi hesablanacaq:

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

Axzı dərəcələrlə, vaxtı isə saniyələrlə ifadə etsək, bu dəyər deg/s ilə ifadə olunacaq. Bir giroskopun ölçdüyü budur.

Praktikada bir giroskop (xüsusi bir rəqəmsal giroskop olmadığı təqdirdə) nadir hallarda deg/s ilə ifadə olunan bir dəyər verəcəkdir. Eynilə oxşar bir düsturdan istifadə edərək deg/s -ə çevirməyiniz lazım olan bir ADC dəyərini alacaqsınız. 2, akselerometr üçün təyin etdiyimiz. ADC -ni giroskop üçün deg/s dönüşüm formuluna təqdim edək (10 bitlik ADC modulu istifadə etdiyimizi güman edirik, 8 bit ADC üçün 1023 -ü 255 ilə, 12 bit ADC üçün 1023 -ü 4095 ilə əvəz edirik).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Həssaslıq Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / Həssaslıq

AdcGyroXZ, AdcGyroYZ - adc modulumuzdan alınır və YZ təyyarələrində XZ -də R vektorunun proyeksiyasının müvafiq olaraq Y və X oxları ətrafında fırlandığını deyənə bərabər olan rotasiyanı ölçən kanalları təmsil edir.

Vref - VzeroRate -in altındakı nümunədə 3.3V istifadə edəcəyimiz ADC istinad gərginliyidir - sıfır dərəcəli gərginlikdir, başqa sözlə, giroskopun heç bir fırlanmaya məruz qalmadığı zaman çıxardığı gərginlikdir, Acc_Gyro lövhəsi üçün məsələn 1.23V (spesifikasiyalarda bu dəyərləri tapa bilərsiniz) Həssaslıq - giroskopunuzun həssaslığıdır, mV / deg / s olaraq ifadə olunur, mV / deg / s olaraq yazılır, əsasən sizə neçə mV olacağını bildirir fırlanma sürətini bir deg/s artırsanız, jiroskopun çıxışı artır. Acc_Gyro lövhəsinin həssaslığı məsələn 2mV/deg/s və ya 0.002V/deg/s -dir

Bir nümunə götürək, fərz edək ki, ADC modulumuz aşağıdakı dəyərləri qaytardı:

AdcGyroXZ = 571 AdcGyroXZ = 323

Yuxarıdakı düsturdan istifadə edərək və Acc_Gyro lövhəsinin xüsusiyyətlərindən istifadə edərək əldə edəcəyik:

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 dərəcə/s

Başqa sözlə, cihaz Y oxu ətrafında (və ya XZ müstəvisində döndüyünü deyə bilərik) 306 dərəcə/s sürətlə və X oxu ətrafında (və ya YZ müstəvisində döndüyünü deyə bilərik) sürətlə - 94 dərəcə/s. Diqqət yetirin ki, mənfi işarə cihazın şərti müsbət istiqamətdən əks istiqamətdə döndüyünü bildirir. Konvensiyaya görə bir fırlanma istiqaməti müsbətdir. Yaxşı bir giroskop spesifikasiyası vərəqi sizə hansı istiqamətin müsbət olduğunu göstərəcək, əks halda cihazla təcrübə edərək və fırlanma istiqamətinin çıxış pinindəki gərginliyin artmasına səbəb olduğunu qeyd edərək tapmalı olacaqsınız. Bu ən yaxşı bir osiloskop istifadə etməklə edilir, çünki fırlanmanı dayandıran kimi gərginlik sıfır dərəcəsi səviyyəsinə enəcək. Bir multimetr istifadə edirsinizsə, ən azı bir neçə saniyə sabit bir fırlanma sürətini saxlamalı və bu fırlanma zamanı gərginliyi qeyd etməlisiniz, sonra sıfır dərəcəli gərginliklə müqayisə edin. Sıfır dərəcəli gərginlikdən böyükdürsə, bu, fırlanma istiqamətinin müsbət olduğunu göstərir.

Addım 3: Accelerometer və Gyro -nu birləşdirin

Hamısını bir araya gətirmək - akselerometr və giroskop məlumatlarını birləşdirmək

Bu məqaləni oxuyursanız, ehtimal ki, bir IMU cihazı əldə etmisiniz və ya əldə etməyi planlaşdırırsınız və ya ehtimal ki, ayrı bir akselerometr və giroskop cihazlarından bir qurma hazırlayırsınız.

Bir akselerometr və bir giroskopu birləşdirən birləşmiş IMU cihazı istifadə etmənin ilk addımı, koordinat sistemlərini hizalamaqdır. Bunu etmək üçün ən asan yol, referans koordinat sisteminiz olaraq akselerometrin koordinat sistemini seçməkdir. Əksər akselerometr məlumat vərəqlərində fiziki çipin və ya cihazın görüntüsünə nisbətən X, Y, Z oxlarının istiqaməti göstəriləcək. Məsələn, Acc_Gyro lövhəsi üçün spesifikasiyalarda göstərildiyi kimi X, Y, Z oxlarının istiqamətləri:

Növbəti addımlar bunlardır:

Yuxarıda müzakirə olunan RateAxz, RateAyz dəyərlərinə uyğun gyroscope çıxışlarını təyin edin. Jiroskopun akselerometrə nisbətən fiziki mövqeyi səbəbindən bu çıxışların ters çevrilməsinin lazım olub olmadığını müəyyənləşdirin

Bir giroskopun X və ya Y işarəli bir çıxışı varsa, bu çıxış IMU vahidinin bir hissəsi olsa belə, akselerometr koordinat sistemindəki hər hansı bir oxa uyğun olacağını düşünməyin. Ən yaxşı yolu sınamaqdır. Akselerometrə nisbətən giroskopun mövqeyini təyin etdiyinizi düşünsək. Gyro və akselerometr sərhədlərinin bir -birinə paralel olduğu güman edilir, yəni girroları akselerometr çipinə nisbətən 90 dərəcə çox bucaq altında yerləşdirirsiniz. Bir IMU idarə heyəti əldə etmisinizsə, şansları artıq bu şəkildə hizalanmışdır. Bu məqalədə, giroskopun akselerometrə (məsələn, 45 və ya 30 dərəcə) nisbətən düzensiz bir açıda yerləşdirildiyi modelləri müzakirə etməyəcəyik, baxmayaraq ki, bu bəzi tətbiqlərdə faydalı ola bilər.

Hansı giroskopun yuxarıda müzakirə olunan RateAxz dəyərinə uyğun olduğunu müəyyən etmək üçün nümunə bir ardıcıllıqdır.

- cihazı üfüqi vəziyyətdə yerləşdirməklə başlayın. Həm akselerometrin X və Y çıxışları sıfır g gərginliyi çıxaracaq (məsələn, Acc_Gyro lövhəsi üçün bu 1.65V)

- növbəti dəfə cihazı Y oxu ətrafında fırlamağa başlayın, bunu söyləməyin başqa bir yolu, cihazı XZ müstəvisində döndərdiyinizdir ki, X və Z sürətləndiricilərin çıxışları dəyişsin və Y çıxışı sabit qalsın. - Cihazı giroskop çıxışının dəyişdiyi sabit bir sürətlə döndərərkən, digər giroskop çıxışları sabit qalmalıdır - Y oxu ətrafında fırlanma zamanı dəyişən giroskop çıxışı (XZ müstəvisində fırlanma) AdcGyroXZ üçün giriş dəyərini təmin edəcək. RateAxz hesablayırıq - son addım, fırlanma istiqamətinin modelimizə uyğun olmasını təmin etməkdir, bəzi hallarda giroskopun akselerometrə nisbətən fiziki mövqeyi səbəbindən RateAxz dəyərini çevirmək məcburiyyətində qala bilərsiniz - cihazı yuxarıya fırladaraq yuxarıdakı testi təkrar edin. Y oxu, bu dəfə sürətləndiricinin X çıxışını izləyin (modelimizdə AdcRx). AdcRx böyüyərsə (üfüqi mövqedən ilk 90 dərəcə fırlanma), onda AdcGyroXZ də böyüməlidir. Əks təqdirdə, RateAxz -ı tərs çevirməlisiniz, buna bərabər olaraq Eq.3 -də bir işarə faktoru tətbiq edərək nail ola bilərsiniz:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Həssaslıq, burada InvertAxz 1 və ya -1 -dir

Cihazı X oxu ətrafında döndərməklə eyni test çubuğu RateAyz üçün edilir və hansı jiroskop çıxışının RateAyz -a uyğun olduğunu və ters çevrilməli olduğunu müəyyən edə bilərsiniz. InvertAyz üçün bir dəyər əldə etdikdən sonra RateAyz hesablamaq üçün aşağıdakı düsturdan istifadə etməlisiniz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Həssaslıq

Bu testləri Acc_Gyro lövhəsində etsəniz, aşağıdakı nəticələri alacaqsınız:

- RateAxz üçün çıxış pin GX4 və InvertAxz = -1 -dir. - RateAyz üçün çıxış pin GY4 və InvertAyz = -1 -dir

Bu nöqtədən etibarən, IMU'nuzu Axr, Ayr, Azr (hissə 1 -də göstərildiyi kimi) və RateAxz, RateAyz üçün doğru dəyərləri hesablaya biləcəyiniz bir şəkildə qurduğunuzu nəzərə alacağıq. 2 -ci hissədə. Jiroskop). Bundan sonra, cihazın yer səthinə nisbətən meylini daha dəqiq qiymətləndirmək üçün faydalı olan bu dəyərlər arasındakı əlaqələri təhlil edəcəyik.

Bu nöqtədə özünüzə sual verə bilərsiniz, əgər akselerometr modeli bizə Axr, Ayr, Azr meyl açılarını versəydi, niyə giroskop məlumatları ilə narahat olmaq istəyirik? Cavab sadədir: akselerometr məlumatlarına həmişə 100%etibar etmək olmaz. Bir neçə səbəb var, unutmayın ki, akselerometr inertial qüvvəni ölçür, belə bir qüvvə cazibə qüvvəsindən (və ideal olaraq yalnız cazibə qüvvəsindən) yarana bilər, həm də cihazın sürətlənməsindən (hərəkətindən) qaynaqlana bilər. Nəticədə, akselerometr nisbətən sabit vəziyyətdə olsa belə, ümumiyyətlə vibrasiyaya və ümumiyyətlə mexaniki səs -küyə çox həssasdır. IMU sistemlərinin əksəriyyətində akselerometr səhvlərini düzəltmək üçün giroskopdan istifadə edilməsinin əsas səbəbi budur. Amma bu necə edilir? Və giroskop səs -küydən azaddırmı?

Jiroskop səs-küydən azad deyildir, lakin fırlanmanı ölçdüyü üçün xətti mexaniki hərəkətlərə, akselerometrdən əziyyət çəkən səs-küy növünə daha az həssasdır, lakin giroskopların sürüşmə (sıfır dərəcəsi dəyərinə qayıtmamaq) kimi digər problemləri var. fırlanma dayandıqda). Buna baxmayaraq, akselerometr və giroskopdan alınan məlumatları ortalamaqla, təkcə akselerometr məlumatlarını istifadə etməklə əldə edəcəyimizdən daha cari cihazın meylini daha yaxşı qiymətləndirə bilərik.

Növbəti addımlarda Kalman filtrində istifadə olunan bəzi fikirlərdən ilhamlanan bir alqoritmi təqdim edəcəyəm. Bundan əvvəl alqoritmimizin nəyi hesablamaq istədiyini görək. Axr, Axr, Ayr, Azr və ya cosX, cozy, cosZ kimi digər dəyərləri əldə edə biləcəyimiz cazibə qüvvəsi vektoru R = [Rx, Ry, Rz] istiqamətidir ki, bu da cihazımızın meyli haqqında bizə fikir verəcəkdir. yer müstəvisinə nisbətən, bu dəyərlər arasındakı əlaqəni 1 -ci hissədə müzakirə edirik. Deyə bilərik - 1 -ci hissədəki Eq.2 -dən Rx, Ry, Rz dəyərlərinə sahib deyilikmi? Bəli, amma unutmayın ki, bu dəyərlər yalnız akselerometr məlumatlarından alınmışdır, buna görə də bunları birbaşa tətbiqinizdə istifadə etsəniz, tətbiqinizin dözə biləcəyindən daha çox səs -küy əldə edə bilərsiniz. Başqa bir qarışıqlığın qarşısını almaq üçün, akselerometr ölçmələrini aşağıdakı kimi yenidən təyin edək:

Racc - akselerometrlə ölçülən inertial qüvvə vektorudur və aşağıdakı komponentlərdən ibarətdir (X, Y, Z oxlarında proqnozlar):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / Həssaslıq RyAcc = (AdcRy * Vref / 1023 - VzeroG) / Həssaslıq RzAcc = (AdcRz * Vref / 1023 - VzeroG) / Həssaslıq

İndiyə qədər sırf akselerometr ADC dəyərlərindən əldə edə biləcəyimiz ölçülmüş dəyərlər dəsti var. Bu məlumat dəstini "vektor" adlandıracağıq və aşağıdakı qeyddən istifadə edəcəyik.

Racc = [RxAcc, RyAcc, RzAcc]

Racc -in bu komponentləri akselerometr məlumatlarından əldə edilə biləcəyi üçün bunu alqoritmimizə bir giriş hesab edə bilərik.

Diqqət yetirin ki, Racc cazibə qüvvəsini ölçdüyü üçün aşağıdakı vektorun uzunluğunun 1g -ə bərabər və ya yaxın olduğunu düşünsəniz doğru olarsınız.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Ancaq əmin olmaq üçün bu vektoru aşağıdakı kimi yeniləməyin mənası var:

Racc (normallaşdırılmış) = [RxAcc/| Racc |, RyAcc/| Racc |, RzAcc/| Racc |].

Bu, normallaşdırılmış Racc vektorunuzun uzunluğunun həmişə 1 olmasını təmin edəcək.

Sonra yeni bir vektor təqdim edəcəyik və buna da ad verəcəyik

İstirahət = [RxEst, RyEst, RzEst]

Bu, alqoritmimizin çıxışı olacaq, bunlar giroskop məlumatlarına və keçmiş təxmin edilən məlumatlara əsaslanaraq düzəldilmiş dəyərlərdir.

Alqoritmimizin nə edəcəyi budur: - akselerometr bizə deyir: "İndi Racc mövqesindəsiniz" - "Təşəkkür edirəm, amma yoxlamağa icazə verin" deyirik, - sonra bu məlumatları giroskop məlumatları ilə, eləcə də keçmiş İstirahət məlumatları ilə düzəldin və yeni bir təxmini vektor buraxırıq. - Rest -ı cihazın mövcud mövqeyinə görə "ən yaxşı bahisimiz" hesab edirik.

Bunu necə işlədə biləcəyimizi görək.

Ardıcıllığımızı akselerometrimizə etibar edərək təyin edərək başlayacağıq:

İstirahət (0) = Racc (0)

Yeri gəlmişkən, Rest və Racc -in vektorlar olduğunu unutmayın, buna görə yuxarıdakı tənlik 3 tənlik yazmaq və təkrarlanmamaq üçün sadə bir yoldur:

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

Sonra, T saniyəsinin bərabər vaxt aralığında nizamlı ölçülər aparacağıq və Racc (1), Racc (2), Racc (3) və s. İstirahət (1), İstirahət (2), İstirahət (3) və sair hər zaman aralığında yeni təxminlər verəcəyik.

Tutaq ki, n addımdayıq. İstifadə etmək istədiyimiz iki dəyər dəsti var:

Rest (n -1) - Rest (0) = Racc (0) Racc (n) ilə əvvəlki təxminimiz

Rest (n) hesablamadan əvvəl, giroskopumuzdan və əvvəlki bir təxminimizdən əldə edə biləcəyimiz yeni bir ölçülmüş dəyər təqdim edək.

Buna Rgyro deyəcəyik və eyni zamanda 3 komponentdən ibarət bir vektordur:

Rgyro = [RxGyro, RyGyro, RzGyro]

Bu vektoru bir anda bir komponent hesablayacağıq. RxGyro ilə başlayacağıq.

Jiroskop modelimizdə Rz və Rxz tərəfindən düzbucaqlı üçbucaqdan aşağıdakı əlaqəni müşahidə edərək başlayaq:

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

Atan2, daha əvvəl heç istifadə etmədiyiniz bir funksiya ola bilər, atana bənzəyir, ancaq atanın geri qaytardığı kimi (-PI/2, PI/2) aralığında (-PI, PI) aralığında dəyərlər qaytarır və Bir əvəzinə 2 arqument. Rx, Rz iki dəyərini 360 dərəcə (-PI-PI) aralığında açılara çevirməyimizə imkan verir. Atan2 haqqında daha çox məlumatı buradan oxuya bilərsiniz.

Beləliklə, RxEst (n-1) və RzEst (n-1) bilməklə tapa bilərik:

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

Jiroskopun Axz açısının dəyişmə sürətini ölçdüyünü unutmayın. Beləliklə, Axz (n) yeni bucağını aşağıdakı kimi qiymətləndirə bilərik:

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

Unutmayın ki, RateAxz -ı ADC oxuduğumuz giroskopdan əldə edə bilərsiniz. Daha dəqiq bir düstur, aşağıdakı kimi hesablanan orta fırlanma sürətindən istifadə edə bilər:

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

Eyni şəkildə tapa bilərik:

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

Yaxşı, indi Axz (n) və Ayz (n) var. RxGyro/RyGyro çıxmaq üçün buradan hara gedək? Denk. 1 Rgyro vektorunun uzunluğunu belə yaza bilərik:

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

Racc vektorumuzu normallaşdırdığımız üçün uzunluğunun 1 olduğunu və fırlandıqdan sonra dəyişmədiyini düşünə bilərik, buna görə yazmaq nisbətən təhlükəsizdir:

| Rgyro | = 1

Aşağıdakı hesablamalar üçün müvəqqəti daha qısa bir notasiya qəbul edək:

x = RxGyro, y = RyGyro, z = RzGyro

Yuxarıdakı əlaqələrdən istifadə edərək yaza bilərik:

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

Sayı və məxrəci SQRT (x^2 + z^2) ilə bölək.

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

X / SQRT (x^2 + z^2) = sin (Axz) olduğunu unutmayın, beləliklə:

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

İndi SQRT daxilində olan hissənin sayını və məxrəcini z^2 ilə vurun

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

Qeyd edək ki, z / SQRT (x^2 + z^2) = cos (Axz) və y / z = tan (Ayz), nəhayət:

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

Nümunəmizə qayıdaraq əldə edirik:

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

eyni şəkildə tapırıq

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

İndi nəhayət tapa bilərik:

RzGyro = İşarə (RzGyro)*SQRT (1 - RxGyro^2 - RyGyro^2).

RzGyro> = 0 olduqda İşarə (RzGyro) = 1, RzGyro <0 olduqda İşarə (RzGyro) = -1.

Bunu qiymətləndirməyin sadə yollarından biri götürməkdir:

İşarə (RzGyro) = İşarə (RzEst (n-1))

Praktiki olaraq RzEst (n-1) 0-a yaxın olduqda diqqətli olun. Bu vəziyyətdə girro fazasını tamamilə atıb təyin edə bilərsiniz: Rgyro = İstirahət (n-1). Rz, Axz və Ayz bucaqlarının hesablanması üçün istinad olaraq istifadə olunur və 0 -a yaxın olduqda dəyərlər axa bilər və pis nəticələrə səbəb ola bilər. Tan () / atan () funksiyası tətbiqlərinin dəqiqlikdən məhrum ola biləcəyi böyük üzən nöqtə nömrələri sahəsində olacaqsınız.

Beləliklə, indiyə qədər əldə etdiklərimizi ümumiləşdirək, alqoritmimizin n -ci pilləsindəyik və aşağıdakı dəyərləri hesablamışıq:

Racc - Rgyro sürətləndiricimizin cari oxunuşları - Rest (n -1) və cari giroskop oxunuşlarından əldə edilir

Yenilənmiş Rest (n) smetasını hesablamaq üçün hansı dəyərlərdən istifadə edirik? Yəqin ki, ikisini də istifadə edəcəyimizi təxmin etdiniz. Ağırlıqlı ortalamadan istifadə edəcəyik, belə ki:

İstirahət (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Fraksiyanın həm sayını, həm də məxrəcini w1 -ə bölməklə bu düsturu sadələşdirə bilərik.

İstirahət (n) = (Racc * w1/w1 + Rgyro * w2/w1)/(w1/w1 + w2/w1)

və w2/w1 = wGyro ilə əvəz etdikdən sonra əldə edirik:

İstirahət (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

Yuxarıdakı forumda wGyro, akselerometrimizlə müqayisədə ciromuza nə qədər etibar etdiyimizi söyləyir. Bu dəyər eksperimental olaraq seçilə bilər, ümumiyyətlə 5..20 arasındakı dəyərlər yaxşı nəticələr verər.

Bu alqoritmin Kalman filtrindən əsas fərqi, bu çəkinin nisbətən sabit olmasıdır, halbuki Kalman filtrində, akselerometr oxunuşlarının ölçülmüş səs -küyünə əsasən çəkilər daimi olaraq yenilənir. Kalman filtri sizə "ən yaxşı" nəzəri nəticələr verməyə yönəlmişdir, halbuki bu alqoritm sizə praktik tətbiqiniz üçün "kifayət qədər yaxşı" nəticələr verə bilər. Ölçdüyünüz bəzi səs -küy amillərindən asılı olaraq wGyro -nu tənzimləyən bir alqoritm tətbiq edə bilərsiniz, lakin sabit dəyərlər əksər tətbiqlər üçün yaxşı işləyəcək.

Yenilənmiş təxmini dəyərlərimizi əldə etməyə bir addım qalmışıq:

RxEst (n) = (RxAcc + RxGyro * wGyro) / (1 + wGyro) RyEst (n) = (RyAcc + RyGyro * wGyro) / (1 + wGyro) RzEst (n) = (RzAcc + RzGyro * wGyro) / (1 + wGyro)

İndi bu vektoru yenidən normallaşdıraq:

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

Və döngəmizi təkrar etməyə hazırıq.

Bu bələdçi əvvəlcə starlino.com saytında göründü, bir neçə yüngül düzəlişlər etdim və icazə ilə yenidən yerləşdirdim. Təşəkkürlər Starlino!