BÀI 63 – SENSOR FUSION THỰC CHIẾN
ENCODER + MPU6050 + EKF + ODOMETRY X, Y, θ TRÊN STM32F103
Đây là bài ghép toàn bộ những gì chúng ta đã học từ Bài 45 đến Bài 62 thành một hệ thống robot 2 bánh tương đối hoàn chỉnh.
Mục tiêu cuối bài:
ENCODER LEFT ──┐
│
ENCODER RIGHT ─┤
├──→ EKF ──→ X,Y,θ ──→ ROBOT CONTROLLER
│
MPU6050 GYRO ──┘Và STM32F103 sẽ liên tục biết:
X → robot đang ở đâu theo trục X
Y → robot đang ở đâu theo trục Y
θ → robot đang quay về hướng nào
V → robot đang chạy nhanh bao nhiêu
ω → robot đang quay nhanh bao nhiêu1. Bài 63 khác Bài 62 ở đâu?
Bài 62 chủ yếu tập trung vào:
Toán học của EKF.
Bài 63 chuyển sang:
Triển khai thực tế trên STM32F103.
Chúng ta sẽ ghép:
TIM Encoder
↓
Wheel Speed
↓
V, ω
↓
┌───────────┐
MPU6050│ │
Gyro Z ┤ EKF ├──→ X,Y,θ,V,ω
│ │
Encoder│ │
└───────────┘
↓
Odometry2. Phần cứng
Một hệ thống tối thiểu:
STM32
STM32F103C8T6Motor
Motor trái
Motor phảiEncoder
Encoder trái
Encoder phảiIMU
MPU6050Motor driver
Ví dụ:
TB6612FNG
L298N
MX15083. Kết nối MPU6050
MPU6050 sử dụng I²C.
Trên STM32F103:
PB6 → SCL
PB7 → SDAThông thường:
MPU6050
│
├── VCC
├── GND
├── SCL → PB6
└── SDA → PB7Địa chỉ thường gặp:
0x68nếu chân AD0 ở LOW.
Nếu AD0 HIGH:
0x694. Đừng nhầm địa chỉ I²C
Một lỗi rất thường gặp.
Datasheet có thể ghi:
0xD0hoặc:
0x68Trong HAL STM32 thường sử dụng địa chỉ dạng:
0x68 << 1tức:
#define MPU6050_ADDR \
(0x68 << 1)5. Kiểm tra MPU6050 trước
Trước khi làm EKF, hãy chắc chắn MPU6050 hoạt động.
Đọc:
WHO_AM_IThanh ghi:
0x75Giá trị thường:
0x68Code:
uint8_t whoami;
HAL_I2C_Mem_Read(
&hi2c1,
MPU6050_ADDR,
0x75,
I2C_MEMADD_SIZE_8BIT,
&whoami,
1,
100
);Nếu:
whoami == 0x68→ MPU6050 phản hồi.
6. Đánh thức MPU6050
Sau reset, MPU6050 thường ở trạng thái sleep.
Thanh ghi:
PWR_MGMT_1
0x6Bghi:
uint8_t data = 0x00;
HAL_I2C_Mem_Write(
&hi2c1,
MPU6050_ADDR,
0x6B,
I2C_MEMADD_SIZE_8BIT,
&data,
1,
100
);7. Cấu hình Gyroscope
Ví dụ:
±250 °/sFS_SEL:
0Thanh ghi:
0x1Buint8_t data = 0x00;
HAL_I2C_Mem_Write(
&hi2c1,
MPU6050_ADDR,
0x1B,
I2C_MEMADD_SIZE_8BIT,
&data,
1,
100
);Sensitivity:
[
131\ LSB/(°/s)
]
8. Đọc Gyro
Các thanh ghi:
0x43 → GYRO_XOUT_H
0x45 → GYRO_YOUT_H
0x47 → GYRO_ZOUT_HĐọc 6 byte:
uint8_t buffer[6];
HAL_I2C_Mem_Read(
&hi2c1,
MPU6050_ADDR,
0x43,
I2C_MEMADD_SIZE_8BIT,
buffer,
6,
100
);Ghép:
int16_t gyro_z_raw;
gyro_z_raw =
(int16_t)(
((uint16_t)buffer[4] << 8)
| buffer[5]
);9. Đổi sang °/s
Với ±250°/s:
[
gyro_{dps}
\frac{raw}{131}
]
Code:
float gyro_z_dps =
gyro_z_raw / 131.0f;10. Đổi sang rad/s
EKF của chúng ta nên dùng radian.
[
1°=\frac{\pi}{180}
]
Do đó:
float gyro_z_rad =
gyro_z_dps *
0.01745329252f;Hoặc:
float gyro_z_rad =
gyro_z_dps *
(M_PI / 180.0f);11. Tại sao tôi khuyên dùng rad?
Vì:
sinf()
cosf()của C sử dụng:
radianKhông phải độ.
Do đó:
θ = 90°phải đổi:
[
90°=\frac{\pi}{2}
]
12. Calibration Gyro
Đây là một bước bắt buộc.
Đặt robot:
ĐỨNG YÊNkhông rung.
Lấy:
N = 1000mẫu.
float gyro_bias = 0.0f;
for (int i = 0; i < 1000; i++)
{
float gz = MPU6050_ReadGyroZ();
gyro_bias += gz;
HAL_Delay(2);
}
gyro_bias /= 1000.0f;Sau đó:
gyro_z =
gyro_z -
gyro_bias;13. Tại sao calibration quan trọng?
Ví dụ gyro thực tế:
0.02 rad/skhi robot đứng yên.
Sau 60 giây:
[
\theta=0.02\times60
]
[
=1.2rad
]
≈ 69°.
Robot đứng yên nhưng hệ thống tưởng nó quay gần 70°.
14. Encoder
Giả sử:
Wheel diameter = 65 mm
Wheel base = 150 mm
Encoder = 390 pulse/revĐây chỉ là ví dụ.
Bạn phải thay bằng thông số thực tế của robot.
15. Chu vi bánh
[
C=\pi D
]
Với:
[
D=65mm
]
thì:
[
C≈204.2mm
]
16. Khoảng cách mỗi pulse
[
d_{pulse}
\frac{\pi D}{PPR}
]
Ví dụ:
[
d_{pulse}
\frac{204.2}{390}
]
≈
[
0.524mm
]
17. Encoder → vận tốc bánh
Nếu trong dt có:
Δcountthì:
[
distance=
\Delta count
\times d_{pulse}
]
và:
[
V_L=
\frac{distance_L}{dt}
]
[
V_R=
\frac{distance_R}{dt}
]
18. Từ hai bánh → V
[
\boxed{
V=
\frac{V_R+V_L}{2}
}
]
19. Từ hai bánh → ω
Với:
W = wheel baseta có:
[
\boxed{
\omega=
\frac{V_R-V_L}{W}
}
]
Đơn vị:
rad/s20. Ví dụ
Giả sử:
VL = 300 mm/s
VR = 360 mm/s
W = 150 mmTa có:
[
V=
\frac{300+360}{2}
330mm/s
]
và:
[
\omega=
\frac{360-300}{150}
0.4rad/s
]
21. Đây là dữ liệu đầu vào EKF
Bây giờ chúng ta có:
Encoder
↓
VL
VR
↓
Vencoder
ωencodervà:
MPU6050
↓
gyro_z
↓
ωgyro22. EKF state
Ta dùng:
typedef struct
{
float x[5];
float P[5][5];
} EKF_t;Quy ước:
x[0] = X
x[1] = Y
x[2] = theta
x[3] = V
x[4] = omega23. Khởi tạo
void EKF_Init(EKF_t *ekf)
{
memset(ekf, 0, sizeof(EKF_t));
ekf->P[0][0] = 1.0f;
ekf->P[1][1] = 1.0f;
ekf->P[2][2] = 0.1f;
ekf->P[3][3] = 10.0f;
ekf->P[4][4] = 1.0f;
}24. Prediction state
void EKF_Predict(
EKF_t *ekf,
float dt)
{
float X =
ekf->x[0];
float Y =
ekf->x[1];
float theta =
ekf->x[2];
float V =
ekf->x[3];
float omega =
ekf->x[4];
ekf->x[0] =
X +
V * cosf(theta) * dt;
ekf->x[1] =
Y +
V * sinf(theta) * dt;
ekf->x[2] =
theta +
omega * dt;
ekf->x[2] =
NormalizeAngle(
ekf->x[2]);
}25. Đây chính là Odometry
Nếu chỉ dùng:
Encoderta đã có:
OdometryNhưng ở đây:
Encoder
+
Gyro
↓
EKF
↓
θ
↓
X,Yta có một hệ thống state estimation tốt hơn.
26. Prediction Jacobian
Tạo:
float F[5][5] = {0};Sau đó:
F[0][0] = 1.0f;
F[0][2] =
-V * sinf(theta) * dt;
F[0][3] =
cosf(theta) * dt;
F[1][1] = 1.0f;
F[1][2] =
V * cosf(theta) * dt;
F[1][3] =
sinf(theta) * dt;
F[2][2] = 1.0f;
F[2][4] = dt;
F[3][3] = 1.0f;
F[4][4] = 1.0f;Đây chính là ma trận:
[
F=\frac{\partial f}{\partial x}
]
27. Q
Ta bắt đầu với:
float Q[5] =
{
0.001f,
0.001f,
0.001f,
0.1f,
0.01f
};Sau đó:
for (int i = 0; i < 5; i++)
{
ekf->P[i][i] += Q[i];
}Đây là phiên bản đơn giản hóa để học.
Trong implementation chuyên nghiệp, ta sẽ thực hiện đầy đủ:
[
P=FPF^T+Q
]
28. Vì sao tôi cố tình chưa đưa ngay ma trận 5×5 đầy đủ?
Vì nếu viết toàn bộ:
F × P × Fᵀbằng tay ngay lúc này, code sẽ rất dài và dễ làm bạn mất bản chất.
Ở Bài 63 chúng ta ưu tiên hiểu:
Sensor
↓
Measurement
↓
EKF
↓
StateSau đó mới tối ưu matrix operations.
29. Encoder Measurement
Ta có:
z_encoder =
[
Vencoder,
ωencoder
]Prediction:
h(x) =
[
V,
ω
]Innovation:
y =
[
Vencoder - V,
ωencoder - ω
]30. Gyro Measurement
Gyro:
zgyro = gyro_zPrediction:
h(x) = omegaInnovation:
[
y=
\omega_{gyro}
\omega_{EKF}
]
31. H của Encoder
V = x[3]
ω = x[4]nên:
H =
[0 0 0 1 0]
[0 0 0 0 1]32. H của Gyro
H =
[0 0 0 0 1]Đây là một ví dụ rất hay để hiểu bản chất của measurement Jacobian.
33. Fusion thực tế
Ta có:
Encoder
│
┌─────┴─────┐
↓ ↓
V ωenc
│
│
Gyro ───────────→ ωgyro
│
▼
EKF
│
┌───────┴────────┐
↓ ↓ ↓
X Y θ34. Nhưng EKF còn có thể làm tốt hơn
Một vấn đề của hệ thống:
Encoder → V
Gyro → ωlà X,Y vẫn phụ thuộc vào tích phân.
Nếu:
θsai một chút:
[
X=V\cos\theta
]
[
Y=V\sin\theta
]
sẽ bắt đầu sai.
Ví dụ:
θ = 0°thực tế.
Nhưng EKF nghĩ:
θ = 5°Robot chạy:
1 msẽ xuất hiện sai số ngang đáng kể.
Đây là lý do ước lượng θ rất quan trọng.
35. MPU6050 Accelerometer có giúp sửa θ không?
Nếu θ ở đây là yaw:
Không trực tiếp.
Accelerometer chỉ nhìn trọng lực.
Nó rất tốt cho:
roll
pitchnhưng không cung cấp yaw tuyệt đối khi robot chỉ quay quanh trục Z trên mặt phẳng.
36. Muốn sửa yaw lâu dài
Bước sau có thể thêm:
MPU6050
+
Magnetometerhoặc:
Encoder
+
IMU
+
GPShoặc:
Encoder
+
IMU
+
LiDARhoặc:
Encoder
+
IMU
+
CameraKhi đó EKF mới thực sự trở thành:
Multi-Sensor Fusion37. Timer 1 ms
Chúng ta đã học ở Bài 36.
Giả sử:
volatile uint32_t tick_ms = 0;Timer interrupt:
void HAL_TIM_PeriodElapsedCallback(
TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM2)
{
tick_ms++;
}
}38. Scheduler
Ta muốn EKF:
100 Hztức:
10 msCode:
static uint32_t last_ekf = 0;
if (tick_ms - last_ekf >= 10)
{
last_ekf += 10;
Robot_EKF_Update(0.01f);
}39. Một chu kỳ EKF
Mỗi 10 ms:
1. Read encoder
2. Calculate VL/VR
3. Calculate V/ω
4. Read gyro
5. Remove gyro bias
6. EKF Predict
7. Encoder Update
8. Gyro Update
9. Normalize θ
10. Output X,Y,θ40. Pseudocode hoàn chỉnh
void Robot_EKF_Update(float dt)
{
float vl;
float vr;
Encoder_GetSpeed(
&vl,
&vr
);
float v =
(vl + vr) * 0.5f;
float omega_enc =
(vr - vl)
/ WHEEL_BASE;
float gyro =
MPU6050_GetGyroZ();
gyro -= gyro_bias;
EKF_Predict(
&ekf,
dt
);
EKF_UpdateEncoder(
&ekf,
v,
omega_enc
);
EKF_UpdateGyro(
&ekf,
gyro
);
}41. Kết quả
Sau mỗi 10 ms:
float X =
ekf.x[0];
float Y =
ekf.x[1];
float theta =
ekf.x[2];
float V =
ekf.x[3];
float omega =
ekf.x[4];Đây chính là trạng thái robot.
42. UART Debug
Tôi rất khuyên bạn gửi:
X
Y
theta
V
omega
gyro
omega_encoder
omega_ekfVí dụ:
X=125.3
Y=2.1
TH=0.015
V=301.4
W=0.012
GYRO=0.011
ENC_W=0.014Sau đó có thể mở Serial Plotter để xem đường cong.
43. Test robot chạy thẳng
Ra lệnh:
VL = VRKỳ vọng:
V > 0
ω ≈ 0
θ ≈ constant
Y ≈ constant
X tăngNếu:
Ytăng rất nhanh trong khi robot chạy thẳng:
→ kiểm tra:
gyro bias
encoder calibration
wheel diameter
wheel base44. Test quay tại chỗ
Ra lệnh:
VL = -200
VR = +200Kỳ vọng:
V ≈ 0
ω ≠ 0
X ≈ constant
Y ≈ constant
θ thay đổiNếu:
X/Ytrôi mạnh:
→ có thể do:
encoder mismatch
gyro bias
wheel slip45. Test vòng tròn
Ví dụ:
VL = 200
VR = 300Ta có:
V > 0
ω > 0Robot đi vòng tròn.
EKF phải tạo:
X(t)
Y(t)
θ(t)liên tục.
46. Từ X,Y,θ đến Controller
Đây là phần cực kỳ quan trọng.
Giả sử trajectory yêu cầu:
Xd
Yd
θdEKF cho:
X
Y
θSai số:
[
e_x=X_d-X
]
[
e_y=Y_d-Y
]
[
e_\theta=\theta_d-\theta
]
Nhưng không nên trực tiếp đưa ex, ey vào motor.
Ta phải chuyển sai số về hệ tọa độ robot.
47. Sai số trong robot frame
[
e_x^r=
\cos\theta e_x+
\sin\theta e_y
]
[
e_y^r=
-\sin\theta e_x+
\cos\theta e_y
]
và:
[
e_\theta=
normalize(\theta_d-\theta)
]
Sau đó controller:
ex_robot
ey_robot
etheta
↓
Controller
↓
V,ω48. Đây là nơi Bài 55–58 quay trở lại
Bạn đã học:
Trajectory Trackingbây giờ:
Trajectory
↓
Desired X,Y,θ
↓
← EKF ← Encoder + IMU
↓
Error
↓
Controller
↓
V,ω
↓
Inverse Kinematics
↓
VL,VRVòng kín hoàn chỉnh.
49. Đây chính là kiến trúc robot mà bạn đang hướng tới
TRAJECTORY
│
▼
Xd,Yd,θd,Vd,ωd
│
▼
TRAJECTORY CONTROL
│
▼
V,ω
│
▼
INVERSE KINEMATICS
↙ ↘
VLd VRd
│ │
▼ ▼
PID LEFT PID RIGHT
│ │
▼ ▼
PWM L PWM R
│ │
▼ ▼
MOTOR L MOTOR R
│ │
▼ ▼
ENCODER L ENCODER R
\ /
\ /
▼ ▼
ENCODER
│
│
┌──────┴──────┐
│ │
▼ ▼
V ω
│ │
└──────┬───────┘
│
▼
EKF
▲
│
MPU6050
│
▼
GYRO Z
│
▼
X,Y,θ,V,ω
│
└───────────────┐
│
▼
TRAJECTORY CONTROL50. Một lưu ý quan trọng về implementation
Ở Bài 63 này, chưa nên cố nhét tất cả vào một file main.c.
Hãy bắt đầu tổ chức firmware:
Core/
└── Src/
├── main.c
├── encoder.c
├── motor.c
├── pid.c
├── mpu6050.c
├── ekf.c
├── odometry.c
└── robot.c
Core/
└── Inc/
├── encoder.h
├── motor.h
├── pid.h
├── mpu6050.h
├── ekf.h
├── odometry.h
└── robot.hĐây là lúc bạn bắt đầu chuyển từ code học tập sang firmware có kiến trúc.
51. Bài tập thực hành quan trọng nhất
Đừng vội làm toàn bộ robot.
Hãy chia thành 5 bước.
Bước 1
Kiểm tra MPU6050:
WHO_AM_I = 0x68Bước 2
In:
gyro_zkhi robot đứng yên.
Bước 3
Calibration:
gyro_biasBước 4
Đọc encoder:
VL
VR
V
ωBước 5
Cuối cùng mới:
Encoder + Gyro
↓
EKF
↓
X,Y,θĐừng làm ngược thứ tự.
Nếu EKF cho kết quả sai, bạn sẽ không biết lỗi nằm ở:
I2C?
MPU6050?
Gyro?
Encoder?
Units?
dt?
Jacobian?
Q?
R?
EKF?52. Bài test vàng
Sau khi hoàn thành, hãy đặt robot ở:
X=0
Y=0
θ=0Cho robot chạy:
1 mthẳng.
Kỳ vọng gần:
X ≈ 1000 mm
Y ≈ 0
θ ≈ 0Sau đó quay:
90°và chạy:
1 mKỳ vọng:
X ≈ 1000 mm
Y ≈ 1000 mm
θ ≈ 90°Sai số thực tế chắc chắn tồn tại. Chính việc đo sai số rồi hiệu chỉnh D, W, encoder scale, gyro bias, Q và R mới là phần kỹ thuật quan trọng.
53. Một điều cần sửa so với cách hiểu đơn giản
Có một điểm tôi muốn nhấn mạnh để bạn không xây sai hệ thống:
MPU6050 + Encoder không đủ để EKF biết vị trí tuyệt đối X,Y trong thế giới.
Nó chỉ giúp ước lượng tốt hơn:
relative posetừ vị trí ban đầu.
Nếu robot chạy càng lâu:
t → lớnthì drift vẫn có thể tăng.
Muốn robot tự định vị lâu dài, bước tiếp theo sẽ phải đưa thêm:
LiDAR / GPS / camera / landmark / UWBvào bộ lọc.
54. Tóm tắt Bài 63
Bạn cần nắm được chuỗi này:
[
\boxed{
Encoder
\rightarrow
V,\omega
}
]
[
\boxed{
MPU6050
\rightarrow
\omega_{gyro}
}
]
sau đó:
[
\boxed{
Encoder+Gyro
\rightarrow EKF
}
]
và:
[
\boxed{
EKF
\rightarrow
X,Y,\theta,V,\omega
}
]
cuối cùng:
[
\boxed{
X,Y,\theta
\rightarrow
Trajectory\ Controller
}
]
Đây là bước chuyển rất lớn:
Bài 45–52
↓
Điều khiển motor
Bài 53–60
↓
Điều khiển chuyển động
Bài 61–63
↓
Ước lượng trạng thái robotBÀI 64
Bài tiếp theo tôi đề xuất là:
BÀI 64 – EKF NÂNG CAO: FUSION ENCODER + MPU6050 + GPS/LiDAR, GIẢM DRIFT X,Y,θ VÀ ĐỊNH VỊ ROBOT TRONG THẾ GIỚI
Ở đó chúng ta sẽ giải quyết đúng vấn đề còn lại của Bài 63:
Encoder + IMU
↓
EKF
↓
X,Y,θ
↓
DRIFTvà thêm một nguồn tham chiếu bên ngoài:
Encoder
│
▼
┌─────┐
MPU6050 ───────→ │ EKF │ ←──── GPS / LiDAR
└──┬──┘
↓
X,Y,θ ổn định hơnĐó mới là nền tảng để tiến tới SLAM, Navigation và Autonomous Robot ở các bài sau.