基于偏振光的導航定姿自適應(yīng)濾波算法

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

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

0 引言

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

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

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

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

1 偏振光傳感器測角原理

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

(1)

式中：α為太陽赤緯；β為觀測點緯度；t為太陽時角，與當?shù)亟?jīng)度有關(guān)。

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

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

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

(2)

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

(3)

2 自適應(yīng)互補濾波算法設(shè)計

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

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

(4)

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

(5)

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

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

(6)

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

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

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

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

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

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

(7)

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

2.3 自適應(yīng)調(diào)節(jié)機制

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

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

(8)

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

(9)

根據(jù)上述分析，本文設(shè)計的自適應(yīng)互補濾波姿態(tài)估計算法流程如圖2所示。

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

3 實驗結(jié)果及分析

3.1 實驗平臺

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

3.2 靜態(tài)實驗

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

圖3 陀螺儀量測姿態(tài)角Fig.3 Attitude angles measured by gyroscope

圖4 靜態(tài)姿態(tài)角對比圖Fig.4 Static attitude angles comparison diagram

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

3.3 機動實驗

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

圖6 機動姿態(tài)角對比Fig.6 Dynamic attitude angles comparison diagram

3.4 天空水汽異常實驗

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

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

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

4 結(jié)論

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

考慮到基于偏振光的導航精度與傳感器的數(shù)據(jù)采集精度密切相關(guān)，后續(xù)研究中將優(yōu)化信號濾波方法，減少噪聲信號對采集到偏振光信息的干擾，進一步提高偏振光導航系統(tǒng)的定姿精度。