CN115790580A - 基于变分贝叶斯自适应滤波的直升机着舰相对导航方法 - Google Patents

基于变分贝叶斯自适应滤波的直升机着舰相对导航方法 Download PDF

Info

Publication number
CN115790580A
CN115790580A CN202211439939.9A CN202211439939A CN115790580A CN 115790580 A CN115790580 A CN 115790580A CN 202211439939 A CN202211439939 A CN 202211439939A CN 115790580 A CN115790580 A CN 115790580A
Authority
CN
China
Prior art keywords
relative
navigation
ship
prediction
vector
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
CN202211439939.9A
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.)
China Helicopter Research and Development Institute
Original Assignee
China Helicopter Research and Development Institute
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 China Helicopter Research and Development Institute filed Critical China Helicopter Research and Development Institute
Priority to CN202211439939.9A priority Critical patent/CN115790580A/zh
Publication of CN115790580A publication Critical patent/CN115790580A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Navigation (AREA)

Abstract

本发明属于相对导航与组合导航领域，涉及基于变分贝叶斯自适应滤波的直升机着舰相对导航方法。该方法包括：基于直升机和舰船之间的相对运动建立系统的相对惯性导航方程，以视觉传感器测量的机载导航相机与舰船上光标点间的相对视线矢量作为量测建立量测方程；从而建立惯性/视觉相对导航系统模型；利用变分贝叶斯自适应容积卡尔曼滤波对惯性导航和视觉导航系统输出的信息进行滤波融合，同时对相对导航系统状态和视觉量测噪声协方差进行估计，以得到精确的舰机相对位置、速度和姿态信息。

Description

基于变分贝叶斯自适应滤波的直升机着舰相对导航方法
技术领域
本发明属于相对导航与组合导航领域，涉及基于变分贝叶斯自适应滤波的直升机着舰相对导航方法。
背景技术
直升机具有垂直起降、可定点悬停、低空飞行、机动灵活、无需专用机场/滑行跑道等特点，在海战中可用于执行空中预警、反潜反舰、远程打击、战场监视与侦查等多种任务，已然成为海上武器装备的一个重要种类。舰载直升机得以成功大范围应用的重要前提是解决舰载直升机的着舰问题。为了保证直升机顺利在不断运动的舰船上顺利自主着陆，精确的舰机相对导航信息是必不可少的。
惯性/视觉组合相对导航是一种无需与外界进行信息交互而可以提供舰机间相对位置、速度和姿态信息的方法。该方法以机载和舰载惯性导航系统输出为基础建立舰机相对惯性模型，基于机载导航相机对安装在舰船上的光标点进行测量，通过容积卡尔曼滤波等非线性滤波方法对惯性传感器数据和导航相机测量的相对视线矢量信息进行融合，从而估计出舰机间的相对位置、速度和姿态。传统的相对导航方法需要精确的传感器量测噪声统计特性来设计相对导航滤波器，但是在实际系统中，视觉传感器的量测噪声统计特性往往是未知的。在这种情况下，传统的相对导航方法估计精度急剧下降甚至出现发散现象。
发明内容
本发明的目的：为了解决现有惯性/视觉组合相对导航方法在视觉量测噪声统计特性先验信息不足情况下导航精度差的问题，提出了基于变分贝叶斯自适应滤波的直升机着舰相对导航方法。
本发明的技术方案：
基于变分贝叶斯自适应滤波的直升机着舰相对导航方法，包括：
基于直升机和舰船之间的相对运动建立系统的相对惯性导航方程，以视觉传感器测量的机载导航相机与舰船上光标点间的相对视线矢量作为量测建立量测方程；从而建立惯性/视觉相对导航系统模型；
利用变分贝叶斯自适应容积卡尔曼滤波对惯性导航和视觉导航系统输出的信息进行滤波融合，同时对相对导航系统状态和视觉量测噪声协方差进行估计，以得到精确的舰机相对位置、速度和姿态信息；
其中，变分贝叶斯自适应容积卡尔曼滤波器是将系统状态和量测噪声协方差联合分布建模为高斯分布×逆威舍特分布形式，根据变分贝叶斯理论获得系统状态和量测噪声协方差递推模型，采用三阶球面-径向容积准则计算非线性系统中高斯加权积分，通过高斯-牛顿迭代法获得系统状态和量测噪声协方差的估计值，构建得到的。
基于直升机和舰船之间的相对运动建立系统的相对惯性导航方程，以视觉传感器测量的机载导航相机与舰船上光标点间的相对视线矢量作为量测建立量测方程，包括：
建立相对姿态运动方程、相对质心运动方程；
相对姿态运动方程为：
Figure BDA0003948245200000021
其中，
Figure BDA0003948245200000022
直升机本体系h相对于舰船本体系w的角速度在h系下的投影；qh|w为相对姿态四元数；
Figure BDA0003948245200000023
其中，
Figure BDA0003948245200000024
q13为相对姿态四元数矢量部分，q4为相对姿态四元数标量部分；
相对质心运动方程为：
Figure BDA0003948245200000025
Figure BDA0003948245200000026
和
Figure BDA0003948245200000027
分别为
Figure BDA0003948245200000028
的一、二阶导，I为惯性坐标系，
Figure BDA0003948245200000029
为直升机本体系相对于惯性坐标系I的角速度在h系下的表示，
Figure BDA00039482452000000210
为
Figure BDA00039482452000000211
的二阶导，
Figure BDA00039482452000000212
为
Figure BDA00039482452000000213
的二阶导，
Figure BDA00039482452000000214
为直升机相对于舰船的位置矢量在直升机本体系下的表示，
Figure BDA00039482452000000215
和
Figure BDA00039482452000000216
分别为直升机和舰船在惯性坐标系中的位置矢量在惯性坐标系下的表示，
Figure BDA00039482452000000217
表示惯性坐标系至直升机本体系的姿态转换矩阵；
相对视线矢量作为量测建立量测方程为：
Figure BDA0003948245200000031
其中，
Figure BDA0003948245200000032
为相机坐标系至直升机本体系的姿态转换矩阵；bs为单位视线矢量；光标点在舰船上的位置为[xws yws zws]T(s＝1,2,…,Ns)，Ns为光标点数，[x y z]T为直升机与舰船之间的相对位置矢量
Figure BDA0003948245200000033
利用变分贝叶斯自适应容积卡尔曼滤波对惯性导航和视觉导航系统输出的信息进行滤波融合，同时对相对导航系统状态和视觉量测噪声协方差进行估计，以得到精确的舰机相对位置、速度和姿态信息，包括：
(1)选择相对导航系统状态向量；
采用三参数罗德里格斯参数表示姿态偏差来传播和更新相对姿态四元数，定义状态矢量xk为：
Figure BDA0003948245200000034
其中，δsk为罗德里格斯参数，
Figure BDA0003948245200000035
为相对位置矢量，
Figure BDA0003948245200000036
为相对速度矢量，
Figure BDA0003948245200000037
为加速度计漂移，
Figure BDA0003948245200000038
为陀螺仪漂移；
(2)计算经相对惯性导航模型传播后的导航状态；
将k-1时刻罗德里格斯参数转换成相对姿态四元数偏差；
Figure BDA0003948245200000039
Figure BDA00039482452000000310
Figure BDA00039482452000000311
其中，
Figure BDA00039482452000000312
为第k-1时刻相对姿态四元数的偏差，i＝1,2,…,N为三阶球面-径向容积点序列号，N＝2n为容积点个数，n为状态向量xk的维数；α是取值范围在0到1之间的参数，λ是尺度因子，容积点
Figure BDA00039482452000000313
计算如下：
Figure BDA0003948245200000041
其中，
Figure BDA0003948245200000042
和Pk-1/k-1分别为k-1时刻状态向量和状态协方差，
Figure BDA0003948245200000043
[1]i为集合[1]中的第i个列向量：
Figure BDA0003948245200000044
根据k-1时刻估计的相对姿态四元数和相对姿态四元数误差得到：
Figure BDA0003948245200000045
Figure BDA0003948245200000046
求取姿态微分
Figure BDA0003948245200000047
和速度微分
Figure BDA0003948245200000048
方程，
Figure BDA0003948245200000049
Figure BDA00039482452000000410
其中，
Figure BDA00039482452000000411
其中，
Figure BDA00039482452000000412
为舰船本体系相对于惯性坐标系I的角速度在w系下的表示；
利用四阶-五步龙格库塔积分获得经相对惯性导航模型传播后的相对姿态
Figure BDA00039482452000000413
相对位置
Figure BDA00039482452000000414
相对速度
Figure BDA00039482452000000415
加速度计偏移预测
Figure BDA00039482452000000416
和陀螺仪偏移预测
Figure BDA00039482452000000417
计算传播后的相对姿态四元数误差
Figure BDA00039482452000000418
Figure BDA00039482452000000419
由传播后的相对姿态四元数误差计算传播后的罗德里格斯参数
Figure BDA0003948245200000051
Figure BDA0003948245200000052
其中，
Figure BDA0003948245200000053
根据罗德里格斯参数预测
Figure BDA0003948245200000054
位置预测
Figure BDA0003948245200000055
速度预测
Figure BDA0003948245200000056
加速度计偏移预测
Figure BDA0003948245200000057
和陀螺仪偏移预测
Figure BDA0003948245200000058
计算得到经相对惯性导航模型传播后的状态
Figure BDA0003948245200000059
及其协方差Pk/k-1；同时计算量测噪声协方差阵所需变量自由度参数预测
Figure BDA00039482452000000510
和逆尺度矩阵预测
Figure BDA00039482452000000511
(3)融合视觉相对视线矢量信息得到修正后的相对导航状态；
计算量测预测
Figure BDA00039482452000000512
Figure BDA00039482452000000513
其中，第s个信标光点量测
Figure BDA00039482452000000514
Figure BDA00039482452000000515
Figure BDA00039482452000000516
利用视觉导航量测信息和量测预测修正系统状态
Figure BDA00039482452000000517
Figure BDA00039482452000000518
按下式计算修正后的相对姿态：
Figure BDA00039482452000000519
其中，
Figure BDA0003948245200000061
可通过将修正后罗德里格斯参数
Figure BDA0003948245200000062
转换得到，具体转换公式为
Figure BDA0003948245200000063
Figure BDA0003948245200000064
Figure BDA0003948245200000065
至此可以获得经视觉量测修正后的相对位置
Figure BDA0003948245200000066
相对速度
Figure BDA0003948245200000067
和姿态
Figure BDA0003948245200000068
能够为直升机着舰提供精确的相对导航信息。
根据罗德里格斯参数预测
Figure BDA0003948245200000069
位置预测
Figure BDA00039482452000000610
速度预测
Figure BDA00039482452000000611
加速度计偏移预测
Figure BDA00039482452000000612
和陀螺仪偏移预测
Figure BDA00039482452000000613
计算得到经相对惯性导航模型传播后的状态
Figure BDA00039482452000000614
及其协方差Pk/k-1，包括：
计算状态预测
Figure BDA00039482452000000615
及其协方差Pk/k-1：
Figure BDA00039482452000000616
Figure BDA00039482452000000617
其中，
Figure BDA00039482452000000618
为权重；Qk-1为过程噪声协方差矩阵，经相对惯性导航模型传播后的容积点χi,k/k-1为：
Figure BDA00039482452000000619
计算量测噪声协方差阵所需变量自由度参数预测
Figure BDA00039482452000000620
和逆尺度矩阵预测
Figure BDA00039482452000000621
包括：
计算自由度参数预测
Figure BDA0003948245200000071
和逆尺度矩阵预测
Figure BDA0003948245200000072
Figure BDA0003948245200000073
Figure BDA0003948245200000074
其中，β是满足0<β≤1的遗忘因子，
Figure BDA0003948245200000075
m为量测向量的维数。
利用视觉导航量测信息和量测预测修正系统状态，包括：
计算量测预测值：
Figure BDA0003948245200000076
计算协方差
Figure BDA0003948245200000077
和互协方差
Figure BDA0003948245200000078
Figure BDA0003948245200000079
Figure BDA00039482452000000710
高斯-牛顿迭代过程如下：
计算滤波增益
Figure BDA00039482452000000711
Figure BDA00039482452000000712
其中，j＝1,2,…,Nd，Nd是迭代次数；
计算第j次迭代系统状态估计
Figure BDA00039482452000000713
及其协方差
Figure BDA00039482452000000714
Figure BDA00039482452000000715
Figure BDA00039482452000000716
计算第j次迭代量测噪声协方差更新所需的容积点：
Figure BDA00039482452000000717
计算第j次迭代逆尺度矩阵估计值
Figure BDA00039482452000000718
Figure BDA00039482452000000719
其中，
Figure BDA00039482452000000720
为k时刻的量测估计值，计算过程与
Figure BDA00039482452000000721
一致；
计算第j次迭代量测噪声协方差矩阵
Figure BDA00039482452000000722
Figure BDA00039482452000000723
用最终迭代值对系统状态及其协方差、逆尺度矩阵进行赋值，即
Figure BDA0003948245200000081
设定迭代初值：逆威舍特分布的自由度参数和逆尺度矩阵分别为
Figure BDA0003948245200000082
量测噪声协方差初值
Figure BDA0003948245200000083
为：
Figure BDA0003948245200000084
一种计算机可读的存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现任一所述的方法。
本发明的有益效果：
本发明针对直升着舰过程中惯性/视觉组合相对导航的视觉量测噪声统计特性未知或先验信息不足情况，设计了基于变分贝叶斯自适应容积卡尔曼滤波的直升机着舰相对导航系统，将变分贝叶斯理论和三阶球面-径向容积准则相结合以估计视觉量测噪声协方差，用于调节导航滤波器增益，提高导航系统对视觉量测信息的使用精度，从而有效提高直升机着舰相对导航精度。
附图说明
图1是直升机自主着舰系统示意图。
图2是直升机和舰船运动轨迹图。
图3是采用本发明方法估计的Rk(1,1)与真实值对比图。
图4是采用本发明方法和传统方法估计的相对导航位置误差对比图。
图5是采用本发明方法和传统方法估计的相对导航速度误差对比图。
图6是采用本发明方法和传统方法估计的相对导航姿态误差对比图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面将结合附图对本发明作进一步的详细说明。
基于变分贝叶斯自适应滤波的直升机着舰相对导航方法，包括：
基于直升机和舰船之间的相对运动建立系统的相对惯性导航方程，以视觉传感器测量的机载导航相机与舰船上光标点间的相对视线矢量作为量测建立量测方程；从而建立惯性/视觉相对导航系统模型；
利用变分贝叶斯自适应容积卡尔曼滤波对惯性导航和视觉导航系统输出的信息进行滤波融合，同时对相对导航系统状态和视觉量测噪声协方差进行估计，以得到精确的舰机相对位置、速度和姿态信息；
其中，变分贝叶斯自适应容积卡尔曼滤波器是将系统状态和量测噪声协方差联合分布建模为高斯分布×逆威舍特分布形式，根据变分贝叶斯理论获得系统状态和量测噪声协方差递推模型，采用三阶球面-径向容积准则计算非线性系统中高斯加权积分，通过高斯-牛顿迭代法获得系统状态和量测噪声协方差的估计值，构建得到的。
本发明的基于变分贝叶斯自适应滤波的直升机着舰相对导航方法，包括以下几个步骤：
1.惯性/视觉相对导航系统建模：定义相对姿态四元数，给出相对姿态运动方程，定义相对位置矢量，给出相对质心运动方程，建立相对惯性导航模型；以机载导航相机和舰面光标点间的相对视线矢量为量测信息建立视觉导航系统量测模型。
(1)建立相对惯性导航模型
①相对姿态运动方程
采用四元数表示姿态，定义相对姿态四元数qh|w为：
Figure BDA0003948245200000091
其中，qh和qw分别直升机和舰船的姿态四元数。
相对姿态微分方程为：
Figure BDA0003948245200000092
其中，
Figure BDA0003948245200000093
直升机本体系h相对于舰船本体系w的角速度在h系下的投影，Ξ(qh|w)按下式计算：
Figure BDA0003948245200000094
其中，
Figure BDA0003948245200000095
②相对质心运动方程
相对位置表示为：
Figure BDA0003948245200000096
其中，
Figure BDA0003948245200000101
为直升机相对于舰船的位置矢量在直升机本体系下的表示，I为惯性坐标系，
Figure BDA0003948245200000102
和
Figure BDA0003948245200000103
分别为直升机和舰船在惯性坐标系中的位置矢量在各自本体系下的表示，
Figure BDA0003948245200000104
和
Figure BDA0003948245200000105
分别为直升机和舰船在惯性坐标系中的位置矢量在惯性坐标系下的表示，
Figure BDA0003948245200000106
为舰船本体系至直升机本体系的姿态转换矩阵，
Figure BDA0003948245200000107
表示惯性坐标系至直升机本体系的姿态转换矩阵。
对式(4)求二阶导，得到
Figure BDA0003948245200000108
其中，
Figure BDA0003948245200000109
为直升机本体系相对于惯性坐标系I的角速度在h系下的表示。
(2)视觉导航系统量测模型
如图1所示视觉导航系统主要包括两部分，其一为视觉导航相机，其二为光标点，并且这两部分分别安装在直升机和舰船甲板上；因此，视觉导航相机的测量值为直升机与舰船之间的相对视线矢量。光标点在舰船上的位置为[xws yws zws]T(s＝1,2,…,Ns)，[x yz]T为直升机与舰船之间的相对位置矢量
Figure BDA00039482452000001010
则单位视线矢量为
Figure BDA00039482452000001011
其中，
Figure BDA00039482452000001012
为相机坐标系至直升机本体系的姿态转换矩阵。
将量测表示成关于状态向量函数的形式
Figure BDA00039482452000001013
其中，z是量测向量，h(x)是非线性量测函数，v是量测噪声。
2.基于三阶球面-径向容积准则和变分贝叶斯理论构建变分贝叶斯自适应容积卡尔曼滤波器。
(1)三阶球面-径向容积准则
计算带权为正态分布
Figure BDA0003948245200000111
的高斯加权积分
Figure BDA0003948245200000112
其中，f(x)是非线性函数，n为系统状态x维数，N为积分点数目。
采用三阶球面-径向容积准则，共有N＝2n个积分点，每个容积点及权重为：
Figure BDA0003948245200000113
Figure BDA0003948245200000114
其中，
Figure BDA0003948245200000115
[1]i为集合[1]中的第i个列向量：
Figure BDA0003948245200000116
(2)变分贝叶斯滤波理论
将系统状态和量测噪声协方差联合先验分布近似为：
Figure BDA0003948245200000117
其中，IW(R；u,U)是受自由度参数u和逆尺度矩阵
Figure BDA0003948245200000118
控制的逆威舍特(IW)分布，m为量测向量维数，逆威舍特分布是高斯分布未知协方差的共轭先验分布。
通过选择式(12)所表示的联合先验分布形式，并且确保联合预测分布为高斯-逆威舍特分布，那么联合后验分布的形式也将与联合先验和预测分布一样。根据Chapman–Kolmogorov方程，系统状态xk的预测分布可以按下式计算
Figure BDA0003948245200000119
其中，Qk-1是过程噪声协方差，状态预测
Figure BDA00039482452000001110
及其协方差Pk/k-1可以根据三阶球面-径向容积准则计算。
量测噪声协方差预测
Figure BDA0003948245200000121
选择式(15)和(16)中R的传播模型可以保证量测噪声协方差依然是逆威舍特分布。
Figure BDA0003948245200000122
Figure BDA0003948245200000123
其中，β是满足0<β≤1的遗忘因子，
Figure BDA0003948245200000124
m为量测向量维数。
系统状态和量测噪声协方差的预测分布是相互独立的，联合预测分布可以写为：
Figure BDA0003948245200000125
根据xk和Rk是独立的假设，联合后验分布可以表示为自由形式的变分贝叶斯近似
p(xk,Rk|z1:k)≈qx(xk)qR(Rk) (18)
其中，qx(xk)和qR(Rk)分别是高斯分布和逆威舍特分布。通过最小化Kullback-Leibler散度可以获得变分贝叶斯近似形式：
qx(xk)∝exp(∫logp(zk,xk,Rk∫z1:k-1)qR(Rk)dRk) (19)
qR(Rk)∝exp(∫logp(zk,xk,Rk|z1:k-1)qx(xk)dxk) (20)
式(19)和(20)中的指数函数积分可以写为：
Figure BDA0003948245200000126
Figure BDA0003948245200000131
其中，<·>x＝∫(·)qx(xk)dxk可以根据三阶球面-径向容积准则计算，tr{A}是矩阵A的迹，C1和C2是常数。将式(21)和(22)分别代入式(19)和(20)中，比较式(19)和(20)等号左右两边的相关项，可以得到
Figure BDA0003948245200000132
Figure BDA0003948245200000133
Figure BDA0003948245200000134
Figure BDA0003948245200000135
Figure BDA0003948245200000136
Figure BDA0003948245200000137
Figure BDA0003948245200000138
Figure BDA0003948245200000139
Figure BDA00039482452000001310
Figure BDA00039482452000001311
由于系统状态和量测噪声协方差更新过程是耦合的，因此将采用高斯-牛顿迭代法(定点迭代)进行求解。
(3)变分贝叶斯自适应容积卡尔曼滤波算法的基本步骤
状态估计初值及其协方差初始化：
Figure BDA00039482452000001312
Figure BDA00039482452000001313
量测噪声协方差分布自由度参数和逆尺度矩阵初始化：
Figure BDA0003948245200000141
Figure BDA0003948245200000142
对于k＝1,2,3,…运行如下步骤：
计算状态预测所需容积点χi,k-1/k-1；
Figure BDA0003948245200000143
Figure BDA0003948245200000144
Figure BDA0003948245200000145
计算自由度参数预测
Figure BDA0003948245200000146
和逆尺度矩阵预测
Figure BDA0003948245200000147
Figure BDA0003948245200000148
Figure BDA0003948245200000149
计算状态更新所需容积点
Figure BDA00039482452000001410
计算量测预测值：
Figure BDA00039482452000001411
计算协方差
Figure BDA00039482452000001412
和互协方差
Figure BDA00039482452000001413
Figure BDA00039482452000001414
Figure BDA00039482452000001415
设定迭代初值：逆威舍特分布的自由度参数和逆尺度矩阵分别为
Figure BDA00039482452000001416
量测噪声协方差初值
Figure BDA00039482452000001417
为：
Figure BDA00039482452000001418
高斯-牛顿迭代过程如下：
计算第j次迭代的滤波增益
Figure BDA00039482452000001419
Figure BDA00039482452000001420
其中，j＝1,2,…,Nd，Nd是迭代次数。
计算第j次迭代的系统状态估计
Figure BDA0003948245200000151
及其协方差
Figure BDA0003948245200000152
Figure BDA0003948245200000153
Figure BDA0003948245200000154
计算第j次迭代的量测噪声协方差更新所需的容积点
Figure BDA0003948245200000155
计算第j次迭代的逆尺度矩阵估计值
Figure BDA0003948245200000156
Figure BDA0003948245200000157
计算第j次迭代的量测噪声协方差矩阵
Figure BDA0003948245200000158
Figure BDA0003948245200000159
用最终迭代值对系统状态及其协方差、逆尺度矩阵进行赋值，即
Figure BDA00039482452000001510
3.利用变分贝叶斯自适应容积卡尔曼滤波估计相对导航系统状态和视觉量测噪声协方差，为直升机着舰提供精确的舰机相对位置、速度和姿态信息。
(1)相对导航系统状态向量选择
由于直接用四维相对姿态四元数表示只有三个自由度的姿态进行滤波将导致状态协方差不满秩，因此本发明采用三参数罗德里格斯参数表示相对姿态偏差来传播和更新相对姿态四元数，定义状态矢量为：
Figure BDA00039482452000001511
其中，δsk为罗德里格斯参数，
Figure BDA00039482452000001512
为相对位置矢量，
Figure BDA00039482452000001513
为相对速度矢量，
Figure BDA00039482452000001514
为加速度计漂移，
Figure BDA00039482452000001515
为陀螺仪漂移。
(2)计算经相对惯性导航模型传播后的导航状态
将k-1时刻罗德里格斯参数转换成相对姿态四元数偏差
Figure BDA00039482452000001516
Figure BDA0003948245200000161
Figure BDA0003948245200000162
其中，α是取值范围在0到1之间的参数，λ是尺度因子，本发明分别设置为α＝0、λ＝2；容积点
Figure BDA0003948245200000163
按式(37)计算得到
Figure BDA0003948245200000164
根据k-1时刻估计的相对姿态四元数和相对姿态四元数误差得到：
Figure BDA0003948245200000165
Figure BDA0003948245200000166
求取姿态微分
Figure BDA0003948245200000167
和速度微分
Figure BDA0003948245200000168
方程
Figure BDA0003948245200000169
Figure BDA00039482452000001610
其中
Figure BDA00039482452000001611
其中，
Figure BDA00039482452000001612
为舰船本体系相对于惯性坐标系I的角速度在w系下的表示。
利用四阶-五步龙格库塔积分获得经相对惯性导航模型传播后的相对姿态
Figure BDA00039482452000001613
相对位置
Figure BDA00039482452000001614
相对速度
Figure BDA00039482452000001615
加速度计偏移预测
Figure BDA00039482452000001616
和陀螺仪偏移预测
Figure BDA00039482452000001617
计算传播后的相对姿态四元数误差
Figure BDA00039482452000001618
由传播后的相对姿态四元数误差计算传播后的罗德里格斯参数
Figure BDA0003948245200000171
其中，
Figure BDA0003948245200000172
根据罗德里格斯参数预测
Figure BDA0003948245200000173
位置预测
Figure BDA0003948245200000174
速度预测
Figure BDA0003948245200000175
加速度计偏移预测
Figure BDA0003948245200000176
和陀螺仪偏移预测
Figure BDA0003948245200000177
根据式(38)和(39)计算得到经相对惯性导航模型传播后的状态
Figure BDA0003948245200000178
及其协方差Pk/k-1；同时根据式(40)和(41)可得用于计算量测噪声协方差阵所需变量自由度参数预测
Figure BDA0003948245200000179
和逆尺度矩阵预测
Figure BDA00039482452000001710
(3)融合视觉相对视线矢量信息得到修正后的相对导航状态
计算量测预测
Figure BDA00039482452000001711
其中，Ns为信标光点数量，第s个信标光点量测
Figure BDA00039482452000001712
Figure BDA00039482452000001713
Figure BDA00039482452000001714
将式(65)代入式(43)～(52)得到利用视觉导航信息修正的系统状态。
Figure BDA00039482452000001715
按下式计算修正后的相对姿态
Figure BDA0003948245200000181
其中，
Figure BDA0003948245200000182
可根据式(54)～(56)将修正后罗德里格斯参数
Figure BDA0003948245200000183
获得。
经过视觉量测信息修正后可以获得精确的舰机相对位置
Figure BDA0003948245200000184
速度
Figure BDA0003948245200000185
和
Figure BDA0003948245200000186
姿态信息，为直升机着舰提供引导。
下面通过数学仿真来说明本发明方法的效果：
如图2所示直升机从距离舰船105m高处开始进行相对导航，引导直升机至舰船正上方5m处结束；舰船以0.2m/s的速度匀速直线运动。7个光标点在舰船甲板上的分布如图1所示，惯性和视觉传感器的偏差相关参数以及变分贝叶斯自适应容积卡尔曼滤波(VBACKF)算法参数初值如表1所示。
表1惯性和视觉传感器偏差参数
Figure BDA0003948245200000187
由于视觉量测噪声的统计特性未知，基于容积卡尔曼滤波(CKF)的相对导航滤波器的量测噪声协方差阵Rk设置为
Figure BDA0003948245200000188
图3为VBACKF估计的量测噪声协方差与实际量测噪声协方差值的比较图。图4～图6是基于CKF和VBACKF算法的直升机着舰相对导航系统100次蒙特卡洛打靶仿真三维相对位置、速度、姿态均方根误差的比较结果。仿真结果表明，基于VBACKF的相对导航精度远高于基于CKF的导航精度，这是由于VBACKF具有自适应性，即其可在线估计量测噪声协方差阵用于调节滤波增益；基于VBACKF的惯性/视觉相对导航能够有效提高着陆段舰机间的相对导航精度，显著提高直升机着舰过程的安全性。
以上所述，仅为本发明的具体实施例，对本发明进行详细描述，未详尽部分为常规技术。但本发明的保护范围不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。本发明的保护范围应以所述权利要求的保护范围为准。

Claims (8)

1.基于变分贝叶斯自适应滤波的直升机着舰相对导航方法，其特征在于，包括：
基于直升机和舰船之间的相对运动建立系统的相对惯性导航方程，以视觉传感器测量的机载导航相机与舰船上光标点间的相对视线矢量作为量测建立量测方程；从而建立惯性/视觉相对导航系统模型；
利用变分贝叶斯自适应容积卡尔曼滤波对惯性导航和视觉导航系统输出的信息进行滤波融合，同时对相对导航系统状态和视觉量测噪声协方差进行估计，以得到精确的舰机相对位置、速度和姿态信息；
其中，变分贝叶斯自适应容积卡尔曼滤波器是将系统状态和量测噪声协方差联合分布建模为高斯分布×逆威舍特分布形式，根据变分贝叶斯理论获得系统状态和量测噪声协方差递推模型，采用三阶球面-径向容积准则计算非线性系统中高斯加权积分，通过高斯-牛顿迭代法获得系统状态和量测噪声协方差的估计值，构建得到的。
2.根据权利要求1所述的方法，其特征在于，基于直升机和舰船之间的相对运动建立系统的相对惯性导航方程，以视觉传感器测量的机载导航相机与舰船上光标点间的相对视线矢量作为量测建立量测方程，包括：
建立相对姿态运动方程、相对质心运动方程；
相对姿态运动方程为：
Figure FDA0003948245190000011
其中，
Figure FDA0003948245190000012
直升机本体系h相对于舰船本体系w的角速度在h系下的投影；qh|w为相对姿态四元数；
Figure FDA0003948245190000013
其中，
Figure FDA0003948245190000014
q13为相对姿态四元数矢量部分，q4为相对姿态四元数标量部分；
相对质心运动方程为：
Figure FDA0003948245190000015
Figure FDA0003948245190000021
和
Figure FDA0003948245190000022
分别为
Figure FDA0003948245190000023
的一、二阶导，I为惯性坐标系，
Figure FDA0003948245190000024
为直升机本体系相对于惯性坐标系I的角速度在h系下的表示，
Figure FDA0003948245190000025
为
Figure FDA0003948245190000026
的二阶导，
Figure FDA0003948245190000027
为
Figure FDA0003948245190000028
的二阶导，
Figure FDA0003948245190000029
为直升机相对于舰船的位置矢量在直升机本体系下的表示，
Figure FDA00039482451900000210
和
Figure FDA00039482451900000211
分别为直升机和舰船在惯性坐标系中的位置矢量在惯性坐标系下的表示，
Figure FDA00039482451900000212
表示惯性坐标系至直升机本体系的姿态转换矩阵；
相对视线矢量作为量测建立量测方程为：
Figure FDA00039482451900000213
其中，
Figure FDA00039482451900000214
为相机坐标系至直升机本体系的姿态转换矩阵；bs为单位视线矢量；光标点在舰船上的位置为[xws yws zws]T(s＝1,2,…,Ns)，Ns为光标点数，[x y z]T为直升机与舰船之间的相对位置矢量
Figure FDA00039482451900000215
3.根据权利要求2所述的方法，其特征在于，利用变分贝叶斯自适应容积卡尔曼滤波对惯性导航和视觉导航系统输出的信息进行滤波融合，同时对相对导航系统状态和视觉量测噪声协方差进行估计，以得到精确的舰机相对位置、速度和姿态信息，包括：
(1)选择相对导航系统状态向量；
采用三参数罗德里格斯参数表示姿态偏差来传播和更新相对姿态四元数，定义状态矢量xk为：
Figure FDA00039482451900000216
其中，δsk为罗德里格斯参数，
Figure FDA00039482451900000217
为相对位置矢量，
Figure FDA00039482451900000218
为相对速度矢量，
Figure FDA00039482451900000219
为加速度计漂移，
Figure FDA00039482451900000220
为陀螺仪漂移；
(2)计算经相对惯性导航模型传播后的导航状态；
将k-1时刻罗德里格斯参数转换成相对姿态四元数偏差；
Figure FDA00039482451900000221
Figure FDA0003948245190000031
Figure FDA0003948245190000032
其中，
Figure FDA0003948245190000033
为第k-1时刻相对姿态四元数的偏差，i＝1,2,…,N为三阶球面-径向容积点序列号，N＝2n为容积点个数，n为状态向量xk的维数；α是取值范围在0到1之间的参数，λ是尺度因子，容积点
Figure FDA0003948245190000034
计算如下：
Figure FDA0003948245190000035
其中，
Figure FDA0003948245190000036
和Pk-1/k-1分别为k-1时刻状态向量和状态协方差，
Figure FDA0003948245190000037
[1]i为集合[1]中的第i个列向量：
Figure FDA0003948245190000038
根据k-1时刻估计的相对姿态四元数和相对姿态四元数误差得到：
Figure FDA0003948245190000039
Figure FDA00039482451900000310
求取姿态微分
Figure FDA00039482451900000311
和速度微分
Figure FDA00039482451900000312
方程，
Figure FDA00039482451900000313
Figure FDA00039482451900000314
其中，
Figure FDA00039482451900000315
其中，
Figure FDA0003948245190000041
为舰船本体系相对于惯性坐标系I的角速度在w系下的表示；
利用四阶-五步龙格库塔积分获得经相对惯性导航模型传播后的相对姿态
Figure FDA0003948245190000042
相对位置
Figure FDA0003948245190000043
相对速度
Figure FDA0003948245190000044
加速度计偏移预测
Figure FDA0003948245190000045
和陀螺仪偏移预测
Figure FDA0003948245190000046
计算传播后的相对姿态四元数误差
Figure FDA0003948245190000047
Figure FDA0003948245190000048
由传播后的相对姿态四元数误差计算传播后的罗德里格斯参数
Figure FDA0003948245190000049
Figure FDA00039482451900000410
其中，
Figure FDA00039482451900000411
根据罗德里格斯参数预测
Figure FDA00039482451900000412
位置预测
Figure FDA00039482451900000413
速度预测
Figure FDA00039482451900000414
加速度计偏移预测
Figure FDA00039482451900000415
和陀螺仪偏移预测
Figure FDA00039482451900000416
计算得到经相对惯性导航模型传播后的状态
Figure FDA00039482451900000417
及其协方差Pk/k-1；同时计算量测噪声协方差阵所需变量自由度参数预测
Figure FDA00039482451900000418
和逆尺度矩阵预测
Figure FDA00039482451900000419
(3)融合视觉相对视线矢量信息得到修正后的相对导航状态；
计算量测预测
Figure FDA00039482451900000420
Figure FDA00039482451900000421
其中，第s个信标光点量测
Figure FDA00039482451900000422
Figure FDA00039482451900000423
Figure FDA00039482451900000424
利用视觉导航量测信息和量测预测修正系统状态
Figure FDA00039482451900000425
Figure FDA0003948245190000051
按下式计算修正后的相对姿态：
Figure FDA0003948245190000052
其中，
Figure FDA0003948245190000053
可通过将修正后罗德里格斯参数
Figure FDA0003948245190000054
转换得到，具体转换公式为
Figure FDA0003948245190000055
Figure FDA0003948245190000056
Figure FDA0003948245190000057
至此可以获得经视觉量测修正后的相对位置
Figure FDA0003948245190000058
相对速度
Figure FDA0003948245190000059
和姿态
Figure FDA00039482451900000510
能够为直升机着舰提供精确的相对导航信息。
4.根据权利要求3所述的方法，其特征在于，根据罗德里格斯参数预测
Figure FDA00039482451900000511
位置预测
Figure FDA00039482451900000512
速度预测
Figure FDA00039482451900000513
加速度计偏移预测
Figure FDA00039482451900000514
和陀螺仪偏移预测
Figure FDA00039482451900000515
计算得到经相对惯性导航模型传播后的状态
Figure FDA00039482451900000516
及其协方差Pk/k-1，包括：
计算状态预测
Figure FDA00039482451900000517
及其协方差Pk/k-1：
Figure FDA00039482451900000518
Figure FDA00039482451900000519
其中，
Figure FDA00039482451900000520
为权重；Qk-1为过程噪声协方差矩阵，经相对惯性导航模型传播后的容积点χi,k/k-1为：
Figure FDA0003948245190000061
5.根据权利要求3所述的方法，其特征在于，计算量测噪声协方差阵所需变量自由度参数预测
Figure FDA0003948245190000062
和逆尺度矩阵预测
Figure FDA0003948245190000063
包括：
计算自由度参数预测
Figure FDA0003948245190000064
和逆尺度矩阵预测
Figure FDA0003948245190000065
Figure FDA0003948245190000066
Figure FDA0003948245190000067
其中，β是满足0<β≤1的遗忘因子，
Figure FDA0003948245190000068
m为量测向量的维数。
6.根据权利要求3所述的方法，其特征在于，利用视觉导航量测信息和量测预测修正系统状态，包括：
计算量测预测值：
Figure FDA0003948245190000069
计算协方差
Figure FDA00039482451900000610
和互协方差
Figure FDA00039482451900000611
Figure FDA00039482451900000612
Figure FDA00039482451900000613
高斯-牛顿迭代过程如下：
计算滤波增益
Figure FDA00039482451900000614
其中，j＝1,2,…,Nd，Nd是迭代次数；
计算第j次迭代系统状态估计
Figure FDA00039482451900000615
及其协方差
Figure FDA00039482451900000616
Figure FDA00039482451900000617
Figure FDA00039482451900000618
计算第j次迭代量测噪声协方差更新所需的容积点：
Figure FDA0003948245190000071
计算第j次迭代逆尺度矩阵估计值
Figure FDA0003948245190000072
Figure FDA0003948245190000073
其中，
Figure FDA0003948245190000074
为k时刻的量测估计值，计算过程与
Figure FDA0003948245190000075
一致；
计算第j次迭代量测噪声协方差矩阵
Figure FDA0003948245190000076
Figure FDA0003948245190000077
用最终迭代值对系统状态及其协方差、逆尺度矩阵进行赋值，即
Figure FDA0003948245190000078
7.根据权利要求6所述的方法，其特征在于，设定迭代初值：逆威舍特分布的自由度参数和逆尺度矩阵分别为
Figure FDA0003948245190000079
量测噪声协方差初值
Figure FDA00039482451900000710
为：
Figure FDA00039482451900000711
8.一种计算机可读的存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-7中任一所述的方法。
CN202211439939.9A 2022-11-17 2022-11-17 基于变分贝叶斯自适应滤波的直升机着舰相对导航方法 Pending CN115790580A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211439939.9A CN115790580A (zh) 2022-11-17 2022-11-17 基于变分贝叶斯自适应滤波的直升机着舰相对导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211439939.9A CN115790580A (zh) 2022-11-17 2022-11-17 基于变分贝叶斯自适应滤波的直升机着舰相对导航方法

Publications (1)

Publication Number Publication Date
CN115790580A true CN115790580A (zh) 2023-03-14

Family

ID=85438473

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211439939.9A Pending CN115790580A (zh) 2022-11-17 2022-11-17 基于变分贝叶斯自适应滤波的直升机着舰相对导航方法

Country Status (1)

Country Link
CN (1) CN115790580A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222582A (zh) * 2023-05-10 2023-06-06 北京航空航天大学 一种基于变分贝叶斯推断多物理场自适应组合导航方法
CN116255988A (zh) * 2023-05-11 2023-06-13 北京航空航天大学 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222582A (zh) * 2023-05-10 2023-06-06 北京航空航天大学 一种基于变分贝叶斯推断多物理场自适应组合导航方法
CN116222582B (zh) * 2023-05-10 2023-07-25 北京航空航天大学 一种基于变分贝叶斯推断多物理场自适应组合导航方法
CN116255988A (zh) * 2023-05-11 2023-06-13 北京航空航天大学 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法

Similar Documents

Publication Publication Date Title
CN115790580A (zh) 基于变分贝叶斯自适应滤波的直升机着舰相对导航方法
CN109655066B (zh) 一种基于Q(λ)算法的无人机路径规划方法
CN110109470B (zh) 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统
CN108227735B (zh) 基于视觉飞行自稳定的方法、计算机可读介质和系统
Wu Coordinated path planning for an unmanned aerial-aquatic vehicle (UAAV) and an autonomous underwater vehicle (AUV) in an underwater target strike mission
Mammarella et al. Machine vision/GPS integration using EKF for the UAV aerial refueling problem
JP2931348B2 (ja) 慣性空間中の目標の位置および速度を決定する方法およびシステム
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
Wang et al. On vision-based target tracking and range estimation for small UAVs
Kim et al. Development of an electro-optical system for small UAV
Gur fil et al. Partial aircraft state estimation from visual motion using the subspace constraints approach
CN112498744B (zh) 纵横向松耦合在线轨迹规划方法及电子设备
CN113295162A (zh) 基于无人机状态信息的广义因子图融合导航方法
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
Cho et al. Autonomous ship deck landing of a quadrotor UAV using feed-forward image-based visual servoing
CN113268074A (zh) 一种基于联合优化的无人机航迹规划方法
CN107957686B (zh) 基于预见控制的无人直升机自动着舰控制系统
Lin et al. Design of anti-interference system for fully autonomous UAV based on ADRC-EKF algorithm
Cristofalo et al. Vision-based control for fast 3-d reconstruction with an aerial robot
CN113587926A (zh) 一种航天器空间自主交会对接相对导航方法
Kawamura Integrated targeting, guidance, navigation, and control for unmanned aerial vehicles
Emter et al. Stochastic cloning for robust fusion of multiple relative and absolute measurements
CN117369507A (zh) 一种自适应粒子群算法的无人机动态路径规划方法
Liu et al. Multi-UAV cooperative navigation algorithm based on federated filtering structure
Wu et al. Improved EKF-SLAM algorithm of unmanned helicopter autonomous landing on ship

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