craic/docs/localization.md

CRAIC 定位系统文档

激光 SLAM 建图 + AMCL 精准定位 + 示教记录点位。移植自 lzu_robot / move_try （同一赛事的参考实现），适配 CRAIC 麦轮底盘 + ESP32 + 机械臂平台。

---

1. 概述

• 目标：在已知（预先测量）的竞赛场地上对底盘做绝对、无累积漂移的精准定位，并提供示教记录关键点位的工具。
• 方案：YDLiDAR → rf2o 激光里程计提供连续 odom；slam_gmapping 建一次场地地图；nav2 AMCL 用激光匹配已知地图做全局校正。
• 运行环境：ROS 2 Humble（conda/robostack ros2_humble）。机器人端工作区在 ~/Desktop/ros2（下文命令默认在此目录、且已 source install/setup.bash）。

设计上原打算用轮式里程计做运动来源，但实测 CRAIC 底盘板上电后只短时上报里程计（见 §8）， 故改用 rf2o（激光里程计）提供 odom→base，与 lzu_robot/move_try 一致。轮式里程计节点保留， 待底盘板问题解决后可用 EKF 融合提精度。

---

2. 系统架构

TF 树

map ──(amcl，激光匹配已知地图)──> odom
odom ──(rf2o，来自连续 /scan)──────> base_footprint
base_footprint ──(静态外参)────────> laser_frame      # 激光安装位，lidar_yaw 等参数
base_footprint ──(静态恒等)────────> base_link        # 兼容 gmapping/Nav2 默认基准帧

数据流

YDLiDAR ──/dev/ttylzulaser──> ydlidar 驱动 ──> /scan(10Hz)
/scan ──> rf2o ──> /odom + TF(odom→base_footprint)
/scan + odom ──> slam_gmapping ──> /map + TF(map→odom)        # 建图阶段
/scan + 已知地图 ──> amcl ──> TF(map→odom) = 绝对位姿            # 定位阶段
底盘速度命令: keyboard_control ──UDP──> ESP32 ──UART──> 底盘板   # 与定位互不干扰
轮式里程计(可选): 底盘板 ──/dev/ttylzucar──> chassis_odometry ──> /odom(独立测试用)

---

3. 软件清单

自建包 ros2/src/craic_localization

文件	作用
craic_localization/chassis_odometry.py	轮式里程计节点（读 /dev/ttylzucar，发 /odom+TF）。默认不参与建图/定位，独立测试用
craic_localization/teach_points.py	示教节点：记录 map 系下位姿存 YAML
craic_localization/navigate_to_point.py	导航节点：自动驱动到示教点（读 taught_points.yaml + AMCL 闭环 + UDP 发 XYW）
craic_localization/show_points.py	在 rviz 显示所有示教点位（MarkerArray 箭头+文字标签）
launch/lidar.launch.py	YDLiDAR 驱动 + 静态外参(base_footprint→laser_frame、→base_link)
launch/bringup.launch.py	轮式里程计 + 激光 + rviz（传感器自检）
launch/mapping.launch.py	激光 + rf2o + slam_gmapping（建图）
launch/localization.launch.py	激光 + rf2o + map_server + amcl + lifecycle_manager（AMCL 定位）
config/lidar.yaml	YDLiDAR TminiPro 参数（端口 /dev/ttylzulaser）
config/gmapping.yaml	gmapping 建图参数（/** 通配键，base_footprint/odom/map）
config/amcl.yaml	map_server + amcl 参数（omni 运动模型）
rviz/localization.rviz	rviz 配置（TF/Map/LaserScan/Odometry）
maps/	建图存盘输出（craic.pgm + craic.yaml）
config/taught_points.yaml	示教输出（运行时生成）

移植包

• ros2/src/ydlidar_ros2_driver —— 来自 move_try，依赖系统 ydlidar_sdk（已装在 conda env）。
• ros2/src/rf2o_laser_odometry —— 来自 lzu_robot，激光里程计（ICRA'16）。

外部依赖（Nav2，已装入 conda env）

nav2_amcl(amcl)、nav2_map_server(map_server, map_saver_cli)、nav2_lifecycle_manager(lifecycle_manager)、slam_gmapping/openslam_gmapping。

---

4. 环境准备与编译

一次性安装 Nav2 定位组件（若环境无）：

conda install -n ros2_humble -c robostack-staging -c conda-forge \
    ros-humble-nav2-amcl ros-humble-nav2-map-server ros-humble-nav2-lifecycle-manager

编译（在工作区根目录）：

conda activate ros2_humble
cd ~/Desktop/ros2
colcon build --symlink-install --packages-select \
    ydlidar_ros2_driver rf2o_laser_odometry craic_localization
source install/setup.bash

开发机 ↔ 机器人同步：代码在开发机维护，需同步到机器人 ~/Desktop/ros2 后重新 colcon build。 新增/改动包时务必确认源码已同步（尤其新包 ydlidar_ros2_driver、rf2o_laser_odometry）。

---

5. 硬件接口

设备	接口	说明
底盘 MCU	/dev/ttylzucar @115200	上报 32 字节里程计包；命令通过 ESP32 转发，非直连
激光雷达	/dev/ttylzulaser @230400	YDLiDAR TminiPro，发 /scan 10Hz
底盘命令	UDP 192.168.4.1:8888 → ESP32 → UART	XYW:<x>:<y>:<w>:XZHY（麦轮速度）

底盘里程计协议（移植自 lzu seriallzucar）：32 字节，帧尾 'L''Z''U'；packet[0]=校验和=sum(packet[1:30])%256； packet[1:29]=<fffffff>=x,y,yaw,4轮位置(x/y 单位 mm)；packet[29]=模式字节，'D'=有效。 打开串口后下发 UP0LZU 使能上报（参数 enable_cmd）。

---

6. 使用流程

前置：每个终端都先 cd ~/Desktop/ros2 && source install/setup.bash。

6.1 传感器自检（可选）

ros2 launch craic_localization bringup.launch.py

rviz 看 /scan、TF。用于核对激光朝向（调 lidar_yaw 等，见 §7）。

6.2 建图

# 终端1：建图栈（激光 + rf2o + gmapping + rviz）
ros2 launch craic_localization mapping.launch.py
# 终端2：遥控慢速走遍场地（见 6.6）
# 终端3：地图满意后存盘（gmapping 保持运行）
mkdir -p src/craic_localization/maps
ros2 run nav2_map_server map_saver_cli \
    -f src/craic_localization/maps/craic \
    --ros-args -p save_map_timeout:=10000.0

走图要点：慢、贴墙走遍每条边界、最后绕回起点；rviz Fixed Frame 设 map，墙应是单条清晰线、无重影。 起点即原点：建图启动时机器人所在位置=map(0,0)、正前方=map +X。建议从固定起点开始。

存盘得到 craic.pgm + craic.yaml。确认 craic.yaml 含 free_thresh（缺则 map_server 加载失败）：

tail -1 src/craic_localization/maps/craic.yaml   # 应为 free_thresh: 0.25
# 若缺： printf '\nfree_thresh: 0.25\n' >> src/craic_localization/maps/craic.yaml

存盘后重编一次让默认地图安装：colcon build --symlink-install --packages-select craic_localization。

地图四周大片灰色是 gmapping 预留画布（xmin/xmax ±10 → 20m×20m），属正常，不影响 AMCL。

6.3 定位（AMCL）

ros2 launch craic_localization localization.launch.py \
    map:=$HOME/Desktop/ros2/src/craic_localization/maps/craic.yaml
# 起点不在地图原点时给初值：加 init_x:= init_y:= init_yaw:=，或 rviz 工具栏 "2D Pose Estimate" 点

正常表现：rviz(Fixed Frame=map) 红色激光点贴合墙体，移动时始终咬住墙、位姿稳定不漂。

6.4 定位精度测试

ros2 run tf2_ros tf2_echo map base_footprint    # 实时读 x/y/yaw
ros2 topic echo /amcl_pose                       # 位姿 + 协方差

• 重复定位：标记一物理点记位姿 → 开一圈回同点再记 → 差值 < 3cm 为佳。
• 定长移动：精确前进 1.00m，AMCL 报的位移误差应在几 cm 内。
• (可选) rviz Add → PoseArray → /particlecloud，粒子收紧=收敛。

6.5 示教记录点位

# 终端1：定位栈(6.3) 终端2：遥控(6.6) 终端3：示教
ros2 run craic_localization teach_points --ros-args -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml

交互命令：回车/r 记录当前预设点并前进；p 查看当前位姿；name <X> 记自定义点； del <X> 删除；list 列出；skip/back 跳过/回退；save 存盘；q 退出。 预设顺序 B_1..B_6, C_1, D_1, E_1, F_1（每记一点自动存盘）。记 A 区用 name A_1…，或改 points 参数。 输出（map 系，米/弧度）：

points:
  B_1: {x: 1.234, y: 0.456, yaw: 1.571}
  ...

在 rviz 看示教点：localization.launch.py 已自动启动 show_points（青色箭头=位置+朝向，白字=点名，话题 /taught_points，rviz 内置 MarkerArray 显示）。 想在示教过程中实时看到新点，给定位 launch 传 points_file:=<你的 taught_points.yaml 绝对路径>，或单独运行：

ros2 run craic_localization show_points --ros-args \
    -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml

6.6 自动导航到示教点

读取 taught_points.yaml，用 AMCL 闭环把底盘开到指定点（麦轮 holonomic P 控制，经 UDP 发 XYW）。 首次务必先 dry_run 验证方向，再低速实测，手放 Ctrl+C（退出自动停车）。 前置：定位栈(6.3) 在运行。

# ① 干跑：只打印命令不发，确认逻辑与方向符号
ros2 run craic_localization navigate_to_point --ros-args \
    -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml \
    -p dry_run:=true -p goal:=B_1
# ② 交互选点（低速实测）
ros2 run craic_localization navigate_to_point --ros-args \
    -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml \
    -p max_linear:=40.0 -p max_angular:=20.0
# ③ 一次性去某点 / 供上层任务程序用话题指定
ros2 run craic_localization navigate_to_point --ros-args -p points_file:=... -p goal:=C_1
ros2 topic pub -1 /goto std_msgs/String "{data: 'C_1'}"

交互命令：输入点名→导航；s 停车；q 退出。到达 pos_tolerance(5cm)/yaw_tolerance(~3°) 容差内停车。 若实车方向相反：翻转 sign_x/sign_y/sign_w（默认 -1，按 keyboard_control 推导）。如左右反了 -p sign_x:=1。

6.7 遥控

ros2 run udp_teleop keyboard_control --ros-args \
    --params-file src/udp_teleop/config/params.yaml \
    -p chassis_linear_speed:=40 -p chassis_angular_speed:=20    # 建图建议降速

按键：W/S 前后、A/D 左右平移、Q/E 左右转；Ctrl+C 退出。须 ros2 run、交互终端。

---

7. 配置 / 关键参数

lidar.launch.py：lidar_x/y/z、lidar_yaw(默认 -3.14159=-180°，使机体 X 轴对准底盘实际前进方向；若前后颠倒改 0.0)、 intensity(true；基础 Tmini 无强度时改 false)、sample_rate(4)、baudrate(230400)、base_frame、laser_frame。

localization.launch.py：map(地图 yaml)、init_x/init_y/init_yaw(初始位姿)、use_rviz。

mapping.launch.py：use_rviz。范围 xmin/xmax/ymin/ymax(±10) 在 config/gmapping.yaml，可收小到 ±3 得更紧凑地图（需重新建图）。

amcl.yaml：odom_model_type: omni(麦轮)、base_frame_id: base_footprint、laser_max_range: 12.0、alpha1..5(里程计噪声)。

chassis_odometry（独立测试）：port、enable_cmd(UP0LZU)、enable_cmd_period(0=仅打开时发一次)、require_mode_d(true)。

navigate_to_point：points_file、goal(一次性目标)、max_linear(60)/max_angular(30)、kp_linear(150)/kp_angular(40)、pos_tolerance(0.05)/yaw_tolerance(0.05)、sign_x/sign_y/sign_w(默认 -1，方向修正)、goal_timeout(30s)、dry_run、udp_ip/udp_port。

---

8. 故障排查

现象	原因	处理
激光狂刷 Check Sum X != Y	有第二个进程在读 /dev/ttylzulaser，字节流被撕裂	pgrep -af ydlidar；pkill -f ydlidar_ros2_driver_node；保证单实例。若仍报：试 intensity:=false（Tmini 无强度）
gmapping "base_link" ... does not exist	gmapping 硬编码基准帧 base_link，而 TF 树只有 base_footprint	已修：lidar.launch.py 发 base_footprint→base_link 恒等 TF
gmapping Message Filter ... queue is full、地图/位置不动	odom→base TF 冻结：轮式里程计板上电后只短时上报	已修：改用 rf2o 从 /scan 连续提供 odom→base（见 §1）
map_server 加载地图失败	craic.yaml 缺 free_thresh	补 free_thresh: 0.25（见 6.2）
rviz 地图上一串绿箭头	Odometry 显示保留历史(Keep)	已改默认 Keep:1；或 rviz 里取消勾选 Odometry（不影响地图）
轮式里程计只在上电后几帧有数据	底盘板上电后停止上报（固有行为，UP0LZU 未能维持）	建图/定位已不依赖它；如需启用试 enable_cmd_period:=1.0 保活，或确认主机串口 TX 已接、或经 ESP32 通道使能
时间戳显示 ~2000 年	机器人系统时钟未对时	单机不影响；多机协同前用 NTP/RTC 对时
导航时往错误方向开 / 原地打转	XYW 速度符号与实车不一致	翻转 sign_x/sign_y/sign_w(默认 -1)；先 dry_run:=true 核对，再低速实测
rviz 里机体朝向/前进方向与遥控差 90°	激光外参 lidar_yaw 未校准	设 lidar_yaw=-180°(前后颠倒则 0.0)；改后 yaw 变了需重新示教，建议重建图
rviz 里示教点/地图整体乱飘跳动(机器人/激光却平滑)	Fixed Frame 设成了 odom，map 帧内容随 AMCL 每次校正跳变	rviz Fixed Frame 改 map(已设为默认)
平移一段再返回，回位坐标误差大(静止时稳定)	激光里程计 rf2o 运动跟踪偏差 + 麦轮物理漂移；非地图大小问题	慢速驱动；根治用轮式里程计+EKF(见 §9)；先做物理标记/对比 rf2o 与 AMCL 定位区分原因
激光点云形状对、但整体偏离墙线，AMCL 不往墙上贴	sigma_hit 太小(0.05)→偏差超几 cm 就无梯度，AMCL 无法纠正	调大 sigma_hit(0.2)、z_hit(0.9)、laser_max_range(≥场地对角线)、laser_likelihood_max_dist(2.0)；并给准初始位姿

通用排查：ros2 run tf2_ros tf2_echo map base_footprint、ros2 topic hz /scan /odom /map、 ros2 run tf2_tools view_frames（看 TF 树连通）。

---

9. 已知限制与后续

• 激光外参标定：lidar_yaw 已校准为 -180°，使机体 X 轴对准底盘实际前进方向（rviz 显示与遥控一致，导航默认符号 sign_x/y/w=-1 也随之正确）。平移 lidar_x/y 仍为 0 占位，需要更高位置精度可再标。 改动 lidar_yaw 后：机体朝向变了 —— 旧地图仍可定位（AMCL 自动补偿），但示教点的 yaw 需重新记录，建议重新建图让地图朝向也一致。
• 轮式里程计：底盘板“持续上报”问题待解；解决后可用 robot_localization EKF 融合 轮速 + rf2o 再喂 AMCL 提精度。
• 地图坐标系：map(0,0) = 建图起点。比赛复现时让机器人从同一物理起点开机，或用 “2D Pose Estimate” 给初值。

---

10. 参考来源

• lzu_robot/src/cgbot/cgbot/seriallzucar.py（里程计协议）、maps/amcl_config.yaml、src/slam_gmapping、src/rf2o_laser_odometry、src/ydlidar_ros2_driver-humble
• move_try/src/ydlidar_ros2_driver/params/TminiPro.yaml、src/move_try（同赛事场地/规划参考）