滤波器方法（贝叶斯/EKF/UKF/ESKF/MSCKF）

1. 背景概述移动机器人、无人机或者无人船等是不能够像工业机器人利用关节处的力矩传感器和编码器的读数直接进行位姿的解算的抛开工业机械设计制造及其装配时带来的误差移动机器人、无人机或者无人船等内置的传感器往往会因为轮子打滑、imu噪声等问题引入难以忽略的误差由此机器人学中的状态确认就成了一种概率性质的状态估计与更新方法论了。简单的问题分类与描述如下建图---让机器“理解”周围是什么包围了它定位---让机器“理解”它在环境中的位置建图定位---结合成了SLAM问题建图、定位、SLAM的方法都是基于状态估计与更新那么结合上面的问题和描述需要解决的事情大致就这么几件明确状态量的组成变量定位地图模型获取各个位置的传感器读数、整合传感器数据生成地图、将传感器读数与地图模型建立关系、计算在地图模型中的位置、估计位置和模型的确定性、提升状态估计的正确性。针对以上问题各理论方法在过去几十年的时间内被提出和应用在机器人学中的基于概率贝叶斯理论的位姿估计问题上从80年代甚至更早高斯模型、线性近似模型和位置跟踪的方法论“组成”了最初的Kalman Filters框架在95年代离散的方法论开始在拓扑型表征和栅格模型表征上进行全局定位、重定位和不确定处理 (POMDPs) 的研究再到了99年无参滤波器Particle Filters开始在基于采样方法的定位、重定位方向上大展身手而千禧年后各个假说和前提条件下的滤波器研究成了研究的热门如EKF的各个变体ESKF、MSCKF和IKF都是针对不同的应用情况和追求被提出并得到了广泛的应用和优化。本文将从贝叶斯公式及其相关的概率基础知识入手遍历KF、EKF和UKF等基本的带参数的滤波器原理与公式再谈谈ESKF、IKF和MSCKF这类EKF的变体滤波器最后进入无参滤波器PF及其优化的策略。2. 概率基础2.1. 贝叶斯公式联合概率密度分解成条件概率密度和边缘概率密度的乘积重新整理可以得到贝叶斯公式如果移植先验概率密度函数P(x)以及传感器模型P(y|x)那么就可以根据贝叶斯公式推断出后验概率密度即以上过程即贝叶斯推断也称为贝叶斯估计。2.2. 高斯概率密度函数一维高斯分布多维高斯分布例如二维高斯分布为其中为x, y的相关系数当x, y线性无关时上式即等于0。2.3. 联合高斯概率密度函数若有高斯分布则它们的联合概率密度函数为根据舒尔补公式称为矩阵D关于原矩阵的舒尔补。利用舒尔补公式联合分布的方差矩阵可以写成联合高斯分布的指数部分的二次项包括以下内容因此有2.4. 高斯随机变量的线性分布假设 yGxnG是一个常量矩阵n为零均值白噪声在实际中指的是观测噪声。则有3. 贝叶斯滤波所谓的机器人状态估计其实就是估计机器人在当前时刻的状态。这是一个向量包含了我们想要估计的量比如机器人的位置、姿态和速度。估计所需的信息来自两个方面。内部传感器提供的信息记作通常是运动信息比如里程计或IMU提供的位移、速度等信息。外部传感器提供的信息记作通常是观测信息比如激光测距仪提供的距离信息、相机提供的像素信息等。于是状态估计问题可以描述为在给定从1时刻到t时刻的所有传感器信息后求条件概率该条件概率称为贝叶斯后验概率记作bel()。所谓“后验”是结合了观测之后的概率。既然如此当然就存在一个“先验”概率即结合观测之前的概率记作为了计算简便不可能每次都用所有时刻的传感器信息来估计后验概率。最明智的做法是迭代估计用上一时刻的后估计当前时刻的后验其中分母与无关可以当做常量用归一化因子代替。注意这里用的是所谓的条件贝叶斯公式始终作为条件变量不受贝叶斯公式影响。这个概率所依赖的条件太多为了能够方便地求解它我们不妨做一假设马尔可夫假设系统当前时刻的状态只与上一个时刻有关与之前任意时刻的状态都没有关系。当然马尔可夫假设的成立是有条件的它要求状态变量具有完整性换句话说系统的全部信息都包含在了中不存在其它随机变量对系统状态产生影响。考虑马尔可夫假设后后验概率可以进一步简化为也就是说当前时刻的观测只依赖于当前状态与之前的状态和传感器数据无关。仔细观察s上式可以发现右侧的第二个概率恰好是即现在唯一的问题是后验概率如何计算将其按照全概率公式展开这里利用了连续情况的全概率公式考虑所有可能的所能生成的。再次考虑马尔可夫假设将上式简化其中第一行去掉了对的依赖因为它们所产生的影响已经完全包含在中。第二行去掉了对的依赖因为不可能对上一时刻的状态产生影再者可以发现积分号中的第二个概率恰好是上一时刻的后验概率即至此我们实际上把贝叶斯滤波分成了两个步骤。第一步称为预测从预测。第二步称为更新从更新到其后验概率。在应用贝叶斯滤波时需要事先根据具体的传感器类型确定两个条件概率然后利用传感器数据和不断进行预测和更新得到当前时刻机器人的状态。4. 卡尔曼滤波在实际应用中如何根据具体的传感器类型确刚刚上述的那两个条件概率往往会使得计算很复杂甚至无法顺利进行特别是还包括一个积分。卡尔曼滤波则是为我们提供了一套针对在线性系统中的解决方案保证求解的时效性。在卡尔曼滤波中系统的状态改变和观测服从下面的线性方程其中状态量中引入高斯随机变量表示由状态转移引进的不确定性观测量中也是同理引入分布是均值为0方差为 Q 的高斯随机变量。那么刚刚上述的那两个条件概率分别表示为对于条件概率来说已知的条件已经不再是随机变量因此第一个式子中的和第二个式子中的应按照常量对待。只在状态量和观测量中引入的噪声是服从高斯分布的随机变量。因此两个式子得到的结果仍然是高斯分布4.1. KF第一部分预测接下来仍然按照贝叶斯滤波的两个步骤直接求积分很困难但可以尝试利用概率密度函数来求。我们就可以把拆分成两部分上右边的第一项满足高斯分布指数部分的形式第二部分是后面的常数项。利用以下操作为了计算二次型的系数计算的一阶、二阶导数假设令有因此可以将二次型函数定义为最后可以得到预测部分的分布为4.2. KF第二部分更新5. 扩展卡尔曼滤波算法 (EKF)卡尔曼滤波的前提是系统的状态改变和观测是线性的但是实际应用中系统是非线性的。EKF通过一个一阶泰勒展开的线性系统来近似非线性。假设系统的状态和观测方程如下6. 无迹卡尔曼滤波算法 (UKF)该算法的核心思想是采用UT变换利用一组Sigma采样点来描述随机变量的高斯分布然后通过非线性函数的传递再利用加权统计线性回归技术来近似非x线性函数的后验均值和方差。相比于EKFUKF的估计精度能够达到泰勒级数展开的二阶精度。其本质是近似非线性函数的均值和方差远比近似非线性函数本身更容易。6.1. UT变换yf(x)是一个非线性变换x为 n 维随机变量UT变换根据一定的采样策略获得一组Sigma采样点并设定均值权值W和方差权值来近似非线性函数的后验均值和方差。利用选取的Sigma采样点集进行非线性函数传递可得其中为Sigma采样经过非线性函数传递后对应的点。根据加权统计线性回归技术可以近似得到y的统计特性6.2. Sigma点采样策略下面介绍两种经常使用的采样策略比例采样和比例修正对称采样。根据Sigma点采样策略不同相应的Sigma点以及均值权值和方差权值也不尽相同因此UT变换的估计精度也会有差异。但总体来说其估计精度能够达到泰勒级数展开的二阶精度。使用参数beta对高斯表示的附加的分布信息进行编码。如果分布是精确的高斯分布则beta2是最佳的选择。6.3. UKF采样原则为保证随机变量x经过采样之后得到的Sigma采样点仍具有原变量的必要特性所以采样点的选取应满足式中{}内的符号分别为Sigma采样点均值权值和方差权值组成的集合L为采样点个数P为随机变量x的密度函数g[~]确定x的相关信息。如果密度函数P(x)只有一、二阶矩时可以写成6.4. UKF的特点UKF相比于EKF的精度更高一些其精度相当于二阶泰勒展开但速度会略慢一点。UKF另一个巨大优势是不需要计算雅克比矩阵而有些时候雅克比矩阵也确实的我们无法获得的。另外UKF与PF粒子滤波也有相似之处只是无迹变换中选择的粒子是明确的而粒子滤波中的粒子是随机的。随机的好处是可以用于任意分布但也具有其局限性。因此对于分布近似为高斯分布的采用UKF将更有效。7. Error-state卡尔曼滤波 (ESKF)ESKF是一种典型的间接法滤波其预测和更新过程都是针对系统的误差状态再用修正后的误差状态修正系统状态。其核心在于将系统状态真值分解为系统名义状态值和误差状态值的结合。状态分解为两个部分组成真实状态运动学名义状态运动学于是可以直接得到误差状态的运动学ESKF的优缺点KF、EKF、UKF的系统模型都是直接描述系统状态属于直接法滤波ESKF的模型系统描述的是系统误差而不是直接描述系统状态需要通过转换得到系统状态属于间接法滤波。ESKF的Error-State的值一般趋近于0可以避免一些一阶部分可能出现的奇点应用于惯导系统时的万向锁问题提供所有时间内的线性有效性保证。而且Error-State很小可以保证在泰勒展开中的二次部分忽略不计使得雅可比的计算简单快速计算复杂度UKFEKFESKFKF。由于Error-State的动力系统通常比较缓慢因此所有较大的分量被集中到计算名义状态以为着可以使卡尔曼滤波的更新过程频率低于预测过程。8. 迭代卡尔曼滤波 (IKF\IEKF)IKF全称为Iterated Kalman filter是基于 Extended Kalman filter (EKF) 的滤波框架在其中应用高斯牛顿迭代法而改良的方法该方法的策略是牺牲掉少量计算时间以优化EKF线性近似时产生的近似误差。本质上是在EKF框架中构建非线性优化模型以求解状态后验的最大似然以提升EKF的精度。IKF的预测与EKF几乎相同IKF算法的主要贡献在于其更新阶段的优化同时算法将预测阶段的状态估计值作为了当前阶段的观测值。以典型的IKF实现案例Fast-LIO系列来说整个系统中有两个关键的时间点利用每一次IMU的数据进行积分预测利用每一次激光数据进行观测迭代更新。引申到通用的模式下其实也就是需要在更新开始前确认清楚当前的预测状态量、对应的预测状态量协方差、当前的观测量及其观测量协方差以找到最好的位姿状态估计及其协方差估计。IKF的算法流程如下预测积分残差计算迭代更新针对第一个步骤预测积分就是先根据IMU的读数进行状态量预测和协方差预测上式中的f即代表状态转移方程Q为高斯白噪声w的协方差Fw和Fx的定义式如下在上述中提到的两个关键时间点该预测积分的过程会在每一次IMU读数到来的时候进行积分传播直到下一帧激光数据来临时才结束进行更新操作。对于协方差的迭代更新使用Matrix Inversion Lemma上式中P即为状态量的协方差矩阵H表达为的简写。进一步的分析可以看到状态量的迭代更新与观测量z及其协方差R、预测状态及其协方差、观测方程g及其一阶导数H整个迭代更新过程的converge条件就是达到局部最小值最终后验状态估计及其协方差为由此可得整个状态向量的迭代过程若仅仅进行一次迭代则IKF系统就成了EKF。9. 多状态约束下的卡尔曼滤波 (MSCKF)MSCKF是用于估算VIO的滤波算法全称是Multi-State Constraint Kalman Filter最早是在论文《A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation》中出现。MSCKF是沿用EKF的框架优化具体的实现以解决EKF-SLAM的维数爆炸问题为目标。在传统融合IMU数据和VO的EKF-SLAM框架中会将VO中的各个特征点信息加入到待解算的状态向量中其中还包括了IMU的数据信息状态也就是随着系统的运行VO的关键帧越来越多其VO中的特征点也会随之变得非常多导致需要进行解算的状态向量维数会变得非常大。MSCKF则通过将不同时刻的VO相机关键位姿加入到状态向量而不再将特征点加入到状态向量如此操作的原因在于特征点可以被多个相机看到从而能够与多个相机状态Multi-State之间构建约束Constraint进而利用约束的信息构建观测模型对EKF进行更新。由于相机位姿的个数会远小于特征点的个数MSCKF状态向量的维度相较EKF-SLAM大大降低并通过滑动窗口Sliding Window历史的VO状态会不断移除从而对MSCKF后端的计算量进行限定由此MSCKF的计算消耗较之传统方法更低运行更加鲁棒。所以根据上述MSCKF的原理其中预测过程和EKF一致利用IMU数据信息进行积分关键的在于使用VO的信息来构建约束利用3D特征点到VO相机的重投影坐标和实际观测到坐标构建误差方程以更新IMU预测的状态向量。MSCKF算法步骤如下预测IMU积分先利用IMU加速度和角速度对状态向量中的IMU状态进行预测一般会处理多帧IMU观测数据。相机状态扩增每来一张图片后计算当前相机状态并加入到状态向量中同时扩充状态协方差.特征点三角化然后根据历史相机状态三角化估计3D特征点特征点跟丢时才进行操作特征更新再利用特征点对多个历史相机状态的约束来更新状态向量。注意这里不只修正历史相机状态因为历史相机状态和IMU状态直接也存在关系相机与IMU的外参所以也会同时修正IMU状态。滑动窗口历史相机状态移除如果相机状态个数超过N则剔除最老或最近的相机状态以及对应的协方差。重复1-5上述步骤中MSCKF中观测更新的时机是特征点跟丢此时利用所有的历史观测信息进行三角化以尽可能地确保三角化的精度。10. 粒子滤波10.1. 蒙特卡洛定位算法 (MCL)蒙特卡洛定位算法时将合适的概率运动和感知模型带入到particle_filters中MCL定位在概率滤波下同样用条件概率表示置信度AMCL定位在概率滤波下同样用条件概率表示置信度在粒子滤波中使用一系列从后验中得到的随机状态采样后来表示后验。后验分布的样本表示为粒子集式中Xt机器人t时刻的位姿状态的一个具体实例每个粒子都是根据真实状态在时刻的一种可能假设N粒子集的粒子数量。粒子滤波就是用一系列粒子来近似状态X近似置信度伪代码如下程序第3行基于粒子和控制量产生t时刻机器人可能位姿的粒子集通过从采样获取。程序第4行计算重要性因子通过观测量在粒子采样状态下的概率获取程序第8~11行是粒子滤波的重采样过程根据计算重要性因子从粒子集中采样获取N个粒子获得新的粒子集使用该粒子集来表示机器人近似表示机器人状态X置信bel。10.2. 随机粒子蒙特卡洛定位失效恢复在蒙特卡洛定位算法中在重采样过程中可能意外的丢失所有正确位姿附近的粒子机器人绑架或者全局定位失效问题可以通过探索算法解决增加随机粒子到粒子集合。将增加随机粒子跟平均测量概率结合在多个时间步上对这个测量概率求平均来平滑。设测量似然的长期平均短期平均。在重采样过程中随机采样以以下概率增加增加随机采样的概率考虑长期似然和短期似然平均的散度短期似然优于或等于长期似然时不增加随机采样。根据短期平均的指数滤波器的衰减率更加接近当前的实际似然值。如果存在说明当前似然值高于之前则当前的定位情况比之前好则无需增加随机粒子。并在RBpf算法的基础上改进提议分布和选择性重采样改善粒子退化的问题。10.3. KLD采样粒子滤波通过粒子来实现机器人的位姿估计粒子数量N直接影响到位姿估计的精度和定位算法的效率。粒子集的数量N必须足够大才能避免重采样过程中的粒子消耗引起的发散问题减少“粒子退化”带来的影响保证机器人的定位精度。但是粒子集的数量过大则算法的数据处理计算量增加造成定位的效率低。在定位早期阶段需要使用数量足够大的粒子集来精确表示机器人的状态置信。但是一旦机器人的位姿确定只需要很小一部分粒子就可以跟踪机器人的位置。AMCL定位通过KLD (Kullback-Leibler Divergence, KLD) 采样根据采样的近似质量来调整所需的粒子数在保证定位精度的前提下减少用于定位的粒子的数量。KLD的伪代码如下程序第18行给出了采样所需的粒子数。粒子越分散k表示直方图中的非空bin值越大M越大采样的粒子数也就越多。在机器人导航中机器人的初始位姿未知使用初始置信来反映机器人的初始位姿信息通过在地图上所有自由位姿空间的均匀分布来初始化式中X地图中所有位姿空间的体积。通过初始置信在机器人的自由位姿空间上采样获得机器人初始状态下的粒子分布。11. 优化和滤波的区别卡尔曼滤波器从k-1时刻后验推k时刻先验从k时刻先验推k时刻后验马尔可夫假设扩展卡尔曼滤波器对卡尔曼滤波器进行修正针对不是线性的情况采用一阶泰勒展开近似线性马尔可夫假设BA优化把一路上的所有坐标点像素坐标对应的空间点等与位姿整体放在一起作为自变量进行非线性优化PoseGraph优化先通过一路递推方式算出的各点位姿通过数学方式计算得到一个位姿的变换A再通过单独拿出两张图像来算出一个位姿变换B争取让BA增量因子图优化保留中间结果每加入一个点对不需要重新计算的就直接用之前的中间结果需要重新计算的再去计算从而避免冗余计算。iSAM是增量的处理后端优化。由于机器人是运动的不同的边和节点会被不断加入图中。每加入一个点普通图优化是对整个图进行优化所以比较麻烦和耗时。iSAM的话相当于是保留中间结果每加入一个点对不需要重新计算的就直接用之前的中间结果对需要重新计算的再去计算。以这种方式加速计算避免冗余计算。从全文的整理来看特别是在IKF章节我们可以发现滤波算法其实就是将Sliding Window的大小设置为1的优化算法不论是优化算法还是滤波算法都是期望求解出问题概率模型的最大似然估计MLE本质上滤波就是基于马尔可夫假设将优化问题的建模进行了“特征化”的处理。再进一步分析正因为滤波器进行了范围上维度的“简化”、模型的近似处理所以其计算消耗较于优化算法更低但由此引发的代价就是精度上的损失。另一个需要考虑的方面是滤波器是由先验信息运动模型观测信息三个方面点顺序执行以实现位姿状及其协方差的估计和更新也正因为滤波器的框架如此若是先验上一时刻状态出现了问题比如位姿跟丢、计算错误等那么在该时刻之后的状态都会出现问题以致纠正不回来了而基于优化方法的位姿求解则会考虑更多时刻关键帧下的状态信息和观测信息即使有某一时刻的状态量和协方差是outlier系统也有一定的能力维稳。如果在处理器算力充足且精度为第一需求的前提下那么在位姿估算问题处理上是首推优化算法但若是效率是第一前提条件那么就需要根据实际的应用情况和机器人问题模型选择合适的滤波器了。