《Driving on Point Clouds: Motion Planning, Trajectory Optimization, and Terrain Assessment in Generic Nonplanar Environments》
引入
大规模户外环境的自主导航对机器人系统至关重要。
除了定位与地图构建之外，运动规划是任何自主导航系统的关键功能。在一般形式中，运动规划是计算连接机器人状态空间中两个状态之间的可行路径。对地面机器人来说，运动规划可以自动找到一条将车辆引导越过地形到达指定目标姿势的路径，同时避免碰撞和其他危险。
任何地面机器人的运动都受到两个因素的制约：地形的形状和车辆的物理特性。如果忽略这些限制条件：即假设地形是平坦的、机器人是holonomic的，且没有运动学约束，我们可以获得一个简化的运动规划问题。
问题描述
运动规划
对于给定点云地形图
M
M
M中的起始位姿
T
M
S
T_{MS}
TMS和终点位姿
T
M
G
T_{MG}
TMG，计算轨迹，轨迹应该满足以下条件：1. 地形接触，沿着轨迹的每个位姿必须位于地形表面。2. 静态可穿越性，每个位姿的地形粗糙度
ρ
\rho
ρ，不超过阈值
ρ
m
a
x
\rho_{max}
ρmax。3. Nonholonomic运动约束：轨迹不可侧向移动。4. 曲率：轨迹上任何一点的轨迹，曲率
k
k
k都在有限范围内。5. 连续性：存在一个恒定的、非零的前进速度。
运动规划器主要分为三个部分：1. 使用RRT生成初始轨迹；2. 基于RRT*的全局轨迹优化；3. 细粒度局部优化。
轨迹表达
基于平面路径的短程轨迹生成
算法的整体路径使用基于树与结点的方式生成，上述三个步骤会生成基于结点的规划路径，我们通过使用one shot方法连接每个路径结点。
基于RRT的初始轨迹生成
基于RRT生成连接起始位置和目标位置的初始轨迹。生成的初始轨迹没有碰撞、且符合车辆和地形约束。其目的是在最短的时间内获得可用的解决方案。同时在起点和终点生长RRT树，每当不同树的两个顶点彼此靠近时，就会尝试连接树。一旦找到可行的连接，该算法即终止。
基于RRT*的全局轨迹优化
基于RRTs产生的轨迹通常能够产生合理的短轨迹，但是他可能包含大量的弯路。我们通过RRT*的全局轨迹优化方法解决这个问题。当添加新顶点 v n e w v_{new} vnew，检查邻域顶点集合 V n e a r V_{near} Vnear，从中遴选出 v n e w v_{new} vnew的父节点，以使得 v n e w v_{new} vnew的总路径成本最小，并修改树的结构。
局部轨迹优化
通过RRTs计算和RRT*优化的规划路径（planner trajectory）符合系统的要求。但就其他标准，如转向（涉及到路径曲率）或者地形粗糙度，轨迹并非最优的。我们针对成本函数
f
c
o
s
t
(
T
)
∈
R
f_{cost}(T)\in R
fcost(T)∈R对轨迹进行局部优化。
在不失一般性地前提下，我们将可行轨迹的成本定义为路径长度、曲率、地形可穿越性的函数，并用
c
i
c_i
ci表示
N
i
N_i
Ni和
N
i
+
1
N_{i+1}
Ni+1之间轨迹的成本。那么，整个轨迹的成本函数为
f
c
o
s
t
(
T
)
:
=
∑
i
=
1
N
n
−
1
c
i
f_{\mathrm{cost}}(\mathcal{T}):=\sum_{i=1}^{N_{\mathrm{n}}-1} c_{i}
fcost(T):=i=1∑Nn−1ci
轨迹优化算法的基本原理如下图。在成本函数的指导下，轨迹逐步迭代变形，直到达到
f
c
o
s
t
f_{cost}
fcost的局部最小值。在每次迭代中，当前轨迹在每个节点Ni处的两侧新增两个结点，那么相邻节点间就会出现九种短轨迹，由初始顶点到目标顶点就会有
3
N
n
−
2
3^{N_n-2}
3Nn−2种轨迹，使用标准图搜索技术，寻找最优路径。该路径也将会用于下一次迭代。
两次局部轨迹优化循环。
地形评估
地形评估是运动规划的前提。基于点云图的地形评估方法，该方法基于对局部邻域中地图点的几何分布的统计数据的计算，避免任何额外的建图过程。通过计算由地形引起的车辆位姿（以及可穿越性估计值）来隐式地重建地形。
满足地形条件的位姿计算
可穿越性计算
对每个地图点计算粗糙值
ρ
i
\rho_i
ρi，该值与机器人的参数无关，仅需要根据所在位置进行局部邻域点计算。
机器人在某个位姿的最终可穿越性不仅取决于所在地面的粗糙值，还有其自身的姿态（roll和pitch）。
多层环境
考虑构建多层环境中，上层地面与下层天花板点云相互牵连，如何在评估地面点时剔除下层的天花板点云。
自主导航系统
SLAM系统维护基于点云的地图，并以子地图的方式存储在数据库中。
在导航时，我们在子地图graph中搜索车辆的当前位置到目标位置的路径，沿该路径的所有子地图都被合并到起始位置的子地图上，因为子地图半径大于子地图间距离，因此子地图的合并会提高用于规划的点云图密度。
运动控制
针对动态环境，运动控制算法遵循以下方法：1. 轨迹规划仅考虑地图中的静态点。2. 建立一个“安全层”保障机器人不会碰撞动态物体。
安全层进行高频率的前向仿真循环，防止机器人与动态物体进行碰撞。