[发明专利]基于双目与超声波融合的无人机三维避撞方法及系统

权利要求书

1.一种基于双目与超声波融合的无人机三维避撞方法，其特征在于，包括如下步骤：

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

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

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

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

步骤A1：预先对双目摄像头进行标定，得到双目摄像头的内参矩阵P1与P2，两个摄像头的畸变向量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°为张角形成的圆进行密集采样，并将采样后的点加入到点云PCl内，返回点云PC_l。

2.根据权利要求1所述的基于双目与超声波融合的无人机三维避撞方法，其特征在于，所述障碍物地图生成步骤包括：

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

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

3.根据权利要求2所述的基于双目与超声波融合的无人机三维避撞方法，其特征在于，所述步骤C2中将无人机导航模块提供的位置信息记为：Loc＝{Locx,Locy,Locz}，将无人机在障碍物世界坐标系下的四元数记为：q＝{x,y,z,w}；通过计算，由四元数q得到旋转矩阵R，如公式1所示：

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

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

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

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

4.根据权利要求1所述的基于双目与超声波融合的无人机三维避撞方法，其特征在于，所述最优路径规划步骤中的A*算法是指：基于2.5维，在三维空间的垂直z方向上进行采样，得到一个在z方向上稀疏的障碍物地图Graph_z*(V,E)。

5.根据权利要求1所述的基于双目与超声波融合的无人机三维避撞方法，其特征在于，还包括无人机导航步骤：基于障碍物地图，输出无人机位置信息和姿态信息。