基于天空光偏振模式的導航姿態(tài)最優(yōu)化解算方法

金仁成 謝林達 褚金奎 蔚彥昭 魏巍

摘?要：針對傳統(tǒng)導航系統(tǒng)自主性差、抗電磁干擾能力弱的問題，本文提出一種基于天空光偏振模式的仿生導航方案。利用自主研發(fā)的偏振光傳感器實現(xiàn)導航定向功能，并結(jié)合慣性測量單元設(shè)計了一種導航姿態(tài)最優(yōu)化解算方法?，F(xiàn)有的姿態(tài)最優(yōu)化求解策略一般是采用步長固定的搜索算法，但是其直接影響了運動狀態(tài)下姿態(tài)估計的準確性，為此本文采取動態(tài)步長搜索機制。實驗結(jié)果表明：本文提出的算法能有效抑制陀螺儀漂移誤差，而且對高頻噪聲干擾有明顯濾除效果，表現(xiàn)出良好的靜態(tài)性能;同時，本文提出的方法較常規(guī)算法具有更高的動態(tài)精度，進一步提高了偏振光導航系統(tǒng)的穩(wěn)定性與可靠性，為無人機飛行控制提供更準確的參數(shù)信息。

關(guān)鍵詞：偏振模式;仿生導航;優(yōu)化;動態(tài)步長;姿態(tài)解算

中圖分類號：TJ765;V249?文獻標識碼：A文章編號：1673-5048（2020）01-0039-07

0?引言

在晴朗無云的天氣條件下，太陽發(fā)出的自然光被大氣層中的粒子散射后，整個天宇范圍內(nèi)會形成相對穩(wěn)定的大氣偏振模式[1-2]。自然界中的許多生物能夠通過自身的偏振視覺系統(tǒng)感知天空中的偏振模式實現(xiàn)導航。比如沙蟻離開巢穴覓食后，能夠利用偏振光信息找到歸巢的最短路線[3-4]。一些候鳥可以利用天空偏振光來矯正自身的生物磁羅盤完成遷徙活動[5-6]。偏振光導航作為一種新型的仿生導航技術(shù)，不僅比衛(wèi)星導航有著更強的抗電磁干擾能力，而且與慣性導航相比，具有誤差不隨時間積累的優(yōu)勢。

目前，國內(nèi)外學者在偏振光導航方面做了大量研究工作。20世紀90年代，國外學者Lambrinos 等利用偏振光導航機理，設(shè)計出仿生偏振光導航傳感器并應(yīng)用于機器人中，通過實驗證明了將偏振光信息用于導航定向的可行性[7]。2012年澳大利亞Chahl 等研究人員仿照蜻蜓利用偏振光導航的過程，通過簡化飛行模型將偏振光導航傳感器應(yīng)用于無人機中完成初步航向測量[8]。大連理工大學褚金奎教授團隊率先在國內(nèi)對偏振光導航機理展開研究，通過分析昆蟲復眼偏振敏感單元設(shè)計了偏振光導航傳感器，并在地面機器人實驗中成功應(yīng)用[9]。同時，北京大學、哈爾濱工業(yè)大學、國防科技大學等國內(nèi)高校對偏振光的散射特性以及傳輸特性開展了大量研究[10-12]。

現(xiàn)階段，偏振光傳感器在地面導航中已得到成功應(yīng)用，但是在無人機上的應(yīng)用仍處于起步階段。事實上，目前的偏振光導航策略主要有兩種：

一是利用偏振光傳感器實現(xiàn)完全的自主定姿過程[13];二是基于卡爾曼濾波的偏振光輔助定姿方法[14]。然而這兩種方法均存在一定局限性：方法一在航向信息解算過程中存在偏振方位角模糊性問題，而且導航姿態(tài)解算精度與飛行器水平姿態(tài)的誤差精度密切相關(guān)，水平傾角只有在小角度范圍內(nèi)變化時才能達到較高解算精度[13]，因此極大限制了飛行器的空間姿態(tài)解算。方法二利用卡爾曼濾波模型實現(xiàn)偏振光傳感器與慣性測量單元輸出值的數(shù)據(jù)融合。然而在每次迭代過程中其計算量相對較大，實時性難以保證，同時偏振光傳感器的模型誤差受多種因素影響[14]，無法確?？柭鼮V波器中誤差項的準確性，進一步制約了導航系統(tǒng)性能的改善。

本文基于天空光偏振模式提出了一種姿態(tài)最優(yōu)化解算方法，并且融合偏振光傳感器和慣性測量單元的量測數(shù)據(jù)實現(xiàn)了動態(tài)步長搜索功能，最后通過實驗驗證了所提出的方法能夠有效提高偏振光導航系統(tǒng)的穩(wěn)定性和精度，為無人機完成實際飛行任務(wù)提供了可靠保證。

1?偏振光組合導航系統(tǒng)定姿過程

1.1?基于四元數(shù)的姿態(tài)更新

常見的姿態(tài)解算方法主要有歐拉角法、方向余弦法和四元數(shù)法等。歐拉角法利用坐標軸的有序轉(zhuǎn)動實現(xiàn)姿態(tài)變換，雖然計算量小但方程可能出現(xiàn)奇異值，不能全姿態(tài)解算;方向余弦法和四元數(shù)法在解算過程中不會存在奇異解，但是方向余弦法需要計算旋轉(zhuǎn)矩陣的9個參數(shù)，計算量通常較大;相比之下，四元數(shù)法的更新方程只需求解4維數(shù)據(jù)，計算量大幅下降[15]，因此本文采用四元數(shù)作為姿態(tài)解算的數(shù)學依據(jù)。

本文定義姿態(tài)四元數(shù)為Q，且有

本文設(shè)計的無人機導航系統(tǒng)主要涉及載體坐標系b系和導航坐標系n系，其坐標系間的位置關(guān)系如圖1所示。圖中，基于地球坐標系OeXeYeZe確定b系和n系的空間位置關(guān)系。其中，b系原點與機體質(zhì)心重合、Xb軸沿載體縱軸向前、Yb軸沿載體橫軸向右，Zb軸與Xb和Yb軸構(gòu)成右手笛卡爾坐標系。n系各軸的選取規(guī)則類似，其中Xn軸、Yn軸和Zn軸分別指向地理的北向、東向和地心方向。

無人機在飛行過程中姿態(tài)不斷變化，載體坐標系相對導航坐標系的旋轉(zhuǎn)過程可由四元數(shù)表示。同時，將導航坐標系到載體坐標系的姿態(tài)矩陣定義為Cbn，其表達式為

再根據(jù)姿態(tài)矩陣與四元數(shù)的對應(yīng)關(guān)系，全姿態(tài)信息的解算結(jié)果為

1.2?偏振光傳感器導航機理

太陽發(fā)出的自然光經(jīng)過空氣分子的散射作用會形成穩(wěn)定的大氣偏振分布模式[16]。根據(jù)瑞利散射模型原理，偏振光E矢量方向總是垂直于太陽入射光與觀察者所確定的平面，因此可以利用這種方位信息進行導航定向，同時利用天空光偏振分布模式，建立了基于偏振光傳感器的導航定向模型，如圖2所示。

為說明偏振光傳感器的空間位置，這里引入偏振光測量坐標系m系。圖2中，O點為偏振光傳感器所在位置，Xm軸為偏振光傳感器自身參考軸方向，Zm軸為偏振光傳感器觀測方向，Ym軸與二者構(gòu)成右手笛卡爾坐標系。O點到太陽投影點S的連線表示太陽矢量，Hs與As分別為太陽高度角和太陽方位角，二者與時間和位置信息相關(guān)，計算公式為

偏振光傳感器輸出值為偏振方位角α，是傳感器參考軸方向與偏振光E矢量方向的夾角。同時，根據(jù)偏振光傳感器量測輸出，可將偏振光E矢量在m系中的投影坐標表示為

根據(jù)瑞利散射模型，偏振光E矢量與觀測矢量和太陽矢量形成幾何上的垂直關(guān)系，由此計算偏振光E矢量在m系中的映射形式為

2?基于仿生偏振光的姿態(tài)最優(yōu)化求解方法

梯度優(yōu)化模型利用負梯度方向搜索尋優(yōu)，并保證每次迭代更新后目標函數(shù)逐步減小，通常用于求解函數(shù)最優(yōu)解[17-18]。本文通過偏振光傳感器和加速度計量測數(shù)據(jù)，構(gòu)建基于四元數(shù)的目標誤差函數(shù)模型，并求解誤差函數(shù)最小值，以獲得偏振光導航系統(tǒng)的最優(yōu)姿態(tài)四元數(shù)，從而求解姿態(tài)最優(yōu)估計值。

2.1?基于偏振光傳感器探測值的誤差模型

本文利用偏振光E矢量在不同坐標系中投影值的確定關(guān)系，構(gòu)建偏振誤差矢量模型求解偏振梯度搜索方向，以矯正偏振方位角的量測偏差。

采集偏振光傳感器輸出數(shù)據(jù)，則偏振光E矢量在m系中的表達式為

查詢天文年歷和經(jīng)緯度信息，按式（5）計算太陽高度角Hs和太陽方位角As，以確定太陽矢量的投影位置Sn，再按式（7）計算偏振矢量在m系中的映射值Ems。此時利用m系中偏振光傳感器的測量值Emc與瑞利散射模型計算出的偏振矢量Ems構(gòu)造偏振誤差矢量：

為建立偏振梯度搜索模型，對偏振誤差矢量求導，獲得相應(yīng)的雅可比矩陣，即

2.2?基于加速度計量測數(shù)據(jù)的誤差模型

加速度計具有良好的靜態(tài)特性，長時間使用不會引入積分誤差。本文利用重力矢量在載體坐標系與導航坐標系之間的旋轉(zhuǎn)變換關(guān)系建立重力誤差矢量模型，通過重力梯度搜索方向矯正加速度計的輸出偏差。

假設(shè)重力矢量垂直向下，則導航坐標系中歸一化后表示為

同時利用旋轉(zhuǎn)四元數(shù)矩陣將Ang轉(zhuǎn)換到載體坐標系下，則有

加速度計在載體坐標系下實際測得的重力矢量表示為

為建立重力梯度搜索模型，對重力誤差矢量Emp求導，獲得相應(yīng)的雅可比矩陣：

2.3?最優(yōu)化模型中的動態(tài)步長機制

根據(jù)偏振誤差矢量和重力誤差矢量構(gòu)建偏振光導航系統(tǒng)的姿態(tài)誤差矢量：

對e（Q）取歐式范數(shù)的平方，從而得到梯度尋優(yōu)目標誤差函數(shù)：

此時，對姿態(tài)四元數(shù)的優(yōu)化轉(zhuǎn)換為求目標誤差函數(shù)F（Q）的最小值。為求解誤差最小值的姿態(tài)四元數(shù)，將姿態(tài)誤差矢量的雅可比矩陣定義為

將目標誤差函數(shù)的梯度及其一階偏導數(shù)定義為

根據(jù)最優(yōu)化精確搜索理論，可推導出梯度優(yōu)化模型的動態(tài)步長因子為

傳統(tǒng)的梯度模型需要多次迭代才能獲得最優(yōu)解，但是在姿態(tài)解算中，每個解算周期只需進行一次迭代即可獲得最優(yōu)姿態(tài)四元數(shù)。本文通過旋轉(zhuǎn)四元數(shù)微分方程建立偏振光傳感器、加速度計與陀螺儀之間的數(shù)據(jù)融合關(guān)系。利用動態(tài)步長λ計算姿態(tài)四元數(shù)最優(yōu)解：

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

本文搭建了偏振導航實驗平臺，所使用的飛控板上集成了慣性測量單元MPU6050和電子羅盤HMC5883L等設(shè)備。飛控板內(nèi)部采用精度較高的卡爾曼濾波算法求解姿態(tài)角，可作為姿態(tài)輸出參考值。為了評測算法的性能，分別通過靜態(tài)實驗和動態(tài)實驗與步長固定的梯度搜索方法對比分析，以驗證本文設(shè)計的動態(tài)步長梯度優(yōu)化方法的可行性和有效性。

3.1?靜態(tài)實驗測試及分析

靜態(tài)實驗于2018年5月25日17點20分開展，實驗場地選在大連理工大學機械工程學院室外。通過查詢天文年歷表，可以獲得太陽位置相關(guān)信息，其中，太陽高度角為20.54°，太陽方位角為-100.89°。

為了保證實驗結(jié)果的可靠性，將偏振光導航實驗平臺靜止放置約400 s，經(jīng)過不同方法解算的姿態(tài)角如圖3所示。

圖3中，本文設(shè)計的動態(tài)步長梯度優(yōu)化方法估計的姿態(tài)角不僅消除了陀螺儀積分漂移問題，而且與飛控板參考算法相比，獲得的姿態(tài)信息具有更好的穩(wěn)定性以及更高的靜態(tài)精度。同時考慮到動態(tài)步長梯度優(yōu)化方法的優(yōu)勢主要體現(xiàn)在運動環(huán)境中，因此在靜態(tài)實驗中，與固定步長梯度優(yōu)化方法估計的姿態(tài)角具有相當?shù)慕馑憔取?/p>

3.2?動態(tài)實驗測試及分析

動態(tài)實驗通過變化飛控板姿態(tài)，以驗證優(yōu)化方法的運動適應(yīng)性。選取的實驗地點不變，實驗時間為2018年5月25日18點，此時查詢天文年歷表，可以獲得太陽高度角為12.98°，太陽方位角為-106.62°。經(jīng)過不同方法解算的姿態(tài)角如圖4所示。同時，兩種梯度優(yōu)化方法的姿態(tài)估計偏差曲線如圖5所示。

根據(jù)圖4結(jié)果可知，本文設(shè)計的動態(tài)步長梯度優(yōu)化方法能夠很好貼合姿態(tài)變化過程，通過融合偏振光傳感器數(shù)據(jù)實現(xiàn)導航姿態(tài)的動態(tài)解算。同時從圖5的姿態(tài)估計偏差曲線可以清晰看到，在姿態(tài)變化的全過程，動態(tài)步長方法的姿態(tài)偏差值要明顯小于固定步長方法，從而表明本文提出的動態(tài)步長機制能夠保證運動環(huán)境下姿態(tài)解算的準確性，有效提高了偏振光導航姿態(tài)估計的動態(tài)精度。

4?結(jié)論

本文從偏振光導航機理入手，通過引入仿生導航傳感器設(shè)計偏振光組合導航系統(tǒng)，以彌補現(xiàn)有導航方式不足，從而提供一種自主性好、抗電磁干擾能力強、無誤差累積效應(yīng)的新型導航方案。同時，為提高偏振光導航系統(tǒng)的姿態(tài)估計精度，設(shè)計了基于天空光偏振模式的導航姿態(tài)最優(yōu)化解算方法，并分別通過靜態(tài)實驗與動態(tài)實驗測試分析。

從實驗結(jié)果可以看到，采用動態(tài)步長機制的梯度優(yōu)化方法不僅具有良好的靜態(tài)穩(wěn)定性，而且在運動環(huán)境中的姿態(tài)估計精度要明顯優(yōu)于固定步長梯度優(yōu)化法，表明該方法能有效提高偏振光導航系統(tǒng)姿態(tài)解算的動態(tài)精度，可以進一步增強導航平臺的可靠性與穩(wěn)定性。但偏振光傳感器的精度與天氣條件關(guān)系緊密，下一步工作將考慮大氣湍流、霧霾、煙塵等不同天氣因素對偏振光導航姿態(tài)估計精度的影響，從而進一步優(yōu)化現(xiàn)有姿態(tài)解算模型，以提高偏振光導航系統(tǒng)的魯棒性能。

參考文獻：

[1] El Jundi B，Smolka J，Baird E，et al. Diurnal Dung Beetles Use the Intensity Gradient and the Polarization Pattern of the Sky for Orientation[J]. Journal of Experimental Bio-logy，2014，217（13）：2422-2429.

[2] El Jundi B，Pfeiffer K，Heinze S，et al. Integration of Polarization and Chromatic Cues in the Insect Sky Compass[J]. Journal of Comparative Physiology A，2014，200（6）：575-589.

[3] Lebhardt F，Ronacher B. Transfer of Directional Information between the Polarization Compass and the Sun Compass in Desert Ants[J]. Journal of Comparative Physiology A，2015，201（6）：599-608.

[4] Lebhardt F，Koch J，Ronacher B. The Polarization Compass Dominates over Idiothetic Cues in Path Integration of Desert Ants[J]. Journal of Experimental Biology，2012，215（3）：526-535.

[5] Melnikov V，Leskinen M，Koistinen J. Doppler Velocities at Orthogonal Polarizations in Radar Echoes from Insects and Birds[J]. IEEE Geoscience & Remote Sensing Letters，2014，11（3）：592-596.

[6] Muheim R. Behavioural and Physiological Mechanisms of Polarized Light Sensitivity in Birds[J]. Philosophical Transactions of the Royal Society of London，2011，366（1565）：763-771.

[7] Lambrinos D，Mller R，Labhart T，et al. A Mobile Robot Employing Insect Strategies for Navigation[J]. Robotics and Autonomous Systems，2000，30（1）：39-64.

[8] Chahl J，Mizutani A . Biomimetic Attitude and Orientation Sensors[J]. IEEE Sensors Journal，2012，12（2）：289-297.

[9] Chu Jinkui，Wang Hongqing，Chen Wenjing，et al. Application of a Novel Polarization Sensor to Mobile Robot Navigation[C]∥International Conference on Mechatronics and Automation，Changchun，2009：3750-3755.

[10] 晏磊，關(guān)桂霞，陳家斌，等. 基于天空偏振光分布模式的仿生導航定向機理初探[J]. 北京大學學報：自然科學版，2009，45（4）：616-620.

Yan Lei，Guan Guixia，Chen Jiabin，et al. The Bionic Orientation Mechanism in the Skylight Polarization Pattern[J]. Acta Scientiarum Naturalium Universitatis Pekinensis，2009，45（4）：616-620. （in Chinese）

[11] 黃顯林，姜肖楠，盧鴻謙，等. 自主視覺導航方法綜述[J]. 吉林大學學報：信息科學版，2010，28（2）：158-165.

Huang Xianlin，Jiang Xiaonan，Lu Hongqian，et al.Survey of Vision for Autonomous Navigation[J]. Journal of Jilin University：Information Science Edition，2010，28（2）：158-165.（in Chinese）

[12] 范晨，胡小平，何曉峰，等. 天空偏振模式對仿生偏振光定向的影響及實驗[J]. 光學精密工程，2015，23（9）：2429-2437.

Fan Chen，Hu Xiaoping，He Xiaofeng，et al.Influence of Skylight Polarization Pattern on Bionic Polarized Orientation and Corresponding Experiments[J]. Optics and Precision Engineering，2015，23（9）：2429-2437. （in Chinese）

[13] 王玉杰，胡小平，練軍想，等. 仿生偏振光定向算法及誤差分析[J].宇航學報，2015，36（2）：211-216.

Wang Yujie，Hu Xiaoping，Lian Junxiang，et al.Algorithms and Error Analysis of Bionic Orientation Based on Polarized Light[J]. Journal of Astronautics，2015，36（2）：211-216. （in Chinese）

[14] 盧鴻謙，黃顯林，尹航. 三維空間中的偏振光導航方法[J]. 光學技術(shù)，2007，33（3）：412-415.

Lu Hongqian，Huang Xianlin，Yin Hang. Polarized-Light-Based Navigation in Three-Dimensional Space[J]. Optical Technique，2007，33（3）：412-415. （in Chinese）

[15] Fresk E，Nikolakopoulos G . Full Quaternion Based Attitude Control for a Quadrotor[C]∥ 2013 European Control Conference，Zurich，2013：3864-3869.

[16] Gál J，Horváth G，Meyer-Rochow B V，et al. Polarization Patterns of the Summer Sky and Its Neutral Points Measured by Full-Sky Imaging Polarimetry in Finnish Lapland North of the Arctic Circle[J]. Proceedings of the Royal Society A：Mathematical，Physical and Engineering Sciences，2001，457（2010）：1385-1399.

[17] 黃平. 最優(yōu)化理論與方法[M]. 北京：清華大學出版社，2009：82-84.

Huang Ping. Optimal Theories and Methods[M]. Beijing：Tsinghua University Press，2009：82-84. （in Chinese）

[18] Basak R，Das A，Sanyal A. Cost Optimal Design of a Power Inductor by Sequential Gradient Search[J]. Journal of the Institution of Engineers （India）：Series B，2018，99（4）：413-418.