葉泳駿，陳新度，吳 磊,刁世普,2

(1. 廣東工業(yè)大學(xué) a.廣東省計(jì)算機(jī)集成制造重點(diǎn)實(shí)驗(yàn)室，廣州 510006; b.省部共建精密電子制造技術(shù)與裝備國(guó)家重點(diǎn)實(shí)驗(yàn)室，廣州 510006; 2. 廣東技術(shù)師范大學(xué)機(jī)電學(xué)院，廣州 510665)

0 引言

基于SLAM導(dǎo)航[1-2]的自主移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)相較于傳統(tǒng)的磁導(dǎo)航[3-4]，二維碼導(dǎo)航[5]，反射板導(dǎo)航[6]等方式，擁有著無(wú)需改造導(dǎo)航環(huán)境，可自主規(guī)劃路徑等優(yōu)勢(shì)，已經(jīng)廣泛應(yīng)用于多種室內(nèi)移動(dòng)機(jī)器人中。當(dāng)前學(xué)者們已經(jīng)從地圖構(gòu)建，定位，感知，路徑規(guī)劃與運(yùn)動(dòng)控制五大基礎(chǔ)模塊開(kāi)展了大量的工作，來(lái)賦能移動(dòng)機(jī)器人自主化，智能化。

眾所周知，定位是機(jī)器人導(dǎo)航的最核心模塊，其他模塊需基于定位準(zhǔn)確的條件來(lái)保證機(jī)器人穩(wěn)定工作，而SLAM導(dǎo)航是利用外部傳感器信息與先驗(yàn)地圖數(shù)據(jù)關(guān)聯(lián)和內(nèi)部傳感器信息自身估計(jì)，來(lái)實(shí)現(xiàn)機(jī)器人精確定位。因此為了提高自主移動(dòng)機(jī)器人SLAM導(dǎo)航定位性能，開(kāi)展地圖與定位相關(guān)聯(lián)的工作是十分有必要的。文獻(xiàn)[7]提出了一種基于ORB[8]特征點(diǎn)的SLAM系統(tǒng)，利用相機(jī)采集圖像來(lái)構(gòu)建視覺(jué)特征點(diǎn)地圖，從而實(shí)現(xiàn)重定位與連續(xù)位姿估計(jì)，但是該算法生成的地圖不保存障礙信息，不能應(yīng)用于機(jī)器人路徑規(guī)劃與避障當(dāng)中。文獻(xiàn)[9]提出了基于圖優(yōu)化的激光SLAM系統(tǒng)，構(gòu)建出便于機(jī)器人路徑規(guī)劃的激光柵格地圖，利用激光等傳感器的數(shù)據(jù)關(guān)聯(lián)實(shí)現(xiàn)定位，但是由于單線激光信息并不那么豐富，從而導(dǎo)致重定位效果不佳。文獻(xiàn)[10]提出了一種基于ROS的包含建圖、定位、路徑規(guī)劃的機(jī)器人導(dǎo)航框架，是少有綜合多模塊的開(kāi)源工作，為機(jī)器人落地提供了許多實(shí)用的設(shè)計(jì)。

基于前人的研究基礎(chǔ)上，本文提出了一種結(jié)合視覺(jué)與陀螺儀的室內(nèi)移動(dòng)機(jī)器人改進(jìn)粒子濾波定位方法。該定位方法是在ROS導(dǎo)航框架[11]AMCL定位模塊[12]的基礎(chǔ)上實(shí)現(xiàn)，并改進(jìn)其初始化定位效果不穩(wěn)定與過(guò)程定位效果的不足。本文基于視覺(jué)與陀螺儀組合，通過(guò)利用數(shù)據(jù)關(guān)聯(lián)與多傳感器融合技術(shù)，提升定位效果，為機(jī)器人配送、運(yùn)輸?shù)葓?chǎng)景提供更穩(wěn)定更準(zhǔn)確的技術(shù)方案。

1 問(wèn)題描述

AMCL定位模塊實(shí)際應(yīng)用中發(fā)現(xiàn)存在初始化與過(guò)程定位方面的問(wèn)題。一方面是在室內(nèi)和需要滿足自主的條件下，靠模塊內(nèi)置的GPS或人為指定初始位置的方法是不理想的，而讓粒子布滿全圖再慢慢收斂的內(nèi)置方法又會(huì)因單幀激光信息不豐富導(dǎo)致很大可能會(huì)導(dǎo)致錯(cuò)誤定位，如圖1a情況。一方面是由于輪速計(jì)存在較大累積誤差，原模塊僅靠輪速計(jì)進(jìn)行粒子群預(yù)測(cè)過(guò)程定位質(zhì)量欠佳。在機(jī)器人導(dǎo)航過(guò)程中，特別是當(dāng)機(jī)器人旋轉(zhuǎn)時(shí)，會(huì)出現(xiàn)當(dāng)前激光數(shù)據(jù)與地圖存在重合度較差，定位質(zhì)量較低的情況，如圖1b情況。

(a)初始化定位問(wèn)題 (b) 過(guò)程定位問(wèn)題

2 改進(jìn)的定位方法

ROS導(dǎo)航框架的定位模塊AMCL，是一種自適應(yīng)的粒子濾波方法，其一般使用輪速計(jì)與激光雷達(dá)兩個(gè)傳感器數(shù)據(jù)以及先驗(yàn)地圖信息進(jìn)行機(jī)器人的位姿估計(jì)。本文針對(duì)模塊中的不足，在原方法基礎(chǔ)上進(jìn)行改進(jìn)，如圖2所示，其中黑色實(shí)線框部分為原方法概況，淺色虛線框部分為改進(jìn)部分。對(duì)于模塊中初始定位與過(guò)程定位方面的問(wèn)題，本文提出對(duì)應(yīng)的改進(jìn)措施。一方面是通過(guò)獲取視覺(jué)地圖坐標(biāo)，并地圖坐標(biāo)轉(zhuǎn)換，從而提高初始化定位穩(wěn)定性，另一方面是通過(guò)對(duì)視覺(jué)里程計(jì)、陀螺儀、輪速計(jì)分別得到的位姿估計(jì)進(jìn)行卡爾曼濾波融合，從而提高過(guò)程定位質(zhì)量。

圖2 AMCL定位模塊概況與改進(jìn)部分

2.1 基于數(shù)據(jù)關(guān)聯(lián)的初始化定位

2.1.1 獲取視覺(jué)地圖坐標(biāo)

獲取視覺(jué)地圖坐標(biāo)主要由詞袋模型[13]與PNP(Pespective-N-Point)兩個(gè)部分組成。定位系統(tǒng)通過(guò)相機(jī)最新獲取的當(dāng)前圖像幀，根據(jù)詞袋模型與地圖中的關(guān)鍵幀進(jìn)行圖像匹配，得到當(dāng)前幀特征點(diǎn)與三維地圖點(diǎn)的對(duì)應(yīng)關(guān)系。然后再通過(guò)PNP恢復(fù)出當(dāng)前相機(jī)相對(duì)于視覺(jué)地圖的變換關(guān)系，從而獲取得到視覺(jué)地圖坐標(biāo)。

圖像匹配中利用詞袋模型分為兩個(gè)步驟。第一是產(chǎn)生一個(gè)詞袋，通過(guò)對(duì)大量的圖片提取特征點(diǎn)，并對(duì)這些特征點(diǎn)進(jìn)行k叉樹(shù)聚類，得到的葉子節(jié)點(diǎn)則為每一個(gè)詞。第二是使用詞袋模型，圖片可以通過(guò)詞袋模型來(lái)進(jìn)行編碼，得到詞向量，作為進(jìn)行圖片相似度與快速匹配的依據(jù)。詞向量是關(guān)于詞索引與其權(quán)重的向量，可以通過(guò)式(1)來(lái)判斷兩圖像詞向量v1與v2的相似度，得分越大則越相似。

(1)

當(dāng)獲取當(dāng)前幀特征點(diǎn)與地圖點(diǎn)的匹配關(guān)系，則可以根據(jù)2D-3D點(diǎn)的對(duì)應(yīng)關(guān)系使用PNP算法來(lái)得到當(dāng)前幀相機(jī)的位姿。先假設(shè)當(dāng)前相機(jī)位姿李代數(shù)ξ與最相似圖片的位姿一樣，又已知相機(jī)內(nèi)參K，地圖點(diǎn)尺度si，地圖點(diǎn)三維坐標(biāo)Pi，對(duì)應(yīng)的二維點(diǎn)像素坐標(biāo)ui，通過(guò)把地圖點(diǎn)投影到當(dāng)前幀上，利用彼此重投影誤差的最小二乘形式來(lái)構(gòu)建代價(jià)函數(shù),如式(2)，通過(guò)優(yōu)化得到最優(yōu)相機(jī)位姿。

(2)

2.1.2 地圖坐標(biāo)轉(zhuǎn)換

如圖2所示，改進(jìn)方法采用了從ORBSLAM2[7]與Cartographer[9]得到的兩個(gè)地圖，即視覺(jué)的特征點(diǎn)地圖[14]與激光的灰度柵格地圖[15]，且兩個(gè)地圖為獨(dú)立構(gòu)建，后續(xù)需要采樣擬合求取彼此變換關(guān)系，通過(guò)把獲取得到的視覺(jué)地圖坐標(biāo)變換至機(jī)器人常用的灰度柵格地圖中，方可使用。變換關(guān)系見(jiàn)式(3)：

GP=GTFFP

(3)

其中，GP為灰度柵格地圖中的齊次坐標(biāo)，F(xiàn)P為特征點(diǎn)地圖上的齊次坐標(biāo)，GTF為從特征點(diǎn)地圖到灰度柵格地圖的變換矩陣。

通過(guò)讓機(jī)器人運(yùn)動(dòng)至均勻散布在活動(dòng)范圍的多個(gè)位置，根據(jù)第i位置從兩SLAM系統(tǒng)得到的GPi與FPi是指代同一物理位置，兩者之間差GTF變換的原因，可利用兩組三維點(diǎn)對(duì)應(yīng)關(guān)系計(jì)算式(4)～式(8)，從而得到GTF。

(4)

(5)

(6)

(7)

(8)

2.2 基于多傳感器融合的過(guò)程定位

2.2.1 輪速計(jì)與陀螺儀運(yùn)動(dòng)估計(jì)

視覺(jué)里程計(jì)、陀螺儀、輪速計(jì)，皆可估計(jì)機(jī)器人的運(yùn)動(dòng)狀態(tài)，得到機(jī)器人位姿變換。視覺(jué)里程計(jì)得到位姿的方法原理與前述一樣。而輪速計(jì)與機(jī)器人運(yùn)動(dòng)機(jī)構(gòu)相關(guān)，本文采用的是差速輪，即僅利用左右輪獨(dú)立驅(qū)動(dòng)實(shí)現(xiàn)前行與旋轉(zhuǎn)的運(yùn)動(dòng)機(jī)構(gòu)。在采樣時(shí)間間隔Δt中，從兩輪編碼器中經(jīng)比率換算得到的左右輪走過(guò)的距離dl與dr，已知L為左右輪距，(xk,yk,θk)為k時(shí)刻下機(jī)器人的運(yùn)動(dòng)狀態(tài)，輪速計(jì)模型根據(jù)式(9)～式(11)求出(xk+1,yk+1,θk+1)的機(jī)器人狀態(tài)。

(9)

(10)

(11)

(12)

(13)

2.2.2 卡爾曼濾波

對(duì)于上述各種的機(jī)器人的運(yùn)動(dòng)狀態(tài)預(yù)測(cè)，都有各自的位姿估計(jì)，不過(guò)根據(jù)各個(gè)傳感器的特性與數(shù)據(jù)獲取方式，皆有對(duì)應(yīng)的優(yōu)劣，針對(duì)這些優(yōu)劣，采用卡爾曼濾波數(shù)據(jù)融合的方法根據(jù)各數(shù)據(jù)的協(xié)方差對(duì)應(yīng)加權(quán)，得到優(yōu)勢(shì)互補(bǔ)的最優(yōu)位姿估計(jì)。

(14)

(15)

(16)

(17)

(18)

本文根據(jù)上述公式，通過(guò)協(xié)方差來(lái)加權(quán)得到較好的估計(jì)位姿。在位置x,y估計(jì)方面，視覺(jué)里程計(jì)與輪速計(jì)估計(jì)協(xié)方差設(shè)置一樣，即結(jié)果各取一半。在姿態(tài)θ方面，把陀螺儀估計(jì)協(xié)方差設(shè)置很小，輪速計(jì)與視覺(jué)里程計(jì)估計(jì)協(xié)方差設(shè)置較大，即結(jié)果主要取自陀螺儀。

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

3.1 實(shí)驗(yàn)準(zhǔn)備

本文實(shí)驗(yàn)使用的平臺(tái)如圖3a所示，包括一個(gè)差速輪自制底盤(pán)，一個(gè)SCIK的tim-561型號(hào)單線激光雷達(dá)，一個(gè)小覓智能的S1040型號(hào)雙目相機(jī)，一個(gè)ALUBI的IMLPMS-ME1型號(hào)IMU和一臺(tái)CPU為i7-9750H的戴爾筆記本。實(shí)驗(yàn)場(chǎng)地為一個(gè)回字形的室內(nèi)辦公場(chǎng)地，如圖3b所示，在此場(chǎng)地進(jìn)行關(guān)于初始化定位，過(guò)程定位，到點(diǎn)定位的3個(gè)實(shí)驗(yàn)。初始化實(shí)驗(yàn)則在該場(chǎng)地中機(jī)器人運(yùn)動(dòng)范圍，即紅色箭頭線所示位置附近范圍，對(duì)比測(cè)試兩種方法的初始化定位平均成功率與平均定位時(shí)間。過(guò)程定位實(shí)驗(yàn)則是讓機(jī)器人運(yùn)動(dòng)如圖3b中紅色箭頭線所示，進(jìn)行兩種方法的定位質(zhì)量比較。到點(diǎn)定位實(shí)驗(yàn)則是從系統(tǒng)最終到點(diǎn)定位精度來(lái)整體衡量改進(jìn)效果。

(a) 實(shí)驗(yàn)平臺(tái) (b) 實(shí)驗(yàn)場(chǎng)地

3.2 初始化定位

該實(shí)驗(yàn)讓機(jī)器人在活動(dòng)范圍均勻分布的位置進(jìn)行初始化定位，對(duì)于改進(jìn)方法則是讓其當(dāng)前幀與視覺(jué)特征點(diǎn)地圖進(jìn)行數(shù)據(jù)關(guān)聯(lián)獲取全局位置，對(duì)于原方法則是讓AMCL中的粒子全局分布，經(jīng)過(guò)旋轉(zhuǎn)與激光采樣對(duì)粒子收斂，獲取全局位置。經(jīng)過(guò)各40組實(shí)驗(yàn)測(cè)試，結(jié)果見(jiàn)表1，新系統(tǒng)的成功得到準(zhǔn)確的初始化定位次數(shù)為38次，失敗2次，成功率為95%，平均定位時(shí)間為2.67 s；舊系統(tǒng)成功得到準(zhǔn)確的初始化定位次數(shù)為17次，失敗23次，成功率為42.5%，平均定位時(shí)間為16.68 s。改進(jìn)方法失敗主要由于早晚靠窗處的光照強(qiáng)度差異過(guò)大，導(dǎo)致視覺(jué)特征點(diǎn)地圖中的特征與當(dāng)前幀的特征關(guān)聯(lián)不上從而導(dǎo)致失敗。

表1 初始化定位

3.3 過(guò)程定位

按照既定軌跡改進(jìn)方法與原方法的定位質(zhì)量對(duì)比如圖4所示。

圖4 過(guò)程定位質(zhì)量圖

該定位質(zhì)量是單幀激光通過(guò)定位估計(jì)通過(guò)變換，看其激光點(diǎn)對(duì)于地圖的重合度百分比來(lái)定義的。由數(shù)據(jù)可見(jiàn)，添加了視覺(jué)與陀螺儀對(duì)系統(tǒng)的定位效果有了明顯的提高，而在40～70 s的區(qū)間內(nèi)新系統(tǒng)的定位質(zhì)量出現(xiàn)下降趨勢(shì)，是因?yàn)樵撀范未嬖谳^多椅子與部分可移動(dòng)的障礙物，在地圖構(gòu)建到測(cè)試的時(shí)間間隔中被挪動(dòng)，從而出現(xiàn)定位質(zhì)量效果下降，但整體仍然比原方法的效果要好。

3.4 到點(diǎn)定位

為了觀察機(jī)器人系統(tǒng)整體的定位效果，以機(jī)器人重復(fù)到點(diǎn)定位精度作為指標(biāo)對(duì)比改進(jìn)前后的精度提升情況。實(shí)驗(yàn)過(guò)程先讓機(jī)器人記錄下某一指定地點(diǎn)，根據(jù)機(jī)器人底盤(pán)的鋁合金板后側(cè)與右側(cè)作為標(biāo)準(zhǔn)，把貼紙貼在地面，以此作為標(biāo)記，記作位置0，后續(xù)讓機(jī)器人規(guī)劃離開(kāi)，再重新到該點(diǎn)，以同樣方法作標(biāo)記，兩個(gè)系統(tǒng)各重復(fù)3組，一組10次，測(cè)量其x,y,θ上的誤差，并統(tǒng)計(jì)其平均誤差。

實(shí)驗(yàn)數(shù)據(jù)與改進(jìn)方法實(shí)驗(yàn)效果見(jiàn)表2與圖5。表2可見(jiàn)到點(diǎn)定位精度有一定幅度的提升，位置平均誤差提高了10 mm～30 mm不等，角度平均誤差提高1°左右。圖5中改進(jìn)方法試驗(yàn)效果中可見(jiàn)機(jī)器人到點(diǎn)定位的分布比較穩(wěn)定，效果也不錯(cuò)。重復(fù)到點(diǎn)定位精度的提升反映出在初始化定位與過(guò)程定位的提升對(duì)系統(tǒng)整體的定位效果有一定幫助。

表2 到點(diǎn)定位精度對(duì)比

圖5 第二組改進(jìn)方法到點(diǎn)定位效果

4 結(jié)束語(yǔ)

本文針對(duì)現(xiàn)有AMCL模塊的初始化定位穩(wěn)定性以及過(guò)程定位精度的不足，提出了基于視覺(jué)與陀螺儀組合的移動(dòng)機(jī)器人改進(jìn)定位方法。該方法基于視覺(jué)數(shù)據(jù)關(guān)聯(lián)，以提高初始化定位穩(wěn)定性；基于卡爾曼濾波的傳感器數(shù)據(jù)融合方法，為粒子群提供了更好的預(yù)測(cè)，以提高過(guò)程定位精度。實(shí)驗(yàn)結(jié)果表明，所提出的改進(jìn)定位方法相較于已有方法，在定位穩(wěn)定性和定位精度方面都有一定提升。因此，本文提出的移動(dòng)機(jī)器人改進(jìn)定位方法，在高精度魯棒定位方面有良好的借鑒價(jià)值。