Lập trình robot tự hành 2 bánh vi sai với ESP32: từ điều khiển động cơ, SLAM trên vi điều khiển đến ROS2

Tài liệu hướng dẫn lắp ghép phần cứng và lập trình firmware ESP-IDF hoàn chỉnh cho robot tự hành 2 bánh vi sai (AUTONOMY ROBOT) chạy trên một vi điều khiển ESP32-WROOM-32 (không dùng PSRAM). Hệ thống được thiết kế theo 3 giai đoạn trên cùng một codebase:

Giai đoạn 1 — Điều khiển và cấu hình qua WiFi: ESP32 phát SoftAP kèm Captive Portal, giao diện web nhúng trong flash, điều khiển vận tốc 2 bánh bằng vòng PI 50 Hz, đọc encoder 1 kênh bằng PCNT, tính odometry và tinh chỉnh tham số runtime lưu NVS.
Giai đoạn 2 — Tự nhận diện LiDAR + SLAM + Tự hành ngay trên ESP32: Driver UART tự nhận diện model LiDAR (LD14, LD06/LD19, Camsense X1), dựng bản đồ Occupancy Grid 200×200 ô (40 KB RAM), sửa trôi bằng Correlative Scan Matching 2 lớp và tự hành khám phá bản đồ theo thuật toán Frontier Exploration.
Giai đoạn 3 — Cầu nối UDP sang ROS2 trên máy tính: Chuyển tác vụ dựng bản đồ và điều hướng sang máy tính chạy slam_toolbox / Nav2 / RViz qua giao thức UDP định điểm tốn chưa tới 3 KB RAM.

Ba thành phần phần cứng cơ sở của robot đã có bài hướng dẫn riêng kèm chương trình kiểm tra độc lập:

Cụm động cơ và encoder: Điều khiển cụm động cơ bánh xe có encoder bằng ESP32 và TB6612FNG
Cảm biến LiDAR lựa chọn 1: Đọc lidar LD14 bằng ESP32 và xem ảnh quét ngay trên trình duyệt
Cảm biến LiDAR lựa chọn 2: Camsense X1 + ESP32: đọc LiDAR và hiển thị bản đồ 2D trên trình duyệt

Môi trường: ESP-IDF v5.x hoặc v6.x (đã kiểm tra trên v6.0.2), Python 3.10+, ROS2 Humble/Jazzy (cho giai đoạn 3).

Code đầy đủ: github.com/dungtsbb/autonomy_robot

Hướng dẫn vận hành (dành cho người dùng): Hướng dẫn sử dụng AUTONOMY ROBOT

1. Kiến trúc hệ thống

1.1 Sơ đồ khối phần cứng

        ┌──────────── Pin (VBAT) ────────────┬──────────────┐
        │                                    │              │
        ▼                                    ▼              ▼
   ┌─────────┐                        ┌────────────┐   ┌─────────┐
   │ TB6612  │ VM                     │ Hạ áp 5V   │   │ Cầu phân│
   │  FNG    │                        │  (buck)    │   │ áp đo pin│
   └────┬────┘                        └──┬──────┬──┘   └────┬────┘
        │ 4 dây động cơ                  │      │           │ (tuỳ chọn)
        ▼                                ▼      ▼           ▼
   ┌─────────┐                      ┌────────┐ ┌──────┐  GPIO 34
   │ 2 motor │───encoder 1 kênh───► │ ESP32  │ │LiDAR │
   │ + bánh  │      GPIO 18/19      │ WROOM  │ │ 2D   │
   └─────────┘                      └───┬────┘ └───┬──┘
                                        │ GPIO 16 ◄┘ (TX LiDAR)
                     TẤT CẢ GND NỐI CHUNG MỘT ĐIỂM

1.2 Sơ đồ phân tầng phần mềm

Firmware tách thành 18 component trong firmware/components/ và thư viện thuật toán slam/ thuần C (không gọi API ESP-IDF hay FreeRTOS, biên dịch và chạy kiểm thử hồi quy trực tiếp trên máy tính):

                 ┌────────────────────────────────────────────┐
 Nguồn lệnh      │ web_ctrl     navigator      ros2_bridge     │
                 │  (GĐ1)         (GĐ2)  ◄──loại trừ──►  (GĐ3) │
                 └─────┬────────────┬─────────────────┬────────┘
                       │  cmd_vel   │    cmd_vel      │  cmd_vel
                       ▼            ▼                 ▼
                 ┌────────────────────────────────────────────┐
 Trọng tài       │  motion  ◄── robot_core (mode manager)     │
                 │  - phân quyền theo mode (MANUAL/AUTO/ESTOP)│
                 │  - watchdog 500 ms → dừng động cơ          │
                 │  - đổi (v, ω) → tốc độ 2 bánh + vòng PI    │
                 └───────────────────┬────────────────────────┘
                                     ▼
                 ┌────────────────────────────────────────────┐
 Driver & SLAM   │  motor_driver (TB6612)   encoder → odometry│
                 │  lidar (tự nhận diện)  → mapping (slam/)   │
                 └───────────────────┬────────────────────────┘
                                     ▼
                 ┌────────────────────────────────────────────┐
 Nền tảng        │  bsp (pin map) + params (NVS) + trace (TCP)│
                 └────────────────────────────────────────────┘

Năm nguyên tắc giữ cho hệ thống không bị rối khi ghép nhiều tầng:

cmd_vel là giao diện điều khiển duy nhất. Mọi nguồn lệnh — joystick trên web (CMD_SOURCE_TELEOP), bộ tự hành trên chip (CMD_SOURCE_NAV), hay máy tính ROS2 (CMD_SOURCE_EXTERNAL) — đều chỉ gửi cặp vận tốc dài và vận tốc góc (linear_mps, angular_rps) theo quy ước REP-103 (x tiến, y trái, θ ngược kim đồng hồ). Component motion là nơi duy nhất chạm vào driver động cơ.
Trọng tài theo chế độ (robot_core). Robot luôn ở một trong 3 chế độ:
- MANUAL: chỉ nhận lệnh từ CMD_SOURCE_TELEOP.
- AUTO: nhận lệnh từ CMD_SOURCE_NAV hoặc CMD_SOURCE_EXTERNAL.
- ESTOP: phanh ngắn mạch lập tức và chặn mọi nguồn lệnh.
Loại trừ tường minh giữa 2 bộ não ở chế độ AUTO. Khi bật cầu ROS2 (ros2_bridge), firmware tự động tắt cả navigator lẫn mapping trên ESP32. Hai bộ điều hướng không bao giờ cùng phát cmd_vel, và ESP32 nhường toàn bộ CPU/RAM cho việc đọc cảm biến và đẩy gói UDP.
An toàn mặc định (Watchdog 500 ms). Khi bánh đang quay, nếu quá 500 ms không có lệnh cmd_vel hợp lệ mới, watchdog_task tự cắt động cơ.
Pin map và thông số tách khỏi logic. Toàn bộ chân GPIO nằm trong firmware/components/bsp/include/bsp.h; 18 tham số cơ khí, PID, SLAM và điều hướng nằm trong params (lưu NVS, chỉnh trực tiếp trên trình duyệt).

2. Phần cứng, Pin Map và Thông số cơ khí

2.1 Danh sách linh kiện

Linh kiện	Số lượng	Ghi chú
ESP32-WROOM-32 DevKit	1	Bản 38 chân, flash ≥ 4 MB, không cần PSRAM
Driver động cơ TB6612FNG	1	Điều khiển 2 kênh (1.2 A liên tục / 3.2 A đỉnh mỗi kênh)
Cụm động cơ bánh xe có encoder	2	Motor DC + hộp giảm tốc 46.85:1 + đĩa encoder 20 xung + bánh D=60 mm
Cảm biến LiDAR 2D	1	LDROBOT LD14, LD06/LD19, hoặc Camsense X1
Mạch hạ áp (Buck) 5V ≥ 2A	1	Cấp nguồn riêng cho ESP32 và mô-tơ quay của LiDAR
Tụ hoá 100–470 µF + tụ gốm 0.1 µF	1 bộ	Lắp sát chân `VM` – `GND` của TB6612FNG và 2 cực motor
Cầu phân áp 10 kΩ / 20 kΩ	2 bộ	Dùng nếu tín hiệu `OUT` của encoder ở mức 5V

2.2 Bảng đấu nối chân (Pin Map)

Nguồn sự thật của sơ đồ chân nằm tại firmware/components/bsp/include/bsp.h. Kênh A của TB6612FNG lái bánh PHẢI, kênh B lái bánh TRÁI:

GPIO ESP32	Hướng	Nối tới	Thiết bị	Ghi chú kỹ thuật
GPIO 12	OUT	`STBY`	TB6612FNG	Strapping pin (MTDI). Nối thẳng vào `STBY`, không mắc trở kéo lên 3.3V ngoài
GPIO 25	OUT	`PWMA`	TB6612FNG	PWM bánh PHẢI (LEDC kênh 1, 20 kHz)
GPIO 26	OUT	`AIN1`	TB6612FNG	Chiều bánh PHẢI
GPIO 27	OUT	`AIN2`	TB6612FNG	Chiều bánh PHẢI
GPIO 32	OUT	`PWMB`	TB6612FNG	PWM bánh TRÁI (LEDC kênh 0, 20 kHz)
GPIO 33	OUT	`BIN1`	TB6612FNG	Chiều bánh TRÁI
GPIO 14	OUT	`BIN2`	TB6612FNG	Chiều bánh TRÁI
GPIO 18	IN	`OUT` (Trái)	Encoder trái	Đếm xung PCNT, bật sẵn pull-up nội ~45 kΩ
GPIO 19	IN	`OUT` (Phải)	Encoder phải	Đếm xung PCNT, bật sẵn pull-up nội ~45 kΩ
GPIO 16	IN	`TX` LiDAR	LiDAR 2D	`UART2 RX` (mức logic 3.3V cho cả LD14, LD06 và Camsense X1)

Lưu ý khi lắp phần cứng:

Chân VCC của TB6612FNG nối vào 3.3V của ESP32 (nguồn logic), chân VM nối vào nguồn pin động cơ.

Nếu bánh quay ngược chiều lệnh, không cần hàn lại dây: đổi BSP_MOTOR_L_INVERT hoặc BSP_MOTOR_R_INVERT thành true trong bsp.h.

Ba loại LiDAR hỗ trợ đều dùng đúng 3 dây (5V, GND, TX → GPIO 16). Với LD14, chân PWM thả nổi để LiDAR tự chạy tốc độ mặc định.

2.3 Thông số cơ khí mặc định

Các thông số cơ khí khai báo trong firmware/components/params/include/robot_config.h:

Thông số	Ký hiệu / Key NVS	Giá trị mặc định	Đại lượng suy ra
Bán kính bánh xe	`wheel_r`	`30.0 mm`	Chu vi bánh `2πR ≈ 188.5 mm`
Khoảng cách 2 bánh (wheel base)	`wheel_b`	`109.35 mm`	Hiệu chuẩn thực nghiệm bằng phép xoay 10 vòng
Xung encoder / vòng trục motor	`enc_ppr`	`20.0`	Gắn ở trục motor trước hộp số
Tỉ số truyền hộp giảm tốc	`GEAR_RATIO`	`4123 / 88 ≈ 46.85`	`937 xung` trên mỗi vòng bánh (`0.2012 mm/xung`)
Duty tối thiểu bù ma sát tĩnh	`min_duty`	`0.28` (28%)	Dưới mức này động cơ không thắng nổi ma sát tĩnh

3. Tầng chấp hành: Encoder 1 kênh, Vòng PI và Odometry

3.1 Bài toán điểm mù chiều quay của encoder 1 kênh

Cụm động cơ sử dụng encoder quang 1 kênh (1 dây tín hiệu xung OUT, không có pha B lệch 90°). Bộ đếm phần cứng PCNT trên ESP32 chỉ biết số xung tăng lên, còn dấu (+1 tiến hay -1 lùi) phải do phần mềm gán.

Có hai lỗi kinh điển khi ghép encoder 1 kênh với bộ điều khiển PI:

Gán chiều theo vận tốc đích (target) thay vì duty thực tế: Khi robot đang giảm tốc hoặc vòng PI kéo ngược để hãm bánh vượt tốc, dấu của duty ngược với dấu của target. Nếu gán chiều đếm theo target, encoder báo vận tốc sai dấu → khâu I cộng dồn sai số theo chiều ngược → duty ghim cứng ở biên ±1.0 và bánh tự khoá ở tốc độ tối đa.
Đảo chiều đếm tức thì khi bánh còn quán tính: Ngay khoảnh khắc duty đổi dấu, bánh xe vẫn còn quay theo chiều cũ vài chục mili-giây do quán tính. Đảo dấu đếm ngay lập tức cũng làm vận tốc phản hồi bị lật dấu.

Cách giải quyết trong firmware nằm ở hai hàm:

Trong motion_control.c, hàm drive(duty_l, duty_r) là cổng duy nhất xuất PWM ra TB6612FNG và báo chiều trực tiếp theo duty thực đang cấp (khi duty == 0 thì giữ nguyên chiều cũ theo quán tính):

static void drive(float duty_l, float duty_r)
{
    s_duty_l = duty_l;
    s_duty_r = duty_r;

    if (duty_l != 0.0f) {
        encoder_set_direction(ENCODER_WHEEL_LEFT, duty_l > 0.0f ? 1 : -1);
    }
    if (duty_r != 0.0f) {
        encoder_set_direction(ENCODER_WHEEL_RIGHT, duty_r > 0.0f ? 1 : -1);
    }

    tb6612_set_speed(TB6612_MOTOR_LEFT, duty_l);
    tb6612_set_speed(TB6612_MOTOR_RIGHT, duty_r);
}

Trong encoder.c (chu kỳ lấy mẫu 20 ms / 50 Hz), chiều mới được lưu vào pending_dir và chỉ áp dụng trong apply_pending_dir() khi tốc độ xung đã hạ xuống ngưỡng gần dừng (rate <= 2 xung / 20 ms, tương đương ~0.02 m/s) hoặc sau thời gian chờ cưỡng bức 8 chu kỳ (160 ms), đồng thời xoá bộ lọc tốc độ EMA về 0:

static void apply_pending_dir(wheel_state_t *w)
{
    if (w->pending_dir == w->dir) {
        w->flip_wait = 0;
        return;
    }
    w->flip_wait++;
    if (w->rate <= DIR_FLIP_PULSE_MAX ||
        w->flip_wait >= DIR_FLIP_TIMEOUT_SAMPLES) {
        w->dir = w->pending_dir;
        w->speed_mps = 0.0f;
        w->flip_wait = 0;
    }
}

3.2 Mixing vi sai, bù vùng chết và vòng điều khiển PI 50 Hz

Khi nhận lệnh (v, ω), motion_control.c tính vận tốc dài đích của từng bánh:

v_L = v − ω × (wheel_base / 2)
v_R = v + ω × (wheel_base / 2)

Task pid_task chạy ở 50 Hz (20 ms), kết hợp Feedforward + PI và ánh xạ qua hàm bù vùng chết compensate_deadband():

float el = tl - encoder_get_speed_mps(ENCODER_WHEEL_LEFT);
float er = tr - encoder_get_speed_mps(ENCODER_WHEEL_RIGHT);
integ_l = clampf(integ_l + el * dt, -0.5f, 0.5f);   /* anti-windup */
integ_r = clampf(integ_r + er * dt, -0.5f, 0.5f);

float duty_l = tl / max_lin + kp * el + ki * integ_l;   /* feedforward + PI */
float duty_r = tr / max_lin + kp * er + ki * integ_r;

drive(compensate_deadband(clampf(duty_l, -1.0f, 1.0f)),
      compensate_deadband(clampf(duty_r, -1.0f, 1.0f)));

Hàm compensate_deadband(duty) có một vùng chết nhỏ DUTY_DEADZONE = 0.02f quanh điểm 0 trước khi nâng lên min_duty (0.28): nếu không có ngưỡng 0.02f này, nhiễu tính toán làm đầu ra PI chớm từ +0.001 sang -0.001 sẽ tạo bước nhảy duty từ +0.28 sang -0.28 (56% toàn dải) và kích hoạt đảo chiều encoder liên tục.

3.3 Odometry tích phân điểm giữa (Midpoint Integration)

Task odometry đọc quãng đường tích luỹ của 2 bánh mỗi 20 ms (50 Hz) và cập nhật pose (x, y, θ) bằng phương pháp tích phân điểm giữa — lấy góc hướng ở giữa bước θ + Δθ / 2 để giảm sai số khi robot vừa tiến vừa xoay:

float d = (dl + dr) * 0.5f;
float dtheta = (dr - dl) / (params_get(P_WHEEL_BASE_MM) * 0.001f);

float mid = s_pose.theta + dtheta * 0.5f;
s_pose.x += d * cosf(mid);
s_pose.y += d * sinf(mid);
s_pose.theta = wrap_angle(s_pose.theta + dtheta);

4. Driver LiDAR đa giao thức tự nhận diện (`lidar`)

Thay vì biên dịch riêng từng bản firmware cho từng loại LiDAR, component lidar tách phần giải mã gói tin ra giao diện lidar_proto_t thuần C (lidar_proto.h):

Model	Baud	Cỡ gói	Header	Điểm / gói	Kiểm toàn vẹn
LDROBOT LD14	115200	47 byte	`0x54 0x2C`	12	CRC-8 (đa thức `0x4D`)
LDROBOT LD06 / LD19	230400	47 byte	`0x54 0x2C`	12	CRC-8 (đa thức `0x4D`)
Camsense X1	115200	36 byte	`0x55 0xAA 0x03 0x08`	8	Checksum 15-bit (gập 17 word)

Cơ chế tự nhận diện và cắm nóng

Lúc khởi động (hoặc khi mất gói hợp lệ quá 3 giây), hàm detect() thử lần lượt từng mức baud (115200, sau đó 230400).
Ở mỗi mức baud, probe_at_baud() đọc các khối 512 byte rời nhau và đếm số gói tin thoả mãn đồng thời cả header lẫn CRC/checksum cho mọi giao thức cùng baud đó.
Giao thức nào đạt 3 gói hợp lệ (PROBE_MIN_HITS = 3) trước sẽ được chọn. Vì LD14 và Camsense X1 cùng chạy ở 115200 baud, cả hai được kiểm tra trong cùng một lượt đọc (~15 ms).
Các điểm đo hợp lệ được bù góc lắp đặt (lid_ofs, lid_flip) và gom vào mảng tĩnh 360 ô (1°/bin) theo cơ chế buffer kép (s_building và s_complete). Khi góc đầu gói mới nhỏ hơn gói trước (vừa quét qua mốc 360° → 0°), s_building được chép sang s_complete trong vùng khoá spinlock vài micro-giây.

5. SLAM chạy trực tiếp trên ESP32 (`slam/`, `mapping`)

ESP32-WROOM-32 chỉ có 520 KB SRAM tổng cộng; sau khi khởi tạo WiFi, TCP/IP, HTTP server và các task FreeRTOS, lượng heap khả dụng còn khoảng 180–220 KB. Toàn bộ thư viện slam/ được viết bằng C thuần, không cấp phát động ngầm, sử dụng đúng 40 KB RAM cho bản đồ.

5.1 Bản đồ Occupancy Grid Log-Odds (`slam_grid.c`)

Bản đồ có kích thước 200 × 200 ô, độ phân giải 5 cm/ô (phủ diện tích 10 × 10 m, robot khởi đầu tại tâm (0, 0)). Mỗi ô là một số nguyên có dấu 1 byte (int8_t) biểu diễn giá trị log-odds trong dải [-100 .. +100]:

= 0: chưa biết (unknown).
< 0: trống (free) — mỗi tia đi qua trừ miss_gain = 3 bằng thuật toán vẽ đoạn thẳng Bresenham.
> 0: có vật cản (occupied) — ô cuối của tia (nếu khoảng cách nằm trong [120 .. 6000] mm) được cộng hit_gain = 12.

Trọng số bất đối xứng (+12 so với -3) giữ cho bức tường thật không bị xoá mất khi có một vài tia quét lướt qua ở góc hẹp.

5.2 Correlative Scan Matching 2 lớp và Cổng chất lượng (`slam_match.c`)

Ở chu kỳ 10 Hz, khi có vòng quét LiDAR mới, mapping.c cộng độ dịch chuyển (Δx, Δy, Δθ) từ odometry vào pose SLAM làm pose dự đoán, sau đó gọi slam_scan_match() để tìm pose khớp với các bức tường đã có trên bản đồ nhất:

Hàm mục tiêu (score_pose): Đổi các tia LiDAR sang toạ độ điểm (px, py) trong hệ quy chiếu robot một lần duy nhất, rồi thử xoay/tịnh tiến theo các ứng viên (x, y, θ) và cộng tổng giá trị log-odds của các ô mà đầu tia rơi vào.
Tìm kiếm 2 lớp (Coarse-to-Fine):
- Lớp thô: Quét trong cửa sổ ±20 cm (bước 2 ô = 10 cm) và ±12° (bước 2°), chỉ lấy mỗi 3 tia 1 lần (coarse_stride = 3).
- Lớp tinh: Quanh nghiệm tốt nhất của lớp thô, quét bước 1 ô (5 cm) và 1° trên toàn bộ số tia hợp lệ.

Hai cơ chế bảo vệ bản đồ khỏi bị phá hỏng trong thực tế:

Cổng chất lượng (min_score_per_beam): Hàm tìm kiếm luôn trả về điểm cực đại trong cửa sổ tìm kiếm, kể cả khi điểm cực đại đó rất thấp hoặc âm (ví dụ robot trượt bánh mạnh hoặc môi trường thay đổi). Firmware chỉ chấp nhận sửa pose nếu điểm trung bình mỗi tia đạt ≥ match_min (mặc định 20 trên thang 100). Nếu tổng điểm âm (score < 0 — vòng quét mâu thuẫn với bản đồ), firmware tạm ngừng bồi vòng quét đó vào bản đồ để không vẽ đè tường giả.
Chống nhoè khi xoay nhanh (map_max_w): Ở ~300–360 vòng/phút, một vòng quét 360° mất ~167 ms. Khi robot xoay với vận tốc góc lớn, các tia trong cùng một vòng quét bị lệch góc đáng kể. Khi |ω| > map_max_w (mặc định 0.70 rad/s), mapping_task vẫn cập nhật định vị nhưng tạm ngừng ghi vào bản đồ.

6. Lập kế hoạch Frontier Exploration và Tự hành (`slam_plan.c`, `navigator.c`)

6.1 Thuật toán tìm đường `slam_plan`

Để giảm bộ nhớ cho hàng đợi BFS, lưới bản đồ 200×200 (5 cm) được hạ mẫu 2 lần xuống lưới lập kế hoạch 100×100 ô (10 cm/ô, workspace tốn 40 KB). Quy trình mỗi lần lập kế hoạch (do_plan) gồm 5 bước:

Đánh dấu Frontier trên bản đồ gốc (trước khi phình): Ô trống (log-odds < -5) nằm sát ít nhất một ô chưa biết được gắn cờ ST_FRONTIER, sau đó lan cờ này ra bán kính radius + 1 ô (spread_frontier). Việc đánh dấu trước khi phình vật cản là bắt buộc: trong phòng kín, ranh giới trống/chưa-biết thường nằm sát mép tường hoặc góc khuất; nếu phình vật cản trước rồi mới tìm frontier thì vùng phình sẽ xoá sạch các frontier ở lối đi hẹp và báo nhầm “đã khám phá xong”.
Phình vật cản nhân tròn + Bậc thang nới (inflate_obstacles): Các ô vật cản (log-odds > 15) được phình bằng nhân hình tròn (dx² + dy² <= r², không dùng nhân vuông vì đường chéo nhân vuông phình dư 41% làm bít lối đi), trừ vùng bảo vệ ngay dưới thân robot. Nếu ở bán kính phình đầy đủ (robot_r = 0.13 m → 2 ô) BFS không tìm được đường ra, thuật toán tự hạ bậc phình xuống 1 ô rồi thử lại, nhưng không bao giờ hạ xuống 0 để tránh dẫn robot quệt sát tường.
Tìm kiếm chiều rộng (BFS): Xuất phát từ vị trí robot, loang qua các ô trống tới ô đi được đầu tiên mang cờ ST_FRONTIER (chế độ tự khám phá) hoặc tới ô đích do người dùng click trên trang /map (slam_plan_to_point).
Kéo thẳng đường đi (Line-of-Sight String Pulling): Đường đi lấy ra từ BFS 4 hướng có dạng bậc thang vuông góc 90°. Nếu bám trực tiếp, robot phải dừng và xoay tại chỗ liên tục, gây đảo chiều bánh và làm lộ điểm mù của encoder 1 kênh. Thuật toán dùng hàm los_clear() (kiểm tra tầm nhìn thẳng bằng Bresenham) để nối thẳng từ điểm hiện tại tới ô xa nhất trên đường đi không vướng vật cản, rút gọn hàng chục nấc bậc thang thành 2–4 đoạn thẳng dài.
Loại bỏ waypoint đã nằm trong tầm tới (NAV_WP_REACHED_M = 0.18 m): Tránh vòng lặp lập kế hoạch tại chỗ khi waypoint đầu tiên nằm quá sát tâm robot.

6.2 Máy trạng thái điều hướng 10 Hz (`navigator.c`)

Task navigator chạy ở chu kỳ 100 ms (10 Hz) với 6 trạng thái:

                ┌──────────────────────────────┐
                ▼                              │ (mỗi 7s hoặc hết waypoint)
            ┌────────┐     có đường      ┌───────────┐
            │  PLAN  ├──────────────────►│  FOLLOW   │
            └───┬────┘                   │ (+ROTATE) │
                │ không có đường         └─────┬─────┘
                ▼                              │ cản trước < nav_stop
            ┌────────┐                   ┌─────▼─────┐
            │ WANDER ├──────────────────►│   AVOID   │
            └────────┘   cản trước       └───────────┘
                ▲
                │ kẹt > 7s (đi < 12cm) & phía sau thoáng > 35cm
            ┌───┴────┐
            │ ESCAPE │ (lùi -0.10 m/s có kiểm tra LiDAR phía sau)
            └────────┘

Ba kỹ thuật điều khiển thực tế trong navigator.c:

Kiểm tra vật cản theo dải chữ nhật đi tới (corridor_mm), không dùng hình quạt: Một hình quạt cố định ±20° ở cự ly 30 cm chỉ rộng ±10.9 cm, hẹp hơn bán kính thân robot (13 cm) nên vật cản ở mép vai xe không lọt vào vùng kiểm tra. Hàm corridor_mm() chiếu từng điểm LiDAR sang toạ độ (x, y) của robot và kiểm tra dải hình chữ nhật có bề rộng bằng robot_r + 3 cm dọc theo chiều tiến (hoặc chiều lùi khi ESCAPE).
Chống dao động hướng trong FOLLOW / ROTATE: Khi lệch hướng > 0.9 rad (~52°), robot vào pha ROTATE, chốt cứng chiều xoay s_rot_dir ngay lúc vào (không đổi chiều xoay giữa chừng dù pose SLAM nhảy nhẹ), giảm tốc độ xoay tỉ lệ với sai số góc (turn_rate) và sau khi thoát ROTATE phải nghỉ ít nhất 600 ms mới được vào lại. Khi đang tiến bám waypoint, |ω| bị kẹp ở 0.8 rad/s với v >= 0.08 m/s để hai bánh luôn quay tiến, không đảo chiều.
Phát hiện kẹt theo quãng đường thực (update_stuck) và thoát kẹt bằng lùi (ST_ESCAPE): Thay vì đếm số lần lập kế hoạch lỗi, firmware kiểm tra robot có thực sự dịch chuyển được ít nhất 12 cm trong cửa sổ 7 giây hay không. Nếu không nhích được và dải hành lang phía sau thoáng > 35 cm, robot chuyển sang ST_ESCAPE, lùi thẳng ở -0.10 m/s cho tới khi lùi được 12 cm, phía sau còn < 20 cm, hoặc hết 3 giây.

7. Giao diện Web, Captive Portal, Tham số NVS và Luồng Trace

7.1 Captive Portal 3 lớp (`captive`)

Khi nối điện thoại/máy tính vào WiFi SoftAP AUTONOMY_ROBOT (mật khẩu robot1234), trang điều khiển tự động bật lên nhờ 3 lớp phối hợp trong captive.c và captive_dns.c:

DHCP Option 114 (RFC 8910): Gửi trực tiếp URI http://192.168.4.1 trong gói DHCP cho Android 11+ và iOS 14+.
DNS Hijack có lọc subnet: Trả mọi truy vấn tên miền về 192.168.4.1, nhưng chỉ trả lời các gói đến từ subnet của SoftAP (192.168.4.x) để không làm hỏng DNS mạng LAN khi robot đồng thời nối vào router nhà ở chế độ APSTA.
HTTP 302 Redirect trên handler 404: Bắt toàn bộ URL dò kết nối của hệ điều hành (/generate_204, /hotspot-detect.html, /connecttest.txt) và chuyển hướng kèm header Cache-Control: no-store.

7.2 Sáu trang điều khiển nhúng trong Flash (`webui/` & `web_ctrl`)

Toàn bộ giao diện HTML/CSS/JS nằm trong webui/ và được nhúng thẳng vào phân vùng flash qua EMBED_TXTFILES:

Đường dẫn	Chức năng chính
`/`	Joystick ảo gửi `cmd_vel` qua WebSocket (10 Hz), nút E-STOP, chuyển `MANUAL` ⇄ `AUTO`, bản đồ thu nhỏ, hiệu chuẩn tự động và bảng chỉnh 18 tham số NVS
`/map`	Bản đồ Occupancy Grid thời gian thực toàn màn hình, hiển thị quỹ đạo/frontier, click vào điểm bất kỳ trên bản đồ để robot tự hành tới đó (`/map/goto`)
`/lidar`	Màn hình radar 360°, hiển thị model LiDAR tự nhận diện, RPM, số điểm hợp lệ, lỗi CRC, nút chỉnh bù góc lắp `±1°` / `±90°` và đảo chiều
`/ros2`	Bật/tắt cầu nối UDP sang ROS2 (lưu NVS), hiển thị IP máy tính đang kết nối và bộ đếm gói `tx_scan`, `tx_odom`, `rx_cmd`
`/wifi`	Quét WiFi xung quanh và kết nối vào router gia đình (chế độ `APSTA` — giữ nguyên AP dự phòng)
`/update`	Nạp firmware OTA qua trình duyệt (tự động khoá robot vào `ESTOP` trước khi ghi flash)

7.3 Bảng 18 tham số Runtime lưu NVS (`params`)

Mọi tham số trong robot_params.c đều chỉnh được trực tiếp từ mục Tham số trên trang chủ và có hiệu lực ngay lập tức mà không cần biên dịch lại:

Key	Ý nghĩa	Mặc định	Dải hợp lệ
`wheel_r`	Bán kính bánh xe (mm)	`30.0`	`10 .. 100`
`wheel_b`	Khoảng cách hiệu dụng 2 bánh (mm)	`109.35`	`50 .. 300`
`enc_ppr`	Xung encoder / vòng trục motor	`20`	`1 .. 2000`
`max_lin`	Vận tốc dài tối đa (m/s)	`0.50`	`0.1 .. 2.0`
`max_ang`	Vận tốc góc tối đa (rad/s)	`3.00`	`0.5 .. 10.0`
`min_duty`	Duty tối thiểu bù ma sát tĩnh	`0.28`	`0.0 .. 0.6`
`use_pid`	Bật vòng điều khiển PI vận tốc bánh (0/1)	`1`	`0 .. 1`
`pid_kp` / `pid_ki`	Hệ số tỉ lệ và tích phân của vòng PI	`0.8` / `2.0`	`0 .. 10` / `0 .. 20`
`lid_ofs` / `lid_flip`	Bù góc lắp LiDAR (độ) và đảo chiều góc (0/1)	`270` / `0`	`0 .. 359` / `0 .. 1`
`scanmatch`	Bật Correlative Scan Matching (0/1)	`1`	`0 .. 1`
`match_min`	Điểm khớp tối thiểu mỗi tia để nhận sửa pose	`20`	`0 .. 100`
`nav_speed`	Tốc độ hành trình khi tự hành (m/s)	`0.22`	`0.05 .. 0.5`
`nav_stop`	Khoảng cách vật cản phía trước thì dừng/né (m)	`0.35`	`0.15 .. 1.5`
`turn_spd`	Tốc độ xoay tại chỗ khi tự hành (rad/s)	`0.60`	`0.2 .. 2.0`
`map_max_w`	Ngưỡng `ω` tạm ngừng bồi bản đồ (rad/s)	`0.70`	`0.1 .. 5.0`
`robot_r`	Bán kính phình vật cản khi lập kế hoạch (m)	`0.13`	`0.05 .. 0.40`

7.4 Luồng Trace chẩn đoán nhị phân qua TCP (`trace`)

Để chẩn đoán lỗi thời gian thực giữa các tầng mà không tốn CPU gọi snprintf trong vòng lặp điều khiển, component trace phát các bản ghi nhị phân định điểm (~20 byte/bản ghi) qua cổng TCP 3333 trên 7 kênh: NAV, PLAN, SLAM, MOT, ENC, LIDAR, LOG. Khi không có máy tính kết nối, biến g_trace_mask = 0 và mọi macro TRACE() rút gọn còn một phép kiểm tra biến nguyên.

Trên máy tính, dùng công cụ tools/trace_decode.py (chỉ dùng thư viện chuẩn Python):

# Chế độ --live: chỉ in khi đổi trạng thái, lập kế hoạch lỗi, encoder bị ép đảo chiều,
# scan matching nhảy pose hoặc lỗi CRC — đồng thời lưu đầy đủ 100% bản ghi vào file:
python3 tools/trace_decode.py 192.168.4.1 --live -o phien_chay.log

8. Cầu nối UDP sang ROS2 (`ros2_bridge` & `ros2_ws`)

8.1 Lý do dùng cầu UDP định điểm thay vì micro-ROS trên WROOM

Trên ESP32-WROOM không có PSRAM, việc nhúng стек Micro-XRCE-DDS của micro-ROS tiêu tốn thêm 50–80 KB heap và kéo colcon vào luồng build firmware. Thay vào đó, component ros2_bridge đóng gói dữ liệu cảm biến thành các struct C định điểm (__attribute__((packed)), little-endian, chốt kích thước bằng _Static_assert) và bắn qua UDP tốn chưa tới 3 KB RAM. Ở phía máy tính, node Python bridge_node.py dịch sang các topic ROS2 chuẩn:

Header chung (16 byte): magic 0x52 ('R') | version 1 | type | _ | len | _ | seq | t_ms
  type 1 SCAN  (Robot → PC, UDP 8888): rpm(u16) + bins(u16) + dist_mm[360](u16) = 724 byte
  type 2 ODOM  (Robot → PC, UDP 8888): x_mm, y_mm(i32) + theta_mrad, v_mms, w_mrads(i16)
                                       + enc_l, enc_r(i32) + mode, lidar_ok(u8) =  28 byte
  type 3 CMD   (PC → Robot, UDP 8889): v_mms(i16) + w_mrads(i16)                =   4 byte
  type 4 HELLO (PC → Robot, UDP 8889): gói rỗng giữ nhịp 2 Hz

Ba chi tiết thiết kế quan trọng của cầu ROS2:

Gửi Odometry thô (Raw Wheel Odometry): Gói ODOM gửi pose tích phân từ bánh xe (odometry_get_pose), không gửi pose đã qua scan matching trên ESP32. Theo chuẩn REP-105 của ROS, topic /odom phải liên tục và phản ánh odometry thô, còn phép sửa trôi là nhiệm vụ của slam_toolbox thông qua phép biến đổi TF map → odom.
Tự học địa chỉ IP máy tính & Khoá an toàn 2 giây: Không cần khai báo IP máy tính trong firmware. Node trên PC gửi gói HELLO ở tần số 2 Hz tới cổng 8889 của robot; robot ghi nhận địa chỉ nguồn đó để đẩy SCAN và ODOM về cổng 8888. Nếu mất gói HELLO quá 2 giây (tắt node, rớt WiFi), robot tự dừng động cơ.
Xử lý tia 0 mm: Khoảng cách 0 mm (không có phản xạ) được bridge_node.py dịch thành float('inf') trong sensor_msgs/LaserScan. Nếu dịch thành 0.0, slam_toolbox sẽ vẽ một bức tường tròn bao quanh robot.

9. Hướng dẫn Build, Nạp và Vận hành từng bước

Bước 1 — Tải mã nguồn và nạp chương trình chẩn đoán bánh xe (`wheel_debug`)

Khi mới lắp phần cứng lần đầu, nạp công cụ chẩn đoán độc lập wheel_debug để kiểm tra chân STBY, mức logic encoder và chiều quay từng bánh trước khi nạp firmware chính:

# 1. Kích hoạt môi trường ESP-IDF (v5.x / v6.x)
source ~/esp/esp-idf/export.sh

# 2. Clone repository
git clone https://github.com/dungtsbb/autonomy_robot.git
cd autonomy_robot

# 3. Nạp công cụ chẩn đoán phần cứng bánh xe (tuỳ chọn khi mới lắp mạch)
cd wheel_debug
idf.py set-target esp32
idf.py -p /dev/ttyUSB0 flash monitor

Bước 2 — Build và nạp Firmware chính

cd ../firmware
idf.py set-target esp32
idf.py build
idf.py -p /dev/ttyUSB0 flash monitor

Khi khởi động thành công, Serial Monitor in thứ tự khởi tạo các component:

I (312) main: === AUTONOMY ROBOT — khởi động ===
I (325) params: 18 tham số sẵn sàng (bánh R=30.00mm, base=109.35mm, PID=bật)
I (338) encoder: init xong: L=GPIO18 R=GPIO19, pull-up nội: bật, 0.2012 mm/xung
I (346) odometry: init xong (chu kỳ 20ms, wheel base 109.35mm)
I (354) motion: init xong (PID: bật — đổi runtime qua tham số use_pid, timeout 500ms)
I (365) lidar: init: UART2 RX=GPIO16, đang dò loại lidar (3 giao thức)...
I (382) mapping: init: 200x200 ô @ 50mm = 10.0x10.0m (40 KB RAM), scan match: bật
I (391) navigator: init xong: frontier exploration, lưới kế hoạch 100x100 (40KB)
I (540) captive: Captive portal bật: nối WiFi robot là trang tự mở (http://192.168.4.1)
I (552) main: Sẵn sàng. Mode: MANUAL

Bước 3 — Hiệu chuẩn LiDAR và chạy SLAM trên ESP32

Kết nối điện thoại hoặc máy tính vào WiFi AUTONOMY_ROBOT (mật khẩu robot1234). Trang điều khiển tự mở tại http://192.168.4.1.
Mở trang /lidar: kiểm tra dòng trạng thái trên cùng đã hiện đúng tên model LiDAR (LD14, LD06/LD19 hoặc Camsense X1), tốc độ quay ~300–360 RPM và số gói lỗi CRC không tăng. Dùng các nút ±90° / ±1° và Đảo chiều sao cho vật cản đặt trước mũi robot hiển thị đúng ở phía trên màn hình radar.
Quay lại trang chủ / hoặc /map, đặt robot xuống sàn và chuyển chế độ sang AUTO: robot bắt đầu quét bản đồ, tìm các đường biên frontier và tự lái khám phá phòng. Có thể click trực tiếp lên một điểm trên trang /map để ra lệnh cho robot tự tìm đường chạy tới điểm đó.

Bước 4 — Chuyển SLAM sang máy tính chạy ROS2

Vào http://192.168.4.1/wifi để nối robot vào mạng WiFi gia đình (ghi nhớ địa chỉ IP STA của robot, ví dụ 192.168.1.42).
Vào http://<ip-robot>/ros2 và bấm Bật cầu ROS2 (navigator và mapping trên ESP32 sẽ tự động tạm nghỉ).
Kiểm tra luồng UDP từ máy tính bằng script chuẩn (không cần cài ROS2):

python3 tools/ros2_bridge_test.py 192.168.1.42
# Kết quả mẫu:
# scan  142 | 300rpm 328/360 tia 0.12-6.44m | odom  467 | (+1.24,-0.31) +47.2° ... | 6.4 scan/s

Biên dịch và khởi chạy workspace ROS2 (gồm bridge_node, slam_toolbox và RViz):

cd ros2_ws
colcon build --symlink-install
source install/setup.bash
ros2 launch autonomy_robot_bringup bringup.launch.py robot_ip:=192.168.1.42

Để điều khiển robot từ bàn phím máy tính hoặc từ Nav2, chuyển robot sang chế độ AUTO trên giao diện web (ở chế độ MANUAL, lệnh từ máy tính bị chặn để ưu tiên người cầm joystick), sau đó chạy:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

10. Kiểm thử đơn vị trên máy tính (Host Tests)

Do tầng giao thức LiDAR và thư viện slam/ được viết bằng C thuần không phụ thuộc ESP-IDF, bạn có thể biên dịch bằng gcc và chạy bộ kiểm thử hồi quy ngay trên máy tính trước khi nạp xuống mạch:

# Kiểm thử 44 ca giao thức LiDAR (LD14, LD06, Camsense X1 + chống nhận diện nhầm chéo)
gcc -I firmware/components/lidar/include -I firmware/components/lidar \
    tools/test_lidar_proto.c \
    firmware/components/lidar/lidar_proto.c \
    firmware/components/lidar/lidar_ld14.c \
    firmware/components/lidar/lidar_camsense.c \
    -o /tmp/test_lidar && /tmp/test_lidar

# Kiểm thử thuật toán phình nhân tròn, bậc thang nới, tách frontier và kéo thẳng đường đi
gcc -I slam/include slam/src/*.c tools/test_slam_plan.c -lm \
    -o /tmp/test_slam && /tmp/test_slam

11. Xử lý sự cố

Hiện tượng	Nguyên nhân & Cách khắc phục
ESP32 không boot được khi cắm vào mạch	Chân `GPIO 12` (`STBY`) là strapping pin MTDI — gỡ điện trở kéo lên 3.3V ở chân này (chỉ nối thẳng GPIO 12 vào STBY)
Robot tự khởi động lại khi đảo chiều nhanh, UI báo `rst: SỤT ÁP`	Xung dòng động cơ làm sụt áp đường nguồn — hàn tụ hoá 100–470 µF song song tụ gốm 0.1 µF sát chân `VM` – `GND` của TB6612FNG; tách riêng đường nguồn động cơ và buck 5V ngay từ pin
Động cơ quay nhưng encoder đếm loạn hoặc đứng im	Thiếu mass chung giữa nguồn động cơ, encoder và ESP32; hoặc tín hiệu encoder 5V chưa qua cầu phân áp 10 kΩ / 20 kΩ
Trang `/lidar` báo `chưa nhận diện` hoặc lỗi CRC tăng liên tục	Kiểm tra dây `TX` của LiDAR đã nối vào `GPIO 16`; cấp nguồn 5V đủ dòng qua mạch buck (không lấy 5V từ cổng USB khi chạy pin)
Đổi từ LD14 sang Camsense X1 thì bản đồ SLAM bị xoay lệch hoặc lộn ngược	Hai cảm biến có mốc 0° và chiều quét khác nhau — vào `/lidar` chỉnh lại `lid_ofs` và nút Đảo chiều
Thêm endpoint HTTP mới làm ESP32 reboot liên tục (boot loop)	Vượt giới hạn `config.max_uri_handlers` của `esp_http_server` (đã nâng lên `40` trong `web_ctrl.c`)
`ros2_bridge_test.py` không nhận được gói nào	Chưa bật cầu tại trang `/ros2`, hoặc tường lửa máy tính chặn cổng UDP `8888`
Chạy `teleop_twist_keyboard` trên ROS2 nhưng bánh không quay	Robot đang ở chế độ `MANUAL` hoặc `ESTOP` — chuyển sang `AUTO` trên trang điều khiển để cho phép nguồn lệnh `CMD_SOURCE_EXTERNAL`

Cấu trúc thư mục dự án

autonomy_robot/
├── firmware/
│   ├── main/app_main.c           — Điểm vào, khởi tạo và ghép các component
│   └── components/               — 18 module độc lập (bsp, motor_driver, encoder,
│                                   odometry, motion, lidar, mapping, navigator,
│                                   web_ctrl, captive, params, ros2_bridge, trace, ota...)
├── slam/                         — Thư viện SLAM thuần C (slam_grid, slam_match, slam_plan)
├── webui/                        — Giao diện HTML5/Canvas nhúng thẳng vào flash
├── ros2_ws/                      — Package ROS2 autonomy_robot_bringup + launch slam_toolbox
├── tools/                        — trace_decode.py, ros2_bridge_test.py, bộ test C trên PC
├── wheel_debug/                  — Chương trình chẩn đoán phần cứng 2 bánh qua UART
├── hardware/pinout.md            — Bảng đấu nối chân chi tiết
└── docs/                         — Tài liệu kiến trúc (ARCHITECTURE, ROADMAP, TRACE)

Toàn bộ mã nguồn dự án: github.com/dungtsbb/autonomy_robot.