MPU 6050 Gyro, การสื่อสารด้วยมาตรความเร่งกับ Arduino (Atmega328p): 5 ขั้นตอน

2024 ผู้เขียน: John Day | [email protected]. แก้ไขล่าสุด: 2024-01-30 13:03

MPU6050 IMU มีทั้งมาตรความเร่งแบบ 3 แกนและไจโรสโคปแบบ 3 แกนที่รวมอยู่ในชิปตัวเดียว

ไจโรสโคปวัดความเร็วในการหมุนหรืออัตราการเปลี่ยนแปลงของตำแหน่งเชิงมุมเมื่อเวลาผ่านไปตามแกน X, Y และ Z

เอาต์พุตของไจโรสโคปมีหน่วยเป็นองศาต่อวินาที ดังนั้นเพื่อให้ได้ตำแหน่งเชิงมุม เราจำเป็นต้องรวมความเร็วเชิงมุมเข้าด้วยกัน

ในทางกลับกัน มาตรความเร่ง MPU6050 จะวัดความเร่งโดยการวัดความเร่งโน้มถ่วงตาม 3 แกน และใช้คณิตศาสตร์ตรีโกณมิติ เราสามารถคำนวณมุมที่ตำแหน่งเซ็นเซอร์อยู่ได้ ดังนั้น หากเราหลอมรวมหรือรวมข้อมูลมาตรความเร่งและไจโรสโคป เราจะได้รับข้อมูลที่แม่นยำมากเกี่ยวกับการวางแนวเซ็นเซอร์

ไจโรสโคป 3 แกน MPU-6050 ประกอบด้วยไจโรสโคป 3 แกน ซึ่งสามารถตรวจจับความเร็วการหมุนตามแนวแกน x, y, z ด้วยเทคโนโลยีไมโครอิเล็กโทรกลระบบ (MEMS) เมื่อเซ็นเซอร์หมุนไปตามแกนใด ๆ จะเกิดการสั่นขึ้นเนื่องจากเอฟเฟกต์ Coriolis ซึ่งตรวจพบโดย MEMS.16 บิต ADC จะใช้เพื่อแปลงแรงดันไฟฟ้าเป็นดิจิทัลเพื่อสุ่มตัวอย่างแต่ละแกน+/- 250, +/- 500, +/- 1000, +/- 2000 คือช่วงเอาต์พุตเต็มสเกล ความเร็วเชิงมุมวัดตามแกนแต่ละแกนในหน่วยองศาต่อวินาที

ลิงค์ที่เป็นประโยชน์:…………….

บอร์ด Arduino:.……….

MPU6050 IMU ……………https://compoindia.com/product/mpu6050-3-axis-accelerometer-and-gyroscope-sensor/

ขั้นตอนที่ 1: โมดูล MPU-6050

โมดูล MPU-6050 มี 8 พิน

INT: พินเอาต์พุตดิจิตอลขัดจังหวะ

AD0: พิน LSB ที่อยู่ทาส I2C นี่คือบิตที่ 0 ในที่อยู่ทาส 7 บิตของอุปกรณ์ หากเชื่อมต่อกับ VCC จะถูกอ่านว่าเป็นลอจิกหนึ่งและการเปลี่ยนแปลงที่อยู่รอง

XCL: พินนาฬิกาอนุกรมเสริม พินนี้ใช้เพื่อเชื่อมต่อเซ็นเซอร์ SCL ที่เปิดใช้งานอินเทอร์เฟซ I2C อื่น ๆ กับ MPU-6050

XDA: พินข้อมูลอนุกรมเสริม พินนี้ใช้เพื่อเชื่อมต่อเซ็นเซอร์ SDA ที่เปิดใช้งานอินเทอร์เฟซ I2C อื่น ๆ กับ MPU-6050

SCL: เข็มนาฬิกาแบบอนุกรม เชื่อมต่อพินนี้กับพิน SCL ของไมโครคอนโทรลเลอร์ SDA: พินข้อมูลอนุกรม เชื่อมต่อพินนี้กับพิน SDA ของไมโครคอนโทรลเลอร์

GND: ขากราวด์ เชื่อมต่อพินนี้กับการเชื่อมต่อกราวด์

VCC: พินพาวเวอร์ซัพพลาย เชื่อมต่อพินนี้กับแหล่งจ่ายไฟ DC +5V โมดูล MPU-6050 มีที่อยู่ทาส (เมื่อ AD0 = 0 นั่นคือไม่ได้เชื่อมต่อกับ Vcc) เช่น

ที่อยู่การเขียนทาส (SLA+W): 0xD0

ที่อยู่อ่านทาส (SLA+R): 0xD1

ขั้นตอนที่ 2: การคำนวณ

ข้อมูลเซ็นเซอร์ Gyroscope และ Accelerometer ของโมดูล MPU6050 ประกอบด้วยข้อมูลดิบ 16 บิตในรูปแบบเสริมของ 2

ข้อมูลเซ็นเซอร์อุณหภูมิของโมดูล MPU6050 ประกอบด้วยข้อมูล 16 บิต (ไม่อยู่ในรูปแบบเสริมของ 2)

สมมุติว่าเราได้เลือกแล้ว

• - มาตรวัดความเร่งแบบเต็มช่วง +/- 2g พร้อม Sensitivity Scale Factor ที่ 16, 384 LSB (จำนวน)/g
• - Gyroscope เต็มรูปแบบช่วง +/- 250 °/s พร้อม Sensitivity Scale Factor ที่ 131 LSB (จำนวน)/°/s แล้ว,

ในการรับข้อมูลดิบของเซ็นเซอร์ เราต้องดำเนินการเสริม 2 อย่างกับข้อมูลเซ็นเซอร์ของมาตรความเร่งและไจโรสโคปก่อน หลังจากได้รับข้อมูลดิบของเซ็นเซอร์แล้ว เราสามารถคำนวณความเร่งและความเร็วเชิงมุมได้โดยการหารข้อมูลดิบของเซ็นเซอร์ด้วยปัจจัยระดับความไวดังนี้ --

ค่าความเร่งในหน่วย g (แรง g)

• การเร่งความเร็วตามแกน X = (ข้อมูลดิบแกน X ของมาตรความเร่ง/16384) g.
• การเร่งความเร็วตามแกน Y = (ข้อมูลดิบแกน Y ของมาตรความเร่ง/16384) g.
• การเร่งความเร็วตามแนวแกน Z = (ข้อมูลดิบแกน Z ของมาตรความเร่ง/16384) g.

ค่า Gyroscope เป็น °/s (องศาต่อวินาที)

• ความเร็วเชิงมุมตามแนวแกน X = (ข้อมูลดิบแกน Gyroscope X/131) °/s
• ความเร็วเชิงมุมตามแนวแกน Y = (ข้อมูลดิบแกน Gyroscope Y/131) °/s
• ความเร็วเชิงมุมตามแนวแกน Z = (ข้อมูลดิบของแกน Gyroscope Z/131) °/s

ค่าอุณหภูมิเป็น°/c (องศาต่อเซลเซียส)

อุณหภูมิเป็นองศา C = ((ข้อมูลเซ็นเซอร์อุณหภูมิ)/340 + 36.53) °/c

ตัวอย่างเช่น, สมมติว่าหลังจากส่วนเสริม 2 'เราได้รับค่าแกน accelerometer X ดิบ = +15454

แล้วขวาน = +15454/16384 = 0.94 กรัม

มากกว่า,

เรารู้ว่าเรากำลังวิ่งที่ความไว +/-2G และ +/- 250deg/s แต่ค่าของเราสอดคล้องกับความเร่ง/มุมเหล่านั้นอย่างไร

เหล่านี้เป็นทั้งกราฟเส้นตรงและเราสามารถคำนวณได้ว่าสำหรับ 1G เราจะอ่าน 16384 และสำหรับ 1 องศา/วินาที เราจะอ่าน 131.07 (แม้ว่า. 07 จะถูกละเว้นเนื่องจากไบนารี) ค่าเหล่านี้เพิ่งคิดออกโดยการวาด กราฟเส้นตรงที่มี 2G ที่ 32767 และ -2G ที่ -32768 และ 250/-250 ที่ค่าเดียวกัน

ตอนนี้เรารู้ค่าความไวของเราแล้ว (16384 และ 131.07) เราแค่ต้องลบการชดเชยจากค่าของเราแล้วหารด้วยความไว

สิ่งเหล่านี้จะทำงานได้ดีสำหรับค่า X และ Y แต่เมื่อ Z ถูกบันทึกที่ 1G และไม่ใช่ 0 เราจะต้องลบ 1G (16384) ก่อนที่เราจะหารด้วยความไวของเรา

ขั้นตอนที่ 3: การเชื่อมต่อ MPU6050-Atmega328p

เพียงเชื่อมต่อทุกอย่างตามที่ระบุในแผนภาพ…

การเชื่อมต่อจะได้รับดังนี้:-

MPU6050 Arduino นาโน

VCC 5v ขาออก

GND กราวด์พิน

พิน SDA A4 // ข้อมูลซีเรียล

ขา SCL A5 // นาฬิกาอนุกรม

การคำนวณ Pitch and Roll: การหมุนคือการหมุนรอบแกน x และ pitch คือการหมุนตามแกน y

ผลลัพธ์เป็นเรเดียน (แปลงเป็นองศาโดยการคูณ 180 และหารด้วย pi)

ขั้นตอนที่ 4: รหัสและคำอธิบาย

/*

Arduino และ MPU6050 Accelerometer และ Gyroscope Sensor Tutorial โดย Dejan, https://howtomechatronics.com */ #include const int MPU = 0x68; // ที่อยู่ MPU6050 I2C ลอย AccX, AccY, AccZ; ลอย GyroX, GyroY, GyroZ; ลอย accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ; ม้วนลอย, ขว้าง, หันเห; ลอย AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ; float elapsedTime, currentTime, เวลาก่อนหน้า; int c = 0; การตั้งค่าเป็นโมฆะ () { Serial.begin (19200); Wire.begin(); // เริ่มต้นการสื่อสาร Wire.beginTransmission(MPU); // เริ่มการสื่อสารด้วย MPU6050 // MPU=0x68 Wire.write(0x6B); // คุยกับรีจิสเตอร์ 6B Wire.write(0x00); // ทำการรีเซ็ต - วาง 0 ลงในการลงทะเบียน 6B Wire.endTransmission(true); // สิ้นสุดการส่ง /* // กำหนดค่าความไวของตัวตรวจวัดความเร่ง - ช่วงเต็มสเกล (ค่าเริ่มต้น +/- 2g) Wire.beginTransmission(MPU); Wire.write(0x1C); //พูดคุยกับ ACCEL_CONFIG register (1C hex) Wire.write(0x10); // ตั้งค่าบิตรีจิสเตอร์เป็น 00010000 (+/- 8g full scale range) Wire.endTransmission(true); // กำหนดค่าความไวของไจโร - ช่วงเต็มสเกล (ค่าเริ่มต้น +/- 250deg/s) Wire.beginTransmission(MPU); Wire.write(0x1B); // คุยกับรีจิสเตอร์ GYRO_CONFIG (1B hex) Wire.write(0x10); // ตั้งค่าบิตรีจิสเตอร์เป็น 00010000 (1000deg/s เต็มสเกล) Wire.endTransmission(true); ล่าช้า (20); */ // เรียกใช้ฟังก์ชันนี้หากคุณต้องการรับค่าความผิดพลาดของ IMU สำหรับโมดูลของคุณ ล่าช้า (20); } วงเป็นโมฆะ () { // === อ่านข้อมูลการเร่งความเร็ว === // Wire.beginTransmission (MPU); Wire.write(0x3B); // เริ่มต้นด้วยการลงทะเบียน 0x3B (ACCEL_XOUT_H) Wire.endTransmission (เท็จ); Wire.requestFrom(MPU, 6, จริง); // อ่านทั้งหมด 6 รีจิสเตอร์ แต่ละค่าแกนจะถูกเก็บไว้ใน 2 รีจิสเตอร์ //สำหรับช่วง +-2g เราต้องหารค่าดิบด้วย 16384 ตามแผ่นข้อมูล AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // ค่าแกน X AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // ค่าแกน Y AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // ค่าแกน Z // การคำนวณ Roll และ Pitch จากข้อมูลมาตรความเร่ง accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) ดูฟังก์ชัน calcAngleY ที่คำนวณได้เอง calcAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58) // === อ่านข้อมูลไจโรสโคป === // เวลาก่อนหน้า = currentTime; // เวลาก่อนหน้าจะถูกเก็บไว้ก่อนเวลาจริงที่อ่าน currentTime = millis(); // เวลาปัจจุบัน อ่านเวลาจริงที่ผ่านไป = (currentTime - PreviousTime) / 1000; // หารด้วย 1,000 เพื่อรับวินาที 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 เราต้องหารค่าดิบก่อนด้วย 131.0 ตามแผ่นข้อมูล GyroY = (Wire.read() << 8 | Wire.read()) / 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) // ปัจจุบันค่า raw เป็นองศาต่อวินาที deg/s ดังนั้นเราจึงต้องคูณด้วย sendonds เพื่อให้ได้มุมเป็นองศา gyroAngleX = gyroAngleX + GyroX * elapsedTime; // องศา/วินาที * s = องศา gyroAngleY = gyroAngleY + GyroY * เวลาที่ผ่านไป; yaw = หันเห + GyroZ * เวลาที่ผ่านไป; // ตัวกรองเสริม - รวมค่า acceleromter และ gyro angle roll = 0.96 * gyroAngleX + 0.04 * accAngleX; ระยะห่าง = 0.96 * gyroAngleY + 0.04 * accAngleY; // พิมพ์ค่าบนจอภาพอนุกรม Serial.print(roll); Serial.print("/"); Serial.print(พิทช์); Serial.print("/"); Serial.println (หันเห); } void คำนวณ_IMU_error() {// เราสามารถเรียกใช้ฟังก์ชันนี้ในส่วนการตั้งค่าเพื่อคำนวณข้อผิดพลาดของข้อมูลมาตรความเร่งและไจโร จากที่นี่เราจะได้ค่าความผิดพลาดที่ใช้ในสมการด้านบนที่พิมพ์บน Serial Monitor // โปรดทราบว่าเราควรวาง IMU แบนเพื่อรับค่าที่เหมาะสม เพื่อให้เราสามารถค่าที่ถูกต้อง // อ่านค่า accelerometer 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)); ค ++; } //หารผลรวมด้วย 200 เพื่อรับค่าความผิดพลาด AccErrorX = AccErrorX / 200; AccErrorY = AccErrorY / 200; ค = 0; // อ่านค่าไจโร 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); ค ++; } //หารผลรวมด้วย 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() เราขออ่านการลงทะเบียนทั้ง 6 รายการสำหรับแกน X, Y และ Z จากนั้นเราอ่านข้อมูลจากการลงทะเบียนแต่ละครั้ง และเนื่องจากผลลัพธ์เป็นส่วนประกอบสองส่วน เราจึงรวมเข้าด้วยกันอย่างเหมาะสมเพื่อให้ได้ค่าที่ถูกต้อง

ขั้นตอนที่ 5: ทำความเข้าใจ Tilt Angle

มาตรความเร่ง

แรงโน้มถ่วงของโลกเป็นการเร่งความเร็วคงที่ซึ่งแรงจะชี้ลงสู่ศูนย์กลางของโลกเสมอ

เมื่อมาตรความเร่งขนานกับแรงโน้มถ่วง ความเร่งที่วัดได้จะเป็น 1G เมื่อมาตรความเร่งตั้งฉากกับแรงโน้มถ่วง มันจะวัดค่า 0G

มุมเอียงสามารถคำนวณได้จากความเร่งที่วัดได้โดยใช้สมการนี้:

θ = sin-1 (ความเร่งที่วัดได้ / การเร่งแรงโน้มถ่วง)

GyroGyro (เซ็นเซอร์วัดอัตรา a.k.a.) ใช้เพื่อวัดความเร็วเชิงมุม (ω)

เพื่อให้ได้มุมเอียงของหุ่นยนต์ เราจำเป็นต้องรวมข้อมูลจากไจโรตามที่แสดงในสมการด้านล่าง:

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

ฟิวชั่นเซ็นเซอร์ไจโรและมาตรความเร่งหลังจากศึกษาคุณลักษณะของทั้งไจโรและมาตรความเร่งแล้ว เรารู้ว่าพวกมันมีจุดแข็งและจุดอ่อนของตัวเอง มุมเอียงที่คำนวณได้จากข้อมูลมาตรความเร่งจะมีเวลาตอบสนองที่ช้า ในขณะที่มุมเอียงที่ผสานรวมจากข้อมูลไจโรจะมีความเบี่ยงเบนในช่วงระยะเวลาหนึ่ง กล่าวอีกนัยหนึ่ง เราสามารถพูดได้ว่าข้อมูลมาตรความเร่งมีประโยชน์ในระยะยาว ในขณะที่ข้อมูลไจโรมีประโยชน์ในระยะสั้น

ลิงค์เพื่อความเข้าใจที่ดีขึ้น: คลิกที่นี่