一种天空偏振光组合导航在线滤波与平滑方法与流程

本发明涉及无人机天空偏振光组合导航领域，尤其涉及一种天空偏振光组合导航在线滤波与平滑方法。

背景技术

生物经过亿万年地进化具有了高超的导航能力。1985年，科学家wehner发现沙蚁可以利用天空中的偏振光进行导航，此外科学研究表明自然界中的帝王蝶、蜣螂等昆虫也将偏振导航作为自己的主要或者辅助的导航方式。受昆虫利用偏振光导航的启发，近些年来，偏振光导航逐步发展成为一种新型的导航方式。大气层中的偏振光分布模式稳定，所以具有误差不随时间积累、在较大范围内不易受人为因素干扰、自主性强等优点，是一种精度高、鲁棒性强的全自主导航方式。

现有的传统捷联式组合导航算法通常首先进行捷联导航解算，在gnss、光流、地磁等导航纠正数据采集完毕时再进行集中式卡尔曼滤波，对组合导航中的误差量进行估计，然后将各个误差量的最优估计值反馈给系统完成校正。在滤波过程中，通常仅利用当前时刻的捷联解算结果，又因为捷联解算的频率要远远高于卡尔曼滤波的频率，在对姿态、速度、位置等导航结果进行误差修正的时候会产生锯齿波，影响组合导航系统的精度甚至造成滤波器发散。同时锯齿波还会对后续控制系统带来很大的影响，如果飞行器控制率选取不当，会对无人机的飞行带来巨大的安全隐患。

为提高组合导航系统整体精度，通常还需要对组合导航过程中的数据进行后处理，补偿组合导航中各传感器采集时间不同步问题，平滑组合导航过程中的锯齿波现象。但是后处理通常需要事后离线进行解算，无法实时对导航滤波结果进行平滑。

技术实现要素：

本发明的技术解决问题是：克服现有组合导航方法在误差修正时会产生锯齿波的缺点，提供一种天空偏振光组合导航在线滤波与平滑方法，通过双处理器协同分工，共同完成导航解算。相比以往的捷联组合导航方法，本发明借助天空偏振光信息对组合导航中三轴失准角进行修正，提高了三轴平台失准角的可观测度，此外利用了历史导航数据对捷联解算结果进行在线平滑处理，减小了修正误差时所产生的锯齿波现象，提高了组合导航系统整体精度。

本发明的技术解决方案是：一种天空偏振光组合导航在线滤波与平滑方法，通过下列步骤实现：

(1)导航处理器1采集惯性导航数据，进行捷联导航解算，将姿态、速度和位置解算结果发给导航处理器2；

(2)导航处理器2将采集到的卫星导航模块、偏振传感器的数据，以及结合前n秒接收到的捷联解算历史数据，共同代入r-t-s滤波器进行平滑处理，对组合导航中状态量进行最优估计；

(3)导航处理器2将最优估计中的组合导航系统状态量发送给导航处理器1，导航处理器1对捷联解算结果进行导航误差修正；

(4)如此往复上述(1)-(3)步骤，直至组合导航结束。

所述步骤(2)中，最优估计具体过程为：

(21)根据组合导航误差方程建立滤波模型，系统方程为：

其15维系统状态量为：

分别是三轴平台失准角、东北天速度误差、经纬高位置误差、三轴陀螺仪零偏、三轴加速度计零偏；

系统状态转移矩矩阵f为：

其中fn为对应的9维基本导航参数系统阵，由捷联惯性导航系统误差模型组成，fs为：

上式中为载体坐标系b到导航坐标系n的姿态转移矩阵；

系统噪声转移矩阵g为：

系统噪声由陀螺仪和加速度计的随机误差组成w为：

分别为三轴加速度计和三轴陀螺仪随机漂移，并且噪声服从零均值正态分布；

(22)导航处理器2采集到卫星导航模块数据为地理系下位置pgnss和速度vgnss,偏振传感器数据为载体系下太阳矢量s^b，建立系统量测方程为：

z＝hx+v,

其中为观测向量观测转移矩阵为为含有平台失准角的计算坐标系n’到载体系b的姿态转换矩阵，为上一次姿态转移矩阵滤波修正后的最优估计值，sⁿ为地理系下太阳矢量，可由天文年历计算得到；

(23)将系统方程与量测方程以及历史量测数据共同代入r-t-s滤波器进行平滑处理，对组合导航中状态量进行最优估计。首先执行前向标准卡尔曼滤波，其公式为：

然后执行平滑处理，其公式为：

其中i为单位阵，p为状态协方差阵，k为卡尔曼增益阵。

本发明与现有技术相比的优点在于：

(1)本发明相比以往的捷联组合导航方法，利用了历史导航数据对捷联解算结果进行在线平滑处理，减小了修正误差时所产生的锯齿波现象，在保证滤波实时性的同时，提高了组合导航系统整体精度。

(2)本发明采用的偏振导航方式不易受外界干扰，可以在复杂环境下实现无源、无辐射导航工作，是一种精度高、鲁棒性强的全自主导航方式。

附图说明

图1为本发明方法实现流程图。

具体实施方式

下面结合附图及实施例对本发明进行详细说明。

如图1所示，本发明的具体实现步骤如下：

(1)导航处理器1采集三轴陀螺仪角速度去除有害角速度后得到根据等效旋转矢量法对姿态进行更新。处理器1采集到三轴加速度计为比力f^b，去除有害加速度后一次积分得到速度vsins、二次积分得到位置psins。最后处理器1将姿态、速度、位置等解算结果发给导航处理器2；

(2)根据组合导航误差方程建立滤波模型，系统方程为：

其15维系统状态量为：

分别是三轴平台失准角、东北天速度误差、经纬高位置误差、三轴陀螺仪零偏、三轴加速度计零偏；

系统状态转移矩矩阵f为：

其中fn为对应的9维基本导航参数系统阵，由捷联惯性导航系统误差模型组成，fs为：

上式中为载体坐标系b到导航坐标系n的姿态转移矩阵；

系统噪声转移矩阵g为：

系统噪声由陀螺仪和加速度计的随机误差组成w为：

分别为三轴加速度计和三轴陀螺仪随机漂移，并且噪声服从零均值正态分布；

导航处理器2采集到卫星导航模块数据为地理系下位置pgnss和速度vgnss,偏振传感器数据为载体系下太阳矢量s^b，建立系统量测方程为：

z＝hx+v,

其中为观测向量观测转移矩阵为为含有平台失准角的计算坐标系n’到载体系b的姿态转换矩阵，为上一次姿态转移矩阵滤波修正后的最优估计值，sⁿ为地理系下太阳矢量，可由天文年历计算得到；

将系统方程与量测方程以及历史量测数据共同代入r-t-s滤波器进行平滑处理，对组合导航中状态量进行最优估计。首先执行前向标准卡尔曼滤波，其公式为：

然后执行平滑处理，其公式为：

其中i为单位阵，p为状态协方差阵，k为卡尔曼增益阵。

(3)导航处理器2将估计的组合导航系统状态量发送给导航处理器1，导航处理器1对捷联解算结果进行导航误差修正，速度、位置直接减去误差项完成修正，姿态修正方法为

其中是计算坐标系n’到导航坐标系n的姿态转换矩阵，φ为步骤(2)中所估计得的三轴平台失准角。

(4)如此往复上述(1)-(3)步骤，直至组合导航结束。

以上是所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改动应视为本发明的保护范围。本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。