一种基于多源信息融合的目标交汇制导方法

1.本发明涉及飞行器空中对接的控制方法，具体涉及一种基于多源信息融合的目标交汇制导方法。


背景技术：

2.合作目标运动信息的获取和估计是无人机执行对接任务的重点关注问题。目前，对于合作目标的定位和运动状态的估计主要依赖于全球卫星定位系统(gps)和机载视觉。在遮挡和其它信号干扰下，gps定位精度不够理想，当无人机靠近合作目标时，机身之间可能会由于遮挡而影响卫星信号。而单一基于机载视觉的相对位姿估计方法，对识别背景和光线有一定要求，并且目标检测距离有限，无法满足全过程目标定位要求。
3.无人机与飞机、车辆等合作目标的结合能够大幅增强无人机的续航能力和部署半径，但对无人机与合作目标的精准对接都有较高要求，而实现精准对接的前提就是对合作目标运动状态的准确获取。现有方法大致分为三类：一、单纯依赖全球卫星定位系统(gps)获取目标运动状态，但其信号容易被干扰，尤其是接近合作目标时噪声较大；二、单纯利用机载视觉获取目标运动状态，但目标检测距离有限，且视觉算法对识别背景和光线有一定要求，无法满足全过程目标定位要求；三、利用机载视觉、gps等传感器进行多源信息融合，但现有融合算法计算计算复杂度大、没有考虑不同采样率传感器数据的时间同步性以及任一传感器失效后不适用的问题，工程应用上难以获得较高精度。
4.基于上述问题，本发明人对目标交汇制导方法做了深入研究，通过野值剔除、多速率算法、多源信息融合的方式获得了准确可靠的合作目标运动信息实时获得方案，基于此进行飞行器空中对接能够取得良好的对接效果。


技术实现要素：

5.为了克服上述问题，本发明人进行了锐意研究，设计出一种基于多源信息融合的目标交汇制导方法，该方法中，通过gps信号获得目标的初步位置，在无人机靠近目标的过程中通过其上搭载的视觉系统捕获目标；无人机的视觉系统捕获并锁定目标后，实时估计获得无人机和目标构成系统的状态向量；基于状态向量中的信息控制无人机的飞行姿态，直至完成无人机和目标的对接；其中，利用融合gps和机载视觉信息的目标运动状态估计方法，避免了gps在遮蔽情况下精度不足、机载视觉出现图像检漏等导致的信息不准确问题，运用野值剔除方法，去除传感器错误的量测值从而解决传感器失效问题；从而完成本发明。
6.具体来说，本发明的目的在于提供一种基于多源信息融合的目标交汇制导方法，该方法包括如下步骤：
7.步骤1，通过gps信号获得目标的初步位置，在无人机靠近目标的过程中通过其上搭载的视觉系统捕获目标；
8.步骤2，在无人机的视觉系统捕获并锁定目标后，实时估计获得无人机和目标构成系统的状态向量；
9.步骤3，基于状态向量中的信息控制无人机的飞行姿态，直至完成无人机和目标的对接；
10.xk表示所述状态向量，
11.xk＝(δx,δy,δz,δv
x
,δvy,δvz,a
tx
,a
ty
,a
tz
,a
ux
,a
uy
,a
uz
)
t
12.其中，δx表示在惯性坐标系中目标与无人机相对位置的x轴分量；
13.δy表示在惯性坐标系中目标与无人机相对位置的y轴分量；
14.δz表示在惯性坐标系中目标与无人机相对位置的z轴分量；
15.δv
x
表示在惯性坐标系中目标与无人机相对速度的x轴分量；
16.δvy表示在惯性坐标系中目标与无人机相对速度的y轴分量；
17.δvz表示在惯性坐标系中目标与无人机相对速度的z轴分量；
18.a
tx
表示在惯性坐标系中目标加速度的x轴分量；
19.a
ty
表示在惯性坐标系中目标加速度的y轴分量；
20.a
tz
表示在惯性坐标系中目标加速度的z轴分量；
21.a
ux
表示在惯性坐标系中无人机加速度的x轴分量；
22.a
uy
表示在惯性坐标系中无人机加速度的y轴分量；
23.a
uz
表示在惯性坐标系中无人机加速度的z轴分量。
24.所述状态向量实时通过下式(一)迭代获得：
25.x
k|k
＝x
k|k-1
+kk(z
k-h(x
k|k-1
))
ꢀꢀꢀꢀ
(一)
26.其中，x
k|k
表示k时刻的状态向量；
27.x
k|k-1
表示k时刻状态向量的先验估计值；
28.kk表示k时刻的卡尔曼增益；
29.zk表示k时刻的量测输出量；
30.h()表示非线性的量测方程。
31.本发明所具有的有益效果包括：
32.(1)、根据本发明提供的基于多源信息融合的目标交汇制导方法，为了降低状态估计中使用的量测信息对估计结果的影响，先对量测信息进行野值剔除，再考虑更新频率不一致问题，使用多模型多速率扩展卡尔曼滤波器进行系统状态变量的估计；
33.(2)、根据本发明提供的基于多源信息融合的目标交汇制导方法，针对单一传感器易受干扰导致获取信息不准确的问题，利用融合gps和机载视觉信息的目标运动状态估计方法，避免了gps在遮蔽情况下精度不足、机载视觉出现图像检漏等导致的信息不准确问题；
34.(3)、根据本发明提供的基于多源信息融合的目标交汇制导方法，运用野值剔除方法，去除传感器错误的量测值从而解决传感器失效问题；
35.(4)、根据本发明提供的基于多源信息融合的目标交汇制导方法，运用多采样率联邦滤波算法，在同一时刻根据不同采样率的传感器数据确定不同的量测矩阵、噪声协方差矩阵和量测输出，从而解决传感器采样频率不一致的问题。
附图说明
36.图1示出本技术的基于多源信息融合的目标交汇制导方法整体逻辑示意图；
37.图2示出实施例中两个对接飞行器的相对位置曲线；
38.图3示出实施例中两个对接飞行器的相对速度曲线；
39.图4示出实施例中两个对接飞行器的视线角曲线
40.图5示出实施例中两个对接飞行器的视线角速率曲线；
41.图6示出实施例中两个对接飞行器的深度信息曲线；
42.图7示出实施例中两个对接飞行器的相对位置估计曲线；
43.图8示出实施例中两个对接飞行器的相对速度估计曲线。
具体实施方式
44.下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。
45.在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
46.根据本发明提供的一种基于多源信息融合的目标交汇制导方法，如图1中所示，该方法包括如下步骤：
47.步骤1，通过gps信号获得目标的初步位置，在无人机靠近目标的过程中通过其上搭载的视觉系统捕获目标；所述gps信号包括目标提供的基于目标接收的gps信号解算获得的目标位置，还包括无人机上搭载的gps接收机接收卫星信号转化得到的位置信息。
48.步骤2，在无人机的视觉系统捕获并锁定目标后，实时估计获得无人机和目标构成系统的状态向量；
49.步骤3，基于状态向量中的信息控制无人机的飞行姿态，直至完成无人机和目标的对接；从而完成预期的对接工作，可以完成无人机的回收、充电、信息交互、携带物资的更换补给等作业。本技术中，在步骤3中，基于状态向量的具体控制方案可以采用贾配洋,彭晓东,周武根.四旋翼无人机自主移动降落方法研究[j].计算机科学,2017,44(s2):520-523.中给出的控制方案。
[0050]
xk表示所述状态向量，该状态向量是实时更新的。
[0051]
xk＝(δx,δy,δz,δv
x
,δvy,δvz,a
tx
,a
ty
,a
tz
,a
ux
,a
uy
,a
uz
)
t
[0052]
其中，δx表示在惯性坐标系中目标与无人机相对位置的x轴分量；
[0053]
δy表示在惯性坐标系中目标与无人机相对位置的y轴分量；
[0054]
δz表示在惯性坐标系中目标与无人机相对位置的z轴分量；
[0055]
δv
x
表示在惯性坐标系中目标与无人机相对速度的x轴分量；
[0056]
δvy表示在惯性坐标系中目标与无人机相对速度的y轴分量；
[0057]
δvz表示在惯性坐标系中目标与无人机相对速度的z轴分量；
[0058]atx
表示在惯性坐标系中目标加速度的x轴分量；
[0059]aty
表示在惯性坐标系中目标加速度的y轴分量；
[0060]atz
表示在惯性坐标系中目标加速度的z轴分量；
[0061]aux
表示在惯性坐标系中无人机加速度的x轴分量；
[0062]auy
表示在惯性坐标系中无人机加速度的y轴分量；
[0063]auz
表示在惯性坐标系中无人机加速度的z轴分量。
[0064]
在一个优选的实施方式中，所述状态向量实时通过下式(一)迭代获得：
[0065]
x
k|k
＝x
k|k-1
+kk(z
k-h(x
k|k-1
))
ꢀꢀꢀꢀ
(一)
[0066]
其中，x
k|k
表示k时刻的状态向量；
[0067]
x
k|k-1
表示k时刻状态向量的先验估计值；
[0068]kk
表示k时刻的卡尔曼增益；
[0069]
zk表示量测输出量；
[0070]
h()表示非线性的量测方程。
[0071]
优选地，所述k时刻状态向量的先验估计值x
k|k-1
通过下式(二)获得：
[0072]
x
k|k-1
＝a
·
x
k-1|k-1
ꢀꢀꢀ
(二)
[0073]
其中，a表示状态转移矩阵；
[0074]
x
k-1|k-1
表示k-1时刻的状态向量，即为上一时刻通过式一获得的状态变量。在第一次通过式一获得状态向量时，该上一时刻的状态变量即该x
k-1|k-1
的初始值的前9个元素通过一次测量获得，后三个元素取值为0。
[0075]
优选地，所述状态转移矩阵a如下式(三)所示：
[0076][0077]
其中，δt表示扩展卡尔曼滤波器的迭代时间周期，本技术中δt的取值优选为0.02s。
[0078]
优选地，k时刻的卡尔曼增益kk表通过下式(四)获得：
[0079]kk
＝p
k|k-1hkt
(hkp
k|k-1hkt
+rk)-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(四)
[0080]
其中，p
k|k-1
表示k时刻状态向量先验估计值x
k|k-1
的估计误差协方差；
[0081]hk
表示k时刻的量测矩阵；
[0082]rk
表示协方差噪声矩阵。
[0083]
优选地，k时刻状态向量先验估计值x
k|k-1
的估计误差协方差p
k|k-1
过下式(五)获得：
[0084]
p
k|k-1
＝a
·
p
k-1|k-1
·at
+qkꢀꢀꢀꢀꢀ
(五)
[0085]
其中，a表示状态转移矩阵；
[0086]
p
k-1|k-1
表示k-1时刻状态向量x
k-1|k-1
的估计误差协方差；即上一时刻获得的p
k|k
，在第一次通过式十七获得p
k|k
时，该上一时刻的状态变量即该p
k-1|k-1
的初始值取值根据对状态向量各分量的误差经验选取，例如可以设置为p0＝(1,1,1,1,1,1,0.5,0.5,0.5,3,3,3)
t
；
[0087]
q表示k时刻的过程噪声协方差矩阵；
[0088]
具体取值需要根据外场试验进行参数调节；
[0089]
例如可以取值为q＝diag[01×3,01×3,0.011×3,0.51×3]。
[0090]
k时刻的过程噪声协方差矩阵的具体取值根据外场试验进行参数调节。
[0091]
在一个优选的实施方式中，当无人机在k时刻获得gps信号，且无人机的视觉系统在k时刻获得观测信号时，
[0092]
所述k时刻的量测矩阵hk如下式(六)所示：
[0093][0094]
当无人机在k时刻获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，
[0095]
所述k时刻的量测矩阵hk如下式(七)所示：
[0096][0097]
当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻获得观测信号时，
[0098]
所述k时刻的量测矩阵hk如下式(八)所示：
[0099][0100]
当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，
[0101]
所述k时刻的量测矩阵hk如下式(九)所示：
[0102][0103]
其中，其中，qy表示俯仰视线角，
[0104]
qz表示偏航视线角，
[0105]
表示俯仰视线角速度，
[0106]
表示偏航视线角速度，
[0107]
d表示测量深度。
[0108]
在一个优选的实施方式中，当无人机在k时刻获得gps信号，且无人机的视觉系统在k时刻获得观测信号时，
[0109]
所述协方差噪声矩阵r为下述式(十)所示的对角矩阵：
[0110][0111]
当无人机在k时刻获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，
[0112]
所协方差噪声矩阵r为下述式(十一)所示的对角矩阵：
[0113][0114]
当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻获得观测信号时，
[0115]
所述协方差噪声矩阵r为下述式(十二)所示的对角矩阵：
[0116][0117]
当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，
[0118]
所述协方差噪声矩阵r为下述式(十三)所示的对角矩阵：
[0119][0120]
其中，各自独立地表示预设值。本技术中，所述协方差矩阵r根据传感器噪声确定，是基于硬件传感器设置的固定值，例如可以设置为r＝diag[11×3,0.5
21
×3,0.5
21
×3,0.0524
21
×2,0.014
21
×2,1]。
[0121]
优选地，所述量测矩阵hk中具体元素的取值都是基于gps信号及视觉系统给出的
量测值获得。
[0122]
本技术中，通过下述方法判断无人机是否获得gps信号以及是否获得视觉系统的观测信号；
[0123]
滤波器的运算周期为δt，即输出状态向量的周期为δt；gps信号的获得周期为t1，视觉系统观测信号的输出周期为t2，设定tj＝nj·
δt,nj∈z
+
，令αδt表示t1和t2的最小公倍数，kδt表示从步骤2开始，获得第k组无人机和目标构成系统的状态向量所经历的时间；
[0124]
当f
state,1
＝0&f
state,2
＝0&mod(k,nj)＝0&mod(k,α)≠0时，认为无人机在k时刻获得gps信号，且无人机的视觉系统在k时刻获得观测信号；
[0125]
当f
state,1
＝0&mod(k,α)＝0时，无人机在k时刻获得gps信号，无人机的视觉系统在k时刻未获得观测信号；
[0126]
当f
state,2
＝0&mod(k,α)＝0时，无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻获得观测信号；
[0127]
当不满足上述条件时，无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻未获得观测信号；
[0128]
其中，f
state,1
＝0表示通过gps信号测量得到的观测信息量，对应的预测残差使得判别式成立，
[0129]fstate,2
＝0表示通过视觉系统测量得到的观测信息量，对应的预测残差使得判别式成立；
[0130]
mod(k,ni)＝0表示k能够被nj整除；
[0131]
mod(k,α)＝0表示k能够被α整除；
[0132]
mod(k,α)≠0表示k不能够被α整除；
[0133]
所述通过gps信号测量得到的观测信息量包括：
[0134]
δx,δy,δz,δv
x
,δvy,δvz；
[0135]
所述通过视觉系统测量得到的观测信息量包括：
[0136][0137]
所述预测残差dk通过下式(十四)获得：
[0138]dk
＝zk′‑hk
x
k|k-1 (十四)
[0139][0140]
所述判别式如式(十五)所示：
[0141][0142]dk
(i)表示dk中的第i个元素；此处，当需要判断通过gps信号测量得到的观测信息量时，所述i的取值为1-6中的任意整数，当需要判断通过视觉系统测量得到的观测信息量时，所述i的取值为10-14中的任意整数；
[0143]
c表示常量；其取值为3或4，优选的为3。
[0144]
在一个优选的实施方式中，所述k时刻的量测输出量zk通过下述子步骤获得；
[0145]
子步骤1，通过接收到的gps信号和视觉系统的观测信号获得k时刻初步的量测输
出量zk′
，
[0146]
其中，
[0147]
子步骤2，通过下式(十四)获得k时刻的预测残差dk，
[0148]dk
＝zk′‑hk
x
k|k-1
ꢀꢀꢀ
(十四)
[0149]
其中，hk表示k时刻的量测矩阵；
[0150]
x
k|k-1
表示k时刻状态向量的先验估计值；
[0151]
子步骤3，通过下式(十五)逐一判断dk中每个元素是否为野值，
[0152][0153]
其中，i的取值为1-14中的任意整数；
[0154]dk
(i)表示dk中的第i个元素；
[0155]
c表示常量；取值为3或4，优选地取值为3。
[0156]
p
k|k-1
表示k时刻状态向量先验估计值x
k|k-1
的估计误差协方差；
[0157]
r表示协方差噪声矩阵；
[0158]r(i,i)
表示r中对角线上的第i个元素；
[0159]
使得式(十五)成立的dk中元素对应的zk′
中元素为正确观测量，使得式(十五)不成立的dk中元素对应的zk′
中元素为野值；例如，zk′
中第5个元素为δvy，对应的dk中第5个元素如果使得式(十五)成立，则δvy为正确观测量；对应的dk中第5个元素如果使得式(十五)不成立，则δvy为野值。
[0160]
子步骤4，将zk′
中判断为野值的元素替换为0，得到k时刻的量；
[0161]
在一个优选的实施方式中，所述h()的输出量为：
[0162][0163]
该输出量通过下式(十六)获得：
[0164][0165]
其中，qy表示俯仰视线角，
[0166]
qz表示偏航视线角，
[0167]
表示俯仰视线角速度，
[0168]
表示偏航视线角速度，
[0169]
d表示测量深度；
[0170]
在式十六中，qy在二象限中求解，qy在四象限中求解；本技术中，式十六中等号右侧的参数的取值都来自于x
k|k-1
。
[0171]
在一个优选的实施方式中，在获得状态向量时，还同步获得k时刻状态向量x
k|k
的估计误差协方差p
k|k
；
[0172]
所述k时刻状态向量x
k|k
的估计误差协方差p
k|k
通过下式(十七)获得；
[0173]
p
k|k
＝(i-k
khk
)p
k|k-1
ꢀꢀꢀ
(十七)
[0174]
其中，i表示单位矩阵；
[0175]kk
表示k时刻的卡尔曼增益；
[0176]hk
表示k时刻的量测矩阵；
[0177]
p
k|k-1
表示k-1时刻状态向量x
k-1|k-1
的估计误差协方差。
[0178]
所述p
k|k
实时获得，并存储，以便于估计下一时刻的状态向量实时进行调取。
[0179]
实施例
[0180]
设置目标无人机，在目标无人机上装载贴有直径为30cm的apriltag的靶标作为视觉识别基准，模拟实际空中加油或空中回收中母机的对接靶标；
[0181]
对接无人机为四旋翼无人机，该四旋翼无人机上安装pixhawk固件，装载nx机载电脑，集成imu惯性测量元件，装载gps，在机头的正下方处安装光电吊舱，光电吊舱内部包括可见摄像头，用于视觉检测，还装有两自由度陀螺仪，可实现快速跟踪运动目标，并测量光轴旋转的角度和角速度。
[0182]
设定目标与无人机初始相对位置为地理坐标系下的(-16,6,-5)m，相对初始速度为地理坐标系下的(-2.4,-0.1,0)m/s；对接无人机初始速度为0m/s，目标无人机做初速度为(-2.4,-0.1,0)m/s后逐渐加速到(-5,0,0)m/s的直线运动。
[0183]
量测噪声矩阵为q＝diag[01×3,01×3,0.011×3,0.51×3]，
[0184]
过程噪声矩阵为r＝diag[11×3,0.5
21
×3,0.5
21
×3,0.0524
21
×2,0.014
21
×2,1]，
[0185]
对接无人机设置两架，分别进行具体实验，
[0186]
第一架对接无人机采用了pid控制方案进行控制，识别目标并控制其自身飞向目标，所述pid控制方案方案详见张立.基于串级pid控制算法的四旋翼无人机控制系统设计[j].信息技术与信息化,2023(01):100-103.
[0187]
第二架对接无人机上灌装本技术提供的基于多源信息融合的目标交汇制导方法；该基于多源信息融合的目标交汇制导方法包括如下步骤：
[0188]
步骤1，通过gps信号获得目标的初步位置，在无人机靠近目标的过程中通过其上搭载的视觉系统捕获目标；
[0189]
步骤2，在无人机的视觉系统捕获并锁定目标后，实时估计获得无人机和目标构成系统的状态向量；
[0190]
步骤3，基于状态向量中的信息控制无人机的飞行姿态，直至完成无人机和目标的对接；
[0191]
所述状态向量:
[0192]
xk＝(δx,δy,δz,δv
x
,δvy,δvz,a
tx
,a
ty
,a
tz
,a
ux
,a
uy
,a
uz
)
t
[0193]
状态向量实时通过下式(一)迭代获得：
[0194]
x
k|k
＝x
k|k-1
+kk(z
k-h(x
k|k-1
))
ꢀꢀꢀꢀꢀꢀꢀ
(一)
[0195]
其中，x
k|k
表示k时刻的状态向量；
[0196]
所述k时刻状态向量的先验估计值x
k|k-1
通过下式(二)获得：
[0197]
x
k|k-1
＝a
·
x
k-1|k-1
ꢀꢀꢀꢀꢀ
(二)
[0198]
所述状态转移矩阵a如下式(三)所示：
[0199][0200]
δt取值为0.02s；
[0201]
k时刻的卡尔曼增益kk通过下式(四)获得：
[0202]kk
＝p
k|k-1hkt
(hkp
k|k-1hkt
+rk)-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(四)
[0203]
k时刻状态向量先验估计值x
k|k-1
的估计误差协方p
k|k-1
通过下式(五)获得：
[0204]
p
k|k-1
＝a
·
p
k-1|k-1
·at
+q
ꢀꢀꢀꢀꢀ
(五)
[0205]
所述k时刻的量测矩阵hk和协方差噪声矩阵r都基于k时刻是否获得gps信号和视觉系统的观测信号来确定：
[0206]
当无人机在k时刻获得gps信号，且无人机的视觉系统在k时刻获得观测信号时，
[0207]
所述k时刻的量测矩阵hk如下式(六)所示：
[0208][0209]
当无人机在k时刻获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，
[0210]
所述k时刻的量测矩阵hk如下式(七)所示：
[0211][0212]
当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻获得观测信号时，
[0213]
所述k时刻的量测矩阵hk如下式(八)所示：
[0214][0215]
当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，
[0216]
所述k时刻的量测矩阵hk如下式(九)所示：
[0217][0218]
关于协方差噪声矩阵r：
[0219]
当无人机在k时刻获得gps信号，且无人机的视觉系统在k时刻获得观测信号时，
[0220]
所述协方差噪声矩阵r为下述式(十)所示的对角矩阵：
[0221]
r＝diag[11×3,0.5
21
×3,0.5
21
×3,0.0524
21
×2,0.014
21
×2,1](十)
[0222]
当无人机在k时刻获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，
[0223]
所协方差噪声矩阵r为下述式(十一)所示的对角矩阵：
[0224]
r＝diag[11×3,0.5
21
×3,0.5
21
×3,0,0,0,0,0](十一)
[0225]
当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻获得观测信号时，
[0226]
所述协方差噪声矩阵r为下述式(十二)所示的对角矩阵：
[0227]
r＝diag[0,0,0,0,0,0,0.5
21
×3,0.0524
21
×2,0.014
21
×2,1](十二)
[0228]
当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，
[0229]
所述协方差噪声矩阵r为下述式(十三)所示的对角矩阵：
[0230]
r＝diag[0,0,0,0,0,0,0.5
21
×3,0,0,0,0,0]
ꢀꢀꢀ
(十三)
[0231]
所述k时刻的量测输出量zk通过下述子步骤获得；
[0232]
子步骤1，通过接收到的gps信号和视觉系统的观测信号获得k时刻初步的量测输出量zk′
，
[0233]
其中，
[0234]
子步骤2，通过下式(十四)获得k时刻的预测残差dk，
[0235]dk
＝zk′‑hk
x
k|k-1
ꢀꢀꢀ
(十四)
[0236]
子步骤3，通过下式(十五)逐一判断dk中每个元素是否为野值，
[0237][0238]
其中，i的取值为1-14中的任意整数；
[0239]dk
(i)表示dk中的第i个元素；
[0240]
c表示常量；取值为3；
[0241]r(i,i)
表示r中对角线上的第i个元素；
[0242]
使得式(十五)成立的dk中元素对应的zk′
中元素为正确观测量，使得式(十五)不成立的dk中元素对应的zk′
中元素为野值；
[0243]
子步骤4，将zk′
中判断为野值的元素替换为0，得到k时刻的量测输出量zk。
[0244]
所述非线性的量测方程h()输出量通过下式(十六)获得：：
[0245][0246]
所述k时刻状态向量x
k|k
的估计误差协方差p
k|k
通过下式(十七)获得；
[0247]
p
k|k
＝(i-k
khk
)p
k|k-1
(十七)
[0248]
在具体实验过程中，人为地控制gps信息从5.24秒到7.24秒内不更新，两个对接飞行器获得的gps量测曲线包括如图2中所示的相对位置曲线和如图3中所示的相对速度曲线，两个飞行器获得的光电吊舱量测曲线包括如图4中所示的视线角曲线和如图5中所示的视线角速率曲线；两个飞行器获得的深度信息曲线如图6中所示。
[0249]
最终得到的两个飞行器的相对位置估计曲线如图7中所示，两个飞行器的相对速度估计曲线如图8中所示。
[0250]
根据图7和图8所示，在单个传感器失效时，第一个飞行器，的控制方案不能克服由此带来的数据异常，在gps不更新期间，估计结果不准确，有发散的趋势，并且在系统状态变量值较大时，发散越明显。
[0251]
第二个飞行器，即本技术提供的基于多源信息融合的目标交汇制导方法在gps不更新期间，仍然能够较好地保持真实的运动物理特性，能够有效的抑制其带来的负面影响，给无人机制导算法提供一个相对可靠的信息源；同时，还提高了信息源的频率，使得制导指令的信息源频率不再过分依赖于传感器采样频率。在2秒的gps失效状况中，位置估计误差《0.5m，速度估计误差《0.4m/s，加速度估计误差《0.8m/s。
[0252]
以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。

技术特征：
1.一种基于多源信息融合的目标交汇制导方法，其特征在于，该方法包括如下步骤：步骤1，通过gps信号获得目标的初步位置，在无人机靠近目标的过程中通过其上搭载的视觉系统捕获目标；步骤2，在无人机的视觉系统捕获并锁定目标后，实时估计获得无人机和目标构成系统的状态向量；步骤3，基于状态向量中的信息控制无人机的飞行姿态，直至完成无人机和目标的对接；x
k
表示所述状态向量，x
k
＝(δx,δy,δz,δv
x
,δv
y
,δv
z
,a
tx
,a
ty
,a
tz
,a
ux
,a
uy
,a
uz
)
t
其中，δx表示在惯性坐标系中目标与无人机相对位置的x轴分量；δy表示在惯性坐标系中目标与无人机相对位置的y轴分量；δz表示在惯性坐标系中目标与无人机相对位置的z轴分量；δv
x
表示在惯性坐标系中目标与无人机相对速度的x轴分量；δv
y
表示在惯性坐标系中目标与无人机相对速度的y轴分量；δv
z
表示在惯性坐标系中目标与无人机相对速度的z轴分量；a
tx
表示在惯性坐标系中目标加速度的x轴分量；a
ty
表示在惯性坐标系中目标加速度的y轴分量；a
tz
表示在惯性坐标系中目标加速度的z轴分量；a
ux
表示在惯性坐标系中无人机加速度的x轴分量；a
uy
表示在惯性坐标系中无人机加速度的y轴分量；a
uz
表示在惯性坐标系中无人机加速度的z轴分量。2.根据权利要求1所述的基于多源信息融合的目标交汇制导方法，其特征在于，所述状态向量实时通过下式(一)迭代获得：x
k|k
＝x
k|k-1
+k
k
(z
k-h(x
k|k-1
))
ꢀꢀꢀꢀ
(一)其中，x
k|k
表示k时刻的状态向量；x
k|k-1
表示k时刻状态向量的先验估计值；k
k
表示k时刻的卡尔曼增益；z
k
表示k时刻的量测输出量；h()表示非线性的量测方程。3.根据权利要求2所述的基于多源信息融合的目标交汇制导方法，其特征在于，所述k时刻状态向量的先验估计值x
k|k-1
通过下式(二)获得：x
k|k-1
＝a
·
x
k-1|k-1
ꢀꢀꢀ
(二)其中，a表示状态转移矩阵；x
k-1|k-1
表示k-1时刻的状态向量；优选地，所述状态转移矩阵a如下式(三)所示：
其中，δt表示扩展卡尔曼滤波器的迭代时间周期。4.根据权利要求2所述的基于多源信息融合的目标交汇制导方法，其特征在于，所述k时刻的卡尔曼增益k
k
通过下式(四)获得：k
k
＝p
k|k-1
h
kt
(h
k
p
k|k-1
h
kt
+r
k
)-1
ꢀꢀꢀꢀꢀ
(四)其中，p
k|k-1
表示k时刻状态向量先验估计值x
k|k-1
的估计误差协方差；h
k
表示k时刻的量测矩阵；r
k
表示k时刻的协方差噪声矩阵。5.根据权利要求4所述的基于多源信息融合的目标交汇制导方法，其特征在于，k时刻状态向量先验估计值x
k|k-1
的估计误差协方差p
k|k-1
通过下式(五)获得：p
k|k-1
＝a
·
p
k-1|k-1
·
a
t
+q
ꢀꢀꢀꢀ
(五)其中，a表示状态转移矩阵；p
k-1|k-1
表示k-1时刻状态向量x
k-1|k-1
的估计误差协方差；q表示过程噪声协方差矩阵。6.根据权利要求4所述的基于多源信息融合的目标交汇制导方法，其特征在于，当无人机在k时刻获得gps信号，且无人机的视觉系统在k时刻获得观测信号时，所述k时刻的量测矩阵h
k
如下式(六)所示：
当无人机在k时刻获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，所述k时刻的量测矩阵h
k
如下式(七)所示：当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻获得观测信号时，所述k时刻的量测矩阵h
k
如下式(八)所示：
当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，所述k时刻的量测矩阵h
k
如下式(九)所示：其中，其中，q
y
表示俯仰视线角，q
z
表示偏航视线角，表示俯仰视线角速度，表示偏航视线角速度，d表示测量深度。7.根据权利要求4所述的基于多源信息融合的目标交汇制导方法，其特征在于，
当无人机在k时刻获得gps信号，且无人机的视觉系统在k时刻获得观测信号时，所述协方差噪声矩阵r为下述式(十)所示的对角矩阵：当无人机在k时刻获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，所协方差噪声矩阵r为下述式(十一)所示的对角矩阵：当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻获得观测信号时，所述协方差噪声矩阵r为下述式(十二)所示的对角矩阵：当无人机在k时刻未获得gps信号，无人机的视觉系统在k时刻未获得观测信号时，所述协方差噪声矩阵r为下述式(十三)所示的对角矩阵：其中，各自独立地表示预设值。8.根据权利要求2所述的基于多源信息融合的目标交汇制导方法，其特征在于，所述k时刻的量测输出量z
k
通过下述子步骤获得；子步骤1，通过接收到的gps信号和视觉系统的观测信号获得k时刻初步的量测输出量z
k
′
，其中，子步骤2，通过下式(十四)获得k时刻的预测残差d
k
，d
k
＝z
k
′‑
h
k
x
k|k-1
ꢀꢀꢀꢀ
(十四)其中，h
k
表示k时刻的量测矩阵；x
k|k-1
表示k时刻状态向量的先验估计值；子步骤3，通过下式(十五)逐一判断d
k
中每个元素是否为野值，其中，i的取值为1-14中的任意整数；d
k
(i)表示d
k
中的第i个元素；c表示常量；p
k|k-1
表示k时刻状态向量先验估计值x
k|k-1
的估计误差协方差；r表示协方差噪声矩阵；r
(i,i)
表示r中对角线上的第i个元素；使得式(十五)成立的d
k
中元素对应的z
k
′
中元素为正确观测量，使得式(十五)不成立的d
k
中元素对应的z
k
′
中元素为野值；子步骤4，将z
k
′
中判断为野值的元素替换为0，得到k时刻的量测输出量z
k
。9.根据权利要求2所述的基于多源信息融合的目标交汇制导方法，其特征在于，所述非线性的量测方程h()的输出量为：
该输出量通过下式(十六)获得：其中，q
y
表示俯仰视线角，q
z
表示偏航视线角，表示俯仰视线角速度，表示偏航视线角速度，d表示测量深度。10.根据权利要求2所述的基于多源信息融合的目标交汇制导方法，其特征在于，在获得k时刻的状态向量x
k|k
时，还同步获得k时刻状态向量x
k|k
的估计误差协方差p
k|k
；所述k时刻状态向量x
k|k
的估计误差协方差p
k|k
通过下式(十七)获得；p
k|k
＝(i-k
k
h
k
)p
k|k-1
ꢀꢀꢀ
(十七)其中，i表示单位矩阵；k
k
表示k时刻的卡尔曼增益；h
k
表示k时刻的量测矩阵；p
k|k-1
表示k-1时刻状态向量x
k-1|k-1
的估计误差协方差。

技术总结
本发明公开了一种基于多源信息融合的目标交汇制导方法，该方法中，采用目标能观性主动增强制导律进行制导控制，在目标能观性主动增强制导律中，将弹目视线角速率的积分作为最优控制的定量指标，通过将弹目视线角速率的积分最大化来实现目标能观性的最大化，最优控制的优化目标由终端零控脱靶量和飞行过程中的目标能观性指标来定义，同时希望飞行过程中能量消耗尽量小。在飞行器末制导过程的初始阶段，着重使目标能观性最大化，而在飞行器即将命中目标时，着重使命中精度最优，由此，可以使制导律实现同时具有增强能观性以获得良好估计性能和保证命中精度的双重作用。计性能和保证命中精度的双重作用。计性能和保证命中精度的双重作用。


技术研发人员：张志远 宋韬 李若萱 冷晏龙
受保护的技术使用者：北京理工大学
技术研发日：2023.07.24
技术公布日：2023/10/11