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

说明书

本发明提供了一种基于约束卡尔曼滤波的集群组网协同导航方法及系统，涉及协同导航技术领域，该方法包括：步骤S1：确定集群组网节点数量，并测量各节点间的相对距离；步骤S2：基于测量的各节点间的相对距离，建立集群组网协同导航滤波方程；步骤S3：设计集群组网各节点惯导位置误差加权和最小约束条件；步骤S4：构建集群组网协同导航的约束卡尔曼滤波；步骤S5：利用约束卡尔曼滤波估计出的各节点惯导误差估计值对纯惯导误差补偿。本发明能够实时递推估计出惯导误差对惯导系统进行修正，抑制各个节点纯惯导误差发散。

技术领域

本发明涉及协同导航技术领域，具体地，涉及一种基于约束卡尔曼滤波的集群组网协同导航方法及系统。

背景技术

目前，惯性导航技术已经在航天、航空、无人驾驶、水下导航、室内定位等生活中各个场景广泛应用，成为一种常用的导航方式。

公开号为CN110207691A的中国发明专利，公开了一种基于数据链测距的多无人车协同导航方法，利用数据链对无人车之间相对距离进行测量，进而通过多导航传感器信息融合方法对编队中每辆无人车节点的位置进行估计。无人车编队中每辆无人车节点搭载惯性导航系统、车辆里程计、数据链收发装置，首先通过卡尔曼滤波器对每辆无人车节点中的惯性导航系统、车辆里程计进行融合，得到每辆无人车节点的速度、位置、姿态信息；进而利用数据链测得的无人车之间的距离信息，通过等式约束对每辆无人车的位置估计结果进行优化。通过本方法能够提高无人车编队在卫星拒止条件下的导航定位精度。

综上所述，在现有技术当中，协同导航时必须使用其它导航传感器辅助构建约束条件，如车辆里程计等，带来了额外的经济成本，且在许多场景使用受限。本发明在仅依靠每个载体自身所带惯性导航系统，利用数据链测距，通过设计几何约束条件，构建约束卡尔曼滤波，解决惯导系统存在长时误差快速发散，无法用于长时间导航的问题。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于约束卡尔曼滤波的集群组网协同导航方法及系统。

根据本发明提供的一种基于约束卡尔曼滤波的集群组网协同导航方法及系统，所述方案如下：

第一方面，提供了一种基于约束卡尔曼滤波的集群组网协同导航方法，所述方法包括：

步骤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中的集群组网协同导航滤波方程，还包括测量方程，所述测量方程具体为：