課件基于濾波器的激光方法grid based

1、基于濾波的激光SLAM方法(Grid-based)越凡創(chuàng)新技術(shù)597457483 .com 主講人曾書(shū)格課程內(nèi)容1、濾波2、粒子濾波Filter-based SLAM3、FastSLAM的原理及優(yōu)化4、Gmapping的介紹課程內(nèi)容1、濾波2、粒子濾波Filter-based SLAM3、FastSLAM的原理及優(yōu)化4、Gmapping的介紹估計(jì)數(shù)學(xué)概念條件概率公式：𝑥, 𝑦𝑥 𝑦𝑦 𝑥𝑃 𝑥, 𝑦 = 𝑃 𝑥 

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

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

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

5、9909;|𝑧)𝑝(𝑦|𝑧)p 𝑥= 𝑝 𝑥 𝑦 𝑝𝑦 𝑑𝑦濾波介紹濾波特性1. 估計(jì)的是概率分布，不是具體的數(shù)值2. 是一大類(lèi)方法的統(tǒng)稱3. 是一個(gè)抽象的表達(dá)形式對(duì)于不同問(wèn)題有不同的實(shí)現(xiàn)方式( 濾波)、粒子4. 迭代估計(jì)形式濾波濾波的推導(dǎo)𝑥𝑡 𝑝(𝑥𝑡|𝑥𝑡1, 𝑢ү

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

7、𝑡1, 𝑢1:𝑡1 𝑑𝑥𝑡1功能：已知狀態(tài)量t 1時(shí)刻的概率分布，在給定𝑡時(shí)刻的觀測(cè)數(shù)據(jù) 𝑧𝑡, 𝑢𝑧 的情況下估計(jì)出狀態(tài)量在𝑡時(shí)刻的概率分布濾波流程濾波濾波的推導(dǎo)令：目標(biāo)：在已知𝑝得到𝑝 𝑥𝑡、𝑢𝑡、𝑧𝑡的情況下，。𝑥𝑡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濾波濾波實(shí)例𝑏𝑒𝑙 𝑥𝑡= 𝑝 𝑥𝑡𝑧1:𝑡, 𝑢1:𝑡= 𝜂𝑝 𝑥𝑡 𝑧𝑡 𝑏𝑒𝑙 𝑥𝑡= 𝜂𝑝 𝑥𝑡 𝑧

16、9905; 𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡) 𝑝𝑥𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1 𝑑𝑥𝑡1課程內(nèi)容1、濾波2、粒子濾波Filter-based SLAM3、FastSLAM的原理及優(yōu)化4、Gmapping的介紹粒子濾波器示意圖特性估計(jì)器的一種實(shí)現(xiàn)方式能處理非線性情況能處理多峰分布的情況用一系列的粒子(particle)近似概率分布粒

17、子近似分布示意圖非參濾波器粒子濾波器示意圖流程𝑋 = 𝑥𝑖, 𝑤𝑖|𝑖 = 1, , 𝑛1. 用粒子進(jìn)行狀態(tài)𝑡𝑡𝑥𝑖𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑖𝑥𝑖表示一個(gè)狀態(tài)的假設(shè) 人位姿𝑡𝑡1𝑡𝑤𝑖表示假設(shè)的權(quán)重 跟地

18、圖的匹配度2. 評(píng)估每一個(gè)粒子的權(quán)重𝑡𝑤𝑖=𝜂𝑝 𝑧𝑡|𝑥𝑡𝑡3. 根據(jù)權(quán)重進(jìn)行重采樣以𝑤𝑖的概率接受𝑥𝑖，權(quán)重清零𝑡𝑡粒子濾波器狀態(tài)模型：𝑏𝑒𝑙 𝑥𝑡已知𝑡 1時(shí)刻的概率分布(粒子分布)：= 𝑝(𝑥𝑡

19、;|𝑥𝑡1, 𝑢𝑡) 𝑝𝑥𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1𝑑𝑥𝑡1𝑥𝑖, 𝑤𝑖|i = 1, 𝑛𝑝𝑥𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1= 𝑡1𝑡1根據(jù)數(shù)據(jù)&#

20、119906;𝑡𝑡 1時(shí)刻的概率分布(粒子分布)：𝑥𝑖𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑖𝑡𝑡1i = 1, 𝑛粒子濾波器狀態(tài)運(yùn)動(dòng)學(xué)模型回顧：因此：𝑑𝑥 + 𝜀𝑥𝑑𝑦 + 𝜀𝑦𝑑𝜃 + 𝜀𝜃

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

22、𝜃cos 𝜃𝑡1sin 𝜃𝑡10 sin 𝜃𝑡1cos 𝜃𝑡10001=+=+從分布即為運(yùn)動(dòng)學(xué)模型中進(jìn)行采樣：設(shè)𝑡 1時(shí)刻第𝑖個(gè)粒子的位姿𝑥𝑖𝑡1 = (𝑥𝑡1, 𝑦𝑡1, 𝜃𝑡1)設(shè)𝑡 1時(shí)刻的𝑢𝑡 = (𝑥, &

23、#119910;, 𝜃)設(shè)𝑡時(shí)刻第𝑖個(gè)粒子的位姿𝑥𝑖 = (𝑥𝑡, 𝑦𝑡, 𝜃𝑡)𝑡噪聲為為0均值的高斯分布，分別為𝑁𝑥, 𝑁𝑦, 𝑁𝑧粒子濾波器權(quán)重評(píng)估對(duì)于某一個(gè)粒子：無(wú)法知道人位姿的實(shí)際分布b𝑒𝑙 𝑥= 𝜂𝑝 𝑧

24、𝑥𝑝 𝑥𝑥, 𝑢𝑏𝑒𝑙(𝑥)𝑡𝑡𝑡𝑡𝑡1𝑡𝑡1從人的分布進(jìn)行采樣，人的后延概率分布𝑏𝑒𝑙 𝑥𝑡= 𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡)bel(𝑥&

25、#119905;1)權(quán)重一起近似因此權(quán)重為：權(quán)重用來(lái)評(píng)估實(shí)際人的分布和實(shí)𝜂𝑝 𝑧𝑡 𝑥𝑡 𝑝 𝑥𝑡𝑥𝑡1, 𝑢𝑡𝑏𝑒𝑙(𝑥𝑡1)𝑤 =際分布的差，差越大，權(quán)重越小。𝑝(𝑥 |𝑥, 𝑢 ) 𝑏𝑒

26、9897;(𝑥)𝑡𝑡1𝑡𝑡1= 𝜂𝑝(𝑧𝑡|𝑥𝑡)𝑤𝑖 = 𝑤𝑖1p 𝑧𝑡 𝑥𝑡 不重采樣的更新方式權(quán)重的定義：bel(𝑥𝑡)w =𝑏𝑒𝑙 𝑥𝑡粒子濾波器權(quán)重評(píng)估權(quán)重評(píng)估proposa

27、l采樣粒子濾波器重采樣到目前為止，新的粒子群是根據(jù)proposal分布進(jìn)行采樣的，并且用觀測(cè)模型計(jì)算權(quán)重，而最終的目的是用粒子群來(lái)近似后延概率分布對(duì)粒子群進(jìn)行重采樣，對(duì)于某一個(gè)粒子𝑥𝑖 來(lái)說(shuō)，以 𝑤𝑖 的概率接受這個(gè)粒子。生成一個(gè)隨機(jī)數(shù)，根據(jù)其落在的區(qū)間決定接受的粒子，重復(fù)N次。重采樣示意圖粒子濾波器權(quán)重評(píng)估權(quán)重評(píng)估重采樣后的粒子分布粒子濾波器算法流程存在的問(wèn)題粒子耗散問(wèn)題維數(shù)當(dāng)proposal比較差的時(shí)候，需要用很多的粒子才能較好的表示布人的后延概率分課程內(nèi)容1、濾波2、粒子濾波Filter-based SLAM3、FastSL

28、AM的原理及優(yōu)化4、Gmapping的介紹Fast-SLAM原理Fast-SLAM介紹為估計(jì)人路徑SLAM：在給定傳感器數(shù)據(jù)的情況下，𝑝𝑥1:𝑡 𝑢1:𝑡, 𝑧1:𝑡同時(shí)估計(jì)人位姿和環(huán)境地圖：𝑝(𝑚|𝑥1:𝑡, 𝑧1:𝑡)為給定人位姿和傳感器p(𝑥, 𝑚|𝑢, 𝑧)觀測(cè)數(shù)據(jù)的情況，進(jìn)行地圖構(gòu)建。本問(wèn)題可以實(shí)現(xiàn)close-for

29、m的求解。1:𝑡1:𝑡1:𝑡SLAM可以分解成兩個(gè)問(wèn)題：用粒子濾波來(lái)估計(jì)人的位姿，然后分1.人的別為每一個(gè)粒子計(jì)算地圖即可。因此一個(gè)粒子包含以下數(shù)據(jù)：2. 基于已知人位姿的構(gòu)圖1.人的軌跡𝑥1:𝑡p𝑥1:𝑡, 𝑚 𝑢1:𝑡, 𝑧1:𝑡2. 對(duì)應(yīng)的環(huán)境地圖= 𝑝= 𝑝𝑥1:𝑡 𝑢1:𝑡, 𝑧1:&

30、#119905;𝑥1:𝑡 𝑢1:𝑡, 𝑧1:𝑡𝑝𝑝𝑚 𝑥1:𝑡, 𝑢1:𝑡, 𝑧1:𝑡𝑚 𝑥1:𝑡, 𝑧1:𝑡Fast-SLAM算法流程回顧公式和估計(jì)，可得：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上式把對(duì)𝑥1:𝑡的估計(jì)，轉(zhuǎn)換為一個(gè)增量估計(jì)問(wèn)題對(duì)于之后的粒子，用觀測(cè)模型進(jìn)行權(quán)重計(jì)算，用粒子群表示𝑝𝑥1:𝑡1 𝑧1:𝑡1, 𝑢1:𝑡1并且根據(jù)估計(jì)的位姿構(gòu)建地圖。每個(gè)粒子用運(yùn)動(dòng)學(xué)模型𝑝

34、19909;𝑡進(jìn)行𝑥𝑡1, 𝑢𝑡Fast-SLAM優(yōu)化-1存在的問(wèn)題及優(yōu)化問(wèn)題：每一個(gè)粒子都包含的柵格地圖。對(duì)于稍微大一點(diǎn)的環(huán)境來(lái)說(shuō)，每一個(gè)粒子都會(huì)占用比較大的內(nèi)存。如果人的里誤差比較大，即proposal分布跟實(shí)際分布相差較大，則需人位姿的后延概率分布，會(huì)造成內(nèi)存要較多的粒子才能比較好的表示目的：要保持粒子的數(shù)量在一個(gè)比較小的數(shù)值。方法：提升proposal分布采樣的位姿質(zhì)量。𝑥𝑖𝑎𝑟𝑔 max 𝑝 𝑧

35、𝑡 𝑥𝑡, 𝑚𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑖=𝑥𝑖𝑖𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑡1𝑡𝑡1𝑡𝑥𝑡Fast-SLAM優(yōu)化-2存在的問(wèn)題及優(yōu)化問(wèn)題：粒子耗散問(wèn)題，因此每一次進(jìn)行重采樣都有一定的隨機(jī)性。隨著

36、重采樣次數(shù)的加多，粒子的多樣性會(huì)耗散掉，即最終的所有粒子都來(lái)自同一個(gè)粒子或者少數(shù)的幾個(gè)粒子的。目的：盡量緩解粒子耗散的問(wèn)題。方法：減少重采樣的次數(shù)，用一個(gè)量來(lái)表示當(dāng)前估計(jì)和真實(shí)分布的差異性:1𝑁𝑒𝑓𝑓= (𝑤𝑖)2當(dāng)𝑁eff較大時(shí)，說(shuō)明差異性很小，不進(jìn)行重采樣；當(dāng)𝑁eff較小于時(shí)，說(shuō)明差異性很大，因此進(jìn)行重采樣。極大的減少了重采樣的次數(shù)，緩解了粒子耗散問(wèn)題Fast-SLAM優(yōu)化-3進(jìn)一步優(yōu)化proposal分布上面的優(yōu)化方式：首先從proposal分布進(jìn)行采樣，然后進(jìn)

37、行極大似然估計(jì)提升采樣的質(zhì)量。本次優(yōu)化方式：考慮最近一幀的觀測(cè)，把proposal分布限制在一個(gè)狹小的有效區(qū)域。然后在正常的對(duì)proposal分布進(jìn)行采樣。假設(shè)：激光的匹配比里的測(cè)量精確很多，匹配的方差要比里從分布上來(lái)說(shuō)，激光模型的方差小很多Fast-SLAM優(yōu)化-3進(jìn)一步優(yōu)化proposal分布Proposal分布：𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡)𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡, &

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

39、9909;𝑡, 𝑚)在的區(qū)域(𝐿(𝑖)占主導(dǎo)地位，此時(shí)𝑝(𝑥𝑡|𝑥𝑡1, 𝑢𝑡)的值不再重要，為常數(shù)，因此： 𝐿(𝑖)，激光匹配的方差比里要小𝑝 𝑥𝑡 𝑥𝑡1, 𝑢𝑡, 𝑧𝑡, 𝑚= 𝜂𝑝 w

40、911;𝑡 𝑥𝑡, 𝑚𝑥𝑡很多，如果proposal分布用激光匹配來(lái)表示，則可以把采樣范圍限制在一個(gè)比較小的區(qū)域，因此可以用更少的粒子即覆蓋機(jī)器人的概率分布。即proposal分布從里觀測(cè)模型觀測(cè)模型變換到了激光Fast-SLAM優(yōu)化-3進(jìn)一步優(yōu)化proposal分布激光觀測(cè)模型的方差較小，假設(shè)其服高斯分布的求解：從高斯分布：1. 極大似然估計(jì)得到局部極值𝑝𝑥𝑡𝑥𝑡1, 𝑢𝑡, w

41、911;𝑡, 𝑚 𝑁(𝜇, )𝑥𝑥 |𝑢 , 𝑥𝑖=𝑎𝑟𝑔 max 𝑝 𝑧𝑥 , 𝑚𝑝𝑡𝑡𝑡𝑡𝑡𝑡1𝑥𝑡2. 認(rèn)為𝑥離高斯分布的均值比較近，因此𝑡在𝑥附

42、近采樣得到K個(gè)位姿。𝑡𝑥𝑗| 𝑥𝑗 𝑥< 𝑡3. 對(duì)這K個(gè)位姿進(jìn)行打分𝑝 𝑧𝑡 𝑥𝑗, 𝑚 ，并認(rèn)為這K個(gè)位姿服從高斯分布，即可求解得到高斯分布的表。Fast-SLAM優(yōu)化-3進(jìn)一步優(yōu)化proposal分布高斯分布的表𝐾：權(quán)重的計(jì)算方式：𝑝 𝑧𝑡 𝑥𝑡, 𝑚 𝑝 𝑥𝑡|𝑢𝑡, 𝑥𝑖1𝑏𝑒𝑙(𝑥𝑡1)𝑤 = 𝜂𝑡1𝑝 𝑥𝑡 𝑥𝑡1, 𝑢𝑡, 𝑧𝑡, 𝑚