网站导航
> 技术文档 > 【传感器标定（四）：多传感器融合定位系统中的标定与时间同步方案】

【传感器标定（四）：多传感器融合定位系统中的标定与时间同步方案】

网友: 技术文档 2025-07-26 21:04:31

1. 系统框架概述

本方案采用\"三层标定框架\"，整体架构如下图所示：

#mermaid-svg-WhuG9fzKdHSAzSNh {font-family:\"trebuchet ms\",verdana,arial,sans-serif;font-size:16px;fill:#333;}#mermaid-svg-WhuG9fzKdHSAzSNh .error-icon{fill:#552222;}#mermaid-svg-WhuG9fzKdHSAzSNh .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-WhuG9fzKdHSAzSNh .edge-thickness-normal{stroke-width:2px;}#mermaid-svg-WhuG9fzKdHSAzSNh .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-WhuG9fzKdHSAzSNh .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-WhuG9fzKdHSAzSNh .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-WhuG9fzKdHSAzSNh .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-WhuG9fzKdHSAzSNh .marker{fill:#333333;stroke:#333333;}#mermaid-svg-WhuG9fzKdHSAzSNh .marker.cross{stroke:#333333;}#mermaid-svg-WhuG9fzKdHSAzSNh svg{font-family:\"trebuchet ms\",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-WhuG9fzKdHSAzSNh .label{font-family:\"trebuchet ms\",verdana,arial,sans-serif;color:#333;}#mermaid-svg-WhuG9fzKdHSAzSNh .cluster-label text{fill:#333;}#mermaid-svg-WhuG9fzKdHSAzSNh .cluster-label span{color:#333;}#mermaid-svg-WhuG9fzKdHSAzSNh .label text,#mermaid-svg-WhuG9fzKdHSAzSNh span{fill:#333;color:#333;}#mermaid-svg-WhuG9fzKdHSAzSNh .node rect,#mermaid-svg-WhuG9fzKdHSAzSNh .node circle,#mermaid-svg-WhuG9fzKdHSAzSNh .node ellipse,#mermaid-svg-WhuG9fzKdHSAzSNh .node polygon,#mermaid-svg-WhuG9fzKdHSAzSNh .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-WhuG9fzKdHSAzSNh .node .label{text-align:center;}#mermaid-svg-WhuG9fzKdHSAzSNh .node.clickable{cursor:pointer;}#mermaid-svg-WhuG9fzKdHSAzSNh .arrowheadPath{fill:#333333;}#mermaid-svg-WhuG9fzKdHSAzSNh .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-WhuG9fzKdHSAzSNh .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-WhuG9fzKdHSAzSNh .edgeLabel{background-color:#e8e8e8;text-align:center;}#mermaid-svg-WhuG9fzKdHSAzSNh .edgeLabel rect{opacity:0.5;background-color:#e8e8e8;fill:#e8e8e8;}#mermaid-svg-WhuG9fzKdHSAzSNh .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-WhuG9fzKdHSAzSNh .cluster text{fill:#333;}#mermaid-svg-WhuG9fzKdHSAzSNh .cluster span{color:#333;}#mermaid-svg-WhuG9fzKdHSAzSNh div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:\"trebuchet ms\",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-WhuG9fzKdHSAzSNh :root{--mermaid-font-family:\"trebuchet ms\",verdana,arial,sans-serif;} 单传感器标定 传感器间标定 时空统一框架 在线误差补偿

系统组成：

  • 感知层：Velodyne VLP-16激光雷达、Stereo ZED2摄像头、Xsens MTi-30 IMU、Ublox F9P GPS
  • 计算层：Intel NUC with Ubuntu 20.04 + ROS Noetic
  • 融合层：基于Kalman Filter的紧耦合融合架构

2. 传感器间标定（外参标定）

传感器的内参标定可以看我之前的文章：【传感器标定（一）：张正友相机标定法原理与实现详解】、【传感器标定（二）：IMU标定原理与实现详解】、【传感器标定（三）：激光雷达标定原理与实现详解】。

2.1 激光雷达-相机标定

标定原理：

通过棋盘格建立两种传感器观测的几何约束，目标函数为：

T ^ c a m l i d a r= arg min ⁡ T∑ i = 1 N ∥ π ( T ⋅ P i l i d a r) − p i c a m ∥ 2 \\hat{T}_{cam}^{lidar} = \\argmin_{T} \\sum_{i=1}^{N} \\| \\pi(T \\cdot P_i^{lidar}) - p_i^{cam} \\|^2 T^camlidar​=Targmin​i=1∑N​∥π(T⋅Pilidar​)−picam​∥2

其中：

  • T ∈ S E ( 3 ) T \\in SE(3) T∈SE(3)为待求变换矩阵
  • P i l i d a r P_i^{lidar} Pilidar​为激光点云中的棋盘格角点
  • p i c a m p_i^{cam} picam​为图像中的对应角点
  • π ( ⋅ ) \\pi(\\cdot) π(⋅)为相机投影模型
实施步骤：
  1. 采集同步的棋盘格图像和点云数据（至少15组不同姿态）
  2. 使用OpenCV的findChessboardCorners检测图像角点
  3. 使用基于平面拟合的算法提取点云角点
  4. 采用Levenberg-Marquardt算法优化目标函数

2.2 IMU-激光雷达标定

旋转轴激励法：

通过主动旋转激励激发IMU的角速度响应：

ω i m u= R l i d a r i m u⋅ ω l i d a r \\omega_{imu} = R_{lidar}^{imu} \\cdot \\omega_{lidar} ωimu​=Rlidarimu​⋅ωlidar​

其中 ω l i d a r \\omega_{lidar} ωlidar​通过连续两帧点云的ICP获得。

平移标定：

利用运动过程中IMU加速度积分与激光雷达观测的约束：

t ^ = arg min ⁡ t ∑ ∥ ( v k + 1 l i d a r− v k l i d a r) − R i m u l i d a r ∫ t k t k + 1 ( a i m u− b a ) d t ∥ 2 \\hat{t} = \\argmin_{t} \\sum \\| (v_{k+1}^{lidar} - v_k^{lidar}) - R_{imu}^{lidar} \\int_{t_k}^{t_{k+1}} (a_{imu} - b_a)dt \\|^2 t^=targmin​∑∥(vk+1lidar​−vklidar​)−Rimulidar​∫tk​tk+1​​(aimu​−ba​)dt∥2

2.3 GPS-IMU标定

杆臂效应补偿：

GPS天线相位中心与IMU中心的偏移向量 l b l^b lb满足：

p G P S w = p I M U w + R w bl b p_{GPS}^w = p_{IMU}^w + R_w^b l^b pGPSw​=pIMUw​+Rwb​lb

通过静态多位置观测建立方程组求解。

3. 时间同步方案

3.1 硬件同步

采用PTP (IEEE 1588)协议实现µs级同步：

#mermaid-svg-1MVIo3VxAad6cDZY {font-family:\"trebuchet ms\",verdana,arial,sans-serif;font-size:16px;fill:#333;}#mermaid-svg-1MVIo3VxAad6cDZY .error-icon{fill:#552222;}#mermaid-svg-1MVIo3VxAad6cDZY .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-1MVIo3VxAad6cDZY .edge-thickness-normal{stroke-width:2px;}#mermaid-svg-1MVIo3VxAad6cDZY .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-1MVIo3VxAad6cDZY .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-1MVIo3VxAad6cDZY .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-1MVIo3VxAad6cDZY .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-1MVIo3VxAad6cDZY .marker{fill:#333333;stroke:#333333;}#mermaid-svg-1MVIo3VxAad6cDZY .marker.cross{stroke:#333333;}#mermaid-svg-1MVIo3VxAad6cDZY svg{font-family:\"trebuchet ms\",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-1MVIo3VxAad6cDZY .actor{stroke:hsl(259.6261682243, 59.7765363128%, 87.9019607843%);fill:#ECECFF;}#mermaid-svg-1MVIo3VxAad6cDZY text.actor>tspan{fill:black;stroke:none;}#mermaid-svg-1MVIo3VxAad6cDZY .actor-line{stroke:grey;}#mermaid-svg-1MVIo3VxAad6cDZY .messageLine0{stroke-width:1.5;stroke-dasharray:none;stroke:#333;}#mermaid-svg-1MVIo3VxAad6cDZY .messageLine1{stroke-width:1.5;stroke-dasharray:2,2;stroke:#333;}#mermaid-svg-1MVIo3VxAad6cDZY #arrowhead path{fill:#333;stroke:#333;}#mermaid-svg-1MVIo3VxAad6cDZY .sequenceNumber{fill:white;}#mermaid-svg-1MVIo3VxAad6cDZY #sequencenumber{fill:#333;}#mermaid-svg-1MVIo3VxAad6cDZY #crosshead path{fill:#333;stroke:#333;}#mermaid-svg-1MVIo3VxAad6cDZY .messageText{fill:#333;stroke:#333;}#mermaid-svg-1MVIo3VxAad6cDZY .labelBox{stroke:hsl(259.6261682243, 59.7765363128%, 87.9019607843%);fill:#ECECFF;}#mermaid-svg-1MVIo3VxAad6cDZY .labelText,#mermaid-svg-1MVIo3VxAad6cDZY .labelText>tspan{fill:black;stroke:none;}#mermaid-svg-1MVIo3VxAad6cDZY .loopText,#mermaid-svg-1MVIo3VxAad6cDZY .loopText>tspan{fill:black;stroke:none;}#mermaid-svg-1MVIo3VxAad6cDZY .loopLine{stroke-width:2px;stroke-dasharray:2,2;stroke:hsl(259.6261682243, 59.7765363128%, 87.9019607843%);fill:hsl(259.6261682243, 59.7765363128%, 87.9019607843%);}#mermaid-svg-1MVIo3VxAad6cDZY .note{stroke:#aaaa33;fill:#fff5ad;}#mermaid-svg-1MVIo3VxAad6cDZY .noteText,#mermaid-svg-1MVIo3VxAad6cDZY .noteText>tspan{fill:black;stroke:none;}#mermaid-svg-1MVIo3VxAad6cDZY .activation0{fill:#f4f4f4;stroke:#666;}#mermaid-svg-1MVIo3VxAad6cDZY .activation1{fill:#f4f4f4;stroke:#666;}#mermaid-svg-1MVIo3VxAad6cDZY .activation2{fill:#f4f4f4;stroke:#666;}#mermaid-svg-1MVIo3VxAad6cDZY .actorPopupMenu{position:absolute;}#mermaid-svg-1MVIo3VxAad6cDZY .actorPopupMenuPanel{position:absolute;fill:#ECECFF;box-shadow:0px 8px 16px 0px rgba(0,0,0,0.2);filter:drop-shadow(3px 5px 2px rgb(0 0 0 / 0.4));}#mermaid-svg-1MVIo3VxAad6cDZY .actor-man line{stroke:hsl(259.6261682243, 59.7765363128%, 87.9019607843%);fill:#ECECFF;}#mermaid-svg-1MVIo3VxAad6cDZY .actor-man circle,#mermaid-svg-1MVIo3VxAad6cDZY line{stroke:hsl(259.6261682243, 59.7765363128%, 87.9019607843%);fill:#ECECFF;stroke-width:2px;}#mermaid-svg-1MVIo3VxAad6cDZY :root{--mermaid-font-family:\"trebuchet ms\",verdana,arial,sans-serif;} Master Slave Sync Message (t1) Delay_Req (t3) Follow_Up (t1) Delay_Resp (t4) Master Slave

时钟偏移计算：
θ = ( t 2 − t 1 ) − ( t 4 − t 3 ) 2 \\theta = \\frac{(t2 - t1) - (t4 - t3)}{2} θ=2(t2−t1)−(t4−t3)​

3.2 软件同步

对于无法硬件同步的传感器（如相机），采用基于双缓冲的插值算法：

m ( t ) = α m ( t i ) + ( 1 − α ) m ( t i + 1) ,   α = t i + 1 − t t i + 1 − t i m(t) = \\alpha m(t_i) + (1-\\alpha)m(t_{i+1}), \\ \\alpha = \\frac{t_{i+1}-t}{t_{i+1}-t_i} m(t)=αm(ti​)+(1−α)m(ti+1​), α=ti+1​−ti​ti+1​−t​

3.3 运动补偿

对于激光雷达的扫描式采集，采用IMU辅助的运动补偿：

P k w = T t 0 t k⋅ P k l i d a r P_k^w = T_{t_0}^{t_k} \\cdot P_k^{lidar} Pkw​=Tt0​tk​​⋅Pklidar​

其中 T t 0t k T_{t_0}^{t_k} Tt0​tk​​由IMU积分得到。

4. 在线标定与误差补偿

4.1 基于EKF的在线标定

状态向量包含标定参数：

x = [ p , v , q ⏟ 状态 , b a , b g ⏟ I M U 偏置, t l i d a r , θ c a m ⏟ 标定参数 ] x = [\\underbrace{p,v,q}_{状态}, \\underbrace{b_a,b_g}_{IMU偏置}, \\underbrace{t_{lidar},\\theta_{cam}}_{标定参数}] x=[状态 p,v,q​​,IMU偏置 ba​,bg​​​,标定参数 tlidar​,θcam​​​]

系统模型：
x ˙ = f ( x , u ) + w ,    u = [ a i m u, ω i m u] \\dot{x} = f(x,u) + w, \\ \\ u=[a_{imu},\\omega_{imu}] x˙=f(x,u)+w,  u=[aimu​,ωimu​]

观测模型：
z = h ( x ) + v ,    z = [ G P S , L i d a r _ F e a t u r e , C a m _ F e a t u r e ] z = h(x) + v, \\ \\ z=[GPS, Lidar\\_Feature, Cam\\_Feature] z=h(x)+v,  z=[GPS,Lidar_Feature,Cam_Feature]

4.2 标定质量评估

采用Mahalanobis距离检测标定退化：

D M = ( z − h ( x ) ) T S − 1 ( z − h ( x ) ) D_M = \\sqrt{(z-h(x))^T S^{-1} (z-h(x))} DM​=(z−h(x))TS−1(z−h(x)) ​

当 D M > χ d f , 0.95 2 D_M > \\chi^2_{df,0.95} DM​>χdf,0.952​时触发重新标定。

5. 完整实施流程

阶段1：离线标定

  1. 单传感器标定（参见前文）
  2. 传感器对间标定（2.1-2.3节）
  3. 时间同步校准（3.1节）

阶段2：在线维护

  1. 启动时验证标定参数
  2. 运行时监控标定状态
  3. 动态更新标定参数（4.1节）

6. 实验验证方案

定量评估指标：

  1. 重投影误差（激光-相机）：
    ϵ r e p r o j = 1 N ∑ i = 1 N ∥ π ( T ⋅ P i ) − p i ∥ \\epsilon_{reproj} = \\frac{1}{N}\\sum_{i=1}^N \\|\\pi(T\\cdot P_i) - p_i \\| ϵreproj​=N1​i=1∑N​∥π(T⋅Pi​)−pi​∥

  2. 轨迹对齐误差（GPS-激光）：
    ϵ a l i g n = RMSE ( t r a j l i d a r , t r a j G P S ) \\epsilon_{align} = \\text{RMSE}(traj_{lidar}, traj_{GPS}) ϵalign​=RMSE(trajlidar​,trajGPS​)

  3. 时间同步精度：
    ϵ s y n c = max ⁡ ∣ t t r i g g e r − t a c t u a l ∣ \\epsilon_{sync} = \\max |t_{trigger} - t_{actual}| ϵsync​=max∣ttrigger​−tactual​∣

7. 关键实现细节

标定工具推荐：

  1. 激光-相机：Autoware的calibration_camera_lidar工具
  2. IMU-激光：Kalibr工具箱的imu_lidar_calibration
  3. 时间同步：ROS的message_filters包

代码片段示例（时间对齐）：

class TimeAligner: def __init__(self, max_delay=0.1): self.buffer = {} self.max_delay = max_delay def add_msg(self, topic, msg): self.buffer.setdefault(topic, []).append((msg.header.stamp, msg)) def get_synced_msgs(self, sync_topic): sync_msgs = {} for topic in self.buffer: idx = bisect.bisect_left( [ts for ts,_ in self.buffer[topic]],  self.buffer[sync_topic][0][0]) if idx > 0 and abs(self.buffer[topic][idx-1][0] - self.buffer[sync_topic][0][0]) < self.max_delay: sync_msgs[topic] = self.buffer[topic][idx-1][1] return sync_msgs

8. 常见问题解决方案

标定失败排查：

  1. 特征提取不足：确保标定板在两种传感器中均清晰可见
  2. 运动激励不足：IMU标定需充分激励所有轴
  3. 时间戳异常：检查ROS的/use_sim_time参数设置

精度提升技巧：

  1. 在标定期间保持环境光照稳定
  2. 对GPS标定选择开阔场地
  3. 标定温度接近实际工作温度
网络标签：

相关问题