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

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

0 引 言

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

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

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

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

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

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

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

2 雷達(dá)建圖

2.1 Hough變換檢測(cè)直線(xiàn)

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

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

圖2 原空間共線(xiàn)點(diǎn)與極坐標(biāo)系曲線(xiàn)

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

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

2.2 最小二乘法擬合直線(xiàn)

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

y=f(x)+ξ

(1)

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

(2)

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

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

(3)

3 EKF定位導(dǎo)航

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

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

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

3.1 機(jī)器人運(yùn)動(dòng)模型

(4)

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

(5)

(6)

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

(7)

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

(8)

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

(9)

3.3 狀態(tài)更新

(10)

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

(11)

更新?tīng)顟B(tài)向量,加入新的特征點(diǎn),狀態(tài)向量的增廣矩陣如式(12)所示

(12)

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

(13)

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

4 實(shí)驗(yàn)與仿真

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

圖3 建圖測(cè)試

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

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

圖4 仿真結(jié)果

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

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

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

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

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

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

5 結(jié) 論

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