CN104656663B - 一种基于视觉的无人机编队感知与规避方法 - Google Patents

Abstract

本发明公开了一种基于视觉的无人机编队感知与规避方法，通过编队中每一架无人机上设置的机载视觉传感器检测出目标障碍物在其对应的图像平面坐标系中的位置，推导出目标障碍物对应的相机坐标系到对应的世界坐标系的转移矩阵M；然后，选取任意两架机载视觉传感器得到的目标障碍物对应的相机坐标系到对应的世界坐标系的转移矩阵M，计算出目标障碍物在世界坐标系中的位置；确定目标障碍物是否对虚拟掌机和虚拟安全区域造成威胁；确定编队采取的避撞方式。使编队在保持队形不变以及编队队形打破重新组合的方式规避目标障碍物。

Description

一种基于视觉的无人机编队感知与规避方法

技术领域

本发明属于无人机导航与控制技术领域，具体涉及一种基于视觉的无人机编队感知与规避方法。

背景技术

无人机编队飞行，即多架无人机为适应任务要求而进行的某种队形排列和任务分配的组织模式，它既包含编队飞行的队形产生、保持和变化，也包括飞行任务的规划和组织。多无人机编队(Multi-UAV，MUAV)应世事需求而生：在军用领域，通过数以百计、千计的无人机能够实现快速构建战区立体化传感器网络，实现整个战场的全方位，多视角环境感知。在民用领域，多无人机可以实现大视场的环境监控，灾害快速反应、快速的地图生成，全局视频直播等任务。无人机编队协同能够使实现微小型无人机的效能最大化，不仅能够扩大其在军事领域的作战能力，也能在民用领域给国民经济带来巨大的推动作用。

现代的无人机越来越密集，无人机在大量的军民两用的使用中变得越来越广泛，随着微小飞行器的快速发展，未来的空域会变得越来越密集。开展无人机感知与规避(Sense and avoid，SAA)研究变得越来越紧迫。随着信息化水平的快速提高和航空技术的飞速发展，无人机在军事领域以及民用领域的不断拓展，一些远程的大型无人机不断涌现，飞行范围、运行时间成倍增长，远程无人机使用空域运行已经是必然趋势。无人机感知与规避技术是指无人机利用机载传感器或者地面监视系统完成对空域飞行环境的监视和飞行目标的状态获取，对潜在的碰撞威胁的目标进行规避路径规划，完成规避机动任务，从而保证无人机航路飞行安全。

随着飞机自动控制技术与传感器技术的发展，空域中飞行器对于碰撞威胁的规避能力也在逐步的改进和完善。早期的飞机防撞系统都是在预先规划的航路及设定操作规程的基础上建立的，由于需要对碰撞威胁做出分析和判断，因此飞行员的操作在早期的飞机防撞系统中是必不可少的环节，但是，飞行员的操作会容易受到各种因素的影响，由此产生了安装在飞机上的自动防撞系统，这些自动防撞系统利用传感器数据预测飞机发生碰撞的机率，有的自动防撞系统还可以产生相应的避障制导指令。上世纪九十年代初，功能相对简单的飞机空中防撞系统已经作为空中交通管理系统(Air TrafficControl System)的一部分，如空中交通碰撞预警和规避系统(Traffic Alert andCollision Avoidance System,TCAS)。近年来，美国研制了更为先进的交通碰撞探测和规避系统，这些系统利用包含当前空域中所有飞机航路的数据链信息，保证飞机在空域中的飞行安全，如广播式自动相关监视系统(Automatic Dependent Surveillance-Broadcast System,ADS-B)。

现代无人机在尺寸、重量、作战距离和高度上都有很大不同。目前世界各国所用的无人机也像北约在科索沃使用的七种无人机一样,执行的主要任务是监视、侦察和目标截获,提供图像情报。其所用的任务有效载荷都是光学照相机、红外行扫描器、电视摄像机、前视红外等无源成像探测设备。无人机执行监视、侦察和目标截获任务最主要是用光学照相机、红外行扫描器、电视摄像机和前视红外(热成像器)等任务有效载荷获取敌方图像情报提供指挥作战。无人机自身体积小，有效载荷相对较小，分布在10克～1千克，因此需要搭载的传感器尽量的体积小，质量轻。

发明内容

本发明所要解决的技术问题在于针对上述现有技术的不足，提供一种基于视觉的无人机编队感知与规避方法，使编队在保持队形不变以及编队队形打破重新组合的方式规避目标障碍物。

利用多机编队的双目测距，根据检测的信息，实时跟踪与测量，通过建立飞行安全区域，在保持队形不变以及编队队形打破重新组合的。

为解决上述技术问题，本发明采用的技术方案是，一种基于视觉的无人机编队感知与规避方法，包括以下步骤：

步骤一、通过编队中每一架无人机上设置的机载视觉传感器检测出目标障碍物在其对应的图像平面坐标系中的位置，推导出目标障碍物对应的相机坐标系到对应的世界坐标系的转移矩阵M；

根据机载视觉传感器监测到的目标障碍物的信息，计算出其在图像平面坐标系中的位置；然后依次计算出目标障碍物在成像平面坐标系中的位置、目标障碍物在相机坐标系中的位置、目标障碍物相对于机体坐标系的位置、目标障碍物在世界坐标系中的位置；其中选用的相机坐标系，采用四元算法进行姿态转移，以解决姿态转移中的奇异值问题；

步骤二、选取步骤一中所得的任意两架机载视觉传感器得到的目标障碍物对应的相机坐标系到对应的世界坐标系的转移矩阵M，通过双目视觉测距的原理，计算出目标障碍物在世界坐标系中的位置；

选取编队中任意两架无人机，将由步骤一中所得的对应的目标障碍物的转移矩阵M建立方程组，通过最小二乘法，计算出目标障碍物在世界坐标系中的位置；

步骤三、在编队中定义虚拟掌机和虚拟安全区域，由步骤二中所得的目标障碍物在世界坐标系中的位置，计算得出目标障碍物与虚拟掌机及虚拟安全区域的距离，确定目标障碍物是否对虚拟掌机和虚拟安全区域造成威胁；具体过程如下：

3.1.计算出虚拟掌机的位置，其中：P_GC为编队中心位置，是第i架无人机的飞行位置；

其中，n为编队中无人机的架数；i为自然数，且i属于1，2……n；

3.2.确定虚拟掌机的虚拟安全区域，将虚拟安全区域定义为椭球飞行包络：

其中，编队采取安全飞行模式，定义编队飞行方向为x方向，z轴沿编队位置向上，y轴满足右手规则；a为x轴上的长半轴，b为y轴上的短半轴，c为z轴方向上的短半轴，无人机飞行过程中，以飞行安全碰撞时间为长半轴，这样定义最短碰撞距离d，定义b＝c，选取黄金分割点：这样就完成了椭球飞行包络的定义；

则d_s的方向与编队飞行方向一致，即编队速度方向；其中：d_s是飞行的安全距离，R为定制，根据编队队形进行系统性定义；D为飞行安全距离；T为飞行安全最短时间；为编队飞行速度；这样，当a＝‖d_s‖计算出虚拟掌机的虚拟安全区域，即椭球飞行包络；

3.3.计算编队中单架次无人机的飞行安全区域，

对编队每一架飞机进行飞行安全区域划分，计算每一架飞机的最小分离距离

当时，为其飞行安全区域；

其中：为目标障碍物的相对运动速度，其中和分别是目标障碍物的运动速度以及单架次无人机的运动速度，定义相对运动速度径向距离为无人机与目标障碍物的径向距离；

步骤四、根据步骤三所得的结果，当目标障碍物不对虚拟掌机和虚拟安全区域造成威胁时，编队保持原航路飞行；

当目标障碍物对虚拟掌机和虚拟安全区域造成威胁时，编队中相应的无人机进行规避，完成避撞后，进入原来的编队，编队回归原来的航路。

进一步地，该步骤四中的具体过程如下：

计算由单架次无人机的转弯半径：

和单架次无人机的最大转弯加速度

其中：V_FLN∈(V_min,V_max),a_cent≤A(V_L,R_L,V_max)，V_FLN为单架次无人机的转弯速度，V_L为单架次无人机在转弯处左侧的速度，R_L为单架次无人机在转弯处左侧的半径，f是无人机能够提供的最大推力或侧向力矩，A表示函数；

无人机在飞行过程中的速度必须满足(V_min，V_max)，其中Vmin为单架次无人机的最小的速度值，Vmax为单架次无人机的最大的速度值；

计算编队中的单架次无人机由虚拟掌机左侧避撞与右侧避撞的最大转弯半径，要使单架次无人机满足的飞行要求，通过信息交互，当所有编队成员具有满足同一侧避撞需求时，即进行无人机编队同侧避撞；当某几架无人机不能满足飞行要求时，必须离开编队，打破原来的队形，组成独立的编队，进行避撞；完成避撞后，打开的独立编队重新整合，进入原来的编队，编队回归原来的航路。

本发明一种基于视觉的无人机编队感知与规避方法,通过机载视觉传感器，在视觉测距缺失的情况下，引入了多机编队的双目测距方法。根据检测到的目标障碍物的信息，实时跟踪与测量，通过建立的椭球形飞行包络，来达到判断是否存在威胁的目的。最后，通过威胁检测范围的划分，在保持队形不变以及编队队形打破重新组合的方法来满足避撞与任务完成的要求。

附图说明

图1是本发明算法步骤图。

图2是本发明双机双目目标位置检测示意图。

图3是本发明中无人机编队与入侵机示意图。

图4是本发明无人机编队飞行安全包络。

图5是本发明编队进行避撞无需打开队形示意图。

图6本发明编队需要打开编队队形进行避撞示意图。

具体实施方式

如图1所示，本发明一种基于视觉的无人机编队感知与规避方法，包括以下步骤：

一、通过编队中每一架无人机上设置的机载视觉传感器检测出目标障碍物在其对应的图像平面坐标系中的位置，推导出目标障碍物对应的相机坐标系到对应的世界坐标系的转移矩阵M；

根据机载视觉传感器监测到的目标障碍物的信息，计算出其在图像平面坐标系中的位置；然后依次计算出目标障碍物在成像平面坐标系中的位置、目标障碍物在相机坐标系中的位置、目标障碍物相对于机体坐标系的位置、目标障碍物在世界坐标系中的位置；其中选用的相机坐标系，采用四元算法进行姿态转移，以解决姿态转移中的奇异值问题；具体过程如下：

通过机载传感器与机载信息处理系统，计算出目标障碍物在图像坐标系O₀-uv中的坐标(u,v)；

计算目标障碍物在成像平面坐标系O₁-uv中的位置(x,y)，其中dx,dy分

别为x轴和y轴上的焦距分量、v₀,u₀为中心坐标，s₁为相机偏移系数；

根据目标障碍物在成像平面坐标系中的位置(x,y)，得出目标障碍物在相机坐标系Oc-XcYcZc中的位置(Xc,Yc,Zc),其中，Zc与摄相机的光轴重合，f是有效焦距；

根据相机坐标系与机体坐标系的转换关系，计算目标障碍物相对于机体坐标系Ox-xyz的位置(Xx,Yy,Zz)；

设相机云台具有三轴方向的旋转，且相机坐标系与机体坐标系、世界坐标系具有相同的方向。这样，设云台与三轴方向的姿态角为：为了解决姿态转移中的奇异值问题，采用四元数法进行姿态转移，则四元数表示法有：

则姿态旋转矩阵为：

相机坐标系转移到机体坐标系之间的转移关系为：

Px＝R*Pc+T(5)；

其中，T为相机坐标系相对机体坐标系的位置偏移量。则通过公式(5)，可

以得到式(6)，即：

通过公式(6)计算目标障碍物在世界坐标系O-XwYwZw下的位置(XwYwZw)；

其中，Tw(x,y,z)为无人机相对于世界坐标系的位置偏移，Rw 为无人机的姿态转移矩阵。

通过从式(1)至式(7)的推导关系，可以建立目标障碍物在图像平面坐标系到世界坐标系之间的位置转移关系：

由于公式(8)中只有两个已知量(u,v)需要计算出目标障碍物在世界坐标系下的位置(XwYwZw)是一个不可解量。

二、选取如上所得的任意两架机载视觉传感器得到的目标障碍物对应的相机坐标系到对应的世界坐标系的转移矩阵M，通过双目视觉测距的原理，应用最小二乘法，计算出目标障碍物在世界坐标系中的位置；具体过程如下：

如图2所示，根据双目测距原理，根据两个相机中目标障碍物在图像平面坐标系的位置，可以计算出目标障碍物在世界坐标系中位置关系。和通常的双目测距不同的是，相机的内参标定已知，而外参的标定时通过成像平面坐标系与云台坐标系，云台坐标系与机体坐标系，机体坐标系与世界坐标系之间的转换关系得到。相机坐标系与机体坐标系，机体坐标系与世界坐标系之间的姿态角是通过角度测量仪得到，因此可以知道矩阵M是一个时变矩阵，则通过时变矩阵关系得到。

通过式(8)中第i,j架飞机的转移矩阵M，可以建立方程组，得到i,j双目测距下的目标障碍物的位置P_ij(X_w,Y_w,Z_w)；

其中，(u₁,v₁)，(u₂,v₂)为两架飞机在图像平面坐标系下的坐标，则(9)(10)联立得：

由这个方程组可以解出相机坐标系下三个解，得到P_ij(X_w,Y_w,Z_w)。

通过最小二乘原理，设定一个无人机编队中具有N架无人机，则依据两架飞机可以计算出目标的当前位置，根据数值加权可得到目标的位置：

其中是第i个飞机和第j个飞机的所确定的无人机的位置坐标。

三、在编队中定义虚拟掌机和虚拟安全区域，由步骤二中所得的目标障碍物在世界坐标系中的位置，计算得出目标障碍物与虚拟掌机及虚拟安全区域的距离，确定目标障碍物是否对虚拟掌机和虚拟安全区域造成威胁；

定义编队虚拟掌机：

其中，是第i架无人机的飞行位置，P_GC为编队中心位置。其中，n为编队中无人机的架数；i为自然数，且i属于1，2……n。由编队飞行规则可知，无人机编队飞行过程中，始终跟着飞行编队方向飞行，无人机与虚拟掌机按照一种跟随模式飞行。飞行过程中，虚拟掌机采用矢量叠加法，按照预先定好的位置，即本机相对于虚拟掌机的矢量为：按照事先预定好的编队队形进行飞行。通过获取虚拟掌机的GPS数据与本机GPS数据检测出虚拟掌机与本机的位置矢量，改变编队成员的位置来保持位置矢量的不变性。虚拟掌机为编队中的领导机。

如图4所示，确定虚拟掌机的虚拟安全区域，将虚拟安全区域定义为椭球飞行包络：

其中，编队采取安全飞行模式，定义编队飞行方向为x方向，z轴沿编队位置向上，y轴满足右手规则；a为x轴上的长半轴，b为y轴上的短半轴，c为z轴方向上的短半轴，无人机飞行过程中，以飞行安全碰撞时间为长半轴，这样定义最短碰撞距离d，定义b＝c，选取黄金分割点：这样就完成了椭球飞行包络的定义；

则d_s的方向与编队飞行方向一致，即编队速度方向；其中：d_s是飞行的安全距离，R为定制，根据编队队形进行系统性定义；D为飞行安全距离；T为飞行安全最短时间；为编队飞行速度；这样，当a＝‖d_s‖计算出虚拟掌机的虚拟安全区域，即椭球飞行包络。

当目标障碍物进入椭球飞行包络后，即可认为是具有威胁，进行告警预示。同时开启避撞检测算法。当具有威胁时，则立即建立避撞系统。

同时，需对每一架飞机进行飞行安全区域的划分，计算每一架飞机的最小分离距离

对编队每一架飞机进行飞行安全区域划分，计算每一架飞机的最小分离距离

当时，为其飞行安全区域；

其中：为目标障碍物的相运动速度，其中和分别是目标障碍物的运动速度以及单架次无人机的运动速度，定义相对运动速度径向距离为无人机与目标障碍物的径向距离。

进入椭球飞行包络范围的无人机可以进行避撞主要采用两种避撞策略，即①采用编队队形统一避撞，②编队队形打开进行避撞，避撞完成后编队会重新组合。根据上述步骤计算出的无人机飞行安全等级，在满足避撞需求的情况下，建立避撞系统。

四、根据步骤三所得的结果，当目标障碍物不对虚拟掌机和虚拟安全区域造成威胁时，编队保持原航路飞行；

如图3所示，当目标障碍物对虚拟掌机和虚拟安全区域造成威胁时，编队中相应的无人机进行规避，完成避撞后，进入原来的编队，编队回归原来的航路。

根据无人机的性能，计算由单架次无人机的转弯半径：

和单架次无人机的最大转弯加速度由以上可得：

其中：V_FLN∈(V_min,V_max),a_cent≤A(V_L,R_L,V_max)；V_FLN为单架次无人机的转弯速度，V_L为单架次无人机在转弯处左侧的速度，R_L为单架次无人机在转弯处左侧的半径，f是无人机能够提供的最大推力或侧向力矩；A表示函数。

无人机在飞行过程中的速度必须满足(V_min，V_max)，其中Vmin为单架次无人机的最小的速度值，Vmax为单架次无人机的最大的速度值。

对于编队飞行的无人机而言，为了保持无人机编队队形的一致性，必须使得每架无人机保持相同的转弯半径R_FLN，但是，这不符合每架无人机的飞行安全机制。根据中单架次飞机的飞行安全距离，无人机的相对避撞可以有两种避撞策略；

编队中的每架成员计算虚拟掌机左侧避撞与右侧避撞的最大转弯半径的情况下的，本机能够满足编队最小安全距离的飞行要求。通过信息交互，编队成员将具有的左侧避撞与右侧避撞计算，当所有编队成员具有满足同一侧避撞需求时，即进行无人机编队同侧避撞，如图5所示。当某几架无人机不能满足飞行要求时，必须离开编队，打破原来的队形，组成独立的编队，进行避撞，如图6所示。

完成避撞，打开独立编队重新整合，进入原来的编队，编队回归原来的航路。

对于轻小型编队飞行无人机，光电传感器作为对非合作目标是一种非常重要和有效的感知与规避方法。本发明适用于军民两用领域。

Claims (2)

1.一种基于视觉的无人机编队感知与规避方法，其特征在于，包括以下步骤：

步骤一、通过编队中每一架无人机上设置的机载视觉传感器检测出目标障碍物在其对应的图像平面坐标系中的位置，推导出目标障碍物对应的相机坐标系到对应的世界坐标系的转移矩阵M；

根据机载视觉传感器监测到的目标障碍物的信息，计算出其在图像平面坐标系中的位置；然后依次计算出目标障碍物在成像平面坐标系中的位置、目标障碍物在相机坐标系中的位置、目标障碍物相对于机体坐标系的位置、目标障碍物在世界坐标系中的位置；其中选用的相机坐标系，采用四元算法进行姿态转移，以解决姿态转移中的奇异值问题；

步骤二、选取步骤一中所得的任意两架机载视觉传感器得到的目标障碍物对应的相机坐标系到对应的世界坐标系的转移矩阵M，通过双目视觉测距的原理，计算出目标障碍物在世界坐标系中的位置；

选取编队中任意两架无人机，将由步骤一中所得的对应的目标障碍物的转移矩阵M建立方程组，通过最小二乘法，计算出目标障碍物在世界坐标系中的位置；

步骤三、在编队中定义虚拟掌机和虚拟安全区域，由步骤二中所得的目标障碍物在世界坐标系中的位置，计算得出目标障碍物与虚拟掌机及虚拟安全区域的距离，确定目标障碍物是否对虚拟掌机和虚拟安全区域造成威胁；具体过程如下：

3.1.计算出虚拟掌机的位置，其中：P_GC为编队中心位置， 是第i架无人机的飞行位置；

其中，n为编队中无人机的架数；i为自然数，且i属于1，2……n；

3.2.确定虚拟掌机的虚拟安全区域，将虚拟安全区域定义为椭球飞行包络：

其中，编队采取安全飞行模式，定义编队飞行方向为x方向，z轴沿编队位置向上，y轴满足右手规则；a为x轴上的长半轴，b为y轴上的短半轴，c为z轴方向上的短半轴，无人机飞行过程中，以飞行安全碰撞时间为长半轴，这样定义最短碰撞距离d，定义b＝c，选取黄金分割点： 这样就完成了椭球飞行包络的定义；

则d_s的方向与编队飞行方向一致，即编队速度方向；其中：d_s是飞行的安全距离，R为定制，根据编队队形进行系统性定义；D为飞行安全距离；T为飞行安全最短时间；为编队飞行速度；这样，当a＝‖d_s‖计算出虚拟掌机的虚拟安全区域，即椭球飞行包络；

3.3.计算编队中单架次无人机的飞行安全区域，

对编队每一架飞机进行飞行安全区域划分，计算每一架飞机的最小分离距离

当时，为其飞行安全区域；

其中：为目标障碍物的相对运动速度，其中和分别是目标障碍物的运动速度以及单架次无人机的运动速度，定义相对运动速度径向距离为无人机与目标障碍物的径向距离；

步骤四、根据步骤三所得的结果，当目标障碍物不对虚拟掌机和虚拟安全区域造成威胁时，编队保持原航路飞行；

当目标障碍物对虚拟掌机和虚拟安全区域造成威胁时，编队中相应的无 人机进行规避，完成避撞后，进入原来的编队，编队回归原来的航路。

2.按照权利要求1所述的一种基于视觉的无人机编队感知与规避方法，其特征在于，所述步骤四中的具体过程如下：

计算由单架次无人机的转弯半径：

和单架次无人机的最大转弯加速度

其中：V_FLN∈(V_min,V_max),a_cent≤A(V_L,R_L,V_max)，V_FLN为单架次无人机的转弯速度，V_L为单架次无人机在转弯处左侧的速度，R_L为单架次无人机在转弯处左侧的半径，f是无人机能够提供的最大推力或侧向力矩，A表示函数；

无人机在飞行过程中的速度必须满足(V_min，V_max)，其中V_min为单架次无人机的最小的速度值，V_max为单架次无人机的最大的速度值；

计算编队中的单架次无人机由虚拟掌机左侧避撞与右侧避撞的最大转弯半径，要使单架次无人机满足的飞行要求，通过信息交互，当所有编队成员具有满足同一侧避撞需求时，即进行无人机编队同侧避撞；当某几架无人机不能满足飞行要求时，必须离开编队，打破原来的队形，组成独立的编队，进行避撞；完成避撞后，打开的独立编队重新整合，进入原来的编队，编队回归原来的航路。