移動(dòng)平臺(tái)多傳感器組合導(dǎo)航系統(tǒng)設(shè)計(jì)

吳焱明，李 昂，李 正，仲?gòu)┝?/p>

(合肥工業(yè)大學(xué) 機(jī)械工程學(xué)院，安徽 合肥 230009)

0 引言

牽引變電所有很多高壓設(shè)備，人在進(jìn)行設(shè)備檢測(cè)時(shí)存在安全問(wèn)題，因此隨著科技的不斷進(jìn)步，實(shí)現(xiàn)無(wú)人化巡檢是電氣化鐵路發(fā)展的必然趨勢(shì)。當(dāng)前移動(dòng)平臺(tái)采用磁條導(dǎo)航的方式，在控制室路面上鋪設(shè)磁條，工控機(jī)通過(guò)磁導(dǎo)航傳感器采集磁條信息控制移動(dòng)平臺(tái)的運(yùn)動(dòng)。定位方式采用RFID技術(shù)，提前在地標(biāo)卡中寫(xiě)入信息，移動(dòng)平臺(tái)運(yùn)行時(shí)利用車(chē)載讀卡器讀取信息內(nèi)容，判斷當(dāng)前所處的位置。但是磁條導(dǎo)航方式存在脫軌的問(wèn)題，需要人為將移動(dòng)平臺(tái)推回軌道上。因此，本文作者結(jié)合視覺(jué)SLAM技術(shù)改進(jìn)移動(dòng)平臺(tái)的導(dǎo)航和定位系統(tǒng)，以實(shí)現(xiàn)變電所的無(wú)人化和自動(dòng)化巡檢。

1 移動(dòng)平臺(tái)的結(jié)構(gòu)

移動(dòng)平臺(tái)上方需搭載檢測(cè)機(jī)器人，考慮承載能力和操作的便利性，設(shè)計(jì)的機(jī)械結(jié)構(gòu)如圖1所示。車(chē)體長(zhǎng)為750 mm，寬為560 mm，高為400 mm。

1-后端殼；2-主動(dòng)輪；3-前端殼；4-電機(jī)；5-從動(dòng)輪；6-底盤(pán)

2 控制系統(tǒng)的硬件設(shè)計(jì)

移動(dòng)平臺(tái)組合導(dǎo)航系統(tǒng)采用以計(jì)算機(jī)為核心的上位機(jī)和以可編程邏輯控制器(Programmable Logic Controller，PLC)為核心的下位機(jī)聯(lián)合控制方案[1]。上位機(jī)采用研華PPC-3120S型工控機(jī)和裝有Ubuntu 16.04系統(tǒng)的筆記本。PLC與工控機(jī)之間采用Modbus TCP協(xié)議的網(wǎng)口通訊，與伺服驅(qū)動(dòng)器之間采用CAN總線(xiàn)通訊，因此采用臺(tái)達(dá)AS200系列的PLC[2]。雙目和單目相機(jī)需要依靠運(yùn)動(dòng)時(shí)的圖像特征匹配來(lái)計(jì)算深度，而深度相機(jī)可以直接測(cè)量距離，節(jié)省計(jì)算時(shí)間，因此選擇深度相機(jī)。Kinect V2相機(jī)在設(shè)備上的安裝圖如圖2所示。

磁導(dǎo)航傳感器采用歐米麥克MK-0216B系列，該系列傳感器的測(cè)量精度能達(dá)到±1 mm。除此之外，還有用于定位的EMR-05型RFID射頻傳感器。控制系統(tǒng)總體結(jié)構(gòu)框圖如圖3所示。

圖2 設(shè)備安裝圖 圖3 控制系統(tǒng)總體結(jié)構(gòu)框圖

3 控制系統(tǒng)軟件設(shè)計(jì)

移動(dòng)平臺(tái)控制系統(tǒng)分為上位機(jī)軟件和下位機(jī)軟件兩個(gè)部分。

3.1 下位機(jī)軟件設(shè)計(jì)

下位機(jī)軟件實(shí)現(xiàn)磁條導(dǎo)航時(shí)的點(diǎn)動(dòng)、旋轉(zhuǎn)和到目標(biāo)運(yùn)動(dòng)，視覺(jué)SLAM導(dǎo)航的自主運(yùn)動(dòng)，以及脫離軌道和重回軌道時(shí)導(dǎo)航方式的切換等功能。

3.2 上位機(jī)軟件設(shè)計(jì)

3.2.1 地圖構(gòu)建模塊

(1) ORB-SLAM2[3]算法：包括相機(jī)跟蹤、地圖構(gòu)建和回環(huán)檢測(cè)三個(gè)模塊。相機(jī)跟蹤模塊的作用是根據(jù)深度相機(jī)獲取到的信息估計(jì)位姿并檢測(cè)新的關(guān)鍵幀。地圖構(gòu)建模塊負(fù)責(zé)插入檢測(cè)到的新關(guān)鍵幀，經(jīng)過(guò)局部BA優(yōu)化后剔除不符合條件的地圖點(diǎn)和關(guān)鍵幀。回環(huán)檢測(cè)模塊的作用是檢測(cè)是否到達(dá)過(guò)該場(chǎng)景，消除系統(tǒng)的累計(jì)誤差。

(2) 稠密地圖構(gòu)建：ORB-SLAM2系統(tǒng)只能構(gòu)建稀疏地圖，無(wú)法用于導(dǎo)航和路徑規(guī)劃，因此改進(jìn)該算法實(shí)現(xiàn)稠密建圖。相機(jī)的成像過(guò)程涉及到三個(gè)坐標(biāo)平面，即相機(jī)坐標(biāo)平面、像素平面和物理成像平面。三者之間的關(guān)系如圖4所示。

圖4 相機(jī)模型

假設(shè)點(diǎn)P在相機(jī)坐標(biāo)系下的坐標(biāo)Pc為[X,Y,Z]T，投影在物理成像平面上的坐標(biāo)為[X′,Y′,Z′]T。設(shè)相機(jī)的焦距為f,則：

(1)

像素平面相對(duì)于物理成像平面在水平方向上縮放了α倍，豎直方向縮放了β倍，原點(diǎn)平移了[cx,cy]T。把αf記作fx,βf記作fy，可以推出點(diǎn)P在像素平面下的坐標(biāo)[u,v]T：

(2)

其中:fx、fy、cx和cy為相機(jī)內(nèi)參。

RGB-D相機(jī)采集圖像的像素坐標(biāo)值，測(cè)量目標(biāo)的深度d，根據(jù)式(2)，相機(jī)坐標(biāo)Pc可以表示為：

(3)

其中:s為深度比例系數(shù)。由ORB-SLAM2算法的相機(jī)跟蹤模塊估計(jì)出相機(jī)的外參，即相機(jī)坐標(biāo)系變換到世界坐標(biāo)系的旋轉(zhuǎn)矩陣R和平移向量t，因此可推出P點(diǎn)的世界坐標(biāo)：

Pw=RPc+t.

(4)

首先采用點(diǎn)云庫(kù)[4]中PointXYZRGBA類(lèi)型存儲(chǔ)世界坐標(biāo)值和顏色信息，然后再進(jìn)行點(diǎn)云拼接即可得到點(diǎn)云地圖。經(jīng)體素濾波器[5]優(yōu)化后，保證了在某一個(gè)確定的體素內(nèi)僅有一個(gè)點(diǎn)，減少了內(nèi)存空間的占用。

(3) 八叉樹(shù)地圖[6]構(gòu)建：點(diǎn)云地圖只能添加地圖點(diǎn)，不能動(dòng)態(tài)地移除某一時(shí)刻消失在地圖中的點(diǎn)，因此不能用于導(dǎo)航和路徑規(guī)劃。本文采用八叉樹(shù)地圖解決上述問(wèn)題，它是一種三維柵格地圖，可以動(dòng)態(tài)地描述節(jié)點(diǎn)的占據(jù)狀態(tài)。

3.2.2 導(dǎo)航模塊

基于ROS平臺(tái)設(shè)計(jì)視覺(jué)SLAM導(dǎo)航系統(tǒng)。全局路徑規(guī)劃采用A*算法[7]計(jì)算從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑。局部路徑規(guī)劃采用時(shí)間彈性帶算法[8]躲避障礙物，并計(jì)算出該行駛周期內(nèi)的角速度和線(xiàn)速度。

如果移動(dòng)平臺(tái)脫離磁條軌道，則導(dǎo)航系統(tǒng)切換至視覺(jué)SLAM導(dǎo)航，以系統(tǒng)中預(yù)設(shè)的軌道上的點(diǎn)為目標(biāo)點(diǎn)，自主導(dǎo)航回到軌道。

4 基于ROS的算法測(cè)試

移動(dòng)平臺(tái)要搭載檢測(cè)機(jī)器人進(jìn)行巡檢，現(xiàn)場(chǎng)平面布局如圖5所示。其中Pi(i=1,…,6)為SLAM導(dǎo)航的目標(biāo)點(diǎn)，其余為各個(gè)巡檢位置。

圖5 工作環(huán)境平面布局

基于ROS系統(tǒng)測(cè)試改進(jìn)的ORB-SLAM2算法，濾波后的稠密點(diǎn)云地圖和八叉樹(shù)地圖如圖6所示。

圖6 供電段環(huán)境地圖

圖6(a)是濾波后的稠密點(diǎn)云地圖，大小為264.7 MB，圖6(b)為相對(duì)應(yīng)的八叉樹(shù)地圖，大小為236.3 kB。投影在二維平面的柵格地圖如圖7所示。

圖7 二維柵格地圖

5 設(shè)備調(diào)試

首先，設(shè)定好目標(biāo)位置g，移動(dòng)平臺(tái)通過(guò)磁條導(dǎo)航的方式運(yùn)行，在圖8所示位置脫離磁條軌道。

圖8 導(dǎo)航示意圖

當(dāng)磁導(dǎo)航傳感器檢測(cè)到移動(dòng)平臺(tái)脫離磁條，切換為視覺(jué)SLAM導(dǎo)航方式，自主運(yùn)行到軌道前方較近的SLAM導(dǎo)航目標(biāo)點(diǎn)P5。調(diào)整好位姿之后，視覺(jué)SLAM導(dǎo)航方式結(jié)束，磁導(dǎo)航傳感器讀取到信息，切換回磁導(dǎo)航方式，繼續(xù)運(yùn)行至目標(biāo)位置。

6 結(jié)束語(yǔ)

目前，基于視覺(jué)SLAM和磁條的移動(dòng)平臺(tái)組合導(dǎo)航系統(tǒng)已經(jīng)開(kāi)發(fā)完成。經(jīng)測(cè)試，磁條導(dǎo)航脫離磁條軌道后，切換為SLAM導(dǎo)航方式可以自主導(dǎo)航回到軌道。該系統(tǒng)實(shí)現(xiàn)了變電站巡檢的無(wú)人化、自動(dòng)化和智能化，提高了巡檢效率。