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

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

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

1 引 言

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

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

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

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

2 問題的描述

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

圖1 坐標系轉換關系

(1)

3 地圖的特征描述

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

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

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

(2)

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

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

4 定位算法

圖3 全局定位算法流程

4.1 預構建全局地圖特征向量

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

(3)

(4)

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

4.2 數據掃描與預處理

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

(5)

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

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

圖4 數據預處理基本流程圖

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

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

(6)

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

4.3 構建局部地圖特征向量

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

(7)

(8)

(9)

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

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

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

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

(10)

4.4 特征向量匹配定位

(11)

(12)

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

5 非凸多邊形切分

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

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

圖5 非凸多邊形圖形切分

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

改進后的特征匹配方法解決了非凸多邊形地圖內雷達會出現漏掃的現象,其具體步驟如下所示:

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

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

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

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

6 實驗驗證

為了高效地驗證本文算法的有效性及其定位精度,本文在進行全局定位測試時,將預設好的有界多邊形地圖導入,在MATLAB上通過仿真實驗模仿不同位姿時二維單線激光雷達在地圖內的掃描過程,并運用本文提出的全局定位算法解算出車輛的坐標和朝向。

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

(13)

(14)

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

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

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

圖6 位置定位誤差

圖7 朝向定位誤差

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

7 結束語

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