基于激光雷達(dá)的有界區(qū)域快速全局定位算法

吳海欣,文梓豪,熊弘俊,崔 彥,林軍記,張 輝

(1.中山大學(xué)智能工程學(xué)院,廣東 廣州 510006；2.廣東省智能交通系統(tǒng)重點實驗室,廣東 廣州 510006)

1 引 言

精準(zhǔn)定位是自動駕駛、無人機(jī)器人等一系列重要應(yīng)用的基礎(chǔ),分為室外定位和室內(nèi)定位。在室外環(huán)境下,全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system,GNSS)可以提供精確的位置信息[1],但在像停車場等的一些室內(nèi)環(huán)境以及像隧道等被嚴(yán)重遮擋的室外環(huán)境下,其定位精度已無法滿足我們的定位精度需求。因此,近年來國內(nèi)外各界都在致力尋求更高精度和可靠的室內(nèi)定位技術(shù),使之在特定環(huán)境條件下也可以獲得準(zhǔn)確的位置信息。

目前,國內(nèi)外的室內(nèi)定位領(lǐng)域主要包括有射頻識別(RFID)[2]、Wi-Fi、ZigBee、超寬帶(UWB)、慣性導(dǎo)航、超聲波[3]、激光、紅外線、藍(lán)牙[4]等定位技術(shù)。這些定位技術(shù)的區(qū)別在于其傳感器和數(shù)據(jù)傳輸方式的不同,而其后臺核心定位算法才是對定位效果起關(guān)鍵作用的因素,包括有鄰近信息法、多邊測量法、雙曲線定位法、三角定位法、指紋定位法和航位推算法等。就目前的技術(shù)發(fā)展而言,每個定位技術(shù)都有其優(yōu)點和局限性,不具有同時滿足定位精度、成本、功耗和信息安全多個方面的普適性技術(shù)。與此同時,不同的定位算法所適用的定位技術(shù)不同,因此各界也在致力尋求一種可靠、穩(wěn)定的定位技術(shù)與算法的結(jié)合,實現(xiàn)室內(nèi)定位精度和功耗等方面的最佳化。

另一方面,室內(nèi)定位作為機(jī)器人必不可少的一個關(guān)鍵技術(shù),可以分為位置跟蹤和全局定位兩類。位置跟蹤是指在已知初始位姿以及未知地圖的前提下,對運動狀態(tài)下的機(jī)器人進(jìn)行定位,如激光SLAM定位[5]和視覺SLAM定位[6]。但SLAM定位技術(shù)不可避免地會產(chǎn)生隨著車輛里程的增大而增大的累積誤差。相對而言,全局定位則要求在對初始位姿沒有任何先驗知識和已知地圖的情況下,獲取當(dāng)前時刻機(jī)器人的位置信息?；谝阎貓D的全局定位與SLAM不同,其過程是將當(dāng)前時刻采集到的環(huán)境點云與事先建好的地圖作匹配,每次幀間配準(zhǔn)相對獨立,所以其不存在累積誤差[7]。當(dāng)然全局定位也更具難度和挑戰(zhàn)性,目前主要有全局視覺定位[8]和基于激光雷達(dá)的全局定位[9]等。

由于基于攝像機(jī)的全局定位會受到光照因素的影響,相比而言激光雷達(dá)則具有不受環(huán)境光影響以及精度更高的優(yōu)勢,本文以激光雷達(dá)為單一的傳感器,提出一種室內(nèi)有界區(qū)域下的快速全局定位算法,將激光定位技術(shù)與全局定位算法有效地結(jié)合,并在仿真實驗下取得可靠、穩(wěn)定的定位效果。

2 問題的描述

全局定位實際上是一個坐標(biāo)轉(zhuǎn)換的問題,其過程就是建立地面坐標(biāo)系與車輛坐標(biāo)系之間的對應(yīng)關(guān)系[10]。在本文的研究中,地面坐標(biāo)系是不變的、絕對的,而車輛坐標(biāo)系是隨著車輛位姿的變換而變的。因此,地面坐標(biāo)系下的全局地圖也是不變的。但在雷達(dá)實時定位建圖過程中,智能車需要構(gòu)建環(huán)境地圖,用于對自身的精確定位,而環(huán)境地圖的構(gòu)建又與智能車在每個時刻的位置息息相關(guān)[11],即車輛坐標(biāo)系中的局部地圖也將隨著車輛位姿的變換而改變。因此,要實現(xiàn)激光雷達(dá)的全局定位,就要將變換的車輛坐標(biāo)系下的局部地圖和絕對的地面坐標(biāo)系下的全局地圖進(jìn)行匹配,最終得出車輛在地面坐標(biāo)系下的位姿,包括其坐標(biāo)以及方向角。

圖1 坐標(biāo)系轉(zhuǎn)換關(guān)系

(1)

3 地圖的特征描述

在地圖只有邊界的情況下,任意一個凸多邊形地圖都可以采用一個特征向量來對其進(jìn)行描述,每個凸多邊形對應(yīng)特定且唯一的特征向量。該特征向量我們定義如式(2)所示。

V=[Xg,Yg,Lmax,αmax,Lmin,αmin,

L1,α1,L2,α2,L3,α3,…]T

(2)

其中,Xg,Yg為地圖邊界的重心坐標(biāo);Lmax,αmax分別表示重心到最遠(yuǎn)點的距離和方向角;Lmin,αmin分別表示重心到最近點的距離和方向角。αi,Li為αmax,αmin之間分割出來的第i個角及重心到該方向角上邊界點的距離。

圖2 凸多邊形地圖特征描述

4 定位算法

圖3 全局定位算法流程

4.1 預(yù)構(gòu)建全局地圖特征向量

在已知地圖和已知地面坐標(biāo)系的情況下,首先需要構(gòu)建出全局地圖的特征向量V=[Xg,Yg,

(3)

(4)

其中,X(i),Y(i)(i=1,2,3,…,k)表示凸多邊形地圖第i個頂點的全局坐標(biāo);Di表示第i個頂點與其下一個頂點的歐氏距離。

4.2 數(shù)據(jù)掃描與預(yù)處理

激光雷達(dá)通常采用飛行時間法測距,即通過測量從發(fā)射激光脈沖到接收發(fā)射光的時間間隔,根據(jù)光的行程和飛行時間計算距離[12],如式(5)所示。

(5)

式中,r表示目標(biāo)距離;Δt表示發(fā)射波與反射波的時間延遲,光速c=3×108m/s。

當(dāng)二維單線激光雷達(dá)處于地圖區(qū)域內(nèi)時,實時地對地圖的邊界進(jìn)行掃描,采集到地圖邊界的n組離散的點云數(shù)據(jù),包括n個邊界點到雷達(dá)的距離和角度。采集完點云數(shù)據(jù)后,數(shù)據(jù)預(yù)處理基本流程如圖4所示。

圖4 數(shù)據(jù)預(yù)處理基本流程圖

大部分激光雷達(dá)的測距范圍都有一定的閾值,當(dāng)雷達(dá)靠近或者遠(yuǎn)離地圖邊界時會出現(xiàn)部分邊界點的距離數(shù)據(jù)為0等異常的情況,所以首先要識別并剔除該部分異常數(shù)據(jù)。同時值得注意的是,激光雷達(dá)的飛行時間法測距本身也存在誤差,包括有系統(tǒng)誤差和噪聲誤差。其中系統(tǒng)誤差包括計時誤差、時刻鑒別誤差以及大氣折射率誤差,而隨機(jī)誤差則主要是噪聲引起的誤差[13]。因此,在剔除異常數(shù)據(jù)外,還需要對點云數(shù)據(jù)的誤差進(jìn)行校正。

校正誤差后,由于激光雷達(dá)所獲取的點云是極坐標(biāo)數(shù)據(jù),而在后續(xù)定位中需要用到的是直角坐標(biāo)數(shù)據(jù),因此需要將點云數(shù)據(jù)轉(zhuǎn)換為其在車輛坐標(biāo)系下的直角坐標(biāo)數(shù)據(jù),構(gòu)建局部地圖。其轉(zhuǎn)換公式如式(6)所示:

(6)

式中,x(i),y(i)表示掃描到的第i個邊界點在車輛坐標(biāo)系中的橫坐標(biāo)和縱坐標(biāo);r(i)表示該點到激光雷達(dá)的距離數(shù)據(jù);ω(i)則表示該點的角度數(shù)據(jù)。

4.3 構(gòu)建局部地圖特征向量

對數(shù)據(jù)進(jìn)行預(yù)處理并構(gòu)建局部地圖后,要對局部地圖進(jìn)行特征提取,提取局部地圖重心到最遠(yuǎn)點的距離lmax和方向角βmax,重心到最近點的距離lmin和方向角βmin,以及βmax,βmin之間分割出來的第i個角βi和重心到該方向角上邊界點的距離li。其中,局部地圖重心坐標(biāo)xg,yg可由式(7)～(9)解得:

(7)

(8)

(9)

式中,di(i=1,2,…,n)表示局部地圖中相鄰兩個邊界點的距離。

由此,提取出局部地圖中的特征信息(如重心坐標(biāo)、重心與邊界點的距離以及方向角)后,可以構(gòu)建出局部地圖在車輛坐標(biāo)系中的特征向量v,如式(10)所示:

v=[xg,yg,lmax,βmax,lmin,βmin,

l1,β1,l2,β2,l3,β3,…]T

(10)

4.4 特征向量匹配定位

(11)

(12)

該算法提取激光雷達(dá)采集的點云數(shù)據(jù)特征,將其與數(shù)據(jù)庫中事先存儲的全局地圖特征數(shù)據(jù)進(jìn)行匹配,只需找出全局地圖與局部地圖中一個對應(yīng)的特征點——重心,并利用重心與特定邊界點的方向角,最終實現(xiàn)車輛在有界區(qū)域內(nèi)的快速全局定位,大大減少了計算量,從而降低了定位算法的復(fù)雜度。

5 非凸多邊形切分

值得注意的是,當(dāng)機(jī)器人處于非凸多邊形地圖內(nèi),激光雷達(dá)位于某些特定區(qū)域時會出現(xiàn)被地圖邊界遮擋而漏掃的現(xiàn)象,構(gòu)建出不完整的局部地圖,這可能導(dǎo)致兩個坐標(biāo)系中地圖邊界的重心不是一一對應(yīng)的特征點,因此無法直接將局部地圖與全局地圖進(jìn)行特征向量匹配。此時需要對地圖進(jìn)行圖形切分,再運用改進(jìn)后的邊界特征匹配方法來進(jìn)行定位。

本文的圖形切分原則為,將原非凸多邊形地圖看作多個凸多邊形的疊加組合,并根據(jù)這些凸多邊形的重合情況來切分地圖。如圖5所示的非凸多邊形地圖ABCDEF,可將其視為凸多邊形ABCG與凸多邊形AHEF的疊加組合,此時根據(jù)其重合的情況可將地圖內(nèi)切分為①,②,③三個區(qū)域。

圖5 非凸多邊形圖形切分

雷達(dá)在不同的區(qū)域中只能完整地識別出其所在凸多邊形與原地圖的交集部分。如當(dāng)雷達(dá)處于區(qū)域②時,其所屬的凸多邊形ABCG與原地圖的交集為多線段GABCD,此時雷達(dá)可以完整掃描出該多線段,但受頂點D的影響無法完整掃描出線段DEFG,區(qū)域①③同理。

改進(jìn)后的特征匹配方法解決了非凸多邊形地圖內(nèi)雷達(dá)會出現(xiàn)漏掃的現(xiàn)象,其具體步驟如下所示:

Step 1:非凸多邊形地圖可視為多個凸多邊形的疊加組合,計算這些凸多邊形與原地圖邊界的交集的特征向量Vi;

Step 2:計算激光雷達(dá)掃描的點云圖中每相鄰兩個點的距離di以及地圖上的最短邊長Dmin,判斷是否存在di>Dmin。若不存在,則雷達(dá)點云圖無漏掃,按照凸多邊形地圖定位方法進(jìn)行定位；若存在,則雷達(dá)點云圖存在漏掃,跳至步驟3；

Step 3:在雷達(dá)點云圖中剔除不能完整掃描出的多線段部分,保留可完整掃描出的多線段部分；

Step 4:計算此時點云圖的特征向量v以及|Vi-v|。將Vm中的參數(shù)帶入式(11)和式(12)中計算得出車輛位姿。其中|Vm-v|=min(|Vi-v|)。

6 實驗驗證

為了高效地驗證本文算法的有效性及其定位精度,本文在進(jìn)行全局定位測試時,將預(yù)設(shè)好的有界多邊形地圖導(dǎo)入,在MATLAB上通過仿真實驗?zāi)７虏煌蛔藭r二維單線激光雷達(dá)在地圖內(nèi)的掃描過程,并運用本文提出的全局定位算法解算出車輛的坐標(biāo)和朝向。

本文定位誤差的評估通過位置誤差比和朝向誤差來描述。位置誤差比的定義如式(13)所示:

(13)

(14)

由于非凸多邊形地圖可通過圖形切分為多個區(qū)域,既可以驗證凸多邊形快速定位算法又可以驗證改進(jìn)后的特征匹配方法,因此本次實驗選取了圖5中的有界非凸多邊形為實驗地圖,該地圖具有代表性。

本次實驗的位置誤差狀況如圖6所示。由圖6(a)可知雷達(dá)在無漏掃區(qū)域時整體誤差較小,而當(dāng)雷達(dá)處于漏掃區(qū)域時位置誤差比有所增大。由圖6(b)可知本次實驗雷達(dá)采用的3251個位置中,92.7 %以上的位置誤差比小于1.9 %,平均位置誤差比為0.791 %。

而本次實驗的朝向誤差狀況如圖7所示。由圖7(a)可知雷達(dá)在無漏掃區(qū)域時整體誤差較小,而當(dāng)雷達(dá)處于漏掃區(qū)域時朝向誤差有所增大,該特點與位置誤差一致。由圖7(b)可知采用的3251個樣本點中,94.2 %以上的朝向誤差小于1°,最小朝向誤差幾乎為0,而平均朝向誤差為0.3883°。

圖6 位置定位誤差

圖7 朝向定位誤差

綜上,雷達(dá)在凸多邊形地圖內(nèi)或非凸多邊形地圖內(nèi)的無漏掃區(qū)域時,該算法的準(zhǔn)確性最高,而在非凸多邊形地圖內(nèi)的漏掃區(qū)域時算法精度有所下降。

7 結(jié)束語

本文提出了一種基于激光雷達(dá)的有界區(qū)域快速全局定位算法,為機(jī)器人在室內(nèi)使用激光雷達(dá)傳感器完成自身全局定位的需求提供了新思路和有效的算法方案。利用非凸多邊形可由多個凸多邊形疊加組合的原則對圖形進(jìn)行切分,并提取已知有界地圖的特征向量,將全局定位問題轉(zhuǎn)化為地圖邊界在車輛坐標(biāo)系與地面坐標(biāo)系上的特征向量匹配問題,實現(xiàn)了對激光雷達(dá)的精確全局定位。最終通過實驗驗證了該算法的有效性和準(zhǔn)確性,發(fā)現(xiàn)該定位算法平均位置誤差比為0.791 %,平均朝向誤差為0.3883°。