2、传感器误差模型与标定：确定性误差与随机误差，传感器标定方法（IMU六面法、激光雷达外参标定、摄像头内参标定），Allan方差分析

各位同学，咱们今天聊点实在的。传感器这东西，说白了就是机器人的眼睛和耳朵。但你想过没有，再好的眼睛也有看走眼的时候。我做了这么多年机器人，最深的体会就是：不搞定误差，后面全是白搭。

传感器误差分两类：确定性误差和随机误差。前者像是一个秤永远少一两，后者像是你称体重时数字忽大忽小。嗯，咱们一个一个来拆解。

2.1 确定性误差：能算清楚的账

确定性误差，说白了就是有规律的偏差。比如IMU的零偏——静止时它告诉你还在动。还有尺度因子误差，你转90度，它告诉你转了91度。这些误差一旦标定好，就能在软件里补偿掉。

我记得有一次做无人机飞控，IMU的零偏没标好，飞机悬停时一直往北飘。折腾了两天，最后发现就是零偏补偿参数写错了。从那以后，我每次上电第一件事就是做静态零偏校准。

2.2 随机误差：捉摸不定的噪声

随机误差就麻烦多了。它没有固定规律，只能用统计方法处理。比如白噪声、随机游走、量化噪声等等。你想想看，如果传感器输出一直在跳，你怎么知道真实值是多少？

我刚开始做激光雷达时，发现点云数据一直在抖动。一开始以为是硬件坏了，后来用Allan方差一分析，发现是随机游走噪声太大。换了个型号的陀螺仪，问题就解决了。所以啊，别急着骂硬件，先看看噪声模型对不对。

2.3 传感器标定方法：把误差揪出来

标定，就是给传感器做体检。不同传感器有不同的标定方法，我挑三个最常用的讲讲。

2.3.1 IMU六面法

这个方法名字听着玄乎，其实很简单。把IMU分别放在六个面上（每个轴正反各一次），记录输出数据。通过最小二乘法，就能解出零偏、尺度因子和交叉耦合系数。

我曾经在一个项目里用六面法标定MEMS IMU，结果发现交叉耦合误差比想象中大得多。如果不标定，融合出来的姿态角误差能到5度以上。标定后，误差降到了0.5度以内。

// 六面法标定示例（简化版）
// 采集六个面的数据：+X, -X, +Y, -Y, +Z, -Z
// 每个面采集N个样本，取均值
double accel_data[6][3]; // 六个面的加速度计读数
double gravity = 9.81;   // 当地重力加速度

// 构建观测方程：A * x = b
// x = [bias_x, bias_y, bias_z, scale_x, scale_y, scale_z]
// 用最小二乘法求解
Eigen::MatrixXd A(6, 6);
Eigen::VectorXd b(6);

for (int i = 0; i < 6; i++) {
    // 填充矩阵A和向量b
    // 具体推导略，核心思想是让六个面的模长都等于重力加速度
}

Eigen::VectorXd x = A.colPivHouseholderQr().solve(b);

2.3.2 激光雷达外参标定

激光雷达的外参，就是它相对于机器人本体的位置和姿态。这个标定不好，点云和实际位置就对不上。

常用的方法是找一个特征明显的环境（比如墙角、柱子），让机器人走一圈，然后通过点云配准来解算外参。我习惯用ICP算法，但要注意初始值不能差太远，否则会收敛到局部最优。

有一次我在室外标定激光雷达，风太大导致点云抖动。后来我改成室内标定，用三个互相垂直的平面做特征，效果就好多了。嗯，环境选择很重要。

2.3.3 摄像头内参标定

摄像头内参标定，最经典的就是张正友标定法。用棋盘格拍十几张不同角度的照片，然后解算内参矩阵和畸变系数。

我个人习惯用OpenCV的calibrateCamera函数。但要注意几点：

• 棋盘格要打印平整，贴在硬板上
• 照片要覆盖整个视野，特别是边缘
• 每张照片的棋盘格角度要不同，不能全是正面

import cv2
import numpy as np

# 棋盘格参数
pattern_size = (9, 6)  # 内角点数量
square_size = 0.025    # 每个格子边长（米）

# 准备世界坐标系中的点
objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2)
objp *= square_size

# 存储所有图像的点
objpoints = []  # 世界坐标系中的点
imgpoints = []  # 图像坐标系中的点

# 遍历所有标定图像
for fname in image_files:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
    # 查找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    
    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

# 标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    objpoints, imgpoints, gray.shape[::-1], None, None
)

print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

2.4 Allan方差分析：读懂噪声的DNA

Allan方差，说白了就是分析传感器噪声在不同时间尺度上的表现。它能帮你识别出白噪声、随机游走、量化噪声等各种成分。

我刚开始接触Allan方差时，觉得公式很复杂。后来发现，其实就是把数据按不同时间窗口切分，然后算方差。时间窗口短时，看到的是高频噪声；窗口长时，看到的是低频漂移。

具体步骤是这样的：

1. 采集长时间静态数据（至少1小时）
2. 计算不同时间间隔τ下的Allan方差
3. 在双对数坐标下画曲线
4. 根据曲线斜率识别噪声类型

噪声类型	Allan方差曲线斜率	典型来源
量化噪声	-1	ADC量化
白噪声	-1/2	热噪声、电子噪声
随机游走	+1/2	积分漂移
速率斜坡	+1	温度漂移

import numpy as np
import matplotlib.pyplot as plt

def allan_variance(data, max_tau_ratio=0.1):
    """
    计算Allan方差
    data: 传感器数据（一维数组）
    max_tau_ratio: 最大时间间隔比例
    """
    n = len(data)
    max_tau = int(n * max_tau_ratio)
    
    taus = []
    variances = []
    
    for tau in range(1, max_tau):
        # 计算Allan方差
        diff = data[2*tau:] - 2*data[tau:-tau] + data[:-2*tau]
        var = np.mean(diff**2) / (2 * tau**2)
        
        taus.append(tau)
        variances.append(var)
    
    return np.array(taus), np.array(variances)

# 使用示例
# 假设gyro_data是陀螺仪静态数据
taus, variances = allan_variance(gyro_data)

# 双对数坐标绘图
plt.loglog(taus, variances)
plt.xlabel('时间间隔 τ (s)')
plt.ylabel('Allan方差')
plt.grid(True)
plt.show()

好了，这一章的内容就到这里。传感器误差模型和标定，是机器人感知的基石。你想想看，如果连传感器的误差都搞不清楚，后面做再复杂的融合算法也是白费力气。下一章咱们聊聊多传感器时间同步，这也是个容易踩坑的地方。