CN116519015A - 一种基于相对距离约束的分布式协同导航方法及*** - Google Patents

一种基于相对距离约束的分布式协同导航方法及*** Download PDF

Info

Publication number
CN116519015A
CN116519015A CN202310311639.0A CN202310311639A CN116519015A CN 116519015 A CN116519015 A CN 116519015A CN 202310311639 A CN202310311639 A CN 202310311639A CN 116519015 A CN116519015 A CN 116519015A
Authority
CN
China
Prior art keywords
aircraft
state
cluster
matrix
relative
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.)
Pending
Application number
CN202310311639.0A
Other languages
English (en)
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.)
Nanjing University of Aeronautics and Astronautics
General Designing Institute of Hubei Space Technology Academy
Original Assignee
Nanjing University of Aeronautics and Astronautics
General Designing Institute of Hubei Space Technology Academy
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 Nanjing University of Aeronautics and Astronautics, General Designing Institute of Hubei Space Technology Academy filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202310311639.0A priority Critical patent/CN116519015A/zh
Publication of CN116519015A publication Critical patent/CN116519015A/zh
Pending legal-status Critical Current

Links

Classifications

    • 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
    • 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
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • 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)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Software Systems (AREA)
  • Automation & Control Theory (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • General Engineering & Computer Science (AREA)
  • Operations Research (AREA)
  • Manufacturing & Machinery (AREA)
  • Computing Systems (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于相对距离约束的分布式协同导航方法及***，包括以下步骤：各飞行器搭载惯性传感器和数据链***。各飞行器利用当前时刻惯性传感器输出信息进行捷联惯性解算。当飞行器与集群中某个协同飞行器成功通讯时，将协同飞行器的状态量增广至算法状态量中，通过卡尔曼滤波预测状态观测量和状态协方差矩阵。当飞行器获得与集群中某个协同飞行器的相对距离约束时进入滤波估计程序，构建相对量测方程。通过算法输出结果对两飞行器状态量进行修正。本发明能够分布运行于集群中各个飞行器上，无需中心节点进行导航解算，各个飞行器节点根据自身可获取的量测信息即可进行导航定位。

Description

一种基于相对距离约束的分布式协同导航方法及***
技术领域
本发明属于自主导航与制导技术领域，具体涉及一种基于相对距离约束的分布式协同导航方法及***。
背景技术
实时精确的导航信息是保证飞行器编队能够自主地进行编队飞行队形保持与队形重构的先决条件，飞行器的定位精度将直接影响编队执行任务的成功率。目前针对飞行器常用的组合导航方式为惯导/卫星松组合或紧组合导航，其优点是能够全地域、全天候实时导航，定位误差不随时间累积发散，具有较高的定位和测速精度，已成为目前针对飞行器通用的导航定位方式。然而在复杂环境下，卫星信号容易受到外界的电子干扰，不能提供连续的位置速度信息。因而飞行器编队协同成为研究热点。传统主从式飞行器协同极度依赖主飞行器的高精度位置信息及其播发的信息，若主飞行器遭遇损毁或进入卫星信号拒止区域，从飞行器定位精度将不可避免地受到影响。针对飞行器自主编队协同作战这一特殊的任务背景，研究飞行器编队在卫星间接拒止、全拒止条件下，采用飞行器编队中相互的测量感知手段，通过飞行器编队分布式协同的方式提高飞行器的定位/定姿精度已成为研究重点，提高飞行器在恶劣环境下的生存能力，从而提高飞行器编队执行任务的成功率。
发明内容
本发明旨在解决现有技术的不足，提出了一种基于相对距离约束的分布式协同导航方法及***，可实现在卫星拒止和集群分时通信条件下的集群分布式协同导航。
为实现上述目的，本发明提供了如下方案：
一种基于相对距离约束的分布式协同导航方法，包括以下步骤：
S1：各飞行器搭载惯性传感器和数据链***，其中，所述惯性传感器用于提供位置信息，各飞行器的数据链***以轮询方式与集群中其他飞行器进行相互通讯和相对测距；
S2：各飞行器利用当前时刻惯性传感器的输出信息进行捷联惯性解算，更新飞行器状态信息；
S3：当更新状态信息后的飞行器i与集群中某个协同飞行器j成功通讯时，将对应协同飞行器的状态量增广至算法状态量中，通过卡尔曼滤波预测状态观测量和状态协方差矩阵；
S4：基于所述状态观测量和所述状态协方差矩阵，当飞行器i获得与集群中某个协同飞行器j的相对距离约束时进入滤波估计，构建相对量测方程；
S5：基于所述相对量测方程，通过算法输出结果对两飞行器状态量进行修正。
优选的，所述S1中，所述各飞行器的数据链***以轮询方式与集群中其他飞行器进行相互通讯和相对测距的方法包括：
S11：获取当前飞行器集群中成员结构并对各飞行器成员进行编号；
S12：利用固一轮转编排法编排编号后的飞行器集群通讯编排表；
S13：飞行器集群内各成员按照集群通讯编排表与集群中其他飞行器成员进行相互通讯和相对测距。
优选的，所述S2中，利用当前时刻惯性传感器的输出信息进行捷联惯性解算的方法包括：四元数法姿态预测、速度计算和位置计算；
其中，所述四元数法姿态预测的方法包括：
利用当前时刻飞行器机载传感器导航信息，更新四元数：
式中，k为当前时刻，Q(k)为k时刻的四元数；k-1时刻为当前时刻前一采样时间，Q(k-1)为k-1时刻的四元数；T为矩阵转置，Δt是离散采样周期；ω(k)为解算中间变量，ω(k)定义为：
式中，表示飞行器坐标系相对地理坐标系的角速度在飞行器坐标系轴向的分量构成的列矢量；/>为/>在x,y,z轴向上的分量；
计算转换矩阵
式中，λ0,λ1,λ2,λ3为当前时刻的四元数，Q(k)＝[λ0,λ1,λ2,λ3]；
利用转换矩阵计算姿态角：
式中，θ为俯仰角，γ为横滚角，ψ为航向角，Tij表示为四元数计算姿态矩阵的元素(i,j＝1,2,3)；
其中，所述速度计算的方法包括：
由比力方程得到速度预测公式为：
式中，是vn(k)在x、y、z方向上的分量，vn(k)是k时刻机体系相对导航系的线速度在导航系下的表示；/>是vn(k-1)时刻机体系相对导航系的线速度在导航系下的表示；g为重力加速度；/>是an(k)在x、y、z方向上的分量，an(k)是k时刻机体系相对导航系的线加速度在导航系下的表示；
式中，是k时刻机体系相对导航系的加速度在机体系下的表示；
其中，所述位置计算的方法包括：
式中，xn(k)、yn(k)、zn(k)是k时刻飞行器在导航系下的三个方向位置；xn(k-1)、yn(k-1)、zn(k-1)是前一时刻飞行器在导航系下的三个方向位置。
优选的，所述S3的方法包括：
S31：对单飞行器选取惯导***平台失准角、速度误差、位置误差、陀螺零偏和漂移以及加速度计漂移作为状态量；
S32：在飞行器i通过集群通讯策略与飞行器j建立通讯并获得与飞行器j的相对量测后，飞行器i同时获取飞行器j的状态量信息以及协方差信息，构建自身分布式双状态卡尔曼滤波算法的***状态量和***状态的协方差矩阵，基于所述***状态量和所述协方差矩阵，建立状态方程；
S33：对所述状态方程进行离散化，根据所选***状态量和离散化后的状态方程构建状态一步预测方程和均方误差一步预测方程；
其中，所述S31的计算方法包括：
式中，为飞行器绕各轴方向旋转的平台失准角；δVE、δVN、δVU为飞行器沿东北天方向的速度估计误差；δL、δλ、δh为飞行器的位置估计误差；εbx、εby、εbz为陀螺仪沿各轴的零偏；εrx、εry、εrz为陀螺仪沿各轴的一阶马尔可夫过程随机噪声；/>为加速度计沿各轴的一阶马尔可夫过程随机噪声；
其中，所述S32中，自身分布式双状态卡尔曼滤波算法的***状态量为：
***状态的协方差矩阵为：
式中，Pi,k为飞行器i在k时刻的协方差矩阵，Pj,k为飞行器j在k时刻的协方差矩阵；
建立的状态方程为：
式中Fi为***转移矩阵，
式中FiN为对应9个基本导航参数的***阵，FS为飞行器i惯性仪表状态系数矩阵；
式中，为飞行器i坐标系到地理坐标系的变换矩阵，Tigx、Tigy、Tigz为飞行器i陀螺仪沿各轴的一阶马尔可夫过程相关时间，Tiax、Tiay、Tiaz为飞行器i加速度计沿各轴的一阶马尔可夫过程相关时间；
Gi为飞行器i导航量和惯性仪表误差系数矩阵，W为飞行器i惯性仪表白噪声随机误差矢量；
Wi＝[ωigx ωigy ωigz ωirx ωiry ωirz ωiax ωiay ωiaz]T
式中，ωigx、ωigy、ωigz为飞行器i陀螺仪白噪声，ωirx、ωiry、ωirz为飞行器i陀螺仪一阶马尔可夫过程白噪声，ωiax、ωiay、ωiaz为飞行器i加速度计一阶马尔可夫过程白噪声；
其中，对所述状态方程进行离散化的方法包括：
FL＝I+FI*TD+FI*FI/2.0*TD*TD
GL＝(I+FI/2.0*TD+FI*FI/6.0*TD*TD)*GI*TD
式中，FL为离散化后的状态系数矩阵，I为单位矩阵，TD为离散周期；GL为离散化后的误差噪声矩阵；
其中，所述状态一步预测方程为：
所述均方误差一步预测方程为：
式中，k为当前时刻，为k时刻状态Xk的卡尔曼滤波估计值，/>为利用/>计算得到的对Xk的一步预测，Φk+1,k为k时刻到k+1时刻的状态转移矩阵，为FL，Wk为k时刻***噪声，方差为Qk，/>为状态估值/>的均方误差阵。
优选的，所述S4的方法包括：
S41：当飞行器i获得与集群中某个协同飞行器j的相对测距信息时，将获取的距离信息进行滤波估计；
S42：基于滤波估计后的距离信息，计算飞行器i与集群中某个协同飞行器j的伪距将伪距/>在飞行器i位置真值(xi,yi,zi)处泰勒展开并仅取一阶项；通过飞行器数据链***测得飞行器i与集群中某个协同飞行器j间的相对伪距/>将取一阶项后的伪距/>与相对伪距/>相减并改写成矩阵形式，基于地理坐标系与地心地固坐标系之间的转换关系和所述矩阵形式，得出飞行器i与集群中某个协同飞行器j间相对伪距量测方程；
其中，所述伪距的表达式为：
式中(xsi,ysi,zsi)、(xsj,ysj,zsj)为飞行器i和飞行器j惯性传感器解算位置在地心地固坐标系下的坐标，
其中，将伪距在飞行器i位置真值(xi,yi,zi)处泰勒展开并仅取一阶项后的表达式为：/>
其中，所述相对伪距表达式为：
式中ρij为飞行器i与集群中某个协同飞行器j间的相对距离真值，vij为数据链相对测距白噪声，
所述矩阵形式为：
式中ei1,ei2,ei3为泰勒展开过程中在真值(xi,yi,zi)处的雅可比矩阵，ej1,ej2,ej3为泰勒展开过程中在真值(xj,yj,zj)处的雅可比矩阵，
所述相对伪距量测方程伪：
Zρ＝δρ＝Hρ(t)X(t)+Vρ(t)
式中Hρ＝[O1×6 Hρi O1×9 O1×6 -Hρj O1×9]；X(t)为***状态量；Vρ(t)为***相对伪距量测噪声；其中Hρi＝[ai1 ai2 ai3]，ai1,ai2,ai3可分别表示为：
ai3＝ei1cosLicosλi+ei2cosLisinλi+ei3sinLi
Hρj＝[aj1 aj2 aj3]，aj1,aj2,aj3可分别表示为：
aj3＝ej1 cosLj cosλj+ej2 cosLj sinλj+ej3 sinLj
式中(λi,Li,hi),(λj,Lj,hj)分别为飞行器i,j当前经度、纬度和高度，为对应卯酉圈地球曲率半径。
优选的，所述S5的具体方法为：通过输出融合解算结果，对进行通信的两飞行器状态进行修正，并将状态中的惯性参数估计值带入下一次惯性递推，对惯性误差进行修正。
优选的，所述S5中的算法分布运行于集群中各个飞行器上，无需中心节点进行导航解算，各个飞行器节点根据自身可获取的量测信息进行导航定位。
本发明还提供了一种基于相对距离约束的分布式协同导航***，包括：搭载模块、解算模块、预测模块、构建模块和修正模块；
所述搭载模块用于各飞行器搭载惯性传感器和数据链***，其中，所述惯性传感器用于提供位置信息，各飞行器的数据链***以轮询方式与集群中其他飞行器进行相互通讯和相对测距；
所述解算模块用于各飞行器利用当前时刻惯性传感器的输出信息进行捷联惯性解算，更新飞行器状态信息；
所述预测模块用于当更新状态信息后的飞行器i与集群中某个协同飞行器j成功通讯时，将对应协同飞行器的状态量增广至算法状态量中，通过卡尔曼滤波预测状态观测量和状态协方差矩阵；
所述构建模块用于基于所述状态观测量和所述状态协方差矩阵，当飞行器i获得与集群中某个协同飞行器j的相对距离约束时进入滤波估计，构建相对量测方程；
所述修正模块用于基于所述相对量测方程，通过算法输出结果对两飞行器状态量进行修正。
与现有技术相比，本发明的有益效果为：
本发明公开了一种基于相对距离约束的分布式协同导航方法及***，设计了一种集群分时通讯策略，各飞行器按照集群通讯编排进行相互通讯和相对测距，各飞行器在与协同飞行器通讯与测距后将相应协同飞行器的状态拓展至自身卡尔曼滤波器状态量中，利用相对距离约束对两飞行器的状态量进行修正同时对惯性传感器参数进行估计，并将估计参数带入下次惯性递推。本发明可以实现飞行器集群在卫星拒止和分时通讯条件下分布式协同导航，能够分布运行于集群中各个飞行器上，无需中心节点进行导航解算，对各飞行器惯性传感器给出的飞行器状态量进行修正，各个飞行器节点根据自身可获取的量测信息即可进行导航定位。
附图说明
为了更清楚地说明本发明的技术方案，下面对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明的一种基于相对距离约束的分布式协同导航方法流程示意图；
图2为本发明的集群通讯策略示意图；
图3为本发明的集群中低精度节点1三维定位误差对比图；
图4为本发明的集群中低精度节点2三维定位误差对比图；
图5为本发明的集群中低精度节点3三维定位误差对比图；
图6为本发明的集群中低精度节点4三维定位误差对比图；
图7为本发明的集群中低精度节点5三维定位误差对比图；
图8为本发明的集群中低精度节点6三维定位误差对比图；
图9为本发明的集群中低精度节点7三维定位误差对比图；
图10为本发明的集群中低精度节点8三维定位误差对比图；
图11为本发明的集群中低精度节点9三维定位误差对比图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例一
如图1所示，本发明公开了一种基于相对距离约束的分布式协同导航方法，
具体包括以下步骤：
S1、各飞行器搭载惯性传感器和数据链***，惯性传感器提供位置信息，各飞行器的数据链***以轮询方式与集群中其他飞行器进行相互通讯和相对测距。参照图2所示，本发明提供了一种集群分时通讯方案，具体包括S11、S12、S13步骤：
S11、获取当前飞行器集群中成员结构并对各飞行器成员进行编号。
S12、利用固一轮转编排法编排飞行器集群通讯编排表。
S13、飞行器集群内各成员按照集群通讯编排表与集群中其他飞行器成员进行相互通讯和相对测距。
S2、各飞行器利用当前时刻惯性传感器输出信息进行捷联惯性解算。具体包括以下子步骤：
四元数法姿态预测：
利用当前时刻飞行器机载传感器导航信息，更新四元数：
其中，k为当前时刻，Q(k)为k时刻的四元数；k-1时刻为当前时刻前一采样时间，Q(k-1)为k-1时刻的四元数；T为矩阵转置，Δt是离散采样周期；ω(k)为解算中间变量，ω(k)定义为以下公式计算：
其中，表示飞行器坐标系相对地理坐标系的角速度在飞行器坐标系轴向的分量构成的列矢量；/>为/>在x,y,z轴向上的分量。
计算转换矩阵如下：
其中λ0,λ1,λ2,λ3为当前时刻的四元数，Q(k)＝[λ0,λ1,λ2,λ3]；
利用转换矩阵计算姿态角如下：
其中θ为俯仰角，γ为横滚角，ψ为航向角。Tij表示为四元数计算姿态矩阵的元素(i,j＝1,2,3)。
速度计算：
由比力方程得到速度预测公式为：
其中，是vn(k)在x、y、z方向上的分量，vn(k)是k时刻机体系相对导航系的线速度在导航系下的表示；/>是vn(k-1)时刻机体系相对导航系的线速度在导航系下的表示；g为重力加速度；/>是an(k)在x、y、z方向上的分量，an(k)是k时刻机体系相对导航系的线加速度在导航系下的表示。
其中，是k时刻机体系相对导航系的加速度在机体系下的表示。
位置计算：
其中，xn(k)、yn(k)、zn(k)是k时刻飞行器在导航系下的三个方向位置；xn(k-1)、yn(k-1)、zn(k-1)是前一时刻飞行器在导航系下的三个方向位置；
S3、当飞行器i与集群中某个协同飞行器j成功通讯时，将协同飞行器的状态量增广至算法状态量中，通过卡尔曼滤波预测状态观测量和状态协方差矩阵。具体包括以下子步骤：
S31、对单飞行器选取惯导***平台失准角、速度误差、位置误差、陀螺零偏和漂移以及加速度计漂移作为状态量，即
其中，为飞行器绕各轴方向旋转的平台失准角；δVE、δVN、δVU为飞行器沿东北天方向的速度估计误差；δL、δλ、δh为飞行器的位置(经度、纬度、高度)估计误差；εbx、εby、εbz为陀螺仪沿各轴的零偏；εrx、εry、εrz为陀螺仪沿各轴的一阶马尔可夫过程随机噪声；为加速度计沿各轴的一阶马尔可夫过程随机噪声；
S32、在飞行器i通过集群通讯策略与飞行器j建立通讯并获得与飞行器j的相对量测后，飞行器i同时获取飞行器j的状态量信息以及协方差信息，构建自身分布式双状态卡尔曼滤波算法的***状态量为
***状态的协方差矩阵可表示为
其中，Pi,k为飞行器i在k时刻的协方差矩阵，Pj,k为飞行器j在k时刻的协方差矩阵。
建立状态方程为
其中Fi为***转移矩阵，可表示为
其中FiN为对应9个基本导航参数的***阵，FS为飞行器i惯性仪表状态系数矩阵；
其中，为飞行器i坐标系到地理坐标系的变换矩阵，Tigx、Tigy、Tigz为飞行器i陀螺仪沿各轴的一阶马尔可夫过程相关时间，Tiax、Tiay、Tiaz为飞行器i加速度计沿各轴的一阶马尔可夫过程相关时间。
Gi为飞行器i导航量和惯性仪表误差系数矩阵，W为飞行器i惯性仪表白噪声随机误差矢量。
Wi＝[ωigx ωigy ωigz ωirx ωiry ωirz ωiax ωiay ωiaz]T
其中，ωigx、ωigy、ωigz为飞行器i陀螺仪白噪声，ωirx、ωiry、ωirz为飞行器i陀螺仪一阶马尔可夫过程白噪声，ωiax、ωiay、ωiaz为飞行器i加速度计一阶马尔可夫过程白噪声。
S33、对状态方程进行离散化：
FL＝I+FI*TD+FI*FI/2.0*TD*TD
GL＝(I+FI/2.0*TD+FI*FI/6.0*TD*TD)*GI*TD
其中，FL为离散化后的状态系数矩阵，I为单位矩阵，TD为离散周期；GL为离散化后的误差噪声矩阵；
根据所选***状态量构建状态一步预测方程：
均方误差一步预测方程为：
其中，k为当前时刻，为k时刻状态Xk的卡尔曼滤波估计值，/>为利用/>计算得到的对Xk的一步预测。Φk+1,k为k时刻到k+1时刻的状态转移矩阵，即为FL。Wk为k时刻***噪声，其方差为Qk。/>为状态估值/>的均方误差阵。
S4、当飞行器i获得与集群中某个协同飞行器j的相对距离约束时进入滤波估计程序，构建相对量测方程。具体包括以下子步骤：
S41、当飞行器i获得与集群中某个协同飞行器j的相对测距信息时，将获取的距离信息输入滤波估计程序。
S42、飞行器i与集群中某个协同飞行器j的计算伪距可表示为
其中(xsi,ysi,zsi)、(xsj,ysj,zsj)为飞行器i和飞行器j惯性传感器解算位置在地心地固坐标系下的坐标。将上式在飞行器i位置真值(xi,yi,zi)处泰勒展开并仅取一阶项可得：
通过飞行器数据链***测得飞行器i与集群中某个协同飞行器j间的相对伪距可表示为
其中ρij为飞行器i与集群中某个协同飞行器j间的相对距离真值，vij为数据链相对测距白噪声。
将与/>相减并改写成矩阵形式为
式中ei1,ei2,ei3为泰勒展开过程中在真值(xi,yi,zi)处的雅可比矩阵，ej1,ej2,ej3为泰勒展开过程中在真值(xj,yj,zj)处的雅可比矩阵。考虑到地理坐标系与地心地固坐标系之间的转换关系可得出飞行器i与集群中某个协同飞行器j间相对伪距量测方程：.
Zρ＝δρ＝Hρ(t)X(t)+Vρ(t)
其中Hρ＝[O1×6 Hρi O1×9 O1×6 -Hρj O1×9]；X(t)为***状态量；Vρ(t)为***相对伪距量测噪声；其中Hρi＝[ai1 ai2 ai3]，ai1,ai2,ai3可分别表示为
ai3＝ei1cosLicosλi+ei2cosLisinλi+ei3sinLi
Hρj＝[aj1 aj2 aj3]，aj1,aj2,aj3可分别表示为
aj3＝ej1 cosLj cosλj+ej2 cosLj sinλj+ej3 sinLj
式中(λi,Li,hi),(λj,Lj,hj)分别为飞行器i,j当前经度、纬度和高度，为对应卯酉圈地球曲率半径。
S43、***滤波增益方程如下：
其中Kk是k时刻***的滤波增益，Rρ为量测噪声，Hρ为量测矩阵。
***状态更新方程可表示为
***状态的协方差矩阵更新方程可表示为
其中为k+1时刻估计均方误差。
S5、通过算法输出结果对两飞行器状态量进行修正。具体包括通过输出融合解算结果，对进行通信的两飞行器状态进行修正，并将状态中的惯性参数估计值带入下一次惯性递推，对惯性误差进行修正。
仿真测试：
设置飞行器集群中有1个高精度节点搭载高精度惯性传感器，9个低精度节点搭载低精度惯性传感器，飞行器集群飞行于卫星全拒止区域，其中某低精度飞行器三向定位误差对比图和三维定位误差对比图如图3至图11所示，相较纯惯性导航终点定位精度平均提高了52.34％。
综上，本发明在卫星拒止和集群分时通讯条件下提出了一种集群轮询通讯策略，利用扩展卡尔曼滤波器和飞行器节点之间的相对距离约束实现了飞行器分布式协同导航定位，提高了导航定位精度。
实施例二
本发明还提供了一种基于相对距离约束的分布式协同导航***，包括：搭载模块、解算模块、预测模块、构建模块和修正模块；
搭载模块用于各飞行器搭载惯性传感器和数据链***，其中，惯性传感器用于提供位置信息，各飞行器的数据链***以轮询方式与集群中其他飞行器进行相互通讯和相对测距；
解算模块用于各飞行器利用当前时刻惯性传感器的输出信息进行捷联惯性解算，更新飞行器状态信息；
预测模块用于当更新状态信息后的飞行器i与集群中某个协同飞行器j成功通讯时，将对应协同飞行器的状态量增广至算法状态量中，通过卡尔曼滤波预测状态观测量和状态协方差矩阵；
构建模块用于基于状态观测量和状态协方差矩阵，当飞行器i获得与集群中某个协同飞行器j的相对距离约束时进入滤波估计，构建相对量测方程；
修正模块用于基于相对量测方程，通过算法输出结果对两飞行器状态量进行修正。
以上所述的实施例仅是对本发明优选方式进行的描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (8)

1.一种基于相对距离约束的分布式协同导航方法，其特征在于，包括以下步骤：
S1：各飞行器搭载惯性传感器和数据链***，其中，所述惯性传感器用于提供位置信息，各飞行器的数据链***以轮询方式与集群中其他飞行器进行相互通讯和相对测距；
S2：各飞行器利用当前时刻惯性传感器的输出信息进行捷联惯性解算，更新飞行器状态信息；
S3：当更新状态信息后的飞行器i与集群中某个协同飞行器j成功通讯时，将对应协同飞行器的状态量增广至算法状态量中，通过卡尔曼滤波预测状态观测量和状态协方差矩阵；
S4：基于所述状态观测量和所述状态协方差矩阵，当飞行器i获得与集群中某个协同飞行器j的相对距离约束时进入滤波估计，构建相对量测方程；
S5：基于所述相对量测方程，通过算法输出结果对两飞行器状态量进行修正。
2.根据权利要求1所述的基于相对距离约束的分布式协同导航方法，其特征在于，所述S1中，所述各飞行器的数据链***以轮询方式与集群中其他飞行器进行相互通讯和相对测距的方法包括：
S11：获取当前飞行器集群中成员结构并对各飞行器成员进行编号；
S12：利用固一轮转编排法编排编号后的飞行器集群通讯编排表；
S13：飞行器集群内各成员按照集群通讯编排表与集群中其他飞行器成员进行相互通讯和相对测距。
3.根据权利要求1所述的基于相对距离约束的分布式协同导航方法，其特征在于，所述S2中，利用当前时刻惯性传感器的输出信息进行捷联惯性解算的方法包括：四元数法姿态预测、速度计算和位置计算；
其中，所述四元数法姿态预测的方法包括：
利用当前时刻飞行器机载传感器导航信息，更新四元数：
式中，k为当前时刻，Q(k)为k时刻的四元数；k-1时刻为当前时刻前一采样时间，Q(k-1)为k-1时刻的四元数；T为矩阵转置，Δt是离散采样周期；ω(k)为解算中间变量，ω(k)定义为：
式中，表示飞行器坐标系相对地理坐标系的角速度在飞行器坐标系轴向的分量构成的列矢量；/>为/>在x,y,z轴向上的分量；
计算转换矩阵
式中，λ0,λ1,λ2,λ3为当前时刻的四元数，Q(k)＝[λ0,λ1,λ2,λ3]；
利用转换矩阵计算姿态角：
式中，θ为俯仰角，γ为横滚角，ψ为航向角，Tij表示为四元数计算姿态矩阵的元素(i,j＝1,2,3)；
其中，所述速度计算的方法包括：
由比力方程得到速度预测公式为：
式中，是vn(k)在x、y、z方向上的分量，vn(k)是k时刻机体系相对导航系的线速度在导航系下的表示；/>是vn(k-1)时刻机体系相对导航系的线速度在导航系下的表示；g为重力加速度；/>是an(k)在x、y、z方向上的分量，an(k)是k时刻机体系相对导航系的线加速度在导航系下的表示；
式中，是k时刻机体系相对导航系的加速度在机体系下的表示；
其中，所述位置计算的方法包括：
式中，xn(k)、yn(k)、zn(k)是k时刻飞行器在导航系下的三个方向位置；xn(k-1)、yn(k-1)、zn(k-1)是前一时刻飞行器在导航系下的三个方向位置。
4.根据权利要求1所述的基于相对距离约束的分布式协同导航方法，其特征在于，所述S3的方法包括：
S31：对单飞行器选取惯导***平台失准角、速度误差、位置误差、陀螺零偏和漂移以及加速度计漂移作为状态量；
S32：在飞行器i通过集群通讯策略与飞行器j建立通讯并获得与飞行器j的相对量测后，飞行器i同时获取飞行器j的状态量信息以及协方差信息，构建自身分布式双状态卡尔曼滤波算法的***状态量和***状态的协方差矩阵，基于所述***状态量和所述协方差矩阵，建立状态方程；
S33：对所述状态方程进行离散化，根据所选***状态量和离散化后的状态方程构建状态一步预测方程和均方误差一步预测方程；
其中，所述S31的计算方法包括：
式中，为飞行器绕各轴方向旋转的平台失准角；δVE、δVN、δVU为飞行器沿东北天方向的速度估计误差；δL、δλ、δh为飞行器的位置估计误差；εbx、εby、εbz为陀螺仪沿各轴的零偏；εrx、εry、εrz为陀螺仪沿各轴的一阶马尔可夫过程随机噪声；/>为加速度计沿各轴的一阶马尔可夫过程随机噪声；
其中，所述S32中，自身分布式双状态卡尔曼滤波算法的***状态量为：
***状态的协方差矩阵为：
式中，Pi,k为飞行器i在k时刻的协方差矩阵，Pj,k为飞行器j在k时刻的协方差矩阵；
建立的状态方程为：
式中Fi为***转移矩阵，
式中FiN为对应9个基本导航参数的***阵，FS为飞行器i惯性仪表状态系数矩阵；
式中，为飞行器i坐标系到地理坐标系的变换矩阵，Tigx、Tigy、Tigz为飞行器i陀螺仪沿各轴的一阶马尔可夫过程相关时间，Tiax、Tiay、Tiaz为飞行器i加速度计沿各轴的一阶马尔可夫过程相关时间；
Gi为飞行器i导航量和惯性仪表误差系数矩阵，W为飞行器i惯性仪表白噪声随机误差矢量；
Wi＝[ωigx ωigy ωigz ωirx ωiry ωirz ωiax ωiay ωiaz]T
式中，ωigx、ωigy、ωigz为飞行器i陀螺仪白噪声，ωirx、ωiry、ωirz为飞行器i陀螺仪一阶马尔可夫过程白噪声，ωiax、ωiay、ωiaz为飞行器i加速度计一阶马尔可夫过程白噪声；
其中，对所述状态方程进行离散化的方法包括：
FL＝I+FI*TD+FI*FI/2.0*TD*TD
GL＝(I+FI/2.0*TD+FI*FI/6.0*TD*TD)*GI*TD
式中，FL为离散化后的状态系数矩阵，I为单位矩阵，TD为离散周期；GL为离散化后的误差噪声矩阵；
其中，所述状态一步预测方程为：
所述均方误差一步预测方程为：
式中，k为当前时刻，为k时刻状态Xk的卡尔曼滤波估计值，/>为利用/>计算得到的对Xk的一步预测，Φk+1,k为k时刻到k+1时刻的状态转移矩阵，为FL，Wk为k时刻***噪声，方差为Qk，/>为状态估值/>的均方误差阵。
5.根据权利要求1所述的基于相对距离约束的分布式协同导航方法，其特征在于，所述S4的方法包括：
S41：当飞行器i获得与集群中某个协同飞行器j的相对测距信息时，将获取的距离信息进行滤波估计；
S42：基于滤波估计后的距离信息，计算飞行器i与集群中某个协同飞行器j的伪距将伪距/>在飞行器i位置真值(xi,yi,zi)处泰勒展开并仅取一阶项；通过飞行器数据链***测得飞行器i与集群中某个协同飞行器j间的相对伪距/>将取一阶项后的伪距/>与相对伪距/>相减并改写成矩阵形式，基于地理坐标系与地心地固坐标系之间的转换关系和所述矩阵形式，得出飞行器i与集群中某个协同飞行器j间相对伪距量测方程；
其中，所述伪距的表达式为：
式中(xsi,ysi,zsi)、(xsj,ysj,zsj)为飞行器i和飞行器j惯性传感器解算位置在地心地固坐标系下的坐标，
其中，将伪距在飞行器i位置真值(xi,yi,zi)处泰勒展开并仅取一阶项后的表达式为：/>
其中，所述相对伪距表达式为：
式中ρij为飞行器i与集群中某个协同飞行器j间的相对距离真值，vij为数据链相对测距白噪声，
所述矩阵形式为：
式中ei1,ei2,ei3为泰勒展开过程中在真值(xi,yi,zi)处的雅可比矩阵，ej1,ej2,ej3为泰勒展开过程中在真值(xj,yj,zj)处的雅可比矩阵，
所述相对伪距量测方程伪：
Zρ＝δρ＝Hρ(t)X(t)+Vρ(t)
式中Hρ＝[O1×6 Hρi O1×9 O1×6-Hρj O1×9]；X(t)为***状态量；Vρ(t)为***相对伪距量测噪声；其中Hρi＝[ai1 ai2 ai3]，ai1,ai2,ai3可分别表示为：
ai1＝(RNi+hi)[-ei1sinLicosλi-ei2sinLisinλi]+[RNi(1-f)2+hi]ei3cosLi
ai2＝(RNi+hi)[ei2cosLicosλi-ei1cosLisinλi]
ai3＝ei1cosLicosλi+ei2cosLisinλi+ei3sinLi
Hρj＝[aj1 aj2 aj3]，aj1,aj2,aj3可分别表示为：
aj1＝(RNj+hj)[-ej1sinLjcosλj-ej2sinLjsinλj]+[RNj(1-f)2+hj]ej3cosLj
aj2＝(RNj+hj)[ej2cosLjcosλj-ej1cosLjsinλj]
aj3＝ej1cosLjcosλj+ej2cosLjsinλj+ej3sinLj
式中(λi,Li,hi),(λj,Lj,hj)分别为飞行器i,j当前经度、纬度和高度，为对应卯酉圈地球曲率半径。
6.根据权利要求1所述的基于相对距离约束的分布式协同导航方法，其特征在于，所述S5的具体方法为：通过输出融合解算结果，对进行通信的两飞行器状态进行修正，并将状态中的惯性参数估计值带入下一次惯性递推，对惯性误差进行修正。
7.根据权利要求1所述的基于相对距离约束的分布式协同导航方法，其特征在于，所述S5中的算法分布运行于集群中各个飞行器上，无需中心节点进行导航解算，各个飞行器节点根据自身可获取的量测信息进行导航定位。
8.一种基于相对距离约束的分布式协同导航***，其特征在于，包括：搭载模块、解算模块、预测模块、构建模块和修正模块；
所述搭载模块用于各飞行器搭载惯性传感器和数据链***，其中，所述惯性传感器用于提供位置信息，各飞行器的数据链***以轮询方式与集群中其他飞行器进行相互通讯和相对测距；
所述解算模块用于各飞行器利用当前时刻惯性传感器的输出信息进行捷联惯性解算，更新飞行器状态信息；
所述预测模块用于当更新状态信息后的飞行器i与集群中某个协同飞行器j成功通讯时，将对应协同飞行器的状态量增广至算法状态量中，通过卡尔曼滤波预测状态观测量和状态协方差矩阵；
所述构建模块用于基于所述状态观测量和所述状态协方差矩阵，当飞行器i获得与集群中某个协同飞行器j的相对距离约束时进入滤波估计，构建相对量测方程；
所述修正模块用于基于所述相对量测方程，通过算法输出结果对两飞行器状态量进行修正。
CN202310311639.0A 2023-03-28 2023-03-28 一种基于相对距离约束的分布式协同导航方法及*** Pending CN116519015A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310311639.0A CN116519015A (zh) 2023-03-28 2023-03-28 一种基于相对距离约束的分布式协同导航方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310311639.0A CN116519015A (zh) 2023-03-28 2023-03-28 一种基于相对距离约束的分布式协同导航方法及***

Publications (1)

Publication Number Publication Date
CN116519015A true CN116519015A (zh) 2023-08-01

Family

ID=87389412

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310311639.0A Pending CN116519015A (zh) 2023-03-28 2023-03-28 一种基于相对距离约束的分布式协同导航方法及***

Country Status (1)

Country Link
CN (1) CN116519015A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118129765A (zh) * 2024-05-10 2024-06-04 济南大学 卫星导航拒止下飞行器集群协同定位方法、设备及产品

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118129765A (zh) * 2024-05-10 2024-06-04 济南大学 卫星导航拒止下飞行器集群协同定位方法、设备及产品

Similar Documents

Publication Publication Date Title
CN108180925B (zh) 一种里程计辅助车载动态对准方法
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航***的量测误差补偿方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
De Agostino et al. Performances comparison of different MEMS-based IMUs
CA3003298A1 (en) Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter
CN111024070A (zh) 一种基于航向自观测的惯性足绑式行人定位方法
CN110440830B (zh) 动基座下车载捷联惯导***自对准方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN112504275B (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航***及方法
CN110243377B (zh) 一种基于分层式结构的集群飞行器协同导航方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN111121766A (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN112880669A (zh) 一种航天器星光折射和单轴旋转调制惯性组合导航方法
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN116007620A (zh) 一种组合导航滤波方法、***、电子设备及存储介质
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN112525204B (zh) 一种航天器惯性和太阳多普勒速度组合导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN113503872A (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及***
CN111337056A (zh) 基于优化的LiDAR运动补偿位置姿态***对准方法

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