(3-8-01)速度控制算法：基于Carla的十字路口车的辆控制程序

码农三叔

已于 2024-04-08 16:15:55 修改

阅读量622

点赞数 5

分类专栏：《运动控制算法--Python篇》文章标签： python 人工智能运动控制自动驾驶机器人速度控制

于 2024-04-08 14:19:17 首次发布

本文链接：https://blog.csdn.net/asd343442/article/details/137507463

版权

《运动控制算法--Python篇》专栏收录该内容

76 篇文章 28 订阅

订阅专栏

Carla是一个用于自动驾驶研究的开源模拟器，本项目的主要功能是在Carla中的控制四轮车辆的速度。本项目实现了在Carla仿真环境中控制车辆的功能，包括使用油门和刹车进行速度控制，并展示了不同速度阶段的仿真过程。通过实现车辆控制系统和提供可视化结果，该项目为研究和测试车辆控制算法提供了基础框架。

在本项目中，文件multiple_vehicle_control.py是基于CARLA仿真平台的十字路口车辆控制程序，通过定义类VehicleControl和 multiple_vehicle_control 函数，实现了对多辆车辆的纯追踪控制，包括速度控制、安全距离考虑、遵守交通灯等功能，通过CARLA提供的仿真环境模拟车辆在十字路口的行为。

实例3-10：基于Carla的车速控制系统（源码路径：codes\3\speed\Carla-Speed-Control）

文件multiple_vehicle_control.py的具体是流程如下所示。

（1）定义一个名为 VehicleControl 的类，该类用于控制CARLA仿真环境中的车辆。该类的构造函数 __init__ 接收三个参数：env 表示CARLA环境，vehicle_config 表示车辆的配置信息，delta_seconds 表示仿真步长。车辆配置信息包括车辆的唯一标识符、行驶命令、轨迹、参考速度列表等。类的属性包括PI控制器和纯追踪模型的常数、存储状态空间值和速度信息的队列、离散步长等。还包括一些用于可视化的存储速度、节流阀和参考速度的列表。

# 颜色定义，用于调试
red = carla.Color(255, 0, 0)
green = carla.Color(0, 255, 0)
blue = carla.Color(47, 210, 231)
cyan = carla.Color(0, 255, 255)
yellow = carla.Color(255, 255, 0)
orange = carla.Color(255, 162, 0)
white = carla.Color(255, 255, 255)

class VehicleControl(object):
    def __init__(self, env, vehicle_config, delta_seconds):
        '''
        初始化方法

        Parameters
        ----------
        env : CARLA_ENV
            CARLA仿真环境的自定义类
        
        vehicle_config : ConfigObj
            车辆的配置文件，由交叉口的add_vehicle函数创建。
            包含车辆的轨迹和速度参考信息。配置文件还应指示该车辆是否应该行驶。

        delta_seconds : float
            两个相邻仿真步骤之间的时间。用于创建离散控制器

        Returns
        -------
        None.
        '''
        
        self.env = env
        self.vehicle_config = vehicle_config
        self.model_uniquename = self.vehicle_config["uniquename"]
        self.command = self.vehicle_config["command"]
        self.trajectory = self.vehicle_config["trajectory"]
        self.ref_speed_list = self.vehicle_config["ref_speed_list"]
        self.obey_traffic_lights = self.vehicle_config["obey_traffic_lights"]
        self.run = self.vehicle_config["run"]
        self.safety_distance = self.vehicle_config["safety_distance"]
        self.stop_choice = self.vehicle_config["stop_choice"]
        self.penetrate_distance = self.vehicle_config["penetrate_distance"]
        self.stop_ref_point = self.vehicle_config["stop_ref_point"]
        
        # 离散步长
        self.delta_seconds = delta_seconds
        
        # PI控制器常数
        self.KI = 0.02
        self.KP = 0.5
        
        # 纯追踪模型常数
        self.k = 0.1  # 前瞻的系数
        self.Lfc = 4.0  # 前瞻距离
        self.L = self.vehicle_config["bounding_box"].x  # 车辆轴距
        
        # 控制器所需的关键存储
        self.init_values = deque(maxlen=2)  # 系统状态空间值的存储队列
        self.ref_speeds = deque(maxlen=2)  # 参考/目标速度的存储队列
        self.curr_speeds = deque(maxlen=2)  # 车辆测得的速度的存储队列
        
        # 用于可视化参考速度、节流阀和测得速度的存储
        self.speed = []
        self.throttles = []
        self.reference_speed = []
    
        # 初始化存储的初始值，假设车辆从静止开始，没有初始速度或加速度
        self.init_values.append(0) 
        self.ref_speeds.append(0)
        self.curr_speeds.append(0)
    
        self.current_ref_speed = 0
        self.index = 0
        
        # 获取车辆的PI控制器
        self._get_PI_controller()
        
        # 启用绘制车辆轨迹的调试
        self.debug_vehicle = True
        self.vehicle_pose = deque(maxlen=2)
        
        # 指示车辆是否在红绿灯处停止
        self.blocked_by_light = False

（2）函数get_vehicle_transform的功能是获取车辆在3D环境中的变换矩阵。

def get_vehicle_transform(self):
    '''
    获取车辆的变换矩阵
    '''
    transform = self.env.get_transform_3d(self.model_uniquename)
    return transform

（3）函数_get_PI_controller(self)的功能是创建离散状态空间的PI（比例-积分）控制器，通过设置比例和积分常数，生成状态空间模型，用于控制车辆速度。

def _get_PI_controller(self):

    '''

    创建一个离散状态空间PI控制器

    '''

    num_pi = [self.KP, self.KI] # PI控制器传递函数的分子(KP * s + KI)

    den_pi = [1.0, 0.01 * self.KI / self.KP] # PI控制器传递函数的分母(s + 0.01 * KI/KP)

    

    sys = control.tf(num_pi, den_pi) # 获取PI控制器的传递函数（由于分母有一个小项0.01 * KI/KP，实际上是一个滞后补偿器）

    sys = control.sample_system(sys, self.delta_seconds) # 将传递函数离散化（从连续的s域到离散的z域）

                                                        # 由于我们的仿真是离散的

    sys = control.tf2ss(sys) # 将传递函数转换为状态空间形式。

    self.sys = sys  # 为此车辆创建系统

（4）函数speed_control(self)的功能是基于参考速度和当前速度计算差值，将其作为PI控制器的输入，获取新的油门控制量，实现速度控制。

def speed_control(self):
    '''
    获取参考速度、当前（测得的）速度和初始值
    使用差值 e = ref_speeds - curr_speeds 作为PI控制器的输入，得到新的节流阀
    
    Parameters
    ----------
    self.sys : control.ss 
        状态空间控制器 
    self.ref_speeds : list of float
        我们需要的目标速度
    self.curr_speeds : list of float
        当前速度
    self.init_values : the initial_values of the system
        系统的初始值。

    Returns
    -------
    throttle : float type
        节流阀的值。

    '''
    U0 = np.array(self.ref_speeds) - np.array(self.curr_speeds)
    _, y0, x0 = control.forced_response(self.sys, U=U0, X0=self.init_values[0]) 
    # y0是下一个值，x0是状态演化，详见 https://python-control.readthedocs.io/en/0.8.3/generated/control.forced_response.html#control.forced_response 
    self.init_values.append(x0[-1])
    throttle = y0[-1]
    return throttle

（5）函数get_target_index(self, location_2d, current_forward_speed, trajectory)的功能是根据当前车辆位置、速度和轨迹，获取车辆需要导航至的目标点索引，同时判断是否到达轨迹终点。

def get_target_index(self, location_2d, current_forward_speed, trajectory):
    '''
    获取车辆导航到的目标
    
    Parameters
    ----------
    location_2d : (x,y)
        车辆的当前位置。
    current_forward_speed : float
        车辆的当前速度。
    trajectory : numpy 2d array
        插值得到的路径点。

    Returns
    -------
    ind : TYPE
        描述。
    end_trajectory : TYPE
        描述。

    '''
    distance = np.sum((trajectory - location_2d) ** 2, axis=1) ** 0.5
    ind = np.argmin(distance)
    l0 = 0.0
    Lf = self.k * current_forward_speed + self.Lfc
    while Lf > l0 and (ind + 1) < len(trajectory):
        delta_d = sum((trajectory[ind + 1] - trajectory[ind]) ** 2) ** 0.5
        l0 += delta_d
        ind += 1
    if ind >= len(trajectory) - 1:
        end_trajectory = True
    else:
        end_trajectory = False
    return ind, end_trajectory

（6）函数pure_pursuit_control的功能是根据车辆当前位置、速度、轨迹和参考速度列表，计算并返回车辆的转向角、当前参考速度、目标索引以及是否接近目标。

def pure_pursuit_control(self, vehicle_pos_2d, current_forward_speed, trajectory, ref_speed_list, prev_index):
    '''
    控制车辆的航向角
    
    Parameters
    ----------
    vehicle_pos_2d : (location_2d, yaw)
        二维车辆位置和航向的元组。
        location_2d : (x, y)，x和y均以米为单位
        yaw：偏航角 **注意** yaw以度为单位
    current_forward_speed : float
        车辆的当前速度。
    trajectory : numpy 2d array
        插值得到的路径点。
    ref_speed_list : list
        每个路径点对应的参考速度
    prev_index : int
        先前的索引

    Returns
    -------
    delta : float
        车辆的转向角。
    current_ref_speed : the reference speed
        描述。
    index : int
        目标的索引。
    end_trajectory : boolean
        是否已经足够接近目的地。

    '''
    location_2d, yaw = vehicle_pos_2d
    yaw = np.deg2rad(yaw) # 将角度转换为弧度
    index, end_trajectory = self.get_target_index(location_2d, current_forward_speed, trajectory)
    if prev_index >= index:
        index = prev_index
    if index < len(trajectory):
        tx = trajectory[index][0]
        ty = trajectory[index][1]
    else:
        tx = trajectory[-1][0]
        ty = trajectory[-1][1] 
    alpha = math.atan2(ty - location_2d[1], tx - location_2d[0]) - yaw
    if current_forward_speed < 0: #后退，在我们的情况下不应该发生
        alpha = math.pi - alpha
    Lf = self.k * current_forward_speed + self.Lfc
    delta = math.atan2(2.0 * self.L * math.sin(alpha) / Lf, 1.0)
    current_ref_speed = ref_speed_list[index]
    return delta, current_ref_speed, index, end_trajectory

未完待续

码农三叔

关注

5
点赞
踩
8

收藏

觉得还不错? 一键收藏
打赏
1
评论
(3-8-01)速度控制算法：基于Carla的十字路口车的辆控制程序

Carla是一个用于自动驾驶研究的开源模拟器，本项目的主要功能是在Carla中的控制四轮车辆的速度。本项目实现了在Carla仿真环境中控制车辆的功能，包括使用油门和刹车进行速度控制，并展示了不同速度阶段的仿真过程。通过实现车辆控制系统和提供可视化结果，该项目为研究和测试车辆控制算法提供了基础框架。
复制链接

扫一扫