【论文阅读记录】基于视觉SLAM建图的无人机路径规划 作者：王海

---

一、论文前瞻问题

智能体在陌生环境中的一次自主导航任务可以划分为三个问题：
 “我在哪里”
 “我所处的环境如何”
 “我该怎么走才能到达目的地”
而 SLAM（定位与建图） 可以解决前两个问题，最后一个就交给 路径规划


 在这篇文章中针对视觉SLAM建图技术中的特征提取和特征匹配过程进行展开，对现有的特征提取技术，如OPENCV中集成的提取算法，进行改进，提升特征提取的质量和特征匹配的准确性，最终实现高精度的三维建图。
 在基于SLAM的建图完成后，通过对RRT算法进行与人工势场（APF）算法的融合改进，实现了高效的无人机三维路径规划。

---

二、论文内容概述

1.SLAM建图与路径规划综述

 SLAM分为两类，从前多用激光slam，但是由于造价高、只能形成光栅图，逐渐被现如今的视觉slam替代，以下则是视觉slam中的功能划分。

 三维路径规划基本分为4类：

类型	代表算法
基于几何模型或者图搜索的算法	A*算法
基于导航函数和虚拟力场的算法	人工势场（APF)算法
基于生物智能仿生的算法	蚁群/粒子群算法
基于数学最优化的算法	/

---

2.关键问题

 这里主要围绕路径规划中的关键问题进行介绍，在算法的使用中基于三维空间选用了APF和RTT算法的融合。
APF:势场函数的选择很重要，需要在接近障碍时的斥力足够大，远离目标点时的引力足够大才能确保无人机顺利的完成任务，但是要注意的是若函数选择不当，会导致局部最优点的产生，无人机会停滞或往复运动。
RTT：在二维时有很强的随机性，也可以很快速的规划，但在三维空间时增加了一个方向，可能会导致高低起伏的路径，如何减少采样点，完成一个平滑的轨迹是关键问题。

---

3.SLAM中前后端设计部分

 对这一部分我的理解还不够充足，其中涉及到的很多优化方法都不够了解，所以暂时没有深入阅读，通过作者的叙述，本文中，使用ORB特征作为实现视觉里程计的数据关联的方法，提出了图像分区域后再在各个图像块中进行特定数量的ORB特征提取的改进方法，这种方法能使特征点在图像上的分布足够均匀，这种改进解决了特征点扎堆的问题。进而提出对图像特征点进行编号的改进，加快了图像与局部地图进行特征匹配的速度，保证了视觉里程计的实时性。在特征匹配的过程中进行了特征点深度滤波的操作，这一项改进降低了深度图像的噪声，能够对匹配特征点对进行进一步的筛选，剔除了深度信息无效的特征点，能够进一步保证匹配内点的质量。提出了结合ICP算法和PnP算法共同实现视觉里程计的设计。最后提出了一种四线程实现稠密建图的视觉SLAM系统整体设计，实验表明，该系统能够构建精确的三维地图。首先，在视觉里程计中加入了基于非线性图优化的全局后端优化，使得前端在各个时刻估计出的相机位姿得到了最大程度的优化，消除了估计的运动轨迹发生漂移的位姿误差不断累积的现象。系统在进行全局BA优化后，新加了一条线程专门用于构建稠密点云地图。

---

4.三维路径规划

 由SLAM构建好环境的三维地图后，就可以在地图中为无人机需要执行的飞行任务进行路径规划。
 接下来首先对RRT的过程进行简单介绍，算法流程分为五步：

1.设置任务起点作为树根；
2.随机获得一个随机点q_rand， 找到与之距离最近的点q_nearest；
3.从q_nearest出发，随着q_nearestq_rand方向取一个新的候选点q_new;
4.连接q_nearestq_new，得到路径，对这条路径进行碰撞检测，若无碰撞则把q_new作为真正的节点，否则进行第二步；
5.重复直至q_goal

 正是由于算法的随机性可以让它在不搜索完全空间的情况下找到路径，所以RRT有在三维空间发挥作用的潜力，在三维空间中，采用欧氏距离作为两点之间距离的衡量单位。

 虽然RRT算法具有概率完备性，但是由于它的随机性，也导致在实际过程中会探索很多目标点之外的区域，这必将导致其算法收敛速度以及路径质量的下降，因此作者提出在RRT中融合人工势场（APF)算法进行随机树生长方向的改进，以势场合力的方向作为随机生长树的启发因子，将算法命名为IRRT(Improved-RRT)。
 这一改进不仅能够克服APF算法的缺点，比如该算法易于陷入局部最优，避免了难以选择势场函数的困难，而且大大地提高了改进RRT算法的收敛速度和性能。此外，因为随机树生长的方向为人工势场合力的方向，所以IRRT算法规划出的路径也会 趋于 最优。

随后需要确定IRRT算法中APF部分引力场和斥力场的函数：

引力场函数：由定义可知，无人机离目标点q_goal越远，受到的引力应当越大，所以可以简单的设置为以下公式，其中X_g为目标点位置，X为无人机位置，k为人工设置的比例参数。

斥力场函数：由定义可以知，在随着无人机远离障碍物到一个距离时，将不再受到该障碍的斥力，同时越接近障碍物受到的斥力越大，所以将斥力场函数设置为以下形式，其中ρ₀为不受到障碍影响的临界距离，η为人工标定的系数。

最后根据图5.3进行IRRT算法流程梳理：
在地图中进行一次随机采样确定随机节点的位置之后，我们第一步要做的就是找到最靠近它的树节点，然后计算当前合力方向即向量AB的方向来确定随机树生长的方向。接着，还要计算由最近点q_near和随机点q_rand确定的向量AC以及计算向量AB和向量AC之间的角度，根据角度的大小来决定随机树是否沿着AC的方向生长新节点。如果这两个向量之间的角度大于预设的角度阈值，就放弃向当前随机节点的方向扩展，重新产生一个随机节点。随机树仅在随机点满足角度限制的条件之才会生长，考虑到随机树在生长的过程中可能会遇到障碍物，本文将角度阈值设置为３0°。

 最终作者在MATLAB中利用RRT和IRRT进行了全局规划实验，以及使用IRRT进行了局部动态规划实验，得到的结果显示IRRT的路径明显优于传统RRT，后者的轨迹偏向于一个曲折的S型。但是在过程中需要对轨迹进行平滑处理，作者的方法是简单的判断三点(A、B、C)中的AC之间连线是否通过障碍物，如果无障碍物则取AC平均值作为点B的新坐标进行平滑处理。
 最终得到的轨迹图如下

三、拓展

本文的参考文献提到了：
39：在APF算法中加入梯度函数，结合遗传算法，将规划算法从二维应用到三维
53：蚁群算法
54：遗传算法
55：粒子群算法
56：将以上三种算法进行了融合，用于水下机器人的路径规划