[发明专利]一种基于环路和积数据关联算法的协同导航方法

权利要求书

1.一种基于环路和积数据关联算法的协同导航，其特征在于：协同导航系统内有多个平台，平台自身用卫星(GPS)和惯导(INS)进行绝对导航，平台间用GPS和超宽带(UWB)进行相对导航，且平台间可相互通信，它包括以下步骤：

步骤一：平台自身采用惯导，卫星等传感器进行绝对导航，获取平台的位置、速度、姿态等信息；

步骤二：平台间采用卫星差分和超宽带的测距测速，借助绝对导航输出结果进行相对导航，获取平台间的相对位置、速度信息；

步骤三：平台间互相传递信息，构建基于环路和积数据关联算法的协同导航滤波器，估计出所有平台的位置、速度信息。

2.根据权力要求1所述的基于环路和积数据关联算法的协同导航，其特征在于：步骤一所述的绝对导航，为INS和GPS紧组合导航，主要根据GPS接收机计算出的卫星星历数据和惯导系统计算出的速度位置，解算出对应于惯性导航系统的伪距和伪距率，再与GPS接收机测量到的伪距伪距率相比较，估计出惯性导航系统和GPS接收机的误差量，然后将估出的参数反馈至INS内部，得到平台自身的运动状态，以平台i为例进行说明：

绝对导航采用坐标系为东北天地理坐标系，其状态包括9维的INS误差，6维的INS元件误差，2维的GPS误差，状态方程可写作：

其中：

代表k时刻编号为i的平台状态，为东北天三个方向的平台误差角，δv_E,δv_N,δv_U和δL,δλ,δh为三个方向的速度和位置误差，ε_x,ε_y,ε_z和Δ_x,Δ_y,Δ_z陀螺仪和加速度计在三个方向的偏置误差，δt_u为时钟误差等效的测距误差，δt_ru为时钟频率误差等效的距离变化率,F为状态转移矩阵，G为噪声系数矩阵，W为噪声向量，上角标i为平台编号，下角标k代表时间；

设平台在地球大地坐标系下惯导输出位置为经度λ_I，纬度L_I，高度h_I，则平台的真实位置的λ,L,h可表示成如下形式：

量测为GPS接收机测得的四颗卫星的伪距和伪距率，量测方程可写作：

其中ρ^j为伪距测量，具体展开如下：

式中，为卫星j在地心地球固连(ECEF)坐标系下各方向位置，下角标s代表卫星，上角标j为卫星编号，x,y,z为平台自身在ECEF坐标系下各方向位置，v_ρ为伪距测量噪声，ECEF坐标系与东北天地理坐标系转换关系如下：

式中，R_N为地球半径，f为地球的椭圆扁率；

为伪距率测量，具体展开如下：

式中，为平台自身在ECEF坐标系下各方向速度，为卫星在ECEF坐标系下各方向速度，其与v_E,v_N,v_U的转换关系如下：