ROS2 自动定位与导航小车 — 完整知识手册

树莓派 4B + Ubuntu 24.04 + ROS2 Jazzy + STM32F103 + 激光雷达
从零搭建一台能自主导航的 ROS2 小车

---

第 1 章：树莓派 4B 环境搭建

本章目标： 给树莓派 4B 装上 Ubuntu 24.04 系统，能通过电脑 SSH 远程连接。

1.1 烧录系统

所需工具

工具	作用
Raspberry Pi Imager	烧录系统到 SD 卡
SD 卡（32G 以上）	存储系统
IP Scanner	扫描树莓派 IP
MobaXterm	SSH 远程连接

烧录步骤

1. PC 插入 SD 卡
2. 打开 Pi Imager
   • Device: Raspberry Pi 4
     
   • OS: Other general purpose OS → Ubuntu → Ubuntu Server 24.04.3 LTS (64-bit)
     - SD Card: 你的 SD 卡
     
3. 点击 NEXT → Edit Settings，设置 WiFi 名称和密码，开启 SSH
4. 点击 WRITE → 等待烧录完成
5. SD 卡插入树莓派 4B → 上电

查找树莓派 IP

1. 手机/电脑连接同一个 WiFi

2. 打开 IP Scanner，扫描 192.168.<WiFi> 网段

3. 找到设备名称为 raspberrypi 的 IP

4. 记下这个 IP

SSH 远程连接

打开 MobaXterm → Session → SSH，输入 IP 地址 → 22 端口 → OK
用户名：ubuntu，密码：ubuntu（首次登录会要求修改）

1.2 安装 ROS2 Jazzy

# 一键安装 ROS（推荐）
wget http://fishros.com/install -O fishros && . fishros ## 鱼香ROS开源的一键安装工具

输入密码后选择 [5]一键配置：系统源 。
接着选择 [2]:更换系统源并清理第三方源 。

完成后选择 [2]:不添加ROS/ROS2源 。

更新软件包：

输入以下命令：
sudo apt update && sudo apt upgrade -y
编译并安装 raspberrypi-lvgl-terminal（树莓派4B终端） ：

输入以下命令：

cd ~
sudo apt install git
git clone https://github.com/CaddonThaw/raspberrypi-lvgl-terminal.git
./scripts/build.sh
sudo ./scripts/install.sh --enable

等待安装完成后可以看到显示的画面：

# 验证
ros2 run turtlesim turtlesim_node

能看到小海龟窗口则安装成功。

1.3 固定 USB 设备名

激光雷达和 STM32 通过 USB 连接树莓派，默认名称为 ttyUSB0/ttyUSB1。每次插入顺序不同，名称可能互换。

解决方案： 编写 udev 规则，根据 USB 芯片 ID 固定设备名。

# 查看 USB 设备 ID
lsusb
# 输出示例：
# Bus 001 Device 014: ID 1a86:7523  ← 激光雷达
# Bus 001 Device 015: ID 1a86:7523  ← STM32

# 创建 udev 规则
sudo touch /etc/udev/rules.d/stm32_dev.rules
sudo nano /etc/udev/rules.d/stm32_dev.rules

写入：

KERNEL=="ttyUSB0", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777", SYMLINK+="laser_scan"
KERNEL=="ttyUSB1", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777", SYMLINK+="stm32_dev"

生效：

sudo udevadm control --reload-rules
sudo udevadm trigger
ls -l /dev/ | grep laser_scan
ls -l /dev/ | grep stm32_dev

效果： 激光雷达永远固定在 /dev/laser_scan，STM32 固定在 /dev/stm32_dev。

---

第 2 章：虚拟机 Ubuntu 24.04 环境搭建

本章目标： 在 Windows PC 上用 VMware 安装 Ubuntu 24.04。

2.1 VMware 安装 Ubuntu

1. 下载 VMware Workstation Pro

2. 下载 Ubuntu 24.04 镜像

3. 新建虚拟机 → 选择 Ubuntu 镜像

4. 配置虚拟网络：
   退出VMware，右键选择 以管理员身份运行 。
   

点击新建的 Ubuntu24.04 虚拟机 -> 编辑虚拟机设置 -> 网络适配器 -> 将网络连接设置为 桥接模式 -> 确定。

5. 设置用户名密码 → 分配 CPU/内存

6. 完成安装 → 安装 VMWare Tools：

   sudo apt-get install open-vm-tools
   sudo apt-get install open-vm-tools-desktop
   

2.2 安装 ROS2 Jazzy

wget http://fishros.com/install -O fishros && . fishros# 鱼香ROS开源的一键安装工具
# 输入密码后选择 [5]一键配置：系统源 。
# 接着选择 [2]:更换系统源并清理第三方源 。
完成后选择 [2]:不添加ROS/ROS2源 。
更新软件包：
输入以下命令：sudo apt update && sudo apt upgrade -y
安装并配置 VSCode 工具：参考 Linux基础课程 2.1.4 安装 VScode 。

2.3 VSCode 远程开发

PC 端安装 VSCode → Remote-SSH 插件，连接 ssh ubuntu@<树莓派IP>。

---

第 3 章：ROS2 基础

本章目标： 理解 ROS2 的四个核心概念，学会创建工作空间和包。

先问一个问题：为什么需要上位机和下位机两层？树莓派不能直接控制电机吗？

树莓派跑 Linux，Linux 不是实时系统。你让它"每 1ms 算一次 PID"，它可能 3ms 后才响应——因为 Linux 在忙着处理网络或显示。

但电机控制必须实时。角度环要 2ms 算一次，慢了车就抖甚至失控。

所以分工是这样的：

STM32（下位机）：实时控制 P I D、读编码器、读陀螺仪 —— 每 1ms 都不能断
树莓派（上位机）：算里程计、跑 SLAM、跑导航 —— 偶尔卡一下没问题

一句话：STM32 保证车不倒，树莓派保证车走到。

3.1 核心概念

概念	本质	类比	本项目实例
Node（节点）	一个独立运行的程序	一个独立的人	car_node、key_control_node
Topic（话题）	数据通道	微信群	/odom、/scan、/cmd_vel
Publisher（发布者）	往 Topic 里写数据	往群里发消息	car_node 发布 /odom
Subscriber（订阅者）	从 Topic 里读数据	收群消息	Navigation2 订阅 /cmd_vel

发布者和订阅者不需要知道对方的存在——这就是解耦。

为什么 ROS2 这么设计？

假设你的车有 10 个传感器：激光雷达、摄像头、GPS、IMU……
每个传感器驱动是一个程序，控制算法是另一个程序，显示又是另一个。
如果每个程序都要"知道对方在哪才能通信"，代码会变成一团乱麻。

ROS2 的做法： 谁发了数据往 Topic 上一扔，谁想用自己订阅。
发布者不关心有几个订阅者，订阅者不关心数据从哪来的。
这就是"解耦"——每个模块独立开发、独立调试、独立崩溃。

3.2 关键命令

ros2 run <pkg> <node>               # 运行一个节点
ros2 launch <pkg> <launch_file>     # 同时启动多个节点
ros2 topic list                      # 查看有哪些 Topic
ros2 topic echo <topic>              # 查看数据
ros2 topic pub <topic> <type> <data> # 手动发布
ros2 node list                       # 查看节点列表
ros2 node info <node>                # 查看节点详情
colcon build                         # 编译工作空间
source install/setup.bash            # 让系统找到新包

3.3 消息类型

• Twist（速度指令）→ linear.x（线速度），angular.z（角速度）
• Odometry（里程计）→ pose（位置+朝向），twist（速度）
• LaserScan（激光雷达）→ ranges[]（距离数组），angle_min/max
• TransformStamped（TF）→ frame_id（父坐标系），child_frame_id

3.4 创建 ROS2 工作空间

mkdir -p ~/ros2_ws/src
cd ros2_ws/src
sudo apt install python3-colcon-common-extensions
ros2 pkg create test_pkg --build-type ament_python --dependencies rclpy

test_node.py：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node

class TestNode(Node):
    def __init__(self, name):
        super().__init__(name)
        self.get_logger().info("Hello, ROS2!")

def main(args=None):
    rclpy.init(args=args)
    node = TestNode("test_node")
    rclpy.spin(node)          # 阻塞，直到 Ctrl+C
    rclpy.shutdown()

setup.py 配置入口点：

entry_points={
    'console_scripts': [
        "test_node = test_pkg.test_node:main"
    ],
}

编译运行：

cd ~/ros2_ws
colcon build
source install/setup.bash
ros2 run test_pkg test_node

3.5 发布者与订阅者

发布者：

self.pub = self.create_publisher(String, "publisher", 10)
self.timer = self.create_timer(0.5, self.timer_callback)

def timer_callback(self):
    msg = String()
    msg.data = "Hello, I am Pi"
    self.pub.publish(msg)

订阅者：

self.sub = self.create_subscription(String, "publisher", self.sub_callback, 10)

def sub_callback(self, msg):
    self.get_logger().info("Received: {}".format(msg.data))

---

第 4 章：激光雷达驱动

本章目标： 让 COIN-D6 激光雷达 在 ROS2 中跑起来，能在 rviz2 中看到点云。

4.1 硬件连接

COIN-D6 激光雷达（TOF 测距，12m 范围，360°，230400bps）
    │
    ├─ VCC → USB 转 TTL 的 5V
    ├─ GND → USB 转 TTL 的 GND
    ├─ RXD → USB 转 TTL 的 TXD
    └─ TXD → USB 转 TTL 的 RXD
             │
         树莓派 USB 口

TOF（Time of Flight）测距原理： 激光发射出去、反射回来，测量飞行时间。

距离 = 光速 × 飞行时间 / 2

相比超声波：速度快、精度高、不受温度影响。

4.2 安装驱动

scp cspc_lidar_sdk_ros2_D4_20250731.tar.gz ubuntu@<ip>:~/
cd ~/
tar -xvf cspc_lidar_sdk_ros2_D4_20250731.tar.gz
cp -r cspc_lidar_sdk ~/ros2_ws/src/
cd ~/ros2_ws/ && colcon build

4.3 配置端口

编辑 params/cspc_lidar.yaml，将 port 改为：

port: /dev/laser_scan

4.4 启动并验证

source install/setup.bash
ros2 launch cspc_lidar lidar_launch.py

验证数据：

ros2 topic echo /scan

rviz2 可视化：

rviz2 → Add → By topic → /scan → OK
Fixed Frame: laser_link

看到周围环境的点云（墙、障碍物）即成功。

---

第 5 章：STM32 驱动传感器

本章目标： STM32F103 读取 MPU6050 陀螺仪和编码器数据，通过串口发送给树莓派。

里程计在ROS2中的作用

• 里程计是ROS2中用于估计机器人位置和姿态的重要传感器数据来源，通过集成轮式编码器和惯性测量单元（IMU）等传感器，提供机器人在二维平面上的相对运动信息。

• 通过轮式编码器测量轮子的转动情况，计算机器人在平面上的线速度和角速度，从而估计其位 置变化。

• 结合IMU（如MPU6050）提供的加速度和角速度数据，能够更准确地估计机器人的姿态变 化，尤其是在轮子打滑或不平坦地形上。

• 里程计数据通常发布包含机器人的位置、姿态以及线速度和角速度信息，供导航和定位模块使 用。

• 在ROS2导航栈中，里程计数据是实现自主导航、路径规划和避障的基础，有助于机器人构建 环境地图并实现精准定位。

安装STM32CubeIDE配置STM32

下载并安装： STM32CubeIDE
注意：选择安装1.19.0版本的。

## 5.1 硬件配置

外设	通信方式	引脚	用途
MPU6050	I2C2	PB10(SCL), PB11(SDA)	陀螺仪 Z 轴角速度
左编码器	TIM3 编码器模式	PA6(CH1), PA7(CH2)	左轮速度
右编码器	TIM1 编码器模式	PA8(CH1), PA9(CH2)	右轮速度
USART2	串口	PA2(TX), PA3(RX)	与树莓派通信（115200）
TIM2	PWM	—	电机速度控制

5.2 创建项目

• 配置及汉化教程：STM32CubeIDE配置及汉化教程
• 新建工程并配置：
• 选择 文件 -> 新建 -> STM32 Project 。
• 选择芯片型号 STM32F103C8T6 ，点击下一步。
• 填写项目名称 ros2_car ，其他选项保持默认，点击完成。
• 打开 Pinout & Configuration 选项卡：
  • 点击 RCC ，将 HSE 设置为 Cystal/Ceramic Resonator 。
  • 点击 SYS ，将 DEBUG 设置为 Serial Wire 。
• 打开 Clock Configuration 选项卡：
  • 选择 HSE 频率为 8MHz 。
  • 将 SYSCLK 设置为 72MHz 。

导入外设驱动：

git clone https://github.com/CaddonThaw/stm32-hal-devices.git Devices

5.3 MPU6050 陀螺仪

关于 MPU6050： 内部有一个陀螺仪（测角速度）和一个加速度计（测加速度）。本项目只用 Z 轴角速度。

原始值 → rad/s 的换算：

float MPU6050_DMP_GetROSGyroz(void)
{
    do {
        result = dmp_read_fifo(gyro, accel, quat,
                               &sensor_timestamp, &sensors, &more);
    } while (more);  // 全部读完，拿到最新值

    float gyrozDps = ((float)gyro[2]) / 16.4f;
    return gyrozDps * MPU6050_DEG2RAD;
}

换算公式：

MPU6050 量程：±2000°/s
16 位 ADC 范围：-32768 ~ +32767
灵敏度系数：32768 ÷ 2000 ≈ 16.4

原始值 -500：
  → -500 ÷ 16.4 = -30.5 °/s
  → -30.5 × π/180 = -0.53 rad/s

知识点：DMP（Digital Motion Processor）——MPU6050 内部协处理器，专门处理姿态数据。
知识点：while(more)——DMP FIFO 可能堆积了多个数据包，全部读完才能拿到最新的。

5.4 编码器测速

编码器原理： 电机轴上有磁编码盘，旋转时霍尔传感器检测脉冲。TIM 编码器模式（TI12）同时检测 CH1 和 CH2 的上升沿+下降沿，4 倍频提高精度。

void Encoder_Read(float *left, float *right)
{
    count_left = __HAL_TIM_GET_COUNTER(&htim3);   // 左
    count_right = __HAL_TIM_GET_COUNTER(&htim1);  // 右
    __HAL_TIM_SET_COUNTER(&htim3, 0);
    __HAL_TIM_SET_COUNTER(&htim1, 0);

    float distance_per_pulse = (0.65 * 3.14159) / 2000.0;
    *left = (count_left * distance_per_pulse) / dt;
    *right = (count_right * distance_per_pulse) / dt;
}

知识点：为什么读取后要归零？ 不清零计数器会不断累积。每次清零后，下次读到的是"这一段时间内的新脉冲数"。

轮子转一圈的距离： π × 65mm ≈ 204mm，每圈 2000 脉冲，每个脉冲 ≈ 0.1mm。

5.5 printf 重定向

#include <stdio.h>
int fputc(int ch, FILE *f) {
    HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, 0xFFFF);
    return ch;
}

注意： STM32 的 printf 不是打印到电脑屏幕，而是通过串口发给树莓派。

5.6 主循环

while (1) {
    gyroz = MPU6050_DMP_GetROSGyroz();
    Encoder_Read(&left_speed, &right_speed);

    OLED_ShowSignedFloat(1, 7, gyroz, 4);
    OLED_ShowSignedFloat(2, 7, left_speed, 4);
    OLED_ShowSignedFloat(3, 7, -right_speed, 4);

    printf("(%.2f,%.2f,%.2f)\r\n", gyroz, left_speed, -right_speed);

    if (rx_flag == 1) { /* 解析树莓派指令 */ }
    HAL_Delay(10);
}

OLED 显示：

gyroz: -0.12
left:  34.56 mm/s
right: 67.89 mm/s

---

第 6 章：串口通信 + 里程计

本章目标： 树莓派和 STM32 通过串口收发数据，树莓派计算里程计并发布。

6.1 串口协议

下行：树莓派 → STM32（控制指令）：

(x=0.500000,z=0.000000)
  ↑线速度(m/s)  ↑角速度(rad/s)

上行：STM32 → 树莓派（传感器数据）：

(-0.12,34.56,78.90)       ← gyroz, left, -right

6.2 STM32 串口接收状态机

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (ch == '(') { state = 1; rx_index = 0; }
    else if (ch == ')') { state = 0; rx_flag = 1; }
    else if (state == 1) { rx_data[rx_index++] = ch; }

    HAL_UART_Receive_IT(&huart2, &rx_byte, 1);  // 重新收下一个字节
}

核心知识点： HAL 的 HAL_UART_Receive_IT 是一次性的——收完一个字节就停。必须在回调中重新调用，否则串口永久沉默。

状态机图解：

等待 '(' → 收到 '(' → 开始收集 → 收到 ')' → 置标志 → 回到等待
state=0       state=1      存数据        rx_flag=1

6.3 数据安全

if (rx_flag == 1) {
    char temp_rx_data[64];
    strcpy(temp_rx_data, (char *)rx_data);   // ① 先拷贝
    rx_flag = 0;                               // ② 再清标志
    target_speed = cmd_vel("x=", temp_rx_data);
    target_angle = cmd_vel("z=", temp_rx_data);
}

为什么先拷贝再清标志： rx_data 是中断和主循环共享的变量。不先拷贝，解析到一半被中断改写就全乱了。

6.4 树莓派 car_node.py

线程架构：

• 主线程（ROS2 spin）：处理订阅回调 + 每 100ms 发布里程计
• 子线程（car_loop，每 5ms）：串口收发

为什么要独立线程： 串口 readline() 可能阻塞。放在主线程会卡住所有 ROS2 回调。

class CarNode(Node):
    def __init__(self):
        super().__init__('car_node')
        self.odometry_pub = self.create_publisher(Odometry, '/odom', 10)
        self.odometry_broadcaster = TransformBroadcaster(self)
        self.create_timer(0.1, self.publish_odom)

        self.key_control_sub = self.create_subscription(
            Twist, '/key_control', cb, 10)
        self.cmd_vel_sub = self.create_subscription(
            Twist, 'cmd_vel', cb, 10)

        self.serial_port = serial.Serial(
            '/dev/stm32_dev', 115200, timeout=0.001)
        self.car_thread = threading.Thread(target=self.car_loop)
        self.car_thread.start()

6.5 car_loop（串口通信线程）

def car_loop(self):
    while rclpy.ok():
        # 发：速度指令
        msg = '(x={:.6f},z={:.6f})\r\n'.format(
            self.target_speed, self.target_angle)
        self.serial_port.write(msg.encode('utf-8'))

        # 收：传感器数据
        if self.serial_port.in_waiting > 0:
            raw = self.serial_port.readline().decode().strip()
            if raw.startswith('(') and raw.endswith(')'):
                g, l, r = raw[1:-1].split(',')
                self.gyroz = -float(g)           # 取反（方向修正）
                self.left_speed = float(l)
                self.right_speed = float(r)

        time.sleep(0.005)

6.6 里程计计算（三个核心公式）

def publish_odom(self):
    dt = 0.1
    d = (self.left_speed + self.right_speed) / 2

    # ★ 三个核心公式：
    self.angle += self.gyroz * dt                  # 角度 = 角速度 × 时间
    self.pos_x += d * math.cos(self.angle)          # X = 速度 × cos(角度)
    self.pos_y += d * math.sin(self.angle)          # Y = 速度 × sin(角度)

    # 发布 /odom
    odom.pose.pose.position.x = self.pos_x / 66.6
    odom.pose.pose.position.y = self.pos_y / 66.6
    self.odometry_pub.publish(odom)

    # 发布 TF
    self.odometry_broadcaster.sendTransform(transform)

66.6 是什么： 实验标定值。让小车走 1 米，看 pos_x 累积了多少，反推出从毫米到米的换算系数。不同小车这个值不同（轮胎充气程度都影响）。

为什么里程计会有累积误差？（我的理解）

里程计不是"测"位置，是"推算"位置。

当前的位置 = 之前的位置 + 这次走的位移

每次推算都有小误差（轮子打滑 1mm、陀螺仪漂移 0.1°），
走了 1 米误差 1cm 没关系，走了 100 米误差可能到 1 米了。
这就是为什么里程计短距离准，长距离漂。

后面 Cartographer 和 AMCL 的作用就是修正这个累积误差。

6.7 坐标系系统

odom（起点，里程计原点）
  ↓ TF（每 100ms 更新）
base_footprint（地面投影，z=0）
  ↓ URDF（静态）
base_link（车体中心）
  ↓ URDF（静态）
laser_link（激光雷达，x=0.057, z=0.0745）

• odom：起点 (0,0)，随时间累积的里程计位置
• base_footprint：小车在地面的投影，导航算法用这个
• base_link：小车物理中心，PID 控制用
• laser_link：激光雷达位置，/scan 数据就是这个坐标系下的

---

第 7 章：TB6612 + PID 控制

本章目标： 写 TB6612 电机驱动 和 PID 控制，让小车能按指令运动。

7.1 TB6612 电机驱动

AIN1	AIN2	电机状态
1	0	正转
0	1	反转
1	1	刹车
0	0	滑行

PWM 决定速度，0 = 停，7200 = 全速。

// 正数 = 正转，负数 = 反转，绝对值 = PWM
TB6612_Motor(1500, 1500);     // 两轮正转
TB6612_Motor(-800, 1200);     // 左反转，右正转（左转）

7.2 P 控制器（角度环）

float p_calc(float kp, float target, float current) {
    return kp * (target - current);    // 误差 × 比例系数
}
// 每 2ms：kp = -1500
angle_out = p_calc(-1500.0, target_angle, gyroz);

物理含义： 小车偏了 → 误差产生 → angle_out 纠正 → 回正。

kp = -1500： 负号是因为陀螺仪和差速正方向相反，属于硬件适配。

7.3 PI 控制器（速度环）

float pi_calc(float kp, float ki, float *err_sum, float ki_max,
              float target, float current) {
    float err = target - current;
    *err_sum += err;                              // 积分累积
    if (*err_sum > ki_max) *err_sum = ki_max;      // 限幅
    if (*err_sum < -ki_max) *err_sum = -ki_max;
    return kp * err + ki * *err_sum;
}
// 每 10ms：kp=1000, ki=200
left_out = pi_calc(1000.0, 200.0, &left_err_sum, 10000.0, target_speed, avg);

P 和 I 的分工（面试常考）：

• P（比例）：马上响应——慢了就加大，快了就减小。问题：存在稳态误差。
• I（积分）：慢慢累积——持续存在的微小误差通过积分最终弥补。问题：积分饱和（车轮卡住时误差不断累积，放开后过冲）。

PID 像一个洗澡调水温的过程（我的理解）：

你要水温 40°C。先开热水——水冷了再开大点，热了关小点。这就是 P。
但发现水总是 38°C，差 2°C 到不了 40°C。你慢慢把热水阀再拧大一点点，保持住。这就是 I。
这时候有人用冷水，水温骤降，你一下把热水开到最大。这就是 D（虽然本项目没用 D）。

P 解决"现在差多少"，I 解决"一直差一点"的问题。

积分限幅（ki_max = 10000）的作用：防积分饱和。

7.4 差速合成

// 左轮 = 总速度 + 角度修正
// 右轮 = 总速度 - 角度修正
TB6612_Motor(left_speed_out + angle_out,
             right_speed_out - angle_out);

差速原理：

• 直走：angle_out=0 → 左 = 右 = speed_out
• 左转：angle_out<0 → 左 < 右
• 右转：angle_out>0 → 左 > 右

7.5 控制周期

TIM4 配置： Prescaler=720-1, Period=100-1 → 1ms 中断

72MHz / 720 = 100kHz → 100kHz / 100 = 1kHz = 1ms

控制环	周期	频率	说明
角度环（P）	2ms	500Hz	角度变化快，需频繁纠正
速度环（PI）	10ms	100Hz	速度受惯性影响，慢一点也可
电机输出	2ms	500Hz	—

7.6 键盘控制节点

class KeyboardControl(Node):
    def __init__(self):
        self.publisher = self.create_publisher(Twist, '/key_control', 10)

    def run(self):
        tty.setraw(sys.stdin.fileno())   # 按键立即触发
        while rclpy.ok():
            key = self.get_key()
            msg = Twist()
            if key == 'w': msg.linear.x = 0.5
            if key == 's': msg.linear.x = -0.5
            if key == 'a': msg.angular.z = 1.0
            if key == 'd': msg.angular.z = -1.0
            self.publisher.publish(msg)

知识点： tty.setraw() 把终端改成原始模式——按键不经过输入缓冲，按下立即触发。退出时要用 termios.tcsetattr 恢复终端设置，否则没法正常打字。

7.7 限幅保护

# 键盘控制（人操作）
self.key_control_sub = self.create_subscription(Twist, '/key_control', cb, 10)
# 导航控制（自动）
self.cmd_vel_sub = self.create_subscription(Twist, 'cmd_vel', cb, 10)

# 限幅：
if target_speed > 0.5: target_speed = 0.5           # 最快 0.5 m/s
elif target_speed > 0 and target_speed < 0.2:
    target_speed = 0.2  # 电机死区补偿

为什么限最低速： 电机有死区——PWM 太小电机不转。0.2m/s 以下是"给了信号但不动"的区域。

---

第 8 章：URDF 模型

本章目标： 用 URDF 描述小车，在 rviz2 中显示三维模型。

8.1 car.urdf

<robot name="car">
  <link name="base_footprint"/>

  <joint name="base_footprint_to_base_link" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 0.043"/>
  </joint>

  <link name="base_link">
    <visual>
      <geometry><cylinder length="0.02" radius="0.092"/></geometry>
      <material name="blue"/>
    </visual>
  </link>

  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <origin xyz="0.057 0 0.0745"/>
  </joint>
</robot>

坐标系链路：

base_footprint（地面投影，z=0）
    ↓ fixed, z=0.043
base_link（车体中心，蓝色圆盘，r=9.2cm）
    ├─ left_wheel（左轮，continuous 旋转关节）
    ├─ right_wheel（右轮，continuous 旋转关节）
    ├─ caster_wheel（万向轮，fixed）
    └─ laser_link（激光雷达，x=0.057, z=0.0745）

知识点：

• type="fixed"：关节固定不动
• type="continuous"：关节持续旋转（轮子）
• <visual>：定义外观，纯 rviz2 显示用

8.2 robot_state_publisher

ros2 run robot_state_publisher robot_state_publisher car.urdf

读取 URDF → 自动发布所有静态 TF（base_footprint → base_link → laser_link 的固定偏移）。

8.3 完整的 TF 变换链

• car_node.py 发布：odom → base_footprint（动态，每 100ms 更新）
• URDF 发布：base_footprint → base_link → laser_link（静态）
• Cartographer 发布：map → odom（动态，修正里程计漂移）

为什么需要 TF 树？ 激光雷达扫描到墙上的点 P，这个点是在 laser_link 坐标系下测量的。通过 TF 树换算到 map 坐标系，才能画到地图上。

8.4 成果

cd ~/ros2_ws && colcon build
source install/setup.bash
ros2 launch car_pkg car_display.launch.py
rviz2

rviz2 中出现小车的 3D 模型（蓝色底盘、黑色轮子、激光雷达支架）。

查看 tf关系 ：

   ros2 run tf2_tools view_frames

---

第 9 章：Cartographer SLAM 建图

本章目标： 用 Cartographer 建图，小车一边走一边画房间地图。

9.1 配置文件（backpack_2d.lua）

options = {
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "odom",
  use_odometry = true,
  num_laser_scans = 1,
}

TRAJECTORY_BUILDER_2D.min_range = 0.10    -- 忽略 10cm 以内（车体自身）
TRAJECTORY_BUILDER_2D.max_range = 10      -- 忽略 10m 以外
TRAJECTORY_BUILDER_2D.use_imu_data = false

9.2 启动建图

ros2 launch car_pkg car_mapping.launch.py

启动的节点：

1. car_display.launch.py → URDF → 静态 TF
2. cspc_lidar → /scan（激光雷达数据）
3. car_node.py → /odom + TF（里程计）
4. cartographer_node → 接收 /scan + /odom → 建图
5. key_control_node → 键盘控制
6. rviz2 → 可视化

9.3 SLAM 工作流程

/scan（激光雷达）+ /odom（里程计）+ TF 树
  → Cartographer：子图匹配 → 闭环检测
  → /map（栅格地图）
  → map → odom（修正里程计漂移）

操作： 键盘遥控小车在房间里走一圈
效果： rviz2 中逐渐画出地图——黑 = 障碍物，白 = 可通行，灰 = 未知

地图颜色是怎么来的？（我的理解）

Cartographer 把空间划分成一个一个的栅格（grid cell），每个格子 5cm × 5cm。

激光打到一个格子，说明那里有障碍物→标记黑色。
激光穿过一个格子，说明那里是空的→标记白色。
激光没打过的格子→灰色（未知）。

多次扫描同一区域：如果同一个格子 10 次中有 8 次打到墙，那它大概率真是墙。
这就是概率栅格地图——不是"是墙 / 不是墙"的二值判断，而是"有多大概率是墙"。

9.4 保存地图

ros2 run nav2_map_server map_saver_cli -f ~/map1

生成两个文件：

• map1.yaml：地图描述文件（分辨率、原点坐标）

• map1.pgm：图像格式的地图

  map1.yaml

  image: map1.pgm
  resolution: 0.050 # 每个像素代表 5cm
  origin: [-1.0, -1.8, 0] # 地图左下角位置

---

第 10 章：Navigation2 自主导航

本章目标： 加载建好的地图，让小车自动规划路径并导航到目标点。

10.1 三大核心模块

① AMCL（定位）——粒子滤波

• 输入：/scan（激光雷达）、/odom（里程计）、/map（地图）
• 输出：map → odom 变换（修正里程计累积误差）
• 原理：撒 2000 个猜测粒子 → 激光匹配 → 淘汰不匹配的 → 收敛

rviz2 中可以看到：刚开始箭头散在整个地图上，几秒后聚拢到小车实际位置。

② 全局规划器（Planner）—— A* 算法

• 输入：全局代价地图 + 起点 + 终点
• 输出：一条从起点到终点的路径（rviz2 中的红线）

③ 局部规划器（Controller）—— DWB / MPPI

• 输入：全局路径 + 实时 /scan（激光数据）
• 输出：cmd_vel（每 50ms 更新一次速度指令）
• 遇到障碍物：临时绕开；绕不过就请求全局重规划

为什么分全局和局部两个规划器？（我的理解）

全局规划器像导航地图——它知道从北京到上海怎么走，但它不知道当前路口有没有施工。
局部规划器像司机看路——它知道大概方向，但具体怎么绕过前边的坑、避让旁边的车，它自己判断。

全局规划反应慢（算一次要几百毫秒），但考虑的是整条路。
局部规划反应快（每 50ms 算一次），但只看眼前几米。

如果只用一个规划器：
要么反应慢（撞上障碍物才重算，来不及）
要么只看得近（算不出从 A 到 B 的长远路径）

10.2 启动命令

ros2 launch car_pkg car_nav.launch.py

10.3 关键配置参数（nav2_params.yaml）

amcl:
  robot_model_type: "nav2_amcl::DifferentialMotionModel"  # 差速模型
  base_frame_id: "base_footprint"
  global_frame_id: "map"

controller_server:
  vx_max: 0.5              # 最大线速度
  wz_max: 1.9              # 最大角速度

local_costmap:
  rolling_window: true     # 窗口跟着车移动
  width: 3 / height: 3    # 只看周围 3 米
  robot_radius: 0.096      # 小车半径
  inflation_radius: 0.20   # 障碍物外扩

global_costmap:
  robot_radius: 0.096
  inflation_radius: 0.70   # 外扩更大（更保守）

10.4 导航完整流程

1. 加载地图 → AMCL 定位（粒子收敛）
2. 用户设目标（rviz2 中 Nav2 Goal）
3. 全局规划：A* 在地图上算路径 → 输出红线
4. 局部规划 + 控制：DWB 沿红线走，每 50ms 输出 cmd_vel
   → car_node.py → STM32 → 电机
5. 反馈：编码器+陀螺仪 → 里程计 → AMCL 更新定位
6. 到达 → 停止

---

第 11 章：完整数据链路

11.1 控制链路（键盘 → 轮子）

键盘按 W
  → key_control_node 发布 /key_control
  → car_node.py: target_speed = 0.5
  → car_loop(5ms): '(x=0.5,z=0.0)'
  → USB 串口
  → STM32 状态机解析
  → TIM4 中断 PID（1ms）
    角度环(2ms): −1500 × (0 − gyroz)
    速度环(10ms): 1000 × err + 200 × ∫err
    差速合成: TB6612_Motor(speed+angle, speed-angle)
  → TB6612 → 电机 → 轮子转

11.2 感知链路（轮子 → 里程计）

左编码器 → TIM3 → count
右编码器 → TIM1 → count
MPU6050 → gyroz
  → printf("(gyroz,left,right)")
  → USB 串口
  → car_loop: split → self.gyroz/left/right
  → publish_odom(100ms):
    d=(left+right)/2
    angle+=gyroz*dt
    pos_x+=d*cos(angle)
    pos_y+=d*sin(angle)
  → 发布 /odom + TF: odom→base_footprint

11.3 建图链路

/scan + /odom + TF
  → Cartographer：子图匹配 → 闭环检测
  → /map（栅格地图）
  → map→odom（修正里程计漂移）

11.4 导航链路

map_server 加载地图
  → AMCL 粒子滤波定位
  → 用户设目标点
  → 全局规划 A* 算路径
  → 局部规划 DWB 输出 cmd_vel
  → STM32 → 电机
  → 编码器反馈 → 里程计更新
  → AMCL 更新定位
  → 循环直到到达

---

第 12 章：调试笔记

PID 参数不匹配

现象： 小车抖动或响应慢
调整： 先调 P 到刚好不震荡，再加 I 消除稳态误差

• P 太大 → 震荡（电机疯狂开关）
• P 太小 → 响应慢（推一下才动）
• I 太大 → 过冲（停不下来）
• I 太小 → 稳态误差（永远差一点）

串口乱码

排查： 两边波特率是否一致（115200）？GND 是否共地？

激光雷达不转

排查： ls /dev/laser_scan 是否存在 → dmesg 查看 USB 识别 → 检查 udev 规则

TF 树不完整

ros2 run tf2_tools view_frames

查看生成的 frames.pdf 中是否所有坐标系都连上了。常见缺失：map → odom（Cartographer 没启动）。

rviz2 不显示模型

• 检查 car.urdf 路径是否正确
• 检查 robot_state_publisher 是否运行

---

附录：参数速查

PID 参数

环	类型	kp	ki	ki_max	频率
角度环	P	-1500	—	—	500Hz
左速度环	PI	1000	200	10000	100Hz
右速度环	PI	1000	200	10000	100Hz

定时器

定时器	模式	Prescaler	Period	频率	用途
TIM1	编码器	0	65535	—	右编码器
TIM3	编码器	0	65535	—	左编码器
TIM2	PWM	0	7200-1	—	电机 PWM
TIM4	定时器	720-1	100-1	1kHz	PID 控制

串口

参数	值
波特率	115200
数据位	8
停止位	1
校验	无
USART2	PA2(TX), PA3(RX)
接收方式	单字节中断

单位换算

传感器	原始数据	换算过程	最终单位
MPU6050	16 位整数	÷16.4 → °/s, ×π/180	rad/s
编码器	脉冲计数	×(π×0.65/2000)÷dt	mm/s
位置	pos_x	÷66.6	米

控制周期总结

STM32 主循环：    10ms  （传感器读取 + 串口发送）
TIM4 中断：       1ms   （PID + 电机输出）
  ├─ 角度环：      2ms   （500Hz）
  ├─ 速度环：     10ms   （100Hz）
  └─ 电机输出：    2ms   （500Hz）
树莓派 car_loop：  5ms   （串口收发）
树莓派 odom：    100ms   （里程计发布）