Акселерометр мен гиро оқулығы: 3 қадам

2024 Автор: John Day | [email protected]. Соңғы өзгертілген: 2024-01-30 10:29

Кіріспе

Бұл нұсқаулық электроника жобаларында акселерометрлер мен гироскоптарды, сондай -ақ біріктірілген IMU құрылғыларын (инерциялық өлшеу бірлігі) қолдануға қызығушылық танытқандардың барлығына арналған.

Біз қарастырамыз:

• Акселерометр нені өлшейді?
• Гироскоп (гироскоп) нені өлшейді?
• Осы сенсордан алынған аналогты-цифрлық (ADC) көрсеткіштерді физикалық бірліктерге қалай түрлендіру керек (бұл акселерометр үшін г, гироскоп үшін град/с)
• Құрылғының жердегі жазықтыққа қатысты бейімділігі туралы нақты ақпарат алу үшін акселерометр мен гироскоп көрсеткіштерін қалай біріктіруге болады

Мақала бойы мен математиканы минимумға түсіруге тырысамын. Егер сіз Синус/Косинус/Тангенс дегеннің не екенін білсеңіз, онда сіз қандай идеяларды қолданып жатқаныңызға қарамастан жобаңызда осы идеяларды түсінуіңіз керек: Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC және т.

IMU қондырғысын қолдану үшін сізге күрделі математика қажет деп санайтын адамдар бар (күрделі FIR немесе IIR сүзгілері, мысалы Калман, Паркс-Макклеллан сүзгілері және т. Сіз бәрін зерттеп, керемет, бірақ күрделі нәтижелерге қол жеткізе аласыз. Менің түсіндіру әдісім қарапайым математиканы қажет етеді. Мен қарапайымдылыққа үлкен сенемін. Менің ойымша, қарапайым жүйені басқару және бақылау оңай, сонымен қатар көптеген енгізілген құрылғыларда матрицалық есептеулерді қажет ететін күрделі алгоритмдерді жүзеге асыру үшін күш пен ресурстар жоқ.

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

- LIS331AL (мәліметтер парағы) - үштік осьті 2G акселерометр - LPR550AL (мәліметтер парағы) - қос осьті қадам және орама, 500 градус/сек гироскоп

Олар бірге 5-дәрежелі бостандық инерциалды өлшем бірлігін білдіреді. Енді бұл керемет атау! Соған қарамастан, сәнді атаудың артында бұл нұсқаулықта егжей -тегжейлі қарастыратын және түсіндіретін өте пайдалы комбинациялық құрылғы тұр.

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

Бұл құрылғыны түсіну үшін біз акселерометрден бастаймыз. Акселерометрлер туралы ойланған кезде көбінесе шар тәрізді қорапты бейнелеу пайдалы болады. Сіз печенье немесе пончик сияқты басқа нәрсені елестете аласыз, бірақ мен допты елестетемін:

Егер біз бұл қорапты гравитациялық өрісі жоқ жерде немесе доптың орналасуына әсер етуі мүмкін басқа өрістері жоқ жерде алсақ - доп жәшіктің ортасында жайылып қалады. Сіз қорапты ғарыш кеңістігінде кез келген ғарыштық денелерден алыс жерде елестете аласыз, немесе егер мұндай жерді табу қиын болса, кем дегенде планетаның айналасында айналатын ғарыш аппаратын елестетіңіз, онда бәрі салмақсыз күйде. Жоғарыдағы суреттен біз әр оське жұп қабырға тағайындайтынымызды көреміз (қораптың ішіне қарау үшін Y+ қабырғасын алып тастадық). Әр қабырға қысымға сезімтал екенін елестетіп көріңіз. Егер біз қорапты кенеттен солға жылжытсақ (біз оны 1г = 9,8м/с^2 үдеуімен үдетеміз), доп X- қабырғаға соғылады. Содан кейін біз шардың қабырғаға қолданылатын қысым күшін өлшейміз және X осінде -1g мәнін шығарамыз.

Назар аударыңыз, акселерометр үдеу векторынан қарама -қарсы бағытталған күшті анықтайды. Бұл күш көбінесе инерциялық күш немесе жалған күш деп аталады. Бұдан үйренуге болатын бір нәрсе - акселерометр оның қабырғаларының біріне қолданылатын күш арқылы жанама түрде үдеуді өлшейді (біздің модельге сәйкес, бұл көктем немесе нақты өмір акселерометрлерінде басқа нәрсе болуы мүмкін). Бұл күш үдеуден туындауы мүмкін, бірақ келесі мысалда көретініміздей, ол әрдайым үдеудің әсерінен болмайды.

Егер біз өз моделімізді алып, оны жерге қойсақ, онда доп Z-қабырғаға түседі және төмендегі суретте көрсетілгендей төменгі қабырғаға 1г күш түсіреді:

Бұл жағдайда қорап қозғалмайды, бірақ біз Z осінде -1g көрсеткішін аламыз. Доптың қабырғаға тигізетін қысымы тартылыс күшінің әсерінен болды. Теорияда бұл күштің басқа түрі болуы мүмкін - мысалы, егер сіз біздің допты металл деп ойласаңыз, онда магнит қораптың жанына қойылса, ол басқа қабырғаға соғылады. Бұл тек акселерометрдің үдеуді емес, күш беретінін дәлелдеу үшін айтылды. Тек үдеу үдеткіштің күш анықтайтын механизмімен ұсталатын инерциялық күшті тудырады.

Бұл модель 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) алмастыру арқылы

Теорияның ұзақ кіріспесінен кейін біз нақты өмір акселерометрлеріне жақындай түсеміз. 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,3В деп есептейік. 10 биттік қосылу мәнін кернеуге түрлендіру үшін келесі формуланы қолданамыз:

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г сәйкес келеді. Қол қойылған кернеудің мәнін алу үшін осы деңгейден ығысуды есептеу керек. Айталық, біздің 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В/г дейік. Сезімталдық мәндерін акселерометрдің сипаттамасынан табуға болады. 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 осьтері мен күш векторының арасындағы бұрыштар. Біз бұл бұрыштарды 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 деп атайды және ол негізінен біздің 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, мұны 1-ші теңдеуден және одан жоғары теңдеулерден алуға болады немесе оны R және Ryz R^2 = Ryz^2 + Rx^2 құрған тік бұрышты үшбұрыштан алуға болады.

Біз бұл мақалада бұл формулаларды қолданбаймыз, бірақ біздің модельдегі барлық мәндердің арасындағы байланысты ескеру пайдалы.

Оның орнына біз Z осі мен Rxz, Ryz векторлары арасындағы бұрышты анықтаймыз:

Axz - бұл Rxz (XZ жазықтығындағы R проекциясы) мен Z осінің арасындағы Айз - 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 модулін қолданамыз деп ойлаймыз, 8 биттік ADC үшін 1023 -ді 255 -ке, 12 -битті 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.3В қолданатын ADC сілтеме кернеуі - бұл нөлдік жылдамдықтағы кернеу, басқаша айтқанда, гироскоп ешқандай айналымға ұшырамаған кезде шығаратын кернеу, бұл Acc_Gyro тақтасы үшін мысалы, 1.23В (бұл мәндерді техникалық сипаттамадан табуға болады) Сезімталдық - бұл сіздің гироскопыңыздың сезімталдығы, ол mV / (град / с) жиі 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

Құрылғыны X осінің айналасында айналдыру арқылы RateAyz үшін дәл сол тест таяқшасы жасалады, және сіз қандай гироскоп шығысы 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, cozy, cosZ сияқты басқа мәндерді алуға болады, бұл бізге құрылғымыздың бейімділігі туралы түсінік береді. Жердегі жазықтыққа қатысты біз 1 -бөлімде осы мәндер арасындағы байланысты талқылаймыз. Біреу айтуы мүмкін - бізде 1 -бөлімдегі 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)

Айтпақшы, 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) - біздің бұрынғы бағалауымыз, Rest (0) = Racc (0) Racc (n) - біздің қазіргі акселерометрдің өлшемі

Rest (n) есептеуге кіріспес бұрын, біз гироскоптан және бұрынғы бағадан алатын жаңа өлшенген мәнді енгізейік.

Біз оны Rgyro деп атаймыз, сонымен қатар ол 3 компоненттен тұратын вектор:

Rgyro = [RxGyro, RyGyro, RzGyro]

Біз бұл векторды бір уақытта бір компоненттен есептейміз. Біз RxGyro -дан бастаймыз.

Гироскоп үлгісінде келесі қатынасты бақылай отырып бастайық, Rz мен Rxz түзген тік бұрышты үшбұрыштан мынаны шығара аламыз:

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

Atan2 сіз бұрын қолданбаған функция болуы мүмкін, ол атанға ұқсас, бірақ ол (-PI, PI) диапазонындағы мәндерді қайтарады (-PI/2, PI/2) atan қайтарады, және ол қажет Бір аргументтің орнына 2 дәлел. Ол Rx, Rz екі мәнін 360 градус (-PI-PI) диапазонындағы бұрыштарға түрлендіруге мүмкіндік береді. Атан2 туралы толығырақ мына жерден оқи аласыз.

Сондықтан 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> = 0 болғанда белгісі (RzGyro) = 1, ал RzGyro <0 болғанда белгісі (RzGyro) = -1.

Мұны бағалаудың қарапайым әдістерінің бірі:

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

Іс жүзінде RzEst (n-1) 0-ге жақын болған кезде абай болыңыз. Бұл жағдайда гиро фазаны мүлдем өткізіп жіберуге болады: Rgyro = Rest (n-1). Rz Axz және Ayz бұрыштарын есептеу үшін сілтеме ретінде пайдаланылады және ол 0 -ге жақындағанда, шамадан тыс ағып кетуі мүмкін және нашар нәтижелерге әкелуі мүмкін. Сіз tan () / 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 арасындағы мәндер жақсы нәтижеге әкеледі.

Бұл алгоритмнің Калман сүзгісінен басты айырмашылығы - бұл салмақ салыстырмалы түрде бекітілген, ал Калман сүзгісінде акселерометр көрсеткіштерінің өлшенген шуылына негізделген салмақтар тұрақты түрде жаңартылып отырады. 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 сайтында пайда болды, мен бірнеше жеңіл өңдеулер енгіздім және рұқсатпен қайта орналастырдым. Рахмет Старлино!