CN115291536B - 基于视觉的无人机跟踪地面目标半物理仿真平台验证方法 - Google Patents
基于视觉的无人机跟踪地面目标半物理仿真平台验证方法 Download PDFInfo
- Publication number
- CN115291536B CN115291536B CN202210799590.3A CN202210799590A CN115291536B CN 115291536 B CN115291536 B CN 115291536B CN 202210799590 A CN202210799590 A CN 202210799590A CN 115291536 B CN115291536 B CN 115291536B
- Authority
- CN
- China
- Prior art keywords
- target
- aerial vehicle
- unmanned aerial
- image
- simulation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000004088 simulation Methods 0.000 title claims abstract description 98
- 230000000007 visual effect Effects 0.000 title claims abstract description 42
- 238000000034 method Methods 0.000 title claims abstract description 25
- 238000012795 verification Methods 0.000 title claims abstract description 16
- 230000033001 locomotion Effects 0.000 claims abstract description 29
- 238000012545 processing Methods 0.000 claims abstract description 28
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 17
- 230000004438 eyesight Effects 0.000 claims abstract description 14
- 238000001514 detection method Methods 0.000 claims description 28
- 239000011159 matrix material Substances 0.000 claims description 24
- 230000001133 acceleration Effects 0.000 claims description 15
- 238000004891 communication Methods 0.000 claims description 15
- 230000009471 action Effects 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 9
- 230000003287 optical effect Effects 0.000 claims description 9
- 238000005457 optimization Methods 0.000 claims description 7
- 238000013016 damping Methods 0.000 claims description 4
- 238000011161 development Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- 238000007476 Maximum Likelihood Methods 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical group C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 2
- 238000005094 computer simulation Methods 0.000 claims description 2
- 238000002360 preparation method Methods 0.000 claims description 2
- 230000009467 reduction Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 8
- 238000002474 experimental method Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 230000004044 response Effects 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 230000004304 visual acuity Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B17/00—Systems involving the use of models or simulators of said systems
- G05B17/02—Systems involving the use of models or simulators of said systems electric
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，该平台由上位机、实时仿真计算机、自驾仪和图像处理板卡组成。上位机运行仿真软件VegaPrime(VP)，显示无人机的运动状态；利用VP开发视景显示系统，可以直观地显示无人机动力学特性，并模拟摄像机拍摄，通过UDP输出视频流信息给图像处理板卡；图像处理板卡利用YOLOv5算法和ECO算法检测和跟踪目标，计算目标位置，利用串口输入到自驾仪。自驾仪根据目标位置利用协调转弯制导律进行控制律解算，发送控制指令到实时仿真计算机，实时仿真计算机运行Simulink模型，将无人机及目标的姿态位置信息发送到上位机，驱动视景更新，从而完成闭环仿真。本发明仿真平台操作简单、成本低、仿真还原度高。
Description
技术领域
本发明属于无人机技术领域，具体涉及一种无人机跟踪地面目标半物理仿真平台验证方法。
背景技术
无人机良好的隐身性、机动性和跟踪能力使其广泛应用于侦察监视、搜救等领域。无人机在执行敌情侦察、视觉导航、目标检测和目标跟踪等任务时，视觉系统是其中不可缺少的一部分。
无人机的视觉系统由云台、可见光及不可见光摄像机，以及图像传输设备等部分组成，主要用于对地面目标的检测，定位和跟踪。基于视觉的战场目标自动检测与跟踪技术系统已成为无人机对战场实现态势感知与精准打击的基本手段。无人机的视觉系统对战场环境进行图像采集，通过对图像进行一定的预处理、感兴趣区域分析和目标特征提取，实现对感兴趣目标的自动检测和跟踪，并计算出目标的位置，为攻击武器提供目标位置等信息以实现跟踪和精确打击。
但是在无人机跟踪地面目标的过程中，无人机容易受到多种因素的影响，如天气环境、风和传感器故障等。此外，由于无人机飞行速度快、起降困难和空域开放限制，实际试飞成本高、难度大且危险性大。因此，如何降低实验成本，有效开展实验是一个亟待解决的问题。
发明内容
为了克服现有技术的不足，本发明提供了一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，该平台由上位机、实时仿真计算机、自驾仪和图像处理板卡组成。上位机运行仿真软件VegaPrime(VP)，显示无人机的运动状态；利用VP开发视景显示系统，可以直观地显示无人机动力学特性，并模拟摄像机拍摄，通过UDP输出视频流信息给图像处理板卡；图像处理板卡利用YOLOv5算法和ECO算法检测和跟踪目标，计算目标位置，利用串口输入到自驾仪。自驾仪根据目标位置利用协调转弯制导律进行控制律解算，发送控制指令到实时仿真计算机，实时仿真计算机运行Simulink模型，将无人机及目标的姿态位置信息发送到上位机，驱动视景更新，从而完成闭环仿真。本发明仿真平台操作简单、成本低、仿真还原度高。
本发明解决其技术问题所采用的技术方案包括如下步骤：
步骤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：世界坐标系和像素坐标系之间的关系用旋转矩阵Ri和平移向量T表示，假设得到不同条件下的n幅图片，每幅图片上均有一个汽车的像点，因此目标图像模型表示为：
式中：Pt,i表示第i幅图片上汽车像点的实际坐标，表示计算得到的Pt,i在图像上的坐标，Mi表示第i幅图像对应的内参数；
定义Pf,i为第i幅图片上汽车的实际像素点坐标，则像点Pf,i的概率密度函数为：
式中，σ表示标准差参数；
构造似然函数：
为了能够让L取得最大值，将问题转化为：
式(3)是一个非线性优化问题，使用Levenberg-Marquardt方法进行优化，利用人工标记作为初始值，不断进行迭代得到内参矩阵最优解；
步骤3：目标检测定位系统检测图片序列中的地面目标即汽车，将目标位置信息发送到无人机控制系统；
步骤3-1：使用YOLOv5算法进行目标检测，随后利用ECO图像跟踪算法对目标进行持续跟踪，得到目标在图像中的像素位置；目标的像素位置使用矩形框进行描述，而目标所在矩形框中心点的像素坐标则作为汽车的像素坐标；
步骤3-2：根据无人机的位置和姿态以及图像保存时云台的姿态，利用目标图像模型计算目标在世界坐标系中的位置；
无人机在跟踪过程中，摄像机和云台的位置重合，跟随云台转动；目标的位置Pt＝[xt,yt,zt]T表示为：
Pt＝P1+P2+P3 (5)
式中P1＝[x1,y1,z1]T为无人机在世界坐标系下的位置；P2＝[x2,y2,z2]T为无人机质心到摄像机光心的距离，由式(6)得到
式中为摄像机在机体坐标系下的位置，/>为机体坐标系到世界坐标系的转换矩阵；
向量P3表示摄像机到目标的距离，满足如下关系：
式中为目标在摄像机坐标系下的位置，zc为图像深度，表示摄像机的光心沿光轴到目标的距离，/>为摄像机坐标系到机体坐标系的转换矩阵；
式(5)转化为：
在考虑摄像头畸变的情况下，得到：
式中，θ表示摄像头的畸变角度，f表示镜头的焦距。
由式(8)和式(9)得目标图像模型的表达式为：
式中，(u,v)表示目标图像在像素坐标系中的位置。
为了估计zc，假定地面高程地图已知，则地面目标的海拔高度zt已知；现在设则代入式(10)得：
式中表示/>的第3行第j(j＝1,2,3)列元素，κ31,κ32,κ33分别表示κ的第3行第j(j＝1,2,3)列元素。
则zc通过下式计算得到
获得深度信息zc之后，式(10)右侧的剩余变量通过传感器量测获得，则利用式(13)估算出目标的在世界坐标系下的位置：
式中和κij分别为/>和κ的第i行第j列元素；
步骤3-3：将目标位置发送给无人机控制系统，用于计算飞行制导指令；
步骤4：无人机控制系统接收到目标在世界坐标系下的位置，利用制导律生成控制指令，并通过控制律生成控制面偏转指令；自动驾驶仪向实时仿真计算机发送控制命令；
假设地面目标的位置在世界坐标系下的位置为(xt,yt,zt)，无人机在世界坐标系下的位置为(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为重力加速度，至此能进行对地面静止目标的跟踪仿真验证；
在跟踪地面移动目标时，需要将制导律进行扩展，增加相对变量来修正制导律：
其中Vr为无人机相对地面目标的移动速度，ηr为相对侧轴角；ar垂直于Vr，首先使用将ar转化为垂直于V的加速命令a，再用式(18)即将加速度a转化为滚转角指令；
步骤5：Simulink模型系统运行无人机模型和目标模型，将无人机和目标的状态信息发送给无人机控制系统，同时接收无人机控制系统发送的控制指令；实时仿真计算机将无人机和目标的状态信息发送给视景显示系统，驱动视景更新，从而实现闭环仿真。
优选地，所述上位机与图像处理板卡的通信方式为UDP；自驾仪与实时仿真计算机的通信方式为串口；图像处理板卡与自驾仪的通信方式为串口；上位机与实时仿真计算机的通信方式为UDP。
优选地，所述上位机为惠普工作站Z240；所述自驾仪型号为DreamBuilder01，处理器是STM32F429；所述图像处理板卡，型号为Jetson Xavier NX；所述实时仿真计算机型号为研华生产的IPC-610H。
优选地，所述无人机控制系统利用协调转弯制导律生成控制指令，并通过PID控制律生成控制面偏转指令。
本发明的有益效果如下：
本发明的一种基于视觉的无人机检测跟踪地面目标仿真平台验证方法，通过包括：运行于上位机的视觉显示系统和地面站、运行于图像处理板卡的目标检测定位系统、运行于自驾仪的无人机控制系统和运行于实时仿真计算机的Simulink模型系统，实现了无人机跟踪地面目标的仿真验证，有效降低了实验成本，所得结果具有工程价值。本发明仿真平台操作简单、成本低、仿真还原度高，并且各个子系统采用模块化设计，方便进行推广和替换，有效解决了无人机跟踪地面目标实验难度高成本大的问题。
附图说明
图1为本发明实仿真平台的系统构成示意图。
图2为本发明仿真平台的整体构成示意图。
图3为本发明实施例的视景显示系统输出示意图。
图4为本发明仿真平台的通信方式示意图。
图5为本发明实施例提供的仿真平台的目标图像模型示意图。
图6为本发明实施例提供的无人机跟踪地面目标二维运动模型示意图。
图7为本发明实施例提供的制导方法跟踪静止目标的数字仿真结果；(a)水平面运动轨迹；(b)水平面相对距离；(c)滚转角指令与响应；(d)高度。
图8为本发明实施例提供的真实运动目标的数据。(a)目标实际运动轨迹；(b)目标实际运动速度。(a)水平面运动轨迹；(b)水平面相对距离；(c)滚转角指令与响应；(d)高度。
图9为本发明实施例提供的制导方法跟踪真实运动目标的数字仿真结果。
图10为本发明实施例提供的仿真平台的运行示意图。
图11为本发明实施例提供的仿真平台的图像跟踪结果。
图12为本发明实施例提供的仿真平台的直线运动仿真结果；(a)三维位置轨迹；(b)水平面位置轨迹；(c)相对距离；(d)舵面偏角；(e)制导指令和响应；(f)速度。
图13为本发明实施例提供的仿真平台的曲线运动仿真结果；(a)三维位置轨迹；(b)水平面位置轨迹；(c)相对距离；(d)舵面偏角；(e)制导指令和响应；(f)速度。
具体实施方式
下面结合附图和实施例对本发明进一步说明。
本发明实施例提出一种基于视觉的无人机检测跟踪地面目标半物理仿真平台验证方法，以解决无人机跟踪地面目标实验，由于无人机飞行速度快，起降难度大，空域管控等问题导致试飞实验成本大，难以开展的问题。
仿真平台包括：视景显示系统、目标检测定位系统、无人机控制系统和Simulink模型系统；
视景显示系统，用于直观地研究飞机的动力学特性，模拟摄像机进行拍摄并输出视频流；
目标检测定位系统，用于跟踪检测地面目标并计算目标位置；
无人机控制系统，用于根据目标定位结果控制无人机飞行，引导无人机飞向目标；
Simulink模型系统，用于无人机的数学模型的搭建，进行无人机六自由度非线性模型解算。
无人机检测跟踪地面目标仿真平台的硬件条件包括上位机，自驾仪，图像处理板卡，工控机(实时仿真计算机)；
上位机，为惠普工作站Z240，运行视景显示系统和地面站；
自驾仪，型号为DreamBuilder01，处理器是STM32F429，运行无人机制导和控制系统；
图像处理板卡，型号为Jetson Xavier NX，运行目标检测定位系统；
工控机，型号为研华生产的IPC-610H，运行Simulink模型系统，具有严格的实时解算能力。
无人机检测跟踪地面目标仿真平台中视觉仿真系统采用3D软件Vege Prime进行开发，包括：视景更新模块，使用UDP接收Simulink模型系统发送的无人机的状态量，驱动视景更新；图像捕获模块，采用window句柄方式截取视景软件显示的图像，模拟摄像机的拍摄，并进行图片压缩，方便进行UDP发送。
目标检测定位系统运行基于深度学习的目标检测算法和基于相关滤波的目标跟踪算法实现对目标的定位和跟踪，并利用一种目标图像模型，实现单目相机对地面目标的定位。
无人机控制系统根据目标的位置，使用协调转弯制导律和PID控制律，生成控制指令，实现无人机对目标的持续跟踪。
Simulink模型系统采用Matlab/Xpc技术，在实时仿真计算机上启动一个32位保护模式的实时内核，运行无人机的Simulink模型。
无人机检测跟踪地面目标仿真平台中，上位机与图像处理板卡的通信方式为UDP；自驾仪与实时仿真计算机的通信方式为串口；图像处理板卡与自驾仪的通信方式为串口，上位机与实时仿真计算机的通信方式为UDP。
目标图像模型采用地形匹配方式获取图像深度信息，并实现对视景显示系统中虚拟摄像机的标定。
仿真平台搭建：
仿真系统由视景显示系统、目标检测定位系统、无人机控制系统和Simulink模型系统组成。系统的结构如图1所示。整个跟踪系统的示意图如图2所示。
视景显示系统采用3D仿真软件Vege Prime作为开发平台。在仿真无人机底部设置了一个Transform模块来模拟云台。通过设置Transform模块的俯仰角和水平旋转角来模拟云台的俯仰角和水平旋转。视景仿真系统中的主窗口从Transform模块的角度显示场景。无人机的运动和云台的俯仰与水平旋转将改变摄像机的视场，主窗口将显示不同的场景。主窗口的屏幕不断保存，以模拟摄像机的图像捕获。视景显示系统的输出如图3所示。
目标跟踪定位系统运行在Jetson Xavier NX硬件平台上。首先使用YOLOv5算法进行目标检测，随后利用ECO图像跟踪算法对感兴趣的目标进行持续跟踪。一旦ECO开始跟踪，它将得到目标在图像中的像素位置。目标的像素位置使用矩形框进行描述，而目标所在矩形框中心点的像素坐标则作为汽车的像素坐标。然后，根据无人机的位置和姿态以及图像保存时云台的姿态，利用目标图像模型计算目标在世界坐标系中的位置。随后将目标位置发送给无人机控制系统，用于计算飞行制导指令。为了更好的训练YOLOv5网络，本发明实例收集了2347张含有航拍汽车的图片，标记了共34900辆汽车。训练出的YOLOv5网络可以对航拍汽车进行有效的检测，航拍车辆检测准确率达到67.7％，召回率达到75.5％。
无人机控制系统运行在自驾仪上，无人机控制系统接收到目标在世界坐标系下的位置，利用制导律生成所需的控制指令，并通过控制律生成控制面偏转指令。然后，自动驾驶仪通过RS232串口向实时仿真计算机发送控制命令。
Simulink模型系统运行在由三个子系统组成：无人机子系统、汽车子系统和导弹子系统。无人机子系统的主要任务是运行无人机的六自由度模型，完成飞行任务。目标子系统的主要任务是运行目标模型，控制车辆的航向角和速度。目标子系统独立于其他子系统。因此，无人机子系统无法访问汽车的运动状态。
系统通信：
半物理仿真平台的通信示意图如图4所示。
视景显示系统不断保存图片序列，并通过UDP发送给图像处理板卡，与此同时，在发送该图片序列的同时，无人机的位置和姿态以及云台的姿态也通过UDP发送到图像处理板卡。
图像处理板卡接收到图像数据后，首先检测出目标在图像中的位置，然后利用目标图像模型计算出目标在世界坐标系下的位置，最后利用RS232串口，将目标位置信息发送到自驾仪。
自动驾驶仪接收到目标的位置后，利用制导律生成控制指令，并通过求解控制律生成操纵面偏转指令。然后，自动驾驶仪通过RS232串口向实时仿真计算机发送操纵面控制命令。
实时仿真计算机运行固定翼无人机模型和目标模型，通过RS232串口将无人机和目标的状态信息发送给自动驾驶仪，同时接收自驾仪发送的控制指令。此外，实时仿真计算机通过UDP协议发送给可视化显示软件，驱动视景更新，从而实现闭环仿真。
目标图像模型：
通过图像信息获取到目标在世界坐标系下的位置是进行目标跟踪的首要条件。因此本文根据无人机、云台(摄像机)和目标三者的几何关系提出了目标图像模型。无人机在跟踪过程中目标、云台、摄像机和无人机的相对位置关系如图5所示。摄像机和云台的位置重合，跟随云台转动。根据几何分析，目标的位置Pt＝[xt,yt,zt]T表示为
Pt＝P1+P2+P3. (1-1)
式中P1＝[x1,y1,z1]T为无人机在世界坐标系下的位置，P2＝[x2,y2,z2]T为无人机质心到摄像机光心的距离，可由下式得到
式中为摄像机在机体坐标系下的位置，/>为机体坐标系到世界坐标系的转换矩阵。
式(1-1)中向量P3表示摄像机到目标的距离，满足如下关系
式中为目标在摄像机坐标系下的位置，zc为图像深度，表示摄像机的光心沿光轴到目标的距离，/>为摄像机坐标系到机体坐标系的转换矩阵。因此，式(1-1)转化为
在考虑摄像头畸变的情况下，可得
式中M表示相机的内参矩阵，需要进行标定。
由式(1-4)和式(1-5)可得目标图像模型的表达式为
图像深度zc通常是未知的。为了估计zc，假定地面高程地图已知，则地面目标的海拔高度zt已知。现在设则代入式(1-6)得
则zc通过下式计算得到
获得深度信息zc之后，式(1-6)右侧的剩余变量可通过传感器量测获得，则利用式(1-9)可估算出目标的在世界坐标系下的位置。
式中和κij分别为/>和κ的第i行第j列元素。
摄像机标定：
为了利用式(1-6)计算目标位置，首先需要对视景软件的内参矩阵进行标定。实际工程中，最常用的标定方法是张正友提出的基于单平面棋盘格的标定方法。该方法只需利用摄像机拍摄若干张棋盘格照片，步骤简单，实用性较强。但是在仿真系统中，视景系统是使用了虚拟摄像机来模拟真实摄像机的拍摄，无法拍摄棋盘格，则张正友标定法无法应用。受到张正友标定的启发，本发明提出一种精确可靠的标定方法，可直接标定视景仿真软件中的内参矩阵。
目标图像模型如式(1-6)所示。通过在视景软件中设置无人机、汽车和云台的位置和姿态信息，可获得一张对应的图片。汽车在图像中的位置通过人工标记获得，深度信息zc的解算如式(1-8)，因此，式(1-6)中仅有内参矩阵未知。内参矩阵共有5个参数，标记一张图片获得两个方程，因此利用与类似的三张图片就可求解出内参矩阵M，但是这样计算出的结果并没有实际的物理意义，并且存在误差。为进一步增加标定结果的可靠性，将使用最大似然估计来优化上面得到的结果。
世界坐标系和像素坐标系之间的关系用一个旋转矩阵R和平移向量T表示，假设得到不同条件下的n幅图片，每幅图片上均有一个汽车的像点，因此目标图像模型表示为
式中：Pt,i表示第i幅图片上汽车像点的实际坐标，表示由式(1-10)计算得到的Pt,i在图像上的坐标，Mi表示第i幅图像对应的内参数。定义Pf,i为第i幅图片上汽车的实际像素点坐标，则像点Pf,i的概率密度函数为
构造似然函数
为了能够让L取得最大值，将问题转化为
式(1-13)是一个非线性优化问题，将使用Levenberg-Marquardt方法进行优化。利用最开始标定得到的解作为初始值，不断进行迭代得到最优解。
视景仿真软件分辨率设置为1280×960，横向和纵向的视场角均设置为60°，经过计算，模拟摄像机的内参矩阵的值为
至此可以获取地面目标的位置，假设地面目标的位置在世界坐标系下的位置为(xt,yt,zt)，无人机在世界系下的位置为(x,y,z)，假设无人机和地面目标均在固定高度上运动，因此仅考虑横向二维平面。无人机与地面目标之间的水平距离为无人机与地面目标之间的相对运动关系如图6所示：
/>
其中η为侧轴角，横向加速度方向与无人机目标中心连线的夹角，V为无人机的速度，a为控制输入，即需要设计的制导律。控制目标就是无人机在制导律的作用下，在经过一段时间飞行后，收敛到期望的跟踪圆上，即r→R，R为收敛半径。
无人机在围绕目标以半径R进行协调转弯飞行时，所需的横向加速度指令为：
当无人机距离目标较远时，这种指令不在适用。于是在协调转弯运动的基础上引入相对距离偏差r＝ρ-R和侧轴角η，提出如下的协调转弯制导律：
其中k>0表示制导增益，ξ>0表示阻尼比。相对距离偏差d的引入可以消除跟踪静差，侧轴角η的引入可以增加系统阻尼，减少收敛时的超调量。系统的运动(1-15)在制导律(1-17)的作用下是全局渐进收敛的，选取如下的李雅普诺夫候选函数：
对(1-18)两端同时求导得：等号仅在平衡点(r,η)＝(R,2nπ),n＝0,±1,±2,…。因此，在制导律(1-18)的作用下，无人机在任意位置任意航向运动在经过一段时间的飞行均可收敛到期望的参考圆上。在获取到横向制导指令后，无法直接输入到控制系统中，需要对制导指令转化成滚转角指令，转化公式为：
φg为滚转角指令，g为重力加速度，至此可以进行对地面静止目标的跟踪仿真验证。
图7显示了无人机对于地面静止目标跟踪的数字仿真结果，可以看到无人机可以快速稳定地收敛到跟踪圆上。
在跟踪地面移动目标时，需要将制导律进行扩展，增加相对变量来修正制导律：
其中Vr为无人机相对地面目标的移动速度，ηr为相对侧轴角。ar垂直于Vr，因此，需要首先使用将ar转化为垂直于V的加速命令a，再用式(1-19)即可将加速度a转化为滚转角指令。
选取法国瓦尔德马恩总部门委员会收集的真实数据集，这组数据集中地面车辆的运动是多种多样的，包括高速运动、低速运动和动-停-转运动，而且由于实际数据是在早高峰和晚高峰时段采集的，受交通拥堵和红绿灯的影响，汽车有时行驶缓慢，有时甚至会停下来，如图8所示。
利用制导律(1-20)来跟踪真实移动目标，数字仿真结果如图9所示，制导律仍可实现对目标的稳定跟踪，且无人机的飞行状态保持平稳。至此，可以形成整个仿真平台的闭环控制。
仿真平台的运行场景如图10所示。在半物理仿真时，VP视景仿真软件不断输出视频流信号，Jetson Xavier NX运行YOLOv5检测算法对视频流信号进行检测，一旦发现目标，无人机将使用ECO算法和制导律对目标进行跟踪。在仿真过程中，跟踪算法的部分输出如图11所示，绿色框表示跟踪算法ECO的输出。从结果可以看出，ECO算法可以较为准确的定位汽车在图片中的位置。
图12显示了跟踪直线运动目标的半物理仿真结果，直线运动的目标在东向和北向的速度分量分别为8m/s和8m/s。
图13显示了跟踪变速变方向运动目标的半物理仿真的跟踪结果，目标的运动轨迹为一个正弦曲线。
Claims (4)
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：世界坐标系和像素坐标系之间的关系用旋转矩阵Ri和平移向量T表示，假设得到不同条件下的n幅图片，每幅图片上均有一个汽车的像点，因此目标图像模型表示为：
式中：Pt,i表示第i幅图片上汽车像点的实际坐标，表示计算得到的Pt,i在图像上的坐标，Mi表示第i幅图像对应的内参数；
定义Pf,i为第i幅图片上汽车的实际像素点坐标，则像点Pf,i的概率密度函数为：
式中，σ表示标准差参数；
构造似然函数：
为了能够让L取得最大值，将问题转化为：
式(3)是一个非线性优化问题，使用Levenberg-Marquardt方法进行优化，利用人工标记作为初始值，不断进行迭代得到内参矩阵最优解；
步骤3：目标检测定位系统检测图片序列中的地面目标即汽车，将目标位置信息发送到无人机控制系统；
步骤3-1：使用YOLOv5算法进行目标检测，随后利用ECO图像跟踪算法对目标进行持续跟踪，得到目标在图像中的像素位置；目标的像素位置使用矩形框进行描述，而目标所在矩形框中心点的像素坐标则作为汽车的像素坐标；
步骤3-2：根据无人机的位置和姿态以及图像保存时云台的姿态，利用目标图像模型计算目标在世界坐标系中的位置；
无人机在跟踪过程中，摄像机和云台的位置重合，跟随云台转动；目标的位置Pt＝[xt,yt,zt]T表示为：
Pt＝P1+P2+P3 (5)
式中P1＝[x1,y1,z1]T为无人机在世界坐标系下的位置；P2＝[x2,y2,z2]T为无人机质心到摄像机光心的距离，由式(6)得到
式中为摄像机在机体坐标系下的位置，/>为机体坐标系到世界坐标系的转换矩阵；
向量P3表示摄像机到目标的距离，满足如下关系：
式中为目标在摄像机坐标系下的位置，zc为图像深度，表示摄像机的光心沿光轴到目标的距离，/>为摄像机坐标系到机体坐标系的转换矩阵；
式(5)转化为：
在考虑摄像头畸变的情况下，得到：
式中，θ表示摄像头的畸变角度，f表示镜头的焦距；
由式(8)和式(9)得目标图像模型的表达式为：
式中，(u,v)表示目标图像在像素坐标系中的位置；
为了估计zc，假定地面高程地图已知，则地面目标的海拔高度zt已知；现在设κ∈R3×3，则代入式(10)得：
式中表示/>的第3行第j(j＝1,2,3)列元素，κ31,κ32,κ33分别表示κ的第3行第j(j＝1,2,3)列元素；
则zc通过下式计算得到
获得深度信息zc之后，式(10)右侧的剩余变量通过传感器量测获得，则利用式(13)估算出目标的在世界坐标系下的位置：
式中和κij分别为/>和κ的第i行第j列元素；
步骤3-3：将目标位置发送给无人机控制系统，用于计算飞行制导指令；
步骤4：无人机控制系统接收到目标在世界坐标系下的位置，利用制导律生成控制指令，并通过控制律生成控制面偏转指令；自动驾驶仪向实时仿真计算机发送控制命令；
假设地面目标的位置在世界坐标系下的位置为(xt,yt,zt)，无人机在世界坐标系下的位置为(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为重力加速度，至此能进行对地面静止目标的跟踪仿真验证；
在跟踪地面移动目标时，需要将制导律进行扩展，增加相对变量来修正制导律：
其中Vr为无人机相对地面目标的移动速度，ηr为相对侧轴角；ar垂直于Vr，首先使用将ar转化为垂直于V的加速命令a，再用式(18)即将加速度a转化为滚转角指令；
步骤5：Simulink模型系统运行无人机模型和目标模型，将无人机和目标的状态信息发送给无人机控制系统，同时接收无人机控制系统发送的控制指令；实时仿真计算机将无人机和目标的状态信息发送给视景显示系统，驱动视景更新，从而实现闭环仿真。
2.根据权利要求1所述的一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，其特征在于，所述上位机与图像处理板卡的通信方式为UDP；自驾仪与实时仿真计算机的通信方式为串口；图像处理板卡与自驾仪的通信方式为串口；上位机与实时仿真计算机的通信方式为UDP。
3.根据权利要求1所述的一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，其特征在于，所述上位机为惠普工作站Z240；所述自驾仪型号为DreamBuilder01，处理器是STM32F429；所述图像处理板卡，型号为Jetson Xavier NX；所述实时仿真计算机型号为研华生产的IPC-610H。
4.根据权利要求1所述的一种基于视觉的无人机跟踪地面目标半物理仿真平台验证方法，其特征在于，所述无人机控制系统利用协调转弯制导律生成控制指令，并通过PID控制律生成控制面偏转指令。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210799590.3A CN115291536B (zh) | 2022-07-06 | 2022-07-06 | 基于视觉的无人机跟踪地面目标半物理仿真平台验证方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210799590.3A CN115291536B (zh) | 2022-07-06 | 2022-07-06 | 基于视觉的无人机跟踪地面目标半物理仿真平台验证方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115291536A CN115291536A (zh) | 2022-11-04 |
| CN115291536B true CN115291536B (zh) | 2024-04-19 |
Family
ID=83821919
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210799590.3A Active CN115291536B (zh) | 2022-07-06 | 2022-07-06 | 基于视觉的无人机跟踪地面目标半物理仿真平台验证方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115291536B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116661334B (zh) * | 2023-06-21 | 2024-03-08 | 哈尔滨工业大学 | 基于ccd相机的导弹跟踪目标半物理仿真平台验证方法 |
| CN117784621B (zh) * | 2024-02-27 | 2024-05-28 | 北京航空航天大学 | 一种垂直起降飞行器飞行控制律需求分析方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102789171A (zh) * | 2012-09-05 | 2012-11-21 | 北京理工大学 | 一种可视化无人机飞行控制半实物仿真测试方法及系统 |
| WO2017219296A1 (zh) * | 2016-06-22 | 2017-12-28 | 深圳市大疆创新科技有限公司 | 无人机仿真系统、无人机仿真方法及无人机 |
| CN111856965A (zh) * | 2020-06-22 | 2020-10-30 | 拓攻（南京）机器人有限公司 | 一种无人机可视化半实物仿真系统及其仿真方法 |
| CN112925223A (zh) * | 2021-02-03 | 2021-06-08 | 北京航空航天大学 | 基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统 |
| CN112965396A (zh) * | 2021-02-08 | 2021-06-15 | 大连大学 | 一种四旋翼无人机硬件在环可视化仿真方法 |
-
2022
- 2022-07-06 CN CN202210799590.3A patent/CN115291536B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102789171A (zh) * | 2012-09-05 | 2012-11-21 | 北京理工大学 | 一种可视化无人机飞行控制半实物仿真测试方法及系统 |
| WO2017219296A1 (zh) * | 2016-06-22 | 2017-12-28 | 深圳市大疆创新科技有限公司 | 无人机仿真系统、无人机仿真方法及无人机 |
| CN111856965A (zh) * | 2020-06-22 | 2020-10-30 | 拓攻（南京）机器人有限公司 | 一种无人机可视化半实物仿真系统及其仿真方法 |
| WO2021258327A1 (zh) * | 2020-06-22 | 2021-12-30 | 拓攻（南京）机器人有限公司 | 一种无人机可视化半实物仿真系统及其仿真方法 |
| CN112925223A (zh) * | 2021-02-03 | 2021-06-08 | 北京航空航天大学 | 基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统 |
| CN112965396A (zh) * | 2021-02-08 | 2021-06-15 | 大连大学 | 一种四旋翼无人机硬件在环可视化仿真方法 |
Non-Patent Citations (3)
| Title |
|---|
| 小型无人机地面目标跟踪系统机载云台自适应跟踪控制;辛哲奎;方勇纯;张雪波;;控制理论与应用;20100815(第08期);全文 * |
| 无人机视觉自主着陆仿真验证系统时间延迟测定研究;吴成富;钱全;陈怀民;马松辉;王亮;;计算机测量与控制;20100925(第09期);全文 * |
| 无人机跟踪系统仿真平台的设计与实现;林传健;章卫国;史静平;吕永玺;;哈尔滨工业大学学报;20200925(第10期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115291536A (zh) | 2022-11-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115291536B (zh) | 基于视觉的无人机跟踪地面目标半物理仿真平台验证方法 | |
| Roelofsen et al. | Reciprocal collision avoidance for quadrotors using on-board visual detection | |
| CN110347186B (zh) | 基于仿生双目联动的地面运动目标自主跟踪系统 | |
| CN107390704B (zh) | 一种基于imu姿态补偿的多旋翼无人机光流悬停方法 | |
| US20200191556A1 (en) | Distance mesurement method by an unmanned aerial vehicle (uav) and uav | |
| CN105652891A (zh) | 一种旋翼无人机移动目标自主跟踪装置及其控制方法 | |
| CN110231828B (zh) | 基于非奇异快速终端滑模的四旋翼无人机视觉伺服控制方法 | |
| CN103365297A (zh) | 基于光流的四旋翼无人机飞行控制方法 | |
| CN111474953B (zh) | 多动态视角协同的空中目标识别方法及系统 | |
| Dijkshoorn et al. | Integrating sensor and motion models to localize an autonomous ar. drone | |
| CN104932535B (zh) | 利用仿真系统对机载前视红外搜索设备闭环测试的方法 | |
| CN105717933A (zh) | 无人机以及无人机防撞方法 | |
| US10650694B2 (en) | Unmanned vehicle simulator | |
| Prabowo et al. | Hardware in-the-loop simulation for visual servoing of fixed wing UAV | |
| CN117455960B (zh) | 时变观测噪声条件下机载光电系统对地无源定位滤波方法 | |
| Zhao et al. | Vision-based tracking control of quadrotor with backstepping sliding mode control | |
| Moore et al. | UAV altitude and attitude stabilisation using a coaxial stereo vision system | |
| CN114488848A (zh) | 面向室内建筑空间的无人机自主飞行系统及仿真实验平台 | |
| CN110887486A (zh) | 一种基于激光线辅助的无人机视觉导航定位方法 | |
| Luo et al. | Docking navigation method for UAV autonomous aerial refueling | |
| Martínez et al. | Trinocular ground system to control UAVs | |
| Kehoe et al. | State estimation using optical flow from parallax-weighted feature tracking | |
| Strydom et al. | UAS stealth: target pursuit at constant distance using a bio-inspired motion camouflage guidance law | |
| CN116661334B (zh) | 基于ccd相机的导弹跟踪目标半物理仿真平台验证方法 | |
| Koumis et al. | Estimating metric scale visual odometry from videos using 3d convolutional networks |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |