使用CARLA模拟器实现DQN自动驾驶（三）导航系统

CARLA中有一系列封装好的自动驾驶导航函数库，全部在server的PythonAPI/carla/agents/navigation包中。使用时，可将agents包复制在python文件的同一目录内，方便导入。

navigation包中的文件分为两类： planning and control 和 agent behaviors。从字面意义上看，第一类是路线规划和控制，第二类是自动驾驶agent行为偏好设置。

文档解释：

Planning and control

• controller.py: Combines longitudinal and lateral PID controllers into a single class, VehiclePIDController, used for low-level control of vehicles from the client side of CARLA.
• global_route_planner.py: Gets detailed topology from the CARLA server to build a graph representation of the world map, providing waypoint and road option information for the Local Planner.
• local_planner.py: Follows waypoints based on control inputs from the VehiclePIDController. Waypoints can either be provided by the Global Route Planner or be calculated dynamically, choosing random paths at junctions, similar to the Traffic Manager.

Agent behaviors

• basic_agent.py: Contains an agent base class that implements a Basic Agent that roams around the map or reaches a target destination in the shortest distance possible, avoiding other vehicles, responding to traffic lights but ignoring stop signs.
• behavior_agent.py: Contains a class that implements a more complex Behavior Agent that can reach a target destination in the shortest distance possible, following traffic lights, signs, and speed limits while tailgating other vehicles. There are three predefined types that condition how the agent behaves.
• behavior_types.py: Contains the parameters for the behavior types that condition the Behavior Agent; Cautious, Normal, and Aggressive.

CARLA中使用的车辆控制器是PID Controller，由比例单元(P)，积分单元(I)和微分单元(D)组成。透过Kp，Ki和Kd三个参数的设定来实现控制。应用在汽车控制中，分为横向参数(lateral arguments)和纵向参数(longitudinal arguments)。纵向参数负责控制汽车的油门量/刹车量来调整汽车行进的速度，横向参数负责控制汽车的方向盘角度，使得汽车可以一直朝着路径规划好的方向行驶。具体的数学原理和函数定义可以查看controller.py文件中的VehiclePIDController()类。（也可以看我另外两篇文章中关于PID的原理介绍和代码实现，分为速度跟踪和航向控制）

除此之外，CARLA中的路线规划也有集成好的函数库，即global_route_planner.py，其中的GlobalRoutePlanner()类可以直接调取当前地图的地理位置信息，将需要规划的路径的起点和终点坐标两个参数输入，即可输出一条规划好的行车路径，以距起点由近及远的一连串坐标点来表示。

有了合理的行车路径之后，对执行自动驾驶的agent行为偏好还可以有进一步设置，比如“谨慎”，“正常”，“激进”的不同类型，可以模拟不同种类驾驶员习惯的最大行驶速度、巡航速度、刹车反应等驾车行为偏好。

当前为了测验导航系统，先将驾驶环境设置地较为简单，大街上除了测试车辆，无行人，无其他车辆。我分别用“自动控制”和“手动控制”两种方式对导航系统进行了测试。自动控制就是直接调用basic_agent包，由PID Controller对汽车进行控制。手动控制是只从后台调取地图信息生成路径，再根据下一个路径点的位置和距离，手动设置汽车的油门\刹车\转向等参数（简易版的PID Controller）。

无论哪种控制方式，对carla环境的搭建都是一样的，需要生成一个地图，在地图中实例化一辆小车，并设置一个目的地（target_transform）。

import abc
import glob
import os
import sys
from types import LambdaType
from collections import deque
from collections import namedtuple

try:
    sys.path.append(glob.glob('.../WindowsNoEditor/PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass

import carla
import random 
import time
import numpy as np

client = carla.Client('localhost',2000)
client.set_timeout(10.0)
world = client.get_world()
blueprint_library = world.get_blueprint_library()
model_3 = blueprint_library.filter('model3')[0]

actor_list = []
transform = world.get_map().get_spawn_points()[100] #spwan_points共265个点，选第一个点作为初始化小车的位置
#random.choice(self.world.get_map().get_spawn_points())
vehicle = world.spawn_actor(model_3 , transform)

actor_list.append(vehicle)   

target_transform = world.get_map().get_spawn_points()[110]

如图所示，小车的初始位置在蓝圈，目的地的位置是红叉，两者的直线距离是292m左右，可以按照绿色虚线的导航行驶。

 一、自动控制

我选择了basic_agent测试（和其他偏好的agent原理都是一样的），需要导入BasicAgent类，将汽车作为参数，实例化一个agent。

from agents.navigation.basic_agent import BasicAgent
agent = BasicAgent(vehicle)

之后需要设置导航终点的坐标，须以[x,y,z]的列表形式输入函数。

destination = [target_transform.location.x,target_transform.location.y,target_transform.location.z]
agent.set_destination(destination)

设置好后即可开始模拟，通过while循环可以使小车慢慢靠近目的地，直到到达结束循环。（规划的路径不止一条，汽车可能会在十字路口处随机选择可行的路径）

while True:
    if agent.done():
        print("The target has been reached, stopping the simulation")
        break
    control=agent.run_step()
    vehicle.apply_control(control)

for agent in actor_list:    
    agent.destroy()
print('done')

这种方式非常简单便捷，使用现有的集成函数，可以保持汽车一直行驶在车道的正中间，不随意变道；前方遇到红绿灯也会自动识别，不违反交规；如果检测到周围有其他车，还会主动避让。但正是因为这种方式直接从地图底层信息读取的数据太多，甚至达到了“万物互联”的高度，与真实环境的条件相差甚远，很难进一步向现实应用推广了（目前的真实地理因素不可能从环境底层代码获取到红绿灯和周围汽车的信息）。因此我考虑了另一种手动控制的方式，即只能从地图信息中读取到规划的路径点（现有的手机地图导航技术即可完成），其他的环境因素还是需要通过传感器来获取，再根据传感器获取的数据来确定驾驶的参数。

二、手动控制

在简单的测试环境下（暂时不考虑移动障碍物或信号灯），可以将汽车的巡航速度设置成一个恒定值。寻路的重点在于，如何根据汽车自身的坐标和方向，及目标点的坐标，计算出转向的角度。对于路线在海拔上的高度变化，汽车无法通过方向盘控制，所以目前只考虑平面上的移动（xOy坐标系）。 

（carla地图是左手系不是右手系让我也很疑惑）

由图中所示，汽车的朝向

从汽车到目标地点的方向向量

两向量之间的夹角θ可以用夹角公式来计算

但是这样计算的θ是没有正负的，θ的取值范围是[0,pi],这时候可以通过判断两向量的叉乘的符号来确定向量夹角的正负。

当叉乘结果大于0时，θ大于0，表示目标地点在汽车当前方向的右侧，反之，表示目标地点在汽车当前方向的左侧。θ的取值范围变成了[-pi,pi]。

但控制方向的参数steer的范围是[-1,1]（代表方向盘从最左转到最右），如果将θ值直接用作方向盘的旋转量，当目标地点的角度超过1弧度（57°），方向盘就会打死。除此之外，在方向角有细微变动的时候，汽车的方向盘也会跟随变化，容易导致汽车左右来回摇摆。因此，考虑采用量化的方式，完成方向角到方向盘旋转量的转化。

（阈值我根据目前的测试结果设置的一个较为效果较好的值，可以根据之后测试效果再调整）

用代码实现时，需要导入两个类，其中GlobalRoutePlannerDAO类是数据访问层，用于从GlobalRoutePlanner实例中获取数据

from agents.navigation.global_route_planner import GlobalRoutePlanner
from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO

对二者进行实例化，需要将当前地图作为参数输入，再对路径规划的实例进行初始化。

env_map = world.get_map()
dao = GlobalRoutePlannerDAO(env_map,3)#3表示每隔3m采样一个路径点，路径点越密集，路径的精度越高
grp = GlobalRoutePlanner(dao)
grp.setup()

将方向角转化用代码表示：

def get_angle(angle):
    if angle>1.39:#79.64°
        return 1
    elif angle<-1.39:
        return -1
    elif angle>1:#57.3°
        return 0.5
    elif angle<-1:
        return -0.5
    elif angle>0.2:#11.46°
        return 0.3
    elif angle<-0.2:
        return -0.3
    else:
        return 0

开始进行模拟，由于路径规划出的最后路径点，与目的地的坐标可能会有一些差异，接近目的地时，可能会重新规划路线。所以设置当路径规划的路径点少于3个时，不再考虑路径点，而直接朝向终点的坐标开。每次汽车做出一个action等待0.2秒，当距离目的地的坐标小于5米时，可以认为已经到达终点，汽车停止驾驶，结束循环。

while True:
    route_way_point =grp.trace_route(vehicle.get_transform().location,target_transform.location)

    if len(route_way_point)>3:
        temp_target = route_way_point[1][0]#
        target_mat=temp_target.transform.get_matrix()
    else:
        temp_target = target_transform
        target_mat=temp_target.get_matrix()
    target_dis = target_transform.location.distance(vehicle.get_location())
    car_mat=vehicle.get_transform().get_matrix()    
    car_dir=np.array([car_mat[0][0],car_mat[1][0]])
    s_dir = np.array([target_mat[0][3]-car_mat[0][3],target_mat[1][3]-car_mat[1][3]])

    cos_theta=np.dot(car_dir,s_dir)/(np.linalg.norm(car_dir)*np.linalg.norm(s_dir))

    left_right = abs(np.cross(car_dir,s_dir))/np.cross(car_dir,s_dir)
    angle = np.arccos(cos_theta)*left_right
    vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=get_angle(angle), brake=0.0, hand_brake=False, reverse=False))
    time.sleep(0.2)
    v = vehicle.get_velocity()
    kmh = int(3.6 * np.sqrt(v.x**2 + v.y**2 + v.z**2))
    print('v:',kmh,'kmh\t','left_distance:',int(target_dis),'m')
    if target_dis<5:
        vehicle.apply_control(carla.VehicleControl(throttle=0.0, steer=0, brake=1, hand_brake=False, reverse=False))
        print('arrive target location!')
        break

for agent in actor_list:
    agent.destroy()
print('done')