[发明专利]一种用于仿生导航的姿态最优化解算方法

摘要

本发明提供一种用于仿生导航的姿态最优化解算方法，属于偏振光导航姿态求解技术领域。该方法利用偏振矢量和重力矢量构建目标误差函数模型，并根据不同运动状态求解最优姿态四元数，以提高复杂机动条件下的姿态解算精度。本发明通过求解偏振方位角误差搜索方向实现无人机航向信息补偿修正，避免直接求解出现的角度模糊性问题。本发明设计的偏振光导航方案解耦航向与水平姿态求解过程，以克服姿态解算的空间位置约束，同时采取搜索因子可变的梯度优化方法，可以根据飞行器不同运动状态实时改变搜索步长大小，提高偏振光导航系统的测量精度和鲁棒性。

主权项

1.一种用于仿生导航的姿态最优化解算方法，其特征在于，具体步骤如下：步骤一：采集偏振光传感器和加速度计的输出数据，设偏振光传感器在模块坐标系下的量测值为加速度计在载体坐标系下的量测值为将二者作为目标误差函数实测项；其中，的上标m表示投影到模块坐标系，下标c表示该变量是偏振光传感器的实测值；的上标b表示投影到载体坐标系，下标c表示该变量是加速度计的实测值；下标x,y,z表示矢量在三个坐标轴上各自分量；步骤二：通过查询天文年历和经纬度信息，根据瑞利散射模型计算偏振矢量参考输出值再根据坐标系旋转变换关系将重力矢量从导航坐标系投影到载体坐标系，以得到重力矢量参考输出值将二者作为目标误差函数理论项；步骤三：利用和组成偏振误差矢量由和构成重力误差矢量从而得到姿态误差矢量为使姿态误差最小需建立目标误差函数模型F(q)；其中，其中，q＝[q0 q1 q2 q3]T，为导航坐标系对载体坐标系的旋转四元数；步骤四：为求解误差最小值的姿态四元数，采用梯度优化理论中一维精确搜索方法，根据目标误差函数的梯度与其导数H(q)计算搜索步长因子λ；其中，步骤五：在姿态解算中，每个解算周期只需进行一次迭代即可获得最优姿态四元数，从而通过旋转四元数微分方程建立偏振光传感器、加速度计与陀螺仪之间的数据融合关系；在已知第k次最优姿态估计值Q(k)情况下，由式(7)计算当前最优四元数变化率再结合步骤四求解的搜索步长因子λ迭代更新得到下一时刻姿态四元数最优解Q(k+1)；其中，ωx(k),ωy(k),ωz(k)表示陀螺仪在载体坐标系下x,y,z轴各自的角速度分量；Δt表示姿态解算周期；步骤六：利用步骤五得到的姿态四元数最优解，解算出当前时刻飞行器横滚角γ、俯仰角θ以及航向角ψ的最优估计值；