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

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

背景技术：

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

2、因此研究一种自主组合导航方法是解决这一问题的关键。

技术实现思路

1、本发明的目的在于克服上述现有技术的不足，提供一种基于视觉的无人机组合导航方法。

2、为了解决技术问题，本发明的技术方案是：一种基于视觉的无人机组合导航方法，包括以下步骤：

3、步骤1：首先通过视觉系统进行视觉信息采集，然后通过采集到的图像信息与标签库进行匹配，匹配到标签之后输出识别信息，根据识别信息计算无人机相对于标签中心的位置信息；

4、步骤2：选取组合导航的状态向量xk，构建状态向量的预测方程，然后选取步骤1的位置信息作为量测，构建量测预测方程，计算量测矩阵hk，通过量测向量在ekf滤波中对状态向量进行修正，完成组合导航模型的建立；

5、步骤3：在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，完成基于视觉的无人机组合导航方法设计。

6、优选的，所述步骤1具体为：

7、步骤1-1：通过视觉系统获取到图像信息后与标签库进行匹配，匹配到标签之后输出识别信息；

8、步骤1-2：对标签坐标系和图像像素坐标系进行定义，标签坐标系的定义：坐标原点位于标签中心，x轴指向标签前方，y轴指向标签右方；图像像素坐标系的定义：图像像素坐标系的原点在图像的左上角，坐标单位为像素，单个像素的大小为w；因此识别信息包括：标签x轴长度在图像像素坐标系中的像素个数lxp、标签y轴长度在图像像素坐标系中的像素个数lyp、标签中心相对于图像中心的距离在图像像素坐标系x轴所占像素的大小dxp、标签中心相对于图像中心的距离在图像像素坐标系y轴所占像素的大小dyp，以及目标识别状态；

9、步骤1-3：根据标签在图像像素坐标系的大小(lxp，lyp)、标签实际大小(lx，ly)、单个像素的大小w以及标签中心相对于图像中心的距离在图像像素坐标系所占像素的大小(dxp，dyp)计算无人机相对于标签中心的位置信息，即水平位置和垂直位置；

10、所述水平位置计算公式为：

11、

12、

13、其中：

14、dx为无人机距离标签的x方向的距离；

15、dy为无人机距离标签的y方向的距离；

16、所述垂直位置计算公式为：

17、

18、其中：

19、dz为无人机距离标签的z方向的距离；

20、f为视觉系统的相机焦距。

21、优选的，所述步骤1-1中的标签为正方形，标签中可嵌套多个标签图案，最大的标签图案边长为1m，最小的标签图案为0.05m。

22、优选的，所述步骤1-2中的目标识别状态包括图像异常、查询无目标、识别结果不稳定和识别稳定。

23、优选的，所述步骤2具体为：

24、步骤2-1：选取无人机的姿态四元数、速度、位置、陀螺零偏与加速度计零偏为状态向量xk，即：

25、其中q0为无人机姿态四元数实部，q1、q2、q3为无人机姿态四元数虚部，vn为无人机北向速度，ve为无人机东向速度，vd为无人机地向速度，pn为无人机北向位置，pe为无人机东向位置，pd为无人机地向位置，εx为x轴陀螺零偏，εy为y轴陀螺零偏，εz为z轴陀螺零偏，为x轴加速度计零偏，为y轴加速度计零偏，为z轴加速度计零偏；

26、步骤2-2：构建状态向量的预测方程；

27、

28、其中：

29、xk-1为k-1时刻状态向量；

30、φk/k-1为状态转移矩阵；

31、为k时刻状态向量预测值；

32、通过计算状态向量预测值与状态向量的雅克比矩阵计算出状态转移矩阵φk/k-1；

33、步骤2-3：选取步骤1的位置信息作为无人机的位置量测zk＝[dx dy dz]t，构建无人机位置量测预测方程为：

34、

35、其中：

36、为无人机位置量测预测值；

37、为无人机位置状态预测值；

38、hk为量测矩阵，为3×3单位阵；

39、通过计算位置量测预测值与位置状态预测值的雅克比矩阵即计算出位置的量测矩阵hk；

40、步骤2-4：ekf滤波，利用量测向量在ekf滤波中对状态向量进行修正；

41、步骤2-4-1：状态预测

42、

43、步骤2-4-2：状态误差协方差矩阵预测

44、

45、步骤2-4-3：滤波器增益

46、

47、步骤2-4-4：状态误差协方差矩阵更新

48、

49、步骤2-4-5：进行数据融合，对状态进行更新：

50、

51、其中：

52、pk-1为k-1时刻ekf滤波状态误差协方差矩阵；

53、为k时刻ekf滤波状态误差协方差矩阵预测值；

54、qk-1为k-1时刻系统噪声矩阵；

55、rk为k时刻量测噪声矩阵；

56、kk为k时刻滤波器增益矩阵；

57、pk为k时刻ekf滤波状态误差协方差矩阵；

58、为一步状态转移矩阵；

59、为量测矩阵的转置；

60、xk为状态向量；

61、zk为量测向量。

62、优选的，所述步骤2-2中无人机的姿态四元数预测方程为：

63、

64、其中：

65、为k时刻无人机姿态四元数预测；

66、qk-1为k-1时刻无人机姿态四元数；

67、δq为k-1至k时刻无人机姿态四元数变化量；

68、为四元数相乘；

69、k-1至k时刻无人机姿态四元数变化量计算公式为：

70、

71、其中：

72、δθx为k-1至k时刻无人机x轴角增量；

73、δθy为k-1至k时刻无人机y轴角增量；

74、δθz为k-1至k时刻无人机z轴角增量；

75、k-1至k时刻无人机各轴角增量计算公式为：

76、δθx＝(ωx-εx)·δt

77、δθy＝(ωy-εy)·δt

78、δθz＝(ωz-εz)·δt

79、其中：

80、ωx为无人机x轴陀螺输出的角速度；

81、εx为无人机x轴陀螺零偏；

82、ωy为无人机y轴陀螺输出的角速度；

83、εy为无人机y轴陀螺零偏；

84、ωz为无人机z轴陀螺输出的角速度；

85、εz为无人机z轴陀螺零偏；

86、δt为k-1至k时刻时间间隔。

87、优选的，所述步骤2-2中无人机的速度预测方程为：

88、

89、其中：

90、为无人机k时刻速度预测；

91、vk-1为无人机k-1时刻速度；

92、g0为无人机飞行地点当地重力加速度；

93、为载体坐标系至导航坐标系的坐标转换矩阵；

94、δv为k-1至k时刻速度增量；

95、载体坐标系至导航坐标系的坐标转换矩阵的计算公式为：

96、

97、k-1至k时刻速度增量的计算公式为：

98、

99、

100、

101、其中：

102、ax为无人机x轴加速度计输出的加速度；

103、为无人机x轴加速度计零偏；

104、ay为无人机y轴加速度计输出的加速度；

105、为无人机y轴加速度计零偏；

106、az为无人机z轴加速度计输出的加速度；

107、为无人机z轴加速度计零偏。

108、优选的，所述步骤2-2中无人机的位置预测方程为：

109、

110、其中：

111、为k时刻无人机位置状态预测值；

112、pk-1为k-1时刻无人机位置；

113、vk-1为k-1时刻无人机速度。

114、优选的，所述步骤2-2中无人机陀螺零偏与加速度计零偏的预测方程为：

115、

116、

117、其中：

118、为k时刻无人机陀螺零偏预测值；

119、εk-1为k-1时刻无人机陀螺零偏；

120、为k时刻无人机加速度计零偏预测值；

121、为k-1时刻无人机加速度计零偏。

122、优选的，所述步骤3中在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测具体为：在进行ekf滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行协方差矩阵正定性检测，即步骤2-4-4，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，即步骤2-4-1和步骤2-4-2；

123、所述量测一致性检测方法为：

124、假设北向位置量测为pn，北向位置量测预测值为将北向位置量测值与北向位置量测预测值做差记为假设量测噪声阵rk中对应的北向位置量测噪声为误差协方差矩阵中对应的北向位置误差为量测一致性检测公式如下：

125、

126、其中k为系数；

127、其它量测的一致性检测与北向位置的一致性检测相同；

128、所述协方差矩阵正定性检测方法为：

129、设在进行步骤2-4-4时，需要保证pk为正定矩阵，根据步骤2-4-4可知需要保证的每个对角线元素分别大于khp对应对角线元素，滤波器协方差矩阵正定性检测公式如下：

130、

131、与现有技术相比，本发明的优点在于：

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

133、(2)本发明提出的一种基于视觉的无人机组合导航方法，可在gps拒止环境下通过视觉系统对标签进行图像采样，然后根据标签信息，实时计算无人机相对于标签中心的位置信息，并实时输出，然后与imu数据融合，提供高精度的姿态、速度以及位置信息；

134、(3)本发明提出的一种基于视觉的无人机组合导航方法，在组合导航模型中加入量测一致性检测与协方差矩阵正定性检测，当进行ekf滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行协方差矩阵正定性检测，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，将一致性检测设置在步骤2-4-2状态误差协方差矩阵预测和步骤2-4-3滤波器增益之间，如一致性检测未通过则不需计算滤波器增益，可大大减少cpu的计算量，本发明导航方法计算量小，实时性高；

135、(4)本发明提出的一种基于视觉的无人机组合导航方法可实时对量测信息进行一致性检测，防止误差较大的量测对滤波器造成不利的影响，增加了系统的鲁棒性；本发明还可实时对滤波器协方差矩阵进行正定性检测，防止滤波器发散，并及时对滤波器进行调整。