本文提出了一个算法，用以在未知环境中进行路径规划来到达目标。

关联特征点

在机器人观测到的地表点少于阈值时，可以认为跟踪失败，本文利用这一点，来判断机器人可选的前进地点。

关联特征点需要满足如下三个约束：

• 地标点在像素坐标系下的坐标在图像范围内。
• 根据描述子的尺度因子，地标点的深度在比例不变区域内。
• 地标点的观测角度和现有关键帧的观测角度接近。
  由于满足上述条件的地标点可能也无法顺利进行特征关联。在计算阈值时 ，每个地标点的贡献正比于其关联成功率。

导航和路径规划

本文采用八叉树进行地图构建，然后将地图投影为栅格地图，栅格分为四类：1、未知区域。2、占用区域。3、自由区域。4、边界区域。边界区域值虽然本身未被占用，但是有未知的领域。在路径规划的过程中，虽然会避免靠近未知区域和边界区域，但是会旋转朝向这些区域来降低地图的不确定度。
路径规划改自RRT*算法，根据关联特征点得到的结果对可选区域进行限制。