张楫的CMU团队提出了几个上层规划算法，包括在未知环境中的探索算法和全局路径规划算法，以及全套算法的系统集成和扩展应用。目前开源的是autonomous_exploration, dsv_planner, tare_planner, far_planner。 Autonomous Exploration Development Environment里自带的local planner只是用来做局部路径规划和小范围避障的，目标点太远时容易卡在死胡同里。如果需要长距离的路径规划建议使用far planner。
TARE算法的论文获得了Best Paper Award and Best System Paper Award of RSS 2021。 CMU-OSU参加DARPA SubT 2021挑战赛用的是M-TARE的初级版本(没有开源)。地面机器人同样用的LOAM。
我目前只详细研究了autonomous_exploration的代码。
CMU提供的避障算法优势在于能实时高效地应对复杂环境,同时最大化到达目标点的概率。算法的主要思想在于尽可能地让计算在线下完成。具体来说，一个避障算法最耗时的计算一般在于检测机器人在未来时间点与环境发生碰撞的可能。我们的算法先通过离线生成一个庞大的轨迹库（trajectory library）来模拟机器人在未来一段时间内可能走过的轨迹。接下来对于所有轨迹覆盖的空间，我们计算其内部所有点（在一定解析度下）与所有轨迹发生碰撞的可能性。经过这样的离线计算，我们可以得到一个空间内3D点到轨迹的对应关系。在实时运行的过程中，一旦空间内的某个点上有障碍物，我们可以马上知道哪些轨迹将会受到影响。我们的算法会降低选择这些轨迹作为最终路径的可能性。因为大量的计算都在线下进行，线上运行的时候只需要实时选择无碰撞的轨迹，我们的算法可以在几毫秒之内规划出一条无碰撞而最接近目标点的路径。
ROS里经常出现一种情况：规划出的全局路径没有考虑车是否能过窄通道，结果车到了窄通道附近要么又重新规划到其他方向，要么强行过窄通道。本算法会优先规划路径到开阔的地方。 另外也就避免了全局路径的无限规划问题
测试平台
测试平台是室外机器人，车上用了velodyne-16激光雷达，IMU是xsens mti-200。速度只能达到2m/s，所以高速机器人尚需验证。工控机CPU为4.1GHz i7。 定位使用vLOAM的商业版，参考论文。 运行系统要求是18.04或者20.04，其实是因为仿真环境使用Gazebo7，如果不用Gazebo，在16.04上也可以使用。
使用registered scans的好处：
- 使scan数据和state estimation之间的processing对时间同步不太敏感
- 多个registered scans可以 stacked 到一起，从中提取丰富的几何环境信息
目前我实现的效果
由于LOAM的问题，z坐标不可靠，所以我在代码里都把里程计返回的z按0处理，so far只在没有上下坡的情况下测试过。目前基本可以正常导航，环境中可以有动态障碍，可以调整线速度和角速度，可以倒退，不会有路径混乱、花费时间长的情况。
目前在线速度0.5m/s的情况下，还是有一定误差的，亟待优化