3. 1 扩展卡尔曼滤波

卡尔曼滤波 (Kalman filtering, KF) 是一种利用线性系统状态方程, 通过系统 输人输出观测数据, 对系统状态进行Zui优估计的算法, 是一种高效的递归滤波 器( 自回归滤波器)。在观测数据的过程中, 会受到系统中噪声的干扰, 因 此, Zui优估计也是一种滤波过程。

扩展卡尔曼滤波 (extended Kalman filter, EKF) 首先由 Bucy, Sunahara 等人 提出, 其基本思想是将非线性系统线性化, 然后进行卡尔曼滤波, 从而突破了 卡尔曼滤波限制, 将卡尔曼滤波理论进一步应用到非线性系统中。因此, 当测量方程或状态方程为非线性时, 一般会采用扩展卡尔曼滤波。为了将非线 性问题线性化, 扩展卡尔曼滤波会对非线性函数进行泰勒展开, 并舍弃展开式的高阶项。但是当状态估计系统是强非线性时, 忽略泰勒展开式中的高阶项会带来很大误差, 导致滤波器发散。因此, 一般来说, 扩展卡尔曼滤波并不是Zui 优的, 而是一种次优滤波。在满足线性系统、高斯白噪声以及所有随机变量服 从高斯分布的条件下, 扩展卡尔曼滤波的性能依赖于局部非线性度, 因此它是 Zui小方差准则下的次优滤波器。

扩展卡尔曼滤波大致由两个部分组成: 预测阶段和更新阶段, 如图 5-5 所 示, 预测阶段主要计算状态的预测值和状态误差协方差预测值。更新阶段主要 工作是计算所构造的扩展卡尔曼滤波器的增益, 更新状态误差协方差矩阵, 并更新所预报的状态值。

扩展卡尔曼滤波流程大致分为五步，如图 5-6 所示。

具体步骤及公式如下 :

扩展卡尔曼滤波是通过对观测量  的更新, 获取对状态向量  的估计, 二者满足如下关系：

  （5-18）

其中,  由前一时刻  的值估计得到; 方程  和  是非线性的;  是 可以获得的观测向量;  和  分别代表过程噪声和测量噪声。假设时间间隔为  , 扩展卡尔曼滤波针对此模型进行如下计算：

1) 计算状态预测值

  （5-19）

其中,  为状态转换矩阵;  为  时刻状态的值;   为在  时刻对  时刻状态的预测值。

2) 状态误差协方差矩阵预测

  （5-20）

其中,  为  时刻状态协方差的值;  为在   时刻对  时刻状态协方差的预测值;  是过程噪声。

3)计算卡尔曼滤波增益

  （5-21）

其中,  为在  时刻的卡尔曼滤波增益, 其控制着收敛速度。

4) 状态预测值更新

  （5-22）

其中,   来源于式(5-18) ;  为雅可比矩阵, 对应于  时刻状态预测值的值。

5) 状态误差协方差矩阵更新

  （5-23）

其中,  为单位阵。

3.2 估计模型

基于扩展卡尔曼滤波的参数误差估计模型以第 4 章误差测量模型的测量值 为输人, 对第 3 章推导的机器人末端误差模型进行估计。假设机器人共有   个关节并依据 DH 模型建模, 误差测量模型中传感器到机器人末端的位姿转换 参数为 4 个, 则算法要考虑的参数总数为  。在基于扩展卡尔曼滤波的参数误差估计模型中, 算法的预测状态  基于这  个参数构建。状态预测值和状态误差协方差矩阵预测值为：

  （5-24）

其中,  为  时刻的系统噪声的协方差矩阵。

由第 3 章的误差模型, 我们可以得到  时刻的雅可比矩阵  、测量误差冗余  以及冗余协方差矩阵  满足式(5-25)关系:

  （5-25）

其中,  为姿态测量值;  为  时刻的测量噪声的协方差矩阵。 

因此, 扩展卡尔曼滤波的滤波增益为:

  （5-26）

由式 (5-26)计算的卡尔曼滤波增益, 可更新状态预测值和状态误差协方差矩阵:

  （5-27）

利用基于扩展卡尔曼滤波的参数误差估计模型求解机器人运动学参数误差 模型就是对上述步骤的不断重复, 直到满足算法终止条件为止。算法输出的结果即为机器人实际的运动学参数与理论运动学参数的误差。

4 本章小结

机器人运动学参数辨识过程是机器人运动学标定的一个重点和难点, 其以 误差测量模型测得的机器人末端实际位姿和理论计算位姿的误差值为输人来求 解机器人的误差模型, 实时估计机器人的运动学参数误差, 并通过补偿算法对机器人的理论运动学参数进行补偿, 改善机器人的运动的准确性。

一直以来, 许多学者都对机器人运动学参数误差估计模型进行了深人研究, 提出了各自的基于不同估计算法的机器人运动学参数辨识方法, 如基于Zui小二 乘法的参数辨识法、基于 Levenberg-Marquardt 算法的参数辨识法以及基于蒙特卡洛法和进化模拟退火算法的参数辨识法等等。

本章以作者实验室的研究为基础, 总结了三种目前较为简单和常见的参数辨 识方法一一Zui小二乘法、遗传算法以及扩展卡尔曼滤波法。Zui小二乘法原理简单易懂, 且不受扰动信息影响, 但实际应用过程需要机器人沿固定的轨迹运动, 灵活性低且计算量大。遗传算法以生物进化为模型, 具备很好的收敛性, 在计算精度要求时计算时间少、鲁棒性高, 但也存在不少缺陷, 比如在适应度函数选择不当的情况下很容易造成局部收敛，无法搜索到全局Zui优解。扩展卡尔曼滤波是解决非线性参数估计系统的经典方法, 能克服潜在的不确定性问题, 但该方法易受参数误差分布的不良影响，导致算法无法收玫，估计精度得不到保障。

在目前提出的众多参数误差估计模型中, 并没有哪种模型有juedui优势或劣 势, 因此在实际应用中, 要根据实际的机器人的运动学模型和参数误差模型合理选择误差估计模型。