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

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

1 引 言

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

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

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

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

2 問(wèn)題的描述

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

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

(1)

3 地圖的特征描述

在地圖只有邊界的情況下,任意一個(gè)凸多邊形地圖都可以采用一個(gè)特征向量來(lái)對(duì)其進(jìn)行描述,每個(gè)凸多邊形對(duì)應(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)點(diǎn)的距離和方向角;Lmin,αmin分別表示重心到最近點(diǎn)的距離和方向角。αi,Li為αmax,αmin之間分割出來(lái)的第i個(gè)角及重心到該方向角上邊界點(diǎn)的距離。

圖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個(gè)頂點(diǎn)的全局坐標(biāo);Di表示第i個(gè)頂點(diǎn)與其下一個(gè)頂點(diǎn)的歐氏距離。

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

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

(5)

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

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

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

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

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

(6)

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

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

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

(7)

(8)

(9)

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

由此,提取出局部地圖中的特征信息(如重心坐標(biāo)、重心與邊界點(diǎn)的距離以及方向角)后,可以構(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á)采集的點(diǎn)云數(shù)據(jù)特征,將其與數(shù)據(jù)庫(kù)中事先存儲(chǔ)的全局地圖特征數(shù)據(jù)進(jìn)行匹配,只需找出全局地圖與局部地圖中一個(gè)對(duì)應(yīng)的特征點(diǎn)——重心,并利用重心與特定邊界點(diǎn)的方向角,最終實(shí)現(xiàn)車輛在有界區(qū)域內(nèi)的快速全局定位,大大減少了計(jì)算量,從而降低了定位算法的復(fù)雜度。

5 非凸多邊形切分

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

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

圖5 非凸多邊形圖形切分

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

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

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

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

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

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

6 實(shí)驗(yàn)驗(yàn)證

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

本文定位誤差的評(píng)估通過(guò)位置誤差比和朝向誤差來(lái)描述。位置誤差比的定義如式(13)所示:

(13)

(14)

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

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

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

圖6 位置定位誤差

圖7 朝向定位誤差

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

7 結(jié)束語(yǔ)

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