汪 勇， 華志剛， 陳建國(guó)， 朱繼濤， 吳宏亮， 范佳卿， 林潤(rùn)達(dá)

(1．上海發(fā)電設(shè)備成套設(shè)計(jì)研究院有限責(zé)任公司， 上海 200240；2．國(guó)家電力投資集團(tuán)有限公司戰(zhàn)略研究院， 北京 100029；3．國(guó)家電投集團(tuán)江西電力有限公司， 南昌 330096)

在電廠鍋爐爐膛內(nèi)會(huì)發(fā)生GPS信號(hào)失效、地磁干擾等問(wèn)題，同時(shí)由于該環(huán)境內(nèi)部密閉無(wú)光，視覺(jué)定位技術(shù)面臨極大挑戰(zhàn)，無(wú)法實(shí)現(xiàn)無(wú)人機(jī)的定位導(dǎo)航。近年來(lái)，國(guó)內(nèi)外學(xué)者針對(duì)無(wú)人機(jī)在密閉空間內(nèi)的定位導(dǎo)航進(jìn)行了大量的研究。

Gohl等[1-2]研究了地下礦山的空間檢測(cè)和礦山結(jié)構(gòu)三維重建方法。Burri等[3-4]設(shè)計(jì)并開發(fā)了用于鍋爐爐膛檢測(cè)的無(wú)人機(jī)，通過(guò)將機(jī)載前視攝像頭系統(tǒng)與慣性測(cè)量進(jìn)行耦合來(lái)估算無(wú)人機(jī)的位姿，基于模型預(yù)測(cè)控制器實(shí)現(xiàn)了高效的軌跡跟蹤，并且無(wú)人機(jī)能在鍋爐表面附近飛行。Nieuwenhuisen等[5-6]開發(fā)出利用多旋翼無(wú)人機(jī)進(jìn)行工業(yè)煙囪巡檢的無(wú)人機(jī)導(dǎo)航系統(tǒng)?？滋旌愕萚7-8]基于擴(kuò)展卡爾曼濾波器(EKF)和D&C SLAM算法并配合激光雷達(dá)實(shí)現(xiàn)了室內(nèi)定位和建圖。孫朋[9]對(duì)SRT算法進(jìn)行了改進(jìn)，通過(guò)加入沿墻導(dǎo)航策略克服了激光雷達(dá)因檢測(cè)不到環(huán)境特征而無(wú)法定位的缺陷，實(shí)現(xiàn)了無(wú)人機(jī)在未知室內(nèi)環(huán)境下的自主探索。陳杰[10]采用EKF的算法框架，在激光雷達(dá)的定位算法中融合了慣導(dǎo)、氣壓計(jì)、光流等定位手段，提高了定位精度。唐鄧清[11]采用光流法跟蹤激光雷達(dá)數(shù)據(jù)，獲得圖像的深度信息，再通過(guò)PnP解算和非線性優(yōu)化過(guò)程精確地估計(jì)出無(wú)人機(jī)位姿。

目前，關(guān)于密閉空間飛行定位的研究工作分為兩類，一類是針對(duì)特定的工業(yè)環(huán)境，此類算法具有高度的定制性，另一類則針對(duì)室內(nèi)環(huán)境，此類算法需借助外部標(biāo)記或視覺(jué)傳感器。而本文的研究目的是設(shè)計(jì)一種能夠盡量適應(yīng)鍋爐密閉空間的無(wú)人機(jī)定位導(dǎo)航系統(tǒng)，在無(wú)光照條件下無(wú)法依賴視覺(jué)傳感器，且需要算法對(duì)不同環(huán)境具有更普遍的適應(yīng)性。

1 密閉空間無(wú)人機(jī)位姿估計(jì)

在無(wú)人機(jī)應(yīng)用場(chǎng)景中，無(wú)人機(jī)飛行時(shí)姿態(tài)變化相較于地面車輛姿態(tài)變化更劇烈，當(dāng)無(wú)人機(jī)遠(yuǎn)離地面時(shí)激光雷達(dá)不能有效檢測(cè)到地面特征點(diǎn)，因此在無(wú)人機(jī)應(yīng)用場(chǎng)景中LOAM的姿態(tài)估計(jì)和高度估計(jì)均會(huì)出現(xiàn)較大誤差。由于在密閉空間內(nèi)空氣流動(dòng)性很差，因此可以以氣壓計(jì)作為無(wú)人機(jī)高度的輔助測(cè)量手段，筆者基于互補(bǔ)濾波的姿態(tài)估計(jì)算法設(shè)計(jì)了結(jié)合陀螺儀、加速度計(jì)、氣壓計(jì)和激光雷達(dá)的多傳感器融合的位姿估計(jì)算法。

1.1 算法框架

基于多傳感器融合的位姿估計(jì)算法融合了慣導(dǎo)系統(tǒng)的無(wú)人機(jī)俯仰角和橫滾角估計(jì)、氣壓計(jì)高度估計(jì)以及激光雷達(dá)偏航角估計(jì)，實(shí)現(xiàn)了無(wú)人機(jī)在密閉空間內(nèi)的定位導(dǎo)航。圖1給出了基于多傳感器融合的位姿估計(jì)算法的整體框架。該算法待估計(jì)的位姿矩陣為TL,W=[τL,WrL,W]T，其中位置矩陣τL,W=[xyz]T，姿態(tài)角矩陣rL,W=[φθψ]T，x、y和z為三維坐標(biāo)，φ、θ和ψ分別為無(wú)人機(jī)的俯仰角、橫滾角和偏航角。將三維坐標(biāo)和無(wú)人機(jī)的姿態(tài)角重新排列，定義λL,W=[φθz]T，σL,W=[xyψ]T，將慣導(dǎo)系統(tǒng)與氣壓計(jì)獲得數(shù)據(jù)融合可得到λL,W，在此基礎(chǔ)上基于激光雷達(dá)可得到σL,W。

圖1 基于多傳感器融合的位姿估計(jì)算法Fig.1 Pose estimation algorithm based on multi-sensor fusion

通過(guò)陀螺儀、加速度計(jì)和氣壓計(jì)分別獲得無(wú)人機(jī)的姿態(tài)角、加速度和高度，將傳感器獲得的數(shù)據(jù)通過(guò)互補(bǔ)濾波等算法進(jìn)行優(yōu)化，并將其與激光雷達(dá)數(shù)據(jù)融合后傳給地圖，構(gòu)建模塊和控制模塊。

1.2 俯仰角、橫滾角和高度估計(jì)

不同傳感器具有不同的物理特性，因此在對(duì)同一信號(hào)進(jìn)行測(cè)量時(shí)可能會(huì)產(chǎn)生不同特性的噪聲。圖2給出了基于互補(bǔ)濾波器的俯仰角和橫滾角估計(jì)算法。通過(guò)陀螺儀可以得到無(wú)人機(jī)的姿態(tài)角，考慮到存在誤差，通過(guò)加速度計(jì)的輔助，使激光雷達(dá)的掃描平面經(jīng)過(guò)互補(bǔ)濾波器后，得到優(yōu)化后俯仰角和橫滾角的精準(zhǔn)值。將獲得數(shù)據(jù)和目標(biāo)數(shù)據(jù)進(jìn)行比較，從而進(jìn)行飛行控制。

圖2 基于互補(bǔ)濾波器的俯仰角和橫滾角估計(jì)Fig.2 Pitch and roll angle estimation algorithm basedon complementary filter

單獨(dú)利用陀螺儀進(jìn)行姿態(tài)角的估計(jì)，隨著時(shí)間的累積，由于隨機(jī)漂移的影響，導(dǎo)致估計(jì)值發(fā)散。加速度計(jì)容易受到機(jī)體振動(dòng)等干擾，短時(shí)間內(nèi)穩(wěn)定性差，難以收斂到最優(yōu)解。對(duì)于陀螺儀隨機(jī)漂移產(chǎn)生的低頻噪聲干擾，其輸出顯示為高通特性，應(yīng)采用高通濾波器濾除；加速度計(jì)的動(dòng)態(tài)響應(yīng)差，其輸出顯示為低通特性，采用低通濾波器濾除。由于陀螺儀和加速度計(jì)的輸出具有頻率互補(bǔ)特性，因此互補(bǔ)濾波算法可以降低不同傳感器的噪聲特性，利用低成本的嵌入式系統(tǒng)即可滿足姿態(tài)測(cè)量要求。

采用加速度計(jì)和氣壓計(jì)融合算法進(jìn)行無(wú)人機(jī)的高度估計(jì)。通常氣壓計(jì)測(cè)量得到的高度信息容易受到外界氣流的干擾，在室外情況下風(fēng)力因素會(huì)對(duì)氣壓計(jì)的高度測(cè)量產(chǎn)生很大影響。因此，在無(wú)人機(jī)飛控中氣壓計(jì)傳感器上通常會(huì)覆蓋海綿類材料以減小干擾氣流的影響。在密閉空間中基本為無(wú)風(fēng)環(huán)境，空間內(nèi)氣流相對(duì)穩(wěn)定，氣壓計(jì)測(cè)量高度的效果更好。因此，對(duì)通過(guò)加速度計(jì)得到的數(shù)據(jù)進(jìn)行積分，得到高度數(shù)據(jù)，并對(duì)其進(jìn)行低通濾波后再與氣壓計(jì)的高度數(shù)據(jù)進(jìn)行融合。氣壓計(jì)測(cè)量得到的高度數(shù)據(jù)是根據(jù)氣壓值計(jì)算得到的絕對(duì)高度，因此相對(duì)無(wú)人機(jī)導(dǎo)航坐標(biāo)系的高度需要通過(guò)記錄初始高度進(jìn)行換算。

1.3 水平方向位置與偏航角估計(jì)

由慣性測(cè)量單元和氣壓計(jì)得到t時(shí)刻下的λL,W,t=[φtθtzt]T的可靠估計(jì)。在此基礎(chǔ)上，估計(jì)得到t時(shí)刻下的σL,W,t=[xtytψt]T。

1.3.1 特征提取

圖3給出了在3面豎直圍墻的環(huán)境下激光雷達(dá)掃描得到的點(diǎn)云信息。在掃描平面中，具有平面物理特征的墻體表面在點(diǎn)云中呈現(xiàn)為直線，而具有直線物理特征的墻角呈現(xiàn)為拐點(diǎn)，在實(shí)際場(chǎng)景中這2種特征可以根據(jù)點(diǎn)與周圍點(diǎn)的平滑度來(lái)提取。

圖3 單束激光雷達(dá)掃描測(cè)量點(diǎn)云Fig.3 Single laser scanning measurement data

設(shè)激光雷達(dá)掃描平面中的點(diǎn)為Xk,i，Xk,i位于序列中間，其中k為掃描平面編號(hào)。S為掃描平面的連續(xù)序列，其序列數(shù)為10，Xk,i的平滑度C(k,i)可表示為：

(1)

對(duì)所有點(diǎn)的平滑度進(jìn)行排序，C(k,i)小說(shuō)明不平滑，C(k,i)>CCorner時(shí)為線特征點(diǎn)，C(k,i)

圖4給出了在四面圍墻環(huán)境中激光雷達(dá)點(diǎn)云特征提取結(jié)果。從圖4可以看出，4個(gè)墻角均被識(shí)別為線特征點(diǎn)，相鄰激光雷達(dá)掃描平面中的線特征點(diǎn)組成4條對(duì)應(yīng)4個(gè)墻角的直線，而面特征點(diǎn)則均勻分布在墻面上。

圖4 激光雷達(dá)點(diǎn)云特征提取效果Fig.4 Feature extraction results of laser point cloud

1.3.2 特征匹配與運(yùn)動(dòng)估計(jì)

以t-1～t時(shí)間范圍為例，記t-1和t時(shí)刻激光雷達(dá)得到的點(diǎn)云點(diǎn)集分別為L(zhǎng)t-1和Lt，提取Lt中的線、面特征點(diǎn)的點(diǎn)集，分別為wt和ft。記t-2～t-1時(shí)間內(nèi)x和y方向的增量分別為xt-2,t-1和yt-2,t-1，t-1時(shí)刻的偏航角為ψt-1，即σt-2,t-1=[xt-2,t-1yt-2,t-1ψt-1]T；記t-1時(shí)刻下的位姿矩陣為TL,W,t-1=[τL,W,t-1rL,W,t-1]T；t時(shí)刻下λL,W,t=[φtθtzt]T。

將wt和ft變換到t-1時(shí)刻的激光雷達(dá)坐標(biāo)系下，假設(shè)激光雷達(dá)相鄰兩幀間勻速，將上次估計(jì)的增量作為本次估計(jì)的估計(jì)值進(jìn)行變換。在λL,W,t已知的情況下，wt和ft的變換更加精確。根據(jù)當(dāng)前和上個(gè)時(shí)刻的高度，求世界坐標(biāo)系下高度增量矩陣τz,W,δ=[0 0zW,t-1,t]T，記t-1～t時(shí)間內(nèi)x和y方向的增量分別為xt-1,t和yt-1,t，即τδ=[xt-1,tyt-1,t0]T，則t-1～t時(shí)間段內(nèi)雷達(dá)平移增量矩陣τδ,t-1,t可表示為：

(2)

式中：RL,W,t-1為兩幀之間的旋轉(zhuǎn)矩陣；τδ,W為兩幀之間的平移矩陣。

(3)

在Lt-1中尋找特征點(diǎn)的匹配點(diǎn)，可構(gòu)建線特征點(diǎn)的匹配函數(shù)fW，該函數(shù)可表示為：

fW(Xt,k,σL,W)=dW，Xt,k∈wt

(4)

式中：dW為線特征點(diǎn)在擬合方程中的差值；wt為t時(shí)刻的線特征點(diǎn)集。

面特征點(diǎn)的匹配函數(shù)ff可表示為：

ff(Xt,k,σL,W)=df，Xt,k∈ft

(5)

式中：df為面特征點(diǎn)在擬合方程中的差值；ft為t時(shí)刻的面特征點(diǎn)集。

1.3.3 位姿優(yōu)化

將當(dāng)前點(diǎn)云變換到世界坐標(biāo)系下，在相同的位置尋找匹配點(diǎn)，此時(shí)λL,W=[φtθtzt]T中3個(gè)變量已知，對(duì)σL,W=[xyψ]T進(jìn)行優(yōu)化，最終構(gòu)建方程：

國(guó)外學(xué)者認(rèn)為，智慧校園利用信息技術(shù)來(lái)改變學(xué)生的學(xué)習(xí)和教師的工作，“為人們創(chuàng)造服務(wù)”[5]?？傊?，智慧校園綜合運(yùn)用智能物聯(lián)網(wǎng)、高速互聯(lián)網(wǎng)、高速無(wú)線網(wǎng)、虛擬化技術(shù)、云計(jì)算、統(tǒng)一存儲(chǔ)和分布式存儲(chǔ)以及大數(shù)據(jù)平臺(tái)等信息技術(shù)，全方位感知校園物理環(huán)境，智能識(shí)別師生個(gè)體，實(shí)現(xiàn)以人為本的個(gè)性化服務(wù)。

f(σL,W)=d

(6)

式中：d為殘差。

對(duì)式(6)的未知量取偏導(dǎo)得到極值點(diǎn)，可解出σL,W的最優(yōu)解，最終實(shí)現(xiàn)對(duì)水平位置x、y和偏航角ψ的可靠估計(jì)。

2 仿真實(shí)驗(yàn)

2.1 低速飛行情況下的仿真結(jié)果

結(jié)合實(shí)際鍋爐圖紙，利用Solidworks軟件進(jìn)行1∶1建模，將其導(dǎo)入U(xiǎn)buntu的工作空間，添加無(wú)人機(jī)和相關(guān)傳感器模型后，在機(jī)器人操作系統(tǒng)(ROS)環(huán)境下基于Gazebo軟件進(jìn)行仿真。無(wú)人機(jī)在仿真飛行和實(shí)際飛行過(guò)程中，其運(yùn)行指標(biāo)以飛行目標(biāo)位置與實(shí)際位置的關(guān)系為準(zhǔn)，二者之間的差值越小，其飛行精度越高，定位越準(zhǔn)確。

將激光雷達(dá)的數(shù)據(jù)與其他傳感器的數(shù)據(jù)融合，通過(guò)算法計(jì)算下一時(shí)刻的目標(biāo)位置，將當(dāng)前自身定位的位置和下一時(shí)刻的目標(biāo)位置進(jìn)行比較，并通過(guò)避障等相關(guān)程序計(jì)算無(wú)人機(jī)的速度和加速度等參數(shù)。

2.1.1 建圖結(jié)果對(duì)比

圖5給出了低速情況下多傳感器融合算法的爐膛建圖結(jié)果。該算法能較好地進(jìn)行整個(gè)飛行過(guò)程的位姿估計(jì)和地圖構(gòu)建，算法估計(jì)的無(wú)人機(jī)飛行軌跡基本與設(shè)定的飛行軌跡吻合，點(diǎn)云地圖也較好地重現(xiàn)了鍋爐爐膛模型的結(jié)構(gòu)，未出現(xiàn)重影、彎曲等現(xiàn)象，且在點(diǎn)云地圖中鍋爐模型的4個(gè)直角也未出現(xiàn)明顯的偏差。將點(diǎn)云數(shù)據(jù)和實(shí)際鍋爐圖紙進(jìn)行比較，可以發(fā)現(xiàn)點(diǎn)云數(shù)據(jù)可靠，高度精準(zhǔn)，且能反映實(shí)際情況。

2.1.2 位置估計(jì)結(jié)果對(duì)比

圖6給出了鍋爐爐膛環(huán)境下多傳感器融合算法估計(jì)的無(wú)人機(jī)飛行軌跡與GPS參考軌跡的對(duì)比。在實(shí)際環(huán)境中鍋爐內(nèi)不存在GPS，通過(guò)在仿真環(huán)境中添加GPS環(huán)境，將GPS環(huán)境下的數(shù)據(jù)作為參考值，驗(yàn)證在有、無(wú)GPS環(huán)境下無(wú)人機(jī)飛行定位的精確度。從圖6可以看出，基于多傳感器融合算法得到的無(wú)人機(jī)飛行軌跡與GPS參考軌跡基本重合，且在不同高度上均得到了較理想的位置估計(jì)結(jié)果。

(a) 側(cè)視圖

(a) x軸、激光雷達(dá)

(c) x軸、多傳感器融合算法

2.1.3 水平位置對(duì)比及誤差分析

圖7為鍋爐爐膛內(nèi)x軸和y軸方向的位置誤差?；诙鄠鞲衅魅诤纤惴ǖ奈恢谜`差整體維持在較低水平，與在建筑物房間內(nèi)的位置誤差基本持平。由圖6和圖7可知，在仿真環(huán)境下以理想GPS環(huán)境為參考，無(wú)人機(jī)位置在x軸和y軸的位置誤差均在0.5 m以內(nèi)，可以精準(zhǔn)控制飛行。

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

圖8給出了基于多傳感器融合算法的鍋爐爐膛建圖結(jié)果。在不同激光雷達(dá)分辨率下點(diǎn)云地圖均能較好地恢復(fù)鍋爐爐膛的內(nèi)部結(jié)構(gòu)。如圖8(b)所示，4個(gè)直角部分在鍋爐爐膛外部出現(xiàn)了點(diǎn)云，這是由于鍋爐4個(gè)直角部位的燃燒器上存在孔洞，激光雷達(dá)透過(guò)孔洞測(cè)量到燃燒器的內(nèi)部數(shù)據(jù)。由于實(shí)驗(yàn)場(chǎng)地?zé)o人機(jī)所處平臺(tái)為金屬網(wǎng)狀結(jié)構(gòu)，激光雷達(dá)可以透過(guò)金屬網(wǎng)測(cè)量得到部分平臺(tái)下方的信息，因此從圖8中還可以看出鍋爐爐膛底部的梯形結(jié)構(gòu)。激光雷達(dá)配置為單線2 048個(gè)測(cè)量點(diǎn)，由于其分辨率更高，底部的梯形結(jié)構(gòu)也更加明顯。

(a) x軸

(b) y軸圖7 鍋爐爐膛內(nèi)x軸和y軸方向的位置誤差Fig.7 Position errors at x and y directions in the boiler furnace

(a) 側(cè)視圖

圖9給出了鍋爐爐膛內(nèi)基于多傳感器融合算法的偏航角變化。當(dāng)通過(guò)磁力計(jì)和多傳感器融合算法得到的偏航角均足夠準(zhǔn)確時(shí)，由于無(wú)人機(jī)的初始朝向與地磁北極未經(jīng)過(guò)對(duì)準(zhǔn)，因此理論上這2條偏航角曲線間的差值應(yīng)保持恒定。從圖9可以看出，采用多傳感器融合算法得到的曲線基本符合這一結(jié)果。

在實(shí)際環(huán)境中，將通過(guò)磁力計(jì)得到的偏航角與計(jì)算獲得的偏航角進(jìn)行比較，可以看出兩者存在一定的差距，但偏航角的整體變化趨勢(shì)一致，且誤差在可接受范圍之內(nèi)，控制精度較高。

(a) 1 024點(diǎn)數(shù)下的偏航角

(b) 2 048點(diǎn)數(shù)下的偏航角圖9 基于多傳感器融合算法得到的偏航角變化Fig.9 Yaw angle variation based on multi-sensor fusion algorithm

3 結(jié) 論

(1) 通過(guò)對(duì)無(wú)人機(jī)的飛行進(jìn)行控制，由鍋爐爐膛內(nèi)的實(shí)驗(yàn)驗(yàn)證了多傳感器融合算法的準(zhǔn)確性。

(2) 激光雷達(dá)建立的點(diǎn)云圖清晰無(wú)重疊，無(wú)偏斜。

(3) 位置和姿態(tài)角的實(shí)際數(shù)據(jù)與理想數(shù)據(jù)之間的偏差在理想范圍內(nèi)波動(dòng)，無(wú)人機(jī)的飛行路徑?jīng)]有發(fā)生突變等情況，飛行效果與目標(biāo)效果相近。