Акселерометр жана гиро үйрөткүчү: 3 кадам

2024 Автор: John Day | [email protected]. Акыркы өзгөртүү: 2024-01-30 10:44

Киришүү

Бул колдонмо акселерометрлерди жана гироскопторду, ошондой эле IMU бириккен түзмөктөрүн (инерциалдык өлчөө бирдиги) электрондук долбоорлорунда колдонууга кызыккан ар бир адамга арналган.

Биз камтыйбыз:

• Акселерометр эмнени өлчөйт?
• Гироскоп (ака гиро) эмнени өлчөйт?
• Бул сенсордон алган аналогдук-санариптик (ADC) көрсөткүчтөрдү физикалык бирдиктерге кантип которуу керек (бул акселерометр үчүн г, гироскоп үчүн deg/s болмок)
• Жердин тегиздигине карата түзмөгүңүздүн эңкейиши жөнүндө так маалымат алуу үчүн акселерометр менен гироскоптун көрсөткүчтөрүн кантип айкалыштыруу керек

Макалада мен математиканы минимумга чейин сактоого аракет кылам. Эгерде сиз Sine/Cosine/Tangent деген эмне экенин билсеңиз, анда сиз кайсы платформаны колдонбосоңуз дагы, бул идеяларды долбооруңузда түшүнүп жана колдоно билишиңиз керек: Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC ж.

IMU бирдигин колдонуу үчүн татаал математика керек деп эсептеген адамдар бар (Калман чыпкалары, Паркс-Макклеллан чыпкалары ж. Сиз мунун баарын изилдеп, сонун, бирок татаал натыйжаларга жетише аласыз. Менин нерселерди түшүндүрүү жолум жөнөкөй математиканы талап кылат. Мен жөнөкөйлүккө чоң ишенген адаммын. Менимче, жөнөкөй системаны көзөмөлдөө жана көзөмөлдөө оңой, башкача айтканда, көптөгөн камтылган түзүлүштөр матрицалык эсептөөлөрдү талап кылган татаал алгоритмдерди ишке ашыруу үчүн күчкө жана ресурстарга ээ эмес.

Мен мисал катары жаңы IMU бирдигин колдоном, Acc_Gyro Accelerometer + Gyro IMU. Бул түзмөктүн параметрлерин төмөндөгү мисалдарда колдонобуз. Бул түзмөк баштоо үчүн жакшы түзмөк, анткени ал 2 түзмөктөн турат:

- LIS331AL (маалымат баракчасы) - үч фазалуу 2G акселерометр - LPR550AL (маалымат баракчасы) - эки октук кадам жана ролл, 500 град/сек гироскоп

Алар чогуу 5-даражадагы Эркиндиктин инерциялык өлчөө бирдигин билдирет. Эми бул кооз ат! Ошентсе да, кооз аталыштын артында бул колдонмодо кеңири түшүндүрө турган абдан пайдалуу айкалышкан түзмөк турат.

1 -кадам: Акселерометр

Бул бирдикти түшүнүү үчүн акселерометр менен баштайбыз. Акселерометрлер жөнүндө ойлонгондо, көбүнчө ичиндеги шар менен куб формасындагы кутуну элестетүү пайдалуу болот. Сиз печенье же пончик сыяктуу башка нерсени элестете аласыз, бирок мен топту элестетем:

Эгерде биз бул кутуну гравитациялык талаалары жок жерге алып барсак же бул үчүн топтун абалына таасир этиши мүмкүн болгон башка талаалары жок болсо - топ жөн эле кутунун ортосунда сүзүп кетет. Сиз кутуча космостук денелерден алыс аралыкта экенин элестете аласыз, же андай жерди табуу кыйын болсо, жок дегенде планетанын айланасында айланып бараткан космостук кемени элестетип көрүңүз. Жогорудагы сүрөттөн биз ар бир огко бир жуп дубал ыйгарып жатканыбызды көрө алабыз (биз кутучанын ичин карай алышыбыз үчүн Y+ дубалын алып салдык). Ар бир дубал басымга сезгич экенин элестетиңиз. Эгерде биз кутуну күтүүсүздөн солго жылдырсак (биз аны 1g = 9.8m/s^2 ылдамдатуу менен ылдамдайбыз), топ дубалга X- тийет. Биз андан кийин топтун дубалга тиешелүү басым күчүн өлчөп, X огунда -1г маанисин чыгарабыз.

Сураныч, акселерометр ылдамдануу векторунан карама -каршы багытталган күчтү аныктайт. Бул күч көбүнчө инерциялык күч же ойдон чыгарылган күч деп аталат. Мындан бир нерсени үйрөнүшүң керек: акселерометр анын дубалдарынын бирине колдонулган күч аркылуу кыйыр түрдө ылдамдатууну өлчөйт (биздин моделге ылайык, бул реалдуу жашоо акселерометрлеринде жаз же башка нерсе болушу мүмкүн). Бул күч ылдамдануудан келип чыгышы мүмкүн, бирок кийинки мисалда көрө турганыбыздай, ал дайыма ылдамдануудан эмес.

Эгерде биз өзүбүздүн моделибизди алып, жерге койсок, анда топ Z- дубалына түшөт жана төмөнкү дубалда 1г күчтү колдонот, төмөнкү сүрөттө көрсөтүлгөндөй:

Бул учурда куту жылбай жатат, бирок биз дагы деле Z огунда -1g көрсөткүчүн алабыз. Топтун дубалга тийгизген басымы тартылуу күчүнөн келип чыккан. Теорияда бул башка күч болушу мүмкүн - мисалы, эгер сиз биздин топ металлдык экенин элестетсеңиз, анда магнитти кутунун жанына коюп, топту башка дубалга уруп кетиши мүмкүн. Бул акселерометрдин өлчөөлөрү ылдамдатууга эмес, күчкө ээ экенин далилдөө үчүн эле айтылды. Ылдамдануу акселерометрдин күч аныктоо механизми тарабынан кармалып турган инерциялык күчкө алып келет.

Бул модель MEMS сенсорунун түзүлүшү боюнча так эмес болсо да, ал акселерометрге байланыштуу көйгөйлөрдү чечүүдө пайдалуу. Чындыгында окшош сенсорлор бар, алардын ичинде металл шарлары бар, алар жантайгычтар деп аталат, бирок алар примитивдүү жана көбүнчө алар түзмөктүн кандайдыр бир диапазондо эмес, кандайдыр бир чекте болгонун гана айта алышат.

Азырынча биз акселерометрдин бир окто өндүрүшүн анализдеп чыктык жана бул бир октун акселерометрлеринин жардамы менен болот. Үч осалдуу акселерометрлердин чыныгы баалуулугу алар үч огунда тең инерция күчтөрүн аныктай алышынан келип чыгат. Келгиле, кутуча моделибизге кайтып баралы жана кутуну 45 градуска оңго буралы. Топ азыр 2 дубалга тийет: Z- жана X- төмөнкү сүрөттө көрсөтүлгөндөй:

0.71 мааниси ыктыярдуу эмес, алар чындыгында SQRT (1/2) үчүн болжолдуу болуп саналат. Бул акселерометрдин кийинки моделин киргизгенибизде айкыныраак болот.

Мурунку моделде биз тартылуу күчүн оңдоп, элестеткен кутучаны айландырдык. Акыркы 2 мисалда биз өндүрүштү 2 башка кутуча абалында анализдедик, ал эми күч вектору туруктуу бойдон калды. Бул акселерометрдин сырткы күчтөр менен кандайча өз ара аракеттешүүсүн түшүнүүдө пайдалуу болгону менен, эгер координат системасын акселерометрдин огуна бекитип, күч вектору бизди айланып жүргөнүн элестетсек, эсептөөлөрдү жүргүзүү практикалык болот.

Сураныч, жогорудагы моделди карап көрүңүз, мен балтанын түстөрүн сактап койгом, сиз мурунку моделден жаңы моделге психикалык өтүүнү жасай аласыз. Жаңы моделдин ар бир огу мурунку модельдеги кутунун тиешелүү бетине перпендикуляр экенин элестетип көрүңүз. R вектору - акселерометр өлчөп жаткан күч вектору (бул тартылуу күчү же инерция күчү болушу мүмкүн, же жогорудагы мисалдардан же экөөнүн тең айкалышы). Rx, Ry, Rz X векторунун 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)

Теориянын узак преамбуласынан кийин биз чыныгы жашоо акселерометрлерине жакындап келе жатабыз. 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 экенин эске алыңыз.

Келгиле, жөнөкөй мисалды карап көрөлү, биздин 10 бит 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 = ~ 1.81V

Ар бир акселерометрдин нөл-г чыңалуу деңгээли бар, аны спецификадан таба аласыз, бул 0гге туура келген чыңалуу. Кол коюлган чыңалуу маанисин алуу үчүн, биз ушул деңгээлден жылышууну эсептешибиз керек. Келгиле, биздин 0g чыңалуу деңгээли VzeroG = 1.65V. Биз нөл-g чыңалуусунан чыңалуу жылышын төмөнкүчө эсептейбиз:

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.5mV/g = 0.4785V/g дейли. Сезгичтик баалуулуктарын акселерометрдин спецификациясында табууга болот. G менен туюнтулган акыркы күч маанилерин алуу үчүн төмөнкү формуланы колдонобуз:

Rx = DeltaVoltsRx / Сезгичтик

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) / Сезгичтик (Экв.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Сезгичтик Rz = (AdcRz * Vref / 1023 - VzeroG) / Сезгичтик

Азыр бизде инерция күчүнүн векторун аныктоочу 3 компонент бар, эгерде түзмөк тартылуудан башка күчтөргө баш ийбесе, бул биздин тартылуу күчү векторунун багыты деп божомолдоого болот. Эгерде сиз түзмөктүн жерге болгон жантаюусун эсептегиңиз келсе, анда бул вектор менен Z огунун ортосундагы бурчту эсептей аласыз. Эгерде сизди октун жантаюу багыты кызыктырса, анда бул натыйжаны 2 компонентке бөлө аласыз: X жана Y огунда ийилүү, аны гравитациялык вектор менен X / Y огунун ортосундагы бурч катары эсептесе болот. Бул бурчтарды эсептөө сиз ойлогондон алда канча жөнөкөй, азыр биз Rx, Ry жана Rz үчүн баалуулуктарды эсептеп чыктык. Келгиле, акыркы акселерометр моделибизге кайтып келип, кошумча белгилерди кылалы:

Бизди кызыктырган бурчтар - бул X, Y, Z огунун жана R векторунун ортосундагы бурчтар. Биз бул бурчтарды 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 cosy = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

Бул триплет көбүнчө Косинус багыты деп аталат жана ал негизинен биздин R вектору менен бирдей болгон бирдик векторун (узундугу 1 болгон вектор) билдирет. Сиз муну оңой эле текшере аласыз:

SQRT (cosX^2 + жайлуу^2 + cosZ^2) = 1

Бул жагымдуу касиет, анткени ал бизди R векторунун модулун (узундугун) байкоодон бошотот. Көп учурда, эгерде биз инерциялык векторубуздун багыты менен гана кызыкдар болсок, анда башка эсептөөлөрдү жөнөкөйлөтүү үчүн анын модулун нормалдаштыруунун мааниси бар.

2 -кадам: Гироскоп

Биз акселерометрге окшош гироскоптун эч кандай эквиваленттик моделин киргизбейбиз, анын ордуна биз экинчи акселерометрдин моделине түз секирип барабыз жана гироскоптун бул моделге ылайык эмне экенин көрсөтөбүз.

Ар бир гироскоп каналы октун биринин айланасындагы айланууну өлчөйт. Мисалы, 2-октук гироскоп X жана Y огунун айланасындагы айланууну өлчөйт (же кээ бирөөлөр "жөнүндө" деши мүмкүн). Бул айланууну сандар менен билдирүү үчүн кээ бир белгилөөлөрдү кылалы. Адегенде аныктайлы:

Rxz - инерция күчүнүн векторунун R XZ тегиздигиндеги проекциясы Ryz - инерция күчүнүн векторунун R YZ тегиздигиндеги проекциясы

Пифагор теоремасын колдонуу менен Rxz жана Rz түзгөн тик бурчтуу үч бурчтуктан биз:

Rxz^2 = Rx^2 + Rz^2 жана ушуга окшош: Ryz^2 = Ry^2 + Rz^2

ошондой эле белгиле:

R^2 = Rxz^2 + Ry^2, бул Eq.1 жана жогорудагы теңдемелерден, же R жана Ryz тарабынан түзүлгөн тик бурчтуу үч бурчтуктан алынышы мүмкүн R^2 = Ryz^2 + Rx^2

Биз бул макалада бул формулаларды колдонбойбуз, бирок биздин моделдин бардык баалуулуктарынын ортосундагы байланышты белгилөө пайдалуу.

Анын ордуна биз Z огу менен Rxz, Ryz векторлорунун ортосундагы бурчун төмөнкүчө аныктайбыз:

Axz - Rxz (XZ тегиздигиндеги R проекциясы) менен Z огу Ayz ортосундагы бурч - Ryz (YZ тегиздигиндеги R проекциясы) менен Z огунун ортосундагы бурч

Эми биз гироскоп өлчөгөн нерсеге жакындадык. Гироскоп жогоруда аныкталган бурчтардын өзгөрүү ылдамдыгын өлчөйт. Башкача айтканда, бул бурчтардын өзгөрүү ылдамдыгына сызыктуу байланышкан маанини чыгарат. Муну түшүндүрүү үчүн биз t огунда Y огунун айланасындагы бурулуш бурчун (бул Axz бурчу болмок) өлчөдүк деп эсептейли жана аны Axz0 деп аныктайбыз, кийинчерээк бул бурчту t1 кийинчерээк өлчөдүк жана ал Axz1 болду. Өзгөрүүнүн ылдамдыгы төмөнкүчө эсептелет:

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

Эгерде биз Axzди градус менен, ал эми убакытты секунда менен билдирсек, анда бул маани deg/s менен көрсөтүлөт. Бул гироскоптун өлчөөсү.

Иш жүзүндө гироскоп (эгер бул атайын санариптик гироскоп болбосо) сейрек сизге deg/s менен берилген маанини берет. Акселерометрдегидей эле, сиз ADC маанисин аласыз, аны сиз эквивалентке окшош формуланы колдонуп deg/sге айландырышыңыз керек. 2 биз акселерометр үчүн аныктаганбыз. Келгиле, ADCти гироскоп үчүн deg/s конверсиялоо формуласына киргизели (биз 10 бит ADC модулун колдонуп жатабыз деп ойлойбуз, 8bit ADC үчүн 1023 менен 255ти алмаштырыңыз, 12bit ADC үчүн 1023ти 4095 менен алмаштырыңыз).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Сезгичтик Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / Сезгичтик

AdcGyroXZ, AdcGyroYZ - биздин adc модулунан алынган жана алар YZ тегиздиктеринде XZдеги R векторунун проекциясынын айлануусун өлчөөчү каналдарды билдирет, бул айлануу тиешелүүлүгүнө жараша Y жана X огунун тегерегинде болгон деп барабар.

Vref - биз VzeroRateтин астындагы мисалда 3.3V колдоно турган ADC маалымдама чыңалуусу - нөлдүк ылдамдыктагы чыңалуу, башкача айтканда, гироскоптун эч кандай айланууга дуушар болбогондо чыгарган чыңалуусу, бул Acc_Gyro тактасы үчүн мисалы, 1.23V (спецификациядан бул баалуулуктарды таба аласыз) Сезгичтик - бул сиздин гироскоптун сезгичтиги 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/s) = ~ -94 градус/с

Башкача айтканда, түзмөк Y огунун айланасында (же XZ тегиздигинде айланат деп айта алабыз) 306 град/с ылдамдыкта жана X огунун айланасында (же биз аны YZ тегиздигинде айланат деп айтууга болот) ылдамдык менен айланат - 94 град/с. Сураныч, терс белги түзмөктүн кадимки оң багыттан карама -каршы багытта айланарын билдирет. Келишим боюнча, бир айлануу багыты оң. Жакшы гироскоптун спецификациялык баракчасы сизге кайсы багыт оң экенин көрсөтөт, антпесе аны түзмөк менен эксперимент жүргүзүү жана айлануу багытынын чыгуучу пиндеги чыңалууну жогорулатуу менен белгилөө аркылуу табууга туура келет. Бул эң жакшы осциллографтын жардамы менен жасалат, анткени айланууну токтотсоңуз, чыңалуу нөлдүк деңгээлге түшөт. Эгерде сиз мультиметрди колдонуп жатсаңыз, анда жок дегенде бир нече секундада туруктуу айлануу ылдамдыгын кармап турууңуз керек жана бул айлануу учурунда чыңалууга көңүл буруңуз, анан аны нөлдүк ылдамдыктагы чыңалуу менен салыштырыңыз. Эгерде ал нөлдүк чыңалуудан чоң болсо, анда айлануу багыты оң болот.

3 -кадам: Акселерометр менен гирону бириктирүү

Бардыгын бириктирүү - акселерометр менен гироскоптун маалыматтарын бириктирүү

Эгерде сиз бул макаланы окуп жатсаңыз, балким сиз IMU түзмөгүн сатып алдыңыз же сатып алууну пландап жатасыз, же сиз өзүнчө акселерометр менен гироскоптун аппараттарынан бирөөсүн курууну пландап жатсаңыз керек.

Акселерометр менен гироскопту айкалыштырган IMU айкалышкан аппаратты колдонуунун биринчи кадамы - бул алардын координат системаларын тегиздөө. Мунун эң оңой жолу - акселерометрдин координат системасын шилтеме координат системасы катары тандоо. Көпчүлүк акселерометр маалымат баракчалары физикалык чиптин же түзмөктүн сүрөтүнө карата X, Y, Z огунун багытын көрсөтөт. Мисалы, Acc_Gyro тактасынын спецификациясында көрсөтүлгөндөй X, Y, Z огунун багыттары:

Кийинки кадамдар:

Жогоруда талкууланган RateAxz, RateAyz баалуулуктарына туура келген гироскоптун жыйынтыгын аныктаңыз. Акселерометрге салыштырмалуу гироскоптун физикалык абалына байланыштуу бул жыйынтыктарды тескери буруш керекпи, аныктаңыз

Эгерде гироскоптун X же Y деп белгиленген чыгышы бар болсо, ал акселерометрдин координаттар системасынын каалаган огуна туура келет деп ойлобоңуз, ал тургай бул чыгаруу ӨИКтин бирдигинин бир бөлүгү болсо да. Эң жакшы жолу - аны сынап көрүү. Сиз акселерометрге салыштырмалуу гироскоптун ордун бекиттиңиз дейли. Бул гиро менен акселерометрдин чек аралары бири -бирине параллелдүү деп болжолдонот, башкача айтканда, сиз акселерометрдин чипине салыштырмалуу гироду 90 градустук бурчка коюп жатасыз. Эгер сиз ӨИКтин коллегиясына ээ болсоңуз, анда алар буга чейин эле дал келтирилген. Биз бул макалада гироскоп акселерометрге (мисалы, 45 же 30 градуска) карата туура эмес бурчта жайгашкан моделдерди талкуулабайбыз, бирок бул кээ бир колдонмолордо пайдалуу болушу мүмкүн.

Бул жерде гироскоптун кайсы чыгарылышы жогоруда талкууланган RateAxz маанисине туура келерин аныктоо үчүн үлгү иретинде келтирилген.

- түзмөктү горизонталдык абалга коюудан баштаңыз. Акселерометрдин X жана Y чыгуулары нөл-г чыңалуусун чыгармак (мисалы, Acc_Gyro тактасы үчүн бул 1.65В)

- түзмөктү 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) / Сезгичтик, бул жерде InvertAxz 1 же -1

RateAyz үчүн ошол эле камыш түзмөктү X огунун айланасында айландыруу менен жасалат жана кайсы гироскоптун чыгышы RateAyzке туура келерин, эгерде аны тескери буруш керек болсо, аныктай аласыз. Сиз InvertAyz үчүн мааниге ээ болгондон кийин, RateAyz эсептөө үчүн төмөнкү формуланы колдонушуңуз керек:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Сезгичтик

Эгерде сиз бул сыноолорду 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, cosy, cosZ сыяктуу башка баалуулуктарды ала алабыз, бул бизге биздин түзмөктүн эңкейиши жөнүндө түшүнүк берет. Жердин тегиздигине салыштырмалуу, биз 1 -бөлүктө бул баалуулуктардын ортосундагы байланышты талкуулайбыз. Бирөө мындай деп айта алат - бизде 1 -бөлүктөгү Eq.2ден Rx, Ry, Rz баалуулуктары бар эмеспи? Ооба, бирок бул баалуулуктар акселерометрдин маалыматынан гана алынганын унутпаңыз, андыктан эгер сиз аларды түздөн -түз тиркемеңизде колдоно турган болсоңуз, анда сиздин колдонмоңузга караганда көбүрөөк ызы -чуу пайда болушу мүмкүн. Мындан ары чаташтырбоо үчүн акселерометрдин өлчөөлөрүн төмөнкүчө кайрадан аныктайлы:

Racc - акселерометр менен өлчөнүүчү инерция күчүнүн вектору, ал төмөнкү компоненттерден турат (X, Y, Z огуна проекциялар):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / Сезгичтик RyAcc = (AdcRy * Vref / 1023 - VzeroG) / Сезгичтик RzAcc = (AdcRz * Vref / 1023 - VzeroG) / Сезгичтик

Азырынча бизде 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]

Бул биздин алгоритмдин чыгышы болот, бул гироскоптун маалыматына негизделген жана өткөн болжолдуу маалыматтарга негизделген түзөтүлгөн баалуулуктар.

Бул жерде биздин алгоритм эмне кылат: - акселерометр бизге мындай дейт: "Сиз азыр Racc абалындасыз" - биз "рахмат, бирок мени текшерүүгө уруксат бериңиз" дейбиз, - анда бул маалыматты гироскоптун маалыматтары менен, ошондой эле мурунку Эс алуу маалыматтары менен оңдоңуз. биз жаңы болжолдуу векторду чыгарабыз. - биз эс алууну аппараттын учурдагы абалына карата "эң жакшы коюм" деп эсептейбиз.

Келгиле, аны кантип иштете аларыбызды карап көрөлү.

Биз ырааттуулукту акселерометрибизге ишенүү менен баштайбыз:

Эс алуу (0) = Racc (0)

Айтмакчы, эс алуу менен 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), келгиле, биздин гироскоптон жана мурунку баадан ала турган жаңы өлчөнгөн маанини киргизели.

Биз аны Rgyro деп атайбыз, ал дагы 3 компоненттен турган вектор:

Rgyro = [RxGyro, RyGyro, RzGyro]

Биз бул векторду бир убакта бир компоненттен эсептейбиз. Биз RxGyro менен баштайбыз.

Гироскоп моделибизде төмөнкү мамилени байкоо менен баштайлы, Rz жана Rxz түзгөн тик бурчтуу үч бурчтуктан биз муну чыгара алабыз:

тан (Axz) = Rx/Rz => Axz = atan2 (Rx, Rz)

Atan2 сиз мурда колдонбогон функция болушу мүмкүн, ал атанга окшош, бирок ал (-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

Ошол эле жол менен биз таба алабыз:

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

Макул, азыр бизде Axz (n) жана Ayz (n) бар. Бул жерден RxGyro/RyGyro чыгарып салуу үчүн кайда барабыз? Экв. 1 биз Rgyro векторунун узундугун төмөнкүчө жаза алабыз:

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

Ошондой эле, биз Racc векторун нормалдаштыргандыктан, анын узундугу 1 деп ойлошубуз мүмкүн жана ал айлангандан кийин өзгөргөн жок, ошондуктан жазуу салыштырмалуу коопсуз:

| Rgyro | = 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) = sin (Axz), ошондуктан:

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

Эми SQRT ичиндеги фракциянын бөлгүчүн жана бөлүгүн z^2ге көбөйтүңүз

x = sin (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 = sin (Axz) / SQRT (1 + cos (Axz)^2 * tan (Ayz)^2)

Белгилерибизге кайтып келсек:

RxGyro = sin (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 болгондо, жана RzGyro <0 болгондо (RzGyro) = -1.

Муну баалоонун бир жөнөкөй жолу:

Белги (RzGyro) = Белги (RzEst (n-1))

Иш жүзүндө RzEst (n-1) 0гө жакын болгондо этият болуңуз. Бул учурда гиро фазасын таптакыр өткөрүп жиберип, дайындоого болот: Rgyro = Rest (n-1). Rz Axz жана Ayz бурчтарын эсептөө үчүн шилтеме катары колдонулат жана 0гө жакын болгондо, баалуулуктар ашыкча агып, жаман натыйжаларга алып келиши мүмкүн. Тан () / atan () функциясын ишке ашырууда тактык жок болушу мүмкүн болгон чоң өзгөрмөлүү чекиттердин доменинде болосуз.

Келгиле, буга чейин болгон нерселерди жыйынтыктайлы, биз алгоритмибиздин n баскычында турабыз жана төмөнкү баалуулуктарды эсептеп чыктык:

Racc - биздин акселерометр Rgyro учурдагы көрсөткүчтөрү - Rest (n -1) жана учурдагы гироскоптун көрсөткүчтөрүнөн алынган

Жаңыртылган баа Rest (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 ортосундагы маанилер жакшы натыйжаларга алып келет.

Бул алгоритмдин Калман чыпкасынан негизги айырмачылыгы - бул салмак салыштырмалуу туруктуу, ал эми Калман чыпкасында акселерометрдин көрсөткүчтөрүнүн өлчөнгөн ызы -чуусунун негизинде салмактар биротоло жаңыртылып турат. Калман чыпкасы сизге "эң жакшы" теориялык жыйынтыктарды берүүгө багытталган, ал эми бул алгоритм сизге практикалык колдонууңуз үчүн "жетишерлик жакшы" жыйынтыктарды бере алат. Сиз өлчөгөн кээ бир ызы -чуу факторлоруна жараша 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!