[发明专利]基于约束卡尔曼滤波的集群组网协同导航方法及系统

权利要求书

1.一种基于约束卡尔曼滤波的集群组网协同导航方法，其特征在于，包括：

步骤S1：确定集群组网节点数量，并测量各节点间的相对距离；

步骤S2：基于测量的各节点间的相对距离，建立集群组网协同导航滤波方程；

步骤S3：设计集群组网各节点惯导位置误差加权和最小约束条件；

步骤S4：构建集群组网协同导航的约束卡尔曼滤波；

步骤S5：利用约束卡尔曼滤波估计出的各节点惯导误差估计值对纯惯导误差补偿；

所述步骤S2中的集群组网协同导航滤波方程，包括状态方程，所述状态方程具体为：

X(t)＝F(t)X(t)+G(t)W(t)

式中，分别为集群中各个节点的地理坐标系下的三轴位置误差，n为集群节点数量，F(t)为状态转移矩阵，G(t)为系统噪声系数矩阵，W(t)为系统噪声矩阵；

其中，

式中，V_eⁱ表示i节点东向速度；L_i表示i节点纬度；h_i表示i节点高度；R_N表示地球卯酉圈曲率半径；R_M表示地球子午圈曲率半径；V_nⁱ表示i节点北向速度；

所述步骤S2中的集群组网协同导航滤波方程，还包括测量方程，所述测量方程具体为：

L(t)＝HX(t)+Δ

式中，L(t)是(n-1)×1维观测量矩阵，n-1为观测量数量，即测距值数量，H为(n-1)×3n维观测系数矩阵，Δ是(n-1)×1维观测噪声矩阵；

其中，

式中，为集群中节点i与节点j间的测量距离，为节点i与节点j间惯导计算距离，L随弹群节点n的数量变化实时变化；D_a为地理坐标系下纬经高误差转换至地心地固坐标系下XYZ三轴误差的转换矩阵；

所述步骤S4采用投影法将约束条件投影至传统卡尔曼滤波，构建集群组网协同导航的约束卡尔曼滤波为如下：

约束条件矩阵：B_k＝G^Tp_xD_a；

状态一步预测：

约束修正后状态一步预测：

状态估计：

滤波增益：

一步预测均方误差：P_k/k-1＝φ_k,k-₁P_k-1φ^T_k,k-1+Q_k；

估计均方误差：P_k＝(I-K_kH_k)P_k/k-1；

式中，B_k为约束条件矩阵，为一步预测状态量矩阵，为前次状态量矩阵，φ_k,k-1为状态转移矩阵，为约束修正后预测状态量矩阵，P_k-₁为状态误差协方差矩阵，K_k为滤波增益矩阵，Z_k为观测量矩阵，H_k为观测系数矩阵，P_k/k-1为一步预测状态误差协方差矩阵，R_k为观测噪声矩阵，Q_k为状态噪声矩阵，I为单位矩阵；G^T是由H^TPH的零特征值对应的特征向量组成的6×3n维矩阵，P为L的对角权阵，H^TPH为L的对角权阵的正定矩阵，P_x为根据各节点惯导系统精度水平确定的待估参数向量X的3n×3n维权阵，n为集群节点数量。

2.根据权利要求1所述的基于约束卡尔曼滤波的集群组网协同导航方法，其特征在于，所述步骤S3中集群组网各节点惯导位置误差加权和最小约束条件，其中，约束条件包括：

G^Tp_xD_aX＝O

式中，为集群节点i在地心地固系下惯导位置，P_x随集群节点n的数量变化实时变化。