4. 传感器选型与数据接入：摄像头、IMU、轮速编码器、GPS/RTK 的数据格式与 ROS2 驱动集成

各位同学，欢迎来到第四讲。

上一章我们把系统架构搭起来了，但说实话，那只是个空壳子。真正让车道保持系统“活”起来的，是传感器数据。这一章，咱们就聊聊怎么选传感器，怎么把数据接进来。

我个人习惯把传感器分成两类：感知类和状态类。摄像头是感知，告诉你路在哪；IMU、轮速、GPS 是状态，告诉你车在哪、车在怎么动。缺一不可。

4.1 摄像头选型：单目还是双目？

先说摄像头。这是车道保持系统的“眼睛”。

单目摄像头，说白了就是一个镜头。它便宜、成熟、量产方案多。但有个硬伤——它测不出深度。你想想看，一张二维图片，怎么知道前车离你 50 米还是 100 米？全靠算法猜。我在项目中遇到过，单目在光线好的高速上表现不错，但一到隧道出入口，或者遇到大曲率弯道，深度估计就容易飘。

双目摄像头呢？两个镜头，像人的两只眼睛。通过视差算深度，精度高很多。但代价也大：成本翻倍、标定复杂、计算量大。我记得有一次在量产项目上，客户非要上双目，结果标定工厂搞了三个月才把产线标定搞定。

我的建议：

  • 如果做 L2 级辅助驾驶，单目足够。配合毫米波雷达补盲，性价比最高。
  • 如果做 L3+ 或需要纯视觉方案，双目是必选项。但要做好标定和算力的准备。

数据格式方面，ROS2 里最常用的是 sensor_msgs/msg/Image。我一般用 CompressedImage 来传，省带宽。但注意，压缩会丢细节，车道线检测这种任务，建议用原始图像。

# 摄像头驱动示例（ROS2 Python）
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge

class CameraNode(Node):
    def __init__(self):
        super().__init__('camera_node')
        self.pub = self.create_publisher(Image, '/camera/image_raw', 10)
        self.bridge = CvBridge()
        self.cap = cv2.VideoCapture(0)  # 实际项目用GStreamer管道
        self.timer = self.create_timer(0.033, self.publish_frame)  # 30fps

    def publish_frame(self):
        ret, frame = self.cap.read()
        if ret:
            msg = self.bridge.cv2_to_imgmsg(frame, 'bgr8')
            self.pub.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = CameraNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

避坑指南：我曾经在某个项目里直接用 OpenCV 的 VideoCapture 读 USB 摄像头，结果发现延迟高达 100ms。后来换成 V4L2 直接操作，延迟降到 30ms 以内。量产项目千万别用 OpenCV 的默认读取方式。

4.2 IMU：车的“内耳”

IMU 测量加速度和角速度。说白了，就是知道车在加速还是减速，在转弯还是直行。

数据格式很简单：sensor_msgs/msg/Imu。包含三轴加速度、三轴角速度，还有四元数姿态。但这里有个坑——IMU 的原始数据噪声很大。你想想看，一个静止的 IMU，输出的加速度值可能一直在 9.8 附近跳来跳去。

注意：IMU 数据必须做标定和滤波。我习惯先用 Allan 方差分析噪声特性，再用互补滤波或 EKF 融合。千万别把原始数据直接扔给控制模块，否则车会像喝醉了一样乱晃。

# IMU 数据发布示例
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion

imu_msg = Imu()
imu_msg.header.stamp = self.get_clock().now().to_msg()
imu_msg.header.frame_id = 'imu_link'

# 加速度（m/s^2）
imu_msg.linear_acceleration.x = accel_x
imu_msg.linear_acceleration.y = accel_y
imu_msg.linear_acceleration.z = accel_z

# 角速度（rad/s）
imu_msg.angular_velocity.x = gyro_x
imu_msg.angular_velocity.y = gyro_y
imu_msg.angular_velocity.z = gyro_z

# 姿态（四元数）
imu_msg.orientation = Quaternion(x=qx, y=qy, z=qz, w=qw)

4.3 轮速编码器：最可靠的里程计

轮速编码器，说白了就是数轮子转了多少圈。这东西便宜、可靠、不依赖外部信号。我在项目中遇到过 GPS 信号丢失的情况，全靠轮速编码器撑着。

数据格式一般用 nav_msgs/msg/Odometry，但只填速度部分。或者直接用 geometry_msgs/msg/TwistStamped。

关键点：轮速编码器有两个致命缺陷：

  • 打滑时数据完全不可信。雨雪天尤其明显。
  • 长时间积分会累积误差。跑 1 公里可能偏 5 米。

所以它必须和 IMU、GPS 做融合。单独用？不行的。

# 轮速编码器数据解析（CAN总线）
def parse_wheel_speed(can_frame):
    # 假设CAN ID 0x100 包含左后轮和右后轮速度
    left_speed = (can_frame.data[0] << 8 | can_frame.data[1]) * 0.01  # m/s
    right_speed = (can_frame.data[2] << 8 | can_frame.data[3]) * 0.01
    
    # 计算车辆速度（后轮驱动）
    vehicle_speed = (left_speed + right_speed) / 2.0
    
    # 计算横摆角速度（差速）
    wheel_base = 1.5  # 轮距，单位米
    yaw_rate = (right_speed - left_speed) / wheel_base
    
    return vehicle_speed, yaw_rate

4.4 GPS/RTK：绝对定位的“定海神针”

普通 GPS 精度 3-5 米，做车道保持？别想了。车道才 3.5 米宽，误差比车道还大。

RTK（实时动态差分）能把精度做到厘米级。原理很简单：一个基站放在已知位置，把差分信号发给车上的流动站，消除大气误差。

我的经验：RTK 在城市峡谷、隧道、高架桥下会失效。我做过一个测试，在上海市中心，RTK 的固定解率只有 60%。所以千万别把 RTK 当唯一依赖。一定要和 IMU+轮速做组合导航。

数据格式用 sensor_msgs/msg/NavSatFix 传经纬度，用 geometry_msgs/msg/PoseWithCovarianceStamped 传位姿。RTK 的状态（固定解、浮点解、单点解）要特别关注。

# GPS/RTK 数据解析（NMEA 0183 协议）
def parse_nmea_sentence(sentence):
    if sentence.startswith('$GPGGA'):
        parts = sentence.split(',')
        # 时间
        time = parts[1]
        # 纬度
        lat = float(parts[2][:2]) + float(parts[2][2:]) / 60.0
        if parts[3] == 'S':
            lat = -lat
        # 经度
        lon = float(parts[4][:3]) + float(parts[4][3:]) / 60.0
        if parts[5] == 'W':
            lon = -lon
        # 海拔
        altitude = float(parts[9])
        # 定位质量（0=无效，1=单点，2=差分，4=RTK固定，5=RTK浮点）
        quality = int(parts[6])
        
        return lat, lon, altitude, quality
    return None

4.5 多传感器时间同步：最容易被忽视的坑

好了，现在四个传感器都接进来了。但有个问题——它们的时间戳不一样。

摄像头是 30fps，IMU 是 200Hz，轮速是 100Hz，GPS 是 10Hz。你想想看，如果拿 100ms 前的 IMU 数据和最新的摄像头数据做融合，结果会怎样？

我曾经踩过的坑：在一个项目中，我直接用 ROS2 的默认时间戳，结果发现车道保持系统在高速上总是滞后。查了两天才发现，IMU 的驱动里时间戳用的是系统时间，而摄像头用的是硬件时间，差了 50ms。50ms 在 120km/h 下就是 1.7 米的误差。

解决方案：

  • 所有传感器用同一个硬件时钟源（PTP 或 GPS 秒脉冲）。
  • ROS2 里用 message_filters::ApproximateTimeSynchronizer 做近似时间同步。
  • 或者自己写一个时间对齐模块，用插值法把高频数据对齐到低频数据的时间戳上。
# 多传感器时间同步示例
from message_filters import ApproximateTimeSynchronizer, Subscriber

# 创建订阅器
cam_sub = Subscriber(self, Image, '/camera/image_raw')
imu_sub = Subscriber(self, Imu, '/imu/data_raw')
gps_sub = Subscriber(self, NavSatFix, '/gps/fix')

# 时间同步（容忍 50ms 的时间差）
sync = ApproximateTimeSynchronizer(
    [cam_sub, imu_sub, gps_sub], 
    queue_size=10, 
    slop=0.05  # 50ms
)
sync.registerCallback(self.sensor_callback)

def sensor_callback(self, cam_msg, imu_msg, gps_msg):
    # 此时三个消息的时间戳已经对齐
    # 可以开始融合了
    pass

4.6 总结与实战建议

这一章内容不少，我帮你捋一下重点：

  • 摄像头：单目够用就单目，别盲目上双目。数据用 Image 或 CompressedImage。
  • IMU：必须标定和滤波。原始数据是垃圾，处理过的才是黄金。
  • 轮速编码器：可靠但会打滑。和 IMU 做互补。
  • GPS/RTK：厘米级精度但会丢信号。别当唯一依赖。
  • 时间同步：这是系统集成的灵魂。50ms 的误差就能让系统崩溃。

我的个人习惯：在量产项目中，我会先做一个传感器健康监测模块。每个传感器都发心跳包，一旦某个传感器数据异常或时间戳跳变，系统立即降级。安全第一，功能第二。

下一章，咱们聊聊怎么用这些传感器数据做车道线检测。那是整个系统的核心算法部分，也是最有意思的部分。到时候见。

← 上一章 📖 返回目录 下一章 →