一种用于仿生偏振光导航的姿态无约束优化求解方法与流程

本发明属于仿生导航姿态解算技术领域，涉及一种用于仿生偏振光导航的姿态无约束优化求解方法，特别是涉及一种采用动态步长因子的共轭梯度优化姿态求解方法。

背景技术：

导航技术是信息化社会和武器装备信息化的重要支撑之一。航姿参考系统(ahrs，attitudeandheadingreferencesystem)可以为导航提供稳定的姿态参数信息。目前，航姿参考系统一般采用低成本的mems惯性测量单元作为姿态解算的主要部件。惯性测量单元由陀螺仪和加速度计组成，两者进行姿态数据融合可以修正陀螺仪的水平姿态角漂移。为修正航向误差，一般在惯性测量单元的基础上，引入磁强计或gps作进一步融合。磁强计也叫地磁传感器，通过测量地磁场强度和方向获得载体方位信息，但机体振动或控制电路产生的电磁场会降低其测量精度，信息化战争中电磁权是双方争夺的首要制高点，其工作环境无法得到安全保障。gps卫星导航作为非自主导航方式之一，卫星信号易受人为干扰甚至导航卫星存在被摧毁的可能，不适用于某些特殊场合。

针对磁强计和gps存在的显著缺点，国内外学者在寻找和利用全新的导航方法方面做了大量研究。偏振光导航是近几年兴起的新型导航方式，其原理是通过大气偏振光在天空中的偏振分布模式进行航向测量，具有完全自主、误差不随时间累积、稳定性好的优势。现阶段偏振光导航方法主要有两种：一是利用偏振光传感器进行自主定位；二是基于卡尔曼滤波与惯性测量单元进行数据融合，由测量所得的偏振方位角修正姿态误差。利用偏振光传感器进行自主定位由于其精度较低所以并不适合实际应用。基于卡尔曼滤波的姿态数据融合由于每次迭代过程中计算量大，实时性难以保证，同时偏振光传感器测量误差受多种因素影响，无法准确获得噪声模型，因此很多还停留在仿真阶段。

技术实现要素：

针对现有技术的不足之处，本发明提出一种用于仿生偏振光导航的姿态无约束优化求解方案。为克服一般梯度下降法收敛速度慢的缺点，本发明设计共轭参数保证姿态迭代寻优过程中前后搜索方向相互共轭；由实时四元数微分构建动态步长因子预测姿态更新趋势，使搜索方向具有目的性，提高姿态求解效率，避免了一般梯度优化方法中步长需要求二阶导数带来的计算复杂性。最后，以无约束梯度优化理论为基础，构建融合陀螺仪、加速度计、偏振光传感器的共轭梯度优化模型，实现偏振光辅助定姿的三维全姿态角求解，保证了偏振光辅助导航系统的精度与可靠性。

本发明的技术方案如下：

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

步骤一：采集陀螺仪在载体坐标系下的测量值、加速度计在载体坐标系下的测量值、偏振光传感器在测量坐标系下的测量值，陀螺仪测量值设为ωb＝[ωxωyωz]^t，加速度计测量值设为ab＝[axayaz]^t，偏振光传感器测量值设为pm＝[pxpypz]^t；其中，下标b表示载体坐标系；下标m表示偏振光传感器测量坐标系；下标x,y,z表示测量值在三轴的投影分量；

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

步骤三：计算重力矢量误差函数，设为eg；计算偏振矢量误差函数，设为ep；对重力矢量误差函数求导获得雅可比矩阵，设为jg；对偏振矢量误差函数求导获得雅可比矩阵，设为jp；将偏振矢量误差函数与重力矢量误差函数及其雅可比矩阵组合成综合目标误差函数与综合雅可比矩阵，分别设为e＝[egep]^t、j＝[jgjp]^t；其中q＝[q0q1q2q3]为姿态角的四元数表示；ax、ay、az和px、py、pz分别为步骤一所测量得到的加速度计和偏振光传感器输出值；和分别为步骤二所计算得到的理论重力矢量和理论偏振矢量；

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

gk＝j^te(8)

d0＝-g0(9)

dk＝-gk+βkdk-1(k≠0)(10)

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

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

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

本发明的有益效果：

(1)相比基于卡尔曼滤波的偏振光辅助定姿方法，本发明不需要建立噪声估计模型，减少了迭代过程中的计算量，提高了导航求解的实时性，降低了对硬件处理能力的要求，适用于低成本的航姿参考系统。

(2)本发明使用天空偏振光作为导航信息来源，可以避免使用磁力计所带来的电磁场干扰，在gps卫星信号丢失的情况下仍能进行正常导航，具有完全自主、稳定性高的优势。

(3)本发明所提出的共轭梯度优化方法以梯度优化理论为基础，通过建立姿态误差函数去修正陀螺仪漂移，避免偏振光矢量作为双向矢量所导致的角度求解模糊性问题，能够将偏振光传感器用于三维空间全姿态角解算。

(4)本发明引入共轭参数保证前后搜索方向相互共轭，克服了梯度下降法收敛速度慢的缺点，减小了角度突变时的滞后现象，提高了姿态角获取的效率。引入四元数微分构建动态步长因子，可以实时预测姿态变化趋势，提高动态环境下的姿态求解精度与稳定性。

附图说明

图1是本发明的数据融合过程原理图；

图2是本发明的动态步长因子共轭梯度优化方法流程图；

图3(a)～图3(c)是静态实验中本发明解算的姿态角与梯度下降法、参考值对比图；

图3(d)是静态实验中本发明的动态步长因子变化曲线图；

图4(a)～图4(d)是动态实验中本发明解算的横滚角与梯度下降法、参考值对比图及其局部放大图；

图5(a)～图5(d)是动态实验中本发明解算的俯仰角与梯度下降法、参考值对比图及其局部放大图；

图6(a)～图6(d)是动态实验中本发明解算的航向角与梯度下降法、参考值对比图及其局部放大图；

图7是动态实验中本发明的动态步长因子变化曲线图.

具体实施方式

下面结合附图和技术方案对本发明的具体实施方式作进一步阐述：

图1和图2展示了本发明姿态求解方法的计算原理图。

下面通过实验对本发明提出的姿态求解方法进行静态性能与动态性能验证：

实验一为静态测试实验，地点位置为：东经121.31°，北纬38.52°，时间是2019年5月26日下午17时45分，查询天文历表，可以获得太阳高度角为15.78°，太阳方位角为-104.67°。将配有偏振光传感器的导航实验平台静止放置约400秒，采集陀螺仪、加速度计、偏振光传感器的原始输出数据，验证静态条件下本发明求解姿态角的稳定性。

图3(a)～图3(d)展示了静态实验中本发明解算的姿态角与梯度下降法、参考值的对比曲线和动态步长因子变化曲线，参考值为卡尔曼滤波算法解算的姿态角。表1列出了本实施例中解算的姿态角与梯度下降法、参考值的均方差统计对比数据，表1中的参考算法为卡尔曼滤波算法。从姿态角对比曲线图和均方差数据可以看出，本发明用于姿态角求解时静态性能良好，由于本发明的优势主要体现在动态环境中，因此与梯度下降法的静态精度与稳定性相当；图3中动态步长因子曲线的变化较为平缓，符合静态实验条件的特点。

表1静态实验均方差统计对比数据

实验二为动态测试实验，实验地点不变，时间是2019年5月26日下午18时10分，查询天文历表，可以获得太阳高度角为11.1°，太阳方位角为-108.24°。将偏振光导航实验台进行绕轴三维转动约250秒，采集陀螺仪、加速度计、偏振光传感器的原始输出数据，验证动态条件下本发明求解姿态角的精度与效率。

图4(a)～图4(d)、图5(a)～图5(d)、图6(a)～图6(d)和图7展示了动态实验中本发明所解算的姿态角与梯度下降法、参考值的对比曲线图、局部放大图和动态步长因子变化曲线图，参考值为卡尔曼滤波算法解算的姿态角。从姿态角对比曲线图可以看出，在姿态大幅度变化时，本发明设计的共轭梯度优化方法能准确跟踪姿态角的变化；从姿态对比曲线局部放大图可以看出，相比梯度下降法，由于本发明设计了共轭参数保证前后搜索方向相互共轭，因此在姿态急剧变化处有更快的收敛速度，本发明设计的方法可以提高姿态求解的精度与效率；从动态步长因子的变化曲线图来看，动态步长因子能根据运动状态进行实时调整，符合动态实验条件的特点，表明对姿态误差发散起到了调控作用。

本发明设计的采用动态步长因子的共轭梯度优化姿态求解方法，能够避免由于偏振光矢量双向性导致的角度求解模糊性问题，可以实现三维空间全姿态角解算；相比梯度下降法，具有更快的收敛速度，提高了姿态求解的精度与效率；动态步长因子可以实时预测姿态更新趋势，保证了偏振光导航系统在复杂运动状态下的可靠性与准确性。