基于多傳感器融合的無人機三維狀態估計

王琦 王偉 楊溢 南京信息工程大學 自動化學院

引言

2020年爆發的新冠肺炎可以算的上是整個人類面臨的挑戰，在這場“戰疫”中涌現出許多具有奉獻精神逆行者。同樣在這此挑戰面前，無人機也更進一步的進入了大眾的視野。從正常存在于百米高空的空曠區域飛行至熙熙攘攘鬧市街頭，無人機空投喊話已經成為現實，為這一特殊時期的治安管理等方面做出了不小的貢獻。同樣的，對于公共場所的消毒，使用植保類無人機裝載消毒液即可完成任務，在這樣的高危環境下，使用無人機替代人工作業很大程度上保護了工作人員的安全。對于無人機行業來說，這次天災給無人機帶來的不僅僅是機遇，其實也更是一次挑戰。

無人機從空曠、無人的高空環境到擁擠復雜的街頭，除了要考慮環境對各個傳感器干擾外，還需要考慮無人機飛行時的安全問題。這就要求無人機系統具有較強的穩定性，能應對傳感器如GPS之類的信號丟失等功能。目前在無GPS環境下實現位置估計以及導航一般使用激光雷達與IMU融合或者視覺相機與IMU融合的方案。但在大多數情況下，在無GPS環境下光線不是很充足，而視覺導航對環境的可見度是有要求的，在室內特別是光線比較昏暗的條件下使用視覺相機可能無法取得較理想的效果，甚至會導致無法進行定位以及導航，具有較大的安全隱患。

針對這一情況，本文旨在采用低成本的二維激光雷達傳感器完成可以滿足上述要求的狀態估計系統，為后續的二次功能開發打好基礎。

1 簡介

本系統使用自主研發的飛控，結合IMU，GPS，地磁計，光流，定高雷達以及激光雷達傳感器，實現對在室內環境下無人機的位置，速度以及姿態進行估計。整套系統搭載在四旋翼無人機上，各模塊之間采用Can通信的形式。圖1即為本文的實驗平臺。

圖1 四旋翼無人機平臺

對于室內環境下，采用基于圖優化的激光slam算法完成對無人機的定位以及地圖的構建，結合IMU與光流模塊，使用擴展卡爾曼濾波器融合出無人機機體坐標系下的xy方向的速度信息，采用簡易融合的方式，結合氣壓計以及定高雷達傳感器獲取高度信息；在室外環境下，則使用GPS傳感器，結合IMU模塊與氣壓計推算出無人機實時的經緯度，速度以及高度信息。

2 姿態估計框架

結合上文所述，本文所提出的多傳感器融合的狀態估計結構如圖2所示：

圖2 狀態估計框架

由圖2可知，IMU作為核心傳感器經過坐標系轉換后經過積分后為EKF的預測部分；光流以及GPS可以提供速度信息，GPS與激光雷達可以提供位置信息，定高雷達與氣壓計可以提供高度信息，這些傳感器的數據經過卡方檢測以保證數據的有效性，經過坐標轉換后作為EKF的觀測部分，經過EKF融合之后，生成的結果即為所求。具體的，在室內條件下使用的融合框架為：通過機體坐標系下的IMU的加速度數據積分后與光流的速度數據進行融合，高度方向由IMU的加速度數據的二次積分與氣壓計和定高雷達數據進行融合，判斷無人機處于移動或者靜止狀態來切換氣壓計或者定高雷達作為觀測值；在室外條件下，水平方向上，使用IMU的xy軸加速度數據與GPS的速度數據進行融合，具體為將機體坐標系下的xy軸的加速度轉換到NED坐標系下，作為卡爾曼濾波器的輸入，GPS測量的數據信息則為觀測值，經過卡爾曼濾波器后輸出的結果即為無人機控制系統所使用的無人機當前的速度值。

3 融合算法

從創建到現在的半個世紀之中，卡爾曼濾波(KF)理論目前已經廣泛應用于無人機的狀態估計之中。到目前為止仍然是最有效的估計算法之一。但是在實際使用的當中，大多數系統都屬于非線性系統。如本文中使用的無人機系統就是一個典型的非線性系統。很多研究人員提出了各種的近似估計方法，考慮到所使用的硬件條件的限制，本文選擇使用EKF來進行傳感器數據的融合。

系統的預測方程如下：

在本文當中，狀態量 x=[α,β,γ,p,v,bgyro,bacc]；其中α,β,γ表示無人機的滾轉角，俯仰角以及偏航角，p=[px,py,pz]表示無人機的在NED坐標系下的位置信息，v=[vx,vy,vz]表示無人機在NED坐標系下的速度信息，bgyro,bacc分別表示陀螺儀以及加速度計的偏移量；w_k為高斯白噪聲。

系統的矯正方程如下：

在這項研究中，在室內環境下的估計是基于激光雷達完成的，其測量值為相對距離。在室外環境下的估計是基于GPS傳感器完成的，其測量值為絕對距離。

4 實驗數據及分析

本論文采用的實驗方案為：無人機從室外環境起飛后懸停，之后通過飛手操作直線飛進室內環境，觀察此時無人機是否能在無人工干預的情況下保持定點懸停，觀察這期間無人機的狀態變化，主要觀察無人機姿態角的變化以及由GPS速度到光流傳感器的速度切換是否會出現跳變。

實驗數據表明，在整個飛行過程中，滾轉角以及偏航角方向基本沒有變化，飛機機體坐標系y軸方向的速度也在±0.1范圍之內變動，基本保持不變，經過融合后的速度信息與實際飛行的速度信息基本一致。在實際飛行過程中，GPS信號斷開后，位置融合使用的數據源由GPS傳感器切換至光流傳感器，此時y軸方向的融合出的速度與實際測量值的誤差較GPS條件下的誤差要稍微大一點。導致這種現象的原因主要為光流傳感器的測量值不夠精確導致，但在GPS有效時的速度信息切換至室內光流速度信息時，整個估計系統并未發生較明顯的數據跳變，整體上滿足使用要求，因此，通過實驗可知本文所述的狀態估計方法為實際可行的。

5 結束語

本文以四旋翼無人機系統為例，通過搭載各傳感器進行數據融合實現在室內室外條件下無人機實時狀態的估計。首先介紹了姿態估計的框架，其次介紹融合所使用的算法，最后通過室內以及室外實際飛行試驗驗證算法的有效性。本系統目前已經經過無人機多個架次的實際飛行測試，對于室內無人機的定位與導航具有重要意義，更為無人機系統的二次開發打好基礎。