隧道內(nèi)無人機(jī)位姿估計(jì)與控制

陳艷超，田 偉，樓 勇，陳俊柱，沈潤杰*

（1.華東天荒坪抽水蓄能有限責(zé)任公司，浙江安吉 313302；2.同濟(jì)大學(xué)電子與信息工程學(xué)院，上海 201804）

大型的基礎(chǔ)設(shè)施建設(shè)，如水電站高壓輸水管道、隧道等，隨著使用年限的增加，難免會(huì)出現(xiàn)局部破損現(xiàn)象。為了保證生產(chǎn)和作業(yè)的安全，必須定期進(jìn)行檢修。目前，對(duì)上述工業(yè)場(chǎng)景進(jìn)行檢修，大多采用傳統(tǒng)的人工方法，需要搭建大量的腳手架，耗費(fèi)大量的人力和物力資源，檢修效率低下，并且存在較大的安全隱患。隨著無人機(jī)技術(shù)的發(fā)展，越來越多的工業(yè)環(huán)境逐步借助無人機(jī)進(jìn)行檢修和勘察，提高了檢修效率，降低了安全隱患。實(shí)現(xiàn)無人機(jī)導(dǎo)航與控制的關(guān)鍵技術(shù)在于對(duì)無人機(jī)進(jìn)行位姿估計(jì)。在室外GPS 信號(hào)較強(qiáng)的場(chǎng)景，可以依靠GPS 對(duì)無人機(jī)進(jìn)行定位。遺憾的是，對(duì)于上述工業(yè)場(chǎng)景，其相對(duì)封閉，存在GPS 信號(hào)不強(qiáng)甚至被屏蔽的現(xiàn)象。因此，依靠GPS 進(jìn)行定位的方法在上述場(chǎng)景將無法使用。近年來，隨著人工智能、機(jī)器視覺和激光雷達(dá)技術(shù)的發(fā)展，這類技術(shù)在室內(nèi)地面移動(dòng)機(jī)器人中的應(yīng)用較為成熟。因此，激光雷達(dá)和機(jī)器視覺等人工智能方法是研究封閉空間內(nèi)無人機(jī)定位的可行方案。然而，由于無人機(jī)不同于地面移動(dòng)機(jī)器人，其具有更多的運(yùn)動(dòng)自由度，運(yùn)動(dòng)的不確定性增大，給無人機(jī)的室內(nèi)定位提出更大的挑戰(zhàn)。近年來，國內(nèi)外研究者對(duì)此展開了大量的研究。

1 研究現(xiàn)狀

國內(nèi)外的研究者對(duì)于封閉空間內(nèi)無人機(jī)定位與導(dǎo)航的研究，主要有兩大方法，一種是基于激光雷達(dá)，另一種是基于視覺。其中，基于視覺的方法又可分為機(jī)載視覺和外部視覺。此外，為了進(jìn)一步提高無人機(jī)在無GPS 環(huán)境下定位的準(zhǔn)確度，有研究者采用基于激光雷達(dá)和視覺相結(jié)合的多傳感器融合的方法。

天津大學(xué)古訓(xùn)等人利用二維激光雷達(dá)，通過對(duì)激光雷達(dá)返回的數(shù)據(jù)進(jìn)行分析匹配，結(jié)合慣性導(dǎo)航單元計(jì)算出無人機(jī)的相對(duì)位置姿態(tài)信息［1］。長春理工大學(xué)漢鑫團(tuán)隊(duì)基于地標(biāo)的單目相機(jī)，實(shí)現(xiàn)無人機(jī)在倉庫環(huán)境中對(duì)自身的精確定位［2］。北京航空航天大學(xué)吳琦團(tuán)隊(duì)采用光流和地標(biāo)融合的方法，給出了無人機(jī)在室內(nèi)的精確位置和速度估計(jì)［3］。但是，這種基于地標(biāo)的方法在類似隧道這樣的場(chǎng)景很難實(shí)現(xiàn)。

也有國外研究者對(duì)此展開相關(guān)研究。麻省理工學(xué)院Abraham Bachrach團(tuán)隊(duì)采用掃描匹配算法和視覺里程測(cè)量法實(shí)現(xiàn)無人機(jī)相對(duì)位置的估計(jì)，利用擴(kuò)展卡爾曼濾波（EKF）將這些相對(duì)位置估計(jì)與慣性測(cè)量數(shù)據(jù)相融合，估計(jì)出微型飛行器的完整狀態(tài)［5］?？▋?nèi)基梅隆大學(xué)張繼等人采用三維激光雷達(dá)、攝像頭和IMU 數(shù)據(jù)構(gòu)建環(huán)境地圖，實(shí)現(xiàn)了在高度動(dòng)態(tài)運(yùn)動(dòng)以及黑暗、無紋理、無結(jié)構(gòu)環(huán)境中的位姿估計(jì)，在9. 3 公里的導(dǎo)航范圍內(nèi)相對(duì)位置漂移率僅為0. 22%［6］。賓夕法尼亞大學(xué)Tolga ?zaslan 等人設(shè)計(jì)了搭載IMU、四個(gè)攝像頭和兩個(gè)激光雷達(dá)的水壩管道巡檢無人機(jī)，通過機(jī)載照明進(jìn)行補(bǔ)光，實(shí)現(xiàn)弱光環(huán)境下沿管道軸線方向的位置估計(jì)［7］。

綜合國內(nèi)外對(duì)無人機(jī)在封閉空間內(nèi)的位姿估計(jì)研究狀況，目前單靠激光雷達(dá)進(jìn)行無人機(jī)位姿估計(jì)的研究仍然較少，大多數(shù)需要融合視覺傳感器或者進(jìn)行外部標(biāo)識(shí)，而視覺傳感器對(duì)于室內(nèi)環(huán)境的特征點(diǎn)有較高的要求，對(duì)于特征點(diǎn)不明顯的場(chǎng)景則很難適用。對(duì)于水電站高壓輸水管道和隧道等光線較差的場(chǎng)景，需要進(jìn)行補(bǔ)光來提高圖像的識(shí)別度。并且由于這類工業(yè)場(chǎng)景的特殊性，特征不明顯且難以進(jìn)行外部標(biāo)識(shí)，種種約束條件給無人機(jī)在上述封閉空間中的定位與導(dǎo)航提出了巨大的挑戰(zhàn)。本文的研究目的是設(shè)計(jì)一種能夠適應(yīng)隧道場(chǎng)景的無人機(jī)位姿估計(jì)與控制方案。

2 無人機(jī)位姿估計(jì)算法

由國內(nèi)外研究可知，目前對(duì)于封閉空間中無人機(jī)的定位主要采用的是激光SLAM 和視覺SLAM 的方法。由于隧道中存在光線較弱的情況，以視覺主導(dǎo)的方法很難有較好的效果。因此本方案采用激光SLAM的方法。而對(duì)于激光雷達(dá)定位的方法，從激光雷達(dá)的角度劃分，有采用單線二維激光雷達(dá)，也有采用多線三維激光雷達(dá)。由于多旋翼無人機(jī)的載重有限，考慮到多線激光雷達(dá)重量較重的問題，本方案采用兩個(gè)正交放置的單線激光雷達(dá)對(duì)無人機(jī)進(jìn)行位姿估計(jì)。水平放置的激光雷達(dá)采用Hector SLAM［8］算法對(duì)水平方向的位姿進(jìn)行估計(jì)，豎直放置的激光雷達(dá)對(duì)垂直方向上的環(huán)境進(jìn)行感知，對(duì)無人機(jī)的高度進(jìn)行定位，從而實(shí)現(xiàn)無人機(jī)在隧道中的三維位姿估計(jì)。

假設(shè)在一個(gè)連續(xù)的地圖中，有一點(diǎn)的坐標(biāo)為Pm，如圖1 所示。 用M(Pm) 表示該坐標(biāo)的占有值，則M(Pm)沿x軸和y軸的方向梯度為

圖1 柵格坐標(biāo)圖Fig.1 Grid diagram

其中，M(Pm)可由最鄰近點(diǎn)P00，P01，P10，P11估算獲得。

其結(jié)果為

進(jìn)一步可計(jì)算得到M(Pm)的方向梯度為

Hector SLAM 算法采用掃描匹配的方法，將激光掃描數(shù)據(jù)與當(dāng)前已建立的地圖進(jìn)行匹配，通過高斯牛頓方法尋找當(dāng)前的掃描點(diǎn)在地圖中的最優(yōu)位姿。其計(jì)算過程如下：

式（5）中，ξ= (px，py，ψ)T為剛體旋轉(zhuǎn)變換矩陣，即無人機(jī)在世界坐標(biāo)系下的位置坐標(biāo)，Si(ξ)為激光掃描點(diǎn)si=(si，x，si，y)T在世界坐標(biāo)系下的坐標(biāo)。Si(ξ)的計(jì)算公式如下：

在給定ξ的初值后，需要估計(jì)Δξ，可通過以下方法對(duì)測(cè)量誤差進(jìn)行優(yōu)化，即

對(duì)式（7）進(jìn)行一階泰勒展開并且求解關(guān)于Δξ的偏導(dǎo)數(shù)，可得到位姿變化量為

式（8）中，

至此，完成了無人機(jī)水平位置以及偏航角的估計(jì)。在此基礎(chǔ)上，通過豎直放置的激光雷達(dá)對(duì)垂直方向上的環(huán)境進(jìn)行感知，對(duì)無人機(jī)的高度進(jìn)行定位，從而實(shí)現(xiàn)了無人機(jī)在隧道中的三維位姿估計(jì)。

3 無人機(jī)飛行位置控制策略

在完成了無人機(jī)在隧道中的位姿估計(jì)后，為了使無人機(jī)能夠沿隧道的中軸線飛行，需要估算出隧道的中軸線。以水平方向?yàn)槔?，以水平方向的激光雷達(dá)中心Os為坐標(biāo)原點(diǎn)建立平面直角坐標(biāo)系XsOsYs，如圖2所示。其中兩側(cè)的豎直直線代表隧道內(nèi)壁，以O(shè)s為起點(diǎn)的射線為激光線束，d為Os到隧道內(nèi)壁的距離，?為線束與Xs軸正方向的夾角。

圖2 水平激光雷達(dá)掃描圖Fig.2 Horizontal lidar scan

單線激光雷達(dá)獲取的點(diǎn)云數(shù)據(jù)是以極坐標(biāo)(dsi，θsi)的形式表示的，通過該數(shù)據(jù)可以將點(diǎn)云數(shù)據(jù)轉(zhuǎn)換為激光雷達(dá)坐標(biāo)系下的直角坐標(biāo)。轉(zhuǎn)換公式如下：

另外，由于無人機(jī)在飛行的過程中可能存在一定的滾轉(zhuǎn)角φ，水平面和垂直面的激光雷達(dá)掃描面不是相對(duì)于世界坐標(biāo)系下的水平面和豎直面，其水平面的掃描面如圖3 所示，其中d為獲取的距離數(shù)據(jù)，d′為實(shí)際的距離數(shù)據(jù)。

圖3 機(jī)體非水平時(shí)的激光雷達(dá)掃描圖Fig.3 Lidar scan of the body when it is not horizontal

激光掃描面與水平面存在一定的夾角φ。為了得到水平面的實(shí)際距離，需要對(duì)獲取的點(diǎn)云數(shù)據(jù)進(jìn)行修正，采用融合IMU 的方法。通過IMU 獲取機(jī)體的滾轉(zhuǎn)角φ，根據(jù)下式進(jìn)行修正，即

修正后，在激光雷達(dá)坐標(biāo)系下激光點(diǎn)云的直角坐標(biāo)為

無人機(jī)飛行過程中存在俯仰角和偏航角時(shí)的修正方法與上述方法類似。

在得到點(diǎn)云的空間直角坐標(biāo)后，隧道內(nèi)壁就已確定，接著通過隧道內(nèi)壁的坐標(biāo)確定中軸線上點(diǎn)的坐標(biāo)。由于激光雷達(dá)在隧道中的掃描具有對(duì)稱性，以水平面為例，在誤差允許的范圍內(nèi)，可以找到關(guān)于中軸線對(duì)稱的兩個(gè)點(diǎn)(xs1，ys1)和(xs2，ys2)，接著利用以下公式求出兩個(gè)點(diǎn)的中點(diǎn)，即為中軸線上的點(diǎn)。

垂直方向的中軸線解算方法與水平方向類似。將水平方向的中軸線和垂直方向的中軸線融合，即可得到隧道的中軸線。至此，就得到了在激光雷達(dá)坐標(biāo)系下的中軸線坐標(biāo)。最后，通過激光雷達(dá)坐標(biāo)系與世界坐標(biāo)系之間的變換關(guān)系可求得在世界坐標(biāo)系下的中軸線坐標(biāo)。

將解算的隧道中軸線作為無人機(jī)飛行的期望位置，同時(shí)結(jié)合位姿估計(jì)算法估計(jì)無人機(jī)當(dāng)前位置，實(shí)現(xiàn)無人機(jī)在隧道中位置的閉環(huán)控制。

4 無人機(jī)仿真實(shí)驗(yàn)

為了驗(yàn)證無人機(jī)位姿估計(jì)算法和位置控制策略的有效性和準(zhǔn)確度，本文進(jìn)行了仿真實(shí)驗(yàn)。該仿真實(shí)驗(yàn)基于ROS 和Gazebo 仿真環(huán)境。該仿真環(huán)境可以模擬機(jī)器人的運(yùn)動(dòng)情況，也可以導(dǎo)入相關(guān)的傳感器插件，在機(jī)器人上安置不同的傳感器，如攝像頭、激光雷達(dá)和IMU 等，通過這些傳感器可以獲取機(jī)器人當(dāng)前所在仿真環(huán)境中的感知數(shù)據(jù)。通過SolidWorks繪制簡易的隧道場(chǎng)景，并通過Urdf插件生成Gazebo仿真環(huán)境。仿真環(huán)境如圖4 所示。該場(chǎng)景為田字形隧道，隧道的最大長度為20 m，最大寬度為2 m。

圖4 仿真實(shí)驗(yàn)場(chǎng)景Fig.4 Simulation experiment scene

實(shí)驗(yàn)時(shí)，無人機(jī)通過位姿估計(jì)算法估計(jì)無人機(jī)當(dāng)前的位姿，同時(shí)根據(jù)設(shè)定的目標(biāo)運(yùn)行軌跡和隧道中軸線解算方法獲得無人機(jī)的期望位置，無人機(jī)沿隧道中軸線自主飛行。實(shí)驗(yàn)中，水平環(huán)境的建圖效果和無人機(jī)的飛行軌跡如圖5所示?？梢钥吹?，水平環(huán)境的建圖效果和真實(shí)場(chǎng)景相符，無人機(jī)的飛行軌跡與預(yù)設(shè)的航跡一致。

圖5 無人機(jī)飛行軌跡與建圖效果Fig.5 Uav’s flight trajectory and building diagram effect

為了驗(yàn)證位姿估計(jì)算法的準(zhǔn)確度，在實(shí)驗(yàn)過程中，采用兩種方法對(duì)無人機(jī)進(jìn)行位姿估計(jì)。一種是本文所提出的位姿估計(jì)算法，另一種是通過在仿真環(huán)境中無人機(jī)所搭載的GPS 進(jìn)行無人機(jī)的位姿估計(jì)。通過記錄兩種方法估計(jì)的無人機(jī)位姿數(shù)據(jù)，繪制無人機(jī)的飛行軌跡曲線如圖6所示。從圖6中可以看出，通過本文所提出的位姿估計(jì)算法估計(jì)出來的無人機(jī)飛行軌跡與通過GPS獲得的飛行軌跡基本重合。

圖6 GPS與本文算法估計(jì)的位置對(duì)比圖Fig.6 A comparison of positions estimated by GPS and the algorithm

為了定量地描述位姿估計(jì)算法與GPS 估計(jì)的誤差，進(jìn)一步求出同一時(shí)刻用位姿估計(jì)算法估計(jì)的位置與用GPS估計(jì)的位置的距離。實(shí)驗(yàn)位置誤差如圖7所示。其誤差主要受激光雷達(dá)的掃描頻率和無人機(jī)飛行狀況的影響。由位置誤差圖可見，在當(dāng)前的實(shí)驗(yàn)條件下，通過位姿估計(jì)算法估計(jì)出來的無人機(jī)位置誤差較小，準(zhǔn)確度較高，滿足無人機(jī)定位需求。

圖7 位置誤差圖Fig.7 Position error diagram

5 結(jié)論

本文針對(duì)隧道內(nèi)無人機(jī)無法通過GPS 進(jìn)行定位的問題，設(shè)計(jì)了一套無人機(jī)位姿估計(jì)與控制方案。通過在Gazebo 仿真環(huán)境中開展相關(guān)的實(shí)驗(yàn)，驗(yàn)證了該方案的有效性和準(zhǔn)確度，為隧道無人機(jī)巡檢提出了可行的定位和控制方案。