一种基于鲁棒控制的四旋翼目标检测跟踪方法

本发明涉及一种基于鲁棒控制的四旋翼目标检测跟踪方法，属于无人机自动控制。

背景技术：

1、四旋翼无人机以其出众的侦察能力、稳定飞行、结构简单、垂直起降、制造成本相对较低和容易携带等优秀能力，因此广泛运用于目标检测跟踪在科研领域完成复杂任务。尽管技术在不断进步，但是现有的无人机在目标检测跟踪方向存在一些挑战和问题。

2、现有的目标检测技术分为深度学习和传统方法。传统目标检测方法在处理目标检测任务时存在一系列局限性。首先，这些方法通常需要手工设计和提取图像特征，这限制了它们在复杂、多样化目标和场景中的适用性。此外，传统方法对光照、尺度和旋转等因素敏感，难以应对各种变化；在处理遮挡和复杂背景时表现不佳，难以捕捉上下文信息和语义信息。此外，传统方法通常是为特定类型的目标检测任务设计的，通用性有限，需要重新设计和调整算法以适应不同的应用。最后，传统方法在大规模数据集上的性能通常不如深度学习方法，后者能够更好地从大量数据中学习特征和模式。相比之下，深度学习方法通过自动学习特征和模式，具有更高的适应性和通用性，能够更好地处理复杂的目标检测任务，克服传统方法的弊端。

3、现有的跟踪技术有基于图像的视觉伺服方法和基于位置的视觉伺服方法。尽管基于位置的视觉伺服方法在伺服控制领域具有主导地位，但它们也存在一些显著的弊端。这些方法通常依赖于位置传感器和准确的机器定位，而这些传感器可能受到噪声、漂移或精度限制的影响。此外，基于位置的方法通常无法提供有关目标外观和特征的信息，因此在处理复杂目标、非结构化环境或需要高度感知的应用中可能受到限制。它们对于动态场景和遮挡情况的应对能力也有限，因为它们通常只关注目标的位置而忽略了目标的运动和外观变化。因此，虽然基于位置的视觉伺服方法在某些应用中仍然有用，但它们的限制和弊端表明了基于图像的视觉伺服方法的重要性，特别是在需要更全面、灵活和智能的伺服控制环境中。

4、但是在实际运用当中，由于复杂环境无法很好的检测到目标位置以及无法得知跟踪目标的位置和速度，同时在gps受限的情况下如何确定无人机和目标的相对位置也是一个需要解决的问题。因此目标检测跟踪一直以来都是充满挑战和创新的研究领域。近年来随着科技不断发展和完善，无人机在目标检测跟踪领域主要使用的是基于位置的视觉伺服算法，这就需要对无人机和目标的高精度准确的建模，因此在面对复杂环境的情况下无法保证无人机能够很好的完成目标，这就需要一个性能更好，可靠性更高的方法。

技术实现思路

1、针对现有技术存在的对外部环境敏感及建模精度需求高的问题，本发明的主要目的是提出一种基于鲁棒控制的四旋翼目标检测跟踪方法。将目标检测算法与深度相机捕获的目标特征点作为输入，用于获取目标的深度信息以计算深度距离；然后，通过图像雅可比矩阵直接生成控制信号；最后，通过鲁棒性控制，实现无人机对目标的实时检测与跟踪控制。无需gps并降低对建模精准度的依赖、提高对参数的抗性及抗外界干扰的能力。

2、本发明的目的是通过以下技术方案实现的：

3、本发明公开的一种基于鲁棒控制的四旋翼目标检测跟踪方法，具体步骤如下：

4、步骤一、建立四旋翼无人机的六自由度模型；

5、fi＝{oixiyizi}是建立在地球上的惯性坐标系，fb＝{obxbybzb}和fc＝{ocxcyczc}分别是四旋翼和相机的体坐标系。其中，oi、ob、oc为惯性系坐标原点，xi、xb、xc，yi、yb、yc，zi、zb、zc依次为x，y，z三个方向的坐标。

6、四旋翼的运动学方程为

7、

8、

9、其中，是无人机在惯性坐标系fe当中的重心位置是p的导数。表示在fe中的线速度，表示在惯性系fe下的加速度，表示一个3×1的欧几里得空间。g代表重力加速度，m代表质量，代表在无人机框架当中的推力，表示一个1×1的欧几里得空间。旋转矩阵从飞机坐标系到惯性系与欧拉角γ＝[φ,θ,ψ]t相关的旋转矩阵由下式确定：

10、

11、其中φ,θ,ψ分别代表四旋翼无人机的横滚角、俯仰角和航向角，是坐标系fe当中的沿z轴方向的单位向量。是x，y，z三个方向的扰动加速度。

12、四旋翼的动力学方程为

13、

14、

15、其中是在坐标系fb下的角速度。是在坐标系fe下的角速度。是惯性矩阵，是使用的扭矩，是扰动扭矩。

16、p和ω各自三个自由度的运动学和动力学方程即为四旋翼六自由度模型。

17、步骤二、获取相对位置，建立在四旋翼坐标系fb下的相对位置运动学和动力学方程；

18、采用深度学习算法作为目标检测算法，逐帧检测目标并计算中心位置，将中心位置和深度相机捕获目标的特征点的深度值作为输入，并对深度值进行降噪和滤波处理，再使用视觉伺服方法计算相对位置。

19、相对位置的获取方法如下：

20、定义相机的像素坐标系为fcc＝{u,v}

21、采用如下图像视觉伺服方法建立像素坐标系fcc和步骤一相机坐标系fc之间的关系：

22、

23、上述图像视觉伺服方法中s为深度距离。αx＝l/dx和αy＝l/dy，表示分别在像素坐标系fc中u轴和v轴上的归一化的焦距。[u1,v1]t表示在相机像素坐标系fcc当中的目标位置，[x1,y1,z1]t表示在相机坐标系fc当中的目标位置。[u0,v0]t是相机像素坐标中心点。此外参数(αx,αy,u0,v0)仅与照相机的内部参数相关。因此通过上述图像视觉伺服方法实现目标位置在二维像素坐标[u1,v1]t和三维坐标[x1,y1,z1]t之间的转换。

24、根据视觉目标在相机平面上的透视投影，提取以下图像特征来描述无人机的平移运动：

25、

26、其中深度距离s的求解方法为：

27、首先进行目标检测，读取目标位置，若检测到目标则返回目标边界框四个顶点坐标；其次计算其索引深度的中心像素位置，在边界框范围内将图像划分为48个部分；随后分别对每个部分计算深度信息，并存入列表当中；最后再求列表所存深度信息的均值作为深度距离。

28、由于摄像机固定在无人机上，且摄像机相对于无人机的三个欧拉角φ,θ,ψ均为0°。因此，无人机和摄像机的耦合，使得无人机和相机坐标系重合。只需要通过平移变换就能够通过相机和目标间的相对位置得到四旋翼和目标间的相对位置。相机和四旋翼之间的相对距离qt＝[x*,0,z*]t，x*和z*分别为四旋翼和相机在fb的水平和垂直距离。根据式(1)及式(2)建立在坐标系fb之下的相对位置运动学方程和动力学方程：

29、

30、

31、在上述相对位置动力学方程中代表坐标系fb下的目标位置代表的导数表示坐标系fb下的目标速度。f为力控制器，γc表示坐标系fb到惯性系的期望欧拉角。

32、根据式(5)及式(6)能够得到根据已知期望距离s*，推导出目标和相机之间的相对位置在相机坐标系fc之下的关系。建立坐标系fc中的相对位置观测器：

33、

34、步骤三、根据给定的期望相对位置，基于步骤二中的相对位置，通过鲁棒性控制，计算控制目标四旋翼的三个控制输入量，得到相对位置控制器；

35、将偏差(δqx,δqy,δqz)最小化，并将它们趋向于零。建立动力学估计模型：

36、

37、

38、建立力控制器模型：

39、

40、其中γ2＝diag(γ2x,γ2y,γ2z)，表示个3×3的对角矩阵其对角线元素为γ2x,γ2y,γ2z，是状态估计器，γ1＝diag(γ1x,γ1y,γ1z)表示3×3的对角矩阵其对角线元素为γ1x,γ1y,γ1z，是以为边界的饱和函数，tanh(x)是正切函数。通过饱和函数的控制使得干扰的影响趋于平滑，属于相对位置的鲁棒性控制的一部分。

41、得到鲁棒性相对位置控制器如下：

42、

43、其中表示所控制的干扰，体现出相对位置鲁棒性控制。

44、步骤四、根据给定期望姿态，通过鲁棒控制律，计算四旋翼的偏航角控制输入量，得到相对姿态控制器。

45、其中姿态的导数通过图像坐标下的目标中心位置获取，使得四旋翼和目标之间的姿态差被消除，从而四旋翼实时跟踪目标。

46、根据步骤一欧拉角，采用非线性变换如下

47、

48、φ,θ即为步骤一中飞机坐标系到惯性系与欧拉角γ＝[φ,θ,ψ]t。非线性变换定义在范围内。定义λ(0)∈(-π/2,π/2)。对λ求偏导数可得

49、

50、由于导数大于0因此单调递增。

51、给定

52、

53、确保λ→λc。

54、定义ξ＝[ξφ,ξθ,ξψ]t，ξ的一阶导数为

55、

56、θ＝diag((π2/4+φ2)/(π2/4-φ2)2,(π2/4+θ2)/(π2/4-θ2)2,1)的3×3的对角矩阵其对角线元素为(π2/4+φ2)/(π2/4-φ2)2,(π2/4+θ2)/(π2/4-θ2)2,1。

57、定义为θ的导数，可得：

58、的3×3的对角矩阵其对角线元素为

59、定义姿态跟踪误差用于姿态鲁棒性控制：

60、

61、定义姿态误差观测器：

62、

63、k1＞0是一个正常数，由于θ为对角矩阵因此可逆，定义θ-1为θ的逆矩阵。

64、为姿态的鲁棒性控制引入ωz，定义ωz为对时间的导数：

65、

66、其中和分别为ξc和γ的二阶导数，τ为步骤一中的扭矩，uτ的估计器如下：

67、

68、其中∈r>0是常数参数，zz和zτ由以下辅助系统生成：

69、

70、

71、根据初始状态zz(0)＝0和zτ(0)＝0，扭矩控制器为

72、

73、通过引入不确定性的误差或干扰根据姿态的误差动力学模型式(17)及式(20)，计算估计器式(24)，得到相对姿态控制器：

74、

75、其中k2>0是一个常量，是估计器误差。

76、步骤五、基于误差动力学方程，使得由步骤三所得的相对位置控制器及步骤四所得的得到相对姿态控制器构成的闭环系统最终一致有界，实现四旋翼无人机对目标的检测跟踪，无需gps并降低对建模精准度的依赖、提高对参数的抗性及抗外界干扰的能力。

77、有益效果：

78、1、本发明公开的一种基于鲁棒控制的四旋翼目标检测跟踪方法，采用鲁棒性算法对位置和姿态进行控制，抑制相机传输数据的延迟及四旋翼安装误差等模型的不确定性，以及外界光照和风速等扰动对系统的干扰影响；

79、2、本发明公开的一种基于鲁棒控制的四旋翼目标检测跟踪方法，使用基于图像的视觉伺服方法，不需要在不同的环境下重新建模，降低对建模精准度的依赖，对复杂环境具有适应性，简化控制系统设计；

80、3、本发明公开的一种基于鲁棒控制的四旋翼目标检测跟踪方法，采用基于深度学习的目标检测算法计算相对位置，解决无人机在无法使用gps以及目标速度和位置信息未知情况下的跟踪问题，提升再复杂环境下的执行任务的能力。