CN112904901B - 一种基于双目视觉slam与融合算法的路径规划方法 - Google Patents

Abstract

本发明公开了一种基于双目视觉slam与融合算法的路径规划方法。具体涉及到多旋翼无人机路径规划领域。具体方法如下：由前端利用改进型的双目视觉slam技术来完成对当前场景地图的精准构建，给出初始坐标点以及终点坐标点后运行RRT与人工势场法融合的路径规划算法，直至完成一次路径规划任务，若出现迭代次数过多等问题时则进入故障处理程序，进而提高无人机工作的可靠性。本发明是将改进的双目视觉slam技术与融合路径规划算法结合在一起，其结构简单可靠，可以自身构建环境地图并且具备全局路径规划和局部重规划的能力，大大提高了无人机的飞行工作效率，对无人机自主导航技术的发展具有重要意义。

Description

一种基于双目视觉slam与融合算法的路径规划方法

技术领域

本发明涉及多旋翼无人机路径规划领域，更具体地说，涉及一种基于双目视觉slam与融合算法的路径规划方法。

背景技术

在人工智能的热潮下，无人机技术得到快速发展。其中，无人机的自主导航技术是无人机领域中一个重要研究方向，而路径规划是实现自主导航的核心内容之一。

主流的民用无人机路径规划技术是在GPS+IMU的基础上来实现的，这种技术成熟且应用广泛。但是缺陷也十分明显，在GPS信号微弱或者环境恶劣的情况下，无人机将无法完成对目标点的路径规划计算，不能自主到达指定地点。

此外，虽然目前路径规划的算法很多，比如A^*、D^*算法等，而上述算法受限于自身原理的问题，存在着各种算力不足、概率不完备等缺点，使得应用场景变得十分有限。同时，实际应用的主流路径规划算法仅仅是针对静态障碍物的路径搜索，附加一些局部避障的能力，这就使得无人机在复杂、微弱制导信号情况下的自主导航效果并不理想，无法有效及时的规避突发移动的障碍物。

发明内容

为了克服上述缺陷，本发明提供了一种基于双目视觉slam与融合算法的路径规划方法，同时完成了对无人机前端地图的精准构建以及路径规划算法的融合设计。能够在没有GPS或者制导信号的情况下，实现无人机快速高效的自主导航作业，在一定程度上能够有效规避移动的障碍物，大大提高了无人机的自主路径规划的能力。

本发明采用的技术方案如下：

一种基于双目视觉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、对上述所得点云图使用统计滤波的方法来消除离群点，进行体素滤波处理，最终获得当前场景点云模型地图。

进一步的，所述步骤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)

进一步的，所述步骤S3的故障处理程序设计具体如下：

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

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

本发明的有益效果是：

1.本发明提供了新的一种路径规划算法，其不仅具备全局路径规划的能力，还具备局部重规划的动态避障要求。

2.本发明结合改进后的slam技术，为无人机的路径规划提供了精准的三维场景地图。

3.本发明可以在无常规导航信号的情况下实现自主场景地图构建，自主路径规划，有效拓宽了无人机自主作业应用场合，对无人机自主导航技术的发展具有重要意义。

附图说明

图1是本发明的总流程图。

图2是本发明的主程序流程图。

图3是本发明的故障程序流程图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明：

本发明是一种基于双目视觉slam与融合算法的路径规划方法，总的工作流程图如图1所示。

本发明的具体实施方式如下：

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所示，所述步骤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所示，所述步骤S3的故障处理程序设计具体如下：

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

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

Claims (3)

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算法进行返航飞行；模式二：当前位置点降落。

Images