基于天空光偏振模式的導航姿態最優化解算方法

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

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

關鍵詞：偏振模式;仿生導航;優化;動態步長;姿態解算

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

0?引言

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

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

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

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

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

1?偏振光組合導航系統定姿過程

1.1?基于四元數的姿態更新

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

本文定義姿態四元數為Q，且有

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

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

再根據姿態矩陣與四元數的對應關系，全姿態信息的解算結果為

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

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

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

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

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

2?基于仿生偏振光的姿態最優化求解方法

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

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

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

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

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

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

2.2?基于加速度計量測數據的誤差模型

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

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

同時利用旋轉四元數矩陣將Ang轉換到載體坐標系下，則有

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

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

2.3?最優化模型中的動態步長機制

根據偏振誤差矢量和重力誤差矢量構建偏振光導航系統的姿態誤差矢量：

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

此時，對姿態四元數的優化轉換為求目標誤差函數F（Q）的最小值。為求解誤差最小值的姿態四元數，將姿態誤差矢量的雅可比矩陣定義為

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

根據最優化精確搜索理論，可推導出梯度優化模型的動態步長因子為

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

3?實驗測試及結果分析

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

3.1?靜態實驗測試及分析

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

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

圖3中，本文設計的動態步長梯度優化方法估計的姿態角不僅消除了陀螺儀積分漂移問題，而且與飛控板參考算法相比，獲得的姿態信息具有更好的穩定性以及更高的靜態精度。同時考慮到動態步長梯度優化方法的優勢主要體現在運動環境中，因此在靜態實驗中，與固定步長梯度優化方法估計的姿態角具有相當的解算精度。

3.2?動態實驗測試及分析

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

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

4?結論

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

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

參考文獻：

[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] 晏磊，關桂霞，陳家斌，等. 基于天空偏振光分布模式的仿生導航定向機理初探[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]. 光學技術，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] 黃平. 最優化理論與方法[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.