基于激光雷達(dá)的巡檢機(jī)器人控制與定位

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

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

0 引 言

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

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

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

1 系統(tǒng)構(gòu)成

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

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

圖1 系統(tǒng)結(jié)構(gòu)與巡檢機(jī)器人

2 雷達(dá)建圖

2.1 Hough變換檢測直線

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

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

圖2 原空間共線點與極坐標(biāo)系曲線

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

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

2.2 最小二乘法擬合直線

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

y=f(x)+ξ

(1)

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

(2)

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

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

(3)

3 EKF定位導(dǎo)航

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

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

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

3.1 機(jī)器人運動模型

(4)

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

(5)

(6)

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

(7)

3.2 狀態(tài)預(yù)測

(8)

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

(9)

3.3 狀態(tài)更新

(10)

巡檢機(jī)器人在運行的過程中,通過關(guān)聯(lián)特征數(shù)據(jù)來判斷當(dāng)前環(huán)境特征是否與觀測特征和預(yù)測特征相對應(yīng),進(jì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é)方差矩陣估計作為輸入,預(yù)測k+1時刻的狀態(tài),并迭代運行。

4 實驗與仿真

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

圖3 建圖測試

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

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

圖4 仿真結(jié)果

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

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

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

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

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

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

5 結(jié) 論

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