2024年电赛自动驾驶小车：基于ROS2的实时避障与路径规划实战

共计 2326 个字符，预计需要花费 6 分钟才能阅读完成。

背景痛点

在往届电赛中，常见两个典型问题：

1. 传统 PID 的轨迹偏移：当小车在 40cm/ s 速度下进行 60°急转时，仅依赖编码器的 PID 控制会产生 8 -12cm 的横向偏移（实测数据）。环氧地坪（摩擦系数 0.7）上的陡坡路段，电机输出饱和导致 PID 积分项累积，进一步加剧轨迹偏差

2. 超声波传感器的局限性：在组委会设置的动态障碍场景中（移动速度≥0.5m/s），HC-SR04 超声波模块的 10Hz 刷新率导致漏检率高达 35%，且锥形探测区域易产生镜面反射误判

技术选型

ROS1 vs ROS2 实时性对比

在树莓派 4B（4GB 内存）上的测试数据：

• 100Hz 消息频率下，ROS1 的通信延迟波动范围（15-50ms），ROS2（使用 DDS 中间件）稳定在 8 -12ms
• Micro-ROS 架构将控制节点内存占用从 ROS1 的 120MB 压缩至 45MB，适合资源受限的嵌入式场景

路径规划算法实测

测试环境：2m×3m 地图分辨率 5cm，树莓派 4B 单核负载 60%

核心实现

RPLIDAR A1 数据预处理

# ros2 humble + rplidar_ros 2.1.1
import numpy as np
from scipy.signal import savgol_filter

def scan_filter(scan_msg):
    ranges = np.array(scan_msg.ranges)
    # 替换无穷大值为 3m（比赛场地最大范围）ranges[ranges == np.inf] = 3.0
    # 滑动窗口滤波（窗口长度 11，多项式阶数 2）filtered = savgol_filter(ranges, 11, 2)
    # 排除 5cm 以内的噪声（雷达安装支架反射）filtered[filtered < 0.05] = 0.0
    return filtered

全局路径规划 launch 配置

<!-- nav2_bringup 1.1.2 -->
<launch>
  <node pkg="nav2_planner" type="planner_server" name="planner_server">
    <param name="expected_planner_frequency" value="20.0"/>
    <param name="use_sim_time" value="false"/>
    <rosparam file="$(find my_pkg)/config/costmap_params.yaml"/>
  </node>

  <!-- 膨胀层设置 0.15m（小车宽度 +5cm 余量）-->
  <node pkg="nav2_costmap_2d" type="costmap_2d_markers" name="inflation_layer">
    <param name="inflation_radius" value="0.15"/>
  </node>
</launch>

改进 DWA 评价函数

# numpy 1.23.5
def dynamic_window_approach(): 
    # 速度采样空间（线速度 0.1-0.5m/s，角速度±1.5rad/s）v_samples = np.linspace(0.1, 0.5, 10)
    w_samples = np.linspace(-1.5, 1.5, 20)

    # 调整权重系数（原版[0.3,0.3,0.4]→改进版[0.2,0.4,0.4]）weights = np.array([0.2, 0.4, 0.4])  # [目标朝向, 障碍距离, 速度]

    # 矩阵化计算（比 for 循环快 8 倍）trajectories = np.meshgrid(v_samples, w_samples)
    scores = weights[0]*heading_score(trajectories) + \
             weights[1]*clearance_score(trajectories) + \
             weights[2]*velocity_score(trajectories)

    return trajectories[np.argmax(scores)]

性能验证

资源占用（树莓派 4B 100% 负载测试）

刹车距离测试

测试条件：紧急制动指令延迟 200ms，橡胶轮胎与环氧地坪摩擦系数 0.7

避坑指南

TF 坐标树报错排查

1. 时间戳同步 ：检查所有节点是否使用相同的use_sim_time 参数（实测发现混用 true/false 会导致 25% 的 TF 断裂）
2. 父子坐标系命名：确保 base_link→laser 的 TF 命名与 URDF 完全一致（大小写敏感）
3. 发布频率：TF 静态变换建议使用 10Hz 以上频率发布（低于 5Hz 会导致控制节点丢帧）

激光雷达与 IMU 同步

在 /etc/chrony/chrony.conf 添加 PTP 配置：

refclock PHC /dev/ptp0 poll 3
server 127.127.1.0
local stratum 10

通过 ptpd2 服务实现 μs 级同步，时间戳抖动从±15ms 降低到±2ms

延伸思考

当引入 OV2640 摄像头（30fps）做视觉识别时，建议：

1. 将激光雷达的障碍物坐标与视觉检测框进行欧式空间映射
2. 改进卡尔曼滤波的状态向量：

   x_k = [x_{lidar}, y_{lidar}, v_{odom}, \theta_{imu}, w_{cam}]^T

3. 使用 Mahalanobis 距离剔除异常匹配（实测可降低 15% 的误关联）

这套方案在 2024 年省赛测试赛道上实现了全程无人工干预的稳定运行，后续可通过 FPGA 加速 DWA 的矩阵运算进一步提升实时性。