Урок за акселерометър и жироскоп: 3 стъпки

2024 Автор: John Day | [email protected]. Последно модифициран: 2024-01-30 07:56

Въведение

Това ръководство е предназначено за всички, които се интересуват от използването на акселерометри и жироскопи, както и комбинирани IMU устройства (инерционно измервателно устройство) в своите проекти за електроника

Ще покрием:

• Какво измерва акселерометърът?
• Какво измерва жироскопът (известен още като жироскоп)?
• Как да конвертирате аналогово-цифрови (ADC) показания, които получавате от този сензор, във физически единици (това би било g за акселерометър, deg/s за жироскоп)
• Как да комбинирате показанията на акселерометъра и жироскопа, за да получите точна информация за наклона на вашето устройство спрямо равнината на земята

В цялата статия ще се опитам да сведа математиката до минимум. Ако знаете какво представляват Sine/Cosine/Tangent, тогава би трябвало да можете да разберете и използвате тези идеи във вашия проект, без значение каква платформа използвате: Arduino, Propeller, Basic Stamp, чипове Atmel, Microchip PIC и т.н.

Има хора, които вярват, че имате нужда от сложна математика, за да използвате IMU единица (сложни FIR или IIR филтри като филтри на Kalman, Parks-McClellan филтри и т.н.). Можете да проучите всичко това и да постигнете прекрасни, но сложни резултати. Моят начин да обясня нещата изискват само основна математика. Аз силно вярвам в простотата. Мисля, че една проста система е по -лесна за управление и наблюдение, освен това много вградени устройства нямат силата и ресурсите да прилагат сложни алгоритми, изискващи матрични изчисления.

Ще използвам като пример нова IMU единица, Acc_Gyro акселерометър + Gyro IMU. Ще използваме параметрите на това устройство в нашите примери по -долу. Това устройство е добро устройство за начало, тъй като се състои от 2 устройства:

- LIS331AL (лист с данни) - триаксиален 2G акселерометър - LPR550AL (лист с данни) - двуосен ход и ролка, жироскоп 500 градуса/сек

Заедно те представляват единица за инерционно измерване на 5 градуса свобода. Това е фантастично име! Независимо от това, зад фантастичното име стои много полезно комбинирано устройство, което ще разгледаме и обясним подробно в това ръководство.

Стъпка 1: Акселерометърът

За да разберем тази единица, ще започнем с акселерометъра. Когато мислите за акселерометри, често е полезно да си представите кутия във формата на куб с топка вътре в нея. Можете да си представите нещо друго като бисквитка или поничка, но аз ще си представя топка:

Ако вземем тази кутия на място без гравитационни полета или без други полета, които биха могли да повлияят на позицията на топката - топката просто ще плува в средата на кутията. Можете да си представите, че кутията е в космическото пространство, много далеч от всякакви космически тела, или ако е трудно да се намери такова място, представете си поне космически кораб, който обикаля около планетата, където всичко е в безтегловност. От горната снимка можете да видите, че ние присвояваме на всяка ос двойка стени (премахнахме стената Y+, за да можем да погледнем вътре в кутията). Представете си, че всяка стена е чувствителна към натиск. Ако внезапно преместим кутията наляво (ускоряваме я с ускорение 1g = 9.8m/s^2), топката ще удари стената X-. След това измерваме силата на натиск, която топката прилага към стената и извежда стойност -1g по оста X.

Моля, обърнете внимание, че акселерометърът действително ще открие сила, която е насочена в обратна посока от вектора на ускорението. Тази сила често се нарича инерционна сила или фиктивна сила. Едно нещо, което трябва да научите от това, е, че акселерометърът измерва ускорението косвено чрез сила, която се прилага към една от стените му (според нашия модел, това може да е пружина или нещо друго в акселерометрите в реалния живот). Тази сила може да бъде причинена от ускорението, но както ще видим в следващия пример, тя не винаги е причинена от ускорение.

Ако вземем нашия модел и го поставим на Земята, топката ще падне върху Z-стената и ще приложи сила от 1 g върху долната стена, както е показано на снимката по-долу:

В този случай кутията не се движи, но все пак получаваме отчитане от -1 g по оста Z. Натискът, който топката е приложила върху стената, е причинен от гравитационна сила. На теория това може да е различен вид сила - например, ако си представите, че нашата топка е метална, поставянето на магнит до кутията може да премести топката, така че да удари друга стена. Това беше казано само за да докаже, че по същество акселерометърът измерва сила, а не ускорение. Просто се случва ускорението да предизвика инерционна сила, която се улавя от механизма за откриване на сила на акселерометъра.

Въпреки че този модел не е точно как е конструиран 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 g, можем да проверим, че:

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.

Нека продължим, като разгледаме един прост пример, да предположим, че нашият 10 -битов ADC модул ни даде следните стойности за трите канала (оси) на акселерометъра:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Всеки ADC модул ще има референтно напрежение, нека приемем в нашия пример, че е 3.3V. За да преобразуваме 10 -битова стойност на 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

Всеки акселерометър има ниво на напрежение нула g, можете да го намерите в спецификации, това е напрежението, което съответства на 0g. За да получим подписана стойност на напрежението, трябва да изчислим изместването от това ниво. Да приемем, че нивото на напрежение 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 m/s^2), за да извършим окончателното преобразуване, прилагаме чувствителността на акселерометъра, обикновено изразена в mV/g. Да кажем, че нашата чувствителност = 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 () (функцията inverse cos ()):

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

Изминахме дълъг път, за да обясним модела на акселерометъра, само за да стигнем до тези формули. В зависимост от вашите приложения може да искате да използвате всички междинни формули, които сме извлекли. Скоро ще представим и модела на жироскопа и ще видим как данните за акселерометъра и жироскопа могат да бъдат комбинирани, за да осигурят още по -точни оценки на наклона.

Но преди да направим това, нека направим още някои полезни нотации:

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

Този триплет често се нарича посока косинус и по същество представлява единичния вектор (вектор с дължина 1), който има същата посока като нашия вектор R. Можете лесно да проверите, че:

SQRT (cosX^2 + уютен^2 + cosZ^2) = 1

Това е хубаво свойство, тъй като ни освобождава от наблюдението на модула (дължината) на R вектора. Често, ако просто се интересуваме от посоката на нашия инерционен вектор, има смисъл да нормализираме неговия модул, за да опростим други изчисления.

Стъпка 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 (проекция на R върху равнината XZ) и оста Z Ayz - е ъгълът между Ryz (проекция на R върху равнината YZ) и оста Z

Сега се приближаваме до това, което измерва жироскопът. Жироскопът измерва скоростта на промените на ъглите, определени по -горе. С други думи, той ще изведе стойност, която е линейно свързана със скоростта на промяна на тези ъгли. За да обясним това, нека приемем, че сме измерили ъгъла на въртене около оста Y (това би бил ъгъл на Axz) в момент t0 и го дефинираме като Axz0, след това измерихме този ъгъл по -късно t1 и той беше Axz1. Скоростта на промяна ще бъде изчислена, както следва:

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

Ако изразим Axz в градуси, а времето в секунди, тогава тази стойност ще бъде изразена в deg/s. Това измерва жироскопът.

На практика жироскопът (освен ако не е специален цифров жироскоп) рядко ще ви даде стойност, изразена в градуси/сек. Същото като за акселерометъра ще получите ADC стойност, която ще трябва да преобразувате в deg/s, като използвате формула, подобна на Eq. 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 модул и представляват каналите, които измерват въртенето на проекцията на R вектор в XZ съответно в YZ равнини, което е еквивалентно на това, че въртенето е направено съответно около осите Y и X.

Vref - е референтното напрежение на ADC, което ще използваме 3.3V в примера по -долу VzeroRate - е напрежението с нулева скорост, с други думи напрежението, което извежда жироскопът, когато не подлежи на никакво въртене, за платката Acc_Gyro е например 1.23V (можете да намерите тези стойности в спецификациите) Чувствителност - чувствителността на вашия жироскоп е изразена в mV / (deg / s), често записана като mV / deg / s, по същество ви казва колко mV ще увеличаването на мощността на жироскопа, ако увеличите скоростта на въртене с един градус/сек. Чувствителността на платката 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 deg/s и около оста 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 изходите на акселерометъра ще изведат напрежение нула g (например за платка Acc_Gyro това е 1.65V)

- следващият път започнете да завъртате устройството около оста Y, друг начин да го кажете е, че завъртате устройството в равнина XZ, така че изходите на акселерометъра X и Z се променят, а изходът Y остава постоянен. - докато въртите устройството с постоянна скорост, отбележете кой изход на жироскопа се променя, другите изходи на жироскопа трябва да останат постоянни - изходът на жироскопа, който се промени по време на въртенето около оста Y (въртене в равнина XZ), ще осигури входната стойност за AdcGyroXZ, от която изчисляваме RateAxz - последната стъпка е да се гарантира, че посоката на въртене съответства на нашия модел, в някои случаи може да се наложи да обърнете стойността RateAxz поради физическото положение на жироскопа спрямо акселерометъра - извършете отново горния тест, завъртайки устройството около оста Y, този път следете X изхода на акселерометъра (AdcRx в нашия модел). Ако AdcRx расте (първите 90 градуса на въртене от хоризонтално положение), тогава AdcGyroXZ също трябва да расте. В противен случай трябва да обърнете RateAxz, можете да постигнете това, като въведете знаков фактор в уравнение 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 системи използват жироскоп, за да изгладят грешките на акселерометъра. Но как се прави това? И дали жироскопът е без шум?

Жироскопът не е свободен от шум, но тъй като измерва въртенето, той е по-малко чувствителен към линейни механични движения, вида на шума, от който страда акселерометърът, но жироскопите имат други видове проблеми като например дрейф (не се връща към нулева скорост когато въртенето спре). Въпреки това чрез усредняване на данни, които идват от акселерометър и жироскоп, можем да получим относително по -добра оценка на наклона на текущото устройство, отколкото бихме получили, като използваме само данните от акселерометъра.

В следващите стъпки ще представя алгоритъм, вдъхновен от някои идеи, използвани във филтъра на Kalman, но той е далеч по -прост и по -лесен за изпълнение на вградени устройства. Преди това нека да видим първо какво искаме да изчисли нашия алгоритъм. Е, векторът на силата на гравитация R = [Rx, Ry, Rz], от който можем да извлечем други стойности като Axr, Ayr, Azr или cosX, cozy, cosZ, ще ни даде представа за наклона на нашето устройство спрямо земната равнина, ние обсъждаме връзката между тези стойности в част 1. Може да се каже - не разполагаме ли вече с тези стойности Rx, Ry, Rz от уравнение 2 в част 1? Е, да, но не забравяйте, че тези стойности са получени само от данни за акселерометър, така че ако искате да ги използвате директно в приложението си, може да получите повече шум, отколкото приложението ви може да понесе. За да избегнем по-нататъшно объркване, нека предефинираме измерванията на акселерометъра, както следва:

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 g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Въпреки това, за да сме сигурни, че има смисъл да актуализираме този вектор, както следва:

Racc (нормализиран) = [RxAcc/| Racc |, RyAcc/| Racc |, RzAcc/| Racc |].

Това ще гарантира, че дължината на вашия нормализиран Racc вектор винаги е 1.

След това ще въведем нов вектор и ще го наречем

Почивка = [RxEst, RyEst, RzEst]

Това ще бъде резултатът от нашия алгоритъм, това са коригирани стойности въз основа на данни от жироскоп и въз основа на данни от предишни оценки.

Ето какво ще направи нашият алгоритъм: - акселерометърът ни казва: „Сега сте на позиция Racc“- казваме „Благодаря, но нека да проверя“, - след това коригирайте тази информация с данни от жироскопа, както и с минали данни за почивка и извеждаме нов прогнозен вектор Rest. - считаме, че почивката е нашият „най -добър залог“по отношение на текущата позиция на устройството.

Нека да видим как можем да го накараме да работи.

Ще започнем нашата последователност, като се доверим на нашия акселерометър и присвоим:

Почивка (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) и т.н. Също така ще издаваме нови оценки на всеки интервал от време Rest (1), Rest (2), Rest (3) и т.н.

Да предположим, че сме на стъпка n. Имаме два известни набора от стойности, които бихме искали да използваме:

Rest (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 може да е функция, която никога не сте използвали досега, тя е подобна на 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

По същия начин можем да намерим:

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).

Където Sign (RzGyro) = 1, когато RzGyro> = 0, и Sign (RzGyro) = -1, когато RzGyro <0.

Един прост начин да прецените това е да вземете:

Знак (RzGyro) = Знак (RzEst (n-1))

На практика бъдете внимателни, когато RzEst (n-1) е близо до 0. В този случай можете напълно да пропуснете жирофазата и да зададете: Rgyro = Rest (n-1). Rz се използва като отправна точка за изчисляване на Axz и Ayz ъгли и когато е близо до 0, стойностите могат да преливат и да предизвикат лоши резултати. Ще бъдете в домейн на големи числа с плаваща запетая, където реализациите на функцията tan () / atan () може да нямат точност.

Така че нека резюмираме това, което имаме досега, ние сме на стъпка n от нашия алгоритъм и сме изчислили следните стойности:

Racc - текущи показания от нашия акселерометър Rgyro - получени от почивка (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!