Accelerometer & Gyro Tutorial: 3 Kauj Ruam

2024 Tus sau: John Day | [email protected]. Kawg hloov kho: 2024-01-30 09:31

Taw qhia

Phau ntawv qhia no yog npaj rau txhua tus neeg nyiam siv Accelerometers thiab Gyroscopes nrog rau ua ke IMU cov cuab yeej (Chav Ntsuas Ntsuas Ntsuas) hauv lawv cov haujlwm hluav taws xob

Peb yuav npog:

• Tus ntsuas ntsuas ntsuas ntsuas yog dab tsi?
• Dab tsi ntsuas lub gyroscope (aka gyro) ntsuas?
• Yuav ua li cas hloov pauv analog-rau-digital (ADC) nyeem ntawv uas koj tau txais los ntawm cov ntsuas no mus rau lub cev lub cev (cov ntawd yuav yog g rau kev ntsuas lub ntsuas, deg/s rau lub tshuab ntsuas kab mob)
• Yuav ua li cas los ua ke ntsuas qhov ntsuas thiab ntsuas lub ntsuas ntsuas kom tau txais cov ntaub ntawv raug tseeb txog kev nyiam ntawm koj lub cuab yeej txheeb ze rau lub dav hlau hauv av

Thoob plaws hauv kab lus Kuv yuav sim ua lej kom tsawg. Yog tias koj paub tias Sine/Cosine/Tangent yog dab tsi ces koj yuav tsum nkag siab thiab siv cov tswv yim no hauv koj txoj haujlwm tsis hais lub platform twg koj tab tom siv: Arduino, Propeller, Stamp yooj yim, Atmel chips, Microchip PIC, thiab lwm yam.

Muaj cov neeg nyob ntawd uas ntseeg tias koj xav tau cov lej sib thooj txhawm rau siv chav IMU (cov FIR nyuaj lossis IIR lim xws li lim Kalman, Chaw lim dej Parks-McClellan, thiab lwm yam). Koj tuaj yeem tshawb fawb txhua qhov ntawd thiab ua tiav cov txiaj ntsig zoo tab sis nyuaj. Kuv txoj kev piav qhia txhua yam xav tau ua lej yooj yim. Kuv yog ib tug neeg ntseeg zoo hauv kev yooj yim. Kuv xav tias cov txheej txheem uas yooj yim yog yooj yim los tswj thiab saib xyuas, ntxiv rau ntau yam khoom siv hauv lub cev tsis muaj lub zog thiab peev txheej los siv cov txheej txheem nyuaj uas xav tau kev suav ua lej.

Kuv yuav siv ua piv txwv chav IMU tshiab, Acc_Gyro Accelerometer + Gyro IMU. Peb yuav siv qhov ntsuas ntawm cov cuab yeej no hauv peb cov piv txwv hauv qab no. Chav tsev no yog lub cuab yeej zoo los pib nrog vim nws muaj 2 yam khoom siv:

- LIS331AL (cov ntaub ntawv) - triaxial 2G lub ntsuas cua ntsuas - LPR550AL (cov ntawv xov xwm) - ob -axis suab thiab yob, 500 deg/sec gyroscope

Ua ke lawv sawv cev rau 5-Degrees of Freedom Inertial Measurement Unit. Tam sim no uas yog lub npe zoo nkauj! Txawm li cas los xij, tom qab lub npe zoo nkauj yog cov cuab yeej siv ua ke zoo uas peb yuav npog thiab piav qhia ntxaws hauv phau ntawv qhia no.

Kauj ruam 1: Accelerometer

Txhawm rau nkag siab chav tsev no peb yuav pib nrog lub ntsuas cua. Thaum xav txog kev ntsuas tus ntsuas nws feem ntau muaj txiaj ntsig zoo rau lub npov zoo li lub voos xwmfab nrog lub pob hauv nws. Koj tuaj yeem xav txog lwm yam zoo li ncuav qab zib lossis donut, tab sis kuv yuav xav txog lub pob:

Yog tias peb nqa lub npov no rau hauv ib qho chaw uas tsis muaj kev sib nqus lossis rau qhov teeb meem uas tsis muaj lwm qhov chaw uas yuav cuam tshuam rau pob txoj haujlwm - pob yuav yooj yim ntab nyob nruab nrab ntawm lub thawv. Koj tuaj yeem xav txog lub thawv nyob sab nraud-qhov chaw deb-deb ntawm ib lub cev cosmic, lossis yog tias qhov chaw no nyuaj rau nrhiav pom yam tsawg kawg yog qhov chaw khoom siv tes ua ncig lub ntiaj chaw uas txhua yam nyob hauv lub xeev tsis hnyav. Los ntawm daim duab saum toj no koj tuaj yeem pom tias peb muab rau txhua lub axis ob peb phab ntsa (peb tshem tawm phab ntsa Y+ yog li peb tuaj yeem saib sab hauv lub thawv). Xav txog tias txhua lub phab ntsa muaj kev nkag siab siab. Yog tias peb txav lub thawv mus rau sab laug (peb nrawm nws nrog nrawm 1g = 9.8m/s^2), lub pob yuav tsoo rau phab ntsa X-. Peb tom qab ntsuas lub zog siab uas lub pob siv rau ntawm phab ntsa thiab tso tawm tus nqi ntawm -1g ntawm X axis.

Thov nco ntsoov tias lub ntsuas hluav taws xob yuav ntsuas lub zog uas tau coj ncaj qha los ntawm qhov nrawm nrawm. Lub zog no feem ntau hu ua Inertial Force lossis Fictitious Force. Ib yam uas koj yuav tsum tau kawm los ntawm qhov no yog qhov ntsuas lub ntsuas qhov ntsuas nrawm los ntawm lub zog uas siv rau ib qho ntawm nws cov phab ntsa (raws li peb tus qauv, nws yuav yog lub caij nplooj ntoo hlav lossis lwm yam hauv lub neej tiag tiag accelerometers). Lub zog no tuaj yeem tshwm sim los ntawm kev nrawm, tab sis raws li peb yuav pom hauv qhov piv txwv tom ntej nws tsis yog ib txwm tshwm sim los ntawm kev nrawm.

Yog tias peb coj peb tus qauv thiab muab tso rau hauv ntiaj teb lub pob yuav poob rau Z-wall thiab yuav siv lub zog ntawm 1g ntawm phab ntsa hauv qab, raws li qhia hauv daim duab hauv qab no:

Hauv qhov no lub thawv tsis txav mus tab sis peb tseem tau nyeem -1g ntawm Z axis. Lub siab uas lub pob tau thov rau ntawm phab ntsa tau tshwm sim los ntawm lub zog nqus. Hauv txoj kev xav nws tuaj yeem yog hom sib txawv ntawm kev quab yuam - piv txwv li, yog tias koj xav tias peb lub pob yog xim hlau, tso cov hlau nplaum nyob ib sab ntawm lub thawv tuaj yeem txav pob kom nws tsoo lwm phab ntsa. Qhov no tau hais tsuas yog los ua pov thawj tias qhov tseem ceeb accelerometer ntsuas lub zog tsis nrawm. Nws tsuas yog tshwm sim tias kev nrawm nrawm ua rau lub zog inertial uas tau ntes los ntawm lub zog ntsuas xyuas lub tshuab ntawm lub ntsuas cua.

Thaum tus qauv no tsis yog raws nraim li cas MEMS sensor tau tsim nws feem ntau muaj txiaj ntsig zoo hauv kev daws teeb meem ntsig txog kev ntsuas lub ntsuas. Muaj qhov ua tau zoo sib xws uas muaj cov xim hlau nyob sab hauv, lawv hu ua qaij hloov, txawm li cas los xij lawv yog cov txheej thaum ub thiab feem ntau lawv tsuas tuaj yeem qhia yog tias lub cuab yeej xav tau nyob hauv qee qhov lossis tsis yog, tsis yog qhov kev nyiam.

Txog tam sim no peb tau tshuaj xyuas qhov ntsuas qhov ntsuas hluav taws xob ntawm ib lub axis thiab qhov no yog txhua yam koj yuav tau txais nrog rau ib lub axis nrawm. Tus nqi tiag tiag ntawm triaxial accelerometers los ntawm qhov tseeb tias lawv tuaj yeem txheeb xyuas cov zog inertial ntawm tag nrho peb txoj kab. Cia peb rov qab mus rau peb lub thawv qauv, thiab cia peb tig lub thawv 45 degrees rau sab xis. Lub pob yuav kov 2 phab ntsa tam sim no: Z- thiab X- raws li qhia hauv daim duab hauv qab no:

Qhov tseem ceeb ntawm 0.71 tsis yog qhov xav tau, lawv yog qhov kwv yees rau SQRT (1/2). Qhov no yuav ua kom meej dua thaum peb qhia peb cov qauv tom ntej rau lub ntsuas cua.

Hauv tus qauv yav dhau los peb tau kho lub zog nqus thiab tig peb lub npov kev xav. Hauv 2 qhov piv txwv dhau los peb tau tshuaj xyuas cov zis hauv 2 lub thawv sib txawv, thaum lub zog vector tseem nyob tas li. Thaum qhov no tau txais txiaj ntsig zoo hauv kev nkag siab li cas tus ntsuas kev sib cuam tshuam nrog cov tub sab sab nraud, nws muaj txiaj ntsig ntau dua los ua cov lus teb yog tias peb txhim kho cov txheej txheem kev sib koom ua ke rau lub axis ntawm lub ntsuas hluav taws xob thiab xav txog tias lub zog vector tig ncig peb.

Thov saib cov qauv saum toj no, Kuv tau khaws cov xim ntawm cov kab xev kom koj tuaj yeem hloov pauv kev xav ntawm tus qauv yav dhau los mus rau qhov tshiab. Tsuas yog xav txog tias txhua lub axis hauv tus qauv tshiab yog sib npaug rau lub ntsej muag ntawm lub npov hauv tus qauv dhau los. Vector R yog qhov quab yuam vector uas ntsuas qhov ntsuas tau ntsuas (nws tuaj yeem yog lub zog nqus lossis lub zog inertial los ntawm cov piv txwv saum toj no lossis ua ke ntawm ob qho tib si). Rx, Ry, Rz yog qhov projection ntawm R vector ntawm X, Y, Z axes. Thov ceeb toom cov kev sib raug zoo hauv qab no:

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

uas yog qhov sib npaug ntawm Pythagorean theorem hauv 3D.

Nco ntsoov tias ua ntej me ntsis kuv tau hais rau koj tias qhov txiaj ntsig ntawm SQRT (1/2) ~ 0.71 tsis yog qhov xwm txheej. Yog tias koj ntsaws rau hauv cov qauv saum toj no, tom qab rov hais dua tias peb lub zog nqus yog 1 g peb tuaj yeem txheeb xyuas tau tias:

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

yooj yim los ntawm kev hloov R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) hauv Eq.1

Tom qab ntev preamble ntawm txoj kev xav peb nyob ze rau lub neej tiag tiag accelerometers. Qhov tseem ceeb Rx, Ry, Rz yog qhov tseeb cuam tshuam nrog cov txiaj ntsig uas koj lub neej tiag tiag lub ntsuas hluav taws xob yuav tawm thiab koj tuaj yeem siv rau kev ua ntau yam kev suav.

Ua ntej peb mus txog ntawd tham me ntsis txog txoj kev uas cov neeg ntsuas kev ntsuas yuav xa cov ntaub ntawv no rau peb. Feem ntau cov accelerometers yuav poob rau hauv ob pawg: digital thiab analog. Digital accelerometers yuav muab cov ntaub ntawv rau koj siv cov txheej txheem raws li I2C, SPI lossis USART, thaum analog cov ntsuas hluav taws xob yuav tso tawm qhov ntsuas hluav taws xob nyob hauv thaj tsam uas tau txiav txim siab ua ntej uas koj yuav tsum hloov pauv mus rau tus lej digital siv ADC (analog to digital converter) module. Kuv yuav tsis nkag mus ntau yam txog yuav ua li cas ADC ua haujlwm, ib nrab vim tias nws yog cov ncauj lus nthuav dav thiab ib nrab vim tias nws txawv ntawm ib lub platform mus rau lwm qhov. Qee lub microcontroller yuav muaj ADC cov qauv tsim ib qho ntawm lawv yuav xav tau cov khoom siv sab nraud txhawm rau ua ADC kev hloov pauv. Tsis muaj teeb meem dab tsi hom ADC module koj siv koj yuav xaus nrog tus nqi hauv qee yam. Piv txwv li 10 -ntsis ADC module yuav tso tawm tus nqi hauv thaj tsam ntawm 0..1023, nco ntsoov tias 1023 = 2^10 -1. 12-ntsis ADC module yuav tso tawm tus nqi hauv thaj tsam ntawm 0..4095, nco ntsoov tias 4095 = 2^12-1.

Cia peb txav mus los ntawm kev txiav txim siab piv txwv yooj yim, xav tias peb 10bit ADC module muab peb cov txiaj ntsig hauv qab no rau peb qhov kev ntsuas nrawm (axes):

AWM = 586 AWV = 630 AWM = 561

Txhua tus qauv ADC yuav muaj qhov ntsuas hluav taws xob, cia peb xav hauv peb qhov piv txwv nws yog 3.3V. Txhawm rau hloov tus nqi 10 ntsis adc rau qhov hluav taws xob peb siv cov qauv hauv qab no:

VoltsRx = AdcRx * Vref / 1023

Cov ntawv ceeb toom sai ntawm no: uas rau 8bit ADC qhov sib faib zaum kawg yuav yog 255 = 2 ^ 8 -1, thiab rau 12 ntsis ADC qhov faib faib zaum kawg yuav yog 4095 = 2 ^ 12 -1.

Ua ntawv thov cov mis no rau txhua 3 txoj hauv kev peb tau txais:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (peb muab tag nrho cov txiaj ntsig mus rau 2 lub zauv zauv) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Txhua lub ntsuas hluav taws xob muaj qib x-g qhov hluav taws xob, koj tuaj yeem pom nws hauv qhov tshwj xeeb, qhov no yog qhov hluav taws xob uas sib haum rau 0g. Txhawm rau kom tau txais tus nqi hluav taws xob kos npe peb yuav tsum suav qhov hloov pauv los ntawm qib no. Cia peb hais peb qib 0g voltage yog VzeroG = 1.65V. Peb xam qhov hloov pauv hluav taws xob los ntawm xoom-g voltage raws li hauv qab no::

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

Tam sim no peb muaj peb qhov ntsuas qhov ntsuas hauv Volts, nws tseem tsis nyob hauv g (9.8 m/s^2), los ua qhov kev hloov pauv zaum kawg peb siv qhov ntsuas qhov ntsuas qhov tseeb, feem ntau qhia hauv mV/g. Cia peb hais peb qhov kev nkag siab = 478.5mV/g = 0.4785V/g. Cov txiaj ntsig rhiab heev tuaj yeem pom hauv cov lus qhia tshwj xeeb. Txhawm rau kom tau txais qhov txiaj ntsig qhov kawg hais tawm hauv g peb siv cov qauv hauv qab no:

Rx = DeltaVoltsRx / rhiab heev

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

Peb tuaj yeem ua tiav ua ke txhua qib hauv ib tus qauv, tab sis kuv tau dhau los ntawm txhua qib kom paub meej tias koj mus li cas los ntawm ADC nyeem mus rau qhov quab yuam vector tivthaiv qhia hauv g.

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

Tam sim no peb muaj tag nrho 3 yam uas piav qhia peb lub zog inertial vector, yog tias lub cuab yeej tsis raug rau lwm lub zog uas tsis yog lub ntiajteb txawj nqus, peb tuaj yeem xav tias qhov no yog kev coj ntawm peb lub zog quab yuam vector. Yog tias koj xav suav nrog qhov xav tau ntawm cov cuab yeej txheeb ze rau hauv av koj tuaj yeem xam lub kaum sab xis ntawm cov duab no thiab Z axis. Yog tias koj tseem txaus siab rau ib-txoj kev taw qhia ntawm txoj kev xav koj tuaj yeem faib qhov txiaj ntsig no ua 2 yam: kev nyiam ntawm X thiab Y axis uas tuaj yeem suav ua lub kaum sab xis ntawm gravitation vector thiab X / Y axes. Kev suav cov ces kaum no yooj yim dua li qhov koj xav, tam sim no peb tau suav qhov tseem ceeb rau Rx, Ry thiab Rz. Cia peb rov qab mus rau peb tus qauv ntsuas qhov ntsuas kawg thiab ua qee qhov cim ntxiv:

Cov ces kaum uas peb xav tau yog cov ces kaum ntawm X, Y, Z axes thiab quab yuam vector R. Peb yuav txhais cov ces kaum li Axr, Ayr, Azr. Koj tuaj yeem pom los ntawm daim duab peb sab sab xis tsim los ntawm R thiab Rx uas:

cos (Axr) = Rx / R, thiab zoo ib yam: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Peb tuaj yeem txiav tawm ntawm Eq.1 uas R = SQRT (Rx^2 + Ry^2 + Rz^2).

Peb tuaj yeem pom tam sim no peb cov ces kaum los ntawm kev siv arccos () muaj nuj nqi (theverse cos () muaj nuj nqi):

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

Peb tau mus ntev ntev los piav qhia tus qauv ntsuas tus ntsuas, tsuas yog los txog rau cov qauv no. Nyob ntawm koj daim ntawv thov koj yuav xav siv ib qho qauv nruab nrab uas peb tau txais. Peb tseem yuav qhia tus qauv gyroscope sai sai no, thiab peb yuav pom yuav ua li cas cov ntsuas qhov ntsuas thiab ntsuas cov ntaub ntawv tuaj yeem ua ke txhawm rau muab kev kwv yees ntau dua qhov tseeb.

Tab sis ua ntej peb ua qhov ntawd cia ua qee qhov kev qhia muaj txiaj ntsig ntau dua:

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

Qhov kev mus ncig no feem ntau hu ua Direction Cosine, thiab nws ib txwm sawv cev rau chav vector (vector nrog qhov ntev 1) uas muaj tib qho kev qhia ib yam li peb R vector. Koj tuaj yeem txheeb xyuas tau yooj yim tias:

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

Qhov no yog cov cuab yeej zoo vim nws tshem tawm peb los ntawm kev saib xyuas tus qauv (ntev) ntawm R vector. Feem ntau lub sijhawm yog tias peb tsuas yog xav nyob rau hauv cov lus qhia ntawm peb cov inertial vector, nws ua rau kev nkag siab zoo rau nws qhov kev hloov pauv kom yooj yim rau lwm qhov kev suav.

Kauj ruam 2: Gyroscope

Peb yuav tsis qhia ib lub thawv sib piv rau lub gyroscope zoo li peb tau ua rau lub ntsuas qhov ntsuas, tsis yog peb yuav dhia ncaj qha mus rau tus qauv ntsuas tus ntsuas thib ob thiab peb yuav qhia qhov ntsuas ntsuas ntsuas raws li tus qauv no li cas.

Txhua lub gyroscope channel ntsuas kev sib hloov ib ncig ntawm ib txoj kab. Piv txwv li 2-axis gyroscope yuav ntsuas kev sib hloov ib puag ncig (lossis qee qhov yuav hais "txog") X thiab Y axes. Txhawm rau qhia qhov kev sib hloov no hauv cov lej peb ua qee qhov kev xav. Ua ntej peb txhais:

Rxz - yog qhov kev kwv yees ntawm lub zog inertial vector R ntawm lub dav hlau XZ Ryz - yog qhov kev kwv yees ntawm qhov inertial force vector R ntawm lub dav hlau YZ

Los ntawm txoj cai kaum sab xis tsim los ntawm Rxz thiab Rz, siv Pythagorean theorem peb tau txais:

Rxz^2 = Rx^2 + Rz^2, thiab zoo ib yam: Ryz^2 = Ry^2 + Rz^2

kuj nco tias:

R^2 = Rxz^2 + Ry^2, qhov no tuaj yeem muab tau los ntawm Eq.1 thiab saum toj no cov kab zauv, lossis nws tuaj yeem muab tau los ntawm txoj cai kaum sab xis tsim los ntawm R thiab Ryz R^2 = Ryz^2 + Rx^2

Peb yuav tsis siv cov qauv no hauv kab lus no tab sis nws yog qhov muaj txiaj ntsig los nco txog kev sib raug zoo ntawm txhua qhov txiaj ntsig hauv peb tus qauv.

Hloov chaw peb yuav txhais lub kaum sab xis ntawm Z axis thiab Rxz, Ryz vectors raws li hauv qab no:

Axz - yog lub kaum sab xis nruab nrab ntawm Rxz (kev kwv yees ntawm R ntawm XZ dav hlau) thiab Z axis Ayz - yog lub kaum sab xis nruab nrab ntawm Ryz (kev kwv yees ntawm R ntawm lub dav hlau YZ) thiab Z axis

Tam sim no peb tab tom los ze zog rau qhov ntsuas ntsuas gyroscope. Gyroscope ntsuas qhov kev hloov pauv ntawm cov ces kaum uas tau piav qhia saum toj no. Hauv lwm lo lus nws yuav tso tawm tus nqi uas muaj feem cuam tshuam nrog rau qhov kev hloov pauv ntawm cov ces kaum no. Txhawm rau piav qhia qhov no peb xav tias peb tau ntsuas lub kaum sab xis ncig ntawm axis Y (uas yuav yog Axz lub kaum ntse ntse) thaum lub sijhawm t0, thiab peb txhais nws li Axz0, tom ntej no peb ntsuas lub kaum sab xis tom qab lub sijhawm t1 thiab nws yog Axz1. Tus nqi hloov pauv yuav raug xam raws li hauv qab no:

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

Yog tias peb qhia Axz hauv qib, thiab sijhawm hauv vib nas this, tom qab ntawd tus nqi no yuav qhia tawm hauv deg/s. Qhov no yog qhov ntsuas ntsuas gyroscope.

Hauv kev siv lub gyroscope (tshwj tsis yog nws yog qhov tshwj xeeb digital gyroscope) yuav tsis tshua muab rau koj tus nqi qhia hauv deg/s. Ib yam li rau qhov ntsuas qhov ntsuas koj yuav tau txais tus nqi ADC uas koj yuav xav tau los hloov mus rau deg/s siv cov qauv zoo ib yam li Eq. 2 uas peb tau hais tseg rau kev ntsuas qhov ntsuas. Wb qhia ADC rau deg/s hloov pauv mis rau gyroscope (peb xav tias peb siv 10bit ADC module, rau 8bit ADC hloov 1023 nrog 255, rau 12bit ADC hloov 1023 nrog 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Rhiab heev Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / rhiab heev

AdcGyroXZ, AdcGyroYZ - tau txais los ntawm peb cov adc module thiab lawv sawv cev rau cov channel uas ntsuas kev sib hloov ntawm kev kwv yees ntawm R vector hauv XZ feem hauv YZ dav hlau, uas yog qhov sib npaug rau hais tias kev sib hloov tau ua tiav ib puag ncig Y thiab X feem.

Vref - yog ADC qhov ntsuas hluav taws xob peb yuav siv 3.3V hauv qhov piv txwv hauv qab no VzeroRate - yog xoom -tus nqi hluav taws xob, hauv lwm lo lus qhov hluav taws xob uas lub gyroscope tso tawm thaum nws tsis raug rau kev sib hloov, rau Acc_Gyro board nws yog piv txwv li 1.23V (koj tuaj yeem pom qhov txiaj ntsig no hauv qhov tshwj xeeb) Rhiab heev - yog qhov rhiab ntawm koj lub gyroscope nws tau hais tawm hauv mV / (deg / s) feem ntau sau ua mV / deg / s, nws pib qhia koj pes tsawg mV yuav lub gyroscope tso zis ntau ntxiv, yog tias koj nce kev sib hloov nrawm los ntawm ib deg/s. Qhov rhiab heev ntawm Acc_Gyro board yog piv txwv li 2mV/deg/s lossis 0.002V/deg/s

Cia peb ua piv txwv, xav tias peb qhov ADC module tau rov qab los raws li qhov txiaj ntsig:

AWM = 571 AdcGyroXZ = 323

Siv cov qauv saum toj no, thiab siv cov kev qhia tshwj xeeb ntawm Acc_Gyro board peb yuav tau txais:

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 deg/s

Hauv lwm lo lus lub cuab yeej tig ncig Y axis (lossis peb tuaj yeem hais nws tig hauv XZ dav hlau) nrog nrawm ntawm 306 deg/s thiab ncig X axis (lossis peb tuaj yeem hais nws tig hauv YZ dav hlau) nrog nrawm ntawm - 94 degree/feeb Thov nco ntsoov tias qhov cim tsis zoo txhais tau tias lub cuab yeej tig rov qab los ntawm txoj kev coj ua zoo ib yam. Los ntawm kev pom zoo ib qho kev taw qhia ntawm kev sib hloov yog qhov zoo. Daim ntawv qhia kev ntsuas lub ntsej muag zoo yuav qhia koj tias txoj kev taw qhia twg yog qhov zoo, txwv tsis pub koj yuav tsum pom nws los ntawm kev sim nrog lub cuab yeej thiab ceeb toom qhov kev taw qhia ntawm kev sib hloov ua rau nce qhov hluav taws xob ntawm tus pin tso tawm. Qhov no ua tau zoo tshaj plaws siv oscilloscope txij li thaum koj nres qhov kev sib hloov qhov hluav taws xob yuav poob rov qab mus rau qib xoom. Yog tias koj siv lub multimeter koj yuav tsum tswj hwm qhov sib hloov tas li tsawg kawg ob peb vib nas this thiab nco ntsoov qhov hluav taws xob thaum lub sijhawm hloov pauv no, tom qab ntawd sib piv nws nrog qhov ntsuas hluav taws xob xoom. Yog tias nws ntau dua li qhov xoom-tus nqi hluav taws xob nws txhais tau tias kev taw qhia ntawm kev sib hloov yog qhov zoo.

Kauj Ruam 3: Ua ke ntawm Accelerometer thiab Gyro

Muab nws tag nrho ua ke - Ua ke nrog cov ntaub ntawv ntsuas qhov ntsuas thiab ntsuas cov ntaub ntawv gyroscope

Yog tias koj tab tom nyeem kab ntawv no tej zaum koj yuav tau txais lossis tab tom npaj kom tau txais IMU cov cuab yeej, lossis tej zaum koj tab tom npaj tsim ib qho los ntawm kev cais cais lub ntsuas cua thiab cov cuab yeej ntsuas lub ntsej muag.

Thawj kauj ruam hauv kev siv cov cuab yeej IMU ua ke uas suav nrog lub ntsuas hluav taws xob thiab lub tshuab ntsuas lub ntsej muag yog ua kom sib haum lawv cov kab ke. Txoj hauv kev yooj yim tshaj plaws los ua yog xaiv cov txheej txheem kev sib koom ua ke ntawm lub ntsuas kev ntsuas raws li koj qhov kev siv ua haujlwm sib tham. Feem ntau cov ntaub ntawv ntsuas qhov ntsuas yuav tso saib X, Y, Z axis txheeb ze rau cov duab ntawm lub cev lossis lub cuab yeej. Piv txwv li ntawm no yog cov lus qhia ntawm X, Y, Z axes raws li qhia hauv cov lus tshwj xeeb rau Pawg Acc_Gyro:

Cov kauj ruam tom ntej yog:

Txheeb xyuas cov txiaj ntsig gyroscope uas sib haum rau RateAxz, RateAyz qhov tseem ceeb tau tham los saum toj no

Tsis txhob xav tias yog lub gyroscope muaj cov cim cim X lossis Y, nws yuav sib haum rau txhua lub axis hauv kev sib koom ua ke nrawm, txawm hais tias qhov tso tawm no yog ib feem ntawm chav IMU. Txoj hauv kev zoo tshaj yog sim nws. Piv txwv tias koj tau kho txoj haujlwm ntawm gyroscope txheeb ze rau lub ntsuas cua. Nws tau kwv yees tias tus ciam teb gyro thiab tus ntsuas tus lej sib npaug sib luag rau ib leeg, piv txwv li koj tab tom tso lub gyro ntawm lub kaum sab xis ntau ntawm 90deg txheeb ze rau cov nti accelerometer. Yog tias koj tau txais IMU lub rooj tsav xwm muaj txoj hauv kev yog tias lawv tau ua raws txoj hauv kev no. Peb yuav tsis tham txog hauv kab lus no ua qauv uas lub gyroscope tau muab tso rau ntawm lub kaum sab xis tsis sib xws txheeb ze rau lub ntsuas cua (cia hais 45 lossis 30 degrees), txawm hais tias qhov no yuav muaj txiaj ntsig zoo hauv qee daim ntawv thov.

Nov yog cov qauv ua ntu zus los txiav txim seb qhov khoom siv ntawm gyroscope sib raug rau RateAxz tus nqi sib tham saum toj no.

- pib los ntawm tso lub cuab yeej rau txoj haujlwm kab rov tav. Ob qho X thiab Y cov txiaj ntsig ntawm qhov ntsuas hluav taws xob yuav ua rau qhov xoom-g voltage (piv txwv li rau Acc_Gyro board qhov no yog 1.65V)

- tom ntej no pib tig lub cuab yeej nyob ib ncig ntawm Y axis, lwm txoj hauv kev hais tias nws yog qhov koj tig lub cuab yeej hauv XZ lub dav hlau, yog li X thiab Z lub ntsuas cua nrawm hloov pauv thiab Y cov zis tseem nyob tas li. - thaum tig lub cuab yeej ntawm qhov nrawm nrawm uas qhov hloov pauv ntawm qhov ntsuas hluav taws xob, lwm qhov kev ntsuas qhov ntsuas hluav taws xob yuav tsum nyob tas li - qhov ntsuas hluav taws xob gyroscope uas hloov pauv thaum hloov pauv ib puag ncig Y axis (kev sib hloov hauv XZ dav hlau) yuav muab tus nqi nkag rau AdcGyroXZ, los ntawm qhov uas peb xam RateAxz - cov kauj ruam kawg yog kom ntseeg tau qhov kev sib hloov coj mus rau peb tus qauv, qee zaum koj yuav tsum tau hloov tus nqi RateAxz vim yog txoj haujlwm ntawm lub cev ntawm gyroscope txheeb ze rau lub ntsuas cua - ua dua qhov ntsuas saum toj no, tig lub cuab yeej ib puag ncig lub Y axis, lub sijhawm no saib xyuas X qhov tso tawm ntawm lub ntsuas cua (AdcRx hauv peb tus qauv). Yog tias AdcRx loj hlob (thawj 90 degrees ntawm kev sib hloov los ntawm txoj haujlwm kab rov tav), tom qab ntawd AdcGyroXZ yuav tsum loj hlob tuaj. Txwv tsis pub koj yuav tsum tau hloov pauv RateAxz, koj tuaj yeem ua tiav qhov no los ntawm kev qhia lub cim kos npe hauv Eq.3, raws li hauv qab no:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / rhiab heev, qhov InvertAxz yog 1 lossis -1

kev ntsuas kab ib yam yuav ua tiav rau RateAyz, los ntawm kev hloov lub cuab yeej nyob ib ncig ntawm X axis, thiab koj tuaj yeem txheeb xyuas qhov ntsuas qhov ntsuas hluav taws xob uas sib xws rau RateAyz, thiab yog tias nws xav tau hloov pauv. Thaum koj muaj tus nqi rau InvertAyz, koj yuav tsum siv cov qauv hauv qab no los xam RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / rhiab heev

Yog tias koj yuav ua qhov kev xeem no ntawm Acc_Gyro board koj yuav tau txais cov txiaj ntsig hauv qab no:

- tus pin tso tawm rau RateAxz yog GX4 thiab InvertAxz = -1. - tus pin tso tawm rau RateAyz yog GY4 thiab InvertAyz = -1

Los ntawm qhov no peb yuav txiav txim siab tias koj tau teeb tsa koj li IMU hauv txoj hauv kev uas koj tuaj yeem suav qhov tseeb rau Axr, Ayr, Azr (raws li tau hais tseg Tshooj 1. Accelerometer) thiab RateAxz, RateAyz (raws li tau hais tseg hauv Ntu 2. Gyroscope)). Tom ntej no peb yuav tshuaj xyuas kev sib raug zoo ntawm cov txiaj ntsig no uas ua rau muaj txiaj ntsig kom tau txais kev kwv yees ntau dua ntawm kev nyiam ntawm lub cuab yeej txheeb ze rau lub dav hlau hauv av.

Tej zaum koj yuav tau nug koj tus kheej los ntawm cov ntsiab lus no, yog tias tus qauv ntsuas qhov ntsuas tau muab rau peb lub kaum sab xis ntawm Axr, Ayr, Azr vim li cas peb thiaj xav thab thab nrog cov ntaub ntawv gyroscope? Cov lus teb yog yooj yim: cov ntaub ntawv ntsuas ntsuas tsis tuaj yeem ntseeg tau 100%. Muaj ntau qhov laj thawj, nco ntsoov tias lub ntsuas cua ntsuas lub zog inertial, lub zog zoo li no tuaj yeem tshwm sim los ntawm kev nqus (thiab lub hom phiaj tsuas yog los ntawm kev nqus), tab sis nws kuj tseem yuav tshwm sim los ntawm kev nrawm (txav) ntawm lub cuab yeej. Raws li qhov tshwm sim txawm tias lub ntsuas hluav taws xob nyob hauv lub xeev ruaj khov, nws tseem tseem rhiab heev rau kev co thiab suab nrov hauv tshuab feem ntau. Qhov no yog qhov laj thawj tseem ceeb vim li cas feem ntau IMU cov tshuab siv lub tshuab ntsuas lub ntsej muag kom du tawm ib qho kev ntsuas tsis raug. Tab sis qhov no ua tiav li cas? Thiab puas yog lub gyroscope tsis muaj suab nrov?

Lub gyroscope tsis pub dawb los ntawm lub suab nrov txawm li cas los xij vim nws ntsuas kev sib hloov nws tsis muaj kev nkag siab yooj yim rau kev txav cov tshuab, hom suab nrov uas lub suab ntsuas lub cev raug mob, txawm li cas los xij gyroscopes muaj lwm yam teeb meem zoo li piv txwv drift (tsis rov qab los rau tus lej-tus nqi thaum kev sib hloov nres). Txawm li cas los xij los ntawm qhov nruab nrab cov ntaub ntawv uas los ntawm kev ntsuas tus ntsuas thiab ntsuas lub ntsej muag peb tuaj yeem tau txais kwv yees kwv yees zoo dua ntawm cov cuab yeej siv tam sim no ntau dua li peb xav tau los ntawm kev siv cov ntaub ntawv ntsuas qhov ntsuas ib leeg.

Hauv cov kauj ruam tom ntej no kuv yuav qhia cov txheej txheem uas tau tshoov siab los ntawm qee lub tswv yim siv hauv Kalman lim, txawm li cas los nws yog qhov yooj yim dua thiab yooj yim dua los siv rau ntawm cov khoom siv. Ua ntej ntawd cia saib ua ntej dab tsi peb xav kom peb lub algorithm los xam. Zoo, nws yog qhov kev taw qhia ntawm lub zog nqus vector R = [Rx, Ry, Rz] los ntawm qhov uas peb tuaj yeem tau txais lwm qhov txiaj ntsig zoo li Axr, Ayr, Azr lossis cosX, xis nyob, cosZ uas yuav muab tswv yim rau peb txog kev nyiam ntawm peb lub cuab yeej txheeb ze rau lub dav hlau hauv av, peb tham txog qhov sib piv ntawm cov txiaj ntsig no hauv Tshooj 1. Ib tus yuav hais - tsis yog peb twb muaj cov txiaj ntsig no Rx, Ry, Rz los ntawm Eq.2 hauv Ntu 1? Yog lawm, tab sis nco ntsoov tias cov txiaj ntsig no tau los ntawm cov ntaub ntawv ntsuas tus ntsuas nkaus xwb, yog li yog tias koj yuav siv lawv ncaj qha rau hauv koj daim ntawv thov koj yuav tau txais lub suab nrov ntau dua li koj daim ntawv thov tuaj yeem tso siab tau. Txhawm rau zam kev tsis meej pem ntxiv peb rov txhais cov ntsuas kev ntsuas raws li hauv qab no:

Racc - yog lub zog inertial vector raws li ntsuas los ntawm lub ntsuas cua, uas muaj cov khoom hauv qab no (kwv yees ntawm X, Y, Z axes):

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

Txog tam sim no peb muaj txheej txheej ntsuas ntsuas uas peb tuaj yeem tau txais los ntawm qhov ntsuas qhov ntsuas ADC qhov tseem ceeb. Peb yuav hu cov ntaub ntawv no "vector" thiab peb yuav siv cov cim hauv qab no.

Racc = [RxAcc, RyAcc, RzAcc]

Vim tias cov khoom no ntawm Racc tuaj yeem tau txais los ntawm cov ntaub ntawv ntsuas ntsuas, peb tuaj yeem txiav txim siab nws qhov kev nkag mus rau peb cov txheej txheem.

Thov nco ntsoov tias vim tias Racc ntsuas lub zog nqus pa koj yuav raug yog tias koj xav tias qhov ntev ntawm cov kab ntawv no tau piav raws li hauv qab no yog sib npaug lossis ze rau 1g.

| Raug | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Txawm li cas los xij kom ntseeg tau tias nws tsim nyog los hloov kho vector no raws li hauv qab no:

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

Qhov no yuav ua kom ntseeg tau qhov ntev ntawm koj li qub Racc vector yog ib txwm 1.

Tom ntej no peb yuav qhia lub vector tshiab thiab peb yuav hu nws

So = [RxEst, RyEst, RzEst]

Qhov no yuav yog qhov tsim tawm ntawm peb cov txheej txheem, cov no yog kho qhov muaj txiaj ntsig raws li cov ntaub ntawv ntsuas qhov ntsuas thiab raws li cov ntaub ntawv kwv yees yav dhau los.

Nov yog qhov peb cov txheej txheem yuav ua li cas: - ntsuas lub zog qhia peb: "Tam sim no koj nyob ntawm txoj haujlwm Racc" - peb hais "Ua tsaug, tab sis cia kuv tshuaj xyuas", - tom qab ntawd kho cov ntaub ntawv no nrog cov ntaub ntawv gyroscope nrog rau yav dhau los Cov ntaub ntawv so thiab peb tso tawm qhov kwv yees tshiab vector So. - peb txiav txim siab So kom yog peb "thawj koom ruam zoo tshaj" raws li txoj haujlwm tam sim no ntawm lub cuab yeej.

Cia saib seb peb tuaj yeem ua kom nws ua haujlwm li cas.

Peb yuav pib peb qhov ua ntu zus los ntawm kev ntseeg peb tus accelerometer thiab muab:

So (0) = Racc (0)

Los ntawm txoj kev nco ntsoov So thiab Racc yog vectors, yog li qhov sib npaug saum toj no tsuas yog txoj hauv kev yooj yim los sau 3 pawg sib npaug, thiab zam kev rov ua dua:

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

Tom ntej no peb yuav ntsuas ntsuas ib ntus ntawm T vib nas this, thiab peb yuav tau txais kev ntsuas tshiab uas peb yuav txhais raws li Racc (1), Racc (2), Racc (3) thiab ntxiv rau. Peb tseem yuav tshaj tawm qhov kwv yees tshiab ntawm txhua lub sijhawm sib nrug So (1), So (2), So (3) thiab ntxiv rau.

Piv txwv tias peb nyob ntawm theem n. Peb muaj ob qhov paub txog cov txiaj ntsig uas peb xav siv:

So (n -1) - peb qhov kwv yees yav dhau los, nrog So (0) = Racc (0) Racc (n) - peb qhov ntsuas tam sim no

Ua ntej peb tuaj yeem suav So (n), cia peb qhia tus nqi ntsuas tshiab, uas peb tuaj yeem tau txais los ntawm peb lub gyroscope thiab kwv yees yav dhau los.

Peb yuav hu nws Rgyro, thiab nws tseem yog cov vector uas muaj 3 yam sib xws:

Rgyro = [RxGyro, RyGyro, RzGyro]

Peb yuav suav qhov vector no ib feem ntawm ib lub sijhawm. Peb yuav pib nrog RxGyro.

Cia peb pib los ntawm kev soj ntsuam cov kev sib raug zoo hauv qab no hauv peb tus qauv gyroscope, los ntawm txoj cai kaum sab xis tsim los ntawm Rz thiab Rxz peb tuaj yeem tau txais qhov ntawd:

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

Atan2 tej zaum yuav yog txoj haujlwm uas koj tsis tau siv ua ntej, nws zoo ib yam li atan, tshwj tsis yog nws rov qab qhov txiaj ntsig hauv ntau ntawm (-PI, PI) tsis zoo li (-PI/2, PI/2) raws li xa rov qab los ntawm atan, thiab nws siv 2 kev sib cav hloov ib qho. Nws tso cai rau peb hloov ob qhov tseem ceeb ntawm Rx, Rz rau cov ces kaum nyob rau hauv tag nrho ntawm 360 degrees (-PI rau PI). Koj tuaj yeem nyeem ntxiv txog atan2 ntawm no.

Yog li paub RxEst (n-1), thiab RzEst (n-1) peb tuaj yeem pom:

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

Nco ntsoov tias gyroscope ntsuas qhov kev hloov pauv ntawm Axz lub kaum ntse ntse. Yog li peb tuaj yeem kwv yees lub kaum ntse ntse tshiab Axz (n) raws li hauv qab no:

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

Nco ntsoov tias RateAxz tuaj yeem tau txais los ntawm peb lub gyroscope ADC nyeem ntawv. Cov mis ntau dua tuaj yeem siv qhov nruab nrab qhov sib piv xam raws li hauv qab no:

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

Tib txoj kev uas peb tuaj yeem pom:

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

Ok tam sim no peb muaj Axz (n) thiab Ayz (n). Peb mus qhov twg los ntawm qhov no txhawm rau txiav RxGyro/RyGyro? Los ntawm Eq. 1 peb tuaj yeem sau qhov ntev ntawm vector Rgyro raws li hauv qab no:

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

Tsis tas li vim peb tau siv peb li qub Racc vector, peb yuav xav tias nws ntev yog 1 thiab nws tsis tau hloov pauv tom qab kev sib hloov, yog li nws muaj kev nyab xeeb los sau:

| Rwg | = 1

Cia peb saws cov ntawv luv luv ib ntus rau cov lej hauv qab no:

x = RxGyro, y = RyGyro, z = RzGyro

Siv kev sib raug zoo saum toj no peb tuaj yeem sau:

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

Cia peb faib tus lej thiab cov lej ntawm feem ntawm SQRT (x^2 + z^2)

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

Nco ntsoov tias x / SQRT (x^2 + z^2) = sin (Axz), yog li:

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

Tam sim no suav tus lej thiab cov lej ntawm cov feem hauv SQRT los ntawm z^2

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

Nco ntsoov tias z / SQRT (x^2 + z^2) = cos (Axz) thiab y / z = tan (Ayz), yog li thaum kawg:

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

Rov qab mus rau peb qhov kev ceeb toom peb tau txais:

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

tib txoj kev peb pom tias

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

Tam sim no, thaum kawg peb tuaj yeem pom:

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

Qhov twg Kos Npe (RzGyro) = 1 thaum RzGyro> = 0, thiab Kos Npe (RzGyro) = -1 thaum RzGyro <0.

Ib txoj hauv kev yooj yim los kwv yees qhov no yog coj:

Kos Npe (RzGyro) = Kos Npe (RzEst (n-1))

Hauv kev xyaum ceev faj thaum RzEst (n-1) ze rau 0. Koj tuaj yeem hla qib gyro zoo rau qhov no thiab muab: Rgyro = So (n-1). Rz tau siv los ua tus lej rau xam Axz thiab Ayz cov ces kaum thiab thaum nws ze rau 0, qhov txiaj ntsig yuav dhau mus thiab ua rau cov txiaj ntsig tsis zoo. Koj yuav yog tus lej ntawm cov lej loj ntab nyob qhov twg tan () / atan () kev ua haujlwm ua haujlwm yuav tsis muaj qhov tseeb.

Yog li cia peb rov qab saib seb peb muaj dab tsi txog tam sim no, peb nyob rau theem n ntawm peb cov txheej txheem thiab peb tau suav cov hauv qab no qhov tseem ceeb:

Racc - kev nyeem tam sim no los ntawm peb lub ntsuas cua Rgyro - tau txais los ntawm So (n -1) thiab kev ntsuas ntsuas tam sim no

Cov txiaj ntsig twg peb siv los laij qhov kwv yees hloov kho so (n)? Tej zaum koj tau kwv yees tias peb yuav siv ob qho tib si. Peb yuav siv qhov hnyav nruab nrab, yog li:

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

Peb tuaj yeem yooj yim cov mis no los ntawm kev faib ob tus lej thiab cov lej ntawm cov feem ntawm w1.

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

thiab tom qab hloov w2/w1 = wGyro peb tau txais:

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

Hauv cov rooj sab laj saum toj no wGyro qhia peb ntau npaum li cas peb ntseeg peb cov gyro piv rau peb lub ntsuas cua. Tus nqi no tuaj yeem xaiv ua qhov kev sim feem ntau ntawm 5..20 yuav ua rau muaj txiaj ntsig zoo.

Qhov sib txawv tseem ceeb ntawm cov txheej txheem no los ntawm Kalman lim yog qhov hnyav no tau teeb tsa ruaj khov, thaum nyob hauv Kalman lim qhov hnyav tau hloov kho tas mus li raws li ntsuas suab nrov ntawm kev ntsuas lub ntsuas. Kalman lim tau tsom rau muab koj "qhov zoo tshaj plaws" cov txiaj ntsig kev xav, qhov twg cov txheej txheem no tuaj yeem muab cov txiaj ntsig rau koj "zoo txaus" rau koj daim ntawv thov. Koj tuaj yeem siv qhov txheej txheem uas hloov wGyro nyob ntawm qee yam suab nrov uas koj ntsuas, tab sis cov txiaj ntsig ruaj khov yuav ua haujlwm zoo rau feem ntau daim ntawv thov.

Peb yog ib kauj ruam deb ntawm qhov tau txais peb qhov kwv yees kwv yees qhov txiaj ntsig:

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

Tam sim no cia ua qhov vector no ib txwm:

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

Thiab peb npaj tau rov ua peb lub voj dua.

Phau ntawv qhia no tau tshwm sim thawj zaug ntawm starlino.com, Kuv tau hloov kho ob peb lub teeb thiab rov tso nws nrog kev tso cai. Ua tsaug Starlino!