课件基于滤波器的激光方法grid based

1、基于滤波的激光SLAM方法(Grid-based)越凡创新技术597457483 .com 主讲人曾书格课程内容1、滤波2、粒子滤波Filter-based SLAM3、FastSLAM的原理及优化4、Gmapping的介绍课程内容1、滤波2、粒子滤波Filter-based SLAM3、FastSLAM的原理及优化4、Gmapping的介绍估计数学概念条件概率公式：𝑥, 𝑦𝑥 𝑦𝑦 𝑥𝑃 𝑥, 𝑦 = 𝑃 𝑥 

2、9910; 𝑃 𝑦 = 𝑃 𝑦 𝑥 𝑃(𝑥)𝑝𝑝𝑝= 𝑝 𝑥 𝑝= 𝑝 𝑥= 𝑝(𝑦)𝑦公式：= 𝑝 𝑥 𝑦𝑝 𝑥, 𝑦𝑝 𝑦= 𝑝 𝑦 ⻖

3、9; 𝑝 𝑥条件：𝑥, 𝑦 𝑧𝑥 𝑦, 𝑧𝑦 𝑥, 𝑧𝑝𝑦 𝑥 𝑝(𝑥) 𝑝𝑥 𝑦= 𝜂𝑝 𝑦 𝑥𝑝(𝑥)p𝑝𝑝= 𝑝= 𝑝ү

4、09; 𝑧 𝑝𝑥 𝑧𝑦 𝑧𝑝(𝑦)= 𝑝(𝑦|𝑧)条件公式：全概率公式：𝑝 𝑦 𝑥, 𝑧 𝑝(𝑥|𝑧)𝑝 𝑥 𝑦, 𝑧= 𝜂𝑝 𝑦𝑥, 𝑧 𝑝(

5、9909;|𝑧)𝑝(𝑦|𝑧)p 𝑥= 𝑝 𝑥 𝑦 𝑝𝑦 𝑑𝑦滤波介绍滤波特性1. 估计的是概率分布，不是具体的数值2. 是一大类方法的统称3. 是一个抽象的表达形式对于不同问题有不同的实现方式( 滤波)、粒子4. 迭代估计形式滤波滤波的推导𝑥𝑡 𝑝(𝑥𝑡|𝑥𝑡1, 𝑢ү

6、05;) 𝑝𝑏𝑒𝑙𝑥𝑡= 𝑝𝑥𝑡𝑧1:𝑡, 𝑢1:𝑡= 𝜂𝑝𝑧𝑡𝑥𝑡𝑏𝑒𝑙𝑥𝑡= 𝜂𝑝𝑧𝑡𝑥𝑡1 𝑧1:

7、𝑡1, 𝑢1:𝑡1 𝑑𝑥𝑡1功能：已知状态量t 1时刻的概率分布，在给定𝑡时刻的观测数据 𝑧𝑡, 𝑢𝑧 的情况下估计出状态量在𝑡时刻的概率分布滤波流程滤波滤波的推导令：目标：在已知𝑝得到𝑝 𝑥𝑡、𝑢𝑡、𝑧𝑡的情况下，。𝑥𝑡1 𝑧1:

8、𝑡1, 𝑢1:𝑡1的表𝑧1:𝑡, 𝑢1:𝑡𝑏𝑒𝑙 𝑥𝑡= 𝑝(𝑥𝑡|𝑧1:𝑡, 𝑢1:𝑡)𝑏𝑒𝑙(𝑥𝑡) = 𝑝(𝑥𝑡|𝑧1:𝑡1, &

9、#119906;1:𝑡)𝑝 𝑧𝑡=𝑥𝑡 , 𝑧1:𝑡1, 𝑢1:𝑡 𝑝(𝑥𝑡|𝑧1:𝑡1, 𝑢1:𝑡)𝑝 𝑥 𝑧, 𝑢𝑏𝑒𝑙 𝑥表示𝑥 的后延概率分布𝑡1:ү

10、05;1:𝑡𝑡𝑡𝑝(𝑧𝑡|𝑧1:𝑡1, 𝑢1:𝑡)𝑥𝑡 , 𝑧1:𝑡1, 𝑢1:𝑡 𝑝(𝑥𝑡|𝑧1:𝑡1, 𝑢1:𝑡)= 𝜂𝑝 𝑧𝑡𝑏𝑒

11、𝑙(𝑥𝑡)表示𝑥𝑡的(𝑝𝑟𝑜𝑝𝑜𝑠𝑎𝑙)概率分布其中：则：𝑝𝑧𝑡𝑥𝑡, 𝑧1:𝑡1, 𝑢1:𝑡= 𝑝(𝑧𝑡|𝑥𝑡)𝑏𝑒𝑙

12、 𝑥𝑡= 𝜂𝑝(𝑧𝑡|𝑥𝑡)𝑏𝑒𝑙(𝑥𝑡)𝑝𝑥𝑡𝑧1:𝑡1, 𝑢1:𝑡= 𝑝 𝑥𝑡𝑥𝑡1, 𝑧1:𝑡1, 𝑢1:𝑡𝑥&#

13、119905;1 𝑧1:𝑡1, 𝑢1:𝑡𝑝𝑑𝑥𝑡1𝑏𝑒𝑙 𝑥𝑡= 𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡) 𝑝𝑥𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1 𝑑𝑥&

14、#119905;1𝑝𝑥𝑡𝑥𝑡1, 𝑧1:𝑡1, 𝑢1:𝑡= 𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡)𝑝𝑥𝑡1 𝑧1:𝑡1, 𝑢1:𝑡= 𝑝𝑥𝑡1 𝑧1:𝑡1,

15、19906;1:𝑡1滤波滤波实例𝑏𝑒𝑙 𝑥𝑡= 𝑝 𝑥𝑡𝑧1:𝑡, 𝑢1:𝑡= 𝜂𝑝 𝑥𝑡 𝑧𝑡 𝑏𝑒𝑙 𝑥𝑡= 𝜂𝑝 𝑥𝑡 𝑧

16、9905; 𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡) 𝑝𝑥𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1 𝑑𝑥𝑡1课程内容1、滤波2、粒子滤波Filter-based SLAM3、FastSLAM的原理及优化4、Gmapping的介绍粒子滤波器示意图特性估计器的一种实现方式能处理非线性情况能处理多峰分布的情况用一系列的粒子(particle)近似概率分布粒

17、子近似分布示意图非参滤波器粒子滤波器示意图流程𝑋 = 𝑥𝑖, 𝑤𝑖|𝑖 = 1, , 𝑛1. 用粒子进行状态𝑡𝑡𝑥𝑖𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑖𝑥𝑖表示一个状态的假设 人位姿𝑡𝑡1𝑡𝑤𝑖表示假设的权重 跟地

18、图的匹配度2. 评估每一个粒子的权重𝑡𝑤𝑖=𝜂𝑝 𝑧𝑡|𝑥𝑡𝑡3. 根据权重进行重采样以𝑤𝑖的概率接受𝑥𝑖，权重清零𝑡𝑡粒子滤波器状态模型：𝑏𝑒𝑙 𝑥𝑡已知𝑡 1时刻的概率分布(粒子分布)：= 𝑝(𝑥𝑡

19、;|𝑥𝑡1, 𝑢𝑡) 𝑝𝑥𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1𝑑𝑥𝑡1𝑥𝑖, 𝑤𝑖|i = 1, 𝑛𝑝𝑥𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1= 𝑡1𝑡1根据数据&#

20、119906;𝑡𝑡 1时刻的概率分布(粒子分布)：𝑥𝑖𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑖𝑡𝑡1i = 1, 𝑛粒子滤波器状态运动学模型回顾：因此：𝑑𝑥 + 𝜀𝑥𝑑𝑦 + 𝜀𝑦𝑑𝜃 + 𝜀𝜃

21、9909;𝑦𝜃𝑥𝑦𝜃cos 𝜃sin 𝜃0 sin 𝜃cos 𝜃0001𝑥𝑡𝑦𝑡𝜃𝑡𝑥𝑡1𝑦𝑡1𝜃𝑡1𝑥 + 𝑁𝑥𝑦 + 𝑁𝑦𝜃 + 𝑁

22、𝜃cos 𝜃𝑡1sin 𝜃𝑡10 sin 𝜃𝑡1cos 𝜃𝑡10001=+=+从分布即为运动学模型中进行采样：设𝑡 1时刻第𝑖个粒子的位姿𝑥𝑖𝑡1 = (𝑥𝑡1, 𝑦𝑡1, 𝜃𝑡1)设𝑡 1时刻的𝑢𝑡 = (𝑥, &

23、#119910;, 𝜃)设𝑡时刻第𝑖个粒子的位姿𝑥𝑖 = (𝑥𝑡, 𝑦𝑡, 𝜃𝑡)𝑡噪声为为0均值的高斯分布，分别为𝑁𝑥, 𝑁𝑦, 𝑁𝑧粒子滤波器权重评估对于某一个粒子：无法知道人位姿的实际分布b𝑒𝑙 𝑥= 𝜂𝑝 𝑧

24、𝑥𝑝 𝑥𝑥, 𝑢𝑏𝑒𝑙(𝑥)𝑡𝑡𝑡𝑡𝑡1𝑡𝑡1从人的分布进行采样，人的后延概率分布𝑏𝑒𝑙 𝑥𝑡= 𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡)bel(𝑥&

25、#119905;1)权重一起近似因此权重为：权重用来评估实际人的分布和实𝜂𝑝 𝑧𝑡 𝑥𝑡 𝑝 𝑥𝑡𝑥𝑡1, 𝑢𝑡𝑏𝑒𝑙(𝑥𝑡1)𝑤 =际分布的差，差越大，权重越小。𝑝(𝑥 |𝑥, 𝑢 ) 𝑏𝑒

26、9897;(𝑥)𝑡𝑡1𝑡𝑡1= 𝜂𝑝(𝑧𝑡|𝑥𝑡)𝑤𝑖 = 𝑤𝑖1p 𝑧𝑡 𝑥𝑡 不重采样的更新方式权重的定义：bel(𝑥𝑡)w =𝑏𝑒𝑙 𝑥𝑡粒子滤波器权重评估权重评估proposa

27、l采样粒子滤波器重采样到目前为止，新的粒子群是根据proposal分布进行采样的，并且用观测模型计算权重，而最终的目的是用粒子群来近似后延概率分布对粒子群进行重采样，对于某一个粒子𝑥𝑖 来说，以 𝑤𝑖 的概率接受这个粒子。生成一个随机数，根据其落在的区间决定接受的粒子，重复N次。重采样示意图粒子滤波器权重评估权重评估重采样后的粒子分布粒子滤波器算法流程存在的问题粒子耗散问题维数当proposal比较差的时候，需要用很多的粒子才能较好的表示布人的后延概率分课程内容1、滤波2、粒子滤波Filter-based SLAM3、FastSL

28、AM的原理及优化4、Gmapping的介绍Fast-SLAM原理Fast-SLAM介绍为估计人路径SLAM：在给定传感器数据的情况下，𝑝𝑥1:𝑡 𝑢1:𝑡, 𝑧1:𝑡同时估计人位姿和环境地图：𝑝(𝑚|𝑥1:𝑡, 𝑧1:𝑡)为给定人位姿和传感器p(𝑥, 𝑚|𝑢, 𝑧)观测数据的情况，进行地图构建。本问题可以实现close-for

29、m的求解。1:𝑡1:𝑡1:𝑡SLAM可以分解成两个问题：用粒子滤波来估计人的位姿，然后分1.人的别为每一个粒子计算地图即可。因此一个粒子包含以下数据：2. 基于已知人位姿的构图1.人的轨迹𝑥1:𝑡p𝑥1:𝑡, 𝑚 𝑢1:𝑡, 𝑧1:𝑡2. 对应的环境地图= 𝑝= 𝑝𝑥1:𝑡 𝑢1:𝑡, 𝑧1:&

30、#119905;𝑥1:𝑡 𝑢1:𝑡, 𝑧1:𝑡𝑝𝑝𝑚 𝑥1:𝑡, 𝑢1:𝑡, 𝑧1:𝑡𝑚 𝑥1:𝑡, 𝑧1:𝑡Fast-SLAM算法流程回顾公式和估计，可得：p𝑥1:𝑡 𝑢1:𝑡, 𝑧1:ү

31、05;=𝜂𝑝𝜂𝑝𝜂𝑝𝜂𝑝𝑧𝑡𝑧𝑡𝑧𝑡𝑧𝑡𝑥1:𝑡, 𝑢1:𝑡, 𝑧1:𝑡1𝑝𝑥1:𝑡 𝑧1:𝑡1, 𝑢1:𝑡𝑥w

32、905;𝑥𝑡𝑥𝑡𝑝𝑥1:𝑡 𝑧1:𝑡1, 𝑢1:𝑡𝑝 𝑥𝑡𝑝 𝑥𝑡𝑥1:𝑡1, 𝑧1:𝑡1, 𝑢1:𝑡𝑝𝑥1:𝑡1 𝑧1:𝑡1, 𝑢1:

33、𝑡𝑥𝑡1, 𝑢𝑡𝑝𝑥1:𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1上式把对𝑥1:𝑡的估计，转换为一个增量估计问题对于之后的粒子，用观测模型进行权重计算，用粒子群表示𝑝𝑥1:𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1并且根据估计的位姿构建地图。每个粒子用运动学模型𝑝

34、19909;𝑡进行𝑥𝑡1, 𝑢𝑡Fast-SLAM优化-1存在的问题及优化问题：每一个粒子都包含的栅格地图。对于稍微大一点的环境来说，每一个粒子都会占用比较大的内存。如果人的里误差比较大，即proposal分布跟实际分布相差较大，则需人位姿的后延概率分布，会造成内存要较多的粒子才能比较好的表示目的：要保持粒子的数量在一个比较小的数值。方法：提升proposal分布采样的位姿质量。𝑥𝑖𝑎𝑟𝑔 max 𝑝 𝑧

35、𝑡 𝑥𝑡, 𝑚𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑖=𝑥𝑖𝑖𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑡1𝑡𝑡1𝑡𝑥𝑡Fast-SLAM优化-2存在的问题及优化问题：粒子耗散问题，因此每一次进行重采样都有一定的随机性。随着

36、重采样次数的加多，粒子的多样性会耗散掉，即最终的所有粒子都来自同一个粒子或者少数的几个粒子的。目的：尽量缓解粒子耗散的问题。方法：减少重采样的次数，用一个量来表示当前估计和真实分布的差异性:1𝑁𝑒𝑓𝑓= (𝑤𝑖)2当𝑁eff较大时，说明差异性很小，不进行重采样；当𝑁eff较小于时，说明差异性很大，因此进行重采样。极大的减少了重采样的次数，缓解了粒子耗散问题Fast-SLAM优化-3进一步优化proposal分布上面的优化方式：首先从proposal分布进行采样，然后进

37、行极大似然估计提升采样的质量。本次优化方式：考虑最近一帧的观测，把proposal分布限制在一个狭小的有效区域。然后在正常的对proposal分布进行采样。假设：激光的匹配比里的测量精确很多，匹配的方差要比里从分布上来说，激光模型的方差小很多Fast-SLAM优化-3进一步优化proposal分布Proposal分布：𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡)𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡, &

38、#119911;𝑡, 𝑚)𝑝 𝑥𝑡 𝑥𝑡1, 𝑢𝑡, 𝑧𝑡, 𝑚= 𝜂𝑝 𝑧𝑡 𝑥𝑡, 𝑚 𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡)𝑝(𝑧𝑡|

39、9909;𝑡, 𝑚)在的区域(𝐿(𝑖)占主导地位，此时𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡)的值不再重要，为常数，因此： 𝐿(𝑖)，激光匹配的方差比里要小𝑝 𝑥𝑡 𝑥𝑡1, 𝑢𝑡, 𝑧𝑡, 𝑚= 𝜂𝑝 w

40、911;𝑡 𝑥𝑡, 𝑚𝑥𝑡很多，如果proposal分布用激光匹配来表示，则可以把采样范围限制在一个比较小的区域，因此可以用更少的粒子即覆盖机器人的概率分布。即proposal分布从里观测模型观测模型变换到了激光Fast-SLAM优化-3进一步优化proposal分布激光观测模型的方差较小，假设其服高斯分布的求解：从高斯分布：1. 极大似然估计得到局部极值𝑝𝑥𝑡𝑥𝑡1, 𝑢𝑡, w

41、911;𝑡, 𝑚 𝑁(𝜇, )𝑥𝑥 |𝑢 , 𝑥𝑖=𝑎𝑟𝑔 max 𝑝 𝑧𝑥 , 𝑚𝑝𝑡𝑡𝑡𝑡𝑡𝑡1𝑥𝑡2. 认为𝑥离高斯分布的均值比较近，因此𝑡在𝑥附

42、近采样得到K个位姿。𝑡𝑥𝑗| 𝑥𝑗 𝑥< 𝑡3. 对这K个位姿进行打分𝑝 𝑧𝑡 𝑥𝑗, 𝑚 ，并认为这K个位姿服从高斯分布，即可求解得到高斯分布的表。Fast-SLAM优化-3进一步优化proposal分布高斯分布的表𝐾：权重的计算方式：𝑝 𝑧𝑡 𝑥𝑡, 𝑚 𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑖1𝑏𝑒𝑙(𝑥𝑡1)𝑤 = 𝜂𝑡1𝑝 𝑥𝑡 𝑥𝑡1, 𝑢𝑡, 𝑧𝑡, 𝑚