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.

Sau bài này bạn sẽ:
  • 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

Hình 1. Hình học Pure Pursuit: chọn điểm trên đường cách xe L_d và đi theo cung tròn qua điểm đó.
Hình 1. Hình học Pure Pursuit: chọn điểm trên đường cách xe L_d và đi theo cung tròn qua điểm đó.

Ý 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. Mô phỏng thật với ba khoảng nhìn trước: L_d nhỏ bám sát nhưng dao động, L_d lớn mượt nhưng cắt góc.
Hình 2. Mô phỏng thật với ba khoảng nhìn trước: L_d nhỏ bám sát nhưng dao động, L_d lớn mượt nhưng cắt gó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 PursuitStanley
Tham số chínhKhoảng nhìn trướcHệ số k
Ưu điểmMượt, bền với nhiễu, dễ chỉnhBám sát đường, sai số ngang nhỏ ở tốc độ thấp
Nhược điểmCắt góc khi Ld lớnNhạy với nhiễu hướng, cần đường mượt (đạo hàm liên tục)
Hợp vớiXe vi sai, AGV khoXe 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
AGV bám line ảo: nếu quy trình yêu cầu xe luôn đi trên một số tuyến cố định (như AGV từ dẫn truyền thống), hãy tạo path từ danh sách điểm tuyến định sẵn và gửi thẳng cho action /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.

Tài liệu tham khảo