[发明专利]一种用于仿生偏振光导航的姿态无约束优化求解方法

权利要求书

1.一种用于仿生偏振光导航的姿态无约束优化求解方法，其特征在于，具体步骤如下：

步骤一：采集陀螺仪在载体坐标系下的测量值、加速度计在载体坐标系下的测量值、偏振光传感器在测量坐标系下的测量值，陀螺仪测量值设为ω_b＝[ω_xω_yω_z]^T，加速度计测量值设为a_b＝[a_x a_y a_z]^T，偏振光传感器测量值设为p_m＝[p_x p_y p_z]^T；其中，下标b表示载体坐标系；下标m表示偏振光传感器测量坐标系；下标x,y,z表示测量值在三轴的投影分量；

步骤二：假定导航坐标系三轴x-y-z对应北东地三个方向，载体坐标系三轴x-y-z对应载体前右下三个方向，太阳坐标系三轴x-y-z对应西南天三个方向，偏振光传感器测量坐标系三轴x-y-z对应载体前左上三个方向；将重力矢量在导航坐标系下的投影[00g]^T由变换矩阵变换到载体坐标系，计算出理论重力矢量，设为将太阳位置矢量在太阳坐标系下的投影[cos(h_s)sin(A_s)cos(h_s)cos(A_s)sin(h_s)]^T由变换矩阵变换到导航坐标系，再由变换矩阵变换到载体坐标系，最后由变换矩阵变换到偏振光传感器测量坐标系，根据瑞利散射模型计算出理论偏振矢量，设为其中g表示重力加速度的大小；h_s和A_s分别表示太阳高度角和太阳方位角，通过查询天文历表获得；表示太阳坐标系到导航坐标系的变换矩阵；表示导航坐标系到载体坐标系的变换矩阵；表示载体坐标系到偏振光传感器测量坐标系的变换矩阵；上标*表示重力矢量和偏振矢量分别在载体坐标系和偏振光传感器测量坐标系下的理论测量值；下标x,y,z表示理论测量值在三轴的投影分量；

步骤三：计算重力矢量误差函数，设为e_g；计算偏振矢量误差函数，设为e_p；对重力矢量误差函数求导获得雅可比矩阵，设为J_g；对偏振矢量误差函数求导获得雅可比矩阵，设为J_p；将偏振矢量误差函数与重力矢量误差函数及其雅可比矩阵组合成综合目标误差函数与综合雅可比矩阵，分别设为e＝[e_g e_p]^T、J＝[J_g J_p]^T；其中q＝[q₀ q₁ q₂ q₃]为姿态角的四元数表示；a_x、a_y、a_z和p_x、p_y、p_z分别为步骤一所测量得到的加速度计和偏振光传感器输出值；

步骤四：根据无约束优化方法中的梯度优化理论，由式(8)计算综合目标误差函数梯度g_k；由式(9)计算初始迭代的搜索方向d₀；由式(10)计算第k次迭代的搜索方向d_k，其中，k≠0，β_k为保证前后搜索方向相互共轭的参数，式(11)给出β_k的计算公式；式(12)给出步长λ_k的计算公式；其中，下标k表示四元数姿态求解过程中的第k次迭代；

g_k＝J^Te(8)

d₀＝-g₀(9)

d_k＝-g_k+β_kd_k-1，k≠0(10)

步骤五：为根据载体运动状态实时调整步长大小，由式(13)求解陀螺仪四元数微分方程，并由式(14)构建动态步长因子α_k，预测四元数更新趋势；在已知第k次迭代的姿态四元数q(k)后，根据迭代公式(15)更新得到下一时刻的姿态四元数q(k+1)；其中，表示陀螺仪四元数微分；ω_x、ω_y、ω_z为步骤一测量得到的三轴角速度值；Δt表示姿态更新周期；

步骤六：利用步骤五迭代得到的姿态四元数，由式(16)换算出当前载体的横滚角γ、俯仰角θ和航向角ψ；

步骤七：重复步骤一至步骤六，实现偏振光辅助航姿参考系统测量定姿。