传感器标定与系统集成

1. 相机内参标定——张正友标定法

针孔相机模型

$K$就是内参矩阵——$f_x, f_y$是焦距（像素），$c_x, c_y$是主点。相机精度等级就体现在这5个数上。

import cv2, numpy as np, glob
pattern = (9, 6); square_size = 25.0  # mm
objp = np.zeros((pattern[0]*pattern[1], 3), np.float32)
objp[:,:2] = np.mgrid[0:pattern[0], 0:pattern[1]].T.reshape(-1,2) * square_size
objpoints, imgpoints = [], []
for f in glob.glob('calib_images/*.jpg'):
    img = cv2.imread(f); gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern, None)
    if ret: objpoints.append(objp); imgpoints.append(corners)
ret, K, dist, _, _ = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
print(f"内参 K:\n{K}\n重投影误差: {ret:.4f} px")

2. 手眼标定——AX=XB

def hand_eye_calibration(A_list, B_list):
    """Tsai-Lenz手眼标定: AX=XB"""
    Ra = [A[:3,:3] for A in A_list]
    ta = [A[:3,3] for A in A_list]
    Rb = [B[:3,:3] for B in B_list]
    tb = [B[:3,3] for B in B_list]
    N = len(A_list)
    # 最小二乘求解
    alpha = np.vstack([np.eye(3) - Ra[i] for i in range(N)])
    beta  = np.vstack([(ta[i] - Ra[i] @ tb[i]).reshape(-1,1) for i in range(N)])
    t_X = np.linalg.lstsq(alpha, beta, rcond=None)[0].flatten()
    return t_X

3. 多传感器时间同步

LiDAR-Camera外参标定

激光雷达给出3D点云，相机给出2D图像。标定目标是找到它们之间的6D变换。

IMU内参标定

IMU有偏置(bias)、尺度因子(scale)、轴偏差(misalignment)。让IMU静止在多个姿态记录输出，然后用最小二乘拟合——与力传感器标定逻辑完全一样。

4. LiDAR-Camera外参标定

问题背景

激光雷达给出3D点$(X,Y,Z)$，相机给出2D像素$(u,v)$。标定目标：找到$T_{lidar}^{cam}$使得激光点投影到图像上恰好落在对应像素上。

def project_lidar_to_camera(points_3d, K, R, t):
    """激光点云 → 图像像素坐标"""
    points_cam = (R @ points_3d.T + t.reshape(3,1)).T
    points_img = (K @ points_cam.T).T
    uv = points_img[:, :2] / points_img[:, 2:]
    return uv

IMU内参标定

IMU有偏置、尺度因子、轴偏差。让IMU静止在多个姿态，记录加速度计和陀螺仪输出，最小二乘拟合——与力传感器标定逻辑完全一样。

5. Tsai-Lenz手眼标定——完整数学推导与代码

为什么手眼标定如此重要

在机器人视觉伺服中，相机看到物体→机械臂去抓取。这个过程中你必须知道相机坐标系相对于机械臂末端（Eye-in-Hand）或基座（Eye-to-Hand）的变换。一个0.5°的角度误差在500mm距离上就是4.4mm的位置偏差——足以让抓取失败。

AX=XB问题的数学结构

其中：$A_i$是机械臂末端在基座坐标系下的位姿变化（已知），$B_i$是相机观测到的标定板位姿变化（已知），$X$是相机到手（或基座）的固定变换（待求解）。

旋转部分分离求解

将齐次变换拆成旋转和平移：

展开得到两个方程：

Tsai-Lenz两步法

第一步：求解旋转。利用旋转矩阵的轴角表示。对每组$A_i, B_i$计算轴角向量，然后建立最小二乘问题。

import numpy as np
from scipy.linalg import logm, expm

def rotation_to_axis_angle(R):
    """旋转矩阵 → 轴角向量 (θ·n)"""
    # 提取旋转角
    theta = np.arccos((np.trace(R) - 1) / 2)
    if abs(theta) < 1e-10:
        return np.zeros(3)
    # 提取旋转轴
    axis = np.array([R[2,1] - R[1,2],
                     R[0,2] - R[2,0],
                     R[1,0] - R[0,1]]) / (2 * np.sin(theta))
    return theta * axis

def tsai_lenz_rotation(A_list, B_list):
    """Tsai-Lenz法求解旋转部分R_X"""
    N = len(A_list)
    
    # 提取旋转矩阵
    Ra = [A[:3, :3] for A in A_list]
    Rb = [B[:3, :3] for B in B_list]
    
    # 转换为轴角向量
    Pa = np.array([rotation_to_axis_angle(R) for R in Ra])
    Pb = np.array([rotation_to_axis_angle(R) for R in Rb])
    
    # 构建3N×3的最小二乘问题: skew(P_a + P_b) * P_X' = P_b - P_a
    # 其中P_X'是旋转向量P_X的偏斜对称形式变换
    M = np.zeros((3 * N, 3))
    v = np.zeros(3 * N)
    
    for i in range(N):
        a, b = Pa[i], Pb[i]
        # skew(a + b)
        S = np.array([
            [0,        -(a[2]+b[2]),  a[1]+b[1]],
            [ a[2]+b[2],  0,        -(a[0]+b[0])],
            [-(a[1]+b[1]), a[0]+b[0],  0]
        ])
        M[3*i:3*(i+1)] = S
        v[3*i:3*(i+1)] = b - a
    
    # 最小二乘求解P_X'
    P_X_prime = np.linalg.lstsq(M, v, rcond=None)[0]
    
    # 从P_X'恢复旋转矩阵
    theta = np.linalg.norm(P_X_prime)
    if theta < 1e-10:
        return np.eye(3)
    # 从P_X'反算原始轴角P_X
    P_X = 2 * P_X_prime / np.sqrt(1 + np.sum(P_X_prime**2))
    # 轴角 → 旋转矩阵（Rodrigues公式）
    theta = np.linalg.norm(P_X)
    axis = P_X / theta
    return expm(np.array([
        [0, -axis[2], axis[1]],
        [axis[2], 0, -axis[0]],
        [-axis[1], axis[0], 0]
    ]) * theta)

第二步：求解平移。旋转已知后，平移退化为线性最小二乘：

def tsai_lenz_translation(A_list, B_list, R_X):
    """已知旋转，求解平移部分t_X"""
    N = len(A_list)
    Ra = [A[:3, :3] for A in A_list]
    ta = [A[:3, 3] for A in A_list]
    tb = [B[:3, 3] for B in B_list]
    
    # 构建: (I - R_A) t_X = t_A - R_X t_B
    M = np.vstack([np.eye(3) - Ra[i] for i in range(N)])
    v = np.hstack([ta[i] - R_X @ tb[i] for i in range(N)])
    
    t_X = np.linalg.lstsq(M, v, rcond=None)[0]
    return t_X

为什么至少需要2组非平行旋转轴的运动

如果所有$A_i$的旋转轴都平行，方程组退化为秩亏——你只能求解绕该轴的分量，垂直于轴的分量无法确定。这就像你校准一个夹具时，如果只在同一个方向上加载，你只能确定该方向的刚度。必须至少有两组旋转轴夹角>15°的运动，方程组才满秩。

6. IMU Allan方差分析与内参标定

IMU噪声的五种成分

Allan方差计算方法

def compute_allan_variance(data, dt, max_clusters=200):
    """计算Allan方差（非重叠法）"""
    N = len(data)
    taus = []  # 聚类时间（平均时间）
    allan_vars = []
    
    for m in range(1, min(N//3, max_clusters)):
        tau = m * dt
        n_clusters = N // m
        
        # 将数据分成m长度的簇
        clusters = data[:n_clusters * m].reshape(n_clusters, m)
        cluster_means = clusters.mean(axis=1)
        
        # Allan方差 = 1/2 * <相邻簇均值差的平方的平均>  
        diff = np.diff(cluster_means)
        avar = 0.5 * np.mean(diff ** 2)
        adev = np.sqrt(avar)
        
        taus.append(tau)
        allan_vars.append(adev)
    
    return np.array(taus), np.array(allan_vars)

# 从Allan方差曲线提取噪声参数
def extract_imu_noise_params(taus, adev):
    """从ADEV曲线提取ARW、Bias Instability、RRW"""
    # 角度随机游走: tau=1s处的ADEV（应位于-1/2斜率段）
    idx_1s = np.argmin(abs(taus - 1.0))
    ARW = adev[idx_1s]  # 单位: rad/sqrt(s) 或 deg/sqrt(hr)
    
    # 偏置不稳定性: ADEV曲线最小值
    bias_instability = np.min(adev)
    
    # 速率随机游走: tau=100s附近的ADEV（+1/2斜率段）
    idx_100s = np.argmin(abs(taus - 100.0))
    RRW = adev[idx_100s] / np.sqrt(taus[idx_100s])
    
    return {'ARW': ARW, 'BiasInstability': bias_instability, 'RRW': RRW}

7. 多传感器时间戳同步——为什么这是最难的部分

异步传感器的时间错位问题

每个传感器有自己的时钟：相机30Hz、LiDAR 10Hz、IMU 200Hz、机械臂编码器1000Hz。它们之间没有物理同步线时，同一个物理时刻在不同传感器上的时间戳可能差几十毫秒。对于移动中的机器人，30ms的时间差在1m/s速度下就是3cm的位置误差。

硬件同步 vs 软件同步

时间戳插值——最实用的软件同步方法

def interpolate_at_timestamp(data, data_timestamps, query_timestamp):
    """在任意时间戳t处插值获取传感器数据"""
    # 找到query_timestamp前后的两个数据点
    idx = np.searchsorted(data_timestamps, query_timestamp)
    if idx == 0 or idx >= len(data_timestamps):
        return None  # 超出范围
    
    t1, t2 = data_timestamps[idx-1], data_timestamps[idx]
    d1, d2 = data[idx-1], data[idx]
    
    # 线性插值
    alpha = (query_timestamp - t1) / (t2 - t1)
    return d1 + alpha * (d2 - d1)

def sync_multisensor_data(camera_data, camera_ts, lidar_data, lidar_ts, imu_data, imu_ts):
    """将多传感器数据对齐到统一时间轴"""
    # 以最慢传感器的时间为基准
    base_ts = np.intersect1d(
        np.round(camera_ts, 3),
        np.round(lidar_ts, 3)
    )
    
    synced = []
    for t in base_ts:
        cam = interpolate_at_timestamp(camera_data, camera_ts, t)
        lid = interpolate_at_timestamp(lidar_data, lidar_ts, t)
        imu = interpolate_at_timestamp(imu_data, imu_ts, t)
        if all(x is not None for x in [cam, lid, imu]):
            synced.append((t, cam, lid, imu))
    
    return synced

标定质量评估——你的工程直觉

作为机械工程师，你天然理解"测量→误差分析→验收"流程。标定也一样：

• 重投影误差 < 0.5px：相机内参标定合格的工业标准
• 手眼标定精度 < 2mm@500mm：用验证位姿（未参与标定的数据）测试
• LiDAR-Camera投影误差 < 3px：点云投影到图像后与边缘对齐
• IMU零偏稳定性 < 10°/hr：Allan方差谷底值

8. 标定流程实战——从理论到完整pipeline

工业级标定的完整流程

Step 1: 准备工作——打印标定板（棋盘格10×7,方格30mm）、固定相机、清洁镜头
Step 2: 数据采集——移动标定板覆盖相机视野的四个角和中心，每张图片标定板姿态变化 > 15°
Step 3: 角点检测——亚像素精度角点提取
Step 4: 内参标定——张正友法求解K和畸变参数
Step 5: 手眼标定——机械臂运动N个位姿，记录末端位姿和标定板位姿
Step 6: 质量验证——用验证集评估重投影误差、手眼标定精度
Step 7: 保存标定结果——YAML/JSON格式保存供后续使用

import yaml

class CalibrationPipeline:
    def __init__(self, checkerboard_size=(9,6), square_size=0.03):
        self.checkerboard_size = checkerboard_size
        self.square_size = square_size
        self.K = None
        self.dist = None
        self.hand_eye_X = None
    
    def calibrate_camera(self, images):
        """张正友相机内参标定"""
        obj_points = []
        img_points = []
        
        objp = np.zeros((self.checkerboard_size[0] * self.checkerboard_size[1], 3), np.float32)
        objp[:, :2] = np.mgrid[0:self.checkerboard_size[0],
                                   0:self.checkerboard_size[1]].T.reshape(-1, 2)
        objp *= self.square_size
        
        for img in images:
            gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
            ret, corners = cv2.findChessboardCorners(gray, self.checkerboard_size)
            if ret:
                # 亚像素精细化
                corners_sub = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1),
                    (cv2.TermCriteria_EPS + cv2.TermCriteria_MAX_ITER, 30, 0.001))
                obj_points.append(objp)
                img_points.append(corners_sub)
        
        ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
            obj_points, img_points, gray.shape[::-1], None, None)
        
        self.K = K
        self.dist = dist
        return K, dist, ret
    
    def save_calibration(self, filepath):
        """保存标定结果供后续加载"""
        calib_data = {
            'camera_matrix': self.K.tolist(),
            'dist_coeffs': self.dist.tolist(),
            'hand_eye_transform': self.hand_eye_X.tolist() if self.hand_eye_X else None,
        }
        with open(filepath, 'w') as f:
            yaml.dump(calib_data, f, default_flow_style=False)