欒佳寧,張 偉*,孫 偉,張 奧,韓 冬
(1.同濟(jì)大學(xué)電子與信息工程學(xué)院,上海 201804;2.西安電子科技大學(xué)空間科學(xué)與技術(shù)學(xué)院,西安 710126)
(*通信作者電子郵箱zhang_wi@#edu.cn)
在一個(gè)已知環(huán)境中,移動(dòng)機(jī)器人為在環(huán)境中進(jìn)行移動(dòng),需要借助自身的環(huán)境感知能力來(lái)計(jì)算出自己在環(huán)境中的位姿,這個(gè)問(wèn)題被稱為機(jī)器人的定位問(wèn)題。對(duì)于以倉(cāng)儲(chǔ)機(jī)器人為代表的室內(nèi)移動(dòng)機(jī)器人而言,其對(duì)于定位算法的要求集中在高精度、低成本、高可靠性。由于其工作環(huán)境較為固定,可以提前構(gòu)建高精度地圖且通過(guò)人為構(gòu)建特征的方式提升機(jī)器人的定位精度。
最近幾年以O(shè)RB-SLAM[1]為代表的一系列視覺(jué)即時(shí)定位與建圖(Simultaneous Localization And Mapping,SLAM)算法以及包括SVO(Semi-direct monocular Visual Odometry)[2]在內(nèi)的視覺(jué)里程計(jì)算法都有了很大的發(fā)展,但是由于僅使用一個(gè)相機(jī)無(wú)法有效對(duì)物體的深度進(jìn)行估計(jì)[3],所以視覺(jué)SLAM算法往往需要使用雙目相機(jī)系統(tǒng)或者是RGB-D 相機(jī),在僅擁有單目相機(jī)的機(jī)器人系統(tǒng)上無(wú)法成功應(yīng)用。同時(shí),也有學(xué)者通過(guò)學(xué)習(xí)的方法來(lái)解決定位問(wèn)題,文獻(xiàn)[4]中作者在學(xué)習(xí)階段提取環(huán)境特征點(diǎn)并進(jìn)行降維存儲(chǔ),并在后續(xù)利用匹配的方式計(jì)算自身的位姿;但是,相對(duì)于較為成熟的激光SLAM 算法,純視覺(jué)SLAM 算法容易因?yàn)橥饨绻饩€變化或是特征稀疏等問(wèn)題導(dǎo)致跟蹤失敗。
在已有高精度地圖的情況下,目前使用較多的是基于蒙特卡羅的定位算法。該算法最早由Dellaert 等[5]提出,其利用自身的運(yùn)動(dòng)模型進(jìn)行采樣,并利用激光傳感器的數(shù)據(jù)評(píng)估每個(gè)粒子,并將得分最高粒子的位姿作為自身的最終位姿。大量研究人員在該算法基礎(chǔ)上進(jìn)行了改進(jìn),例如ROS(Robot Operation System)中搭載了自適應(yīng)蒙特卡羅定位(Adaptive Monte Carlo Localization,AMCL)算法[6],使用自適應(yīng)KL 散度(Kullback-Leibler Divergence,KLD)[7]來(lái)更新粒子,可有效降低總體采樣粒子數(shù),也讓機(jī)器人面對(duì)綁架問(wèn)題具有更好的魯棒性,并在業(yè)界得到了廣泛使用。厲茂海等[8]通過(guò)減少采樣粒子數(shù)量來(lái)優(yōu)化采樣過(guò)程,提出了以混合高斯模型為基礎(chǔ)的蒙特卡羅算法,進(jìn)一步降低了計(jì)算量。在這些算法中,由于其先驗(yàn)傳感器信息只有里程計(jì)和激光雷達(dá),且無(wú)有效算法對(duì)機(jī)器人的打滑或漂移等現(xiàn)象進(jìn)行預(yù)測(cè)或限制,因此機(jī)器人在出現(xiàn)上述情況時(shí)定位會(huì)產(chǎn)生較大偏移,甚至可能導(dǎo)致算法開(kāi)始全局重定位,另一方面由于缺乏全局的環(huán)境信息,所以全局定位時(shí)有可能收斂至全局最優(yōu)點(diǎn)且很難自恢復(fù)。
此外,在AMCL 算法的自適應(yīng)調(diào)整機(jī)制執(zhí)行中,自適應(yīng)機(jī)制通過(guò)計(jì)算粒子的平均分?jǐn)?shù)來(lái)判斷機(jī)器人是否遇到綁架問(wèn)題,并利用全局隨機(jī)播撒粒子的方式試圖解決此問(wèn)題。但在實(shí)際執(zhí)行時(shí),算法對(duì)于機(jī)器人是否被綁架的判斷常常失準(zhǔn),很容易因?yàn)橐恍┩饨绺蓴_或是機(jī)器人打滑等問(wèn)題導(dǎo)致整體分?jǐn)?shù)突然降低;同時(shí)該算法收斂較慢,且容易收斂至局部最優(yōu)點(diǎn),導(dǎo)致定位出現(xiàn)差錯(cuò),例如在完全對(duì)稱的環(huán)境中可能出現(xiàn)定位翻轉(zhuǎn)。
除基于粒子濾波的算法之外,以Cartographer[9]為代表的基于優(yōu)化的激光SLAM 算法也可以在擁有高精度地圖的情況下完成定位任務(wù),Cartographer 引入子圖這一概念,在前端完成當(dāng)前幀與子圖的匹配,并在后端完成回環(huán)檢測(cè),以進(jìn)行位姿的優(yōu)化。但是在已經(jīng)完成建圖的情況下,利用Cartographer 定位仍存在著某些問(wèn)題,其中最主要的便是隨著地圖的增大,Cartographer 在進(jìn)行回環(huán)檢測(cè)時(shí)消耗的算力會(huì)大幅上漲,一些研究者將回環(huán)檢測(cè)的搜索任務(wù)分配給GPU(Graphics Processing Unit)進(jìn)行計(jì)算以減少時(shí)間,但是對(duì)于沒(méi)有搭載GPU 的機(jī)器人而言,利用Cartographer 進(jìn)行高幀率的定位仍存在著地圖大小等環(huán)境因素的限制。
為實(shí)現(xiàn)低計(jì)算量的高幀率定位,研究人員也嘗試通過(guò)人工構(gòu)建特征的方式(如利用二維碼或其他路標(biāo))來(lái)完成定位任務(wù)[10-13]。Nazemzadeh 等[14]使 用QR 碼(Quick Response Code)和里程計(jì)做融合,提出了一種基于擴(kuò)展H∞濾波器的定位算法,Zhang 等[15]提出了一種利用天花板上的二維碼進(jìn)行定位,利用激光雷達(dá)進(jìn)行避障的定位算法。這些算法利用二維碼信息進(jìn)行定位,有效消除了里程計(jì)的累積誤差,但是為保證定位的準(zhǔn)確度,這些算法都需在環(huán)境中布置大量的二維碼,同時(shí)也需要機(jī)器人按照一定的路徑運(yùn)動(dòng)。
在此基礎(chǔ)上,胡章芳等[16]提出了融入二維碼信息的自適應(yīng)蒙特卡羅定位算法,利用二維碼信息提供的絕對(duì)位置信息對(duì)AMCL 算法進(jìn)行初始化和矯正,使得機(jī)器人能夠在一定程度上脫離二維碼進(jìn)行定位,但是仍對(duì)二維碼的布置和機(jī)器人的運(yùn)動(dòng)路徑有著一定的限制,且定位精度不高。
上述算法引入了二維碼信息輔助定位,有效消除了里程計(jì)的累積誤差,但是由于二維碼均布置在地面或天花板上,易磨損玷污、部署復(fù)雜,需要經(jīng)常維護(hù),同時(shí)需要極大數(shù)量的二維碼才能保證整體環(huán)境內(nèi)的定位精度,且由于該算法直接將二維碼的識(shí)別結(jié)果送入算法進(jìn)行位姿解算,對(duì)二維碼識(shí)別錯(cuò)誤等有可能出現(xiàn)的問(wèn)題缺乏處理機(jī)制,算法整體魯棒性不高。
針對(duì)以上問(wèn)題,本文提出了一種融合二維碼視覺(jué)和激光雷達(dá)的定位算法,可將二維碼部署于障礙物表面或墻上,相較于之前的純單目二維碼定位或是二維碼-激光融合算法,本算法對(duì)機(jī)器人的運(yùn)行軌跡無(wú)任何要求,可實(shí)現(xiàn)環(huán)境中的任意運(yùn)動(dòng)且在此同時(shí)環(huán)境中布置二維碼的數(shù)量大大減少;由于引入了二維碼的絕對(duì)位姿信息,所以相對(duì)于AMCL 等雷達(dá)定位算法,算法無(wú)需初始位姿先驗(yàn),也可有效應(yīng)對(duì)機(jī)器人綁架問(wèn)題。系統(tǒng)首先利用二維碼攜帶的絕對(duì)位姿信息進(jìn)行粗定位,并結(jié)合激光雷達(dá)數(shù)據(jù)對(duì)該位姿進(jìn)行優(yōu)化提升位姿精度,同時(shí)引入里程計(jì)信息對(duì)二維碼的位姿進(jìn)行監(jiān)督,防止二維碼識(shí)別出現(xiàn)錯(cuò)誤,同時(shí)保證在未觀測(cè)到二維碼的時(shí)候仍能夠較準(zhǔn)確地定位,適用于解決倉(cāng)儲(chǔ)機(jī)器人為代表的室內(nèi)機(jī)器人的定位問(wèn)題。
為獲取機(jī)器人的準(zhǔn)確位姿,需要解算出機(jī)器人與二維碼之間的相對(duì)位姿,首先利用張正友標(biāo)定法[17]對(duì)相機(jī)進(jìn)行標(biāo)定得到相機(jī)內(nèi)參。然后,當(dāng)視覺(jué)傳感器獲取一幀圖像后,識(shí)別算法會(huì)首先在圖像上尋找二維碼所攜帶的特征點(diǎn),將其解析并利用其作為索引去檢索數(shù)據(jù)庫(kù)以得到當(dāng)前二維碼坐標(biāo)系與地圖坐標(biāo)系之間的變換Rw2t、Tw2t,并可通過(guò)求解PnP(Perspective n Points)問(wèn)題計(jì)算出機(jī)器人基坐標(biāo)系(在此假設(shè)機(jī)器人相機(jī)坐標(biāo)系與基坐標(biāo)系重合)與二維碼坐標(biāo)系間的變換Rt2b、Tt2b。本文采用AprilTag[18]開(kāi)源算法進(jìn)行二維碼的識(shí)別和特征提取后利用文獻(xiàn)[19]提出的線性PnP解算算法進(jìn)行PnP解算,AprilTag 算法具有實(shí)時(shí)性好、準(zhǔn)確率高等特點(diǎn),最后利用式(1)、(2)獲取機(jī)器人基坐標(biāo)系相對(duì)于世界系的變換Rw2b、Tw2b:
相較于傳統(tǒng)二維碼定位算法使用二維碼作為地標(biāo),本文算法將其貼在障礙物和墻上,克服了作為地標(biāo)易磨損玷污的缺點(diǎn)且減少了二維碼在環(huán)境中需求的數(shù)量,同時(shí)在機(jī)器人的運(yùn)行過(guò)程中可能會(huì)有多個(gè)二維碼出現(xiàn)在機(jī)器人的視野中。但與此同時(shí),由于相機(jī)的標(biāo)定存在一定誤差、二維碼布置存在一定誤差且因成本考慮僅使用單目視覺(jué)進(jìn)行識(shí)別,相較于將二維碼布置到地面或是天花板抑或是利用Kinect進(jìn)行輔助的算法[20],得到的變換關(guān)系的準(zhǔn)確度相對(duì)較差。雖然系統(tǒng)并不利用二維碼解算到的位姿直接作為結(jié)果輸出,但仍希望視覺(jué)定位的位姿能盡量準(zhǔn)確,因此本文提出以下算法對(duì)多個(gè)二維碼返回的位姿進(jìn)行融合,提升二維碼返回位姿的準(zhǔn)確度。
1.2.1 異常識(shí)別二維碼去除
設(shè)將位姿映射到世界坐標(biāo)系后,返回位姿集P={P1,P2,…,Pn},定義位姿Pi與真值之間具有誤差ei,記由于噪聲或者標(biāo)定不完全出現(xiàn)的自然誤差構(gòu)成的集合為N,由于二維碼污損或其他問(wèn)題導(dǎo)致的識(shí)別錯(cuò)誤引入的誤差集合為M。異常位姿去除階段不考慮自然誤差,故其目標(biāo)可定義為尋找集合Φ∈[1,n]使:
在絕大多數(shù)情況下噪聲較小,且標(biāo)定誤差不大,所以ei一般較小,即|ei-ej|較小,本文利用這一特點(diǎn)來(lái)切分位姿集,由式(4),實(shí)際計(jì)算時(shí)可利用|pi-pj|來(lái)切分位姿集。設(shè)當(dāng)前位姿真值為Ptrue,則:
利用式(4)的結(jié)論,以位姿Pi為中心點(diǎn),δdis為半徑,δyaw為高,在x-y-yaw空間內(nèi)構(gòu)建置信域Ωi。則若Pj∈Ωi,則可大概率認(rèn)為ei、ej∈N。將該結(jié)論推廣,對(duì)位姿集中所有點(diǎn)按上述方式構(gòu)建置信域,并將所有在置信域中的位姿放入Pi的內(nèi)點(diǎn)集ψi,并定義num(ψi)代表ψi中的元素?cái)?shù)目,則:
1.2.2 多二維碼位姿融合
由于二維碼布置在障礙物的表面,其相對(duì)于機(jī)器人的距離和角度都會(huì)隨著機(jī)器人的運(yùn)動(dòng)發(fā)生巨大的變化,又因?yàn)闄C(jī)器人有時(shí)候會(huì)在很遠(yuǎn)的距離上對(duì)二維碼進(jìn)行觀測(cè),所以會(huì)使機(jī)器人觀測(cè)多二維碼時(shí)存在著一定的重投影誤差。如圖1 所示的情況,機(jī)器人對(duì)4 個(gè)紅點(diǎn)所示位置的二維碼進(jìn)行觀測(cè)時(shí),生成了4 個(gè)相互不重疊的位姿(4 個(gè)紅色小箭頭)。本文在對(duì)誤差進(jìn)行研究時(shí),假設(shè)誤差的主要來(lái)源為相機(jī)標(biāo)定不完全以及存在著觀測(cè)噪聲。
圖1 重投影誤差示意圖Fig.1 Schematic diagram of re-projection error
為了能夠獲得標(biāo)定誤差和實(shí)際投影誤差的關(guān)系,本文利用一種基于幾何特征的PnP 解算算法來(lái)進(jìn)行PnP 計(jì)算[18],該算法針對(duì)呈矩形分布的P4P 問(wèn)題提出了一種線性的求解方式,由于該算法避免了非線性迭代的過(guò)程,所以本文基于該算法進(jìn)行了相機(jī)內(nèi)參誤差和解算位姿誤差間的關(guān)系推導(dǎo)。
如圖2所示,設(shè)q0、q1、q2、q3分別為圖像中二維碼的4個(gè)角點(diǎn),其在像素坐標(biāo)系下的坐標(biāo)分別為(x0,y0)、(x1,y1)、(x2,y2)、(x3,y3),f為相機(jī)的焦距,P0、P1、P2、P3分別為世界坐標(biāo)系中二維碼的角點(diǎn)。
圖2 三維位姿測(cè)量示意圖Fig.2 Schematic diagram of 3D pose measurement
相機(jī)內(nèi)參矩陣和焦距與內(nèi)參矩陣的關(guān)系分別表示為:
點(diǎn)q0的相機(jī)坐標(biāo)系下的坐標(biāo)為:
其他3個(gè)點(diǎn)同理,記它們的坐標(biāo)分別為:(X0,Y0,f)、(X1,Y1,f)、(X2,Y2,f)、(X3,Y3,f)。
對(duì)于由光心Oc和q2、q3組成的平面π1,其法向量N1可表示為:
由于P0P1‖π1,所以可得:
繼而可得:
整理式(13),并定義:
將N1的表達(dá)式代入,可得:
k1同理,所以可得h與fx、fy、cx、cy無(wú)關(guān),即標(biāo)定結(jié)果不對(duì)h的值產(chǎn)生影響。
考慮到世界坐標(biāo)系中|P0P1|=d1,可知:
變換式(16)得到:
求k對(duì)各變量的導(dǎo)數(shù):
由多元函數(shù)的泰勒展開(kāi),在僅保留一階小量時(shí),可得:
同時(shí)可得到位置誤差與標(biāo)定誤差的關(guān)系:
而后,利用式(28)計(jì)算R并將上文解出的誤差關(guān)系代入后可解出ΔR:
在獲得標(biāo)定誤差和位姿誤差的對(duì)應(yīng)關(guān)系后,假設(shè)相機(jī)的標(biāo)定誤差服從隨機(jī)的高斯分布,則利用式(25)~(28)將可得到觀測(cè)結(jié)果的均值和方差。
設(shè)對(duì)第k個(gè)二維碼的觀測(cè)的結(jié)果為:
利用式(32)最大高斯概率的密度函數(shù)歸一化積對(duì)各變量進(jìn)行多高斯分布融合:
式(32)的結(jié)果為多高斯分布融合后的均值,對(duì)各變量分別求均值后利用結(jié)果作為視覺(jué)定位的最終輸出。
在獲得融合二維碼信息得到的位姿后,繼續(xù)利用點(diǎn)云對(duì)準(zhǔn)的方式對(duì)位姿進(jìn)一步優(yōu)化,本文選擇一種基于旋轉(zhuǎn)矩陣求解的點(diǎn)云對(duì)準(zhǔn)算法[21],定義每一個(gè)激光點(diǎn)的誤差為:
其中:pj為激光點(diǎn)云在機(jī)器人基坐標(biāo)系上的坐標(biāo),yj為pj在地圖上歐氏距離最近的障礙物點(diǎn)坐標(biāo),C和r分別為激光雷達(dá)坐標(biāo)系和地圖坐標(biāo)系的旋轉(zhuǎn)矩陣和平移矩陣。
同時(shí)定義點(diǎn)云在雷達(dá)坐標(biāo)系中的質(zhì)心和其在地圖上歐氏距離最近的障礙物點(diǎn)的質(zhì)心分別為:
利用上述定義構(gòu)建優(yōu)化函數(shù):
則求解目標(biāo)為尋找雷達(dá)與地圖之間的旋轉(zhuǎn)變換關(guān)系C及平移變換關(guān)系r使得J最小。
定義平移量d的表達(dá)式為:
將d代入誤差函數(shù),得到:
通過(guò)這種表達(dá)方式,可以分離C和d并分別對(duì)兩項(xiàng)進(jìn)行優(yōu)化。
對(duì)于含有C的各項(xiàng):
式中:
定義如下的代價(jià)函數(shù)對(duì)C進(jìn)行優(yōu)化:
對(duì)變量依次求導(dǎo)得到:
式(43)、(44)分別為旋轉(zhuǎn)矩陣C的約束條件,對(duì)于式(42),令=0,可得:
由于旋轉(zhuǎn)矩陣是一個(gè)正交矩陣,所以可得:
求解出機(jī)器人的最優(yōu)旋轉(zhuǎn)矩陣后,將C代入式(37)中,并令平移量d=0,得到:
利用式(48)、(49)的結(jié)果對(duì)機(jī)器人基坐標(biāo)系的位姿進(jìn)行不斷修正,便可使式(36)的誤差不斷減小,直至達(dá)到最大迭代次數(shù)或是誤差小于一定值后停止迭代。
在實(shí)際系統(tǒng)運(yùn)行過(guò)程中,為保證系統(tǒng)的魯棒性,需要保證在機(jī)器人視野中沒(méi)有二維碼時(shí)仍能夠順利完成定位,故引入了基于里程計(jì)的位姿計(jì)算。首先里程計(jì)數(shù)據(jù)代入式(50)計(jì)算出當(dāng)前時(shí)刻的先驗(yàn)位姿,并利用2.1 節(jié)所述的點(diǎn)云對(duì)準(zhǔn)方式進(jìn)行對(duì)準(zhǔn),相較于AMCL的KLD采樣,該算法在高幀率的情況下具有更高的準(zhǔn)確度以及更少的算力消耗。
實(shí)際應(yīng)用中,由于二維碼污損或是張貼不平整等外部原因會(huì)導(dǎo)致視覺(jué)傳感器返回位姿錯(cuò)誤,繼而可能會(huì)導(dǎo)致定位算法無(wú)法搜索到正確的位姿,因此為了保證系統(tǒng)的魯棒性,本文在這里提出了里程計(jì)-二維碼定位矯正算法。設(shè):Pl(xl,yl,θl)、Pv(xv,yv,θv)分別為上一時(shí)刻機(jī)器人的位姿和優(yōu)化的得到的最優(yōu)位姿。Δx、Δy、Δθ分別為利用機(jī)器人里程計(jì)計(jì)算的位姿差,則機(jī)器人利用運(yùn)動(dòng)模型計(jì)算得到的位姿為:
得到該位姿后,基于連續(xù)性假設(shè),由式(51)、(52)對(duì)機(jī)器人的位姿進(jìn)行檢測(cè),并利用式(53)輸出矯正后的位姿,Podomcal為3.2節(jié)提出的基于里程計(jì)計(jì)算得到的位姿:
在此同時(shí),考慮到機(jī)器人綁架問(wèn)題的存在,若在短時(shí)間內(nèi)多幀圖像均被系統(tǒng)判定為識(shí)別錯(cuò)誤,則有可能是因?yàn)闄C(jī)器人被人為挪動(dòng),此時(shí)算法選取與環(huán)境匹配度更高的位姿作為結(jié)果輸出,定義maxpoint 函數(shù)的輸出為輸入中具有最高分?jǐn)?shù)的位姿,則輸出位姿表示為:
基于以上的各算法,在此給出本文提出的融合算法的算法流程,該算法有效解決了視覺(jué)傳感器和雷達(dá)數(shù)據(jù)的融合問(wèn)題,且具有較好的魯棒性,能夠應(yīng)對(duì)二維碼識(shí)別錯(cuò)誤、沒(méi)有找到二維碼等多種問(wèn)題。
記ξvisal為利用每個(gè)識(shí)別到的二維碼信息計(jì)算的機(jī)器人基坐標(biāo)系在世界坐標(biāo)系下的坐標(biāo),其內(nèi)部可能包含有多個(gè)元素,也有可能不包含任何元素,即視野中可以沒(méi)有任何二維碼,Plast為上一時(shí)刻的位姿,Δx、Δy、Δθ分別為利用機(jī)器人里程計(jì)計(jì)算兩時(shí)刻位姿差,則整體的算法流程如下:
算法1 Pose Enhancement Algorithm。
輸入Plast,ζ,Δx,Δy,Δθ;
輸出Pcurrent。
使用如圖3 所示的室內(nèi)移動(dòng)機(jī)器人系統(tǒng)進(jìn)行實(shí)驗(yàn),搭建了一個(gè)模擬倉(cāng)庫(kù)環(huán)境,場(chǎng)地尺寸為8 540 mm×5 100 mm,共布置32 個(gè)二維碼,均位于障礙物表面,二維碼使用DJI Robomaster S1 系列的二維碼,3D 場(chǎng)地效果圖如圖4 所示。機(jī)器人采用IMU 結(jié)合編碼器生成里程計(jì)數(shù)據(jù);同時(shí)利用DAHENG MERS133-54 攝像頭采集圖像,攝像頭固定在云臺(tái)的yaw軸上。機(jī)器人自身攜帶一個(gè)TX2 處理器作為運(yùn)算單元。
圖3 室內(nèi)移動(dòng)機(jī)器人Fig.3 Indoor mobile robot
圖4 測(cè)試場(chǎng)地示意圖Fig.4 Schematic diagram of test field
為模擬機(jī)器人的真實(shí)運(yùn)動(dòng)場(chǎng)景同時(shí)保證評(píng)判的公平性,由實(shí)驗(yàn)人員手動(dòng)遙控機(jī)器人在場(chǎng)地中進(jìn)行隨機(jī)運(yùn)動(dòng),并在運(yùn)行過(guò)程中包含無(wú)法觀測(cè)到二維碼、只觀測(cè)到一個(gè)二維碼和觀測(cè)到多個(gè)二維碼幾種情況,運(yùn)行軌跡如圖5 所示,將全過(guò)程記錄并分別用視覺(jué)里程計(jì)定位、AMCL 算法和本文算法進(jìn)行定位。
圖5 機(jī)器人的運(yùn)動(dòng)軌跡及二維碼布置Fig.5 Robot’s moving trajectory and two-dimensional code layout
在本次實(shí)驗(yàn)中,因?yàn)閳?chǎng)地缺乏運(yùn)動(dòng)捕捉系統(tǒng),所以無(wú)法獲取機(jī)器人的精確位姿信息,為了能夠最大限度地給出算法的精確度信息,本次實(shí)驗(yàn)并沒(méi)有用建圖算法進(jìn)行建圖,而是使用了場(chǎng)地的高精度地圖,以保證評(píng)價(jià)的準(zhǔn)確性。同時(shí)本文利用以下指標(biāo)來(lái)對(duì)位姿進(jìn)行評(píng)判:
其中yj、ej的定義與式(33)中相同。需要特別說(shuō)明的是,采樣點(diǎn)距離障礙物的平均距離并不等同于機(jī)器人在世界坐標(biāo)系上的誤差,在機(jī)器人姿態(tài)角存在誤差時(shí),遠(yuǎn)處采樣點(diǎn)誤差較大。該指標(biāo)與定位誤差呈正相關(guān)關(guān)系,但不存在絕對(duì)的線性關(guān)系。
3.3.1 算法對(duì)比
為了提升算法的可比較性,機(jī)器人運(yùn)行全程利用ROS 的錄包功能錄制機(jī)器人運(yùn)行全程傳感器返回的數(shù)據(jù),并使用一臺(tái)處理器為i5-6300HQ 的計(jì)算機(jī)分別用4 種算法對(duì)數(shù)據(jù)進(jìn)行處理。
本文共對(duì)四種算法進(jìn)行對(duì)比,參數(shù)與設(shè)置與說(shuō)明如下:
1)二維碼里程計(jì)疊加(TAG-ODOM)算法:將所有利用二維碼計(jì)算得到的位姿做平均后直接使用,若沒(méi)有識(shí)別到二維碼,則在上一時(shí)刻位姿的基礎(chǔ)上疊加利用里程計(jì)數(shù)據(jù)計(jì)算出的兩時(shí)刻位姿差。
2)AMCL 算法:初始位姿已知,最大粒子數(shù)1 000,最小粒子數(shù)50,里程計(jì)的噪聲分別設(shè)置為1.0,1.0,1.0,0.8,0.8。
3)基于二維碼視覺(jué)與激光雷達(dá)融合定位算法(fusion method):點(diǎn)云對(duì)準(zhǔn)算法的最大迭代次數(shù)為50。
4)Cartographer 算法:初始位置已知,輸入里程計(jì)和雷達(dá)的數(shù)據(jù),建圖分辨率為0.05 m。
圖6 為機(jī)器人運(yùn)行全程中點(diǎn)云與地圖的匹配度對(duì)比,可以明顯看出,二維碼視覺(jué)與激光雷達(dá)融合算法在機(jī)器人運(yùn)行的全程表現(xiàn)良好,而AMCL 算法則表現(xiàn)欠佳,總體準(zhǔn)確度與本文算法差距較大,其主要原因在于在于運(yùn)動(dòng)過(guò)程中速度較快且速度變化比較劇烈,導(dǎo)致里程計(jì)誤差較大。二維碼里程計(jì)疊加算法因沒(méi)有對(duì)二維碼的觀測(cè)進(jìn)行校準(zhǔn),所以精度會(huì)隨著機(jī)器人與二維碼之間的距離變化波動(dòng)顯著。Cartographer 算法定位精度介于AMCL和本文算法之間,表現(xiàn)較好。
圖6 四種算法定位精度對(duì)比Fig.6 Localization accuracy comparison of four algorithms
表1 給出了實(shí)驗(yàn)數(shù)據(jù)的綜合對(duì)比。在實(shí)際測(cè)試過(guò)程中,AMCL 算法在粒子完全收斂后的處理速度與本文算法大致相同,但是變化劇烈的運(yùn)動(dòng)速度導(dǎo)致了AMCL 粒子不能很好地收斂,所以在進(jìn)行高機(jī)動(dòng)性的運(yùn)動(dòng)時(shí),本文算法的處理時(shí)間要遠(yuǎn)短于AMCL算法。
表1 不同算法的采樣點(diǎn)平均誤差與單次位姿計(jì)算耗時(shí)對(duì)比Tab.1 Comparison of different algorithms inaverage sampling point error and time cost per pose calculation
基于以上實(shí)驗(yàn),表2對(duì)上文提到的4種算法以及文獻(xiàn)[1]、文獻(xiàn)[16]、文獻(xiàn)[22]算法進(jìn)行對(duì)比。
表2 各算法性能對(duì)比Tab.2 Performance comparison of different algorithms
由上述對(duì)比可以看出,AMCL 和Cartographer 兩種算法只基于雷達(dá),對(duì)環(huán)境要求較低,但是Cartographer 算力消耗較大,AMCL 精度較為一般的同時(shí)抗機(jī)器人綁架性能較差。文獻(xiàn)[22]算法通過(guò)大量的二維碼結(jié)合里程計(jì)獲得了較好的定位效果,但是需要嚴(yán)格按照二維碼路徑運(yùn)行,因此部署難度較大。I-AMCL(AMCL Integrated with two-dimensional code information)算法[16]利用二維碼提供的信息進(jìn)行初始化并糾正機(jī)器人的運(yùn)動(dòng),但是整體精度較差。ORB-SLAM 雖然不需對(duì)環(huán)境進(jìn)行任何布置,但是只能基于雙目或RGB-D 相機(jī)運(yùn)行且對(duì)于外界環(huán)境較為敏感。本文算法雖然對(duì)于環(huán)境布置有一定要求,但是在定位精度上表現(xiàn)較好,同時(shí)對(duì)于機(jī)器人運(yùn)行路徑基本沒(méi)有要求,具有較強(qiáng)的靈活性。
3.3.2 算法魯棒性測(cè)試
本文算法更多地考慮了實(shí)際運(yùn)行過(guò)程中可能出現(xiàn)的問(wèn)題,圖7 給出定位精度和識(shí)別到二維碼數(shù)量的關(guān)系??梢钥闯觯谶\(yùn)行過(guò)程中,即使在一段時(shí)間內(nèi)找不到二維碼,定位仍能在一段時(shí)間內(nèi)保持穩(wěn)定,可見(jiàn)本文算法能很好地應(yīng)對(duì)由于二維碼污損或是環(huán)境中二維碼數(shù)量較少引起的信息缺失問(wèn)題。
圖7 定位精度和識(shí)別到二維碼數(shù)量的關(guān)系Fig.7 Relationship between localization accuracy and the number of two-dimensional codes recognized
針對(duì)二維碼在實(shí)際運(yùn)行過(guò)程中可能出現(xiàn)的誤識(shí)別問(wèn)題,同樣給出測(cè)試結(jié)果。在實(shí)驗(yàn)時(shí)實(shí)驗(yàn)人員故意將字母V對(duì)應(yīng)二維碼在地圖上的位姿設(shè)置錯(cuò)誤,以此來(lái)測(cè)試二維碼識(shí)別錯(cuò)誤對(duì)系統(tǒng)帶來(lái)的影響,針對(duì)圖8 的圖像,圖9 給出了定位系統(tǒng)結(jié)果的展示。圖中左側(cè)小坐標(biāo)的位姿為視覺(jué)識(shí)別的輸出結(jié)果,但是經(jīng)過(guò)系統(tǒng)的矯正,給出了右側(cè)小坐標(biāo)的輸出結(jié)果,而根據(jù)點(diǎn)云的匹配程度和圖10 給出的高視角無(wú)人機(jī)照片可以看出系統(tǒng)成功地對(duì)視覺(jué)傳感器的誤識(shí)別進(jìn)行了矯正。
圖8 誤識(shí)別圖像Fig.8 Wrongly recognized image
圖9 誤識(shí)別的矯正Fig.9 Correctness of wrong recognition
圖10 機(jī)器人在場(chǎng)地中的位置Fig.10 Robot’s position in field
由以上兩個(gè)測(cè)試可以看出,本文算法能有效應(yīng)對(duì)因?yàn)槎S碼布置量較少或是污損導(dǎo)致的信息缺失問(wèn)題,以及由于二維碼污損導(dǎo)致攜帶信息錯(cuò)誤或是場(chǎng)地布置時(shí)出現(xiàn)人為失誤引起的識(shí)別錯(cuò)誤問(wèn)題。
本文提出了一種基于二維碼信息的視覺(jué)-激光定位融合算法,通過(guò)解算并融合自身相對(duì)于二維碼的位姿獲取機(jī)器人在地圖上的大致位姿,并將其作為激光定位的初始位姿,然后利用點(diǎn)云對(duì)準(zhǔn)進(jìn)行優(yōu)化,有效地解決了傳統(tǒng)二維碼算法以及二維碼-激光融合算法存在的對(duì)機(jī)器人運(yùn)動(dòng)軌跡有限制的問(wèn)題。相較于傳統(tǒng)雷達(dá)定位算法,由于系統(tǒng)擁有了更多的環(huán)境信息,所以能不依賴初始位姿的設(shè)定進(jìn)行定位的同時(shí)提高了系統(tǒng)的整體精度。經(jīng)過(guò)測(cè)試,本文算法相較AMCL 算法采樣點(diǎn)平均誤差減小了92%,單次位姿耗時(shí)縮短了88%;同時(shí)引入里程計(jì)-視覺(jué)傳感器監(jiān)督機(jī)制,提升了系統(tǒng)的魯棒性,在環(huán)境中可能出現(xiàn)二維碼信息缺失以及二維碼識(shí)別錯(cuò)誤的情況下仍能成功地獲得機(jī)器人在地圖上的正確位姿。
得益于進(jìn)行在觀測(cè)到多個(gè)二維碼時(shí)的位姿融合以及使用激光雷達(dá)的二次定位,在環(huán)境的布置上,本文利用32 個(gè)二維碼實(shí)現(xiàn)了43 m2環(huán)境內(nèi)的準(zhǔn)確定位,相較傳統(tǒng)將二維碼作為地標(biāo)的算法[22]在布置二維碼數(shù)目有了較大的減少的同時(shí)并沒(méi)有損失定位精度,并且將二維碼布置在墻上的布置方式使得二維碼更不易磨損,維護(hù)難度降低。同時(shí),本文算法在地圖上的任意位置均可進(jìn)行定位,對(duì)于機(jī)器人的移動(dòng)軌跡無(wú)任何要求。
下一步工作希望在已有算法的基礎(chǔ)上,融入更多的傳感器信息,例如引入能夠觀測(cè)場(chǎng)地的攝像頭,給予機(jī)器人更多的先驗(yàn)信息,進(jìn)一步提升定位精度。