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

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

【技术实现步骤摘要】
一种基于双目视觉slam与融合算法的路径规划方法


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

技术介绍

[0002]在人工智能的热潮下，无人机技术得到快速发展。其中，无人机的自主导航技术是无人机领域中一个重要研究方向，而路径规划是实现自主导航的核心内容之一。
[0003]主流的民用无人机路径规划技术是在GPS+IMU的基础上来实现的，这种技术成熟且应用广泛。但是缺陷也十分明显，在GPS信号微弱或者环境恶劣的情况下，无人机将无法完成对目标点的路径规划计算，不能自主到达指定地点。
[0004]此外，虽然目前路径规划的算法很多，比如A
*
、D
*
算法等，而上述算法受限于自身原理的问题，存在着各种算力不足、概率不完备等缺点，使得应用场景变得十分有限。同时，实际应用的主流路径规划算法仅仅是针对静态障碍物的路径搜索，附加一些局部避障的能力，这就使得无人机在复杂、微弱制导信号情况下的自主导航效果并不理想，无法有效及时的规避突发移动的障碍物。

技术实现思路

[0005]为了克服上述缺陷，本专利技术提供了一种基于双目视觉slam与融合算法的路径规划方法，同时完成了对无人机前端地图的精准构建以及路径规划算法的融合设计。能够在没有GPS或者制导信号的情况下，实现无人机快速高效的自主导航作业，在一定程度上能够有效规避移动的障碍物，大大提高了无人机的自主路径规划的能力。
[0006]本专利技术采用的技术方案如下：
[0007]一种基于双目视觉slam与融合算法的路径规划方法，包括有以下步骤：
[0008]S1、由双目视觉slam系统构建无人机三维点云地图，实现无人机的精准定位。
[0009]S2、采用RRT与人工势场相结合的算法，实现无人机的全局路径规划与局部路径规划的能力。
[0010]S3、设计出融合算法的故障处理程序，大幅提高无人机的实机运行的可靠性。
[0011]进一步的，所述步骤S1的双目视觉系统是由双目摄像头与迷你PC构成，并将上述系统耦合到现有无人机实验平台中。双目视觉系统应用改进后的ORB
‑
SLAM2的方案，其具体改进处理流程如下：
[0012]S101、先对双目图像进行预处理，包括各自提取ORB特征点，进行匹配后获取双目关键点，再将双目视觉传感器捕捉到的深度图像转换成关键点云，按照关键帧位姿规则，将变换后的关键帧点云加载到场景点云中。
[0013]S102、在滞后5个关键帧的情况下才向场景点云中添加关键帧，并使用基于视线约束的滤波方法，利用八叉树地图中的反向传感器模型消除点云中的误差点。其具体表达式为：
[0014][0015]其中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
具体表达式如下：
[0016][0017]其中d(u
i,j
,v
i,j
)为投影点深度，设总阈值为R
i,j
，则并对R
i,j
大于5的点进行删除操作，进而实现视线约束的点云滤波。
[0018]S103、以远近特征点差异为插入关键帧条件，并在点云构建中，经常插入关键帧并且剔除冗余关键帧。
[0019]S104、对上述所得点云图使用统计滤波的方法来消除离群点，进行体素滤波处理，最终获得当前场景点云模型地图。
[0020]进一步的，所述步骤S2的融合算法是将RRT与人工势场法相结合，实现全局与局部路径规划。其融合算法流程具体如下：
[0021]S201、对前端已经生成的三维空间点云地图运行RRT算法，并以当前点为起始点，设立的每个辅助规划点为每次局部路径规划的终点。其中辅助规划点D
i
的选取规则是：全局路径规划获得点云数组，并对当前点云点放入到新的队列中，重新编号，按照飞行避障精度来选择间隔取点的个数。
[0022]S202、以辅助规划点D
i
为每次局部路径规划的飞行终点，在辅助点D
i
与辅助点D
i+1
之间运行人工势场算法，进行局部重规划，引导无人机去执行局部生成的路径。
[0023]S203、当满足覆盖路径条件时，再次执行S201～S202的操作。否则，检测是否到达终点，若到达终点则结束当前路径规划的任务，否则进入故障处理程序。
[0024]S204、覆盖路径条件说明：
[0025](1)若无人机顺利完成重规划任务，并且到达辅助规划点，此时由于点云地图的刷新，使得当前状态空间的地图障碍物信息进一步被丰富起来，并且在已经规划出的全局路径上检测出障碍物出现时，需要重新规划。
[0026](2)无人机陷入局部极小值问题时，在无外界环境的干预下，无人机陷入一个领域点并且长时间无法移动脱离。判定条件：在引入人工势力场的情况下，无人机的合外力为0，覆盖路径迭代次数小于设定值5。
[0027]S205、进入故障处理程序后，当检测到人工势场合外力为0，迭代次数超过设定值时，则重新规划出全局路径，执行该路径任务T1时间，指引无人机飞出局部极小值区域，再进入路径规划主程序。
[0028]若检测到路径迭代次数过多，或者执行规划路径程序时间过长，则进入故障报错提醒。
[0029]S206、所述RRT算法基本过程如下：
[0030](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
不会被加入新的树枝当中，重新继续下一次树的扩展。
[0031](2)获得新的树枝后，继续通过Sample随机采样函数向空间中撒点，获得新的采样点x
rand
。然后在新的采样点x
rand
领域内寻找一个最近的树枝节点x
near
...

【技术保护点】

【技术特征摘要】
1.一种基于双目视觉slam与融合算法的路径规划方法，其特征在于：包括有以下步骤：S1、由双目视觉slam系统构建无人机三维点云地图，实现无人机的精准定位；S2、采用RRT与人工势场相结合的算法，实现无人机的全局路径规划与局部路径规划的能力；S3、设计出融合算法的故障处理程序，大幅提高无人机的实机运行的可靠性。2.根据权利要求1所述的一种基于双目视觉slam与融合算法的路径规划方法，其特征在于：所述步骤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、对上述所得点云图使用统计滤波的方法来消除离群点，进行体素滤波处理，最终获得当前场景点云模型地图。3.根据权利要求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、进入故障处...

【专利技术属性】
技术研发人员：高巍，龙伟，高泽天，于祥跃，白宇，
申请(专利权)人：吉林大学，
类型：发明
国别省市：