隧道內無人機位姿估計與控制

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

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

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

1 研究現狀

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

天津大學古訓等人利用二維激光雷達，通過對激光雷達返回的數據進行分析匹配，結合慣性導航單元計算出無人機的相對位置姿態信息［1］。長春理工大學漢鑫團隊基于地標的單目相機，實現無人機在倉庫環境中對自身的精確定位［2］。北京航空航天大學吳琦團隊采用光流和地標融合的方法，給出了無人機在室內的精確位置和速度估計［3］。但是，這種基于地標的方法在類似隧道這樣的場景很難實現。

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

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

2 無人機位姿估計算法

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

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

圖1 柵格坐標圖Fig.1 Grid diagram

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

其結果為

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

Hector SLAM 算法采用掃描匹配的方法，將激光掃描數據與當前已建立的地圖進行匹配，通過高斯牛頓方法尋找當前的掃描點在地圖中的最優位姿。其計算過程如下：

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

在給定ξ的初值后，需要估計Δξ，可通過以下方法對測量誤差進行優化，即

對式（7）進行一階泰勒展開并且求解關于Δξ的偏導數，可得到位姿變化量為

式（8）中，

至此，完成了無人機水平位置以及偏航角的估計。在此基礎上，通過豎直放置的激光雷達對垂直方向上的環境進行感知，對無人機的高度進行定位，從而實現了無人機在隧道中的三維位姿估計。

3 無人機飛行位置控制策略

在完成了無人機在隧道中的位姿估計后，為了使無人機能夠沿隧道的中軸線飛行，需要估算出隧道的中軸線。以水平方向為例，以水平方向的激光雷達中心Os為坐標原點建立平面直角坐標系XsOsYs，如圖2所示。其中兩側的豎直直線代表隧道內壁，以Os為起點的射線為激光線束，d為Os到隧道內壁的距離，?為線束與Xs軸正方向的夾角。

圖2 水平激光雷達掃描圖Fig.2 Horizontal lidar scan

單線激光雷達獲取的點云數據是以極坐標(dsi，θsi)的形式表示的，通過該數據可以將點云數據轉換為激光雷達坐標系下的直角坐標。轉換公式如下：

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

圖3 機體非水平時的激光雷達掃描圖Fig.3 Lidar scan of the body when it is not horizontal

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

修正后，在激光雷達坐標系下激光點云的直角坐標為

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

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

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

將解算的隧道中軸線作為無人機飛行的期望位置，同時結合位姿估計算法估計無人機當前位置，實現無人機在隧道中位置的閉環控制。

4 無人機仿真實驗

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

圖4 仿真實驗場景Fig.4 Simulation experiment scene

實驗時，無人機通過位姿估計算法估計無人機當前的位姿，同時根據設定的目標運行軌跡和隧道中軸線解算方法獲得無人機的期望位置，無人機沿隧道中軸線自主飛行。實驗中，水平環境的建圖效果和無人機的飛行軌跡如圖5所示。可以看到，水平環境的建圖效果和真實場景相符，無人機的飛行軌跡與預設的航跡一致。

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

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

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

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

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

5 結論

本文針對隧道內無人機無法通過GPS 進行定位的問題，設計了一套無人機位姿估計與控制方案。通過在Gazebo 仿真環境中開展相關的實驗，驗證了該方案的有效性和準確度，為隧道無人機巡檢提出了可行的定位和控制方案。