4. 移动底盘基础：差速模型、里程计发布、激光雷达驱动与数据预处理
各位同学，欢迎来到第四讲。从这节课开始，我们正式进入「硬件落地」环节。你想想看，Nav2 导航再厉害，它也得有个「身体」去执行——这个身体就是移动底盘。
我见过不少做机械臂集成的朋友，上来就调 Nav2 参数，结果机器人原地打转、定位漂移。为什么？底盘模型没搞对，里程计数据是错的。说白了，导航的根基就是底盘。
这一章，我们就把底盘的三件套讲透：差速运动模型、里程计发布、激光雷达驱动与数据预处理。这三样东西，是 Nav2 能跑起来的前提。
本章核心目标：让你的机器人底盘能正确运动、能准确汇报位置、能感知周围环境。任何一个环节出问题，导航都是空中楼阁。
4.1 差速运动模型：机器人的「两条腿」
差速模型，说白了就是两个轮子各自独立驱动，通过转速差来实现转向。你想想看，坦克怎么转弯？左边履带停、右边履带走，它就左转。差速底盘一个道理。
我最早做机器人时，用的是四轮差速底盘，结果发现转弯时轮子在地上「搓」得厉害。后来换成两轮差速加万向轮，才明白——差速模型的核心是「纯滚动、无滑动」，但这只是理想情况。
4.1.1 正运动学：从轮速到机器人速度
正运动学，就是已知左右轮转速，求机器人的线速度和角速度。公式很简单：
v = (v_right + v_left) / 2
ω = (v_right - v_left) / d其中 v_right 和 v_left 是左右轮的线速度，d 是轮间距。嗯，这里要注意——轮间距 d 的测量精度直接影响角速度计算。我见过有人用尺子量轮距，结果差了 2 毫米，导航时机器人就一直在画弧。
我的经验：轮间距 d 最好用「标定法」确定。让机器人原地旋转 10 圈，对比实际角度和理论角度，反推出精确的 d 值。别偷懒，这一步值得花时间。
4.1.2 逆运动学：从目标速度到轮速
逆运动学反过来，给定目标线速度和角速度，算出左右轮应该转多快：
v_left = v - ω * d / 2
v_right = v + ω * d / 2这个公式在 Nav2 的 cmd_vel 话题下发速度指令时，会被底层控制器反复调用。如果你的底盘是 Arduino 或 STM32 直驱，就得在 MCU 里实现这个公式。
| 参数 | 符号 | 单位 | 说明 |
|---|---|---|---|
| 机器人线速度 | v | m/s | 前进方向速度 |
| 机器人角速度 | ω | rad/s | 绕中心旋转速度 |
| 轮间距 | d | m | 左右轮中心距离 |
| 车轮半径 | r | m | 用于编码器换算 |
4.2 里程计发布：机器人的「记忆」
里程计，就是机器人对自己位置的「记忆」。它通过编码器数据，不断推算自己走了多远、转了多少度。这个过程叫航迹推算（Dead Reckoning）。
但你要知道——里程计是有累积误差的。轮子打滑、地面不平、编码器分辨率不够，都会让误差越滚越大。Nav2 之所以需要激光雷达和 AMCL，就是为了修正这个误差。
4.2.1 编码器数据采集
编码器通常装在电机尾部，每转一圈输出固定数量的脉冲。比如 500 线编码器，转一圈输出 500 个脉冲。通过脉冲计数，就能算出轮子转了多少圈：
wheel_distance = pulse_count / pulses_per_rev * 2 * π * wheel_radius我在项目中遇到过一个问题：编码器脉冲频率太高，Arduino 的中断服务函数处理不过来，导致丢步。后来换了带硬件计数器的 STM32，才彻底解决。
注意：编码器数据必须做「防抖」处理。电机启停瞬间会产生机械抖动，导致脉冲误计数。我一般会在软件里加一个 5ms 的去抖延时，或者用硬件施密特触发器。
4.2.2 构建 Odometry 消息
在 ROS2 中，里程计数据通过 nav_msgs/msg/Odometry 消息发布。这个消息包含位置、姿态、线速度、角速度，以及对应的协方差矩阵。
# 伪代码示例：发布里程计
odom_msg = Odometry()
odom_msg.header.stamp = now()
odom_msg.header.frame_id = "odom"
odom_msg.child_frame_id = "base_footprint"
# 位置
odom_msg.pose.pose.position.x = x
odom_msg.pose.pose.position.y = y
odom_msg.pose.pose.orientation.z = sin(theta/2)
odom_msg.pose.pose.orientation.w = cos(theta/2)
# 速度
odom_msg.twist.twist.linear.x = v
odom_msg.twist.twist.angular.z = omega
odom_pub.publish(odom_msg)这里有个坑——frame_id 和 child_frame_id 必须和 TF 树一致。我见过有人把 frame_id 写成 "map"，结果 TF 树直接乱掉，AMCL 死活定位不准。
4.2.3 TF 变换发布
Nav2 依赖 TF 树来知道各个坐标系之间的关系。对于差速底盘，最基本的 TF 关系是：
- odom → base_footprint：由里程计节点发布，表示机器人在地图中的位置
- base_footprint → base_link：通常固定，表示机器人本体坐标系
- base_link → laser_frame：由激光雷达驱动发布，表示雷达安装位置
我个人习惯把里程计发布和 TF 发布放在同一个节点里，保证时间戳同步。分开写的话，容易出现「里程计更新了，但 TF 没更新」的尴尬情况。
4.3 激光雷达驱动与数据预处理
激光雷达是 Nav2 的「眼睛」。没有它，建图和定位都无从谈起。市面上常见的雷达有 RPLIDAR、YDLIDAR、Sick、Hokuyo 等，驱动方式大同小异。
4.3.1 驱动节点配置
以 RPLIDAR A1 为例，ROS2 驱动节点启动命令如下：
ros2 launch sllidar_ros2 sllidar_launch.py
serial_port:=/dev/ttyUSB0
frame_id:=laser_frame
angle_min:=-3.14159
angle_max:=3.14159启动后，雷达会发布 /scan 话题，类型为 sensor_msgs/msg/LaserScan。你可以用 ros2 topic echo /scan 查看原始数据。
避坑指南：我曾经因为 USB 转串口芯片的驱动没装好，折腾了一整天。建议用 ls -l /dev/ttyUSB* 确认设备存在，再用 sudo chmod 666 /dev/ttyUSB0 给权限。或者更优雅的方式——写一个 udev 规则。
4.3.2 LaserScan 消息结构
理解 LaserScan 消息的结构，对后续数据预处理至关重要：
| 字段 | 类型 | 说明 |
|---|---|---|
| header.frame_id | string | 雷达坐标系名称 |
| angle_min | float32 | 扫描起始角度 (rad) |
| angle_max | float32 | 扫描结束角度 (rad) |
| angle_increment | float32 | 角度分辨率 (rad) |
| time_increment | float32 | 每帧时间间隔 (s) |
| scan_time | float32 | 扫描周期 (s) |
| range_min | float32 | 最小有效距离 (m) |
| range_max | float32 | 最大有效距离 (m) |
| ranges | float32[] | 距离数据数组 |
| intensities | float32[] | 信号强度数组 |
4.3.3 数据预处理：去畸变与滤波
原始雷达数据不能直接用。为什么？因为雷达扫描需要时间（通常 50-100ms），如果机器人在这段时间内运动，扫描结果就会「扭曲」。这叫运动畸变。
去畸变的方法：
- 获取时间戳：记录每帧扫描的起始和结束时间
- 查询里程计：在这段时间内，机器人移动了多少
- 反向补偿：把每个激光点「还原」到扫描起始时刻的位置
除了去畸变，还有几个常见的滤波操作：
- 去除无效值：ranges 中的 0 或 inf 要过滤掉
- 限幅滤波：超出 range_min/range_max 的数据直接丢弃
- 中值滤波：消除孤立的噪点（比如灰尘、小飞虫）
注意：滤波不能过度。我见过有人把中值滤波窗口设得太大，结果墙角都被「磨平」了，建图时房间轮廓全变形。窗口大小 3-5 就够用。
4.3.4 坐标系标定
雷达安装位置必须精确测量。你需要知道雷达相对于机器人中心的 x、y、z 偏移 和 roll、pitch、yaw 角度。
这些参数通常在雷达驱动的 URDF 文件中配置：
<joint name="laser_joint" type="fixed">
<parent link="base_link"/>
<child link="laser_frame"/>
<origin xyz="0.2 0.0 0.1" rpy="0.0 0.0 0.0"/>
</joint>如果标定不准，Nav2 的 costmap 会出现「偏移」——明明前面是墙，地图上却显示是空的。我遇到过最离谱的一次，雷达装歪了 3 度，结果机器人导航时一直贴着墙走，怎么调参数都没用。最后拿水平尺一量，才发现问题。