晋城企业网站建设价格,西安市未央区建设局官方网站,网站备案新增域名,o2o网站建设方案讲解1. 初识MSCKF#xff1a;它到底是什么#xff0c;能解决什么问题#xff1f; 如果你玩过无人机或者研究过机器人定位#xff0c;肯定听说过VIO#xff08;视觉惯性里程计#xff09;这个词。简单来说#xff0c;VIO就是让机器同时用“眼睛”#xff08;相机#xff09…1. 初识MSCKF它到底是什么能解决什么问题如果你玩过无人机或者研究过机器人定位肯定听说过VIO视觉惯性里程计这个词。简单来说VIO就是让机器同时用“眼睛”相机和“感觉”IMU惯性测量单元来知道自己在哪里、怎么动。MSCKFMulti-State Constraint Kalman Filter多状态约束卡尔曼滤波就是VIO领域里一个非常经典且高效的算法框架。我第一次接触MSCKF是在做一个室内无人机项目的时候。当时试过直接做视觉里程计发现一旦画面模糊或者快速运动定位就很容易飘掉也试过纯IMU积分短时间还行时间一长误差累积得吓人。后来发现把两者结合起来才是正解而MSCKF提供了一种特别“聪明”的结合方式。它不像有些方法那样把看到的所有路标点都当作状态来估计那样状态向量会爆炸式增长。MSCKF的核心思想是相机拍到的特征点我用完就扔但把拍下这些特征点的相机位姿“记住”一段时间。这样状态向量里主要维护的是当前的IMU状态和最近的一系列相机位姿规模是固定的计算量可控。那么MSCKF具体是怎么工作的呢你可以把它想象成一个不断“预测-修正”的循环。IMU数据来得快通常几百赫兹它负责高频地预测机器人的运动状态位置、速度、姿态。相机图像来得慢通常几十赫兹每来一张新图MSCKF就做两件大事一是根据最新的IMU状态“克隆”出一个新的相机状态加入到记忆列表中二是检查那些已经“跟丢”的特征点利用它们被多个历史相机位姿观测到的事实形成一个几何约束反过来修正所有相关的状态包括当前的IMU状态和那些历史相机位姿。这个“克隆”相机状态的过程就是状态扩增它是连接IMU预测和视觉观测的关键桥梁。理解了从IMU状态预测到相机状态扩增这个完整流程你就算抓住了MSCKF的魂。2. IMU状态预测高频运动的“预言家”IMU是机器人感知自身运动的“小脑”它每秒能输出几百次加速度和角速度数据。在MSCKF中IMU数据的核心任务就是状态预测也就是在两次相机图像到来的间隙不断地推算机器人当前的位置、速度和姿态。这部分完全属于卡尔曼滤波中的“预测”步骤。2.1 IMU状态传播用数值积分“拼”出运动轨迹IMU给出的是瞬时加速度和角速度我们想要的是位置和姿态的变化这中间需要通过积分来连接。MSCKF通常采用4阶龙格-库塔法来进行积分。为什么是它因为它是在精度和计算量之间一个很好的平衡点。我来打个比方。假设你要从A点开车到B点只知道每秒的速度计读数。最简单的办法欧拉积分是用这一秒初的速度乘以1秒得到这一秒的位移。但车速可能在变化这个结果就不太准。好一点的办法中点法是用这一秒中间时刻的速度来算。而4阶龙格-库塔法则更“聪明”它在这一秒内采样四个点的“速度”实际上是导数值进行加权平均得到的结果最接近真实的运动轨迹。在代码里这个过程是迭代进行的。每收到一个IMU数据就执行一次姿态预测用当前的角速度测量值减去估计的零偏来更新旋转四元数。这里要注意处理角速度很小时的数值稳定性问题。速度预测用更新后的姿态将加速度计测量值减去零偏转换到世界坐标系并扣除重力然后对加速度进行积分得到速度变化。位置预测对速度进行积分得到位置变化。具体到公式对于旋转我们使用四元数微分方程进行更新。当角速度较大时采用标准的指数映射更新当角速度很小时采用一阶近似以避免数值问题。对于速度和位置则直接应用龙格-库塔积分公式。这个过程只更新IMU的p, v, q位置、速度、姿态而加速度计和陀螺仪的零偏ba, bg在这个预测阶段被认为是常数它们会在后续的观测更新中被修正。2.2 协方差矩阵传播不确定性如何“生长”光预测出状态值还不够我们还得知道这个预测有多“不确定”这就是协方差矩阵P的意义。它描述了状态估计误差的不确定性大小以及不同状态误差之间的关联关系。在IMU预测过程中随着时间推移由于传感器噪声的存在我们对状态估计的信心会下降不确定性会增大反映为协方差矩阵P的“膨胀”。协方差的传播基于误差状态的线性化模型。我们定义误差状态向量比如姿态误差用3维旋转向量表示然后推导出它的连续时间微分方程δx_dot F * δx G * n。其中F是状态转移矩阵描述了误差如何随时间演化G是噪声驱动矩阵n是IMU的噪声向量加速度计和陀螺仪的白噪声。在离散时间即每个IMU周期下我们需要将这个连续模型离散化得到误差状态的递推方程δx_k Φ * δx_{k-1} n_d。这里的Φ是离散状态转移矩阵可以通过对F矩阵进行指数积分或一阶近似得到n_d是离散化的噪声项。有了这个协方差矩阵的更新就清晰了P_{k|k-1} Φ * P_{k-1|k-1} * Φ^T Q_d。Q_d是离散过程噪声协方差矩阵由G和噪声强度n计算得来。这个公式直观地告诉我们上一时刻的不确定性P_{k-1}经过状态转移Φ被传递和变换同时叠加了本步骤新增的过程噪声Q_d共同构成了当前预测状态的不确定性P_{k|k-1}。在实际的MSCKF实现中比如开源的S-MSCKFIMU状态可能是21维包含位置、速度、姿态、加速度零偏、角速度零偏以及时间偏移等。因此P矩阵中对应IMU状态的左上角21x21块会在每个IMU数据到来时按照上述规则进行更新。而P矩阵中与相机状态相关的部分在纯IMU预测阶段暂时保持不变。3. 相机状态扩增架起视觉与IMU的桥梁每到来一帧新的图像MSCKF就要执行一个关键操作状态扩增。这是MSCKF区别于传统滤波SLAM的一个标志性设计。它的目的是为后续的视觉观测更新准备好“约束对象”。3.1 计算相机位姿从IMU“衍生”出相机状态扩增的第一步很简单就是根据当前的IMU状态和已知的相机-IMU外参计算出当前时刻相机在世界坐标系下的位姿。公式非常直观相机姿态q_cq_imu*q_i2c。这里q_imu是IMU在当前时刻的姿态四元数q_i2c是从IMU坐标系到相机坐标系的旋转外参两者相乘就得到了相机坐标系相对于世界坐标系的旋转。相机位置p_cp_imuR_imu*p_i2c。p_imu是IMU的位置R_imu是IMU姿态对应的旋转矩阵p_i2c是IMU到相机的平移向量外参。这个计算就是把IMU坐标系下的相机原点坐标转换到世界坐标系下。计算出来的这个6自由度的相机位姿3维位置3维姿态就被作为一个新的状态追加到整个滤波器的状态向量末尾。于是状态向量从[X_imu, X_cam1, ..., X_camN]变成了[X_imu, X_cam1, ..., X_camN, X_camNew]。3.2 扩展协方差矩阵建立新状态与旧状态的联系仅仅把相机位姿值加进去是不够的更重要的是更新协方差矩阵P以描述这个新状态的不确定性以及它和所有已有状态尤其是IMU状态之间的相关性。新的协方差矩阵维度变大了。假设原来有(216N)个状态新矩阵就是(216(N1))阶方阵。我们需要计算新状态自身的协方差P_new,new这个直接来自于计算相机位姿时传递的IMU状态不确定性。通过对相机位姿计算公式求关于IMU状态的雅可比矩阵J我们可以得到P_new,new J * P_imu,imu * J^T。这里P_imu,imu是IMU状态块的协方差。新状态与旧状态特别是IMU状态的互协方差P_new,old这是关键它建立了相机状态与IMU状态之间的关联。计算公式为P_new,old J * P_imu,old。其中P_imu,old是IMU状态与所有旧状态之间的协方差块。由于新相机状态只由当前IMU状态直接计算得出与更早的历史相机状态没有直接关系所以新状态与历史相机状态的互协方差为零。因此扩展后的协方差矩阵具有以下分块结构P_augmented [ P_old,old P_old,imu * J^T ] [ J * P_imu,old J * P_imu,imu * J^T ]状态扩增的意义就在这里凸显了通过P_new,old尤其是P_new,imu这些非零的互协方差块新加入的相机状态就和IMU状态“绑定”在了一起。后续当视觉观测来修正这个相机状态的误差时由于它们之间存在相关性IMU状态的误差也会被一并修正。这就巧妙地通过相机状态这个“中介”实现了视觉信息对IMU状态的间接更新。4. 测量更新用视觉观测来“纠偏”预测和扩增都是在“铺路”真正的“收获”来自于测量更新。这是利用视觉观测信息来修正所有状态估计的步骤也是卡尔曼滤波中“修正”或“更新”的环节。4.1 特征点三角化从多视角重建3D点MSCKF不会立即使用当前帧观测到的特征点。它会持续跟踪这些特征点直到它们在图像中“跟丢”比如移出视野或匹配失败。为什么这么做因为一个特征点被跟踪的帧数越多用于三角化其3D位置的信息就越充分结果就越可靠。当一个特征点被标记为“跟丢”后算法会收集所有观测到该特征点的历史相机位姿这些位姿都作为状态保存在滤波器里。然后通过最小二乘法来三角化该特征点的3D坐标。具体来说对于每个观测我们都有一条从相机光心出发指向归一化图像平面上特征点的射线。理想情况下所有观测同一特征点的射线应该交于一点即该特征点的3D位置。由于噪声存在它们不会精确相交因此我们求解一个最小二乘问题找到那个使得所有射线“距离”之和最小的3D点。这里有一个常用的技巧为了数值稳定性通常会选取一个观测作为参考帧比如第一次观测到该特征的相机将其他所有观测都转换到该参考帧坐标系下再进行三角化。4.2 构建观测模型重投影误差及其雅可比得到特征点的3D坐标估计后我们就可以为每一个观测构建重投影误差。对于第i个相机状态观测到特征点f_j重投影误差r_{ij}定义为该特征点在图像上的实际观测坐标z_{ij}与其根据相机位姿估计和3D点坐标投影得到的预测坐标z_hat_{ij}之间的差。r_{ij} z_{ij} - z_hat_{ij}这个误差r_{ij}是一个2维向量对于单目或4维向量对于双目左右目各2维。我们的目标是建立这个误差与滤波器状态向量主要是相关的相机状态和IMU状态的误差δx之间的线性关系r ≈ H * δx。这里的H就是观测雅可比矩阵需要通过链式法则对投影函数求导得到涉及对相机位姿、以及可能对IMU状态通过相机-IMU外参的偏导数。但这里有个问题误差r不仅依赖于状态δx还依赖于我们刚刚三角化出来的特征点3D坐标f_j而f_j本身也是有误差的。一个直接的观测模型会是r H_x * δx H_f * δf n其中δf是特征点坐标的误差。我们并不想将δf纳入状态向量进行估计。4.3 左零空间投影消除特征点依赖的“魔法”MSCKF一个精妙之处就在于它用左零空间投影巧妙地消除了对特征点坐标δf的依赖。其核心思想是既然特征点坐标f_j是由所有观测它的相机位姿通过最小二乘“共识”出来的那么由此产生的重投影误差r在理想情况下应该垂直于由H_f张成的空间即与特征点误差相关的方向。数学上我们对H_f矩阵进行QR分解或SVD找到一个矩阵V其列向量张成了H_f的左零空间满足V^T * H_f 0。用这个V^T左乘原始的观测方程V^T * r V^T * H_x * δx V^T * n看H_f * δf这一项被完美地消去了我们得到了一个只与状态误差δx相关的新的观测模型r_o H_o * δx n_o。其中r_o V^T * rH_o V^T * H_x。这个操作不仅消除了特征点状态还通常起到了降维和去相关的作用使得最终的观测噪声n_o更接近白噪声满足卡尔曼滤波的假设。4.4 执行EKF更新修正所有状态对于每一个“跟丢”的特征点我们都能得到一组形如r_o H_o * δx的观测方程。我们将所有特征点对应的r_o和H_o堆叠起来形成最终的全局残差向量和观测矩阵。然后就是标准的扩展卡尔曼滤波更新公式计算卡尔曼增益K P * H_o^T * (H_o * P * H_o^T R_o)^{-1}。这里P是预测的协方差矩阵R_o是投影后的观测噪声协方差。更新状态误差估计δx K * r_o。将这个误差估计值加到对应的名义状态上对于姿态需要用指数映射更新。更新协方差矩阵P (I - K * H_o) * P。为了数值稳定性通常会采用约瑟夫形式或其它能保证协方差矩阵对称正定的更新形式。经过这一步所有与当前观测特征点相关的历史相机状态以及通过协方差关联的当前IMU状态都得到了修正。这正是MSCKF融合视觉和IMU信息的核心体现。5. 状态管理滑动窗口与相机状态移除MSCKF的状态向量不能无限制增长否则计算量会越来越大。因此它采用了一个滑动窗口机制来管理历史相机状态。5.1 何时移除通常设定一个最大相机状态数量N例如10-20个。当状态向量中的相机状态数量超过N时就需要移除旧的相机状态。移除的时机一般在一次测量更新完成之后。移除哪个状态也有策略通常移除最老的状态以保持窗口覆盖一段固定的时间跨度。但在某些情况下例如机器人静止不动相机状态非常相似移除最新的状态可能更合适以避免窗口内状态过于共线。5.2 如何移除移除操作在数学上很简单从状态向量中删除对应相机状态的6个维度同时从协方差矩阵P中删除对应的行和列。这相当于对状态和协方差进行了一次降维。5.3 移除前的“价值榨取”这里有一个重要的细节当一个相机状态即将被移出窗口时如果某些特征点仍然被它和窗口内的其他状态共同观测到并且这些特征点还未“跟丢”那么直接丢弃这个状态会导致这部分观测信息永久丢失。开源实现S-MSCKF采用了一种优化策略在移除相机状态前会检查那些同时被待移除状态和至少一个窗口内其他状态观测到的特征点。对于这些特征点算法会立即利用它们对待移除状态以及另一个与之配对的观测状态的观测进行一次“局部”的测量更新。这样即使这个相机状态马上要被丢弃它携带的观测信息也已经被“榨取”出来用于修正了系统状态。这充分体现了MSCKF在信息利用上的高效性。6. 代码流程纵览从理论到实践理解了原理再看代码就清晰多了。我们以经典的Kumar Robotics开源的msckf_vio双目版本为例串讲一下整个流程。源码结构清晰是学习MSCKF不可多得的材料。6.1 前端ImageProcessor前端模块独立运行负责接收原始图像和IMU数据输出跟踪好的双目特征点。数据入口stereoCallback函数处理同步过来的左右目图像。核心流程IMU积分预测利用两帧图像之间累积的IMU角速度数据积分得到一个粗略的相机旋转估计。这个估计用于给前后帧的特征跟踪提供一个好的初始值大幅提升光流跟踪的成功率和速度。前后帧跟踪对上一帧左图的特征点使用LK光流法在当前帧左图上进行跟踪。这里就用到了上一步IMU积分预测的旋转来初始化特征点位置。左右目匹配对当前帧左图上跟踪到的特征点使用LK光流法在右图上寻找匹配点。这里利用已知的双目外参来预测右图对应点位置作为光流初始值。离群点剔除分别对左图前后帧跟踪结果、右图前后帧跟踪结果进行RANSAC例如2点法基于旋转模型剔除误匹配。提取新特征为了保证特征点均匀分布将左图划分成网格。在每个网格内使用FAST角点检测器提取新的特征点并通过左右目匹配得到其在右图上的对应点。最后发布当前帧所有成功跟踪和匹配的双目特征点对。6.2 后端MsckfVio后端是MSCKF算法的核心采用误差状态卡尔曼滤波框架。初始化前200帧要求设备静止用于初始化重力方向、加速度计和陀螺仪的零偏。数据入口featureCallback函数接收前端发来的特征点数据触发一次后端处理循环。核心流程IMU预测(batchImuProcessing)取出上一帧图像到当前帧图像之间缓存的所有IMU数据按顺序进行迭代积分更新IMU状态[p, v, q, ba, bg]和对应的协方差矩阵P。状态扩增(stateAugmentation)根据预测后的最新IMU状态和相机-IMU外参计算当前相机位姿将其作为新状态加入状态向量并扩展协方差矩阵。特征管理(addFeatureObservations)将当前帧观测到的特征点加入到地图管理器(map_server)中。每个特征点对象维护一个列表记录它被哪些相机状态用ID标识观测到及其像素坐标。移除丢失特征并更新(removeLostFeatures)检查哪些特征点在前端当前帧中跟丢了。对于跟丢的特征如果其被观测次数大于等于3保证能三角化则进行三角化求其3D位置然后构建观测模型进行左零空间投影最后执行EKF更新。更新完成后将该特征点从地图中移除。滑动窗口管理(pruneCamStateBuffer)检查相机状态数量是否超过上限例如20。如果超过则选择要移除的状态通常是最老的两个。在移除前会查找那些同时被这两个待移除状态观测到的特征点利用这些特征点对这两个状态进行最后一次观测更新“榨取”信息然后再将状态从向量和协方差矩阵中移除。整个流程就这样循环往复IMU高频预测图像到来时扩增状态、管理特征、并在特征跟丢或状态溢出时触发更新。这种设计使得MSCKF在计算效率和精度之间取得了出色的平衡成为VIO领域一个历久弥新的经典算法。在实际调试中你需要特别关注IMU噪声参数、相机-IMU外参的标定精度以及前端光流跟踪的质量这些往往是影响最终性能的关键。