移動平臺多傳感器組合導航系統設計

吳焱明，李 昂，李 正，仲彥霖

(合肥工業大學 機械工程學院，安徽 合肥 230009)

0 引言

牽引變電所有很多高壓設備，人在進行設備檢測時存在安全問題，因此隨著科技的不斷進步，實現無人化巡檢是電氣化鐵路發展的必然趨勢。當前移動平臺采用磁條導航的方式，在控制室路面上鋪設磁條，工控機通過磁導航傳感器采集磁條信息控制移動平臺的運動。定位方式采用RFID技術，提前在地標卡中寫入信息，移動平臺運行時利用車載讀卡器讀取信息內容，判斷當前所處的位置。但是磁條導航方式存在脫軌的問題，需要人為將移動平臺推回軌道上。因此，本文作者結合視覺SLAM技術改進移動平臺的導航和定位系統，以實現變電所的無人化和自動化巡檢。

1 移動平臺的結構

移動平臺上方需搭載檢測機器人，考慮承載能力和操作的便利性，設計的機械結構如圖1所示。車體長為750 mm，寬為560 mm，高為400 mm。

1-后端殼；2-主動輪；3-前端殼；4-電機；5-從動輪；6-底盤

2 控制系統的硬件設計

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

磁導航傳感器采用歐米麥克MK-0216B系列，該系列傳感器的測量精度能達到±1 mm。除此之外，還有用于定位的EMR-05型RFID射頻傳感器?？刂葡到y總體結構框圖如圖3所示。

圖2 設備安裝圖 圖3 控制系統總體結構框圖

3 控制系統軟件設計

移動平臺控制系統分為上位機軟件和下位機軟件兩個部分。

3.1 下位機軟件設計

下位機軟件實現磁條導航時的點動、旋轉和到目標運動，視覺SLAM導航的自主運動，以及脫離軌道和重回軌道時導航方式的切換等功能。

3.2 上位機軟件設計

3.2.1 地圖構建模塊

(1) ORB-SLAM2[3]算法：包括相機跟蹤、地圖構建和回環檢測三個模塊。相機跟蹤模塊的作用是根據深度相機獲取到的信息估計位姿并檢測新的關鍵幀。地圖構建模塊負責插入檢測到的新關鍵幀，經過局部BA優化后剔除不符合條件的地圖點和關鍵幀?；丨h檢測模塊的作用是檢測是否到達過該場景，消除系統的累計誤差。

(2) 稠密地圖構建：ORB-SLAM2系統只能構建稀疏地圖，無法用于導航和路徑規劃，因此改進該算法實現稠密建圖。相機的成像過程涉及到三個坐標平面，即相機坐標平面、像素平面和物理成像平面。三者之間的關系如圖4所示。

圖4 相機模型

假設點P在相機坐標系下的坐標Pc為[X,Y,Z]T，投影在物理成像平面上的坐標為[X′,Y′,Z′]T。設相機的焦距為f,則：

(1)

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

(2)

其中:fx、fy、cx和cy為相機內參。

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

(3)

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

Pw=RPc+t.

(4)

首先采用點云庫[4]中PointXYZRGBA類型存儲世界坐標值和顏色信息，然后再進行點云拼接即可得到點云地圖。經體素濾波器[5]優化后，保證了在某一個確定的體素內僅有一個點，減少了內存空間的占用。

(3) 八叉樹地圖[6]構建：點云地圖只能添加地圖點，不能動態地移除某一時刻消失在地圖中的點，因此不能用于導航和路徑規劃。本文采用八叉樹地圖解決上述問題，它是一種三維柵格地圖，可以動態地描述節點的占據狀態。

3.2.2 導航模塊

基于ROS平臺設計視覺SLAM導航系統。全局路徑規劃采用A*算法[7]計算從起始點到目標點的最優路徑。局部路徑規劃采用時間彈性帶算法[8]躲避障礙物，并計算出該行駛周期內的角速度和線速度。

如果移動平臺脫離磁條軌道，則導航系統切換至視覺SLAM導航，以系統中預設的軌道上的點為目標點，自主導航回到軌道。

4 基于ROS的算法測試

移動平臺要搭載檢測機器人進行巡檢，現場平面布局如圖5所示。其中Pi(i=1,…,6)為SLAM導航的目標點，其余為各個巡檢位置。

圖5 工作環境平面布局

基于ROS系統測試改進的ORB-SLAM2算法，濾波后的稠密點云地圖和八叉樹地圖如圖6所示。

圖6 供電段環境地圖

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

圖7 二維柵格地圖

5 設備調試

首先，設定好目標位置g，移動平臺通過磁條導航的方式運行，在圖8所示位置脫離磁條軌道。

圖8 導航示意圖

當磁導航傳感器檢測到移動平臺脫離磁條，切換為視覺SLAM導航方式，自主運行到軌道前方較近的SLAM導航目標點P5。調整好位姿之后，視覺SLAM導航方式結束，磁導航傳感器讀取到信息，切換回磁導航方式，繼續運行至目標位置。

6 結束語

目前，基于視覺SLAM和磁條的移動平臺組合導航系統已經開發完成。經測試，磁條導航脫離磁條軌道后，切換為SLAM導航方式可以自主導航回到軌道。該系統實現了變電站巡檢的無人化、自動化和智能化，提高了巡檢效率。