第238篇 SLAM中的优化vs滤波——两大技术路线的选择

上一篇把概率论和贝叶斯估计的基础讲清楚了。这篇来聊一个SLAM领域绕不开的话题优化和滤波到底选哪个这个问题在SLAM的发展史上争论了二十多年。早期大家都在做滤波觉得这就是SLAM的标准答案。后来优化方法异军突起凭借更高的精度和更好的一致性把滤波打得节节败退。但滤波并没有死而是在特定场景下找到了自己的位置。面试的时候如果你能把这两条路线的优劣讲清楚面试官会觉得你对SLAM的理解是有深度的。滤波路线递推估计的优雅滤波的核心思想是走一步看一步。每个时刻做两件事预测根据运动模型估计当前位姿和更新用观测数据修正估计。最经典的代表是EKF-SLAM。状态向量包含机器人的位姿和所有路标点的位置协方差矩阵描述它们的不确定性和相关性。每来一个新观测卡尔曼滤波就用一套漂亮的公式更新状态估计和协方差。滤波的优势很明显。它是递推的每次只处理当前时刻的数据内存占用恒定。理论上它给出了最优的高斯近似解数学上很优雅。对于小规模问题滤波的速度非常快。但滤波有个致命缺陷一致性。EKF在做线性化的时候用的是当前估计值而不是真实值。这个线性化误差会累积导致协方差矩阵越来越小系统越来越自信但实际上估计可能已经偏了。这就是所谓的估计不一致问题。# EKF-SLAM的核心步骤 def ekf_update(x, P, z, H, R): # x: 状态向量, P: 协方差矩阵 # z: 观测, H: 观测雅可比, R: 观测噪声 K P H.T np.linalg.inv(H P H.T R) x x K (z - h(x)) P (np.eye(len(x)) - K H) P return x, P而且滤波的复杂度是O(n²)n是路标点的数量。地图一大协方差矩阵的更新就成了瓶颈。这就是为什么EKF-SLAM很难处理大规模环境。优化路线全局视角的力量优化方法的思路完全不同。它不是走一步看一步而是把所有位姿和路标点都当成优化变量把所有观测都当成约束构建一个大的最小二乘问题然后一次性求解。目标函数是所有观测误差的加权和min Σ ||z - h(x, m)||²_R这里的x是所有位姿m是所有路标点R是观测噪声的协方差。求解这个优化问题就能得到全局最优的位姿和地图估计。优化的优势在于它用的是所有历史数据来做估计不会出现滤波那种线性化点不对导致的不一致问题。而且优化可以利用稀疏性——SLAM的观测天然是稀疏的每个观测只涉及相邻的几个位姿和少量路标点对应的海塞矩阵是稀疏的用特殊的矩阵分解方法可以高效求解。# 优化方法的直觉高斯-牛顿法 for iteration in range(max_iter): J compute_jacobian(x, landmarks) # 计算雅可比 r compute_residual(x, landmarks) # 计算残差 delta solve(J.T J, J.T r) # 求解增量方程 x x delta # 更新优化的缺点也很明显。每次优化要处理所有数据计算量大。如果地图很大一次优化的耗时可能超过实时要求。所以纯优化方法很难做到严格的实时。折中方案滑动窗口优化纯滤波和纯优化都有明显短板实际中很多系统用的是折中方案——滑动窗口优化。思路是这样的不优化所有历史数据只优化最近N帧的位姿和路标点。窗口之外的变量被边缘化marginalize掉它们的信息被压缩成一个先验约束保留在窗口内。VINS-Mono就是典型的滑动窗口方案。它维护一个固定大小的滑动窗口默认10帧IMU视觉数据每次窗口向前滑动一帧最老的一帧被边缘化。这样既保证了优化问题的规模可控实时性又比纯滤波更准确因为同时优化了多个变量。边缘化有个副作用它会在因子图中引入填充fill-in破坏稀疏性。这就是为什么VINS-Mono在某些情况下精度不如全量BA。但工程上这是个可以接受的妥协。# 滑动窗口的直觉 window_size 10 for new_frame in data_stream: window.add(new_frame) if len(window) window_size: oldest window.pop_oldest() marginalize(oldest) # 边缘化最老的帧 optimize(window) # 优化窗口内的变量滑动窗口优化可以看成滤波和优化之间的桥梁。它既有滤波的实时性固定窗口大小又有优化的精度同时优化多个变量。现代SLAM的选择前后端分离现在主流的SLAM系统都采用前后端分离的架构把两种方法的优势结合起来。前端负责实时的位姿估计。用轻量级的方法PnP、ICP、光流法快速算出当前帧的位姿。前端不追求全局最优只求快和稳。后端负责全局优化。积累了一定量的数据之后后端做一次全局优化消除前端的累积误差。后端可以用优化方法BA、位姿图优化也可以用滤波。回环检测是第三个模块。它负责发现机器人回到了之前经过的地方给后端提供额外的约束。没有回环检测累积误差无法消除。这个架构的好处是前端保证了实时性后端保证了精度。两者各司其职互不干扰。ORB-SLAM2、VINS-Mono、Cartographer都是这个架构的典型代表。面试中怎么聊这个话题面试官问优化和滤波的区别你要讲清楚三个层次。第一层是思想区别。滤波是递推的每来一帧更新一次优化是批量的把所有数据放在一起求解。第二层是优缺点对比。滤波实时性好但有一致性问题优化精度高但计算量大。滤波的复杂度O(n²)优化可以利用稀疏性做到接近线性。第三层是实际选择。现代SLAM普遍用优化做后端但前端的状态估计比如VINS-Mono里的滑动窗口优化本质上也是一种受限的滤波。两者不是非此即彼的关系。面试官可能还会追问为什么优化方法能利用稀疏性 因为SLAM的因子图里每个因子只连接少数几个变量。对应的海塞矩阵大部分元素是零。用Cholesky分解或者Schur补这些技巧可以跳过零元素的计算大幅加速。滤波真的被淘汰了吗 没有。信息滤波卡尔曼滤波的信息形式在协同SLAM和多机器人SLAM里还有应用。粒子滤波在全局定位不知道初始位置场景下是唯一的方案。IMU预积分的数学本质也是滤波。边缘化是什么意思 简单说就是把一个变量从优化问题里消掉同时把它对其他变量的约束信息保留下来。数学上就是对联合高斯分布做条件概率的推导。边缘化之后剩下的变量之间会产生新的约束填充这就是为什么稀疏性会被破坏。面试的时候如果被问到你更倾向优化还是滤波别急着站队。好的回答是分析场景实时性要求高、资源受限的嵌入式平台滤波可能更合适精度要求高、有足够计算资源的平台优化更好。大多数现代系统用的是两者的混合方案。两种方法的代码对比# 滤波递推更新 def filtering_loop(observations): x, P init_state() for z in observations: x, P predict(x, P) # 预测步 x, P update(x, P, z) # 更新步 return x # 只保留当前状态 # 优化批量求解 def optimization_loop(observations): graph build_factor_graph() for z in observations: graph.add_factor(z) # 添加因子 result graph.optimize() # 一次性求解 return result # 得到所有时刻的解滤波返回的是当前时刻的估计优化返回的是所有时刻的估计。这是本质区别。优化和滤波不是对立的而是互补的。现代SLAM系统把两者的优点融合在一起才有了我们今天看到的成熟方案。理解了这两条路线的本质区别和各自的适用场景你面对面试问题时就能从容应对。上一篇第237篇 SLAM数学基础——概率论与贝叶斯估计下一篇我们进入视觉SLAM前端聊聊特征提取——ORB、SIFT、SuperPoint这三种方法各自有什么特点面试中怎么回答它们的区别。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力