CN113467500A - 一种基于双目视觉的无人机非合作目标追踪系统 - Google Patents

Abstract

本发明公开一种基于双目视觉的非合作目标无人机追踪系统，所述系统由视觉检测、飞行控制、航迹规划、地面控制等软硬件模块和算法构成，其中视觉检测模块，用于检测目标和障碍的单元；飞控模块，用于对无人机位置、姿态控制的单元；航迹规划模块，用于接收视觉检测模块和飞控模块输出的数据信息，并将接收的数据进行协调处理，为无人机规划出最佳航迹；地面控制模块，用于对无人机动作指令发送和无人机位置、姿态的监控；系统软件包括检测进程单元和规划进程单元；本发明为无人机规划出可以实现避障的目标追踪路径，同时考虑追踪过程中无人机的偏航角，适用于未知环境下无人机对非合作目标的精准追踪。

Description

一种基于双目视觉的无人机非合作目标追踪系统

技术领域

本发明属于无人机规划与控制领域系统，涉及一种基于双目视觉的无人机非合作目标追踪系统。

背景技术

四旋翼无人机由于具有控制简单，起降方便，低成本、高机动性等优点，被广泛应用于军事、商用、民用领域，其中未知环境下面向非合作目标的无人机追踪已经成为无人机领域的热点问题。非合作目标与无人机系统之间没有通信，无人机系统无法直接获取其位置，环境中的障碍也会对无人机的飞行安全造成威胁，因此对于未知环境下的非合作目标追踪问题，目标定位与障碍检测是首要问题。

针对环境中的非合作目标，基于视觉的目标定位方法是目前主流方案，其定位主要依赖于目标检测与目标深度信息计算，而对于环境中的障碍信息，可以利用点云来进行处理，从而获得可以用于无人机规划的环境障碍信息。

当获取到目标位置与障碍信息后，需要对无人机追踪航迹进行规划，保证无人机可以实时跟随目标，躲避追踪路径上的障碍，同时要求无人机能够调整偏航角，使目标始终处于机载相机视野范围之内。当目标被障碍物遮挡后，需要对目标位置进行预测，通过预测位置对无人机进行引导。因此，对追踪过程中的检测、规划、预测的研究是非常必要的。

通常的目标检测及其改进方法更注重于检测精度的提升，即使有针对检测速度的提升，也较少可以应用于实际的系统中，更多地停留在理论阶段。对于航迹规划的优化目前主要停留在对航迹光滑程度的改进，以及对动力学等约束的考虑阶段，然而实际应用中，无人机难以对十分光滑的航迹进行跟踪，并且由于不同的优化方案需要与其它复杂算法相结合，规划时间较长，难以在实际系统中实现实时规划。同时，对于目标被遮挡情况下的目标预测，目前研究相对较少。

发明内容

考虑到以上问题，本发明针对未知环境下面向非合作目标的追踪问题，基于四旋翼无人机平台，搭建由多层嵌入式软硬件模块和相应算法构成的无人机追踪系统。首先利用基于Tiny-YOLOv3和SGBM的目标检测与定位方法，获取目标位置信息；然后通过点云和八叉树算法实现障碍检测，采用卡尔曼滤波完成遮挡情况下的目标位置预测；并由改进RRT算法进行追踪航迹的规划，再利用Bresenham算法对冗余航点裁剪，最终实现具有避障功能的目标追踪无人机系统。

为了解决现有技术中存在问题，本发明采用如下技术方案予以实施：

一种面向非合作目标的无人机追踪系统，所述系统基于四旋翼无人机平台，并集成双目相机、机载计算机、底层飞控等硬件，所实现的功能包括

视觉检测模块，利用双目相机，对目标和障碍进行检测的单元；

飞控模块，利用底层飞控对无人机位置、姿态进行控制的单元；

航迹规划模块，基于机载计算机，接收视觉检测模块和飞控模块输出的数据信息，并将接收的数据进行协调处理，为无人机规划出最佳航迹；

地面控制模块，利用地面站完成对无人机动作指令发送，同时监控无人机位置和姿态；

上述各模块之间通信方式为：

所述地面控制模块通过局域网将控制指令发送到航迹规划模块，航迹规划模块接收指令后，利用串口通信控制视觉检测模块，开始对目标和环境中的障碍进行检测与定位。

所述航迹规划模块通过串口通信分别从视觉检测模块和飞控模块获取目标和无人机当前位置，经过航迹规划后，将计算所得航点与偏航角通过串口传递给飞控模块，对无人机进行位置控制和姿态控制，实现无人机的目标追踪与避障功能。

所述无人机追踪系统软件框架包括检测进程单元和规划进程单元。

进一步，所述检测进程单元通过如下步骤，实现对所述视觉检测模块输出目标、障碍数据的处理：

S101、在机载计算机上利用TensorRT对Tiny-YOLOv3算法进行优化，用于实时目标快速检测，并利用SGBM算法获得检测到目标的视差图，进行深度计算；

S102、将检测结果与深度计算结果结合，解算出目标在机体坐标系下的位置，经过旋转矩阵的坐标变换，获得ENU坐标系下目标位置；其中：

对无人机追踪环境中的障碍，利用SGBM算法获取视差图后，将障碍信息处理成稠密点云，并对稠密点云进行稀疏化处理。当目标因障碍遮挡而无法被检测到时，采用目标运动模型与卡尔曼滤波相结合的方法对目标位置进行预测。

进一步，所述规划进程单元中航迹规划的算法步骤：

判断当前无人机位置与期望目标点之间的距离，如果距离小于设定值，则使无人机保持在当前位置，否则采用改进RRT算法进行航迹规划，并对所规划航迹中冗余航点进行裁剪；其中：

S201、从飞控模块获取无人机位置与偏航角，从视觉检测模块获取目标位置和障碍信息，判断无人机当前位置与目标之间是否存在障碍，如果存在障碍，则规划进程单元采用改进RRT算法产生避障飞行航迹，并将新航点与偏航角发送给飞控模块；否则进入下一步；

S202、通过飞控模块获取无人机位置信息，判断无人机是否到达期望目标点；如果达到，则进入下一步，否则规划进程单元采用改进RRT算法输出规划航迹，调整无人机位置、偏航角；

S203、判断目标当前位置与航迹规划时目标所在位置的距离是否超过限值；如果没有，则无人机到达期望目标点；否则返回步骤S201；其中：

所述改进RRT算法，主要是优化随机点的选取，具体改进为：

在选择随机点时，如果以期望目标点作为随机点进行扩展，航迹不经过障碍，则将期望目标点设为随机点；若航迹经过障碍，则设置概率p，并在0～1之间生成随机数，当概率p大于该随机数时，在接近期望目标点方向的备选点中任意选择一个点作为随机点，当概率p小于该随机数时，选择远离期望目标点方向的备选点作为随机点。

有益效果

本发明涉及一种基于双目视觉的无人机非合作目标追踪系统。该系统基于四旋翼无人机平台，由多层嵌入式软硬件模块和相应算法构成。首先在机载计算机上通过TensorRT对Tiny-YOLOv3算法进行优化，完成对非合作目标的检测，利用SGBM算法确定目标深度信息，再通过坐标转换解算出目标位置，同时在检测到周围环境中障碍后，利用八叉树算法对障碍点云信息进行稀疏化处理。在获取非合作目标定位及未知环境下障碍信息后，通过改进RRT算法规划航迹，再采用Bresenham算法对冗余航点裁剪，为无人机规划出可以实现避障的目标追踪航迹，同时考虑追踪过程中无人机的偏航角，适用于未知环境下无人机对非合作目标的精准追踪。

附图说明

图1是系统总体框架图；

图2是系统通信网络图；

图3是系统软件框架图；

图4是系统规划进程图；

图5是系统算法流程图；

图6是坐标系相对位置图；

图7是期望距离计算示意图；

图8是期望目标点计算示意图；

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图与实例对本发明做详细的论述，以下实例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。

本发明涉及一种面向非合作目标的无人机追踪系统，用以在未知环境下对目标进行追踪；系统基于四旋翼无人机平台，采用多层嵌入式模式，由双目相机、机载计算机、底层飞控等硬件组成，所实现的功能包括：视觉检测模块，利用双目相机对目标和障碍进行检测的单元；飞控模块，利用底层飞控对无人机位置、姿态控制的单元；航迹规划模块，基于机载计算机，接收视觉检测模块和飞控模块输出的数据信息，并将接收的数据进行协调处理，为无人机规划出最佳航迹；地面控制模块，利用地面站完成对无人机动作指令发送，同时监控无人机位置和姿态；系统软件框架包括检测进程单元和规划进程单元；其中：

本发明中的视觉检测模块利用机载双目相机完成对环境的感知和目标的检测与定位，航迹规划模块利用机载计算机进行航迹规划，达到在未知环境下追踪目标的目的。根据实际需要，基于四旋翼无人机平台，进行系统的硬件选型与搭建。选择IntelRealSenseD435i双目相机作为视觉检测模块的机载相机；航迹控制模块控制选择NvidiaTX2核心模块作为机载计算机；底层以基于PX4固件的飞控为主要控制单元，选择680KV电机作为执行机构。搭建完成后的无人机硬件系统如图1所示。

如图2所示，本发明的无人机追踪系统通信功能是通过局域网将地面控制模块的指令发送到机载计算机上，即航迹规划模块。机载计算机接收指令后，利用串口通信控制机载相机，即视觉检测模块，开始对目标进行检测与定位，并同时对环境中的障碍进行检测。此外，借助于串口通信从飞控模块获取无人机的当前位置，经过航迹规划后，将计算所得航点与偏航角通过串口传递给飞控模块，进行位置控制和姿态控制，实现无人机的目标追踪与避障功能。另外，地面控制模块通过数传与无人机通信，实时监控无人机的状态。

本发明的系统软件框架如图3所示。系统软件主要有两大进程：检测进程单元和规划进程单元。

检测进程单元是无人机目标追踪系统的基础，主要用于获取目标位置和障碍信息。首先利用TensorRT对Tiny-YOLOv3算法进行优化，完成目标检测，对于检测到的目标，利用SGBM算法获得视差图，进行深度计算，并结合检测结果解算目标在机体坐标系下的位置，再经过旋转矩阵的坐标变换，获得ENU坐标系下的目标位置；而对于环境中的障碍，先利用SGBM算法获取视差图，然后将障碍信息处理成稠密点云，最后对点云进行稀疏化处理。目标位置与障碍点云信息均用于规划航迹。

规划进程单元是无人机目标追踪系统的主要进程，如图4所示。该进程首先判断当前无人机位置与期望目标点之间的距离，如果距离小于设定值，则使无人机保持在当前位置，否则利用改进RRT算法进行航迹规划。完成规划后，取航迹上的新航点与偏航角，判断无人机当前位置与新航点之间是否存在障碍，作为重规划条件，最终将新航点与偏航角发送给飞控。航点发布后，需要进一步判断无人机是否到达该航点位置。在飞行过程中，如果目标当前位置与航迹规划时目标所在位置间的距离超过限值，需要进行重规划。另外，根据目标当前位置，调整无人机的航偏角，保证目标始终位于无人机的视野范围内；当无人机到达该航点位置后，取新的航迹点并重复上述步骤。

本发明的系统算法主要有目标检测、深度计算、位置解算、目标预测、点云获取、航迹规划六大部分，系统算法流程如图5所示。目标检测部分主要提供目标的像素坐标；深度计算部分主要提供目标到机载相机的深度值；位置解算部分主要根据目标的像素坐标和深度值解算出目标在相机坐标系下坐标，并将其转化为ENU坐标系下的坐标；目标预测部分主要根据目标遮挡前的状态，完成对被遮挡目标位置的预测；点云获取主要用于获得无人机周围环境中的障碍信息；航迹规划则根据无人机当前位置、目标位置以及障碍信息完成航迹规划，并将规划的航迹发布给飞控，引导无人机实现避障，完成目标追踪任务。

本发明基于视觉的目标定位与障碍检测

目标检测

本发明采用Tiny-YOLOv3算法实现目标检测。Tiny-YOLOv3是一种基于深度学习的目标检测算法，其网络结构较为简单，并具有较高的检测精度和检测速度。该算法检测到目标后，可以输出目标检测框左上角像素坐标、以及检测框的长宽。

为了进一步提升Tiny-YOLOv3的检测速度，本发明在机载计算机上使用TensorRT来进行网络优化。作为一款神经网络推断加速引擎，TensorRT通过层间融合和数据精度校准来实现推理过程的加速，提升检测速度。

加速后，离线视频的检测速度可以接近10帧/秒。为了进一步验证实时检测效果，在搭建的无人机系统上进行测试，其检测速度为9帧/秒，可以满足追踪的实时性要求。

深度计算

本发明基于SGBM算法获取目标深度信息。首先根据相机的左右图像，利用SGBM算法获得视差图。在计算目标深度值的过程中，目标视差为目标检测框区域的平均视差值。设目标检测框左上角的像素坐标为(u₀,v₀)，令检测框长为l_en，宽为wid，视差图中任意一个像素点(i,j)所对应的视差值为d_ij，则目标检测框区域的平均视差值d可根据以下公式计算获得：

深度信息通常以毫米为单位，而视差值的单位为像素，二者之间可根据公式(2)进行转换，其中D为深度值，f为归一化的焦距，baseline表示两镜头间的基线距离。

D＝(f*baseline)÷d (2)

位置解算

为了更加清晰地描述目标的实际位置，首先定义若干坐标系，包括相机坐标系x_C-y_C-z_C，机体坐标系x_B-y_B-z_B，NED坐标系x_N-y_N-z_N，ENU坐标系x_E-y_E-z_E，其相对位置关系如图6所示。在所定义坐标系的基础上完成目标位置的解算，并通过坐标变换，将目标位置变换到航迹规划所需要的坐标系下。

解算过程如下：

首先，将检测框的中心像素坐标作为目标在像素坐标系下坐标，解算出相机坐标系下目标的三维坐标，然后通过坐标变换，依次将目标位置转换至机体坐标系、NED坐标系和ENU坐标系。

目标位置预测

当目标因障碍遮挡而无法被检测到时，采用目标运动模型与卡尔曼滤波相结合的方法对目标位置进行预测。

障碍检测

为了保证无人机飞行安全，需要对环境中的障碍进行检测，获取视差图后，去除图中视差值趋于0的像素点，将剩余像素点转换为ENU坐标系下的三维坐标，即可获得无人机周围环境中的障碍信息。然后将检测到的地面信息进行过滤，剩余的数据以点云形式进行发布，即可获取无人机周围环境的稠密点云。由于稠密点云的数据量过于庞大，不方便机载计算机存储、使用和实时计算，因此利用八叉树地图对稠密点云进行稀疏化处理，获得稀疏点云，再用于航迹规划。

本发明基于改进RRT算法的追踪航迹规划

期望目标点选择和偏航角计算

为了确保追踪过程中，目标始终位于无人机机载相机画面的中间位置，不能直接使用目标的实际位置，而应该计算期望目标位置，作为目标点进行航迹规划，这个期望位置可以根据目标的实际位置、相机参数和无人机飞行状态计算得到。期望目标点的计算过程如下：

计算无人机与目标之间的期望距离，如图7所示，实线为相机视角的中心线，两虚线之间为相机垂直方向视场角的范围，设无人机悬停时期望飞行高度为h，机载相机垂直方向视场角为β，机载相机与无人机之间的夹角为α，为保证目标更好地居于图像中央，加入距离补偿d₀，期望距离L可由下列公式求得。

在获取到无人机与目标之间的期望距离之后，定义在ENU坐标系下目标位置为(x_E,y_E,z_E)，无人机位置为(x_UE,y_UE,z_UE)，期望目标点为(x_T,y_T,h)，由图8和公式(4)可计算出追踪过程中无人机的期望目标点。

同时，为了保证追踪过程中无人机机头方向始终朝向目标，需要为规划的每一航点计算相对应的偏航角。设航点i的坐标为(x_i,y_i,z_i)，则每一航点对应的偏航角为：

改进RRT算法

由于传统RRT算法随机性强，不易收敛，算法中概率p影响新节点趋向于目标点的可能性，进一步影响航迹规划时间、航迹的长度和光滑程度，因此本发明提出一种基于概率p的改进RRT算法，对随机树生长阶段随机点的选取进行改进。为了描述改进RRT的算法原理，首先假设规划任务的起始点为q_init，目标点为q_goal，规划空间为O，安全区域为O_free，障碍区域为O_obs，且满足O_obs∪O_free＝O，

设定规划步长为l，概率为p∈(0,1)，规划的航点总数为n，则节点序列的列表edge为n×2维，航迹列表path为n×1维。改进算法的随机树生长阶段流程如下：

Step1:将规划任务的起始点q_init设为随机树的根节点，并令其为节点q_new，节点序号设为0，然后将该节点存入航迹列表path；

Step2:以q_new为起点，目标点q_goal为终点构建向量，并由q_new开始在向量上截取一个步长l，得到新的节点q_new；

Step3:如果q_new∈O_free，则令节点q_new序号为i(i＝1,2……)，执行Step 5，否则执行Step 4；

Step4:如果q_new∈O_obs，则放弃节点q_new，然后设置概率p′＝0.8，在0～1之间生成随机数p_r′，当p_r′≤p′时，在当前点的上下左右或者前向(接近目标点方向)的备选点中随机选择一个点作为随机点，当p_r′＞p′时，选择当前点后向(远离目标点方向)的备选点作为随机点，并以该随机点为终点构建向量，求取新的节点q_new，直到获得满足q_new∈O_free的新节点，令新节点序号为i，执行Step 5；

Step5:判断已存入航迹列表path的节点中距离新节点q_new最近的节点q_near，并记录其节点序号为ε，将节点次序(ε,i)存入edge，将节点q_new存入航迹列表path；

Step6:如果节点q_new与目标点q_goal之间的距离小于设定值，则认为已经到达目标点，随机树生长过程结束，否则返回Step 2。

冗余航点裁剪

通过改进RRT算法规划出追踪航迹后，为进一步提高航迹的光滑性，本发明提出采用Bresenham算法对规划的航迹中的冗余航点进行裁剪，剔除符合裁剪条件的中间航点，改善无人机飞行稳定性。

通过IntelRealSenseD435i双目相机、NvidiaTX2核心模块及基于PX4固件的底层飞控所搭建的小型四旋翼无人机系统平台，结合上述技术方案中所提算法，完成目标追踪实验，经实验测试，目标未被遮挡时，无人机可以跟随目标向前运动；当无人机遇到障碍时，可以利用环境点云信息规划飞行航迹，引导无人机绕过障碍；当目标被障碍物遮挡时，检测算法无法检测到目标，判定目标丢失，启用目标位置预测算法，引导无人机绕过障碍，无人机飞过障碍后，机载相机再次捕捉到目标，系统继续执行目标追踪任务。

实验结果表明，本发明所提的方法，可以使无人机系统在未知环境下完成对非合作目标的追踪任务，同时，当目标被障碍遮挡时，能够基于目标预测位置进行避障，继续追踪，并且能够保证飞行过程中航迹的光滑性，证明本发明所提方法的有效性。

本发明并不限于上文描述的实施方式。以上对具体实施方式的描述旨在描述和说明本发明的技术方案，上述的具体实施方式仅仅是示意性的，并不是限制性的。在不脱离本发明宗旨和权利要求所保护的范围情况下，本领域的普通技术人员在本发明的启示下还可做出很多形式的具体变换，这些均属于本发明的保护范围之内。

Claims (3)

1.一种面向非合作目标的无人机追踪系统，所述系统基于四旋翼无人机平台，主要由双目相机、机载计算机、底层飞控组成，所述系统还包括

视觉检测模块，利用双目相机，对目标和障碍进行检测的单元；

飞控模块，利用底层飞控对无人机位置、姿态进行控制的单元；

航迹规划模块，基于机载计算机，接收视觉检测模块和飞控模块输出的数据信息，并将接收的数据进行协调处理，为无人机规划出最佳航迹；

地面控制模块，利用地面站完成对无人机动作指令发送，同时监控无人机位置和姿态；

上述各模块之间通信方式为：

所述地面控制模块通过局域网将控制指令发送到航迹规划模块，航迹规划模块接收指令后，利用串口通信控制视觉检测模块，开始对目标和环境中障碍进行检测与定位；

所述航迹规划模块通过串口通信分别从视觉检测模块和飞控模块获取目标和无人机当前位置，经过航迹规划后，将计算所得航点与偏航角通过串口传递给飞控模块，对无人机进行位置控制和姿态控制，实现无人机的目标追踪与避障功能；

所述无人机追踪系统软件主要包括检测进程单元和规划进程单元。

2.根据权利要求1所述一种面向非合作目标的无人机追踪系统，其特征在于：通过如下步骤，实现所述检测进程单元对所述视觉检测模块输出目标、障碍数据的处理：

S101、在机载计算机上利用TensorRT对Tiny-YOLOv3算法进行优化，用于实时目标快速检测，并利用半全局块匹配(SGBM)算法获得检测到目标的视差图，进行深度计算；

S102、将检测结果与深度计算结果结合，解算出目标在机体坐标系下的位置，经过旋转矩阵的坐标变换，获得ENU坐标系下目标位置；其中：

对无人机追踪环境中的障碍，利用SGBM算法获取视差图后，将障碍信息处理成稠密点云，再对稠密点云进行稀疏化处理。当目标被障碍物遮挡后，利用卡尔曼滤波对目标位置进行预测。

3.根据权利要求1所述一种面向非合作目标的无人机追踪系统，其特征在于：所述规划进程单元中航迹规划的算法步骤：

判断当前无人机位置与期望目标点之间的距离，如果距离小于设定值，则使无人机保持在当前位置，否则采用改进快速扩展随机树(RRT)算法进行航迹规划，并对所规划航迹中冗余航点进行裁剪；其中：

S201、从飞控模块获取无人机位置与偏航角，从视觉检测模块获取目标位置和障碍信息，判断无人机当前位置与目标之间是否存在障碍，如果存在障碍，则规划进程单元采用改进RRT算法产生避障飞行航迹，并将新航点与偏航角发送给飞控模块；否则进入下一步；

S202、通过飞控模块获取无人机位置信息，判断无人机是否到达期望目标点；如果达到，则进入下一步，否则规划进程单元采用改进RRT算法输出规划航迹，调整无人机位置、偏航角；

S203、判断目标当前位置与航迹规划时目标所在位置的距离是否超过距离限值；如果没有，则无人机到达期望目标点；否则返回步骤S201；其中：

所述改进RRT算法，主要是优化随机点的选取，具体改进为：

在选择随机点时，如果以无人机期望目标点作为随机点进行扩展，航迹不经过障碍，则将期望目标点设为随机点；若航迹经过障碍，则设置概率p，并在0～1之间生成随机数，当概率p大于该随机数时，在接近期望目标点方向的备选点中任意选择一个点作为随机点，当概率p小于该随机数时，选择远离期望目标点方向的备选点作为随机点。

Images