基于视觉的无人机跟踪地面目标半物理仿真平台验证方法
1.本发明属于无人机技术领域，具体涉及一种无人机跟踪地面目标半物理仿真平台验证方法。
背景技术：
2.无人机良好的隐身性、机动性和跟踪能力使其广泛应用于侦察监视、搜救等领域。无人机在执行敌情侦察、视觉导航、目标检测和目标跟踪等任务时，视觉系统是其中不可缺少的一部分。
3.无人机的视觉系统由云台、可见光及不可见光摄像机，以及图像传输设备等部分组成，主要用于对地面目标的检测，定位和跟踪。基于视觉的战场目标自动检测与跟踪技术系统已成为无人机对战场实现态势感知与精准打击的基本手段。无人机的视觉系统对战场环境进行图像采集，通过对图像进行一定的预处理、感兴趣区域分析和目标特征提取，实现对感兴趣目标的自动检测和跟踪，并计算出目标的位置，为攻击武器提供目标位置等信息以实现跟踪和精确打击。
4.但是在无人机跟踪地面目标的过程中，无人机容易受到多种因素的影响，如天气环境、风和传感器故障等。此外，由于无人机飞行速度快、起降困难和空域开放限制，实际试飞成本高、难度大且危险性大。因此，如何降低实验成本，有效开展实验是一个亟待解决的问题。
技术实现要素：
5.为了克服现有技术的不足，本发明提供了一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，该平台由上位机、实时仿真计算机、自驾仪和图像处理板卡组成。上位机运行仿真软件vegaprime(vp)，显示无人机的运动状态；利用vp开发视景显示系统，可以直观地显示无人机动力学特性，并模拟摄像机拍摄，通过udp输出视频流信息给图像处理板卡；图像处理板卡利用yolov5算法和eco算法检测和跟踪目标，计算目标位置，利用串口输入到自驾仪。自驾仪根据目标位置利用协调转弯制导律进行控制律解算，发送控制指令到实时仿真计算机，实时仿真计算机运行simulink模型，将无人机及目标的姿态位置信息发送到上位机，驱动视景更新，从而完成闭环仿真。本发明仿真平台操作简单、成本低、仿真还原度高。
6.本发明解决其技术问题所采用的技术方案包括如下步骤：
7.步骤1：所述半物理仿真平台的硬件部分包括上位机、自驾仪、图像处理板卡和实时仿真计算机；所述上位机与图像处理板卡电连接，图像处理板卡与自驾仪电连接，自驾仪与实时仿真计算机电连接，实时仿真计算机与上位机电连接，形成一个闭环系统；
8.所述半物理仿真平台的软件部分包括视景显示系统、目标检测定位系统、无人机控制系统和simulink模型系统；
9.所述视景显示系统运行在上位机中，用于模拟摄像机进行拍摄并输出视频流；
10.所述目标检测定位系统运行在图像处理板卡中，用于跟踪检测地面目标并计算目标位置；
11.所述无人机控制系统运行在自驾仪中，用于根据目标定位结果控制无人机飞行，引导无人机飞向目标；
12.所述simulink模型系统运行在实时仿真计算机中，用于无人机模型的搭建；
13.步骤2：视景显示系统输出图片序列到目标检测定位系统；
14.步骤2-1：在simulink模型系统的无人机仿真模型底部设置transform模块模拟云台，通过设置transform模块的俯仰角和水平旋转角模拟云台的俯仰角和水平旋转；视景显示系统中的主窗口从transform模块的角度显示场景；无人机的运动和云台的俯仰与水平旋转改变仿真摄像机的视场，视景显示系统中的主窗口显示不同的场景，主窗口的屏幕不断保存，以模拟摄像机的图像捕获；
15.步骤2-2：视景显示系统采用3d仿真软件vege prime作为开发平台；包括：视景更新模块，接收simulink模型系统发送的无人机的状态量，驱动视景更新；图像捕获模块，采用window句柄方式截取视景显示系统显示的图像，模拟摄像机的拍摄，并进行图片压缩；视景显示系统不断保存图片序列，发送给图像处理板卡；
16.步骤2-3：求解视景显示系统的内参矩阵；
17.步骤2-3-1：在视景显示系统中设置无人机、汽车和云台的位置及姿态信息，获取一张对应的图片；汽车在图像中的位置通过人工标记获得，视景显示系统的内参矩阵共有5个参数，标记一张图片获得两个方程，利用三张图片求解出内参矩阵m，再使用最大似然估计继续进行优化；
18.步骤2-3-2：世界坐标系和像素坐标系之间的关系用旋转矩阵ri和平移向量t表示，假设得到不同条件下的n幅图片，每幅图片上均有一个汽车的像点，因此目标图像模型表示为：
[0019][0020]
式中：p
t,i
表示第i幅图片上汽车像点的实际坐标，表示计算得到的p
t,i
在图像上的坐标，mi表示第i幅图像对应的内参数；
[0021]
定义p
f,i
为第i幅图片上汽车的实际像素点坐标，则像点p
f,i
的概率密度函数为：
[0022][0023]
式中，σ表示标准差参数；
[0024]
构造似然函数：
[0025][0026]
为了能够让l取得最大值，将问题转化为：
[0027][0028]
式(3)是一个非线性优化问题，使用levenberg-marquardt方法进行优化，利用人工标记作为初始值，不断进行迭代得到内参矩阵最优解；
[0029]
步骤3：目标检测定位系统检测图片序列中的地面目标即汽车，将目标位置信息发送到无人机控制系统；
[0030]
步骤3-1：使用yolov5算法进行目标检测，随后利用eco图像跟踪算法对目标进行持续跟踪，得到目标在图像中的像素位置；目标的像素位置使用矩形框进行描述，而目标所在矩形框中心点的像素坐标则作为汽车的像素坐标；
[0031]
步骤3-2：根据无人机的位置和姿态以及图像保存时云台的姿态，利用目标图像模型计算目标在世界坐标系中的位置；
[0032]
无人机在跟踪过程中，摄像机和云台的位置重合，跟随云台转动；目标的位置p
t
＝[x
t
,y
t
,z
t
]
t
表示为：
[0033]
p
t
＝p1+p2+p3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0034]
式中p1＝[x1,y1,z1]
t
为无人机在世界坐标系下的位置；p2＝[x2,y2,z2]
t
为无人机质心到摄像机光心的距离，由式(6)得到
[0035][0036]
式中为摄像机在机体坐标系下的位置，为机体坐标系到世界坐标系的转换矩阵；
[0037]
向量p3表示摄像机到目标的距离，满足如下关系：
[0038][0039]
式中为目标在摄像机坐标系下的位置，zc为图像深度，表示摄像机的光心沿光轴到目标的距离，为摄像机坐标系到机体坐标系的转换矩阵；
[0040]
式(5)转化为：
[0041][0042]
在考虑摄像头畸变的情况下，得到：
[0043][0044]
式中，θ表示摄像头的畸变角度，f表示镜头的焦距。
[0045]
由式(8)和式(9)得目标图像模型的表达式为：
[0046][0047]
式中，(u,v)表示目标图像在像素坐标系中的位置。
[0048]
为了估计zc，假定地面高程地图已知，则地面目标的海拔高度z
t
已知；现在设则代入式(10)得：
[0049][0050]
式中表示的第3行第j(j＝1,2,3)列元素，κ
31
,κ
32
,κ
33
分别表示κ的第3行第j(j＝1,2,3)列元素。
[0051]
则zc通过下式计算得到
[0052][0053]
获得深度信息zc之后，式(10)右侧的剩余变量通过传感器量测获得，则利用式(13)估算出目标的在世界坐标系下的位置：
[0054][0055]
式中和κ
ij
分别为和κ的第i行第j列元素；
[0056]
步骤3-3：将目标位置发送给无人机控制系统，用于计算飞行制导指令；
[0057]
步骤4：无人机控制系统接收到目标在世界坐标系下的位置，利用制导律生成控制指令，并通过控制律生成控制面偏转指令；自动驾驶仪向实时仿真计算机发送控制命令；
[0058]
假设地面目标的位置在世界坐标系下的位置为(x
t
,y
t
,z
t
)，无人机在世界坐标系下的位置为(x,y,z)，假设无人机和地面目标均在固定高度上运动，因此仅考虑横向二维平面；无人机与地面目标之间的水平距离为无人机与地面目标之间的相对运动关系：
[0059][0060]
其中η为侧轴角，即横向加速度方向与无人机目标中心连线的夹角，v为无人机的速度，a为控制输入，即需要设计的制导律；控制目标就是无人机在制导律的作用下，在经过一段时间飞行后，收敛到期望的跟踪圆上，即r
→
r，r为收敛半径；
[0061]
无人机在围绕目标以半径r进行协调转弯飞行时，所需的横向加速度指令为：
[0062][0063]
引入相对距离偏差r＝ρ-r和侧轴角η，得到如下的协调转弯制导律：
[0064][0065]
其中k》0表示制导增益，ξ》0表示阻尼比；系统的运动(14)在制导律(16)的作用下是全局渐进收敛的，选取如下的李雅普诺夫候选函数：
[0066][0067]
对式(17)两端同时求导得：等号仅在平衡点(r,η)＝(r,2nπ),n＝0,
±
1,
±
2,
…
；因此，在制导律式(17)的作用下，无人机在任意位置任意航向运动在经过一段时间的飞行均能收敛到期望的参考圆上；在获取到横向制导指令后，无法直接输入到控制系统中，需要对制导指令转化成滚转角指令，转化公式为：
[0068][0069]
φg为滚转角指令，g为重力加速度，至此能进行对地面静止目标的跟踪仿真验证；
[0070]
在跟踪地面移动目标时，需要将制导律进行扩展，增加相对变量来修正制导律：
[0071][0072]
其中vr为无人机相对地面目标的移动速度，ηr为相对侧轴角；ar垂直于vr，首先使用将ar转化为垂直于v的加速命令a，再用式(18)即将加速度a转化为滚转角指令；
[0073]
步骤5：simulink模型系统运行无人机模型和目标模型，将无人机和目标的状态信息发送给无人机控制系统，同时接收无人机控制系统发送的控制指令；实时仿真计算机将无人机和目标的状态信息发送给视景显示系统，驱动视景更新，从而实现闭环仿真。
[0074]
优选地，所述上位机与图像处理板卡的通信方式为udp；自驾仪与实时仿真计算机的通信方式为串口；图像处理板卡与自驾仪的通信方式为串口；上位机与实时仿真计算机的通信方式为udp。
[0075]
优选地，所述上位机为惠普工作站z240；所述自驾仪型号为dreambuilder01，处理器是stm32f429；所述图像处理板卡，型号为jetson xavier nx；所述实时仿真计算机型号为研华生产的ipc-610h。
[0076]
优选地，所述无人机控制系统利用协调转弯制导律生成控制指令，并通过pid控制律生成控制面偏转指令。
[0077]
本发明的有益效果如下：
[0078]
本发明的一种基于视觉的无人机检测跟踪地面目标仿真平台验证方法，通过包
括：运行于上位机的视觉显示系统和地面站、运行于图像处理板卡的目标检测定位系统、运行于自驾仪的无人机控制系统和运行于实时仿真计算机的simulink模型系统，实现了无人机跟踪地面目标的仿真验证，有效降低了实验成本，所得结果具有工程价值。本发明仿真平台操作简单、成本低、仿真还原度高，并且各个子系统采用模块化设计，方便进行推广和替换，有效解决了无人机跟踪地面目标实验难度高成本大的问题。
附图说明
[0079]
图1为本发明实仿真平台的系统构成示意图。
[0080]
图2为本发明仿真平台的整体构成示意图。
[0081]
图3为本发明实施例的视景显示系统输出示意图。
[0082]
图4为本发明仿真平台的通信方式示意图。
[0083]
图5为本发明实施例提供的仿真平台的目标图像模型示意图。
[0084]
图6为本发明实施例提供的无人机跟踪地面目标二维运动模型示意图。
[0085]
图7为本发明实施例提供的制导方法跟踪静止目标的数字仿真结果；(a)水平面运动轨迹；(b)水平面相对距离；(c)滚转角指令与响应；(d)高度。
[0086]
图8为本发明实施例提供的真实运动目标的数据。(a)目标实际运动轨迹；(b)目标实际运动速度。(a)水平面运动轨迹；(b)水平面相对距离；(c)滚转角指令与响应；(d)高度。
[0087]
图9为本发明实施例提供的制导方法跟踪真实运动目标的数字仿真结果。
[0088]
图10为本发明实施例提供的仿真平台的运行示意图。
[0089]
图11为本发明实施例提供的仿真平台的图像跟踪结果。
[0090]
图12为本发明实施例提供的仿真平台的直线运动仿真结果；(a)三维位置轨迹；(b)水平面位置轨迹；(c)相对距离；(d)舵面偏角；(e)制导指令和响应；(f)速度。
[0091]
图13为本发明实施例提供的仿真平台的曲线运动仿真结果；(a)三维位置轨迹；(b)水平面位置轨迹；(c)相对距离；(d)舵面偏角；(e)制导指令和响应；(f)速度。
具体实施方式
[0092]
下面结合附图和实施例对本发明进一步说明。
[0093]
本发明实施例提出一种基于视觉的无人机检测跟踪地面目标半物理仿真平台验证方法，以解决无人机跟踪地面目标实验，由于无人机飞行速度快，起降难度大，空域管控等问题导致试飞实验成本大，难以开展的问题。
[0094]
仿真平台包括：视景显示系统、目标检测定位系统、无人机控制系统和simulink模型系统；
[0095]
视景显示系统，用于直观地研究飞机的动力学特性，模拟摄像机进行拍摄并输出视频流；
[0096]
目标检测定位系统，用于跟踪检测地面目标并计算目标位置；
[0097]
无人机控制系统，用于根据目标定位结果控制无人机飞行，引导无人机飞向目标；
[0098]
simulink模型系统，用于无人机的数学模型的搭建，进行无人机六自由度非线性模型解算。
[0099]
无人机检测跟踪地面目标仿真平台的硬件条件包括上位机，自驾仪，图像处理板
卡，工控机(实时仿真计算机)；
[0100]
上位机，为惠普工作站z240，运行视景显示系统和地面站；
[0101]
自驾仪，型号为dreambuilder01，处理器是stm32f429，运行无人机制导和控制系统；
[0102]
图像处理板卡，型号为jetson xavier nx，运行目标检测定位系统；
[0103]
工控机，型号为研华生产的ipc-610h，运行simulink模型系统，具有严格的实时解算能力。
[0104]
无人机检测跟踪地面目标仿真平台中视觉仿真系统采用3d软件vege prime进行开发，包括：视景更新模块，使用udp接收simulink模型系统发送的无人机的状态量，驱动视景更新；图像捕获模块，采用window句柄方式截取视景软件显示的图像，模拟摄像机的拍摄，并进行图片压缩，方便进行udp发送。
[0105]
目标检测定位系统运行基于深度学习的目标检测算法和基于相关滤波的目标跟踪算法实现对目标的定位和跟踪，并利用一种目标图像模型，实现单目相机对地面目标的定位。
[0106]
无人机控制系统根据目标的位置，使用协调转弯制导律和pid控制律，生成控制指令，实现无人机对目标的持续跟踪。
[0107]
simulink模型系统采用matlab/xpc技术，在实时仿真计算机上启动一个32位保护模式的实时内核，运行无人机的simulink模型。
[0108]
无人机检测跟踪地面目标仿真平台中，上位机与图像处理板卡的通信方式为udp；自驾仪与实时仿真计算机的通信方式为串口；图像处理板卡与自驾仪的通信方式为串口，上位机与实时仿真计算机的通信方式为udp。
[0109]
目标图像模型采用地形匹配方式获取图像深度信息，并实现对视景显示系统中虚拟摄像机的标定。
[0110]
仿真平台搭建：
[0111]
仿真系统由视景显示系统、目标检测定位系统、无人机控制系统和simulink模型系统组成。系统的结构如图1所示。整个跟踪系统的示意图如图2所示。
[0112]
视景显示系统采用3d仿真软件vege prime作为开发平台。在仿真无人机底部设置了一个transform模块来模拟云台。通过设置transform模块的俯仰角和水平旋转角来模拟云台的俯仰角和水平旋转。视景仿真系统中的主窗口从transform模块的角度显示场景。无人机的运动和云台的俯仰与水平旋转将改变摄像机的视场，主窗口将显示不同的场景。主窗口的屏幕不断保存，以模拟摄像机的图像捕获。视景显示系统的输出如图3所示。
[0113]
目标跟踪定位系统运行在jetson xavier nx硬件平台上。首先使用yolov5算法进行目标检测，随后利用eco图像跟踪算法对感兴趣的目标进行持续跟踪。一旦eco开始跟踪，它将得到目标在图像中的像素位置。目标的像素位置使用矩形框进行描述，而目标所在矩形框中心点的像素坐标则作为汽车的像素坐标。然后，根据无人机的位置和姿态以及图像保存时云台的姿态，利用目标图像模型计算目标在世界坐标系中的位置。随后将目标位置发送给无人机控制系统，用于计算飞行制导指令。为了更好的训练yolov5网络，本发明实例收集了2347张含有航拍汽车的图片，标记了共34900辆汽车。训练出的yolov5网络可以对航拍汽车进行有效的检测，航拍车辆检测准确率达到67.7％，召回率达到75.5％。
1)转化为
[0131][0132]
在考虑摄像头畸变的情况下，可得
[0133][0134]
式中m表示相机的内参矩阵，需要进行标定。
[0135]
由式(1-4)和式(1-5)可得目标图像模型的表达式为
[0136][0137]
图像深度zc通常是未知的。为了估计zc，假定地面高程地图已知，则地面目标的海拔高度z
t
已知。现在设则代入式(1-6)得
[0138][0139]
则zc通过下式计算得到
[0140][0141]
获得深度信息zc之后，式(1-6)右侧的剩余变量可通过传感器量测获得，则利用式(1-9)可估算出目标的在世界坐标系下的位置。
[0142][0143]
式中和κ
ij
分别为和κ的第i行第j列元素。
[0144]
摄像机标定：
[0145]
为了利用式(1-6)计算目标位置，首先需要对视景软件的内参矩阵进行标定。实际工程中，最常用的标定方法是张正友提出的基于单平面棋盘格的标定方法。该方法只需利用摄像机拍摄若干张棋盘格照片，步骤简单，实用性较强。但是在仿真系统中，视景系统是使用了虚拟摄像机来模拟真实摄像机的拍摄，无法拍摄棋盘格，则张正友标定法无法应用。受到张正友标定的启发，本发明提出一种精确可靠的标定方法，可直接标定视景仿真软件中的内参矩阵。
[0146]
目标图像模型如式(1-6)所示。通过在视景软件中设置无人机、汽车和云台的位置和姿态信息，可获得一张对应的图片。汽车在图像中的位置通过人工标记获得，深度信息zc的解算如式(1-8)，因此，式(1-6)中仅有内参矩阵未知。内参矩阵共有5个参数，标记一张图片获得两个方程，因此利用与类似的三张图片就可求解出内参矩阵m，但是这样计算出的结果并没有实际的物理意义，并且存在误差。为进一步增加标定结果的可靠性，将使用最大似然估计来优化上面得到的结果。
[0147]
世界坐标系和像素坐标系之间的关系用一个旋转矩阵r和平移向量t表示，假设得到不同条件下的n幅图片，每幅图片上均有一个汽车的像点，因此目标图像模型表示为
[0148][0149]
式中：p
t,i
表示第i幅图片上汽车像点的实际坐标，表示由式(1-10)计算得到的p
t,i
在图像上的坐标，mi表示第i幅图像对应的内参数。定义p
f,i
为第i幅图片上汽车的实际像素点坐标，则像点p
f,i
的概率密度函数为
[0150][0151]
构造似然函数
[0152][0153]
为了能够让l取得最大值，将问题转化为
[0154][0155]
式(1-13)是一个非线性优化问题，将使用levenberg-marquardt方法进行优化。利用最开始标定得到的解作为初始值，不断进行迭代得到最优解。
[0156]
视景仿真软件分辨率设置为1280
×
960，横向和纵向的视场角均设置为60
°
，经过计算，模拟摄像机的内参矩阵的值为
[0157][0158]
至此可以获取地面目标的位置，假设地面目标的位置在世界坐标系下的位置为(x
t
,y
t
,z
t
)，无人机在世界系下的位置为(x,y,z)，假设无人机和地面目标均在固定高度上运动，因此仅考虑横向二维平面。无人机与地面目标之间的水平距离为无人机与地面目标之间的相对运动关系如图6所示：
[0159][0160]
其中η为侧轴角，横向加速度方向与无人机目标中心连线的夹角，v为无人机的速度，a为控制输入，即需要设计的制导律。控制目标就是无人机在制导律的作用下，在经过一段时间飞行后，收敛到期望的跟踪圆上，即r
→
r，r为收敛半径。
[0161]
无人机在围绕目标以半径r进行协调转弯飞行时，所需的横向加速度指令为：
[0162][0163]
当无人机距离目标较远时，这种指令不在适用。于是在协调转弯运动的基础上引入相对距离偏差r＝ρ-r和侧轴角η，提出如下的协调转弯制导律：
[0164][0165]
其中k》0表示制导增益，ξ》0表示阻尼比。相对距离偏差d的引入可以消除跟踪静差，侧轴角η的引入可以增加系统阻尼，减少收敛时的超调量。系统的运动(1-15)在制导律(1-17)的作用下是全局渐进收敛的，选取如下的李雅普诺夫候选函数：
[0166][0167]
对(1-18)两端同时求导得：等号仅在平衡点(r,η)＝(r,2nπ),n＝0,
±
1,
±
2,
…
。因此，在制导律(1-18)的作用下，无人机在任意位置任意航向运动在经过一段时间的飞行均可收敛到期望的参考圆上。在获取到横向制导指令后，无法直接输入到控制系统中，需要对制导指令转化成滚转角指令，转化公式为：
[0168][0169]
φg为滚转角指令，g为重力加速度，至此可以进行对地面静止目标的跟踪仿真验证。
[0170]
图7显示了无人机对于地面静止目标跟踪的数字仿真结果，可以看到无人机可以快速稳定地收敛到跟踪圆上。
[0171]
在跟踪地面移动目标时，需要将制导律进行扩展，增加相对变量来修正制导律：
[0172][0173]
其中vr为无人机相对地面目标的移动速度，ηr为相对侧轴角。ar垂直于vr，因此，需要首先使用将ar转化为垂直于v的加速命令a，再用式(1-19)即可将加速度a转化为滚转角指令。
[0174]
选取法国瓦尔德马恩总部门委员会收集的真实数据集，这组数据集中地面车辆的运动是多种多样的，包括高速运动、低速运动和动-停-转运动，而且由于实际数据是在早高峰和晚高峰时段采集的，受交通拥堵和红绿灯的影响，汽车有时行驶缓慢，有时甚至会停下来，如图8所示。
[0175]
利用制导律(1-20)来跟踪真实移动目标，数字仿真结果如图9所示，制导律仍可实现对目标的稳定跟踪，且无人机的飞行状态保持平稳。至此，可以形成整个仿真平台的闭环控制。
[0176]
仿真平台的运行场景如图10所示。在半物理仿真时，vp视景仿真软件不断输出视频流信号，jetson xavier nx运行yolov5检测算法对视频流信号进行检测，一旦发现目标，无人机将使用eco算法和制导律对目标进行跟踪。在仿真过程中，跟踪算法的部分输出如图11所示，绿框表示跟踪算法eco的输出。从结果可以看出，eco算法可以较为准确的定位汽车在图片中的位置。
[0177]
图12显示了跟踪直线运动目标的半物理仿真结果，直线运动的目标在东向和北向的速度分量分别为8m/s和8m/s。
[0178]
图13显示了跟踪变速变方向运动目标的半物理仿真的跟踪结果，目标的运动轨迹为一个正弦曲线。
技术特征：
1.一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，其特征在于，包括如下步骤：步骤1：所述半物理仿真平台的硬件部分包括上位机、自驾仪、图像处理板卡和实时仿真计算机；所述上位机与图像处理板卡电连接，图像处理板卡与自驾仪电连接，自驾仪与实时仿真计算机电连接，实时仿真计算机与上位机电连接，形成一个闭环系统；所述半物理仿真平台的软件部分包括视景显示系统、目标检测定位系统、无人机控制系统和simulink模型系统；所述视景显示系统运行在上位机中，用于模拟摄像机进行拍摄并输出视频流；所述目标检测定位系统运行在图像处理板卡中，用于跟踪检测地面目标并计算目标位置；所述无人机控制系统运行在自驾仪中，用于根据目标定位结果控制无人机飞行，引导无人机飞向目标；所述simulink模型系统运行在实时仿真计算机中，用于无人机模型的搭建；步骤2：视景显示系统输出图片序列到目标检测定位系统；步骤2-1：在simulink模型系统的无人机仿真模型底部设置transform模块模拟云台，通过设置transform模块的俯仰角和水平旋转角模拟云台的俯仰角和水平旋转；视景显示系统中的主窗口从transform模块的角度显示场景；无人机的运动和云台的俯仰与水平旋转改变仿真摄像机的视场，视景显示系统中的主窗口显示不同的场景，主窗口的屏幕不断保存，以模拟摄像机的图像捕获；步骤2-2：视景显示系统采用3d仿真软件vege prime作为开发平台；包括：视景更新模块，接收simulink模型系统发送的无人机的状态量，驱动视景更新；图像捕获模块，采用window句柄方式截取视景显示系统显示的图像，模拟摄像机的拍摄，并进行图片压缩；视景显示系统不断保存图片序列，发送给图像处理板卡；步骤2-3：求解视景显示系统的内参矩阵；步骤2-3-1：在视景显示系统中设置无人机、汽车和云台的位置及姿态信息，获取一张对应的图片；汽车在图像中的位置通过人工标记获得，视景显示系统的内参矩阵共有5个参数，标记一张图片获得两个方程，利用三张图片求解出内参矩阵m，再使用最大似然估计继续进行优化；步骤2-3-2：世界坐标系和像素坐标系之间的关系用旋转矩阵r
i
和平移向量t表示，假设得到不同条件下的n幅图片，每幅图片上均有一个汽车的像点，因此目标图像模型表示为：式中：p
t,i
表示第i幅图片上汽车像点的实际坐标，表示计算得到的p
t,i
在图像上的坐标，m
i
表示第i幅图像对应的内参数；定义p
f,i
为第i幅图片上汽车的实际像素点坐标，则像点p
f,i
的概率密度函数为：式中，σ表示标准差参数；
构造似然函数：为了能够让l取得最大值，将问题转化为：式(3)是一个非线性优化问题，使用levenberg-marquardt方法进行优化，利用人工标记作为初始值，不断进行迭代得到内参矩阵最优解；步骤3：目标检测定位系统检测图片序列中的地面目标即汽车，将目标位置信息发送到无人机控制系统；步骤3-1：使用yolov5算法进行目标检测，随后利用eco图像跟踪算法对目标进行持续跟踪，得到目标在图像中的像素位置；目标的像素位置使用矩形框进行描述，而目标所在矩形框中心点的像素坐标则作为汽车的像素坐标；步骤3-2：根据无人机的位置和姿态以及图像保存时云台的姿态，利用目标图像模型计算目标在世界坐标系中的位置；无人机在跟踪过程中，摄像机和云台的位置重合，跟随云台转动；目标的位置p
t
＝[x
t
,y
t
,z
t
]
t
表示为：p
t
＝p1+p2+p3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)式中p1＝[x1,y1,z1]
t
为无人机在世界坐标系下的位置；p2＝[x2,y2,z2]
t
为无人机质心到摄像机光心的距离，由式(6)得到式中为摄像机在机体坐标系下的位置，为机体坐标系到世界坐标系的转换矩阵；向量p3表示摄像机到目标的距离，满足如下关系：式中为目标在摄像机坐标系下的位置，z
c
为图像深度，表示摄像机的光心沿光轴到目标的距离，为摄像机坐标系到机体坐标系的转换矩阵；式(5)转化为：在考虑摄像头畸变的情况下，得到：
式中，θ表示摄像头的畸变角度，f表示镜头的焦距；由式(8)和式(9)得目标图像模型的表达式为：式中，(u,v)表示目标图像在像素坐标系中的位置；为了估计z
c
，假定地面高程地图已知，则地面目标的海拔高度z
t
已知；现在设κ∈r3×3，则代入式(10)得：式中表示的第3行第j(j＝1,2,3)列元素，κ
31
,κ
32
,κ
33
分别表示κ的第3行第j(j＝1,2,3)列元素；则z
c
通过下式计算得到获得深度信息z
c
之后，式(10)右侧的剩余变量通过传感器量测获得，则利用式(13)估算出目标的在世界坐标系下的位置：式中和κ
ij
分别为和κ的第i行第j列元素；步骤3-3：将目标位置发送给无人机控制系统，用于计算飞行制导指令；步骤4：无人机控制系统接收到目标在世界坐标系下的位置，利用制导律生成控制指令，并通过控制律生成控制面偏转指令；自动驾驶仪向实时仿真计算机发送控制命令；假设地面目标的位置在世界坐标系下的位置为(x
t
,y
t
,z
t
)，无人机在世界坐标系下的位置为(x,y,z)，假设无人机和地面目标均在固定高度上运动，因此仅考虑横向二维平面；无人机与地面目标之间的水平距离为无人机与地面目标之间的相对运动关系：
其中η为侧轴角，即横向加速度方向与无人机目标中心连线的夹角，v为无人机的速度，a为控制输入，即需要设计的制导律；控制目标就是无人机在制导律的作用下，在经过一段时间飞行后，收敛到期望的跟踪圆上，即r
→
r，r为收敛半径；无人机在围绕目标以半径r进行协调转弯飞行时，所需的横向加速度指令为：引入相对距离偏差r＝ρ-r和侧轴角η，得到如下的协调转弯制导律：其中k>0表示制导增益，ξ>0表示阻尼比；系统的运动(14)在制导律(16)的作用下是全局渐进收敛的，选取如下的李雅普诺夫候选函数：对式(17)两端同时求导得：等号仅在平衡点(r,η)＝(r,2nπ),n＝0,
±
1,
±
2,
…
；因此，在制导律式(17)的作用下，无人机在任意位置任意航向运动在经过一段时间的飞行均能收敛到期望的参考圆上；在获取到横向制导指令后，无法直接输入到控制系统中，需要对制导指令转化成滚转角指令，转化公式为：φ
g
为滚转角指令，g为重力加速度，至此能进行对地面静止目标的跟踪仿真验证；在跟踪地面移动目标时，需要将制导律进行扩展，增加相对变量来修正制导律：其中v
r
为无人机相对地面目标的移动速度，η
r
为相对侧轴角；a
r
垂直于v
r
，首先使用将a
r
转化为垂直于v的加速命令a，再用式(18)即将加速度a转化为滚转角指令；步骤5：simulink模型系统运行无人机模型和目标模型，将无人机和目标的状态信息发送给无人机控制系统，同时接收无人机控制系统发送的控制指令；实时仿真计算机将无人机和目标的状态信息发送给视景显示系统，驱动视景更新，从而实现闭环仿真。2.根据权利要求1所述的一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，其特征在于，所述上位机与图像处理板卡的通信方式为udp；自驾仪与实时仿真计算机的通信方式为串口；图像处理板卡与自驾仪的通信方式为串口；上位机与实时仿真计算机
的通信方式为udp。3.根据权利要求1所述的一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，其特征在于，所述上位机为惠普工作站z240；所述自驾仪型号为dreambuilder01，处理器是stm32f429；所述图像处理板卡，型号为jetson xavier nx；所述实时仿真计算机型号为研华生产的ipc-610h。4.根据权利要求1所述的一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，其特征在于，所述无人机控制系统利用协调转弯制导律生成控制指令，并通过pid控制律生成控制面偏转指令。
技术总结
本发明公开了一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，该平台由上位机、实时仿真计算机、自驾仪和图像处理板卡组成。上位机运行仿真软件VegaPrime(VP)，显示无人机的运动状态；利用VP开发视景显示系统，可以直观地显示无人机动力学特性，并模拟摄像机拍摄，通过UDP输出视频流信息给图像处理板卡；图像处理板卡利用YOLOv5算法和ECO算法检测和跟踪目标，计算目标位置，利用串口输入到自驾仪。自驾仪根据目标位置利用协调转弯制导律进行控制律解算，发送控制指令到实时仿真计算机，实时仿真计算机运行Simulink模型，将无人机及目标的姿态位置信息发送到上位机，驱动视景更新，从而完成闭环仿真。本发明仿真平台操作简单、成本低、仿真还原度高。仿真还原度高。仿真还原度高。
