CN113175931A - 基于约束卡尔曼滤波的集群组网协同导航方法及系统 - Google Patents

基于约束卡尔曼滤波的集群组网协同导航方法及系统 Download PDF

Info

Publication number
CN113175931A
CN113175931A CN202110361651.3A CN202110361651A CN113175931A CN 113175931 A CN113175931 A CN 113175931A CN 202110361651 A CN202110361651 A CN 202110361651A CN 113175931 A CN113175931 A CN 113175931A
Authority
CN
China
Prior art keywords
node
matrix
cluster
inertial navigation
representing
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110361651.3A
Other languages
English (en)
Other versions
CN113175931B (zh
Inventor
秦峰
宋振华
刘伟鹏
崔闪
李云龙
吴镇
李勇
袁杰波
韩琳
王晖辉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Institute of Electromechanical Engineering
Original Assignee
Shanghai Institute of Electromechanical Engineering
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Institute of Electromechanical Engineering filed Critical Shanghai Institute of Electromechanical Engineering
Priority to CN202110361651.3A priority Critical patent/CN113175931B/zh
Publication of CN113175931A publication Critical patent/CN113175931A/zh
Application granted granted Critical
Publication of CN113175931B publication Critical patent/CN113175931B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

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

Description

基于约束卡尔曼滤波的集群组网协同导航方法及系统
技术领域
本发明涉及协同导航技术领域，具体地，涉及一种基于约束卡尔曼滤波的集群组网协同导航方法及系统。
背景技术
目前，惯性导航技术已经在航天、航空、无人驾驶、水下导航、室内定位等生活中各个场景广泛应用，成为一种常用的导航方式。
公开号为CN110207691A的中国发明专利，公开了一种基于数据链测距的多无人车协同导航方法，利用数据链对无人车之间相对距离进行测量，进而通过多导航传感器信息融合方法对编队中每辆无人车节点的位置进行估计。无人车编队中每辆无人车节点搭载惯性导航系统、车辆里程计、数据链收发装置，首先通过卡尔曼滤波器对每辆无人车节点中的惯性导航系统、车辆里程计进行融合，得到每辆无人车节点的速度、位置、姿态信息；进而利用数据链测得的无人车之间的距离信息，通过等式约束对每辆无人车的位置估计结果进行优化。通过本方法能够提高无人车编队在卫星拒止条件下的导航定位精度。
综上所述，在现有技术当中，协同导航时必须使用其它导航传感器辅助构建约束条件，如车辆里程计等，带来了额外的经济成本，且在许多场景使用受限。本发明在仅依靠每个载体自身所带惯性导航系统，利用数据链测距，通过设计几何约束条件，构建约束卡尔曼滤波，解决惯导系统存在长时误差快速发散，无法用于长时间导航的问题。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于约束卡尔曼滤波的集群组网协同导航方法及系统。
根据本发明提供的一种基于约束卡尔曼滤波的集群组网协同导航方法及系统，所述方案如下：
第一方面，提供了一种基于约束卡尔曼滤波的集群组网协同导航方法，所述方法包括：
步骤S1：确定集群组网节点数量，并测量各节点间的相对距离；
步骤S2：基于测量的各节点间的相对距离，建立集群组网协同导航滤波方程；
步骤S3：设计集群组网各节点惯导位置误差加权和最小约束条件；
步骤S4：构建集群组网协同导航的约束卡尔曼滤波；
步骤S5：利用约束卡尔曼滤波估计出的各节点惯导误差估计值对纯惯导误差补偿。
优选的，所述步骤S2中的集群组网协同导航滤波方程，包括状态方程，所述状态方程具体为：
X(t)＝F(t)X(t)+G(t)W(t)
式中，
Figure BDA0003005782290000021
分别为集群中各个节点的地理坐标系下的三轴位置误差，n为集群节点数量，F(t)为状态转移矩阵，G(t)为系统噪声系数矩阵，W(t)为系统噪声矩阵；
其中，
Figure BDA0003005782290000022
Figure BDA0003005782290000023
式中，Ve i表示i节点东向速度；Li表示i节点纬度；hi表示i节点高度；RN表示地球卯酉圈曲率半径；RM表示地球子午圈曲率半径；Vn i表示i节点北向速度。
优选的，所述步骤S2中的集群组网协同导航滤波方程，还包括测量方程，所述测量方程具体为：
L(t)＝HX(t)+Δ
式中，L(t)是(n-1)×1维观测量矩阵，n-1为观测量数量，即测距值数量，H为(n-1)×3n维观测系数矩阵，Δ是(n-1)×1维观测噪声矩阵；
其中，
Figure BDA0003005782290000031
Figure BDA0003005782290000032
式中，
Figure BDA0003005782290000033
为集群中节点i与节点j间的测量距离，
Figure BDA0003005782290000034
为节点i与节点j间惯导计算距离，L随弹群节点n的数量变化实时变化。
优选的，所述步骤S3中集群组网各节点惯导位置误差加权和最小约束条件，其中，约束条件包括：
GTPxDaX＝0
式中，GT是由HT PH的零特征值对应的特征向量组成的6×3n维矩阵，P为L的对角权阵，HT PH为L的对角权阵的正定矩阵；Px为根据各节点惯导系统精度水平确定的待估参数向量X的3n×3n维权阵，n为集群节点数量，
Figure BDA0003005782290000035
式中，
Figure BDA0003005782290000036
为集群节点i在地心地固系下惯导位置，Px随集群节点n的数量变化实时变化。
优选的，所述步骤S3中集群组网各节点惯导位置误差加权和最小约束条件，地理坐标系下纬经高误差转换至地心地固坐标系下XYZ三轴误差的转换矩阵为：
Figure BDA0003005782290000041
Figure BDA0003005782290000042
式中，Rn表示表示地球卯酉圈曲率半径；
Figure BDA0003005782290000043
表示i节点惯导解算高度；
Figure BDA0003005782290000044
表示i节点惯导解算纬度；
Figure BDA0003005782290000045
表示i节点惯导解算经度；e2表示地球曲率。
优选的，所述步骤S4采用投影法将约束条件投影至传统卡尔曼滤波，构建集群组网协同导航的约束卡尔曼滤波为如下：
约束条件矩阵：Bk＝GTPxDa；
状态一步预测：
Figure BDA0003005782290000046
约束修正后状态一步预测：
Figure BDA0003005782290000047
状态估计：
Figure BDA0003005782290000048
滤波增益：
Figure BDA0003005782290000049
一步预测均方误差：
Figure BDA00030057822900000410
估计均方误差：Pk＝(I-KkHk)Pk/k-1；
式中，Bk为约束条件矩阵，
Figure BDA00030057822900000411
为一步预测状态量矩阵，
Figure BDA00030057822900000412
为前次状态量矩阵，φk，k-1为状态转移矩阵，
Figure BDA00030057822900000413
为约束修正后预测状态量矩阵，Pk-1为状态误差协方差矩阵，Kk为滤波增益矩阵，Zk为观测量矩阵，Hk为观测系数矩阵，Pk/k-1为一步预测状态误差协方差矩阵，Rk为观测噪声矩阵，Qk为状态噪声矩阵，I为单位矩阵。
优选的，所述步骤S5利用约束卡尔曼滤波估计出的各节点惯导误差估计值
Figure BDA00030057822900000414
对纯惯导误差补偿方法如下：
Figure BDA00030057822900000415
Figure BDA00030057822900000416
Figure BDA00030057822900000417
式中，
Figure BDA0003005782290000051
表示i节点惯导解算高度；
Figure BDA0003005782290000052
表示i节点惯导解算纬度；
Figure BDA0003005782290000053
表示i节点惯导解算经度，
Figure BDA0003005782290000054
为i节点估计出的纬度误差，
Figure BDA0003005782290000055
为i节点估计出的经度误差，
Figure BDA0003005782290000056
为i节点估计出的高度误差。
第二方面，提供了一种基于约束卡尔曼滤波的集群组网协同导航系统，所述系统包括：
模块M1：确定集群组网节点数量，并测量各节点间的相对距离；
模块M2：基于测量的各节点间的相对距离，建立集群组网协同导航滤波方程；
模块M3：设计集群组网各节点惯导位置误差加权和最小约束条件；
模块M4：构建集群组网协同导航的约束卡尔曼滤波；
模块M5：利用约束卡尔曼滤波估计出的各节点惯导误差估计值对纯惯导误差补偿。
优选的，所述模块M2中的集群组网协同导航滤波方程，包括状态方程，所述状态方程具体为：
X(t)＝F(t)X(t)+G(t)W(t)
式中，
Figure BDA0003005782290000057
分别为集群中各个节点的地理坐标系下的三轴位置误差，n为集群节点数量，F(t)为状态转移矩阵，G(t)为系统噪声系数矩阵，W(t)为系统噪声矩阵；
其中，
Figure BDA0003005782290000058
Figure BDA0003005782290000059
式中，Ve i表示i节点东向速度；Li表示i节点纬度；hi表示i节点高度；RN表示地球卯酉圈曲率半径；RM表示地球子午圈曲率半径；Vn i表示i节点北向速度。
优选的，所述模块M2中的集群组网协同导航滤波方程，还包括测量方程，所述测量方程具体为：
L(t)＝HX(t)+Δ
式中，L(t)是(n-1)×1维观测量矩阵，n-1为观测量数量，即测距值数量，H为(n-1)×3n维观测系数矩阵，Δ是(n-1)×1维观测噪声矩阵；
其中，
Figure BDA0003005782290000061
Figure BDA0003005782290000062
式中，
Figure BDA0003005782290000063
为集群中节点i与节点j间的测量距离，
Figure BDA0003005782290000064
(i，j＝1，…，n且i≠j)为节点i与节点j间惯导计算距离，L随弹群节点n的数量变化实时变化。
与现有技术相比，本发明具有如下的有益效果：
本发明不依赖其它导航设备，仅利用集群中节点间组网测距，并设计了组网几何约束条件，基于约束条件构建状态方程和量测方程，设计了新的卡尔曼滤波器，实时递推估计出惯导误差对惯导系统进行修正，抑制各个节点纯惯导误差发散。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为根据本发明实施例的一种基于约束卡尔曼滤波的集群组网协同导航方法流程图；
图2为约束卡尔曼滤波更新过程图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
本发明实施例提供了一种基于约束卡尔曼滤波的集群组网协同导航方法，针对惯导误差长时累积问题，利用集群中节点间组网测距，并设计了组网约束条件，构建了约束卡尔曼滤波，估计出惯导误差，抑制纯惯导误差发散。方案如下：参照图1和图2所示，首先，确定集群组网节点数量，测量两节点间相对距离。
在本发明实施例中，根据几何构型确定参与组网的节点，通过数据链、毫米波雷达等手段实现组网各个节点间距离测量，作为协同导航各个节点间的量测距离，用于测距组网。
其次，建立集群组网协同导航滤波方程，包括状态方程和测量方程，其中，协同导航滤波状态方程：
X(t)＝F(t)X(t)+G(t)W(t)
式中，
Figure BDA0003005782290000071
分别为集群中各个节点的地理坐标系下的三轴位置误差，n为集群节点数量，F(t)为状态转移矩阵，G(t)为系统噪声系数矩阵，W(t)为系统噪声矩阵；
其中，
Figure BDA0003005782290000072
Figure BDA0003005782290000073
式中，Ve i表示i节点东向速度；Li表示i节点纬度；hi表示i节点高度；RN表示地球卯酉圈曲率半径；RM表示地球子午圈曲率半径；Vn i表示i节点北向速度。
协同导航滤波量测方程具体为：
L(t)＝HX(t)+Δ
式中，L(t)是(n-1)×1维观测量矩阵，n-1为观测量数量，即测距值数量，H为(n-1)×3n维观测系数矩阵，Δ是(n-1)×1维观测噪声矩阵；
其中，
Figure BDA0003005782290000074
Figure BDA0003005782290000081
式中，
Figure BDA0003005782290000082
为集群中节点i与节点j间的测量距离，
Figure BDA0003005782290000083
(i，j＝1，…，n且i≠j)为节点i与节点j间惯导计算距离，L随弹群节点n的数量变化实时变化。
随后，再设计集群组网各节点惯导位置误差加权和最小约束条件。
具体地，在本发明实施例中，集群组网各节点惯导位置误差加权和最小约束条件为：
GTPxDaX＝0
式中，GT是由HTPH的零特征值对应的特征向量组成的6×3n维矩阵，P为L的对角权阵，HTPH为L的对角权阵的正定矩阵；Px为根据各节点惯导系统精度水平确定的待估参数向量X的3n×3n维权阵，n为集群节点数量，
Figure BDA0003005782290000084
式中，
Figure BDA0003005782290000085
为集群节点i在地心地固系下惯导位置，Px随集群节点n的数量变化实时变化。
地理坐标系下纬经高误差转换至地心地固坐标系下XYZ三轴误差的转换矩阵为：
Figure BDA0003005782290000086
Figure BDA0003005782290000087
式中，Rn表示表示地球卯酉圈曲率半径；
Figure BDA0003005782290000088
表示i节点惯导解算高度；
Figure BDA0003005782290000089
表示i节点惯导解算纬度；
Figure BDA00030057822900000810
表示i节点惯导解算经度；e2表示地球曲率。
构建集群组网协同导航的约束卡尔曼滤波。
本发明实施例中，采用“投影法”将约束条件投影至传统卡尔曼滤波，构建集群组网协同导航的约束卡尔曼滤波为如下：
约束条件矩阵：Bk＝GTPxDa；
状态一步预测：
Figure BDA0003005782290000091
约束修正后状态一步预测：
Figure BDA0003005782290000092
状态估计：
Figure BDA0003005782290000093
滤波增益：
Figure BDA0003005782290000094
一步预测均方误差：Pk/k-1＝φk，k-1Pk-1φT k，k-1+Qk；
估计均方误差：Pk＝(I-KkHk)Pk/k-1；
式中，Bk为约束条件矩阵，
Figure BDA0003005782290000095
为一步预测状态量矩阵，
Figure BDA0003005782290000096
为前次状态量矩阵，φk，k-1为状态转移矩阵，
Figure BDA0003005782290000097
为约束修正后预测状态量矩阵，Pk-1为状态误差协方差矩阵，Kk为滤波增益矩阵，Zk为观测量矩阵，Hk为观测系数矩阵，Pk/k-1为一步预测状态误差协方差矩阵，Rk为观测噪声矩阵，Qk为状态噪声矩阵，I为单位矩阵。
最后，利用约束卡尔曼滤波估计出的各节点惯导误差估计值对纯惯导误差补偿。
在本发明实施例中，利用约束卡尔曼滤波估计出的各节点惯导误差估计值
Figure BDA0003005782290000098
对纯惯导误差补偿方法如下：
Figure BDA0003005782290000099
Figure BDA00030057822900000910
Figure BDA00030057822900000911
式中，
Figure BDA00030057822900000912
表示i节点惯导解算高度；
Figure BDA00030057822900000913
表示i节点惯导解算纬度；
Figure BDA00030057822900000914
表示i节点惯导解算经度，
Figure BDA00030057822900000915
为i节点估计出的纬度误差，
Figure BDA00030057822900000916
为i节点估计出的经度误差，
Figure BDA00030057822900000917
为i节点估计出的高度误差。
本发明实施例提供了一种基于约束卡尔曼滤波的集群组网协同导航方法，不依赖其它导航设备，仅利用集群中节点间组网测距，并设计了组网约束条件，基于约束条件构建状态方程和量测方程，设计了新的卡尔曼滤波器，实时递推估计出惯导误差对惯导系统进行修正，抑制各个节点纯惯导误差发散，实现纯惯导高精度定位。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种基于约束卡尔曼滤波的集群组网协同导航方法，其特征在于，包括：
步骤S1：确定集群组网节点数量，并测量各节点间的相对距离；
步骤S2：基于测量的各节点间的相对距离，建立集群组网协同导航滤波方程；
步骤S3：设计集群组网各节点惯导位置误差加权和最小约束条件；
步骤S4：构建集群组网协同导航的约束卡尔曼滤波；
步骤S5：利用约束卡尔曼滤波估计出的各节点惯导误差估计值对纯惯导误差补偿。
2.根据权利要求1所述的基于约束卡尔曼滤波的集群组网协同导航方法，其特征在于，所述步骤S2中的集群组网协同导航滤波方程，包括状态方程，所述状态方程具体为：
X(t)＝F(t)X(t)+G(t)W(t)
式中，
Figure FDA0003005782280000011
Figure FDA0003005782280000012
分别为集群中各个节点的地理坐标系下的三轴位置误差，n为集群节点数量，F(t)为状态转移矩阵，G(t)为系统噪声系数矩阵，W(t)为系统噪声矩阵；
其中，
Figure FDA0003005782280000013
Figure FDA0003005782280000014
式中，Ve i表示i节点东向速度；Li表示i节点纬度；hi表示i节点高度；RN表示地球卯酉圈曲率半径；RM表示地球子午圈曲率半径；Vn i表示i节点北向速度。
3.根据权利要求1所述的基于约束卡尔曼滤波的集群组网协同导航方法，其特征在于，所述步骤S2中的集群组网协同导航滤波方程，还包括测量方程，所述测量方程具体为：
L(t)＝HX(t)+Δ
式中，L(t)是(n-1)×1维观测量矩阵，n-1为观测量数量，即测距值数量，H为(n-1)×3n维观测系数矩阵，Δ是(n-1)×1维观测噪声矩阵；
其中，
Figure FDA0003005782280000021
Figure FDA0003005782280000022
式中，
Figure FDA0003005782280000023
为集群中节点i与节点j间的测量距离，
Figure FDA0003005782280000024
为节点i与节点j间惯导计算距离，L随弹群节点n的数量变化实时变化。
4.根据权利要求3所述的基于约束卡尔曼滤波的集群组网协同导航方法，其特征在于，所述步骤S3中集群组网各节点惯导位置误差加权和最小约束条件，其中，约束条件包括：
GTPxDaX＝0
式中，GT是由HTPH的零特征值对应的特征向量组成的6×3n维矩阵，P为L的对角权阵，HTPH为L的对角权阵的正定矩阵，Px为根据各节点惯导系统精度水平确定的待估参数向量X的3n×3n维权阵，n为集群节点数量，
Figure FDA0003005782280000025
式中，
Figure FDA0003005782280000026
为集群节点i在地心地固系下惯导位置，Px随集群节点n的数量变化实时变化。
5.根据权利要求4所述的基于约束卡尔曼滤波的集群组网协同导航方法，其特征在于，所述步骤S3中集群组网各节点惯导位置误差加权和最小约束条件，地理坐标系下纬经高误差转换至地心地固坐标系下XYZ三轴误差的转换矩阵为：
Figure FDA0003005782280000031
Figure FDA0003005782280000032
式中，Rn表示表示地球卯酉圈曲率半径；
Figure FDA0003005782280000033
表示i节点惯导解算高度；
Figure FDA0003005782280000034
表示i节点惯导解算纬度；
Figure FDA0003005782280000035
表示i节点惯导解算经度；e2表示地球曲率。
6.根据权利要求1所述的基于约束卡尔曼滤波的集群组网协同导航方法，其特征在于，所述步骤S4采用投影法将约束条件投影至传统卡尔曼滤波，构建集群组网协同导航的约束卡尔曼滤波为如下：
约束条件矩阵：Bk＝GTPxDa；
状态一步预测：
Figure FDA0003005782280000036
约束修正后状态一步预测：
Figure FDA0003005782280000037
状态估计：
Figure FDA0003005782280000038
滤波增益：
Figure FDA0003005782280000039
一步预测均方误差：
Figure FDA00030057822800000310
估计均方误差：Pk＝(I-KkHk)Pk/k-1；
式中，Bk为约束条件矩阵，
Figure FDA00030057822800000311
为一步预测状态量矩阵，
Figure FDA00030057822800000312
为前次状态量矩阵，φk，k-1为状态转移矩阵，
Figure FDA00030057822800000313
为约束修正后预测状态量矩阵，Pk-1为状态误差协方差矩阵，Kk为滤波增益矩阵，Zk为观测量矩阵，Hk为观测系数矩阵，Pk/k-1为一步预测状态误差协方差矩阵，Rk为观测噪声矩阵，Qk为状态噪声矩阵，I为单位矩阵。
7.根据权利要求6所述的基于约束卡尔曼滤波的集群组网协同导航方法，其特征在于，所述步骤S5利用约束卡尔曼滤波估计出的各节点惯导误差估计值
Figure FDA00030057822800000314
对纯惯导误差补偿方法如下：
Figure FDA00030057822800000315
Figure FDA0003005782280000041
Figure FDA0003005782280000042
式中，
Figure FDA0003005782280000043
表示i节点惯导解算高度；
Figure FDA0003005782280000044
表示i节点惯导解算纬度；
Figure FDA0003005782280000045
表示i节点惯导解算经度，
Figure FDA0003005782280000046
为i节点估计出的纬度误差，
Figure FDA0003005782280000047
为i节点估计出的经度误差，
Figure FDA0003005782280000048
为i节点估计出的高度误差。
8.一种基于约束卡尔曼滤波的集群组网协同导航系统，其特征在于，包括：
模块M1：确定集群组网节点数量，并测量各节点间的相对距离；
模块M2：基于测量的各节点间的相对距离，建立集群组网协同导航滤波方程；
模块M3：设计集群组网各节点惯导位置误差加权和最小约束条件；
模块M4：构建集群组网协同导航的约束卡尔曼滤波；
模块M5：利用约束卡尔曼滤波估计出的各节点惯导误差估计值对纯惯导误差补偿。
9.根据权利要求8所述的基于约束卡尔曼滤波的集群组网协同导航系统，其特征在于，所述模块M2中的集群组网协同导航滤波方程，包括状态方程，所述状态方程具体为：
X(t)＝F(t)X(t)+G(t)W(t)
式中，
Figure FDA0003005782280000049
Figure FDA00030057822800000410
分别为集群中各个节点的地理坐标系下的三轴位置误差，n为集群节点数量，F(t)为状态转移矩阵，G(t)为系统噪声系数矩阵，W(t)为系统噪声矩阵；
其中，
Figure FDA00030057822800000411
Figure FDA00030057822800000412
式中，Ve i表示i节点东向速度；Li表示i节点纬度；hi表示i节点高度；RN表示地球卯酉圈曲率半径；RM表示地球子午圈曲率半径；Vn i表示i节点北向速度。
10.根据权利要求6所述的基于约束卡尔曼滤波的集群组网协同导航系统，其特征在于，所述模块M2中的集群组网协同导航滤波方程，还包括测量方程，所述测量方程具体为：
L(t)＝HX(t)+Δ
式中，L(t)是(n-1)×1维观测量矩阵，n-1为观测量数量，即测距值数量，H为(n-1)×3n维观测系数矩阵，Δ是(n-1)×1维观测噪声矩阵；
其中，
Figure FDA0003005782280000051
Figure FDA0003005782280000052
式中，
Figure FDA0003005782280000053
为集群中节点i与节点j间的测量距离，
Figure FDA0003005782280000054
为节点i与节点j间惯导计算距离，L随弹群节点n的数量变化实时变化。
CN202110361651.3A 2021-04-02 2021-04-02 基于约束卡尔曼滤波的集群组网协同导航方法及系统 Active CN113175931B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110361651.3A CN113175931B (zh) 2021-04-02 2021-04-02 基于约束卡尔曼滤波的集群组网协同导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110361651.3A CN113175931B (zh) 2021-04-02 2021-04-02 基于约束卡尔曼滤波的集群组网协同导航方法及系统

Publications (2)

Publication Number Publication Date
CN113175931A true CN113175931A (zh) 2021-07-27
CN113175931B CN113175931B (zh) 2022-08-16

Family

ID=76923105

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110361651.3A Active CN113175931B (zh) 2021-04-02 2021-04-02 基于约束卡尔曼滤波的集群组网协同导航方法及系统

Country Status (1)

Country Link
CN (1) CN113175931B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114279466A (zh) * 2021-12-23 2022-04-05 中国电子科技集团公司第十四研究所 一种传感器误差修正方法
CN114964228A (zh) * 2022-05-05 2022-08-30 上海机电工程研究所 基于秩亏约束级联滤波的惯性导航协同方法
CN116147429A (zh) * 2023-03-10 2023-05-23 中国电子科技集团公司第二十六研究所 一种基于数据链和雷达的辅助弹体导航方法
CN116401618A (zh) * 2023-03-03 2023-07-07 南京航空航天大学 基于几何分布采样的跨域无人集群协同导航信息融合方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102901514A (zh) * 2012-09-25 2013-01-30 北京航空航天大学 一种基于多惯组信息约束的协同初始对准方法
CN109471103A (zh) * 2018-10-23 2019-03-15 湖北航天技术研究院总体设计所 一种弹载双基sar数据融合定位误差修正方法
CN109471102A (zh) * 2018-10-23 2019-03-15 湖北航天技术研究院总体设计所 一种惯组误差修正方法
CN110208792A (zh) * 2019-06-26 2019-09-06 哈尔滨工业大学 同时估计目标状态和轨迹参数的任意直线约束跟踪方法
CN110207691A (zh) * 2019-05-08 2019-09-06 南京航空航天大学 一种基于数据链测距的多无人车协同导航方法
CN110285804A (zh) * 2019-06-26 2019-09-27 南京航空航天大学 基于相对运动模型约束的车辆协同导航方法
CN110988880A (zh) * 2019-12-12 2020-04-10 南京莱斯电子设备有限公司 一种基于smr目标轨迹的地理信息提取及目标跟踪方法
CN111221018A (zh) * 2020-03-12 2020-06-02 南京航空航天大学 一种用于抑制海上多路径的gnss多源信息融合导航方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102901514A (zh) * 2012-09-25 2013-01-30 北京航空航天大学 一种基于多惯组信息约束的协同初始对准方法
CN109471103A (zh) * 2018-10-23 2019-03-15 湖北航天技术研究院总体设计所 一种弹载双基sar数据融合定位误差修正方法
CN109471102A (zh) * 2018-10-23 2019-03-15 湖北航天技术研究院总体设计所 一种惯组误差修正方法
CN110207691A (zh) * 2019-05-08 2019-09-06 南京航空航天大学 一种基于数据链测距的多无人车协同导航方法
CN110208792A (zh) * 2019-06-26 2019-09-06 哈尔滨工业大学 同时估计目标状态和轨迹参数的任意直线约束跟踪方法
CN110285804A (zh) * 2019-06-26 2019-09-27 南京航空航天大学 基于相对运动模型约束的车辆协同导航方法
CN110988880A (zh) * 2019-12-12 2020-04-10 南京莱斯电子设备有限公司 一种基于smr目标轨迹的地理信息提取及目标跟踪方法
CN111221018A (zh) * 2020-03-12 2020-06-02 南京航空航天大学 一种用于抑制海上多路径的gnss多源信息融合导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
PEYMAN HADI MOHAMMADABADI: "Cooperative Node Positioning In Vehicular Networks Using Inter-Node Distance Measurements", 《2014 IEEE 25TH INTERNATIONAL SYMPOSIUM ON PERSONAL, INDOOR AND MOBILE RADIO COMMUNICATIONS》 *
刘俊成: "基于相互测距信息的机群组网协同定位技术", 《北 京 航 空 航 天 大 学 学 报》 *
张京娟: "基于飞行器三维组网测距信息的导航误差估计技术研究", 《空天防御》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114279466A (zh) * 2021-12-23 2022-04-05 中国电子科技集团公司第十四研究所 一种传感器误差修正方法
CN114279466B (zh) * 2021-12-23 2024-02-27 中国电子科技集团公司第十四研究所 一种传感器误差修正方法
CN114964228A (zh) * 2022-05-05 2022-08-30 上海机电工程研究所 基于秩亏约束级联滤波的惯性导航协同方法
CN116401618A (zh) * 2023-03-03 2023-07-07 南京航空航天大学 基于几何分布采样的跨域无人集群协同导航信息融合方法
CN116401618B (zh) * 2023-03-03 2023-12-01 南京航空航天大学 基于几何分布采样的跨域无人集群协同导航信息融合方法
CN116147429A (zh) * 2023-03-10 2023-05-23 中国电子科技集团公司第二十六研究所 一种基于数据链和雷达的辅助弹体导航方法
CN116147429B (zh) * 2023-03-10 2024-07-19 中国电子科技集团公司第二十六研究所 一种基于数据链和雷达的辅助弹体导航方法

Also Published As

Publication number Publication date
CN113175931B (zh) 2022-08-16

Similar Documents

Publication Publication Date Title
CN113175931B (zh) 基于约束卡尔曼滤波的集群组网协同导航方法及系统
CN110487301B (zh) 一种雷达辅助机载捷联惯性导航系统初始对准方法
Li et al. A robust graph optimization realization of tightly coupled GNSS/INS integrated navigation system for urban vehicles
US8594927B2 (en) Navigation filter for a navigation system using terrain correlation
US8452536B2 (en) Method of definition of a navigation system
CN111337020A (zh) 引入抗差估计的因子图融合定位方法
CN111156987A (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN102353378B (zh) 一种矢量形式信息分配系数的组合导航系统自适应联邦滤波方法
CN109883426A (zh) 基于因子图的动态分配与校正多源信息融合方法
CN111337029B (zh) 基于自学习多速率残差校正的偏振光惯性严密组合导航方法
CN109631883B (zh) 一种基于节点信息共享的载机局部姿态精确估计方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
US20120203519A1 (en) Method for calculating a navigation phase in a navigation system involving terrain correlation
CN108931799A (zh) 基于递归快速正交搜索的列车组合定位方法及系统
CN114264301B (zh) 车载多传感器融合定位方法、装置、芯片及终端
CN112346104A (zh) 一种无人机信息融合定位方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN115711616B (zh) 一种室内室外穿越无人机的平顺定位方法及装置
CN114689047A (zh) 基于深度学习的组合导航方法、装置、系统及存储介质
CN112525188A (zh) 一种基于联邦滤波的组合导航方法
Wang et al. An adaptive federated filter based on variational Bayes with application to multisource navigation
Taghizadeh et al. A low-cost integrated navigation system based on factor graph nonlinear optimization for autonomous flight
CN117824632A (zh) 基于噪声分布假设的重力匹配二级滤波方法及装置
CN113434806A (zh) 一种抗差自适应多模型滤波方法
CN112683265A (zh) 一种基于快速iss集员滤波的mimu/gps组合导航方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant