王鵬程， 徐世許， 耿延龍， 張志勇

(1.青島大學 自動化學院，山東 青島 266071； 2.青島卷煙廠，山東 青島 266101)

0 引 言

某工廠電纜溝線路承擔整個工廠的供電任務,需要定期檢查線路溫度,以防發(fā)生危險,影響工廠工業(yè)生產(chǎn)。為解決這一問題,研發(fā)自動巡檢系統(tǒng),能夠定時定點檢查設備運行情況,保證設備線路安全,及時發(fā)現(xiàn)設備故障。在工業(yè)復雜環(huán)境中,人工巡檢費事費力,存在危險和空間局限等問題。應用巡檢機器人能夠將人解放出來,并且能高效可靠地執(zhí)行巡檢任務。機器人高精度導航定位是完成巡檢任務的關鍵,直接影響機器人運動的準確性與巡檢工作效率[1,2]。

目前,巡檢機器人定位導航方式大多采用有軌方式或全球定位系統(tǒng)(global positioning system,GPS)定位方式。有軌方式需要鋪設預設軌道,對工廠環(huán)境破壞較大，成本較高,GPS定位方式精度較低,不能滿足在室內狹小空間的精準定位。因此,以激光雷達為核心的定位導航方式是一種高精度、高實時性、成本低廉的解決方案。

本文設計了一種激光雷達巡檢機器人導航系統(tǒng)，建立了環(huán)境地圖模型，并運用擴展卡爾曼濾波(extended Kalman filtering，EKF)算法實時定位和預測走向。

1 系統(tǒng)構成

巡檢系統(tǒng)主要由巡檢機器人、工廠局域網(wǎng)、無線路由器和中控遠程計算機組成。機器人用激光雷達采集周邊信息透傳到WiFi模塊,點云數(shù)據(jù)通過無線路由器尋找中控計算機IP,將點云數(shù)據(jù)發(fā)送到上位機軟件中,待上位機后期處理。系統(tǒng)結構如圖1(a)所示。

巡檢機器人作為系統(tǒng)的執(zhí)行機構,部署在現(xiàn)場完成日常巡檢、網(wǎng)絡通信和數(shù)據(jù)處理等功能,搭載?？低暰W(wǎng)絡攝像機、思嵐激光雷達、12 V直流電動機、STM32F103控制板、里程計等巡檢所需傳感器[3,4]。如圖1(b)所示。

圖1 系統(tǒng)結構與巡檢機器人

2 雷達建圖

2.1 Hough變換檢測直線

Hough變換是根據(jù)數(shù)學對偶性提出的,廣泛應用于對圖像進行直線檢測,其基本思想是交換方程中參數(shù)與變量,將原坐標系中直線檢測的問題轉換為參數(shù)坐標系下的峰值問題。本文以雷達掃描出的云點數(shù)據(jù)為基礎,找出共線點數(shù)據(jù),并對描述物體數(shù)據(jù)進行降噪[5]。

由圖2可以看出,原空間中的一條直線在極坐標下(p,θ)只有與之對應,改變p或θ的參數(shù)值,對應原空間中的直線相應發(fā)生改變。假設原空間中一條直線上的三個點為(xi,yi),其中，i=1,2,3,對于經(jīng)過每一個點的所有直線,在極坐標系下在極坐標下的三條曲線都會經(jīng)過同一個點。

圖2 原空間共線點與極坐標系曲線

極坐標系下的三條曲線同時經(jīng)過一點(p′,θ′),說明在原空間內有三個共線點,這三個點確定原空間內的一條直線[6]。

建立一個具有累加功能的二維矩陣,根據(jù)適當精度將參數(shù)空間量化為若干個小格,每個小格相當于一個累加器。對于原空間中的點(xi,yi),i=1,2,…n,n為原空間中數(shù)據(jù)的個數(shù),將原空間中的點(xi,yi)轉換為極坐標系下的n條正弦線。正弦線經(jīng)過一個小格,小格對應累加器加一。過同一小格的全部正弦線對應原空間中的某直線的所有近似共線點,小格累加器的數(shù)值即為原空間的共線點個數(shù),找出數(shù)值最大或高于設置閾值的累加器,該累加器就對應原始空間中的一條直線,累加器數(shù)值較小的小格為非共線點或噪聲,舍棄不用,取到的點為近似共線點[7]。

2.2 最小二乘法擬合直線

激光雷達掃描的數(shù)據(jù)經(jīng)過直線檢測后得到一組垂直坐標系下的近似共線點數(shù)據(jù),這些數(shù)據(jù)是對周邊環(huán)境中存在的直線的粗略估計,要得到精確的線段估計,需要對檢測后的數(shù)據(jù)點進行最小二乘法直線擬合[8,9]。假設Hough變換之后得到垂直坐標系下的有序數(shù)據(jù)集為S={(xi,yi)|i=1,2,…n},以垂直坐標系下一條直線為例,這些離散的數(shù)據(jù)點分布在直線方程附近。則這些離散點可以表示為如式(1)所示

y=f(x)+ξ

(1)

式中f(x)假設為擬合之后的直線方程,ξ為數(shù)據(jù)點距離直線方程的豎直距離。最小誤差平方和可以表示為

(2)

式中M為最小誤差平方和,直線方程f(x)用斜截式來表示。

根據(jù)極值定理,分別對k和b求導,可得k和b的值,從而確定擬合直線方程f(x)。求解過程如下

(3)

3 EKF定位導航

解決定位導航問題的方法有很多種,主要有模型預測控制(model predictive control,MPC)、卡爾曼濾波預測控制等。

模型預測控制是車輛自主導航的常用算法,多用于自動駕駛領域,此算法是基于模型做出預測并迅速決策的控制方式,其魯棒性強,模型誤差和環(huán)境干擾也能及時矯正,但計算量較大,采樣周期長,不能很好解決非線性問題,不符合時變、實時應用系統(tǒng)的需求。

EKF是解決定位導航問題的一種經(jīng)典算法,應用于對非線性系統(tǒng)的估計,是目前機器人自主導航比較常用的方法,主要包括狀態(tài)預測、狀態(tài)更新等環(huán)節(jié)[10]。巡檢機器人在工作過程中,當前時刻的位置信息需要通過上一個時刻的位置信息來預測,并通過激光雷達觀測當前時刻的環(huán)境信息,同步更新機器人的狀態(tài)信息,如此迭代運行。通過分析仿真實驗的結果,本文選用卡爾曼濾波作為機器人定位導航算法。

3.1 機器人運動模型

(4)

式中ρk,βk為機器人坐標系特征點坐標;nk為觀測激勵噪聲;若此時機器人為勻速運動,行進速度為vk,其角度偏轉為θk。則其運動模型如式(5)所示

(5)

(6)

在SLAM中的所有變量均是正態(tài)分布,系統(tǒng)過程噪聲Wk和傳感器觀測噪聲nk的均值為0,方差分別為Qk和Rk,同時滿足條件如式(7)所示

(7)

3.2 狀態(tài)預測

(8)

將其協(xié)方差矩陣pk用分塊矩陣形式表示,如式(9)所示

(9)

3.3 狀態(tài)更新

(10)

巡檢機器人在運行的過程中,通過關聯(lián)特征數(shù)據(jù)來判斷當前環(huán)境特征是否與觀測特征和預測特征相對應,進而更新系統(tǒng)的狀態(tài)。如果在環(huán)境中發(fā)現(xiàn)了新的特征,則需要更新地圖信息[11]。

(11)

更新狀態(tài)向量,加入新的特征點,狀態(tài)向量的增廣矩陣如式(12)所示

(12)

更新后其協(xié)方差矩陣如式(13)所示

(13)

更新后k時刻下的狀態(tài)估計與協(xié)方差矩陣估計作為輸入,預測k+1時刻的狀態(tài),并迭代運行。

4 實驗與仿真

為檢驗Hough變換檢測直線、最小二乘法擬合直線算法的有效性,在實驗室環(huán)境中進行模擬實驗,在測試環(huán)境中放置一個紙箱,模擬實際環(huán)境中的障礙物,紙箱尺寸為35 cm×25 cm×35 cm,機器人使用激光雷達掃描周邊環(huán)境,建立簡易環(huán)境地圖,測試結果如圖3所示。

圖3 建圖測試

在圖3中,當測試環(huán)境里有障礙物時,激光雷達能掃描出障礙物的位置和輪廓,在測試過程中障礙物的高度應高于機器人激光雷達位置。

為驗證EKF對巡檢機器人定位導航的有效性,對其進行仿真實驗,建立機器人運動模型,設置環(huán)境障礙物,模擬所需避障環(huán)境,使用MATLAB 2016a進行EKF 即時定位與地圖構建(simultaneous localization and mapping,SLAM)算法仿真,巡檢機器人運動模型的仿真數(shù)據(jù)設置為：巡檢環(huán)境長為70 m,巡檢環(huán)境寬為60 m,機器人運行速度為1.5 m/s,驅動齒輪間距為0.25 m,機器人運動頻率為40 Hz,激光雷達觀測頻率為20 Hz。仿真結果如圖4所示。

圖4 仿真結果

在70 m×60 m的巡檢環(huán)境中,設置了42個環(huán)境特征點和8個路標點,巡檢機器人從起始位置開始,以1.5 m/s的速度依次通過8個路標點,設置機器人的最大轉角為30°,轉角誤差為8°,速度誤差為0.2 m/s,激光雷達的最大觀測距離為8 m,誤差為0.15 m,觀測角度誤差為1°,在仿真過程中誤差將隨機產(chǎn)生。EKF SLAM算法仿真效果如圖4(a)所示。

圖4(a)中,*代表障礙物的特征點,實心圓代表機器人運行環(huán)境中的路標,虛線代表實際路徑,實線表示估計路徑,空心圓代表巡檢機器人,圓心為當前機器人的位置,大圓圈表示激光雷達的掃描范圍,與機器人相連接的“*”表示被觀測到的特征點。通過圖4(a)可以看出,巡檢機器人應用EKF算法,在整個運行過程中定位都比較精確,與機器人的實際路徑幾乎完全重合。巡檢機器人的位姿誤差,如圖4(b)所示。

分析誤差數(shù)據(jù)可得,基于EKF SLAM算法的預測精度較高,機器人運行過程中共采樣1 133次,機器人在方向上的誤差小于2 m,在方向上的誤差小于1 m,偏移角誤差小于0.1°。誤差數(shù)據(jù)分析如表1所示。

表1 誤差數(shù)據(jù)分析

EKF相較于傳統(tǒng)的傳感器模型預測控制優(yōu)勢十分顯著,具有更高的預測精度,而且在實際問題中多為非線性數(shù)據(jù),EKF在處理非線性問題上更具優(yōu)勢。圖4(c)為相同測試條件下,模型預測控制與卡爾曼濾波控制的仿真效果對比圖。其中,虛線表示模型預測控制軌跡,實線表示EKF預測軌跡,從仿真效果看,模型預測控制的預測精度較差,EKF更符合實驗預期。

圖4(d)表示模型預測控制與EKF的位置誤差變化。EKF誤差較小,控制在合理范圍內；模型預測控制誤差較大,控制效果不理想。

5 結 論

通過本文仿真數(shù)據(jù)可以看出,巡檢機器人在工作過程中應用EKF SLAM方法具有較好的定位精度,預測路徑與真實路徑誤差較小,誤差控制在合理范圍內,理論上可以滿足巡檢機器人自動巡檢的需求。