李 昕，孟碩林，黃觀文，張 勤，梁峻唯

（長安大學(xué) 地質(zhì)工程與測繪學(xué)院，西安 710054）

近年來，旋翼無人機（Unmanned Aerial Vehicle,UAV）技術(shù)得到了迅速發(fā)展。與固定翼飛行器相比，旋翼無人機具備更高的飛行靈活性和更強的適應(yīng)性，因此在各個領(lǐng)域都得到了廣泛應(yīng)用。然而，要實現(xiàn)旋翼無人機的精準(zhǔn)姿態(tài)控制和位置控制，關(guān)鍵在于導(dǎo)航模塊必須提供穩(wěn)定可靠的導(dǎo)航數(shù)據(jù)。通常情況下，這些導(dǎo)航數(shù)據(jù)是通過基于卡爾曼濾波的GNSS/SINS（Global Navigation Satellite System/Strapdown Inertial Navigation System）組合導(dǎo)航算法計算獲得[1]。目前，采用的慣性基濾波算法通常是基于小失準(zhǔn)角[2-4]，要求在初始對準(zhǔn)階段獲得高精度的姿態(tài)信息，以確保后續(xù)濾波算法的精度和穩(wěn)定性，否則可能會引入較大的模型誤差，導(dǎo)致濾波的收斂速度減慢，甚至無法收斂。另一方面，由于旋翼無人機通常會受到強烈的高頻振動影響，導(dǎo)致慣性測量單元（Inertial Measurement Unit,IMU）的測量噪聲水平顯著升高[5]，這可能會加劇初始姿態(tài)不準(zhǔn)確所導(dǎo)致的導(dǎo)航誤差。因此，為了解決這一問題，需要考慮采用更精細的姿態(tài)控制和導(dǎo)航方法，以提高UAV 在各種工作環(huán)境下的導(dǎo)航性能。

UAV 可以使用磁力計獲得較為準(zhǔn)確的初始方位，然而搭載的電子羅盤易受到電力線等強電磁干擾，僅依靠磁力計難以保證準(zhǔn)確的姿態(tài)。受限于載重能力和成本，UAV 的組合導(dǎo)航模塊多采用低成本微機電系統(tǒng)（Micro Electro Mechanical System,MEMS）IMU，其陀螺儀零偏噪聲一般較大，靜基座無法精確地敏感到地球自轉(zhuǎn)信息[6]，因此常采用動態(tài)對準(zhǔn)方案。對于旋翼機，任意時刻的飛行方向具有不確定性，車載和固定翼飛行器常用的速度矢量法等簡單可靠的動態(tài)粗對準(zhǔn)方案不再適用。目前有三種常用解決方案應(yīng)用于UAV 初始對準(zhǔn)：1）基于比力方程的動態(tài)粗對準(zhǔn)算法，通過GNSS 計算載體在導(dǎo)航系下的加速度，構(gòu)造載體系與導(dǎo)航系的轉(zhuǎn)換關(guān)系，進而求解姿態(tài)角。文獻[6]采用載體短時直線加速運動實現(xiàn)粗對準(zhǔn)，對載體的運動有一定的限制；文獻[7]利用慣性導(dǎo)航系統(tǒng)的比力方程建立姿態(tài)的精確關(guān)系表達式，并采用數(shù)值求解算法實現(xiàn)空中對準(zhǔn)。2）基于大失準(zhǔn)角條件下的非線性誤差模型與濾波的對準(zhǔn)算法，結(jié)合非線性濾波器處理非線性誤差模型。文獻[8]提出了一種結(jié)合大失準(zhǔn)角SINS 誤差模型、回溯法和非線性卡爾曼濾波的對準(zhǔn)算法，實現(xiàn)了在大失準(zhǔn)角條件下高精度的初始對準(zhǔn)；3）基于優(yōu)化的初始對準(zhǔn)方案，將慣導(dǎo)對準(zhǔn)轉(zhuǎn)化為尋找最小特征向量的優(yōu)化問題。文獻[9]根據(jù)推導(dǎo)的速度/位置積分公式，提出了一種基于優(yōu)化的粗對準(zhǔn)方法，利用GNSS位置/速度信息作為輸入，在短時間內(nèi)獲得高精度的初始航向角，但對初始噪聲方差信息較為敏感。上述這些傳統(tǒng)對準(zhǔn)方法在對準(zhǔn)精度、收斂速度以及載體機動需求等方面還存在一定的提升空間。

GNSS/SINS 組合導(dǎo)航及對準(zhǔn)中傳統(tǒng)擴展卡爾曼濾波（Extended Kalman Filter,EKF）在定義位置和速度等狀態(tài)誤差時僅顧及大小差異，而忽視方向上的差異，這會導(dǎo)致濾波初值特別是姿態(tài)失準(zhǔn)角誤差較大時，狀態(tài)誤差坐標(biāo)系定義不一致的問題[10,11]。近些年基于李群和流形理論發(fā)展了一種新的濾波方法，即SEk(3)-EKF。這種濾波將傳統(tǒng)歐氏空間的姿態(tài)、速度、位置等狀態(tài)及誤差定義在了群空間，通過利用李群的乘法封閉特性以及與李代數(shù)之間的仿射特性，構(gòu)造出相應(yīng)的非線性誤差量，進而推導(dǎo)出獨立于狀態(tài)的線性狀態(tài)空間模型。由于狀態(tài)空間模型獨立于狀態(tài)估值，能夠保持“不變”，因此稱之為不變EKF[12]。由于新濾波重新定義的速度、位置誤差中包含了姿態(tài)項，能夠更加準(zhǔn)確地表征真正的誤差[13]。文獻[14]對SEk(3)-EKF 的特性和相應(yīng)濾波框架進行了深入研究，并給出慣性基的左不變和右不變EKF 在常見坐標(biāo)框架下的具體數(shù)學(xué)表達式；文獻[15]提出了一種基于李群的等變?yōu)V波方法，通過實驗證明在大失準(zhǔn)角條件下，左不變等變?yōu)V波方法表現(xiàn)出更好的性能，優(yōu)于傳統(tǒng)的EKF 方法；文獻[16]對速度誤差進行變換，實現(xiàn)了基于不變EKF 的準(zhǔn)靜基座大失準(zhǔn)角的初始對準(zhǔn)，該方法在載體晃動下表現(xiàn)出較好的性能。

本文研究了旋翼無人機SINS 的動態(tài)初始對準(zhǔn)問題，并基于MEMS 慣導(dǎo)機械編排理論推導(dǎo)出一種不依賴磁力計的基于GNSS 速度輔助的左不變SE(3)-EKF方法?？紤]到GNSS 定位結(jié)果可能在固定解、精度較低的浮點解和單點解之間頻繁切換，而GNSS 多普勒測速精度相對穩(wěn)定[17]，且速度測量可以更快地修正方位，因此本方法將姿態(tài)和速度用于構(gòu)建更簡潔的矩陣?yán)钊?，并在此基礎(chǔ)上進行GNSS 速度量測更新。不同于SE2(3)-EKF，本文僅采用速度之差作為量測向量，稱之為SE(3)-EKF，對比分析了該方法相對于傳統(tǒng)EKF 和非線性無跡卡爾曼濾波（Unscented Kalman Filter,UKF）的優(yōu)勢，通過旋翼無人機GNSS/SINS 組合導(dǎo)航飛行實驗，驗證了不同初始失準(zhǔn)角誤差情況下本文初始對準(zhǔn)算法的性能。

1 基于SE(3)-EKF 的動態(tài)對準(zhǔn)模型

SE(3)-EKF 和傳統(tǒng)EKF 一般都使用誤差作為待估狀態(tài)，主要區(qū)別是SE(3)-EKF 不是簡單地直接采用估計狀態(tài)與真實狀態(tài)的差值作為誤差，而是更為細致地考慮了狀態(tài)定義的坐標(biāo)框架一致性問題，并通過在李群空間重新定義誤差，提供了更為嚴(yán)密和緊湊的數(shù)學(xué)形式。本文采用GNSS 速度輔助SINS 對準(zhǔn)，為了描述狀態(tài)誤差，設(shè)計了包含姿態(tài)、速度的李群狀態(tài)誤差模型，并將陀螺零偏誤差和加速度計零偏誤差仍定義在傳統(tǒng)向量空間，在此基礎(chǔ)上，推導(dǎo)了狀態(tài)誤差的微分方程以及GNSS 速度測量的更新方程。

1.1 李群框架下慣導(dǎo)誤差

本文選擇地心地固坐標(biāo)系（Earth-Centered Earth-Fixed,ECEF）作為組合導(dǎo)航系統(tǒng)的投影坐標(biāo)系，記為e系。使用ECEF 坐標(biāo)系下的慣導(dǎo)機械編排算法進行慣導(dǎo)更新[18]，定義投影到e系下的姿態(tài)、速度狀態(tài)為一個群（矩陣）χ。

根據(jù)李群的性質(zhì)有：

其中，χ-1表示χ的逆，同樣滿足χ-1∈SE(3)。

若χ表示真實狀態(tài)，表示標(biāo)稱狀態(tài)，定義姿態(tài)和速度狀態(tài)誤差為η∈SE(3)，區(qū)別于傳統(tǒng)狀態(tài)矢量(X-)做差的數(shù)學(xué)形式。

李群空間中誤差定義分為左不變η=和右不變η=兩種類型。本文選擇左不變誤差進行處理，根據(jù)式(1)(2)，左不變誤差η以及零偏誤差ζ的具體表達式為：

通過式(3)定義的誤差形式可發(fā)現(xiàn)，在李群框架下定義的速度誤差中包含了姿態(tài)項，同時考慮了真值和估值在數(shù)值大小和方向兩個方面的差異，因此誤差定義上相對于傳統(tǒng)向量空間做差的方法更為合理、緊湊，解決了速度誤差狀態(tài)定義的基準(zhǔn)不一致問題。

1.2 李群框架下慣導(dǎo)左不變誤差方程

其中，θ=為軸角。

若φ為較小角度，在流形上微分可得與之對應(yīng)的切空間，李群的幺元處切空間則為對應(yīng)李代數(shù)[13]，由李群與李代數(shù)的特性，誤差η滿足：

其中，姿態(tài)、速度誤差表示為ξ=[φ]T，^符號表示將向量空間映射到李代數(shù)的線性同構(gòu)。

依據(jù)MEMS 慣導(dǎo)誤差模型，可推導(dǎo)出李群狀態(tài)誤差的微分方程，忽略高階的誤差項，李群空間下的姿態(tài)和速度誤差方程具體形式為[14]：

零偏誤差可建模為隨機游走過程：

其中，wbg、wba為陀螺和加速度計零偏白噪聲。

所有的狀態(tài)誤差項為δ x，系統(tǒng)噪聲項為w，則SE(3)-EKF 狀態(tài)方程為：

其中，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣；G為系統(tǒng)噪聲的分配矩陣。

式(8)-(11)參數(shù)化的具體形式為：

IMU 采樣間隔為Δt，離散化后的狀態(tài)轉(zhuǎn)移矩陣記為Φ，過程噪聲矩陣滿足Q=E[wwT]，濾波預(yù)測的狀態(tài)誤差δ xk,k-1及對應(yīng)的協(xié)方差矩陣Pk,k-1遞推公式為：

其中，q是白噪聲w對應(yīng)的方差強度矩陣，其量綱單位與功率譜密度單位一致。

1.3 SE(3)-EKF 速度量測方程

GNSS 可提供位置和速度信息作為卡爾曼濾波的量測信息，用于抑制IMU 推算結(jié)果的發(fā)散。實際情況下，慣導(dǎo)系統(tǒng)的初始對準(zhǔn)經(jīng)常面臨復(fù)雜環(huán)境，GNSS輸出結(jié)果可能會在固定解、浮動解和單點解之間頻繁切換，導(dǎo)致GNSS 輸出的位置信息精度不一致；GNSS多普勒測速并不依賴于基準(zhǔn)站，相對較為穩(wěn)定。另外，對于MEMS 慣導(dǎo)，可以忽略由位置誤差引起的重力加速度誤差以及由地球自轉(zhuǎn)引起的導(dǎo)航坐標(biāo)系旋轉(zhuǎn)誤差，位置誤差的微分方程僅與速度誤差相關(guān)，本文關(guān)注的姿態(tài)對準(zhǔn)不會受益于位置誤差的微分方程。因此，本文僅采用GNSS 測量得到的速度作為量測，并給出SE(3)-EKF 的速度量測方程的推導(dǎo)。

假設(shè)IMU與GNSS天線相位中心之間的桿臂量為lb，滿足：

其中，vGNSS為GNSS 速度。(×)可表示為：

將式(19)代入式(18)，并利用反對稱矩陣相似變換性質(zhì)[18]：

考慮速度和姿態(tài)存在誤差，并顧及IMU 陀螺零偏，由IMU 推算的GNSS 天線相位中心的速度為：

對式(22)進行擾動，得到：

其中，δ ve為e系中的速度誤差。

忽略二階小量對式(23)整理得到：

GNSS 實際測量的速度為：

其中，nv為GNSS 測速白噪聲，滿足=Rv。

式(24)和式(25)做差，得到濾波的量測方程：

因此，量測矩陣H參數(shù)化形式為：

SE(3)-EKF 與常規(guī)EKF 的增益矩陣形式一致，狀態(tài)更新與協(xié)方差矩陣計算方法也一致。

其中，δ zk代表k時刻的觀測誤差量。

根據(jù)式(31)更新狀態(tài)誤差δ xk，進行誤差閉環(huán)修正。若bk為陀螺和加速度計的零偏向量，則得到更新后的狀態(tài)為：

2 實驗與分析

本次無人機飛行實驗于2022 年11 月在西安市雁塔區(qū)進行，使用了大疆經(jīng)緯MATRICE 600 PRO 六旋翼無人機，該無人機搭載了POMS-G6615D6 組合導(dǎo)航設(shè)備，其中MEMS 陀螺儀和加速度計的零偏穩(wěn)定性分別為10 °/h 及0.1 mg，隨機游走參數(shù)分別為及。IMU 的采樣率為100 Hz，GNSS 板卡采用的是諾瓦泰718D。通過使用IE 軟件進行緊組合，前后向平滑處理實驗數(shù)據(jù)得到姿態(tài)的準(zhǔn)確值。本次實驗設(shè)備和無人機飛行的三維軌跡如圖1 所示。

圖1 設(shè)備安裝圖和無人機飛行軌跡圖Fig.1 Equipment installation and trajectory of UAV

由于旋翼無人機在飛行時通常不會出現(xiàn)太大的俯仰角和橫滾角變化，但航向角的變化較大，因此，在無人機飛行的初始階段模擬了三種不同的初始失準(zhǔn)角場景，旨在比較傳統(tǒng)的EKF 對準(zhǔn)方法、UKF 對準(zhǔn)方法以及本文提出的SE(3)-EKF 對準(zhǔn)方法，并對這三種方法的性能進行評估。本文選擇飛行過程約200 s 的數(shù)據(jù)進行計算和分析。第一種場景存在較小的橫滾和俯仰誤差，但航向誤差較大，失準(zhǔn)角α=[3 ° 4 ° 45 °]；第二種場景存在較小的橫滾和俯仰誤差，但航向誤差更大，失準(zhǔn)角α=[3 ° 4 ° 95 °]；第三種場景是航向、俯仰和橫滾角均嚴(yán)重失準(zhǔn)，失準(zhǔn)角α=[20 °20 ° 175 °]。

2.1 實驗一

在實驗一中，三個方向上的失準(zhǔn)角模擬為α=[3 ° 4 ° 45 °]。圖2 顯示了三種方法估計的姿態(tài)角誤差，可以看出在該場景下，三種方法都能夠?qū)⒆藨B(tài)角收斂到相對穩(wěn)定的范圍。從局部放大圖可發(fā)現(xiàn)，三種方法的水平姿態(tài)角收斂時間差異不大，航向在100 s 左右均趨于收斂。表1 給出姿態(tài)角收斂后的均方根誤差（Root Mean Squared Error,RMSE）。從表1 可以看出，三種方法對準(zhǔn)后的姿態(tài)角精度相差不大，水平姿態(tài)約為0.1 °，航向達到0.5 °左右。總的來說，在實驗一的失準(zhǔn)角情況下，三種方法的對準(zhǔn)精度和效率差距不大，SE(3)-EKF 表現(xiàn)最優(yōu)。這是因為模擬的初始失準(zhǔn)角相對較小，傳統(tǒng)EKF 的非線性程度不算嚴(yán)重，傳統(tǒng)速度誤差定義在向量空間造成的基準(zhǔn)不一致性相對較小。

表1 姿態(tài)角誤差的RMSE（單位：°）Tab.1 RMSE of attitude angle error (Unit: °)

圖2 實驗1 橫滾角、俯仰角和航向角誤差Fig.2 The roll,pitch,and yaw error of experiment 1

2.2 實驗二

在實驗二中，航向角誤差進一步增大，此時失準(zhǔn)角為α=[3 ° 4 ° 95 °]。圖3 展示了三種方法估計的姿態(tài)角誤差。當(dāng)初始航向角誤差進一步增大，傳統(tǒng)EKF線性模型的非線性程度也相應(yīng)增加，導(dǎo)致的模型誤差進一步增大，需要更多的GNSS 量測校正，因此EKF在姿態(tài)的收斂速度上比UKF方法和SE(3)-EKF方法要慢。UKF 方法對模型的非線性程度沒有限制，并且誤差模型沒有小失準(zhǔn)角假設(shè)限制，SE(3)-EKF 方法在李群框架下重新定義了更為嚴(yán)謹(jǐn)?shù)乃俣日`差模型，此二者對大失準(zhǔn)角情況下收斂速度更快。在大失準(zhǔn)角的實驗場景中，雖然UKF 方法能在一定程度上彌補EKF的缺陷，但仍然比SE(3)-EKF 方法慢，并且收斂精度也更低。相比于實驗一，UKF 對準(zhǔn)精度也有所下降。因此，在初始失準(zhǔn)角較大的情況下，將速度誤差定義為估計值與真值的直差形式已經(jīng)不再合理，此時采用大失準(zhǔn)角誤差模型結(jié)合UKF 濾波對準(zhǔn)方案的效果會受到一定影響，而SE(3)-EKF 方法在速度群誤差定義方面更加嚴(yán)謹(jǐn)，因此在這種情況下表現(xiàn)較為穩(wěn)定。表2 統(tǒng)計的姿態(tài)角誤差RMSE 表明：SE(3)-EKF 方法在短時間內(nèi)（200 s）實現(xiàn)了水平姿態(tài)對準(zhǔn)精度約為0.1 °，其精度相對于傳統(tǒng)EKF 方法提高了50%以上，相對于UKF 方法提高了20%以上；航向失準(zhǔn)角精度約為0.4 °，相對于EKF 方法提高了45%以上，相對于UKF方法提高了43%以上。

圖3 實驗2 橫滾角、俯仰角和航向角誤差Fig.3 The roll,pitch and yaw error of experiment 2

圖4 實驗3 橫滾角、俯仰角和航向角誤差Fig.4 The roll,pitch and yaw error of experiment 3

2.3 實驗三

實驗三模擬了極端失準(zhǔn)的情況，失準(zhǔn)角為α=[20 °20 ° 175 °]。在這種情況下，模型的非線性程度非常嚴(yán)重，導(dǎo)致傳統(tǒng)的EKF 和UKF 方法對應(yīng)的航向誤差在200 s 內(nèi)發(fā)散，而SE(3)-EKF 方法仍然能夠迅速地實現(xiàn)姿態(tài)角收斂，表明了SE(3)-EKF 中基于李代數(shù)的線性狀態(tài)誤差能夠很好地表征李群上的非線性誤差，具有更高的濾波精度。表3 統(tǒng)計的姿態(tài)角誤差RMSE 表明：在極端失準(zhǔn)情況下，SE(3)-EKF 計算的姿態(tài)角誤差仍然保持著與實驗一和實驗二相當(dāng)?shù)乃剑阶藨B(tài)精度約0.1 °，航向約0.5 °，突顯了SE(3)-EKF 對準(zhǔn)方法的穩(wěn)健性。

表3 姿態(tài)角誤差的RMSE（單位：°）Tab.3 RMSE of attitude angle error (Unit: °)

進一步對比圖2-4 可發(fā)現(xiàn)，SE(3)-EKF 的航向角誤差都會迅速降至較小值，然后繼續(xù)收斂到更高、更穩(wěn)定的精度。在三個實驗場景中，SE(3)-EKF 的航向角誤差曲線如圖5 所示，可看出在約10 s 內(nèi)，航向角誤差已從初始誤差降至8 °以內(nèi)，然后持續(xù)向更小的誤差值收斂。盡管此時航向誤差仍然較大，但已經(jīng)能夠滿足一些對初始姿態(tài)要求不高的組合導(dǎo)航應(yīng)用。此外，還可以發(fā)現(xiàn)，SE(3)-EKF 方法的航向角收斂性能基本上與初始姿態(tài)誤差無關(guān)。

圖5 SE(3)-EKF 航向角誤差Fig.5 The yaw error of SE(3)-EKF

3 結(jié)論

本文提出了一種 GNSS 速度輔助的基于SE(3)-EKF 框架的旋翼無人機空中對準(zhǔn)方法。該方法重新定義了速度誤差形式，設(shè)計了GNSS 速度量測更新方法。在飛行實驗中模擬了三種不同大小的初始姿態(tài)角誤差，對比分析了EKF、UKF 和SE(3)-EKF 的性能。實驗結(jié)果顯示，在較小初始失準(zhǔn)角的場景下，EKF和UKF 方法能夠?qū)崿F(xiàn)姿態(tài)收斂；而基于SE(3)-EKF的對準(zhǔn)方法在不同失準(zhǔn)角的場景中，水平姿態(tài)誤差均可收斂到約0.1 °，方位誤差可收斂到約0.5 °，并且比EKF 和UKF 具有更快的收斂速度。由于SE(3)-EKF在濾波計算過程中參與計算的矩陣維度和EKF 相同，所以兩種方法的計算效率相當(dāng)。因此，基于SE(3)-EKF的對準(zhǔn)方法具備較好的工程應(yīng)用價值。