MPU 6050 Gyro, Accelerometer Arduino -тай харилцах (Atmega328p): 5 алхам

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

MPU6050 IMU нь нэг тэнхлэгт 3 тэнхлэгт акселерометр ба 3 тэнхлэгтэй гироскоптой.

Гироскоп нь X, Y, Z тэнхлэгийн дагуу цаг хугацааны явцад эргэх хурд эсвэл өнцгийн байрлалын өөрчлөлтийн хурдыг хэмждэг.

Гироскопын гаралт нь секундэд градус байдаг тул өнцгийн байрлалыг олж авахын тулд бид зөвхөн өнцгийн хурдыг нэгтгэх хэрэгтэй.

Нөгөө талаар, MPU6050 акселерометр нь 3 тэнхлэгийн дагуу таталцлын хурдатгалыг хэмжих замаар хурдатгалыг хэмжиж, тригонометрийн математик ашиглан мэдрэгчийн байрлаж буй өнцгийг тооцоолж болно. Тиймээс, хэрэв бид акселерометр ба гироскопын өгөгдлийг хайлуулж эсвэл нэгтгэвэл мэдрэгчийн чиглэлийн талаар маш үнэн зөв мэдээлэл авах боломжтой болно.

3 тэнхлэгтэй гироскоп MPU-6050 нь 3 тэнхлэгтэй гироскопоос бүрдэх бөгөөд x, y, z тэнхлэгийн дагуух эргэлтийн хурдыг микро цахилгаан механик систем (MEMS) ашиглан илрүүлэх боломжтой. Мэдрэгчийг аль ч тэнхлэгийн дагуу эргүүлэх үед MEMS-ээр илрүүлсэн Coriolis эффектийн улмаас чичиргээ үүсдэг.16 битийн ADC нь тэнхлэг тус бүрийн дээж авах хүчдэлийг дижитал хэлбэрт оруулдаг. +/- 250, +/- 500, +/- 1000, +/- 2000 нь гаралтын бүрэн хэмжээний хүрээ бөгөөд өнцгийн хурдыг тэнхлэг бүрийн дагуу секунд тутамд нэгжээр хэмждэг.

Хэрэгтэй холбоос: …………….

Arduino самбар: ……….

MPU6050 IMU ……………

Алхам 1: MPU-6050 модуль

MPU-6050 модуль нь 8 голтой,

INT: Дижитал гаралтын зүүг таслах.

AD0: I2C боолын хаяг LSB зүү. Энэ бол төхөөрөмжийн 7 битийн боол хаяг дахь 0-р бит юм. Хэрэв VCC -т холбогдсон бол үүнийг логик болон боолын хаягийн өөрчлөлт гэж уншдаг.

XCL: Туслах цуваа цагийн зүү. Энэхүү зүү нь бусад I2C интерфэйстэй SCL зүүг MPU-6050 руу холбоход хэрэглэгддэг.

XDA: Туслах серийн өгөгдлийн зүү. Энэхүү зүү нь бусад I2C интерфэйстэй SDA зүүг MPU-6050-т холбоход хэрэглэгддэг.

SCL: Цуваа цагийн зүү. Энэ зүүг SCL зүү микроконтроллертой холбоно уу. SDA: Цуваа өгөгдлийн зүү. Энэ зүүг микроконтроллер SDA зүү рүү холбоно уу.

GND: Газрын зүү. Энэ зүүг газардуулгатай холбоно уу.

VCC: Цахилгаан тэжээлийн зүү. Энэ зүүг +5V DC тэжээлд холбоно уу. MPU-6050 модуль нь Slave хаягтай (AD0 = 0 байх үед, өөрөөр хэлбэл Vcc-тэй холбогдоогүй)

Боол бичих хаяг (SLA+W): 0xD0

Боол унших хаяг (SLA+R): 0xD1

Алхам 2: Тооцоолол

MPU6050 модулийн гироскоп ба Accelerometer мэдрэгчийн өгөгдөл нь 2-ийн нэмэлт хэлбэрээр 16 битийн түүхий өгөгдлөөс бүрдэнэ.

MPU6050 модулийн температур мэдрэгчийн өгөгдөл нь 16 битийн өгөгдлөөс бүрдэнэ (2-ийн нэмэлт хэлбэрээр биш).

Одоо бид сонгосон гэж бодъё,

• - Акселерометрийн бүрэн масштабтай хүрээ нь +/- 2g, Мэдрэмжийн хуваарийн хүчин зүйл 16, 384 LSB (Count)/g.
• - 131 LSB (Count)/°/s-ийн Мэдрэмжийн хуваарийн хүчин зүйл бүхий +/- 250 °/s-ийн бүрэн хэмжээний хүрээ. дараа нь,

Мэдрэгчийн түүхий өгөгдлийг авахын тулд бид эхлээд Accelerometer болон гироскопын мэдрэгчийн өгөгдөл дээр 2 -ийн нэмэлтийг хийх ёстой. Мэдрэгчийн түүхий өгөгдлийг авсны дараа бид мэдрэгчийн түүхий өгөгдлийг мэдрэмжийн хуваарийн коэффициентэд хувааж хурдатгал ба өнцгийн хурдыг тооцоолж болно.

Акселерометрийн утга g (g хүч)

• X тэнхлэгийн дагуух хурдатгал = (Accelerometer X тэнхлэгийн түүхий өгөгдөл/16384) g.
• Y тэнхлэгийн дагуух хурдатгал = (Accelerometer Y тэнхлэгийн түүхий өгөгдөл/16384) g.
• Z тэнхлэгийн дагуух хурдатгал = (Accelerometer Z тэнхлэгийн түүхий өгөгдөл/16384) g.

Гироскопын утга °/с (секундэд градус)

• X тэнхлэгийн дагуух өнцгийн хурд = (Гироскоп X тэнхлэгийн түүхий өгөгдөл/131) °/с.
• Y тэнхлэгийн дагуух өнцгийн хурд = (Гироскоп Y тэнхлэгийн түүхий өгөгдөл/131) °/с.
• Z тэнхлэгийн дагуух өнцгийн хурд = (Гироскоп Z тэнхлэгийн түүхий өгөгдөл/131) °/с.

Температурын утга °/c (Цельсийн градус)

Температур C градус ((температур мэдрэгчийн мэдээлэл)/340 + 36.53) °/c.

Жишээлбэл, 2 'нэмэлтийн дараа бид X тэнхлэгийн хурдасгуурын түүхий утга = +15454 болно гэж бодъё

Дараа нь Ax = +15454/16384 = 0.94 гр.

Бусад,

Тиймээс бид +/- 2G ба +/- 250deg/s мэдрэмжтэй ажиллаж байгаа гэдгээ мэдэж байгаа боловч бидний үнэ цэнэ эдгээр хурдатгал/өнцөгт хэрхэн нийцэж байна.

Эдгээр нь хоёулаа шулуун шугамын график бөгөөд 1G -ийн хувьд 16384, 1 градус/сек -ийн хувьд 131.07 -ийг унших болно (хэдийгээр.07 нь хоёртын улмаас үл тоомсорлогдох боловч) эдгээр утгыг зөвхөн зурах замаар боловсруулсан болно. 32767 дээр 2G, -32768 дээр -2G ба 250/-250 -тэй ижил утгатай шулуун шугамын график.

Тиймээс одоо бид мэдрэмтгий байдлынхаа утгыг (16384 ба 131.07) мэдэж байгаа бөгөөд бид зөвхөн өөрсдийн үнэ цэнийн зөрүүг хасч, дараа нь мэдрэмжийг нь хуваах хэрэгтэй.

Эдгээр нь X ба Y утгуудын хувьд сайн ажиллах болно, гэхдээ Z -ийг 0 -т биш харин 1G -д бүртгэсэн тул мэдрэмтгий байдлаа хуваахаасаа өмнө 1G (16384) -ийг хасах шаардлагатай болно.

Алхам 3: MPU6050-Atmega328p холболтууд

Диаграммд үзүүлсэн шиг бүх зүйлийг холбоно уу

Холболтыг дараах байдлаар өгсөн болно.

MPU6050 Arduino Nano

VCC 5V зүү

GND газардуулгын зүү

SDA A4 зүү // цуваа өгөгдөл

SCL A5 зүү // цуваа цаг

Давирхай ба өнхрөх тооцоо: Roll бол x тэнхлэгийг тойрон эргэх, давирхай нь y тэнхлэгийн дагуух эргэлт юм.

Үр дүн нь радианаар илэрхийлэгддэг. (180 -аар үржүүлж, пи -д хуваах замаар градус болгон хөрвүүлэх)

Алхам 4: Код ба тайлбар

/*

Arduino болон MPU6050 Accelerometer and Gyroscope Sensor Tutorial by Dejan, https://howtomechatronics.com */#include const int MPU = 0x68; // MPU6050 I2C хаяг float AccX, AccY, AccZ; float GyroX, GyroY, GyroZ; float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ; хөвөх өнхрөх, давирхай, эвхэх; float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ; float elapsedTime, currentTime, previousTime; int c = 0; void setup () {Serial.begin (19200); Wire.begin (); // Wire.beginTransmission (MPU) холболтыг эхлүүлэх; // MPU6050 -тай харилцаж эхлэх // MPU = 0x68 Wire.write (0x6B); // 6B Wire.write бүртгэлтэй ярилцана уу (0x00); // Дахин тохируулах - 6B бүртгэлд 0 оруулах Wire.endTransmission (үнэн); // дамжуулалтыг дуусгах/* // Accelerometer Sensitivity -ийг тохируулах - Бүтэн масштабын хүрээ (анхдагч +/- 2g) Wire.beginTransmission (MPU); Wire.write (0x1C); // ACCEL_CONFIG регистр (1C hex) Wire.write (0x10) -тэй ярилцана уу; // Бүртгэлийн битүүдийг 00010000 (+/- 8g бүрэн хэмжээний муж) болгон тохируулна уу Wire.endTransmission (үнэн); // Gyro Sensitivity -ийг тохируулах - Бүрэн хэмжээний хуваарь (анхдагч +/- 250deg/s) Wire.beginTransmission (MPU); Wire.write (0x1B); // GYRO_CONFIG регистртэй ярилцана уу (1B hex) Wire.write (0x10); // Бүртгэлийн битүүдийг 00010000 (1000deg/s бүрэн хэмжээний) Wire.endTransmission (үнэн) болгож тохируулна уу; саатал (20); */ // Хэрэв та модулийнхаа IMU алдааны утгыг авах шаардлагатай бол энэ функцийг дуудах calc_IMU_error (); саатал (20); } void loop () {// === acceleromter өгөгдлийг унших === // Wire.beginTransmission (MPU); Wire.write (0x3B); // 0x3B регистрээс эхэл (ACCEL_XOUT_H) Wire.endTransmission (худал); Wire.requestFrom (MPU, 6, үнэн); // Нийт 6 регистрийг уншина уу, тэнхлэг бүрийн утга 2 регистрт хадгалагдана // +-2g-ийн хувьд AccX = (Wire.read () << 8 мэдээллийн хүснэгтийн дагуу бид түүхий утгыг 16384-т хуваах хэрэгтэй. | Wire.read ()) / 16384.0; // X тэнхлэгийн утга AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Y тэнхлэгийн утга AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Z тэнхлэгийн утга // AccAngleX = (atan (AccY / sqrt (pow (AccX, 2) + pow (AccZ, 2))) * 180 / PI) хурдасгуурын өгөгдлөөс Roll болон Pitch -ийг тооцоолох - 0.58; // AccErrorX ~ (0.58) Дэлгэрэнгүй мэдээллийг calc_IMU_error () захиалгын функцээс үзнэ үү accAngleY = (atan (-1 * AccX / sqrt (pow (AccY, 2) + pow (AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~ (-1.58) // === Гироскопын өгөгдлийг унших === // previousTime = currentTime; // Өмнөх цагийг бодит цагийн уншихаас өмнө хадгалдаг currentTime = millis (); // Одоогийн цаг уншсан бодит цаг elapsedTime = (currentTime - previousTime) / 1000; // Секунд авахын тулд 1000 -д хуваана Wire.beginTransmission (MPU); Wire.write (0x43); // Gyro өгөгдөл эхлээд бүртгүүлэх хаяг 0x43 Wire.endTransmission (худал); Wire.requestFrom (MPU, 6, үнэн); // Нийт 4 регистрийг уншина уу, тэнхлэг бүрийн утга 2 регистрт хадгалагддаг GyroX = (Wire.read () << 8 | Wire.read ()) / 131.0; // 250deg/ s -ийн хувьд GyroY = (Wire.read () << 8 | Wire.read ())/ 131.0 мэдээллийн хүснэгтийн дагуу бид эхлээд түүхий утгыг 131.0 -т хуваах ёстой. GyroZ = (Wire.read () << 8 | Wire.read ()) / 131.0; // Гаралтыг GyroX = GyroX + 0.56 тооцоолсон алдааны утгуудаар засах; // GyroErrorX ~ (-0.56) GyroY = GyroY - 2; // GyroErrorY ~ (2) GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8) // Одоогоор түүхий утга нь секундэд градус, градус/сек байгаа тул өнцгийг gyroAngleX = gyroAngleX + GyroX * elapsedTime градусаар авахын тулд sendonds (s) -ээр үржүүлэх шаардлагатай; // deg/s * s = deg gyroAngleY = gyroAngleY + GyroY * өнгөрсөн цаг; yaw = yaw + GyroZ * өнгөрсөн цаг; // Нэмэлт шүүлтүүр - хурдасгуур болон гиро өнцгийн утгыг нэгтгэх = 0.96 * gyroAngleX + 0.04 * accAngleX; давирхай = 0.96 * gyroAngleY + 0.04 * accAngleY; // утгыг цуваа дэлгэц дээр хэвлэх Serial.print (өнхрөх); Serial.print ("/"); Цуваа.хэвлэх (давирхай); Serial.print ("/"); Serial.println (yaw); } void calc_IMU_error () {// Бид хурдасгуур болон гиро өгөгдлийн алдааг тооцоолохын тулд тохиргооны хэсэгт энэ функцийг дуудаж болно. Эндээс бид Цуваа монитор дээр хэвлэсэн дээрх тэгшитгэлд ашигласан алдааны утгыг авах болно. // Тохирох утгыг олж авахын тулд бид IMU -г тэгш байрлуулах ёстойг анхаарна уу, ингэснээр бид зөв утгыг олж чадна // Акселерометрийн утгыг 200 удаа уншина уу (c <200) {Wire.beginTransmission (MPU); Wire.write (0x3B); Wire.endTransmission (худал); Wire.requestFrom (MPU, 6, үнэн); AccX = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Бүх уншилтыг нэгтгэх AccErrorX = AccErrorX + ((atan ((AccY) / sqrt (pow ((AccX), 2) + pow ((AccZ), 2))) * 180 / PI)); AccErrorY = AccErrorY + ((atan (-1 * (AccX) / sqrt (pow ((AccY), 2) + pow ((AccZ), 2))) * 180 / PI)); c ++; } // нийлбэрийг 200 -д хувааж AccErrorX = AccErrorX /200 гэсэн алдааны утгыг авна. AccErrorY = AccErrorY / 200; c = 0; // gyro утгыг 200 удаа уншина уу (c <200) {Wire.beginTransmission (MPU); Wire.write (0x43); Wire.endTransmission (худал); Wire.requestFrom (MPU, 6, үнэн); GyroX = Wire.read () << 8 | Wire.read (); GyroY = Wire.read () << 8 | Wire.read (); GyroZ = Wire.read () << 8 | Wire.read (); // Бүх уншилтыг нэгтгэх GyroErrorX = GyroErrorX + (GyroX / 131.0); GyroErrorY = GyroErrorY + (GyroY / 131.0); GyroErrorZ = GyroErrorZ + (GyroZ / 131.0); c ++; } // нийлбэрийг 200 -д хувааж GyroErrorX = GyroErrorX /200 алдааны утгыг авна. GyroErrorY = GyroErrorY / 200; GyroErrorZ = GyroErrorZ / 200; // Алдааны утгыг Serial Monitor Serial.print дээр хэвлэх ("AccErrorX:"); Serial.println (AccErrorX); Serial.print ("AccErrorY:"); Serial.println (AccErrorY); Serial.print ("GyroErrorX:"); Serial.println (GyroErrorX); Serial.print ("GyroErrorY:"); Serial.println (GyroErrorY); Serial.print ("GyroErrorZ:"); Serial.println (GyroErrorZ); } ------------------------------------------------- ---------------------------------------------- Үр дүн:-X = Y = Z = --------------------------------------------- ----------------------------------------------- Анхаарах зүйл: -----------------

Циклийн хэсэгт бид акселерометрийн өгөгдлийг уншиж эхэлдэг. Тэнхлэг бүрийн өгөгдлийг 2 байт буюу регистрт хадгалдаг бөгөөд эдгээр регистрийн хаягийг мэдрэгчийн мэдээллийн хуудаснаас харж болно.

Бүгдийг уншихын тулд бид эхний бүртгэлээс эхэлж, RequiestFrom () функцийг ашиглан X, Y, Z тэнхлэгийн 6 бүртгэлийг уншихыг хүсдэг. Дараа нь бид бүртгэл тус бүрийн өгөгдлийг уншдаг бөгөөд гаралт нь хоёр нэмэлт байдаг тул тэдгээрийг зохих ёсоор нэгтгэн зөв утгыг олж авдаг.

Алхам 5: Налуу өнцгийг ойлгох

Хурдасгуур

Дэлхийн таталцал бол хүч дэлхийн төв рүү үргэлж чиглэсэн тогтмол хурдатгал юм.

Хурдатгал хэмжигч нь хүндийн жинтэй параллель байвал хэмжигдэх хурдатгал нь 1G байх болно.

Энэхүү тэгшитгэлийг ашиглан хазайлтын өнцгийг хэмжсэн хурдатгалаас тооцоолж болно.

θ = sin-1 (Хэмжилсэн хурдатгал / хүндийн хүчний хурдатгал)

GyroGyro (хурдны мэдрэгч) нь өнцгийн хурдыг (ω) хэмжихэд ашиглагддаг.

Роботын хазайлтын өнцгийг олж авахын тулд бид доорхи тэгшитгэлд үзүүлсэн шиг gyro -ийн өгөгдлийг нэгтгэх хэрэгтэй.

ω = dθ / dt, θ = ∫ ω dt

Гиро ба акселерометрийн шинж чанарыг судалсны дараа тэд өөрсдийн давуу болон сул талуудтай гэдгийг бид мэднэ. Акселерометрийн өгөгдлөөс тооцоолсон хазайлтын өнцөг нь удаан хариу үйлдэл үзүүлэх хугацаатай байдаг бол гиро өгөгдлөөс нэгтгэсэн хазайлтын өнцөг нь тодорхой хугацаанд хазайхад хүргэдэг. Өөрөөр хэлбэл, акселерометрийн өгөгдөл урт хугацааны хувьд ашигтай, гиро өгөгдөл нь богино хугацааны хувьд ашигтай гэж бид хэлж чадна.

Илүү сайн ойлгохын тулд холбоос: Энд дарна уу