🚗 BÀI 54 – INVERSE KINEMATICS ROBOT 2 BÁNH
Từ V, ω → tốc độ 2 bánh + điều khiển quỹ đạo trên STM32F103
Đây là bài rất quan trọng trong chuỗi học Robotics của bạn.
Ở Bài 53, chúng ta đã làm:
Encoder
↓
Odometry
↓
X, Y, θ
↓
GoTo(X,Y)Bây giờ ta đi theo chiều ngược lại:
Robot muốn chuyển động như thế nào?
↓
V và ω
↓
INVERSE KINEMATICS
↓
┌──────┴──────┐
↓ ↓
Tốc độ bánh L Tốc độ bánh R
↓ ↓
PID LEFT PID RIGHT
↓ ↓
PWM LEFT PWM RIGHT1. Trước hết: V và ω là gì?
Robot 2 bánh có hai đại lượng chuyển động cơ bản:
V – vận tốc tuyến tính
Cho biết robot chạy nhanh về phía trước bao nhiêu.
Đơn vị:
mm/sVí dụ:
V = 200;→ robot muốn chạy về phía trước với tốc độ:
200 mm/sω – vận tốc góc
Cho biết robot đang quay nhanh như thế nào.
Đơn vị:
rad/sVí dụ:
omega = 1.0;→ robot quay với tốc độ:
1 rad/s2. Hãy hình dung robot
↑ hướng robot
│
┌───────┴───────┐
│ │
LEFT RIGHT
wheel wheel
● ●
│<----- W ----->│Trong đó:
W = khoảng cách giữa 2 bánhVí dụ:
#define WHEEL_BASE_MM 150.0f3. Nếu ω = 0
Giả sử:
V = 200 mm/s
ω = 0 rad/sRobot không quay.
Do đó:
Left = 200 mm/s
Right = 200 mm/sRobot đi thẳng:
●──────────────→4. Nếu V = 0
Ví dụ:
V = 0
ω = 1 rad/sRobot không tiến nhưng quay tại chỗ.
Hai bánh phải quay ngược chiều:
↑
↺ ↻
● ●Cụ thể:
Left = -75 mm/s
Right = +75 mm/snếu:
W = 150 mm5. Công thức Inverse Kinematics
Đây là công thức quan trọng nhất của bài:
[
V_L=V-\frac{\omega W}{2}
]
[
V_R=V+\frac{\omega W}{2}
]
Trong đó:
V = vận tốc robot
ω = vận tốc góc
W = khoảng cách 2 bánh
VL = tốc độ bánh trái
VR = tốc độ bánh phải6. Ví dụ 1 – Đi thẳng
V = 200
ω = 0
W = 150Ta có:
[
V_L=200-\frac{0\times150}{2}=200
]
[
V_R=200+\frac{0\times150}{2}=200
]
Kết quả:
LEFT = 200 mm/s
RIGHT = 200 mm/s7. Ví dụ 2 – Quay trái trong khi tiến
V = 200
ω = +1
W = 150Ta có:
[
V_L=200-75=125
]
[
V_R=200+75=275
]
Robot:
↗
/
/
●
L R
125 275Bánh phải nhanh hơn → robot quay trái.
8. Ví dụ 3 – Quay phải
V = 200
ω = -1Kết quả:
VL = 275
VR = 125Bánh trái nhanh hơn → robot quay phải.
9. Ví dụ 4 – Quay tại chỗ
V = 0
ω = 1
W = 150VL = -75
VR = +75Robot:
↻
│
●──┼──●
│
↺10. Viết hàm C
Ta tạo:
typedef struct
{
float left;
float right;
} WheelVelocity_t;Sau đó:
WheelVelocity_t InverseKinematics(
float V,
float omega)
{
WheelVelocity_t result;
result.left =
V - omega * WHEEL_BASE_MM / 2.0f;
result.right =
V + omega * WHEEL_BASE_MM / 2.0f;
return result;
}Đây chính là Inverse Kinematics cho robot differential-drive.
11. Nhưng robot thực tế không thể chạy vô hạn
Ví dụ:
V = 5000 mm/slà không thực tế đối với robot nhỏ.
Ta cần:
#define MAX_WHEEL_SPEED_MM_S 300.0f12. Hàm giới hạn
float ClampFloat(
float value,
float min,
float max)
{
if (value < min)
return min;
if (value > max)
return max;
return value;
}13. Một vấn đề rất quan trọng: saturation
Giả sử:
V = 300
ω = 2
W = 150Ta có:
VL = 150
VR = 450Nhưng motor tối đa:
300 mm/sNếu chỉ giới hạn:
VR = 300thì:
VL = 150
VR = 300Tỷ lệ chuyển động đã bị thay đổi.
Điều này có thể làm robot quay khác với yêu cầu.
14. Cách tốt hơn: scale cả hai bánh
Nếu:
VL = 150
VR = 450và giới hạn:
MAX = 300ta lấy:
scale = 300 / 450
= 0.667Sau đó:
VL = 150 × 0.667 = 100
VR = 450 × 0.667 = 300Tỷ lệ giữa hai bánh vẫn được giữ nguyên.
15. Hàm giới hạn tốc độ 2 bánh
void LimitWheelVelocity(
float *left,
float *right)
{
float max_speed;
max_speed = fabsf(*left);
if (fabsf(*right) > max_speed)
max_speed = fabsf(*right);
if (max_speed > MAX_WHEEL_SPEED_MM_S)
{
float scale;
scale =
MAX_WHEEL_SPEED_MM_S /
max_speed;
*left *= scale;
*right *= scale;
}
}Đây là một kỹ thuật rất đáng nhớ.
16. Tạo hàm cấp cao
Bây giờ chúng ta có thể viết:
void Robot_SetVelocity(
float V,
float omega)
{
WheelVelocity_t wheel;
wheel =
InverseKinematics(
V,
omega);
LimitWheelVelocity(
&wheel.left,
&wheel.right);
Robot_SetWheelSpeed(
wheel.left,
wheel.right);
}Từ bây giờ người dùng không cần quan tâm trực tiếp tới 2 bánh.
17. Ví dụ
Robot_SetVelocity(200, 0);→ đi thẳng.
Robot_SetVelocity(200, 0.5f);→ tiến và quay trái.
Robot_SetVelocity(200, -0.5f);→ tiến và quay phải.
Robot_SetVelocity(0, 1.0f);→ quay tại chỗ.
18. Đây là một thay đổi rất lớn về tư duy
Trước đây:
SetLeftPWM(100);
SetRightPWM(120);Bạn đang nói:
“Motor trái chạy PWM 100, motor phải PWM 120.”
Bây giờ:
Robot_SetVelocity(
200,
0.5);Bạn đang nói:
“Robot hãy tiến với 200 mm/s và quay với 0.5 rad/s.”
Đây chính là điều khiển robot ở tầng chuyển động.
19. Ba tầng điều khiển
Từ đây robot của chúng ta nên có 3 tầng:
TẦNG 1
POSITION CONTROL
X,Y,θ
↓
TẦNG 2
MOTION CONTROL
V, ω
↓
TẦNG 3
WHEEL CONTROL
VL, VR
↓
PID SPEED
↓
PWMĐây là kiến trúc rất quan trọng.
20. Tầng 1 – Position
Ví dụ:
Robot_GoTo(1000, 500);Controller tính:
distance
heading errorsau đó tạo:
V
ω21. Tầng 2 – Motion
Robot_SetVelocity(
V,
omega);Inverse kinematics chuyển thành:
VL
VR22. Tầng 3 – Wheel
VL target
↓
Encoder đo VL thực tế
↓
PID
↓
PWMvà tương tự cho bánh phải.
23. Đây là Cascade Control
TARGET X,Y
│
▼
┌─────────────┐
│ Position │
│ Controller │
└──────┬──────┘
│
V,ω
│
▼
┌─────────────┐
│ Inverse │
│ Kinematics │
└──────┬──────┘
│
VL,VR
│
▼
┌─────────────┐
│ Speed PID │
└──────┬──────┘
│
PWM
│
▼
MOTOR
│
▼
ENCODER24. Bây giờ chúng ta sẽ dùng nó để tạo quỹ đạo
Đây mới là phần thú vị.
Thay vì:
GoTo một điểmta có thể tạo:
đường thẳng
đường tròn
đường cong
hình số 825. Quỹ đạo đường thẳng
Ta muốn:
V = 200
ω = 0thì:
Robot_SetVelocity(200, 0);Robot chạy:
●──────────────────→26. Quỹ đạo đường tròn
Muốn robot chạy thành vòng tròn:
●
↗ ↘
● ●
↖ ↙
●ta giữ:
V = constant
ω = constantVí dụ:
Robot_SetVelocity(
200,
0.5f);27. Bán kính đường tròn
Công thức:
[
R=\frac{V}{\omega}
]
Ví dụ:
V = 200 mm/s
ω = 0.5 rad/sthì:
[
R=400mm
]
Robot sẽ chạy vòng tròn bán kính khoảng:
400 mm28. Đây là công thức cực kỳ quan trọng
V
R = ───────
ωSuy ra:
ω = V / RMuốn robot chạy vòng tròn bán kính 500 mm:
V = 200 mm/sthì:
ω = 200 / 500
= 0.4 rad/s29. Tốc độ hai bánh khi chạy vòng tròn
Ví dụ:
V = 200
ω = 0.4
W = 150Ta có:
VL = 200 - 30 = 170
VR = 200 + 30 = 230Bánh ngoài chạy nhanh hơn bánh trong.
Đó chính là nguyên lý của robot differential-drive.
30. Tạo hàm chạy vòng tròn
void Robot_Circle(
float linear_speed,
float radius)
{
float omega;
if (radius == 0.0f)
return;
omega =
linear_speed / radius;
Robot_SetVelocity(
linear_speed,
omega);
}Ví dụ:
Robot_Circle(
200.0f,
500.0f);→ chạy vòng tròn bán kính 500 mm.
31. Chạy vòng tròn ngược chiều
Nếu:
radius = -500;thì:
ω < 0→ robot quay theo hướng ngược lại.
32. Quỹ đạo hình số 8
Đây là bài test rất hay.
Ta có thể thay đổi:
ωtheo thời gian.
Ví dụ ý tưởng:
omega =
0.5f * sinf(time);Trong khi:
V = 200;thì robot sẽ tạo quỹ đạo uốn cong thay đổi theo thời gian.
33. Nhưng có một khái niệm quan trọng hơn: trajectory
Ta phân biệt:
Position
X,Y,θVelocity
V,ωAcceleration
a
αTrong bài hôm nay chúng ta mới làm:
Position → Velocityvà:
Velocity → Wheel velocity34. Tại sao không nên đổi tốc độ đột ngột?
Ví dụ:
0 mm/s
↓
300 mm/sngay lập tức.
Motor có thể:
giậtbánh xe:
trượtencoder:
saiodometry:
drift35. Vì vậy cần Velocity Ramp
Thay vì:
0
300hãy:
0
20
40
60
80
...
30036. Tạo acceleration limiter
Ví dụ:
float RampVelocity(
float current,
float target,
float max_accel,
float dt)
{
float max_delta;
max_delta =
max_accel * dt;
if (target > current + max_delta)
return current + max_delta;
if (target < current - max_delta)
return current - max_delta;
return target;
}37. Ví dụ
current = 100 mm/s
target = 300 mm/s
accel = 500 mm/s²
dt = 0.01 sTa có:
max_delta = 500 × 0.01
= 5 mm/sDo đó tốc độ mới:
105 mm/schứ không nhảy ngay lên:
300 mm/s38. Đây là cách nên đặt trong hệ thống
TARGET V,ω
↓
Velocity Ramp
↓
Limited V,ω
↓
Inverse Kinematics
↓
VL,VR
↓
Wheel PID
↓
PWM39. Một module motion.c
Bạn có thể tổ chức:
typedef struct
{
float target_v;
float target_omega;
float current_v;
float current_omega;
} MotionControl_t;40. Hàm đặt vận tốc
void Motion_SetVelocity(
float v,
float omega)
{
motion.target_v = v;
motion.target_omega = omega;
}41. Hàm cập nhật
void Motion_Update(float dt)
{
motion.current_v =
RampVelocity(
motion.current_v,
motion.target_v,
MAX_ACCELERATION,
dt);
motion.current_omega =
RampVelocity(
motion.current_omega,
motion.target_omega,
MAX_ANGULAR_ACCEL,
dt);
Robot_SetVelocity(
motion.current_v,
motion.current_omega);
}Đây đã là một motion controller khá sạch.
42. Bây giờ ghép với Bài 53
Bài 53:
GoTo(X,Y)sẽ tính:
distance
heading errorsau đó:
V
ωvà đưa cho:
Motion_SetVelocity(V, omega);43. Hệ thống hoàn chỉnh
TARGET
X,Y
│
▼
┌─────────────────┐
│ GO-TO CONTROLLER│
└────────┬────────┘
│
V,ω
│
▼
┌─────────────────┐
│ VELOCITY RAMP │
└────────┬────────┘
│
V,ω smooth
│
▼
┌─────────────────┐
│ INVERSE │
│ KINEMATICS │
└────────┬────────┘
│
VL,VR
┌────┴────┐
▼ ▼
PID LEFT PID RIGHT
│ │
▼ ▼
PWM L PWM R
│ │
▼ ▼
MOTOR L MOTOR R
│ │
▼ ▼
ENCODER L ENCODER R
└────┬────┘
▼
ODOMETRY
│
X,Y,θ
│
└──────────feedbackĐây chính là kiến trúc mà bạn nên hướng tới cho robot 2 bánh của mình.
44. Một bộ API rất đẹp cho robot
Sau khi hoàn thành bài này, bạn có thể thiết kế API:
Robot_SetVelocity(V, omega);Robot_Stop();Robot_GoTo(x, y);Robot_MoveDistance(distance);Robot_Turn(angle);Robot_Circle(speed, radius);và:
Robot_GetPose(&pose);Trong đó:
typedef struct
{
float x;
float y;
float theta;
} RobotPose_t;45. Một ví dụ hoàn chỉnh
Giả sử:
#define WHEEL_BASE_MM 150.0f
#define MAX_WHEEL_SPEED_MM_S 300.0f
#define MAX_ACCELERATION 500.0f
#define MAX_ANGULAR_ACCEL 2.0fTa gọi:
Motion_SetVelocity(
200.0f,
0.5f);Motion controller sẽ tăng dần:
V:
0
5
10
15
...
200sau đó inverse kinematics:
VL = 200 - 0.5×150/2
= 162.5 mm/s
VR = 200 + 0.5×150/2
= 237.5 mm/sSau đó:
162.5 → LEFT PID → PWM LEFT
237.5 → RIGHT PID → PWM RIGHTRobot chạy theo một cung tròn.
46. Điều bạn cần đặc biệt hiểu
Đừng nhầm:
PWM ≠ tốc độ robotPWM chỉ là:
tín hiệu điều khiển motor.
Encoder mới cho biết:
motor thực sự đang chạy bao nhiêu.
Vì vậy:
Target speed
↓
Encoder feedback
↓
PID
↓
PWM47. Và đừng nhầm V với PWM
V = 200 mm/skhông có nghĩa:
PWM = 200Ví dụ có thể:
V = 200 mm/scần:
PWM = 137ở một điều kiện tải.
Khi robot lên dốc, có thể cần:
PWM = 180để vẫn giữ:
200 mm/sĐó chính là lý do chúng ta cần PID tốc độ.
48. Bài thực hành STM32F103
Bạn nên test theo đúng thứ tự.
TEST 1 – đi thẳng
Robot_SetVelocity(
100,
0);Kiểm tra:
VL ≈ VRTEST 2 – quay trái
Robot_SetVelocity(
100,
0.5);Kiểm tra:
VR > VLTEST 3 – quay phải
Robot_SetVelocity(
100,
-0.5);Kiểm tra:
VL > VRTEST 4 – quay tại chỗ
Robot_SetVelocity(
0,
1);Kiểm tra:
VL = -VRTEST 5 – vòng tròn
Robot_Circle(
100,
500);Robot phải chạy thành cung tròn.
49. Một bài test cực kỳ quan trọng
In ra UART:
V=...
W=...
VL=...
VR=...
EncoderL=...
EncoderR=...
X=...
Y=...
Theta=...Ví dụ:
V=200.0
W=0.50
VL=162.5
VR=237.5
X=315.2
Y=102.7
Theta=21.4Khi làm robot thật, debug kiểu này cực kỳ hữu ích.
50. Những lỗi thường gặp
Lỗi 1 – Robot quay ngược
Kiểm tra dấu:
VL = V - ωW/2
VR = V + ωW/2và convention:
ω > 0 = quay tráiLỗi 2 – Robot chạy thẳng nhưng lệch
Có thể:
VL ≠ VRdo:
- motor khác nhau
- bánh khác đường kính
- encoder khác nhau
- PID chưa cân bằng
Lỗi 3 – Robot chạy vòng tròn nhưng bán kính sai
Kiểm tra:
WHEEL_BASE_MMvà:
encoder calibrationLỗi 4 – Robot giật
Kiểm tra:
velocity rampvà PID.
Lỗi 5 – Robot quay quá mạnh
Giảm:
MAX_ANGULAR_SPEEDhoặc:
GO_TO_ANGLE_KP51. Điều quan trọng nhất của Bài 54
Bạn cần nắm được chuỗi này:
ROBOT COMMAND
│
▼
V , ω
│
▼
┌────────────────────┐
│ INVERSE KINEMATICS │
└─────────┬──────────┘
│
┌────┴────┐
▼ ▼
VL VR
│ │
▼ ▼
SPEED PID SPEED PID
│ │
▼ ▼
PWM PWM
│ │
▼ ▼
MOTOR MOTORVà công thức trung tâm:
[
\boxed{V_L=V-\frac{\omega W}{2}}
]
[
\boxed{V_R=V+\frac{\omega W}{2}}
]
52. Từ Bài 53 → Bài 54
Hai bài này ghép với nhau thành:
X,Y TARGET
│
▼
GO-TO CONTROL
│
▼
V,ω
│
▼
INVERSE KINEMATICS
│
▼
VL,VR
│
▼
SPEED PID
│
▼
MOTOR
│
▼
ENCODER
│
▼
ODOMETRY
│
▼
X,Y,θ
│
└─────────────┐
│
└── feedbackĐây chính là nền móng của một robot tự hành 2 bánh thực sự.
🚀 Bài 55 nên học tiếp
Sau Bài 54, bước rất tự nhiên là:
BÀI 55 – TRAJECTORY TRACKING: ROBOT BÁM ĐƯỜNG THẲNG, ĐƯỜNG TRÒN VÀ ĐƯỜNG CONG BẰNG X, Y, θ
Lúc đó chúng ta sẽ không chỉ nói:
Robot_GoTo(1000, 500);mà sẽ cho robot bám một quỹ đạo đã định trước:
●───────●
╱ ╲
● ●
╲ ╱
●─────────●và bắt đầu đi vào các khái niệm rất quan trọng của mobile robotics như:
- Path tracking
- Trajectory tracking
- Cross-track error
- Heading error
- Pure Pursuit
- Look-ahead distance
- Stanley Controller
- và sau đó mới tiến tới robot tự hành tránh vật cản + lập bản đồ + định vị.