Carla中实现车辆轨迹获得

pipi_ya

已于 2023-08-18 11:05:55 修改

阅读量8.3k

点赞数 9

分类专栏： Carla 文章标签： python 人工智能

于 2021-05-10 11:28:13 首次发布

本文链接：https://blog.csdn.net/pipi_ya/article/details/114501804

版权

Carla 专栏收录该内容

2 篇文章 6 订阅

订阅专栏

Carla中实现车辆循迹及车道保持（使用MPC和强化学习）

GitHub：[Carla-Env](https://github.com/its-ant-bupt/Carla-Env)
需要用到的 python api
强化学习部分
- 方法

GitHub：Carla-Env

需要用到的 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。

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

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

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

reinforcement learning
使用A3C训练网络

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

实验结果

在这里插入图片描述
表中的数值为成功率，可以看到三个方法中modular pipeline总体表现更好，RL则更差

模仿学习（Imitation Learning）

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

前言

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

建模方法：

目标： $\operatorname{minimize}_{\theta} \sum_{i} l\left(F\left(o_{i} ; \theta\right), a_{i}\right)$ ，其中F表示agent的策略，a表示专家的动作。
通常的模仿学习方法，有一个隐含的假设：专家的行为可以通过其观察来完全的解释。如果这个假设成立，那么一个很强的近似器就能学会专家的行为。但是这个假设是有问题的。专家在十字路口转弯，这个行为不能够通过十字路口的观察来解释，这实际上来自于专家的内部状态：他的意图。如果我们直接使用随机的策略来最大化专家动作的这个likelihood，那么学到的agent最后肯定就是在十字路口瞎转悠。
假设专家存在一个隐变量h，专家的动作是隐变量和观察共同导致的： $a_{i}=E\left(o_{i}, h_{i}\right)$ ，E表示专家的policy
将专家的意图暴露给之后的控制器，以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（命令条件模仿学习）的判决便是： $\operatorname{minimize}_{\theta} \sum_{i} l\left(F\left(o_{i}, c_{i} ; \theta\right), a_{i}\right)$

实现细节

在这里插入图片描述

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