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

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

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

0 引言

在一個已知環(huán)境中，移動機器人為在環(huán)境中進(jìn)行移動，需要借助自身的環(huán)境感知能力來計算出自己在環(huán)境中的位姿，這個問題被稱為機器人的定位問題。對于以倉儲機器人為代表的室內(nèi)移動機器人而言，其對于定位算法的要求集中在高精度、低成本、高可靠性。由于其工作環(huán)境較為固定，可以提前構(gòu)建高精度地圖且通過人為構(gòu)建特征的方式提升機器人的定位精度。

最近幾年以O(shè)RB-SLAM［1］為代表的一系列視覺即時定位與建圖（Simultaneous Localization And Mapping，SLAM）算法以及包括SVO（Semi-direct monocular Visual Odometry）［2］在內(nèi)的視覺里程計算法都有了很大的發(fā)展，但是由于僅使用一個相機無法有效對物體的深度進(jìn)行估計［3］，所以視覺SLAM算法往往需要使用雙目相機系統(tǒng)或者是RGB-D 相機，在僅擁有單目相機的機器人系統(tǒng)上無法成功應(yīng)用。同時，也有學(xué)者通過學(xué)習(xí)的方法來解決定位問題，文獻(xiàn)［4］中作者在學(xué)習(xí)階段提取環(huán)境特征點并進(jìn)行降維存儲，并在后續(xù)利用匹配的方式計算自身的位姿；但是，相對于較為成熟的激光SLAM 算法，純視覺SLAM 算法容易因為外界光線變化或是特征稀疏等問題導(dǎo)致跟蹤失敗。

在已有高精度地圖的情況下，目前使用較多的是基于蒙特卡羅的定位算法。該算法最早由Dellaert 等［5］提出，其利用自身的運動模型進(jìn)行采樣，并利用激光傳感器的數(shù)據(jù)評估每個粒子，并將得分最高粒子的位姿作為自身的最終位姿。大量研究人員在該算法基礎(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］來更新粒子，可有效降低總體采樣粒子數(shù)，也讓機器人面對綁架問題具有更好的魯棒性，并在業(yè)界得到了廣泛使用。厲茂海等［8］通過減少采樣粒子數(shù)量來優(yōu)化采樣過程，提出了以混合高斯模型為基礎(chǔ)的蒙特卡羅算法，進(jìn)一步降低了計算量。在這些算法中，由于其先驗傳感器信息只有里程計和激光雷達(dá)，且無有效算法對機器人的打滑或漂移等現(xiàn)象進(jìn)行預(yù)測或限制，因此機器人在出現(xiàn)上述情況時定位會產(chǎn)生較大偏移，甚至可能導(dǎo)致算法開始全局重定位，另一方面由于缺乏全局的環(huán)境信息，所以全局定位時有可能收斂至全局最優(yōu)點且很難自恢復(fù)。

此外，在AMCL 算法的自適應(yīng)調(diào)整機制執(zhí)行中，自適應(yīng)機制通過計算粒子的平均分?jǐn)?shù)來判斷機器人是否遇到綁架問題，并利用全局隨機播撒粒子的方式試圖解決此問題。但在實際執(zhí)行時，算法對于機器人是否被綁架的判斷常常失準(zhǔn)，很容易因為一些外界干擾或是機器人打滑等問題導(dǎo)致整體分?jǐn)?shù)突然降低；同時該算法收斂較慢，且容易收斂至局部最優(yōu)點，導(dǎo)致定位出現(xiàn)差錯，例如在完全對稱的環(huán)境中可能出現(xiàn)定位翻轉(zhuǎn)。

除基于粒子濾波的算法之外，以Cartographer［9］為代表的基于優(yōu)化的激光SLAM 算法也可以在擁有高精度地圖的情況下完成定位任務(wù)，Cartographer 引入子圖這一概念，在前端完成當(dāng)前幀與子圖的匹配，并在后端完成回環(huán)檢測，以進(jìn)行位姿的優(yōu)化。但是在已經(jīng)完成建圖的情況下，利用Cartographer 定位仍存在著某些問題，其中最主要的便是隨著地圖的增大，Cartographer 在進(jìn)行回環(huán)檢測時消耗的算力會大幅上漲，一些研究者將回環(huán)檢測的搜索任務(wù)分配給GPU（Graphics Processing Unit）進(jìn)行計算以減少時間，但是對于沒有搭載GPU 的機器人而言，利用Cartographer 進(jìn)行高幀率的定位仍存在著地圖大小等環(huán)境因素的限制。

為實現(xiàn)低計算量的高幀率定位，研究人員也嘗試通過人工構(gòu)建特征的方式（如利用二維碼或其他路標(biāo)）來完成定位任務(wù)［10-13］。Nazemzadeh 等［14］使 用QR 碼（Quick Response Code）和里程計做融合，提出了一種基于擴展H∞濾波器的定位算法，Zhang 等［15］提出了一種利用天花板上的二維碼進(jìn)行定位，利用激光雷達(dá)進(jìn)行避障的定位算法。這些算法利用二維碼信息進(jìn)行定位，有效消除了里程計的累積誤差，但是為保證定位的準(zhǔn)確度，這些算法都需在環(huán)境中布置大量的二維碼，同時也需要機器人按照一定的路徑運動。

在此基礎(chǔ)上，胡章芳等［16］提出了融入二維碼信息的自適應(yīng)蒙特卡羅定位算法，利用二維碼信息提供的絕對位置信息對AMCL 算法進(jìn)行初始化和矯正，使得機器人能夠在一定程度上脫離二維碼進(jìn)行定位，但是仍對二維碼的布置和機器人的運動路徑有著一定的限制，且定位精度不高。

上述算法引入了二維碼信息輔助定位，有效消除了里程計的累積誤差，但是由于二維碼均布置在地面或天花板上，易磨損玷污、部署復(fù)雜，需要經(jīng)常維護，同時需要極大數(shù)量的二維碼才能保證整體環(huán)境內(nèi)的定位精度，且由于該算法直接將二維碼的識別結(jié)果送入算法進(jìn)行位姿解算，對二維碼識別錯誤等有可能出現(xiàn)的問題缺乏處理機制，算法整體魯棒性不高。

針對以上問題，本文提出了一種融合二維碼視覺和激光雷達(dá)的定位算法，可將二維碼部署于障礙物表面或墻上，相較于之前的純單目二維碼定位或是二維碼-激光融合算法，本算法對機器人的運行軌跡無任何要求，可實現(xiàn)環(huán)境中的任意運動且在此同時環(huán)境中布置二維碼的數(shù)量大大減少；由于引入了二維碼的絕對位姿信息，所以相對于AMCL 等雷達(dá)定位算法，算法無需初始位姿先驗，也可有效應(yīng)對機器人綁架問題。系統(tǒng)首先利用二維碼攜帶的絕對位姿信息進(jìn)行粗定位，并結(jié)合激光雷達(dá)數(shù)據(jù)對該位姿進(jìn)行優(yōu)化提升位姿精度，同時引入里程計信息對二維碼的位姿進(jìn)行監(jiān)督，防止二維碼識別出現(xiàn)錯誤，同時保證在未觀測到二維碼的時候仍能夠較準(zhǔn)確地定位，適用于解決倉儲機器人為代表的室內(nèi)機器人的定位問題。

1 視覺傳感器信息處理

1.1 二維碼相對位姿解算

為獲取機器人的準(zhǔn)確位姿，需要解算出機器人與二維碼之間的相對位姿，首先利用張正友標(biāo)定法［17］對相機進(jìn)行標(biāo)定得到相機內(nèi)參。然后，當(dāng)視覺傳感器獲取一幀圖像后，識別算法會首先在圖像上尋找二維碼所攜帶的特征點，將其解析并利用其作為索引去檢索數(shù)據(jù)庫以得到當(dāng)前二維碼坐標(biāo)系與地圖坐標(biāo)系之間的變換Rw2t、Tw2t，并可通過求解PnP（Perspective n Points）問題計算出機器人基坐標(biāo)系（在此假設(shè)機器人相機坐標(biāo)系與基坐標(biāo)系重合）與二維碼坐標(biāo)系間的變換Rt2b、Tt2b。本文采用AprilTag［18］開源算法進(jìn)行二維碼的識別和特征提取后利用文獻(xiàn)［19］提出的線性PnP解算算法進(jìn)行PnP解算，AprilTag 算法具有實時性好、準(zhǔn)確率高等特點，最后利用式（1）、（2）獲取機器人基坐標(biāo)系相對于世界系的變換Rw2b、Tw2b：

1.2 多二維碼位姿融合

相較于傳統(tǒng)二維碼定位算法使用二維碼作為地標(biāo)，本文算法將其貼在障礙物和墻上，克服了作為地標(biāo)易磨損玷污的缺點且減少了二維碼在環(huán)境中需求的數(shù)量，同時在機器人的運行過程中可能會有多個二維碼出現(xiàn)在機器人的視野中。但與此同時，由于相機的標(biāo)定存在一定誤差、二維碼布置存在一定誤差且因成本考慮僅使用單目視覺進(jìn)行識別，相較于將二維碼布置到地面或是天花板抑或是利用Kinect進(jìn)行輔助的算法［20］，得到的變換關(guān)系的準(zhǔn)確度相對較差。雖然系統(tǒng)并不利用二維碼解算到的位姿直接作為結(jié)果輸出，但仍希望視覺定位的位姿能盡量準(zhǔn)確，因此本文提出以下算法對多個二維碼返回的位姿進(jìn)行融合，提升二維碼返回位姿的準(zhǔn)確度。

1.2.1 異常識別二維碼去除

設(shè)將位姿映射到世界坐標(biāo)系后，返回位姿集P={P1，P2，…，Pn}，定義位姿Pi與真值之間具有誤差ei，記由于噪聲或者標(biāo)定不完全出現(xiàn)的自然誤差構(gòu)成的集合為N，由于二維碼污損或其他問題導(dǎo)致的識別錯誤引入的誤差集合為M。異常位姿去除階段不考慮自然誤差，故其目標(biāo)可定義為尋找集合Φ∈[1，n]使：

在絕大多數(shù)情況下噪聲較小，且標(biāo)定誤差不大，所以ei一般較小，即|ei-ej|較小，本文利用這一特點來切分位姿集，由式（4），實際計算時可利用|pi-pj|來切分位姿集。設(shè)當(dāng)前位姿真值為Ptrue，則：

利用式（4）的結(jié)論，以位姿Pi為中心點，δdis為半徑，δyaw為高，在x-y-yaw空間內(nèi)構(gòu)建置信域Ωi。則若Pj∈Ωi，則可大概率認(rèn)為ei、ej∈N。將該結(jié)論推廣，對位姿集中所有點按上述方式構(gòu)建置信域，并將所有在置信域中的位姿放入Pi的內(nèi)點集ψi，并定義num(ψi)代表ψi中的元素數(shù)目，則：

1.2.2 多二維碼位姿融合

由于二維碼布置在障礙物的表面，其相對于機器人的距離和角度都會隨著機器人的運動發(fā)生巨大的變化，又因為機器人有時候會在很遠(yuǎn)的距離上對二維碼進(jìn)行觀測，所以會使機器人觀測多二維碼時存在著一定的重投影誤差。如圖1 所示的情況，機器人對4 個紅點所示位置的二維碼進(jìn)行觀測時，生成了4 個相互不重疊的位姿（4 個紅色小箭頭）。本文在對誤差進(jìn)行研究時，假設(shè)誤差的主要來源為相機標(biāo)定不完全以及存在著觀測噪聲。

圖1 重投影誤差示意圖Fig.1 Schematic diagram of re-projection error

為了能夠獲得標(biāo)定誤差和實際投影誤差的關(guān)系，本文利用一種基于幾何特征的PnP 解算算法來進(jìn)行PnP 計算［18］，該算法針對呈矩形分布的P4P 問題提出了一種線性的求解方式，由于該算法避免了非線性迭代的過程，所以本文基于該算法進(jìn)行了相機內(nèi)參誤差和解算位姿誤差間的關(guān)系推導(dǎo)。

如圖2所示，設(shè)q0、q1、q2、q3分別為圖像中二維碼的4個角點，其在像素坐標(biāo)系下的坐標(biāo)分別為(x0，y0)、(x1，y1)、(x2，y2)、(x3，y3)，f為相機的焦距，P0、P1、P2、P3分別為世界坐標(biāo)系中二維碼的角點。

圖2 三維位姿測量示意圖Fig.2 Schematic diagram of 3D pose measurement

相機內(nèi)參矩陣和焦距與內(nèi)參矩陣的關(guān)系分別表示為：

點q0的相機坐標(biāo)系下的坐標(biāo)為：

其他3個點同理，記它們的坐標(biāo)分別為：(X0，Y0，f)、(X1，Y1，f)、(X2，Y2，f)、(X3，Y3，f)。

對于由光心Oc和q2、q3組成的平面π1，其法向量N1可表示為：

由于P0P1‖π1，所以可得：

繼而可得：

整理式（13），并定義：

將N1的表達(dá)式代入，可得：

k1同理，所以可得h與fx、fy、cx、cy無關(guān)，即標(biāo)定結(jié)果不對h的值產(chǎn)生影響。

考慮到世界坐標(biāo)系中|P0P1|=d1，可知：

變換式（16）得到：

求k對各變量的導(dǎo)數(shù)：

由多元函數(shù)的泰勒展開，在僅保留一階小量時，可得：

同時可得到位置誤差與標(biāo)定誤差的關(guān)系：

而后，利用式（28）計算R并將上文解出的誤差關(guān)系代入后可解出ΔR：

在獲得標(biāo)定誤差和位姿誤差的對應(yīng)關(guān)系后，假設(shè)相機的標(biāo)定誤差服從隨機的高斯分布，則利用式（25）～（28）將可得到觀測結(jié)果的均值和方差。

設(shè)對第k個二維碼的觀測的結(jié)果為：

利用式（32）最大高斯概率的密度函數(shù)歸一化積對各變量進(jìn)行多高斯分布融合：

式（32）的結(jié)果為多高斯分布融合后的均值，對各變量分別求均值后利用結(jié)果作為視覺定位的最終輸出。

2 基于多傳感器融合的位姿增強

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

在獲得融合二維碼信息得到的位姿后，繼續(xù)利用點云對準(zhǔn)的方式對位姿進(jìn)一步優(yōu)化，本文選擇一種基于旋轉(zhuǎn)矩陣求解的點云對準(zhǔn)算法［21］，定義每一個激光點的誤差為：

其中：pj為激光點云在機器人基坐標(biāo)系上的坐標(biāo)，yj為pj在地圖上歐氏距離最近的障礙物點坐標(biāo)，C和r分別為激光雷達(dá)坐標(biāo)系和地圖坐標(biāo)系的旋轉(zhuǎn)矩陣和平移矩陣。

同時定義點云在雷達(dá)坐標(biāo)系中的質(zhì)心和其在地圖上歐氏距離最近的障礙物點的質(zhì)心分別為：

利用上述定義構(gòu)建優(yōu)化函數(shù)：

則求解目標(biāo)為尋找雷達(dá)與地圖之間的旋轉(zhuǎn)變換關(guān)系C及平移變換關(guān)系r使得J最小。

定義平移量d的表達(dá)式為：

將d代入誤差函數(shù)，得到：

通過這種表達(dá)方式，可以分離C和d并分別對兩項進(jìn)行優(yōu)化。

對于含有C的各項：

式中：

定義如下的代價函數(shù)對C進(jìn)行優(yōu)化：

對變量依次求導(dǎo)得到：

式（43）、（44）分別為旋轉(zhuǎn)矩陣C的約束條件，對于式（42），令=0，可得：

由于旋轉(zhuǎn)矩陣是一個正交矩陣，所以可得：

求解出機器人的最優(yōu)旋轉(zhuǎn)矩陣后，將C代入式（37）中，并令平移量d=0，得到：

利用式（48）、（49）的結(jié)果對機器人基坐標(biāo)系的位姿進(jìn)行不斷修正，便可使式（36）的誤差不斷減小，直至達(dá)到最大迭代次數(shù)或是誤差小于一定值后停止迭代。

2.2 基于里程計的位姿估計與搜索

在實際系統(tǒng)運行過程中，為保證系統(tǒng)的魯棒性，需要保證在機器人視野中沒有二維碼時仍能夠順利完成定位，故引入了基于里程計的位姿計算。首先里程計數(shù)據(jù)代入式（50）計算出當(dāng)前時刻的先驗位姿，并利用2.1 節(jié)所述的點云對準(zhǔn)方式進(jìn)行對準(zhǔn)，相較于AMCL的KLD采樣，該算法在高幀率的情況下具有更高的準(zhǔn)確度以及更少的算力消耗。

2.3 里程計-二維碼定位的交叉位姿驗證

實際應(yīng)用中，由于二維碼污損或是張貼不平整等外部原因會導(dǎo)致視覺傳感器返回位姿錯誤，繼而可能會導(dǎo)致定位算法無法搜索到正確的位姿，因此為了保證系統(tǒng)的魯棒性，本文在這里提出了里程計-二維碼定位矯正算法。設(shè)：Pl(xl，yl，θl)、Pv(xv，yv，θv)分別為上一時刻機器人的位姿和優(yōu)化的得到的最優(yōu)位姿。Δx、Δy、Δθ分別為利用機器人里程計計算的位姿差，則機器人利用運動模型計算得到的位姿為：

得到該位姿后，基于連續(xù)性假設(shè)，由式（51）、（52）對機器人的位姿進(jìn)行檢測，并利用式（53）輸出矯正后的位姿，Podomcal為3.2節(jié)提出的基于里程計計算得到的位姿：

在此同時，考慮到機器人綁架問題的存在，若在短時間內(nèi)多幀圖像均被系統(tǒng)判定為識別錯誤，則有可能是因為機器人被人為挪動，此時算法選取與環(huán)境匹配度更高的位姿作為結(jié)果輸出，定義maxpoint 函數(shù)的輸出為輸入中具有最高分?jǐn)?shù)的位姿，則輸出位姿表示為：

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

基于以上的各算法，在此給出本文提出的融合算法的算法流程，該算法有效解決了視覺傳感器和雷達(dá)數(shù)據(jù)的融合問題，且具有較好的魯棒性，能夠應(yīng)對二維碼識別錯誤、沒有找到二維碼等多種問題。

記ξvisal為利用每個識別到的二維碼信息計算的機器人基坐標(biāo)系在世界坐標(biāo)系下的坐標(biāo)，其內(nèi)部可能包含有多個元素，也有可能不包含任何元素，即視野中可以沒有任何二維碼，Plast為上一時刻的位姿，Δx、Δy、Δθ分別為利用機器人里程計計算兩時刻位姿差，則整體的算法流程如下：

算法1 Pose Enhancement Algorithm。

輸入Plast，ζ，Δx，Δy，Δθ；

輸出Pcurrent。

3 實驗及結(jié)果分析

3.1 實驗平臺

使用如圖3 所示的室內(nèi)移動機器人系統(tǒng)進(jìn)行實驗，搭建了一個模擬倉庫環(huán)境，場地尺寸為8 540 mm×5 100 mm，共布置32 個二維碼，均位于障礙物表面，二維碼使用DJI Robomaster S1 系列的二維碼，3D 場地效果圖如圖4 所示。機器人采用IMU 結(jié)合編碼器生成里程計數(shù)據(jù)；同時利用DAHENG MERS133-54 攝像頭采集圖像，攝像頭固定在云臺的yaw軸上。機器人自身攜帶一個TX2 處理器作為運算單元。

圖3 室內(nèi)移動機器人Fig.3 Indoor mobile robot

圖4 測試場地示意圖Fig.4 Schematic diagram of test field

為模擬機器人的真實運動場景同時保證評判的公平性，由實驗人員手動遙控機器人在場地中進(jìn)行隨機運動，并在運行過程中包含無法觀測到二維碼、只觀測到一個二維碼和觀測到多個二維碼幾種情況，運行軌跡如圖5 所示，將全過程記錄并分別用視覺里程計定位、AMCL 算法和本文算法進(jìn)行定位。

圖5 機器人的運動軌跡及二維碼布置Fig.5 Robot’s moving trajectory and two-dimensional code layout

3.2 實驗評判指標(biāo)

在本次實驗中，因為場地缺乏運動捕捉系統(tǒng)，所以無法獲取機器人的精確位姿信息，為了能夠最大限度地給出算法的精確度信息，本次實驗并沒有用建圖算法進(jìn)行建圖，而是使用了場地的高精度地圖，以保證評價的準(zhǔn)確性。同時本文利用以下指標(biāo)來對位姿進(jìn)行評判：

其中yj、ej的定義與式（33）中相同。需要特別說明的是，采樣點距離障礙物的平均距離并不等同于機器人在世界坐標(biāo)系上的誤差，在機器人姿態(tài)角存在誤差時，遠(yuǎn)處采樣點誤差較大。該指標(biāo)與定位誤差呈正相關(guān)關(guān)系，但不存在絕對的線性關(guān)系。

3.3 實驗結(jié)果及分析

3.3.1 算法對比

為了提升算法的可比較性，機器人運行全程利用ROS 的錄包功能錄制機器人運行全程傳感器返回的數(shù)據(jù)，并使用一臺處理器為i5-6300HQ 的計算機分別用4 種算法對數(shù)據(jù)進(jìn)行處理。

本文共對四種算法進(jìn)行對比，參數(shù)與設(shè)置與說明如下：

1）二維碼里程計疊加（TAG-ODOM）算法：將所有利用二維碼計算得到的位姿做平均后直接使用，若沒有識別到二維碼，則在上一時刻位姿的基礎(chǔ)上疊加利用里程計數(shù)據(jù)計算出的兩時刻位姿差。

2）AMCL 算法：初始位姿已知，最大粒子數(shù)1 000，最小粒子數(shù)50，里程計的噪聲分別設(shè)置為1.0，1.0，1.0，0.8，0.8。

3）基于二維碼視覺與激光雷達(dá)融合定位算法（fusion method）：點云對準(zhǔn)算法的最大迭代次數(shù)為50。

4）Cartographer 算法：初始位置已知，輸入里程計和雷達(dá)的數(shù)據(jù)，建圖分辨率為0.05 m。

圖6 為機器人運行全程中點云與地圖的匹配度對比，可以明顯看出，二維碼視覺與激光雷達(dá)融合算法在機器人運行的全程表現(xiàn)良好，而AMCL 算法則表現(xiàn)欠佳，總體準(zhǔn)確度與本文算法差距較大，其主要原因在于在于運動過程中速度較快且速度變化比較劇烈，導(dǎo)致里程計誤差較大。二維碼里程計疊加算法因沒有對二維碼的觀測進(jìn)行校準(zhǔn)，所以精度會隨著機器人與二維碼之間的距離變化波動顯著。Cartographer 算法定位精度介于AMCL和本文算法之間，表現(xiàn)較好。

圖6 四種算法定位精度對比Fig.6 Localization accuracy comparison of four algorithms

表1 給出了實驗數(shù)據(jù)的綜合對比。在實際測試過程中，AMCL 算法在粒子完全收斂后的處理速度與本文算法大致相同，但是變化劇烈的運動速度導(dǎo)致了AMCL 粒子不能很好地收斂，所以在進(jìn)行高機動性的運動時，本文算法的處理時間要遠(yuǎn)短于AMCL算法。

表1 不同算法的采樣點平均誤差與單次位姿計算耗時對比Tab.1 Comparison of different algorithms inaverage sampling point error and time cost per pose calculation

基于以上實驗，表2對上文提到的4種算法以及文獻(xiàn)［1］、文獻(xiàn)［16］、文獻(xiàn)［22］算法進(jìn)行對比。

表2 各算法性能對比Tab.2 Performance comparison of different algorithms

由上述對比可以看出，AMCL 和Cartographer 兩種算法只基于雷達(dá)，對環(huán)境要求較低，但是Cartographer 算力消耗較大，AMCL 精度較為一般的同時抗機器人綁架性能較差。文獻(xiàn)［22］算法通過大量的二維碼結(jié)合里程計獲得了較好的定位效果，但是需要嚴(yán)格按照二維碼路徑運行，因此部署難度較大。I-AMCL（AMCL Integrated with two-dimensional code information）算法［16］利用二維碼提供的信息進(jìn)行初始化并糾正機器人的運動，但是整體精度較差。ORB-SLAM 雖然不需對環(huán)境進(jìn)行任何布置，但是只能基于雙目或RGB-D 相機運行且對于外界環(huán)境較為敏感。本文算法雖然對于環(huán)境布置有一定要求，但是在定位精度上表現(xiàn)較好，同時對于機器人運行路徑基本沒有要求，具有較強的靈活性。

3.3.2 算法魯棒性測試

本文算法更多地考慮了實際運行過程中可能出現(xiàn)的問題，圖7 給出定位精度和識別到二維碼數(shù)量的關(guān)系?？梢钥闯?，在運行過程中，即使在一段時間內(nèi)找不到二維碼，定位仍能在一段時間內(nèi)保持穩(wěn)定，可見本文算法能很好地應(yīng)對由于二維碼污損或是環(huán)境中二維碼數(shù)量較少引起的信息缺失問題。

圖7 定位精度和識別到二維碼數(shù)量的關(guān)系Fig.7 Relationship between localization accuracy and the number of two-dimensional codes recognized

針對二維碼在實際運行過程中可能出現(xiàn)的誤識別問題，同樣給出測試結(jié)果。在實驗時實驗人員故意將字母V對應(yīng)二維碼在地圖上的位姿設(shè)置錯誤，以此來測試二維碼識別錯誤對系統(tǒng)帶來的影響，針對圖8 的圖像，圖9 給出了定位系統(tǒng)結(jié)果的展示。圖中左側(cè)小坐標(biāo)的位姿為視覺識別的輸出結(jié)果，但是經(jīng)過系統(tǒng)的矯正，給出了右側(cè)小坐標(biāo)的輸出結(jié)果，而根據(jù)點云的匹配程度和圖10 給出的高視角無人機照片可以看出系統(tǒng)成功地對視覺傳感器的誤識別進(jìn)行了矯正。

圖8 誤識別圖像Fig.8 Wrongly recognized image

圖9 誤識別的矯正Fig.9 Correctness of wrong recognition

圖10 機器人在場地中的位置Fig.10 Robot’s position in field

由以上兩個測試可以看出，本文算法能有效應(yīng)對因為二維碼布置量較少或是污損導(dǎo)致的信息缺失問題，以及由于二維碼污損導(dǎo)致攜帶信息錯誤或是場地布置時出現(xiàn)人為失誤引起的識別錯誤問題。

4 結(jié)語

本文提出了一種基于二維碼信息的視覺-激光定位融合算法，通過解算并融合自身相對于二維碼的位姿獲取機器人在地圖上的大致位姿，并將其作為激光定位的初始位姿，然后利用點云對準(zhǔn)進(jìn)行優(yōu)化，有效地解決了傳統(tǒng)二維碼算法以及二維碼-激光融合算法存在的對機器人運動軌跡有限制的問題。相較于傳統(tǒng)雷達(dá)定位算法，由于系統(tǒng)擁有了更多的環(huán)境信息，所以能不依賴初始位姿的設(shè)定進(jìn)行定位的同時提高了系統(tǒng)的整體精度。經(jīng)過測試，本文算法相較AMCL 算法采樣點平均誤差減小了92%，單次位姿耗時縮短了88%；同時引入里程計-視覺傳感器監(jiān)督機制，提升了系統(tǒng)的魯棒性，在環(huán)境中可能出現(xiàn)二維碼信息缺失以及二維碼識別錯誤的情況下仍能成功地獲得機器人在地圖上的正確位姿。

得益于進(jìn)行在觀測到多個二維碼時的位姿融合以及使用激光雷達(dá)的二次定位，在環(huán)境的布置上，本文利用32 個二維碼實現(xiàn)了43 m2環(huán)境內(nèi)的準(zhǔn)確定位，相較傳統(tǒng)將二維碼作為地標(biāo)的算法［22］在布置二維碼數(shù)目有了較大的減少的同時并沒有損失定位精度，并且將二維碼布置在墻上的布置方式使得二維碼更不易磨損，維護難度降低。同時，本文算法在地圖上的任意位置均可進(jìn)行定位，對于機器人的移動軌跡無任何要求。

下一步工作希望在已有算法的基礎(chǔ)上，融入更多的傳感器信息，例如引入能夠觀測場地的攝像頭，給予機器人更多的先驗信息，進(jìn)一步提升定位精度。