[发明专利]一种基于双目视觉slam与融合算法的路径规划方法

权利要求书

1.一种基于双目视觉slam与融合算法的路径规划方法，包括有以下步骤：

S1、由双目视觉slam系统构建无人机三维点云地图，实现无人机的精准定位；

S2、采用RRT与人工势场相结合的算法，实现无人机的全局路径规划与局部路径规划的能力；

S3、设计出融合算法的故障处理程序，大幅提高无人机的实机运行的可靠性；

其特征在于：所述步骤S1的双目视觉系统是由双目摄像头与迷你PC构成，并将上述系统耦合到现有无人机实验平台中；双目视觉系统应用改进后的ORB-SLAM2的方案，其具体改进处理流程如下：

S101、先对双目图像进行预处理，包括各自提取ORB特征点，进行匹配后获取双目关键点，再将双目视觉传感器捕捉到的深度图像转换成关键点云，按照关键帧位姿规则，将变换后的关键帧点云加载到场景点云中；

S102、在滞后5个关键帧的情况下才向场景点云中添加关键帧，并使用基于视线约束的滤波方法，利用八叉树地图中的反向传感器模型消除点云中的误差点；其具体表达式为：

其中Q_j为新关键帧，T_j为该点的变换矩阵，w_i为世界坐标系上的一点，在经过T_j变换后获得坐标F_i,j，C_i,j表示在相机坐标下的坐标，K表示相机的内参矩阵，(u_i,j,v_i,j)为投影点；并设定删选规则阈值为r_i,j，则r_i,j具体表达式如下：

其中d(u_i,j,v_i,j)为投影点深度，设总阈值为R_i,j，则并对R_i,j大于5的点进行删除操作，进而实现视线约束的点云滤波；

S103、以远近特征点差异为插入关键帧条件，并在点云构建中，经常插入关键帧并且剔除冗余关键帧；

S104、对上述所得点云图使用统计滤波的方法来消除离群点，进行体素滤波处理，最终获得当前场景点云模型地图。

2.根据权利要求1所述的一种基于双目视觉slam与融合算法的路径规划方法，其特征在于：所述步骤S2的融合算法是将RRT与人工势场法相结合，实现全局与局部路径规划；其融合算法流程具体如下：

S201、对前端已经生成的三维空间点云地图运行RRT算法，并以当前点为起始点，设立的每个辅助规划点为每次局部路径规划的终点；其中辅助规划点D_i的选取规则是：全局路径规划获得点云数组，并对当前点云点放入到新的队列中，重新编号，按照飞行避障精度来选择间隔取点的个数；

S202、以辅助规划点D_i为每次局部路径规划的飞行终点，在辅助点D_i与辅助点D_i+1之间运行人工势场算法，进行局部重规划，引导无人机去执行局部生成的路径；

S203、当满足覆盖路径条件时，再次执行S201～S202的操作；否则，检测是否到达终点，若到达终点则结束当前路径规划的任务，否则进入故障处理程序；

S204、覆盖路径条件说明：

(1)若无人机顺利完成重规划任务，并且到达辅助规划点，此时由于点云地图的刷新，使得当前状态空间的地图障碍物信息进一步被丰富起来，并且在已经规划出的全局路径上检测出障碍物出现时，需要重新规划；

(2)无人机陷入局部极小值问题时，在无外界环境的干预下，无人机陷入一个领域点并且长时间无法移动脱离；判定条件：在引入人工势力场的情况下，无人机的合外力为0，覆盖路径迭代次数小于设定值5；

S205、进入故障处理程序后，当检测到人工势场合外力为0，迭代次数超过设定值时，则重新规划出全局路径，执行该路径任务T₁时间，指引无人机飞出局部极小值区域，再进入路径规划主程序；若检测到路径迭代次数过多，或者执行规划路径程序时间过长，则进入故障报错提醒；

S206、所述RRT算法基本过程如下：

(1)给出状态空间后的初始起点和终点后，通过Sample随机采样函数向状态空间随机撒点获得随机采样点x_blue，并通过移动距离函数StepSize向x_blue移动一段距离到达点x_red；假设起点到x_red之间的线段为l_edge，若线段l_edge没有触碰到障碍物，则将当前点x_red加入到新的树节点中，并且l_edge成为新的树枝，则新扩展的点x_red和扩展出的树枝l_edge构成了新的树；若l_edge触碰到障碍物，则新生成的x_new不会被加入新的树枝当中，重新继续下一次树的扩展；

(2)获得新的树枝后，继续通过Sample随机采样函数向空间中撒点，获得新的采样点x_rand；然后在新的采样点x_rand领域内寻找一个最近的树枝节点x_near，则x_near通过移动距离函数Stepsize向x_rand移动一段距离得到x_new，并设这段线段l_Ei，若线段l_Ei没有触碰到障碍物，则x_new被加入到新的树节点，并且l_Ei为新的树枝，新扩展的x_new和扩展出的树枝l_Ei构成新的树；

(3)重复(2)中的操作过程，直到x_new和终点x_goal重合或者靠近，接着通过指针回溯遍历父节点，从而实现一次起点到终点的路径规划；

S207、所述人工势场算法基本过程如下：

对当前状态空间引入人工矢量场，并引入引力场函数：

其中U_att(q)为引力场数值，而p_G(q)为到目标位置的距离，k则为引力增益常数，初步取得数值为5；

(1)引入矢量场后，获得状态空间的斥力场函数：

其中U_rep(q)为斥力场数值，p(q)为与障碍物的距离，η＝4为斥力增益常数，p₀为障碍物得到的影响范围；

(2)在获得引力场和斥力场函数后，确定矢量场中引力F_att与斥力F_rep，其具体数学表达式如下：

F_att(q)＝-kp_G(q)

3.根据权利要求1所述的一种基于双目视觉slam与融合算法的路径规划方法，其特征在于：所述步骤S3中，故障处理程序具体如下：

(1)当计算得出人工势场中无人机所受合外力为0，并且迭代次数超过设定值时，则重新规划出全局路径，执行路径任务T时间，指引无人机飞出局部极小值区域，再进入路径规划主程序；

(2)当检测到路径迭代次数过多，其中阈值设为5，或者执行规划程序时间过长，则进行故障报错提醒，并选择两种工作模式：模式一：输入新的返航终点坐标值并执行RRT算法进行返航飞行；模式二：当前位置点降落。