Carla中实现车辆轨迹获得

需要用到的 python api

carla.map

通过carla.map可以获得当前地图的信息，包括道路的信息和航向点的管理的类函数。

methods

• generate_waypoints
  参数 distance
  输出：通过给定的distance生成间隔为此大小的路径点列表(list(carla.waypoint))
  每一个点包括Location和Rotation
• get_spawn_points
  最常使用的函数，获得一个有效的坐标点不会与道路和其他车辆产生冲突。
• get_topology
  生成全局的拓扑结构，用于做A*路径规划
  输出是连接道路的列表list(tuple(carla.waypoint, carla.waypoint))
  获得结果后可以使用networkx工具建立图结构方便分析之后的最短路径，其中使用road-id,section-id,lane-id的模式固定保存节点，同时也可以在其中添加距离等属性。

        # 创建一个空图
        G = nx.Graph()
        for i in range(len(topology)):
            waypoint1 = topology[i][0]
            waypoint2 = topology[i][1]
            waypoint1_info = "%s-%s-%s" % (waypoint1.road_id, waypoint1.section_id, waypoint1.lane_id)
            waypoint2_info = "%s-%s-%s" % (waypoint2.road_id, waypoint2.section_id, waypoint2.lane_id)
            G.add_edge(waypoint1_info, waypoint2_info)
        # nx.draw_networkx(G)
        # plt.show()

建立有向图带权重

        G = nx.DiGraph()
        for i in range(len(topology)):
            waypoint1 = topology[i][0]
            waypoint2 = topology[i][1]
            waypoint1_info = "%s-%s-%s" % (waypoint1.road_id, waypoint1.section_id, waypoint1.lane_id)
            waypoint2_info = "%s-%s-%s" % (waypoint2.road_id, waypoint2.section_id, waypoint2.lane_id)
            distance = math.sqrt(math.pow((waypoint2.transform.location.x - waypoint1.transform.location.x), 2) +
                                 math.pow((waypoint2.transform.location.y - waypoint1.transform.location.y), 2))
            # G.add_edge(waypoint1_info, waypoint2_info)
            G.add_weighted_edges_from([(waypoint1_info, waypoint2_info, distance)])

之后使用A*算法即可得到从起点到终点的路径。

        path = nx.astar_path(G, "8-0--2", "483-0-4")

一个简单的方法实现路径规划

通过地图给出的拓扑结构，以及networkx的最短路径规划功能可以实现一个简单的路径规划，给定两点的Transform，上述所获得的拓扑图G，键值对信息{key（道路名称string）：velue（航向点carla.Waypoint）}，以及地图信息即可获得从起始点到终点的路径规划。注：没有验证是最短路径规划

# 输入起始点和终点的transform，拓扑图，航向点键值对，以及地图，输出从起始点到终点的路径规划
def generate_routelist(start, end, G, waypoints_info,  _map):
    start_waypoint = _map.get_waypoint(start.location)
    start_info = "%s-%s-%s" % (start_waypoint.road_id, start_waypoint.section_id, start_waypoint.lane_id)
    end_waypoint = _map.get_waypoint(end.location)
    end_info = "%s-%s-%s" % (end_waypoint.road_id, end_waypoint.section_id, end_waypoint.lane_id)
    path = nx.astar_path(G, start_info, end_info)
    print(path)
    # print(path)
    RouteList = []
    list_to_end = start_waypoint.next_until_lane_end(0.25)
    for j in range(len(list_to_end)):
        temp_waypoint = list_to_end[j]
        temp_x = temp_waypoint.transform.location.x
        temp_y = temp_waypoint.transform.location.y
        RouteList.append([temp_x, temp_y])
    for i in range(1, len(path)):
        _waypoint = waypoints_info[path[i]]
        list_to_end = _waypoint.next_until_lane_end(0.25)
        for j in range(len(list_to_end)):
            temp_waypoint = list_to_end[j]
            temp_x = temp_waypoint.transform.location.x
            temp_y = temp_waypoint.transform.location.y
            RouteList.append([temp_x, temp_y])
    return RouteList

• get_waypoint
  输入：location
  输出：根据输入的位置返回此位置所处最近的waypoint

carla.Waypoint

carla中的航向点，为一个3D的坐标，其中包括carla.Transform具有表示车帘位置的Location信息和表示车辆方向的Rotation信息，同时存储该点与其有关车道的道路信息。

methods

• next
  输出具当前点distance距离的下一个waypoint
• next_until_lane_end
  输出该waypoint所在道路上从该点到终点的waypoint列表

carla.DebugHelper

十分方便的一个工具包，里面包括了在地图中描点，画箭头、线段，方框的函数
在获得世界信息后使用

debug = world.debug

即可得到实例化类型

carla.World

通过此类，可以获得一个世界对象，world中包括我们要使用的carla.map和carla.DebugHelper。

此外我们可以对该对象进行设置，包括同步或者异步的属性，通过carla.WorldSettings对象可以对world对象进行属性设置。

如果我们要在carla中使用强化学习，那么将世界设置为同步模式是一个十分方便的设置，在同步模式中client与sever的通信使用tick来校准，因此整个过程便成为收集一次world场景中的信息做一次操作，而在此操作的过程中world不会有变化。

同步模式中需要使用carla.World.tick() 向服务器发送确认信息，并且从服务器返回一个新的帧的ID信息，通过此ID信息我们可以对获得信息进行校准。

在执行world.tick()时会自动调用world.on_tick()函数，world.on_tick()的参数包括callback，在初始化时可以将回调函数传入，在每次tick()时便会自动执行回调函数中的操作，on_tick()将一个carla.WorldSnapshot 传入此回调函数。通过回调函数我们可以获得此帧下世界的具体信息，更新所有的参数。例如下面的一个carla官方给的类函数

class CarlaSyncMode(object):
    def __init__(self, world, *sensors, **kwargs):
        self.world = world
        self.sensors = sensors
        self.frame = None
        self.delta_seconds = 1.0 / kwargs.get('fps', 20)
        self._queues = []
        self._settings = None

    def __enter__(self):
        self._settings = self.world.get_settings()
        self.frame = self.world.apply_settings(carla.WorldSettings(
            no_rendering_mode=False,
            synchronous_mode=True,
            fixed_delta_seconds=self.delta_seconds
        ))

        def make_queue(register_event):
            q = queue.Queue()
            register_event(q.put)
            # register_event(push_snapshot)
            self._queues.append(q)

        make_queue(self.world.on_tick)
        for sensor in self.sensors:
            make_queue(sensor.listen)
        return self

    def tick(self, timeout):
        self.frame = self.world.tick()
        data = [self._retrieve_data(q, timeout) for q in self._queues]
        assert all(x.frame == self.frame for x in data)
        return data

    def __exit__(self, *args, **kwargs):
        self.world.apply_settings(self._settings)

    def _retrieve_data(self, sensor_queue, timeout):
        while True:
            data = sensor_queue.get(timeout=timeout)
            if data.frame == self.frame:
                return data

当在程序的某一处使用同步模式时便会执行__enter__中的初始化，将world.on_tick()作为事件传入make_queue中，申请一个队列q，将q.put作为回调函数放入on_tick()中，以后每次执行tick()都会执行该操作。

在该类函数的tick()函数中，每次执行world.tick()函数返回的帧ID都被保存在类变量self.frame中，并且以此从队列中取出carla.WorldSnapshot这是该某一帧下这个世界中所有信息的快照即保存此时刻所有物体的状态。之后通过_retrieve_data中判断该数据的frame是否和此时刻的frame相同，并将此时刻的信息返回。通过返回的信息我们可以获得车辆的当前状态即其他参与者的信息。

强化学习部分

方法

自动驾驶综述

参考链接:知乎-暨华-CARLA: An Open Urban Driving Simulator
参考论文 > CARLA: An Open Urban Driving Simulator
链接: arxiv

此文章中使用CARLA研究了自动驾驶的三种方法，第一种是经典的modular pipeline，第二种是imitation learning, 第三种是reinforcement learning。

• modular pipeline
  文章使用modular pipeline将自动驾驶分为了三个子系统：perception，planning，control。

1. 感知模块使用semantic segmentation，具体使用RefineNet来估计车道、边缘、其他物体的状态同时有一个模型用来判断是否到达了路口。
2. 规划模块利用导航数据生成一系列最近的waypoints，目的是保证在到达目的地时保证避免碰撞。规划模块基于一个状态机模型，包含五种状态：道路跟随、左转弯、右转弯、十字路口直行、紧急停车。
3. 控制模块使用PID，根据当前位置、速度和一个waypoint列表输出方向、油门、刹车。

• imitation learning
  此方法是使用高级别的command生成一系列数据，具体的command有（1）跟随当前车道，（2）直行穿越十字路口，（3）路口向左转弯，（4）路口向右转弯

1. 输入的数据为前置摄像头的数据，在采样时给input添加一点噪音，采用dropout和data augmentation方法。
2. 网络构成：处理图像=>处理标量=>将感知与测量信息融合=>输出控制

• reinforcement learning
  使用A3C训练网络

1. 输入：最近两帧图片，以及一个测量向量包括speed， distance to goal, damage from collision, current high-level command, one-hot encoding。一个CNN和一个MLP分别处理两种数据。
2. 终止条件：碰撞、到达终点或者到达最长时间。
3. 奖励：speed的增值，与终点的距离，碰撞因子，在人行道的时间，反向行驶的时间。

实验结果

表中的数值为成功率，可以看到三个方法中modular pipeline总体表现更好，RL则更差

模仿学习（Imitation Learning）

参考链接:知乎-暨华-End-to-end Driving via Conditional IL
参考论文 > End-to-end Driving via Conditional Imitation Learning
链接: arxiv

• 前言

1. imitation learning假设了最优的动作可以通过感知的输入直接得到。但是直接通过感知数据会引入ambiguity，最终输出的这些动作不一定是人类真的想要的。
2. 相比起传统的模仿学习算法，直接输入观察输出控制信号，本方法model不仅拿到了专家的行为和观察，还拿到了专家的意图(intention)。在测试的时候则可以通过planner或者乘客来提供high-level的command信号。这样的话，本方法就把决策模块和控制模块区分开来，使得神经网络就能够只专注于底层的控制(油门多少、方向盘转多少)，而不考虑高层的决策问题了(前面路口要左转还是右转)。

• 建模方法：

1. 目标：$${\mathrm{minimize}}_{\theta }\sum _{i}l\left(F\left(o_i;\theta \right),a_i\right)$$，其中F表示agent的策略，a表示专家的动作。
2. 通常的模仿学习方法，有一个隐含的假设：专家的行为可以通过其观察来完全的解释。如果这个假设成立，那么一个很强的近似器就能学会专家的行为。但是这个假设是有问题的。专家在十字路口转弯，这个行为不能够通过十字路口的观察来解释，这实际上来自于专家的内部状态：他的意图。如果我们直接使用随机的策略来最大化专家动作的这个likelihood，那么学到的agent最后肯定就是在十字路口瞎转悠。
3. 假设专家存在一个隐变量h，专家的动作是隐变量和观察共同导致的：$$a_i=E\left(o_i,h_i\right)$$，E表示专家的policy
4. 将专家的意图暴露给之后的控制器，以command的形式：c = c(h),若意图是左转，则command就是打左转灯，训练时专家负责提供command，测试时导航或乘客提供command，因此数据集变为：$$\mathcal{D}={\left\{\left(o_i,c_i,a_i\right)\right\}}_{i=1}^N$$，此时command-conditional imitation learning（命令条件模仿学习）的判决便是：$${\mathrm{minimize}}_{\theta }\sum _{i}l\left(F\left(o_i,c_i;\theta \right),a_i\right)$$

• 实现细节

1. command c是一个categorical variable（分类变量），是一个one-hot vector
2. 网络以图像和测量作为输入，以两个连续值steering，acceleration作为输出。
3. 文章使用了两种方式来输入command，第一种为command input，将command c和input一起输入可以支持连续和离散的任意维度的command，第二种为branched， command为离散的，每一个可选的command对应一个branch网路，command c作为开关，选择最合适的brancj作为控制器输入。
4. 损失函数 ：$$l\left(a,a_{gt}\right)={\Vert s-s_{gt}\Vert }^2+{\lambda }_a{\Vert a-a_{gt}\Vert }^2$$，s和a分别表示方向盘和加速度
5. 使用三个前置摄像机
6. 给控制信号添加噪音，之后让其演示如何从这些噪音中恢复，模拟在理想的轨迹上渐渐偏移的现象。噪音大致为一个三角波，通过记录专家数据中对与噪音的反应作为数据

• 实验

1. 设置
   在训练时使用2小时人类驾驶数据，其中12分钟是加入噪音的
2. 使用标准模仿学习和goal-conditional imitation learning 作为 baselines

• 结果
  

1. 首先看到goal-condition的baseline的学习率好一点，但是车辆学会了猛地出界然后抄近路！看到两次交通违章之间的距离要比纯IL小得多。
2. 用branched的网络结构比command input的网络结构要好很多。
3. 使用大网络有非常重大的影响。小的网络无法学会perceptuomotor mapping。

4. 作者在居民区里进行了数据采样。在测试的时候，允许agent错过一次十字路口，不计算missed。上图是测试结果。可以看到不使用data augmentation的话，训练结果非常糟糕。作者在没见过的环境上进行了泛化性测试。效果不错。