[發明專利]一種用于仿生導航的姿態最優化解算方法

有效

• 申請號: 201910014211.3
• 申請日: 2019-01-08
• 公開（公告）號: CN109506660B
• 公開（公告）日: 2022-03-29
• 發明（設計）人: 金仁成;謝林達;蔚彥昭;魏巍;楮金奎
• 申請（專利權）人: 大連理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 大連理工大學專利中心 21200
• 代理人: 梅洪玉;劉秋彤

1.一種用于仿生導航的姿態最優化解算方法，其特征在于，具體步驟如下：

步驟一：采集偏振光傳感器和加速度計的輸出數據，偏振光傳感器在模塊坐標系下的量測值為加速度計在載體坐標系下的量測值為將二者作為目標誤差函數實測項；其中，P_c^m的上標m表示投影到模塊坐標系，下標c表示該變量是偏振光傳感器的實測值；的上標b表示投影到載體坐標系，下標c表示該變量是加速度計的實測值；下標x,y,z表示矢量在三個坐標軸上各自分量；

步驟二：通過查詢天文年歷和經緯度信息，根據瑞利散射模型計算偏振矢量參考輸出值再根據坐標系旋轉變換關系將重力矢量從導航坐標系投影到載體坐標系，以得到重力矢量參考輸出值將二者作為目標誤差函數理論項；

步驟三：利用P_c^m和P_s^m組成偏振誤差矢量由和構成重力誤差矢量從而得到姿態誤差矢量為使姿態誤差最小需建立目標誤差函數模型F(q)；

其中，

其中，q＝[q₀ q₁ q₂ q₃]^T，為導航坐標系對載體坐標系的旋轉四元數；

步驟四：為求解誤差最小值的姿態四元數，采用梯度優化理論中一維精確搜索方法，根據目標誤差函數的梯度與其導數H(q)計算搜索步長因子λ；

其中，

步驟五：在姿態解算中，每個解算周期只需進行一次迭代獲得最優姿態四元數，從而通過旋轉四元數微分方程建立偏振光傳感器、加速度計與陀螺儀之間的數據融合關系；在已知第k次最優姿態估計值Q(k)情況下，由式(7)計算當前最優四元數變化率再結合步驟四求解的搜索步長因子λ迭代更新得到下一時刻姿態四元數最優解Q(k+1)；

其中，ω_x(k),ω_y(k),ω_z(k)表示陀螺儀在載體坐標系下x,y,z軸各自的角速度分量；Δt表示姿態解算周期；

步驟六：利用步驟五得到的姿態四元數最優解，解算出當前時刻飛行器橫滾角γ、俯仰角θ以及航向角ψ的最優估計值；