一种基于视觉的无人机组合导航方法与流程

1.本发明属于无人机技术领域，具体涉及一种基于视觉的无人机组合导航方法。


背景技术：

2.目前，无人机技术的发展突飞猛进，基于无人机的应用领域也越来越广泛。在军事侦查、战场监视、火灾探测、环境与交通监测方面都得到了广泛的应用。无人机在执行任务期间周围环境复杂多变，为了顺利完成任务，优秀的组合导航方法至关重要。目前无人机常用的定位方法为卫星惯性组合导航算法，但是卫星导航系统容易受到干扰或者欺骗，造成无人机无法定位，或者定位不准确，为了提高在gps拒止情况下无人机的生存能力，需要研究一种自主导航方法。
3.因此研究一种自主组合导航方法是解决这一问题的关键。


技术实现要素：

4.本发明的目的在于克服上述现有技术的不足，提供一种基于视觉的无人机组合导航方法。
5.为了解决技术问题，本发明的技术方案是：一种基于视觉的无人机组合导航方法，包括以下步骤：
6.步骤1：首先通过视觉系统进行视觉信息采集，然后通过采集到的图像信息与标签库进行匹配，匹配到标签之后输出识别信息，根据识别信息计算无人机相对于标签中心的位置信息；
7.步骤2：选取组合导航的状态向量xk，构建状态向量的预测方程，然后选取步骤1的位置信息作为量测，构建量测预测方程，计算量测矩阵hk，通过量测向量在ekf滤波中对状态向量进行修正，完成组合导航模型的建立；
8.步骤3：在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，完成基于视觉的无人机组合导航方法设计。
9.优选的，所述步骤1具体为：
10.步骤1-1：通过视觉系统获取到图像信息后与标签库进行匹配，匹配到标签之后输出识别信息；
11.步骤1-2：对标签坐标系和图像像素坐标系进行定义，标签坐标系的定义：坐标原点位于标签中心，x轴指向标签前方，y轴指向标签右方；图像像素坐标系的定义：图像像素坐标系的原点在图像的左上角，坐标单位为像素，单个像素的大小为w；因此识别信息包括：标签x轴长度在图像像素坐标系中的像素个数lx
p
、标签y轴长度在图像像素坐标系中的像素个数ly
p
、标签中心相对于图像中心的距离在图像像素坐标系x轴所占像素的大小dx
p
、标签中心相对于图像中心的距离在图像像素坐标系y轴所占像素的大小dy
p
，以及目标识别状态；
12.步骤1-3：根据标签在图像像素坐标系的大小(lx
p
，ly
p
)、标签实际大小(lx，ly)、单
个像素的大小w以及标签中心相对于图像中心的距离在图像像素坐标系所占像素的大小(dx
p
，dy
p
)计算无人机相对于标签中心的位置信息，即水平位置和垂直位置；
13.所述水平位置计算公式为：
[0014][0015][0016]
其中：
[0017]
dx为无人机距离标签的x方向的距离；
[0018]
dy为无人机距离标签的y方向的距离；
[0019]
所述垂直位置计算公式为：
[0020][0021]
其中：
[0022]
dz为无人机距离标签的z方向的距离；
[0023]
f为视觉系统的相机焦距。
[0024]
优选的，所述步骤1-1中的标签为正方形，标签中可嵌套多个标签图案，最大的标签图案边长为1m，最小的标签图案为0.05m。
[0025]
优选的，所述步骤1-2中的目标识别状态包括图像异常、查询无目标、识别结果不稳定和识别稳定。
[0026]
优选的，所述步骤2具体为：
[0027]
步骤2-1：选取无人机的姿态四元数、速度、位置、陀螺零偏与加速度计零偏为状态向量xk，即：
[0028]
其中q0为无人机姿态四元数实部，q1、q2、q3为无人机姿态四元数虚部，vn为无人机北向速度，ve为无人机东向速度，vd为无人机地向速度，pn为无人机北向位置，pe为无人机东向位置，pd为无人机地向位置，ε
x
为x轴陀螺零偏，εy为y轴陀螺零偏，εz为z轴陀螺零偏，为x轴加速度计零偏，为y轴加速度计零偏，为z轴加速度计零偏；
[0029]
步骤2-2：构建状态向量的预测方程；
[0030][0031]
其中：
[0032]
x
k-1
为k-1时刻状态向量；
[0033]
φ
k/k-1
为状态转移矩阵；
[0034]
为k时刻状态向量预测值；
[0035]
通过计算状态向量预测值与状态向量的雅克比矩阵计算出状态转移矩阵φ
k/k-1
；
[0036]
步骤2-3：选取步骤1的位置信息作为无人机的位置量测zk＝[d
x d
y dz]
t
，构建无人机位置量测预测方程为：
[0037]
[0038]
其中：
[0039]
为无人机位置量测预测值；
[0040]
为无人机位置状态预测值；
[0041]hk
为量测矩阵，为3
×
3单位阵；
[0042]
通过计算位置量测预测值与位置状态预测值的雅克比矩阵即计算出位置的量测矩阵hk；
[0043]
步骤2-4：ekf滤波，利用量测向量在ekf滤波中对状态向量进行修正；
[0044]
步骤2-4-1：状态预测
[0045][0046]
步骤2-4-2：状态误差协方差矩阵预测
[0047][0048]
步骤2-4-3：滤波器增益
[0049][0050]
步骤2-4-4：状态误差协方差矩阵更新
[0051][0052]
步骤2-4-5：进行数据融合，对状态进行更新：
[0053][0054]
其中：
[0055]
p
k-1
为k-1时刻ekf滤波状态误差协方差矩阵；
[0056]
为k时刻ekf滤波状态误差协方差矩阵预测值；
[0057]qk-1
为k-1时刻系统噪声矩阵；
[0058]rk
为k时刻量测噪声矩阵；
[0059]kk
为k时刻滤波器增益矩阵；
[0060]
pk为k时刻ekf滤波状态误差协方差矩阵；
[0061]
为一步状态转移矩阵；
[0062]
为量测矩阵的转置；
[0063]
xk为状态向量；
[0064]
zk为量测向量。
[0065]
优选的，所述步骤2-2中无人机的姿态四元数预测方程为：
[0066][0067]
其中：
[0068]
为k时刻无人机姿态四元数预测；
[0069]qk-1
为k-1时刻无人机姿态四元数；
[0070]
δq为k-1至k时刻无人机姿态四元数变化量；
[0071]
为四元数相乘；
[0072]
k-1至k时刻无人机姿态四元数变化量计算公式为：
[0073][0074]
其中：
[0075]
δθ
x
为k-1至k时刻无人机x轴角增量；
[0076]
δθy为k-1至k时刻无人机y轴角增量；
[0077]
δθz为k-1至k时刻无人机z轴角增量；
[0078]
k-1至k时刻无人机各轴角增量计算公式为：
[0079]
δθ
x
＝(ω
x-ε
x
)
·
δt
[0080]
δθy＝(ω
y-εy)
·
δt
[0081]
δθz＝(ω
z-εz)
·
δt
[0082]
其中：
[0083]
ω
x
为无人机x轴陀螺输出的角速度；
[0084]
ε
x
为无人机x轴陀螺零偏；
[0085]
ωy为无人机y轴陀螺输出的角速度；
[0086]
εy为无人机y轴陀螺零偏；
[0087]
ωz为无人机z轴陀螺输出的角速度；
[0088]
εz为无人机z轴陀螺零偏；
[0089]
δt为k-1至k时刻时间间隔。
[0090]
优选的，所述步骤2-2中无人机的速度预测方程为：
[0091][0092]
其中：
[0093]
为无人机k时刻速度预测；
[0094]vk-1
为无人机k-1时刻速度；
[0095]
g0为无人机飞行地点当地重力加速度；
[0096]
为载体坐标系至导航坐标系的坐标转换矩阵；
[0097]
δv为k-1至k时刻速度增量；
[0098]
载体坐标系至导航坐标系的坐标转换矩阵的计算公式为：
[0099][0100]
k-1至k时刻速度增量的计算公式为：
[0101][0102][0103][0104]
其中：
[0105]ax
为无人机x轴加速度计输出的加速度；
[0106]
为无人机x轴加速度计零偏；
[0107]ay
为无人机y轴加速度计输出的加速度；
[0108]
为无人机y轴加速度计零偏；
[0109]az
为无人机z轴加速度计输出的加速度；
[0110]
为无人机z轴加速度计零偏。
[0111]
优选的，所述步骤2-2中无人机的位置预测方程为：
[0112][0113]
其中：
[0114]
为k时刻无人机位置状态预测值；
[0115]
p
k-1
为k-1时刻无人机位置；
[0116]vk-1
为k-1时刻无人机速度。
[0117]
优选的，所述步骤2-2中无人机陀螺零偏与加速度计零偏的预测方程为：
[0118][0119][0120]
其中：
[0121]
为k时刻无人机陀螺零偏预测值；
[0122]
ε
k-1
为k-1时刻无人机陀螺零偏；
[0123]
为k时刻无人机加速度计零偏预测值；
[0124]
为k-1时刻无人机加速度计零偏。
[0125]
优选的，所述步骤3中在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测具体为：在进行ekf滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行协方差矩阵正定性检测，即步骤2-4-4，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，即步骤2-4-1和步骤2-4-2；
[0126]
所述量测一致性检测方法为：
[0127]
假设北向位置量测为pn，北向位置量测预测值为将北向位置量测值与北向位置量测预测值做差记为假设量测噪声阵rk中对应的北向位置量测噪声为误差协方差矩阵中对应的北向位置误差为量测一致性检测公式如下：
[0128][0129]
其中k为系数；
[0130]
其它量测的一致性检测与北向位置的一致性检测相同；
[0131]
所述协方差矩阵正定性检测方法为：
[0132]
设在进行步骤2-4-4时，需要保证pk为正定矩阵，根据步骤2-4-4可知需要保证的每个对角线元素分别大于khp对应对角线元素，滤波器协方差矩阵正定性检测公式如下：
[0133][0134]
与现有技术相比，本发明的优点在于：
[0135]
(1)本发明提出的一种基于视觉的无人机组合导航方法，先通过视觉系统获取到
图像，计算得到无人机相对于标签中心的位置信息，接着选取组合导航的状态向量，构建状态向量的预测方程，然后选取视觉输出的位置信息作为量测，计算量测矩阵，利用量测向量在ekf滤波中对状态向量进行修正，完成组合导航模型的建立，在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，完成基于视觉的无人机组合导航方法设计，本发明将视觉系统得到的无人机相对于标签中心的位置信息和imu数据进行融合，可直接输出无人机高精度的姿态、速度与位置，不需要建立复杂的误差模型；
[0136]
(2)本发明提出的一种基于视觉的无人机组合导航方法，可在gps拒止环境下通过视觉系统对标签进行图像采样，然后根据标签信息，实时计算无人机相对于标签中心的位置信息，并实时输出，然后与imu数据融合，提供高精度的姿态、速度以及位置信息；
[0137]
(3)本发明提出的一种基于视觉的无人机组合导航方法，在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，当进行ekf滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行协方差矩阵正定性检测，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，将一致性检测设置在步骤2-4-2状态误差协方差矩阵预测和步骤2-4-3滤波器增益之间，如一致性检测未通过则不需计算滤波器增益，可大大减少cpu的计算量，本发明导航方法计算量小，实时性高；
[0138]
(4)本发明提出的一种基于视觉的无人机组合导航方法可实时对量测信息进行一致性检测，防止误差较大的量测对滤波器造成不利的影响，增加了系统的鲁棒性；本发明还可实时对滤波器协方差矩阵进行正定性检测，防止滤波器发散，并及时对滤波器进行调整。
附图说明
[0139]
图1为本发明标签坐标系示意图；
[0140]
图2为本发明图像像素坐标系示意图；
[0141]
图3为本发明水平位置计算示意图；
[0142]
图4为本发明垂直位置计算示意图；
[0143]
图5为本发明一种基于视觉的无人机组合导航方法流程图；
[0144]
图6为本发明序贯滤波的实施流程图；
[0145]
图7为本发明位置融合结果示意图；
[0146]
图8为本发明位置融合结果示意图。
具体实施方式
[0147]
下面结合实施例描述本发明具体实施方式：
[0148]
本发明提出的一种基于视觉的组合导航方法的具体实施方式如下：
[0149]
步骤1：首先通过视觉系统进行视觉信息采集，然后通过采集到的图像信息与标签库进行匹配，匹配到标签之后输出识别信息，根据识别信息计算无人机相对于标签中心的位置信息；
[0150]
步骤2：选取组合导航的状态向量xk，构建状态向量的预测方程，然后选取步骤1的位置信息作为量测，构建量测预测方程，计算量测矩阵hk，通过量测向量在ekf滤波中对状态向量进行修正，完成组合导航模型的建立；
[0151]
步骤3：在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，完成基
于视觉的无人机组合导航方法设计。
[0152]
优选的，所述步骤1具体为：
[0153]
步骤1-1：通过视觉系统获取到图像信息后与标签库进行匹配，匹配到标签之后输出识别信息；
[0154]
步骤1-2：对标签坐标系和图像像素坐标系进行定义，标签坐标系的定义：坐标原点位于标签中心，x轴指向标签前方，y轴指向标签右方；图像像素坐标系的定义：图像像素坐标系的原点在图像的左上角，坐标单位为像素，单个像素的大小为w；因此识别信息包括：标签x轴长度在图像像素坐标系中的像素个数lx
p
、标签y轴长度在图像像素坐标系中的像素个数ly
p
、标签中心相对于图像中心的距离在图像像素坐标系x轴所占像素的大小dx
p
、标签中心相对于图像中心的距离在图像像素坐标系y轴所占像素的大小dy
p
，以及目标识别状态；
[0155]
步骤1-3：根据标签在图像像素坐标系的大小(lx
p
，ly
p
)、标签实际大小(lx，ly)、单个像素的大小w以及标签中心相对于图像中心的距离在图像像素坐标系所占像素的大小(dx
p
，dy
p
)计算无人机相对于标签中心的位置信息，即水平位置和垂直位置；
[0156]
所述水平位置计算公式为：
[0157][0158][0159]
其中：
[0160]
dx为无人机距离标签的x方向的距离；
[0161]
dy为无人机距离标签的y方向的距离；
[0162]
所述垂直位置计算公式为：
[0163][0164]
其中：
[0165]
dz为无人机距离标签的z方向的距离；
[0166]
f为视觉系统的相机焦距。
[0167]
优选的，所述步骤1-1中的标签为正方形，标签中可嵌套多个标签图案，最大的标签图案边长为1m，最小的标签图案为0.05m。
[0168]
优选的，所述步骤1-2中的目标识别状态包括图像异常、查询无目标、识别结果不稳定和识别稳定。
[0169]
优选的，所述步骤2具体为：
[0170]
步骤2-1：选取无人机的姿态四元数、速度、位置、陀螺零偏与加速度计零偏为状态向量xk，即：
[0171]
其中q0为无人机姿态四元数实部，q1、q2、q3为无人机姿态四元数虚部，vn为无人机北向速度，ve为无人机东向速度，vd为无人机地向速度，pn为无人机北向位置，pe为无人机东向位置，pd为无人机地向位置，ε
x
为x轴陀螺零偏，εy为y轴陀螺零偏，εz为z轴陀螺零偏，为x轴加速度计零
偏，为y轴加速度计零偏，为z轴加速度计零偏；
[0172]
步骤2-2：构建状态向量的预测方程；
[0173][0174]
其中：
[0175]
x
k-1
为k-1时刻状态向量；
[0176]
φ
k/k-1
为状态转移矩阵；
[0177]
为k时刻状态向量预测值；
[0178]
通过计算状态向量预测值与状态向量的雅克比矩阵计算出状态转移矩阵φ
k/k-1
；
[0179]
步骤2-3：选取步骤1的位置信息作为无人机的位置量测zk＝[d
x d
y dz]
t
，构建无人机位置量测预测方程为：
[0180][0181]
其中：
[0182]
为无人机位置量测预测值；
[0183]
为无人机位置状态预测值；
[0184]hk
为量测矩阵，为3
×
3单位阵；
[0185]
通过计算位置量测预测值与位置状态预测值的雅克比矩阵即计算出位置的量测矩阵hk；
[0186]
步骤2-4：ekf滤波，利用量测向量在ekf滤波中对状态向量进行修正；
[0187]
步骤2-4-1：状态预测
[0188][0189]
步骤2-4-2：状态误差协方差矩阵预测
[0190][0191]
步骤2-4-3：滤波器增益
[0192][0193]
步骤2-4-4：状态误差协方差矩阵更新
[0194][0195]
步骤2-4-5：进行数据融合，对状态进行更新：
[0196][0197]
其中：
[0198]
p
k-1
为k-1时刻ekf滤波状态误差协方差矩阵；
[0199]
为k时刻ekf滤波状态误差协方差矩阵预测值；
[0200]qk-1
为k-1时刻系统噪声矩阵；
[0201]rk
为k时刻量测噪声矩阵；
[0202]kk
为k时刻滤波器增益矩阵；
[0203]
pk为k时刻ekf滤波状态误差协方差矩阵；
[0204]
为一步状态转移矩阵；
[0205]
为量测矩阵的转置；
[0206]
xk为状态向量；
[0207]
zk为量测向量。
[0208]
优选的，所述步骤2-2中无人机的姿态四元数预测方程为：
[0209][0210]
其中：
[0211]
为k时刻无人机姿态四元数预测；
[0212]qk-1
为k-1时刻无人机姿态四元数；
[0213]
δq为k-1至k时刻无人机姿态四元数变化量；
[0214]
为四元数相乘；
[0215]
k-1至k时刻无人机姿态四元数变化量计算公式为：
[0216][0217]
其中：
[0218]
δθ
x
为k-1至k时刻无人机x轴角增量；
[0219]
δθy为k-1至k时刻无人机y轴角增量；
[0220]
δθz为k-1至k时刻无人机z轴角增量；
[0221]
k-1至k时刻无人机各轴角增量计算公式为：
[0222]
δθ
x
＝(ω
x-ε
x
)
·
δt
[0223]
δθy＝(ω
y-εy)
·
δt
[0224]
δθz＝(ω
z-εz)
·
δt
[0225]
其中：
[0226]
ω
x
为无人机x轴陀螺输出的角速度；
[0227]
ε
x
为无人机x轴陀螺零偏；
[0228]
ωy为无人机y轴陀螺输出的角速度；
[0229]
εy为无人机y轴陀螺零偏；
[0230]
ωz为无人机z轴陀螺输出的角速度；
[0231]
εz为无人机z轴陀螺零偏；
[0232]
δt为k-1至k时刻时间间隔。
[0233]
优选的，所述步骤2-2中无人机的速度预测方程为：
[0234][0235]
其中：
[0236]
为无人机k时刻速度预测；
[0237]vk-1
为无人机k-1时刻速度；
[0238]
g0为无人机飞行地点当地重力加速度；
[0239]
为载体坐标系至导航坐标系的坐标转换矩阵；
[0240]
δv为k-1至k时刻速度增量；
[0241]
载体坐标系至导航坐标系的坐标转换矩阵的计算公式为：
[0242][0243]
k-1至k时刻速度增量的计算公式为：
[0244][0245][0246][0247]
其中：
[0248]ax
为无人机x轴加速度计输出的加速度；
[0249]
为无人机x轴加速度计零偏；
[0250]ay
为无人机y轴加速度计输出的加速度；
[0251]
为无人机y轴加速度计零偏；
[0252]az
为无人机z轴加速度计输出的加速度；
[0253]
为无人机z轴加速度计零偏。
[0254]
优选的，所述步骤2-2中无人机的位置预测方程为：
[0255][0256]
其中：
[0257]
为k时刻无人机位置状态预测值；
[0258]
p
k-1
为k-1时刻无人机位置；
[0259]vk-1
为k-1时刻无人机速度。
[0260]
优选的，所述步骤2-2中无人机陀螺零偏与加速度计零偏的预测方程为：
[0261][0262][0263]
其中：
[0264]
为k时刻无人机陀螺零偏预测值；
[0265]
ε
k-1
为k-1时刻无人机陀螺零偏；
[0266]
为k时刻无人机加速度计零偏预测值；
[0267]
为k-1时刻无人机加速度计零偏。
[0268]
优选的，所述步骤3中在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测具体为：在进行ekf滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行协方差矩阵正定性检测，即步骤2-4-4，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，即步骤2-4-1和步骤2-4-2；
[0269]
所述量测一致性检测方法为：
[0270]
假设北向位置量测为pn，北向位置量测预测值为将北向位置量测值与北向位置量测预测值做差记为假设量测噪声阵rk中对应的北向位置量测噪声为误差协方差矩阵中对应的北向位置误差为量测一致性检测公式如下：
[0271][0272]
其中k为系数；
[0273]
其它量测的一致性检测与北向位置的一致性检测相同；
[0274]
所述协方差矩阵正定性检测方法为：
[0275]
设在进行步骤2-4-4时，需要保证pk为正定矩阵，根据步骤2-4-4可知需要保证的每个对角线元素分别大于khp对应对角线元素，滤波器协方差矩阵正定性检测公式如下：
[0276][0277]
实施例1
[0278]
下面结合附图对本发明提出的一种基于视觉的无人机组合导航方法的原理进行详细说明。
[0279]
步骤1：计算视觉系统的位置信息；
[0280]
视觉系统获取到图像后进行图像匹配，匹配完成后输出识别信息，识别信息包含如下数据：
[0281]
1)标签x轴长度在图像像素坐标系中的像素个数lx
p
；
[0282]
2)标签y轴长度在图像像素坐标系中的像素个数ly
p
；
[0283]
3)标签中心相对于图像中心的距离在图像像素坐标系x轴所占像素的大小dx
p
；
[0284]
4)标签中心相对于图像中心的距离在图像像素坐标系y轴所占像素的大小dy
p
；
[0285]
5)目标识别状态表示sta。
[0286]
sta包含如下几种状态：
[0287]
1)图像异常；
[0288]
2)查询无目标；
[0289]
3)识别结果不稳定；
[0290]
4)识别稳定。
[0291]
如图1所示，标签坐标系的定义：坐标原点位于标签中心，x轴指向标签前方，y轴指向标签右方。
[0292]
如图2所示，图像像素坐标系的定义：图像像素坐标系的原点在图像的左上角，坐标单位为像素，单个像素的大小为(w)。
[0293]
本发明的标签为正方形，可嵌套多个标签图案，最大的标签图案边长为1m，最小的标签图案为0.05m，那么根据标签在图像像素坐标系的大小(lx
p
，ly
p
)、标签实际大小(lx，ly)、单个像素的大小为(w)以及标签中心相对于图像中心的距离在图像像素坐标系所占像素的大小(dx
p
，dy
p
)；
[0294]
如图3所示的水平位置计算示意图，求取水平位置方法如下：
[0295]
根据相似三角形关系那么有如下关系成立，
[0296][0297]
[0298]
其中字母含义如下表示：
[0299]
1)dx：表示无人机距离标签的x方向的距离。
[0300]
2)dy：表示无人机距离标签的y方向的距离。
[0301]
如图4所示的垂直位置计算示意图，求取垂直位置，即高度计算公式如下：
[0302][0303]
其中字母含义如下表示：
[0304]
1)dz：表示无人机距离标签的z方向的距离。
[0305]
2)f：表示相机的焦距。
[0306]
至此视觉系统的位置信息计算完成。
[0307]
步骤2：
[0308]
步骤2-1：选取无人机的姿态四元数、速度、位置、陀螺零偏与加速度计零偏为状态向量xk，即：
[0309][0310]
其中为q0为无人机姿态四元数实部，q1、q2、q3为无人机姿态四元数虚部，vn为无人机北向速度，ve为无人机东向速度，vd为无人机地向速度，pn为无人机北向位置，pe为无人机东向位置，pd为无人机地向位置，ε
x
为x轴陀螺零偏，εy为y轴陀螺零偏，εz为z轴陀螺零偏，为x轴加速度计零偏，为y轴加速度计零偏，为z轴加速度计零偏。
[0311]
步骤2-2：构建状态向量的预测方程：
[0312][0313]
其中：
[0314]
x
k-1
为k-1时刻状态向量；
[0315]
φ
k/k-1
为状态转移矩阵；
[0316]
为k时刻状态向量预测值；
[0317]
下面介绍状态向量预测值与φ
k/k-1
的计算过程：
[0318]
首先介绍无人机姿态四元数的状态预测
[0319][0320]
其中：
[0321]
为k时刻无人机姿态四元数预测；
[0322]qk-1
为k-1时刻无人机姿态四元数；
[0323]
δq为k-1至k时刻无人机姿态四元数变化量；
[0324]
为四元数相乘；
[0325]
k-1至k时刻无人机姿态四元数变化量计算公式为：
[0326][0327]
其中：
[0328]
δθ
x
为k-1至k时刻无人机x轴角增量；
[0329]
δθy为k-1至k时刻无人机y轴角增量；
[0330]
δθz为k-1至k时刻无人机z轴角增量；
[0331]
k-1至k时刻无人机各轴角增量计算公式为：
[0332]
δθ
x
＝(ω
x-ε
x
)
·
δt
[0333]
δθy＝(ω
y-εy)
·
δt
[0334]
δθz＝(ω
z-εz)
·
δt
[0335]
其中：
[0336]
ω
x
为无人机x轴陀螺输出的角速度；
[0337]
ε
x
为无人机x轴陀螺零偏；
[0338]
ωy为无人机y轴陀螺输出的角速度；
[0339]
εy为无人机y轴陀螺零偏；
[0340]
ωz为无人机z轴陀螺输出的角速度；
[0341]
εz为无人机z轴陀螺零偏；
[0342]
δt为k-1至k时刻时间间隔；
[0343]
至此无人机姿态四元数预测完成。
[0344]
下面介绍无人机速度预测
[0345][0346]
其中：
[0347]
为无人机k时刻速度预测；
[0348]vk-1
为无人机k-1时刻速度；
[0349]
g0为无人机飞行地点当地重力加速度；
[0350]
为载体坐标系至导航坐标系的坐标转换矩阵；
[0351]
δv为k-1至k时刻速度增量；
[0352]
载体坐标系至导航坐标系的坐标转换矩阵的计算公式如下：
[0353][0354]
k-1至k时刻速度增量的计算公式如下：
[0355][0356][0357][0358]
其中：
[0359]ax
为无人机x轴加速度计输出的加速度；
[0360]
为无人机x轴加速度计零偏；
[0361]ay
为无人机y轴加速度计输出的加速度；
[0362]
为无人机y轴加速度计零偏；
[0363]az
为无人机z轴加速度计输出的加速度；
[0364]
为无人机z轴加速度计零偏。
[0365]
无人机的位置预测方程为：
[0366][0367]
其中：
[0368]
为k时刻无人机位置状态预测值；
[0369]
p
k-1
为k-1时刻无人机位置；
[0370]vk-1
为k-1时刻无人机速度。
[0371]
至此，无人机位置预测完成。
[0372]
下面介绍无人机陀螺零偏与加速度计零偏的预测
[0373][0374][0375]
其中：
[0376]
为k时刻无人机陀螺零偏预测值；
[0377]
ε
k-1
为k-1时刻无人机陀螺零偏；
[0378]
为k时刻无人机加速度计零偏预测值；
[0379]
为k-1时刻无人机加速度计零偏。；
[0380]
至此无人机的状态向量的预测全部完成，只需计算状态向量预测值与状态向量的雅克比矩阵即可计算出状态转移矩阵φ
k/k-1
。
[0381]
步骤2-3：下面介绍量测矩阵的计算；
[0382]
选取步骤1的位置信息作为无人机的位置量测zk＝[d
x d
y dz]
t
，构建无人机位置量测预测方程为：
[0383][0384]
其中：
[0385]
为无人机位置量测预测值；
[0386]
为无人机位置状态预测值；
[0387]hk
为量测矩阵，为3
×
3单位阵；
[0388]
通过计算位置量测预测值与位置状态预测值的雅克比矩阵即计算出位置的量测矩阵hk，至此已完成位置量测矩阵的计算；
[0389]
步骤2-4：ekf滤波实施过程如下：
[0390]
步骤2-4-1：状态预测
[0391][0392]
步骤2-4-2：状态误差协方差矩阵预测
[0393][0394]
步骤2-4-3：滤波器增益
[0395]
[0396]
步骤2-4-4：状态误差协方差矩阵更新
[0397][0398]
步骤2-4-5：进行数据融合，对状态进行更新
[0399][0400]
其中：
[0401]
p
k-1
为k-1时刻ekf滤波状态误差协方差矩阵；
[0402]
为k时刻ekf滤波状态误差协方差矩阵预测值；
[0403]qk-1
为k-1时刻系统噪声矩阵；
[0404]rk
为k时刻量测噪声矩阵；
[0405]kk
为k时刻滤波器增益矩阵；
[0406]
pk为k时刻ekf滤波状态误差协方差矩阵；
[0407]
为一步状态转移矩阵；
[0408]
为量测矩阵的转置；
[0409]
xk为状态向量；
[0410]
zk为量测向量。
[0411]
步骤3：在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，完成基于视觉的无人机组合导航方法设计。
[0412]
如图6所示，为了增加系统的鲁棒性，避免误差较大的量测对滤波器造成不利的影响，需要实时对量测进行一致性检测，为了实现对每个量测分别进行一致性检测，需要将上述ekf滤波形式设计为序贯滤波的形式，下面介绍对量测进行一致性检测的方法。
[0413]
假设北向位置量测为pn，北向位置量测预测值为将北向位置量测值与北向位置量测预测值做差记为假设量测噪声阵(rk)中对应的北向位置量测噪声为误差协方差矩阵中对应的北向位置误差为量测一致性检测公式如下：
[0414][0415]
其中k为系数，一般认为量测噪声阵(rk)与误差协方差矩阵中对应的量测误差是在1σ下计算的，而且与均为误差的平方，若要使一致性检测在3σ下通过，k一般取9，具体值可根据提供量测数据的传感器进行设置。
[0416]
其它量测的一致性检测与北向位置的一致性检测相同，在进行ekf滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行滤波器协方差矩阵正定性检测，即ekf步骤2-4-4，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，不进行滤波器协方差矩阵正定性检测，即ekf滤波的步骤2-4-1与步骤2-4-2，为了减小cpu的计算量，一致性检测步骤一般ekf滤波步骤2-4-2与步骤2-4-3之间，假如一致性检测未通过则不需计算滤波器增益，可大大减少cpu的计算量。
[0417]
至此量测一致性检测已完成，下面介绍对滤波器协方差矩阵正定性检测；
[0418]
设在进行滤波器协方差矩阵更新时，即ekf滤波步骤2-4-4，需
要保证pk为正定矩阵，根据ekf滤波步骤2-4-4公式可知需要保证的每个对角线元素分别大于khp对应对角线元素，滤波器协方差矩阵正定性检测公式如下：
[0419][0420]
进行ekf滤波时，只有量测通过一致性检测，滤波器协方差矩阵通过正定性检测才可利用该量测对滤波器状态进行修正，只要有一个检测未通过，均不可利用该量测对滤波器状态进行修正，只进行状态预测与状态误差协方差矩阵预测，即ekf滤波的步骤2-4-1与步骤2-4-2。
[0421]
至此基于视觉的无人机组合导航方法设计完成。
[0422]
应用实施例
[0423]
在无人机精准降落过程中，使用本发明提出的组合导航方法，在整个降落过程中，使用飞机挂载的吊舱进行图像数据的采集，将采集的图像实时传输给nx板进行计算，输出识别信息，并将识别信息传输至飞控，飞控将识别信息与imu数据进行融合，实时输出高精度无人机定位结果，实现了低成本的精准降落过程，位置融合结果(视觉系统获得的位置信息与imu数据融合出的位置，即图6中滤波器输出的xk)如图7和8所示，图中实线为imu数据，虚线为视觉系统获得的位置信息，从图中可看出位置精度达到厘米级。
[0424]
所述imu数据为角速度和加速度数据。
[0425]
本发明的原理如下：
[0426]
如图5所示，本发明提出的一种基于视觉的无人机组合导航方法，首先利用视觉系统对标签进行图像数据采样，接着进行图像标签匹配，然后根据标签的识别信息，实时计算无人机相对于标签中心的位置信息(解算图像定位信息)，并实时输出，将位置信息作为量测，构建无人机位置量测预测方程，通过量测向量在ekf滤波中对状态向量进行修正，对imu数据采集，接着进行状态预测(公式1)，进行协方差矩阵预测(公式2)，此时对每个量测进行一致性检测，一致性检测通过后再进行协方差矩阵正定性检测(假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测)，使通过视觉系统得到的无人机相对于标签中心的位置信息与imu数据进行融合(公式5)，输出高精度的姿态、速度以及位置信息，该定位方法在gps拒止条件下，依然可以实时输出高精度的无人机姿态、速度以及位置信息。
[0427]
本发明提出的一种基于视觉的无人机组合导航方法，先通过视觉系统获取到图像计算得到无人机相对于标签中心的位置信息，接着选取组合导航的状态向量，构建状态向量的预测方程，然后选取视觉输出的位置信息作为量测，计算量测矩阵，利用量测向量在ekf滤波中对状态向量进行修正，完成组合导航模型的建立，在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，完成基于视觉的无人机组合导航方法设计，本发明将视觉系统得到的无人机相对于标签中心的位置信息和imu数据进行融合，可直接输出无人机高精度的姿态、速度与位置，不需要建立复杂的误差模型。
[0428]
本发明提出的一种基于视觉的无人机组合导航方法，可在gps拒止环境下通过视觉系统对标签进行图像采样，然后根据标签信息，实时计算无人机相对于标签中心的位置，并实时输出，然后与imu数据融合，提供高精度的姿态、速度以及位置信息。
[0429]
本发明提出的一种基于视觉的无人机组合导航方法，在组合导航模型中加入量测
一致性检测与协方差矩阵正定性检测，当进行ekf滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行协方差矩阵正定性检测，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，将一致性检测设置在步骤2-4-2状态误差协方差矩阵预测和步骤2-4-3滤波器增益之间，如一致性检测未通过则不需计算滤波器增益，可大大减少cpu的计算量，本发明导航方法计算量小，实时性高，系统的鲁棒性高。
[0430]
本发明提出的一种基于视觉的无人机组合导航方法可实时对量测信息进行一致性检测，防止误差较大的量测对滤波器造成影响，还可实时对滤波器协方差矩阵进行正定性检测，防止滤波器发散，并及时对滤波器进行调整。
[0431]
上面对本发明优选实施方式作了详细说明，但是本发明不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。
[0432]
不脱离本发明的构思和范围可以做出许多其他改变和改型。应当理解，本发明不限于特定的实施方式，本发明的范围由所附权利要求限定。

技术特征：
1.一种基于视觉的无人机组合导航方法，其特征在于，包括以下步骤：步骤1：首先通过视觉系统进行视觉信息采集，然后通过采集到的图像信息与标签库进行匹配，匹配到标签之后输出识别信息，根据识别信息计算无人机相对于标签中心的位置信息；步骤2：选取组合导航的状态向量x
k
，构建状态向量的预测方程，然后选取步骤1的位置信息作为量测，构建量测预测方程，计算量测矩阵h
k
，通过量测向量在ekf滤波中对状态向量进行修正，完成组合导航模型的建立；步骤3：在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，完成基于视觉的无人机组合导航方法设计。2.根据权利要求1所述的一种基于视觉的无人机组合导航方法，其特征在于，所述步骤1具体为：步骤1-1：通过视觉系统获取到图像信息后与标签库进行匹配，匹配到标签之后输出识别信息；步骤1-2：对标签坐标系和图像像素坐标系进行定义，标签坐标系的定义：坐标原点位于标签中心，x轴指向标签前方，y轴指向标签右方；图像像素坐标系的定义：图像像素坐标系的原点在图像的左上角，坐标单位为像素，单个像素的大小为w；因此识别信息包括：标签x轴长度在图像像素坐标系中的像素个数lx
p
、标签y轴长度在图像像素坐标系中的像素个数ly
p
、标签中心相对于图像中心的距离在图像像素坐标系x轴所占像素的大小dx
p
、标签中心相对于图像中心的距离在图像像素坐标系y轴所占像素的大小dy
p
，以及目标识别状态；步骤1-3：根据标签在图像像素坐标系的大小(lx
p
，ly
p
)、标签实际大小(lx，ly)、单个像素的大小w以及标签中心相对于图像中心的距离在图像像素坐标系所占像素的大小(dx
p
，dy
p
)计算无人机相对于标签中心的位置信息，即水平位置和垂直位置；所述水平位置计算公式为：所述水平位置计算公式为：其中：dx为无人机距离标签的x方向的距离；dy为无人机距离标签的y方向的距离；所述垂直位置计算公式为：其中：dz为无人机距离标签的z方向的距离；f为视觉系统的相机焦距。3.根据权利要求2所述的一种基于视觉的无人机组合导航方法，其特征在于，所述步骤1-1中的标签为正方形，标签中可嵌套多个标签图案，最大的标签图案边长为1m，最小的标签图案为0.05m。
4.根据权利要求2所述的一种基于视觉的无人机组合导航方法，其特征在于，所述步骤1-2中的目标识别状态包括图像异常、查询无目标、识别结果不稳定和识别稳定。5.根据权利要求1所述的一种基于视觉的无人机组合导航方法，其特征在于，所述步骤2具体为：步骤2-1：选取无人机的姿态四元数、速度、位置、陀螺零偏与加速度计零偏为状态向量x
k
，即：其中q0为无人机姿态四元数实部，q1、q2、q3为无人机姿态四元数虚部，v
n
为无人机北向速度，v
e
为无人机东向速度，v
d
为无人机地向速度，p
n
为无人机北向位置，p
e
为无人机东向位置，p
d
为无人机地向位置，ε
x
为x轴陀螺零偏，ε
y
为y轴陀螺零偏，ε
z
为z轴陀螺零偏，为x轴加速度计零偏，为y轴加速度计零偏，为z轴加速度计零偏；步骤2-2：构建状态向量的预测方程；其中：x
k-1
为k-1时刻状态向量；φ
k/k-1
为状态转移矩阵；为k时刻状态向量预测值；通过计算状态向量预测值与状态向量的雅克比矩阵计算出状态转移矩阵φ
k/k-1
；步骤2-3：选取步骤1的位置信息作为无人机的位置量测z
k
＝[d
x d
y d
z
]
t
，构建无人机位置量测预测方程为：其中：为无人机位置量测预测值；为无人机位置状态预测值；h
k
为量测矩阵，为3
×
3单位阵；通过计算位置量测预测值与位置状态预测值的雅克比矩阵即计算出位置的量测矩阵h
k
；步骤2-4：ekf滤波，利用量测向量在ekf滤波中对状态向量进行修正；步骤2-4-1：状态预测步骤2-4-2：状态误差协方差矩阵预测步骤2-4-3：滤波器增益步骤2-4-4：状态误差协方差矩阵更新
步骤2-4-5：进行数据融合，对状态进行更新：其中：p
k-1
为k-1时刻ekf滤波状态误差协方差矩阵；为k时刻ekf滤波状态误差协方差矩阵预测值；q
k-1
为k-1时刻系统噪声矩阵；r
k
为k时刻量测噪声矩阵；k
k
为k时刻滤波器增益矩阵；p
k
为k时刻ekf滤波状态误差协方差矩阵；为一步状态转移矩阵；为量测矩阵的转置；x
k
为状态向量；z
k
为量测向量。6.根据权利要求5所述的一种基于视觉的无人机组合导航方法，其特征在于，所述步骤2-2中无人机的姿态四元数预测方程为：其中：为k时刻无人机姿态四元数预测；q
k-1
为k-1时刻无人机姿态四元数；δq为k-1至k时刻无人机姿态四元数变化量；为四元数相乘；k-1至k时刻无人机姿态四元数变化量计算公式为：其中：δθ
x
为k-1至k时刻无人机x轴角增量；δθ
y
为k-1至k时刻无人机y轴角增量；δθ
z
为k-1至k时刻无人机z轴角增量；k-1至k时刻无人机各轴角增量计算公式为：δθ
x
＝(ω
x-ε
x
)
·
δtδθ
y
＝(ω
y-ε
y
)
·
δtδθ
z
＝(ω
z-ε
z
)
·
δt其中：ω
x
为无人机x轴陀螺输出的角速度；ε
x
为无人机x轴陀螺零偏；ω
y
为无人机y轴陀螺输出的角速度；
ε
y
为无人机y轴陀螺零偏；ω
z
为无人机z轴陀螺输出的角速度；ε
z
为无人机z轴陀螺零偏；δt为k-1至k时刻时间间隔。7.根据权利要求5所述的一种基于视觉的无人机组合导航方法，其特征在于，所述步骤2-2中无人机的速度预测方程为：其中：为无人机k时刻速度预测；v
k-1
为无人机k-1时刻速度；g0为无人机飞行地点当地重力加速度；为载体坐标系至导航坐标系的坐标转换矩阵；δv为k-1至k时刻速度增量；载体坐标系至导航坐标系的坐标转换矩阵的计算公式为：k-1至k时刻速度增量的计算公式为：1至k时刻速度增量的计算公式为：1至k时刻速度增量的计算公式为：其中：a
x
为无人机x轴加速度计输出的加速度；为无人机x轴加速度计零偏；a
y
为无人机y轴加速度计输出的加速度；为无人机y轴加速度计零偏；a
z
为无人机z轴加速度计输出的加速度；为无人机z轴加速度计零偏。8.根据权利要求5所述的一种基于视觉的无人机组合导航方法，其特征在于，所述步骤2-2中无人机的位置预测方程为：其中：为k时刻无人机位置状态预测值；p
k-1
为k-1时刻无人机位置；v
k-1
为k-1时刻无人机速度。9.根据权利要求5所述的一种基于视觉的无人机组合导航方法，其特征在于，所述步骤2-2中无人机陀螺零偏与加速度计零偏的预测方程为：
其中：为k时刻无人机陀螺零偏预测值；ε
k-1
为k-1时刻无人机陀螺零偏；为k时刻无人机加速度计零偏预测值；为k-1时刻无人机加速度计零偏。10.根据权利要求5所述的一种基于视觉的无人机组合导航方法，其特征在于：所述步骤3中在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测具体为：在进行ekf滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行协方差矩阵正定性检测，即步骤2-4-4，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，即步骤2-4-1和步骤2-4-2；所述量测一致性检测方法为：假设北向位置量测为p
n
，北向位置量测预测值为将北向位置量测值与北向位置量测预测值做差记为假设量测噪声阵r
k
中对应的北向位置量测噪声为误差协方差矩阵中对应的北向位置误差为量测一致性检测公式如下：其中k为系数；其它量测的一致性检测与北向位置的一致性检测相同；所述协方差矩阵正定性检测方法为：设在进行步骤2-4-4时，需要保证p
k
为正定矩阵，根据步骤2-4-4可知需要保证的每个对角线元素分别大于khp对应对角线元素，滤波器协方差矩阵正定性检测公式如下：。

技术总结
本发明公开了一种基于视觉的无人机组合导航方法，先通过视觉系统获取到图像，计算得到无人机相对于标签中心的位置信息，接着选取组合导航的状态向量，构建状态向量的预测方程，然后选取视觉输出的位置信息作为量测，计算量测矩阵，利用量测向量在EKF滤波中对状态向量进行修正，完成组合导航模型的建立，在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，完成基于视觉的无人机组合导航方法设计，本发明将视觉系统得到的无人机相对于标签中心的位置信息和IMU数据进行融合，可直接输出无人机高精度的姿态、速度与位置，不需要建立复杂的误差模型。需要建立复杂的误差模型。需要建立复杂的误差模型。


技术研发人员：江新奇 贺希格图 徐勇超 王京伟 朱宴南 李金柱 张伟 侯玉山 张延续 黄磊
受保护的技术使用者：扎鲁特旗扎哈淖尔煤业有限公司
技术研发日：2023.05.25
技术公布日：2023/9/14