BÀI 62 – EKF CHO ROBOT 2 BÁNH
FUSION ENCODER + IMU + ƯỚC LƯỢNG X, Y, θ, V, ω TRÊN STM32F103
Đây là một bước khó hơn rõ rệt so với Bài 61.
Ở Bài 61, chúng ta mới fusion chủ yếu quanh:
Encoder + Gyro
↓
θ / ωỞ Bài 62, chúng ta bắt đầu xây dựng bộ ước lượng trạng thái của cả robot:
[
\boxed{
\mathbf{x}
[X,;Y,;\theta,;V,;\omega]^T
}
]
Tức là robot liên tục tự trả lời:
Tôi đang ở đâu? Đang hướng nào? Đang chạy nhanh bao nhiêu? Và đang quay nhanh bao nhiêu?
1. EKF là gì?
EKF =
Extended Kalman Filter
hay:
Bộ lọc Kalman mở rộng.
Kalman Filter thông thường thích hợp với hệ thống tuyến tính.
Nhưng robot 2 bánh có phương trình:
[
\dot X=V\cos\theta
]
[
\dot Y=V\sin\theta
]
Đây là phương trình phi tuyến vì có:
sin(θ)
cos(θ)Do đó chúng ta dùng:
EKF
2. Tại sao robot cần EKF?
Giả sử encoder nói:
VL = 500 mm/s
VR = 550 mm/sRobot đang quay.
Ta tính:
[
V=\frac{V_R+V_L}{2}
]
và:
[
\omega=\frac{V_R-V_L}{W}
]
Nhưng encoder có thể bị:
trượt bánh
sai số đường kính bánh
sai số encoderTrong khi IMU:
Gyrolại có:
bias
noise
driftMỗi cảm biến đều không hoàn hảo.
EKF sẽ kết hợp chúng.
3. State vector
Đây là phần quan trọng nhất của bài.
Ta định nghĩa:
[
\boxed{
\mathbf{x}
\begin{bmatrix}
X\
Y\
\theta\
V\
\omega
\end{bmatrix}
}
]
Trong đó:
| State | Ý nghĩa |
|---|---|
X |
tọa độ X |
Y |
tọa độ Y |
θ |
hướng robot |
V |
vận tốc tiến |
ω |
vận tốc góc |
Ví dụ:
X = 1250 mm
Y = 430 mm
θ = 1.57 rad
V = 300 mm/s
ω = 0.2 rad/s4. Hệ tọa độ robot
Ta quy ước:
Y
↑
│
│
│ Robot
│ ↑
│ │
│ │
─────────────┼───────┼────────→ X
│Robot hướng theo:
[
\theta
]
5. Khi robot chạy thẳng
Nếu:
[
\theta=0
]
thì:
[
\dot X=V
]
[
\dot Y=0
]
Robot chạy theo:
→ X6. Khi θ = 90°
[
\theta=\frac{\pi}{2}
]
thì:
[
\cos\theta=0
]
[
\sin\theta=1
]
nên:
[
\dot X=0
]
[
\dot Y=V
]
Robot chạy:
↑ Y7. Đây chính là lý do EKF phi tuyến
Phương trình robot:
[
X_{k+1}
X_k+
V_k\cos\theta_kdt
]
[
Y_{k+1}
Y_k+
V_k\sin\theta_kdt
]
[
\theta_{k+1}
\theta_k+
\omega_kdt
]
Nếu ta dùng cos() và sin():
x += v * cosf(theta) * dt;
y += v * sinf(theta) * dt;
theta += omega * dt;thì mô hình đã phi tuyến.
8. Mô hình động học
Ta có state:
[
x=
[X,Y,\theta,V,\omega]^T
]
Mô hình liên tục:
[
\dot X=V\cos\theta
]
[
\dot Y=V\sin\theta
]
[
\dot\theta=\omega
]
Một mô hình đơn giản cho V và ω:
[
\dot V=0
]
[
\dot\omega=0
]
Điều này có nghĩa:
Trong một chu kỳ EKF ngắn, nếu chưa có measurement mới thì ta giả sử vận tốc gần như giữ nguyên.
9. Mô hình rời rạc
Với:
[
dt=0.01s
]
ta có:
[
X_{k+1}
X_k+
V_k\cos\theta_kdt
]
[
Y_{k+1}
Y_k+
V_k\sin\theta_kdt
]
[
\theta_{k+1}
\theta_k+
\omega_kdt
]
[
V_{k+1}=V_k
]
[
\omega_{k+1}=\omega_k
]
10. Hàm Prediction
Ta có thể viết trước phần đơn giản:
void EKF_PredictState(
EKF_t *ekf,
float dt)
{
float theta = ekf->x[2];
float v = ekf->x[3];
float omega = ekf->x[4];
ekf->x[0] +=
v * cosf(theta) * dt;
ekf->x[1] +=
v * sinf(theta) * dt;
ekf->x[2] +=
omega * dt;
}Trong đó:
x[0] = X
x[1] = Y
x[2] = θ
x[3] = V
x[4] = ω11. Nhưng EKF khác Kalman thường ở đâu?
Kalman thường cần:
[
F
]
là ma trận chuyển trạng thái.
EKF cần:
[
\boxed{F=\frac{\partial f}{\partial x}}
]
hay:
Jacobian của mô hình động học.
Đây chính là phần quan trọng nhất của EKF.
12. Jacobian là gì?
Không cần sợ từ này.
Ta có:
[
f(x)=
\begin{bmatrix}
X+V\cos\theta dt\
Y+V\sin\theta dt\
\theta+\omega dt\
V\
\omega
\end{bmatrix}
]
Ta lấy đạo hàm từng phương trình theo từng state.
13. Phương trình X
[
X’=X+V\cos\theta dt
]
Đạo hàm theo:
Xlà:
[
\frac{\partial X’}{\partial X}=1
]
Theo:
θlà:
[
\frac{\partial X’}{\partial\theta}
-V\sin\theta dt
]
Theo:
Vlà:
[
\frac{\partial X’}{\partial V}
\cos\theta dt
]
14. Phương trình Y
[
Y’=Y+V\sin\theta dt
]
Ta có:
[
\frac{\partial Y’}{\partial Y}=1
]
[
\frac{\partial Y’}{\partial\theta}
V\cos\theta dt
]
[
\frac{\partial Y’}{\partial V}
\sin\theta dt
]
15. Phương trình θ
[
\theta’=\theta+\omega dt
]
Do đó:
[
\frac{\partial\theta’}{\partial\theta}=1
]
[
\frac{\partial\theta’}{\partial\omega}=dt
]
16. Ma trận Jacobian F
Kết quả:
[
\boxed{
F=
\begin{bmatrix}
1&0&-V\sin\theta dt&\cos\theta dt&0\
0&1& V\cos\theta dt&\sin\theta dt&0\
0&0&1&0&dt\
0&0&0&1&0\
0&0&0&0&1
\end{bmatrix}
}
]
Đây là trái tim toán học của EKF bài này.
17. Tại sao phải có F?
Kalman cần cập nhật uncertainty:
[
P
]
Sau prediction:
[
\boxed{
P=FPF^T+Q
}
]
Trong đó:
P = covariance
F = Jacobian
Q = process noise18. Ý nghĩa của P
P nói cho EKF:
“Tôi không chắc về state hiện tại đến mức nào?”
Ví dụ:
P nhỏ→ rất tự tin.
P lớn→ không chắc chắn.
19. Ý nghĩa của Q
Q mô tả:
Sai số của mô hình robot.
Ví dụ robot đang chạy:
V = 500 mm/snhưng thực tế có:
motor torque
ma sát
bánh trượtMô hình:
X += V*cos(theta)*dtkhông hoàn hảo.
Q cho phép EKF thừa nhận điều đó.
20. Prediction đầy đủ
Hai bước:
State
[
x=f(x)
]
Covariance
[
P=FPF^T+Q
]
Đây là:
PREDICT
│
┌────┴────┐
↓ ↓
state P21. Measurement từ Encoder
Encoder cho:
VL
VRTính:
[
V_{enc}
\frac{V_R+V_L}{2}
]
và:
[
\omega_{enc}
\frac{V_R-V_L}{W}
]
Do đó measurement:
[
z=
\begin{bmatrix}
V_{enc}\
\omega_{enc}
\end{bmatrix}
]
22. Measurement từ IMU
Gyro Z:
[
\omega_{gyro}
]
Nếu đã calibration bias:
[
\omega_{gyro,corr}
\omega_{gyro}-b_g
]
Ta có thể sử dụng:
Encoder:
V
ω
Gyro:
ω23. Measurement model
State:
[
x=
[X,Y,\theta,V,\omega]
]
Encoder measurement:
[
z_1=V
]
[
z_2=\omega
]
Gyro:
[
z_3=\omega
]
Do đó:
[
z=
\begin{bmatrix}
V\
\omega\
\omega
\end{bmatrix}
]
24. Measurement Jacobian H
Ta có:
[
H=
\begin{bmatrix}
0&0&0&1&0\
0&0&0&0&1\
0&0&0&0&1
\end{bmatrix}
]
Đây là một ma trận rất đơn giản.
25. Một điều thú vị
Encoder và Gyro cùng đo:
[
\omega
]
Nhưng chúng có lỗi khác nhau.
Ví dụ:
Encoder:
0.51 rad/s
Gyro:
0.55 rad/sEKF sẽ không đơn giản lấy trung bình.
Nó sử dụng covariance để quyết định trọng số.
26. Công thức Update
Innovation:
[
y=z-h(x)
]
Innovation covariance:
[
S=HPH^T+R
]
Kalman Gain:
[
K=PH^TS^{-1}
]
State:
[
\boxed{
x=x+Ky
}
]
Covariance:
[
P=(I-KH)P
]
Đây chính là phần Correction.
27. Toàn bộ EKF
STATE
│
▼
PREDICTION
│
┌────┴────┐
│ │
▼ ▼
x_predict P_predict
│ │
└────┬────┘
│
▼
SENSOR DATA
│
▼
INNOVATION
│
▼
KALMAN GAIN
│
▼
CORRECTION
│
▼
x_estimated28. Cấu trúc EKF trên STM32
Ta tạo:
#define EKF_STATE_SIZE 5và:
typedef struct
{
float x[5];
float P[5][5];
float Q[5][5];
} EKF_t;Trong đó:
x[0] = X
x[1] = Y
x[2] = theta
x[3] = V
x[4] = omega29. Khởi tạo EKF
Ví dụ:
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;
ekf->Q[0][0] = 0.001f;
ekf->Q[1][1] = 0.001f;
ekf->Q[2][2] = 0.001f;
ekf->Q[3][3] = 0.1f;
ekf->Q[4][4] = 0.01f;
}Các số trên không phải thông số chuẩn. Chúng chỉ giúp bạn hiểu cấu trúc; khi làm robot thật phải tuning.
30. Tại sao Q[V] có thể lớn hơn Q[X]?
Vì:
Vthay đổi nhanh hơn vị trí.
Ví dụ:
Xkhông thể nhảy hàng trăm mm trong 1 ms nếu robot bình thường.
Nhưng:
Vcó thể thay đổi nhanh do:
PWM
motor torqueDo đó covariance của V thường cần được mô hình hóa khác.
31. Encoder Update
float v_enc =
(v_left + v_right) * 0.5f;
float omega_enc =
(v_right - v_left)
/ wheel_base;Sau đó:
z_encoder =
[Venc, omega_enc]32. Gyro Update
Ví dụ IMU cho:
float gyro_z;sau calibration:
gyro_z -= gyro_bias;Measurement:
omega_gyro33. Có nên đưa cả encoder và gyro vào cùng một update?
Có thể.
Nhưng về mặt triển khai, với STM32F103 và người mới học EKF, tôi khuyên:
Update 1
Encoder
→ V
→ ωUpdate 2
Gyro
→ ωHoặc gom thành một measurement vector nếu bạn muốn triển khai EKF đầy đủ.
34. Một vấn đề: hai measurement cùng đo ω
Nếu:
Encoder ω
Gyro ωcùng đưa vào EKF thì điều này không sai.
Nhưng chúng phải được mô hình hóa đúng covariance:
R_encoder
R_gyroNếu đặt:
R_encoder = rất nhỏ
R_gyro = rất nhỏEKF sẽ quá tự tin vào cả hai.
35. Rất quan trọng: covariance không phải “tuning tùy tiện”
Nếu cảm biến có noise:
σthì:
[
R=\sigma^2
]
Ví dụ đo velocity có độ lệch chuẩn:
σ = 10 mm/sthì:
[
R=100
]
Đây là cách suy nghĩ đúng hơn so với:
R = 0.01một cách ngẫu nhiên.
36. Cách đo R cho encoder
Đặt robot chạy tốc độ ổn định:
V ≈ 300 mm/sthu thập:
1000 samplesTính:
[
\mu=
\frac1N\sum x_i
]
và:
[
\sigma^2=
\frac1{N-1}
\sum(x_i-\mu)^2
]
Đó là một ước lượng tốt cho measurement noise.
37. Cách đo R cho gyro
Để robot đứng yên:
gyro_zthu thập 1000 mẫu.
Sau khi loại bias:
gyro_correctedtính variance.
Ta có:
[
R_{gyro}
Var(gyro)
]
Đây là cách thực tế để chọn R.
38. Process noise Q
Q khó hơn R.
Bạn có thể bắt đầu từ:
Q nhỏsau đó quan sát.
Nếu EKF:
quá tin model→ tăng Q.
Nếu EKF:
quá bám measurement→ giảm Q hoặc xem lại R.
39. Một ví dụ trực quan
Giả sử:
Prediction:
V = 500Encoder:
V = 490Gyro không liên quan trực tiếp đến V.
Nếu:
R_encoder nhỏEKF sẽ kéo:
500 → gần 490Nếu:
R_encoder lớnEKF có thể giữ:
500nhiều hơn.
40. Với θ
Có một điểm đặc biệt:
[
\theta
]
là góc tuần hoàn.
Ví dụ:
+179°và:
-179°thực tế chỉ cách nhau:
2°không phải:
358°41. Vì vậy phải normalize angle
Hàm:
float NormalizeAngle(float angle)
{
while (angle > M_PI)
angle -= 2.0f * M_PI;
while (angle < -M_PI)
angle += 2.0f * M_PI;
return angle;
}Khi tính innovation:
innovation =
NormalizeAngle(
measurement -
predicted_angle);Điều này rất quan trọng.
42. Nếu không normalize
Ví dụ:
prediction = +179°
measurement = -179°Ta tính:
[
-179-179=-358°
]
EKF sẽ tưởng robot sai:
358°trong khi thực tế chỉ sai:
2°Đây là bug rất nguy hiểm.
43. X và Y không cần normalize
Chỉ:
θlà góc tuần hoàn.
X
Y
V
ωkhông cần wrap như vậy.
44. EKF chạy bao nhiêu Hz?
Với STM32F103:
IMU 100 Hz
Encoder 100 Hz
EKF 100 Hz
Speed PID 100 Hz
Odometry 100 Hzlà một cấu hình khởi đầu hợp lý.
Không nhất thiết phải chạy EKF ở 1 kHz.
45. Kiến trúc hoàn chỉnh
┌─────────────┐
│ TRAJECTORY│
└──────┬──────┘
↓
Controller
↓
V,ω
↓
Inverse Kinematics
↙ ↘
VL VR
↓ ↓
Speed PID Speed PID
↓ ↓
PWM PWM
↓ ↓
Motor L Motor R
↓ ↓
Encoder L Encoder R
│ │
└──────┬──────┘
↓
Vencoder,ωenc
│
│
┌────┴────┐
│ │
│ EKF │←──── IMU Gyro
│ │
└────┬────┘
↓
X,Y,θ,V,ω
↓
ControllerĐây là một vòng kín hoàn chỉnh.
46. Nhưng EKF không tự sửa được mọi thứ
Điều này rất quan trọng.
Nếu robot không có nguồn tham chiếu vị trí tuyệt đối:
Encoder
+
IMUthì:
X,Yvẫn có thể drift theo thời gian.
EKF:
không tạo ra thông tin mới.
Nó chỉ kết hợp thông tin hiện có một cách có cơ sở xác suất.
47. Muốn giảm drift X,Y
Sau này chúng ta cần thêm:
LiDAR
GPS
Camera
AprilTag
Optical Flow
UWBVí dụ:
Encoder + IMU
↓
EKF
↑
LiDARhoặc:
Encoder + IMU
↓
EKF
↑
CameraKhi đó robot có thể biết:
"Tôi đang ở vị trí nào trong thế giới?"chứ không chỉ:
"Tôi đã đi được bao xa từ lúc khởi động?"48. Bài thực hành 1 – EKF không có robot
Trước tiên hãy mô phỏng bằng dữ liệu giả.
Cho robot:
V = 300 mm/s
ω = 0.2 rad/s
dt = 0.01 sTính trajectory thật:
Xtrue
Ytrue
θtrueSau đó tạo measurement giả:
Encoder = true + noise
Gyro = true + bias + noiseRồi đưa vào EKF.
Bạn sẽ thấy:
True
│
├── noisy encoder
│
├── noisy gyro
│
└── EKFEKF thường sẽ cho đường ước lượng mượt hơn measurement thô.
49. Bài thực hành 2 – Robot đứng yên
Mục tiêu:
V = 0
ω = 0Nhưng sensor:
Encoder ≈ 0 + noise
Gyro ≈ bias + noiseQuan sát:
EKF V
EKF ωChúng phải tiến gần:
050. Bài thực hành 3 – Robot chạy thẳng
Cho:
VL ≈ VRTa mong:
[
\omega\approx0
]
và:
Y ≈ constant
θ ≈ constantTrong khi:
X tăng51. Bài thực hành 4 – Robot quay tại chỗ
Cho:
VL = -200
VR = +200Ta có:
[
V\approx0
]
nhưng:
[
\omega\neq0
]
Kết quả:
X ≈ constant
Y ≈ constant
θ thay đổiĐây là test rất tốt để kiểm tra mô hình EKF.
52. Bài thực hành 5 – Robot chạy vòng tròn
Cho:
VL < VRrobot sẽ chạy theo cung tròn.
EKF phải tạo:
X
Y
θthay đổi liên tục.
Đây chính là tiền đề cho:
Trajectory Trackingở các bài trước.
53. Một lỗi rất thường gặp
Không đồng bộ timestamp.
Ví dụ:
Encoder:
t = 100 ms
Gyro:
t = 105 msnhưng code lại giả sử:
cả hai cùng t = 100 ms→ EKF có thể sai.
Do đó nên có:
uint32_t timestamp;cho từng measurement.
54. Một lỗi khác
Dùng:
delay_ms()để quyết định dt.
Không nên.
Hãy lấy dt từ timer:
uint32_t now =
millis();
float dt =
(now - last) *
0.001f;hoặc tốt hơn dùng timer tick có độ phân giải phù hợp.
55. Một lỗi cực kỳ nguy hiểm nữa
Sai đơn vị.
Ví dụ:
Encoder:
mm/snhưng:
wheel_base:
mthì:
[
\omega=
\frac{V_R-V_L}{W}
]
sẽ sai 1000 lần.
Hãy thống nhất:
distance = mm
time = s
velocity = mm/s
angle = rad
omega = rad/sTôi khuyên dùng hệ đơn vị này cho robot của bạn.
56. Bảng đơn vị nên giữ cố định
| Đại lượng | Đơn vị |
|---|---|
| X | mm |
| Y | mm |
| Wheel diameter | mm |
| Wheel base | mm |
| VL | mm/s |
| VR | mm/s |
| V | mm/s |
| θ | rad |
| ω | rad/s |
| dt | s |
Đây là cách tránh rất nhiều lỗi.
57. EKF không phải mục tiêu cuối cùng
Sau Bài 62, chúng ta đã có:
Sensor
↓
State Estimation
↓
X,Y,θ,V,ωBước tiếp theo là:
State
↓
Controller
↓
MotorĐây chính là cầu nối sang:
Advanced Robotics Control
58. Kiến trúc robot lúc này
┌─────────────────┐
│ TRAJECTORY │
│ Xd,Yd,θd,Vd,ωd │
└────────┬────────┘
↓
┌───────────────┐
│ CONTROLLER │
└───────┬───────┘
↓
V,ω
↓
INVERSE KINEMATICS
↙ ↘
VL VR
↓ ↓
SPEED PID SPEED PID
↓ ↓
PWM PWM
↓ ↓
MOTOR MOTOR
↓ ↓
ENCODER L ENCODER R
\ /
\ /
↓ ↓
ENCODER
↓
┌─────┐
│ EKF │ ←── IMU
└──┬──┘
↓
X,Y,θ,V,ω
│
└──────→ CONTROLLERBạn có thể thấy:
Robot bây giờ đã có đủ 3 tầng lớn:
Tầng 1 – Actuator control
PWM
↓
MotorTầng 2 – Motion control
V,ω
↓
Trajectory TrackingTầng 3 – State estimation
Encoder + IMU
↓
EKF
↓
X,Y,θ,V,ωĐây là kiến trúc nền tảng của rất nhiều robot di động hiện đại.
59. Tóm tắt Bài 62
Điều quan trọng nhất cần nhớ:
1.
Robot có state:
[
\boxed{
x=[X,Y,\theta,V,\omega]^T
}
]
2.
Mô hình động học:
[
\boxed{
X’=X+V\cos\theta dt
}
]
[
\boxed{
Y’=Y+V\sin\theta dt
}
]
[
\boxed{
\theta’=\theta+\omega dt
}
]
3.
EKF dùng Jacobian:
[
\boxed{F=\frac{\partial f}{\partial x}}
]
4.
Prediction:
[
\boxed{
P=FPF^T+Q
}
]
5.
Measurement:
Encoder → V,ω
Gyro → ω6.
Correction:
[
\boxed{x=x+Ky}
]
7.
Q:
độ không chắc chắn của model8.
R:
độ nhiễu của sensor9.
θ phải được normalize.
10.
Encoder + IMU không tự tạo ra vị trí tuyệt đối.
60. Bài 63 sẽ là một bước rất thú vị
Sau khi đã có:
B61 → Kalman
B62 → EKFbài tiếp theo hợp lý là:
BÀI 63 – SENSOR FUSION THỰC CHIẾN: ENCODER + IMU MPU6050 + EKF + ODOMETRY X,Y,θ TRÊN STM32F103
Ở Bài 63, chúng ta sẽ không còn chỉ nói lý thuyết. Ta sẽ ghép thành một firmware architecture thực tế:
STM32F103
│
├── TIM Encoder L
├── TIM Encoder R
├── I2C
│ └── MPU6050
│ ├── Gyro Z
│ └── Accel X/Y/Z
│
├── Timer 1 ms
│
├── EKF
│
├── Odometry
│
├── Speed PID
│
└── UART Debugvà đặc biệt sẽ xây luồng dữ liệu Encoder → IMU → EKF → X,Y,θ → Controller để bạn có thể chuyển từ những đoạn code rời rạc của các bài 45–62 thành một kiến trúc firmware robot STM32F103 hoàn chỉnh.