金仁成，謝林達(dá)，蔚彥昭，魏 巍

(大連理工大學(xué) 機(jī)械工程學(xué)院，遼寧 大連 116024)

0 引言

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

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

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

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

1 偏振光傳感器測(cè)角原理

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

(1)

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

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

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

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

(2)

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

(3)

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

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

2.1 加速度計(jì)/陀螺儀互補(bǔ)濾波器

(4)

同時(shí)，將導(dǎo)航坐標(biāo)系下的重力矢量投影到載體坐標(biāo)系并表示為

(5)

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

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

(6)

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

2.2 偏振光傳感器/陀螺儀互補(bǔ)濾波器

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

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

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

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

(7)

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

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

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

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

(8)

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

(9)

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

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

3 實(shí)驗(yàn)結(jié)果及分析

3.1 實(shí)驗(yàn)平臺(tái)

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

3.2 靜態(tài)實(shí)驗(yàn)

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

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

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

圖5 加速度計(jì)輸出值Fig.5 Acceleration measured by accelerometer

3.3 機(jī)動(dòng)實(shí)驗(yàn)

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

圖6 機(jī)動(dòng)姿態(tài)角對(duì)比Fig.6 Dynamic attitude angles comparison diagram

3.4 天空水汽異常實(shí)驗(yàn)

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

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

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

4 結(jié)論

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

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