李 曉, 沈 浩, 趙毅鋒, 黃 浩, 沈潤杰, 郭 帥

(1.華東天荒坪抽水蓄能有限責(zé)任公司,浙江 安吉 313302; 2.同濟(jì)大學(xué) 電子與信息工程學(xué)院,上海 201804; 3.合肥工業(yè)大學(xué) 土木與水利工程學(xué)院,安徽 合肥 230009)

0 引 言

抽水蓄能電站高壓引水隧洞在高速水流的不斷沖刷過程中,存在被沖蝕的風(fēng)險(xiǎn);抽水蓄能電站輸水道普遍長度長、角度大,現(xiàn)階段輸水道在無水情況下通常利用臺(tái)車和蜘蛛人檢查,有水情況下通常采用水下機(jī)器人檢查,但這些檢查手段普遍存在安全風(fēng)險(xiǎn)高的問題,且不適合具有分段水平洞和垂直隧洞情況,因此開發(fā)新型的輸水道系統(tǒng)檢測技術(shù)迫在眉睫。

近年來,隨著傳感器技術(shù)和無人機(jī)技術(shù)的快速發(fā)展,利用多旋翼無人機(jī)搭載采集設(shè)備進(jìn)行隧道內(nèi)部的快速檢測成為可能,但隧道內(nèi)部存在GPS信號(hào)屏蔽和地磁干擾現(xiàn)象,對(duì)無人機(jī)的自主飛行帶來技術(shù)挑戰(zhàn)。國內(nèi)外研究者針對(duì)無人機(jī)在封閉空間內(nèi)的自主飛行做了大量的研究,取得了較豐富的成果。文獻(xiàn)[1-2]開發(fā)了利用多旋翼無人機(jī)進(jìn)行工業(yè)煙囪巡檢的無人機(jī)導(dǎo)航系統(tǒng),該系統(tǒng)搭載了多個(gè)傳感器,包括輕型3D激光掃描儀,用于無人機(jī)的導(dǎo)航和避障;文獻(xiàn)[3]嘗試采用視覺、二維激光雷達(dá)、三維激光雷達(dá)等不同傳感器組合進(jìn)行大壩管道內(nèi)的無人機(jī)位姿估計(jì);通過搭載慣性傳感器、2個(gè)單線激光雷達(dá)及4個(gè)相機(jī)的水壩管道巡檢無人機(jī),文獻(xiàn)[4-5]提出了依賴管道圓柱幾何形狀的融合慣導(dǎo)、激光雷達(dá)、攝像機(jī)的六自由度估計(jì)算法,該算法利用Velodyne16線激光雷達(dá)、慣性測量單元(Inertial Measurement Unit,IMU)、4個(gè)攝像頭進(jìn)行無人機(jī)六自由度位姿估計(jì),該系統(tǒng)具有一定的自主導(dǎo)航和構(gòu)建局部環(huán)境地圖的能力;文獻(xiàn)[6]提出了基于三維激光雷達(dá)的位姿估計(jì)算法,該算法采用Velodyne16線激光雷達(dá),通過在非線性流形上的優(yōu)化過程重建以無人機(jī)為中心的局部地圖,并通過卡爾曼濾波器優(yōu)化最終的六自由度狀態(tài)估計(jì),可以適應(yīng)不同直徑、橫截面及不同彎曲程度的管道環(huán)境,如圖1、圖2所示。

圖1 無人機(jī)在大壩管道中的飛行實(shí)驗(yàn)

圖2 基于三維激光雷達(dá)算法的管道局部地圖

在國內(nèi),對(duì)于無人機(jī)在室內(nèi)、隧道內(nèi)等無GPS信號(hào)空間內(nèi)的自主導(dǎo)航技術(shù)也有一些研究報(bào)道。文獻(xiàn)[7]研究了基于單目視覺的無人機(jī)室內(nèi)導(dǎo)航方法;文獻(xiàn)[8]設(shè)計(jì)開發(fā)了面向隧道內(nèi)的自主偵查無人機(jī)系統(tǒng);文獻(xiàn)[9]研究了針對(duì)小型無人機(jī)在無衛(wèi)星導(dǎo)航信號(hào)條件下的導(dǎo)航問題;同濟(jì)大學(xué)無人機(jī)開發(fā)團(tuán)隊(duì)利用單線激光雷達(dá)融合IMU和氣壓計(jì)等實(shí)現(xiàn)了隧洞的自主飛行技術(shù),并于2019年首次對(duì)天荒坪、桐柏抽水蓄能電站進(jìn)行自主飛行,獲得成功。

本文研究四旋翼無人機(jī)在無GPS信號(hào)環(huán)境中的自主導(dǎo)航方法,提出了基于三維激光雷達(dá)、慣導(dǎo)、氣壓計(jì)的多傳感器融合無人機(jī)位姿估計(jì)算法,實(shí)現(xiàn)了無人機(jī)在電站輸水隧洞內(nèi)的智能感知和自主飛行。仿真模擬和實(shí)驗(yàn)結(jié)果表明本文位置控制算法的有效性。本文研究成果可為抽水蓄能電站輸水隧洞無人機(jī)檢測技術(shù)提供參考。

1 基于隧洞圓形特征無人機(jī)定位方法

考慮到抽水蓄能輸水隧洞為圓形特殊工業(yè)環(huán)境,其隧道的紋理特征貧乏,結(jié)構(gòu)特征重復(fù),難于用傳統(tǒng)的激光雷達(dá)測距與建圖 (LiDAR Odometry and Mapping,LOAM)或視覺的方法對(duì)無人機(jī)進(jìn)行定位,本文提出一種基于隧洞圓形特征(曲率、法線及圓心)的無人機(jī)定位方法。無人機(jī)在飛行時(shí),對(duì)于路徑規(guī)劃和最優(yōu)路徑的選取,都相對(duì)圓心來確定自身位置的定位。因此,本文采用圓心算法——Hough變換[10]和對(duì)最小二乘法進(jìn)行優(yōu)化的Levenberg-Marquardt算法提取圓面特征。

隨機(jī)Hough變換提取圓的基本原理是隨機(jī)采樣3個(gè)點(diǎn)來確定一個(gè)圓。檢測圓心坐標(biāo),是根據(jù)圓的幾何性質(zhì),即圓心為圓周上任意兩條不平行弦的中垂線的交點(diǎn)來確定。點(diǎn)云擬合求解圓心示意圖如圖3所示。

最小二乘法求解圓心坐標(biāo)原理如下。設(shè)圓周上有n個(gè)點(diǎn),任意2點(diǎn)坐標(biāo)為(xi,yi)、(xj,yj)(0

(xi-a)2+(yi-b)2=

(xj-a)2+(yj-b)2

(1)

可以列出n-1個(gè)方程的方程組,簡化成矩陣表達(dá)式為:

V=Ax-l

(2)

其中,A、x、l分別為:

(3)

(4)

(5)

圖3 點(diǎn)云擬合求解圓心示意圖

V(Tδ+ΔTδ)≈V(Tδ)+J(Tδ)ΔTδ

(6)

其中,J(Tδ)為V(Tδ)的雅克比矩陣。

Levenberg-Marquardt算法通過比較近似模型和實(shí)際函數(shù)之間的差異來確定ΔTδ的可信賴范圍,給定近似范圍μ,其增量方程為:

‖Tδ‖2≤μ

(7)

增量方程是帶有不等式約束的優(yōu)化問題,用拉格朗日乘子λ將其轉(zhuǎn)化成無約束優(yōu)化問題,即

(8)

(8)式可以通過對(duì)ΔTδ求偏導(dǎo),逐步迭代求解。

而對(duì)于法線,通過最近鄰(k-nearest neighbors,KNN)算法計(jì)算無人機(jī)激光雷達(dá)采集的點(diǎn)云,通過鄰近點(diǎn)云計(jì)算出小區(qū)域的平面,從而獲得相應(yīng)的法向量。

2 基于多傳感器融合的無人機(jī)姿態(tài)估計(jì)

2.1 框架設(shè)計(jì)

在四旋翼無人機(jī)的位姿控制中,以電子羅盤、IMU及氣壓計(jì)等傳感器為基礎(chǔ)的互補(bǔ)濾波姿態(tài)和高度估計(jì)算法相對(duì)成熟。通常以陀螺儀為主,用加速度計(jì)來輔助檢測無人機(jī)的俯仰角和滾轉(zhuǎn)角,使用電子羅盤修正無人機(jī)的偏航角,使用氣壓計(jì)修正無人機(jī)的高度。其算法框架如圖4所示。

圖4 基于多傳感器融合的位姿估計(jì)算法框架

2.2 俯仰角、滾轉(zhuǎn)角及高度估計(jì)

四旋翼無人機(jī)姿態(tài)解算是將陀螺儀、加速度計(jì)及氣壓計(jì)等測得的數(shù)據(jù)進(jìn)行融合得到姿態(tài)角的。陀螺儀測得的數(shù)據(jù)本身存在溫漂和零漂誤差,導(dǎo)致在長時(shí)間探測時(shí)具有較大的誤差,而加速度計(jì)不具備良好的動(dòng)態(tài)能力,因此單獨(dú)使用加速度計(jì)或陀螺儀不能精確地進(jìn)行姿態(tài)解算,需要對(duì)陀螺儀數(shù)據(jù)和加速度數(shù)據(jù)分別進(jìn)行高通濾波和低通濾波,并將濾波后的數(shù)據(jù)進(jìn)行融合求解,從而提高姿態(tài)解算精度?；诨パa(bǔ)濾波器的滾轉(zhuǎn)角、俯仰角算法流程如圖5所示。

圖5 基于互補(bǔ)濾波器的滾轉(zhuǎn)角、俯仰角算法流程

2.3 水平方向位置和偏航角估計(jì)

通過IMU和氣壓計(jì)獲得無人機(jī)的3個(gè)自由度(滾轉(zhuǎn)角、俯仰角及高度)的相對(duì)準(zhǔn)確值后,還需要估計(jì)水平方向位置和偏航角。對(duì)于偏航估計(jì),同樣可以分為特征提取、特征匹配與運(yùn)動(dòng)估計(jì)、位姿優(yōu)化3步,基本原理相近,僅在算法上,由于已知部分姿態(tài)信息,會(huì)有一些差別。以t1～t2時(shí)間段內(nèi)激光雷達(dá)的運(yùn)動(dòng)估計(jì)為例,假設(shè)已知在該時(shí)間段內(nèi)x、y方向的運(yùn)動(dòng)增量和t1時(shí)刻的偏航角,在未知部分姿態(tài)的情況下,是以上一次估計(jì)的運(yùn)動(dòng)增量作為本次運(yùn)動(dòng)增量的估計(jì)值進(jìn)行轉(zhuǎn)換。在已知滾轉(zhuǎn)角、俯仰角及高度情況下,可以更精準(zhǔn)地轉(zhuǎn)換。

根據(jù)當(dāng)前時(shí)刻和上一時(shí)刻的高度,可以求得高度方向的運(yùn)動(dòng)增量,進(jìn)而可以通過坐標(biāo)轉(zhuǎn)換關(guān)系反求得平移增量。由于在無人機(jī)飛行過程中,無人機(jī)的偏航角變化不大,可以假定當(dāng)前無人機(jī)的偏航角與上一時(shí)刻相同。

2.4 過渡段算法邏輯

本算法考慮隧洞工業(yè)環(huán)境的特殊性,作出如下假設(shè):

(1) 隧道本身呈規(guī)則形狀,即本身的立體空間是以立方體、圓柱體及球體為核心的組合空間體。

(2) 隧道表面不存在劇烈不規(guī)則形變,隧洞管道的平面特征可以粗略獲取。

(3) 無人機(jī)在過渡段不會(huì)有橫截面積的突變。

無人機(jī)自主飛行的機(jī)載傳感器(加速度計(jì)、氣壓計(jì)、IMU、激光/紅外測距儀等)中,在位置控制算法中需要涉及的傳感器主要以16線激光雷達(dá)為主,激光/紅外測距儀作為輔助手段。通過轉(zhuǎn)換關(guān)系將無人機(jī)坐標(biāo)轉(zhuǎn)換為世界坐標(biāo)。通過16線激光雷達(dá)對(duì)前側(cè)有明顯線、面特征轉(zhuǎn)變的面進(jìn)行處理,獲得該面的法向量,同時(shí)在無人機(jī)前進(jìn)方向和通過機(jī)身的法向量形成的平面上做法向量的垂線,以保證無人機(jī)在飛行時(shí)相對(duì)底部障礙物的距離為恒定高度,即軸心飛行。

2.5 傾斜段、垂直段飛行

當(dāng)無人機(jī)進(jìn)入水平激光雷達(dá)點(diǎn)云圖呈封閉圖形時(shí),認(rèn)為無人機(jī)進(jìn)入了工作段,即無人機(jī)飛行下一時(shí)刻的位置坐標(biāo)可以通過前一時(shí)刻的位移增量來估計(jì),或者可以通過激光點(diǎn)云圖計(jì)算出其特征軸線或特征中心作為下一時(shí)刻的飛行位置。

3 隧洞無人機(jī)位姿估計(jì)仿真及實(shí)驗(yàn)

3.1 仿真系統(tǒng)框架設(shè)計(jì)

仿真實(shí)驗(yàn)的主要目的是通過仿真獲得無人機(jī)在各種特殊工業(yè)環(huán)境中的最佳飛行路徑和特殊路徑過渡優(yōu)化,同時(shí)要考慮在不同環(huán)境中激光點(diǎn)云的建圖效果和位置估計(jì)精度。本文借鑒多傳感器融合的位姿估計(jì)算法,提出針對(duì)特殊立體的法線、軸線來優(yōu)化無人機(jī)過渡段飛行穩(wěn)定性算法,并通過仿真模擬來驗(yàn)證算法的有效性。

本文的仿真系統(tǒng)運(yùn)行在無人機(jī)操作系統(tǒng)(Robot Operating System,ROS)下,由PX4仿真飛控、自主飛行系統(tǒng)及Gazebo仿真模型3部分構(gòu)成,仿真系統(tǒng)框架如圖6所示,三維管道模型如圖7所示。

圖6 無人機(jī)位姿仿真系統(tǒng)框架

圖7 引水隧道Gazebo模型

PX4是當(dāng)前最流行的開源飛控板之一,其本身是一個(gè)Firmware軟件系統(tǒng),以Nuttx實(shí)時(shí)ARM系統(tǒng)為核心操作系統(tǒng),內(nèi)含Gazebo中使用的傳感器驅(qū)動(dòng)程序,通過傳感器輸出信息到飛控程序,最后轉(zhuǎn)化為電機(jī)轉(zhuǎn)速,輸出到仿真環(huán)境。在本文中,通過IMU、氣壓計(jì)、激光雷達(dá)等計(jì)算無人機(jī)當(dāng)前的位置姿態(tài)信息,并將其發(fā)布在ROS話題中。

3.2 仿真結(jié)果及分析

隧洞三維點(diǎn)云地圖如圖8所示。在點(diǎn)云圖成像上,因?yàn)轱w行不平穩(wěn)和水平段長度的影響,點(diǎn)云成像會(huì)有一定的偏移,但當(dāng)無人機(jī)飛行至鄰近區(qū)域時(shí),會(huì)對(duì)錯(cuò)誤點(diǎn)云信息進(jìn)行自糾正,所以該偏移不影響無人機(jī)本身的飛行。從飛行路徑來看,無人機(jī)在圓柱形管道中的飛行路徑有一定量的累計(jì)誤差,同時(shí)在到達(dá)過渡目標(biāo)點(diǎn)周圍時(shí),無人機(jī)會(huì)在周圍減速,逐步抬高自身位置,進(jìn)入到斜飛段或垂直段。

圖8 隧洞三維點(diǎn)云地圖

無人機(jī)仿真飛行過程中的y方向世界真值與仿真預(yù)測值對(duì)比如圖9所示。

圖9 y方向位姿世界真值與仿真值對(duì)比

從圖9可以看出:

(1) 在整體趨勢上,世界真值與仿真值保持了高度的一致,能夠精準(zhǔn)描繪出隧洞環(huán)境各位置的信息變化和趨勢。

(2) 仿真值與世界真值相比整體偏小,這在一定程度上避免了無人機(jī)發(fā)生碰撞,降低了無人機(jī)墜機(jī)的可能性,但從最后應(yīng)用于檢測環(huán)境來看,會(huì)損失一定的精度,因此還存在一定的優(yōu)化空間。

偏航角對(duì)于無人機(jī)在隧洞內(nèi)的飛行至關(guān)重要,偏航角世界真值和仿真值對(duì)比如圖10所示。從圖10可以看出,無人機(jī)在仿真飛行過程中兩者變化的趨勢高度相似,但其仍然存在以下2個(gè)問題:

問題1 偏航角仿真值的波動(dòng)非常明顯,雖然波動(dòng)范圍較小,但仍然會(huì)使得無人機(jī)在飛行過程中存在不斷左、右偏斜的問題,不利于無人機(jī)的穩(wěn)定飛行和實(shí)際工作環(huán)境中對(duì)于需要檢測位置的鎖定。

問題2 仿真值整體偏大,這會(huì)導(dǎo)致在實(shí)際飛行過程中存在過補(bǔ)償?shù)膯栴},而過補(bǔ)償?shù)膯栴}會(huì)直接導(dǎo)致無人機(jī)不斷糾正自身的偏航角,因而產(chǎn)生問題1。因此,需要對(duì)無人機(jī)飛行過程中的偏航角進(jìn)行精準(zhǔn)計(jì)算和擬合,以確保無人機(jī)在隧洞類環(huán)境飛行過程中的安全性、穩(wěn)定性。

圖10 偏航角世界真值與仿真值對(duì)比

3.3 控制精度分析

3.3.1 位置精度分析

因垂直段飛行立體空間具有特殊性,位置估計(jì)結(jié)果取另一次仿真實(shí)驗(yàn)中傾斜段的數(shù)據(jù)進(jìn)行分析。無人機(jī)在傾斜段飛行的軌跡如圖11所示。將在局部坐標(biāo)系下估計(jì)得到的局部位置,通過無人機(jī)的高度和管道傾角轉(zhuǎn)換到全局坐標(biāo)系中,作為位置參考。

圖11 傾斜段飛行軌跡

從局部坐標(biāo)位置可以看出在切換飛行模式之后,無人機(jī)逐漸運(yùn)動(dòng)到局部坐標(biāo)系的原點(diǎn)并一直保持在原點(diǎn)附近,對(duì)應(yīng)了管道的中軸線。多傳感器融合算法輸出的位置與局部坐標(biāo)估計(jì)轉(zhuǎn)換到全局坐標(biāo)后的結(jié)果基本吻合,兩者之間的偏差最大不超過0.5 m。

3.3.2 速度精度分析

無人機(jī)飛行過程中軸線速度的仿真值與世界真值對(duì)比如圖12所示。從圖12可以看出:兩者的誤差在0.2 m/s之內(nèi);世界真值波動(dòng)較大,但在可接受范圍之內(nèi)。兩者誤差較小,表明無人機(jī)飛行可以得到精準(zhǔn)控制。

圖12 仿真軸線速度仿真值與世界真值對(duì)比

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

在某水電站引水隧洞進(jìn)行無人機(jī)實(shí)地飛行實(shí)驗(yàn)。當(dāng)無人機(jī)進(jìn)入傾斜段時(shí),考慮到點(diǎn)云信息已經(jīng)是完整的圓,沿軸線飛行意味著x、y方向速度期望均值為0 m/s。

實(shí)際采集的傾斜段x、y方向速度部分?jǐn)?shù)據(jù)如圖13所示。

從圖13可以看出,在無人機(jī)實(shí)際飛行過程中,x、y方向上的速度均值分別為-0.031、0.017 m/s,與預(yù)期速度相符;斜段速度以0為均值上、下波動(dòng),最大的波動(dòng)分別為0.3、0.2 m/s。

圖13 傾斜段實(shí)測x、y方向速度

4 結(jié) 論

(1) 從無人機(jī)飛行點(diǎn)云圖成像可以看出,激光雷達(dá)的成像與實(shí)際飛行環(huán)境的情況之間存在一定的誤差,會(huì)出現(xiàn)點(diǎn)云偏離成像,但當(dāng)無人機(jī)位置與管道壁相對(duì)較近時(shí),激光雷達(dá)的成像會(huì)自行糾正本身的錯(cuò)誤成像。激光雷達(dá)的偏離成像會(huì)在一定程度上干擾無人機(jī)在過渡段的飛行,但控制無人機(jī)自身的飛行速度可以將錯(cuò)誤成像的影響降低,實(shí)現(xiàn)較為精準(zhǔn)的飛行路徑規(guī)劃。激光雷達(dá)成像在垂直方向仰角的限制,會(huì)導(dǎo)致激光雷達(dá)成像并不封閉,觀察激光雷達(dá)成像,在圓柱管道仿真環(huán)境內(nèi)飛行上方成像缺失,但其本身并不影響無人機(jī)的飛行,可以忽略。

(2) 無人機(jī)在隧洞類仿真環(huán)境中的飛行,其路徑、飛行速度、位姿估計(jì)等輸出結(jié)果與世界真值比較,誤差均在可以接受范圍之內(nèi),表明本文提出的算法能夠高效、安全地解決無人機(jī)在缺乏GPS信號(hào)、受地磁干擾、光照不足條件下的隧洞類環(huán)境中的自主飛行問題。