基于双目与超声波融合的无人机三维避撞方法及系统与流程

本发明涉及无人机控制技术领域，具体地，涉及基于双目与超声波融合的无人机三维避撞方法及系统。

背景技术：

近年来，随着无人机技术的不断发展和完善，它已经由军事领域逐步扩展到民事领域。并且随着航拍旋翼无人机的出现，使其逐步被大众所知晓，并产生了无人机热潮。旋翼无人机的普及使得其安全性被更多的人所关注，如何使得旋翼无人机在完成特定任务的同时，保证自身与周围人员的安全是如今民用无人机亟待解决的问题之一。

目前，现有的民用无人机所采用的避撞技术均可称为紧急避撞，即对无人机飞行过程中出现的单一障碍物进行躲避。但是，许多常规场景下的障碍物分布比较密集，如室内环境中墙壁、桌椅等，在这种环境下，不仅仅需要无人机做出紧急避撞，而且需要其对障碍物的分布进行分析，并规划出一条最优路线，使旋翼无人机在躲避障碍物的同时，到达预定的目的地，正常完成任务。本发明提出了一种基于粗糙障碍物地图的三维避撞方法。

技术实现要素：

针对现有技术中的缺陷，本发明的目的是提供一种基于双目与超声波融合的无人机三维避撞方法及系统。

根据本发明提供的基于双目与超声波融合的无人机三维避撞方法，包括如下步骤：

障碍物探测步骤：通过双目摄像头和超声波传感器探测环境中的障碍物，并将障碍物信息以点云的形式进行保存后等待融合；

障碍物地图生成步骤：对三维空间进行网格划分，分析每一帧得到的局部双目点云落在网格中的概率，对局部双目点云进行坐标系转换后通过融合生成障碍物地图；

最优路径规划步骤：基于障碍物地图，采用A*算法遍历无人机从起点到终点的所有可能的路径点，选择出最优路径。

优选地，所述障碍物探测步骤包括将双目摄像头作为视觉传感器，通过图像匹配与空间几何计算的方法，得到当前帧的深度点云PC₀，具体地，包括如下步骤：

步骤A1：预先对双目摄像头进行标定，得到双目摄像头的内参矩阵P₁与P₂，两个摄像头的畸变向量K₁与K₂，以及两个摄像头之间的位移向量T和旋转矩阵R；

步骤A2：对双目摄像头得到的图像进行校正，使得双目摄像头处于同一水平面；然后，使用SGBM算法对校正后的左右两幅图像进匹配，并算出每一对匹配点的视差，得到相应的视差图；

步骤A3：通过空间几何学以及反投影公式，将所述视差图中的每一个点在图中的位置坐标(x,y)及视差信息d投影到双目摄像头的空间内的一点P，形成一个含噪的局部空间点云，记为PC₀。

优选地，所述障碍物探测步骤还包括通过超声波传感器探测环境中的障碍物，并通过超声波传感器的探测信息对双目摄像头得到的局部空间点云进行滤波处理，消除局部空间点云的部分噪声；具体地，滤波处理的步骤如下：

步骤B1：判断含噪点云PC₀中的每一个点与摄像头的距离，若距离大于5m，则滤除该点，若距离小于等于5m，则执行步骤B2；

步骤B2：判断含噪点云PC₀中的每一个点与摄像头中心线的夹角是否大于30°，若大于30°，则滤除该点，若夹角小于等于30°，则执行步骤B3；

步骤B3：判断含噪点云PC₀中的每一个点与摄像头的距离是否大于超声波探测模块返回的距离，若大于超声波探测模块返回的距离，则滤除该点；若距离小于等于超声波探测模块返回的距离，则执行步骤B4；

步骤B4：对含噪点云PC₀内的所有点进行滤波运算，若有效点的个数大于等于1，则终止滤波，返回滤波后的点云，记为PC_l；若有效点的个数为0，且超声波模块返回的探测距离小于5m，则证明在无人机前方有不能被视觉探测到的障碍物出现，则对探测距离平面内以30°为张角形成的圆进行密集采样，并将采样后的点加入到点云PC_l内，返回点云PC_l。

优选地，所述障碍物地图生成步骤包括：

步骤C1：将三维空间进行网格状划分，即将三维空间分成a*a*a的小立方体网格，每一个网格表示一个障碍物单元e，且以p_e表示此网格中可能含有障碍物的概率；其中：a表示任一维度的网格数；

步骤C2：将每一帧得到的局部双目点云进行坐标系转换，并融合到障碍物地图中。

优选地，所述步骤C2中将无人机导航模块提供的位置信息记为：Loc＝{Loc_x,Loc_y,Loc_z}，将无人机在障碍物世界坐标系下的四元数记为：q＝{x,y,z,w}；通过计算，由四元数q得到旋转矩阵R，如公式1所示：

式中：w表示四元数的实部分量，x,y,z分别表示四元数的四个虚部分量；

对每一个PC_l中的点P_l进行如下坐标系转换得到对应的点

式中：表示点P_l转换到障碍物世界坐标系的对应点；

所有转换后的点组成转换后的世界坐标系中的局部点云PC_lw。

优选地，所述最优路径规划步骤中的A*算法是指：基于2.5维，在三维空间的垂直z方向上进行采样，得到一个在z方向上稀疏的障碍物地图Graph_z*(V,E)。

优选地，还包括无人机导航步骤：基于障碍物地图，输出无人机位置信息和姿态信息。

根据本发明提供的基于双目与超声波融合的无人机三维避撞系统，包括：Pixhawk飞控模块、电机、机载Odroid系统、数传模块、光流计；在旋翼无人机主体的基础上搭载了障碍物探测装置；所述障碍物探测装置包括：双目摄像头、MPU-9255IMU模块和MB1242超声波测距模块；其中：双目摄像头为两个单独的摄像头组成，以固定帧率向主控板传回图像信息；超声波测距模块通过I2C传回测距信息，有效探测范围为3cm～5m。

与现有技术相比，本发明具有如下的有益效果：

1、本发明提供的无人机三维空间避撞方法中融合了超声波传感器与双目摄像头的深度数据，能够检测出多种不同材质的障碍物，提高了系统的鲁棒性和无人机的安全性；其中超声波传感器的加入使得本发明的系统可以探测到一些基于视觉的方法无法探测到的障碍物，如透明的玻璃，无纹理的墙面等；同时，超声波的加入可以通过滤波的形式在一定程度上对双目摄像头的点云进行降噪处理，去除双目点云中的外点及单独的近镜噪点，提高了行过程中无人机的安全性。

2、本发明提供的无人机三维空间避撞方法中使用最近几帧的局部点云和全局粗糙地图进行融合，得到对应帧的障碍物地图。本发明的优选方案中基于所述障碍物地图，利用三维空间内基于垂直方向采样的2.5维A*避撞技术来计算最优路径，控制旋翼无人机按照最优路径飞行，提高了无人机的飞行效率。

3、本发明中提出的全局地图与局部地图结合的方法可以更为有效的计算更优路径，而不是简单的紧急避撞，且优化后的路径规划算法可以大大降低传统方法的计算时间，提高计算效率。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明的方法流程图；

图2(a)为A*路径规划算法的2维示意图；

图2(b)为A*路径规划算法的2.5维示意图；

图2(c)为A*路径规划算法的3维示意图；

图3为障碍物地图构建及路径规划结果示意图(此障碍物地图和路径规划结果采用OctoMap库进行维护和可视化显示)。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

本发明采用自行研发的无人机主体系统，包括Pixhawk飞控模块、电机、机载Odroid系统、数传模块、光流计等。在旋翼无人机主体的基础上搭载了障碍物探测装置。所述障碍物探测装置包括：双目摄像头、MPU-9255IMU模块和MB1242超声波测距模块。其中：双目摄像头为两个单独的摄像头组成，以固定帧率向主控板传回图像信息。超声波测距模块通过I2C传回测距信息，有效探测范围为3cm～5m。

根据本发明提供的无人机三维空间避撞方法，包括如下步骤：

障碍物探测步骤：通过双目摄像头和超声波传感器探测环境中的障碍物，并将障碍物信息以点云形式进行保存后发送至障碍物地图融合模块等待融合；

障碍物地图生成步骤：对三维空间进行网格划分，分析每一帧得到的局部双目点云落在网格中的概率，通过障碍物地图融合模块对局部双目点云进行坐标系转换后生成障碍物地图；

最优路径规划步骤：采用A*算法遍历无人机从起点到终点的所有可能的路径点，选择出最优路径。

所述障碍物探测步骤包括将双目摄像头作为视觉传感器，通过图像匹配与空间几何计算的方法，得到当前帧的深度点云PC₀，具体地，包括如下步骤：

步骤A1：预先对双目摄像头(包括左右两个处于同一水平面的摄像头)进行标定，得到双目摄像头的内参矩阵P₁与P₂(包含摄像头的焦距、主点等信息)，两个摄像头的畸变向量K₁与K₂，以及两个摄像头之间的位移向量T和旋转矩阵R；

步骤A2：对双目摄像头得到的图像进行校正，使得双目摄像头处于同一水平面；然后，使用SGBM(Semi-global Block Matching)算法对校正后的左右两幅图像进匹配，并算出每一对匹配点的视差，得到一张视差图(或视差矩阵)Disp；

步骤A3：通过空间几何学以及反投影公式，将所述视差图中的每一个点在图中的位置坐标(x,y)及视差信息d投影到双目摄像头的空间内的一点P，形成一个含噪的局部空间点云，记为PC₀。

所述障碍物探测步骤还包括通过超声波传感器探测环境中的障碍物，并通过超声波传感器的探测信息对双目摄像头得到的局部空间点云进行滤波处理(由于实际环境中的障碍物并不都是纹理丰富的障碍物，如白墙和透明玻璃，其上纹理稀疏甚至理想状况下并无纹理，也就不能被双目摄像头所探测到从而进行躲避。超声波传感器测可以很好的处理这种情况，在这种情况下超声波的探测信息与双目摄像头形成了互补作用。此外，双目摄像头探测得到的局部空间点云PC₀含有噪点和外点，超声波的信息可以对其进行滤波，滤除其中明显的噪声。对于含噪点云PC₀中的每一个点P₀，滤波后有选择性的选择滤除或者保留，并把保留的点加入最终的局部点云PC_l)，消除局部空间点云的部分噪声；具体地，滤波处理的步骤如下：

步骤B1：判断含噪点云PC₀中的每一个点与摄像头的距离(根据双目深度信息抽取的不稳定性和噪声规律，认为距离双目越远的点在点云中的误差也越大)，若距离大于5m，则滤除该点，若距离小于等于5m，则执行步骤B2；

步骤B2：判断含噪点云PC₀中的每一个点与摄像头中心线的夹角是否大于30°(由于摄像头存在畸变，且越靠近图像边缘的点畸变越严重，即使恢复后都存在不同程度的误差，故在过程中选择性的淘汰边缘的点，以提高准确性及安全性。经过实验及经验)，若大于，则滤除该点，若夹角小于等于30°(30°的张角恰与超声波的探测角度一直，便于进行下一步滤波)，则执行步骤B3；

步骤B3：判断含噪点云PC₀中的每一个点与摄像头的距离是否大于超声波探测模块返回的距离(根据超声波返回探测范围内最近的点的特性)，若大于超声波探测模块返回的距离，则滤除该点；若距离小于等于超声波探测模块返回的距离，则执行步骤B4；

步骤B4：对含噪点云PC₀内的所有点进行滤波运算，若有效点的个数大于等于1，则终止滤波，返回滤波后的点云，记为PC_l；若有效点的个数为0，且超声波模块返回的探测距离小于5m，则证明在无人机前方有不能被视觉探测到的障碍物出现(如透明玻璃)，则对探测距离平面内，以30°为张角形成的圆进行密集采样，并将采样后的点加入到点云PC_l内，返回点云PC_l。

所述障碍物地图生成步骤包括：

步骤C1：将三维空间进行网格状划分，即将三维空间分成a*a*a的小立方体网格，每一个网格表示一个障碍物单元e，且以p_e表示此网格中可能含有障碍物的概率；

为了提高计算效率，本发明采取双重精度对障碍物地图进行维护与计算，在障碍物探测及地图融合更新时，障碍物网格精度a＝0.05m；在路径规划计算时，障碍物网格精度a＝0.4m。

步骤C2：将每一帧得到的局部双目点云进行坐标系转换，并融合到障碍物地图中。

具体地，无人机导航模块提供的信息包括位置信息Loc＝{Loc_x,Loc_y,Loc_z}和无人机在障碍物世界坐标系下的四元数q＝{x,y,z,w}。通过计算，由四元数q得到旋转矩阵R，如公式1所示：

对每一个PC_l中的点P_l进行如下坐标系转换得到对应的点

式中：表示点P_l转换到障碍物世界坐标系的对应点；

所有转换后的点组成转换后的世界坐标系中的局部点云PC_lw。

由于无人机导航信息及双目点云提取均存在不同程度的误差，并且此误差可以在障碍物地图更新的过程中进行累积，故对其探测的权重采取时间衰减的方式进行分配，且只取最近m帧的局部点云进行融合，本发明中m取10。每一时刻t进行融合时，选取最近m帧的点云进行融合，记为PC_lw,t-n,n＝0,1,...,m-1。设点云PC_lw,t-n中的一点P_lw,t-n落在空间障碍物单元e中，则对障碍物单元存在障碍物的概率p_e进行更新：

p_e＝p_e*(1-W_n)+W_n*100％ (3)

n＝0,1,...,m-1 (5)

式中：p_e表示此单元有障碍物概率，W_n表示t-n时刻的权重。

所述最优路径规划步骤中的A*算法是一类较为快速的路径规划算法，此算法用启发式搜索的方式遍历所有可能的路径点，找到由起点到终点最好的路径。A*算法属于空间离散化的方法，将平面空间按照单位体积的立方体划分为空间网格图Graph(V,E)，空间中的每一个立方体均为图Graph中的一个节点，所有节点的集合为V，任意两个直接相邻的节点之间均有一条边相连，所有边的集合为E。

在算法运行的过程中，建立两个辅助的节点列表，分别为开启节点列表O(Vo)和关闭节点列表C(V_c)，开启节点列表包含所有待搜索的节点，关闭节点列表包含所有确定不再搜索的节点。设起点为s，终点为t，将起点s加入到开启列表O，之后开始进行循环迭代，每一次迭代中，搜索所有与开启节点相邻的节点，若此节点中没有障碍物，则将其加入开启列表O，此开启节点为新加入的开启节点的父节点，在新加入的节点中用指向标记指向它的父节点，对于每一个开启节点，计算他的估值函数F：

F＝G+H (6)

其中，G为由起点s到此节点所需的代价，H为由此节点到终点预计所花的代价。此节点的G由父节点的G与父节点到此节点的代价相加得到；H为由此节点到终点所需水平方向和垂直方向方块数的总和。在本发明中，定义一个节点的相邻节点有6个，分别是x、y、z三个方向的相邻点，每个边的代价为1。每一次循环迭代中，找到F最小的开启节点加入到关闭节点中，并将此节点在开启节点中删除。

经过算法的多次迭代，直到终点出现在开启列表中，则终止迭代。此时，由终止节点开始，按照节点指针所指的父节点方向不断向回走，直到走到起点为止，将所有经过的节点均加入路径队列中，最终得到的路径即为最终规划完成的最佳路径。

传统的三维A*算法在完整的三维空间内进行搜索，计算量大，计算效率低，不适用于实时的避撞技术。本发明提出2.5维A*算法，即在垂直z方向上进行采样，得到一个在z方向上稀疏的障碍物地图Graph_z*(V,E)，大大减少了计算的时间复杂度，使得避撞系统运行更高效。假设一个无人机避撞空间为100*100*40立方体，则传统方法要在完整的空间内进行路径规划。本发明对z方向40个立方体进行采样，随机提取n_s个进行采样，本发明取n_s＝5，得到5层100*100的二维空间，将此五层结合起来得到一个100*100*5的三维空间，在此空间内计算，若无法计算出路径，则重新采样计算，若t次后均无法找到路径，说明空间内障碍物密集程度太高，只有有限的若干路径可以有效进行避撞，则在完整的100*100*40空间内进行最优路径计算，本发明取t＝5。考虑到除三维迷宫等少数场景外，大部分的场景中障碍物的分布不会太极端，本技术完全可以适用，大大降低了运算时间，提高了系统效率。

本发明中的方法还包括无人机导航步骤：基于障碍物地图，输出无人机位置信息和姿态信息。

具体地，本发明所采用的无人机导航方式分为两种，一种是误差为毫米级且没有累计误差的精确视觉导航系统Vicon，另一种是非精确的导航方式，采用对光流计的速度输出进行积分的方式，此方式具有累计误差，平均每1分钟误差在2～4m。这两种方式在本发明中均有所使用，且各有优势与劣势。Vicon系统只能布置在特定区域，且以室内环境为主，不具有移动性，但是具有很高的精度，可用于在已知环境下的导航；光流计虽然精度较差且有累计误差，但对环境的依赖程度比较低，不需要提前额外假设外部定位辅助设备，可以应用于室内外各种环境。无人机导航信息的输出为无人机位置Loc和无人机的姿态四元数q，并输出给障碍物地图融合模块点云坐标系转换时所用。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。