杜 晨, 杜 煜

(1.北京聯(lián)合大學(xué) 北京市信息服務(wù)工程重點(diǎn)實(shí)驗(yàn)室，北京 100101；2.北京聯(lián)合大學(xué) 機(jī)器人學(xué)院，北京 100101)

0 引 言

作為機(jī)器人自主定位導(dǎo)航技術(shù)的關(guān)鍵，同時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)技術(shù)也正成為關(guān)注焦點(diǎn)[1,2]。移動(dòng)機(jī)器人的SLAM技術(shù)包括對(duì)機(jī)器人的狀態(tài)和環(huán)境信息的同時(shí)估計(jì)[3]。

Rao-Blackwellized粒子濾波器(Rao-Blackwellized particle filter,RBPF)是解決SLAM技術(shù)的方法之一。經(jīng)過(guò)多年的發(fā)展也出現(xiàn)了很多改進(jìn)辦法，國(guó)內(nèi)的王田橙[4]、國(guó)外PopovlG[5]等人都在不同程度上對(duì)RBPF-SLAM算法做出了改進(jìn)。

本文在RBPF-SLAM方法的基礎(chǔ)上，將采樣集中在觀測(cè)信息的可能性區(qū)域，減少了與目標(biāo)分布的誤差，使得采樣的粒子更加符合真實(shí)環(huán)境狀態(tài)，建立更準(zhǔn)確的地圖環(huán)境；并且引入分層重采樣優(yōu)化策略，通過(guò)控制閾值，維持盡可能多的粒子多樣性，有效地解決了粒子退化問(wèn)題。最后，為了驗(yàn)證算法的有效性，以Bulldog移動(dòng)機(jī)器人平臺(tái)為基礎(chǔ)，開(kāi)展了相關(guān)實(shí)驗(yàn)。并針對(duì)實(shí)驗(yàn)平臺(tái)，優(yōu)化了掃描處理步驟，加快了掃描處理時(shí)間，有效降低了實(shí)驗(yàn)運(yùn)行時(shí)間。

1 基于RBPF-SLAM算法的優(yōu)化

1.1 RBPF-SLAM原理

RBPF-SLAM問(wèn)題可理解為:初始地圖m0與初始位姿x0已知的情況下，從最初到時(shí)刻t的傳感器觀測(cè)信息z1:t=z1,…,zt，與移動(dòng)機(jī)器人里程計(jì)的控制信息u1:t-1=u1,…,ut-1，來(lái)估計(jì)p(x1:t,m|z1:t,u1:t-1)，即機(jī)器人軌跡x1:t=x1…,xt與地圖m的聯(lián)合后驗(yàn)概率分布。通過(guò)貝葉斯濾波器可以對(duì)其進(jìn)行分解

p(x1:t,m|z1:t,u1:t-1)=p(m|x1:t,z1:t)p(x1:t|z1:t,u1:t-1)

(1)

RBPF-SLAM算法采用粒子濾波器來(lái)估算機(jī)器人的位姿，其中，每一個(gè)粒子代表一條可能的軌跡。包含4個(gè)主要步驟：采樣、權(quán)重計(jì)算、粒子重采樣、地圖更新。在權(quán)重計(jì)算階段可以通過(guò)限制概率密度函數(shù)來(lái)進(jìn)行

(2)

式中η=1/p(zt|z1:t-1,u1:t-1)為貝葉斯定律中歸一化因子，所有粒子擁有相同的η。

1.2 優(yōu)化的RBPF-SLAM

1.2.1 融合觀測(cè)信息的提議分布

機(jī)器人在實(shí)際移動(dòng)過(guò)程中，激光雷達(dá)作為機(jī)器人的外部傳感器具有較高的精度，似然函數(shù)區(qū)域比較集中；而基于里程計(jì)的運(yùn)動(dòng)模型，因里程計(jì)自身誤差較大，似然函數(shù)區(qū)域比較分散，導(dǎo)致了兩者之間重合的地方很少。

融合觀測(cè)信息[6]，將采樣集中在觀測(cè)信息的可能性區(qū)域?？梢缘玫疥P(guān)于粒子權(quán)重方差的最佳提議分布，減少與目標(biāo)分布的誤差，使得采樣的粒子更加符合真實(shí)環(huán)境狀態(tài)，建立更準(zhǔn)確的地圖環(huán)境。此時(shí)的提議分布

進(jìn)一步更新式(2)，得到

在似然函數(shù)的峰值進(jìn)行采樣，可以得到化簡(jiǎn)之后的權(quán)重計(jì)算

1.2.2 重采樣優(yōu)化策略

引入有效粒子數(shù)Neff來(lái)估計(jì)當(dāng)前粒子集代表目標(biāo)函數(shù)的近似程度

為解決粒子退化問(wèn)題[7，8]，本文引入了一種分層重采樣優(yōu)化策略。首先設(shè)置采樣粒子高權(quán)重閾值ωk=2/N和低權(quán)重閾值ωL=1/(2N)，其中N代表粒子數(shù)。根據(jù)設(shè)定的閾值，把粒子權(quán)值分為3個(gè)區(qū)間，較高權(quán)值區(qū)間、較低權(quán)值區(qū)間和中等權(quán)值區(qū)間。其中，對(duì)中等權(quán)值區(qū)間的粒子不做處理，對(duì)具有較高和較低權(quán)值區(qū)間的粒子按權(quán)值大小由高到低進(jìn)行排序，之后再計(jì)算每個(gè)粒子被選擇的概率

p(i)={a+[rank(i)/(Nh+Nl-1)]·

(b-a)}/(Nh+Nl)

(7)

式中 rank(i)為粒子根據(jù)權(quán)重排序后的序號(hào)，a,b為系數(shù)，Nh和Nl分別為高權(quán)重粒子數(shù)和低權(quán)重粒子數(shù)。

1.2.3 優(yōu)化RBPF-SLAM算法流程

1)首先，估計(jì)機(jī)器人初始位姿；2)使用迭代最近鄰ICP算法進(jìn)行點(diǎn)云匹配；3)進(jìn)行采樣，計(jì)算目標(biāo)分布，進(jìn)而求得提議分布；4)在融合觀測(cè)信息的提議分布中采樣求得新粒子集，計(jì)算粒子權(quán)重；5)進(jìn)行優(yōu)化重采樣策略更新粒子權(quán)值；6)地圖更新。

2 移動(dòng)機(jī)器人系統(tǒng)

2.1 移動(dòng)機(jī)器人硬件平臺(tái)

本實(shí)驗(yàn)平臺(tái)為長(zhǎng)90 cm、寬70 cm 、高80 cm的Bulldog機(jī)器人，實(shí)物圖如圖1(a)所示。

本實(shí)驗(yàn)選取Rslidar激光雷達(dá)傳感器以及里程計(jì)進(jìn)行實(shí)驗(yàn)。Rslidar激光雷達(dá)集合了16個(gè)激光收發(fā)元器件，可以按照設(shè)定的頻率進(jìn)行掃描。里程計(jì)能夠記錄機(jī)器人走過(guò)的路程以及方向等信息。其內(nèi)部存在一個(gè)用串口線連接的PC，外部PC連接Bulldog的路由，通過(guò)SSH協(xié)議實(shí)現(xiàn)通信。

Bulldog輪式機(jī)器人包含感知模塊、決策模塊和底盤(pán)控制模塊等。當(dāng)控制層對(duì)Bulldog輪式機(jī)器人進(jìn)行控制時(shí)，感知層中的傳感器會(huì)接收到自身以及周?chē)鷪?chǎng)景的信號(hào)。之后，將信號(hào)節(jié)點(diǎn)發(fā)布到內(nèi)部PC中，外部PC連接Bulldog的路由，實(shí)現(xiàn)與內(nèi)部PC的通信。在訂閱節(jié)點(diǎn)信息的同時(shí)，對(duì)這些數(shù)據(jù)進(jìn)行處理，根據(jù)各自算法的需求，來(lái)完成相應(yīng)的實(shí)驗(yàn)。

2.2 移動(dòng)機(jī)器人軟件系統(tǒng)

機(jī)器人操作系統(tǒng)(robot operating system,ROS)是移動(dòng)機(jī)器人研究的基礎(chǔ)，ROS的提出推動(dòng)了機(jī)器人產(chǎn)業(yè)的發(fā)展。系統(tǒng)框架分為:節(jié)點(diǎn)(node)、節(jié)點(diǎn)管理器(master)、消息(message)、話題(topic)。其中，節(jié)點(diǎn)是核心，可以執(zhí)行簡(jiǎn)單的程序語(yǔ)句[9]。

內(nèi)部PC為L(zhǎng)inux(Ubuntu14.04)上運(yùn)行的ROS，主要節(jié)點(diǎn)簡(jiǎn)化圖如圖1(b)所示。

圖1 移動(dòng)機(jī)器人及ROS的主要節(jié)點(diǎn)

在ROS框架下，激光雷達(dá)傳感器用激光雷達(dá)節(jié)點(diǎn)來(lái)實(shí)現(xiàn)激光雷達(dá)數(shù)據(jù)幀的讀取，由/scan話題完成激光數(shù)據(jù)的發(fā)布工作。里程計(jì)節(jié)點(diǎn)得到里程計(jì)信息，由/odom完成里程計(jì)信息的發(fā)布工作。所有話題都會(huì)發(fā)布到公共主題當(dāng)中，等待其它節(jié)點(diǎn)訂閱。ROS中還提供了一個(gè)可視化工具，可以實(shí)現(xiàn)實(shí)時(shí)在線地圖構(gòu)建的顯示。

由于實(shí)驗(yàn)平臺(tái)實(shí)現(xiàn)的功能較簡(jiǎn)單，僅在移動(dòng)機(jī)器人已經(jīng)行進(jìn)了給定距離后才開(kāi)始處理掃描數(shù)據(jù)，隨著掃描數(shù)據(jù)的增多，運(yùn)行時(shí)間會(huì)加大。因而，在掃描處理上也做出了優(yōu)化改進(jìn)，增加了限定時(shí)間的掃描處理?xiàng)l件，以便更加快速的處理掃描。

3 實(shí)驗(yàn)環(huán)境與分析

3.1 實(shí)驗(yàn)環(huán)境與結(jié)果

選取由簡(jiǎn)單到復(fù)雜的兩個(gè)實(shí)驗(yàn)場(chǎng)景，分別用傳統(tǒng)RBPF-SLAM算法即Gmapping算法、其它改進(jìn)RBPF-SLAM算法[7]以及本文優(yōu)化的RBPF-SLAM算法進(jìn)行實(shí)驗(yàn)。

實(shí)驗(yàn)場(chǎng)景一：近似地圖為80 m×2 m，環(huán)境比較簡(jiǎn)單，包括長(zhǎng)走廊，電梯口等，真實(shí)場(chǎng)景如圖2(a)所示。三種算法使用相同的粒子數(shù)，構(gòu)建地圖結(jié)果如圖2(b)、(c)、(d)所示。

圖2 三種算法分別構(gòu)圖結(jié)果

圖2(b)、(c)、(d)是移動(dòng)機(jī)器人從A點(diǎn)到B點(diǎn)再到C點(diǎn)使用不同算法在RVIZ上顯示的構(gòu)圖情況。選取的柵格大小同樣為5 cm×5 cm。從構(gòu)建地圖的最終結(jié)果來(lái)看，隨著時(shí)間增加，圖2(b)中會(huì)出現(xiàn)邊緣地方不一致性現(xiàn)象，如圖中標(biāo)出的地方；圖2(c)和圖2(d)構(gòu)圖效果較好，可以明顯地看出D點(diǎn)凹凸的輪廓。

在場(chǎng)景一中分別進(jìn)行5次實(shí)驗(yàn)，計(jì)算這兩種算法創(chuàng)建相同一致性地圖時(shí)，需要的平均運(yùn)行時(shí)間。實(shí)驗(yàn)時(shí)，機(jī)器人平均速度為0.4 m/s，改進(jìn)算法需要258 s，本文優(yōu)化的RBPF-SLAM算法需要232 s，比改進(jìn)算法更快。

實(shí)驗(yàn)場(chǎng)景二：近似地圖為6 m×8 m，存在一些障礙物，如辦公桌椅、書(shū)柜、長(zhǎng)桌等，如圖3(a)所示。同樣使用三種算法進(jìn)行對(duì)比實(shí)驗(yàn)。查看構(gòu)建一致性地圖時(shí)，三種算法使用粒子數(shù)的情況。圖3(b)是本文優(yōu)化的RBPF-SLAM算法構(gòu)建的地圖結(jié)果。

圖3 實(shí)驗(yàn)場(chǎng)景及本文算法構(gòu)圖結(jié)果

當(dāng)構(gòu)建的地圖結(jié)果中A點(diǎn)、B點(diǎn)和C點(diǎn)都能清晰看出輪廓時(shí)，記錄三種算法使用粒子數(shù)的情況，傳統(tǒng)RBPF-SLAM算法使用38個(gè)粒子，改進(jìn)RBPF-SLAM算法使用20個(gè)粒子，本文優(yōu)化的RBPF-SLAM算法只需要18個(gè)粒子。

3.2 實(shí)驗(yàn)結(jié)果分析

從構(gòu)建地圖結(jié)果中可以看出，當(dāng)實(shí)驗(yàn)環(huán)境較簡(jiǎn)單(實(shí)驗(yàn)場(chǎng)景一)，使用相同粒子數(shù)時(shí)，其它研究者的改進(jìn)算法以及本文優(yōu)化的RBPF-SLAM算法構(gòu)圖結(jié)果更加可靠，地圖出現(xiàn)不一致性現(xiàn)象的情況較少。說(shuō)明將采樣更集中在觀測(cè)信息的可行性區(qū)域，使得采樣的粒子更加符合真實(shí)環(huán)境的狀態(tài)，可以改善地圖構(gòu)建不一致性。而構(gòu)建相同精度的地圖時(shí)，與改進(jìn)RBPF-SLAM算法相比較，本文優(yōu)化的RBPF-SLAM算法所需要的平均運(yùn)行時(shí)間更短，算法的效率更高。

當(dāng)實(shí)驗(yàn)環(huán)境復(fù)雜一些(實(shí)驗(yàn)場(chǎng)景二)，構(gòu)建相同清晰度的地圖時(shí)，本文優(yōu)化的RBPF-SLAM算法使用的粒子數(shù)更少，有效地降低了算法的計(jì)算量。

為了對(duì)算法的粒子多樣性進(jìn)行驗(yàn)證，同樣用三種算法在使用相同30個(gè)粒子時(shí)，查看重采樣過(guò)程中粒子多樣性變化情況，如圖4所示。

圖4 重采樣過(guò)程中粒子多樣性變化

由圖4可以看出：本文優(yōu)化的RBPF-SLAM算法，在重采樣過(guò)程中，不同粒子數(shù)明顯多于傳統(tǒng)的RBPF-SLAM算法；與改進(jìn)的RBPF-SLAM算法相比較，粒子多樣性的波動(dòng)范圍會(huì)更大一些。也說(shuō)明了優(yōu)化算法不僅可以緩解粒子退化現(xiàn)象，而且粒子的多樣性更好一些。

4 結(jié) 論

本文優(yōu)化的Rao-Blackwellized粒子濾波的同時(shí)定位與地圖構(gòu)建算法，在提議分布中融合了激光雷達(dá)的觀測(cè)信息，將采樣集中在觀測(cè)信息的可能性區(qū)域，減少了與目標(biāo)分布的誤差，提高了準(zhǔn)確度，有效地減少了地圖不一致性。而通過(guò)控制權(quán)值的閾值，維持盡可能多的粒子多樣性的分層重采樣優(yōu)化策略，有效緩解了粒子退化現(xiàn)象。而進(jìn)一步優(yōu)化的掃描處理，增加了限定時(shí)間的掃描處理?xiàng)l件，加快了處理速度，降低了運(yùn)行時(shí)間。下一步工作，嘗試在更大的實(shí)驗(yàn)環(huán)境下驗(yàn)證優(yōu)化RBPF-SLAM算法的魯棒性。