一种基于EKF的无人机多视觉标签定位方法与流程

本发明属于无人机，具体涉及一种基于ekf的无人机多视觉标签定位方法。

背景技术：

1、目前，无人机技术的发展突飞猛进，基于无人机的应用领域也越来越广泛，在军事侦查、战场监视、火灾探测、环境与交通监测方面都得到了广泛的应用。无人机在执行任务期间周围环境复杂多变，为了顺利完成任务，优秀的定位方法至关重要。目前无人机常用的导航方法包括采用互补滤波的ahrs方法与基于误差方程的组合导航定位方法，但是基于互补滤波的ahrs方法不能满足无人机大机动情况下的导航需求，输出的姿态精度不能满足要求，这是互补滤波方法本身存在的缺陷，虽然有一些适用于机动的ahrs方法，但是效果总是不尽人意；基于误差方程的组合导航方法能够满足无人机对导航系统的要求，但是需要建立复杂的导航系统误差方程，并且定位效果过度依赖gps，在gps拒止环境下或者复杂室内环境下均无法满足定位要求。

2、现有技术中计算量大、鲁棒性差、成本高昂，因此研究一种高精度、在gps拒止环境下或者室内仍能提供位置信息的低成本定位方法是解决这一问题的关键。

技术实现思路

1、本发明的目的在于克服上述现有技术的不足，提供一种基于ekf的无人机多视觉标签定位方法。

2、为了解决技术问题，本发明的技术方案是：一种基于ekf的无人机多视觉标签定位方法，包括以下步骤：

3、步骤1：通过视觉系统采集图像信息，然后与标签库进行匹配，最后根据识别的若干个标签分别计算无人机在导航坐标系下的若干个位置信息；

4、步骤2：选取无人机的状态向量xk，构建状态向量的预测方程，通过计算状态向量预测值与状态向量的雅克比矩阵计算出状态转移矩阵φk/k-1，完成状态向量的预测；

5、步骤3：选取步骤1的任意一个位置信息作为量测，构建量测预测方程，通过计算位置量测预测值与位置状态预测值的雅克比矩阵即计算出位置的量测矩阵hk，完成基于ekf的无人机多视觉标签定位的模型建立；

6、步骤4：进行ekf滤波；

7、步骤5：在ekf滤波过程中加入量测一致性检查与协方差矩阵正定性检测，对状态向量进行修正，完成基于ekf的无人机多视觉标签定位方法设计。

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

9、步骤1-1：在地面布置多个标签，形成飞行区域，并给标签进行编号[t1 t2 … tn]，选取第一个标签t1的中心为坐标原点，标签前向为导航坐标系x轴正方向，标签右向为导航坐标系y轴正方向，垂直于标签向下为导航坐标系z轴正方向；

10、步骤1-2：无人机上搭载云台相机，无人机在标签组成的飞行区域内进行飞行，飞行时云台相机垂直朝下，云台处于增稳模式，实时采集垂直于机身的图像信息，图像信息采集完成后，首先与标签库进行匹配，识别出若干个标签；

11、步骤1-3：通过若干个标签的识别信息对每一个标签进行单视觉标签定位，得到单视觉标签定位位置信息；

12、步骤1-4：将若干个单视觉标签定位位置信息转换至导航坐标系的位置信息，计算出无人机在导航坐标系下的若干个位置信息。

13、优选的，所述步骤1-1中标签进行布置时需要保证每时每刻云台相机的视场内至少有一个标签。

14、优选的，所述步骤1-3具体为：

15、步骤1-3-1：对标签坐标系和图像像素坐标系进行定义，标签坐标系的定义：坐标原点位于标签中心，x轴指向标签前方，y轴指向标签右方；图像像素坐标系的定义：图像像素坐标系的原点在图像的左上角，坐标单位为像素，单个像素的大小为w；所述识别信息包括：标签图像在图像像素坐标系x方向像素个数tpx、标签图像在图像像素坐标系y方向像素个数tpy、标签图像中心与视场中心的距离在图像像素坐标系x轴方向像素个数lpx、标签图像中心与视场中心的距离在图像像素坐标系y轴方向像素个数lpy；

16、步骤1-3-2：根据标签在图像像素坐标系的大小(tpx，tpy)、标签实际大小(tx，ty)、单个像素的大小w以及标签中心相对于图像中心的距离在图像像素坐标系所占像素的大小(lpx，lpy)计算无人机相对于标签中心的位置信息，即水平位置和垂直位置；

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

18、

19、

20、其中：

21、px-表示无人机在标签坐标系x方向的位置；

22、py-表示无人机在标签坐标系y方向的位置；

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

24、

25、其中：

26、pz-表示无人机在标签坐标系z方向位置；

27、f-视觉系统的相机焦距。

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

29、优选的，所述步骤1-4中将若干个单视觉标签定位位置信息转换至导航坐标系位置信息的公式为：

30、

31、其中：

32、一根据[t1 t2 … tn]标签信息计算的无人机导航坐标系位置信息；

33、-[t1 t2 … tn]标签在导航坐标系的位置信息；

34、-无人机在[t1 t2 … tn]标签坐标系下的位置信息；

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

36、步骤2-1：选取无人机的速度以及位置为状态向量，即：[vx vy vz px py pz]t，其中vx为无人机x轴方向速度，vy为无人机y轴方向速度，vz为无人机z轴方向速度，px为无人机x轴方向位置，py为无人机y轴方向位置，pz为无人机z轴方向位置；

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

38、

39、其中：

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

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

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

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

44、无人机的速度预测方程为：

45、

46、其中：

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

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

49、所述无人机的位置预测方程为：

50、

51、其中：

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

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

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

55、状态转移矩阵φk/l-1的计算方法：设k时刻无人机的状态向量为k-1时刻无人机的状态向量为[vk-1 pk-1]t，k时刻状态向量的预测公式为上述速度预测方程和位置预测方程，因此只需计算k时刻状态向量与k-1时刻状态向量雅克比矩阵，即为k-1时刻至k时刻的状态转移矩阵φk/k-1。

56、优选的，所述步骤3具体为：

57、步骤3-1：选取步骤1的任意一个导航坐标系下的位置作为无人机的位置量测zk，zk＝[px py pz]t，构建无人机位置量测预测方程为：

58、

59、其中：

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

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

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

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

64、优选的，所述步骤4中ekf滤波过程具体为：

65、步骤4-1：状态预测

66、

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

68、

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

70、

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

72、

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

74、

75、其中：

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

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

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

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

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

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

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

83、为量测矩阵的转置；

84、xk为状态向量；

85、zk为量测向量。

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

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

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

89、

90、其中k为系数；

91、其它量测的一致性检测与x轴方向的一致性检测相同；

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

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

94、

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

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

97、(2)本发明通过多视觉标签定位，在地面布置多个标签，形成飞行区域，标签进行布置时需要保证每时每刻云台相机的视场内至少有一个标签，无人机上搭载云台相机在标签组成的飞行区域内进行飞行，实时采集垂直于机身的图像信息，图像信息采集完成后与标签库进行匹配，识别出若干个标签，分别对若干个标签进行单视觉标签定位，然后将若干个单视觉标签定位位置信息转换至导航坐标系的位置信息，得到若干个导航坐标系的位置信息，每个导航坐标系的位置信息均可作为ekf滤波器的量测，大大提高了定位精度；

98、(3)本发明提出的一种基于ekf的无人机多视觉标签定位方法，该方法可在gps拒止条件下或者室内环境下实时提供高精度的无人机的速度与位置，并可实时对量测信息进行一致性检测，防止误差较大的量测信息对滤波器造成的不利的影响；本发明方法可实时对滤波器的协方差矩阵进行正定性检测，防止滤波器发散，及时对滤波器进行调整；

99、(4)本发明提出的一种基于ekf的无人机多视觉标签定位方法计算量小，实时性高、鲁棒性强、成本低、布置简单，仅需布置多个标签即可提供高精度的速度与位置。