diff --git a/src/autonomous_driving_car/CAS/v4.py b/src/autonomous_driving_car/CAS/v4.py new file mode 100644 index 0000000000..3fbfcf65c4 --- /dev/null +++ b/src/autonomous_driving_car/CAS/v4.py @@ -0,0 +1,338 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +CARLA 弯道自适应转向:根据弯道大小动态调整转弯幅度 +""" + +import sys +import os +import carla +import numpy as np +import math +import pygame +import traceback + +# ===================== 全局配置(动态参数)===================== +# CARLA连接 +CARLA_HOST = "localhost" +CARLA_PORT = 2000 +CARLA_TIMEOUT = 10.0 + +# 车辆配置 +VEHICLE_MODEL = "vehicle.tesla.model3" +VEHICLE_WHEELBASE = 2.9 # 特斯拉Model3轴距(米) +VEHICLE_REAR_AXLE_OFFSET = 1.45 # 后轴偏移(正数) + +# 转向控制(动态参数基准) +LOOKAHEAD_DIST_STRAIGHT = 7.0 # 直道预瞄距离 +LOOKAHEAD_DIST_CURVE = 4.0 # 急弯预瞄距离 +STEER_GAIN_STRAIGHT = 0.7 # 直道转向增益 +STEER_GAIN_CURVE = 1.0 # 急弯转向增益 +STEER_DEADZONE = 0.05 # 转向死区 +STEER_LOWPASS_ALPHA = 0.6 # 低通滤波系数 +STEER_DIR_COEFF = 1.0 # 转向方向校准 +MAX_STEER = 1.0 # 最大转向角 + +# 弯道等级划分(方向变化量阈值) +DIR_CHANGE_GENTLE = 0.03 # 缓弯阈值 +DIR_CHANGE_SHARP = 0.08 # 急弯阈值 + +# 速度控制 +BASE_SPEED = 28.0 # 直道基础速度 +CURVE_SPEED_FACTOR = 0.6 # 弯道速度系数(更明显的速度衰减) +PID_KP = 0.15 +PID_KI = 0.01 +PID_KD = 0.01 + +# 相机 +CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0)) +CAMERA_WIDTH = 800 +CAMERA_HEIGHT = 600 +CAMERA_FOV = 90 + +# ===================== 纯追踪控制器(动态适配)===================== +class AdaptivePurePursuit: + def __init__(self, wheelbase): + self.wheelbase = wheelbase + self.last_steer = 0.0 # 上一帧转向角 + self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT # 上一帧预瞄距离 + + def calculate_steer(self, vehicle_transform, target_point, dir_change): + """ + 自适应纯追踪计算(根据弯道大小调整参数) + :param vehicle_transform: 车辆变换 + :param target_point: 目标点(carla.Location) + :param dir_change: 方向变化量(弯道大小) + :return: 自适应后的转向角 + """ + # 1. 获取车辆后轴位置 + forward_vec = vehicle_transform.get_forward_vector() + rear_axle_loc = carla.Location( + x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET, + y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET, + z=vehicle_transform.location.z + ) + + # 2. 转换到车辆坐标系 + dx = target_point.x - rear_axle_loc.x + dy = target_point.y - rear_axle_loc.y + yaw = math.radians(vehicle_transform.rotation.yaw) + + dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw) + dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw) + + # 3. 动态计算转向增益(根据弯道大小线性插值) + # 方向变化量越大,增益越高 + steer_gain = np.interp( + dir_change, + [0, DIR_CHANGE_SHARP], + [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE] + ) + steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE) + + # 4. 纯追踪核心计算 + if dx_vehicle < 0.1: + steer = self.last_steer + else: + # 纯追踪公式 + 弯道大小系数(dir_change*2 放大差异) + steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * (1 + dir_change * 2), dx_vehicle ** 2 + dy_vehicle ** 2) + steer = steer_rad / math.pi + + # 应用增益和方向校准 + steer *= steer_gain * STEER_DIR_COEFF + + # 5. 应用死区 + if abs(steer) < STEER_DEADZONE: + steer = 0.0 + + # 6. 低通滤波(平滑) + steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer + + # 7. 限制范围 + steer = np.clip(steer, -MAX_STEER, MAX_STEER) + + # 8. 更新状态 + self.last_steer = steer + + return steer + + def get_adaptive_lookahead(self, dir_change): + """ + 动态获取预瞄距离(根据弯道大小线性插值) + :param dir_change: 方向变化量 + :return: 自适应预瞄距离 + """ + lookahead_dist = np.interp( + dir_change, + [0, DIR_CHANGE_SHARP], + [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE] + ) + lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT) + self.last_lookahead = lookahead_dist + return lookahead_dist + +# ===================== 速度控制器(动态调整)===================== +class SpeedController: + def __init__(self, kp, ki, kd): + self.kp = kp + self.ki = ki + self.kd = kd + self.last_error = 0.0 + self.integral = 0.0 + + def calculate(self, target_speed, current_speed): + """PID速度控制""" + error = target_speed - current_speed + + p = self.kp * error + self.integral += self.ki * error + self.integral = np.clip(self.integral, -1.0, 1.0) + i = self.integral + d = self.kd * (error - self.last_error) + self.last_error = error + + output = p + i + d + return np.clip(output, 0.0, 1.0) + +# ===================== 辅助函数:计算方向变化(替代曲率)===================== +def calculate_dir_change(current_wp): + """ + 计算相邻Waypoint的方向变化量(更精细的计算,增加采样点) + :param current_wp: 当前Waypoint + :return: 方向变化的绝对值之和,弯道等级(0=直道,1=缓弯,2=急弯) + """ + waypoints = [current_wp] + # 增加采样点到5个,更准确的判断弯道大小 + for i in range(5): + next_wps = waypoints[-1].next(1.0) + if next_wps: + waypoints.append(next_wps[0]) + else: + break + + if len(waypoints) < 4: + return 0.0, 0 + + # 计算每个相邻点的方向 + dirs = [] + for i in range(1, len(waypoints)): + wp_prev = waypoints[i-1] + wp_curr = waypoints[i] + dir_rad = math.atan2( + wp_curr.transform.location.y - wp_prev.transform.location.y, + wp_curr.transform.location.x - wp_prev.transform.location.x + ) + dirs.append(dir_rad) + + # 计算方向变化的绝对值之和(放大差异) + dir_change = 0.0 + for i in range(1, len(dirs)): + dir_change += abs(dirs[i] - dirs[i-1]) * 2 # 放大差异 + + # 划分弯道等级 + if dir_change < DIR_CHANGE_GENTLE: + curve_level = 0 # 直道 + elif dir_change < DIR_CHANGE_SHARP: + curve_level = 1 # 缓弯 + else: + curve_level = 2 # 急弯 + + return dir_change, curve_level + +# ===================== 相机管理器 ===================== +class CameraManager: + def __init__(self, world, vehicle, display): + self.world = world + self.vehicle = vehicle + self.display = display + self.camera = None + self._create_camera() + + def _create_camera(self): + bp = self.world.get_blueprint_library().find("sensor.camera.rgb") + bp.set_attribute("image_size_x", str(CAMERA_WIDTH)) + bp.set_attribute("image_size_y", str(CAMERA_HEIGHT)) + bp.set_attribute("fov", str(CAMERA_FOV)) + self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle) + self.camera.listen(self._on_image) + + def _on_image(self, image): + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))[:, :, :3] + array = array[:, :, ::-1].swapaxes(0, 1) + self.display.blit(pygame.surfarray.make_surface(array), (0, 0)) + pygame.display.flip() + + def destroy(self): + if self.camera: + self.camera.stop() + self.camera.destroy() + +# ===================== 主函数(核心逻辑)===================== +def main(): + pygame.init() + display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT)) + pygame.display.set_caption("CARLA 弯道自适应转向(最终版)") + + client = None + world = None + vehicle = None + camera_manager = None + pp_controller = None + speed_controller = None + + try: + # 1. 连接CARLA并初始化 + client = carla.Client(CARLA_HOST, CARLA_PORT) + client.set_timeout(CARLA_TIMEOUT) + world = client.get_world() + map = world.get_map() + + # 清理现有车辆 + for actor in world.get_actors().filter("vehicle.*"): + actor.destroy() + + # 生成车辆 + vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL) + spawn_points = map.get_spawn_points() + vehicle = world.spawn_actor(vehicle_bp, spawn_points[0]) + print(f"车辆生成成功:{vehicle.type_id}") + + # 初始化控制器 + pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE) + speed_controller = SpeedController(PID_KP, PID_KI, PID_KD) + camera_manager = CameraManager(world, vehicle, display) + + # 2. 主循环 + print("仿真启动,按ESC退出...") + clock = pygame.time.Clock() + running = True + + while running: + # 事件处理 + for event in pygame.event.get(): + if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE): + running = False + + # 3. 获取关键数据 + vehicle_transform = vehicle.get_transform() + vehicle_vel = vehicle.get_velocity() + current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6 # m/s → km/h + + # 获取当前Waypoint + current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True) + + # 计算方向变化量和弯道等级 + dir_change, curve_level = calculate_dir_change(current_wp) + + # 动态获取预瞄距离 + lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change) + + # 获取目标Waypoint(动态预瞄距离) + target_wps = current_wp.next(lookahead_dist) + target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location + + # 4. 计算目标速度(根据弯道等级调整) + curve_speed_factors = [1.0, 0.7, 0.4] # 直道、缓弯、急弯的速度系数 + speed_factor = curve_speed_factors[min(curve_level, 2)] + target_speed = BASE_SPEED * speed_factor + target_speed = max(10.0, target_speed) + + # 5. 计算转向角(传入方向变化量,实现自适应) + steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change) + + # 6. 计算油门/刹车 + throttle = speed_controller.calculate(target_speed, current_speed) + brake = 1.0 - throttle if current_speed > target_speed + 5 else 0.0 + + # 7. 应用控制 + control = carla.VehicleControl() + control.steer = steer + control.throttle = throttle + control.brake = brake + control.hand_brake = False + vehicle.apply_control(control) + + # 8. 打印状态(调试用,显示弯道等级和动态参数) + curve_names = ["直道", "缓弯", "急弯"] + print(f"速度:{current_speed:.1f}km/h | 目标速度:{target_speed:.1f}km/h | 弯道:{curve_names[curve_level]} | 预瞄:{lookahead_dist:.1f}m | 转向:{steer:.3f}") + + # 控制帧率 + clock.tick(30) + + except Exception as e: + print(f"错误:{e}") + traceback.print_exc() + + finally: + # 清理资源 + print("清理资源...") + if camera_manager: + camera_manager.destroy() + if vehicle: + vehicle.destroy() + pygame.quit() + print("仿真结束") + +if __name__ == "__main__": + main() \ No newline at end of file