从滤波到优化手把手拆解VIO算法核心看懂OpenVINS的MSCKF和ORB-SLAM3的BA到底差在哪视觉惯性里程计VIO技术近年来在机器人导航、增强现实等领域展现出强大的生命力。对于开发者而言理解不同技术流派的核心差异远比记住几个算法名字更有价值。本文将聚焦OpenVINS和ORB-SLAM3这两个代表性框架深入剖析滤波与优化方法在VIO实现中的本质区别。1. 状态估计的两大流派滤波与优化在VIO领域状态估计方法主要分为滤波和优化两大阵营。滤波方法以卡尔曼滤波为代表通过预测-更新的递推方式处理传感器数据而优化方法则基于批量非线性优化通过最小化重投影误差来求解状态。滤波方法的核心特点递推式处理每次只处理当前时刻的观测马尔可夫性假设当前状态只依赖于前一时刻状态计算效率高适合实时系统优化方法的核心特点批量处理同时优化多个时刻的状态利用历史观测信息构建全局一致性精度更高但计算量相对较大提示虽然理论上滤波和优化在理想条件下能达到相同精度但实际实现中的各种近似会导致性能差异。2. OpenVINS的MSCKF滤波方法的典型实现OpenVINS采用了多状态约束卡尔曼滤波MSCKF作为其核心算法。这种滤波方法通过维护一个滑动窗口内的多个相机状态利用特征点在多个帧间的观测构建几何约束。2.1 MSCKF的核心机制MSCKF的关键创新在于状态扩增维护一个包含多个相机位姿的状态向量特征参数化使用逆深度等参数化方式表示路标点观测压缩将多个观测压缩为一个等效观测减少计算量// MSCKF状态向量示例 StateVector { IMU状态: [位置, 速度, 姿态, 陀螺仪bias, 加速度计bias] 相机状态: [位姿1, 位姿2, ..., 位姿N] }2.2 FEJ保持线性化一致性MSCKF采用首次估计雅可比FEJ技术来解决滤波器的线性化不一致问题。FEJ的核心思想是对所有观测使用相同的线性化点避免重复线性化带来的误差保持信息矩阵的一致性FEJ实现效果对比方法线性化点选择计算复杂度一致性保持标准EKF当前估计值低差FEJ-EKF首次估计值中好滑动窗口优化多次线性化高最优3. ORB-SLAM3的BA优化方法的精妙设计ORB-SLAM3采用基于图优化的方法通过捆绑调整BA来实现状态估计。其核心是通过构建视觉重投影误差和IMU预积分误差的联合优化问题。3.1 BA的数学表达ORB-SLAM3的优化问题可以表示为min_{X} (∑||z_vision - h_vision(X)||^2_Σv ∑||z_imu - h_imu(X)||^2_Σi)其中X包含所有待优化状态位姿、速度、bias、路标点h_vision和h_imu分别是视觉和IMU的观测模型Σv和Σi是各观测的协方差矩阵3.2 边缘化策略ORB-SLAM3采用边缘化来处理历史信息将旧状态转换为先验信息保留其对当前状态的影响避免直接优化所有历史状态边缘化实现步骤构建包含待边缘化状态的Hessian矩阵进行Schur补操作得到边缘化后的先验将先验信息加入后续优化问题4. 滤波与优化的本质差异虽然MSCKF和BA在理论上等价但实际实现中存在几个关键差异点4.1 线性化处理方式特性MSCKFBA线性化次数单次多次线性化点固定迭代更新一致性依赖FEJ自然保持4.2 历史信息利用MSCKF的处理方式滑动窗口内状态直接估计窗口外信息通过边缘化丢弃信息利用率较低BA的处理方式通过边缘化保留历史信息影响构建更完整的概率图模型信息利用率高4.3 计算效率对比在EuRoC数据集上的实测表现指标OpenVINSORB-SLAM3平均处理时间15ms45ms内存占用较低较高CPU利用率更均衡有峰值5. 实际应用中的选择建议根据我们在多个实际项目中的经验滤波和优化方法各有最适合的场景选择MSCKF当硬件资源有限需要极高实时性运动较为平缓选择BA当追求最高精度有足够计算资源运动复杂多变一个实用的折中方案是采用基于优化的紧耦合VIO作为前端配合滤波方法进行状态预测这样既能保证精度又能满足实时性要求。