机器人学 —— 机器人感知(Kalman Filter)

时间:2022-06-27 14:18:01

  对于机器人感知任务而言,经常需要预判物体的运动,保证机器人在物体与自身接触之前进行规避。比如无人机与障碍物的碰撞,足球机器人判断足球的位置。预判的前提是对当前状态进行准确的估计,比如足球的速度,障碍物靠近的速度。一般认为,测量是存在误差的 —— 眼见未必为实。

机器人学 —— 机器人感知(Kalman Filter)

1、物体的运动学模型

  物体的运动学模型使用状态向量来表达。以2维空间的质点运动为例,物体的运动学模型可以表达为 x = [ px py vx vy ]' 。其中 px py 表示物体的位置,vx vy 表示物体的速度。如果能够准确估计物体当前状态x 就可以对其一段时间后的状态进行判断。然而当前状态的测量噪声会对x 的估计产生干扰。Kalman Filter 的作用就是去除这些干扰。

  卡尔曼滤波器的原理是使用马尔科夫链来对随机变量进行推测。马尔科夫链是一种特殊的贝叶斯网络,所以卡尔曼滤波器实际上是一种贝叶斯滤波器。其Graph Model 如下所示:

  机器人学 —— 机器人感知(Kalman Filter)机器人学 —— 机器人感知(Kalman Filter)

  这里需要指出的是 Xt-1 实际是未知的,但是我们以其估计量来作为实际值的最优估计。对于给定PGM,我们使用高斯分布来对随机变量进行建模 —— 高斯分布的几大优点见上篇博客。在不考虑子节点(还没有发生),父节点又被认为是给定的情况下,当前节点的CPD可以认为是父节点与观测节点的Factor product. 如果我们使用高斯模型进行建模,则factor 如下:
  机器人学 —— 机器人感知(Kalman Filter)机器人学 —— 机器人感知(Kalman Filter)机器人学 —— 机器人感知(Kalman Filter)

  其中,A代表转移矩阵,给定 t 时刻的状态,t+1时刻则由转移矩阵确定。vm 则表示转移噪声 —— t+1 时刻与 t 时刻中,速度不是常量。 vo 表示观测噪声 —— 我们对测量的肯定程度。使用高斯模型建模后,我们可以得到p(xt+1|xt) 与p(zt|xt) 的分布。第一个定义了xt+1|x的 factor ,第二个定义了观测模型的factor.  于是,我们可以用贝叶斯公式求取 p(xt|xt-1, zt-1 ) .

机器人学 —— 机器人感知(Kalman Filter)

  机器人学 —— 机器人感知(Kalman Filter)

  这里需要指出的是,一旦给定了zt 的父节点 xt , 那么zt 和 xt-1 是 d- Seperate 的。所以 P(zt|xt,xt-1) = P(zt|xt).

2、最大后验估计

  机器人学 —— 机器人感知(Kalman Filter)机器人学 —— 机器人感知(Kalman Filter)机器人学 —— 机器人感知(Kalman Filter)

机器人学 —— 机器人感知(Kalman Filter)机器人学 —— 机器人感知(Kalman Filter) 机器人学 —— 机器人感知(Kalman Filter)

  最大后验估计对象如上,按照上述公式即可实现卡尔曼滤波器。实现后的误差及预测结果如图:

  机器人学 —— 机器人感知(Kalman Filter)

  卡尔曼滤波器的设计技巧在于,如果物体不是匀加速运动(如足球的滚动),那么位置方差可以给的较小(认为我们的运动跟踪算法帧率足够高,且相机检测够准),但是!!!速度方差一定要给的够大,以保证能够模拟变加速状态!!!