趙華東，程蘇全，翟曉彤

（鄭州大學(xué)機(jī)械工程學(xué)院，河南 鄭州 450001）

1 引言

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

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

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

2 地圖構(gòu)建

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

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

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

在二維環(huán)境中，移動(dòng)機(jī)器人的坐標(biāo)通常用G（x，y）表示，為了便于研究，這里的坐標(biāo)采用的是激光雷達(dá)中心的坐標(biāo)，因?yàn)榧す饫走_(dá)相對(duì)移動(dòng)機(jī)器人本體來說是相對(duì)固定的，后期可以通過非常簡單的運(yùn)算轉(zhuǎn)化成移動(dòng)機(jī)器人的實(shí)際坐標(biāo)。定位的目的是算出每一時(shí)刻移動(dòng)機(jī)器人的坐標(biāo)（x，y）。

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

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

反射板的匹配過程即在Mglobal中尋找Mlocal的對(duì)應(yīng)。

3 反射板檢測(cè)

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

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

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

對(duì)于相鄰兩點(diǎn)，如果激光雷達(dá)原點(diǎn)與這兩個(gè)點(diǎn)的距離差小于設(shè)定閾值時(shí)，那么這兩點(diǎn)可視為同屬一個(gè)物體。

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

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

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

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

設(shè)相鄰兩點(diǎn)的距激光雷達(dá)的較小值為dmin，則：

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

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

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

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

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

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

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

式中：j=1，2，…，361；signj—第 j個(gè)點(diǎn)簇；（djb，θjb）—該點(diǎn)簇起始點(diǎn)的極坐標(biāo)；（dje，θje）—該點(diǎn)簇結(jié)束點(diǎn)的極坐標(biāo)；（dj，θj）—該點(diǎn)簇的中心極坐標(biāo)；Lj—該點(diǎn)簇起始點(diǎn)和結(jié)束點(diǎn)之間的直線距離。

3.2 特征提取

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

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

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

滿足上式的點(diǎn)簇，可能便是一個(gè)反射板，不妨稱之為準(zhǔn)反射板，記為Qj，得到局部地圖。反射板的坐標(biāo)取點(diǎn)簇中心的坐標(biāo)，點(diǎn)簇中心用起始點(diǎn)和結(jié)束點(diǎn)連線的中點(diǎn)代替，如圖3所示。

圖3 特征識(shí)別原理圖Fig.3 Schematic Diagram of Feature Recognition

之后，需要用準(zhǔn)路標(biāo)集合（局部地圖）與實(shí)際地圖（全局地圖）進(jìn)行匹配。

3.3 準(zhǔn)路標(biāo)在全局地圖中的匹配

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

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

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

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

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

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

則認(rèn)為準(zhǔn)路標(biāo) Qj、Qj+m可能一一對(duì)應(yīng)路標(biāo) Pt、Pt+n或 Pt+n、Pt。式中：m，n∈N。

重復(fù)式（6）進(jìn)行匹配，則可以找到準(zhǔn)路標(biāo)與路標(biāo)的一一對(duì)應(yīng)關(guān)系，如圖5所示。

圖5 準(zhǔn)路標(biāo)與路標(biāo)匹配示意圖Fig.5 Schematic Diagram of Road Sign Matching

找到對(duì)應(yīng)關(guān)系后，此刻的坐標(biāo)便可通過實(shí)際路標(biāo)反向求得。

4 激光雷達(dá)在全局坐標(biāo)中的坐標(biāo)

由以上分析，將準(zhǔn)路標(biāo)Qj的坐標(biāo)用與其對(duì)應(yīng)路標(biāo)Pt的全局坐標(biāo)代替，根據(jù)三點(diǎn)定位法，以Pt（xt，yt）為圓心，以Qj（dj，θj）中dj為半徑作三個(gè)圓，交點(diǎn)坐標(biāo)即為激光雷達(dá)中心的全局坐標(biāo)，如圖6所示。

圖6 三點(diǎn)定位法示意圖Fig.6 Schematic Diagram of Three Point Positioning

列出公式如下：

上式解得兩個(gè)值，即兩個(gè)焦點(diǎn)：（x1，y1）和（x2，y2）。

接下來，我們需要找到三個(gè)圓的焦點(diǎn)，考慮到誤差，我們?nèi)∽罱咏膬蓚€(gè)點(diǎn)的中點(diǎn)為G（x，y），列出方程組如下：

上式解得兩個(gè)值，即兩個(gè)焦點(diǎn)：（x3，y3）和（x4，y4）。

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

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

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

5 實(shí)驗(yàn)分析

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

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

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

圖8 實(shí)驗(yàn)環(huán)境擬合點(diǎn)云Fig.8 Point Cloud of Test Environmet

表1 準(zhǔn)反射板點(diǎn)Tab.1 Data of Reflector Plate

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

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

表2 激光雷達(dá)坐標(biāo)Tab.2 Data of Laser Radar

6 總結(jié)

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