CN112925223A - 基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统 - Google Patents

Abstract

本发明公开一种基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，具体为：虚拟测试环境模块搭建视觉传感网络的仿真模型，视觉传感网络处理后的有效信息作为视觉定位算法模块的输入；视觉定位算法模块输出目标无人机的观测位姿信息作为无人机跟踪控制模块的反馈输入；无人机跟踪控制模块的输出作为虚拟测试环境模块搭建的目标无人机的仿真模型的控制输入。该系统提供了开放的仿真环境，模块化的结构使得无人机模型、相机网络的组成及布局、网络特性的选择、标定算法、位姿估计算法、控制器的选择易于改变，方便扩展。该系统可大大节省调试时间，为当前三维跟踪领域中存在的更新率低、定位精度不能满足要求、实时性差等问题的解决提供实践平台。

Description

基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统

技术领域

本发明涉及一种基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，属于三维视觉跟踪仿真技术领域。

背景技术

基于视觉传感网络的无人机三维跟踪技术在很多领域有着广泛应用，如无人机安全回收、导弹末制导、虚拟现实、环境监测等。以环境监测为例：利用高效的视频图像处理算法，发挥其精度高、信息丰富、抗干扰能力强等特点，可以替代人进行大范围、全面的监测记录。如何搭建一种无限趋于真实的虚拟测试平台是无人机三维跟踪技术应用的重要问题之一，它对于优化三维跟踪技术相关算法、预防各类实际风险、缩短发展进程有着重大意义。目前的实践中，基于视觉的三维跟踪技术仍然存在更新率低、定位精度不能满足要求、实时性差等问题，如摄像机帧频的差异、触发的异步引起的非同步问题。在真实实验中，反映这些问题的指标难以获得。

当前基于视觉的无人机应用的仿真测试方法有硬件在环仿真，图像在环仿真，虚拟现实环境和数值模拟。数值模拟由于边界条件和模型的简化使得置信度降低。硬件在环仿真通常引入真实的传感器和执行机构，如相机和无人机；图像在环仿真通常引入真实传感器得到的信息，如相机捕捉的图像；两者都需要搭建真实的多相机网络，带来搭建过程引起的复杂性、设备类别引起的扩展性限制。而虚拟现实环境可以解决以上的局限性，在保证可信度的同时，易于扩展，快速开发。目前，在基于视觉传感网络的无人机三维跟踪领域，并未有基于虚拟现实的虚拟环境布置。

而无人机三维跟踪虚拟仿真平台可以记录各类瞬时指标，如特定时刻一定网络传输延迟，一定帧频差异下引起的跟踪误差；对于降低价格成本、缩短研究周期、提高算法精度有重要意义。因此，本发明提出了一种基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，该系统模拟基于视觉的无人机三维跟踪的全过程(包括挥杆、标定、跟踪、控制)，引入现实中的影响因素，通过GUI界面显示整个仿真过程。仿真系统设置多个接口，可以灵活调整摄像机布局结构，网络传输环境，无人机飞行路线等，不受成本、设备与场地限制，可以完成失效条件、极限状况等真实设备难以测试的任务。

发明内容

本发明提供了一种基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，它同时考虑了仿真环节的完整性、真实性和可视性，解决了真实实验参数难以改变、效果难以观测、算法难以调试的问题。该平台以还原真实实验平台为出发点，适用于多种影响因素下的仿真，对实际工程部署有很强的指导意义。

一种基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其组成如图1所示，包括虚拟测试环境模块、视觉定位算法模块、无人机跟踪控制模块三部分。模块的作用及相互关系通过图2说明：

首先简单介绍基于视觉传感网络的无人机三维跟踪的真实实验。由真实实验平台、视觉定位算法模块、PID跟踪控制器组成。在特定实验环境下搭建视觉传感网络。视觉传感网络是由大量视觉传感器节点构成的分布式结构，节点集成了图像传感器、嵌入式处理器和无线收发模块。其在无人机三维跟踪过程中，每个摄像机节点进行跟踪目标识别、特征提取、图像处理、传递有效信息至中央处理器。搭建完设备后，挥动标定杆，放置三角板，根据视觉传感网络处理后的有效信息完成相机标定环节。之后，在此基础上，通过视觉定位算法模块获得目标无人机的实时观测位姿信息。最后，PID跟踪控制器根据反馈的观测位姿信息修正无人机运动。

在仿真实验中，虚拟测试环境模块起到模拟真实实验平台的作用：视觉图像仿真模型模拟视觉传感网路中的相机节点，无线传感网络模型模拟视觉传感网络中的节点通讯，共同组成对于视觉传感网络的仿真；刚体模型模拟标定工具(标定杆和三角板)和目标无人机；环境渲染模型模拟实验环境。

视觉定位算法模块基于视觉传感网络处理的有效信息，实现跟踪目标无人机的任务。

无人机跟踪控制模块基于视觉定位算法模块输出的预估位姿信息，通过闭环控制修正无人机的轨迹。

在介绍完模块单独的功能后，介绍模块间的逻辑关系。如图2所示，虚拟测试环境模块、视觉定位算法模块、无人机跟踪控制模块间的关系为：虚拟测试环境模块搭建视觉传感网络的仿真模型，视觉传感网络处理后的有效信息作为视觉定位算法模块的输入；视觉定位算法模块输出目标无人机的观测位姿信息作为无人机跟踪控制模块的反馈输入；无人机跟踪控制模块的12维位姿输出作为虚拟测试环境模块搭建的目标无人机的仿真模型的控制输入。

在整体介绍了模块功能及整体联系后，根据图1具体说明每个模块的实现：

S1、搭建虚拟测试环境模块，所述的模块包括刚体模型、视觉图像仿真模型，无线传感网络模型和环境渲染模型；如图2，其作用为模拟真实实验平台，故虚拟测试环境模块主要实现以下功能：

环境渲染模型模拟实验环境；刚体模型模拟标定工具和跟踪无人机的运动；视觉图像仿真模型与无线视觉传感网络模型共同模拟视觉传感网络。其中，视觉图像仿真模型实现真实摄像机节点的目标识别、特征提取、图像处理功能；无线视觉传感网络模型模拟无线视觉传感网络节点之间的传输特性，实现传递有效信息至中央处理器的功能；最后，通过GUI界面模拟实验环节中的操作指令，按照图12中相机初始化、标定、设置原点、跟踪等环节需要的数据类型，组合各模型发送对应数据。具体为：

S11、建立环境渲染模型

真实实验的环境布局是在室内环境中完成。中央区域是铺有地毯的地面及所对应的室内空间。在墙体四周挂有相机节点。故所述环境渲染模型主要模拟室内环境以及相机覆盖情况。

建立FBX文件格式的墙体、地毯、相机三维模型，利用着色器进行着色，渲染效果如图6所示。

S12、建立刚体模型，所述刚体模型包括无人机动力学模型、标定工具模型。

S121、建立无人机动力学模型

所述无人机动力学模型作为视觉传感网络的跟踪对象。无线视觉传感网络关注的是多旋翼无人机的位姿信息并且通过输入位姿数据输出控制指令实现无人机的三维跟踪。故在无人机动力学模型基础上加入红外识别点，只关注其位置坐标和欧拉角，如图3。

S122、建立标定工具模型

标定工具模型用于求解相机外参数以及确定世界坐标系下的基准坐标系。在标定工具模型上固定一系列特征点。由于特征点间的距离固定，故可以作为约束求解外参方程以及确定坐标系轴向，具体分为：

S1221、建立一维杆标定工具模型：利用标志点的间距、图像上标志点的像素坐标作为已知数求解相机的外参数，如图4；

S1222、建立三角板标定工具模型：用于把基准坐标系从相机坐标系转换到世界坐标系，如图5。

S13、建立视觉图像仿真模型，所述的视觉图像仿真模型包括相机模型、图像采集模型；

S131、建立相机模型

所述的相机模型描述真实相机捕捉三维空间点投影到相机图片二维平面的关系。这里引入四个坐标系，包括世界坐标系O_w-X_wY_wZ_w、相机坐标系O_c-X_cY_cZ_c、图像坐标系O-XY、像素坐标系UV。跟踪目标点P(X_w,Y_w,Z_w)在相机坐标系中坐标(X_c,Y_c,Z_c)可由旋转平移得到：

其中，R,T分别为旋转矩阵和平移向量。相机坐标系中坐标(X_c,Y_c,Z_c)到图像坐标系坐标(x,y)为透视投影变换，由三角形相似关系可得：

其中，f为相机焦距。图像坐标系坐标(x,y)到像素坐标系坐标(u,v)满足比例关系：

其中，f_x,f_y为横轴分辨率、纵轴分辨率，u₀,v₀为主点坐标。最终可得三维点从世界坐标系变换到像素坐标系的关系为：

其中

为相机内参，

为相机外参。

以上的相机内参是现实中的内参，是一个3×3矩阵，但是Unity3D中相机的投影矩阵(即内参)为一4×4矩阵，故需要将现实中的相机内参与Unity3D中的相机参数对应。Unity3D中，相机成像过程遵守一般计算机图像软件的成像流程，即把视锥体转化为规则观察体(如图6)再进行显示。这是一个归一化的过程，目的是把视锥体中包含的所有点，映射到一个1×1×1的正方体中。易知，此时视锥体靠前的部分的点会被拉伸，而视锥体靠后部分的点会被收缩，最终形成近大远小的效果。因此，求解各个坐标的对应变换公式，之后将各个变换公式化为矩阵形式，得到最终的投影矩阵：

其中，n为焦距，r-l为横轴尺寸(像素尺寸乘横轴分辨率)，t-b为纵轴尺寸(像素尺寸乘纵轴分辨率)，M_相为相机坐标系下的子空间，M_像为像素坐标系下的子空间，如图7相机视角显示对目标无人机特征点的投影结果。

S132、建立图像采集模型

所述图像采集模型模拟相机捕捉图像，进行特征提取、完成图像处理的过程,具体为：

S1321、获取相机节点下的画面。

S1322、将投影到像素坐标系的画面按一定顺序依次读取每个像素的RGB值，保存在数组中；

S1323、在虚拟测试环境模块中引入OpenCv库，利用OpenCV中的方法，将S1322的结果转换为存储灰度的矩阵；

S1324、以S1323所得灰度矩阵为输入，利用OpenCV编写特征点提取函数进行图像处理，即获得有效的特征点，函数流程如图8，包括输入灰度图片矩阵、灰度阈值化、高斯平滑、轮廓获取、特征点提取、输出特征点坐标。

至此，虚拟测试环境模块可以模拟视觉传感网络捕捉目标并得到处理后的有效信息。

S14、建立无线传感网络模型，所述无线传感网络模型包括通信模块、无线传输特性模块。

S141、建立通信模块

其中，每个相机节点挂载通信模块，作为客户端通过SOCKET协议与外部进行网络通信。通讯流程为：

S1411、建立SOCKET连接

S1412、服务端向客户端发送对应的数据包提示客户端。

S1413、在客户端接收到并解析了数据后，发送回对应功能所需数据形式的数据

S142、建立无线传输特性模块

基于视觉的三维跟踪技术现存的问题为更新率低、定位精度不能满足要求、实时性差等问题。其中，定位精度、实时性都与无线传输网络传输特性有着直接的联系。一个有效的仿真平台，必须考虑无线传输带来的影响。主要实现无限延迟、丢包、观测量错序的仿真，具体为：

S1421、无线延迟量：无限延迟量的动态变化通过平均随机抖动叠加随机阶跃信号模拟。

S1422、随即丢包：随机丢包现象通过标准正态分布随机数模拟。

S1423、观测量错序：观测量错序通过模拟后发数据先到、先发数据后到实现。

至此，虚拟测试环境模块可以模拟视觉传感网络发送处理后的有效信息给中央处理器。

S15、GUI界面设计

在该程序运行前或运行中，可以通过挂载的多个脚本，在Inspector属性面板修改各个相机的焦距、像素尺寸、分辨率、近远截面等。同理，还可设置仿真时的最大帧率；高斯噪声的期望和方差；丢帧、杂点出现的概率；相机、刚体的位置；刚体的运动；客户端、服务端的IP地址等。运行后，提供主要功能的界面如图9。

S16、扩展功能

至此，实现了基本的虚拟测试环境模块。除此以外，应对各种复杂的现实情况，虚拟测试平台扩展以下功能，用户可以根据当前环境进行设置，实现更为真实的仿真：

S161、极端情况测试

S1611、识别点遮挡模拟

在刚体运动过程中，常常会遇到某一标志点被遮挡或某两个标志点重叠的情况。在仿真时，用户可以改变识别点位置，通过在虚拟测试环境模块中摆放一些特殊位置对此情况进行测试，实现识别点遮挡模拟。多数情况下，一个点被遮挡或其他点重叠时，是针对某一个相机的。而此时其他相机仍可以看到正常数量的标志点，对识别结果无影响。但是多个相机同时存在遮挡或重叠问题时，视觉定位算法模块就会失效。即所述的特殊位置，是可以造成一点被遮挡或其他点重叠的位置。

S1612、识别点意外丢失模拟

在虚拟测试环境模块发送点的数据时，加入了一个用户可设的丢失概率，用于模仿识别点意外丢失的情况。当丢失触发时，当前点会被置为(0,0)，实现识别点意外丢失模拟。

S162、噪声测试

S1621、杂点模拟

在实际跟踪过程中经常会遇到杂点偶尔出现、消失的现象。在虚拟测试环境模块中，用户可以设置此类杂点出现的概率与持续时间，方便对跟踪算法进行测试。

S1622、高斯噪声模拟

为了模拟图像采集设备、自然环境因素等诸多原因造成的处理图像与真实图像之间的差异，用户可以选择高斯噪声的加入与否，该高斯噪声使用Box-Muller方法产生。

S163、压力测试

S1631、多相机、多刚体测试

受限于计算机硬件，刚体、相机数目越多，跟踪系统可能失效；刚体、相机数目越多，求解外参矩阵的时间将会大幅增加，从而使得标定算法失效。刚体、相机数目越多，标志点坐标结算错误后可能不再收敛，导致跟踪算法失效。故通过调节虚拟测试环境模块GUI界面中的相机、刚体数目，可以测量三维跟踪算法、系统的鲁棒性。

S1632、高帧率测试

受限于计算机硬件，仿真的最大频率在100帧/秒左右；而测试跟踪算法的抗压能力，帧率需要至少200帧/秒。虚拟测试仿真系统提供一种高帧率下的专项测试，这种模式下虚拟仿真模块将放弃图像处理、减少计算量，直接发送模拟的标志点坐标。该过程为：根据标志点在惯性坐标系下的坐标，由给定的相机内参，计算出其在对应相机图片坐标系下的像素坐标。若该像素坐标在给定的分辨率内，则认为是模拟的标志点坐标。

S164、非同步测试

网络传输产生的延迟、丢包、观测量错序现象会造成数据的非同步，从而影响观测位姿信息的准确性。用户可在虚拟测试仿真平台中设置无限延迟量、丢包率、错序发生率，测试跟踪算法的鲁棒性，实现思路如步骤S142所示。

S2、搭建视觉定位算法模块，组成如图1所示。

所述的搭建视觉定位算法模块，其功能是实时和准确地解算捕捉区域内刚体的六自由度位置和姿态，具体包括相机预览模块、相机标定模块、刚体跟踪模块。通过相机预览模块确定合适的相机视场角。之后在此基础上，以虚拟测试环境模块输出的有效信息为输入，通过相机标定模块标定内参、外参、确定原点坐标。其中，由于内参只与相机本身有关，而实验时相机通常固定，故内参为定值，在视觉定位算法模块中不重复测量。以标定结果为基础，以虚拟测试环境模块输出的有效信息为输入，通过刚体跟踪模块获得实时跟踪信息，即观测位姿信息。具体为：

S21、相机预览模块

在相机标定前，需要先检测相机视角。相机视角过高或过低将会导致视场范围不足以覆盖无人机运动范围，使得被跟踪无人机在许多位置无法被多个相机甚至一个相机捕捉到，从而导致跟踪失败。故建立相机预览模块，可以实时显示相机画面，从而根据画面调整相机位置和角度。

S22、相机标定模块，基于步骤一中的相机模型，对多台相机的内、外参数进行离线标定，并且把基准坐标系从某一个相机坐标系转换到世界坐标系。考虑视觉传感网络多跳自组织的特点，节点分配权重，根据最短路径法确定参考相机到其他相机的最优路径。若已知双目标定结果，可以通过坐标系的旋转、平移得到参考摄像机和其他摄像机的转换关系，故这里只对双目标定过程说明：

S221、引入通用成像模型，将(X_w,Y_w,Z_w)与(u,v)的关系转变为球坐标

其中定义R·sinθ为r(θ)，即图像点和主点(相机光轴与图像平面的交点)之间的距离，具体为：

r(θ)＝k₁θ+k₂θ³+k₃θ⁵+k₄θ⁷+k₅θ⁹+… (5)

其中，k₁,k₂,k₃,k₄,k₅为待定参数，θ为光轴与入射光线的夹角。

则(X_w,Y_w,Z_w)与(u,v)的关系转为内参

外参[R T]的确定。

S222、内参标定，通过最小化成像点与成像模型结果的插值，得到内参处置。由于图像中心坐标，标称像素尺寸已知，故具体公式为：

其中，区间[0,θ]插值分为p份(θ₁,θ₂,...,θ_j,...,θ_p)。

S223、外参标定，根据极线几何约束，有

m^TEm＝0 (7)

其中，m为内参确定的球面坐标，E为待解的本质矩阵。对E进行奇异值分解得到旋转矩阵R和平移向量T。

S224、内、外参数优化，优化目标为最小化三维重建误差，通过Levenberg-Marquardt算法求解。

S225、根据参考相机到其他相机的最优路径，依次确定路径上第i＝0,...M号相机相对于第0号相机坐标系的外参数初值

即重复S221-S224环节到计算完第M号相机相对于第0号相机坐标系的外参数初值。

S226、根据以上标定结果，确定第i,i＝0,...M号相机坐标系相对于世界坐标系的外参数

从而把参考坐标系从相机变换到世界坐标系，具体如下：

S2261、利用三角板上特征点，根据标定结果，得到惯性坐标系到第0号相机坐标系的外参数初值

其中{e}为惯性坐标系，{c_i}为第i号相机坐标系；

S2262、

变换关系为：

结合标定内外参数，可以得到特征点到各相机的投影坐标；

S2263、非线性优化，得到优化后的第i,i＝0,...M号相机坐标系相对于世界坐标系的外参数

至此实现了相机标定模块的作用：根据确定的外参数，在同一视场区域下，相机对捕捉同一特征点的投影平面点，基于三角测量原理进行空间特征点的三维重建，将特征点恢复为世界坐标系下的三维位置，如图10。

S23、刚体跟踪模块，以三维重建后的位姿信息为输入，所述的刚体跟踪模块包括过程模型、观测模型和在线位姿估计。

S231、过程模型

由于相机采样时间十分小，在每个采样时间，刚体的运动模型可以简化为匀速运动模型。假设系统变量设为

其中

表示刚体在世界坐标系下的三维位置，且有

表示刚体在世界坐标系下的三维速度，

表示刚体的俯仰角、滚转角和偏航角，

表示刚体在机体系的三维角速度，{b}为机体坐标系，则一般刚体常用的线性匀速运动模型为：

其中，γ₁,γ₂是高斯白噪声，假设T_s表示采样时间，根据一阶向后差分法可以得到离散形式：

x_k＝Ax_k-1+γ_k (9)

S232、观测模型

根据特征点成像

可以得到描述输出测量和系统状态之间关系的观测模型为：

z_k＝h(x_k)+v_k (11)

其中

是第i号摄像机上的n_F个特征点组成的测量向量。v_k中每一个元素是独立同分布、均值为0的高斯白噪声。

S233、在线位姿估计

基于过程模型、观测模型，建立EKF滤波器。首先进行初始化，之后进行预测过程，根据k-1时刻的状态估计

和误差协方差预测P_k-1|k-1可以预测出k时刻的状态估计

和误差协方差P_k|k-1。在预测的基础上进行更新，得到最终更新

和P_k|k，即观测位姿信息。

S3、搭建无人机跟踪控制模块

无人机跟踪控制模块组成如图1，实现根据三维跟踪结果修正偏航路线的功能，由参考运动轨迹、视觉反馈通道、PID跟踪控制器组成。参考运动轨迹根据实际需要具体给出，视觉反馈通道如图2所示，即PID跟踪控制器所在的反馈通道。下面具体对PID跟踪控制器说明：

PID跟踪控制器利用线性化方法简化，分解为三个通道(高度、偏航、水平)的控制。在高度、偏航通道控制部分的设计中，直接利用P控制，其中z_d为期望高度，z_e为观测高度，Ψ_d为期望偏航角，Ψ_e为观测偏航角，x_d为期望横轴坐标，x_e为观测横轴坐标，y_d为期望横轴坐标，y_e为观测横轴坐标，

为观测三维速度。

水平通道控制量是机体控制量，控制俯仰角和滚转角，选用pd控制器，需要常数矩阵进行水平方向分解：

调整参数后，最终的控制器表达式为:

其中，u_x,u_y为控制器的水平通道控制量，u_z为高度通道控制量，u_Ψ为偏航通道控制量。

后接多旋翼模型，考虑真实实现下多旋翼无人机的气动布局。多旋翼模型以控制部分的输出为输入，输出多旋翼在世界坐标系下的坐标、速度、滚转角、俯仰角、偏航角以及对应的角速度。

S4、总体仿真实现

基于以上虚拟测试环境模块、视觉定位算法模块、无人机跟踪控制模块，实现系统的闭环仿真：

S41、通过SOCKET协议建立虚拟测试环境模块、视觉定位模块、控制器模块间的网络连接，如图11。

S42、通过虚拟测试环境模块与视觉定位模块先后完成相机初始化、外参标定、设置原点，跟踪环节，具体流程如图12；

S43、视觉定位模块向控制器模块发送观测的位姿信息，控制器以预估位姿信息与标准位姿信息为输入，输出控制信息并发送给虚拟测试环境模块，修正无人机模型的航线，实现三维跟踪目标，如图13。

本发明提出的一种基于视觉的无人机三维跟踪虚拟测试仿真系统，包括虚拟测试环境模块、视觉定位算法模块、无人机跟踪控制模块三部分。该系统提供了开放的仿真环境，模块化的结构使得无人机模型、相机网络的组成及布局、网络特性的选择、标定算法、位姿估计算法、控制器的选择易于改变，方便扩展。该系统能够使得基于视觉的三维跟踪过程可视化，大大节省调试时间，为当前三维跟踪领域中存在的更新率低、定位精度不能满足要求、实时性差等问题的解决提供实践平台。

附图说明

图1是虚拟测试仿真系统组成示意图；

图2是总体闭环流程示意图；

图3是无人机动力学模型示意图；

图4是一维杆标定工具示意图；

图5是三角板标定工具示意图；

图6是视锥体与规则观察体示意图；

图7是视觉传感网络捕捉无人机示意图；

图8是图像处理函数流程；

图9是GUI界面示意图；

图10是三维重建示意图；

图11是模块间网络通讯示意图；

图12是虚拟测试环境模块与视觉定位算法模块连接流程图；

图13是三维跟踪过程流程图；

图14是标准值与提取特征点坐标的相对平均误差示意图；

图15是闭环跟踪结果示意图。

图中符号说明如下：

图2中的符号说明：u_x,u_y为控制器的水平通道控制量，u_z为高度通道控制量，u_Ψ为偏航通道控制量。

图6中的符号说明：X,Y,Z轴分别为坐标系三轴。

图10中的符号说明：C,C′为两个相机节点，R,T为两个相机节点间的旋转矩阵和平移向量，s_i,s_i′为目标物投影在两个相机节点下的像素点，P(i)为三维重建后恢复的目标点。

具体实施方式

下面结合幅图和实施例，对本发明的技术方案作进一步的说明。

本发明基于虚拟现实环境搭建了虚拟测试环境模块，其中虚拟现实环境指UnrealEngine、Unity3D此类的虚拟引擎。在此基础上，搭建视觉定位算法模块、无人机跟踪控制模块，从而构成闭环的仿真系统，有着可扩展、快速开发、真实性的优势。具体体现为：1)可扩展。系统模块化，每一个模块如同一个黑匣子，只关注输入输出，对于不同的相机、相机布局、无人机，只需修改对应的模块。2)快速开发。可以快速在虚拟现实环境中完成场景布置。现实中多相机网络的搭建往往相对复杂，特别是进行相机上限的测试时，场景布置将会十分复杂。3)真实性。系统考虑了视觉传感网络特性、相机成像模型、无人机动力学模型、室内场景布置、物体遮挡问题等，使得搭建的虚拟环境尽可能逼真。同时，虚拟测试环境模块还考虑了一些现实中很少发生的极端情况，为算法的鲁棒性测试提供了条件。

本发明的一个实施例，提出了基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，并进行了闭环仿真实验验证。仿真实验使用两台电脑进行通讯，所用的电脑配置如下：Intel(R)Core(TM)i7-6700HQ CPU，主频2.6GHz，内存8GB；Intel(R)Core(TM)i5-6200UCPU，主频2.3GHz，内存8GB；虚拟测试环境模块、视觉定位算法模块、无人机跟踪控制模块分别是在Unity 5.6.0f3,Visual Studio 2008,MATLAB R2020b上编写并运行。根据实际需求，选择合理的相机参数；仿真时的最大帧率；高斯噪声的期望和方差；丢帧、杂点出现的概率；相机、刚体的位置；客户端、服务端的IP地址；优化的相机标定、位姿估计算法等，运行仿真程序并得到跟踪轨迹。通过经验判断和理论计算，分析虚拟测试环境模块是否能正确的提取目标跟踪物的特征点，视觉定位算法是否有效，闭环的跟踪控制是否成功。

首先，验证目标跟踪物上特征点提取的准确性。检验方法是在单个相机视野内每次随机产生3个点，共产生一万次。直接记录产生的三个点的位置作为标准值，之后对三个点组成的图片通过图8环节提取特征点坐标，计算标准值与提取特征点坐标的相对平均误差，结果如图14，相对平均误差为0.15％，虚拟测试环境模块可以正确的提取目标跟踪物的特征点，即视觉定位系统有正确的输入。

接着，检验视觉定位算法是否有效，同检验闭环的跟踪控制是否成功一样，都以跟踪结果为指标。根据图2进行闭环的无人机三维跟踪仿真，模拟高度为1米的定高飞行，根据模拟的真实实验环境设置相机参数，高斯噪声，并且设置飞行半径为1米，平均无限延迟10ms，选用一对相机进行跟踪，仿真时的最大帧率100Hz，其他参数按照模拟值设置。闭环实验结果如图15，由X向、Y向标准轨迹与视觉定位模块得出的观测轨迹可知视觉定位算法有效；由三维轨迹可知闭环的跟踪成功。

Claims (10)

1.一种基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，包括虚拟测试环境模块、视觉定位算法模块、无人机跟踪控制模块三部分；其特征在于：虚拟测试环境模块搭建视觉传感网络的仿真模型，视觉传感网络处理后的有效信息作为视觉定位算法模块的输入；视觉定位算法模块输出目标无人机的观测位姿信息作为无人机跟踪控制模块的反馈输入；无人机跟踪控制模块的12维位姿输出作为虚拟测试环境模块搭建的目标无人机的仿真模型的控制输入；

其中，所述的虚拟测试环境模块用于模拟真实实验平台，具体包括刚体模型、视觉图像仿真模型，无线传感网络模型和环境渲染模型：

所述的环境渲染模型模拟实验环境；刚体模型模拟标定工具和目标无人机的运动；视觉图像仿真模型与无线视觉传感网络模型共同模拟视觉传感网络；其中，视觉图像仿真模型实现真实摄像机节点的目标识别、特征提取、图像处理功能；无线视觉传感网络模型模拟无线视觉传感网络节点之间的传输特性，实现传递有效信息至中央处理器的功能；最后，通过GUI界面模拟实验环节中的操作指令，相机初始化、标定、设置原点、跟踪环节需要的数据类型，组合各模型发送对应数据；

其中，所述的视觉定位算法模块用于实时和准确地解算捕捉区域内刚体的六自由度位置和姿态，具体包括相机预览模块、相机标定模块、刚体跟踪模块；通过相机预览模块确定合适的相机视场角，以虚拟测试环境模块输出的有效信息为输入，通过相机标定模块标定内参、外参、确定原点坐标；以标定结果为基础，以虚拟测试环境模块输出的有效信息为输入，通过刚体跟踪模块获得实时跟踪信息，即观测位姿信息；

其中，所述的无人机跟踪控制模块用于实现根据三维跟踪结果修正偏航路线的功能；具体由参考运动轨迹、视觉反馈通道、PID跟踪控制器组成；其中视觉反馈通道即PID跟踪控制器所在的反馈通道；PID跟踪控制器利用线性化方法简化，分解为高度、偏航、水平三个通道的控制。

2.根据权利要求1所述的基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其特征在于：所述的虚拟测试环境模块，具体搭建过程为：

S11、建立环境渲染模型，模拟室内环境以及相机覆盖情况；

S12、建立刚体模型，包括无人机动力学模型、标定工具模型；

S121、建立无人机动力学模型作为视觉传感网络的跟踪对象；无线视觉传感网络关注的是多旋翼无人机的位姿信息并且通过输入位姿数据输出控制指令实现无人机的三维跟踪，故在无人机动力学模型基础上加入红外识别点，只关注其位置坐标和欧拉角；

S122、建立标定工具模型，用于求解相机外参数以及确定世界坐标系下的基准坐标系；在标定工具模型上固定特征点；

S13、建立视觉图像仿真模型，包括相机模型、图像采集模型；所述的相机模型描述真实相机捕捉三维空间点投影到相机图片二维平面的关系；所述图像采集模型模拟相机捕捉图像，进行特征提取、完成图像处理的过程；

S14、建立无线传感网络模型，包括通信模块、无线传输特性模块；

S141、建立通信模块：每个相机节点挂载通信模块，作为客户端通过SOCKET协议与外部进行网络通信；

S142、建立无线传输特性模块，主要实现无限延迟、丢包、观测量错序的仿真。

3.根据权利要求2所述的基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其特征在于：步骤S122中所述的特征点间的距离固定，故可以作为约束求解外参方程以及确定坐标系轴向，具体分为：

S1221、建立一维杆标定工具模型：利用标志点的间距、图像上标志点的像素坐标作为已知数求解相机的外参数；

S1222、建立三角板标定工具模型：把基准坐标系从相机坐标系转换到世界坐标系。

4.根据权利要求2所述的基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其特征在于：所述相机模型，具体建立过程如下：

这里引入四个坐标系，包括世界坐标系O_w-X_wY_wZ_w、相机坐标系O_c-X_cY_cZ_c、图像坐标系O-XY、像素坐标系UV；跟踪目标点P(X_w,Y_w,Z_w)在相机坐标系中坐标(X_c,Y_c,Z_c)可由旋转平移得到：

其中，R,T分别为旋转矩阵和平移向量；相机坐标系中坐标(X_c,Y_c,Z_c)到图像坐标系坐标(x,y)为透视投影变换，由三角形相似关系可得：

其中，f为相机焦距；图像坐标系坐标(x,y)到像素坐标系坐标(u,v)满足比例关系：

其中，f_x,f_y为横轴分辨率、纵轴分辨率，u₀,v₀为主点坐标；最终可得三维点从世界坐标系变换到像素坐标系的关系为：

其中

为相机内参，

为相机外参；

以上的相机内参是现实中的内参，是一个3×3矩阵，但是Unity3D中相机的投影矩阵为一4×4矩阵，故需要将现实中的相机内参与Unity3D中的相机参数对应；Unity3D中，相机成像过程遵守一般计算机图像软件的成像流程，即把视锥体转化为规则观察体再进行显示，这是一个归一化的过程，目的是把视锥体中包含的所有点，映射到一个1×1×1的正方体中；易知，此时视锥体靠前的部分的点会被拉伸，而视锥体靠后部分的点会被收缩，最终形成近大远小的效果；因此，求解各个坐标的对应变换公式，之后将各个变换公式化为矩阵形式，得到最终的投影矩阵：

其中，n为焦距，r-l为横轴尺寸，t-b为纵轴尺寸，M_相为相机坐标系下的子空间，M_像为像素坐标系下的子空间。

5.根据权利要求2所述的基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其特征在于：所述的图像采集模型，具体建立过程如下：

S1321、获取相机节点下的画面；

S1322、将投影到像素坐标系的画面按顺序依次读取每个像素的RGB值，保存在数组中；

S1323、在虚拟测试环境模块中引入OpenCv库，利用OpenCV中的方法，将S1322的结果转换为存储灰度的矩阵；

S1324、以S1323所得灰度矩阵为输入，利用OpenCV编写特征点提取函数进行图像处理，即获得有效的特征点。

6.根据权利要求1所述的基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其特征在于：所述的虚拟测试环境模块进一步包括有：GUI界面设计，在程序运行前或运行中，可以通过挂载的多个脚本，在Inspector属性面板修改各个相机的焦距、像素尺寸、分辨率、近远截面；同理，还可设置仿真时的最大帧率；高斯噪声的期望和方差；丢帧、杂点出现的概率；相机、刚体的位置；刚体的运动；客户端、服务端的IP地址。

7.根据权利要求1所述的基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其特征在于：所述的虚拟测试环境模块进一步包括有：极端情况测试、噪声测试、压力测试、非同步测试其中的一项或者多项的组合。

8.根据权利要求7所述的基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其特征在于：所述的极端情况测试，包括：

S1611、识别点遮挡模拟

在仿真时，用户可以改变识别点位置，通过在虚拟测试环境模块中摆放一些特殊位置对此情况进行测试，实现识别点遮挡模拟，所述的特殊位置，是可以造成一点被遮挡或其他点重叠的位置；

S1612、识别点意外丢失模拟

在虚拟测试环境模块发送点的数据时，加入了一个用户可设的丢失概率，用于模仿识别点意外丢失的情况；丢失触发时，当前点会被置为(0,0)，实现识别点意外丢失模拟。

9.根据权利要求7所述的基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其特征在于：所述的噪声测试，具体包括：

S1621、杂点模拟

在虚拟测试环境模块中，设置杂点出现的概率与持续时间，方便对跟踪算法进行测试；

S1622、高斯噪声模拟

为了模拟图像采集设备、自然环境因素等诸多原因造成的处理图像与真实图像之间的差异，可以选择高斯噪声的加入与否，该高斯噪声使用Box-Muller方法产生。

10.根据权利要求7所述的基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统，其特征在于：所述压力测试，具体包括：

S1631、多相机、多刚体测试

通过调节虚拟测试环境模块GUI界面中的相机、刚体数目，以测量三维跟踪算法、系统的鲁棒性；

S1632、高帧率测试

根据标志点在惯性坐标系下的坐标，由给定的相机内参，计算出其在对应相机图片坐标系下的像素坐标，若该像素坐标在给定的分辨率内，则认为是模拟的标志点坐标。

Images