利用视觉信息实现无人机追踪移动目标的方法

本发明属于无人机目标追踪与轨迹规划研究领域。针对在复杂环境中无人机对位置未知目标的追踪规划任务，提出了一种无人机在未知复杂环境下基于视觉传感器的目标追踪方法。具体涉及利用视觉信息实现无人机追踪移动目标的方法。

背景技术：

1、无人机因其机动灵活的特性和较低的成本，逐渐应用于民用及军用的各个领域。四旋翼无人机已能够在不同的应用场景中完成多种感知、控制与规划任务。在未知复杂的环境中，对移动目标的追踪规划任务仍然具有挑战性，无人机必须用有限的传感器识别目标，并尽量降低障碍物对目标遮挡的影响。为了安全性考虑，无人机还需要在安全避开障碍物的前提下，规划追踪轨迹，这要求轨迹规划具有较高的快速性。另一方面，无人机规划生成的轨迹应该是平滑的，以符合无人机动力学约束。根据以上问题，本发明考虑将未知复杂环境中对移动目标的追踪任务中可以分为目标运动预测与对目标的追踪规划。

2、目标运动预测主要分为基于概率统计的预测方法和基于深度学习的预测方法。基于概率统计的预测方法通过移动目标的历史轨迹数据来构建相应的数学模型。上海机电工程研究所的魏喜庆等人使用扩展卡尔曼滤波方法在制导领域完成与目标运动状态的估计(期刊：航天控制；著者：魏喜庆，顾龙飞，李瑞康，王社阳；出版年月：2017；文章题目：基于singer模型的高超声速飞行器轨迹跟踪与预测；页码：62-66)。但单一卡尔曼滤波器的预测精度较差。在基于深度学习的预测方法中，典型的网络模型有循环神经网络和长短期记忆网络。清华大学的ji xuewu等人利用长短期记忆网络预测高速公路上车辆的未来纵向和横向轨迹(期刊：china journal of highway and transport；著者：ji xuewu,fei cong,hexiangkun,liu yulong and liu yahui；出版年月：2019；文章题目：intentionrecognition and trajectory prediction for vehicles using lstm network；页码：34-42)。但基于深度学习的预测方法需要大量的训练数据进行前期模型训练，且不同的目标对象的训练数据存在差异。另外这类方法往往需要占用较大的计算资源，难以适用计算资源有限的无人机。

3、在无人机对目标追踪的研究中，先前的一些工作开发了基于视觉控制器的目标跟踪算法，此类算法将图像空间上定义的跟踪距离误差作为反馈，作为无人机控制的输入，来进行目标轨迹跟踪。这些方法大多没有考虑安全约束，不适用于复杂环境。近年来，一些工作提出了更完整的目标跟踪算法策略，这些算法考虑了避障功能、动力学可行性和跟踪效率。瑞士联邦理工学院的t等人提出了一种实时轨迹生成方法，这种方法结合模型预测控制，考虑了无人机的状态约束、动力学约束和障碍物的碰撞约束，将轨迹生成变为一个约束优化问题，并且可以实时求解(期刊：ieee robotics and automation letters；著者：t,alonso-mora j,domahidi a,rus d and hilliges o；出版年月：2017；文章题目：real-time motion planning for aerial videography with dynamic obstacleavoidance and viewpoint optimization；页码：1696-1703)。然而，该方法其中的优化公式都是非凸的，这在实现过程中可能导致局部最小问题。浙江大学的han zhichao等人提出了一种安全的目标轨迹追踪规划器，该规划器由目标已知的动力学搜索前端和时空最优轨迹规划后端组成。前端搜索设计了一个启发式函数，同时考虑了目标当前位置和目标未来位置。基于路径搜索的结果，后端算法构建飞行走廊，然后，后端优化器在飞行走廊内生成时空最优安全轨迹(会议：2021ieee international conference on robotics andautomation(icra2021)；著者：han z,zhang r,xu c,pan n and gao f；出版年月：2021；文章题目：fast-tracker:arobust aerial system for tracking agile target incluttered environments；页码：328-334)。然而上述基于优化的方法一般需要构造特殊的优化问题形式，而且选择不同的轨迹表示方式会影响规划轨迹的质量。在轨迹表示方法上，概率模型也是一种有效的方法。多伦多大学的anderson s等人将无人机轨迹表示为高斯过程，并使用概率推理模型进行求解(会议：10th conference on robotics-science andsystems(rss)；著者：anderson s,barfoot td,tong ch and s；出版年月：2015；文章题目：batch nonlinear continuous-time trajectory estimation as exactlysparse gaussian process regression；页码：221-238)。佐治亚理工学院的singh r等人使用了置信传播算法来解决概率模型推理优化问题(期刊：ieee transactions onautomatic control；著者：singh r,haasler i,zhang q s,karlsson j and chen yx；出版年月：2022；文章题目：inference with aggregate data in probabilistic graphicalmodels:an optimal transport approach；页码：4483-4497)。上述工作表明，概率推理模型能够有效地用于无人机轨迹的建模和求解问题，进而可适用于无人机对目标的追踪规划问题。

技术实现思路

1、为克服现有技术的不足，本发明旨在针对传统单一模型对未知目标运动预测存在准确率低的问题，提出一种基于自适应交互式双模型的目标运动预测方法。为此，本发明采取的技术方案是，利用视觉信息实现无人机追踪移动目标的方法，步骤如下：利用无人机上的机载深度相机，一方面将图像传输至深度学习目标检测模型yolov5s进行目标识别，并根据识别结果查找目标的深度距离信息，而后经过坐标转换得到目标在世界坐标系下的三维坐标；然后利用基于双模型交互的目标多步预测方法实现对目标位置的多步预测，预测出目标未来一段时间的位置；另一方面通过深度相机实时感知障碍物信息，根据深度相机的深度图像信息构建局部欧几里德符号距离场esdf地图；

2、无人机的轨迹规划分为前端路径搜索和后端优化两部分，其中，前端路径搜索指利用目标的当前位置和预测路径，在欧几里德符号距离场esdf地图信息的基础上搜索出一条具有目标运动导向的路径，使追踪具有前瞻性；在前端搜索的路径点基础上，后端优化部分构建基于概率推理的无人机轨迹模型，并根据任务要求设计似然函数，最后使用信念传播算法优化概率模型，得到一条光滑且安全的轨迹。

3、所述方法详细步骤如下：

4、步骤1)确定无人机动力学模型；

5、本发明考虑无人机的状态x(t)＝[pt(t),vt(t),at(t)]，其中pt(t)、vt(t)和at(t)分别代表无人机的三维位置、速度和加速度，并采用三阶质点模型作为无人机模型，其动力学方程形式如下：

6、

7、其中模型的输入向量为无人机的三维急速度(jerk)向量，即位置的三阶导数；

8、步骤2)确定目标运动数学模型；

9、将目标的运动模型离散化，其状态方程的形式如下

10、xk＝aixk-1+wk-1                              (2)

11、其中ai为系统转移矩阵，wk-1为系统误差，不同的运动模型具有不同形式的系统转移矩阵；

12、当目标进行匀速直线运动时，目标的状态定义为其中x,y分别代表x方向和y方向的位置，分别代表x方向和y方向的速度。该模型的系统转移矩阵ai的具体形式如下

13、

14、当目标进行圆周运动时，目标的状态定义为其中θ代表目标的偏航角。该模型的系统转移矩阵ai的具体形式如下

15、

16、步骤3)利用基于双模型交互的目标多步预测方法实现对目标位置的多步预测

17、定义目标运动模型的测量公式为

18、

19、其中z是观测矩阵，h是测量矩阵，v是噪声，方差是r，定义和pk-1|k-1分别为模型j在k-1时刻的最优估计状态和协方差矩阵，为模型j用目标k-1时刻的状态预测的状态，根据k-1时刻的概率向量，多模型交互方法会将每个模型的最优估计状态和协方差矩阵进行融合，融合后的估计状态和协方差矩阵为：

20、

21、其中为模型的概率系数，其具体计算形式如下

22、

23、其中pij为概率转移矩阵中的元素，代表目标由模型i转移到模型j的概率，为k-1时刻模型j的正确概率；

24、每个模型会根据融合后的估计状态进行状态估计，模型的似然函数值可以定量表明模型j对目标状态预测的准确程度，其具体形式表示如下

25、

26、其中zk为k时刻目标状态的测量值，为测量误差协方差矩阵，具体形式如下

27、

28、根据k时刻各个模型的似然函数值，模型的正确概率值会进行更新，k时刻模型j的正确概率值更新为：

29、

30、根据k时刻的模型概率向量，k时刻的最优估计状态由各个模型k时刻的最优估计状态加权融合得出，自适应交互式多模型imm算法输出的k时刻目标最优估计状态和协方差矩阵形式如下：

31、

32、定义多模型交互k时刻模型的转换比值为其具体形式如下：

33、

34、那么下一时刻的模型转移概率和更新为：

35、

36、算法会根据上一时刻各模型的似然值，自适应地调整的模型转移概率，当k时刻模型j的似然值大于模型i的似然值，这表明模型j为匹配模型的概率更大，目标在k+1时刻由模型i转移至模型j的概率会增大，模型j转移至模型i的概率会减小；

37、目标在复杂环境中的运动模型主要分为匀速直线运动和圆周运动两种，那么就使用基于双模型交互的目标多步预测方法实现对目标位置的多步预测，目标运动模型在未来短时间n个时间步长内不会改变，模型概率向量u即为时不变向量，根据目标当前k时刻的最优估计状态，目标在k+1时刻状态可以被预测，其计算表达式如下

38、

39、由于模型概率向量是时不变的，目标在k+2时刻的预测状态可由k+1时刻的预测状态计算得出。定义则函数f(xk)的表达式为那么目标在未来n个时间步长时的预测状态的计算公式如下

40、

41、步骤4)利用深度相机的深度图像信息构建局部欧几里德符号距离场esdf地图

42、无人机通过深度相机获取环境的深度图像信息，并基于这些信息构建局部欧几里德符号距离场esdf地图，具体过程如下：首先，利用深度相机捕捉环境的深度图像，获取场景中各点的深度数据；接着，对所获得的深度图像进行预处理，包括去噪、滤波等操作，以确保数据的准确性和可靠性；然后，将预处理后的深度图像转换为三维点云数据，表示场景中的各个点在三维空间中的位置；基于生成的点云数据，计算每个点到最近障碍物的欧几里德距离，从而构建局部欧几里德符号距离场esdf地图；

43、步骤5)设计基于目标运动导向的无人机前端路径搜索方法；

44、根据目标预测路径，定义pn为预测的n个时间步长后的目标位置，那么n个时间步长后的目标速度vn可由pn和pn-1差分计算得出，目标的状态定义为则具有目标运动导向的启发式代价总函数的形式如下：

45、fc＝gc+hc＝c(xsta,xcur)+c(xcur,xpre)                   (16)

46、其中gc＝c(xsta,xcur)代表从目标初始状态到当前状态的代价，hc＝c(xcur,xpre)代表从当前状态到预测状态的代价，目标从状态1到状态2的代价即可转换为求解一个两点边界值最优的问题，进而可求出启发式函数中的代价值；

47、步骤6)设计基于概率推理模型的无人机轨迹建模方法；

48、轨迹规划的任务是找到一条满足无人机动力学模型的最优轨迹x*(t)，后验概率p(x|e)代表事件e发生时轨迹的概率分布，那么轨迹规划的任务转换为找到使后验概率最大的无人机轨迹，如果事件是独立的，那么轨迹规划的任务即为求解以下问题：

49、

50、其中先验概率p(x)表示轨迹本身的自然分布，似然概率πp(ei|x)表示给定轨迹时各独立事件发生概率的乘积；

51、假设无人机轨迹x(t)为一个高斯过程，则其具体形式记为无人机模型也为线性定常系统，则得到鼓励零输入的轨迹，且轨迹是具有稀疏协方差矩阵的高斯过程，其分布形式如下所示：

52、

53、其中和分别是轨迹上初始位置和结束位置的状态约束，figp(xi-1,xi)表示两个相邻状态之间的相关性约束，目标速度跟随似然函数为：

54、

55、其中dtar＝||pi-ptar||代表无人机与目标之间的距离，pi为无人机当前位置，ptar为目标位置，dset为设定的转换距离，vtar为当前或预测的目标速度值，vdes为设定的无人机期望速度，σvel为协方差矩阵，其可以代表速度似然函数的权重系数。

56、目标速度跟随似然函数可以较好地提高对目标的追踪与跟踪效果。当dtar＞dset时，无人机与目标距离较远，此时无人机处于对目标的追踪阶段，速度似然函数使无人机速度趋向设定期望速度，无人机逐渐缩短与目标的距离。当dtar＜dset时，无人机与目标距离较近，此时无人机处于对目标的跟踪阶段；

57、为了使无人机追踪规划的轨迹安全避开障碍物，设计障碍物避碰似然函数为：

58、

59、其中dobs＝||pi-pobs||代表无人机与障碍物之间的距离，pi为无人机当前位置，pobs为障碍物的位置，dθ为设定的距离阈值，为了提高无人机轨迹的平滑性，无人机的加速度应保持在较低水平，设计的加速度最小化似然函数为

60、

61、状态的后验分布可以由设计好的先验分布和似然分布计算得出，设x(τ)是xi和xi+1之间的任何状态，高斯插值公式的具体形式表示为：

62、x(τ)＝λ(τ)xi+ψ(τ)xi+1

63、λ(τ)＝φ(τ,ti)-ψ(τ)φδt

64、

65、因子图由变量节点和因子节点组成，在因子图中，先验分布和似然分布被视为因子节点，支撑状态被视为变量节点，fp是应用于第一个和最后一个状态的状态约束因子节点，fgp是相邻支撑状态之间的高斯先验因子节点，fobs、fvel和facc是每个支撑状态的的似然分布因子节点，fobs(τ)、fvel(τ)和facc(τ)是相邻两个支撑状态之间的某个插值状态的似然分布因子节点，设fintp是两个支撑状态中所有插值状态的似然分布因子乘积；

66、为了提高后续概率模型推理过程的效率，因子图需要进行简化：设fiu为每个变量节点xi的一元因子，其为每个变量节点的状态约束因子和所有似然因子的乘积，fib为相邻两个变量节点之间的二元因子，其为两个相邻变量节点的高斯先验因子和所有插值状态的似然因子的乘积，具体形式如下：

67、fiu＝fiobsfivelfiacc

68、

69、步骤7)设计基于信念传播的概率模型优化求解方法；

70、当前的先验分布和似然分布形式不利于后续的概率模型推理过程，因此分布需要转换成如下标准形式：

71、

72、其中η是消息向量，λ是精度矩阵。这种标准形式可以使因子之间的乘法运算变成向量和矩阵之间的加法运算，提高了后续推理过程的效率。另外该求解形式可以更高效地求解出最大后验概率map状态；

73、为了将先前定义的分布转换为标准形式，本发明考虑状态约束和支撑状态的似然分布为一般高斯分布形式，表达式为

74、

75、其中h(x)是状态x的任意函数，如目标速度保持似然函数，he是期望值，如目标速度。将h(x)在x0处作一阶泰勒展开，e(x)转换为

76、

77、其中j0是h(x)在x0处微分而得到的雅可比矩阵，e0是常数。根据(34)的标准形式，一般高斯分布的消息向量和精度矩阵形式为：

78、

79、高斯先验分布和插值状态的似然分布是与相邻的两个支撑状态相关的二元函数，其对相邻的支撑状态的偏导数为：

80、

81、同理，根据(34)的标准形式，高斯先验分布和插值状态的似然分布的消息向量和精度矩阵形式为：

82、

83、使用消息向量η和精度矩阵λ来表示标准形式的分布，消息传递的过程分为三个：从因子节点到变量节点的消息传递，从变量节点到因子节点的消息传输和在变量节点上的信念更新；

84、定义为一元因子自身的分布消息，为二元因子自身的分布信息，为因子节点到变量节点的传输的消息，为变量节点到因子节点传输的消息，消息的传递过程即代表消息向量与精度矩阵的传递与相加；

85、一元因子节点fiu只与其连接的变量节点有关，其传递给变量节点的消息即是其自己的分布信息二元因子节点fib会接受来自xi的消息然后合并自身的分布消息合并后的精度矩阵和消息向量为：

86、

87、然后对变量xi+1边缘化，得到二元因子节点fib传递给变量节点xi+1的消息为

88、

89、同时二元因子节点fib也会接收来自xi+1的消息并合并自身的分布消息然后边缘化xi得到传递给变量节点xi的消息为：

90、

91、变量节点xi会收集一元因子fiu和二元因子传递来的消息，并将合并的消息发送给二元因子fib，同理会收集二元因子fib传递来的消息，并将合并的消息发送给二元因子发送给二元因子和fib的信息为：

92、

93、变量节点在接收所有相邻因子传递的消息后会更新自身的信息为：

94、

95、在得到边际变量节点的后验分布后，直接计算它们的最大后验概率map状态为：

96、

97、本发明的特点及有益效果是：

98、1.针对目标在未知复杂环境中运动不确定的特点，本发明使用自适应交互式多模型imm(interacting multiple model)算法来预测目标未来时刻的位置。通过将不同模型对目标运动预测结果进行融合，预测目标未来时刻的位置。该方法弥补了单一预测模型在面对运动目标(匀速直线运动及圆周运动)进行多步预测时灵活性差的特点，提高了对目标位置多步预测的精度；

99、2.设计了基于目标运动导向的前端搜索方法。该方法使用目标未来时刻的位置作为无人机前端路径规划的终点，搜索出一条具有目标运动导向的路径，降低了障碍物遮挡对目标跟踪的影响；

100、3.设计了基于概率推理模型的无人机轨迹规划方法。该方法在前端规划的路径点基础上，使用高斯过程表示无人机轨迹，并根据状态约束和避障约束等设计先验分布和似然分布，使用概率推理的方法完成轨迹优化，获得一条具有目标运动导向的、符合无人机动力学约束的安全轨迹。相比于以分段多项式表示无人机轨迹并进行轨迹优化的方法，该方法生成的追踪轨迹更加平滑；

101、4.结合基于深度学习的目标检测方法，使用基于机载传感器对目标进行检测，并结合上述追踪规划方法，得出一种在复杂环境中基于视觉的移动目标的检测与追踪规划策略，并通过仿真和实际飞行实验验证了其可行性。