BÀI 58 – TRAJECTORY TRACKING NÂNG CAO
Controller X, Y, θ + Feedforward + Feedback trên STM32F103
Ở Bài 57, chúng ta đã tạo được quỹ đạo mong muốn:
[x_d(t),\quad y_d(t),\quad \theta_d(t)
]
và:
[V_d(t),\quad \omega_d(t)
]
Nhưng robot thật không bao giờ chạy đúng tuyệt đối.
Ví dụ robot muốn:
Xd = 500 mm
Yd = 300 mm
θd = 45°nhưng thực tế:
X = 480 mm
Y = 285 mm
θ = 40°Vậy controller phải làm gì?
Feedforward đưa robot đi theo chuyển động mà trajectory đã tính trước, còn Feedback nhìn sai số thực tế rồi sửa lại.
Đây chính là nội dung quan trọng nhất của Bài 58.
1. Tư duy từ Bài 57 → Bài 58
Bài 57:
t
↓
Trajectory Generator
↓
Xd Yd θd
Vd ωdBài 58:
Trajectory
│
┌───────┴───────┐
↓ ↓
Xd,Yd,θd Vd,ωd
│ │
↓ ↓
ERROR CALCULATION FEEDFORWARD
│ │
↓ │
FEEDBACK │
│ │
└───────┬───────┘
↓
V,ω
↓
Inverse Kinematics
↙ ↘
VL VR2. Feedforward là gì?
Giả sử trajectory nói:
Vd = 200 mm/s
ωd = 0.5 rad/sController có thể nói ngay:
“Tôi biết robot cần chạy 200 mm/s và quay 0.5 rad/s.”
Đó là:
Feedforward
Tức là ra lệnh dựa trên mô hình/quỹ đạo mong muốn, chưa cần chờ robot sai rồi mới sửa.
3. Feedback là gì?
Giả sử robot thực tế bị chậm:
Robot muốn:
Vd = 200
Robot thực tế:
V = 170hoặc robot lệch khỏi đường:
ey = 30 mmFeedback phát hiện sai số và sửa:
V correction
ω correction4. Công thức tổng quát
Tư tưởng chính:
[\boxed{
u=u_{ff}+u_{fb}
}
]
Trong robot 2 bánh:
[\boxed{
V=V_{ff}+V_{fb}
}
] [
\boxed{
\omega=\omega_{ff}+\omega_{fb}
}
]
Thông thường:
[V_{ff}=V_d
] [
\omega_{ff}=\omega_d
]
5. Feedback sử dụng sai số X,Y,θ
Ta có:
[e_x=x_d-x
] [
e_y=y_d-y
] [
e_\theta=\theta_d-\theta
]
Nhưng đây vẫn là sai số trong hệ tọa độ toàn cục.
Để điều khiển robot 2 bánh tốt hơn, chúng ta chuyển sang hệ tọa độ robot.
6. Hệ tọa độ robot
Giả sử:
Y
↑
│
desired ●
/
/
/
● robot
───────→ XRobot có hướng:
[\theta
]
Ta cần biết điểm mong muốn nằm:
phía trước robot bao nhiêu?
sang trái/phải bao nhiêu?7. Chuyển sai số sang robot frame
Ta có:
[e_x^r=
\cos\theta e_x+
\sin\theta e_y
] [
e_y^r=
-\sin\theta e_x+
\cos\theta e_y
]
Trong đó:
ex_r > 0→ mục tiêu nằm phía trước.
ey_r > 0→ mục tiêu nằm một phía theo quy ước dấu của hệ tọa độ.
8. Sai số góc
[e_\theta=
normalize(\theta_d-\theta)
]
Ví dụ:
θd = 179°
θ = -179°thì:
eθ = +2°chứ không phải:
358°Hàm:
static float NormalizeAngleRad(float angle)
{
while (angle > M_PI)
angle -= 2.0f * M_PI;
while (angle < -M_PI)
angle += 2.0f * M_PI;
return angle;
}9. Controller Feedback đơn giản
Ta có thể bắt đầu bằng:
[V_{fb}=K_xe_x^r
]
[
\omega_{fb}
K_y e_y^r+
K_\theta e_\theta
]
Sau đó:
[V=V_d+K_xe_x^r
] [
\omega=
\omega_d+
K_y e_y^r+
K_\theta e_\theta
]
Đây là controller cực kỳ quan trọng đối với robot vi sai.
10. Vì sao ey điều khiển ω?
Hãy tưởng tượng:
Robot
──────→
Target
●Nếu target lệch sang bên:
Robot
──────→
● Targetrobot cần:
ω > 0để quay về phía target.
Do đó:
ey
↓
ω correction11. Vì sao eθ cũng điều khiển ω?
Giả sử:
Robot:
──────→
Desired:
↗Robot đang ở đúng vị trí nhưng hướng sai.
Khi đó:
ex ≈ 0
ey ≈ 0
eθ ≠ 0Nếu không có:
Kθ eθrobot có thể không xoay đúng hướng.
12. Code Controller cơ bản
typedef struct
{
float x;
float y;
float theta;
} Pose_t;
typedef struct
{
float x;
float y;
float theta;
} DesiredPose_t;Controller:
void TrajectoryController_Update(
const Pose_t *actual,
const DesiredPose_t *desired,
float Vd,
float omega_d,
float *V,
float *omega)
{
float dx;
float dy;
float ex;
float ey;
float etheta;
dx =
desired->x -
actual->x;
dy =
desired->y -
actual->y;
/*
* Global → Robot frame
*/
ex =
cosf(actual->theta) * dx +
sinf(actual->theta) * dy;
ey =
-sinf(actual->theta) * dx +
cosf(actual->theta) * dy;
etheta =
NormalizeAngleRad(
desired->theta -
actual->theta);
/*
* Feedforward + Feedback
*/
*V =
Vd +
KX * ex;
*omega =
omega_d +
KY * ey +
KTHETA * etheta;
}Đây chính là trái tim của Bài 58.
13. Ví dụ cụ thể
Giả sử:
Vd = 200 mm/s
ωd = 0.2 rad/sRobot bị:
ex = 30 mm
ey = 20 mm
eθ = 0.1 radvới:
KX = 0.5
KY = 1.0
KTHETA = 2.0thì:
[V_{fb}=0.5(30)=15
]
nên:
[V=200+15=215
]
và:
[
\omega_{fb}
1(20)+2(0.1)
]
[
=20.2
]
Nếu đơn vị ey là mm thì KY=1 là quá lớn cho robot thực tế; đây chỉ là ví dụ toán học.
Thực tế phải chọn gain phù hợp với đơn vị và tốc độ robot.
14. Đây là lỗi rất thường gặp
Không nên lấy:
KX = 1
KY = 1
Kθ = 1rồi chạy robot ngay.
Vì:
mm
mm
radlà ba đại lượng khác nhau.
Ví dụ:
ex = 100 mm
eθ = 0.1 radthì hai giá trị này không cùng thang đo.
Do đó phải tuning gain cẩn thận.
15. Feedforward có lợi gì?
Giả sử robot đang chạy vòng tròn:
R = 300 mm
Vd = 150 mm/sthì:
[\omega_d=\frac{V_d}{R}
] [
\omega_d=0.5rad/s
]
Feedforward lập tức đưa:
Vff = 150
ωff = 0.5Robot biết trước mình cần quay.
Feedback chỉ cần sửa:
lệch vị trí
lệch hướng16. Nếu chỉ dùng Feedback thì sao?
Ví dụ:
Vd = 0
ωd = 0và chỉ dựa vào:
ex
ey
eθrobot vẫn có thể tiến tới trajectory.
Nhưng nó phải:
thấy sai
→ sửa
→ thấy sai
→ sửa
→ ...Feedforward giúp giảm gánh nặng này.
17. So sánh
Chỉ Feedback
Desired
↓
Error
↓
Controller
↓
MotorFeedforward + Feedback
┌→ Feedforward ─┐
Desired ─────────┤ ├→ Controller → Motor
└→ Feedback ────┘
↑
RobotHệ thống thứ hai thường phản ứng nhanh và tự nhiên hơn khi mô hình tương đối chính xác.
18. Feedforward không có nghĩa là bỏ Feedback
Đây là điều phải nhớ:
Feedforward không thay thế feedback.
Feedforward có thể sai vì:
motor không giống nhau
ma sát
pin yếu
bánh trượt
sàn không phẳng
sai số encoder
tải thay đổiFeedback dùng để sửa những sai lệch đó.
19. Feedforward từ Vd, ωd → tốc độ bánh
Ta có:
[
V_{L,d}
V_d-\frac{\omega_d W}{2}
]
[
V_{R,d}
V_d+\frac{\omega_d W}{2}
]
Đây chính là feedforward cho từng bánh.
Ví dụ:
Vd = 200 mm/s
ωd = 0.4 rad/s
W = 150 mmthì:
[
V_{L,d}
200-\frac{0.4(150)}2
=170
]
[
V_{R,d}
200+\frac{0.4(150)}2
=230
]
Robot đã biết ngay:
Left = 170
Right = 23020. Đây là nơi Motor PID xuất hiện
Bây giờ chúng ta có:
VL_desired
VR_desiredEncoder đo:
VL_actual
VR_actualSai số:
[e_L=V_{L,d}-V_{L}
] [
e_R=V_{R,d}-V_{R}
]
Sau đó:
PID Left
PID Right21. Toàn bộ vòng điều khiển
Trajectory
│
├── Xd Yd θd
│
└── Vd ωd
│
▼
┌────────────────────┐
│ X,Y,θ Controller │
│ Feedforward │
│ + Feedback │
└─────────┬──────────┘
│
V,ω
│
▼
Inverse Kinematics
↙ ↘
VLd VRd
↓ ↓
PID Left PID Right
↓ ↓
PWM Left PWM Right
↓ ↓
Motor Motor
↓ ↓
Encoder Encoder
↘ ↙
Odometry
↓
X,Y,θ
│
└──── feedbackĐây chính là kiến trúc cascade control rất quan trọng.
22. Cascade Control
Robot của chúng ta thực tế có nhiều vòng điều khiển:
Vòng ngoài:
Position / Trajectory
↓
V,ω
↓
Vòng giữa:
Wheel Speed
↓
VL,VR
↓
Vòng trong:
Motor PWMCó thể hình dung:
POSITION
↓
VELOCITY
↓
MOTORVòng ngoài chậm hơn vòng trong.
23. Tần số vòng điều khiển
Ví dụ:
Motor PWM:
20 kHz
Current loop:
5 kHz
Wheel speed PID:
100–500 Hz
Odometry:
100–200 Hz
Trajectory controller:
50–100 HzĐây chỉ là cấu trúc tham khảo; thông số thực tế phụ thuộc motor, encoder và phần cứng.
Với STM32F103, chúng ta có thể bắt đầu đơn giản:
1 ms → system tick
10 ms → motor speed PID
20 ms → odometry + trajectory controller24. Một cải tiến rất hay: Feedback chỉ sửa vừa đủ
Giả sử:
Vd = 300 mm/snhưng:
ex = 500 mmnếu:
KX = 1thì:
V = 800 mm/srất nguy hiểm.
Do đó phải giới hạn:
if (V > MAX_SPEED)
