戈廣雙， 李子龍， 楊 凱， 馬瑞鑫

(交通運(yùn)輸部 天津水運(yùn)工程科學(xué)研究所，天津 300000)

基于激光掃描測距的機(jī)器人粒子濾波定位技術(shù)研究*

戈廣雙， 李子龍， 楊 凱， 馬瑞鑫

(交通運(yùn)輸部天津水運(yùn)工程科學(xué)研究所，天津300000)

傳統(tǒng)的粒子濾波即時(shí)定位與地圖構(gòu)建(SLAM)算法在構(gòu)建地圖和目標(biāo)進(jìn)行自主定位時(shí)，粒子數(shù)量大，占用的內(nèi)存高，重采樣之后容易出現(xiàn)粒子匱乏現(xiàn)象，為了提高機(jī)器人自主定位的效率，提出了一種改進(jìn)的重采樣策略和粒子更新策略，融入系統(tǒng)模型。在裝有機(jī)器人操作系統(tǒng)(ROS)的旅行家移動(dòng)機(jī)器人上進(jìn)行測試，實(shí)驗(yàn)結(jié)果表明：方法能夠有效提升粒子濾波定位的效率。

即時(shí)定位與地圖構(gòu)建； 粒子濾波； 蒙特卡洛定位算法； 機(jī)器人操作系統(tǒng)

0 引 言

即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)是實(shí)現(xiàn)機(jī)器人自主行為控制的關(guān)鍵技術(shù)，機(jī)器人從任意位置開始，在移動(dòng)過程中，用自身的傳感器獲取環(huán)境數(shù)據(jù)，構(gòu)建增量地圖，進(jìn)行自主定位。在已知環(huán)境的自主定位研究已經(jīng)取得了一些實(shí)用的方法，但在未知環(huán)境下，如何構(gòu)建地圖和利用構(gòu)建的地圖進(jìn)行自主定位和導(dǎo)航，還有待進(jìn)一步研究[1,2]。

粒子濾波(particle filtering，PF)算法基于概率統(tǒng)計(jì)的思想具有可逼近任意概率分布的特性，適用于移動(dòng)機(jī)器人定位、地圖構(gòu)建。在此算法基礎(chǔ)上，Montermerlo等人提出的FastSLAM算法降低了粒子數(shù)目和運(yùn)算復(fù)雜度，是SLAM最具影響力的算法。宋凱楠等人[3]提出了一種用室內(nèi)環(huán)境的特征點(diǎn)替代激光掃描數(shù)據(jù)進(jìn)行ICP匹配的方法，減少了匹配時(shí)間，提高了SLAM算法的快速性。任春明等人[4]提出了一種機(jī)器人局部地圖構(gòu)建的新方法，采用加權(quán)最小二乘法對機(jī)器人在導(dǎo)航過程中的局部地圖進(jìn)行構(gòu)建，具有實(shí)現(xiàn)容易、精度較高等特點(diǎn)。但為了很好地近似系統(tǒng)的后驗(yàn)概率密度，PF需要采用大量的粒子樣本，樣本數(shù)量越多，算法的復(fù)雜度就越高。因此，優(yōu)化粒子的采樣策略，減少粒子樣本數(shù)量，同時(shí)避免重采樣階段造成的粒子有效性和多樣性損失導(dǎo)致的樣本匱乏現(xiàn)象為本文算法研究的重點(diǎn)[5～10]。

本文主要研究在已知環(huán)境下，利用激光雷達(dá)作為外部傳感器，探索周圍環(huán)境，使機(jī)器人進(jìn)行柵格地圖構(gòu)建和自主定位，增強(qiáng)機(jī)器人的自主行為控制能力，針對粒子濾波算法復(fù)雜度高的問題，優(yōu)化重采樣策略、降低計(jì)算復(fù)雜度，通過選用合適的重要性函數(shù)，解決粒子匱乏和算法復(fù)雜度高的問題，并基于機(jī)器人操作系統(tǒng)(robot operating system,ROS)對優(yōu)化的算法進(jìn)行驗(yàn)證。

1 粒子濾波定位基本原理

1.1 激光測距掃描傳感器數(shù)學(xué)模型

激光測距掃描傳感器是在某一個(gè)平面上，按照一定的角度分辨率進(jìn)行距離掃描，激光測距傳感器的數(shù)據(jù)以極坐標(biāo)的形式sn=(ρn,φn)T給出，則其直角坐標(biāo)系表示為

zn=(xn,yn)T,n=1,2,…,N

(1)

式中xn=ρncosφn;yn=ρnsinφn;N為激光測距掃描傳感器單個(gè)掃描周期的掃描點(diǎn)數(shù)。

(2)

1.2 機(jī)器人運(yùn)動(dòng)模型

為簡化計(jì)算，設(shè)定機(jī)器人坐標(biāo)系和激光測距傳感器的坐標(biāo)系重合，如圖1所示。

圖1 機(jī)器人坐標(biāo)系與全局坐標(biāo)系

機(jī)器人本體中通過監(jiān)測輪胎轉(zhuǎn)過的角度計(jì)算輪胎的移動(dòng)距離，采用角速度陀螺儀角度積分監(jiān)測角度的變化量，機(jī)器人移動(dòng)姿態(tài)的增量為

un={Δsn,Δθn}T

(3)

式中 Δsn為里程計(jì)增量信息；Δθn為機(jī)器人角度變化量。

1.3 系統(tǒng)模型

地面自主移動(dòng)機(jī)器人SLAM系統(tǒng)模型如圖2所示，假定機(jī)器人在已知環(huán)境中移動(dòng)，環(huán)境信息用mi表示，xt代表t時(shí)刻機(jī)器人的位姿狀態(tài)向量，ut代表里程計(jì)數(shù)據(jù)，zt代表激光傳感器的相對地圖的觀測數(shù)據(jù)。

1.4 蒙特卡洛粒子濾波定位原理

蒙特卡洛粒子濾波定位原理，用一系列樣本(粒子)估計(jì)機(jī)器人的位置信息，用S表示

S={〈s[i],w[i]〉|i=1,2,…,N}

(4)

式中s[i]為第i個(gè)樣本粒子的狀態(tài)向量;w[i]為第i個(gè)樣本的相關(guān)權(quán)重系數(shù)，w[i]為非零且和為1。粒子集概率分布

(5)

式中δs[i]為第i個(gè)粒子的狄拉克函數(shù)。

粒子濾波算法通過t-1時(shí)刻的粒子序列St-1估算t時(shí)刻的粒子序列St，計(jì)算過程由以下3步驟組成:

通常，步驟(1)中使用里程計(jì)信息創(chuàng)建粒子樣本，運(yùn)動(dòng)模型簡化為p(xt|xt-1,ut-1)。

步驟(2)中相關(guān)權(quán)重系數(shù)w[i]的取值通過觀測量計(jì)算。假設(shè)p(s1:t|d)是需要被求解的相關(guān)粒子的后驗(yàn)概率，d代表相關(guān)數(shù)據(jù),則

(6)

(7)

采用卡爾曼濾波計(jì)算粒子相對于地圖的均值和方差

(8)

(9)

(10)

μt

(11)

(12)

式中

μt|t-1E{mt|z1:t-1,u1:t-2},μtE{mt|z1:t,u1:t-1}

∑t|t-1cov(mt|z1:t-1,u1:t-2),∑tcov(mt|z1:t,u1:t-1),Stcov((zt,ut)|z1:t-1,u1:t-2)

對機(jī)器人位置的求解，等價(jià)于求解粒子的聯(lián)合后驗(yàn)概率，該聯(lián)合后驗(yàn)概率可以分解為激光測距數(shù)據(jù)對已知地圖觀測量的后驗(yàn)概率和里程計(jì)在已知地圖情況下的后驗(yàn)概率之積，在已知地圖的情況下，求解機(jī)器人位置的遞歸公式為

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

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

(13)

式中x為機(jī)器人的運(yùn)動(dòng)軌跡x1:t=x1,…,xt;η為歸一化因子;m為地圖數(shù)據(jù);z為觀測信息，里程數(shù)據(jù)u1:t-1=u1,…,ut-1，觀測數(shù)據(jù)z1:t=z1,…,zt。

粒子濾波算法1)輸入:粒子集St-1,控制測量變量ut-1,觀測量zt-12)參數(shù)初始化:S't=?,重采樣函數(shù)初始化,觀測量函數(shù)初始化,狀態(tài)方程初始化3)fori=1toNdo4)draws^s～π(st|s[i]t-1,d)5)^w=η[p(^s|s[i]t-1,d)][π(s|s[i]t-1,d)]-16)S't=S't+<^s,^w>7)End8)St=?9)Forj=1toNdo10)從粒子序列S't獲取樣本粒子s[i]t,s[i]t的取用依據(jù)相關(guān)權(quán)重系數(shù)w[i]t的取值11)St=St+12)End13)returnSt

2 改進(jìn)的粒子濾波定位方法

機(jī)器人位置的定位與機(jī)器人的姿態(tài)和傳感器的測量數(shù)據(jù)高度關(guān)聯(lián)，蒙特卡洛定位的思想是通過遞歸的方式以當(dāng)前接收到的數(shù)據(jù)為條件，對機(jī)器人狀態(tài)變量x進(jìn)行估計(jì)，狀態(tài)變量x在笛卡爾坐標(biāo)系下表示為(x,y,θ)，相關(guān)數(shù)據(jù)分為兩類：環(huán)境感知信息z和感知信息和機(jī)器人姿態(tài)更新信息u。移動(dòng)機(jī)器人在導(dǎo)航過程中受到激光傳感器自身的非線性和車體顛簸的干擾影響會造成其自身測量結(jié)果不準(zhǔn)確，從而造成觀測信息的誤差；受到里程計(jì)、角速度陀螺儀積分存在誤差的影響，也會造成里程及信息存在誤差。因此,在改進(jìn)的粒子濾波定位方法中，引入兩類數(shù)據(jù)的誤差參數(shù)，減小兩類誤差對系統(tǒng)運(yùn)算的影響。

2.1 改進(jìn)粒子采樣算法

對粒子濾波定位中的粒子采樣，分3步驟實(shí)現(xiàn)：

(14)

(15)

式中Q為前文推導(dǎo)出的激光測距儀噪聲的協(xié)方差矩陣。

(16)

(17)

3)通過上述結(jié)果得出的新的粒子樣本的分布

(18)

(19)

2.2 改進(jìn)卡爾曼更新步驟

采用改進(jìn)的優(yōu)化方法[11]:

Fori=2 toNdo

else

End if

改進(jìn)的卡爾曼濾波方式對粒子進(jìn)行重采樣，對權(quán)值高的粒子進(jìn)行卡爾曼濾波，只需要對K(K

2.3 粒子濾波定位算法流程設(shè)計(jì)

改進(jìn)算法流程如圖3所示。

圖3 算法流程設(shè)計(jì)

3 改進(jìn)粒子濾波定位算法實(shí)現(xiàn)

實(shí)驗(yàn)平臺為裝載有SICK LMS2xx系列的激光測距機(jī)器人，該機(jī)器人由2個(gè)驅(qū)動(dòng)輪和1個(gè)被動(dòng)輪組成，安裝有里程計(jì)，可以通過多通道的無線遙控器對機(jī)器人的行走進(jìn)行控制，機(jī)器人本身配備了裝有ROS的筆記本電腦，如圖4所示。

圖4 實(shí)驗(yàn)機(jī)器人平臺

建立機(jī)器人自主移動(dòng)環(huán)境，旅行家機(jī)器人在已知室內(nèi)環(huán)境中做連續(xù)繞圈實(shí)驗(yàn)，由于實(shí)驗(yàn)環(huán)境的確定性，可以更好地檢驗(yàn)算法的計(jì)算效率。實(shí)驗(yàn)環(huán)境為9 m×9 m實(shí)驗(yàn)室，將已知地圖數(shù)據(jù)化，形成已知路標(biāo)，建立全局坐標(biāo)系，如圖5所示。

圖5 實(shí)驗(yàn)環(huán)境數(shù)據(jù)化

根據(jù)SLAM柵格地圖構(gòu)建原理，將實(shí)驗(yàn)環(huán)境數(shù)據(jù)化，定義室內(nèi)環(huán)境全局坐標(biāo)系，機(jī)器人移動(dòng)速度0.5 m/s，記錄機(jī)器人在室內(nèi)的實(shí)際運(yùn)動(dòng)路徑，與算法估算的路徑進(jìn)行比對，結(jié)果如圖6所示。

圖6 粒子分布

選取特征節(jié)點(diǎn)位置，以觀測機(jī)器人定位性能，實(shí)驗(yàn)結(jié)果表明：改進(jìn)前、后算法所需粒子數(shù)目為300，200個(gè)，運(yùn)行時(shí)間為5.362，4.264 s。本文提出的改進(jìn)方法能有效減少粒子濾波器的數(shù)量，提高SLAM算法的實(shí)時(shí)性。

4 結(jié) 論

對在已知地圖環(huán)境下的粒子濾波定位技術(shù)進(jìn)行了研究，對于在未知環(huán)境下的柵格地圖構(gòu)建和軌跡規(guī)劃的研究比較薄弱，有待進(jìn)一步研究。同時(shí)，加強(qiáng)自主行為機(jī)器人的多傳感器數(shù)據(jù)融合、優(yōu)化數(shù)據(jù)關(guān)聯(lián)與回環(huán)檢測、與前端機(jī)構(gòu)處理器集成、提升魯棒性和重定位精度，均為自主行為機(jī)器人SLAM技術(shù)接下來的發(fā)展方向，也是從單一機(jī)器人自主行為逐步向?yàn)槎鄼C(jī)器人協(xié)同工作發(fā)展的必要條件。

[1] 張 文,劉 勇,張超凡,等.基于語義建圖的室內(nèi)機(jī)器人實(shí)時(shí)場景分類[J].傳感器與微系統(tǒng),2017,36(8):18-21，28.

[2] 馬潤澤,余志軍,劉海濤.一種距離無關(guān)的無線傳感器網(wǎng)絡(luò)定位算法[J].傳感器與微系統(tǒng),2011,30(11):131-134.

[3] 宋凱楠,王 彪,唐超穎.基于激光測距掃描的室內(nèi)定位方法研究[J].激光與紅外,2016,46(8):938-942.

[4] 任春明,張建勛.基于激光傳感器的機(jī)器人地圖構(gòu)建方法[J].光電工程,2008(8):83-87.

[5] 張 毅,程鐵鳳,羅 元,等.魯棒的機(jī)器人粒子濾波即時(shí)定位與地圖構(gòu)建的實(shí)現(xiàn)[J].計(jì)算機(jī)應(yīng)用研究,2015,32(8):2339-2342.

[6] 劉洞波.移動(dòng)機(jī)器人粒子濾波定位與地圖創(chuàng)建方法研究[D].長沙：湖南大學(xué),2013.

[7] 劉金華.基于粒子濾波單站無源定位跟蹤技術(shù)研究[D].成都：電子科技大學(xué),2008.

[8] 聶建亮,楊元喜,吳富梅.一種基于改進(jìn)粒子濾波的動(dòng)態(tài)精密單點(diǎn)定位算法[J].測繪學(xué)報(bào),2010,39(4):338-343.

[9] 張 弦.基于改進(jìn)粒子濾波算法的移動(dòng)機(jī)器人定位[D].北京：北京郵電大學(xué),2010.

[10] 涂剛毅,金世俊,祝雪芬,等.基于粒子濾波的移動(dòng)機(jī)器人SLAM算法[J].東南大學(xué)學(xué)報(bào)：自然科學(xué)版,2010,40(1):117-122.

[11] 羅 元,余佳航,汪龍峰,等.改進(jìn)RBPF的移動(dòng)機(jī)器人同步定位與地圖構(gòu)建[J].智能系統(tǒng)學(xué)報(bào),2015,10(3):460-464.

Researchonrobotparticlefilteringlocalizationtechnologybasedonlaserscanningranging*

GE Guang-shuang， LI Zi-long， YANG Kai， MA Rui-xin

(TianjinResearchInstituteofWaterTransportEngineering,MinistryofTransportation,Tianjin300000,China)

The traditional particle filtering simultaneous localization and mapping(SLAM) algorithm in the construction of mapping and autonomous positioning,usually need large number of particles and take up a lot of memory,particles appear deprivation after the resampling process,in order to improve the efficiency of robot autonomous positioning,aiming at particle filtering algorithm,propose an improved resampling strategy and particle update strategy,fuse system model.By testing the algorithm on a mobile robot equipped with robot operating system (ROS),the algorithm is validated by experimental method.The experimental results show that the method can improve the efficiency of particle filtering positioning.

simultaneous localization and mapping(SLAM); particle filtering(PF); Monte Carlo positioning algorithm; robot operating system(ROS)

10.13873/J.1000—9787(2017)12—0036—04

TP 242.6

A

1000—9787(2017)12—0036—04

2017—10—11

中央級公益性科研院所基本科研業(yè)務(wù)費(fèi)項(xiàng)目(TKS160212)

戈廣雙(1989-)，碩士，主要研究方向?yàn)闄C(jī)器人智能控制技術(shù)。李子龍(1987-)，通訊作者，博士，主要從事工程機(jī)械自動(dòng)化與工程機(jī)械無人駕駛技術(shù)研究，E—mail:zilonglibaifoshan@163.com。