CN112325878A - 基于ukf与空中无人机节点辅助的地面载体组合导航方法 - Google Patents

基于ukf与空中无人机节点辅助的地面载体组合导航方法 Download PDF

Info

Publication number
CN112325878A
CN112325878A CN202011188871.2A CN202011188871A CN112325878A CN 112325878 A CN112325878 A CN 112325878A CN 202011188871 A CN202011188871 A CN 202011188871A CN 112325878 A CN112325878 A CN 112325878A
Authority
CN
China
Prior art keywords
error
unmanned aerial
aerial vehicle
state
ukf
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
CN202011188871.2A
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
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202011188871.2A priority Critical patent/CN112325878A/zh
Publication of CN112325878A publication Critical patent/CN112325878A/zh
Pending legal-status Critical Current

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/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
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/06—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/10—Position of receiver fixed by co-ordinating a plurality of position lines defined by path-difference measurements, e.g. omega or decca systems

Landscapes

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

Abstract

本发明公开了一种基于UKF与空中无人机节点辅助的地面载体组合导航方法，属于惯性/空中无人机节点辅助组合导航领域。该方法首先以惯导误差方程建立组合导航系统的非线性状态方程，将惯性传感器误差扩展为系统状态变量，包括陀螺的随机游走误差、陀螺仪的白噪声和加速度计的随机游走误差；随后，将空中无人机辅助节点的位置和与地面载体的相对距离作为量测数据进而构建量测方程；最后在地面载体运动过程中对导航误差进行实时反馈校正，获得误差校正后的惯性导航系统导航结果。本发明能够在载体运动过程中有效利用空中无人机辅助节点提供的位置信息和相对距离信息,实现与惯性导航的有效融合，提高组合导航系统的精度和可靠性，适用于工程应用。

Description

基于UKF与空中无人机节点辅助的地面载体组合导航方法
技术领域
本发明涉及一种基于UKF(无迹卡尔曼滤波)与空中无人机节点辅助的地面载体组合导航方法，属于惯性/空中无人机节点辅助组合导航领域。
背景技术
在现有技术下，广泛应用于地面载体的导航系统大多采用惯性器件(IMU-加速度计和陀螺仪)/卫星组合导航系统实现。但载体在地面执行任务时，卫星信号容易受到建筑物等的遮挡，从而使卫星导航系统失效。无人机作为一种有动力、操控灵活的飞行器，可以加载多种机载设备，在卫星信号不受遮挡的天空飞行，通过确定位置的无人机来对卫星信号易受遮挡的地面载体进行无线测距，从而利用测距信息对地面载体辅助定位，其具有结构简单、成本低等优点，弥补了地面载体在复杂环境下执行任务时卫星信号易受干扰的不足，可最大限度满足地面载体的导航需求。
惯性导航系统的误差主要由惯性传感器(IMU-加速度计和陀螺仪)测量误差引起，惯性导航系统的误差会随时间累积。通过无人机辅助节点对地面载体无线测距来来获得载体的位置，其误差不会随时间发散。传统的多点定位方法是通过牛顿迭代法对地面载体进行位置解算，这种方法在多无人机节点GDOP(几何精度因子)较大时定位误差很大，甚至会出现无法迭代的情况。通过卡尔曼滤波将惯性导航系统信息和无人机辅助节点测距信息有效融合，既可以弥补惯性导航系统定位误差逐渐发散的缺点，又可以避免出现在无人机GDOP较大时无法进行位置解算的情况，但由于测距信息与惯导信息进行组合时，其量测方程是非线性的，因此研究基于UKF和空中多无人机节点辅助的地面载体组合导航方法，从而提高导航系统的准确性，将具有重要的研究意义。
发明内容
本发明提出了一种基于UKF与空中无人机节点辅助的地面载体组合导航方法，在载体运动过程中有效利用空中多无人机节点的无线测距信息,抑制地面载体的捷联惯性导航系统随时间的累积误差发散，实现与惯性导航的最优融合，显著提高了组合导航系统的精准性和可靠性。
本发明为解决上述技术问题采用以下技术方案：
一种基于UKF与空中多无人机节点辅助的地面载体组合导航方法，包括如下步骤：
步骤1，建立组合导航系统的状态方程，将惯性传感器误差扩展为系统状态变量，包括陀螺的随机游走过程、随机游走驱动白噪声、陀螺仪的白噪声和加速度计的随机游走过程和随机游走驱动白噪声；
步骤2，结合空中无人机辅助节点提供的位置和无线测距信息，构建基于UKF的卡尔曼滤波器量测方程；
步骤3，计算状态向量的σ样本点，从而计算一步预测模型值，实现时间更新；
步骤4，到达滤波周期，读取空中无人机辅助节点位置和测距信息，进行UKF量测更新，并采用UKF滤波对系统状态量进行反馈校正，实现对组合导航系统的有效修正。
作为本发明的一种优选方案，步骤1所述系统的状态向量为：
Figure BDA0002752151980000021
其中，X为姿态误差、速度误差、位置误差、IMU随机游走组成的15维状态向量，W为系统白噪声，Xa为将系统噪声增广后的状态向量，
Figure BDA0002752151980000022
为捷联惯导系统中的数学平台误差角，
Figure BDA0002752151980000023
分别为x、y、z的三轴数学平台误差角，δvn为载体的速度误差，
Figure BDA0002752151980000024
为东北天三轴的速度误差，δpn为载体的位置误差，δL、δλ、δh分别为经度、纬度、高度误差，ωb为陀螺仪的随机游走误差，
Figure BDA0002752151980000025
分别为陀螺仪x、y、z轴的随机游走误差，
Figure BDA0002752151980000026
为陀螺仪的随机游走驱动噪声，
Figure BDA0002752151980000027
分别为陀螺仪x、y、z轴的随机游走驱动噪声，
Figure BDA0002752151980000031
为陀螺仪的输出白噪声，
Figure BDA0002752151980000032
分别为陀螺仪x、y、z轴的输出白噪声，
Figure BDA0002752151980000033
为加速度计随机游走驱动噪声，
Figure BDA0002752151980000034
为加速度计的随机游走误差，
Figure BDA0002752151980000035
分别为加速度计x、y、z轴的随机游走误差，
Figure BDA0002752151980000036
分别为加速度计x、y、z轴的随机游走驱动噪声。上标n表示导航坐标系(地理系)，上标b表示载体系。
所述系统的状态方程为：
Figure BDA0002752151980000037
其中，
Figure BDA0002752151980000038
为状态量的一阶导数，
Figure BDA0002752151980000039
为数学平台误差角的一阶导数，
Figure BDA00027521519800000310
为载体速度误差的一阶导数，
Figure BDA00027521519800000311
分别为经度、纬度、高度误差的一阶导数，
Figure BDA00027521519800000312
为陀螺仪随机游走误差的一阶导数，
Figure BDA00027521519800000313
为加速度计随机游走误差的一阶导数，
Figure BDA00027521519800000314
为陀螺仪随机游走驱动噪声的一阶导数，
Figure BDA00027521519800000315
为陀螺仪的输出白噪声，
Figure BDA00027521519800000316
为加速度计随机游走驱动噪声的一阶导数，εb为陀螺漂移误差模型中的随机常数，vn为载体的速度，L为经度，h为载体的及时高度，
Figure BDA00027521519800000317
为地理系相对于惯性系的角速率误差，
Figure BDA00027521519800000318
为地理系相对于惯性系的角速率，
Figure BDA00027521519800000319
表示从载体系到地理系的坐标变换矩阵，fn为地理系下加速度计的输出比力，
Figure BDA00027521519800000320
为地球系相对于惯性系的角速率误差，
Figure BDA00027521519800000321
为地理系相对于地球系的角速率误差，
Figure BDA00027521519800000322
为地球系相对于惯性系的角速率，
Figure BDA00027521519800000323
为地理系相对于地球系的角速率，
Figure BDA00027521519800000324
为重力加速度误差，Rm为卯酉圈半径，Rn为子午圈半径，
Figure BDA0002752151980000041
作为本发明的一种优选方案，步骤2所述基于UKF的卡尔曼滤波器量测方程为：
Figure BDA0002752151980000042
其中，x1、y1、z1为第1个空中无人机节点在ECEF系下的坐标，x2、y2、z2为第2个空中无人机节点在ECEF(地球坐标系)系下的坐标，x3、y3、z3为第3个空中无人机节点在ECEF系下的坐标，x4、y4、z4为第4个空中无人机节点在ECEF系下的坐标，x0、y0、z0为地面载体在ECEF系下的坐标，
Figure BDA0002752151980000043
Figure BDA0002752151980000044
为地面载体在ECEF系下的坐标估值，Δx0、Δy0、Δz0为地面载体位置误差转换到ECEF系下的值，V为无线测距的量测噪声阵，h(·)为状态的量测矩阵方程，d1、d2、d3、d4分别为四个无人机空中节点与地面载体之间的无线测距值。
作为本发明的一种优选方案，所述步骤3的具体过程如下：
1)将UKF滤波器状态方程和量测方程离散化，得到：
Figure BDA0002752151980000045
Figure BDA0002752151980000046
其中，
Figure BDA0002752151980000047
是tk时刻系统的增广状态变量，
Figure BDA0002752151980000048
为tk-1时刻系统增广状态变量，Xk-1为tk-1时刻系统状态变量，f(·)为系统的一步状态转移方程，Wk-1为tk-1时刻系统噪声矩阵，Zk为tk时刻无线测距量测值，Vk为tk时刻量测噪声矩阵；
2)对离散化处理得到的公式加入控制项Uk-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：
Figure BDA0002752151980000049
Figure BDA00027521519800000410
3)计算k-1时刻的2n+1个σ样本点：
Figure BDA00027521519800000411
Figure BDA0002752151980000051
Figure BDA0002752151980000052
其中，
Figure BDA0002752151980000053
为系统增广状态向量的初始样本点，
Figure BDA0002752151980000054
为tk-1时刻系统增广状态向量估值，
Figure BDA0002752151980000055
为tk-1时刻的状态向量估值，
Figure BDA0002752151980000056
为系统增广状态向量的σ样本点，n为状态量个数，Pk-1为tk-1时刻滤波状态估计协方差矩阵，
Figure BDA0002752151980000057
为矩阵下三角分解后的第i列，
Figure BDA0002752151980000058
λ＝α2(n+κ)-n，α是很小的正数，可取10-4≤α≤1，κ＝3-n；
4)计算k时刻UKF一步预测模型值：
Figure BDA0002752151980000059
Figure BDA00027521519800000510
Figure BDA00027521519800000511
Figure BDA00027521519800000512
Figure BDA00027521519800000513
其中，
Figure BDA00027521519800000514
为系统15维状态向量σ样本点的一步预测值，
Figure BDA00027521519800000515
为系统增广状态向量的一步预测值，uk-1为tk-1时刻的控制项，Xk/k-1为系统状态的一步预测模型值，
Figure BDA00027521519800000516
为系统状态的一步预测模型估值，Pk/k-1为系统一步预测均方差阵，权值
Figure BDA00027521519800000517
β一般取2，
Figure BDA00027521519800000518
为量测的一步预测样本值，
Figure BDA00027521519800000519
为系统量测的一步预测值。
作为本发明的一种优选方案，所述步骤4的具体过程如下：
(1)计算系统输出的理论方差阵和协方差阵：
Figure BDA00027521519800000520
Figure BDA00027521519800000521
其中，P(XZ)k/k-1为系统输出的一步预测协方差阵，P(ZZ)k/k-1为系统输出的理论方差阵，Rk为量测噪声阵；
(2)计算滤波增益阵：
Figure BDA0002752151980000061
其中，Kk为滤波增益阵，
Figure BDA0002752151980000062
为一步预测协方差阵的逆矩阵；
(3)计算滤波值：
Figure BDA0002752151980000063
Figure BDA0002752151980000064
其中，
Figure BDA0002752151980000065
为系统状态估值，Pk/k为系统状态估计均方误差阵，Pk/k-1为系统的一步预测均方误差阵，
Figure BDA0002752151980000066
为滤波增益阵的转置，Zk为无限测距量测值；
(4)根据(3)得到的系统状态估计值，包括姿态、位置、速度误差，并用惯性导航系统推算的导航参数减去系统导航误差值，得到基于UKF与空中无人机节点辅助的地面载体组合导航修正值。
本发明的有益效果如下：
1、本发明方法利用空中无人机节点对地面载体进行辅助定位，解决了地面载卫星信号易受遮挡和干扰的问题。
2、本发明方法以无线测距信息构建非线性量测方程，保证了对空中无人机节点测距信息的有效利用，具有广泛的适用性。
3、本发明方法应用UKF滤波方法对惯性/空中无人机节点辅助组合导航系统的状态量实现了最优估计，利用惯性导航和空中无人机节点辅助定位的互补性，有效地提高组合导航系统的精度，适合工程应用。
附图说明
图1是本发明基于UKF与空中无人机节点辅助的地面载体组合导航方法的架构图。
图2是本发明方法所设计地面载体与四架空中无人机节点的航迹图。
图3是本发明方法所估计出的航迹路线与设定航迹对比图。
图4(a)是本发明算法的位置误差曲线与纯惯性导航位置误差曲线的经度误差对比图。
图4(b)是本发明算法的位置误差曲线与纯惯性导航位置误差曲线的纬度误差对比图。
图4(c)是本发明算法的位置误差曲线与纯惯性导航位置误差曲线的高度误差对比图。
图5(a)是本发明算法的位置误差曲线的经度误差图。
图5(b)是本发明算法的位置误差曲线的纬度误差图。
图5(c)是本发明算法的位置误差曲线的高度误差图。
具体实施方式
下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。
如图1所示，本发明所述的基于UKF与空中无人机节点辅助的地面载体组合导航方法的原理是：地面载体通过建立UKF滤波状态方程和量测方程，利用陀螺仪和加速度计原始输出进行捷联惯导解算，UKF的时间更新与惯导解算同步，当到了量测更新周期后，向空中无人机节点发送指令并接收无人机位置和测距信息，进行量测更新，实现对组合导航系统状态量的估计，以提高组合导航系统性能。
本发明的具体实施方式如下：
1、建立基于UKF与空中无人机节点辅助的地面载体组合导航算法的状态方程
首先定义坐标系：地理坐标系选取“东-北-天”为导航系(n)，载体坐标系(b)选取为“右-前-上”。系统的状态向量为：
Figure BDA0002752151980000071
其中，X为姿态误差、速度误差、位置误差、IMU随机游走组成的15维状态向量，W为系统白噪声，Xa为将系统噪声增广后的状态向量，
Figure BDA0002752151980000072
为捷联惯导系统中的数学平台误差角，
Figure BDA0002752151980000081
分别为x、y、z的三轴数学平台误差角，δvn为载体的速度误差，
Figure BDA0002752151980000082
为东北天三轴的速度误差，δpn为载体的位置误差，δL、δλ、δh分别为经度、纬度、高度误差，ωb为陀螺仪的随机游走误差，
Figure BDA0002752151980000083
分别为陀螺仪x、y、z轴的随机游走误差，
Figure BDA0002752151980000084
为陀螺仪的随机游走驱动噪声，
Figure BDA0002752151980000085
分别为陀螺仪x、y、z轴的随机游走驱动噪声，
Figure BDA0002752151980000086
为陀螺仪的输出白噪声，
Figure BDA0002752151980000087
分别为陀螺仪x、y、z轴的输出白噪声，
Figure BDA0002752151980000088
为加速度计的随机游走误差，
Figure BDA0002752151980000089
分别为加速度计x、y、z轴的随机游走误差，
Figure BDA00027521519800000810
分别为加速度计x、y、z轴的随机游走驱动噪声。上标n表示导航坐标系(地理系)，上标b表示载体系。系统的状态方程为：
Figure BDA00027521519800000811
其中，
Figure BDA00027521519800000812
为状态量的一阶导数，
Figure BDA00027521519800000813
为数学平台误差角的一阶导数，
Figure BDA00027521519800000814
为载体速度误差的一阶导数，
Figure BDA00027521519800000815
分别为经度、纬度、高度误差的一阶导数，
Figure BDA00027521519800000816
为陀螺仪随机游走误差的一阶导数，
Figure BDA00027521519800000817
为加速度计随机游走误差的一阶导数，
Figure BDA00027521519800000818
为陀螺仪随机游走驱动噪声的一阶导数，
Figure BDA00027521519800000819
为陀螺仪的输出白噪声，
Figure BDA00027521519800000820
为加速度计随机游走驱动噪声的一阶导数，εb为陀螺漂移误差模型中的随机常数，vn为载体的速度，h为载体的及时高度，
Figure BDA00027521519800000821
为地理系相对于惯性系的角速率误差，
Figure BDA00027521519800000822
为地理系相对于惯性系的角速率，
Figure BDA00027521519800000823
表示从载体系到地理系的坐标变换矩阵，fn为地理系下加速度计的输出比力，
Figure BDA0002752151980000091
为地球系相对于惯性系的角速率误差，
Figure BDA0002752151980000092
为地理系相对于地球系的角速率误差，
Figure BDA0002752151980000093
为地球系相对于惯性系的角速率，
Figure BDA0002752151980000094
为地理系相对于地球系的角速率，δgn′为重力加速度误差，Rm为卯酉圈半径，Rn为子午圈半径，
Figure BDA0002752151980000095
2、建立基于UKF与空中无人机节点辅助的地面载体组合导航算法的量测方程
Figure BDA0002752151980000096
其中，x1、y1、z1为第1个空中无人机节点在ECEF系下的坐标，x2、y2、z2为第2个空中无人机节点在ECEF(地球坐标系)系下的坐标，x3、y3、z3为第3个空中无人机节点在ECEF系下的坐标，x4、y4、z4为第4个空中无人机节点在ECEF系下的坐标，x0、y0、z0为地面载体在ECEF系下的坐标，
Figure BDA0002752151980000097
Figure BDA0002752151980000098
为地面载体在ECEF系下的坐标估值，Δx0、Δy0、Δz0为地面载体位置误差转换到ECEF系下的值，V为无线测距的量测噪声阵，h(·)为状态的量测矩阵方程，d1、d2、d3、d4分别为四个无人机空中节点与地面载体之间的无线测距值。
3、地面载体基于空中无人机节点辅助的UKF滤波
(3.1)将UKF滤波器状态方程和量测方程离散化，得到：
Figure BDA0002752151980000099
其中，
Figure BDA00027521519800000910
是tk时刻系统的增广状态变量，
Figure BDA00027521519800000911
为tk-1时刻系统增广状态变量，Xk-1为tk-1时刻系统状态变量，f(·)为系统的一步状态转移方程，Wk-1为tk-1时刻系统噪声矩阵，Zk为tk时刻无线测距量测值，Vk为tk时刻量测噪声矩阵；
(3.2)对离散化处理得到的公式加入控制项Uk-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：
Figure BDA0002752151980000101
(3.3)计算k-1时刻的2n+1个σ样本点：
Figure BDA0002752151980000102
其中，
Figure BDA0002752151980000103
为系统增广状态向量的初始样本点，
Figure BDA0002752151980000104
为tk-1时刻系统增广状态向量估值，
Figure BDA0002752151980000105
为tk-1时刻的状态向量估值，
Figure BDA0002752151980000106
为系统增广状态向量的σ样本点，n为状态量个数，Pk-1为tk-1时刻滤波状态估计协方差矩阵，
Figure BDA0002752151980000107
为矩阵下三角分解后的第i列，
Figure BDA0002752151980000108
λ＝α2(n+κ)-n，α是很小的正数，可取10-4≤α≤1，κ＝3-n；
(3.4)计算k时刻UKF一步预测模型值：
Figure BDA0002752151980000109
其中，
Figure BDA00027521519800001010
为系统15维状态向量σ样本点的一步预测值，
Figure BDA00027521519800001011
为系统增广状态向量的一步预测值，uk-1为tk-1时刻的控制项，Xk/k-1为系统状态的一步预测模型值，
Figure BDA00027521519800001012
为系统状态的一步预测模型估值，Pk/k-1为系统一步预测均方差阵，权值
Figure BDA00027521519800001013
β一般取2，
Figure BDA00027521519800001014
为量测的一步预测样本值，
Figure BDA00027521519800001015
为系统量测的一步预测值。
(3.5)计算系统输出的理论方差阵和协方差阵：
Figure BDA0002752151980000111
其中，P(XZ)k/k-1为系统输出的一步预测协方差阵，P(ZZ)k/k-1为系统输出的理论方差阵，Rk为量测噪声阵；
(3.6)计算滤波增益阵：
Figure BDA0002752151980000112
其中，Kk为滤波增益阵，
Figure BDA0002752151980000113
为一步预测协方差阵的逆矩阵；
(3.7)计算滤波值：
Figure BDA0002752151980000114
其中，
Figure BDA0002752151980000115
为系统状态估值，Pk/k为系统状态估计均方误差阵，Pk/k-1为系统的一步预测均方误差阵，
Figure BDA0002752151980000116
为滤波增益阵的转置，Zk为无限测距量测值；
(3.8)根据3.7)得到的系统状态估计值，包括姿态、位置、速度误差，并用惯性导航系统推算的导航参数减去系统导航误差值，得到基于UKF与空中无人机节点辅助的地面载体组合导航修正值。
为了验证发明所提出的基于UKF与空中无人机节点辅助的地面载体组合导航的正确性和有效性，采用本发明方法建立模型，利用MATLAB仿真验证。设计地面载体与四架空中无人机节点的航迹如图2所示。
基于本发明所述的基于UKF与空中无人机节点辅助的地面载体组合导航方法进行验证，估计航迹与设定航迹对比曲线如图3所示。
图4(a)、图4(b)、图4(c)分别为本发明算法的位置误差曲线与纯惯性导航位置误差曲线的经度、纬度、高度误差对比图。图4中带上三角形标记符的曲线代表纯惯性导航位置误差曲线。从图4、图5导航位置误差曲线可以看出，采用本发明提出的基于UKF与空中无人机节点辅助的地面载体组合导航方法，导航系统精度相较于纯惯性导航系统有明显提高，具有有益的工程应用价值。
以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.一种基于UKF与空中无人机节点辅助的地面载体组合导航方法，其特征在于，包括如下步骤：
步骤1，建立组合导航系统的状态方程，将惯性传感器误差扩展为系统状态变量，包括陀螺的随机游走过程、随机游走驱动白噪声、陀螺仪的白噪声和加速度计的随机游走过程和随机游走驱动白噪声；
步骤2，结合空中无人机辅助节点提供的位置和无线测距信息，构建基于UKF的卡尔曼滤波器量测方程；
步骤3，计算状态向量的σ样本点，从而计算一步预测模型值，实现时间更新；
步骤4，到达量测周期，读取空中无人机辅助节点位置和测距信息，进行UKF量测更新，并采用UKF滤波对系统状态量进行反馈校正，实现对组合导航系统的有效修正。
2.根据权利要求1所述的基于UKF与空中无人机节点辅助的地面载体组合导航方法，其特征在于，步骤1所述系统的状态向量为：
Figure FDA0002752151970000011
其中，X为姿态误差、速度误差、位置误差、IMU随机游走组成的15维状态向量，W为系统白噪声，Xa为将系统噪声增广后的状态向量，
Figure FDA0002752151970000012
为捷联惯导系统中的数学平台误差角，
Figure FDA0002752151970000013
分别为x、y、z的三轴数学平台误差角，δvn为载体的速度误差，
Figure FDA0002752151970000014
为东北天三轴的速度误差，δpn为载体的位置误差，δL、δλ、δh分别为经度、纬度、高度误差，ωb为陀螺仪的随机游走误差，
Figure FDA0002752151970000015
分别为陀螺仪x、y、z轴的随机游走误差，
Figure FDA0002752151970000016
为陀螺仪的随机游走驱动噪声，
Figure FDA0002752151970000017
分别为陀螺仪x、y、z轴的随机游走驱动噪声，
Figure FDA0002752151970000021
为陀螺仪的输出白噪声，
Figure FDA0002752151970000022
分别为陀螺仪x、y、z轴的输出白噪声，
Figure FDA0002752151970000023
为加速度计的随机游走误差，
Figure FDA0002752151970000024
分别为加速度计x、y、z轴的随机游走误差，
Figure FDA0002752151970000025
为加速度计随机游走驱动噪声，
Figure FDA0002752151970000026
分别为加速度计x、y、z轴的随机游走驱动噪声；上标n表示导航坐标系，上标b表示载体系；
所述系统的状态方程为：
Figure FDA0002752151970000027
其中，
Figure FDA0002752151970000028
为状态量的一阶导数，
Figure FDA0002752151970000029
为数学平台误差角的一阶导数，
Figure FDA00027521519700000210
为载体速度误差的一阶导数，
Figure FDA00027521519700000211
分别为经度、纬度、高度误差的一阶导数，
Figure FDA00027521519700000212
为陀螺仪随机游走误差的一阶导数，
Figure FDA00027521519700000213
为加速度计随机游走误差的一阶导数，
Figure FDA00027521519700000214
为陀螺仪随机游走驱动噪声的一阶导数，
Figure FDA00027521519700000215
为陀螺仪的输出白噪声，
Figure FDA00027521519700000216
为加速度计随机游走驱动噪声的一阶导数，εb为陀螺漂移误差模型中的随机常数，vn为载体的速度，L为经度，h为载体的及时高度，
Figure FDA00027521519700000217
为地理系相对于惯性系的角速率误差，
Figure FDA00027521519700000218
为地理系相对于惯性系的角速率，
Figure FDA00027521519700000219
表示从载体系到地理系的坐标变换矩阵，fn为地理系下加速度计的输出比力，
Figure FDA00027521519700000220
为地球系相对于惯性系的角速率误差，
Figure FDA00027521519700000221
为地理系相对于地球系的角速率误差，
Figure FDA00027521519700000222
为地球系相对于惯性系的角速率，
Figure FDA00027521519700000223
为地理系相对于地球系的角速率，δgn为重力加速度误差，Rm为卯酉圈半径，Rn为子午圈半径，
Figure FDA0002752151970000031
3.根据权利要求2所述的基于UKF与空中无人机节点辅助的地面载体组合导航方法，其特征在于，步骤2所述基于UKF的卡尔曼滤波器量测方程为：
Figure FDA0002752151970000032
其中，x1、y1、z1为第1个空中无人机节点在ECEF系下的坐标，x2、y2、z2为第2个空中无人机节点在ECEF系下的坐标，x3、y3、z3为第3个空中无人机节点在ECEF系下的坐标，x4、y4、z4为第4个空中无人机节点在ECEF系下的坐标，x0、y0、z0为地面载体在ECEF系下的坐标，
Figure FDA0002752151970000033
为地面载体在ECEF系下的坐标估值，Δx0、Δy0、Δz0为地面载体位置误差转换到ECEF系下的值，V为无线测距的量测噪声阵，h(·)为状态的量测矩阵方程，d1、d2、d3、d4分别为四个无人机空中节点与地面载体之间的无线测距值。
4.根据权利要求3所述的基于UKF与空中无人机节点辅助的地面载体组合导航方法，其特征在于，所述步骤3的具体过程如下：
1)将UKF滤波器状态方程和量测方程离散化，得到：
Figure FDA0002752151970000034
Figure FDA0002752151970000035
其中，
Figure FDA0002752151970000036
是tk时刻系统的增广状态变量，
Figure FDA0002752151970000037
为tk-1时刻系统增广状态变量，Xk-1为tk-1时刻系统状态变量，f(·)为系统的一步状态转移方程，Wk-1为tk-1时刻系统噪声矩阵，Zk为tk时刻无线测距量测值，Vk为tk时刻量测噪声矩阵；
2)对离散化处理得到的公式加入控制项Uk-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：
Figure FDA0002752151970000038
Figure FDA0002752151970000039
3)计算k-1时刻的2n+1个σ样本点：
Figure FDA0002752151970000041
Figure FDA0002752151970000042
Figure FDA0002752151970000043
其中，
Figure FDA0002752151970000044
为系统增广状态向量的初始样本点，
Figure FDA0002752151970000045
为tk-1时刻系统增广状态向量估值，
Figure FDA0002752151970000046
为tk-1时刻的状态向量估值，
Figure FDA0002752151970000047
为系统增广状态向量的σ样本点，n为状态量个数，Pk-1为tk-1时刻滤波状态估计协方差矩阵，
Figure FDA0002752151970000048
为矩阵下三角分解后的第i列，
Figure FDA0002752151970000049
λ＝α2(n+κ)-n，α是很小的正数，取10-4≤α≤1，κ＝3-n；
4)计算k时刻UKF一步预测模型值：
Figure FDA00027521519700000410
Figure FDA00027521519700000411
Figure FDA00027521519700000412
Figure FDA00027521519700000413
Figure FDA00027521519700000414
其中，
Figure FDA00027521519700000415
为系统15维状态向量σ样本点的一步预测值，
Figure FDA00027521519700000416
为系统增广状态向量的一步预测值，uk-1为tk-1时刻的控制项，Xk/k-1为系统状态的一步预测模型值，
Figure FDA00027521519700000417
为系统状态的一步预测模型估值，Pk/k-1为系统一步预测均方差阵，权值
Figure FDA00027521519700000418
β取2，
Figure FDA00027521519700000419
为量测的一步预测样本值，
Figure FDA00027521519700000420
为系统量测的一步预测值。
5.根据权利要求4所述的基于UKF与空中无人机节点辅助的地面载体组合导航方法，其特征在于，所述步骤4的具体过程如下：
(1)计算系统输出的理论方差阵和协方差阵：
Figure FDA0002752151970000051
Figure FDA0002752151970000052
其中，P(XZ)k/k-1为系统输出的一步预测协方差阵，P(ZZ)k/k-1为系统输出的理论方差阵，Rk为量测噪声阵；
(2)计算滤波增益阵：
Figure FDA0002752151970000053
其中，Kk为滤波增益阵，
Figure FDA0002752151970000054
为一步预测协方差阵的逆矩阵；
(3)计算滤波值：
Figure FDA0002752151970000055
Figure FDA0002752151970000056
其中，
Figure FDA0002752151970000057
为系统状态估值，Pk/k为系统状态估计均方误差阵，Pk/k-1为系统的一步预测均方误差阵，
Figure FDA0002752151970000058
为滤波增益阵的转置，Zk为无线测距量测值；
(4)根据(3)得到的系统状态估计值，包括姿态、位置、速度误差，并用惯性导航系统推算的导航参数减去系统导航误差值，得到基于UKF与空中无人机节点辅助的地面载体组合导航修正值。
CN202011188871.2A 2020-10-30 2020-10-30 基于ukf与空中无人机节点辅助的地面载体组合导航方法 Pending CN112325878A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011188871.2A CN112325878A (zh) 2020-10-30 2020-10-30 基于ukf与空中无人机节点辅助的地面载体组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011188871.2A CN112325878A (zh) 2020-10-30 2020-10-30 基于ukf与空中无人机节点辅助的地面载体组合导航方法

Publications (1)

Publication Number Publication Date
CN112325878A true CN112325878A (zh) 2021-02-05

Family

ID=74296057

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011188871.2A Pending CN112325878A (zh) 2020-10-30 2020-10-30 基于ukf与空中无人机节点辅助的地面载体组合导航方法

Country Status (1)

Country Link
CN (1) CN112325878A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116192571A (zh) * 2023-02-06 2023-05-30 中国人民解放军火箭军工程大学 一种波束抖动效应下无人机isac信道估计方法
CN116929350A (zh) * 2023-08-23 2023-10-24 南京理工大学 一种基于数据链测距的快速临机重构协同导航系统及方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108731670A (zh) * 2018-05-18 2018-11-02 南京航空航天大学 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109459023A (zh) * 2018-09-18 2019-03-12 武汉三体机器人有限公司 一种基于无人机视觉slam的辅助地面机器人导航方法及装置
CN109916410A (zh) * 2019-03-25 2019-06-21 南京理工大学 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
CN109946730A (zh) * 2019-03-06 2019-06-28 东南大学 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108731670A (zh) * 2018-05-18 2018-11-02 南京航空航天大学 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109459023A (zh) * 2018-09-18 2019-03-12 武汉三体机器人有限公司 一种基于无人机视觉slam的辅助地面机器人导航方法及装置
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
CN109946730A (zh) * 2019-03-06 2019-06-28 东南大学 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN109916410A (zh) * 2019-03-25 2019-06-21 南京理工大学 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
YONGQIANG HAN等: "A Novel Cooperative Localization Method Based on IMU and UWB", 《SENSORS》 *
曾庆化等: "基于UWB优化配置的室内行人导航方法", 《中国惯性技术学报》 *
本书编委会: "《卫星导航定位与北斗系统应用 深化北斗应用•促进产业发展 2018版》", 31 August 2018, 测绘出版社 *
杨菁华等: "基于异类传感器观测信息融合的UKF算法", 《传感器与微系统》 *
潘加亮等: "一种简化的发射系下SINS/GPS/CNS组合导航系统无迹卡尔曼滤波算法", 《兵工学报》 *
熊智等: "发射系下SINS/GPS/CNS组合导航系统联邦粒子滤波算法", 《南京航空航天大学学报》 *
秦永元等: "《卡尔曼滤波与组合导航原理 第2版》", 30 June 2012, 西北工业大学出版社 *
许建新等: "多无人机辅助定位信标的区域导航定位算法", 《航空学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116192571A (zh) * 2023-02-06 2023-05-30 中国人民解放军火箭军工程大学 一种波束抖动效应下无人机isac信道估计方法
CN116192571B (zh) * 2023-02-06 2024-03-08 中国人民解放军火箭军工程大学 一种波束抖动效应下无人机isac信道估计方法
CN116929350A (zh) * 2023-08-23 2023-10-24 南京理工大学 一种基于数据链测距的快速临机重构协同导航系统及方法
CN116929350B (zh) * 2023-08-23 2023-11-21 南京理工大学 一种基于数据链测距的快速临机重构协同导航系统及方法

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
Georgy et al. Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter
CN102519470B (zh) 多级嵌入式组合导航系统及导航方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN106871928B (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN109000640B (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN110285804B (zh) 基于相对运动模型约束的车辆协同导航方法
CN104713555A (zh) 应用全天域中性点辅助定向的车辆自主导航方法
CN109059909A (zh) 基于神经网络辅助的卫星/惯导列车定位方法与系统
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN104215262A (zh) 一种惯性导航系统惯性传感器误差在线动态辨识方法
CN103884340B (zh) 一种深空探测定点软着陆过程的信息融合导航方法
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
CN109959374B (zh) 一种行人惯性导航全时全程逆向平滑滤波方法
CN104374405A (zh) 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法
CN101900573A (zh) 一种实现陆用惯性导航系统运动对准的方法
CN112325878A (zh) 基于ukf与空中无人机节点辅助的地面载体组合导航方法
Gao et al. An integrated land vehicle navigation system based on context awareness
CN117053802A (zh) 一种基于旋转mems imu的车载导航系统定位误差减小的方法
CN102830415B (zh) 一种降维度的基于Carlson滤波算法的快速组合导航方法
CN111220151A (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN104132664A (zh) 一种农用履带机器人滑动量的估计方法

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210205