AGV chạy nhiều ca cần tự sạc mà không có người cắm điện. Cái khó là độ chính xác: AMCL cho sai số khoảng ±5–10 cm, trong khi tiếp điểm sạc thường chỉ cho phép lệch vài cm và vài độ. Vì vậy docking luôn có một giai đoạn điều khiển riêng dựa trên cảm biến nhìn trực tiếp vào trạm sạc.

Sau bài này bạn sẽ:
  • Hiểu các giai đoạn của một quy trình docking tin cậy.
  • Biết các cách nhận diện trạm sạc và viết bộ điều khiển tiếp cận.
  • Giám sát quá trình sạc và xử lý khi docking thất bại.

1. Các giai đoạn

Hình 1. Ba giai đoạn docking: Nav2 đưa xe tới điểm chờ, bộ điều khiển riêng tiếp cận chính xác, dừng khi có dòng sạc.
Hình 1. Ba giai đoạn docking: Nav2 đưa xe tới điểm chờ, bộ điều khiển riêng tiếp cận chính xác, dừng khi có dòng sạc.
  1. Tới staging pose bằng Nav2: điểm cách dock khoảng 0,7–1,5 m, mặt hướng vào dock. Sai số vài cm vẫn chấp nhận được.
  2. Nhận diện dock bằng cảm biến: tính chính xác vị trí, góc của dock so với xe.
  3. Tiếp cận chậm (0,05–0,1 m/s) bằng bộ điều khiển riêng, liên tục cập nhật vị trí dock.
  4. Xác nhận tiếp xúc: dòng sạc hoặc điện áp tiếp điểm, công tắc hành trình; quá quãng đường cho phép mà chưa tiếp xúc thì lùi ra thử lại.

2. Nhận diện trạm sạc

CáchƯu điểmLưu ý
AprilTag / ArUco + cameraRẻ, cho đủ 6 bậc tự do, phân biệt được nhiều dockCần hiệu chuẩn camera, đủ sáng, giữ tag sạch
Hình dạng đặc trưng quét bằng lidar (chữ V, chữ W)Dùng lại lidar sẵn có, không phụ thuộc ánh sángCần thuật toán khớp mẫu, dễ nhầm với vật giống
Băng phản quang (cường độ lidar)Rất nổi bật trong dữ liệu intensityLidar phải xuất intensity
Hồng ngoại / tiếp điểm cơ khí dẫn hướngĐơn giản, tin cậy ở bước cuốiChỉ dùng được ở vài cm cuối

Với AprilTag, gói apriltag_ros phát TF từ camera tới tag; khi đó vị trí dock trong base_link lấy ngay bằng TF (bài #5).

3. Bộ điều khiển tiếp cận

Ý tưởng: tại mỗi chu kỳ, lấy vị trí dock trong khung xe (x tiến, y trái, θ là góc lệch mặt dock), đi thẳng chậm và chỉnh góc để triệt tiêu đồng thời lệch ngang và lệch góc:

import math


def approach_cmd(dock_x, dock_y, dock_yaw, contact_offset=0.05,
                 v_max=0.08, k_y=1.5, k_yaw=1.0, w_max=0.3):
    """dock_* : vị trí/góc của mặt tiếp điểm dock trong khung base_link.
    Trả về (v, w, done)."""
    dist = dock_x - contact_offset
    if dist <= 0.0:
        return 0.0, 0.0, True
    # chậm dần khi gần: tối thiểu 0,02 m/s để thắng ma sát
    v = max(0.02, min(v_max, 0.5 * dist))
    w = k_y * math.atan2(dock_y, max(dist, 0.1)) + k_yaw * dock_yaw
    w = max(-w_max, min(w_max, w))
    return v, w, False


print(approach_cmd(0.60, 0.03, math.radians(2)))   # còn 0,55 m, lệch 3 cm và 2°

Trước khi tiến vào giai đoạn cuối, kiểm tra lệch ngang và góc đã nhỏ (ví dụ < 1 cm, < 1°); nếu chưa, lùi lại và căn lại thay vì cố ép vào tiếp điểm.

Nav2 Docking Server: từ bản Jazzy, Nav2 có sẵn opennav_docking – action server với plugin nhận diện dock (ví dụ SimpleChargingDock dùng pose từ AprilTag hoặc bộ khớp lidar), bộ điều khiển tiếp cận và kiểm tra sạc qua BatteryState. Trên Humble có thể dùng bản backport của dự án hoặc tự viết theo cùng cấu trúc như ở trên.

4. Xác nhận và giám sát sạc

Hình 2. Đường cong sạc CC–CV điển hình của pack LiFePO4 8S (minh họa).
Hình 2. Đường cong sạc CC–CV điển hình của pack LiFePO4 8S (minh họa).

Sạc lithium theo hai pha: dòng không đổi (CC) tới khi điện áp đạt ngưỡng (LiFePO4 khoảng 3,6 V/cell, tức ~28,8 V cho pack 8S 24 V), rồi điện áp không đổi (CV), dòng giảm dần tới khi dưới ngưỡng kết thúc. Bộ sạc và BMS lo việc này; phía ROS cần:

  • Đọc sensor_msgs/BatteryState từ BMS (CAN/RS485): dòng dương ổn định trong 2–3 s sau khi chạm → docking thành công.
  • Theo dõi nhiệt độ pack, dòng sạc bất thường, cảnh báo từ BMS.
  • Chỉ cấp điện ra tiếp điểm khi xe đã nằm đúng chỗ (dock "thông minh" bật điện qua tín hiệu hồng ngoại/không dây) để tiếp điểm không mang điện hở.

5. Khi docking thất bại

Dấu hiệuXử lý
Không thấy dock tại staging poseXoay nhẹ quét tìm; vẫn không thấy → báo lỗi (dock bị che, hỏng tag)
Đi quá quãng mà chưa có dòngLùi 0,3 m, căn lại, thử tối đa 3 lần
Có dòng nhưng chập chờnTiếp điểm bẩn/mòn: báo bảo trì, đẩy nhẹ thêm 1 cm
Dock đang có xe khácMission manager / fleet chọn dock khác (bài #22)

6. Lỗi thường gặp

  • Costmap coi dock là vật cản: ở giai đoạn cuối, bộ điều khiển docking chạy ngoài Nav2 hoặc tắt kiểm tra va chạm cho vùng dock (nhưng vẫn giữ safety lidar với field set "docking" nhỏ).
  • Tag nhìn quá gần bị mất khỏi khung hình: đặt tag cao hoặc dùng hai tag kích thước khác nhau cho xa/gần.
  • Không xử lý rời dock: phải lùi thẳng ra staging pose trước khi giao lại cho Nav2.

Tài liệu tham khảo