路径规划之导航_在有障碍的环境中

\qquad 对于载具来说在有障碍环境使其安全运行非常关键。此外，在大多数情况下，复杂环境在传感器探测范围以外是未知的。因此，载具只能执行传感-导航-传感-监督的导航模式，使载具安全到达目标点。观测单元作为子系统被引入到导航单元中，它的任务是将传感器信息转换为可被导航系统利用的数据。本文主要讲述的内容就是载具在GPS受限条件和有障碍环境运行的路径生成。受限于载具携带处理器的存储空间，故使用旋转地图结构(a rolling map structure)用于处理复杂环境产生的庞大地图。
\qquad 导航系统通过测量数据利用定位和建图方法处理环境信息。测量模块的主要任务包括：
$\bullet$载具定位
$\bullet$生成环境地图
\qquad 结合深度摄像机通过匹配稀疏特征获得相对坐标系转变的初始估计，然后通过最相近点迭代(ICP)提高其精确度。也可以使用2D激光测距在2D平面上提供更精确的距离测量，在室内环境中可以使用GMapping和Hector-SLAM开源包。但建图方法仍然需要大幅改进从而满足我们对于存储和分析环境信息的需要，为促进高效的建图，地图的数据结构应该：
\qquad 1.高效计算给定点和最近障碍物的距离。
\qquad 2.依据障碍物的模糊度评估给定的路径。
\qquad 前者是为了在给定的时间内检查载具和障碍物间的距离，后者是用于碰撞预测和路径选择。导航任务是引导载具在复杂环境中安全的到达目标点而不是重建地图。因此选择基于栅格的旋转地图来存储世界信息，本质上是栅格地图但允许超过地图边界和在其他边再进入(It is essentially a grid map but allowing information to exceed the map’s boundary and re-enter through the other side.)。
\qquad 首先实现真实地图位置到旋转地图栅格坐标系的转换。在地图的建立过程中每个栅格基于到障碍物的距离给定代价值(cost value)。基于欧几里得距离(EDT)获得距离地图，在整个地图上或者每个障碍物栅格周围使用滤波器计算所需的距离信息。通过距离信息给每定栅格代价值，EDF方法距离障碍物越近，网格颜色越暗。代价值基于高斯函数时，距离障碍物越近，网格颜色越亮。通过代价值地图区域可以分为三类，空旷区域代价值接近于0，未知区域代价值为中等，障碍物区域代价值为最高。每个栅格的代价值可以定义为cost(g)，距离障碍物最近的距离可以定义为EDT(g)。结果图可以直接使用基于图形的搜寻方法例如A* 算法。
\qquad 建图的另一个难题是移动的障碍物。如果探测到的障碍物投影在地图上并一直积累，运动的物体就会在栅格地图上形成一堵墙。会阻挡可行的路径使地图失去意义。常用的解决方案就是在每步的扫描中减小栅格的代价值。
$$cost(g)=cost(g)\cdot k_g(1)$$其中$0<k_g<1$，包含运动障碍物留下的轨迹的历史地图信息逐步消失。但也清除了固定障碍物的历史信息，为解决这个问题我们只观测在传感器视线范围内的物体，传感器和数据点之间总是空旷的。因为该方法设计为清空这些区域或降低代价值（Therefore, it is desired to clean these areas or at least lower their cost value）。在算法设计中应用体素遍历方法(the voxel traversal method)寻找传感器和数据点之间的栅格，所有获得的栅格组成列表L。
\qquad 最后构成环境地图，首先对输入进行离散滤波，将原始数据点投影到网格中(first performs a discrete filtering on its input by projecting the raw data point into the grids),如果两个数据点在同一个栅格中，就去掉后一个，这就会去掉大量的数据点。通过体素遍历，清除传感器与障碍物之间的空白区域。最后，所有的障碍点都被添加回来，并带有它们周围的距离信息。随着地图更新，只需访问相应网格的代价值，就可以轻松地检查任何点(包括载具当前位置)到最近障碍物的距离。
\qquad 另一方面，为了评估轨迹的清晰度，将轨迹转化为脚印(foot print)并沿着轨迹组成栅格列表，其中包含载具的大小和定位。为使轨迹无碰撞，设轨迹方程为：
$$Dis{t}_{min}(S_t)=\underset{g\in G_s}{\min }(EDT(g)-r_v>0)(2)$$其中$G_s$是由$S_t$覆盖的栅格列表，$r_v$是由载具中心到其边缘的最大距离。
\qquad 这时获得的轨迹应该是多项式、曲线甚至是经验数据。获取轨迹的方法为所有的轨迹都被一系列的线段近似，这些线段是通过在离散时间的实例中采样原始轨迹，然后使用分割合并的方法得到的。
\qquad 在有障碍环境下的导航问题可以使用数学形式表达，对于线性时不变离散时间系统：
$$\{\begin{array}{l}{x}_{k+1}=A{x}_k+B{u}_k\\ y_k=C{x}_k\end{array}(3)$$其中A,B和C是系统矩阵，$x_k$,$u_k$和$y_k$在k时刻的状态，输入和输出。
\qquad 定义$U=[u_0,u_1,\cdots u_{N-1}]$，任务可以描述为：
$$\begin{gathered} \underset{U}{\min }\left\{J(U)=x_N^{{}^{\prime }}P{x}_N+\sum _{k=0}^{N-1}(x_k^{{}^{\prime }}Q{x}_k+u_k^{{}^{\prime }}R{u}_k)\right\} \\ s.t.x_0=x_{start}{x}_N=x_{goal}(4) \end{gathered}$$ \qquad 直到现在，上式提出的最优化问题仍然没有显示解，包含数值解的最好努力也受限于优化空间的非凸性和动态的复杂性。对于高维度问题的直接解经常会经受数值不稳定和潜在生成危害硬件的命令串的问题。因此，需要对产生的控制序列进行双重验证并进一步增加计算负担。采用两步法对优化问题进行简化和求解。虽然不再保证全局最优性，但通过实际的载具试验，获得了满意的性能。两步法其实就是将路径规划任务分解为两层即内层和外层。
\qquad 外层就是全局规划，简单来说就是点到点，只获取环境信息而忽略载具的动态稳定，实现由起点到终点的路径规划。负责搜索环境的连通性信息，并为局部规划人员提供一般性的指导，防止卡在局部最小点上。通过环境的假设和化简之后，主要使用的算法包括适用在平面中的A* 算法和适用在空间中的RRT算法。
\qquad 内层就是载具运动的轨迹(位姿等)，其负责对任何可能导致碰撞的环境变化和传感器更新进行实时响应。典型的解决方案包括势场算法、向量场直方图法和速度障碍策略。最常用的的方法是样条多项式表示(polynomial spline representation)和 二次优化。这种方法的优点在于可以直接处理多关键帧问题(key frames)，关键帧就是$x_k=x_{chosen}$形式的等式约束，生成免碰撞方案即沿着路径(由全局规划确定的)的确定位置，驱使轨迹远离障碍物。该方法通常产生高质量的轨迹与最小的冲击或最小的突然性质。但其性能受到二次规划求解器质量的限制。一个在结束点之前连续不断的运动轨迹是由不停切换到新的轨迹拼接而成的。如果算法因为不知道的原因停止了，载具至少会停在安全区域。这通常被称为模型预测平衡点控制(model predictive equilibrium point control)策略，经常用于车辆路径规划。此外生成的轨迹应该保持在安全路径的附近。同步技术，数值追踪技术，基于安全路径的定位和方位建立旋转坐标系方法可以防止与安全路径产生大的偏差。在每次导航循环步骤包括：
\qquad 1.更新环境信息
\qquad 2.通过全局规划生成安全路径
\qquad 3.生成安全路径的第一节线段(the first line segment)（基础线段）
\qquad 4.生成参考轨迹
\qquad 5.检查轨迹是否安全，若不安全随机选取安全轨迹
\qquad 以上步骤每隔$t_m$秒重复一次，$t_m$比轨迹的全程持续时间短。如果新轨迹频繁生成，会形成较大的计算量。由于不断的切换轨迹导致运动不连续形成的冲击会使激进算法降低控制性能。
\qquad 为了提高轨迹质量和保留计算资源，必须设置合理的轨迹切换触发事件即满足特定条件时才会切换新轨迹。
\qquad 在每次循环中，算法首先验证当前轨迹的可行性。如果有可能碰撞，就立即切换新的轨迹。否则，如果当前轨迹也恰好进入减速阶段，则切换到新生成的轨迹。首先，通过连续性搜索获得安全路径，找到安全路径第一线段的终点，在其周围生成随机点采样列表，在具有最大清晰度的路径的基础上选择离障碍物最近的轨迹。
\qquad 通过以上算法，轨迹规划只基于两个条件:
\qquad 1.当前路径有碰撞
\qquad 2.到达结束点并盘旋
\qquad 可以减少转化轨迹的时间，考虑追踪误差，轨迹参考的有效性取决于(To consider the tracking error, the validity of trajectory reference depends on)：
\qquad 1.是否免碰撞
\qquad 2.追踪反应(tracking response)是否免碰撞
\qquad 条件2中，载具的追踪反应需要预测。当轨迹误差比较小时，需要使用经验公式，另外还要进行快进仿真。