對激光雷達導引機器人路標定位的研究

趙華東，程蘇全，翟曉彤

（鄭州大學機械工程學院，河南 鄭州 450001）

1 引言

移動機器人的定位問題是指移動機器人通過傳感器信息估算自身位置和姿態(tài)的過程。文獻[1]對移動機器人定位所涉及的設備和基本方法做了闡述。作為移動機器人導航最基本環(huán)節(jié)，定位機器人在工作環(huán)境中相對于全局坐標的位姿[2]。相關領域的研究人員提出過多種定位方法，如航位推算法、慣性導航法、路標定位法等。航位推算法存在標定位置不便、抗干擾性差等缺陷[3]。慣性導航法較航位推算法的定位精度高，但是較航位推算法成本高[4]，也存在難于標定的問題。以上兩種方法均存在累積誤差，無法適用于精確定位的要求[5]。路標定位法是利用在移動機器人上安裝的傳感器掃描識別出放置在行駛路徑周邊環(huán)境中的一些具有明顯特征的人工路標[6]，再將識別出的路標與全局地圖中已知位置的路標進行匹配，之后再利用已知坐標位置反求移動機器人當前的位置，從而實現(xiàn)精確定位[7]。

激光雷達是激光技術與雷達技術相結合的產(chǎn)物[8]。激光測距主要有連續(xù)波測距和脈沖測距[9]兩種方法。在工業(yè)自動化和物流自動化的快速推動作用下，激光導引式移動機器人在柔性制造及智能倉儲領域得到廣泛應用[10]。

通過理論分析，提出了一種基于激光雷達傳感器和人工路標導引的機器人的定位方法，實現(xiàn)了精確定位移動機器人的目的，為激光雷達定位系統(tǒng)的大規(guī)模開發(fā)提供了理論基礎。

2 地圖構建

根據(jù)測量目標維數(shù)的不同，激光可以分為一維、二維和三維。一維激光通常稱為激光測距儀，只在單一方向上測距。二維、三維激光雷達都是在激光測距儀的基礎上，借助掃描系統(tǒng)（旋轉裝置）改變探測方向獲得二維、三維的點集。所采用的激光雷達為基于脈沖測距的二維激光雷達。

基于反射板定位的技術原理是在機器人行駛路徑的周圍安裝位置精確的激光反射板，通過激光雷達發(fā)射激光束，同時采集由反射板反射的激光束，來確定其當前的位置和航向。總體說來，定位過程主要有三步，第一步是從掃描到的點中識別出可能是反射板的點簇（可稱之為“準反射板”）[11]，第二步是計算出所有任意兩塊準反射間的距離并循環(huán)比較地圖中已知的任意兩塊反射板的距離來匹配反射板，第三步是通過已經(jīng)匹配的反射板的確切位置反向求得當前激光雷達所處的位置，即移動機器人的位置。

激光雷達獲得的數(shù)據(jù)量大，每旋轉一圈會獲得361個測量點，我們把這361個測量數(shù)據(jù)稱為一幀。每幀數(shù)據(jù)的點集為：Data=｛dk，θk｝，k=1，2，…361，dk為第 k 個反射點距激光雷達中心的直線距離，θk為該反射點激光雷達所建立的極坐標系的方位角。

在二維環(huán)境中，移動機器人的坐標通常用G（x，y）表示，為了便于研究，這里的坐標采用的是激光雷達中心的坐標，因為激光雷達相對移動機器人本體來說是相對固定的，后期可以通過非常簡單的運算轉化成移動機器人的實際坐標。定位的目的是算出每一時刻移動機器人的坐標（x，y）。

全局地圖指的是由一些有標志性位置的點所組成二維點陣圖形。在實際應用現(xiàn)場，激光反射板就是這些標志性位置點。建立全局地圖需要首先設定全局坐標的原點、X軸、Y軸，坐標系采用平面直角坐標系。然后，實地測出各個激光反射板在全局坐標系下的具體位置。最后，把測得的點繪制在這張圖上，就構成了移動機器人行走的全局地圖。對全局地圖中的其它位置，一律視為無特征區(qū)，理想化成空曠的場地。由此設定全局地圖模型為Mglobal=｛Pi｝，Pi=（xi，yi），i∈N，（xi，yi）表示第 i個反射板的坐標。

激光雷達會根據(jù)其掃描特性，建立一個由若干反射板組成的局部地圖。這個地圖是極坐標系，激光雷達的位置為原點。局部地圖模型為 Mlocal=｛Qj｝，Qj=（dj，θj），j∈N，（dj，θj） 表示第 j個反射板在激光雷達極坐標系中的位置。一般地，j

反射板的匹配過程即在Mglobal中尋找Mlocal的對應。

3 反射板檢測

在機器人行走過程中，激光雷達掃描環(huán)境得到了一組自身建立的極坐標下的點數(shù)據(jù)，需要從這些數(shù)據(jù)中提取出反射板。該過程分為兩個步驟：數(shù)據(jù)分割和特征提取。

3.1 數(shù)據(jù)分割

數(shù)據(jù)分割即點的聚類。由于激光雷達獲得的數(shù)據(jù)量大，每幀數(shù)據(jù)包括361個測量點，而其中部分測量點為不感興趣的點（不是反射板）或者是受環(huán)境影響而產(chǎn)生的噪點。這些點對下一步建立局部地圖不利，因此需要進行預處理。數(shù)據(jù)分割目的是將一幀原始測量點的數(shù)據(jù)分成若干個點簇。先假設一個定義：

對于相鄰兩點，如果激光雷達原點與這兩個點的距離差小于設定閾值時，那么這兩點可視為同屬一個物體。

首先求出相鄰兩點距激光雷達的幾何距離差，如果該差值在某一閾值內，不妨設該閾值為f，則可視為這兩個相鄰的點屬于同一個組。其判斷規(guī)則如下：

那么，如何確定閾值f呢?

根據(jù)激光特性，相鄰點與激光雷達中心連線的夾角為φ。即激光雷達的角度分辨率。在實際計算時，一般會出現(xiàn)傾斜，不妨把該傾斜的角度設為α。為了減少所分割部分對激光雷達到物體距離的依賴性，我們引入?yún)?shù)h，應在較長距離的dk（也有可能是dk+1）上設定一個所能允許的最大偏差h，不妨取垂點，如圖1所示。

圖1 聚類方法的原理圖Fig.1 Schematic of Clustering Method

設相鄰兩點的距激光雷達的較小值為dmin，則：

如上所述，不難得出：β=φ/2，h=tanβ·dmin·sinφ。

為了調節(jié)激光雷達的縱向誤差，可以通過經(jīng)驗值設定一個常數(shù)C1。故閾值f=C1+h。

綜上所述，得出聚類方法判定方法為：

其中，k=1，2，…，361。

通過該方法可以將原始的一幀激光雷達數(shù)據(jù)點（361個）分為若干個組，為進一步處理打下基礎。激光雷達數(shù)據(jù)分割流程，如圖2所示。

圖2 激光雷達數(shù)據(jù)分割流程圖Fig.2 Flow Chart of Clustering Method

對于一幀激光雷達數(shù)據(jù)，在聚類前的數(shù)據(jù)表示為Data=｛dk，θk｝，k=1，2，…361，聚類后則為：

式中：j=1，2，…，361；signj—第 j個點簇；（djb，θjb）—該點簇起始點的極坐標；（dje，θje）—該點簇結束點的極坐標；（dj，θj）—該點簇的中心極坐標；Lj—該點簇起始點和結束點之間的直線距離。

3.2 特征提取

從上面數(shù)據(jù)分割過程中，得到了j個如式（2）表示的物體。實際上，反射板便在這些物體中。反射板的形狀有平面和圓柱形狀?？紤]到要盡可能保證反射板在各個方向都能有良好的反射性能，選擇圓柱狀的反射板。

這里研究時時反射板為圓柱狀。便于研究及使用方便，圓柱反射板的半徑應統(tǒng)一，設為R0。為了調節(jié)激光反射板的徑向誤差，可以通過經(jīng)驗值設定一個常數(shù)C2。

判斷signj是否為反射板可以通過下式判斷：

滿足上式的點簇，可能便是一個反射板，不妨稱之為準反射板，記為Qj，得到局部地圖。反射板的坐標取點簇中心的坐標，點簇中心用起始點和結束點連線的中點代替，如圖3所示。

圖3 特征識別原理圖Fig.3 Schematic Diagram of Feature Recognition

之后，需要用準路標集合（局部地圖）與實際地圖（全局地圖）進行匹配。

3.3 準路標在全局地圖中的匹配

圖4 反射板匹配示意圖Fig.4 Schematic Diagram of Reflector Matching

準路標在全局地圖中的匹配有很多方法，這里給出一種切實可行的方法。為了盡可能減少算法復雜度，用任意兩塊反射板之間的距離作為匹配項實為上策。有一個重要的前提是可以被利用的，在機器人行走過程中，每一時刻的上一時刻的位姿是已知的，可以據(jù)此找到在該位置周圍的所有反射板，這樣可以大大縮短匹配所耗費的時間和計算復雜程度。該區(qū)域可稱之為最佳匹配區(qū)域。這種匹配原則可操作性強，只需要在開機時給定一個初始位姿，關機時保存位姿以備下次調用。反射板匹配示意，如圖4所示。只需要在全局地圖中匹配A、B、C三個反射板即可。

如何找到在最佳匹配區(qū)域內的路標呢？設某一時刻坐標為G0（x0，y0），激光雷達的可視范圍半徑為Rmax，根據(jù)實際經(jīng)驗設定誤差常數(shù)C3，激光雷達距路標的距離為t0，則有：

且滿足 Rmax-C3≤t0≤Rmax+C3。

這樣，我們找到了最佳匹配區(qū)域的所有路標，如下：Mbest=｛pt｝，pt=（xt，yt），t∈N。

經(jīng)過上述轉換，準路標在全局地圖中的匹配問題便可以簡化為準路標在最佳匹配區(qū)域路標的匹配，即Mlocal在Mbest中的匹配。實際上，經(jīng)過轉換，結合合理的路標布置，這個匹配速度和匹配準確度是非常高的。匹配過程可采用兩點距離比較循環(huán)比較法。不妨用Qj，j+m表示Mlocal中第j個與第j+m個準路標間的距離，用Pt，t+n表示Mbest中第t個和第t+n個路標之間的距離，設誤差常數(shù)為C4，則匹配算法可表示：

則認為準路標 Qj、Qj+m可能一一對應路標 Pt、Pt+n或 Pt+n、Pt。式中：m，n∈N。

重復式（6）進行匹配，則可以找到準路標與路標的一一對應關系，如圖5所示。

圖5 準路標與路標匹配示意圖Fig.5 Schematic Diagram of Road Sign Matching

找到對應關系后，此刻的坐標便可通過實際路標反向求得。

4 激光雷達在全局坐標中的坐標

由以上分析，將準路標Qj的坐標用與其對應路標Pt的全局坐標代替，根據(jù)三點定位法，以Pt（xt，yt）為圓心，以Qj（dj，θj）中dj為半徑作三個圓，交點坐標即為激光雷達中心的全局坐標，如圖6所示。

圖6 三點定位法示意圖Fig.6 Schematic Diagram of Three Point Positioning

列出公式如下：

上式解得兩個值，即兩個焦點：（x1，y1）和（x2，y2）。

接下來，我們需要找到三個圓的焦點，考慮到誤差，我們取最接近的兩個點的中點為G（x，y），列出方程組如下：

上式解得兩個值，即兩個焦點：（x3，y3）和（x4，y4）。

不妨用dij來表示點（xi，yi），i=1，2與點（xj，yj），j=3，4的距離，有：

依上式計算可得 d13，d14，d23，d24四個值，比較四者的大小，取最小值dmin，假定為dmn，則有：

綜上，求得 G（x，y）。

5 實驗分析

布置好實驗環(huán)境，單位為毫米，實驗環(huán)境，如圖7所示。其中A（15000，10000）、B（7000，3000）、C（13000，6000）三點為反射板，G（7000，6000）為激光雷達中心位置，并初始化激光雷達的初始角度為X軸方向，反射板直徑為300mm。實驗的目的是利用以上定位算法，計算出激光中心雷達的全局坐標G0（x0，y0），如果G0與給定的位置G（7000，6000）相等，則該定位方法可行。

圖7 實驗環(huán)境示意圖Fig.7 Schematic Diagram of Test Environmet

在同一位置，不同時間點，隨機抽取1幀數(shù)據(jù)。經(jīng)過數(shù)據(jù)分割，得到點云，如圖8所示。數(shù)據(jù)分割，剔除噪點，得到準反射板的點的數(shù)據(jù)，如表1所示。

圖8 實驗環(huán)境擬合點云Fig.8 Point Cloud of Test Environmet

表1 準反射板點Tab.1 Data of Reflector Plate

根據(jù)上述點云，擬合出反射板坐標，匹配反射板坐標，反射板擬合示意圖，如圖9所示。上述過程重復9次，計算得出9組G0，具體數(shù)據(jù)，如表2所示。得出激光雷達坐標為G0（7607.7，6013.8），因精確坐標為 G（7600，6000），二者直線距離為 13.8mm，<20mm，符合定位需求。

圖9 反射板擬合示意圖Fig.9 Schematic Diagram of Reflector Plate

表2 激光雷達坐標Tab.2 Data of Laser Radar

6 總結

（1）針對采用二維激光雷達定位的移動機器人，提出一種基于反射板的定位算法。本質上是利用路標之間距離為匹配特征，將檢測路標與實際路標進行匹配，提高了定位精度，并降低了路標檢測的算法復雜性。（2）提出了一種地圖構建方法以及路標的檢測方法，提高了算法應用的靈活性。為二維激光雷達定位系統(tǒng)的大規(guī)模開發(fā)提供一種理論基礎。