Thực hành hiệu chuẩn tĩnh gia tốc kế MEMS bằng trọng trường: sáu vị trí, độ lệch, độ nhạy chéo

Hiệu chuẩn tĩnh gia tốc kế MPU-6050 bằng trọng trường với ESP32: sáu vị trí ±1 g, tính độ lệch không, độ nhạy, độ nhạy chéo dạng ma trận, lưu hệ số và kiểm tra độ lớn gia tốc ở tư thế bất kỳ.

Bài thực hành cuối của loạt: hiệu chuẩn tĩnh một gia tốc kế MEMS ba trục MPU-6050 mà không cần máy rung, chỉ dùng chuẩn gia tốc có sẵn ở khắp nơi là trọng trường Trái Đất. Đặt cảm biến lần lượt sáu tư thế (mỗi trục hướng lên rồi hướng xuống), chương trình ESP32 đo số đếm trung bình, tính độ lệch không, độ nhạy và độ nhạy chéo của từng trục dưới dạng một ma trận 3×3, lưu hệ số và đổi số đếm ra m/s² theo g tại chỗ. Cách làm này tương ứng với phương pháp hiệu chuẩn bằng trọng trường trong ISO 16063-16 và rất hữu ích cho các ứng dụng đo góc nghiêng, đo gia tốc tĩnh, bù trọng lực.

1. Nguyên lý: trọng trường là chuẩn ±1 g

Gia tốc kế đứng yên chỉ chịu trọng trường. Trục nào hướng thẳng lên sẽ đọc +1 g (gia tốc riêng hướng lên), hướng xuống đọc −1 g, nằm ngang đọc 0. Với hai số đếm A⁺ và A⁻ của cùng một trục khi hướng lên và hướng xuống:

• Độ nhạy S = (A⁺ − A⁻) / (2g), không phụ thuộc độ lệch không.
• Độ lệch không b = (A⁺ + A⁻) / 2, không phụ thuộc độ nhạy.
• Độ nhạy chéo: khi trục i hướng lên hoặc xuống, hai trục kia lẽ ra đọc như nhau (chỉ còn độ lệch không); phần chênh lệch cho biết chúng "nhìn thấy" bao nhiêu gia tốc của trục i, do trục cảm biến không vuông góc tuyệt đối hoặc gắn lệch.

Gom lại, mỗi cột i của ma trận độ nhạy M là (A(+i) − A(−i))/(2g). Khi đo, gia tốc được tính bằng a = M⁻¹ · (số đếm − b). Điều quan trọng: g phải là gia tốc trọng trường tại chỗ (ví dụ khoảng 9,782 m/s² ở TP.HCM, 9,787 m/s² ở Hà Nội), không phải 9,80665.

2. Chuẩn bị và nối dây

• ESP32 DevKit V1 30 chân, module MPU-6050 (GY-521), dây cắm test board.
• Một khối vuông có sáu mặt phẳng, vuông góc (khối nhôm gia công, hộp gỗ vuông vức hoặc khối in 3D cẩn thận); bắt module chặt vào khối, các trục song song với các cạnh.
• Một mặt phẳng ngang (bàn đá, mặt kính) đã chỉnh bằng bọt thủy; dây cắm đủ dài, mềm, để lật khối không kéo lệch module.

3. Chương trình

Chương trình đọc thanh ghi MPU-6050 bằng thư viện Wire, lưu hệ số bằng Preferences; không cần thư viện ngoài. Lệnh gõ trong Serial Monitor (115200 baud, kiểu xuống dòng Newline):

/*
  PYWORLD.VN - Thực hành hiệu chuẩn tĩnh gia tốc kế MEMS MPU-6050 bằng trọng trường: sáu vị trí, độ lệch, độ nhạy, độ nhạy chéo
  MPU-6050 (module GY-521): VCC = 3V3, GND = GND, SDA = GPIO21, SCL = GPIO22, AD0 nối GND (địa chỉ 0x68)
  Lệnh Serial: p1..p6 ghi vị trí (+X, -X, +Y, -Y, +Z, -Z hướng lên), k tính và lưu, g<số> đặt g tại chỗ, m bật/tắt đo
*/

#include <Wire.h>
#include <Preferences.h>
#include <math.h>

#define CHAN_SDA 21
#define CHAN_SCL 22
#define SO_MAU 200                        // Số lần đọc cho mỗi vị trí (khoảng 1 giây)

const uint8_t DIA_CHI_MPU = 0x68;
const uint8_t TG_PWR_MGMT_1 = 0x6B;
const uint8_t TG_CONFIG = 0x1A;           // Bộ lọc thông thấp số
const uint8_t TG_ACCEL_CONFIG = 0x1C;     // 0 = thang ±2 g
const uint8_t TG_ACCEL_XOUT_H = 0x3B;
const char *TEN_VI_TRI[6] = {"+X len", "-X len", "+Y len", "-Y len", "+Z len", "-Z len"};

Preferences boNho;
double soDemViTri[6][3];                  // Số đếm trung bình của ba trục ở từng vị trí
double lechChuanViTri[6][3];
float nhietDoViTri[6];
bool coViTri[6] = {false, false, false, false, false, false};
double lech[3] = {0, 0, 0};               // Độ lệch không (số đếm) của từng trục
double maTranNghich[3][3];                // Đổi số đếm đã trừ lệch ra m/s²
float gDiaPhuong = 9.7821;                // m/s², ví dụ TP.HCM; đặt lại bằng lệnh g
bool dangDo = false;

bool ghiThanhGhi(uint8_t thanhGhi, uint8_t giaTri) {
  Wire.beginTransmission(DIA_CHI_MPU);
  Wire.write(thanhGhi);
  Wire.write(giaTri);
  return Wire.endTransmission() == 0;
}

// Đọc số đếm thô của ba trục gia tốc và nhiệt độ
bool docTho(int16_t truc[3], float *nhietDo) {
  Wire.beginTransmission(DIA_CHI_MPU);
  Wire.write(TG_ACCEL_XOUT_H);
  if (Wire.endTransmission(false) != 0 || Wire.requestFrom(DIA_CHI_MPU, (uint8_t)8) != 8) {
    return false;
  }
  int16_t raw[4];
  for (int i = 0; i < 4; i++) {
    uint8_t cao = Wire.read();
    uint8_t thap = Wire.read();
    raw[i] = (int16_t)((cao << 8) | thap);
  }
  for (int i = 0; i < 3; i++) {
    truc[i] = raw[i];
  }
  *nhietDo = raw[3] / 340.0f + 36.53f;
  return true;
}

bool docTrungBinh(int n, double tb[3], double s[3], float *nhietDo) {
  double tong[3] = {0, 0, 0}, tongBP[3] = {0, 0, 0};
  float tongNhiet = 0;
  for (int k = 0; k < n; k++) {
    int16_t truc[3];
    float t;
    if (!docTho(truc, &t)) {
      return false;
    }
    for (int i = 0; i < 3; i++) {
      tong[i] += truc[i];
      tongBP[i] += (double)truc[i] * truc[i];
    }
    tongNhiet += t;
    delay(5);
  }
  for (int i = 0; i < 3; i++) {
    tb[i] = tong[i] / n;
    double phuongSai = (tongBP[i] - tong[i] * tong[i] / n) / (n - 1);
    s[i] = phuongSai > 0 ? sqrt(phuongSai) : 0;
  }
  *nhietDo = tongNhiet / n;
  return true;
}

// Nghịch đảo ma trận 3x3 bằng phần bù đại số
bool nghichDao(const double m[3][3], double kq[3][3]) {
  double det = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
             - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
             + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
  if (fabs(det) < 1e-9) {
    return false;
  }
  kq[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]) / det;
  kq[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) / det;
  kq[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) / det;
  kq[1][0] = (m[1][2] * m[2][0] - m[1][0] * m[2][2]) / det;
  kq[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) / det;
  kq[1][2] = (m[0][2] * m[1][0] - m[0][0] * m[1][2]) / det;
  kq[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]) / det;
  kq[2][1] = (m[0][1] * m[2][0] - m[0][0] * m[2][1]) / det;
  kq[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) / det;
  return true;
}

// Gia tốc (m/s²) từ số đếm: a = N · (số đếm − lệch)
void tinhGiaToc(const double soDem[3], double a[3]) {
  for (int i = 0; i < 3; i++) {
    a[i] = 0;
    for (int j = 0; j < 3; j++) {
      a[i] += maTranNghich[i][j] * (soDem[j] - lech[j]);
    }
  }
}

void datMacDinh() {
  double danhDinh = 16384.0 / 9.80665;   // số đếm mỗi m/s² theo thông số, thang ±2 g
  for (int i = 0; i < 3; i++) {
    lech[i] = 0;
    for (int j = 0; j < 3; j++) {
      maTranNghich[i][j] = (i == j) ? 1.0 / danhDinh : 0.0;
    }
  }
}

void luuHeSo() {
  boNho.begin("giatocke", false);
  boNho.putBytes("lech", lech, sizeof(lech));
  boNho.putBytes("nghich", maTranNghich, sizeof(maTranNghich));
  boNho.putFloat("g", gDiaPhuong);
  boNho.end();
}

void napHeSo() {
  boNho.begin("giatocke", true);
  if (boNho.getBytesLength("nghich") == sizeof(maTranNghich)) {
    boNho.getBytes("lech", lech, sizeof(lech));
    boNho.getBytes("nghich", maTranNghich, sizeof(maTranNghich));
    gDiaPhuong = boNho.getFloat("g", gDiaPhuong);
    Serial.println("Da nap he so hieu chuan da luu");
  } else {
    datMacDinh();
    Serial.println("Chua co he so luu: dung do nhay danh dinh 16384 dem/g");
  }
  boNho.end();
}

void lenhGhiViTri(int k) {
  if (k < 1 || k > 6) {
    Serial.println("Vi tri tu 1 den 6");
    return;
  }
  double tb[3], s[3];
  float t;
  if (!docTrungBinh(SO_MAU, tb, s, &t)) {
    Serial.println("Khong doc duoc MPU-6050");
    return;
  }
  for (int i = 0; i < 3; i++) {
    soDemViTri[k - 1][i] = tb[i];
    lechChuanViTri[k - 1][i] = s[i];
  }
  nhietDoViTri[k - 1] = t;
  coViTri[k - 1] = true;
  Serial.printf("Vi tri %d (%s): X %.1f | Y %.1f | Z %.1f dem (s = %.1f, %.1f, %.1f) | %.1f C\n",
                k, TEN_VI_TRI[k - 1], tb[0], tb[1], tb[2], s[0], s[1], s[2], t);
}

void lenhTinh() {
  for (int k = 0; k < 6; k++) {
    if (!coViTri[k]) {
      Serial.printf("Thieu vi tri %d (%s)\n", k + 1, TEN_VI_TRI[k]);
      return;
    }
  }
  // Cột i của M: số đếm ba trục khi trục i hướng lên trừ khi hướng xuống, chia 2g (số đếm mỗi m/s²)
  double m[3][3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      m[j][i] = (soDemViTri[2 * i][j] - soDemViTri[2 * i + 1][j]) / (2.0 * gDiaPhuong);
    }
    lech[i] = (soDemViTri[2 * i][i] + soDemViTri[2 * i + 1][i]) / 2.0;
  }
  if (!nghichDao(m, maTranNghich)) {
    Serial.println("Khong tinh duoc: kiem tra lai cac vi tri");
    return;
  }
  const char truc[3] = {'X', 'Y', 'Z'};
  for (int j = 0; j < 3; j++) {
    int a = (j + 1) % 3, b = (j + 2) % 3;
    Serial.printf("Truc %c: lech %+.1f dem (%+.1f mg), do nhay %.1f dem/g (%+.2f %% so voi 16384), nhay cheo %c %+.2f %%, %c %+.2f %%\n",
                  truc[j], lech[j], lech[j] / (m[j][j] * 9.80665) * 1000, m[j][j] * 9.80665, (m[j][j] * 9.80665 / 16384.0 - 1) * 100,
                  truc[a], m[j][a] / m[j][j] * 100, truc[b], m[j][b] / m[j][j] * 100);
  }
  luuHeSo();
  Serial.printf("Da luu he so (g = %.4f m/s2)\n", gDiaPhuong);
}

void inHuongDan() {
  Serial.println("Lenh: p1..p6 ghi vi tri (+X,-X,+Y,-Y,+Z,-Z len) | k tinh va luu | g<so> dat g | m bat/tat do");
}

void xuLyLenh() {
  if (!Serial.available()) {
    return;
  }
  String lenh = Serial.readStringUntil('\n');
  lenh.trim();
  if (lenh.length() == 0) {
    return;
  }
  char ma = lenh.charAt(0);
  String thamSo = lenh.substring(1);
  thamSo.trim();
  if (ma == 'p') {
    lenhGhiViTri(thamSo.toInt());
  } else if (ma == 'k') {
    lenhTinh();
  } else if (ma == 'g') {
    float g = thamSo.toFloat();
    if (g > 9.7 && g < 9.9) {
      gDiaPhuong = g;
      Serial.printf("Da dat g = %.4f m/s2 (go k de tinh lai)\n", gDiaPhuong);
    }
  } else if (ma == 'm') {
    dangDo = !dangDo;
    Serial.println(dangDo ? "Bat che do do" : "Tat che do do");
  } else {
    inHuongDan();
  }
}

void setup() {
  Serial.begin(115200);
  Wire.begin(CHAN_SDA, CHAN_SCL, 400000);
  delay(100);
  Serial.println();
  // Đánh thức chip, lọc thông thấp khoảng 44 Hz, thang ±2 g
  if (!ghiThanhGhi(TG_PWR_MGMT_1, 0x00) || !ghiThanhGhi(TG_CONFIG, 0x03) || !ghiThanhGhi(TG_ACCEL_CONFIG, 0x00)) {
    Serial.println("Khong thay MPU-6050 o dia chi 0x68: kiem tra day SDA, SCL va nguon");
  }
  napHeSo();
  inHuongDan();
}

void loop() {
  xuLyLenh();
  if (!dangDo) {
    return;
  }
  double tb[3], s[3], a[3];
  float t;
  if (!docTrungBinh(20, tb, s, &t)) {
    Serial.println("Khong doc duoc MPU-6050");
    delay(1000);
    return;
  }
  tinhGiaToc(tb, a);
  double doLon = sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
  Serial.printf("ax %+.4f | ay %+.4f | az %+.4f m/s2 | |a| %.4f (lech %+.2f %% so voi g)\n",
                a[0], a[1], a[2], doLon, (doLon / gDiaPhuong - 1) * 100);
  delay(300);
}

• Chương trình bật bộ lọc thông thấp số của MPU-6050 (khoảng 44 Hz) và lấy trung bình 200 lần đọc mỗi vị trí để giảm nhiễu.
• Độ nhạy in ra theo đơn vị số đếm mỗi g chuẩn để dễ so với thông số danh định 16384 số đếm/g ở thang ±2 g.
• Độ lớn |a| khi đứng yên ở tư thế bất kỳ phải bằng g tại chỗ: đây là phép kiểm tra nhanh và độc lập với sáu vị trí đã dùng.

4. Trình tự

1. Bật nguồn, để cảm biến ổn định nhiệt vài phút; nhiệt độ in ra ở mỗi vị trí nên gần nhau.
2. Đặt g tại chỗ bằng lệnh g nếu không ở TP.HCM.
3. Đặt khối với mặt +X hướng lên, chờ yên hẳn (không chạm vào bàn), gõ p1. Lặp lại cho −X (p2), +Y, −Y, +Z, −Z.
4. Gõ k, đọc kết quả từng trục.
5. Gõ m, đặt khối nghiêng tùy ý (tựa vào một vật), kiểm tra |a| gần bằng g.

5. Kết quả mong đợi

• Độ lệch không vài chục mg (trục Z thường lớn nhất ở MPU-6050). Không bù, góc nghiêng tính ra sai tới khoảng 3°.
• Độ nhạy lệch thông số danh định tới 1–2 %; cộng với độ lệch không, độ lớn |a| trước hiệu chuẩn sai tới 7 % trong ví dụ (có cả phần do dùng 9,80665 thay vì g tại chỗ).
• Độ nhạy chéo dưới 1 %, gồm cả độ không vuông góc của khối gá.
• Sau hiệu chuẩn, ở một tư thế không dùng khi tính, |a| chỉ lệch khoảng 0,003 % so với g.

6. Độ không đảm bảo và giới hạn của phương pháp

Trọng trường và mặt phẳng ngang là chuẩn rất tốt: biết g tới vài phần vạn và nghiêng 0,5° chỉ gây sai số 0,004 %. Giới hạn nằm ở chính cảm biến và khối gá: độ phi tuyến khi dùng ngoài ±1 g, ảnh hưởng nhiệt độ, độ vuông góc của khối. Ngoài ra cần nhớ:

• Hiệu chuẩn tĩnh chỉ cho độ nhạy và độ lệch ở tần số 0 (một chiều). Dùng gia tốc kế MEMS để đo rung thì vẫn phải kiểm đáp ứng tần số trên máy rung, như với gia tốc kế áp điện.
• Độ lệch không của MEMS trôi theo nhiệt độ và theo thời gian; ứng dụng cần chính xác nên hiệu chuẩn lại ở nhiệt độ sử dụng hoặc bù theo nhiệt độ.
• Thang đo khác (±4, ±8, ±16 g) có độ nhạy và độ lệch khác, phải hiệu chuẩn riêng.

7. Xử lý lỗi

8. Xem thêm

• Cảm biến gia tốc: cấu tạo MEMS răng lược, áp điện, cách chế tạo và đo góc nghiêng bằng ESP32.
• Các bài khác trong loạt: chuẩn lực và gia tốc trọng trường tại Việt Nam, chuẩn rung và hiệu chuẩn gia tốc kế theo ISO 16063, gá lắp gia tốc kế theo ISO 5348.