自动驾驶仿真：一文解析CARLA的行为规划

自动驾驶仿真：一文解析CARLA的行为规划

附赠自动驾驶最全的学习资料和量产经验：链接

今天，我将会带来一篇非常硬的干货，这将是全网唯一一篇详细阐述carla里自动驾驶行为规划是如何实现的文章！

众所周知，Carla的官方代码里给出过一个automatic_control.py的例子来展示行为规划的实现过程，不过这个例子里也包含了许多pygame相关的class object来单独弹出一个窗口展示画面，读起来十分不清爽，所以我在它的基础上进行了一些改动，只留下了与行为规划直接相关的代码，其他的一律剔除，方便大家理解。

先上本文大致目录来瞧一瞧：

Carla里的behavior planning大执分为全局路线规划、行为规划、轨迹规划与底层控制四大部分。automatic_control.py做的事其实很简单，随机在地图上生成一辆小汽车，随机给个目的地，让汽车自己在限速内安全、丝滑地开到目的地。

2. Behavior Agent初始化

在automatic_control.py里，最重要的不是隶属于Actor class里的vehicle, 而是BehaviorAgent class.它就像vehicle的大脑，一切指令由它下达，vehicle只管按照这个指令去行走。 这个class本身并不是像traffic_manager那样由C++封装好的，它位于PythonAPI/carla/agent中，纯粹由python构成。为了方便大家阅读和测试代码，我把agent相关的代码直接copy到了我的repo里。Agent构建这一块代码只会在最开始被呼叫，不存在于while循环中。

2.1 构建Behavrior Agent Class

BehaviorAgent 在构建时需要两个输入，一个是属于Actor class的vehicle, 另外一个就是车辆驾驶风格（string type).

# automatic_control_revised.py
agent = BehaviorAgent(world.player, behavior='normal')

如果我们走进这个class里面看（agents.navigation.behavior_agent.），可以看到如下部分代码：

# behavior_agent.py
class BehaviorAgent(Agent):
  def __init__(self, vehicle, ignore_traffic_light=False, behavior='normal'):
    self.vehicle = vehicle
    if behavior == 'cautious':
        self.behavior = Cautious()

    elif behavior == 'normal':
        self.behavior = Normal()

    elif behavior == 'aggressive':
        self.behavior = Aggressive()

如代码所展示，输入不同的驾驶风格字符串，agent的成员behavior会选择不同的class进行初始化。而Cautious(), Normal()和Agressive()这三个成员来源于agents/navigation/types_behavior. 在这里我以Normal()和Agressive（)举例：

class Normal(object):
    """Class for Normal agent."""
    max_speed = 50
    speed_lim_dist = 3
    speed_decrease = 10
    safety_time = 3
    min_proximity_threshold = 10
    braking_distance = 5
    overtake_counter = 0
    tailgate_counter = 0

class Aggressive(object):
    """Class for Aggressive agent."""
    max_speed = 70
    speed_lim_dist = 1
    speed_decrease = 8
    safety_time = 3
    min_proximity_threshold = 8
    braking_distance = 4
    overtake_counter = 0
    tailgate_counter = -1

由此可见，types_behavior里主要是定义汽车的限速（max_speed,)、与前车保持的安全时间（safety_time, 基于ttc,后面会详细讲述），与前车的最小安全距离（braking_distance）等安全相关的参数，agressive的车相对normal的车来说速度更快，跟车更紧。

当然了，agent里面还有其他成员变量，后面会一一涉及，在这里暂时就不全部罗列解释了。

2.2 全局路径规划

就像我们人类开车旅游一样，我们在手机上输入一个目的地，算法会自动给算出一个最优全局路线，我们顺着这些路开就行了。carla里也是一样，想让小车跑起来，就得告诉他跑到哪去，怎么个大概跑法。

# automatic_control_revised.py
agent.set_destination(agent.vehicle.get_location(), destination, clean=True)

不错，在automatic_control_revised.py里，只用了一行代码就把全局规划给做完了，但如果我们往里面挖掘，便会发现内部包含了四个步骤。

2.2.1 GlobalRoutePlaner初始化

在set_destination()被呼叫之后，behavior_agent内部会先初始化一个全局路径规划器. 其中GlobalRoutePlannerDAO在我看来是一个比较多余的class,因为它内部就只有两个成员函数，完全可以和GlobalRoutePlanner合并到一起。这两个成员函数一个是carla里的map–wld.get_map(), 另外一个就是sampling_resolution(默认为4.5米，我们知道所谓的路径不过就是一个个node与link连接起来，而sampling resolution变定义了两个node之间的距离是多少）。

# behavior_agent.py, line 151
dao = GlobalRoutePlannerDAO(wld.get_map(), sampling_resolution=self._sampling_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()

2.2.2 Carla Map Topology提取

上面我们提到了GlobalRoutePlannerDAO，它的主要作用就是提取carla 地图的拓朴结构–> self._topology = self._dao.get_topology()。

# inside the grp.setup()   
def setup(self):
      """
      Performs initial server data lookup for detailed topology
      and builds graph representation of the world map.
      """
      self._topology = self._dao.get_topology()
      self._graph, self._id_map, self._road_id_to_edge = self._build_graph()
      self._find_loose_ends()
      self._lane_change_link()

这个get_topology()内部的函数在这里不会细究（我会忽略一些细节，否则内容太多），我们只需要它返回的这个topology是怎样的data format:

[{entry:Waypoint1, exit:Waypoint2, path:[Waypoint1_1, Waypoint1_2,......]},
 {entry:Waypoint1, exit:Waypoint2, path:[Waypoint1_1, Waypoint1_2,......]}, 
 ......]

它的本质是一个list of dictionary, 每一个 dictionary 都是一段在carla的提前定义的**lane segment.如下图所示，entry和exit是一段lane segment的开始和结尾点，path是一个list,里面装着entry 和 exit之间的node, 每个node相距4.5米（即你的sampling resolution)。每一个node都是一个Waypoint class, 它永远位于lane的中央，**同时还可以记录左右lane上平行的waypoint的位置。

图1

2.2.3 Graph 建立

这一步会利用上面提取的topology(list格式）来构建道路的graph(networkx.Diagraph()格式）。其中每一个entry point或者exit point被看作node, path里的多个point组合起来当作edge。这些edge不仅会记录每一个waypoint的xyz坐标，还会记录entry/exit point的yaw angle,从而知道汽车进入这条lane时车头朝向什么位置。

# inside the grp.setup()   
def setup(self):
  """
  graph: networkx.Diagraph(); id_map:a dictionary, the key is the waypoint transform, and value is its
  node id in the graph; road_id_to_edge: eg.{road_id:{section_id:{lane_id:(node_id, node_id)}}}
  """
  # global_route_planner.py
  self._graph, self._id_map, self._road_id_to_edge = self._build_graph()

如以上代码所示，build_graph（）会提取成员变量 topology 返回三个变量。self._graph是我们最终需要的nextoworx.Diagraph()对象，id_map是一个辞典，key是每一个entry/exit waypoint的坐标位置，value是它们在graph里的id. road_id_to_edge 则是一个三层字典，最外成是carla里的道路id,中间一层是carla里的在该道路上的section id, 最后一层是carla 里的lane id， 里面对应着graph里的node id. 这个看起来复杂，其实就是为了做一件事：**为了把networkx graph中的每个node都和carla里的信息一一对应，方便后面做信息互换。**举个例子，node id 10对应着carla地图里 第17号road, 第三号section里的 第二条lane的entry point.至于carla地图如何定义road, section和lane的关系, 请看下图

图2

那么build_graph()里面是怎么运作的呢？它是不断地遍历topology list, 然后通过add_node与add_edge来一步步构建完整的图谱。

• graph.add_node:

• new_id: 由当前已存入的node数量决定，初始为0，每存入一个entry/exit point, id便会加一

• vertex: 该node对应的xyz carla坐标

graph.add_edge:

• n1, n2: lane segment的entry point和exit point在graph中的id

• path: 之前提到过在lane segment中位于entry/exit中间的插入点

• entry_vector, exit_vector: 以向量的形式记录了车辆如果正常驶入和离开这条lane时它的角度是怎样的。

  #注：这里只提取了关键代码，要看完整代码请查看global_route_planner.py line49-106graph.add_node(new_id,vertex=vertex)graph.add_edge(n1,n2,length=len(path)+1,path=path,entry_waypoint=entry_wp,exit_waypoint=exit_wp,entry_vector=np.array([entry_carla_vector.x,entry_carla_vector.y,entry_carla_vector.z]),exit_vector=np.array([exit_carla_vector.x,exit_carla_vector.y,exit_carla_vector.z]),net_vector=vector(entry_wp.transform.location,exit_wp.transform.location),intersection=intersection,type=RoadOption.LANEFOLLOW)

看到这里细心的小伙伴会注意到一个问题——我们虽然根据一段段lane建立起了graph, 但是这个graph有缺陷阿: 它只会顺着一个方向建立edge. 拿图2举例子，section A里的lane2会和section B里的lane2建立edge, 但是并不会和section A里的lane1建立edge, 这就会导致一个严重的问题：**全局规划出来的路线只能顺着一条道开到黑，无法进行变道甚至转弯的操作。**所以carla在建立了初步的graph之后，还加了以下代码：

self._graph.add_edge(
                self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint,
                exit_waypoint=next_waypoint, intersection=False, exit_vector=None,
                path=[], length=0, type=next_road_option, change_waypoint=next_waypoint)

这句代码要做的事情很简单，如果已存在的node里它的左边或右边有可驾驶的lane(node本质是waypoint class, waypoint里会存这个信息），那么读取它左边或右边lane里离自己最近的那个node, 与自己建立起edge. 这样一来，整个graph既有了纵向的链接，又有了横向的链接。

2.2.4 全局导航路线生成

到了这一步，我们终于建立了完整的graph, 从任意一个地方出发，我们都能找到一些列的链接和节点，从而到达目的地。完成这个的函数是紧跟着前面提到的grp.setup()函数之后，给定初始点和目的地，自动产生路线（用的是networkx 里打包的的A* 算法，这里就不细说了）。其中返回的route变量是一个装满tuple的list. tuple里含有两个东西（waypoint, road_option)

self._grp.setup()
route = self._grp.trace_route(
        start_waypoint.transform.location,
        end_waypoint.transform.location)

之前说过，networkx构建的graph的node是entry/exit point, edge是它们之间的插入点，每个相隔4.5米，总而言之，无论是node还是edge, 本质上都是一系列的Waypoint。所以tuple里的waypoint很好理解，就是让汽车朝着这些点一个个开，最后就能开到目的地。road_option是一个enum class, 主要用来表示这个waypoint是不是涉及到变道，为后面的行为规划提供额外的信息。

2.3 局部路线规划器初始化

全局路线规划好以后，agent会初始化局部路线规划器，方便后面的轨迹规划（carla里的轨迹规划严格意义上讲算不上真的轨迹规划，后面会详细讲到）。

# inside set_destination() function in behavior_agent.py
self._local_planner.set_global_plan(route_trace, clean)

这个set_global_plan()里面做了两件事： 第一件事情就是把上面global planner产生的route全部赋值给waypoints_queue（deque类型数据，carla设置的默认缓存长度为1000）。第二件事情，就是把 buffer_size数量（默认为5）的elements(tuple of waypoint and roadoption) 从waypoints_queue里弹出，存到短期缓存waypoint_buffer里（也是deque类型数据）。那么它为什么要用deque 来存waypoint, 又为什么设置两个长短不一的导航点缓存呢？

其实就如同我们人类开车看导航一样，我们不会从当前位置一直看到几千米外，我们一般只关注最多几秒后的路线。同样，waypoint_buffer里存的导航点是carla里短期跟随的路线，它每经过一个点就会把历史数据扔掉，而这种操作用deque/queue数据类型效率最高（FIFO）。当waypoint_buffer里的点都被扔出去变空之后，waypoints_queue就会再次弹出自己的五个点，送给waypoint_buffer。如此循环，随着离目的地越来越近，waypoints_queue会被一点点“掏空”，最终到达目的地时全部清空。

    def set_global_plan(self, current_plan):
        """
        Resets the waypoint queue and buffer to match the new plan. Also
        sets the global_plan flag to avoid creating more waypoints
        :param current_plan: list of (carla.Waypoint, RoadOption)
        :return:
        """

        # Reset the queue
        self._waypoints_queue.clear()
        for elem in current_plan:
            self._waypoints_queue.append(elem)
        self._target_road_option = RoadOption.LANEFOLLOW

        # and the buffer
        self._waypoint_buffer.clear()
        for _ in range(self._buffer_size):
            if self._waypoints_queue:
                self._waypoint_buffer.append(
                    self._waypoints_queue.popleft())
            else:
                break

        self._global_plan = True