多点巡检机器人项目技术总结

Zixhy2026-09-06 19:03

多点巡检机器人项目技术总结

​ 一个完整的AI机器人应该至少包括上位机和下位机两个部分：下位机需要采用RTOS实时调度系统以确保可以实时控制机器人运动，也就是所谓的"小脑"。由于AI机器人需要自主感知环境，并且根据环境信息自动做出正确的运动决策，这就需要各种传感器包括：激光雷达、相机、超声波、GPS等用于获取周围环境信息，并结合AI算法进行计算，然后做出运动决策。下位机通常没有高算力，需要提供较高算力的上位机通过传感器获取周围环境信息，然后采用AI模型进行计算，计算后向下位机发出相应的控制指令，下位机接收到控制指令后，控制机器人运动，这也就是所谓的"大脑"。上位机和下位机的通信方式，包括常见的串口通信，CAN总线通信等，一般通过指定的帧格式实现大小脑间数据交换。

​ 本文按模块讲解多点巡检机器人上位机的实现原理与数据流动过程，基于 ROS2 Humble + Ubuntu 22.04环境，覆盖 底盘驱动 → 传感器 → 位姿融合 → 建图 → 导航 全链路，本文只涉及到多点巡检导航的实现过程。

系统总体架构

#mermaid-svg-oOohF45uRSCNnr0f{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-oOohF45uRSCNnr0f .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-oOohF45uRSCNnr0f .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-oOohF45uRSCNnr0f .error-icon{fill:#552222;}#mermaid-svg-oOohF45uRSCNnr0f .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-oOohF45uRSCNnr0f .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-oOohF45uRSCNnr0f .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-oOohF45uRSCNnr0f .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-oOohF45uRSCNnr0f .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-oOohF45uRSCNnr0f .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-oOohF45uRSCNnr0f .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-oOohF45uRSCNnr0f .marker{fill:#333333;stroke:#333333;}#mermaid-svg-oOohF45uRSCNnr0f .marker.cross{stroke:#333333;}#mermaid-svg-oOohF45uRSCNnr0f svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-oOohF45uRSCNnr0f p{margin:0;}#mermaid-svg-oOohF45uRSCNnr0f .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-oOohF45uRSCNnr0f .cluster-label text{fill:#333;}#mermaid-svg-oOohF45uRSCNnr0f .cluster-label span{color:#333;}#mermaid-svg-oOohF45uRSCNnr0f .cluster-label span p{background-color:transparent;}#mermaid-svg-oOohF45uRSCNnr0f .label text,#mermaid-svg-oOohF45uRSCNnr0f span{fill:#333;color:#333;}#mermaid-svg-oOohF45uRSCNnr0f .node rect,#mermaid-svg-oOohF45uRSCNnr0f .node circle,#mermaid-svg-oOohF45uRSCNnr0f .node ellipse,#mermaid-svg-oOohF45uRSCNnr0f .node polygon,#mermaid-svg-oOohF45uRSCNnr0f .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-oOohF45uRSCNnr0f .rough-node .label text,#mermaid-svg-oOohF45uRSCNnr0f .node .label text,#mermaid-svg-oOohF45uRSCNnr0f .image-shape .label,#mermaid-svg-oOohF45uRSCNnr0f .icon-shape .label{text-anchor:middle;}#mermaid-svg-oOohF45uRSCNnr0f .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-oOohF45uRSCNnr0f .rough-node .label,#mermaid-svg-oOohF45uRSCNnr0f .node .label,#mermaid-svg-oOohF45uRSCNnr0f .image-shape .label,#mermaid-svg-oOohF45uRSCNnr0f .icon-shape .label{text-align:center;}#mermaid-svg-oOohF45uRSCNnr0f .node.clickable{cursor:pointer;}#mermaid-svg-oOohF45uRSCNnr0f .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-oOohF45uRSCNnr0f .arrowheadPath{fill:#333333;}#mermaid-svg-oOohF45uRSCNnr0f .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-oOohF45uRSCNnr0f .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-oOohF45uRSCNnr0f .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-oOohF45uRSCNnr0f .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-oOohF45uRSCNnr0f .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-oOohF45uRSCNnr0f .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-oOohF45uRSCNnr0f .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-oOohF45uRSCNnr0f .cluster text{fill:#333;}#mermaid-svg-oOohF45uRSCNnr0f .cluster span{color:#333;}#mermaid-svg-oOohF45uRSCNnr0f div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-oOohF45uRSCNnr0f .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-oOohF45uRSCNnr0f rect.text{fill:none;stroke-width:0;}#mermaid-svg-oOohF45uRSCNnr0f .icon-shape,#mermaid-svg-oOohF45uRSCNnr0f .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-oOohF45uRSCNnr0f .icon-shape p,#mermaid-svg-oOohF45uRSCNnr0f .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-oOohF45uRSCNnr0f .icon-shape .label rect,#mermaid-svg-oOohF45uRSCNnr0f .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-oOohF45uRSCNnr0f .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-oOohF45uRSCNnr0f .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-oOohF45uRSCNnr0f :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 传感器
SLAM / NAV2
数据预处理与融合
底盘驱动层 turn_on_robot
/odom_raw
/imu/stm32_raw
PowerVoltage
/imu/imu_filtered
/odom_ekfcombined
/map
/scan
/scan
/camera/depth/image_raw/compressed
/camera/color/image_raw/compressed
/cmd_vel
serial_robot_node

串口驱动
EKF

robot_localization
imu_processor

去零点漂移
电压
robot_state_publisher

模型TF/关节
TF树
joint_state_publisher

关节状态
F
SLAM 建图

gmapping / cartographer / slam_toolbox
NAV2 导航

AMCL+planner+controller
激光雷达

robot_lidar
深度相机

robot_camer

数据流一句话 ：底盘(轮子/IMU/电压) → 串口 → 里程计+IMU 话题 → 去漂移/EKF 融合 → 建图/导航 → 又回到底盘执行 /cmd_vel，形成闭环。

模块一：底盘串口驱动（serial.launch.py）

1.1 模块功能

base_serial.launch.py 是整个项目的起点 。它启动底盘驱动主节点 wheeltec_robot_node，负责：

  • 通过串口 （/dev/wheeltec_controller，波特率 115200）和底盘单片机（STM32）通信；
  • 接收下位机上报的：轮速、IMU 六轴原始数据、电池电压（可选：充电/超声波数据）；
  • 发送 上位机的速度指令（/cmd_vel）给下位机，驱动电机；
  • 发布 /odom_raw（原始里程计）、/imu/stm32_raw（原始 IMU）、/PowerVoltage（电压）等话题。

1.2 实现原理：三帧状态机串口解析

下位机以固定格式（帧头 + 数据 + 校验 + 帧尾）持续上报数据，共 3 种帧：

帧类型 帧头 帧尾 长度 内容
基础底盘数据帧 0x7B 0x7D 24 字节 轮速 X/Y/Z、IMU 六轴、电池电压
自动回充数据帧 0x7C 0x7F 8 字节 充电电流、红外信号、充电状态
超声波数据帧 0xFA 0xFC 19 字节 6 路超声波距离 A~F

核心函数 Get_Sensor_Data_New() 采用单字节状态机解析：

  1. 每次调用只从串口读 1 字节，存入三个接收数组；
  2. 用三个计数器（count/count2/count3）分别"攒"三种帧；
  3. 判断"上一字节是帧尾 && 当前字节是帧头" → 新帧开始，计数从 1 重数；
  4. 攒够一帧长度后，先校验帧尾 ，再对前 N 字节做 BCC 异或校验，校验通过才解析数据；
  5. 只有完整收到一帧底盘数据才返回 true，主循环据此决定是否发布话题。

为什么这样设计？ 串口是字节流，没有明确的"消息边界"，必须靠帧头/帧尾 + 长度 + 校验来切分帧，并丢弃被干扰的脏数据，防止污染里程计。

1.3 主循环：读串口帧数据 + 发话题 + 处理回调

复制代码 
while(rclcpp::ok())
  ├─ ① Get_Sensor_Data_New()：读1字节，攒帧、解析
  ├─ ② 有完整一帧？→ Publish_Odom() + Publish_ImuSensor() + Publish_Voltage()
  ├─ ③ 有回充帧？→ Publish_Charging/RED/ChargingCurrent
  ├─ ④ 有超声波帧？→ Publish_distance()
  └─ ⑤ rclcpp::spin_some()：非阻塞处理 /cmd_vel 等订阅回调

关键设计 ：用 spin_some() 而不是 spin()------spin() 会阻塞在回调等待上，导致串口无人读取而丢数据；spin_some() 处理完已排队的回调立即返回，主循环能继续读串口，两不耽误。

1.4 IMU数据话题发布和Odom里程计数据话题发布

IMU数据话题发布(Publish_ImuSensor)

​ 需要先将串口中接收到下位机上传的IMU数据的三轴角速度和三轴线速度进行ADC数值转换单位，然后需要计算姿态四元数。

  1. IMU陀螺仪的 ADC 原始值 → 物理单位 rad/s

    ​ 与STM32初始化IMU的量程有关，这里陀螺仪量程 ±500°/s，因为机器人一般Z轴速度不快，降低ADC采集量程可以提高采集精度，GYROSCOPE_RATIO = 1/65.5/57.30 = 0.00026644（把±32768映射到±500°/s再转弧度rad/s）。

  2. MU加速度计的 ADC 原始值 → 物理单位 m/s²

    ​ 与STM32初始化IMU时的量程有关，这里量程 ±2g = 19.6m/s²，ACCEl_RATIO = 32768/2g = 1671.84（把±32768映射到±19.6m/s²），在静止时 Z 轴不是 0，而是 ≈ +9.8（重力加速度），这是正常现象。

里程计数据话题发布(Publish_Odom)

​ 先进行运动学正解，然后进行里程计积分，计算相对于起点坐标系odom_raw的小车本体位置base_footprintf距离。里程计位置通过对速度做时间积分得到（差速小车运动学正解）：

X′Y′\]=\[cos⁡θ−sin⁡θsin⁡θcos⁡θ\]\[VxVy\] \\begin{bmatrix} X' \\\\ Y' \\end{bmatrix} = \\begin{bmatrix} \\cos\\theta \& -\\sin\\theta \\\\ \\sin\\theta \& \\cos\\theta \\end{bmatrix} \\begin{bmatrix} Vx \\\\ Vy \\end{bmatrix} \[X′Y′\]=\[cosθsinθ−sinθcosθ\]\[VxVy

Xt+1=Xt+(Vxcos⁡θ−Vysin⁡θ)⋅dt,Yt+1=Yt+(Vxsin⁡θ+Vycos⁡θ)⋅dt,θt+1=θt+Vz⋅dt X_{t+1} = X_t + (Vx\cos\theta - Vy\sin\theta)\cdot dt, \quad Y_{t+1} = Y_t + (Vx\sin\theta + Vy\cos\theta)\cdot dt, \quad \theta_{t+1} = \theta_t + Vz\cdot dt Xt+1=Xt+(Vxcosθ−Vysinθ)⋅dt,Yt+1=Yt+(Vxsinθ+Vycosθ)⋅dt,θt+1=θt+Vz⋅dt

​ 其中 dtdtdt = 两次循环的时间差（Sampling_Time），θ\thetaθ = 当前偏航角。发布前会乘以修正系数（odom_x_scale 等，修正编码器系统误差），并把 yaw 转成四元数。

1.5 数据流图

#mermaid-svg-0WD96ZQjIYbmKCHy{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-0WD96ZQjIYbmKCHy .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-0WD96ZQjIYbmKCHy .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-0WD96ZQjIYbmKCHy .error-icon{fill:#552222;}#mermaid-svg-0WD96ZQjIYbmKCHy .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-0WD96ZQjIYbmKCHy .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-0WD96ZQjIYbmKCHy .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-0WD96ZQjIYbmKCHy .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-0WD96ZQjIYbmKCHy .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-0WD96ZQjIYbmKCHy .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-0WD96ZQjIYbmKCHy .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-0WD96ZQjIYbmKCHy .marker{fill:#333333;stroke:#333333;}#mermaid-svg-0WD96ZQjIYbmKCHy .marker.cross{stroke:#333333;}#mermaid-svg-0WD96ZQjIYbmKCHy svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-0WD96ZQjIYbmKCHy p{margin:0;}#mermaid-svg-0WD96ZQjIYbmKCHy .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-0WD96ZQjIYbmKCHy .cluster-label text{fill:#333;}#mermaid-svg-0WD96ZQjIYbmKCHy .cluster-label span{color:#333;}#mermaid-svg-0WD96ZQjIYbmKCHy .cluster-label span p{background-color:transparent;}#mermaid-svg-0WD96ZQjIYbmKCHy .label text,#mermaid-svg-0WD96ZQjIYbmKCHy span{fill:#333;color:#333;}#mermaid-svg-0WD96ZQjIYbmKCHy .node rect,#mermaid-svg-0WD96ZQjIYbmKCHy .node circle,#mermaid-svg-0WD96ZQjIYbmKCHy .node ellipse,#mermaid-svg-0WD96ZQjIYbmKCHy .node polygon,#mermaid-svg-0WD96ZQjIYbmKCHy .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-0WD96ZQjIYbmKCHy .rough-node .label text,#mermaid-svg-0WD96ZQjIYbmKCHy .node .label text,#mermaid-svg-0WD96ZQjIYbmKCHy .image-shape .label,#mermaid-svg-0WD96ZQjIYbmKCHy .icon-shape .label{text-anchor:middle;}#mermaid-svg-0WD96ZQjIYbmKCHy .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-0WD96ZQjIYbmKCHy .rough-node .label,#mermaid-svg-0WD96ZQjIYbmKCHy .node .label,#mermaid-svg-0WD96ZQjIYbmKCHy .image-shape .label,#mermaid-svg-0WD96ZQjIYbmKCHy .icon-shape .label{text-align:center;}#mermaid-svg-0WD96ZQjIYbmKCHy .node.clickable{cursor:pointer;}#mermaid-svg-0WD96ZQjIYbmKCHy .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-0WD96ZQjIYbmKCHy .arrowheadPath{fill:#333333;}#mermaid-svg-0WD96ZQjIYbmKCHy .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-0WD96ZQjIYbmKCHy .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-0WD96ZQjIYbmKCHy .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-0WD96ZQjIYbmKCHy .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-0WD96ZQjIYbmKCHy .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-0WD96ZQjIYbmKCHy .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-0WD96ZQjIYbmKCHy .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-0WD96ZQjIYbmKCHy .cluster text{fill:#333;}#mermaid-svg-0WD96ZQjIYbmKCHy .cluster span{color:#333;}#mermaid-svg-0WD96ZQjIYbmKCHy div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-0WD96ZQjIYbmKCHy .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-0WD96ZQjIYbmKCHy rect.text{fill:none;stroke-width:0;}#mermaid-svg-0WD96ZQjIYbmKCHy .icon-shape,#mermaid-svg-0WD96ZQjIYbmKCHy .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-0WD96ZQjIYbmKCHy .icon-shape p,#mermaid-svg-0WD96ZQjIYbmKCHy .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-0WD96ZQjIYbmKCHy .icon-shape .label rect,#mermaid-svg-0WD96ZQjIYbmKCHy .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-0WD96ZQjIYbmKCHy .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-0WD96ZQjIYbmKCHy .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-0WD96ZQjIYbmKCHy :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} wheeltec_robot_node
底盘硬件
24字节帧 0x7B
8字节帧 0x7C
19字节帧 0xFA
速度+IMU+电压
电压
Cmd_Vel_Callback
速度帧指令
驱动
STM32 下位机
轮子编码器
板载IMU/MPU6050
电池
下位机进行串口收发数据

/dev/controller

115200
Get_Sensor_Data_New

三帧状态机

帧头/帧尾/BCC校验
位姿积分

V·dt 运动学正解
Publish_Voltage

每11次发1次
odom_raw

位置+速度+协方差
odom_raw→base_footprint

TF广播
imu/stm32_raw

四元数+角速度+加速度
PowerVoltage
cmd_vel

订阅

模块二：IMU 去零点漂移（imu_processor.launch.py）

2.1 模块功能

​ 启动 ImuProcessor_node，把底盘发布的原始 IMU （/imu/stm32_raw）修正静止时的零点漂移 问题，输出干净的 /imu/imu_filtered，供 EKF 在位姿融合时使用。

2.2 原理：为什么需要去零点漂移？

陀螺仪（角速度计）存在静态零偏 ------即使静止不动，输出也不为 0。如果直接对原始角速度积分求姿态角，静止时姿态会缓慢漂移（越积越偏），这样会造成一个现象机器人处于静止状态，但内部计算的机器人的偏航角会缓慢移动，导致实际方向和内部计算的方向出现误差。所以需要一个"静止检测 + 姿态解算冻结"策略。

2.3 实现：静止检测 + 姿态解算冻结

核心逻辑在 ImuProcessor::imuCallback：

python 复制代码 
1. 订阅 /odom_raw 获取线速度 linear_vel_ 与角速度 angular_vel_
2. 若 |linear_vel_| < 阈值 且 |angular_vel_| < 阈值 → 静止计数 static_count++
   否则 → static_count = 0（说明在运动，不是静止）
3. 若 static_count >= 10（连续10次判静止）→ Quaternion_Solution.SetFreeze(true)
   否则 → SetFreeze(false)
4. Quaternion_Solution(out)：用 Mahony 互补滤波解算四元数姿态
5. 发布修正后的 IMU 到 /imu/imu_filtered

冻结（freeze）的含义：静止时把姿态解算的"角速度积分"冻结，让陀螺仪零偏不再累积进姿态角；同时用加速度计的重力方向来纠正姿态。运动时解除冻结，正常积分。

姿态解算原理：将IMU的原始xyz三轴线速度和三轴角速度计算姿态四元数，采用Mahony互补滤波（AHRS）算法融合底盘上传的IMU芯片的陀螺仪和加速度计分别测得的三轴角速度和三轴加速度计算姿态角。基本过程是利用陀螺仪测的三轴角速度及上次姿态四元数进行积分计算当前姿态四元数，然后将垂直重力加速度方向在该姿态四元数下旋转，比较旋转后的重力加速度是否和加速度计测的三轴加速度合并后的加速度一致，并根据两者向量叉积误差去修正当前姿态四元数。

2.4 数据流图

#mermaid-svg-Gp5KRqUqrXqUJE1V{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-Gp5KRqUqrXqUJE1V .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-Gp5KRqUqrXqUJE1V .error-icon{fill:#552222;}#mermaid-svg-Gp5KRqUqrXqUJE1V .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-Gp5KRqUqrXqUJE1V .marker{fill:#333333;stroke:#333333;}#mermaid-svg-Gp5KRqUqrXqUJE1V .marker.cross{stroke:#333333;}#mermaid-svg-Gp5KRqUqrXqUJE1V svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-Gp5KRqUqrXqUJE1V p{margin:0;}#mermaid-svg-Gp5KRqUqrXqUJE1V .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-Gp5KRqUqrXqUJE1V .cluster-label text{fill:#333;}#mermaid-svg-Gp5KRqUqrXqUJE1V .cluster-label span{color:#333;}#mermaid-svg-Gp5KRqUqrXqUJE1V .cluster-label span p{background-color:transparent;}#mermaid-svg-Gp5KRqUqrXqUJE1V .label text,#mermaid-svg-Gp5KRqUqrXqUJE1V span{fill:#333;color:#333;}#mermaid-svg-Gp5KRqUqrXqUJE1V .node rect,#mermaid-svg-Gp5KRqUqrXqUJE1V .node circle,#mermaid-svg-Gp5KRqUqrXqUJE1V .node ellipse,#mermaid-svg-Gp5KRqUqrXqUJE1V .node polygon,#mermaid-svg-Gp5KRqUqrXqUJE1V .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-Gp5KRqUqrXqUJE1V .rough-node .label text,#mermaid-svg-Gp5KRqUqrXqUJE1V .node .label text,#mermaid-svg-Gp5KRqUqrXqUJE1V .image-shape .label,#mermaid-svg-Gp5KRqUqrXqUJE1V .icon-shape .label{text-anchor:middle;}#mermaid-svg-Gp5KRqUqrXqUJE1V .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-Gp5KRqUqrXqUJE1V .rough-node .label,#mermaid-svg-Gp5KRqUqrXqUJE1V .node .label,#mermaid-svg-Gp5KRqUqrXqUJE1V .image-shape .label,#mermaid-svg-Gp5KRqUqrXqUJE1V .icon-shape .label{text-align:center;}#mermaid-svg-Gp5KRqUqrXqUJE1V .node.clickable{cursor:pointer;}#mermaid-svg-Gp5KRqUqrXqUJE1V .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-Gp5KRqUqrXqUJE1V .arrowheadPath{fill:#333333;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-Gp5KRqUqrXqUJE1V .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-Gp5KRqUqrXqUJE1V .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-Gp5KRqUqrXqUJE1V .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-Gp5KRqUqrXqUJE1V .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-Gp5KRqUqrXqUJE1V .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-Gp5KRqUqrXqUJE1V .cluster text{fill:#333;}#mermaid-svg-Gp5KRqUqrXqUJE1V .cluster span{color:#333;}#mermaid-svg-Gp5KRqUqrXqUJE1V div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-Gp5KRqUqrXqUJE1V .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-Gp5KRqUqrXqUJE1V rect.text{fill:none;stroke-width:0;}#mermaid-svg-Gp5KRqUqrXqUJE1V .icon-shape,#mermaid-svg-Gp5KRqUqrXqUJE1V .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-Gp5KRqUqrXqUJE1V .icon-shape p,#mermaid-svg-Gp5KRqUqrXqUJE1V .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-Gp5KRqUqrXqUJE1V .icon-shape .label rect,#mermaid-svg-Gp5KRqUqrXqUJE1V .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-Gp5KRqUqrXqUJE1V .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-Gp5KRqUqrXqUJE1V .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-Gp5KRqUqrXqUJE1V :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 输出
ImuProcessor 节点
来自底盘节点
v, ω
静止
运动
是
否
imu/stm32_raw

原始IMU
odom_raw

原始里程计
imuCallback

订阅原始IMU
odomCallback

订阅里程计
线速度和角速度域值判断静止
连续10次静止?
SetFreeze

true冻结/false解除
QuaternionSolution

Mahony 姿态解算
imu/imu_filtered

去漂移IMU
EKF 融合模块

模块三：机器人模型与关节发布及TF变换（robot_mode_description.launch.py）

3.1 模块功能

启动机器人模型描述 + 静态 TF，让上层（RViz、导航、建图）知道"传感器装在机器人哪里"：

  • robot_state_publisher：加载 URDF 模型，发布各连杆link间的joint关节关系的静态 TF 变换(Fixed属性joint)；
  • joint_state_publisher：发布 /joint_states 关节状态话题（配合 URDF 显示连接link间的Continue关系的关节关系的动态 TF 变换）；
  • 多个 static_transform_publisher：发布 base_footprint → 各类传感器（激光、相机、IMU、雷达）的固定静态 TF 变换。

3.2 实现原理

关键设计：OpaqueFunction + 读 yaml 动态生成节点，包括机器人状态发布结点和关节状态发布结点：

  • 机器人状态发布结点：通过加载机器人模型的URDF文件进行发布。
  • 关节状态发布节点：配合机器人状态发布节点，发布机器人模型中的URDF文件发布关节状态关系数据/joint_state。
  • 发布机器人本体投影到各传感器的静态 TF 坐标变换发布(可选，因为保证URDF正确时，TF系统可以通过链式计算得到)。

静态 TF vs 动态 TF 的区别：

类型 发布者 用途 频率
静态 TF static_transform_publisher 传感器相对本体的固定位置 一次（/tf_static）
动态 TF 里程计/EKF/AMCL/joint_state_publisher 本体相对世界的实时位姿 高频（/tf）

关节 TF ：robot_state_publisher 对 URDF 里的 fixed 关节直接按 origin 发布静态 TF；对 continuous/revolute 关节则读取 joint_state_publisher 的 /joint_states，结合关节 origin 动态发布。

坐标系关系（本项目的 TF 树核心）：

复制代码 
map → odom_ekfcombined（AMCL 或 EKF 发布）
  └→ base_footprint（EKF 发布）
       ├→ base_link（static，URDF）
       ├→ laser（static，机器人模型配置）
       ├→ camera_link（static）
       ├→ gyro_link（static）
       └→ radar（static）

3.3 数据流图

#mermaid-svg-VFgZKSVZph2COu0B{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-VFgZKSVZph2COu0B .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-VFgZKSVZph2COu0B .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-VFgZKSVZph2COu0B .error-icon{fill:#552222;}#mermaid-svg-VFgZKSVZph2COu0B .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-VFgZKSVZph2COu0B .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-VFgZKSVZph2COu0B .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-VFgZKSVZph2COu0B .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-VFgZKSVZph2COu0B .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-VFgZKSVZph2COu0B .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-VFgZKSVZph2COu0B .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-VFgZKSVZph2COu0B .marker{fill:#333333;stroke:#333333;}#mermaid-svg-VFgZKSVZph2COu0B .marker.cross{stroke:#333333;}#mermaid-svg-VFgZKSVZph2COu0B svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-VFgZKSVZph2COu0B p{margin:0;}#mermaid-svg-VFgZKSVZph2COu0B .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-VFgZKSVZph2COu0B .cluster-label text{fill:#333;}#mermaid-svg-VFgZKSVZph2COu0B .cluster-label span{color:#333;}#mermaid-svg-VFgZKSVZph2COu0B .cluster-label span p{background-color:transparent;}#mermaid-svg-VFgZKSVZph2COu0B .label text,#mermaid-svg-VFgZKSVZph2COu0B span{fill:#333;color:#333;}#mermaid-svg-VFgZKSVZph2COu0B .node rect,#mermaid-svg-VFgZKSVZph2COu0B .node circle,#mermaid-svg-VFgZKSVZph2COu0B .node ellipse,#mermaid-svg-VFgZKSVZph2COu0B .node polygon,#mermaid-svg-VFgZKSVZph2COu0B .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-VFgZKSVZph2COu0B .rough-node .label text,#mermaid-svg-VFgZKSVZph2COu0B .node .label text,#mermaid-svg-VFgZKSVZph2COu0B .image-shape .label,#mermaid-svg-VFgZKSVZph2COu0B .icon-shape .label{text-anchor:middle;}#mermaid-svg-VFgZKSVZph2COu0B .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-VFgZKSVZph2COu0B .rough-node .label,#mermaid-svg-VFgZKSVZph2COu0B .node .label,#mermaid-svg-VFgZKSVZph2COu0B .image-shape .label,#mermaid-svg-VFgZKSVZph2COu0B .icon-shape .label{text-align:center;}#mermaid-svg-VFgZKSVZph2COu0B .node.clickable{cursor:pointer;}#mermaid-svg-VFgZKSVZph2COu0B .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-VFgZKSVZph2COu0B .arrowheadPath{fill:#333333;}#mermaid-svg-VFgZKSVZph2COu0B .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-VFgZKSVZph2COu0B .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-VFgZKSVZph2COu0B .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-VFgZKSVZph2COu0B .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-VFgZKSVZph2COu0B .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-VFgZKSVZph2COu0B .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-VFgZKSVZph2COu0B .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-VFgZKSVZph2COu0B .cluster text{fill:#333;}#mermaid-svg-VFgZKSVZph2COu0B .cluster span{color:#333;}#mermaid-svg-VFgZKSVZph2COu0B div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-VFgZKSVZph2COu0B .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-VFgZKSVZph2COu0B rect.text{fill:none;stroke-width:0;}#mermaid-svg-VFgZKSVZph2COu0B .icon-shape,#mermaid-svg-VFgZKSVZph2COu0B .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-VFgZKSVZph2COu0B .icon-shape p,#mermaid-svg-VFgZKSVZph2COu0B .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-VFgZKSVZph2COu0B .icon-shape .label rect,#mermaid-svg-VFgZKSVZph2COu0B .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-VFgZKSVZph2COu0B .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-VFgZKSVZph2COu0B .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-VFgZKSVZph2COu0B :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 启动的节点
OpaqueFunction 运行时
配置文件
/joint_states
关节TF
base_footprint→laser
base_footprint→camera_link
base_footprint→gyro_link
base_footprint→radar
wheeltec_param.yaml

car_mode 车型
robot_model.yaml

各车型传感器坐标
wheeltec_robot_urdf 包

car_mode_robot.urdf 模型
读 yaml 取车型
按车型拼URDF路径
按车型生成传感器静态TF节点
robot_state_publisher

加载URDF发布关节TF
joint_state_publisher

发布/joint_states
base_to_laser

static TF
base_to_camera

static TF
base_to_gyro

static TF
base_to_radar

static TF
TF树

模块四：EKF 里程计融合（ekf.launch.py）

4.1 模块功能

启动 robot_localization 包的 EKF 节点 ，把 轮式里程计 + IMU 按可信度（协方差）加权融合，输出更稳定的融合位姿 /odom_ekfcombined，并发布 odom_ekfcombined → base_footprint 的 TF。

4.2 原理：为什么要融合？什么是 EKF？

各自的优缺点：

传感器 优点 缺点
轮式里程计 /odom 短期准确、不会发散 打滑/磨损导致长期漂移
IMU 高频平滑、姿态准 零点漂移、积分发散

EKF（扩展卡尔曼滤波） 把两者按"预测 + 校正"两步融合：

  1. 预测：采用里程计数据/IMU数据计算增量，用运动模型（速度积分）预测下一个位姿。
  2. 校正 ：用传感器测量数据（里程计数据/IMU数据）校正预测值，误差由协方差矩阵决定谁更可信。

协方差越小 = 数据越可信。底盘在发布 /odom_raw 时就提供了两套协方差：静止时用更小的协方差 （编码器没转，误差小，让EKF 多参考 Odom数据），运动时用较大的协方差（可能打滑，让 EKF 多参考 IMU）。

4.3 三种配置（通过 launch 参数选择）

wheeltec_ekf.launch.py 根据 carto_slam / robot_nav 两个参数，动态启动不同节点与配置文件：

模式 触发参数 节点名 配置文件 特点
普通模式 默认 ekf_filter_node ekf.yaml 2D 模式，10Hz
建图模式 carto_slam:=true carto_ekf_filter_node ekf_carto.yaml 3D 模式 ，30Hz，odom0_differential:false
导航模式 robot_nav:=true nav_ekf_filter_node ekf_nav.yaml 2D 模式，10Hz

三者都输出到 /odom_ekfcombined（remap 自 /odometry/filtered），都 publish_tf: true。

三种配置的关键差异与原因：

参数 普通/导航 建图(carto) 原因
two_d_mode true false 建图要完整 3D 位姿（含 z/roll/pitch），应对地面起伏；导航只需平面
frequency 10Hz 30Hz Cartographer 位姿插值需要细粒度时间戳，要高频率
sensor_timeout 1.5s 0.1s 建图对实时性敏感，过时数据立即丢弃
odom0_differential true false 普通/导航用差分平滑抗漂移；建图需要完整绝对轨迹做扫描匹配基准
imu0 /imu/imu_filtered /imu/imu_filtered 都用去漂移后的 IMU
odom0 /odom_raw /odom_raw 都用原始里程计

融合内容 （odom0_config / imu0_config 15 元素数组，顺序为 x,y,z,roll,pitch,yaw,vx,vy,vz,vroll,vpitch,vyaw,ax,ay,az）：

  • odom 融合：x、y、yaw、vx、vy、vyaw（true,true,false, false,false,true, true,true,false, false,false,true, ...）
  • imu 融合：yaw、vyaw（false,false,false, false,false,true, ... false,false,true）

坐标系（REP-105 规范）：

  • map_frame: map：全局固定，可能跳变
  • odom_frame / world_frame: odom_ekfcombined：连续、短期准、长期漂
  • base_link_frame: base_footprint：随机器人动

4.4 数据流图

#mermaid-svg-y2fJYeL8kYVQrFBk{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-y2fJYeL8kYVQrFBk .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-y2fJYeL8kYVQrFBk .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-y2fJYeL8kYVQrFBk .error-icon{fill:#552222;}#mermaid-svg-y2fJYeL8kYVQrFBk .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-y2fJYeL8kYVQrFBk .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-y2fJYeL8kYVQrFBk .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-y2fJYeL8kYVQrFBk .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-y2fJYeL8kYVQrFBk .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-y2fJYeL8kYVQrFBk .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-y2fJYeL8kYVQrFBk .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-y2fJYeL8kYVQrFBk .marker{fill:#333333;stroke:#333333;}#mermaid-svg-y2fJYeL8kYVQrFBk .marker.cross{stroke:#333333;}#mermaid-svg-y2fJYeL8kYVQrFBk svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-y2fJYeL8kYVQrFBk p{margin:0;}#mermaid-svg-y2fJYeL8kYVQrFBk .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-y2fJYeL8kYVQrFBk .cluster-label text{fill:#333;}#mermaid-svg-y2fJYeL8kYVQrFBk .cluster-label span{color:#333;}#mermaid-svg-y2fJYeL8kYVQrFBk .cluster-label span p{background-color:transparent;}#mermaid-svg-y2fJYeL8kYVQrFBk .label text,#mermaid-svg-y2fJYeL8kYVQrFBk span{fill:#333;color:#333;}#mermaid-svg-y2fJYeL8kYVQrFBk .node rect,#mermaid-svg-y2fJYeL8kYVQrFBk .node circle,#mermaid-svg-y2fJYeL8kYVQrFBk .node ellipse,#mermaid-svg-y2fJYeL8kYVQrFBk .node polygon,#mermaid-svg-y2fJYeL8kYVQrFBk .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-y2fJYeL8kYVQrFBk .rough-node .label text,#mermaid-svg-y2fJYeL8kYVQrFBk .node .label text,#mermaid-svg-y2fJYeL8kYVQrFBk .image-shape .label,#mermaid-svg-y2fJYeL8kYVQrFBk .icon-shape .label{text-anchor:middle;}#mermaid-svg-y2fJYeL8kYVQrFBk .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-y2fJYeL8kYVQrFBk .rough-node .label,#mermaid-svg-y2fJYeL8kYVQrFBk .node .label,#mermaid-svg-y2fJYeL8kYVQrFBk .image-shape .label,#mermaid-svg-y2fJYeL8kYVQrFBk .icon-shape .label{text-align:center;}#mermaid-svg-y2fJYeL8kYVQrFBk .node.clickable{cursor:pointer;}#mermaid-svg-y2fJYeL8kYVQrFBk .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-y2fJYeL8kYVQrFBk .arrowheadPath{fill:#333333;}#mermaid-svg-y2fJYeL8kYVQrFBk .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-y2fJYeL8kYVQrFBk .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-y2fJYeL8kYVQrFBk .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-y2fJYeL8kYVQrFBk .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-y2fJYeL8kYVQrFBk .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-y2fJYeL8kYVQrFBk .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-y2fJYeL8kYVQrFBk .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-y2fJYeL8kYVQrFBk .cluster text{fill:#333;}#mermaid-svg-y2fJYeL8kYVQrFBk .cluster span{color:#333;}#mermaid-svg-y2fJYeL8kYVQrFBk div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-y2fJYeL8kYVQrFBk .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-y2fJYeL8kYVQrFBk rect.text{fill:none;stroke-width:0;}#mermaid-svg-y2fJYeL8kYVQrFBk .icon-shape,#mermaid-svg-y2fJYeL8kYVQrFBk .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-y2fJYeL8kYVQrFBk .icon-shape p,#mermaid-svg-y2fJYeL8kYVQrFBk .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-y2fJYeL8kYVQrFBk .icon-shape .label rect,#mermaid-svg-y2fJYeL8kYVQrFBk .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-y2fJYeL8kYVQrFBk .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-y2fJYeL8kYVQrFBk .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-y2fJYeL8kYVQrFBk :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 输出
robot_localization EKF
传感器输入
launch 参数动态选择
odom_raw

原始里程计
imu/imu_filtered

去漂移IMU
普通: ekf.yaml

ekf_filter_node

2D/10Hz
建图: ekf_carto.yaml

carto_ekf_filter_node

3D/30Hz
导航: ekf_nav.yaml

nav_ekf_filter_node

2D/10Hz
① 预测

运动模型积分
② 校正

加权测量/协方差
异常过滤

马氏距离阈值
odom_ekfcombined

融合位姿
odom_ekfcombined→base_footprint

TF广播
SLAM / NAV2

模块五：三种 SLAM 的2D建图方式

SLAM（Simultaneous Localization And Mapping）= 机器人边移动边"定位自己 + 拼出地图"，这里介绍3种常见的2D的slam建图方式，根据需要可以使用3D的slam建图方式。

三种方式对比：

方式 包 核心算法 输入里程计 特点
gmapping slam_gmapping 粒子滤波 EKF（/odom_ekfcombined） 经典、老牌、适合小场景
Cartographer wheeltec_cartographer 图优化 + 扫描匹配 原始（/odom_raw）+原始IMU Google 出品、精度高、可回环
slam_toolbox wheeltec_slam_toolbox 图优化（Ceres） EKF（/odom_ekfcombined） 轻量、支持重定位模式

5.1 gmapping 建图（slam_gmapping.launch.py）

gmapping建图原理：粒子滤波

​ 采用一定数量的粒子，每个粒子代表一个假想的机器人位姿，随着每次激光扫描收到一次激光/scan话题数据，进行以下过程淘汰不符合粒子并保留符合粒子：

  1. 预测：计算每个粒子按照/odom_ekfcombined里程计数据进行计算位姿。
  2. 打分：比对/scan激光扫描结果对每个粒子进行匹配打分。
  3. 重采样：按照分数重新抽样，高分粒子被复制多份，低分粒子直接淘汰，粒子总数不变。
  4. 用高分粒子的位置，叠加当前激光扫描结果贴进地图/map，进行更新地图。
gmapping建图流程：
  1. 启动本launch文件，打开3个结点：ros2 launch slam_gmapping slam_gmapping.launch.py
    1. 启动小车底层（turn_on_wheeltec_robot.launch.py）→ 底盘驱动 + EKF + 模型/TF
    2. 启动激光雷达（wheeltec_lidar.launch.py）→ 提供 /scan 激光数据
    3. 启动 slam_gmapping 节点（核心建图）→ 融合 /scan + /odom_ekfcombined 建图，发布 /map
  2. 遥控小车缓慢走遍周围环境。
  3. 采用命令保存地图，ros2 run nav2_map_server map_saver_cli -f ~/map/my_map
数据流图

#mermaid-svg-V8TFd0CmpC6nZ4A1{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .error-icon{fill:#552222;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .marker{fill:#333333;stroke:#333333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .marker.cross{stroke:#333333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-V8TFd0CmpC6nZ4A1 p{margin:0;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .cluster-label text{fill:#333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .cluster-label span{color:#333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .cluster-label span p{background-color:transparent;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .label text,#mermaid-svg-V8TFd0CmpC6nZ4A1 span{fill:#333;color:#333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .node rect,#mermaid-svg-V8TFd0CmpC6nZ4A1 .node circle,#mermaid-svg-V8TFd0CmpC6nZ4A1 .node ellipse,#mermaid-svg-V8TFd0CmpC6nZ4A1 .node polygon,#mermaid-svg-V8TFd0CmpC6nZ4A1 .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .rough-node .label text,#mermaid-svg-V8TFd0CmpC6nZ4A1 .node .label text,#mermaid-svg-V8TFd0CmpC6nZ4A1 .image-shape .label,#mermaid-svg-V8TFd0CmpC6nZ4A1 .icon-shape .label{text-anchor:middle;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .rough-node .label,#mermaid-svg-V8TFd0CmpC6nZ4A1 .node .label,#mermaid-svg-V8TFd0CmpC6nZ4A1 .image-shape .label,#mermaid-svg-V8TFd0CmpC6nZ4A1 .icon-shape .label{text-align:center;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .node.clickable{cursor:pointer;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .arrowheadPath{fill:#333333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-V8TFd0CmpC6nZ4A1 .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-V8TFd0CmpC6nZ4A1 .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-V8TFd0CmpC6nZ4A1 .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .cluster text{fill:#333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .cluster span{color:#333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-V8TFd0CmpC6nZ4A1 rect.text{fill:none;stroke-width:0;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .icon-shape,#mermaid-svg-V8TFd0CmpC6nZ4A1 .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .icon-shape p,#mermaid-svg-V8TFd0CmpC6nZ4A1 .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .icon-shape .label rect,#mermaid-svg-V8TFd0CmpC6nZ4A1 .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-V8TFd0CmpC6nZ4A1 .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-V8TFd0CmpC6nZ4A1 .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-V8TFd0CmpC6nZ4A1 :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 粒子滤波流程
输入
launch 启动
启动机器人底层结点
robot_lidar

激光雷达
slam_gmapping 节点
odom_ekfcombined

EKFk里程计
scan

激光
① 预测

按里程计更新每个粒子位姿
② 打分

激光与地图匹配评分
③ 重采样

高分复制/低分淘汰
④ 更新地图

高分粒子叠加扫描
map

栅格地图

5.2 Cartographer 建图（cartographer.launch.py）

Cartographer 建图原理：

​ Cartographer 使用原始 odom 和 IMU（remap 到 /odom_raw、/imu/stm32_raw），因为它自带数据处理，配合激光扫描纠正更精确，"里程计+IMU预测 + 激光扫描匹配 + 回环检测优化" 三步，让机器人边走动边"定位自己 + 拼接环境地图"，最后输出一张完整地图：

  1. 猜（预测） ：采用原始/odom_raw里程计数据+原始IMU数据/stm32_raw，通过增量进行运动预测机器人位置。
  2. 纠（扫描匹配） ：在机器人预测位置获取激光雷达/scan话题数据，与已有子图进行扫描匹配，找到最吻合位置。进行修正预测位置，并把新扫描结果贴进子图。
  3. 调（全局优化） ：在机器人扫描到曾经来过的位置时，进行回环检测 确认，然后进行全局优化 ，调整所有相关子图位置，消除累积漂移误差。
cartographer建图流程：
  1. 启动本launch文件ros2 launch wheeltec_cartographer cartographer.launch.py，打开 4 个结点：
    1. 启动小车底层（turn_on_robot.launch.py，传 carto_slam=true）→ 底盘驱动 + EKF（建图配置）+ 模型/TF
    2. 启动激光雷达（robot_lidar.launch.py）→ 提供 /scan 激光数据
    3. 启动 cartographer_node（核心建图节点）→ 融合 /scan + /odom + /imu，实时建图
    4. 启动 cartographer_occupancy_grid_node（地图栅格节点）→ 把 Cartographer 内部子图转成 ROS 标准 /map 地图话题
  2. 控制小车运动，缓慢在房间内扫描周围环境进行建图。
  3. 执行ros2 run nav2_map_server map_saver_cli -f ~/map/my_map命令，生成my_map.pgm格栅地图文件

关键配置（cartographer.lua）：

  • odom_frame = "odom_cartocombined"、provide_odom_frame = true：Cartographer 自己发布 odom_cartocombined → base_footprint 的 TF；
  • use_odometry = true、use_imu_data = true：用里程计+IMU 预测；
  • num_laser_scans = 1：1 路 2D 激光；
  • submaps.num_range_data = 45：45 帧激光生成一个子图；
  • use_online_correlative_scan_matching = true：在线相关扫描匹配。

注意 ：建图模式下 EKF 用 ekf_carto.yaml（3D、30Hz、odom0_differential:false），但 Cartographer 不用 EKF 的数据 ，它吃原始数据。EKF 在此时的作用是为 AMCL 提供完整位姿轨迹 （双 odom 架构：EKF→odom_ekfcombined，Cartographer→odom_cartocombined，两条独立的 TF 边指向 base_footprint）。

数据流图

#mermaid-svg-HwjVNfSQkjxcIlTF{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-HwjVNfSQkjxcIlTF .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-HwjVNfSQkjxcIlTF .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-HwjVNfSQkjxcIlTF .error-icon{fill:#552222;}#mermaid-svg-HwjVNfSQkjxcIlTF .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-HwjVNfSQkjxcIlTF .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-HwjVNfSQkjxcIlTF .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-HwjVNfSQkjxcIlTF .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-HwjVNfSQkjxcIlTF .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-HwjVNfSQkjxcIlTF .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-HwjVNfSQkjxcIlTF .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-HwjVNfSQkjxcIlTF .marker{fill:#333333;stroke:#333333;}#mermaid-svg-HwjVNfSQkjxcIlTF .marker.cross{stroke:#333333;}#mermaid-svg-HwjVNfSQkjxcIlTF svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-HwjVNfSQkjxcIlTF p{margin:0;}#mermaid-svg-HwjVNfSQkjxcIlTF .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-HwjVNfSQkjxcIlTF .cluster-label text{fill:#333;}#mermaid-svg-HwjVNfSQkjxcIlTF .cluster-label span{color:#333;}#mermaid-svg-HwjVNfSQkjxcIlTF .cluster-label span p{background-color:transparent;}#mermaid-svg-HwjVNfSQkjxcIlTF .label text,#mermaid-svg-HwjVNfSQkjxcIlTF span{fill:#333;color:#333;}#mermaid-svg-HwjVNfSQkjxcIlTF .node rect,#mermaid-svg-HwjVNfSQkjxcIlTF .node circle,#mermaid-svg-HwjVNfSQkjxcIlTF .node ellipse,#mermaid-svg-HwjVNfSQkjxcIlTF .node polygon,#mermaid-svg-HwjVNfSQkjxcIlTF .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-HwjVNfSQkjxcIlTF .rough-node .label text,#mermaid-svg-HwjVNfSQkjxcIlTF .node .label text,#mermaid-svg-HwjVNfSQkjxcIlTF .image-shape .label,#mermaid-svg-HwjVNfSQkjxcIlTF .icon-shape .label{text-anchor:middle;}#mermaid-svg-HwjVNfSQkjxcIlTF .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-HwjVNfSQkjxcIlTF .rough-node .label,#mermaid-svg-HwjVNfSQkjxcIlTF .node .label,#mermaid-svg-HwjVNfSQkjxcIlTF .image-shape .label,#mermaid-svg-HwjVNfSQkjxcIlTF .icon-shape .label{text-align:center;}#mermaid-svg-HwjVNfSQkjxcIlTF .node.clickable{cursor:pointer;}#mermaid-svg-HwjVNfSQkjxcIlTF .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-HwjVNfSQkjxcIlTF .arrowheadPath{fill:#333333;}#mermaid-svg-HwjVNfSQkjxcIlTF .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-HwjVNfSQkjxcIlTF .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-HwjVNfSQkjxcIlTF .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-HwjVNfSQkjxcIlTF .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-HwjVNfSQkjxcIlTF .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-HwjVNfSQkjxcIlTF .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-HwjVNfSQkjxcIlTF .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-HwjVNfSQkjxcIlTF .cluster text{fill:#333;}#mermaid-svg-HwjVNfSQkjxcIlTF .cluster span{color:#333;}#mermaid-svg-HwjVNfSQkjxcIlTF div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-HwjVNfSQkjxcIlTF .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-HwjVNfSQkjxcIlTF rect.text{fill:none;stroke-width:0;}#mermaid-svg-HwjVNfSQkjxcIlTF .icon-shape,#mermaid-svg-HwjVNfSQkjxcIlTF .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-HwjVNfSQkjxcIlTF .icon-shape p,#mermaid-svg-HwjVNfSQkjxcIlTF .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-HwjVNfSQkjxcIlTF .icon-shape .label rect,#mermaid-svg-HwjVNfSQkjxcIlTF .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-HwjVNfSQkjxcIlTF .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-HwjVNfSQkjxcIlTF .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-HwjVNfSQkjxcIlTF :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} Cartographer 三步
原始数据输入
launch 启动
turn_on_wheeltec_robot

carto_slam:=true

EKF用ekf_carto 3D/30Hz
wheeltec_lidar

激光雷达
cartographer_node
cartographer_occupancy_grid_node

子图→/map
odom_raw

原始里程计
imu/stm32_raw

原始IMU
scan

激光
① 猜：里程计+IMU 增量预测位姿
② 纠：激光与子图相关扫描匹配

Ceres 精修+贴图
③ 调：回环检测 min_score=0.65

每50节点全局优化
内部子图
map

栅格地图
odom_cartocombined→base_footprint

provide_odom_frame

5.3 slam_toolbox 建图（online_async_launch.py）

slam_toolbox 建图原理：

​ 与cartographer建图原理完全相同，不同在于slam_toobox需要处理好的odom数据/odom_ekfcombined，并且更轻量。

  1. 猜：采用EKF融合后的/odom_ekfcombined里程计数据，通过运动预测机器人位置。
  2. 纠：在机器人预测位置获取激光雷达/scan话题数据，与已有地图进行扫描匹配（相关性搜索），找到最吻合位置，修正预测位置，并把新扫描结果贴进地图。
  3. 调：在机器人扫描到曾经来过的位置时，进行回环检测确认，然后进行全局图优化（Ceres），调整所有相关节点位置，消除累积漂移误差。

slam_toolbox 建图原理与 Cartographer 相同（猜→纠→调），区别在于：

  • 输入里程计 ：用 EKF 处理好的 /odom_ekfcombined（remap 自默认 /odom）；
  • 更轻量：实现更简单；
  • 支持模式切换 ：mode: mapping（建图）/ localization（重定位，可加载已有地图）；
  • 异步（async）：扫描匹配和建图分线程，响应快（本文件用的是 async 模式）。

关键配置（mapper_params_online_async.yaml）：Ceres 求解器、do_loop_closing: true 开回环、resolution: 0.05（5cm/格）、map_update_interval: 5.0（5 秒更新一次地图）。

slam_toolbox建图流程：
  1. 启动launch文件，会启动3个结点，ros2 launch wheeltec_slam_toolbox online_async_launch.py：
    1. 启动小车底层（turn_on_wheeltec_robot.launch.py）→ 底盘驱动 + EKF + 模型/TF
    2. 启动激光雷达（wheeltec_lidar.launch.py）→ 提供 /scan 激光数据
    3. 启动 async_slam_toolbox_node（核心建图）→ 融合 /scan + /odom_ekfcombined 建图，发布 /map
  2. 控制机器人缓慢运动进行边定位边建图。
  3. 采用命令保存地图，ros2 run nav2_map_server map_saver_cli -f ~/map/my_map
数据流图

#mermaid-svg-3jd8xGrXZjyA4wvP{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-3jd8xGrXZjyA4wvP .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-3jd8xGrXZjyA4wvP .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-3jd8xGrXZjyA4wvP .error-icon{fill:#552222;}#mermaid-svg-3jd8xGrXZjyA4wvP .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-3jd8xGrXZjyA4wvP .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-3jd8xGrXZjyA4wvP .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-3jd8xGrXZjyA4wvP .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-3jd8xGrXZjyA4wvP .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-3jd8xGrXZjyA4wvP .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-3jd8xGrXZjyA4wvP .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-3jd8xGrXZjyA4wvP .marker{fill:#333333;stroke:#333333;}#mermaid-svg-3jd8xGrXZjyA4wvP .marker.cross{stroke:#333333;}#mermaid-svg-3jd8xGrXZjyA4wvP svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-3jd8xGrXZjyA4wvP p{margin:0;}#mermaid-svg-3jd8xGrXZjyA4wvP .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-3jd8xGrXZjyA4wvP .cluster-label text{fill:#333;}#mermaid-svg-3jd8xGrXZjyA4wvP .cluster-label span{color:#333;}#mermaid-svg-3jd8xGrXZjyA4wvP .cluster-label span p{background-color:transparent;}#mermaid-svg-3jd8xGrXZjyA4wvP .label text,#mermaid-svg-3jd8xGrXZjyA4wvP span{fill:#333;color:#333;}#mermaid-svg-3jd8xGrXZjyA4wvP .node rect,#mermaid-svg-3jd8xGrXZjyA4wvP .node circle,#mermaid-svg-3jd8xGrXZjyA4wvP .node ellipse,#mermaid-svg-3jd8xGrXZjyA4wvP .node polygon,#mermaid-svg-3jd8xGrXZjyA4wvP .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-3jd8xGrXZjyA4wvP .rough-node .label text,#mermaid-svg-3jd8xGrXZjyA4wvP .node .label text,#mermaid-svg-3jd8xGrXZjyA4wvP .image-shape .label,#mermaid-svg-3jd8xGrXZjyA4wvP .icon-shape .label{text-anchor:middle;}#mermaid-svg-3jd8xGrXZjyA4wvP .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-3jd8xGrXZjyA4wvP .rough-node .label,#mermaid-svg-3jd8xGrXZjyA4wvP .node .label,#mermaid-svg-3jd8xGrXZjyA4wvP .image-shape .label,#mermaid-svg-3jd8xGrXZjyA4wvP .icon-shape .label{text-align:center;}#mermaid-svg-3jd8xGrXZjyA4wvP .node.clickable{cursor:pointer;}#mermaid-svg-3jd8xGrXZjyA4wvP .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-3jd8xGrXZjyA4wvP .arrowheadPath{fill:#333333;}#mermaid-svg-3jd8xGrXZjyA4wvP .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-3jd8xGrXZjyA4wvP .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-3jd8xGrXZjyA4wvP .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-3jd8xGrXZjyA4wvP .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-3jd8xGrXZjyA4wvP .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-3jd8xGrXZjyA4wvP .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-3jd8xGrXZjyA4wvP .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-3jd8xGrXZjyA4wvP .cluster text{fill:#333;}#mermaid-svg-3jd8xGrXZjyA4wvP .cluster span{color:#333;}#mermaid-svg-3jd8xGrXZjyA4wvP div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-3jd8xGrXZjyA4wvP .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-3jd8xGrXZjyA4wvP rect.text{fill:none;stroke-width:0;}#mermaid-svg-3jd8xGrXZjyA4wvP .icon-shape,#mermaid-svg-3jd8xGrXZjyA4wvP .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-3jd8xGrXZjyA4wvP .icon-shape p,#mermaid-svg-3jd8xGrXZjyA4wvP .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-3jd8xGrXZjyA4wvP .icon-shape .label rect,#mermaid-svg-3jd8xGrXZjyA4wvP .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-3jd8xGrXZjyA4wvP .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-3jd8xGrXZjyA4wvP .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-3jd8xGrXZjyA4wvP :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} slam_toolbox 流程
输入3
launch 启动
启动机器人底层结点
robot_lidar

激光雷达
async_slam_toolbox_node
odom_ekfcombined

remap 自 /odom
scan

激光
① 猜：EKF里程计运动预测
② 纠：激光与地图相关性搜索匹配
③ 调：回环检测+ Ceres 全局优化
map

栅格地图
odom_ekfcombined→base_footprint

由EKF发布

模块六：NAV2 多点导航（nav2.launch.py）

6.1 模块功能

nav2.launch.py 是导航总入口，一次启动四组功能：

序号 启动内容 作用
① waypoint_cycle 节点 多点巡航：RViz 点多个航点，按序循环巡逻
② turn_on_robot.launch.py（robot_nav:=true） 底盘驱动 + 去漂移 IMU + EKF(导航配置) + 模型TF
③ robot_lidar.launch.py 激光雷达提供 /scan
④ NAV2 官方 bringup_launch.py AMCL 定位 + 全局规划 + 局部控制 + 行为树

6.3 导航启动流程

​ nav2导航启动命令ros2 launch wheeltec_robot_nav2 wheeltec_nav2.launch.py map:=~/map/my_map.yaml，启动后会打开以下4个结点文件：

  1. 启动 waypoint_cycle（多点巡航节点）：在 RViz 里点多个目标点（航点），它就按顺序发布导航目标，让小车自动走完所有点并循环（巡检/巡航用）。
    1. 接收RViz的/clicked_pose话题在map上点击的导航目标，并通过/path_pose话题在map上发布可视化的导航目标点序号和路径，RViz通过订阅该话题在map上可视化显示。
    2. 通过订阅bt_navigator导航目标完成状态的/navigate_to_pose/_action/status话题，来获取上个导航目标点的完成状态。
      • 在成功时：执行到指定目标导航点的业务逻辑，包括采用相机传感器进行拍照等。然后通过/goal_pose话题向bt_navigator发布下一个导航目标，当终点导航目标被执行后，回到起点导航目标。
      • 在失败时：尝试再次重复进行导航，再次导航失败时，通过/goal_pose话题向bt_navigator发布下一个导航目标，当终点导航目标被执行后，回到起点导航目标。
  2. 启动小车底层（turn_on_robot.launch.py）→ 底盘驱动结点 + EKF结点+ 模型/TF
    • 底盘驱动结点（wheeltec_robot_node）：发布：/odom_raw（原始里程计）、/imu/stm32_raw（原始IMU）、订阅：/cmd_vel（速度指令）
    • 去零点漂移结点（imu_processor）：订阅：/imu/stm32_raw，发布：/imu/imu_filtered（去漂移IMU）
    • EKF 位姿融合结点（ekf_filter_node，用 ekf_nav.yaml）：订阅：/odom_raw + /imu/imu_filtered、发布：/odom_ekfcombined（融合位姿）+ odom_ekfcombined→base TF
    • 模型/TF 结点（robot_state_publisher + joint_state_publisher + static_transform）
  3. 启动激光雷达（wheeltec_lidar.launch.py）→ 提供 /scan 激光数据
  4. 启动 NAV2 核心（nav2官方的bringup_launch.py）→ AMCL 定位 + 全局规划 + 局部控制 + 行为树
    • bringup_launch.py会启动localization_launch.py定位模块、navigation_launch.py导航模块、根据需要可以配置启动slam_launch.py建图模块。
    • localization_launch.py定位模块：map_server进行加载地图yaml文件，并发布/map话题
    • AMCL进行定位所在地图位置，并去除里程计漂移误差，发布map->odom的TF变换
    • navigation_launch.py导航模块：bt_navigator订阅/goal_pose话题获取导航目标，并根据导航行为树决策下步导航动作，先进行全局规划、控制机器人运动、卡住进行恢复并重新规划
    • planner_server进行全局路径规划，根据A*算法规划一条当前位置到目标点的无障碍物路径，并发布到/plan话题中
    • controller_server进行局部控制机器人按照全局路径运动，发布/cmd_vel话题数据
    • behavior_server实现出现运动卡住等异常时进行恢复

第 1 步：加载地图 + 定位

  1. map_server 加载地图 yaml 文件 → 发布 /map 话题（栅格地图）
  2. AMCL 订阅 /scan（激光）+ /odom_ekfcombined（EKF里程计），用类似gmapping建图的粒子滤波算法，通过采用大量粒子结合/odom_ekfcombined里程计数据进行预测机器人位置，并结合/scan数据匹配，增加匹配度高的粒子数量，淘汰匹配度低的粒子，最后粒子进行收敛确定机器人所在位置，并根据激光数据匹配在 map 中定位机器人位置。发布 map→odom_ekfcombined 的 TF 变换。这个过程在导航过程会持续进行，每次接收/scan数据时进行，一边进行定位map位置，并修正里程计漂移误差。

第 2 步：RViz 点击目标 → 开始导航

用户在 RViz 点"2D Nav Goal" → 发布目标到 /goal_pose 话题，之后按以下 4 个子流程执行：

  1. bt_navigator：行为树决策（导航大脑）：bt_navigator 会接收到 /goal_pose话题数据的导航目标，然后开始进行导航。根据 XML 行为树决定下一步动作："先全局规划 → 再控制运动 → 卡住了就恢复并重新规划"。
  2. planner_server：全局路径规划（地图师）：用 A* 算法结合全局代价地图，从当前定位位置到目标点规划一条【无障碍】路径 → 发布到 /plan 话题（全局路径）。
  3. controller_server：局部控制（司机）：结合 /scan 数据和局部代价地图（实时避障）和/odom_ekfcombined数据确定位置，控制机器人沿全局路径/plan运动 → 发布 /cmd_vel 速度指令→ 底盘驱动收到 /cmd_vel → 下位机 → 电机转动。
  4. behavior_server：恢复（救援队）：当机器人运动卡住 / 规划失败时，执行恢复动作（旋转、清除代价地图等）→ 然后 bt_navigator 重新规划。

6.4 waypoint_cycle 多点巡航原理

waypoint_cycle 节点（nav2_waypoint_cycle 包）实现无限循环巡逻，在 RViz 里点多个目标点（航点），它就按顺序发布导航目标，让小车自动走完所有点并循环（巡检/巡航用）。

  1. 接收RViz的/clicked_pose话题在map上点击的导航目标，并通过/path_pose话题在map上发布可视化的导航目标点序号和路径，RViz通过订阅该话题在map上可视化显示。
  2. 通过订阅bt_navigator导航目标完成状态的/navigate_to_pose/_action/status话题，来获取上个导航目标点的完成状态。在成功时：执行到指定目标导航点的业务逻辑，包括采用相机传感器进行拍照等。然后通过/goal_pose话题向bt_navigator发布下一个导航目标，当终点导航目标被执行后，回到起点导航目标。在失败时：尝试再次重复进行导航，再次导航失败时，通过/goal_pose话题向bt_navigator发布下一个导航目标，当终点导航目标被执行后，回到起点导航目标。

导航闭环 ：waypoint_cycle 发 /goal_pose → bt_navigator → 内部转成 NavigateToPose action → 规划/控制 → 底盘 → 完成后 status 话题反馈 → waypoint_cycle 发下一个点。

6.4 数据流图

#mermaid-svg-86Xgj51bR55z8v4E{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-86Xgj51bR55z8v4E .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-86Xgj51bR55z8v4E .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-86Xgj51bR55z8v4E .error-icon{fill:#552222;}#mermaid-svg-86Xgj51bR55z8v4E .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-86Xgj51bR55z8v4E .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-86Xgj51bR55z8v4E .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-86Xgj51bR55z8v4E .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-86Xgj51bR55z8v4E .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-86Xgj51bR55z8v4E .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-86Xgj51bR55z8v4E .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-86Xgj51bR55z8v4E .marker{fill:#333333;stroke:#333333;}#mermaid-svg-86Xgj51bR55z8v4E .marker.cross{stroke:#333333;}#mermaid-svg-86Xgj51bR55z8v4E svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-86Xgj51bR55z8v4E p{margin:0;}#mermaid-svg-86Xgj51bR55z8v4E .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-86Xgj51bR55z8v4E .cluster-label text{fill:#333;}#mermaid-svg-86Xgj51bR55z8v4E .cluster-label span{color:#333;}#mermaid-svg-86Xgj51bR55z8v4E .cluster-label span p{background-color:transparent;}#mermaid-svg-86Xgj51bR55z8v4E .label text,#mermaid-svg-86Xgj51bR55z8v4E span{fill:#333;color:#333;}#mermaid-svg-86Xgj51bR55z8v4E .node rect,#mermaid-svg-86Xgj51bR55z8v4E .node circle,#mermaid-svg-86Xgj51bR55z8v4E .node ellipse,#mermaid-svg-86Xgj51bR55z8v4E .node polygon,#mermaid-svg-86Xgj51bR55z8v4E .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-86Xgj51bR55z8v4E .rough-node .label text,#mermaid-svg-86Xgj51bR55z8v4E .node .label text,#mermaid-svg-86Xgj51bR55z8v4E .image-shape .label,#mermaid-svg-86Xgj51bR55z8v4E .icon-shape .label{text-anchor:middle;}#mermaid-svg-86Xgj51bR55z8v4E .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-86Xgj51bR55z8v4E .rough-node .label,#mermaid-svg-86Xgj51bR55z8v4E .node .label,#mermaid-svg-86Xgj51bR55z8v4E .image-shape .label,#mermaid-svg-86Xgj51bR55z8v4E .icon-shape .label{text-align:center;}#mermaid-svg-86Xgj51bR55z8v4E .node.clickable{cursor:pointer;}#mermaid-svg-86Xgj51bR55z8v4E .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-86Xgj51bR55z8v4E .arrowheadPath{fill:#333333;}#mermaid-svg-86Xgj51bR55z8v4E .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-86Xgj51bR55z8v4E .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-86Xgj51bR55z8v4E .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-86Xgj51bR55z8v4E .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-86Xgj51bR55z8v4E .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-86Xgj51bR55z8v4E .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-86Xgj51bR55z8v4E .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-86Xgj51bR55z8v4E .cluster text{fill:#333;}#mermaid-svg-86Xgj51bR55z8v4E .cluster span{color:#333;}#mermaid-svg-86Xgj51bR55z8v4E div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-86Xgj51bR55z8v4E .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-86Xgj51bR55z8v4E rect.text{fill:none;stroke-width:0;}#mermaid-svg-86Xgj51bR55z8v4E .icon-shape,#mermaid-svg-86Xgj51bR55z8v4E .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-86Xgj51bR55z8v4E .icon-shape p,#mermaid-svg-86Xgj51bR55z8v4E .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-86Xgj51bR55z8v4E .icon-shape .label rect,#mermaid-svg-86Xgj51bR55z8v4E .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-86Xgj51bR55z8v4E .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-86Xgj51bR55z8v4E .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-86Xgj51bR55z8v4E :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 输入输出
NAV2 内部
wheeltec_nav2.launch.py 四组启动
map→odom_ekfcombined TF
路径
/plan
局部代价地图避障
恢复指令
电机驱动
① waypoint_cycle

多点巡航
② 启动机器人底层节点
③ robot_lidar

激光雷达
④ bringup_launch.py

NAV2核心
map_server

加载地图
AMCL

粒子滤波定位
bt_navigator

行为树决策
planner_server

A*全局路径规划
controller_server

MPPI局部控制+避障
behavior_server

卡住恢复
scan
odom_ekfcombined
map
goal_pose
cmd_vel
clicked_point RViz点击
navigate_to_pose/_action/status
TF树
底盘运动

附录：数据流总览（一次完整导航）

#mermaid-svg-oJGt1lFZsXIVG58A{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-oJGt1lFZsXIVG58A .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-oJGt1lFZsXIVG58A .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-oJGt1lFZsXIVG58A .error-icon{fill:#552222;}#mermaid-svg-oJGt1lFZsXIVG58A .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-oJGt1lFZsXIVG58A .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-oJGt1lFZsXIVG58A .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-oJGt1lFZsXIVG58A .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-oJGt1lFZsXIVG58A .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-oJGt1lFZsXIVG58A .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-oJGt1lFZsXIVG58A .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-oJGt1lFZsXIVG58A .marker{fill:#333333;stroke:#333333;}#mermaid-svg-oJGt1lFZsXIVG58A .marker.cross{stroke:#333333;}#mermaid-svg-oJGt1lFZsXIVG58A svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-oJGt1lFZsXIVG58A p{margin:0;}#mermaid-svg-oJGt1lFZsXIVG58A .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-oJGt1lFZsXIVG58A .cluster-label text{fill:#333;}#mermaid-svg-oJGt1lFZsXIVG58A .cluster-label span{color:#333;}#mermaid-svg-oJGt1lFZsXIVG58A .cluster-label span p{background-color:transparent;}#mermaid-svg-oJGt1lFZsXIVG58A .label text,#mermaid-svg-oJGt1lFZsXIVG58A span{fill:#333;color:#333;}#mermaid-svg-oJGt1lFZsXIVG58A .node rect,#mermaid-svg-oJGt1lFZsXIVG58A .node circle,#mermaid-svg-oJGt1lFZsXIVG58A .node ellipse,#mermaid-svg-oJGt1lFZsXIVG58A .node polygon,#mermaid-svg-oJGt1lFZsXIVG58A .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-oJGt1lFZsXIVG58A .rough-node .label text,#mermaid-svg-oJGt1lFZsXIVG58A .node .label text,#mermaid-svg-oJGt1lFZsXIVG58A .image-shape .label,#mermaid-svg-oJGt1lFZsXIVG58A .icon-shape .label{text-anchor:middle;}#mermaid-svg-oJGt1lFZsXIVG58A .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-oJGt1lFZsXIVG58A .rough-node .label,#mermaid-svg-oJGt1lFZsXIVG58A .node .label,#mermaid-svg-oJGt1lFZsXIVG58A .image-shape .label,#mermaid-svg-oJGt1lFZsXIVG58A .icon-shape .label{text-align:center;}#mermaid-svg-oJGt1lFZsXIVG58A .node.clickable{cursor:pointer;}#mermaid-svg-oJGt1lFZsXIVG58A .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-oJGt1lFZsXIVG58A .arrowheadPath{fill:#333333;}#mermaid-svg-oJGt1lFZsXIVG58A .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-oJGt1lFZsXIVG58A .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-oJGt1lFZsXIVG58A .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-oJGt1lFZsXIVG58A .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-oJGt1lFZsXIVG58A .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-oJGt1lFZsXIVG58A .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-oJGt1lFZsXIVG58A .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-oJGt1lFZsXIVG58A .cluster text{fill:#333;}#mermaid-svg-oJGt1lFZsXIVG58A .cluster span{color:#333;}#mermaid-svg-oJGt1lFZsXIVG58A div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-oJGt1lFZsXIVG58A .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-oJGt1lFZsXIVG58A rect.text{fill:none;stroke-width:0;}#mermaid-svg-oJGt1lFZsXIVG58A .icon-shape,#mermaid-svg-oJGt1lFZsXIVG58A .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-oJGt1lFZsXIVG58A .icon-shape p,#mermaid-svg-oJGt1lFZsXIVG58A .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-oJGt1lFZsXIVG58A .icon-shape .label rect,#mermaid-svg-oJGt1lFZsXIVG58A .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-oJGt1lFZsXIVG58A .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-oJGt1lFZsXIVG58A .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-oJGt1lFZsXIVG58A :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} /goal_pose
全局规划请求
/plan 路径
/cmd_vel
串口帧
编码器/IMU上报
/odom_raw /imu/stm32_raw
/odom_ekfcombined
/scan
map→odom TF 修正
定位结果
导航状态反馈
用户在RViz点目标
bt_navigator
planner_server
controller_server
turn_on_robot_node
STM32 底盘
imu_processor+EKF
AMCL 定位
激光雷达
waypoint_cycle

发下一个点

闭环要点：底层驱动（模块1）→ 数据预处理（模块2/3/4）→ 建图/导航（模块5/6）→ 输出速度回到底盘，形成完整控制系统。

相关推荐
小程序设计1 小时前
机械设计光伏电池板清洁机器人
机器人
雯宝1 小时前
7. USB 网络适配器驱动
linux
rest10242 小时前
使用内存探测bpf
linux
重生之我来学Python2 小时前
Docker套装的简介、安装、超级详细教程
linux·docker·容器·eureka·github
Ruiery3 小时前
Linux 6.6内核 PCIe 深度解析（十）：PCIe Port Driver（portdrv）— 一个物理 Port，拆出五个逻辑服务
linux·运维·服务器
Pointer Pursuit3 小时前
C++11特性（二）
前端·c++·算法
沫儿笙3 小时前
安川机器人氩弧焊省气装置
机器人·自动化
tudousisi2223 小时前
CSP 第3题训练 Day 5｜CSP202509C HTTP 头信息｜Blog A
c++
码农小韩4 小时前
Linux应用开发（二）——Linux的文件IO
linux·嵌入式软件·嵌入式操作系统·linux应用
热门推荐
01国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）02GitHub 镜像站点03蜘蛛磁力 搜索引擎大全，如何使用蜘蛛磁力查找磁力链接04ZCode 周末送额度活动开启：3 亿 Token 免费领取05历年考研数学一、数学二、数学三真题试卷及答案PDF062026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？07微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新08Agnes AI 免费 API 接入指南：文本、生图、生视频，一套接口全免费09AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路10VSCode+CodeX扩展，无法启动