卡尔曼滤波算法实例剖析--机器人足球赛场中的定位算法

卡尔曼滤波(Kalman filtering)最早在阿波罗飞船的导航电脑中使用,它对阿波罗计划的轨道预测很有用
卡尔曼滤波是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用。
 
卡尔曼滤波本身是贝叶斯滤波体系的,建立在马尔科夫概率模型基础上。大多数关于卡尔曼滤波的文章都是大量的数学概率公式,看的晕头转向。本文通过机器人足球场上的定位来通俗易懂的讲述该滤波算法原理。
 
1. 机器人足球场景介绍
如图1所示,踢球的机器人为差分轮式机器人,它可以直线或者弧线行走,也就是可以同时以线速度v和角速度ω行动。v和ω都是从传感器测量得到的,这里认为是已知的。
 
如图2,足球场为矩形,内部填充灰色的黑色圆圈代表机器人,它在场内的位置可以表示为(x,y),左下角为原点; 圆圈的黑色直线代表机器人的角度为 θ,以X轴正向为0度,逆时针旋转。
为了便于定位,四个角落和中间两边分别放置一个ROS的AR标签,这些标签完全不同,编号为 1-6, 位置在系统内已知固定。无论在场内任何地方,机器人使用Kinnect或XTionPRO 均可以探测到至少一个AR标签,ROS可以检测到距离AR标签的距离 r 和角度 φ. 它们也是实时获取的已知量。
 
很明显,角度θ+φ 是可以从机器人位置(x,y)和AR标签位置(mx,my)同反正切 atan2(mx - ,my - y) 轻松计算出来。
 
 
 图1  机器人足球场地实景图
 
 
 
图2  机器人足球场逻辑图
 
2. 卡尔曼滤波模型
在任何一个时间点t-1, 机器人的位置为(x, y,θ) , 它以线速度和角速度(vt,ωt)以弧线轨迹移动到下一个时间点t时,新的位置(x', y', θ')通过简单的三角函数数学公式是可以算出来的:
 
 
公式1  坐标转移矩阵
这个过程叫做状态转移,状态x=(x,y) 代表机器人的坐标(状态x 和坐标里面的x只是巧合同一个字母,因为卡尔曼滤波里面的状态就是用x表示的,不要混淆)。
按照公式1不停地累计计算从t 到 t-1,再到t+2,......,t+n,貌似可以得到机器人的实际位置。但是实际效果是什么样子呢?来看一下图3,机器人从左下角开始移动6个时间点,依次经过AR标签2-6, 它的轨迹为实线,白色圆圈代表每个时间点的位置。 而根据公式1的方法计算得到的轨迹为虚线所示,灰色圆圈代表计算机算出的位置。可以清楚的看到,计算误差很大,从一开始的位置计算就有有误差,这种误差不断积累,最终‘失之毫厘,谬以千里’。
 
图3  移动机器人的计算轨迹(虚线)实际轨迹(实线)有很大误差
 
这是为毛呢?原因就是目前所有人类制造的各种仪器都是有误差的。就像GPS定位一样,它也不是能够精确的给出一个坐标,它有个5米到10米的误差。同样机器人传感器得到的线速度v和角速度ω都是不准确的,有噪音。如图4所示,灰色圆圈为公式1计算出来的坐标,但是噪音范围为椭圆形所示,真实位置在里面的任何一点都是有可能的,只是每个点的概率不一样,中间的概率是最高的。
 
图4 测量误差--椭圆区域
2.1 卡尔曼滤波能做什么

看到这里会问了,不是还有AR标签帮助测量定位吗?根据标签位置,距离和角度也可以算出坐标啊! 没错,但是还是那个老问题,这个测量结果也是有误差的,误差大小完全取决于传感器。
我们总结一下,现在有两个已知量: 
  1. 移动的速度和起点坐标 根据公式1算出来的坐标 x = (x,y)  有很大误差噪音
  2. AR标签测量的值  z=(r, φ)  有很大误差噪音
卡尔曼滤波能做什么?它可以根据上面的两个有噪音的测量值融合到一起获得最优的坐标,也就是最优状态。这就是一个降噪的过程,滤波出来的坐标也是有噪音的,但是很小。如图5所示,滤波后的轨迹和真实轨迹非常接近了。灰色椭圆是测量位置的噪音范围,黑色椭圆是卡尔曼滤波后的噪音范围,确实降噪了不少。
 
这个最优的坐标和噪音到了下一个时刻t+1,就作为滤波的输入结合t+1的AR标签观测量和噪音再次推导一遍。机器人不断行走,该算法不断的循环执行就推导出了最优的移动轨迹。每次间隔Δt越小,滤波的精度就越高。所以计算机性能越好,就可以越精确的估计移动轨迹。
图5  卡尔曼滤波效果图 移动机器人的计算轨迹(虚线)实际轨迹(实线)非常接近

********接下来开始讲概率矩阵数学了,东方赤龙提醒您:不想看可以不看了*******
 
2.2 卡尔曼滤波模型参数定义
卡尔曼滤波是为了获取每个时刻t最优的状态,它有许多物理意义的参数:
  • 状态  x = 坐标 (x,y,θ)
  • 状态噪音Σ = 状态 x 的精确度
  • 最优状态 μ  每个时刻卡尔曼滤波估算的最优坐标
  • 状态转移控制量  u = 线速度和角速度(v, ω)
  • 状态转移噪音 R  因为移动所增加的噪音
  • 观测量 z = AR标签测量值 (r, φ)
  • 观测量噪音 Q = AR标签传感器的噪音
还有几个状态转移方程: 
  • xt =At xt-1  +But     根据t-1的坐标和移动速度估计出在t时刻的坐标,也就是上面的公式一。这里At =1,Bt 就是时间间隔 Δt
  • z =C x                        状态量x 到观测量z 的映射关系。这里就是根据下面的三角函数计算出来的, C代表了状态映射矩阵。

 

2.3 卡尔曼滤波过程
有了数据,接下来就是具体操作了。科尔曼滤波或者贝叶斯滤波体系的过程都包含两个步骤:
  1. t-1 到 t时刻的状态预测,得到前验概率
  2. 根据观察量对预测状态进行修正,得到后验概率,也就是最优值
在卡尔曼滤波里面,每个状态都是一个正态分布概率,也就是高斯分布概率:
p(x) =  N( μ, σ )
这里,μ 就是高斯分布的均值,也就是概率最高的值,称为最优值。σ 就是高斯分布的方差,也就是噪音。所以卡尔曼滤波最终算出来的不是一个精确的状态值,而是一个概率最高的最优状态值。这个状态值在我们的例子里面就是坐标。
  
2.3.1 状态预测
根据t-1时刻的最优状态值xt-1 ,带入文章开头的状态转移方程公式1,根据控制量u得到t时刻的新状态μ。就是根据机械运动的线速度和角速度推算出新的坐标。为了便于记忆,我们一般化它为公式一:
 
公式一  状态转移变换方程
上面已经说了,这个预测是有误差噪音的,在t-1时刻的噪音 Σt-1 经过状态转移后被扩大了,也就是说经过移动后,坐标的不确定性增大了。新的噪音为了便于记忆,我们一般化它为公式二:
公式二 状态噪音转移变换方程

2.3.2 卡尔曼增益 K
公式三  卡尔曼增益K
卡尔曼增益 K 相当于一个系数变换矩阵,它包含了两个信息:
  1. 观测量 z 到状态量 x 的的变换矩阵,就是说 x = K z .  相当于2.1节提到的状态转移方程 z = C x 中 C 的逆矩阵。本文例子就相当于从地标AR标签的距离和角度反推出机器人的位置。
  2. 对观测量的可信度百分比。观测量可信度越高,也就是测量噪音Q相对于与状态预测噪音越小,该百分比就越高。反之则越低。

 
2.3.3  后验值修正

既然预测不准确,那就用测量后验值去修正它。先修正状态值x,就是公式四:

公式四  状态修正方程
这个方程直观上很容易理解,就是假设预测状态正确,那它观测到AR标签时的距离和角度对应的值就是,可视为预测观测值。而则代表预测观测值与实际观测值的差距。乘上卡尔曼增益K,就得到了修正量。修正量加上预测值就是修正值或最优值。假设为0,就是说预测AR标签位置和观测到的AR标签位置完全一样,那公式四返回就是预测状态坐标值,和直观常识吻合的非常完美。假设的值比较大,就是说预测AR标签位置和观测到的AR标签位置差别很大,那根据卡尔曼增益K中的百分比信息的小或大,公式四就返回值靠近预测值或靠近测量值多一些,和实际物理意义吻合的还是非常完美。
 
有了修正后的状态坐标值,就需要得到该状态坐标的修正后的噪音大小 Σt  就是公式五:
公式五  状态误差噪音修正方程
修正后的噪音Σt   其值要比预测噪音和感测噪音Q都要小。这样直观上也容易理解,滤波后的状态噪音最小,所以最优。
 
2.4 卡尔曼滤波总结
以上公式一二三四五,就是卡尔曼滤波的五项公式。前面说过在卡尔曼滤波里面,每个状态都是一个正态分布概率,也就是高斯分布概率:
p(x) =  N( μ, σ )
整个滤波过程从高斯曲线上看就如下图6所示:
图6  卡尔曼滤波的高斯分布动态过程
 
a)  初始时刻t-1的状态概率高斯分布,分布图的宽度越宽,误差越大,最优均值概率就越低。
b) t时刻观测到观测量概率高斯分布图,明显观测量噪音低,可信度高。但是两个高斯的均值不一致,就是说观测和预估不一致
c) 卡尔曼滤波后的高斯分布图,该高斯图的噪音比其它两个都小,最优均值概率高于其它两个高斯图,而均值恰好在其它两个高斯均值之间
d) t+1时刻发生的状态转移,状态向右移动,这个过程增加了新的噪音,所以新的状态高斯图噪音增大,最优均值概率下降
e) t+1时刻的观测量高斯分布图
f)  t+1时刻应用卡尔曼滤波融合出来噪音低的最优状态高斯图

3 科尔曼滤波的条件和扩展卡尔曼滤波

本文的例子其实是个扩展卡尔曼滤波。标准的卡尔曼滤波要求2.2小节中的两个状态转移方程为线性方程,也就是要求A、B和C矩阵都为线性矩阵。
  • xt = At xt-1 + But     根据t-1的坐标和移动速度估计出在t时刻的坐标,也就是上面的公式一。这里At =1,Bt 就是时间间隔 Δt
  • z = C x                      状态量x 到观测量z 的映射关系。这里就是根据下面的三角函数计算出来的, C代表了状态映射矩阵。

本文的例子明显不符合这个要求,因为涉及到了三角函数等计算。标准卡尔曼滤波在现实环境中是很难找到的,大部分情况下都是非线性状态转移。而扩展卡尔曼滤波就是应用在非线性状态转移的环境中的。它的算法和标准算法一样,只是把上面的A、B和C矩阵线性化,也就是用一个线性方程最大程度的逼近非线性方程。常用的方法有UT变换Unscented Transformation) 和UKF变换。
 版权声明:本文为博主原创文章,未经博主允许不得转载。
 
©️2020 CSDN 皮肤主题: 大白 设计师: CSDN官方博客 返回首页
实付0元
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值