论文阅读笔记

论文名称：PUTN: A Plane-fifitting based Uneven Terrain Navigation FrameworkPUTN: A Plane-fifitting based Uneven Terrain Navigation FrameworkPUTN: A Plane-fifitting based Uneven Terrain Navigation Framework

  研究背景：地面机器人的自主导航已广泛应用于室内或结构化二维环境，但在室外三维非结构化环境中，尤其是在崎岖不平的地形中，仍然存在许多挑战。为了解决这一问题，本文提出了一种基于平面拟合的非均匀地形导航框架（PUTN）。

    实现过程：PUTN 的实现分为三个步骤。首先，在快速探索随机树（RRT）的基础上，提出了一种改进的基于样本的算法，称为平面拟合 RRT（PF- RRT*），以获得稀疏区域。每个采样点对应于点云上的自定义可遍历性指数和拟合平面。这些平面串联在一起，形成一个可穿过的条带。其次，利用高斯过程回归生成稀疏线段内插的稠密轨迹的可遍历性，并用采样树作为训练集。最后，利用非线性模型预测控制（NMPC）进行局部规划。通过在代价函数中加入可穿越性指标和不确定性，在约束函数中加入实时点云生成的障碍物，可以得到速度平滑、鲁棒性强的安全运动规划算法。

    核心的三部分：1) 基于随机抽样和平面拟合的稀疏全局轨迹生成；2) 基于高斯过程回归 (GPR) 和线性插值的密集路径生成；3) 一种非线性模型预测控制 (NMPC) 规划器。

---

由于不太了解 RRT，我先对 RRT 算法做了一定了解：

RRT 算法

 起始点为左下角，先进行一次随机采样，如图中黄点 Xrand，将红点（起点）与 Xrand 连接后，选择一定长度定为下一个点：

 优先考虑距离采样点最近的 “树” 中的点 Xnear 作为与他的连接点，如果连线遇到障碍物则本次采样无效：

 以此类推，随着采样的进行，会越来越靠近终点：

 当然，如果最终 Xnew 也就是新长出来的结点想要直接和终点重合是概率很小的，我们每次在循环过程中对新结点 Xnew 和终点 Xgoal 进行步长的判断，如果距离小于步长并且没有障碍物，我们就将 Xnew 和终点直接连起来：

 可以看到，RRT 算法规划出来的路径并不一定是最优路径，甚至可以说比较糟糕，考虑的优化策略有，以一定概率设置终点为 Xrand，直接连线 Xnear 和 Xrand 这样也许能规划一条笔直到达终点的路径（如果中途没有障碍物的话）

RRT 的代码实现里的一些总结：

判断障碍物与路径是否碰撞，在此例的简单模型中可以直接以障碍物圆心到直线距离判断，是否小于障碍物半径；

随机采样是以角度 + 长度选定的，如图：

---

 高斯过程回归

高斯过程（Gaussian Process, GP）是随机过程之一，是一系列符合正态分布的随机变量在一指数集（index set）内的集合

我认为该解释中的“指数”可以理解为“维度“，按照机器学习的角度，各个指数上的随机变量可以对应地理解为各个维度上的特征。

对于一个高斯过程：

 

设随机变量  ，其中 n 对应上文中的维数，  即对应各维度上的特征样本。  符合高维正态分布，  为均值向量，  是这  个特征之间的协方差矩阵，将  展开有：

 

其中  表示特征  i 和特征 j 之间的协方差（covariance）。

高斯过程样本与一般机器学习的样本区别在于，高斯过程中样本各特征之间存在相关关系，这种相关关系是通过协方差矩阵  来体现的。比如在一些时间序列模型里面，各个变量输出的时间序列在时间前后都会体现出一种相关性（比如平滑过渡等），这种模型输出就很适合使用高斯过程来模拟。

高斯过程回归（Gaussian Process Regression, GPR）是使用高斯过程（Gaussian Process, GP）先验对数据进行回归分析的非参数模型（non-parameteric model）

其中，回归分析法指利用数据统计原理，对大量统计数据进行数学处理，并确定因变量与某些自变量的相关关系，建立一个相关性较好的回归方程（函数表达式）我的理解就是类似离散点拟合成直线？能透过自变量的变化确定因变量的变化。

关于其中的先验和后验，看到一个很有意思的例子分享如下：

老王要去10公里外的一个地方办事，他可以选择走路，骑自行车或者开车，并花费了一定时间到达目的地。在这个事件中，可以把交通方式（走路、骑车或开车）认为是原因，花费的时间认为是结果。

若老王花了一个小时的时间完成了10公里的距离，那么很大可能是骑车过去的，当然也有较小可能老王是个健身达人跑步过去的，或者开车过去但是堵车很严重。若老王一共用了两个小时的时间完成了10公里的距离，那么很有可能他是走路过去的。若老王只用了二十分钟，那么很有可能是开车。这种先知道结果，然后由结果估计原因的概率分布，p(交通方式|时间)，就是后验概率。

老王早上起床的时候觉得精神不错，想锻炼下身体，决定跑步过去；也可能老王想做个文艺青年试试最近流行的共享单车，决定骑车过去；也可能老王想炫个富，决定开车过去。老王的选择与到达目的地的时间无关。先于结果，确定原因的概率分布，p(交通方式)，就是先验概率。

老王决定步行过去，那么很大可能10公里的距离大约需要两个小时；较小可能是老王平时坚持锻炼，跑步过去用了一个小时；更小可能是老王是个猛人，40分钟就到了。老王决定骑车过去，很可能一个小时就能到；较小可能是老王那天精神不错加上单双号限行交通很通畅，40分钟就到了；还有一种较小可能是老王运气很差，连着坏了好几辆共享单车，花了一个半小时才到。老王决定开车过去，很大可能是20分钟就到了，较小可能是那天堵车很严重，磨磨唧唧花了一个小时才到。这种先确定原因，根据原因来估计结果的概率分布，p(时间|交通方式)，就是似然估计。

回到高斯过程回归，当我们在某些指数上获得了一批观测样本后，这批观测样本将有助于我们对其他指数集上的样本分布进行估计（后验）。我们将这批已观测指数集设为  ，未观测到的指数集设为  。接下来便可使用方法获得在  上样本分布后验概率参数和  ，最后重新对  上的随机变量进行采样。

---

论文的优点：在路径规划前对全局地图进行平面分析非常耗时，文章中考虑在 RRT 算法生成树的时候再进行分析，避免一些不必要点的分析来节省时间。

具体流程：激光雷达探测到周围的地形并返回点云。然后，SLAM 模块基于点云构建一个全局地图。接下来，将全局映射导入到全局规划模块，该模块使用 PF-RRT * 算法生成 RRT 树和稀疏全局路径。在基于 GPR 的插值后，导入一个密集的全局路径添加到基于 NMPC 规划器的本地规划模块。本地规划器向机器人生成控制率，控制机器人的下一步动作。

考虑的点：考虑通过性时，文章中主要从三个点考虑，坡度，平整度和稀疏度。在进行采样时，所选取的每个点为中心，边长为 ls 的正方体区域内的点作平面拟合，考虑上述的三个指标是否符合要求。保证定步长的两个节点之间可通行性符合要求即可。

文中的线性插值：所谓线性插值好像并没有想的特别难，查询了一些资料发现，所谓线性插值就是根据两个点去估计某个点的数值，即平面上有（X，Y）和（M，N）两个点时，连成直线后就可以根据横坐标估计其他点的纵坐标，或者估计点的分布。科学地来说线性插值是指插值函数为一次多项式的插值方式，其在插值节点上的插值误差为零。线性插值相比其他插值方式，如抛物线插值，具有简单、方便的特点。线性插值的几何意义即为利用过 A 点和 B 点的直线来近似表示原函数。线性插值可以用来近似代替原函数，也可以用来计算得到查表过程中表中没有的数值。如图：