文献阅读之DARPA冠军车Boss的轨迹规划
大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。
目录
Autonomous Driving in Urban Environments_Boss and the Urban Challenge
第5节 Mission Planning
第6节 Behavioral Reasoning
Motion Planning in Urban Environments_ Part I
第4节 Trajectory Generation
第5节 On-Road Planning
Motion Planning in Urban Environments_ Part II
第4节 Planning Complex Maneuvers
第5节 Tracking Complex Paths
第6节 Planning in Parking Lots
Autonomous Driving in Urban Environments_Boss and the Urban Challenge
本文是DARPA冠军车“Boss”的系统介绍，主要涉及软件架构、运动规划（3个层次）、感知、错误恢复等模块。运动规划方法的具体细节，在下面的《Motion Planning in Urban Environments》2篇文献中。因本人主要关注决策规划，故只梳理了本文的部分内容。
第5节 Mission Planning
- 由路网信息，计算边的cost，构成graph，以规划mission。
- 车辆行驶过程中，路网图会更新，添加新观察到的数据。路网图应该只反映周边环境，故体量较小，重新规划较快。
- 堵塞（blockage）信息是变化的，车辆可能会revisit以前到过的地方，检查堵塞是否被清除了。当所有path都堵塞时，必须revisit。
- 探测堵塞的算法可能会漏过一些障碍物，如果发生这种情况，会添加一个虚拟blockage，其只能在车到达checkpoint时被消除。
- blockage前后设定为可以U-Turn，且赋予较低的cost，经过blockage给较高的cost，重新规划。
- revisit blockage通过逐步降低经过blockage的cost实现，并介绍了所使用的指数型cost更新函数。
第6节 Behavioral Reasoning
- 环境上下文分3种：road，intersection，zone，相对应的行为也是3种：lane driving, intersection handling, achieving a zone pose。
- precedence estimator 用来判断过路口的优先级，输入是静态的road model 和动态的障碍物信息。根据到达时间、占据多边形来判断优先级（顺序）。通过给到达时间调整bias值，造成右转优先的倾向。重点：将通过路口优先级的问题转变为——将占据多边形按照其修改后（bias）的到达时间升序排列的问题。
- precedence estimator 还用来merge或across 运动的车流。路口yield场景，需要根据预估的时间窗口计算yield polygon。具体细节看不懂。
- 交通堵塞的情景，自车先等15s，还不行就增加该路口的cost，规划另外前往goal的path。若规划失败，执行error recovery。
- merge plan 主要看自车车道是否满足merge过去的空间，目标车道各障碍物之前、之后、之间，是否满足自车merge过来的空间，是否在checkpoint之前，自车与merge目标点前后障碍物车的速度、加速度等指标是否合理、可行。然后在目标车道挑选最好的merge目标点。
- on-road failure, 按照某种原则（主要是距离）在附近选取一系列恢复点。若这些恢复点都不可达，则U-Turn或更激进的开下车道继续推进。
- intersection failure，a：选择路口出口作为goal，而不是全局goal，并将路口更详细的信息传给motion planner；b：将a尝试的出口标记为阻塞，尝试其他出口重新plan mission；c：选择unconstrained goal，比如开出车道。
- zone failure，在非结构化的环境中，如停车场，一般是按某种原则（距离、方向等）选择临时的temp goal，前往temp goal，再次尝试前往origin goal。
-
Motion Planning in Urban Environments_ Part I
1. 整个决策规划模块在逻辑层次上分3层：Mission，Behavioral，Motion planning。
2. Mission planning 是根据静态路网图规划到下一个checkpoint的route。
3. Behavioral planning 根据route、local 的交通和感知到的障碍物信息，做出决策，以及该决策对应的参数，如巡航时发送desired lanes 和速度命令，进停车场时发送终点位姿。
4. Motion planning 负责生成轨迹，在场景上分2类：On-road和unstructured，如停车场。
5. 只在距离保持、路口优先级等事项上，behavioral层控制motion planner。其余场景，motion planner自行确定v和a。
第4节 Trajectory Generation
6. 第四节介绍轨迹生成的思路。本节是产生每一条备选轨迹的过程，对应7.B和7.C，每一条都要优化，一定速度很慢。
6.A 状态约束小节，提到了起点状态（XI已知）经过对控制（运动）过程（参数p未知）的积分，变化到了终点状态（XF临时变量），目标是使终点状态尽可能的接近目标状态（XC已知）。像是一个优化问题，但没有说明约束。岂不是无论过程如何，只要到了目标状态就行？
6.B 车辆模型小节，提到了使用的几个重要参数：controller delay，curvature limit，curvature rate limit，max acceleration and deceleration。
6.C 控制参数小节，将U分解为speed和curvature。speed profile根据任务和场景有4种可选。curvature profile选用了2阶spline。为了平滑，k0 = XI.curvature。
6.D 初始化函数小节，为优化计算一系列接近的初始值，存入lookup table。过程为：离散化5D state为table；采样轨迹参数值，看该轨迹的终点会落入table的哪个state；遍历table中的每一个state，挑选出终点离它最近的轨迹所对应的参数值。以此参数值作为优化的初始值。
6.E 轨迹优化小节没看明白。
第5节 On-Road Planning
7. 对于on-road的场景，规划步骤如第五节所述。
7.A 先提取道路中心线。（参考线）
7.B 沿道路中心线采样（和Lattice类似），以此为临时的目标点生成（smooth，sharp）一对轨迹。（下图中的白色对称线为自车的最小转弯半径示意）
7.C speed profile 要根据环境上下文和各种限制在6.C中选择。若选后造成终点状态误差较大，就换更保守的速度参数尝试。
7.D 评估轨迹，评估项都是典型的平滑、偏离等项，但没有给具体的形式。没有主动的避开动态障碍物，此处有针对动态障碍物的预测和碰撞检测，在参考文献9讲具体方法。
7.E lane change 和 lane keep是类似的，多产生一条沿当前车道的备选轨迹，以防备无法变道的情况。
7.F U-Turn，图中的U-Turn效果和一般人开车掉头的行为不同，没有隔离带障碍物，几乎当成了开阔区域。没提具体方法，参考意义不大。
7.G 防卫措施：当对向来车时，靠边变道减速停车，错开后再重新开上车道。参考意义不大。
7.H 错误检测和恢复：发生错误时，产生一个pose，利用lattice planner（非百度的Lattice）规划前往pose的轨迹，然后恢复行驶。规划细节在part2，脱困逻辑在上一篇文献。
Motion Planning in Urban Environments_ Part II
1. 相对于part1，本文关注unstructured planning，状态是(x,y,theta,v)，少了曲率。相同的车辆模型。
2. 从goal pose开始反向搜索，可以避开静态障碍物，远离动态障碍物周边，输出符合车辆模型的可行驶的path。疑问：输出的path传给local planner还要产生一系列的备选轨迹，把path作为route吗？
3. lattice planner和local planner运行10Hz，速度慢，所以本文特别强调预先规划，不要等待。local planner依照lattice的输出path生成备选轨迹，好像是将path当做了参考线，不太确定。
第4节 Planning Complex Maneuvers
4. 第四节讲述规划方法，就是本文的lattice算法，我认为是以Anytime D*为核心、辅以地图更新的图搜索算法。与百度的lattice不是一回事。
4.1 靠Anytime D*规划。若环境有变化，会触发AD*重新规划，复用之前的信息，规划速度快。
4.2 lattice在action and state space采用多分辨率，靠近终点处等复杂区域采用更小的角度间隔，其他地方采用大的角度间隔。只是朝向角度的多分辨率，与其他状态量无关。
4.3 给出了启发函数的设计。类似Junior。这里没给具体的计算方法。
4.4 提出combined cost map 融合了static map和constraint map，也是栅格代价地图，供lattice使用，含有soft constraint 和 hard constraint。
4.5 对动态障碍物的处理，主要通过更新地图代价实现。动态障碍物占据的位置及很短时间预测轨迹占据的位置，给与hard constraint，考虑检测和预测的不确定性、安全缓冲距离，在周边栅格增加high cost，算是soft constraint。这里说的比较含糊，动态障碍物预测轨迹包含时间信息，而cost map不含，怎么结合在一起？沿预测的障碍物运动趋势方向一定范围内增加cost吗？没有细节。地图的更新频率怎样？运算跟得上吗？
第5节 Tracking Complex Paths
5. 第五节讲如何跟踪tracking四输出的路径。
5.1 基本和part1的轨迹生成环节相同，不同在于：在规划前往goal pose的同时，另一个lattice planner在并行规划一条离开goal pose去temp pose的轨迹。temp pose可以是停车场出口或其他位置。也就是说，此处跟踪每次都生成2条轨迹，1条是进入的，1条是逃逸的，避免车进得去出不来。
5.2 车辆更倾向于向右避开障碍物或对向来车。我觉得这一条无需提。
第6节 Planning in Parking Lots
6. 第六节针对停车场场景
6.1 在车未到停车场时规划由停车场入口进入停车位的轨迹，可以避免stop。就是当前路段还没走完，下一路段已经规划完毕了。这样怎么保证整条轨迹在停车场入口处的平滑连续呢？停车场入口处的位姿又怎么提前确定呢？2个lattice就可以了，lattice1的path在入口处的pose传给lattice2作为输入start pose，在下一个周期跟随改变。
6.2 同5.1,2个lattice planner同时规划进入goal pose和离开的轨迹，不必在goal 处等待规划结果。
7. 第七节针对on road的错误恢复场景。当on road规划失败时，由Behavioral层输出一个或一组goal pose，交由lattice planner规划过去，以摆脱困境。脱困生成goal pose的原则在第一篇文章中有详细的解释。
8. 只要规划失败，Behavioral层就会不断的产生可行的goal pose，继续尝试规划和脱困。
Detection Prediction and Avoidance of Dynamic Obstacles in Urban Environments
- 动态障碍物轨迹预测主要依靠车辆模型、速度、朝向、周边道路环境等信息。不是我关注的重点，不多说了。
- 碰撞检测分3层：a:对自车规划轨迹和动态障碍物预测轨迹整体的bounding box，做overlap检测。b:若a失败，按同步后的时间戳遍历，构造自车和障碍物的bounding circle，overlap check。c:若b失败，常规的车身bounding box（polygon） overlap check。这样的设计是减少因逐个bounding polygon overlap check的耗时，更快。
