[发明专利]一种基于RGB-D SLAM的无人机路径规划方法

权利要求书

1.一种基于RGB-D SLAM的无人机路径规划方法，其特征在于：

S1、由RGB-D SLAM系统构建无人机三维点云地图，实现无人机的精准定位；

S2、采用人工势场法，实现无人机的实时路径规划能力；

S3、设计出路径规划算法的异常处理程序，大幅提高无人机在实际处理故障问题的应对能力。

2.根据权利要求1所述的一种基于RGB-D SLAM的无人机路径规划方法，其特征在于：所述步骤S1的RGB-D SLAM系统是采用的RGB-D传感器与移动PC组成，利用成本较为低廉的RGB-D视觉传感器来获取深度色彩图像，利用移动PC来处理大量的图像数据流，上述系统为无人机的地图前端系统；其中RGB-D SLAM采用的内核算法是ORB-SLAM方案，其具体流程如下：

S101、将RGB-D获得的彩色数据图像进行ORB特征提取，为得到特征的初始点，与上一帧的图像特征点进行初始匹配；并同时对图像进行深度计算，进而获得深度图像；

S102、对上数获得的数据流，进行特征点对数判断，若前后帧间深度图像的特征匹配点大于设定值时，则采用RANSAC算法对前后两帧位姿进行估计；

S103、上述步骤S102中若特征点匹配数小于设定值时，则转向计算当前帧的词袋向量，而本系统的词袋向量是采用DBoW2库建立关键帧ORB特征获取；将当前帧的词袋向量与关键帧数据进行搜素匹配，然后采用RANSAC算法来对当前帧与各位关键帧间进行最佳位姿估计；

S104、对S102、S103获得的数据流利用covisibility graph获取的局部地图进行当前帧姿态优化，进而获得当前帧的位姿、特征点、相机参数等信息，接着进行关键帧的判断；

S105、将经过步骤S104处理的数据一部分送入词袋模型中用于步骤S103的词袋向量与关键帧数据的搜索匹配，另一部分采用g2o对当前正在处理的关键帧与相关联的内部不同时刻的关键帧进行BA调整，最后进行闭环检测；

S106、将上述步骤获得结果再通过八叉树数据结构处理，将无序的点云数据流归整为可索引的有序3D点云数据，由此无人机自主导航的前端地图系统构建完毕。

3.根据权利要求2所述的一种基于RGB-D SLAM的无人机路径规划方法，其特征在于：所述步骤S2的路径规划方法采用的是人工势场法，算法流程具体如下：

S201、对当前无人机的运行的状态空间的某个位置X＝[x,y,z]^T，x、y、z为空间三维坐标，对其引入引力场函数：

其中ε是引力比例因子，ρ(q,q_goal)为无人机q与目标点q_goal的欧式距离,U_att(q)为引力场的函数值；

S202、对上述势场引入势力力函数：

其中F_att(q)为引力场引力值；

斥力对应的势场函数U_req(q)为：

其中η为斥力比例因子，ρ(q,q_obs)为无人机q与障碍物q_obs之间的欧式距离，ρ₀为障碍物的影响范围；进而斥力函数可表示为：

其中：

上式中F_rep1(q)为势力场中排斥无人机远离障碍物的运动分量，F_rep2(q)为势力场中目标点吸引无人机的运动分量；

S203、由S201～S202进一步可得无人机三维状态点云中的力函数为：

4.根据权利要求3所述的一种基于RGB-D SLAM的无人机路径规划方法，其特征在于：所述步骤S3的异常处理程序是为了修正无人机在运行人工势场法时出现的各种局部极小值问题，在此问题下，无人机容易陷入某一个局部最小值点而无法完成全局路径规划，无法到达设计的目标点，具体无人机的异常问题处理程序如下：

S301、设状态空间的一个三维空间球体半径为R，跳点系数为α，悬停阈值时间为T，当无人机到达某点后，引力场的合外力F为0且悬停时间超过设定阈值时间T时，以无人机为圆心生成一个半径为αR的球面；

S302、在此球面上随机选取一点，并判断该点是否位于障碍物上，若在障碍物上，删掉该点，再任意取一点继续重复是否位于障碍物的判断，在限定遍历时间T_search内寻找到一个位置点，并把该点设置为当前中转的局部目标点D，无人机运行人工势场法飞向该点后，再继续向目标点运行人工势场法，直到完成最终的路径规划任务；

S303、上述步骤S302中若在T_search内没有找到局部目标点，则生成故障报错，并选择两种工作模式：模式一输入新的返航终点坐标值并执行人工势场算法进行返航飞行；模式二当前位置点降落，其中T_search的设定值根据搭载PC的运算能力进行调整，跳点系数α则根据飞行地图的精度来设定。