Dù dùng Nav2 hay tự viết, cuối cùng AGV cần một bộ điều khiển biến "đường cần đi" thành "vận tốc góc ngay bây giờ". Pure Pursuit và Stanley là hai thuật toán đơn giản, dễ hiểu, được dùng rộng rãi cho AGV bám line ảo, bám đường dẫn từ planner, và là nền tảng của Regulated Pure Pursuit trong Nav2.
- Hiểu và tự cài đặt Pure Pursuit, Stanley.
- Biết cách chọn khoảng nhìn trước và ảnh hưởng của nó.
- Cấu hình Regulated Pure Pursuit cho AGV công nghiệp.
1. Pure Pursuit

Ý tưởng giống người lái xe nhìn về phía trước: chọn trên đường một điểm cách xe khoảng Ld (lookahead), rồi tính cung tròn đi qua tâm xe và điểm đó, tiếp xúc với hướng hiện tại của xe. Nếu điểm nhìn trước có tọa độ (x, y) trong khung base_link (x tiến, y trái) thì độ cong:
κ = 2·y / Ld² → ω = v · κ (xe vi sai) hoặc δ = atan(L · κ) (xe Ackermann)
import math
def pure_pursuit(pose, path, lookahead, v):
"""pose = (x, y, yaw) trong frame map; path = [(x, y), ...] đã theo thứ tự.
Trả về (v, w). Đơn giản hóa: tìm điểm đầu tiên cách xe >= lookahead."""
x, y, yaw = pose
nearest = min(range(len(path)), key=lambda i: math.hypot(path[i][0] - x, path[i][1] - y))
target = path[-1]
for px, py in path[nearest:]:
if math.hypot(px - x, py - y) >= lookahead:
target = (px, py)
break
dx, dy = target[0] - x, target[1] - y
# đổi sang khung xe
xl = math.cos(yaw) * dx + math.sin(yaw) * dy
yl = -math.sin(yaw) * dx + math.cos(yaw) * dy
ld2 = xl * xl + yl * yl
if ld2 < 1e-6:
return 0.0, 0.0
kappa = 2.0 * yl / ld2
return v, v * kappa
print(pure_pursuit((0.0, 0.0, 0.0), [(i * 0.1, 0.5) for i in range(50)], 0.8, 0.5))
(Dùng Ld² tính từ khoảng cách thật tới điểm đích thay vì giá trị danh định giúp công thức đúng cả khi gần cuối đường.)
2. Chọn khoảng nhìn trước

Hình 2 là mô phỏng thật của thuật toán trên với xe chạy 0,5 m/s, bắt đầu lệch 0,3 m, bám đoạn chuyển làn 1 m:
- Ld = 0,3 m: phản ứng gắt nhất, về đường nhanh nhất. Mô phỏng lý tưởng (không trễ, không nhiễu) nên chưa thấy lắc, nhưng trên xe thật có trễ điều khiển và nhiễu định vị, Ld quá nhỏ chính là nguyên nhân hay gặp nhất khiến xe lắc qua lại.
- Ld = 0,8 m: về đường chậm hơn một chút nhưng mượt, bám tốt đoạn chuyển làn – vùng hợp lý cho AGV 0,5 m/s.
- Ld = 2,0 m: rất mượt nhưng bắt đầu rẽ quá sớm (cắt góc), vọt lố nhẹ sau đoạn chuyển làn và mất nhiều mét mới về sát đường – nguy hiểm khi lối hẹp.
Quy tắc kinh nghiệm: Ld tỉ lệ với vận tốc, Ld = v · tla với tla khoảng 1–2 s, kẹp trong [Lmin, Lmax]. RPP hỗ trợ sẵn qua use_velocity_scaled_lookahead_dist.
3. Stanley controller
Stanley (dùng trên xe tự hành Stanley của Stanford, thắng DARPA Grand Challenge 2005) tính góc lái từ điểm trên đường gần trục trước nhất:
δ = ψe + atan( k · e / (v + ks) )
trong đó ψe là sai số hướng (hướng đường − hướng xe), e là sai số ngang (dương khi đường nằm bên trái), k là hệ số tăng ích, ks nhỏ để tránh chia cho 0 khi chạy chậm. Thành phần thứ nhất xoay xe song song đường, thành phần thứ hai kéo xe về đường.
import math
def stanley(front_xy, yaw, v, path_xy, path_yaw, k=1.0, ks=0.1, max_steer=math.radians(35)):
fx, fy = front_xy
i = min(range(len(path_xy)), key=lambda j: math.hypot(path_xy[j][0] - fx, path_xy[j][1] - fy))
px, py = path_xy[i]
heading_err = math.atan2(math.sin(path_yaw[i] - yaw), math.cos(path_yaw[i] - yaw))
# sai số ngang có dấu: chiếu vector (điểm đường - trục trước) lên pháp tuyến trái của xe
e = -(px - fx) * math.sin(yaw) + (py - fy) * math.cos(yaw)
delta = heading_err + math.atan2(k * e, v + ks)
return max(-max_steer, min(max_steer, delta))
| Pure Pursuit | Stanley | |
|---|---|---|
| Tham số chính | Khoảng nhìn trước | Hệ số k |
| Ưu điểm | Mượt, bền với nhiễu, dễ chỉnh | Bám sát đường, sai số ngang nhỏ ở tốc độ thấp |
| Nhược điểm | Cắt góc khi Ld lớn | Nhạy với nhiễu hướng, cần đường mượt (đạo hàm liên tục) |
| Hợp với | Xe vi sai, AGV kho | Xe Ackermann, xe kéo tốc độ vừa |
4. Regulated Pure Pursuit trong Nav2
RPP bổ sung cho Pure Pursuit những thứ cần cho nhà máy: giảm tốc khi cua gắt, giảm tốc gần vật cản, giảm tốc khi gần đích, xoay tại chỗ về hướng đường trước khi đi, kiểm tra va chạm trên cung sắp đi.
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 0.6
lookahead_dist: 0.8
min_lookahead_dist: 0.4
max_lookahead_dist: 1.5
lookahead_time: 1.5
use_velocity_scaled_lookahead_dist: true
use_regulated_linear_velocity_scaling: true # chậm lại khi cua
regulated_linear_scaling_min_radius: 0.9
use_cost_regulated_linear_velocity_scaling: true # chậm lại gần vật cản
min_approach_linear_velocity: 0.05
approach_velocity_scaling_dist: 0.6
use_rotate_to_heading: true # false cho xe Ackermann
rotate_to_heading_min_angle: 0.785
rotate_to_heading_angular_vel: 0.8
max_angular_accel: 1.5
use_collision_detection: true
max_allowed_time_to_collision_up_to_carrot: 1.0
/follow_path của controller_server, bỏ qua planner. Xe sẽ đi đúng tuyến, dễ đoán với người làm việc xung quanh.5. Lỗi thường gặp
- Xe dao động quanh đường: Ld quá nhỏ hoặc định vị nhiễu; tăng Ld, kiểm tra odom/AMCL.
- Xe cắt góc chạm kệ: Ld quá lớn ở khúc cua – dùng lookahead theo vận tốc và giảm tốc khi cua.
- Sai dấu y: nhầm khung map và khung xe khi tính điểm nhìn trước – xe quẹo ngược.
- Đến đích bị lắc: điểm nhìn trước vượt ra ngoài cuối đường; luôn kẹp mục tiêu là điểm cuối.