一种弹载惯性/卫星紧组合导航方法与流程

技术特征：
1.一种弹载惯性/卫星紧组合导航方法，其特征在于，包括以下步骤：步骤1，SINS初始对准，初始化速度、位置；步骤2，导航计算机分别接收GNSS数据和SINS数据；步骤3，导航计算机进行SINS导航解算，得到载体的速度、位置、姿态信息；导航计算机判断GNSS是否发送完所有通道信息，若发送完则计算与各通道对应的卫星高度角、方位角信息；步骤4，判断可见卫星个数，若可见卫星大于4颗，则通过分布式最佳精度因子选星法选出4颗可见卫星作为导航星；若可见卫星少于4颗，则将所有可见卫星选为导航星；步骤5，对导航卫星的伪距测量误差进行补偿；根据导航卫星的速度、位置信息，以及SINS的速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息；步骤6，对组合导航的系统状态进行判别，并根据GNSS、IMU的工作状态选择匹配的导航策略，构建系统状态方程，并根据可见卫星的个数构建系统量测方程；步骤7，根据系统状态方程和系统量测方程，采用卡尔曼滤波信息融合法进行滤波，并根据滤波结果，对由通讯延时引起的滞后误差，通过基于状态转移的误差补偿方法对系统进行校正得到最终的导航结果，具体如下：(7.1)设GNSS接收机秒脉冲时刻为tk，GNSS数据传输完毕时刻为tk+td，td为GNSS通讯延时；(7.2)利用秒脉冲时刻SINS和GNSS输出进行组合卡尔曼滤波，求取tk时刻导航状态误差量的最优估计(7.3)采用GNSS传输完毕时刻的SINS解算输出求取连续系统下的状态转移矩阵F(tk+td)，采用直接法求取tk至tk+td时刻的系统状态误差转移矩阵其中，I为单位阵；(7.4)利用系统状态误差转移矩阵的性质，将tk时刻导航状态误差量的最优估计递推至当前时刻，并进行反馈修正，得tk+td时刻导航状态误差量的最优估计2.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤2中所述导航计算机分别接收GNSS数据和SINS数据，具体如下：(2.1)导航计算机接收GNSS数据在紧组合导航系统中，GNSS接收机输出多个卫星的信息，每颗卫星信息通过一个通道输出，每个通道的信息包含：通道号，卫星编号，卫星工作状态，世界标准时间，接收机地心地固直角坐标系下的X、Y、Z轴位置和速度，伪距、伪距率量测值，卫星在地心地固直角坐标系下的X、Y、Z轴位置和速度；导航计算机依次接收、存储每个卫星对应的通道信息；(2.2)导航计算机接收SINS数据IMU输出载体加速度、角速度信息，导航计算机接收IMU输出信息，进行导航解算。3.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤3中所述导航计算机进行SINS导航解算，得到载体的速度、位置、姿态信息；导航计算机判断GNSS是否发送完所有通道信息，若发送完则计算与各通道对应的卫星高度角、方位角信息，具体如下：(3.1)采用传统四元数法进行捷联惯导系统姿态更新解算，其中四元数微分方程表达式为：其中，Ω为载体坐标系相对导航系下的角速率构成的反对称矩阵，Q为四元数；通过龙格-库塔求解四元数微分方程，然后由四元数求得姿态矩阵，由姿态矩阵求解载体的三个姿态角；(3.2)进行捷联惯导系统速度解算，速度微分方程如下：其中，Vn、分别为导航系下载体的速度矢量、速度矢量变化率，为载体坐标系到导航坐标系的姿态转换矩阵，fb为加速度计在载体坐标系下的输出值，为地球自转角速率在导航系下的投影，为导航系相对地球系的旋转角速率，gn为当地重力加速度矢量；(3.3)进行捷联惯导系统位置解算，载体的位置微分方程如下：分别为导航系下载体的纬度、经度和高度的变化率，VE,VN,VU分别为导航系下载体的东向、北向和天向速度，RM为椭球子午圈上各点的曲率半径，RN为卯酉圈上各点的曲率半径，L,λ,h分别为导航系下载体的纬度、经度和高度；(3.4)导航计算机通过通道标志，判断GNSS是否发送完所有通道信息：若没有接收完，则继续接收；若接收完则计算与各通道对应的卫星高度角、方位角信息；计算方法如下：其中，[ΔeΔnΔu]T为导航坐标系中载体到卫星的观测向量，[ΔxΔyΔz]T为地心地固直角坐标系中载体到该卫星的观测向量，其中，[XYZ]T为卫星在地心地固直角坐标系中的位置，[xyz]T为载体在地心地固直角坐标系中位置，则卫星的高度角θ、方位角α分别如下：α＝arctan(Δe/Δn)其中，0≤θ≤π/2、0≤α≤2π。4.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤4中所述若可见卫星大于4颗，则通过分布式最佳精度因子选星法选出4颗可见卫星作为导航星，具体如下：将选星算法拆分到多个惯导解算周期内完成，拆分方式为：首先构建一个包含所有4颗可见卫星组合情况的表格，在GNSS数据接收完的下一个惯导解算周期开始选星，每个惯导解算周期内通过查表选择不同的4颗可见卫星组合情况进行几何精度因子GDOP计算，直到表格中所有组合情况的几何精度因子GDOP计算完，选择几何精度因子GDOP最小的一组4颗可见卫星作为导航星；其中几何精度因子GDOP的求取方法如下：式中，θ(σ)、α(σ)分别为一组可见卫星中第σ颗卫星的高度角、方位角，σ＝1,2,3,4。5.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤5中所述对导航卫星的伪距测量误差进行补偿，然后根据导航卫星的速度、位置信息，以及SINS的速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息，具体为：(5.1)对导航卫星的伪距测量误差进行补偿，补偿地球自转误差、对流层误差；地球自转误差：其中，Px,Py分别为卫星在地心地固直角坐标系下x轴、y轴位置，px,py分别为载体在地心地固直角坐标系下x轴、y轴位置，we为地球自转角速率，light_speed为光速；对流层误差：其中，θ为卫星的高度角，light_speed为光速；通过校正，得到导航卫星伪距ρGj、伪距率信息，伪距ρGj为：ρGj＝rj-δtu-vρjδtu＝δt1+δt2其中，δtu为伪距测量误差，vρj为伪距测量白噪声，rj为载体到第j颗卫星Sj的真实无差距离；导航卫星的伪距率公式如下：其中，δtru为钟漂引起的距离率误差，为伪距率测量白噪声，为载体到第j颗卫星Sj的真实无差距离变化率；(5.2)根据导航卫星的速度、位置信息，以及SINS的速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息；载体到第j颗卫星的伪距ρIj为：ρIj＝rj+ej1δx+ej2δy+ej3δz其中，δx、δy、δz分别为载体在地球坐标系中的位置误差在x轴、y轴、z轴分量，ej1、ej2、ej3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦；载体到第j颗卫星的伪距率为：其中分别为载体在地球坐标系中的速度误差在x轴、y轴、z轴分量。6.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤6中所述对组合导航的系统状态进行判别，并根据GNSS、IMU的工作状态选择匹配的导航策略，构建系统状态方程，并根据可见卫星的个数构建系统量测方程，具体如下：(6.1)对组合导航的系统状态进行判别(a)根据IMU的陀螺仪采样值和加速度计采样值判断IMU的工作状态，设A(axis)max为加速度阈值、ω(axis)max为角速度阈值，判断加速度计采样值Aaxis和陀螺采样值ωaxis是否满足以下条件：|Aaxis|＜A(axis)max|ωaxis|＜ω(axis)max当满足上式时，则IMU工作状态正常，否则IMU的工作状态异常；(b)根据GNSS输出值判断GNSS的工作状态，先后进行外层判断和内层判断：①外层判断为收星条件判别，设dop为精度因子门限，判断收星数Nsats和几何精度因子GDOP是否满足以下条件：或1≤Nsats＜4当上式满足其中一个时，继续内层判别，否则认为GNSS的工作状态异常；②内层判断对GNSS量测粗大误差进行判别，设δρ、分别为伪距差阈值、伪距率差阈值，ρGj、分别为第j颗卫星当前时刻量测伪距、伪距率值，ρIj、分别为载体相对第j颗卫星的伪距、伪距率值，则判断下式是否成立：|ρGj-ρIj|＜δρ上式成立时，则认为GNSS的工作状态正常，否则GNSS的工作状态异常；(6.2)根据GNSS、IMU的工作状态选择匹配的导航策略，具体方法如下：(a)当IMU、GNSS的工作状态均正常时，采用紧组合导航：将IMU和GNSS进行位置速度误差组合得到误差方程，经卡尔曼滤波估计出载体的位置、速度和姿态误差，对IMU的位置、速度、横滚角和俯仰角进行反馈校正；(b)当IMU工作状态异常、GNSS工作状态正常时，放弃当前时刻IMU中陀螺仪和加速度计的量测值，用前一时刻的量测值替代：ω(k)axis＝ω(k-1)axisA(k)axis＝A(k-1)axis其中，ω(k)axis为k时刻的角速度，ω(k-1)axis为k-1时刻的角速度，A(k)axis为k时刻的加速度，A(k-1)axis为k-1时刻的加速度；(c)当IMU工作状态正常、GNSS工作状态异常时，采用丢星算法处理；从丢星状态恢复收星时，利用状态误差转移矩阵F估计导航误差并对导航输出进行修正；(d)当IMU、GNSS工作状态均异常时，采用机动目标的轨迹预测方法对载体运动状态进行估计；(6.3)导航系统的姿态、速度、位置、伪距、伪距率误差方程如下：式中，φE、φN、φU分别为东、北、天方向平台失准角，δVE、δVN、δVU分别为载体东、北、天方向速度误差，δL、δλ、δh分别为载体纬度、经度、高度误差，δtu为与时钟等效的距离误差，δtru为与时钟频率等效的距离率误差，RM为椭球子午圈上各点的曲率半径，RN为卯酉圈上各点的曲率半径，wie为地球转动角速率，fE、fN、fU分别是惯导系统的比力在导航系下东、北、天方向上的分量，εE、εN、εU、分别为地理坐标系内陀螺的等效漂移在东、北、天方向的分量，分别为地理坐标系内加速计的等效漂移在东、北、天方向的分量，βtru为反相关时间；(6.4)以导航系统的姿态误差、速度误差、位置误差及伪距、伪距率误差为状态变量，建立惯性/卫星组合导航系统的状态方程：其中，X为系统状态矢量，表示系统状态矢量的导数，F为系统状态转移矩阵，G为系统噪声驱动矩阵，W为系统噪声矢量，具体如下：系统状态矢量：其中，φE、φN、φU分别为东、北、天方向平台失准角，δVE、δVN、δVU分别为载体东、北、天方向速度误差，δL、δλ、δh分别为载体纬度、经度、高度误差，εx、εy、εz分别为载体系下陀螺随机常值漂移在x、y、z轴上的分量，▽x、▽y、▽z分别为载体系下加速度计偏置在x、y、z轴上的分量，δtu为与时钟等效的距离误差，δtru为与时钟频率等效的距离率误差；状态转移矩阵：其中，Fins由步骤(6.3)中误差方程构成，为载体坐标系到导航坐标系的姿态转换矩阵，βtru为反相关时间；系统噪声驱动矩阵为G，且：系统噪声矢量为W，且：其中，wgx、wgy、wgz分别为陀螺仪在x轴、y轴、z轴方向的随机白噪声，wax、way、waz分别为加速度计在x轴、y轴、z轴方向的随机白噪声，wtu、wtru分别为伪距随机白噪声和伪距率随机白噪声；(6.5)构建系统状态方程，并根据可见卫星的个数构建系统量测方程，如下：Z(t)＝H(t)X(t)+V(t)其中，Z(t)为系统观测矢量，H(t)为系统观测矩阵，V(t)为系统观测噪声阵，X为系统状态矢量；量测方程的维数及组合滤波器的维数根据可见卫星数量变化，变化关系如下：其中，N为可见星数量，观测向量Z的维数为：2n×1；系统观测矩阵H为：2n×17；系统观测噪声方差R阵为：2n×2n；卡尔曼滤波增益阵为：17×2n；伪距观测方程如下：式中，为观测矢量，为观测矩阵，为观测噪声阵，为状态矢量，分别为：δρj＝ρIj-ρGj＝ej1δx+ej2δy+ej3δz+δtu+νρj其中，δρj为卫星伪距和载体相对卫星伪距之差，j＝1…n，为各通道伪距测量白噪声，ρIj为载体相对每颗导航卫星的伪距，νρj为伪距量测白噪声，δtu为钟差引起的距离误差，ρGj为导航卫星的伪距，展开如下，j＝1…n,i＝1,2,3则：其中，ej1、ej2、ej3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦，f为地球椭圆度；伪距率观测方程如下所示：式中，为观测矢量，为观测矩阵，为观测噪声阵，为状态矢量，分别为：其中，为卫星伪距率和载体相对卫星伪距率之差，j＝1…n，为各通道伪距率测量白噪声，为载体相对每颗导航卫星伪距率，为伪距率量测白噪声，δtru为钟漂引起的距离率误差，为导航卫星伪距率，展开如下，j＝1…n,i＝1,2,3则：其中，ej1、ej2、ej3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦；综合伪距、伪距率观测方程，得到惯性/卫星组合导航系统观测方程如下：