diff --git a/src/autonomous_driving_car/CAS/v4.py b/src/autonomous_driving_car/CAS/v4.py deleted file mode 100644 index 3fbfcf65c4..0000000000 --- a/src/autonomous_driving_car/CAS/v4.py +++ /dev/null @@ -1,338 +0,0 @@ -#!/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 diff --git a/src/autonomous_driving_car/README.md b/src/autonomous_driving_car/README.md index 0bb5ae9ee2..019a9c1439 100644 --- a/src/autonomous_driving_car/README.md +++ b/src/autonomous_driving_car/README.md @@ -1,62 +1,32 @@ CARLA 模拟器和自动驾驶基础算法学习 - - -\# 无人驾驶汽车项目(基于CARLA模拟器) - -\## 项目简介 - +# 无人驾驶汽车项目(基于CARLA模拟器) +## 项目简介 本项目是基于CARLA开源仿真平台、Python和PyCharm开发的无人驾驶仿真系统,融合计算机视觉、路径规划与控制技术,实现虚拟场景中车辆的自主导航、避障与路径跟踪功能,适用于自动驾驶入门实践。 +## 核心功能 +- 路径规划:A*算法、RRT/RRT*算法实现起点到终点路径生成 +- 障碍物检测:YOLOv8+OpenCV实时识别目标,激光雷达点云聚类定位 +- 车辆控制:PID控制器实现转向、速度精准控制 +- CARLA交互:加载场景、获取传感器数据(摄像头/激光雷达等)、发送控制指令 +- 实时可视化:PyGame显示场景、车辆状态与检测结果 - -\## 核心功能 - -\- 路径规划:A\*算法、RRT/RRT\*算法实现起点到终点路径生成 - -\- 障碍物检测:YOLOv8+OpenCV实时识别目标,激光雷达点云聚类定位 - -\- 车辆控制:PID控制器实现转向、速度精准控制 - -\- CARLA交互:加载场景、获取传感器数据(摄像头/激光雷达等)、发送控制指令 - -\- 实时可视化:PyGame显示场景、车辆状态与检测结果 - - - -\## 技术栈 - +## 技术栈 | 类别 | 具体技术/工具 | - |--------------|---------------------------------------| - | 开发环境 | PyCharm Community Edition 2024+、Windows/Linux | - | 核心语言 | Python 3.8+ | - | 仿真平台 | CARLA 0.9.15/0.9.16、CARLA Python API | - | 计算机视觉 | OpenCV、NumPy、YOLOv8(Ultralytics) | - -| 路径规划 | A\*、RRT/RRT\*算法 | - +| 路径规划 | A*、RRT/RRT*算法 | | 控制理论 | PID控制器 | - | 可视化与数据 | PyGame、Matplotlib、Pandas | - - -\## 快速开始 - -1\. 克隆项目到PyCharm,创建Python 3.8+虚拟环境 - -2\. 安装依赖:`pip install -r requirements.txt`(核心依赖:carla、opencv-python、ultralytics、numpy、pygame) - -3\. 启动CARLA模拟器(运行`CarlaUE4.exe`/`CarlaUE4.sh`) - -4\. 运行主程序:`python main.py`,自动连接CARLA并自主导航 - - +## 快速开始 +1. 克隆项目到PyCharm,创建Python 3.8+虚拟环境 +2. 安装依赖:`pip install -r requirements.txt`(核心依赖:carla、opencv-python、ultralytics、numpy、pygame) +3. 启动CARLA模拟器(运行`CarlaUE4.exe`/`CarlaUE4.sh`) +4. 运行主程序:`python main.py`,自动连接CARLA并启动自主导航 \## 项目结构 @@ -65,14 +35,9 @@ autonomous_driving_car/ ├── MCP/ # 主控制与模块集成:包含感知(检测)、规划(路径)、控制(PID)的核心逻辑 ├── main.py # 项目入口:启动CARLA连接、调用MCP与FPV模块 └── README.md # 项目说明文档 +``` - - -\## 常见问题 - -\- CARLA连接失败:确保模拟器已启动,Python API版本与CARLA一致 - -\- 检测速度慢:使用YOLOv8n轻量模型,或启用GPU加速 - -\- 控制不稳定:调整PID参数或增加路径平滑处理 - +## 常见问题 +- CARLA连接失败:确保模拟器已启动,Python API版本与CARLA一致 +- 检测速度慢:使用YOLOv8n轻量模型,或启用GPU加速 +- 控制不稳定:调整PID参数或增加路径平滑处理 \ No newline at end of file diff --git a/src/autonomous_driving_car/ros/CMakeLists.txt b/src/autonomous_driving_car/ros/CMakeLists.txt index 1627cb3d71..6b3b73737f 100644 --- a/src/autonomous_driving_car/ros/CMakeLists.txt +++ b/src/autonomous_driving_car/ros/CMakeLists.txt @@ -22,9 +22,9 @@ find_package(catkin REQUIRED COMPONENTS ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html # catkin_python_setup() -################################################ +############################################### ## Declare ROS messages, services and actions ## -################################################ +############################################### ## To declare and build messages, services or actions from within this ## package, follow these steps: