林 歡，王 鋒

(億嘉和科技股份有限公司，江蘇 南京 210012)

目前智能電網(wǎng)的建設(shè)過程中需要對變電設(shè)備狀態(tài)進(jìn)行檢測，利用變電站智能巡檢機(jī)器人能夠?qū)崿F(xiàn)變電站全天候、全方位、全自主的智能巡檢和監(jiān)控，有效降低了工人勞動(dòng)強(qiáng)度和變電站自身運(yùn)行維護(hù)的成本，提高了巡檢機(jī)器人正常作業(yè)和管理的智能化、自動(dòng)化水平，為智能變電站提供了創(chuàng)新性的技術(shù)檢測手段和全方位的安全保障[1]。

與傳統(tǒng)的軌道式巡檢機(jī)器人不同，利用二維激光定位導(dǎo)航的智能巡檢機(jī)器人[1]不僅可以節(jié)省軌道鋪設(shè)的成本和工作量，而且巡檢工作更加靈活，可以根據(jù)需要進(jìn)行巡檢路線的規(guī)劃。巡檢機(jī)器人在激光定位的工作過程中，機(jī)器人的初始定位會(huì)影響到整個(gè)巡檢過程的效率和準(zhǔn)確度。初始定位的不準(zhǔn)確不僅會(huì)增加機(jī)器人迷路的可能性，而且對后續(xù)設(shè)備檢測的位置和角度也會(huì)產(chǎn)生不良的影響。因此，初始位姿優(yōu)化方法的設(shè)計(jì)，是開發(fā)變電站自主巡檢機(jī)器人中的關(guān)鍵技術(shù)。

機(jī)器人巡檢過程中，初始定位與實(shí)時(shí)定位之間的主要區(qū)別在于：實(shí)時(shí)定位要求在毫秒級別內(nèi)實(shí)現(xiàn)；而初始定位不受時(shí)間限制，可以進(jìn)行更大范圍的搜索和優(yōu)化計(jì)算。

目前大部分研究都默認(rèn)初始位置已知[2]，因此對初始定位的研究較為簡單。最初使用2D激光測距雷達(dá)將掃描得到的地圖和已知地圖進(jìn)行匹配，得到初始定位點(diǎn)，但是該方法計(jì)算量大，而且對掃描得到的信息無法進(jìn)行精確的辨別，導(dǎo)致算法的魯棒性差，無法適應(yīng)環(huán)境中臨時(shí)出現(xiàn)的輕微變化。TODD等[2]采用全局求互相關(guān)的方法，將當(dāng)前掃描圖與已知地圖中相關(guān)值最大的位置作為機(jī)器人的初始位置，但是該方法同樣存在定位不準(zhǔn)確、計(jì)算量大等問題。項(xiàng)志宇[3]結(jié)合激光測距雷達(dá)數(shù)據(jù)的特點(diǎn)，提出了基于“完整線段”匹配的新方法來實(shí)現(xiàn)初始定位。只把完整的部分特征與已知地圖進(jìn)行匹配定位，雖然大大降低了計(jì)算量，但經(jīng)常會(huì)出現(xiàn)局部匹配不符的情況，從而導(dǎo)致初始定位的不準(zhǔn)確。王璐[4]利用隱馬爾科夫模型(hidden markov model,HMM)對環(huán)境中特殊標(biāo)記物之間的空間關(guān)系進(jìn)行建模，可以較為有效地匹配相似環(huán)境，但無法用于動(dòng)態(tài)環(huán)境中。

針對上述問題，本文提出了一種基于雙三次插值算法的初始位姿優(yōu)化方法。該方法考慮了變電站稀疏空曠的實(shí)際環(huán)境，對激光掃描得到的初始定位估值進(jìn)行優(yōu)化，采用雙三次插值法來實(shí)現(xiàn)優(yōu)化目標(biāo)，可以得到最優(yōu)的初始位姿。本文所提方法采用的硬件系統(tǒng)傳感器包括里程計(jì)(odometer)、慣性導(dǎo)航單元(IMU)、激光傳感器(laser)。軟件系統(tǒng)使用機(jī)器人操作系統(tǒng)ROS(robot operating system)。在ROS系統(tǒng)下實(shí)現(xiàn)一個(gè)Car Localization的定位節(jié)點(diǎn)，在定位節(jié)點(diǎn)啟動(dòng)階段，使用本文初始位姿優(yōu)化方法實(shí)現(xiàn)變電站智能巡檢機(jī)器人高精度的初始定位。

1 初始位姿估計(jì)

機(jī)器人開始巡檢工作前，已經(jīng)構(gòu)建了變電站的二維環(huán)境地圖，地圖和機(jī)器人運(yùn)動(dòng)全部假定在一個(gè)二維平面內(nèi)，建立3個(gè)坐標(biāo)系：地圖坐標(biāo)系、機(jī)器人坐標(biāo)系以及激光傳感器坐標(biāo)系。使用ROS系統(tǒng)中TF(transform)模塊實(shí)現(xiàn)不同坐標(biāo)系中數(shù)據(jù)坐標(biāo)值的轉(zhuǎn)換。巡檢機(jī)器人的位姿Pose可以表示為P(x,y,r)，其中(x,y)表示機(jī)器人在地圖坐標(biāo)系中的位置，r表示機(jī)器人的朝向。

1.1 定位窗口掃描

Car Localization定位節(jié)點(diǎn)獲得當(dāng)前初始定位初值Ps(x0,y0,r0)的方法有兩種：一種是根據(jù)上次工作結(jié)束時(shí)保存在地圖文件中的位姿信息來獲得；另一種是根據(jù)ROS系統(tǒng)/initialpose主題中保存的位姿信息來更新。在獲得初始定位初值Ps(x0,y0,r0)、地圖數(shù)據(jù)以及激光數(shù)據(jù)之后，就可以開始初始位姿的定位窗口掃描[6]，如圖1所示。

圖1 初始定位掃描示意圖

由于實(shí)時(shí)性要求不高，以初值Ps(x0,y0,r0)為中心設(shè)定正方形定位掃描窗口，線性范圍一般為1m，精度為0.1m×0.1m。角度范圍360°，角度步長為1°。每得到一幀符合要求的激光數(shù)據(jù)(laserscan)的情況下，通過定位窗口的掃描，就可以獲得較優(yōu)的初始位姿估計(jì)點(diǎn)Pe(x0,y0,r0)。每幀激光數(shù)據(jù)會(huì)存儲(chǔ)N個(gè)激光點(diǎn)數(shù)據(jù)，這些激光點(diǎn)數(shù)據(jù)經(jīng)過ROS系統(tǒng)自動(dòng)處理后會(huì)得到相應(yīng)的激光點(diǎn)云數(shù)據(jù)(pointcloud)。在定位過程中使用激光點(diǎn)云數(shù)據(jù)來進(jìn)行處理和計(jì)算，過程如下：

Step1，根據(jù)初始定位初值Ps(x0,y0,r0)、激光點(diǎn)云數(shù)據(jù)和用于ROS系統(tǒng)接口調(diào)用的角度掃描參數(shù)θwindow，θstep計(jì)算各角度的激光點(diǎn)云數(shù)據(jù)(rotatedcloud)。

Step2，將Ps(x0,y0,r0)設(shè)定為預(yù)測位置，根據(jù)rotatedcloud數(shù)據(jù)和地圖數(shù)據(jù)計(jì)算各角度點(diǎn)云數(shù)據(jù)中各點(diǎn)對應(yīng)的地圖柵格坐標(biāo)位置，得到不同角度激光點(diǎn)云數(shù)據(jù)對應(yīng)的離散掃描數(shù)據(jù)。

Step3，根據(jù)窗口大小掃描參數(shù)和角度掃描參數(shù)θwindow，θstep計(jì)算定位窗口大小，同時(shí)計(jì)算出可能的位姿候選ζpossible，從而獲得較優(yōu)的初始位姿估計(jì)點(diǎn)Pe(x0,y0,r0)。

1.2 置信度計(jì)算

針對變電站空曠稀疏的環(huán)境特點(diǎn)，利用激光最小和最大的參數(shù)值Lasermin，Lasermax對激光點(diǎn)云數(shù)據(jù)進(jìn)行過濾，去掉了數(shù)據(jù)中相應(yīng)的噪點(diǎn)，將剩余的激光點(diǎn)數(shù)據(jù)作為有效點(diǎn)。把每幀激光數(shù)據(jù)pointcloud中的有效點(diǎn)從N個(gè)降低到h個(gè)，提高了初始定位估計(jì)的置信度。根據(jù)系統(tǒng)過濾的結(jié)果不同，在每幀激光數(shù)據(jù)點(diǎn)數(shù)據(jù)為1 141的傳感器上h約為幾百的量級[7]。

Step1，根據(jù)每個(gè)候選位姿ζpossible對應(yīng)的離散掃描數(shù)據(jù)中的柵格置信度，計(jì)算每個(gè)位姿的置信度αcandidate。每個(gè)離散掃描數(shù)據(jù)中有h個(gè)柵格，每個(gè)柵格的地圖坐標(biāo)為(x,y)，對應(yīng)的置信度是αxy，置信區(qū)間為[0.1～0.9]，根據(jù)當(dāng)前的環(huán)境條件進(jìn)行判斷，從而選擇相應(yīng)的置信度αxy的值。

(1)

Step2， 根據(jù)每個(gè)位姿候選ζpossible與初始位姿估計(jì)點(diǎn)Pe(x0,y0,r0)的位姿差來計(jì)算每個(gè)位姿對應(yīng)的置信度權(quán)重αweight。

αweight=e2q

(2)

(3)

式中：xoffset，yoffset分別為x，y方向坐標(biāo)；αrotation為位姿旋轉(zhuǎn)方向置信度，其置信區(qū)間為[0.1～0.9]。Wtrans，Wrotation分別為位移和旋轉(zhuǎn)權(quán)重，其值需根據(jù)實(shí)際環(huán)境條件獲得。

Step3，將每個(gè)位姿的置信度與置信度權(quán)重的乘積作為當(dāng)前位姿的分值(Score)。

Score=αcandidate·αweight

(4)

Step4,取分值最高的位姿作為初始定位估值Pf(x0,y0,r0)，為了實(shí)現(xiàn)最優(yōu)的初始位姿，本文采用雙三次插值法對其進(jìn)行優(yōu)化。

2 雙三次插值法

根據(jù)機(jī)器人在地圖中的任何一個(gè)位姿都可以計(jì)算出對應(yīng)位姿的激光點(diǎn)云數(shù)據(jù)pointcloud和對應(yīng)的置信度αpose，從而得到分值最高的初始定位估值。然后對Pf(x0,y0,r0)用雙三次插值算法來進(jìn)行二次非線性優(yōu)化計(jì)算，從而獲得最優(yōu)初始位姿P0(x0,y0,r0)。

雙三次插值法利用了目標(biāo)插值點(diǎn)周圍4個(gè)方向上共16個(gè)像素點(diǎn)進(jìn)行插值，如圖2所示。不僅要考慮到周圍4個(gè)直接相鄰像素點(diǎn)灰度值對插值點(diǎn)的影響，還需要考慮各個(gè)方向上相鄰點(diǎn)之間的灰度值變化率[5]。雙三次插值算法的一般公式為：

(5)

圖2 雙三次插值

3 基于雙三次插值法初始位姿優(yōu)化

3.1 雙三次插值算法理論推導(dǎo)

在2中提到的雙三次插值法的基礎(chǔ)上，考慮巡檢機(jī)器人的朝向r，得到新的插值公式：

h(P0,m)=T(rt)Ah(x,m)

(6)

式中：T(rt)為基于機(jī)器人轉(zhuǎn)向角度rt的地圖坐標(biāo)系與機(jī)器人坐標(biāo)系之間的旋轉(zhuǎn)變換矩陣;A=[m11m12…mij…m44]，為系數(shù)矩陣，其中mij是與圖2中16個(gè)像素點(diǎn)對應(yīng)的具有地圖特征的激光點(diǎn)云數(shù)據(jù)，ij即為圖2中像素點(diǎn)的位置。式(5)中多項(xiàng)式系數(shù)aij的計(jì)算依賴于插值數(shù)據(jù)的特性[8]，常用的方法是用4個(gè)頂點(diǎn)的高度以及每個(gè)頂點(diǎn)的3個(gè)導(dǎo)數(shù)來確定，如式(7)～(9)所示。

(7)

(8)

(9)

(10)

(11)

(12)

根據(jù)變電站地圖實(shí)際信息結(jié)合初始位姿估計(jì)點(diǎn)進(jìn)行矩陣計(jì)算[10]，即可得到插值結(jié)果P0(x0,y0,r0)的概率值，從而確定最優(yōu)初始位姿P0(x0,y0,r0)。

3.2 CERES非線性優(yōu)化

本文使用CERES非線性優(yōu)化庫來輔助雙三次插值算法確定最優(yōu)初始位姿[12]。通過雙三次插值可以得到Ps(x0,y0,r0)的概率值，如果要使得該概率值在柵格地圖中最大，那么就需要使得CERES構(gòu)建得到的優(yōu)化目標(biāo)函數(shù)值(殘差residual)最小，即：

(13)

CERES庫輔助求解可以得到置信度、位移、旋轉(zhuǎn)3個(gè)殘差最小的初始位姿估計(jì)點(diǎn)。假定

(14)

式中：Rα為置信度殘差；Rtrans為位移殘差；Rrot為旋轉(zhuǎn)殘差。式(15)為在式(14)的基礎(chǔ)上加入了權(quán)重。

(15)

權(quán)重表示不同參數(shù)對結(jié)果的不同影響，本文中置信度權(quán)重為10/有效激光點(diǎn)數(shù)目的平方根，范圍為0.3～0.5，位移權(quán)重為10，旋轉(zhuǎn)權(quán)重為10。

采用CERES優(yōu)化庫對初始位姿進(jìn)行局部優(yōu)化匹配，局部匹配是調(diào)用real time scan matching來實(shí)現(xiàn)的，需要構(gòu)建OccupiedSpaceCostFunctor這個(gè)代價(jià)函數(shù)的計(jì)算模型。

4 實(shí)驗(yàn)結(jié)果

4.1 實(shí)驗(yàn)平臺

實(shí)驗(yàn)采用億嘉和科技股份有限公司研制的變電站智能巡檢機(jī)器人E100，如圖3所示。E100面向電力、市政、通信和石化等行業(yè)，為智能運(yùn)維系統(tǒng)提供環(huán)境和工業(yè)設(shè)備狀態(tài)的監(jiān)測數(shù)據(jù)。

圖3 E100變電站智能巡檢機(jī)器人

實(shí)現(xiàn)變電站巡檢機(jī)器人初始位姿優(yōu)化的具體程序流程如圖4所示。

圖4 程序總體框圖

4.2 實(shí)驗(yàn)結(jié)果分析

為了證明本文所提方法的優(yōu)越性，在某變電站實(shí)際環(huán)境下進(jìn)行多次實(shí)驗(yàn)，并與基于里程計(jì)和雙線性插值算法的初始位姿估計(jì)進(jìn)行實(shí)驗(yàn)對比，使用位置估計(jì)誤差來進(jìn)行數(shù)據(jù)分析，即位姿參考點(diǎn)和相應(yīng)估計(jì)點(diǎn)之間的歐氏距離。具體實(shí)驗(yàn)數(shù)據(jù)見表1。

表1 初始位姿估計(jì)誤差 m

從表中可以看出，實(shí)驗(yàn)1中，基于雙三次插值的初始位姿估計(jì)的最小誤差明顯小于臨近插值算法；與雙線性插值算法相比，雖然最小誤差相差不大，但是最大誤差明顯較??；整體的誤差平均值以及標(biāo)準(zhǔn)差遠(yuǎn)小于其他兩種方法。

對于實(shí)驗(yàn)2，考慮了變電站的實(shí)際情況，增加了稀疏環(huán)境下的影響因素，增加了障礙物，改變了預(yù)計(jì)估值點(diǎn)相應(yīng)的置信度，所以3種方法整體的誤差都有所提升，但是雙三次插值的位姿估計(jì)誤差提升較小。

根據(jù)實(shí)驗(yàn)可以看出，基于雙三次插值算法具有較好的初始位姿估計(jì)精度，能夠抵抗環(huán)境干擾，具有較高的魯棒性。

5 結(jié)束語

高精度初始位姿的確定，是變電站智能巡檢機(jī)器人開發(fā)中的關(guān)鍵技術(shù)。針對目前初始位姿確定算法魯棒性差，計(jì)算量大，無法適應(yīng)已知環(huán)境中臨時(shí)出現(xiàn)的輕微變化等問題，本文提出了基于雙三次插值算法的巡檢機(jī)器人初始位姿優(yōu)化，把局部匹配也是看成了一個(gè)非線性優(yōu)化問題，主要采用雙三次插值算法優(yōu)化初始位姿估值點(diǎn)，實(shí)現(xiàn)了變電站環(huán)境內(nèi)智能巡檢機(jī)器人高精度的初始定位，魯棒性好，降低了機(jī)器人迷路的發(fā)生頻次，提高了設(shè)備巡檢的效率和檢測數(shù)據(jù)的可靠性。目前已經(jīng)在多個(gè)變電站實(shí)際使用，初始位姿的高度準(zhǔn)確性降低了機(jī)器人迷路的發(fā)生頻次，提高了設(shè)備巡檢的效率和檢測數(shù)據(jù)的可靠性。

參考文獻(xiàn)：

[1] 趙玨. 基于激光傳感器室內(nèi)機(jī)器人自主導(dǎo)航技術(shù)研究[D]. 天津：天津理工大學(xué),2017.

[2] TODD Lupton, SALAH Sukkarieh. Visual-Inertial-Aided navigation for high-dynamic motion in built environments without initial conditions[J]. IEEE Trasactions on Robotics, 2012, 28(1):61-76.

[3] 項(xiàng)志宇. 基于激光雷達(dá)的移動(dòng)機(jī)器人障礙檢測和自定位[D]. 杭州：浙江大學(xué),2002.

[4] 王璐. 未知環(huán)境中移動(dòng)機(jī)器人視覺環(huán)境建模與定位研究[D]. 杭州：浙江大學(xué),2007.

[5] 胡良梅，王竹萌，高雋，等. 可信度雙三次插值的PMD距離信息和RGB圖像數(shù)據(jù)融合[J]. 中國圖象圖形學(xué)報(bào), 2013, 18(6)：621-627.

[6] 韓輊. 基于ROS的室內(nèi)移動(dòng)服務(wù)機(jī)器人定位與導(dǎo)航系統(tǒng)的研究與開發(fā)[D]. 成都：電子科技大學(xué),2017.

[7] 錢堃. 與人共存環(huán)境下服務(wù)機(jī)器人定位導(dǎo)航技術(shù)研究及應(yīng)用[D]. 南京：東南大學(xué),2018.

[8] 陳紅麗，曹義親. 邊緣定向的雙立方插值算法的研究與應(yīng)用[J] . 計(jì)算機(jī)仿真, 2015, 32(4)：180-183.

[9] 李純龍，潘海俠，王華峰. 自適應(yīng)立方卷積圖像插值算法[J]. 北京航空航天大學(xué)學(xué)報(bào), 2014, 40(10):180-183.

[10] ZHOU Jing, CHEN Xiaolin, LI Shijun. Centroid localization algorithm based on bicubic interpolation gray square weighted[J] . Engineering Solution for Manufacturing Process, 2013, 655-657：895-899.

[11] LEE Seung-Mok, JUNG Jongdae. Geomagnetic field-based localization with bicubic interpolation for mobile robots[J]. International Journal of Control,Automation, and Systems, 2015, 13(4)：1-11.

[12] 楊大偉，宋程程. 視覺顯著運(yùn)動(dòng)目標(biāo)初始定位方法[J] . 計(jì)算機(jī)工程與應(yīng)用, 2015, 51(16)：189-191.