基于回波強(qiáng)度的AGV 重定位方法研究

陳遠(yuǎn)浩，吳明暉

（上海工程技術(shù)大學(xué) 機(jī)械與汽車工程學(xué)院，上海 201620）

0 引言

移動機(jī)器人技術(shù)廣泛應(yīng)用于各個場景，如服務(wù)機(jī)器人、農(nóng)業(yè)機(jī)器人、工業(yè)物流等等。自動引導(dǎo)車（Automated Guided Vehicle，AGV）是一種用于物料搬運(yùn)的移動機(jī)器人，激光同時定位與建圖（Simultaneous Localization and Mapping，SLAM）是其核心技術(shù)之一。其中，2D 激光SLAM 按照地圖形式分類，常用的有基于柵格地圖SLAM 和基于特征地圖SLAM 等。

在許多現(xiàn)實(shí)應(yīng)用中，會出現(xiàn)AGV 多樓層導(dǎo)航的需求。但是由于傳感器的限制，2D 激光SLAM 只能建立多個單樓層地圖，而無法直接建立多樓層地圖。目前，一些解決多樓層地圖的方法多應(yīng)用于有電梯的場景。例如：AGV 導(dǎo)航至電梯后，通過基于機(jī)器視覺識別電梯按鈕，并控制車載操作臂按下目標(biāo)樓層按鈕；到達(dá)目標(biāo)樓層后，AGV 自動定位至另一樓層地圖的初始點(diǎn)。然而在具有坡道的多樓層廠區(qū)環(huán)境中，該方法并不適用于這種應(yīng)用場合。由于AGV 進(jìn)行爬坡或下坡后，累積誤差無法快速精準(zhǔn)地定位到另一樓層的初始位置，從而影響AGV 在新樓層的導(dǎo)航。針對上述問題，通過重定位的方法能夠進(jìn)行誤差修正。目前重定位方法主要分為基于激光雷達(dá)和基于視覺傳感器，但兩者都存在定位時間過長等問題。為解決具有坡道的多樓層廠區(qū)環(huán)境中AGV 的重定位問題，本文提出一種基于回波強(qiáng)度的AGV 重定位方法，以解決AGV 爬坡或下坡后在另一樓層的重定位問題。多樓層導(dǎo)航整體流程如圖1 所示。

圖1 多樓層導(dǎo)航流程Fig.1 Multi-floor navigation process

1 基于回波強(qiáng)度的激光雷達(dá)數(shù)據(jù)處理

1.1 數(shù)據(jù)過濾方法

2D 激光雷達(dá)傳感器是AGV 導(dǎo)航的關(guān)鍵傳感器。激光雷達(dá)在工作時，發(fā)射激光信號到目標(biāo)，并經(jīng)過目標(biāo)散射后再接收。激光雷達(dá)根據(jù)激光信號的飛行時間，計(jì)算得出激光雷達(dá)到該目標(biāo)的距離、以及激光雷達(dá)轉(zhuǎn)過的角度，最終得出目標(biāo)相對于激光雷達(dá)的極坐標(biāo)。目前，市面上中、高端的2D 激光雷達(dá)不僅能夠得出目標(biāo)距離，還能測得目標(biāo)的回波強(qiáng)度。激光雷達(dá)的回波功率公式見如下：

其中，P為回波功率；P為發(fā)射功率；D為激光雷達(dá)接受孔徑；為物體表面反射率；為入射角；為目標(biāo)到激光雷達(dá)的距離； η為系統(tǒng)傳輸率；η為大氣傳輸率。

由此可見，回波強(qiáng)度主要與物體表面的反射率及距離有關(guān)。為了得出有效的激光雷達(dá)數(shù)據(jù)，采用3M 鉆石級反光膜制作平板型反光板，因此激光雷達(dá)每一幀數(shù)據(jù)中都會有來自該反射板返回的回波強(qiáng)度偏高的數(shù)據(jù)。同時設(shè)置激光雷達(dá)有效探測距離及有效回波強(qiáng)度閾值，獲得反光板的激光掃描數(shù)據(jù)。圖2 為反光板布置的實(shí)際環(huán)境；圖3（a）、圖3（b）分別為激光雷達(dá)數(shù)據(jù)處理前、后效果?？梢?，基于上述方法可將激光雷達(dá)數(shù)據(jù)進(jìn)行有效過濾，得到反光板的激光掃描數(shù)據(jù)。

圖2 反光板環(huán)境布置Fig.2 Reflector layout environment

圖3 激光雷達(dá)數(shù)據(jù)處理Fig.3 Lidar data processing

1.2 基于DBSCAN 的激光雷達(dá)聚類

完成基于回波強(qiáng)度的激光雷達(dá)數(shù)據(jù)過濾后，為了進(jìn)一步識別不同反光板的ID，需要對激光雷達(dá)數(shù)據(jù)進(jìn)行聚類。具有噪聲的基于密度的聚類方法（Density-Based Spatial Clustering of Applications with Noise，DBSCAN），是經(jīng)典的基于密度的聚類方法。該算法不需要先驗(yàn)的聚類蔟數(shù)量，就能發(fā)現(xiàn)噪聲環(huán)境下不同形狀的聚類簇，通過設(shè)定2 個參數(shù)：全局鄰域半徑E和最小近鄰個數(shù)M，就能達(dá)到很好的聚類效果。

反光板的數(shù)據(jù)特點(diǎn)為：不同反光板的點(diǎn)緊密相連成一類，從而得到了不同的聚類蔟；不同類的數(shù)據(jù)密度均勻，變化不大。以上數(shù)據(jù)特征反映了DBSCAN 適用于該問題的求解。由圖3（c）可見，通過DBSCAN，對反光板數(shù)據(jù)進(jìn)行了正確聚類。

2 反光板局部坐標(biāo)求解及AGV 重定位

2.1 反光板位置擬合

圖4 反光板最小二乘法擬合Fig.4 The least squares fitting of the reflector

2.2 AGV 重定位方法

重定位方法流程如圖5 所示。由圖5 可知，AGV 多樓層重定位方法實(shí)現(xiàn)步驟如下：

圖5 重定位方法流程圖Fig.5 Relocalization method flowchart

在已知反光板設(shè)計(jì)布局的情況下，加載世界坐標(biāo)系下的所有反光板位置。

加載激光雷達(dá)掃描數(shù)據(jù)。

基于回波強(qiáng)度識別反光板數(shù)據(jù)，并基于DBSCAN 及最小二乘法計(jì)算掃描到反光板的數(shù)目及距離信息。

將掃描到的反光板信息與已知布局做匹配，為了快速匹配，反光板布局兩兩距離各不相等，從而得到反光板到機(jī)器人本體的局部坐標(biāo)。

基于匹配結(jié)果，計(jì)算掃描到的反光板在世界坐標(biāo)系下的位置與機(jī)器人的定位信息。

AGV 在新樓層世界坐標(biāo)系下的定位，采用基于幾何關(guān)系的定位方法。對此實(shí)現(xiàn)過程擬給出分述如下。

（1 ）通過激光雷達(dá)掃描到的反光板兩兩之間的距離，識別反光板ID。

（2）求解各個反光板ID 的幾何質(zhì)心，對反光板質(zhì)心矩陣進(jìn)行旋轉(zhuǎn)平移，還原對應(yīng)反光板在世界坐標(biāo)系的位置。通過求解奇異陣的方法，獲得旋轉(zhuǎn)變量和平移變量的最優(yōu)解，此時用到的公式分別見如下：

其中，為已知布局的反光板數(shù)據(jù)，為掃描到的反光板數(shù)據(jù)；式（2）為世界坐標(biāo)系下，反光板質(zhì)心位置與掃描所得反光板質(zhì)心位置的關(guān)系矩陣表達(dá)式；cen和cen為幀激光數(shù)據(jù)求得反光板質(zhì)心點(diǎn)的均值。式（3）～（6）為SVD 求解最優(yōu)和的過程。其中，式（3）是對矩陣進(jìn)行SVD 分解，式（4）、（5）計(jì)算最優(yōu)旋轉(zhuǎn)變量和平移變量，式（6）求得最優(yōu)，并得到機(jī)器人在世界坐標(biāo)系的位置坐標(biāo)。

3 實(shí)驗(yàn)結(jié)果分析

為模擬AGV 在爬坡到新樓層后產(chǎn)生累計(jì)誤差、并通過反光板進(jìn)行重定位的實(shí)際場景，通過gmapping激光SLAM 算法建立新樓層的環(huán)境地圖；再人為地將AGV 放置在偏離初始位置的地方；最后通過基于回波強(qiáng)度的重定位方法進(jìn)行重定位，記錄觀察結(jié)果并驗(yàn)證方法的可行性。

實(shí)驗(yàn)地點(diǎn)選擇在上海工程技術(shù)大學(xué)實(shí)訓(xùn)5 號樓二樓，實(shí)際環(huán)境及建立的柵格地圖如圖6 所示。在初始定位附近布置了3 塊兩兩距離不同的反光板；AGV 為兩輪差速式AGV；激光雷達(dá)型號為鐳神N301，其有效探測距離為0.15～30 m，角度分辨率為0.18°，該激光雷達(dá)可以識別物體回波強(qiáng)度信息，完全滿足實(shí)驗(yàn)需求。

圖6 新樓層環(huán)境布置Fig.6 Environment layout of new floor

將AGV 放置在偏離初始位的位置，模擬AGV爬坡后的累積誤差，并初始化新樓層地圖，此后再進(jìn)行重定位。圖7 為AGV 重定位效果圖。由圖7 可以看出，由于累積誤差重定位前黃色激光雷達(dá)掃描數(shù)據(jù)與柵格地圖實(shí)際環(huán)境不匹配，經(jīng)過重定位后激光雷達(dá)掃描數(shù)據(jù)與柵格地圖基本匹配，說明定位成功，AGV 可以進(jìn)行后續(xù)的導(dǎo)航工作。研究得到的重定位誤差見表1。

圖7 重定位效果Fig.7 Relocalization effect

表1 重定位誤差Tab.1 Relocalization error

4 結(jié)束語

針對AGV 在具有坡道的多樓層導(dǎo)航過程中新樓層的重定位問題，本文設(shè)計(jì)了一種基于回波強(qiáng)度的AGV 重定位方法。首先基于反光板的高回波強(qiáng)度的特性，提取反光板的信息，通過DBSCAN 及最小二乘法對數(shù)據(jù)集進(jìn)行聚類擬合、并計(jì)算反光板的幾何質(zhì)心，然后匹配預(yù)設(shè)的反光板信息，最后通過幾何關(guān)系解算AGV 在地圖世界坐標(biāo)系下的定位。通過實(shí)驗(yàn)驗(yàn)證，使用該方法進(jìn)行重定位效果良好，精度高，具有實(shí)際應(yīng)用價值。接下來的工作將進(jìn)行AGV 多樓層導(dǎo)航系統(tǒng)的程序設(shè)計(jì)，以進(jìn)一步提升方法的實(shí)用性。