Акселерометр ба гиро заавар: 3 алхам

2024 Зохиолч: John Day | [email protected]. Хамгийн сүүлд өөрчлөгдсөн: 2024-01-30 11:06

Танилцуулга

Энэхүү гарын авлага нь электроникийн төслүүддээ Accelerometers, Gyroscopes, мөн IMU хосолсон төхөөрөмжүүдийг (инерцийн хэмжих нэгж) ашиглах сонирхолтой хүмүүст зориулагдсан болно.

Бид хамрах болно:

• Акселерометр юу хэмждэг вэ?
• Гироскоп (гиро гэж нэрлэдэг) юу хэмждэг вэ?
• Эдгээр мэдрэгчээс авсан аналоги-тоон (ADC) заалтыг физик нэгж рүү хэрхэн хөрвүүлэх вэ (эдгээр нь акселерометрийн хувьд g, гироскопын хувьд deg/s байх болно)
• Төхөөрөмжийнхөө газрын хавтгайтай харьцах налуугийн талаар үнэн зөв мэдээлэл олж авахын тулд акселерометр ба гироскопын заалтыг хэрхэн хослуулах вэ?

Өгүүллийн туршид би математикийг аль болох бага байлгахыг хичээх болно. Хэрэв та Синус/Косинус/Тангенс гэж юу болохыг мэддэг бол 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 -р тэгшитгэл)

Энэ нь үндсэндээ 3 хэмжээст Пифагорын теоремтой тэнцүү юм.

Хэсэг хугацааны өмнө би танд SQRT (1/2) ~ 0.71 утгууд санамсаргүй биш гэдгийг хэлж байсныг санаарай. Хэрэв та тэдгээрийг дээрх томъёогоор холбовол манай таталцлын хүч 1 гр байсан гэдгийг сануулсны дараа бид дараах зүйлийг шалгаж болно.

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

1 -р тэгшитгэлд R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) -ийг орлуулснаар л болно.

Удаан хугацааны онолын оршилын дараа бид бодит амьдрал дахь акселерометртэй ойртож байна. Rx, Ry, Rz утгууд нь бодит амьдрал дээрх акселерометрийн гаргадаг утгатай шууд хамааралтай бөгөөд та янз бүрийн тооцоолол хийхэд ашиглаж болно.

Тэнд очихоосоо өмнө акселерометрүүд энэ мэдээллийг бидэнд хэрхэн хүргэх талаар бага зэрэг ярилцъя. Ихэнх акселерометрийг дижитал ба аналог гэсэн хоёр төрөлд хуваах болно. Дижитал хурдатгал хэмжигч нь I2C, SPI эсвэл USART зэрэг цуваа протокол ашиглан мэдээлэл өгөх бол аналог хурдатгал хэмжигч нь хүчдэлийн түвшинг урьдчилан тодорхойлсон хязгаарт гаргадаг бөгөөд та үүнийг ADC (аналогоос тоон хөрвүүлэгч) модулийг ашиглан дижитал утга руу хөрвүүлэх ёстой. Би ADC хэрхэн ажилладаг талаар нэг их дэлгэрэнгүй ярихгүй, зарим талаараа энэ нь маш өргөн хүрээтэй сэдэв бөгөөд нөгөө талаас нэг платформоос нөгөөгөөсөө ялгаатай юм. Зарим микроконтроллер нь ADC-ийн суулгасан модулиудтай байх бөгөөд зарим нь ADC хөрвүүлэлтийг хийхийн тулд гадны бүрэлдэхүүн хэсгүүд хэрэгтэй болно. Та ямар төрлийн ADC модулийг ашиглаж байгаагаас үл хамааран тодорхой хязгаарт хүрэх болно. Жишээлбэл, 10 битийн ADC модуль нь 0..1023 хүртэлх утгыг гаргадаг бөгөөд 1023 = 2^10 -1 болохыг анхаарна уу. 12 битийн ADC модуль нь 0..4095 хүртэлх утгыг гаргадаг бөгөөд 4095 = 2^12-1 болохыг анхаарна уу.

Энгийн жишээг авч үзье. Бидний 10 битийн ADC модуль нь хурдатгал хэмжигч гурван суваг (тэнхлэг) -ийн хувьд дараах утгыг өгсөн гэж бодъё.

AdcRx = 586 AdcRy = 630 AdcRz = 561

ADC модуль бүр лавлах хүчдэлтэй байх болно, жишээн дээр энэ нь 3.3V байна гэж үзье. 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

Хурдатгал хэмжигч бүр тэг-г хүчдэлийн түвшинтэй байдаг бөгөөд үүнийг техникийн үзүүлэлтүүдээс олж болно, энэ нь 0г-тэй тэнцүү хүчдэл юм. Гарын үсэг зурсан хүчдэлийн утгыг авахын тулд бид энэ түвшингээс шилжилтийг тооцоолох хэрэгтэй. Бидний 0g хүчдэлийн түвшин VzeroG = 1.65V байна гэж бодъё. Бид тэг-g хүчдэлээс хүчдэлийн шилжилтийг дараах байдлаар тооцоолно.

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

Бид одоо акселерометрийн заалтыг вольтоор авсан боловч g (9.8 м/с^2) -т ороогүй хэвээр байгаа бөгөөд эцсийн хөрвүүлэлтийг хийхийн тулд акселерометрийн мэдрэмжийг ихэвчлэн 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 -р тэгшитгэл)

Бидэнд инерцийн хүчний векторыг тодорхойлдог бүх 3 бүрэлдэхүүн хэсгүүд байгаа бөгөөд хэрэв төхөөрөмж нь таталцлаас бусад хүчэнд өртөөгүй бол энэ нь манай таталцлын хүчний векторын чиглэл гэж үзэж болно. Хэрэв та төхөөрөмжийн газрын хазайлтыг газартай харьцуулахыг хүсч байвал энэ вектор ба Z тэнхлэгийн хоорондох өнцгийг тооцоолж болно. Хэрэв та нэг тэнхлэгийн өнцгийн чиглэлийг сонирхож байвал энэ үр дүнг 2 бүрэлдэхүүн хэсэгт хувааж болно: таталцлын вектор ба X / Y тэнхлэгүүдийн хоорондох өнцөг гэж тооцож болох X ба Y тэнхлэгийн хазайлт. Эдгээр өнцгийг тооцоолох нь таны бодож байгаагаас хамаагүй энгийн бөгөөд одоо бид Rx, Ry, Rz -ийн утгыг тооцоолсон болно. Сүүлийн акселерометрийн загвар руу буцаж очоод нэмэлт тэмдэглэгээ хийцгээе.

Бидний сонирхож буй өнцөг бол X, Y, Z тэнхлэг ба R хүчний хүчний хоорондох өнцөг юм. Бид эдгээр өнцгийг Axr, Ayr, Azr гэж тодорхойлно. Та R ба Rx-ийн үүсгэсэн тэгш өнцөгт гурвалжингаас дараахь зүйлийг анзаарч болно.

cos (Axr) = Rx / R, мөн үүнтэй адил: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Бид тэгшитгэл 1 -ээс R = SQRT (Rx^2 + Ry^2 + Rz^2) гэдгийг хасч болно.

Бид arccos () функцийг (урвуу cos () функц) ашиглан одоо өнцгөө олох боломжтой болно.

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

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

Гэхдээ үүнийг хийхээсээ өмнө илүү хэрэгтэй тэмдэглэлүүдийг хийцгээе.

cosX = cos (Axr) = Rx / R тухтай = 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 - XZ хавтгай дээрх инерцийн хүчний вектор R -ийн проекц Ryz - YZ хавтгай дээрх инерцийн хүчний вектор R -ийн проекц юм.

Пифагорын теоремыг ашиглан 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 тэнхлэг Ayz хоорондох өнцөг - Ryz (YZ хавтгай дээрх R төсөөлөл) ба Z тэнхлэгийн хоорондох өнцөг юм.

Одоо бид гироскопын хэмждэг зүйл рүү ойртож байна. Гироскоп нь дээр дурдсан өнцгийн өөрчлөлтийн хурдыг хэмждэг. Өөрөөр хэлбэл эдгээр өнцгийн өөрчлөлтийн хурдтай шууд хамааралтай утгыг гаргана. Үүнийг тайлбарлахын тулд бид Y тэнхлэгийг тойрон эргэх өнцгийг (энэ нь Axz өнцөг байх болно) t0 үед хэмжсэн бөгөөд үүнийг Axz0 гэж тодорхойлсон бол дараа нь t1 дараа нь энэ өнцгийг хэмжсэн бөгөөд энэ нь Axz1 байсан гэж үзье. Өөрчлөлтийн түвшинг дараах байдлаар тооцоолно.

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

Хэрэв бид Axz -ийг градусаар, хугацааг секундээр илэрхийлбэл энэ утгыг deg/s -ээр илэрхийлэх болно. Үүнийг гироскопоор хэмждэг.

Практик дээр гироскоп (хэрэв тусгай дижитал гироскоп биш бол) танд deg/s -ээр илэрхийлэгдсэн утгыг өгөх нь ховор байдаг. Акселерометрийн нэгэн адил та тэгшитгэлтэй төстэй томъёог ашиглан deg/s болгон хөрвүүлэх шаардлагатай ADC утгыг авах болно. 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.3V хүчдэлийн ADC - тэг түвшинтэй хүчдэл, өөрөөр хэлбэл эргэлт хийх боломжгүй үед гироскопын гаргаж буй хүчдэл, Acc_Gyro хавтангийн хувьд Жишээ нь 1.23V (та энэ утгыг техникийн үзүүлэлтүүдээс олж болно) Мэдрэмж - таны гироскопын мэдрэмтгий байдлыг ихэвчлэн mV / deg / s гэж бичдэг бөгөөд энэ нь үндсэндээ хэдэн мВ байх болно Хэрэв та эргэлтийн хурдыг нэг градус/сек -ээр нэмэгдүүлэх юм бол гироскопын гаралт нэмэгдэнэ. Acc_Gyro хавтангийн мэдрэмж нь 2мВ/градус/сек эсвэл 0.002В/градус/сек байна

Жишээ авч үзье, манай 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 гэж тэмдэглэсэн гаралттай бол энэ нь IMU нэгжийн хэсэг байсан ч гэсэн акселерометрийн координатын системийн аль ч тэнхлэгт харгалзана гэж бүү бодоорой. Үүнийг туршиж үзэх нь хамгийн сайн арга юм. Та акселерометртэй харьцуулахад гироскопын байрлалыг тогтоосон гэж үзье. Гиро ба акселерометрийн хил хоорондоо параллель байна гэж таамаглаж байна, өөрөөр хэлбэл та акселерометрийн чиптэй харьцуулахад гирог 90 градусын өнцөгт байрлуулна. Хэрэв та ОУХБХ -ны удирдах зөвлөлийг авсан бол тэдгээр нь аль хэдийн ийм байдлаар тохируулагдсан байх магадлал өндөр байна. Энэ нийтлэлд гироскопыг акселерометртэй харьцуулахад жигд бус өнцгөөр (45 эсвэл 30 градус гэж үзье) байрлуулсан загваруудын талаар ярихгүй, гэхдээ энэ нь зарим програмд ашигтай байж магадгүй юм.

Дээр дурдсан RateAxz утгатай гироскопын ямар гарц тохирч байгааг тодорхойлохын тулд дээжийн дарааллыг энд оруулав.

- төхөөрөмжийг хэвтээ байрлалд байрлуулахаас эхэл. Акселерометрийн X ба Y гаралт нь тэг-г хүчдэлийг гаргадаг (жишээлбэл Acc_Gyro хавтангийн хувьд энэ нь 1.65V байна)

- Дараа нь төхөөрөмжийг Y тэнхлэгийн эргэн тойронд эргүүлж эхэл, өөрөөр хэлбэл та төхөөрөмжийг XZ хавтгайд эргүүлэх бөгөөд ингэснээр X ба Z акселерометрийн гаралт өөрчлөгдөж, Y гаралт тогтмол хэвээр байх болно. - Төхөөрөмжийг гироскопын гаралт өөрчлөгдөх тогтмол хурдаар эргүүлэх үед бусад гироскопын гаралт тогтмол байх ёстой - Y тэнхлэгийг эргүүлэх явцад өөрчлөгдсөн гироскопын гаралт (XZ хавтгайд эргэх) нь AdcGyroXZ -ийн оролтын утгыг өгөх болно. Бид RateAxz -ийг тооцоолдог - эцсийн алхам бол эргэлтийн чиглэл нь манай загварт нийцэж байгаа эсэхийг баталгаажуулах явдал юм, зарим тохиолдолд гироскопын физик байрлалаас шалтгаалан акселерометртэй харьцуулахад RateAxz -ийн утгыг эргүүлэх шаардлагатай болж магадгүй юм. Y тэнхлэг, энэ удаад акселерометрийн X гаралтыг хянах (манай загварт AdcRx). Хэрэв AdcRx өсөх юм бол (хэвтээ байрлалаас эргэлтийн эхний 90 градус) AdcGyroXZ бас өсөх ёстой. Үгүй бол та RateAxz -ийг эргүүлэх шаардлагатай бол Eq.3 -т тэмдгийн коэффициентийг дараах байдлаар оруулснаар хүрч болно.

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Мэдрэмж, энд 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 -р хэсэгт заасан Accelerometer) болон 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]

Энэ бол бидний алгоритмын гаралт байх болно, эдгээр нь гироскопын өгөгдөл дээр үндэслэсэн, өнгөрсөн тооцоолсон өгөгдөл дээр үндэслэсэн залруулсан утгууд юм.

Бидний алгоритм дараахь зүйлийг хийх болно. Бид шинэ тооцоолсон векторыг гаргадаг. - Бид 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) гэх мэтээр тодорхойлох шинэ хэмжилтүүдийг авах болно. Бид мөн амрах (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 нь таны урьд өмнө хэзээ ч ашиглаж байгаагүй функц байж болох юм, гэхдээ энэ нь atan-тай төстэй, гэхдээ (-PI, PI) -ийн утгыг буцааж өгдөг (-PI/2, PI/2) нь atan-ээр буцааж өгдөг бөгөөд үүнийг шаарддаг. Нэгийн оронд 2 аргумент. Энэ нь Rx, Rz гэсэн хоёр утгыг 360 градусын өнцөг болгон хөрвүүлэх боломжийг бидэнд олгодог (-PI to 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 векторын уртыг дараах байдлаар бичиж болно.

| Ргиро | = SQRT (RxGyro^2 + RyGyro^2 + RzGyro^2)

Мөн бид Racc вектороо хэвийн болгосон тул түүний урт нь 1, эргүүлсний дараа өөрчлөгдөөгүй гэж үзэж болох тул бичих нь харьцангуй аюулгүй юм.

| Ргиро | = 1

Доорх тооцооллын түр зуурын богино тэмдэглэгээг авч үзье.

x = RxGyro, y = RyGyro, z = RzGyro

Дээрх харилцааг ашиглан бид дараахь зүйлийг бичиж болно.

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

Бутархай тоологч ба хуваагуурыг SQRT (x^2 + z^2) -д хуваацгаая.

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

X / SQRT (x^2 + z^2) = 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 байх үед Sign (RzGyro) = 1, RzGyro <0 байхад Sign (RzGyro) = -1 байна.

Үүнийг тооцоолох нэг энгийн арга бол дараахь зүйлийг хийх явдал юм.

Тэмдэг (RzGyro) = Тэмдэг (RzEst (n-1))

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

Тиймээс одоо байгаа зүйлээ нэгтгэн дүгнэж үзье, бид алгоритмынхаа n -р шатанд байгаа бөгөөд дараах утгуудыг тооцоолсон болно.

Racc - манай Rgyro акселерометрийн одоогийн уншилтууд - Rest (n -1) болон одоогийн гироскопын заалтаас авсан болно.

Rest (n) шинэчилсэн тооцоог тооцоолохдоо бид ямар утгыг ашигладаг вэ? Бид хоёуланг нь ашиглах болно гэж та таамаглаж байсан байх. Бид жигнэсэн дундажийг ашиглах бөгөөд ингэснээр:

Амрах (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Бид энэ томъёог хялбарчлах боломжтой бөгөөд бутархай хэсгийн тоологч ба хуваагчийг хоёуланд нь хувааж болно w1.

Амрах (n) = (Racc * w1/w1 + Rgyro * w2/w1)/(w1/w1 + w2/w1)

w2/w1 = wGyro -г орлуулсны дараа бид дараахь зүйлийг авна.

Амрах (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

Дээрх форумд wGyro нь хурдатгал хэмжигчтэйгээ харьцуулахад гиродоо хэр их итгэж байгаагаа хэлдэг. Энэ утгыг туршилтаар сонгож болно, ихэвчлэн 5..20 хоорондох утгыг сайн үр дүнд хүргэдэг.

Калман шүүлтүүрээс энэ алгоритмын гол ялгаа нь энэ жин харьцангуй тогтмол байдаг бол Калман шүүлтүүрт акселерометрийн заалтын дуу чимээг үндэслэн жинг байнга шинэчилж байдаг. Калман шүүлтүүр нь танд "хамгийн сайн" онолын үр дүнг өгөхөд чиглэсэн байдаг бол энэ алгоритм нь таны практик хэрэглээнд "хангалттай сайн" үр дүнг өгч чадна. Та хэмжиж буй зарим дуу чимээний хүчин зүйлээс хамааран wGyro -ийг тохируулах алгоритмыг хэрэгжүүлж болох боловч тогтмол утгууд ихэнх програмуудад сайн ажиллах болно.

Бид шинэчилсэн тооцоолсон үнэ цэнээ авахад нэг алхам дутуу байна.

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

Одоо энэ векторыг дахин хэвийн болгоё.

R = SQRT (RxEst (n)^2 + RyEst (n)^2 + RzEst (n)^2)

RxEst (n) = RxEst (n)/R RyEst (n) = RyEst (n)/R RzEst (n) = RzEst (n)/R

Мөн бид давталтаа дахин давтахад бэлэн байна.

Энэхүү гарын авлага нь анх starlino.com дээр гарч ирсэн бөгөөд би хэд хэдэн хөнгөн засвар хийж, зөвшөөрөлтэйгээр дахин нийтэлсэн. Баярлалаа Starlino!