车道保持与路口信号通行

01本节目标

• 讲清巡线控制闭环：视觉算偏差 e(t) → PID 出角速度 → /cmd_vel → 底盘执行 → 相机再观测，理解"反馈环画成圈"。
• 完成车道中心线检测节点（ROI 内高对比识别线质心提取），并用 PID 把直道偏差收敛到 ±2cm。
• 完成停止线检测：路口前 20cm 触发减速，结合 EXP-02 的 /traffic/light 实现红灯停、绿灯行、黄灯按距决策。
• 完成斑马线礼让：检测到行人模型（EXP-02 标志/颜色通道复用）30cm 内必停，行人离开 2s 后恢复。
• 实现 IDLE / CROSSING / WAITING 优先级状态机，讲清优先级排序：行人 > 红灯 > 巡线。
• 牢记：低速起步、提前减速——巡线速度超过 0.25m/s，40cm 窄道必出轨。

02核心原理：偏差进、角速度出——PID 与优先级状态机的双人舞

把小车想像成蒙眼走钢丝的人（只不过"眼"是下视 ROI）：它看不见整个世界，只盯着脚下一小段车道线。相机在图像下方取一条 ROI 横带，对高对比识别线做质心提取——车道中心在画面 x = W/2 时偏差 e=0；偏左 e<0、偏右 e>0。接下来是老朋友 PID：P 项按偏差成比例打方向（纠得急），I 项积分消除"轮子左右不对称"造成的恒定跑偏，D 项对偏差变化率提前刹车、抑制过冲震荡。三者相乘相加，得到角速度 ω，连同限速后的线速度 v 一起塞进 /cmd_vel——差速/麦克纳姆轮据此两侧差速，车头回正，完成一圈闭环。

光会沿线还不够，路口要"懂规矩"。优先级状态机把三种约束排出座次：行人 > 红灯 > 巡线。IDLE（默认巡线）、WAITING（红灯/行人导致的等待）、CROSSING（绿灯通行中）。任何时刻多个条件同时成立，高优先级赢——斑马线上有人，哪怕绿灯也得让；红灯变绿的瞬间，还得确认停止线前没有行人残余，才从 WAITING 迁移到 CROSSING。状态迁移要带滞回：判定条件持续 200ms 才迁，避免灯缘抖动让车在路口"点头"。

03硬件与场景

04操作步骤

1. PID 整定顺序（经典 Z-N 简化法）：Ki=Kd=0，Kp 从 0.4 起加倍到小车开始蛇形振荡，回退取 2/3 作为 Kp；加 Kd=0.15 起步压振荡；最后 Ki=0.02 消掉轮差导致的恒偏——一次只动一个参数，每轮跑 5m 直道记偏差曲线。
2. 预见减速：停止线检测到时距 0.2m 开始斜坡降速，而不是到线急刹——"提前减速"写进代码而不是靠反应。
3. 灯态联动：红灯或黄灯且距线 <0.15m 时 v 斜坡归零进入 WAITING；/traffic/light 消失超过 1s 视为故障，按红灯处理（fail-safe 停车而非抢行）。
4. 礼让逻辑：行人模型色块进入斑马线 ROI 即触发（与灯态无关，优先级最高）；离开后连续 2s 无目标才恢复巡线，防止行人模型晃动反复触发。
5. 状态机联调：打印每次迁移的时间戳与理由（trigger），录 ros2 bag record -a，回放核对：全圈 4 路口 + 1 斑马线迁移序列与预期一致。
6. 留痕：直道偏差曲线截图、红灯/礼让 10/10 计数表、状态迁移日志，构成验收三件套。

05关键命令

/cmd_vel、teleop、rosbag 用法沿用《培训方案》xlsx 风格；灯态话题来自 EXP-02。

# ① 起相机（EXP-02 驱动复用）+ 灯态识别节点
ros2 launch ascamera hp60c.launch.py
python3 src/light_node.py --config config/hsv_config.yaml

# ② 启动巡线 + 路口状态机节点（参数包 lane_follow.yaml）
ros2 run car1_control lane_follow_node.py --ros-args \
  --params-file src/car1_control/config/lane_follow.yaml
ros2 topic echo /cmd_vel                       # 看 v/ω 是否平滑
ros2 topic echo /traffic/light                 # R/Y/G 联动输入

# ③ 手动微调 PID（运行中热改，改完立刻跑 5m 直道）
ros2 param set /lane_follow_node kp 0.60
ros2 param set /lane_follow_node kd 0.15
ros2 param set /lane_follow_node ki 0.02

# ④ 无灯时手动注入灯态，联调状态机
ros2 topic pub -r 1 /traffic/light std_msgs/String "{data: 'R'}"

# ⑤ 联调录包回放：迁移日志 + TF + 偏差话题全录
ros2 bag record -a -o lane_interface_bag
ros2 bag play lane_interface_bag
ros2 run tf2_tools view_frames                 # out/frames.pdf 查 TF 完整性

06原理动画：巡线 PID 反馈环与信号灯时序通行

07互动实验：巡线 PID 参数滑杆实时仿真

快速实验：① Kp 拉到 3.0、Kd=0 → 曲线持续振荡（蛇形）；② Kd 拉到 2.0 → 过冲被压平但响应变肉；③ Ki 拉大且 Kp 也大 → 积分饱和，超调久收不回。把三参数调到"两个周期内进带且不越界"，就是实车直道 ≤±2cm 的前置条件。

08常见问题

09拓展阅读

• 《机器人学中的状态估计》第 9 章 PID：从误差动力学看 Kp/Ki/Kd——把互动面板的曲线与书中阶跃响应图对读。
• Nav2 controller 参数对照：DWB 的评分权重与本课 PID 是同一思想在 2D 速度采样域的展开，EXP-04 泊车会回到这个对照。
• 行为树 vs 有限状态机：本课用三级优先级 FSM 够用；当规则再涨一倍（让行+限速+改道+警报），业界会迁移到 BehaviorTree.CPP——留作课程后续。
• rosbag 分析工作流：用 ros2 bag info + rqt_plot 画 e(t) 曲线交作业，比截图更有说服力。
• 上下文：EXP-02 的 /traffic/* 话题是本实验状态机的感知输入，联调异常先回查话题频率。

10思考题 · 自测