基于二維碼視覺與激光雷達融合的高精度定位算法

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

1 視覺傳感器信息處理

1.1 二維碼相對位姿解算

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

1.2 多二維碼位姿融合

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

1.2.1 異常識別二維碼去除

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

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

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

1.2.2 多二維碼位姿融合

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

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

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

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

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

相機內參矩陣和焦距與內參矩陣的關系分別表示為：

點q0的相機坐標系下的坐標為：

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

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

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

繼而可得：

整理式（13），并定義：

將N1的表達式代入，可得：

k1同理，所以可得h與fx、fy、cx、cy無關，即標定結果不對h的值產生影響。

考慮到世界坐標系中|P0P1|=d1，可知：

變換式（16）得到：

求k對各變量的導數：

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

同時可得到位置誤差與標定誤差的關系：

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

在獲得標定誤差和位姿誤差的對應關系后，假設相機的標定誤差服從隨機的高斯分布，則利用式（25）～（28）將可得到觀測結果的均值和方差。

設對第k個二維碼的觀測的結果為：

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

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

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

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

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

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

同時定義點云在雷達坐標系中的質心和其在地圖上歐氏距離最近的障礙物點的質心分別為：

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

則求解目標為尋找雷達與地圖之間的旋轉變換關系C及平移變換關系r使得J最小。

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

將d代入誤差函數，得到：

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

對于含有C的各項：

式中：

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

對變量依次求導得到：

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

由于旋轉矩陣是一個正交矩陣，所以可得：

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

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

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

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

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

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

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

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

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

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

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

算法1 Pose Enhancement Algorithm。

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

輸出Pcurrent。

3 實驗及結果分析

3.1 實驗平臺

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

圖3 室內移動機器人Fig.3 Indoor mobile robot

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

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

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

3.2 實驗評判指標

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

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

3.3 實驗結果及分析

3.3.1 算法對比

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

本文共對四種算法進行對比，參數與設置與說明如下：

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

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

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

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

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

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

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

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

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

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

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

3.3.2 算法魯棒性測試

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

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

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

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

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

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

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

4 結語

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

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

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