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.
- 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

- 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.
- 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.
- 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.
- 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ểm | Lưu ý |
|---|---|---|
| AprilTag / ArUco + camera | Rẻ, cho đủ 6 bậc tự do, phân biệt được nhiều dock | Cầ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áng | Cầ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 intensity | Lidar 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ối | Chỉ 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.
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

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/BatteryStatetừ 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ệu | Xử lý |
|---|---|
| Không thấy dock tại staging pose | Xoay 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òng | Lùi 0,3 m, căn lại, thử tối đa 3 lần |
| Có dòng nhưng chập chờn | Tiếp điểm bẩn/mòn: báo bảo trì, đẩy nhẹ thêm 1 cm |
| Dock đang có xe khác | Mission 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.