Mafunzo ya Accelerometer & Gyro: Hatua 3

2024 Mwandishi: John Day | [email protected]. Mwisho uliobadilishwa: 2024-01-30 12:56

Utangulizi

Mwongozo huu unakusudiwa kwa kila mtu anayevutiwa kutumia Accelerometers na Gyroscopes pamoja na vifaa vya mchanganyiko vya IMU (Kitengo cha Upimaji wa Inertial) katika miradi yao ya elektroniki

Tutashughulikia:

• Je! Kipima kasi kina kipimo gani?
• Gyroscope (aka gyro) inapima nini?
• Jinsi ya kubadilisha usomaji wa Analog-to-digital (ADC) unayopata kutoka kwa sensa hizi kwenda kwa vitengo vya mwili (hizo zingekuwa g za accelerometer, deg / s kwa gyroscope)
• Jinsi ya kuchanganya usomaji wa accelerometer na gyroscope ili kupata habari sahihi juu ya mwelekeo wa kifaa chako ukilinganisha na ndege ya ardhini.

Katika makala yote nitajaribu kuweka hesabu kwa kiwango cha chini. Ikiwa unajua Sine / Cosine / Tangent ni nini basi unapaswa kuelewa na kutumia maoni haya katika mradi wako bila kujali unatumia jukwaa gani: Arduino, Propeller, Stempu ya Msingi, chips za Atmel, Microchip PIC, n.k.

Kuna watu huko nje ambao wanaamini kuwa unahitaji hesabu ngumu ili utumie kitengo cha IMU (vichungi tata vya MOTO au IIR kama vichungi vya Kalman, vichungi vya Parks-McClellan, nk). Unaweza kutafiti yote hayo na kupata matokeo mazuri lakini ngumu. Njia yangu ya kuelezea vitu inahitaji hesabu za kimsingi tu. Mimi ni muumini mzuri wa unyenyekevu. Nadhani mfumo ambao ni rahisi ni rahisi kudhibiti na kufuatilia, badala ya vifaa vingi vilivyopachikwa hazina nguvu na rasilimali za kutekeleza algorithms tata inayohitaji mahesabu ya tumbo.

Nitatumia kama mfano kitengo kipya cha IMU, Acc_Gyro Accelerometer + Gyro IMU. Tutatumia vigezo vya kifaa hiki katika mifano yetu hapa chini. Kitengo hiki ni kifaa kizuri kuanza kwa sababu kina vifaa 2:

- LIS331AL (karatasi ya data) - kasi ya triaxial 2G - LPR550AL (karatasi ya data) - lami na roll mbili-mhimili, gyroscope ya digrii 500 / sec

Kwa pamoja wanawakilisha Digrii 5 za Kitengo cha Upimaji wa Inertial Inertial. Sasa hilo ni jina la kupendeza! Walakini, nyuma ya jina la kupendeza kuna kifaa muhimu cha mchanganyiko ambacho tutafunika na kuelezea kwa kina katika mwongozo huu.

Hatua ya 1: Accelerometer

Ili kuelewa kitengo hiki tutaanza na accelerometer. Wakati wa kufikiria juu ya kuongeza kasi mara nyingi ni muhimu kutafakari sanduku lenye umbo la mchemraba na mpira ndani yake. Unaweza kufikiria kitu kingine kama kuki au donut, lakini nitafikiria mpira:

Ikiwa tutachukua sanduku hili mahali pasipokuwa na uwanja wa uvutano au kwa jambo hilo bila uwanja mwingine wowote ambao unaweza kuathiri msimamo wa mpira - mpira utaelea katikati ya sanduku. Unaweza kufikiria sanduku liko kwenye nafasi ya nje-mbali mbali na miili yoyote ya ulimwengu, au ikiwa mahali kama ni ngumu kupata kufikiria angalau ufundi wa nafasi unaozunguka sayari ambapo kila kitu kiko katika hali isiyo na uzani. Kutoka kwenye picha hapo juu unaweza kuona kwamba tunapeana kila mhimili jozi ya kuta (tumeondoa ukuta Y + ili tuweze kutazama ndani ya sanduku). Fikiria kwamba kila ukuta ni nyeti ya shinikizo. Ikiwa tutahamisha ghafla sanduku kushoto (tunaongeza kasi na kuongeza kasi 1g = 9.8m / s ^ 2), mpira utagonga ukuta X-. Tunapima nguvu ya shinikizo ambayo mpira unatumika kwenye ukuta na kutoa thamani ya -1g kwenye mhimili wa X.

Tafadhali kumbuka kuwa accelerometer itagundua nguvu ambayo imeelekezwa kwa mwelekeo tofauti kutoka kwa vector ya kuongeza kasi. Kikosi hiki mara nyingi huitwa Kikosi cha Inertial au Kikosi cha Kutunga. Jambo moja unalopaswa kujifunza kutoka kwa hii ni kwamba kasi ya kuongeza kasi inaongeza kasi ya moja kwa moja kupitia nguvu ambayo inatumika kwa moja ya kuta zake (kulingana na mfano wetu, inaweza kuwa chemchemi au kitu kingine katika kasi ya maisha halisi). Nguvu hii inaweza kusababishwa na kuongeza kasi, lakini kama tutakavyoona katika mfano unaofuata sio kila wakati husababishwa na kuongeza kasi.

Ikiwa tutachukua mfano wetu na kuiweka Duniani mpira utaanguka kwenye ukuta wa Z na utatumia nguvu ya 1g kwenye ukuta wa chini, kama inavyoonyeshwa kwenye picha hapa chini:

Katika kesi hii sanduku halihami lakini bado tunapata usomaji wa -1g kwenye mhimili wa Z. Shinikizo ambalo mpira umetumia ukutani ulisababishwa na nguvu ya uvutano. Kwa nadharia inaweza kuwa aina tofauti ya nguvu - kwa mfano, ikiwa unafikiria kwamba mpira wetu ni metali, kuweka sumaku karibu na sanduku kunaweza kusogeza mpira kwa hivyo unagonga ukuta mwingine. Hii ilisemwa ili tu kudhibitisha kuwa kiini kasi ya hatua huongeza nguvu sio kuongeza kasi. Inatokea tu kwamba kuongeza kasi kunasababisha nguvu isiyo na nguvu ambayo inakamatwa na utaratibu wa kugundua nguvu wa kiharusi.

Wakati modeli hii sio jinsi sensor ya MEMS inavyojengwa mara nyingi ni muhimu katika kutatua shida zinazohusiana na kasi ya kasi. Kuna sensorer sawa sawa ambazo zina mipira ya chuma ndani, zinaitwa swichi za kuelekeza, hata hivyo ni za zamani zaidi na kawaida zinaweza tu kujua ikiwa kifaa kimeelekezwa katika anuwai fulani au la, sio kiwango cha mwelekeo.

Hadi sasa tumechambua pato la accelerometer kwenye mhimili mmoja na hii ndiyo yote utakayopata na kasi ya mhimili mmoja. Thamani halisi ya accelerometers ya triaxial inakuja kutokana na ukweli kwamba wanaweza kuchunguza nguvu za inertial kwenye axes zote tatu. Turudi kwa mtindo wetu wa sanduku, na hebu tugeuze sanduku kwa digrii 45 kulia. Mpira utagusa kuta 2 sasa: Z- na X- kama inavyoonyeshwa kwenye picha hapa chini:

Thamani za 0.71 sio za kiholela, kwa kweli ni hesabu ya SQRT (1/2). Hii itakuwa wazi zaidi tunapoanzisha modeli yetu inayofuata ya kiharusi.

Katika mtindo uliopita tumerekebisha nguvu ya uvutano na kuzungusha sanduku letu la kufikiria. Katika mifano 2 iliyopita tumechambua pato katika nafasi 2 tofauti za sanduku, wakati vector ya nguvu ilibaki kila wakati. Ingawa hii ilikuwa muhimu kuelewa jinsi kasi ya kasi inaingiliana na nguvu za nje, ni muhimu kufanya mahesabu ikiwa tutarekebisha mfumo wa kuratibu kwa shoka za kasi na kufikiria kuwa vector ya nguvu inazunguka karibu nasi.

Tafadhali angalia mfano hapo juu, nilihifadhi rangi za shoka ili uweze kufanya mabadiliko ya kiakili kutoka kwa mtindo uliopita hadi mpya. Hebu fikiria kwamba kila mhimili katika mtindo mpya ni sawa na nyuso husika za sanduku kwenye modeli iliyopita. Vector R ni vector ya nguvu ambayo accelerometer inapima (inaweza kuwa nguvu ya uvutano au nguvu isiyo na nguvu kutoka kwa mifano hapo juu au mchanganyiko wa zote mbili). Rx, Ry, Rz ni makadirio ya vector R kwenye shoka za X, Y, Z. Tafadhali angalia uhusiano ufuatao:

R ^ 2 = Rx ^ 2 + Ry ^ 2 + Rz ^ 2 (Eq. 1)

ambayo kimsingi ni sawa na nadharia ya Pythagorean katika 3D.

Kumbuka kwamba mapema kidogo nilikwambia kwamba maadili ya SQRT (1/2) ~ 0.71 sio ya kubahatisha. Ikiwa utaziunganisha kwenye fomula hapo juu, baada ya kukumbuka kuwa nguvu yetu ya uvutano ilikuwa 1 g tunaweza kuthibitisha kuwa:

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

tu kwa kubadilisha R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) katika Eq.1

Baada ya utangulizi mrefu wa nadharia tunakaribia kasi ya maisha halisi. Thamani Rx, Ry, Rz kweli zinahusiana sawa na maadili ambayo kasi ya maisha yako halisi itatoa na ambayo unaweza kutumia kwa kufanya mahesabu anuwai.

Kabla ya kufika hapo wacha tuzungumze kidogo juu ya njia ambayo accelerometers itatuletea habari hii. Accelerometer nyingi zitaanguka katika vikundi viwili: dijiti na analog. Accelerometer za dijiti zitakupa habari kwa kutumia itifaki ya serial kama I2C, SPI au USART, wakati accelerometers za analog zitatoa kiwango cha voltage ndani ya anuwai iliyotabiriwa ambayo lazima ubadilishe kuwa nambari ya dijiti kwa kutumia moduli ya ADC (analog to digital converter). Sitaenda kwa undani zaidi juu ya jinsi ADC inavyofanya kazi, kwa sababu kwa sababu ni mada pana na kwa sababu ni tofauti na jukwaa moja hadi lingine. Mdhibiti mdogo atakuwa na moduli za ADC zilizojengwa baadhi yao zitahitaji vifaa vya nje ili kufanya ubadilishaji wa ADC. Haijalishi ni aina gani ya moduli ya ADC unayotumia utaishia na thamani katika anuwai fulani. Kwa mfano moduli ya ADC ya 10-bit itatoa thamani katika anuwai ya 0..1023, kumbuka kuwa 1023 = 2 ^ 10 -1. Moduli ya ADC 12-bit itatoa thamani katika anuwai ya 0..4095, kumbuka kuwa 4095 = 2 ^ 12-1.

Wacha tuendelee kwa kuzingatia mfano rahisi, tuseme moduli yetu ya 10bit ADC ilitupa maadili yafuatayo kwa njia tatu za kasi (axes):

AdcRx = 586 AdcRy = 630 AdcRz = 561

Kila moduli ya ADC itakuwa na voltage ya kumbukumbu, wacha tufikirie katika mfano wetu ni 3.3V. Kubadilisha thamani ya 10bit adc kuwa voltage tunatumia fomula ifuatayo:

VoltsRx = AdcRx * Vref / 1023

Ujumbe wa haraka hapa: kwamba kwa 8bit ADC mgawanyiko wa mwisho atakuwa 255 = 2 ^ 8 -1, na kwa 12bit ADC mgawanyiko wa mwisho atakuwa 4095 = 2 ^ 12 -1.

Kutumia fomula hii kwa njia zote 3 tunazopata:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (tunazunguka matokeo yote kwa alama 2 za desimali) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Kila accelerometer ina kiwango cha voltage sifuri-g, unaweza kuipata katika vielelezo, hii ndio voltage inayofanana na 0g. Ili kupata thamani ya voltage iliyosainiwa tunahitaji kuhesabu mabadiliko kutoka kwa kiwango hiki. Wacha tuseme kiwango chetu cha 0g ni VzeroG = 1.65V. Tunahesabu mabadiliko ya voltage kutoka kwa zero-g voltage kama ifuatavyo::

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

Sasa tuna usomaji wetu wa kasi ya kasi katika Volts, bado haiko katika g (9.8 m / s ^ 2), kufanya uongofu wa mwisho tunatumia usikivu wa kasi, kawaida huonyeshwa kwa mV / g. Hebu sema Usikivu wetu = 478.5mV / g = 0.4785V / g. Thamani za unyeti zinaweza kupatikana katika uainishaji wa kasi. Kupata maadili ya nguvu ya mwisho yaliyoonyeshwa katika g tunatumia fomula ifuatayo:

Rx = DeltaVoltsRx / Usikivu

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

Kwa kweli tunaweza kuchanganya hatua zote kwa fomula moja, lakini nilipitia hatua zote kuifanya iwe wazi jinsi unavyoenda kutoka kwa usomaji wa ADC hadi sehemu ya vector ya nguvu iliyoonyeshwa kwa g.

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

Sasa tuna vifaa vyote 3 ambavyo hufafanua vector yetu ya nguvu isiyo na nguvu, ikiwa kifaa hakiko chini ya nguvu zingine isipokuwa uvutano, tunaweza kudhani huu ndio mwelekeo wa vector yetu ya nguvu ya uvutano. Ikiwa unataka kuhesabu mwelekeo wa kifaa kinachohusiana na ardhi unaweza kuhesabu pembe kati ya vector hii na mhimili wa Z. Ikiwa una nia pia kwa mwelekeo wa kila mhimili wa mwelekeo unaweza kugawanya matokeo haya kuwa vitu viwili: mwelekeo kwenye mhimili wa X na Y ambao unaweza kuhesabiwa kama pembe kati ya vector ya mvuto na shoka za X / Y. Kuhesabu pembe hizi ni rahisi zaidi kuliko unavyofikiria, sasa kwa kuwa tumehesabu maadili ya Rx, Ry na Rz. Wacha turudi kwa mtindo wetu wa mwisho wa kuharakisha na ufanye notisi zingine:

Pembe ambazo tunavutiwa nazo ni pembe kati ya X, Y, Z axes na vector nguvu R. Tutafafanua pembe hizi kama Axr, Ayr, Azr. Unaweza kugundua kutoka pembetatu ya pembe-kulia iliyoundwa na R na Rx kwamba:

cos (Axr) = Rx / R, na vile vile: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Tunaweza kutoa kutoka kwa Eq.1 kwamba R = SQRT (Rx ^ 2 + Ry ^ 2 + Rz ^ 2).

Tunaweza kupata sasa pembe zetu kwa kutumia arccos () kazi (inverse cos () kazi):

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

Tumeenda mbali kuelezea mtindo wa kasi, ili tu kuja na kanuni hizi. Kulingana na maombi yako unaweza kutaka kutumia fomula zozote za kati ambazo tumezipata. Tutaanzisha pia mtindo wa gyroscope hivi karibuni, na tutaona jinsi data ya kasi na gyroscope inaweza kuunganishwa ili kutoa makadirio sahihi zaidi ya mwelekeo.

Lakini kabla ya kufanya hivyo wacha tufanye notisi muhimu zaidi:

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

Katatu hii mara nyingi huitwa Direction Cosine, na inawakilisha vector ya kitengo (vector yenye urefu wa 1) ambayo ina mwelekeo sawa na vector R yetu. Unaweza kuthibitisha kwa urahisi kwamba:

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

Hii ni mali nzuri kwani inatuondolea ufuatiliaji wa moduli (urefu) wa R vector. Mara nyingi ikiwa tunavutiwa na mwelekeo wa vector yetu isiyo na maana, ni busara kurekebisha moduli hiyo ili kurahisisha mahesabu mengine.

Hatua ya 2: Gyroscope

Hatutaanzisha mfano wowote wa sanduku kama gyroscope kama vile tulivyofanya kwa accelerometer, badala yake tutaruka moja kwa moja kwa mtindo wa pili wa kasi na tutaonyesha nini gyroscope inapima kulingana na mtindo huu.

Kila kituo cha gyroscope hupima mzunguko kuzunguka moja ya shoka. Kwa mfano gyroscope ya shoka 2 itapima mzunguko kuzunguka (au wengine wanaweza kusema "kuhusu") shoka za X na Y. Ili kuelezea mzunguko huu kwa nambari wacha tufanye maoni kadhaa. Kwanza hebu tufafanue:

Rxz - ni makadirio ya nguvu ya inertial vector R kwenye ndege ya XZ Ryz - ni makadirio ya vector nguvu ya inertial R kwenye ndege ya YZ

Kutoka kwa pembetatu ya pembe ya kulia iliyoundwa na Rxz na Rz, kwa kutumia nadharia ya Pythagorean tunapata:

Rxz ^ 2 = Rx ^ 2 + Rz ^ 2, na vile vile: Ryz ^ 2 = Ry ^ 2 + Rz ^ 2

kumbuka pia kuwa:

R ^ 2 = Rxz ^ 2 + Ry ^ 2, hii inaweza kutolewa kutoka kwa Eq.1 na juu ya hesabu, au inaweza kutolewa kutoka pembetatu ya pembe-kulia iliyoundwa na R na Ryz R ^ 2 = Ryz ^ 2 + Rx ^ 2

Hatutatumia fomula hizi katika nakala hii lakini ni muhimu kutambua uhusiano kati ya maadili yote katika mfano wetu.

Badala yake tutafafanua pembe kati ya mhimili wa Z na Rxz, vekta za Ryz kama ifuatavyo:

Axz - ni pembe kati ya Rxz (makadirio ya R kwenye ndege ya XZ) na Z axis Ayz - ni pembe kati ya Ryz (makadirio ya R kwenye ndege ya YZ) na mhimili wa Z

Sasa tunakaribia kile gyroscope inachukua. Gyroscope hupima kiwango cha mabadiliko ya pembe zilizoelezwa hapo juu. Kwa maneno mengine itatoa thamani ambayo inahusiana sawa na kiwango cha mabadiliko ya pembe hizi. Ili kuelezea hii wacha tuchukue kuwa tumepima pembe ya kuzunguka kuzunguka mhimili Y (hiyo itakuwa angle ya Axz) wakati t0, na tunaifafanua kama Axz0, baadaye tukapima pembe hii baadaye t1 na ilikuwa Axz1. Kiwango cha mabadiliko kitahesabiwa kama ifuatavyo:

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

Ikiwa tunaelezea Axz kwa digrii, na wakati kwa sekunde, basi thamani hii itaonyeshwa kwa dig / s. Hii ndio hatua ya gyroscope.

Katika mazoezi ya gyroscope (isipokuwa kama ni gyroscope maalum ya dijiti) haitakupa mara chache thamani iliyoonyeshwa kwa dig / s. Sawa na accelerometer utapata thamani ya ADC ambayo utahitaji kuibadilisha kuwa deg / s ukitumia fomula sawa na Eq. 2 ambayo tumeelezea kwa accelerometer. Wacha tuanzishe ADC kwa fomula ya ubadilishaji ya deg / s ya gyroscope (tunadhani tunatumia moduli ya 10bit ADC, kwa 8bit ADC kuchukua nafasi ya 1023 na 255, kwa 12bit ADC kuchukua nafasi ya 1023 na 4095).

KiwangoAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Usikivu Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / Usikivu

AdcGyroXZ, AdcGyroYZ - hupatikana kutoka kwa moduli yetu ya adc na zinawakilisha njia ambazo hupima mzunguko wa makadirio ya vector R katika XZ mtawaliwa katika ndege za YZ, ambayo ni sawa na kusema mzunguko ulifanywa karibu na shoka za Y na X mtawaliwa.

Vref - ni voltage ya kumbukumbu ya ADC tutatumia 3.3V kwa mfano chini ya VzeroRate - ni kiwango cha sifuri, kwa maneno mengine voltage ambayo gyroscope hutoa wakati sio chini ya mzunguko wowote, kwa bodi ya Acc_Gyro ni Kwa mfano 1.23V pato la gyroscope linaongezeka, ikiwa unaongeza kasi ya kuzunguka kwa digrii moja. Usikivu wa bodi ya Acc_Gyro ni kwa mfano 2mV / deg / s au 0.002V / deg / s

Wacha tuchukue mfano, tuseme moduli yetu ya ADC imerudisha maadili yafuatayo:

323. Mchanganyiko haupatikani

Kutumia fomula hiyo hapo juu, na kutumia vigezo vya ubao wa Acc_Gyro tutapata:

KiwangoAxz = (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 dig / s

Kwa maneno mengine kifaa kinazunguka kwenye mhimili wa Y (au tunaweza kusema huzunguka katika ndege ya XZ) na kasi ya 306 dig / s na kuzunguka mhimili wa X (au tunaweza kusema inazunguka katika ndege ya YZ) na kasi ya - 94 digrii / s. Tafadhali kumbuka kuwa ishara hasi inamaanisha kuwa kifaa huzunguka kwa mwelekeo tofauti kutoka kwa mwelekeo mzuri wa kawaida. Kwa mkusanyiko mwelekeo mmoja wa mzunguko ni mzuri. Karatasi nzuri ya uainishaji wa gyroscope itakuonyesha ni mwelekeo upi mzuri, vinginevyo itabidi uupate kwa kujaribu na kifaa na kubainisha ni mwelekeo upi wa mzunguko unasababisha kuongezeka kwa voltage kwenye pini ya pato. Hii inafanywa vizuri kwa kutumia oscilloscope kwani mara tu utakaposimamisha mzunguko voltage itashuka hadi kiwango cha kiwango cha sifuri. Ikiwa unatumia multimeter italazimika kudumisha kiwango cha kuzunguka mara kwa mara kwa angalau sekunde chache na kumbuka voltage wakati wa mzunguko huu, kisha ulinganishe na kiwango cha kiwango cha sifuri. Ikiwa ni kubwa kuliko kiwango cha sifuri inamaanisha kuwa mwelekeo wa mzunguko ni mzuri.

Hatua ya 3: Kuchanganya Accelerometer na Gyro

Kuiweka pamoja - Kuchanganya kasi ya kasi na data ya gyroscope

Ikiwa unasoma nakala hii labda umepata au unapanga kupata kifaa cha IMU, au labda unapanga kujenga moja kutoka kwa vifaa vya kasi vya kasi na vifaa vya gyroscope.

Hatua ya kwanza ya kutumia kifaa cha mchanganyiko cha IMU kinachounganisha accelerometer na gyroscope ni kulinganisha mifumo yao ya kuratibu. Njia rahisi ya kuifanya ni kuchagua mfumo wa kuratibu wa accelerometer kama mfumo wako wa kuratibu kumbukumbu. Karatasi nyingi za data za kasi itaonyesha mwelekeo wa shoka za X, Y, Z zinazohusiana na picha ya chip ya mwili au kifaa. Kwa mfano hapa kuna maagizo ya shoka za X, Y, Z kama inavyoonyeshwa katika maelezo ya bodi ya Acc_Gyro:

Hatua zifuatazo ni:

Tambua matokeo ya gyroscope ambayo yanahusiana na RateAxz, maadili ya RateAyz yaliyojadiliwa hapo juu. Amua ikiwa matokeo haya yanahitaji kugeuzwa kwa sababu ya nafasi ya mwili ya gyroscope inayohusiana na kasi ya kasi

Usifikirie kwamba ikiwa gyroscope ina pato lililowekwa alama X au Y, italingana na mhimili wowote kwenye mfumo wa kuratibu wa kasi, hata ikiwa pato hili ni sehemu ya kitengo cha IMU. Njia bora ni kuijaribu. Kwa kudhani umerekebisha nafasi ya gyroscope jamaa na accelerometer. Inachukuliwa kuwa mipaka ya gyro na accelerometer ni sawa na kila mmoja, i.e.unaweka gyro kwa pembe nyingi ya 90deg inayohusiana na chip ya accelerometer. Ikiwa umepata nafasi ya bodi ya IMU ni kwamba tayari zimepangwa kwa njia hii. Hatutajadili katika modeli za kifungu hiki ambapo gyroscope imewekwa kwa pembe isiyo ya kawaida inayohusiana na accelerometer (wacha tuseme digrii 45 au 30), ingawa hii inaweza kuwa muhimu katika programu zingine.

Hapa kuna mlolongo wa sampuli ya kuamua ni pato gani la gyroscope linalingana na thamani ya RateAxz iliyojadiliwa hapo juu.

- anza kutoka kuweka kifaa katika nafasi ya usawa. Matokeo ya X na Y ya accelerometer yatatoa voltage ya zero-g (kwa mfano kwa bodi ya Acc_Gyro hii ni 1.65V)

- ijayo anza kuzunguka kifaa kuzunguka mhimili wa Y, njia nyingine ya kusema ni kwamba unazungusha kifaa kwenye ndege ya XZ, ili matokeo ya kasi ya X na Z yabadilike na pato la Y libaki kila wakati. - wakati unazungusha kifaa kwa kasi ya mara kwa mara ambayo pato la gyroscope hubadilika, matokeo mengine ya gyroscope yanapaswa kubaki kila wakati - pato la gyroscope lililobadilika wakati wa kuzunguka karibu na mhimili Y tunahesabu RateAxz - hatua ya mwisho ni kuhakikisha mwelekeo wa kuzunguka unalingana na mfano wetu, katika hali zingine huenda ukalazimika kubadilisha thamani ya RateAxz kwa sababu ya nafasi ya mwili ya gyroscope inayohusiana na kasi ya kasi - fanya tena jaribio la hapo juu, ukizungusha kifaa kuzunguka mhimili wa Y, wakati huu fuatilia pato la X la accelerometer (AdcRx katika mfano wetu). Ikiwa AdcRx inakua (digrii 90 za kwanza za mzunguko kutoka nafasi ya usawa), basi AdcGyroXZ inapaswa pia kukua. Vinginevyo unahitaji kugeuza RateAxz, unaweza kufikia hili kwa kuanzisha ishara katika Eq.3, kama ifuatavyo:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Usikivu, ambapo InvertAxz ni 1 au -1

miwa hiyo ya jaribio ifanyike kwa RateAyz, kwa kuzungusha kifaa karibu na mhimili wa X, na unaweza kutambua ni pato gani la gyroscope linalolingana na RateAyz, na ikiwa inahitaji kugeuzwa. Mara baada ya kuwa na thamani ya InvertAyz, unapaswa kutumia fomula ifuatayo kuhesabu RateAyz:

KiwangoAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Usikivu

Ikiwa utafanya majaribio haya kwenye bodi ya Acc_Gyro utapata matokeo yafuatayo:

- pini ya pato ya RateAxz ni GX4 na InvertAxz = -1. - pini ya pato ya RateAyz ni GY4 na InvertAyz = -1

Kuanzia wakati huu na kuendelea tutazingatia kuwa umeweka IMU yako kwa njia ambayo unaweza kuhesabu maadili sahihi ya Axr, Ayr, Azr (kama ilivyoainishwa Sehemu ya 1. Accelerometer) na RateAxz, RateAyz (kama ilivyoainishwa katika Sehemu ya 2. Gyroscope). Ifuatayo tutachambua uhusiano kati ya maadili haya ambayo yanafaa katika kupata makadirio sahihi zaidi ya mwelekeo wa kifaa kulingana na ndege ya ardhini.

Labda unajiuliza kwa hatua hii, ikiwa mfano wa kasi ya kasi tayari umetupa pembe za mwelekeo wa Axr, Ayr, Azr kwanini tunataka kusumbua na data ya gyroscope? Jibu ni rahisi: data ya accelerometer haiwezi kuaminika kila wakati kwa 100%. Kuna sababu kadhaa, kumbuka kuwa accelerometer inachukua nguvu ya inertial, nguvu kama hiyo inaweza kusababishwa na uvutano (na haswa na uvutano), lakini pia inaweza kusababishwa na kuongeza kasi (harakati) ya kifaa. Kama matokeo hata kama accelerometer iko katika hali thabiti, bado ni nyeti sana kwa mtetemo na kelele za kiufundi kwa ujumla. Hii ndio sababu kuu kwa nini mifumo mingi ya IMU hutumia gyroscope kulainisha makosa yoyote ya kasi ya kasi. Lakini hii inafanywaje? Je! Gyroscope haina kelele?

Gyroscope haina uhuru kutoka kwa kelele hata hivyo kwa sababu inapima mzunguko sio nyeti sana kwa harakati za mitambo, aina ya kelele ambayo accelerometer inakabiliwa na, hata hivyo gyroscopes zina aina zingine za shida kama kwa mfano kuteleza (kutorejea kwa kiwango cha kiwango cha sifuri) wakati mzunguko unasimama). Walakini kwa data ya wastani ambayo hutoka kwa accelerometer na gyroscope tunaweza kupata makadirio bora ya mwelekeo wa sasa wa kifaa kuliko tunavyoweza kupata kwa kutumia data ya kasi ya kasi tu.

Katika hatua zifuatazo nitaanzisha algorithm ambayo iliongozwa na maoni kadhaa yaliyotumiwa katika kichujio cha Kalman, hata hivyo ni rahisi zaidi na rahisi kutekeleza kwenye vifaa vilivyopachikwa. Kabla ya hapo wacha tuone kwanza ni nini tunataka hesabu yetu ihesabu. Kweli, ni mwelekeo wa vector ya nguvu ya uvutano R = [Rx, Ry, Rz] ambayo tunaweza kupata maadili mengine kama Axr, Ayr, Azr au cosX, ya kupendeza, cosZ ambayo itatupa wazo juu ya mwelekeo wa kifaa chetu jamaa na ndege ya ardhini, tunajadili uhusiano kati ya maadili haya katika Sehemu ya 1. Mtu anaweza kusema - je! tayari hatuna maadili haya Rx, Ry, Rz kutoka Eq.2 katika Sehemu ya 1? Ndio ndio, lakini kumbuka kuwa maadili haya yanatokana na data ya kasi ya kasi tu, kwa hivyo ikiwa ungetumia moja kwa moja katika programu yako unaweza kupata kelele zaidi kuliko programu yako inaweza kuvumilia. Ili kuepuka machafuko zaidi hebu tufafanue tena vipimo vya accelerometer kama ifuatavyo:

Racc - ni vector ya nguvu isiyo na kipimo kama inavyopimwa na accelerometer, ambayo inajumuisha vitu vifuatavyo (makadirio kwenye shoka za X, Y, Z):

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

Hadi sasa tuna seti ya maadili yaliyopimwa ambayo tunaweza kupata kutoka kwa viwango vya kasi vya ADC. Tutaita seti hii ya data "vector" na tutatumia nukuu ifuatayo.

Racc = [RxAcc, RyAcc, RzAcc]

Kwa sababu vifaa hivi vya Racc vinaweza kupatikana kutoka kwa data ya kasi, tunaweza kuiona kama pembejeo kwa algorithm yetu.

Tafadhali kumbuka kuwa kwa sababu Racc hupima nguvu ya uvutano utakuwa sahihi ikiwa unadhani kuwa urefu wa vector hii iliyoelezwa kama ifuatavyo ni sawa au karibu na 1g.

| Racc | = SQRT (RxAcc ^ 2 + RyAcc ^ 2 + RzAcc ^ 2), Walakini kuwa na hakika ni busara kusasisha vector hii kama ifuatavyo:

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

Hii itahakikisha urefu wa vector yako ya kawaida ya Racc daima ni 1.

Ifuatayo tutaanzisha vector mpya na tutaiita

Pumzika = [RxEst, RyEst, RzEst]

Hii itakuwa pato la algorithm yetu, hizi ni maadili yaliyosahihishwa kulingana na data ya gyroscope na kulingana na data iliyokadiriwa ya zamani.

Hapa ndivyo algorithm yetu itakavyofanya: - accelerometer inatuambia: "Sasa uko katika msimamo wa Racc" - tunasema "Asante, lakini ngoja niangalie", - kisha sahihisha habari hii na data ya gyroscope na vile vile na data ya zamani ya Mapumziko na tunatoa Pumziko mpya la vector. - tunachukulia Pumziko kuwa "bet bora" kwa nafasi ya sasa ya kifaa.

Wacha tuone ni jinsi gani tunaweza kuifanya ifanye kazi.

Tutaanza mlolongo wetu kwa kuamini kipima kasi na kuwapa:

Pumzika (0) = Racc (0)

Kwa njia kumbuka Rest na Racc ni vectors, kwa hivyo equation hapo juu ni njia rahisi tu ya kuandika seti 3 za equations, na epuka kurudia:

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

Ifuatayo tutafanya vipimo vya kawaida kwa vipindi sawa vya sekunde T, na tutapata vipimo vipya ambavyo tutafafanua kama Racc (1), Racc (2), Racc (3) na kadhalika. Tutatoa pia makadirio mapya kila wakati vipindi Pumzika (1), Pumzika (2), Pumzika (3) na kadhalika.

Tuseme tuko kwenye hatua n. Tuna seti mbili za maadili ambazo tungetaka kutumia:

Pumzika (n-1) - makadirio yetu ya awali, na Pumziko (0) = Racc (0) Racc (n) - kipimo chetu cha kasi cha kasi

Kabla ya kuhesabu Pumziko (n), wacha tuanzishe nambari mpya iliyopimwa, ambayo tunaweza kupata kutoka kwa gyroscope yetu na makadirio ya awali.

Tutaiita Rgyro, na pia ni vector inayojumuisha vifaa 3:

Rgyro = [RxGyro, RyGyro, RzGyro]

Tutakokotoa vector hii sehemu moja kwa wakati. Tutaanza na RxGyro.

Wacha tuanze kwa kuona uhusiano ufuatao katika modeli yetu ya gyroscope, kutoka pembetatu ya pembe ya kulia iliyoundwa na Rz na Rxz tunaweza kupata kuwa:

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

Atan2 inaweza kuwa kazi ambayo hujawahi kutumia hapo awali, ni sawa na atan, isipokuwa inarudisha maadili katika anuwai ya (-PI, PI) kinyume na (-PI / 2, PI / 2) kama inarudishwa na atan, na inachukua Hoja 2 badala ya moja. Inaturuhusu kubadilisha maadili mawili ya Rx, Rz kuwa pembe katika safu kamili ya digrii 360 (-PI hadi PI). Unaweza kusoma zaidi kuhusu atan2 hapa.

Kwa hivyo kujua RxEst (n-1), na RzEst (n-1) tunaweza kupata:

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

Kumbuka kwamba gyroscope inapima kiwango cha mabadiliko ya pembe ya Axz. Kwa hivyo tunaweza kukadiria pembe mpya Axz (n) kama ifuatavyo:

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

Kumbuka kwamba RateAxz inaweza kupatikana kutoka kwa usomaji wetu wa gyroscope ADC. Fomu sahihi zaidi inaweza kutumia kiwango cha wastani cha mzunguko kilichohesabiwa kama ifuatavyo:

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

Njia ile ile tunaweza kupata:

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

Sawa sasa tuna Axz (n) na Ayz (n). Tunatoka wapi hapa kutoa RxGyro / RyGyro? Kutoka kwa Eq. 1 tunaweza kuandika urefu wa vector Rgyro kama ifuatavyo:

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

Pia kwa sababu tulirekebisha vector yetu ya Racc, tunaweza kudhani kuwa urefu ni 1 na haujabadilika baada ya kuzunguka, kwa hivyo ni salama kuandika:

| Rgyro | = 1

Wacha tuchukue noti fupi ya muda mfupi kwa mahesabu hapa chini:

x = RxGyro, y = RyGyro, z = RzGyro

Kutumia mahusiano hapo juu tunaweza kuandika:

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

Wacha tugawanye nambari na dhehebu la sehemu na SQRT (x ^ 2 + z ^ 2)

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

Kumbuka kuwa x / SQRT (x ^ 2 + z ^ 2) = dhambi (Axz), kwa hivyo:

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

Sasa zidisha hesabu na idadi kubwa ya sehemu ndani ya SQRT na z ^ 2

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

Kumbuka kuwa z / SQRT (x ^ 2 + z ^ 2) = cos (Axz) na y / z = tan (Ayz), kwa hivyo mwishowe:

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

Kurudi kwenye notation yetu tunapata:

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

njia hiyo hiyo tunapata hiyo

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

Sasa, mwishowe tunaweza kupata:

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

Ishara ya wapi (RzGyro) = 1 wakati RzGyro> = 0, na Ishara (RzGyro) = -1 wakati RzGyro <0.

Njia moja rahisi ya kukadiria hii ni kuchukua:

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

Katika mazoezi kuwa mwangalifu wakati RzEst (n-1) iko karibu na 0. Unaweza kuruka awamu ya gyro kabisa katika kesi hii na kupeana: Rgyro = Pumzika (n-1). Rz hutumiwa kama rejeleo la kuhesabu pembe za Axz na Ayz na inapokaribia 0, maadili yanaweza kufurika na kusababisha matokeo mabaya. Utakuwa katika uwanja wa nambari kubwa za kuelea ambapo utekelezaji wa kazi ya tan () / atan () inaweza kukosa usahihi.

Kwa hivyo wacha turudishe kile tunacho hadi sasa, tuko katika hatua ya n ya algorithm yetu na tumehesabu maadili yafuatayo:

Racc - usomaji wa sasa kutoka kwa kipima kasi Rgyro - uliopatikana kutoka kwa Pumziko (n-1) na usomaji wa sasa wa gyroscope

Je! Ni maadili gani tunayotumia kuhesabu ukadiriaji uliosasishwa wa kupumzika (n)? Labda umefikiria kuwa tutatumia zote mbili. Tutatumia wastani wenye uzito, ili:

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

Tunaweza kurahisisha fomula hii kwa kugawanya hesabu zote mbili na nambari ya sehemu kwa w1.

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

na baada ya kubadilisha w2 / w1 = wGyro tunapata:

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

Katika jukwaa hapo juu wGyro anatuambia ni kiasi gani tunaamini gyro yetu ikilinganishwa na accelerometer yetu. Thamani hii inaweza kuchaguliwa kwa majaribio kwa kawaida kati ya 5..20 itasababisha matokeo mazuri.

Tofauti kuu ya algorithm hii kutoka kwa kichungi cha Kalman ni kwamba uzito huu umesimamishwa, wakati katika chujio cha Kalman uzani husasishwa kabisa kulingana na kelele ya kipimo cha usomaji wa kasi. Kichujio cha Kalman kinazingatia kukupa matokeo bora ya kinadharia, wakati algorithm hii inaweza kukupa matokeo "ya kutosha" kwa programu yako ya vitendo. Unaweza kutekeleza algorithm inayobadilisha wGyro kulingana na sababu kadhaa za kelele ambazo hupima, lakini maadili yaliyowekwa yatatumika vizuri kwa matumizi mengi.

Tuko hatua moja kutoka kupata nambari zetu zilizokadiriwa kusasishwa:

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

Sasa wacha urekebishe vector hii tena:

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

Na tuko tayari kurudia kitanzi chetu tena.

Mwongozo huu awali ulionekana kwenye starlino.com, nimefanya marekebisho machache na kuiweka tena kwa idhini. Asante Starlino!