基于偏振光的導航定姿自適應濾波算法

金仁成，謝林達，蔚彥昭，魏 巍

(大連理工大學 機械工程學院，遼寧 大連 116024)

0 引言

航姿參考系統(attitude and heading reference system, AHRS)中的慣性測量單元(inertial measurement unit, IMU)由角速度陀螺儀和加速度計組成。通過對陀螺儀輸出角速度積分可以獲得小型無人機的姿態角，雖然高成本陀螺儀精度更高、可靠性更好，但是由于其成本高昂,不適合于小型無人機中使用。同時，由于高斯誤差、漂移誤差等干擾因素存在，低成本陀螺儀計算的姿態角隨時間不斷發散，解算精度較低[1]。為了解決上述問題，傳統方法是將慣導/磁力計/GPS構成組合導航系統[2]。雖然磁力計能在一定程度上修正陀螺儀量測信息，獲得精度較高的姿態角，但是其易受電磁場干擾、機載控制電路等環境因素影響，在實際飛行中的應用性能大幅降低。同時在高機動情況下由于衛星信號傳輸頻率與飛行器運動頻率的不一致性顯著增強，導致GPS丟星現象嚴重，影響航向信息的精確解算?？紤]到傳統導航方式的局限性，仿生導航逐漸成為研究熱點。

近年來，人們發現蝴蝶、蜜蜂等生物能利用天空中偏振光進行導航尋跡[3-5]，對其生理結構分析后，進一步發現了大氣偏振分布模式的存在。20世紀90年代，LAMBRINOS等[6]利用偏振光導航機理，設計出偏振光傳感器實驗樣機，并通過機器人實驗平臺驗證了將偏振光信息用于導航定向的可行性。大連理工大學褚金奎教授團隊[7-10]率先對偏振光導航進行了一系列科學研究，成功研制出偏振光傳感器并應用到移動機器人導航平臺中，同時在利用偏振光信息實現導航定位方面進行了創新性探索研究[11]。隨后，盧鴻謙等[12-13]利用卡爾曼濾波算法仿真驗證了基于偏振光傳感器輔助定姿的解算原理，然而其模型中對于偏振光傳感器的噪聲影響只考慮了理想測量噪聲情形，并未考慮測量干擾等因素的不良影響。

綜合上述分析，偏振光傳感器在地面導航環境中已得到廣泛應用，但在無人機上的應用仍處于探索階段。王玉杰等[14]基于標準大氣偏振模型推導了載體傾斜時的航向角計算方法，但是定向誤差與載體的水平角傾斜誤差密切相關，只能在小范圍內傾斜才能保證定向精度。支煒等[15]設計了偏振光與紅外傳感器的輔助慣導定姿方法，通過設計卡爾曼濾波器仿真分析其可行性，但是仿真條件比較理想化，而且未充分考慮姿態解算的實時性要求。

本文總結了現階段基于偏振光傳感器導航的優勢與不足，通過采用偏振光傳感器和慣性測量單元組合導航的方式克服單獨利用偏振光傳感器存在的空間位置約束問題，并設計了一種自適應互補濾波姿態估計算法，在降低計算量滿足姿態解算實時性要求基礎上，采取自適應機制以提高復雜環境下導航系統的姿態解算精度，最后進行了對比實驗，其結果表明該方法能獲得實時性更強、精度更高的姿態信息，滿足小型無人機的實際飛行需求。

1 偏振光傳感器測角原理

太陽射出的自然光在傳輸過程中，受到大氣微粒分子的散射而產生偏振現象，會在天空中形成穩定分布的偏振模式。本文利用偏振光傳感器檢測天空光的偏振分布模式，通過計算偏振光傳感器參考方向與太陽子午線之間的夾角，即偏振方位角δ，以實現偏振光導航定向功能。

(1)

式中：α為太陽赤緯；β為觀測點緯度；t為太陽時角，與當地經度有關。

圖1 偏振光傳感器測量模型Fig.1 Measurement model of polarization sensor

根據偏振光傳感器量測輸出，將偏振光E矢量投影到m系中，并表示為

pm(S)=[sinδcosδ0]T

(2)

根據瑞利散射模型，偏振光E矢量與觀測矢量和太陽矢量所確定的平面構成幾何上的垂直關系，由此得到的E矢量在m系中投影表示為

(3)

2 自適應互補濾波算法設計

通過對角速率陀螺儀的量測輸出積分可以獲得當前姿態角，然而陀螺儀測量值中含有零偏誤差、高斯誤差，以及隨機誤差等干擾因素，這樣陀螺儀單獨解算的姿態角隨時間呈現發散趨勢。從物理意義角度分析，加速度計輸出值是重力矢量在載體坐標系下的投影值，據此可計算橫滾與俯仰的水平姿態信息，雖然其計算角度不存在誤差累積現象，但是電機振動等高頻噪聲會對其性能造成不利影響。同時，本文采用的偏振光傳感器也具有低頻特性良好、易受高頻干擾的特點。因此，利用它們各自在頻域上的優勢，設計了互補濾波算法融合慣導與偏振光傳感器的量測值以解算姿態角信息。然而在實際應用中，運動加速度的干擾，以及天空中云霧等水汽因素都會嚴重影響姿態解算精度，因此本文進一步采取自適應互補濾波機制，以提高偏振光導航系統的魯棒性和穩定性，并分層次建立偏振光自適應互補濾波模型。

2.1 加速度計/陀螺儀互補濾波器

(4)

同時，將導航坐標系下的重力矢量投影到載體坐標系并表示為

(5)

將gb(f)與gb(R)的叉積gb(×)定義為加速度計糾偏矢量并修正陀螺儀輸出值。不同于傳統的PID控制，本文的互補濾波算法采用比例-積分(PI)補償策略，其中比例參數體現了互補頻率的影響，主要衡量加速度計和陀螺儀的置信度，積分參數體現了對陀螺儀誤差的補償性能，且兩種參數的選擇與濾波器截止頻率密切相關。工程經驗表明提高截止頻率可使系統靜態輸出更穩定，但動態輸出滯后；降低截止頻率能提高系統動態特性，但會增加漂移誤差。因此PI參數的選擇需綜合考慮上述各因素并通過實驗驗證其有效性。本文利用加速度計對陀螺儀的補償過程表示為

ωg/a=-kap(gb(×))-kai(gb(×))

(6)

式中：ωg/a是角速度矢量加速度計補償項；kap是加速度計比例參數；kai是加速度計積分參數。然而由式(5)可知，利用加速度計量測信息只能補償陀螺儀解算的橫滾角和俯仰角信息，航向角仍處于發散狀態，因此需引入其他傳感器輔助修正航向誤差。

2.2 偏振光傳感器/陀螺儀互補濾波器

在標準大氣偏振模型中，偏振光傳感器測量的偏振方位角無誤差累積效應，可以為陀螺儀航向信息的補償提供一種行之有效的解決方案。同時考慮到傳感器電路干擾因素以及機體振動等高頻噪聲影響，本文采用低通濾波的方式實現對偏振光傳感器輸出信息的降噪處理。

通過查詢天文年歷獲得觀測點的位置和時間信息，并由式(3)計算得到偏振光E矢量理論參考值在模塊坐標系下的投影pm(R)。再利用式(2)求解偏振光E矢量實際探測值在模塊坐標系下投影值pm(S)。

為有效糾正陀螺儀累積誤差，定義pm(R)與pm(S)的叉積pm(×)為偏振糾偏矢量，并作為航向角補償項。這里采用和上文一致的比例-積分補償策略，其對陀螺儀的補償過程表示為

ωg/p=-kpp(pm(×))-kpi(pm(×))

(7)

式中：ωg/p是角速度矢量偏振光補償項；kpp是偏振光比例參數；kpi是加速度計積分參數。

2.3 自適應調節機制

事實上，在無人機實際飛行過程中會存在復雜運動狀態，其中加減速運動是常見情形，此時如果依然采用常規互補濾波算法，由于加速度計無法區分重力加速度和運動加速度，會降低算法的補償精度，尤其是機體運動加速度較大時，姿態解算的準確性會受到嚴重影響。因此在出現這類情況時，應使互補濾波器的PI控制參數動態可變?；诖四康?，進一步設計了相應的自適應調整方案。

從實時性角度考慮，本文方案設定積分參數kai保持不變，通過動態調整比例參數kap以適應復雜的加減速環境，從而保證機動狀態下全姿態信息的解算精度。本文算法中的自適應濾波系數選取規則為

(8)

太陽光經大氣中粒子散射后在整個天宇范圍內形成穩定的大氣偏振模式，相比傳統導航方式，偏振光導航具有自主性好、抗電磁干擾能力強以及誤差不隨時間累積等諸多優勢。然而，偏振光傳感器的探測性能與天氣狀況密切相關，本文針對觀測方向存在異常水汽干擾情形采取相應措施。利用pm(×)的歐式范數與設定閾值threshold進行比較分析，作為判斷偏振光傳感器是否受到異常水汽干擾的關鍵因素，通過動態調整比例參數kpp，提高偏振光導航系統對異常天氣情況的適應程度。其表達式為

(9)

根據上述分析，本文設計的自適應互補濾波姿態估計算法流程如圖2所示。

圖2 自適應互補濾波算法流程圖Fig.2 Flow chart of adaptive complementary filter algorithm

3 實驗結果及分析

3.1 實驗平臺

搭建了偏振光導航實驗平臺，采用基于ARM Cortex-M3內核的飛控板，其主頻為72 MHz，并集成慣性測量單元MPU-6050、3軸磁力計HMC5883和氣壓計MS5611等設備。將飛控板輸出的姿態信息作為參考值以驗證基于偏振光的自適應濾波算法。通過靜態實驗、機動實驗以及存在異常水汽干擾實驗全面驗證算法有效性，實驗結果表明本文設計的自適應濾波算法具有良好的抗干擾能力，有效提高了偏振光導航系統的穩定性。

3.2 靜態實驗

實驗地點選取大連理工大學機械工程學院室外，地理位置信息為東經121.527°、北緯38.879°，實驗時間為2018年5月8日，天氣晴朗無云，從當天17：00開始將偏振光導航實驗平臺靜止放置約375 s。陀螺儀量測姿態角如圖3所示。由圖3可知，單獨使用陀螺儀解算的姿態角隨時間偏離參考值，呈現出明顯的發散現象，無法滿足姿態解算的精度要求。然而，通過采用本文設計的互補融合策略修正效果顯著。靜態姿態角對比如圖4所示。從圖4中可以清晰看到，本文算法不僅修正了陀螺儀的測量偏差，而且其融合濾波效果較飛控板參考輸出值的靜態穩定性更優，表明本文算法對機載電路噪聲的抗干擾能力更強。由于靜態實驗中運動加速度始終保持零值，因此基于偏振光的自適應濾波值與基于偏振光的常規互補濾波算法解算結果相當。

圖3 陀螺儀量測姿態角Fig.3 Attitude angles measured by gyroscope

圖4 靜態姿態角對比圖Fig.4 Static attitude angles comparison diagram

圖5 加速度計輸出值Fig.5 Acceleration measured by accelerometer

3.3 機動實驗

無人機起飛50 s左右逐漸增大加速度值，穩定一段時間后再逐漸減速。其中，加速度計各軸輸出值的變化趨勢如圖5所示。機動姿態角對比如圖6所示。結果表明，在存在較大運動加速度情況下，如果仍采用常規互補濾波方案，會造成很大的姿態角誤差。此時，雖然橫滾角仍可以滿足精度要求，但是俯仰角最大偏差達到16.25°，而且航向角最大偏差也接近1.58°。當采用本文提出的自適應濾波算法后，姿態角偏差明顯降低，誤差均在±0.3°范圍內，具有較高的解算精度，提高了偏振光導航系統的魯棒性能。

圖6 機動姿態角對比Fig.6 Dynamic attitude angles comparison diagram

3.4 天空水汽異常實驗

偏振光傳感器視場范圍有限，對視線方向水汽分布敏感。如果水汽異常情況顯著，則偏振光傳感器性能會大幅降低，因此在利用偏振光信息導航時應及時監測異常并采取相應措施。

事實上，當天空水汽分布異常時，采用常規互補濾波算法會融入相應測量誤差，嚴重影響航向角解算精度。本文選取晴間多云天氣以驗證偏振光自適應濾波算法有效性。異常水汽影響下航向角對比如圖7所示。由圖可見，在175 s到200 s期間，偏振光傳感器觀測方向存在少量云霧。實驗結果表明，受觀測點水汽異常影響，偏振光傳感器存在短時間內測量偏差較大情況，此時基于偏振光修正航向信息的能力大幅降低，圖中常規互補濾波算法沒有異常處理機制，導致航向角計算偏差為30°～40°，精度嚴重降低。為了改善異常狀態下航向測量準確性，本文利用pm(×)的歐式范數作為判斷異常水汽存在的標準，通過動態調整相應參數來提高偏振光導航系統自適應能力。從實驗結果可以看出，采用本文自適應濾波算法后，能夠很好克服少量云霧干擾情形，進一步提高了偏振光導航系統魯棒性。

圖7 異常水汽影響下航向角對比圖Fig.7 Yaw attitude angle measured in the state of abnormal water vapor comparison diagram

4 結論

為提高導航系統姿態解算精度，設計了基于偏振光傳感器的自適應互補濾波算法，完成了靜態、機動以及天空存在異常水汽干擾等實驗。由于存在高斯誤差、漂移誤差等干擾因素，單獨利用低成本陀螺儀計算的姿態角會隨時間逐漸發散。在采用本文自適應互補濾波策略后，通過融合偏振光傳感器和加速度計的量測數據，能有效抑制陀螺儀發散問題，保證了姿態解算的收斂性，并提高了偏振導航系統的姿態解算精度，為無人機導航領域提供了一種全新方案，具有廣闊的應用前景。

考慮到基于偏振光的導航精度與傳感器的數據采集精度密切相關，后續研究中將優化信號濾波方法，減少噪聲信號對采集到偏振光信息的干擾，進一步提高偏振光導航系統的定姿精度。