Արագացուցիչ և գիրո ձեռնարկ ՝ 3 քայլ

2024 Հեղինակ: John Day | [email protected]. Վերջին փոփոխված: 2024-01-30 09:53

Ներածություն

Այս ուղեցույցը նախատեսված է բոլոր նրանց համար, ովքեր հետաքրքրված են արագացուցիչներ և գիրոսկոպներ, ինչպես նաև համակցված IMU սարքեր (իներցիոն չափման միավոր) իրենց էլեկտրոնիկայի նախագծերում

Մենք լուսաբանելու ենք ՝

• Ի՞նչ է չափում արագացուցիչը:
• Ի՞նչ է չափում գիրոսկոպը (aka gyro):
• Ինչպես փոխարկել անալոգային-թվային (ADC) ընթերցումները, որոնք ստանում եք այս սենսորից ֆիզիկական միավորների (դրանք կլինեն արագացուցիչի համար g, գիրոսկոպի համար ՝ deg/s)
• Ինչպես համատեղել արագացուցիչի և գիրոսկոպի ընթերցումները `ձեր սարքի թեքության վերաբերյալ ճշգրիտ տեղեկություններ ստանալու համար

Հոդվածի ընթացքում ես կփորձեմ մաթեմատիկան նվազագույնի հասցնել: Եթե գիտեք, թե ինչ են Sine/Cosine/Tangent- ն, ապա պետք է կարողանաք հասկանալ և օգտագործել այս գաղափարները ձեր նախագծում ՝ անկախ նրանից, թե ինչ հարթակից եք օգտվում ՝ Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC և այլն:

Կան մարդիկ, ովքեր կարծում են, որ IMU միավորից օգտվելու համար ձեզ անհրաժեշտ են բարդ մաթեմատիկա (բարդ FIR կամ IIR զտիչներ, ինչպիսիք են Kalman ֆիլտրերը, Parks-McClellan զտիչները և այլն): Դուք կարող եք ուսումնասիրել այդ բոլորը և հասնել հրաշալի, բայց բարդ արդյունքների: Իրերը բացատրելու իմ ձևը պահանջում է միայն հիմնական մաթեմատիկա: Ես մեծ հավատացյալ եմ պարզությանը: Կարծում եմ, որ պարզ համակարգը ավելի հեշտ է վերահսկել և վերահսկել, բացի այդ, շատ ներդրված սարքեր չունեն ուժ և միջոցներ մատրիցային հաշվարկներ պահանջող բարդ ալգորիթմներ իրականացնելու համար:

Որպես օրինակ ես կօգտագործեմ IMU- ի նոր միավոր ՝ Acc_Gyro Accelerometer + Gyro IMU: Ստորև բերված մեր օրինակներում մենք կօգտագործենք այս սարքի պարամետրերը: Այս միավորը լավ սարք է սկսելու համար, քանի որ այն բաղկացած է 2 սարքից.

- LIS331AL (տվյալների թերթ) - եռակի առանցքային 2G արագացուցիչ - LPR550AL (տվյալների թերթ) - երկակի առանցքի սկիպիդար և գլան, 500 աստիճան/վրկ գիրոսկոպ

Նրանք միասին ներկայացնում են Ազատության իներցիոն չափման 5 աստիճանի միավոր: Հիմա դա շքեղ անուն է: Այնուամենայնիվ, շքեղ անվան հետևում շատ օգտակար համակցված սարք է, որը մենք կծածկենք և մանրամասն կբացատրենք այս ուղեցույցում:

Քայլ 1: Արագացուցիչ

Այս միավորը հասկանալու համար մենք կսկսենք արագացուցիչից: Արագացուցիչների մասին մտածելիս հաճախ օգտակար է պատկերել խորանարդի տեսքով տուփ, որի մեջ կա գնդակ: Դուք կարող եք պատկերացնել թխվածքաբլիթի կամ բլիթի նման այլ բան, բայց ես պատկերացնում եմ գնդակ.

Եթե մենք վերցնենք այս տուփը մի վայրում, որտեղ ձգողականության դաշտեր չկան, կամ այդ դեպքում առանց այլ դաշտերի, որոնք կարող են ազդել գնդակի դիրքի վրա, ապա գնդակը պարզապես լողալու է տուփի մեջտեղում: Դուք կարող եք պատկերացնել, որ արկղը գտնվում է տիեզերքում ՝ տիեզերական մարմիններից շատ հեռու, կամ եթե նման վայր դժվար է գտնել, պատկերացրեք գոնե մոլորակի շուրջը պտտվող տիեզերանավ, որտեղ ամեն ինչ անկշռելի վիճակում է: Վերևի նկարից կարող եք տեսնել, որ յուրաքանչյուր առանցքի մենք զույգ պատեր ենք վերագրում (մենք հեռացրել ենք պատը Y+, որպեսզի կարողանանք տուփի ներսում նայել): Պատկերացրեք, որ յուրաքանչյուր պատ զգայուն է ճնշման նկատմամբ: Եթե տուփը հանկարծակի տեղափոխենք ձախ (այն արագացնում ենք 1g = 9.8 մ/վ^2 արագացումով), գնդակը կհարվածի X- պատին: Այնուհետև մենք չափում ենք ճնշման ուժը, որը գնդակը կիրառում է պատին և թողնում -1 գ արժեք X առանցքի վրա:

Խնդրում ենք նկատի ունենալ, որ արագացուցիչը իրականում կբացահայտի ուժ, որն ուղղված է արագացման վեկտորից հակառակ ուղղությամբ: Այս ուժը հաճախ կոչվում է իներցիոն ուժ կամ մտացածին ուժ: Մի բան, որից պետք է սովորել, այն է, որ արագացուցիչը անուղղակիորեն չափում է արագացումը ուժի միջոցով, որը կիրառվում է նրա պատերից մեկի վրա (ըստ մեր մոդելի, դա կարող է լինել գարուն կամ այլ բան իրական կյանքի արագացուցիչների համար): Այս ուժը կարող է առաջանալ արագացումից, բայց ինչպես կտեսնենք հաջորդ օրինակում, դա միշտ չէ, որ առաջանում է արագացումից:

Եթե վերցնենք մեր մոդելը և այն դնենք Երկրի վրա, գնդակը կընկնի Z- պատի վրա և 1 գ ուժ կկիրառի ստորին պատին, ինչպես ցույց է տրված ստորև ներկայացված նկարում.

Այս դեպքում տուփը չի շարժվում, բայց մենք դեռ ստանում ենք -1 գ -ի ընթերցում Z առանցքի վրա: Theնշումը, որը գնդակը հասցրել էր պատին, առաջացել էր ձգողական ուժի պատճառով: Տեսականորեն դա կարող է լինել այլ տեսակի ուժ. Օրինակ, եթե պատկերացնում եք, որ մեր գնդակը մետաղական է, տուփի կողքին մագնիս դնելը կարող է գնդակը տեղափոխել այնպես, որ այն բախվի մեկ այլ պատի: Սա ասվեց միայն այն բանի համար, որ ապացուցվի, որ ըստ էության արագացուցիչի չափիչ ուժը ուժ է ոչ թե արագացմանը: Պարզապես պատահում է, որ արագացումը առաջացնում է իներցիոն ուժ, որը գրավվում է արագացուցիչի ուժի հայտնաբերման մեխանիզմով:

Թեև այս մոդելը հենց այն չէ, թե ինչպես է կառուցված MEMS սենսորը, այն հաճախ օգտակար է արագացուցիչի հետ կապված խնդիրների լուծման համար: Իրականում կան նման սենսորներ, որոնց ներսում մետաղական գնդակներ կան, դրանք կոչվում են թեք անջատիչներ, սակայն դրանք ավելի պարզունակ են և սովորաբար նրանք կարող են միայն ասել, արդյոք սարքը թեքված է ինչ -որ միջակայքում, թե ոչ, այլ ոչ թե թեքության աստիճանը:

Մինչ այժմ մենք վերլուծել ենք արագացուցիչի ելքը մեկ առանցքի վրա, և սա այն ամենն է, ինչ դուք կստանաք մեկ առանցքի արագացուցիչ սարքերով: Եռաչափ առանցքի արագացուցիչների իրական արժեքը գալիս է նրանից, որ նրանք կարող են իներցիոն ուժեր հայտնաբերել բոլոր երեք առանցքների վրա: Եկեք վերադառնանք մեր տուփի մոդելին և արկղը պտտենք 45 աստիճան դեպի աջ: Գնդակը հիմա կդիպչի 2 պատի ՝ Z- և X- ինչպես ցույց է տրված ստորև ներկայացված նկարում.

0.71 արժեքները կամայական չեն, դրանք իրականում մոտարկում են SQRT- ի համար (1/2): Սա ավելի պարզ կդառնա, երբ մենք ներկայացնում ենք արագացուցիչի մեր հաջորդ մոդելը:

Նախորդ մոդելում մենք ամրագրել ենք ձգողության ուժը և պտտել մեր երևակայական տուփը: Վերջին 2 օրինակներում մենք վերլուծել ենք ելքը տուփի 2 տարբեր դիրքերում, մինչդեռ ուժի վեկտորը մնացել է հաստատուն: Թեև դա օգտակար էր հասկանալու, թե ինչպես է արագացուցիչը փոխազդում արտաքին ուժերի հետ, ավելի գործնական է հաշվարկներ կատարել, եթե կոորդինատային համակարգը ամրացնենք արագացուցիչի առանցքներին և պատկերացնենք, որ ուժի վեկտորը պտտվում է մեր շուրջը:

Խնդրում ենք հայացք գցել վերը նշված մոդելի վրա, ես պահպանեցի առանցքների գույները, որպեսզի կարողանաք մտավոր անցում կատարել նախորդ մոդելից նորին: Պարզապես պատկերացրեք, որ նոր մոդելի յուրաքանչյուր առանցք ուղղահայաց է նախորդ մոդելի տուփի համապատասխան երեսներին: R վեկտորը ուժի վեկտորն է, որը չափում է արագացուցիչը (դա կարող է լինել կամ ձգողության ուժ, կամ իներցիոն ուժ վերը նշված օրինակներից կամ երկուսի համադրություն): Rx, Ry, Rz են R վեկտորի պրոյեկցիան X, Y, Z առանցքների վրա: Խնդրում ենք նկատել հետևյալ կապը.

R^2 = Rx^2 + Ry^2 + Rz^2 (հավասարություն 1)

որը հիմնականում 3D- ում Պյութագորասի թեորեմի համարժեքն է:

Հիշեք, որ մի փոքր ավելի վաղ ես ձեզ ասացի, որ SQRT (1/2) ~ 0.71 արժեքները պատահական չեն: Եթե դրանք միացնեք վերը նշված բանաձևին, ապա հիշելով, որ մեր ձգողական ուժը 1 գ էր, կարող ենք հաստատել, որ.

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

պարզապես փոխարինելով R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) հավասարության մեջ 1:

Տեսության երկար նախաբանից հետո մենք ավելի ենք մոտենում իրական կյանքի արագացուցիչներին: Rx, Ry, Rz արժեքներն իրականում գծայինորեն կապված են այն արժեքների հետ, որոնք ձեր իրական արագացուցիչը կհանի և որոնք կարող եք օգտագործել տարբեր հաշվարկներ կատարելու համար:

Նախքան այնտեղ հասնելը, եկեք մի փոքր խոսենք այն մասին, թե ինչպես արագացուցիչները մեզ կհասցնեն այս տեղեկատվությունը: Արագացուցիչ սարքերի մեծ մասը դասակարգվում է երկու կատեգորիայի ՝ թվային և անալոգային: Թվային արագացուցիչները ձեզ կտրամադրեն տեղեկություններ ՝ օգտագործելով սերիական արձանագրություն, ինչպիսիք են I2C, SPI կամ USART, մինչդեռ անալոգային արագաչափ սարքերը լարման մակարդակ են թողնում կանխորոշված տիրույթի սահմաններում, որը դուք պետք է փոխարկեք թվային արժեքի ՝ օգտագործելով ADC (անալոգային թվային փոխարկիչ) մոդուլը: Ես շատ չեմ մանրամասնի, թե ինչպես է ADC- ն աշխատում, մասամբ այն պատճառով, որ դա այնքան ծավալուն թեմա է և մասամբ այն պատճառով, որ այն տարբերվում է մի հարթակից մյուսը: Որոշ միկրոկառավարիչներ կունենան ներկառուցված ADC մոդուլներ, որոնցից մի քանիսին անհրաժեշտ կլինեն արտաքին բաղադրիչներ `ADC- ի փոխակերպումները կատարելու համար: Անկախ նրանից, թե ինչ տեսակի ADC մոդուլ եք օգտագործում, դուք որոշակի արժեքի արժեք կունենաք: Օրինակ, 10 -բիթանոց ADC մոդուլը կարտադրի արժեք 0..1023 տիրույթում, նշենք, որ 1023 = 2^10 -1: 12-բիթանոց ADC մոդուլը կարտադրի արժեք 0..4095 տիրույթում, նշենք, որ 4095 = 2^12-1:

Եկեք շարունակենք ՝ դիտարկելով մի պարզ օրինակ, ենթադրենք, որ մեր 10bit ADC մոդուլը մեզ տվեց հետևյալ արժեքները երեք արագացուցիչի ալիքների (առանցքների) համար.

AdcRx = 586 AdcRy = 630 AdcRz = 561

Յուրաքանչյուր ADC մոդուլ կունենա հղման լարման, ենթադրենք, որ մեր օրինակում դա 3.3V է: 10bit adc արժեքը լարման փոխարկելու համար մենք օգտագործում ենք հետևյալ բանաձևը.

VoltsRx = AdcRx * Vref / 1023

Մի արագ նշում այստեղ. Որ 8 բիթ ADC- ի համար վերջին բաժանարարը կլինի 255 = 2 ^ 8 -1, իսկ 12 բիթ ADC- ի դեպքում վերջին բաժանարարը կլինի 4095 = 2 ^ 12 -1:

Այս բանաձևը կիրառելով բոլոր 3 ալիքների վրա ՝ մենք ստանում ենք.

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (բոլոր արդյունքները կլորացնենք մինչև 2 տասնորդական միավոր) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = 8 1.81V

Յուրաքանչյուր արագացուցաչափ ունի զրո-գ լարման մակարդակ, այն կարող եք գտնել տեխնիկական բնութագրերում, սա այն լարումն է, որը համապատասխանում է 0 գ-ի: Ստորագրված լարման արժեք ստանալու համար մենք պետք է հաշվարկենք տեղաշարժը այս մակարդակից: Ենթադրենք, մեր 0 գ լարման մակարդակը VzeroG = 1.65V է: Մենք հաշվարկում ենք զրոյական լարման լարման տեղաշարժերը հետևյալ կերպ.

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

Այժմ մենք ունենք մեր արագացուցիչի ցուցանիշները վոլտերում, այն դեռ g- ում չէ (9.8 մ/վ^2), վերջնական փոխարկումը կատարելու համար կիրառում ենք արագացուցիչի զգայունությունը, որը սովորաբար արտահայտվում է մՎ/գ -ով: Եկեք ասենք, որ մեր զգայունությունը = 478.5 մՎ/գ = 0.4785 Վ/գ: Ensգայունության արժեքները կարելի է գտնել արագացուցիչի բնութագրերում: Գ -ով արտահայտված ուժի վերջնական արժեքները ստանալու համար մենք օգտագործում ենք հետևյալ բանաձևը.

Rx = DeltaVoltsRx / ensգայունություն

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

Մենք, իհարկե, կարող էինք համատեղել բոլոր քայլերը մեկ բանաձևի մեջ, բայց ես անցա բոլոր քայլերը `պարզ դարձնելու համար, թե ինչպես եք ADC- ի ընթերցումներից անցնում ուժի վեկտորի բաղադրիչին` արտահայտված g- ով:

Rx = (AdcRx * Vref / 1023 - VzeroG) / ensգայունություն (Eq.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / ensգայունություն Rz = (AdcRz * Vref / 1023 - VzeroG) / ensգայունություն

Այժմ մենք ունենք բոլոր երեք բաղադրիչները, որոնք սահմանում են մեր իներցիոն ուժի վեկտորը, եթե սարքը գրավիտացիայից բացի այլ ուժերի չի ենթարկվում, կարող ենք ենթադրել, որ սա մեր ձգողության ուժի վեկտորի ուղղությունն է: Եթե ցանկանում եք հաշվարկել սարքի թեքությունը գետնի նկատմամբ, կարող եք հաշվարկել անկյունը այս վեկտորի և Z առանցքի միջև: Եթե ձեզ նույնպես հետաքրքրում է թեքության մեկ առանցքի ուղղությունը, կարող եք այս արդյունքը բաժանել 2 բաղադրիչի. Թեքություն X և Y առանցքների վրա, որը կարելի է հաշվարկել որպես գրավիտացիոն վեկտորի և X / Y առանցքների միջև ընկած անկյուն: Այս անկյունների հաշվարկն ավելի պարզ է, քան դուք կարող եք մտածել, այժմ, երբ մենք հաշվարկել ենք Rx, Ry և Rz արժեքները: Եկեք վերադառնանք մեր վերջին արագացուցաչափի մոդելին և կատարենք լրացուցիչ նշումներ.

Մեզ հետաքրքրող անկյունները X, Y, Z առանցքների և ուժի վեկտորի միջև եղած անկյուններն են: Այս անկյունները կսահմանենք որպես Axr, Ayr, Azr: R և Rx ձևավորված ուղղանկյուն եռանկյունուց կարող եք նկատել, որ.

cos (Axr) = Rx / R, և նմանապես ՝ cos (Ayr) = Ry / R cos (Azr) = Rz / R

Մենք կարող ենք հավասարման 1 -ից հանել, որ R = SQRT (Rx^2 + Ry^2 + Rz^2):

Այժմ մենք կարող ենք գտնել մեր անկյունները ՝ օգտագործելով arccos () գործառույթը (հակադարձ cos () գործառույթը).

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

Մենք երկար ճանապարհ ենք անցել արագացուցիչի մոդելը բացատրելու համար, միայն թե հասնենք այս բանաձևերին: Կախված ձեր ծրագրերից, գուցե ցանկանաք օգտագործել ցանկացած միջանկյալ բանաձևեր, որոնք մենք ստացել ենք: Շուտով մենք կներկայացնենք նաև գիրոսկոպի մոդելը և կտեսնենք, թե ինչպես կարելի է արագաչափաչափի և գիրոսկոպի տվյալները համատեղել `թեքության ավելի ճշգրիտ գնահատումներ ապահովելու համար:

Բայց մինչ դա անելը, եկեք ավելի օգտակար նշումներ անենք.

cosX = cos (Axr) = Rx / R հարմարավետ = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

Այս եռյակը հաճախ կոչվում է Direction Cosine, և այն հիմնականում ներկայացնում է միավոր վեկտորը (1 երկարությամբ վեկտորը), որն ունի նույն ուղղությունը, ինչ մեր R վեկտորը: Դուք կարող եք հեշտությամբ հաստատել, որ.

SQRT (cosX^2 + հարմարավետ^2 + cosZ^2) = 1

Սա գեղեցիկ հատկություն է, քանի որ այն մեզ ազատում է R վեկտորի մոդուլի (երկարության) մոնիտորինգից: Հաճախ, եթե մեզ պարզապես հետաքրքրում է մեր իներցիոն վեկտորի ուղղությունը, իմաստ ունի նորմալացնել դրա մոդուլը `այլ հաշվարկները պարզեցնելու համար:

Քայլ 2: գիրոսկոպ

Մենք չենք պատրաստվում գիրոսկոպի համար տուփի համարժեք մոդել ներկայացնել, ինչպես արագացուցիչի դեպքում, փոխարենը մենք անմիջապես կանցնենք երկրորդ արագացուցիչի մոդելը և ցույց կտանք, թե ինչ է չափում գիրոսկոպը այս մոդելի համաձայն:

Յուրաքանչյուր գիրոսկոպ ալիք չափում է առանցքներից մեկի շուրջ պտույտը: Օրինակ, 2 առանցքի գիրոսկոպը չափելու է X և Y առանցքների շուրջ պտտումը (կամ ոմանք կարող են ասել «մոտ»): Այս պտույտը թվերով արտահայտելու համար եկեք որոշ նշումներ անենք: Նախ սահմանենք.

Rxz - դա իներցիոն ուժի վեկտորի պրոյեկցիան է XZ հարթության վրա Ryz - դա իներցիոն ուժի վեկտորի պրոյեկցիան է YZ հարթության վրա

Rxz- ի և Rz- ի կողմից ձևավորված ուղղանկյուն եռանկյունուց, օգտագործելով Պյութագորասի թեորեմը, մենք ստանում ենք.

Rxz^2 = Rx^2 + Rz^2, և նմանապես ՝ Ryz^2 = Ry^2 + Rz^2

նաև նշենք, որ.

R^2 = Rxz^2 + Ry^2, սա կարող է բխել հավասարման 1-ից և վերևից ստացված հավասարումներից, կամ այն կարող է բխել R- ից և Ryz- ից ձևավորված ուղղանկյուն եռանկյունուց R = 2 = Ryz^2 + Rx^2

Մենք չենք պատրաստվում օգտագործել այս բանաձևերը այս հոդվածում, բայց օգտակար է նշել մեր մոդելի բոլոր արժեքների միջև կապը:

Փոխարենը մենք որոշելու ենք Z առանցքի և Rxz, Ryz վեկտորների միջև եղած անկյունը հետևյալ կերպ.

Axz - դա Rxz- ի (X- ի հարթության վրա R- ի պրոյեկցիայի) և Z առանցքի Ayz- ի անկյունն է Ryz- ի (R- ի կանխատեսումը YZ հարթության վրա) և Z առանցքի միջև:

Այժմ մենք մոտենում ենք գիրոսկոպի չափածին: Գիրոսկոպը չափում է վերը սահմանված անկյունների փոփոխությունների արագությունը: Այլ կերպ ասած, այն կարտադրի մի արժեք, որը գծայինորեն կապված է այդ անկյունների փոփոխության արագության հետ: Սա բացատրելու համար ենթադրենք, որ մենք t0 պահին չափել ենք Y առանցքի շուրջ պտտվող անկյունը (դա կլինի Axz անկյուն), և մենք այն սահմանում ենք որպես Axz0, այնուհետև մենք չափել ենք այս անկյունը t1 ժամանակում և դա Axz1 էր: Փոփոխության տոկոսադրույքը հաշվարկվելու է հետևյալ կերպ.

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

Եթե Axz- ը արտահայտենք աստիճաններով, իսկ ժամանակը վայրկյաններով, ապա այս արժեքը կարտահայտվի deg/s- ով: Ահա թե ինչ է չափում գիրոսկոպը:

Գործնականում գիրոսկոպը (եթե դա հատուկ թվային գիրոսկոպ չէ) հազվադեպ կտա ձեզ մի աստիճան/վ արտահայտված արժեք: Նույնը, ինչ արագացուցիչի դեպքում, դուք կստանաք ADC արժեք, որը ձեզ հարկավոր է փոխակերպել deg/s- ի ՝ օգտագործելով հավասարման նման բանաձև: 2, որը մենք սահմանել ենք արագացուցիչի համար: Եկեք ծանոթացնենք ADC- ին գիրոսկոպի deg/s- ի փոխակերպման բանաձևին (ենթադրում ենք, որ մենք օգտագործում ենք 10bit ADC մոդուլ, 8bit ADC- ի համար 1023 -ը փոխարինեք 255 -ով, 12bit- ի ADC- ի դեպքում 1023 -ը փոխարինեք 4095 -ով):

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensitivity Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / ensգայունություն

AdcGyroXZ, AdcGyroYZ - ստացվել են մեր adc մոդուլից և դրանք ներկայացնում են այն ալիքները, որոնք չափում են R վեկտորի նախագծման պտույտը համապատասխանաբար XZ հարթություններում, ինչը համարժեք է ասել, որ պտույտը կատարվել է համապատասխանաբար Y և X առանցքների շուրջ:

Vref - դա ADC- ի հղման լարումն է, որը մենք կօգտագործենք 3.3V- ից VzeroRate- ի ստորև բերված օրինակում. օրինակ 1.23V (այս արժեքները կարող եք գտնել բնութագրերում) ensգայունություն - դա ձեր գիրոսկոպի զգայունությունն է, որն արտահայտվում է mV / (deg / s) հաճախ գրված mV / deg / s- ով, այն հիմնականում պատմում է ձեզ, թե քանի մՎ կլինի գիրոսկոպի ելքը մեծանում է, եթե բարձրացնեք պտույտի արագությունը մեկ աստիճան/վրկ -ով: Acc_Gyro տախտակի զգայունությունը, օրինակ, 2mV/deg/s կամ 0.002V/deg/s է

Եկեք օրինակ վերցնենք, ենթադրենք, որ մեր ADC մոդուլը վերադարձրեց հետևյալ արժեքները.

AdcGyroXZ = 571 AdcGyroXZ = 323

Օգտագործելով վերը նշված բանաձևը և օգտագործելով Acc_Gyro տախտակի բնութագրերի պարամետրերը, մենք կստանանք.

RateAxz = (571 * 3.3V/1023 - 1.23V)/(0.002V/deg/s) = ~ 306 deg/s RateAyz = (323 * 3.3V/1023 - 1.23V)/(0.002V/deg/վ) = ~ -94 աստիճան/վ

Այլ կերպ ասած, սարքը պտտվում է Y առանցքի շուրջը (կամ կարելի է ասել, որ այն պտտվում է XZ հարթությունում) 306 դեգ/վ արագությամբ և X առանցքի շուրջ (կամ կարելի է ասել, որ այն պտտվում է YZ հարթությունում) ` - արագությամբ: 94 աստիճան/վ Խնդրում ենք նկատի ունենալ, որ բացասական նշանը նշանակում է, որ սարքը սովորական դրական ուղղությամբ հակառակ ուղղությամբ է պտտվում: Ըստ պայմանագրի ՝ պտտման մեկ ուղղությունը դրական է: Գիրոսկոպի հստակեցման թերթիկը ձեզ ցույց կտա, թե որ ուղղությունն է դրական, հակառակ դեպքում դուք պետք է այն գտնեք ՝ փորձարկելով սարքը և նշելով, թե պտտման ո՞ր ուղղությունն է հանգեցնում ելքային քորոցի լարման բարձրացման: Դա լավագույնս արվում է օսլիլոսկոպի միջոցով, քանի որ պտույտը դադարեցնելուն պես լարումը նորից կիջնի զրոյական մակարդակի: Եթե դուք օգտագործում եք մուլտիմետր, ապա պետք է պահպանեք պտույտի կայուն արագությունը առնվազն մի քանի վայրկյան և նշեք լարումը այս պտույտի ընթացքում, այնուհետև համեմատեք այն զրոյական լարման հետ: Եթե այն զրոյական լարման ավելի մեծ է, նշանակում է, որ պտույտի ուղղությունը դրական է:

Քայլ 3. Արագացուցիչի և գիրոյի համատեղում

Այս ամենը միասին դնելը `արագացուցիչի և գիրոսկոպի տվյալների համադրություն:

Եթե կարդում եք այս հոդվածը, հավանաբար ձեռք եք բերել կամ մտադիր եք ձեռք բերել IMU սարք, կամ, հավանաբար, նախատեսում եք այն սարքել առանձին արագացուցաչափ և գիրոսկոպ սարքերից:

Արագացուցիչ և գիրոսկոպ համատեղող IMU սարքի օգտագործման առաջին քայլը նրանց կոորդինատային համակարգերի հավասարեցումն է: Դա անելու ամենահեշտ ձևը արագացուցիչի կոորդինատային համակարգի ընտրությունն է որպես ձեր տեղեկատու կոորդինատային համակարգ: Արագացուցիչի տվյալների թերթերի մեծ մասը կցուցադրի X, Y, Z առանցքների ուղղությունը ՝ համեմատած ֆիզիկական չիպի կամ սարքի պատկերի հետ: Օրինակ ՝ այստեղ են X, Y, Z առանցքների ուղղությունները, ինչպես ցույց է տրված Acc_Gyro տախտակի բնութագրերում.

Հաջորդ քայլերն են.

Բացահայտեք գիրոսկոպի ելքերը, որոնք համապատասխանում են վերը քննարկված RateAxz, RateAyz արժեքներին: Որոշեք, թե արդյոք այդ ելքերը պետք է շրջված լինեն արագաչափի նկատմամբ գիրոսկոպի ֆիզիկական դիրքի պատճառով

Մի՛ ենթադրեք, որ եթե գիրոսկոպն ունի X կամ Y նշանի ելք, այն կհամապատասխանի արագացուցիչի կոորդինատային համակարգի ցանկացած առանցքին, նույնիսկ եթե այս ելքը IMU միավորի մաս է կազմում: Լավագույն միջոցը դա փորձարկելն է: Ենթադրելով, որ դուք ամրագրել եք գիրոսկոպի դիրքը արագացուցիչի համեմատ: Ենթադրվում է, որ գիրո և արագացուցիչի սահմանները միմյանց զուգահեռ են, այսինքն ՝ դուք գիրոն տեղադրում եք 90 աստիճանի բազմապատիկ անկյունում ՝ արագացուցիչի չիպի համեմատ: Եթե դուք ձեռք եք բերել IMU- ի խորհուրդ, հավանականությունն այն է, որ դրանք արդեն համահունչ են այս կերպ: Այս հոդվածում մենք չենք քննարկի մոդելներ, որտեղ գիրոսկոպը տեղադրված է արագացուցիչի նկատմամբ անկանոն անկյան տակ (ասենք 45 կամ 30 աստիճան), չնայած դա կարող է օգտակար լինել որոշ ծրագրերում:

Ահա օրինակելի հաջորդականություն `որոշելու, թե գիրոսկոպի որ ելքը համապատասխանում է վերը քննարկված RateAxz արժեքին:

- սկսեք սարքը հորիզոնական դիրքում դնելուց: Արագացուցիչի X և Y ելքերը կհանգեցնեն զրո-գ լարման (օրինակ, Acc_Gyro տախտակի համար սա 1.65V է)

- հաջորդը սկսեք սարքը պտտել Y առանցքի շուրջը, ասելու մեկ այլ եղանակ ՝ սարքը պտտել XZ հարթությունում, այնպես որ X և Z արագացուցիչի ելքերը փոխվեն, իսկ Y ելքը մնա հաստատուն: - երբ սարքը պտտվում է անընդհատ արագությամբ, որի գիրոսկոպի ելքը փոխվում է, մյուս գիրոսկոպի ելքերը պետք է մնան անփոփոխ. գիրոսկոպի ելքը, որը փոխվել է Y առանցքի շուրջ պտտման ժամանակ (պտույտ XZ հարթությունում), AdcGyroXZ- ի մուտքային արժեքը կապահովի: մենք հաշվարկում ենք RateAxz - վերջնական քայլն այն է, որ պտտման ուղղությունը համապատասխանի մեր մոդելին, որոշ դեպքերում գուցե ստիպված լինեք RateAxz արժեքը շրջել արագաչափի նկատմամբ գիրոսկոպի ֆիզիկական դիրքի պատճառով. կրկին կատարել վերը նշված թեստը ՝ սարքը պտտելով Y առանցքը, այս անգամ վերահսկել արագացուցիչի X ելքը (մեր մոդելում AdcRx): Եթե AdcRx աճում է (հորիզոնական դիրքից պտտման առաջին 90 աստիճանը), ապա AdcGyroXZ- ը նույնպես պետք է աճի: Հակառակ դեպքում դուք պետք է շրջեք RateAxz- ը, դուք կարող եք հասնել դրան ՝ Eq.3- ում նշանի գործոն մտցնելով, հետևյալ կերպ.

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / ensգայունություն, որտեղ InvertAxz- ը 1 կամ -1 է

նույն թեստը կարող է արվել RateAyz- ի համար ՝ սարքը պտտելով X առանցքի շուրջ, և կարող եք որոշել, թե որ գիրոսկոպի ելքը է համապատասխանում RateAyz- ին, և եթե այն պետք է շրջված լինի: InvertAyz- ի արժեքը ստանալուց հետո RateAyz- ը հաշվարկելու համար պետք է օգտագործել հետևյալ բանաձևը.

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / ensգայունություն

Եթե դուք անեիք այս թեստերը Acc_Gyro տախտակի վրա, դուք կստանայիք հետևյալ արդյունքները.

- RateAxz- ի ելքային կապը GX4 է և InvertAxz = -1: - RateAyz- ի ելքային կապը GY4 է և InvertAyz = -1

Այս պահից մենք կհամարենք, որ դուք ձեր IMU- ն կարգավորել եք այնպես, որ կարողանաք ճիշտ արժեքներ հաշվարկել Axr, Ayr, Azr (ինչպես սահմանված է Մաս 1 արագացուցիչ) և RateAxz, RateAyz (ինչպես սահմանված է Մաս 2 -ում):): Հաջորդը, մենք կվերլուծենք հարաբերությունները այս արժեքների միջև, որոնք օգտակար են դառնում սարքի թեքության առավել ճշգրիտ գնահատման համար `գետնին հարթության նկատմամբ:

Գուցե այս պահին ինքներդ ձեզ հարցնեք, եթե արագացուցիչի մոդելը մեզ արդեն տվել է Axr, Ayr, Azr թեքության անկյունները, ինչու՞ մենք կցանկանայինք անհանգստանալ գիրոսկոպի տվյալներով: Պատասխանը պարզ է. Արագացուցիչի տվյալները միշտ չէ, որ կարող են վստահվել 100%-ով: Կան մի քանի պատճառ, հիշեք, որ արագացուցիչը չափում է իներցիոն ուժը, այդպիսի ուժը կարող է առաջանալ ձգողության (և իդեալական դեպքում միայն գրավիտացիայի) պատճառով, բայց դա կարող է նաև առաջանալ սարքի արագացման (շարժման) պատճառով: Արդյունքում, նույնիսկ եթե արագացուցիչը գտնվում է համեմատաբար կայուն վիճակում, այն դեռևս շատ զգայուն է թրթռումների և ընդհանրապես մեխանիկական աղմուկի նկատմամբ: Սա հիմնական պատճառն է, թե ինչու IMU համակարգերի մեծ մասը կիրառում են գիրոսկոպը `արագացուցիչի ցանկացած սխալ հարթելու համար: Բայց ինչպե՞ս է դա արվում: Եվ արդյո՞ք գիրոսկոպը զերծ է աղմուկից:

Գիրոսկոպը զերծ չէ աղմուկից, քանի որ չափում է պտույտը, այն ավելի քիչ զգայուն է գծային մեխանիկական շարժումների նկատմամբ, աղմուկի այն տեսակից, որից տառապում է արագացուցիչը, սակայն գիրոսկոպներն ունեն այլ տիպի խնդիրներ, օրինակ `դրեյֆը (զրոյական արժեքի չվերադառնալը): երբ պտույտը դադարում է): Այնուամենայնիվ, արագացուցիչից և գիրոսկոպից ստացված տվյալների միջինացման միջոցով մենք կարող ենք ձեռք բերել սարքի ընթացիկ թեքության համեմատաբար ավելի լավ գնահատական, քան կստանայինք միայն արագացուցիչի տվյալների օգտագործմամբ:

Հաջորդ քայլերում ես կներկայացնեմ մի ալգորիթմ, որը ներշնչված է եղել Կալման ֆիլտրում օգտագործված որոշ գաղափարներով, սակայն ներդրված սարքերում այն շատ ավելի պարզ և հեշտ է իրականացնել: Մինչ այդ եկեք նախ տեսնենք, թե ինչ ենք ուզում մեր ալգորիթմը հաշվարկել: Դե, դա գրավիտացիոն ուժի վեկտորի ուղղությունն է R = [Rx, Ry, Rz], որից մենք կարող ենք այլ արժեքներ ստանալ, ինչպիսիք են Axr, Ayr, Azr կամ cosX, coz, cosZ, որը մեզ պատկերացում կտա մեր սարքի թեքության մասին հողային հարթության համեմատ, մենք քննարկում ենք այս արժեքների միջև կապը Մաս 1 -ում: Կարելի է ասել. արդյո՞ք մենք արդեն չունենք այս արժեքները Rx, Ry, Rz հավասարման 2 -ից 1 -ին մասում: Դե, այո, բայց հիշեք, որ այս արժեքները բխում են միայն արագացուցիչի տվյալներից, այնպես որ, եթե դրանք ուղղակիորեն օգտագործեք ձեր դիմումի մեջ, կարող եք ավելի շատ աղմուկ ստանալ, քան ձեր դիմումը կարող է հանդուրժել: Հետագա շփոթությունից խուսափելու համար եկեք արագաչափի չափումները վերստին սահմանենք հետևյալ կերպ.

Racc - արագացուցիչով չափվող իներցիոն ուժի վեկտորն է, որը բաղկացած է հետևյալ բաղադրիչներից (կանխատեսումներ X, Y, Z առանցքների վրա).

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / ensգայունություն RyAcc = (AdcRy * Vref / 1023 - VzeroG) / ensգայունություն RzAcc = (AdcRz * Vref / 1023 - VzeroG) / ensգայունություն

Մինչ այժմ մենք ունենք չափված արժեքների մի շարք, որոնք կարող ենք ստանալ զուտ արագացուցիչի ADC արժեքներից: Մենք այս տվյալների հավաքածուն կանվանենք «վեկտոր» և կօգտագործենք հետևյալ նշումը:

Racc = [RxAcc, RyAcc, RzAcc]

Քանի որ Racc- ի այս բաղադրիչները կարելի է ձեռք բերել արագացուցիչի տվյալներից, մենք կարող ենք այն համարել մեր ալգորիթմի մուտքը:

Խնդրում ենք նկատի ունենալ, որ քանի որ Racc- ը չափում է ձգողության ուժը, դուք ճիշտ կլինեք, եթե ենթադրեք, որ հետևյալ վեկտորի երկարությունը հավասար է կամ մոտ է 1 գ -ին:

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Այնուամենայնիվ, վստահ լինելու համար իմաստ ունի թարմացնել այս վեկտորը հետևյալ կերպ.

Racc (նորմալացված) = [RxAcc/| Racc |, RyAcc/| Racc |, RzAcc/| Racc |]:

Սա կապահովի ձեր նորմալացված Racc վեկտորի երկարությունը միշտ 1:

Հաջորդը մենք կներկայացնենք նոր վեկտոր և այն կկոչենք

Հանգիստ = [RxEst, RyEst, RzEst]

Սա կլինի մեր ալգորիթմի ելքը, դրանք ուղղված արժեքներ են ՝ հիմնված գիրոսկոպի տվյալների վրա և հիմնված անցյալի գնահատված տվյալների վրա:

Ահա, թե ինչ է անելու մեր ալգորիթմը. մենք թողարկում ենք նոր գնահատված վեկտորի հանգիստը: - մենք հանգստությունը համարում ենք մեր «լավագույն խաղադրույքը» սարքի ներկա դիրքի վերաբերյալ:

Եկեք տեսնենք, թե ինչպես կարող ենք այն գործի դնել:

Մենք կսկսենք մեր հաջորդականությունը ՝ վստահելով մեր արագացուցիչին և նշանակելով.

Հանգիստ (0) = Racc (0)

Ի դեպ, հիշեք, որ Rest- ը և Racc- ը վեկտորներ են, ուստի վերը նշված հավասարումը պարզապես պարզ միջոց է `գրել հավասարումների 3 հավաքածու և խուսափել կրկնությունից:

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

Հաջորդը մենք կանոնավոր չափումներ կկատարենք T վայրկյան հավասար ժամանակային ընդմիջումներով և կստանանք նոր չափումներ, որոնք մենք կսահմանենք որպես Racc (1), Racc (2), Racc (3) և այլն: Մենք նաև կտրամադրենք նոր գնահատականներ յուրաքանչյուր ժամանակային ընդմիջումներով Հանգիստ (1), Հանգիստ (2), Հանգիստ (3) և այլն:

Ենթադրենք, մենք գտնվում ենք n քայլում: Մենք ունենք արժեքների երկու հայտնի հավաքածու, որոնք մենք կցանկանայինք օգտագործել.

Հանգիստ (n -1) - մեր նախորդ գնահատականը, հանգստությամբ (0) = Racc (0) Racc (n) - մեր ընթացիկ արագացուցիչի չափումը

Նախքան Հանգիստը (n) հաշվարկելը, եկեք ներկայացնենք նոր չափված արժեք, որը կարող ենք ձեռք բերել մեր գիրոսկոպից և նախորդ գնահատականից:

Մենք այն կանվանենք Ռգիրո, և այն նաև վեկտոր է, որը բաղկացած է 3 բաղադրիչից.

Ռգիրո = [RxGyro, RyGyro, RzGyro]

Այս վեկտորը մենք կհաշվենք միաժամանակ: Մենք կսկսենք RxGyro- ով:

Սկսենք ՝ մեր գիրոսկոպի մոդելում դիտելով հետևյալ հարաբերությունները, Rz- ի և Rxz- ի կողմից ձևավորված ուղղանկյուն եռանկյունուց մենք կարող ենք դա բխել.

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

Atan2- ը կարող է լինել այնպիսի գործառույթ, որը դուք նախկինում չէիք օգտագործել, այն նման է atan- ին, բացառությամբ, որ վերադարձնում է արժեքները (-PI, PI) միջակայքում, ի տարբերություն (-PI/2, PI/2), ինչպես վերադարձվում է atan- ի կողմից, և դա տևում է 2 փաստարկ մեկի փոխարեն: Այն թույլ է տալիս մեզ փոխարկել Rx, Rz երկու արժեքները 360 աստիճանի ամբողջ տիրույթում գտնվող անկյուններին (-PI- ից PI): Atan2- ի մասին ավելին կարող եք կարդալ այստեղ:

Այսպիսով, իմանալով RxEst (n-1) և RzEst (n-1), կարող ենք գտնել.

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

Հիշեք, որ գիրոսկոպը չափում է Axz անկյան փոփոխության արագությունը: Այսպիսով, մենք կարող ենք գնահատել Axz (n) նոր անկյունը հետևյալ կերպ.

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

Հիշեք, որ RateAxz- ը կարելի է ձեռք բերել մեր գիրոսկոպի ADC ընթերցումներից: Ավելի ճշգրիտ բանաձևը կարող է օգտագործել ռոտացիայի միջին արագությունը ՝ հաշվարկված հետևյալ կերպ.

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

Նույն կերպ մենք կարող ենք գտնել.

Այզ (n) = Այզ (n-1) + RateAyz (n) * T

Լավ, հիմա մենք ունենք Axz (n) և Ayz (n): Որտե՞ղ ենք գնում այստեղից RxGyro/RyGyro հանելու համար: Հավասարությունից 1 մենք կարող ենք գրել Rgyro վեկտորի երկարությունը հետևյալ կերպ.

| Ռգիրո | = SQRT (RxGyro^2 + RyGyro^2 + RzGyro^2)

Նաև այն պատճառով, որ մենք նորմալացրինք մեր Racc վեկտորը, կարող ենք ենթադրել, որ դրա երկարությունը 1 է և այն չի փոխվել պտույտից հետո, ուստի համեմատաբար անվտանգ է գրել.

| Ռգիրո | = 1

Ստորև բերված հաշվարկների համար ընդունենք ժամանակավոր կարճ նշումներ.

x = RxGyro, y = RyGyro, z = RzGyro

Օգտագործելով վերը նշված հարաբերությունները, մենք կարող ենք գրել.

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

Կոտորակի համարիչն ու հայտարարը բաժանենք SQRT- ով (x^2 + z^2)

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

Նշենք, որ x / SQRT (x^2 + z^2) = մեղք (Axz), այնպես որ.

x = մեղք (Axz) / SQRT (1 + y^2 / (x^2 + z^2))

Այժմ բազմապատկեք SQRT- ի ներսում կոտորակի համարիչն ու հայտարարը z^2 -ով

x = մեղք (Axz) / SQRT (1 + y^2 * z^2 / (z^2 * (x^2 + z^2)))

Նշենք, որ z / SQRT (x^2 + z^2) = cos (Axz) և y / z = tan (Ayz), ուստի վերջապես.

x = մեղք (Axz) / SQRT (1 + cos (Axz)^2 * tan (Ayz)^2)

Վերադառնալով մեր նշմանը, մենք ստանում ենք.

RxGyro = մեղք (Axz (n)) / SQRT (1 + cos (Axz (n))^2 * tan (Ayz (n))^2)

նույն կերպ մենք դա գտնում ենք

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

Այժմ, վերջապես, մենք կարող ենք գտնել.

RzGyro = Նշան (RzGyro)*SQRT (1 - RxGyro^2 - RyGyro^2):

Որտեղ Նշան (RzGyro) = 1, երբ RzGyro> = 0, և Sign (RzGyro) = -1, երբ RzGyro <0:

Սա գնահատելու մեկ պարզ միջոց է վերցնել.

Նշան (RzGyro) = Նշան (RzEst (n-1))

Գործնականում զգույշ եղեք, երբ RzEst (n-1)-ը մոտ է 0-ին: Այս դեպքում կարող եք ընդհանրապես բաց թողնել գիրո փուլը և նշանակել. Rgyro = Հանգիստ (n-1): Rz- ն օգտագործվում է որպես հղում Axz և Ayz անկյունները հաշվարկելու համար, և երբ այն մոտ է 0 -ին, արժեքները կարող են լցվել և առաջացնել վատ արդյունքներ: Դուք կգտնվեք մեծ լողացող կետերի թվերի տիրույթում, որտեղ tan () / atan () ֆունկցիայի իրականացումը կարող է ճշգրտության բացակայություն ունենալ:

Այսպիսով, եկեք ամփոփենք այն, ինչ ունենք մինչ այժմ, մենք գտնվում ենք մեր ալգորիթմի n քայլում և հաշվարկել ենք հետևյալ արժեքները.

Racc - ընթացիկ ընթերցումներ մեր արագացուցաչափ Rgyro- ից - ստացված Rest (n -1) և ընթացիկ գիրոսկոպի ընթերցումներից

Ո՞ր արժեքներն ենք մենք օգտագործում `հանգստանալու (n) նորացված գնահատականը հաշվարկելու համար: Դուք հավանաբար կռահեցիք, որ մենք երկուսն էլ կօգտագործենք: Մենք կօգտագործենք միջին կշռված, որպեսզի ՝

Հանգիստ (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Մենք կարող ենք պարզեցնել այս բանաձևը ՝ կոտորակի համարիչն ու հայտարարը բաժանելով w1- ի:

Հանգիստ (n) = (Racc * w1/w1 + Rgyro * w2/w1)/(w1/w1 + w2/w1)

և w2/w1 = wGyro փոխարինելուց հետո ստանում ենք.

Հանգիստ (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

Վերոնշյալ ֆորումում wGyro- ն մեզ ասում է, թե որքանով ենք մենք վստահում մեր գիրոսկոպին ՝ մեր արագացուցիչի համեմատ: Այս արժեքը կարող է ընտրվել փորձարարականորեն: սովորաբար 5..20 -ի միջև արժեքները լավ արդյունքներ կբերեն:

Այս ալգորիթմի հիմնական տարբերությունը Կալմանի ֆիլտրից այն է, որ այս քաշը համեմատաբար ամրագրված է, մինչդեռ Կալմանի ֆիլտրում կշիռները մշտապես թարմացվում են ՝ ըստ արագացուցիչի ընթերցումների չափված աղմուկի: Kalman ֆիլտրը կենտրոնացած է ձեզ «լավագույն» տեսական արդյունքներ տալու վրա, մինչդեռ այս ալգորիթմը կարող է ձեզ տալ «բավական լավ արդյունքներ» ձեր գործնական կիրառման համար: Դուք կարող եք ներդնել ալգորիթմ, որը կարգավորում է wGyro- ն ՝ կախված որոշ չափվող աղմուկի գործոններից, սակայն ֆիքսված արժեքները լավ կաշխատեն շատ ծրագրերի համար:

Մենք մեկ քայլ հեռու ենք մեր թարմացված գնահատված արժեքները ստանալուց.

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

Հիմա եկեք նորից նորմալացնենք այս վեկտորը.

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

Եվ մենք պատրաստ ենք նորից կրկնել մեր օղակը:

Այս ուղեցույցը սկզբնապես հայտնվեց starlino.com կայքում, ես մի քանի թեթև խմբագրումներ եմ կատարել և նորից տեղադրել եմ թույլտվությամբ: Շնորհակալություն Starlino!