自动驾驶（十五）---------再论轨迹规划

       轨迹规划分为不同层次：导航规划、行为规划、动作规划。这里一一展开简单来说。

1.导航规划

       A*算法一直是一个经典高效的离散空间路径搜索算法，这里导航规划我们就直接等同于A*算法来讲解。

       广度优先算法（Breadth-First-Search, BFS）从起点开始不断向外扩散的方式遍历整个图，只要扩散过程能够到达终点，那么他们之间至少存在一条路径，且由于BFS从中心开始呈放射状扩散的特点，使得它所找到的这一条路径就是最短路径。

                                                          

           BFS搜索最短路径实在太慢了，为了提高BFS的搜索效率，接下来我们从BFS一步步改良到A*算法：

           a. 已经遍历过的点，记录到遍历列表中，下次不再遍历，避免死循环，并且先进入的先遍历其附近的点。

                                                            

           b. 扩散到目标点后，要记录对应的轨迹，这就需要我们每次纪律点的来源，方便回到原点找出轨迹。

                                                            

           c. 当遍历到目标点后，搜索算法就可以终止。

           d.BFS是全图搜索，这样太笨，所以每次搜索朝着目标前进比较好，如何成为朝目标前进呢？计算周围点到目标点的距离，距离越小表示该方向越有利与到达目标点。距离计算简化：abs(a.x - b.x) + abs(a.y - b.y)。

           e.虽然启发式搜索比BFS更快得出结果，但它所生成的路径并不是最优的，其中出现了一些绕弯路的状况。

                                                             

                    从起点到终点总会存在多条路径，之前通过visited数组来避免重复遍历同一个点，然而这导致了先入为主地将最早遍历路径当成最短路径。为了兼顾效率和最短路径，我们来看Dijkstra算法，这种算法的主要思想是从多条路径中选择最短的那一条：记录每个点从起点遍历到它所花费的当前最少长度，当通过另外一条路径再次遍历到这个点的时候，比较一下目前的路径是否比该点最初遍历的路径花费更少，如果更少那就将该点纳入到新的路径当中去。通过记录每个点到起点的当前最短路径花费（长度），这里的cost作为该点在PriorityQueue中的优先级。

      总结A*的步骤：

          创建两个表 OPEN 和 CLOSED，其中 OPEN 表保存所有已经生成而未考察的节点， CLOSED 表保存已访问过的节点。
          1．将 S 点放入OPEN表中，并且将CLOSED表置为空。
          2．重复执行以下步骤:
               (1)遍历 OPEN，查找并将其作为预处理节点。
               (2)将步骤(1)得到的节点移入CLOSED表中。
               (3)对于当前节点的4个相邻节点的每一个节点作如下处理:
                      ①如果其为障碍物或者已经移入CLOSED表中，则忽略掉此节点。
                      ②如果其不在 OPEN 表中， 则将其移入OPEN 表，并将当前节点设置为其父节点，并记录值。
                      ③如果其已经在 OPEN 表中，通过对 OPEN表中节点再进行判断，如果有更小值，则将其父节点设置为当前节点，并重新计算和。
                     ④将目标节点加入 OPEN 表中， 即表示路径已经找到，否则查找失败，且OPEN为空，即无路径。

                (4)保存路径，路径即为从目标节点开始， 沿着父节点移动至起始节点所组成。

2.行为规划

           依据全局导航路线信息，根据当前交通场景和环境感知的信息，加上自身当前驾驶状态，在交通规则的约束和驾驶经验的指导下规划出合理的驾驶行为。

                                              

         这是决策层最核心的部分，做出正确的行为规划，是无人驾驶的最高目标！俗称“聪明的大脑”！目前在没有一个“最佳解决方案”，普遍认可和采用的方法是分层有限状态机，最近也有用基于马尔可夫决策的模型。

         行为决策层需要解决的问题如下：

             a. 所有的路由寻径结果：比如无人车为了达到目的地，需要进入的车道是什么（Target Lane）。

             b. 无人车的当前自身状态：车的位置速度朝向，以及当前主车所在的车道。

             c. 无人车的历史信息：在上一个行为决策周期，无人车所做出的决策是什么？是跟车，停车，转弯或者是换道？

             d. 无人车周边的障碍物信息：无人车周边一定距离范围内的所有障碍物信息。例如周边的车辆所在的车道，邻近的路口有哪些车辆，它们的速度位置如何？以及在一个较短的时间内它们的意图和预测的轨迹。周边是否有自行车或者行人，以及他们的位置速度轨迹等。

             e. 无人车周边的交通标识信息：一定范围内的Lane的变化情况。比如路由寻径的结果是在Lane1的纵向位移10m处换道进入对应的相邻Lane2的纵向位移20m处，那么Lane1的合法的纵向位移换道空间是多大？比如从一个直行Lane行驶结束，需要进入下一个左转Lane，两条Lane的交界处是否有红绿灯或者人行道？

              f. 当地的交通规则：例如道路限速，是否可以红灯右拐等等。

  分层有限状态机

         有限状态机中构造有限数量的状态，外界的输入只能让状态机在这中间切换。分层有限状态机：简单说，就是状态太多时，不好维护，于是将状态分类，将同类的状态作为一个状态机，然后再做一个大的状态机，来维护这些子状态机。

        一个有限状态机通常包含如下几部分： 
             1. 输入集合：通常也叫刺激集合，包含我们考虑到的状态机可能收到的所有输入。

             2. 输出集合：即FSM能够作出的响应的集合，这个集合也是有限的。

             3. 我们通常使用有向图来描述FSM内部的状态和转移逻辑。

             4. FSM通常有一个固定的初始状态。

             5. 结束状态集合。

             6. 转移逻辑：即状态机从一个状态转移到另一个状态的条件。

         下面介绍几种市面上的 分层有限状态机。

          1. 徐亮等人将车辆行为分为 7 种：加速、减速、保持、换到左边车道、换到右边车道、左转和右转。基于MAS系统，提出开放式行为决策模型。(徐亮, 张自力. 基于 MAS 的驾驶行为决策模型的研究[J]. 计算机工程与科学, 2010, 32(5):154-158.）

          2. Vanholme B认为，行为都是非连续量，每个行为都可对应一个状态。只需要要对状态进行识别和处理。这种方法符合驾驶员的逻辑思维，同时可以满足交通规则，省去了大量的计算，因而能够保证决策具有实时性，同时也使决策功能在较短时间内实现。(Vanholme B, Gruyer D, Lusetti B, et al. Highly automated driving on highways based on legal safety[J]. Intelligent Transportation Systems, IEEE Transactions on, 2013, 14(1): 333-347.)

          3. 斯坦福大学的 Montemerlo M 将车辆行为细分，建立了一个拥有 13 个状态的有限状态机组成决策系统。其状态分别为：初始状态、前向驾驶、车道跟随、避障、停止标志前等待、路口处理、等待路口空闲、U-Turn、车辆在 U-Turn 前停止、越过黄线行驶、在停车区域内行驶、通过交通阻塞路段、在不匹配 RNDF 路网文件的情况下在路上行驶、任务结束。（Montemerlo M, Becker J, Bhat S, et al. Junior: The stanford entry in the urban challenge[J]. Journal of field Robotics, 2008, 25(9): 569-597.）

          4. 将驾驶行为的子行为设计为有限状态机中的子状态，形成分层有限状态机。(Ziegler J, Bender P, Schreiber M, et al. Making bertha drive—An autonomous journey on a historicroute[J]. Intelligent Transportation Systems Magazine, IEEE, 2014, 6(2): 8-20.)

          5. 2007 年的美国 Darpa 自动驾驶车辆挑战赛中，大多数自动驾驶汽车的决策模块都是基于规则设计的。（GindeleT,Jagszent D,PitzerB, et al. Design of the plannerof TeamAnnieWAY’s autonomous vehicle used in the DARPA Urban Challenge 2007[C]. Intelligent Vehicles Symposium, 2008 IEEE. IEEE, 2008: 1131-1136.）

         6. 卡耐基梅隆大学的 BOSS 使用行为推理方法进行决策，按照规定的知识及规则实时推理出相应的驾驶行为。基于不同的驾驶环境，BOSS 可以产生 3 个不同的顶层驾驶行为：车道行驶，路口处理和到达一个指定位姿，每一个顶层行为又由一系列子行为组成。其功能为在到达下一个参考路径点的行驶路径中生成间歇性的运动目标。状态估计器与目标选择器共同完成这项功能。状态估计器得到当前车辆在道路模型中的位置，目标选择器根据车辆位置和相应道路模型以及任务规划所产生的全局目标轨迹，产生当前的、即将发生的、未来将要发生的运动目标，具体地说，包括路口处理，道路处理、停车处理等几大模块，以及相应的子模块如换道、跟驰、路口等待、通过路口等等运动命令。（Baker C R, Dolan J M. Traffic interaction in the urban challenge: Putting boss on its best behavior[C].Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on. IEEE, 2008:1752-1758.）

         7. Virginia Tech 和 TORC Technologies 组成了 VictorTango 参赛队,车队及合作者共同研发了 Odin,Odin是由2005版福特混合动力车改装成的自主行驶车辆(Andrew Bacha et al., 2008)。决策模块基于仲裁的方法,采用“胜者为王”的机制。为了区分路口,停车场和普通驾驶场景下的行驶,采用层次有限状态机的方法。

       8. 无人车“Talos”主要由麻省理工大学开发，“Talos”的导航模块负责制定决策任务,它不停地计算出下一个目标点的位置并朝目标点运动。通过路口,会车及路口优先级判定; 超车; 道路阻塞时重新规划; 根据信号灯信息转向。

   另外介绍几种基于统计的决策模型

       1.  UlbrichS 等人为了适应在城市交通场景中不可避免的传感器噪声，在驾驶时能够做出实时决策，提出一种两步算法,使 POMDP 复杂度足够低，在信号处理网络中考虑相对距离、相对速度和碰撞时间。满足自动驾驶车辆决策实时性、一致性、远虑性和可预测性要求。(Ulbrich S, Maurer M. Probabilistic online POMDP decision making for lane changes in fullyautomated driving[C]. Intelligent Transportation Systems-(ITSC), 2013 16th International IEEE Conference on. IEEE, 2013: 2063-2067.)

       2. 基于规则的决策模型具有实时性，但它更侧重于实现功能，而不是实现高驾驶性能。（Guo M, Xu Y, Zhang Y, et al. A decision-making method for unmanned cars based on drivable area cutting[C]. Cloud Computing and Intelligent Systems (CCIS), 2012 IEEE 2nd International Conference on. IEEE, 2012:1210 - 1215.）

        3. Brechtel S 等人指出自动驾驶汽车在决策时必须考虑几个来源的不确定性：时间预测的不确定性，环境感知中存在的不确定性，由于传感器的检测不可避免的含有噪声并且大部分环境都被遮挡，车辆只能感知当前情形的一小部分。将连续状态层次贝叶斯转换模型嵌入 MDP 模型，使自动驾驶汽车在多车双车道高速公路场景可以进行决策。（Brechtel S, Gindele T, Dillmann R. Probabilistic decision-making under uncertainty for autonomousdriving using continuous POMDPs[C]. Intelligent Transportation Systems (ITSC), 2014 IEEE 17th International Conference on. IEEE, 2014: 392-399.）

        4. WeiJ 等人将传感器噪声、感知约束及周围车辆行为作为属性加以决策，使用 Markov 模型进行决策，增强了自动驾驶汽车在单车道内行驶的稳定性。（Wei J, Dolan J M, Snider J M, et al. A point-based MDP for robust single-lane autonomous drivingbehavior under uncertainties[C]. Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011: 2586-2592.）

         5. 使用 POMDP 方法在连续状态和行动领域里不容易找到最优解，因而 Cunningham A G 等人使用多准侧决策方法 (MPDM)进行决策，同时在线评估决策结果，POMDP 被用来表明模型的近似和假设。(Cunningham A G, Galceran E, Eustice R M, et al. MPDM: Multipolicy decision-making in dynamic,uncertain environments for autonomous driving[C]. Proc. IEEE Int. Conf. Robot. and Automation, Seattle,WA, USA. 2015.)

        6. 基于规则的决策模型能够快速实现功能，基于统计的决策模型能够充分考虑不确定性，结合两者优点，为了适应城区环境中复杂的驾驶场景，Furda A 等人使用多属性决策方法增强决策的舒适度和高效性。(Furda A, Vlacic L. Enabling safe autonomous driving in real-world city traffic using multiple criteria decision making[J]. Intelligent Transportation Systems Magazine, IEEE, 2011, 3(1): 4-17.    Furda A. Real-time decision making by driverless city vehicles: a discrete event driven approach[D]. Griffith University, 2010.)

        7. 陈佳佳在采用层次有限状态机的方法行驾驶场景的转换基础上，基于人类驾驶员复杂场景下的决策过程，提出了一种基于多属性决策方法的行为决策模型，抽取行驶过程中驾驶员关注的相关属性，判断、评价进行驾驶状态的择优，使得行为决策过程符合人类驾驶员的思维过程，解决了城市复杂交通场景下自动驾驶车辆的类人决策问题。(陈佳佳. 城市环境下无人驾驶车辆决策系统研究[D]. 中国科学技术大学, 2014.)

        8. 为解决复杂城区道路环境中决策问题，ChenJ 等人采用多属性决策方法来选择自动驾驶汽车的最优策略，多属性决策方法结合了层次分析法（AHP）及优劣解距离法(TOPSIS)，使用 TOPSIS 来排列可选方案，AHP 把经验驾驶员主观和客观的意见变成量化值。(Chen J, Zhao P, Liang H, et al. A Multiple Attribute-based Decision Making model for autonomous vehicle in urban environment[C]. Intelligent Vehicles Symposium Proceedings, 2014 IEEE. IEEE, 2014: 480-485.)

        9. 彭刚、黄心汉等人(2008)为了解决移动机器人在决策中遇到的实际问题,建立了一种新的神经网络决策模型,该模型以足球机器人为实际背景,基于径向基函数,借助于神经元的训练,可以更好的利用多源信息进行规划,提升了决策的准确性。(彭刚,黄心汉,杨涛,等.基于神经网络和模糊推理的移动机器人行为决策与控制[J].华中科技大学学报:自然科学版,2008 (S1).)