小型机器人传感器融合入门

在机器人导航和自动驾驶中，单一传感器往往无法提供完整、准确的环境信息。摄像头受光照影响，激光雷达无法识别颜色，IMU 存在累积误差。传感器融合（Sensor Fusion）技术通过结合多种传感器的数据，提供更可靠、更全面的环境感知能力。

本文将介绍传感器融合的基本原理，以及如何在小型机器人（如 DonkeyCar、JetBot）上实现简单的传感器融合。

为什么需要传感器融合？

每种传感器都有其优势和局限性：

传感器	优势	局限性
摄像头	丰富的视觉信息、成本低	受光照影响、无深度信息
IMU	高频率、不受环境影响	累积漂移
激光雷达	精确距离测量	成本高、无颜色信息
超声波	简单可靠	范围有限、精度低
GPS	全局定位	室内不可用、精度有限

通过融合多种传感器，可以互补各自的不足，获得更准确的位姿估计和环境感知。

传感器融合的基本方法

1. 互补滤波（Complementary Filter）

互补滤波是最简单的融合方法，适用于融合两个传感器数据。核心思想是：对高频信号信任一个传感器，对低频信号信任另一个。

典型应用：融合 IMU 的加速度计和陀螺仪数据。

import numpy as np

class ComplementaryFilter:
    def __init__(self, alpha=0.98):
        self.alpha = alpha  # 陀螺仪权重
        self.angle = 0.0
    
    def update(self, gyro_rate, accel_angle, dt):
        """
        gyro_rate: 陀螺仪角速度 (rad/s)
        accel_angle: 加速度计计算的角度 (rad)
        dt: 时间步长 (s)
        """
        # 陀螺仪积分（高频响应好）
        gyro_angle = self.angle + gyro_rate * dt
        
        # 互补融合
        self.angle = self.alpha * gyro_angle + (1 - self.alpha) * accel_angle
        
        return self.angle

# 使用示例
filter = ComplementaryFilter(alpha=0.98)
dt = 0.01  # 100Hz

for i in range(1000):
    gyro_rate = read_gyroscope()  # rad/s
    accel_angle = read_accelerometer_angle()  # rad
    fused_angle = filter.update(gyro_rate, accel_angle, dt)

2. 卡尔曼滤波（Kalman Filter）

卡尔曼滤波是一种最优估计算法，适用于线性系统。它通过预测-更新两个步骤，递归地估计系统状态。

import numpy as np

class KalmanFilter:
    def __init__(self, dt=0.01):
        self.dt = dt
        
        # 状态向量 [位置, 速度]
        self.x = np.array([[0.0], [0.0]])
        
        # 状态转移矩阵
        self.F = np.array([[1, dt],
                           [0, 1]])
        
        # 控制矩阵（如果有加速度输入）
        self.B = np.array([[0.5 * dt**2], [dt]])
        
        # 测量矩阵（只测量位置）
        self.H = np.array([[1, 0]])
        
        # 协方差矩阵
        self.P = np.eye(2) * 100
        
        # 过程噪声
        self.Q = np.eye(2) * 0.1
        
        # 测量噪声
        self.R = np.array([[1.0]])
    
    def predict(self, u=0):
        """预测步骤"""
        self.x = self.F @ self.x + self.B * u
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x[0, 0]
    
    def update(self, z):
        """更新步骤"""
        y = z - self.H @ self.x  # 残差
        S = self.H @ self.P @ self.H.T + self.R  # 残差协方差
        K = self.P @ self.H.T @ np.linalg.inv(S)  # 卡尔曼增益
        
        self.x = self.x + K @ y
        self.P = (np.eye(2) - K @ self.H) @ self.P
        
        return self.x[0, 0]

# 使用示例
kf = KalmanFilter(dt=0.01)
measurements = [1.2, 1.5, 1.3, 1.8, 2.1, 2.0, 2.3]  # 带噪声的测量值

for z in measurements:
    kf.predict()
    estimated = kf.update(np.array([[z]]))
    print(f"测量值: {z:.2f}, 估计值: {estimated:.2f}")

3. 扩展卡尔曼滤波（EKF）

对于非线性系统（如机器人运动学模型），需要使用扩展卡尔曼滤波。EKF 通过泰勒展开将非线性函数线性化。

class ExtendedKalmanFilter:
    def __init__(self):
        # 状态 [x, y, theta]
        self.x = np.zeros(3)
        self.P = np.eye(3) * 10
        self.Q = np.eye(3) * 0.01
        self.R = np.eye(2) * 0.5  # 测量噪声
    
    def predict(self, v, omega, dt):
        """非线性运动模型"""
        x, y, theta = self.x
        
        # 状态预测
        self.x[0] = x + v * np.cos(theta) * dt
        self.x[1] = y + v * np.sin(theta) * dt
        self.x[2] = theta + omega * dt
        
        # 雅可比矩阵
        F = np.array([
            [1, 0, -v * np.sin(theta) * dt],
            [0, 1, v * np.cos(theta) * dt],
            [0, 0, 1]
        ])
        
        self.P = F @ self.P @ F.T + self.Q
    
    def update(self, z):
        """z = [x_meas, y_meas]"""
        H = np.array([[1, 0, 0],
                      [0, 1, 0]])
        
        y = z - H @ self.x
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)
        
        self.x = self.x + K @ y
        self.P = (np.eye(3) - K @ H) @ self.P

实际应用：融合摄像头和 IMU

在自动驾驶小车中，可以融合摄像头的视觉里程计（Visual Odometry）和 IMU 数据：

class CameraIMUFusion:
    def __init__(self):
        self.kf = KalmanFilter(dt=0.02)  # 50Hz
    
    def update(self, visual_pos, imu_accel, timestamp):
        # 使用 IMU 加速度进行预测
        self.kf.predict(u=imu_accel)
        
        # 使用视觉位置进行更新
        estimated_pos = self.kf.update(np.array([[visual_pos]]))
        
        return estimated_pos

在 Jetson Nano 上实现

使用 Python 和 NumPy 可以在 Jetson Nano 上实时运行简单的卡尔曼滤波。对于更复杂的融合需求，可以使用 ROS（Robot Operating System）的 robot_localization 包：

sudo apt install ros-melodic-robot-localization

配置 ekf_localization_node，输入来自 IMU、轮式里程计和视觉里程计的数据，输出融合后的位姿估计。

传感器同步

多传感器融合的关键挑战之一是时间同步。不同传感器的采样率不同，需要统一时间戳：

from collections import deque

class SensorSyncBuffer:
    def __init__(self, max_delay=0.05):
        self.buffer = deque(maxlen=100)
        self.max_delay = max_delay  # 最大允许延迟（秒）
    
    def add(self, sensor_name, timestamp, data):
        self.buffer.append((sensor_name, timestamp, data))
    
    def get_synchronized(self, target_time):
        """获取最接近目标时间的各传感器数据"""
        result = {}
        for name, ts, data in self.buffer:
            if abs(ts - target_time) < self.max_delay:
                if name not in result or abs(ts - target_time) < abs(result[name][0] - target_time):
                    result[name] = (ts, data)
        return result

总结

传感器融合是机器人感知系统的核心技术。对于小型机器人项目，互补滤波和卡尔曼滤波已经能满足大部分需求。关键是根据应用场景选择合适的融合方法，并注意传感器同步和噪声建模。随着项目复杂度增加，可以逐步引入更高级的融合算法（如粒子滤波、因子图优化）。