欒佳寧，張 偉*，孫 偉，張 奧，韓 冬

（1.同濟(jì)大學(xué)電子與信息工程學(xué)院，上海 201804；2.西安電子科技大學(xué)空間科學(xué)與技術(shù)學(xué)院，西安 710126）

（*通信作者電子郵箱zhang_wi@#edu.cn）

0 引言

在一個(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)題。

1 視覺(jué)傳感器信息處理

1.1 二維碼相對(duì)位姿解算

為獲取機(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：

1.2 多二維碼位姿融合

相較于傳統(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é)定位的最終輸出。

2 基于多傳感器融合的位姿增強(qiáng)

2.1 激光點(diǎn)云對(duì)準(zhǔn)的位姿優(yōu)化

在獲得融合二維碼信息得到的位姿后，繼續(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ù)或是誤差小于一定值后停止迭代。

2.2 基于里程計(jì)的位姿估計(jì)與搜索

在實(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)確度以及更少的算力消耗。

2.3 里程計(jì)-二維碼定位的交叉位姿驗(yà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ù)的位姿，則輸出位姿表示為：

2.4 基于二維碼視覺(jué)與激光雷達(dá)融合定位算法

基于以上的各算法，在此給出本文提出的融合算法的算法流程，該算法有效解決了視覺(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 實(shí)驗(yàn)及結(jié)果分析

3.1 實(shí)驗(yàn)平臺(tái)

使用如圖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

3.2 實(shí)驗(yàn)評(píng)判指標(biāo)

在本次實(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 實(shí)驗(yàn)結(jié)果及分析

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)題。

4 結(jié)語(yǔ)

本文提出了一種基于二維碼信息的視覺(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)一步提升定位精度。