From 5fd1d8dd337e91583d7fd9bce150d01cded24230 Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 4 Nov 2025 08:31:43 +0800 Subject: [PATCH 01/25] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 96c99b37c8..34edee15be 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -13,7 +13,6 @@ settings.synchronous_mode = True # 同步模式，便于控制 world.apply_settings(settings) -# 获取地图 spawn 点 spawn_points = world.get_map().get_spawn_points() if not spawn_points: raise Exception("No spawn points available") @@ -102,7 +101,7 @@ def avoid_obstacle(): # 控制参数 -throttle = 0.4 # 油门 +throttle = 0.5 # 更改油门 steer = 0.0 # 转向角 try: From 28372809ae6ec1f302261ee7061cb36b4cf77c19 Mon Sep 17 00:00:00 2001 From: chen Date: Mon, 10 Nov 2025 10:24:02 +0800 Subject: [PATCH 02/25] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=B0=8F=E8=BD=A6?= =?UTF-8?q?=E8=AF=86=E5=88=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 327 +++++++++++++++++++++++------- 1 file changed, 257 insertions(+), 70 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 34edee15be..7a385b2c71 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -2,148 +2,335 @@ import time import numpy as np import cv2 +import math # -------------------------- # 1. 初始化CARLA连接和环境 # -------------------------- -client = carla.Client('localhost', 2000) # 连接CARLA服务器 +client = carla.Client('localhost', 2000) client.set_timeout(10.0) -world = client.load_world('Town01') # 加载简单地图 +world = client.load_world('Town01') settings = world.get_settings() -settings.synchronous_mode = True # 同步模式，便于控制 +settings.synchronous_mode = True +settings.fixed_delta_seconds = 0.05 # 固定时间步长 world.apply_settings(settings) +# 设置天气 +weather = carla.WeatherParameters( + cloudiness=30.0, + precipitation=0.0, + sun_altitude_angle=70.0 +) +world.set_weather(weather) + +# 获取出生点 spawn_points = world.get_map().get_spawn_points() if not spawn_points: raise Exception("No spawn points available") -spawn_point = spawn_points[0] # 选择第一个 spawn 点 +spawn_point = spawn_points[0] # -------------------------- -# 2. 生成机器人载体（车辆） +# 2. 生成车辆和障碍物 # -------------------------- blueprint_library = world.get_blueprint_library() -vehicle_bp = blueprint_library.find('vehicle.tesla.model3') # 选择特斯拉模型作为机器人载体 + +# 主车辆（红色） +vehicle_bp = blueprint_library.find('vehicle.tesla.model3') +vehicle_bp.set_attribute('color', '255,0,0') # 红色主车辆 vehicle = world.spawn_actor(vehicle_bp, spawn_point) -vehicle.set_autopilot(False) # 关闭自动驾驶，手动控制 +vehicle.set_autopilot(False) + +# 生成随机障碍物车辆 +for i in range(6): + if i >= len(spawn_points): + break + other_vehicles = blueprint_library.filter('vehicle.*') + other_vehicle_bp = np.random.choice(other_vehicles) + spawn_idx = (i + 8) % len(spawn_points) # 分散的出生点 + other_vehicle = world.try_spawn_actor(other_vehicle_bp, spawn_points[spawn_idx]) + if other_vehicle: + other_vehicle.set_autopilot(True) # -------------------------- -# 3. 配置多模态传感器 +# 3. 配置传感器（含第三视角摄像头） # -------------------------- -# 3.1 摄像头（视觉模态） -camera_bp = blueprint_library.find('sensor.camera.rgb') -camera_bp.set_attribute('image_size_x', '640') -camera_bp.set_attribute('image_size_y', '480') -camera_bp.set_attribute('fov', '90') -# 摄像头安装位置（车辆前方） -camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) - -# 3.2 激光雷达（LiDAR，距离感知模态） +# 3.1 前视摄像头（第一视角） +front_camera_bp = blueprint_library.find('sensor.camera.rgb') +front_camera_bp.set_attribute('image_size_x', '640') +front_camera_bp.set_attribute('image_size_y', '480') +front_camera_bp.set_attribute('fov', '90') +front_camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) +front_camera = world.spawn_actor(front_camera_bp, front_camera_transform, attach_to=vehicle) + +# 3.2 第三视角摄像头（跟随车辆） +third_camera_bp = blueprint_library.find('sensor.camera.rgb') +third_camera_bp.set_attribute('image_size_x', '640') +third_camera_bp.set_attribute('image_size_y', '480') +third_camera_bp.set_attribute('fov', '110') # 更宽的视角 +# 安装在车辆后方上方，提供良好的第三视角 +third_camera_transform = carla.Transform( + carla.Location(x=-5.0, y=0.0, z=3.0), # 位置：车后5米，高3米 + carla.Rotation(pitch=-15.0) # 角度：向下倾斜15度 +) +third_camera = world.spawn_actor(third_camera_bp, third_camera_transform, attach_to=vehicle) + +# 3.3 激光雷达 lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') -lidar_bp.set_attribute('channels', '32') # 32线激光雷达 -lidar_bp.set_attribute('range', '50') # 最大探测距离50米 +lidar_bp.set_attribute('channels', '32') +lidar_bp.set_attribute('range', '50') lidar_bp.set_attribute('points_per_second', '100000') -# LiDAR安装位置（车辆顶部） +lidar_bp.set_attribute('rotation_frequency', '10') lidar_transform = carla.Transform(carla.Location(x=0.0, z=2.5)) lidar = world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle) # -------------------------- -# 4. 传感器数据处理回调 +# 4. 传感器数据处理 # -------------------------- -# 存储传感器数据的变量 -camera_image = None +# 存储传感器数据 +front_image = None +third_image = None lidar_data = None +lidar_img = None -# 摄像头数据回调（保存RGB图像） -def camera_callback(image): - global camera_image +def front_camera_callback(image): + global front_image array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) # RGBA格式 - camera_image = array[:, :, :3] # 转为RGB + array = np.reshape(array, (image.height, image.width, 4)) + front_image = array[:, :, :3] + + +def third_camera_callback(image): + global third_image + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + third_image = array[:, :, :3] -# LiDAR数据回调（检测前方障碍物） def lidar_callback(point_cloud): - global lidar_data - # 将点云数据转为numpy数组（x, y, z, 反射率） + global lidar_data, lidar_img + # 处理点云数据 data = np.frombuffer(point_cloud.raw_data, dtype=np.dtype('f4')) data = np.reshape(data, (int(data.shape[0] / 4), 4)) lidar_data = data + # 生成LiDAR可视化图像（鸟瞰图） + lidar_img = np.zeros((400, 400, 3), dtype=np.uint8) + center_x, center_y = 200, 200 + scale = 8 # 缩放因子 + + for point in data: + x, y = point[0], point[1] + if 0 < x < 50 and -25 < y < 25: # 扩大检测范围 + px = int(center_x - y * scale) + py = int(center_y - x * scale) + if 0 <= px < 400 and 0 <= py < 400: + # 距离越近颜色越红 + color_intensity = min(1.0, x / 50.0) + color = ( + int(255 * (1 - color_intensity)), + int(255 * color_intensity), + 0 + ) + cv2.circle(lidar_img, (px, py), 1, color, -1) + + # 绘制车辆位置和方向 + cv2.circle(lidar_img, (center_x, center_y), 5, (0, 0, 255), -1) + cv2.line(lidar_img, (center_x, center_y), (center_x, center_y - 30), (0, 0, 255), 2) + # 绑定回调函数 -camera.listen(lambda data: camera_callback(data)) +front_camera.listen(lambda data: front_camera_callback(data)) +third_camera.listen(lambda data: third_camera_callback(data)) lidar.listen(lambda data: lidar_callback(data)) # -------------------------- -# 5. 多模态导航控制逻辑 +# 5. 优化的避障控制逻辑 # -------------------------- -def avoid_obstacle(): - """基于LiDAR数据判断是否需要避障""" +def detect_obstacles(): + """检测障碍物并分析左右安全区域""" if lidar_data is None: - return False, 0.0 # 无数据时不避障 + return False, 0.0, 0.0, 0.0, 0.0, 0.0 - # 筛选车辆前方±30度范围内的点云（x正方向为前方） - front_points = lidar_data[ - (lidar_data[:, 1] > -5) & # y > -5（左侧边界） - (lidar_data[:, 1] < 5) & # y < 5（右侧边界） - (lidar_data[:, 0] > 0) # x > 0（前方） + # 分区域检测：近距离(0-10m)、中距离(10-20m)、远距离(20-30m) + # 近距离检测范围更窄，中远距离更宽，符合驾驶习惯 + near_points = lidar_data[ + (lidar_data[:, 0] > 0) & (lidar_data[:, 0] < 10) & # 近距离 + (lidar_data[:, 1] > -3) & (lidar_data[:, 1] < 3) # 窄范围 ] + mid_points = lidar_data[ + (lidar_data[:, 0] >= 10) & (lidar_data[:, 0] < 20) & # 中距离 + (lidar_data[:, 1] > -6) & (lidar_data[:, 1] < 6) # 中等范围 + ] + + far_points = lidar_data[ + (lidar_data[:, 0] >= 20) & (lidar_data[:, 0] < 30) & # 远距离 + (lidar_data[:, 1] > -10) & (lidar_data[:, 1] < 10) # 宽范围 + ] + + # 合并所有检测点 + front_points = np.vstack((near_points, mid_points, far_points)) if len(near_points) > 0 or len( + mid_points) > 0 or len(far_points) > 0 else np.array([]) + if len(front_points) == 0: - return False, 0.0 # 无障碍物 + return False, 0.0, 0.0, 0.0, 0.0, 0.0 + + # 计算最近障碍物距离 + min_distance = np.min(front_points[:, 0]) + + # 区分左右障碍物 + left_points = front_points[front_points[:, 1] < 0] + right_points = front_points[front_points[:, 1] > 0] - # 计算前方最近障碍物距离 - min_distance = np.min(front_points[:, 0]) # x坐标即距离 - return min_distance < 10.0, min_distance # 距离小于10米时需要避障 + # 计算左右最近距离 + left_min = np.min(left_points[:, 0]) if len(left_points) > 0 else float('inf') + right_min = np.min(right_points[:, 0]) if len(right_points) > 0 else float('inf') + # 计算左右安全区域大小（障碍物较少的区域） + left_free = np.sum(left_points[:, 0] > 15) if len(left_points) > 0 else 1000 + right_free = np.sum(right_points[:, 0] > 15) if len(right_points) > 0 else 1000 -# 控制参数 -throttle = 0.5 # 更改油门 -steer = 0.0 # 转向角 + return min_distance < 20.0, min_distance, left_min, right_min, left_free, right_free + + +# 控制状态变量 +throttle = 0.5 +steer = 0.0 +avoid_state = 0 # 0: 正常, 1: 右避障, 2: 左避障, 3: 回正 +avoid_timer = 0 +recovery_timer = 0 # 避障后回正计时器 try: - print("多模态导航系统启动，按Ctrl+C停止...") + print("多模态导航系统启动（带第三视角）") + print("控制键: q-退出, w-加速, s-减速, a-左转向, d-右转向, r-重置方向") + while True: - # 刷新世界状态 world.tick() - # 检查障碍物 - need_avoid, distance = avoid_obstacle() + # 检测障碍物 + need_avoid, min_dist, left_min, right_min, left_free, right_free = detect_obstacles() - # 控制逻辑 + # 智能避障逻辑 if need_avoid: - print(f"前方{distance:.2f}米检测到障碍物，开始避障...") - steer = 0.5 # 向右转向避障 + print(f"避障中 - 前方:{min_dist:.1f}m, 左:{left_min:.1f}m, 右:{right_min:.1f}m") + + if recovery_timer > 0: + # 处于回正阶段，继续回正 + recovery_timer -= 1 + steer = steer * 0.8 # 逐渐回正 + if recovery_timer == 0: + avoid_state = 0 + + elif avoid_timer <= 0: + # 选择避障方向：综合考虑距离和安全区域 + # 右侧更安全的条件：右侧距离更远 或 右侧安全区域更大 + if (right_min > left_min + 2) or (right_free > left_free + 50): + avoid_state = 1 + steer = 0.4 # 右转幅度 + avoid_timer = 40 # 避障持续时间 + else: + avoid_state = 2 + steer = -0.4 # 左转幅度 + avoid_timer = 40 + else: + avoid_timer -= 1 + # 避障后期开始准备回正 + if avoid_timer < 15: + steer *= 0.95 + + # 避障结束后进入回正阶段 + if avoid_timer == 0: + recovery_timer = 20 # 回正持续时间 + else: - steer = 0.0 # 直行 + # 无障碍物时保持直行或回正 + if abs(steer) > 0.1: + steer *= 0.85 # 平滑回正 + else: + steer = 0.0 + avoid_state = 0 + avoid_timer = 0 + recovery_timer = 0 - # 控制车辆 + # 动态调整油门：根据障碍物距离 + if need_avoid: + if min_dist < 8.0: + throttle = 0.2 # 很近时减速 + elif min_dist < 15.0: + throttle = 0.3 # 较近时减速 + else: + throttle = 0.4 # 保持一定速度 + else: + throttle = 0.5 # 正常速度 + + # 应用控制 vehicle.apply_control(carla.VehicleControl( throttle=throttle, steer=steer, - brake=0.0 + brake=0.0, + hand_brake=False )) - # 显示摄像头画面（多模态可视化） - if camera_image is not None: - cv2.imshow('Camera View', camera_image) - if cv2.waitKey(1) & 0xFF == ord('q'): + # 可视化显示 + if front_image is not None and third_image is not None and lidar_img is not None: + # 前视摄像头添加信息 + front_display = front_image.copy() + cv2.putText(front_display, f"距离: {min_dist:.1f}m", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + cv2.putText(front_display, f"转向: {steer:.2f}", (10, 70), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + + if need_avoid: + cv2.putText(front_display, "避障中", (10, 110), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) + + # 第三视角添加信息 + third_display = third_image.copy() + cv2.putText(third_display, "第三视角", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) + + # 显示所有窗口 + cv2.imshow('前视摄像头', front_display) + cv2.imshow('第三视角', third_display) + cv2.imshow('LiDAR鸟瞰图', lidar_img) + + # 键盘控制 + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): break + elif key == ord('w'): + throttle = min(1.0, throttle + 0.1) + elif key == ord('s'): + throttle = max(0.0, throttle - 0.1) + elif key == ord('a'): + steer = max(-1.0, steer - 0.1) + elif key == ord('d'): + steer = min(1.0, steer + 0.1) + elif key == ord('r'): + steer = 0.0 # 重置转向 - time.sleep(0.05) + time.sleep(0.01) except KeyboardInterrupt: print("系统已停止") finally: # 清理资源 - camera.stop() + front_camera.stop() + third_camera.stop() lidar.stop() - vehicle.destroy() - camera.destroy() - lidar.destroy() - world.apply_settings(settings) # 恢复世界设置 - cv2.destroyAllWindows() + + # 销毁所有车辆 + for actor in world.get_actors().filter('vehicle.*'): + actor.destroy() + + # 销毁所有传感器 + for actor in world.get_actors().filter('sensor.*'): + actor.destroy() + + # 恢复世界设置 + settings.synchronous_mode = False + world.apply_settings(settings) + cv2.destroyAllWindows() \ No newline at end of file From 2a47aa744c84dcf6901cde3f821b60f17ee87295 Mon Sep 17 00:00:00 2001 From: chen Date: Mon, 10 Nov 2025 11:01:10 +0800 Subject: [PATCH 03/25] =?UTF-8?q?=E8=A1=A5=E5=85=85README.MD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/README.md | 55 +++++++++++++++++++++++++++-- 1 file changed, 53 insertions(+), 2 deletions(-) diff --git a/src/car_navigation_system/README.md b/src/car_navigation_system/README.md index 5e5c71f230..b4cbe64b90 100644 --- a/src/car_navigation_system/README.md +++ b/src/car_navigation_system/README.md @@ -1,2 +1,53 @@ -# 机器人导航系统模块 - +# 机器人导航系统模块 +# 多模态 CARLA 导航避障系统 +# 项目简介 +本项目基于 CARLA 模拟器与神经网络技术，实现了具备多传感器融合能力的智能车辆导航避障系统。系统集成前视摄像头、第三视角摄像头与激光雷达（LiDAR），通过多模态数据感知环境，结合智能避障算法，实现车辆自主行驶与障碍物规避功能。 +# 核心功能 +多传感器融合感知：集成 RGB 摄像头（前视 + 第三视角）与 32 线激光雷达，全面获取环境信息 +智能避障算法：基于激光雷达点云数据，分区域检测障碍物，动态选择最优避障方向（左 / 右） +可视化监控：实时显示前视画面、第三视角跟随画面、LiDAR 鸟瞰图，叠加关键行驶数据 +人机交互控制：支持键盘手动干预（加速、减速、转向、重置），兼容自动 / 手动切换 +环境配置 +# 基础依赖 +操作系统：Windows 10/11 或 Ubuntu 20.04/22.04 +Python 版本：3.7-3.12（需兼容 3.7+） +核心框架：PyTorch（项目约定优先使用，不依赖 TensorFlow） +模拟器：CARLA（需提前启动并监听 2000 端口） +# 依赖安装 +安装 CARLA 模拟器（参考 CARLA 官方文档） +安装 Python 依赖包： +bash +pip install -r requirements.txt +pip install carla numpy opencv-python matplotlib +# 快速启动 +步骤 1：启动 CARLA 模拟器 +bash +# Windows 示例（根据实际安装路径调整） +CarlaUE4.exe -windowed -ResX=800 -ResY=600 +# Ubuntu 示例 +./CarlaUE4.sh -windowed -ResX=800 -ResY=600 +步骤 2：运行导航避障系统 +bash +# 进入项目根目录 +cd D:\nn +# 激活虚拟环境 +source venv/bin/activate # Linux/Mac +venv\Scripts\activate # Windows +# 运行主程序 +python src/robot_navigation_system/main.py +步骤 3：操作说明 +# 按键功能 +q 退出系统 +w 手动加速（提高油门） +s 手动减速（降低油门） +a 手动左转向 +d 手动右转向 +r 重置转向角度（回正） +# 系统架构 +1. 环境初始化模块 +连接 CARLA 服务器（默认 localhost:2000） +加载 Town01 地图，设置同步模式（固定时间步长 0.05s） +配置天气参数（多云 30%、无降水、太阳高度角 70°） +2. 智能体生成模块 +主车辆：特斯拉 Model3（红色），关闭自动驾驶，由自定义算法控制 +障碍物车辆：随机生成 6 辆不同类型车辆，开启自动驾驶，分散分布在地图中 ''' From 72263c09fd5737dd45fe436fddac60dd08c903a8 Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 11 Nov 2025 10:54:10 +0800 Subject: [PATCH 04/25] =?UTF-8?q?=E6=A0=B9=E6=8D=AE=E4=B8=BB=E5=87=BD?= =?UTF-8?q?=E6=95=B0=E4=BF=AE=E6=94=B9README.md=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/README.md | 97 +++++++++++++++-------------- 1 file changed, 51 insertions(+), 46 deletions(-) diff --git a/src/car_navigation_system/README.md b/src/car_navigation_system/README.md index b4cbe64b90..c578c097f9 100644 --- a/src/car_navigation_system/README.md +++ b/src/car_navigation_system/README.md @@ -1,53 +1,58 @@ -# 机器人导航系统模块 # 多模态 CARLA 导航避障系统 -# 项目简介 +## 项目简介 本项目基于 CARLA 模拟器与神经网络技术，实现了具备多传感器融合能力的智能车辆导航避障系统。系统集成前视摄像头、第三视角摄像头与激光雷达（LiDAR），通过多模态数据感知环境，结合智能避障算法，实现车辆自主行驶与障碍物规避功能。 -# 核心功能 -多传感器融合感知：集成 RGB 摄像头（前视 + 第三视角）与 32 线激光雷达，全面获取环境信息 -智能避障算法：基于激光雷达点云数据，分区域检测障碍物，动态选择最优避障方向（左 / 右） -可视化监控：实时显示前视画面、第三视角跟随画面、LiDAR 鸟瞰图，叠加关键行驶数据 -人机交互控制：支持键盘手动干预（加速、减速、转向、重置），兼容自动 / 手动切换 -环境配置 -# 基础依赖 -操作系统：Windows 10/11 或 Ubuntu 20.04/22.04 -Python 版本：3.7-3.12（需兼容 3.7+） -核心框架：PyTorch（项目约定优先使用，不依赖 TensorFlow） -模拟器：CARLA（需提前启动并监听 2000 端口） -# 依赖安装 -安装 CARLA 模拟器（参考 CARLA 官方文档） -安装 Python 依赖包： -bash -pip install -r requirements.txt -pip install carla numpy opencv-python matplotlib -# 快速启动 +## 核心功能 +- 多传感器融合感知：集成 RGB 摄像头（前视 + 第三视角）与 32 线激光雷达，全面获取环境信息 +- 智能避障算法：基于激光雷达点云数据，分区域检测障碍物，动态选择最优避障方向（左 / 右） +- 可视化监控：实时显示前视画面、第三视角跟随画面、LiDAR 鸟瞰图，叠加关键行驶数据 +- 人机交互控制：支持键盘手动干预，兼容自动 / 手动切换 +## 环境配置 +- 操作系统：Windows 10/11 或 Ubuntu 20.04/22.04 +- Python 版本：3.7 +- 核心框架：PyTorch +- 模拟器：CARLA3.11 +## 依赖安装 +- 安装 CARLA 模拟器(参考 CARLA 官网) +- 安装 Python 依赖包： +- ```bash + pip install -r requirements.txt + pip install carla numpy opencv-python matplotlib + ``` +## 快速启动 + 步骤 1：启动 CARLA 模拟器 -bash -# Windows 示例（根据实际安装路径调整） -CarlaUE4.exe -windowed -ResX=800 -ResY=600 -# Ubuntu 示例 -./CarlaUE4.sh -windowed -ResX=800 -ResY=600 +- ``` bash + CarlaUE4.exe -windowed -ResX=800 -ResY=600 #Windows 示例 + ./CarlaUE4.sh -windowed -ResX=800 -ResY=600 #Ubuntu 示例 + ``` 步骤 2：运行导航避障系统 -bash -# 进入项目根目录 -cd D:\nn -# 激活虚拟环境 -source venv/bin/activate # Linux/Mac -venv\Scripts\activate # Windows -# 运行主程序 -python src/robot_navigation_system/main.py +- 进入项目根目录 +- ```bash + cd D:\nn + ``` +- 激活虚拟环境 +- ``` bash + source venv/bin/activate # Linux/Mac + venv\Scripts\activate # Windows + ``` +- 运行主程序 +- ``` bash + python src/robot_navigation_system/main.py + ``` 步骤 3：操作说明 -# 按键功能 -q 退出系统 -w 手动加速（提高油门） -s 手动减速（降低油门） -a 手动左转向 -d 手动右转向 -r 重置转向角度（回正） -# 系统架构 +- 按键 功能描述: +- q 退出系统 +- w 手动加速（提高油门） +- s 手动减速（降低油门） +- a 手动左转向 +- d 手动右转向 +- r 重置转向角度（回正） + +## 系统架构 1. 环境初始化模块 -连接 CARLA 服务器（默认 localhost:2000） -加载 Town01 地图，设置同步模式（固定时间步长 0.05s） -配置天气参数（多云 30%、无降水、太阳高度角 70°） +连接 CARLA 服务器（默认 localhost:2000）。 +加载 Town01 地图，设置同步模式（固定时间步长 0.05s）。 +配置天气参数（多云、无降水、太阳高度角）。 2. 智能体生成模块 -主车辆：特斯拉 Model3（红色），关闭自动驾驶，由自定义算法控制 -障碍物车辆：随机生成 6 辆不同类型车辆，开启自动驾驶，分散分布在地图中 ''' +主车辆：特斯拉 Model3（红色），关闭自动驾驶，由自定义算法控制。 +障碍物车辆：随机生成 6 辆不同类型车辆，开启自动驾驶，分散分布在地图中。 From 13e3cad55d25dc416992c4316808d3319b8d09d5 Mon Sep 17 00:00:00 2001 From: chen Date: Wed, 12 Nov 2025 22:52:40 +0800 Subject: [PATCH 05/25] =?UTF-8?q?=E8=B0=83=E6=95=B4=E6=B2=B9=E9=97=A8?= =?UTF-8?q?=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 7a385b2c71..32284bdad5 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -257,13 +257,13 @@ def detect_obstacles(): # 动态调整油门：根据障碍物距离 if need_avoid: if min_dist < 8.0: - throttle = 0.2 # 很近时减速 + throttle = 0.15 # 很近时减速 elif min_dist < 15.0: - throttle = 0.3 # 较近时减速 + throttle = 0.25 # 较近时减速 else: throttle = 0.4 # 保持一定速度 else: - throttle = 0.5 # 正常速度 + throttle = 0.4 # 正常速度 # 应用控制 vehicle.apply_control(carla.VehicleControl( From a78bb82bf5beb15998481398168bf4fb35e3b984 Mon Sep 17 00:00:00 2001 From: chen Date: Mon, 17 Nov 2025 10:26:18 +0800 Subject: [PATCH 06/25] =?UTF-8?q?=E4=BC=98=E5=8C=96=E8=AF=86=E5=88=AB?= =?UTF-8?q?=E6=95=88=E7=8E=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 597 ++++++++++++++++++++---------- 1 file changed, 394 insertions(+), 203 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 32284bdad5..656f20ac40 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -3,19 +3,23 @@ import numpy as np import cv2 import math +from collections import deque # -------------------------- # 1. 初始化CARLA连接和环境 # -------------------------- client = carla.Client('localhost', 2000) -client.set_timeout(10.0) +client.set_timeout(15.0) world = client.load_world('Town01') + settings = world.get_settings() settings.synchronous_mode = True -settings.fixed_delta_seconds = 0.05 # 固定时间步长 +settings.fixed_delta_seconds = 0.1 +settings.substepping = True +settings.max_substep_delta_time = 0.01 +settings.max_substeps = 10 world.apply_settings(settings) -# 设置天气 weather = carla.WeatherParameters( cloudiness=30.0, precipitation=0.0, @@ -23,11 +27,13 @@ ) world.set_weather(weather) -# 获取出生点 -spawn_points = world.get_map().get_spawn_points() +map = world.get_map() +spawn_points = map.get_spawn_points() if not spawn_points: raise Exception("No spawn points available") -spawn_point = spawn_points[0] + +# 选择一个更好的出生点 +spawn_point = spawn_points[10] # 选择更靠前的出生点 # -------------------------- # 2. 生成车辆和障碍物 @@ -36,68 +42,56 @@ # 主车辆（红色） vehicle_bp = blueprint_library.find('vehicle.tesla.model3') -vehicle_bp.set_attribute('color', '255,0,0') # 红色主车辆 +vehicle_bp.set_attribute('color', '255,0,0') vehicle = world.spawn_actor(vehicle_bp, spawn_point) +if not vehicle: + raise Exception("无法生成主车辆") vehicle.set_autopilot(False) +vehicle.set_simulate_physics(True) + +print(f"车辆生成在位置: {spawn_point.location}") -# 生成随机障碍物车辆 -for i in range(6): +# 生成障碍物 +obstacle_count = 3 +for i in range(obstacle_count): if i >= len(spawn_points): break other_vehicles = blueprint_library.filter('vehicle.*') other_vehicle_bp = np.random.choice(other_vehicles) - spawn_idx = (i + 8) % len(spawn_points) # 分散的出生点 + spawn_idx = (i + 15) % len(spawn_points) other_vehicle = world.try_spawn_actor(other_vehicle_bp, spawn_points[spawn_idx]) if other_vehicle: other_vehicle.set_autopilot(True) # -------------------------- -# 3. 配置传感器（含第三视角摄像头） +# 3. 配置传感器 # -------------------------- -# 3.1 前视摄像头（第一视角） -front_camera_bp = blueprint_library.find('sensor.camera.rgb') -front_camera_bp.set_attribute('image_size_x', '640') -front_camera_bp.set_attribute('image_size_y', '480') -front_camera_bp.set_attribute('fov', '90') -front_camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) -front_camera = world.spawn_actor(front_camera_bp, front_camera_transform, attach_to=vehicle) - -# 3.2 第三视角摄像头（跟随车辆） third_camera_bp = blueprint_library.find('sensor.camera.rgb') third_camera_bp.set_attribute('image_size_x', '640') third_camera_bp.set_attribute('image_size_y', '480') -third_camera_bp.set_attribute('fov', '110') # 更宽的视角 -# 安装在车辆后方上方，提供良好的第三视角 +third_camera_bp.set_attribute('fov', '110') third_camera_transform = carla.Transform( - carla.Location(x=-5.0, y=0.0, z=3.0), # 位置：车后5米，高3米 - carla.Rotation(pitch=-15.0) # 角度：向下倾斜15度 + carla.Location(x=-5.0, y=0.0, z=3.0), + carla.Rotation(pitch=-15.0) ) third_camera = world.spawn_actor(third_camera_bp, third_camera_transform, attach_to=vehicle) -# 3.3 激光雷达 +# 激光雷达配置 lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') lidar_bp.set_attribute('channels', '32') lidar_bp.set_attribute('range', '50') lidar_bp.set_attribute('points_per_second', '100000') lidar_bp.set_attribute('rotation_frequency', '10') +lidar_bp.set_attribute('upper_fov', '15') +lidar_bp.set_attribute('lower_fov', '-25') lidar_transform = carla.Transform(carla.Location(x=0.0, z=2.5)) lidar = world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle) # -------------------------- # 4. 传感器数据处理 # -------------------------- -# 存储传感器数据 -front_image = None third_image = None lidar_data = None -lidar_img = None - - -def front_camera_callback(image): - global front_image - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - front_image = array[:, :, :3] def third_camera_callback(image): @@ -108,195 +102,389 @@ def third_camera_callback(image): def lidar_callback(point_cloud): - global lidar_data, lidar_img - # 处理点云数据 + global lidar_data data = np.frombuffer(point_cloud.raw_data, dtype=np.dtype('f4')) data = np.reshape(data, (int(data.shape[0] / 4), 4)) lidar_data = data - # 生成LiDAR可视化图像（鸟瞰图） - lidar_img = np.zeros((400, 400, 3), dtype=np.uint8) - center_x, center_y = 200, 200 - scale = 8 # 缩放因子 - - for point in data: - x, y = point[0], point[1] - if 0 < x < 50 and -25 < y < 25: # 扩大检测范围 - px = int(center_x - y * scale) - py = int(center_y - x * scale) - if 0 <= px < 400 and 0 <= py < 400: - # 距离越近颜色越红 - color_intensity = min(1.0, x / 50.0) - color = ( - int(255 * (1 - color_intensity)), - int(255 * color_intensity), - 0 - ) - cv2.circle(lidar_img, (px, py), 1, color, -1) - - # 绘制车辆位置和方向 - cv2.circle(lidar_img, (center_x, center_y), 5, (0, 0, 255), -1) - cv2.line(lidar_img, (center_x, center_y), (center_x, center_y - 30), (0, 0, 255), 2) - - -# 绑定回调函数 -front_camera.listen(lambda data: front_camera_callback(data)) -third_camera.listen(lambda data: third_camera_callback(data)) -lidar.listen(lambda data: lidar_callback(data)) + +third_camera.listen(third_camera_callback) +lidar.listen(lidar_callback) + +time.sleep(2.0) # 增加等待时间确保传感器初始化 # -------------------------- -# 5. 优化的避障控制逻辑 +# 5. 路径规划与导航逻辑 # -------------------------- -def detect_obstacles(): - """检测障碍物并分析左右安全区域""" - if lidar_data is None: - return False, 0.0, 0.0, 0.0, 0.0, 0.0 +def get_next_waypoint(vehicle_location, distance=8.0): + """获取车辆前方指定距离的路点""" + waypoint = map.get_waypoint(vehicle_location, project_to_road=True) + + next_waypoints = waypoint.next(distance) + if next_waypoints: + return next_waypoints[0] - # 分区域检测：近距离(0-10m)、中距离(10-20m)、远距离(20-30m) - # 近距离检测范围更窄，中远距离更宽，符合驾驶习惯 - near_points = lidar_data[ - (lidar_data[:, 0] > 0) & (lidar_data[:, 0] < 10) & # 近距离 - (lidar_data[:, 1] > -3) & (lidar_data[:, 1] < 3) # 窄范围 - ] + if waypoint.is_junction: + for wp in waypoint.next(distance): + if wp.road_id == waypoint.road_id: + return wp - mid_points = lidar_data[ - (lidar_data[:, 0] >= 10) & (lidar_data[:, 0] < 20) & # 中距离 - (lidar_data[:, 1] > -6) & (lidar_data[:, 1] < 6) # 中等范围 - ] + if waypoint.lane_change & carla.LaneChange.Right: + right_way = waypoint.get_right_lane() + if right_way: + return right_way.next(distance)[0] + elif waypoint.lane_change & carla.LaneChange.Left: + left_way = waypoint.get_left_lane() + if left_way: + return left_way.next(distance)[0] - far_points = lidar_data[ - (lidar_data[:, 0] >= 20) & (lidar_data[:, 0] < 30) & # 远距离 - (lidar_data[:, 1] > -10) & (lidar_data[:, 1] < 10) # 宽范围 - ] + return waypoint - # 合并所有检测点 - front_points = np.vstack((near_points, mid_points, far_points)) if len(near_points) > 0 or len( - mid_points) > 0 or len(far_points) > 0 else np.array([]) - if len(front_points) == 0: - return False, 0.0, 0.0, 0.0, 0.0, 0.0 +def calculate_steering_angle(vehicle_transform, target_waypoint): + """计算到达目标路点所需的转向角""" + vehicle_location = vehicle_transform.location + target_location = target_waypoint.transform.location + + vehicle_yaw = math.radians(vehicle_transform.rotation.yaw) + dx = target_location.x - vehicle_location.x + dy = target_location.y - vehicle_location.y + + local_x = dx * math.cos(vehicle_yaw) + dy * math.sin(vehicle_yaw) + local_y = -dx * math.sin(vehicle_yaw) + dy * math.cos(vehicle_yaw) + + if abs(local_x) < 0.1: + return 0.0 + + angle = math.atan2(local_y, local_x) + max_angle = math.radians(60) + steering = angle / max_angle + + return np.clip(steering, -1.0, 1.0) + + +# -------------------------- +# 6. 优化避障控制逻辑 +# -------------------------- +class ObstacleAvoidance: + def __init__(self): + self.obstacle_history = deque(maxlen=10) + self.emergency_brake = False + self.last_avoid_direction = 0 + + def detect_obstacles(self, lidar_data, vehicle_speed): + """修复版的障碍物检测算法""" + if lidar_data is None: + return self._get_default_detection_result() + + if len(lidar_data) == 0: + return self._get_default_detection_result() + + try: + # 地面过滤 + ground_threshold = -0.5 + valid_mask = lidar_data[:, 2] > ground_threshold + valid_points = lidar_data[valid_mask] + + if len(valid_points) == 0: + return self._get_default_detection_result() + + # 计算距离和角度 + distances = np.sqrt(valid_points[:, 0] ** 2 + valid_points[:, 1] ** 2) + angles = np.arctan2(valid_points[:, 1], valid_points[:, 0]) + + # 定义检测区域 + front_angle_range = np.radians(75) + front_mask = (np.abs(angles) <= front_angle_range) & (distances > 1.0) + + front_points = valid_points[front_mask] + front_distances = distances[front_mask] + front_angles = angles[front_mask] + + if len(front_points) == 0: + return self._get_default_detection_result() + + # 分区域检测 + near_zone = front_distances < 8.0 + mid_zone = (front_distances >= 8.0) & (front_distances < 20.0) + far_zone = (front_distances >= 20.0) & (front_distances < 35.0) + + # 紧急制动检测 + emergency_points = front_points[near_zone & (front_distances < 4.0)] + self.emergency_brake = len(emergency_points) > 10 + + # 计算最小距离和障碍物角度 + min_distance = np.min(front_distances) + min_idx = np.argmin(front_distances) + obstacle_angle = front_angles[min_idx] if len(front_angles) > min_idx else 0.0 + + # 分左右区域分析 + left_points_distances = front_distances[front_angles > 0] + right_points_distances = front_distances[front_angles < 0] + + # 计算左右侧最小距离 + left_min = np.min(left_points_distances) if len(left_points_distances) > 0 else float('inf') + right_min = np.min(right_points_distances) if len(right_points_distances) > 0 else float('inf') + + # 计算自由空间 + safe_threshold = 15.0 + left_free = np.sum(left_points_distances > safe_threshold) if len(left_points_distances) > 0 else 1000 + right_free = np.sum(right_points_distances > safe_threshold) if len(right_points_distances) > 0 else 1000 + + # 障碍物检测条件 + obstacle_detected = (np.sum(near_zone) > 5 or + np.sum(mid_zone) > 10 or + min_distance < 12.0) + + return { + 'obstacle_detected': obstacle_detected, + 'min_distance': min_distance, + 'left_clearance': left_min, + 'right_clearance': right_min, + 'left_free_space': left_free, + 'right_free_space': right_free, + 'obstacle_angle': obstacle_angle, + 'emergency_brake': self.emergency_brake + } + + except Exception as e: + print(f"障碍物检测错误: {e}") + return self._get_default_detection_result() + + def _get_default_detection_result(self): + """返回默认的检测结果""" + return { + 'obstacle_detected': False, + 'min_distance': 30.0, + 'left_clearance': float('inf'), + 'right_clearance': float('inf'), + 'left_free_space': 1000, + 'right_free_space': 1000, + 'obstacle_angle': 0.0, + 'emergency_brake': False + } + + def decide_avoidance_direction(self, detection_result, current_steer, vehicle_speed): + """避障决策逻辑""" + if not detection_result['obstacle_detected']: + self.last_avoid_direction = 0 + return 0, 0, False + + min_dist = detection_result['min_distance'] + left_clear = detection_result['left_clearance'] + right_clear = detection_result['right_clearance'] + left_free = detection_result['left_free_space'] + right_free = detection_result['right_free_space'] + obstacle_angle = detection_result['obstacle_angle'] + + # 紧急制动情况 + if detection_result['emergency_brake']: + return 0, 1.0, True + + # 基于安全距离的避障决策 + safety_margin = max(3.0, vehicle_speed * 0.5) + + # 计算左右侧的安全得分 + left_score = (left_clear - safety_margin) + (left_free * 0.1) + right_score = (right_clear - safety_margin) + (right_free * 0.1) + + # 考虑当前转向的连续性 + if self.last_avoid_direction != 0: + if self.last_avoid_direction == 1: + right_score += 2.0 + else: + left_score += 2.0 - # 计算最近障碍物距离 - min_distance = np.min(front_points[:, 0]) + # 决策避障方向 + avoid_steer = 0.0 + avoid_brake = 0.0 - # 区分左右障碍物 - left_points = front_points[front_points[:, 1] < 0] - right_points = front_points[front_points[:, 1] > 0] + if min_dist < safety_margin + 2.0: + avoid_brake = 0.3 + (safety_margin - min_dist) * 0.1 - # 计算左右最近距离 - left_min = np.min(left_points[:, 0]) if len(left_points) > 0 else float('inf') - right_min = np.min(right_points[:, 0]) if len(right_points) > 0 else float('inf') + if right_score > left_score + 1.0: + avoid_steer = -0.5 + self.last_avoid_direction = -1 + elif left_score > right_score + 1.0: + avoid_steer = 0.5 + self.last_avoid_direction = 1 + else: + # 两侧条件相似，基于障碍物角度决策 + if obstacle_angle > 0: + avoid_steer = -0.4 + self.last_avoid_direction = 1 + else: + avoid_steer = 0.4 + self.last_avoid_direction = -1 - # 计算左右安全区域大小（障碍物较少的区域） - left_free = np.sum(left_points[:, 0] > 15) if len(left_points) > 0 else 1000 - right_free = np.sum(right_points[:, 0] > 15) if len(right_points) > 0 else 1000 + return avoid_steer, avoid_brake, False - return min_distance < 20.0, min_distance, left_min, right_min, left_free, right_free +# -------------------------- +# 7. 主控制循环 +# -------------------------- +# 初始化避障控制器 +obstacle_avoidance = ObstacleAvoidance() # 控制状态变量 -throttle = 0.5 +throttle = 1.0 # 直接使用最大油门 steer = 0.0 -avoid_state = 0 # 0: 正常, 1: 右避障, 2: 左避障, 3: 回正 -avoid_timer = 0 -recovery_timer = 0 # 避障后回正计时器 +brake = 0.0 +waypoint_distance = 8.0 + +# 获取初始路点 +vehicle_location = vehicle.get_location() +waypoint = get_next_waypoint(vehicle_location, waypoint_distance) + +# 控制平滑滤波器 +steer_filter = deque(maxlen=3) +throttle_filter = deque(maxlen=2) + +print("初始化车辆状态...") +# 确保车辆物理引擎开启 +vehicle.set_simulate_physics(True) + +# 直接应用强力控制 +print("应用强力启动控制...") +vehicle.apply_control(carla.VehicleControl( + throttle=1.0, # 最大油门 + steer=0.0, + brake=0.0, + hand_brake=False +)) try: - print("多模态导航系统启动（带第三视角）") - print("控制键: q-退出, w-加速, s-减速, a-左转向, d-右转向, r-重置方向") + print("自动驾驶系统启动（强力油门版本）") + print("控制键: q-退出, w-加速, s-减速, a-左转向, d-右转向, r-重置方向, 空格-紧急制动") + + frame_count = 0 + stuck_count = 0 + last_position = vehicle.get_location() while True: world.tick() - - # 检测障碍物 - need_avoid, min_dist, left_min, right_min, left_free, right_free = detect_obstacles() - - # 智能避障逻辑 - if need_avoid: - print(f"避障中 - 前方:{min_dist:.1f}m, 左:{left_min:.1f}m, 右:{right_min:.1f}m") - - if recovery_timer > 0: - # 处于回正阶段，继续回正 - recovery_timer -= 1 - steer = steer * 0.8 # 逐渐回正 - if recovery_timer == 0: - avoid_state = 0 - - elif avoid_timer <= 0: - # 选择避障方向：综合考虑距离和安全区域 - # 右侧更安全的条件：右侧距离更远 或 右侧安全区域更大 - if (right_min > left_min + 2) or (right_free > left_free + 50): - avoid_state = 1 - steer = 0.4 # 右转幅度 - avoid_timer = 40 # 避障持续时间 - else: - avoid_state = 2 - steer = -0.4 # 左转幅度 - avoid_timer = 40 + frame_count += 1 + + vehicle_transform = vehicle.get_transform() + vehicle_location = vehicle.get_location() + vehicle_velocity = vehicle.get_velocity() + vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2) + + # 每帧都打印状态信息 + print( + f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 位置=({vehicle_location.x:.1f}, {vehicle_location.y:.1f})") + + # 检测是否卡住 + current_position = vehicle_location + distance_moved = current_position.distance(last_position) + if distance_moved < 0.1: # 几乎没移动 + stuck_count += 1 + else: + stuck_count = 0 + + last_position = current_position + + # 如果卡住超过10帧，尝试强力脱困 + if stuck_count > 10: + print("车辆卡住，尝试强力脱困...") + # 先倒车再前进 + vehicle.apply_control(carla.VehicleControl( + throttle=0.0, + steer=0.0, + brake=1.0, + hand_brake=False, + reverse=True + )) + time.sleep(0.5) + vehicle.apply_control(carla.VehicleControl( + throttle=1.0, + steer=0.0, + brake=0.0, + hand_brake=False, + reverse=False + )) + stuck_count = 0 + + # 更新目标路点 + current_distance = vehicle_location.distance(waypoint.transform.location) + if current_distance < 4.0: + waypoint = get_next_waypoint(vehicle_location, waypoint_distance) + + # 计算基础转向角 + base_steer = calculate_steering_angle(vehicle_transform, waypoint) + + # 检测障碍物并决策避障 + detection_result = obstacle_avoidance.detect_obstacles(lidar_data, vehicle_speed) + avoid_steer, avoid_brake, emergency_brake = obstacle_avoidance.decide_avoidance_direction( + detection_result, steer, vehicle_speed + ) + + # 综合控制输出 - 简化逻辑，专注于让车动起来 + if emergency_brake: + throttle = 0.0 + brake = 1.0 + steer = base_steer * 0.3 + print("!!! 紧急制动 !!!") + elif detection_result['obstacle_detected']: + brake = avoid_brake + throttle = 0.8 # 避障时也保持高油门 + steer = avoid_steer * 0.8 + base_steer * 0.2 + print(f"避障中 - 距离:{detection_result['min_distance']:.1f}m") + else: + # 正常行驶 - 使用强力油门 + brake = 0.0 + steer = base_steer + + # 强力油门策略 + if vehicle_speed < 10.0: # 低速时最大油门 + throttle = 1.0 + elif vehicle_speed < 20.0: + throttle = 0.8 + elif vehicle_speed < 30.0: + throttle = 0.6 else: - avoid_timer -= 1 - # 避障后期开始准备回正 - if avoid_timer < 15: - steer *= 0.95 + throttle = 0.4 - # 避障结束后进入回正阶段 - if avoid_timer == 0: - recovery_timer = 20 # 回正持续时间 + # 应用平滑滤波 + steer_filter.append(steer) + throttle_filter.append(throttle) - else: - # 无障碍物时保持直行或回正 - if abs(steer) > 0.1: - steer *= 0.85 # 平滑回正 - else: - steer = 0.0 - avoid_state = 0 - avoid_timer = 0 - recovery_timer = 0 - - # 动态调整油门：根据障碍物距离 - if need_avoid: - if min_dist < 8.0: - throttle = 0.15 # 很近时减速 - elif min_dist < 15.0: - throttle = 0.25 # 较近时减速 - else: - throttle = 0.4 # 保持一定速度 - else: - throttle = 0.4 # 正常速度 + smoothed_steer = np.mean(steer_filter) + smoothed_throttle = np.mean(throttle_filter) - # 应用控制 - vehicle.apply_control(carla.VehicleControl( - throttle=throttle, - steer=steer, - brake=0.0, - hand_brake=False - )) + # 应用强力控制 + control = carla.VehicleControl( + throttle=smoothed_throttle, + steer=smoothed_steer, + brake=brake, + hand_brake=False, + reverse=False + ) + + print(f"控制输出: 油门={control.throttle:.2f}, 刹车={control.brake:.2f}, 转向={control.steer:.2f}") + vehicle.apply_control(control) # 可视化显示 - if front_image is not None and third_image is not None and lidar_img is not None: - # 前视摄像头添加信息 - front_display = front_image.copy() - cv2.putText(front_display, f"距离: {min_dist:.1f}m", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - cv2.putText(front_display, f"转向: {steer:.2f}", (10, 70), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - - if need_avoid: - cv2.putText(front_display, "避障中", (10, 110), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) - - # 第三视角添加信息 - third_display = third_image.copy() - cv2.putText(third_display, "第三视角", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) - - # 显示所有窗口 - cv2.imshow('前视摄像头', front_display) - cv2.imshow('第三视角', third_display) - cv2.imshow('LiDAR鸟瞰图', lidar_img) - - # 键盘控制 + if third_image is not None: + display_image = third_image.copy() + cv2.putText(display_image, f"Speed: {vehicle_speed * 3.6:.1f} km/h", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 60), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 90), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + + if detection_result['obstacle_detected']: + status_text = f"OBSTACLE: {detection_result['min_distance']:.1f}m" + cv2.putText(display_image, status_text, (10, 120), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) + else: + cv2.putText(display_image, "CLEAR", (10, 120), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) + + cv2.imshow('第三视角 - 强力油门系统', display_image) + key = cv2.waitKey(1) & 0xFF if key == ord('q'): break @@ -309,28 +497,31 @@ def detect_obstacles(): elif key == ord('d'): steer = min(1.0, steer + 0.1) elif key == ord('r'): - steer = 0.0 # 重置转向 + steer = 0.0 + elif key == ord(' '): + brake = 1.0 + throttle = 0.0 time.sleep(0.01) except KeyboardInterrupt: print("系统已停止") +except Exception as e: + print(f"系统错误: {e}") + import traceback + + traceback.print_exc() finally: - # 清理资源 - front_camera.stop() + print("正在清理资源...") third_camera.stop() lidar.stop() - # 销毁所有车辆 - for actor in world.get_actors().filter('vehicle.*'): - actor.destroy() - - # 销毁所有传感器 - for actor in world.get_actors().filter('sensor.*'): - actor.destroy() + for actor in world.get_actors(): + if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'): + actor.destroy() - # 恢复世界设置 settings.synchronous_mode = False world.apply_settings(settings) - cv2.destroyAllWindows() \ No newline at end of file + cv2.destroyAllWindows() + print("资源清理完成") \ No newline at end of file From 1b713090932aba71150bea37e5d2059081f292b3 Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 18 Nov 2025 08:30:26 +0800 Subject: [PATCH 07/25] =?UTF-8?q?=E8=B0=83=E6=95=B4=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 656f20ac40..2cbbff3a3e 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -422,29 +422,29 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp # 综合控制输出 - 简化逻辑，专注于让车动起来 if emergency_brake: - throttle = 0.0 - brake = 1.0 - steer = base_steer * 0.3 + throttle = 0.2 + brake = 0.1 + steer = base_steer * 0.1 print("!!! 紧急制动 !!!") elif detection_result['obstacle_detected']: brake = avoid_brake - throttle = 0.8 # 避障时也保持高油门 - steer = avoid_steer * 0.8 + base_steer * 0.2 + throttle = 0.4 # 避障时也保持高油门 + steer = avoid_steer * 0.1 + base_steer * 0.1 print(f"避障中 - 距离:{detection_result['min_distance']:.1f}m") else: # 正常行驶 - 使用强力油门 brake = 0.0 - steer = base_steer + steer = base_steer*0.1 # 强力油门策略 - if vehicle_speed < 10.0: # 低速时最大油门 - throttle = 1.0 - elif vehicle_speed < 20.0: - throttle = 0.8 - elif vehicle_speed < 30.0: - throttle = 0.6 - else: + if vehicle_speed < 2.0: # 低速时最大油门 + throttle = 0.5 + elif vehicle_speed < 5.0: throttle = 0.4 + elif vehicle_speed < 7.0: + throttle = 0.3 + else: + throttle = 0.2 # 应用平滑滤波 steer_filter.append(steer) From d2340d74df71cce27a9e4e26fc18c34150367860 Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 18 Nov 2025 09:23:40 +0800 Subject: [PATCH 08/25] =?UTF-8?q?=E8=B0=83=E6=95=B4=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 33 +++++-------------------------- 1 file changed, 5 insertions(+), 28 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 4f3fa5f62c..20be0c0720 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -434,48 +434,25 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp else: # 正常行驶 - 使用强力油门 brake = 0.0 - steer = base_steer*0.1 + steer = base_steer * 0.5 # 强力油门策略 if vehicle_speed < 2.0: # 低速时最大油门 - throttle = 0.5 - elif vehicle_speed < 5.0: throttle = 0.4 - elif vehicle_speed < 7.0: + elif vehicle_speed < 5.0: throttle = 0.3 - else: + elif vehicle_speed < 7.0: throttle = 0.2 + else: + throttle = 0.1 # 应用平滑滤波 steer_filter.append(steer) throttle_filter.append(throttle) - main smoothed_steer = np.mean(steer_filter) smoothed_throttle = np.mean(throttle_filter) - else: - # 无障碍物时保持直行或回正 - if abs(steer) > 0.1: - steer *= 0.85 # 平滑回正 - else: - steer = 0.0 - avoid_state = 0 - avoid_timer = 0 - recovery_timer = 0 - - # 动态调整油门：根据障碍物距离 - if need_avoid: - if min_dist < 8.0: - throttle = 0.15 # 很近时减速 - elif min_dist < 15.0: - throttle = 0.25 # 较近时减速 - else: - throttle = 0.4 # 保持一定速度 - else: - throttle = 0.4 # 正常速度 - main - # 应用强力控制 control = carla.VehicleControl( throttle=smoothed_throttle, From 3911e4a48ab84eb7385ce1e8dca9cb4875768451 Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 18 Nov 2025 10:15:24 +0800 Subject: [PATCH 09/25] =?UTF-8?q?=E8=B0=83=E6=95=B4=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 21 +++------------------ 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index f007625706..ee08ce2695 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -429,22 +429,17 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp elif detection_result['obstacle_detected']: brake = avoid_brake throttle = 0.4 # 避障时也保持高油门 - steer = avoid_steer * 0.1 + base_steer * 0.1 - - throttle = 0.0 - brake = 1.0 - steer = base_steer * 0.3 + steer = avoid_steer * 0.1 + base_steer * 0.2 print("!!! 紧急制动 !!!") elif detection_result['obstacle_detected']: brake = avoid_brake - throttle = 0.8 # 避障时也保持高油门 + throttle = 0.3 # 避障时也保持高油门 steer = avoid_steer * 0.8 + base_steer * 0.2 print(f"避障中 - 距离:{detection_result['min_distance']:.1f}m") else: # 正常行驶 - 使用强力油门 brake = 0.0 - steer = base_steer * 0. - steer = base_steer*0.1 + steer = base_steer * 0.5 # 强力油门策略 if vehicle_speed < 2.0: # 低速时最大油门 @@ -458,16 +453,6 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp steer = base_steer - # 强力油门策略 - if vehicle_speed < 10.0: # 低速时最大油门 - throttle = 1.0 - elif vehicle_speed < 20.0: - throttle = 0.8 - elif vehicle_speed < 30.0: - throttle = 0.6 - else: - throttle = 0.4 - # 应用平滑滤波 steer_filter.append(steer) throttle_filter.append(throttle) From 32f5e75387a961d4386f84d76321c334ad565251 Mon Sep 17 00:00:00 2001 From: chen Date: Thu, 20 Nov 2025 16:20:33 +0800 Subject: [PATCH 10/25] =?UTF-8?q?=E4=BF=AE=E6=94=B9README.md?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/src/car_navigation_system/README.md b/src/car_navigation_system/README.md index c578c097f9..1d39c7e3f6 100644 --- a/src/car_navigation_system/README.md +++ b/src/car_navigation_system/README.md @@ -17,6 +17,7 @@ - ```bash pip install -r requirements.txt pip install carla numpy opencv-python matplotlib + pip install setuptools==40.2.0 ``` ## 快速启动 From 9819232d0e1af353c73489ff493a7800d0858824 Mon Sep 17 00:00:00 2001 From: chen Date: Mon, 24 Nov 2025 08:10:27 +0800 Subject: [PATCH 11/25] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/src/car_navigation_system/README.md b/src/car_navigation_system/README.md index 1d39c7e3f6..171c7bf327 100644 --- a/src/car_navigation_system/README.md +++ b/src/car_navigation_system/README.md @@ -18,6 +18,7 @@ pip install -r requirements.txt pip install carla numpy opencv-python matplotlib pip install setuptools==40.2.0 + pip insatll wheel ``` ## 快速启动 From bb5ecc2bbd7db5581700e7bfb7450ca268bf604f Mon Sep 17 00:00:00 2001 From: chen Date: Mon, 24 Nov 2025 09:51:55 +0800 Subject: [PATCH 12/25] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 411 ++++++++++++++++++++---------- 1 file changed, 276 insertions(+), 135 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index ee08ce2695..229c8028b4 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -1,3 +1,7 @@ +# -------------------------- +# 1. 初始化CARLA连接和环境 +# -------------------------- +# 导入必要的库 import carla import time import numpy as np @@ -5,197 +9,290 @@ import math from collections import deque -# -------------------------- -# 1. 初始化CARLA连接和环境 -# -------------------------- +# 连接到本地CARLA服务器，端口2000 client = carla.Client('localhost', 2000) +# 设置超时时间为15秒 client.set_timeout(15.0) +# 加载名为'Town01'的地图 world = client.load_world('Town01') +# 获取并设置世界的运行参数 settings = world.get_settings() +# 启用同步模式，这意味着仿真将等待客户端的每一个tick信号 settings.synchronous_mode = True +# 设置固定的时间步长，单位为秒 settings.fixed_delta_seconds = 0.1 +# 启用子步进，用于更精确的物理模拟 settings.substepping = True +# 设置子步进的最大时间步长 settings.max_substep_delta_time = 0.01 +# 设置最大子步进次数 settings.max_substeps = 10 +# 应用这些设置 world.apply_settings(settings) +# 定义天气参数 weather = carla.WeatherParameters( - cloudiness=30.0, - precipitation=0.0, - sun_altitude_angle=70.0 + cloudiness=30.0, # 云量 + precipitation=0.0, # 降雨量 + sun_altitude_angle=70.0 # 太阳高度角 ) +# 应用天气设置 world.set_weather(weather) +# 获取地图对象 map = world.get_map() +# 获取地图中所有可用的出生点 spawn_points = map.get_spawn_points() if not spawn_points: - raise Exception("No spawn points available") + raise Exception("No spawn points available") # 如果没有找到出生点则抛出异常 -# 选择一个更好的出生点 +# 选择一个更好的出生点（索引为10的点） spawn_point = spawn_points[10] # 选择更靠前的出生点 # -------------------------- # 2. 生成车辆和障碍物 # -------------------------- +# 获取蓝图库，包含所有可生成的actor的蓝图 blueprint_library = world.get_blueprint_library() -# 主车辆（红色） +# 主车辆（红色特斯拉Model3） +# 查找特斯拉Model3的车辆蓝图 vehicle_bp = blueprint_library.find('vehicle.tesla.model3') +# 设置车辆颜色为红色 (RGB) vehicle_bp.set_attribute('color', '255,0,0') +# 在指定的出生点生成主车辆 vehicle = world.spawn_actor(vehicle_bp, spawn_point) if not vehicle: raise Exception("无法生成主车辆") +# 禁用车辆的自动驾驶模式 vehicle.set_autopilot(False) +# 确保车辆的物理模拟是开启的 vehicle.set_simulate_physics(True) print(f"车辆生成在位置: {spawn_point.location}") -# 生成障碍物 -obstacle_count = 3 +# 生成障碍物车辆 +obstacle_count = 3 # 计划生成3个障碍物 for i in range(obstacle_count): if i >= len(spawn_points): break + # 过滤出所有车辆类型的蓝图 other_vehicles = blueprint_library.filter('vehicle.*') + # 随机选择一个车辆蓝图 other_vehicle_bp = np.random.choice(other_vehicles) + # 选择一个与主车辆不同的出生点 spawn_idx = (i + 15) % len(spawn_points) + # 尝试在指定位置生成障碍物车辆 other_vehicle = world.try_spawn_actor(other_vehicle_bp, spawn_points[spawn_idx]) if other_vehicle: + # 为障碍物车辆启用自动驾驶 other_vehicle.set_autopilot(True) # -------------------------- # 3. 配置传感器 # -------------------------- +# 配置第三视角RGB相机 +# 查找RGB相机的传感器蓝图 third_camera_bp = blueprint_library.find('sensor.camera.rgb') +# 设置相机图像宽度 third_camera_bp.set_attribute('image_size_x', '640') +# 设置相机图像高度 third_camera_bp.set_attribute('image_size_y', '480') +# 设置相机视场角 third_camera_bp.set_attribute('fov', '110') +# 定义相机相对于车辆的变换（位置和旋转） third_camera_transform = carla.Transform( - carla.Location(x=-5.0, y=0.0, z=3.0), - carla.Rotation(pitch=-15.0) + carla.Location(x=-5.0, y=0.0, z=3.0), # 在车后5米，高3米处 + carla.Rotation(pitch=-15.0) # 向下倾斜15度 ) +# 生成相机传感器并将其附加到主车辆上 third_camera = world.spawn_actor(third_camera_bp, third_camera_transform, attach_to=vehicle) -# 激光雷达配置 +# 配置激光雷达 (LiDAR) +# 查找激光雷达的传感器蓝图 lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') +# 设置激光雷达的通道数（线数） lidar_bp.set_attribute('channels', '32') +# 设置激光雷达的最大探测范围 lidar_bp.set_attribute('range', '50') +# 设置激光雷达的每秒点数 lidar_bp.set_attribute('points_per_second', '100000') +# 设置激光雷达的旋转频率 lidar_bp.set_attribute('rotation_frequency', '10') +# 设置激光雷达的上视场角 lidar_bp.set_attribute('upper_fov', '15') +# 设置激光雷达的下视场角 lidar_bp.set_attribute('lower_fov', '-25') -lidar_transform = carla.Transform(carla.Location(x=0.0, z=2.5)) +# 定义激光雷达相对于车辆的变换 +lidar_transform = carla.Transform(carla.Location(x=0.0, z=2.5)) # 在车顶中心，高2.5米处 +# 生成激光雷达传感器并将其附加到主车辆上 lidar = world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle) # -------------------------- # 4. 传感器数据处理 # -------------------------- -third_image = None -lidar_data = None - +# 全局变量，用于存储来自传感器的最新数据 +third_image = None # 存储相机图像 +lidar_data = None # 存储激光雷达点云 +# 相机数据回调函数 def third_camera_callback(image): + # 使用global关键字修改全局变量 global third_image + # 将原始图像数据（CARLA的Image对象）转换为numpy数组 array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + # 重塑数组形状以匹配图像的尺寸 (高度, 宽度, 4通道RGBA) array = np.reshape(array, (image.height, image.width, 4)) + # 提取前3个通道（RGB）并存储 third_image = array[:, :, :3] - +# 激光雷达数据回调函数 def lidar_callback(point_cloud): + # 使用global关键字修改全局变量 global lidar_data + # 将原始点云数据转换为numpy数组，每个点由4个32位浮点数表示 (x, y, z, intensity) data = np.frombuffer(point_cloud.raw_data, dtype=np.dtype('f4')) + # 重塑数组形状，每行代表一个点 (x, y, z, intensity) data = np.reshape(data, (int(data.shape[0] / 4), 4)) + # 存储处理后的点云数据 lidar_data = data - +# 开始监听传感器数据，设置回调函数 third_camera.listen(third_camera_callback) lidar.listen(lidar_callback) +# 等待2秒，确保传感器有足够的时间进行初始化并返回第一帧数据 time.sleep(2.0) # 增加等待时间确保传感器初始化 - # -------------------------- # 5. 路径规划与导航逻辑 # -------------------------- def get_next_waypoint(vehicle_location, distance=8.0): - """获取车辆前方指定距离的路点""" + """ + 获取车辆前方指定距离的路点 + :param vehicle_location: 车辆当前的位置 + :param distance: 希望获取的下一个路点距离当前车辆的距离 + :return: 下一个目标路点 (carla.Waypoint) + """ + # 获取车辆当前位置所在的路点，并将位置投影到道路中心线上 waypoint = map.get_waypoint(vehicle_location, project_to_road=True) + # 尝试获取当前路点前方distance米处的路点 next_waypoints = waypoint.next(distance) if next_waypoints: - return next_waypoints[0] + return next_waypoints[0] # 返回第一个找到的路点 + # 如果当前路点在路口(junction)，进行特殊处理 if waypoint.is_junction: + # 尝试在同一条道路上寻找下一个路点 for wp in waypoint.next(distance): if wp.road_id == waypoint.road_id: return wp + # 如果找不到同一条路的路点，则尝试变道 + # 检查是否可以向右变道 if waypoint.lane_change & carla.LaneChange.Right: right_way = waypoint.get_right_lane() if right_way: return right_way.next(distance)[0] + # 检查是否可以向左变道 elif waypoint.lane_change & carla.LaneChange.Left: left_way = waypoint.get_left_lane() if left_way: return left_way.next(distance)[0] + # 如果以上方法都失败，返回当前路点 return waypoint - def calculate_steering_angle(vehicle_transform, target_waypoint): - """计算到达目标路点所需的转向角""" + """ + 计算到达目标路点所需的转向角 + :param vehicle_transform: 车辆当前的变换（位置和旋转） + :param target_waypoint: 目标路点 + :return: 归一化的转向指令 (-1.0 到 1.0 之间) + """ + # 获取车辆当前位置和目标路点位置 vehicle_location = vehicle_transform.location target_location = target_waypoint.transform.location + # 将车辆的偏航角（yaw）从度转换为弧度 vehicle_yaw = math.radians(vehicle_transform.rotation.yaw) + + # 计算目标位置相对于车辆位置的向量 (dx, dy) dx = target_location.x - vehicle_location.x dy = target_location.y - vehicle_location.y + # 将目标位置向量从世界坐标系转换到车辆局部坐标系 + # 这使得我们可以更容易地判断目标在车辆的左、右还是正前方 local_x = dx * math.cos(vehicle_yaw) + dy * math.sin(vehicle_yaw) local_y = -dx * math.sin(vehicle_yaw) + dy * math.cos(vehicle_yaw) + # 如果目标在车辆正后方（或非常接近），返回0转向角以避免除以零 if abs(local_x) < 0.1: return 0.0 + # 计算目标方向与车辆当前朝向的夹角 angle = math.atan2(local_y, local_x) + + # 定义最大转向角（例如60度），并将计算出的角度归一化到[-1, 1]范围 max_angle = math.radians(60) steering = angle / max_angle + # 使用np.clip确保转向值在有效范围内 return np.clip(steering, -1.0, 1.0) - # -------------------------- # 6. 优化避障控制逻辑 # -------------------------- class ObstacleAvoidance: + """ + 障碍物检测与避障决策类 + 封装了使用LiDAR数据进行障碍物检测和生成避障指令的逻辑 + """ def __init__(self): + """初始化避障控制器""" + # 用于存储障碍物检测历史，防止频繁切换决策 self.obstacle_history = deque(maxlen=10) + # 紧急制动标志 self.emergency_brake = False - self.last_avoid_direction = 0 + # 记录上一次的避障方向，用于决策的连续性 + self.last_avoid_direction = 0 # -1: 左, 0: 无, 1: 右 def detect_obstacles(self, lidar_data, vehicle_speed): - """修复版的障碍物检测算法""" + """ + 修复版的障碍物检测算法 + 处理LiDAR点云数据，检测前方障碍物并分析其位置 + :param lidar_data: 原始的LiDAR点云数据 (numpy array) + :param vehicle_speed: 车辆当前速度 (m/s) + :return: 包含障碍物检测结果的字典 + """ + # 如果没有LiDAR数据，返回默认结果（无障碍物） if lidar_data is None: return self._get_default_detection_result() + # 如果点云为空，返回默认结果 if len(lidar_data) == 0: return self._get_default_detection_result() try: - # 地面过滤 - ground_threshold = -0.5 - valid_mask = lidar_data[:, 2] > ground_threshold + # 1. 地面过滤：移除高度低于阈值的点，减少地面点的干扰 + ground_threshold = -0.5 # 地面阈值，单位为米 + valid_mask = lidar_data[:, 2] > ground_threshold # z坐标大于阈值的点被认为是有效的 valid_points = lidar_data[valid_mask] if len(valid_points) == 0: return self._get_default_detection_result() - # 计算距离和角度 + # 2. 计算每个有效点相对于车辆的距离和角度 + # 计算x-y平面上的距离 distances = np.sqrt(valid_points[:, 0] ** 2 + valid_points[:, 1] ** 2) + # 计算与车辆x轴正方向的夹角（弧度） angles = np.arctan2(valid_points[:, 1], valid_points[:, 0]) - # 定义检测区域 - front_angle_range = np.radians(75) + # 3. 定义感兴趣区域（ROI）：只关注车辆正前方的区域 + front_angle_range = np.radians(75) # 前方75度的范围 + # 过滤出在角度范围内且距离大于1米（避免过近的噪声点）的点 front_mask = (np.abs(angles) <= front_angle_range) & (distances > 1.0) front_points = valid_points[front_mask] @@ -205,38 +302,40 @@ def detect_obstacles(self, lidar_data, vehicle_speed): if len(front_points) == 0: return self._get_default_detection_result() - # 分区域检测 - near_zone = front_distances < 8.0 - mid_zone = (front_distances >= 8.0) & (front_distances < 20.0) - far_zone = (front_distances >= 20.0) & (front_distances < 35.0) + # 4. 分区域检测：将前方区域分为近、中、远三个区域 + near_zone = front_distances < 8.0 # 近区：0-8米 + mid_zone = (front_distances >= 8.0) & (front_distances < 20.0) # 中区：8-20米 + far_zone = (front_distances >= 20.0) & (front_distances < 35.0) # 远区：20-35米 - # 紧急制动检测 - emergency_points = front_points[near_zone & (front_distances < 4.0)] - self.emergency_brake = len(emergency_points) > 10 + # 5. 紧急制动检测：检测近区中非常接近的障碍物 + emergency_points = front_points[near_zone & (front_distances < 4.0)] # 4米内的点 + self.emergency_brake = len(emergency_points) > 10 # 如果超过10个点，则触发紧急制动 - # 计算最小距离和障碍物角度 + # 6. 计算最近障碍物的距离和角度 min_distance = np.min(front_distances) min_idx = np.argmin(front_distances) obstacle_angle = front_angles[min_idx] if len(front_angles) > min_idx else 0.0 - # 分左右区域分析 - left_points_distances = front_distances[front_angles > 0] - right_points_distances = front_distances[front_angles < 0] + # 7. 分左右区域分析障碍物分布 + left_points_distances = front_distances[front_angles > 0] # 车辆左侧的点（角度为正） + right_points_distances = front_distances[front_angles < 0] # 车辆右侧的点（角度为负） - # 计算左右侧最小距离 + # 计算左右两侧最近障碍物的距离 left_min = np.min(left_points_distances) if len(left_points_distances) > 0 else float('inf') right_min = np.min(right_points_distances) if len(right_points_distances) > 0 else float('inf') - # 计算自由空间 - safe_threshold = 15.0 + # 8. 计算左右两侧的自由空间（无障碍物的区域大小） + safe_threshold = 15.0 # 认为15米外是安全的 + # 统计左右两侧距离大于安全阈值的点的数量，作为自由空间的度量 left_free = np.sum(left_points_distances > safe_threshold) if len(left_points_distances) > 0 else 1000 right_free = np.sum(right_points_distances > safe_threshold) if len(right_points_distances) > 0 else 1000 - # 障碍物检测条件 - obstacle_detected = (np.sum(near_zone) > 5 or - np.sum(mid_zone) > 10 or - min_distance < 12.0) + # 9. 判断是否检测到障碍物的综合条件 + obstacle_detected = (np.sum(near_zone) > 5 or # 近区有超过5个点 + np.sum(mid_zone) > 10 or # 中区有超过10个点 + min_distance < 12.0) # 最近障碍物距离小于12米 + # 返回包含所有检测信息的字典 return { 'obstacle_detected': obstacle_detected, 'min_distance': min_distance, @@ -249,11 +348,16 @@ def detect_obstacles(self, lidar_data, vehicle_speed): } except Exception as e: + # 如果检测过程中发生错误，打印错误信息并返回默认结果 print(f"障碍物检测错误: {e}") return self._get_default_detection_result() def _get_default_detection_result(self): - """返回默认的检测结果""" + """ + 返回默认的检测结果（无障碍物） + 这是一个辅助函数，用于在没有数据或出错时提供一个安全的默认值 + :return: 默认的检测结果字典 + """ return { 'obstacle_detected': False, 'min_distance': 30.0, @@ -266,11 +370,20 @@ def _get_default_detection_result(self): } def decide_avoidance_direction(self, detection_result, current_steer, vehicle_speed): - """避障决策逻辑""" + """ + 避障决策逻辑 + 根据障碍物检测结果，决定车辆应采取的避障动作（转向和刹车） + :param detection_result: 障碍物检测结果字典 + :param current_steer: 当前的转向值 + :param vehicle_speed: 当前车辆速度 + :return: (避障转向值, 避障刹车值, 是否紧急制动) + """ + # 如果没有检测到障碍物，重置状态并返回无动作 if not detection_result['obstacle_detected']: self.last_avoid_direction = 0 return 0, 0, False + # 从检测结果中提取关键信息 min_dist = detection_result['min_distance'] left_clear = detection_result['left_clearance'] right_clear = detection_result['right_clearance'] @@ -278,74 +391,76 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp right_free = detection_result['right_free_space'] obstacle_angle = detection_result['obstacle_angle'] - # 紧急制动情况 + # 如果需要紧急制动，返回最大刹车力度 if detection_result['emergency_brake']: - return 0, 1.0, True + return 0, 1.0, True # (转向, 刹车, 紧急制动标志) - # 基于安全距离的避障决策 + # 1. 动态安全距离：速度越快，需要的安全距离越大 safety_margin = max(3.0, vehicle_speed * 0.5) - # 计算左右侧的安全得分 + # 2. 计算左右两侧的安全得分，用于决策避障方向 + # 得分越高，表示该方向越安全 left_score = (left_clear - safety_margin) + (left_free * 0.1) right_score = (right_clear - safety_margin) + (right_free * 0.1) - # 考虑当前转向的连续性 + # 3. 加入历史决策的权重，防止决策频繁跳动（ hysteresis 机制） if self.last_avoid_direction != 0: - if self.last_avoid_direction == 1: - right_score += 2.0 - else: - left_score += 2.0 + if self.last_avoid_direction == 1: # 上一次是向右避障 + right_score += 2.0 # 给右侧加分 + else: # 上一次是向左避障 + left_score += 2.0 # 给左侧加分 - # 决策避障方向 - avoid_steer = 0.0 - avoid_brake = 0.0 + # 4. 初始化避障控制量 + avoid_steer = 0.0 # 避障所需的转向 + avoid_brake = 0.0 # 避障所需的刹车 + # 5. 根据与障碍物的距离决定是否需要刹车 if min_dist < safety_margin + 2.0: - avoid_brake = 0.3 + (safety_margin - min_dist) * 0.1 - - if right_score > left_score + 1.0: - avoid_steer = -0.5 - self.last_avoid_direction = -1 - elif left_score > right_score + 1.0: - avoid_steer = 0.5 - self.last_avoid_direction = 1 - else: - # 两侧条件相似，基于障碍物角度决策 - if obstacle_angle > 0: - avoid_steer = -0.4 + avoid_brake = 0.3 + (safety_margin - min_dist) * 0.1 # 距离越近，刹车力度越大 + + # 6. 根据安全得分决定避障方向 + if right_score > left_score + 1.0: # 右侧明显更安全 + avoid_steer = -0.5 # 向右转向（CARLA中，负转向值为右转） + self.last_avoid_direction = 1 # 记录这次是向右避障 + elif left_score > right_score + 1.0: # 左侧明显更安全 + avoid_steer = 0.5 # 向左转向（正转向值为左转） + self.last_avoid_direction = -1 # 记录这次是向左避障 + else: # 两侧安全性相近，根据障碍物位置决定 + if obstacle_angle > 0: # 障碍物在左侧 + avoid_steer = -0.4 # 向右避让 self.last_avoid_direction = 1 - else: - avoid_steer = 0.4 + else: # 障碍物在右侧或正前方 + avoid_steer = 0.4 # 向左避让 self.last_avoid_direction = -1 + # 返回最终的避障决策 return avoid_steer, avoid_brake, False - # -------------------------- # 7. 主控制循环 # -------------------------- -# 初始化避障控制器 +# 初始化避障控制器实例 obstacle_avoidance = ObstacleAvoidance() -# 控制状态变量 -throttle = 1.0 # 直接使用最大油门 -steer = 0.0 -brake = 0.0 -waypoint_distance = 8.0 +# 初始化控制状态变量 +throttle = 1.0 # 油门值 (0.0 到 1.0)，初始设为最大 +steer = 0.0 # 转向值 (-1.0 到 1.0) +brake = 0.0 # 刹车值 (0.0 到 1.0) +waypoint_distance = 8.0 # 目标路点距离 -# 获取初始路点 +# 获取车辆初始位置的下一个路点 vehicle_location = vehicle.get_location() waypoint = get_next_waypoint(vehicle_location, waypoint_distance) -# 控制平滑滤波器 -steer_filter = deque(maxlen=3) -throttle_filter = deque(maxlen=2) +# 使用滑动窗口对控制信号进行平滑滤波，减少控制抖动 +steer_filter = deque(maxlen=3) # 存储最近3次的转向值 +throttle_filter = deque(maxlen=2) # 存储最近2次的油门值 print("初始化车辆状态...") -# 确保车辆物理引擎开启 +# 再次确保车辆物理引擎开启 vehicle.set_simulate_physics(True) -# 直接应用强力控制 +# 应用一个初始的强力启动控制，让车辆动起来 print("应用强力启动控制...") vehicle.apply_control(carla.VehicleControl( throttle=1.0, # 最大油门 @@ -358,63 +473,68 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp print("自动驾驶系统启动（强力油门版本）") print("控制键: q-退出, w-加速, s-减速, a-左转向, d-右转向, r-重置方向, 空格-紧急制动") - frame_count = 0 - stuck_count = 0 - last_position = vehicle.get_location() + frame_count = 0 # 帧计数器 + stuck_count = 0 # 车辆卡住状态计数器 + last_position = vehicle.get_location() # 记录上一帧的位置，用于检测是否卡住 + # 主循环，持续运行直到用户退出 while True: + # 触发仿真世界进行一次步进 world.tick() frame_count += 1 + # 获取车辆当前的状态信息 vehicle_transform = vehicle.get_transform() vehicle_location = vehicle.get_location() vehicle_velocity = vehicle.get_velocity() + # 计算车辆当前的速度 (m/s) vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2) - # 每帧都打印状态信息 + # 每帧打印车辆状态信息 print( f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 位置=({vehicle_location.x:.1f}, {vehicle_location.y:.1f})") - # 检测是否卡住 + # 检测车辆是否卡住 current_position = vehicle_location - distance_moved = current_position.distance(last_position) - if distance_moved < 0.1: # 几乎没移动 + distance_moved = current_position.distance(last_position) # 计算与上一帧位置的距离 + if distance_moved < 0.1: # 如果移动距离小于0.1米，认为车辆卡住了 stuck_count += 1 else: - stuck_count = 0 + stuck_count = 0 # 否则重置卡住计数器 - last_position = current_position + last_position = current_position # 更新上一帧位置 - # 如果卡住超过10帧，尝试强力脱困 + # 如果车辆卡住超过10帧，尝试强力脱困 if stuck_count > 10: print("车辆卡住，尝试强力脱困...") - # 先倒车再前进 + # 先短暂倒车 vehicle.apply_control(carla.VehicleControl( throttle=0.0, steer=0.0, brake=1.0, hand_brake=False, - reverse=True + reverse=True # 启用倒车 )) - time.sleep(0.5) + time.sleep(0.5) # 倒车0.5秒 + # 然后向前猛冲 vehicle.apply_control(carla.VehicleControl( throttle=1.0, steer=0.0, brake=0.0, hand_brake=False, - reverse=False + reverse=False # 关闭倒车 )) - stuck_count = 0 + stuck_count = 0 # 重置卡住计数器 - # 更新目标路点 + # 更新目标路点：如果车辆接近当前路点，则获取下一个 current_distance = vehicle_location.distance(waypoint.transform.location) - if current_distance < 4.0: - waypoint = get_next_waypoint(vehicle_location, waypoint_distance) + if current_distance < 4.0: # 如果距离目标路点小于4米 + waypoint = get_next_waypoint(vehicle_location, waypoint_distance) # 获取新的目标路点 - # 计算基础转向角 + # 计算基础转向角：基于当前路点的导航转向 base_steer = calculate_steering_angle(vehicle_transform, waypoint) - # 检测障碍物并决策避障 + # 使用LiDAR数据检测障碍物并生成避障指令 detection_result = obstacle_avoidance.detect_obstacles(lidar_data, vehicle_speed) avoid_steer, avoid_brake, emergency_brake = obstacle_avoidance.decide_avoidance_direction( detection_result, steer, vehicle_speed @@ -422,45 +542,50 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp # 综合控制输出 - 简化逻辑，专注于让车动起来 if emergency_brake: + # 紧急制动状态 throttle = 0.2 brake = 0.1 steer = base_steer * 0.1 print("!!! 紧急制动 !!!") elif detection_result['obstacle_detected']: + # 检测到障碍物，应用避障控制 brake = avoid_brake - throttle = 0.4 # 避障时也保持高油门 + throttle = 0.4 # 避障时也保持较高油门 steer = avoid_steer * 0.1 + base_steer * 0.2 - print("!!! 紧急制动 !!!") + print("!!! 检测到障碍物，准备避障 !!!") elif detection_result['obstacle_detected']: + # （注：此处原代码逻辑有重复，第二个elif条件与第一个相同，可能是笔误。 + # 它永远不会被执行。保留原样以符合用户"代码不要改变"的要求。） brake = avoid_brake throttle = 0.3 # 避障时也保持高油门 steer = avoid_steer * 0.8 + base_steer * 0.2 print(f"避障中 - 距离:{detection_result['min_distance']:.1f}m") else: - # 正常行驶 - 使用强力油门 + # 正常行驶状态，无障碍物 brake = 0.0 steer = base_steer * 0.5 - # 强力油门策略 - if vehicle_speed < 2.0: # 低速时最大油门 + # 强力油门策略：根据当前速度动态调整油门大小 + if vehicle_speed < 2.0: # 低速时，使用较大油门加速 throttle = 0.4 - elif vehicle_speed < 5.0: + elif vehicle_speed < 5.0: # 中低速时，适当减小油门 throttle = 0.3 - elif vehicle_speed < 7.0: + elif vehicle_speed < 7.0: # 中高速时，进一步减小油门 throttle = 0.2 - else: + else: # 高速时，使用最小油门维持速度 throttle = 0.1 - steer = base_steer + steer = base_steer # 使用基础转向角 - # 应用平滑滤波 + # 应用控制信号平滑滤波 steer_filter.append(steer) throttle_filter.append(throttle) + # 计算滤波后的控制信号 smoothed_steer = np.mean(steer_filter) smoothed_throttle = np.mean(throttle_filter) - # 应用强力控制 + # 构造最终的车辆控制命令 control = carla.VehicleControl( throttle=smoothed_throttle, steer=smoothed_steer, @@ -469,66 +594,82 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp reverse=False ) + # 打印即将应用的控制命令 print(f"控制输出: 油门={control.throttle:.2f}, 刹车={control.brake:.2f}, 转向={control.steer:.2f}") + # 将控制命令应用到车辆上 vehicle.apply_control(control) - # 可视化显示 + # 可视化显示：如果相机图像可用 if third_image is not None: + # 创建图像副本，避免修改原始数据 display_image = third_image.copy() + # 在图像上绘制车辆速度信息 cv2.putText(display_image, f"Speed: {vehicle_speed * 3.6:.1f} km/h", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + # 在图像上绘制油门值 cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + # 在图像上绘制转向值 cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + # 根据障碍物检测结果，在图像上绘制不同的状态文本 if detection_result['obstacle_detected']: status_text = f"OBSTACLE: {detection_result['min_distance']:.1f}m" cv2.putText(display_image, status_text, (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) # 红色文本表示有障碍物 else: cv2.putText(display_image, "CLEAR", (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) # 绿色文本表示无障碍物 + # 显示图像窗口 cv2.imshow('第三视角 - 强力油门系统', display_image) + # 监听键盘输入 key = cv2.waitKey(1) & 0xFF if key == ord('q'): - break + break # 按下'q'键退出程序 elif key == ord('w'): - throttle = min(1.0, throttle + 0.1) + throttle = min(1.0, throttle + 0.1) # 按下'w'键增加油门 elif key == ord('s'): - throttle = max(0.0, throttle - 0.1) + throttle = max(0.0, throttle - 0.1) # 按下's'键减少油门 elif key == ord('a'): - steer = max(-1.0, steer - 0.1) + steer = max(-1.0, steer - 0.1) # 按下'a'键向左转 elif key == ord('d'): - steer = min(1.0, steer + 0.1) + steer = min(1.0, steer + 0.1) # 按下'd'键向右转 elif key == ord('r'): - steer = 0.0 + steer = 0.0 # 按下'r'键重置转向 elif key == ord(' '): - brake = 1.0 + brake = 1.0 # 按下空格键紧急制动 throttle = 0.0 + # 短暂休眠，降低CPU占用率（在同步模式下，这个sleep的影响不大，但仍是个好习惯） time.sleep(0.01) except KeyboardInterrupt: + # 如果用户按下Ctrl+C，捕获中断信号并打印信息 print("系统已停止") except Exception as e: + # 如果程序运行中发生其他错误，打印错误信息和堆栈跟踪 print(f"系统错误: {e}") import traceback - traceback.print_exc() finally: + # 程序退出前的清理工作 print("正在清理资源...") + # 停止传感器数据监听 third_camera.stop() lidar.stop() + # 销毁所有生成的车辆和传感器actor for actor in world.get_actors(): if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'): actor.destroy() + # 恢复世界设置为异步模式，方便下次运行 settings.synchronous_mode = False world.apply_settings(settings) + # 关闭所有OpenCV创建的窗口 cv2.destroyAllWindows() print("资源清理完成") \ No newline at end of file From 794595031dc97a6d829e209c3d013ae95c4f8f4e Mon Sep 17 00:00:00 2001 From: chen Date: Mon, 24 Nov 2025 10:35:38 +0800 Subject: [PATCH 13/25] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 229c8028b4..a95dc98faa 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -550,14 +550,9 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp elif detection_result['obstacle_detected']: # 检测到障碍物，应用避障控制 brake = avoid_brake - throttle = 0.4 # 避障时也保持较高油门 - steer = avoid_steer * 0.1 + base_steer * 0.2 - print("!!! 检测到障碍物，准备避障 !!!") - elif detection_result['obstacle_detected']: - # （注：此处原代码逻辑有重复，第二个elif条件与第一个相同，可能是笔误。 - # 它永远不会被执行。保留原样以符合用户"代码不要改变"的要求。） - brake = avoid_brake - throttle = 0.3 # 避障时也保持高油门 + # 动态调整油门：距离越近，油门越小，以获得更好的操控性 + obstacle_distance_factor = max(0.2, min(1.0, detection_result['min_distance'] / 15.0)) + throttle = 0.3 * obstacle_distance_factor # 基础油门0.3，并根据距离动态调整 steer = avoid_steer * 0.8 + base_steer * 0.2 print(f"避障中 - 距离:{detection_result['min_distance']:.1f}m") else: From 569071971d8331eb1f8445810c786baca224af51 Mon Sep 17 00:00:00 2001 From: chen Date: Mon, 24 Nov 2025 11:24:25 +0800 Subject: [PATCH 14/25] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index a95dc98faa..340aa47031 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -494,13 +494,14 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp print( f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 位置=({vehicle_location.x:.1f}, {vehicle_location.y:.1f})") - # 检测车辆是否卡住 + # 检测车辆是否卡住（速度为零或移动极慢） current_position = vehicle_location - distance_moved = current_position.distance(last_position) # 计算与上一帧位置的距离 - if distance_moved < 0.1: # 如果移动距离小于0.1米，认为车辆卡住了 + distance_moved = current_position.distance(last_position) + is_almost_stopped = vehicle_speed < 0.1 # 新增：判断车辆是否几乎静止 + if distance_moved < 0.1 and is_almost_stopped: # 同时满足位置不动和速度为零 stuck_count += 1 else: - stuck_count = 0 # 否则重置卡住计数器 + stuck_count = 0 last_position = current_position # 更新上一帧位置 @@ -568,7 +569,7 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp elif vehicle_speed < 7.0: # 中高速时，进一步减小油门 throttle = 0.2 else: # 高速时，使用最小油门维持速度 - throttle = 0.1 + throttle = 0.2 steer = base_steer # 使用基础转向角 From cce41cc5aa0a13f7e252f1035ebd2e6abd1fe147 Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 25 Nov 2025 10:20:23 +0800 Subject: [PATCH 15/25] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 2460c70268..53690f5187 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -508,22 +508,24 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp # 如果车辆卡住超过10帧，尝试强力脱困 if stuck_count > 10: print("车辆卡住，尝试强力脱困...") - # 先短暂倒车 + # 记录当前转向角，用于脱困时保持方向 + current_steer = steer + # 先短暂倒车，保持当前转向角 vehicle.apply_control(carla.VehicleControl( throttle=0.0, - steer=0.0, + steer=current_steer, # 保持当前转向 brake=1.0, hand_brake=False, - reverse=True # 启用倒车 + reverse=True )) - time.sleep(0.5) # 倒车0.5秒 - # 然后向前猛冲 + time.sleep(0.5) + # 然后向前猛冲，保持相同转向角 vehicle.apply_control(carla.VehicleControl( throttle=1.0, - steer=0.0, + steer=current_steer, # 保持当前转向 brake=0.0, hand_brake=False, - reverse=False # 关闭倒车 + reverse=False )) stuck_count = 0 # 重置卡住计数器 From b3fc1ab9794920c40bb84d51a09850620663d6f7 Mon Sep 17 00:00:00 2001 From: chen Date: Sun, 30 Nov 2025 22:14:02 +0800 Subject: [PATCH 16/25] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 53690f5187..4557e5233c 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -470,7 +470,7 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp )) try: - print("自动驾驶系统启动（强力油门版本）") + print("自动驾驶系统启动（超级强力油门版本）") print("控制键: q-退出, w-加速, s-减速, a-左转向, d-右转向, r-重置方向, 空格-紧急制动") frame_count = 0 # 帧计数器 @@ -507,7 +507,7 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp # 如果车辆卡住超过10帧，尝试强力脱困 if stuck_count > 10: - print("车辆卡住，尝试强力脱困...") + print("车辆卡住，尝试超级强力脱困...") # 记录当前转向角，用于脱困时保持方向 current_steer = steer # 先短暂倒车，保持当前转向角 @@ -555,15 +555,15 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp brake = avoid_brake # 动态调整油门：距离越近，油门越小，以获得更好的操控性 obstacle_distance_factor = max(0.2, min(1.0, detection_result['min_distance'] / 15.0)) - throttle = 0.3 * obstacle_distance_factor # 基础油门0.3，并根据距离动态调整 - throttle = 0.4 # 避障时也保持较高油门 + throttle = 0.5 * obstacle_distance_factor # 基础油门提高到0.5，更猛的避障油门 + throttle = 0.6 # 避障时也保持更高油门 steer = avoid_steer * 0.1 + base_steer * 0.2 print("!!! 检测到障碍物，准备避障 !!!") elif detection_result['obstacle_detected']: # （注：此处原代码逻辑有重复，第二个elif条件与第一个相同，可能是笔误。 # 它永远不会被执行。保留原样以符合用户"代码不要改变"的要求。） brake = avoid_brake - throttle = 0.3 # 避障时也保持高油门 + throttle = 0.4 # 避障时也保持更高油门 steer = avoid_steer * 0.8 + base_steer * 0.2 print(f"避障中 - 距离:{detection_result['min_distance']:.1f}m") else: @@ -571,15 +571,15 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp brake = 0.0 steer = base_steer * 0.5 - # 强力油门策略：根据当前速度动态调整油门大小 - if vehicle_speed < 2.0: # 低速时，使用较大油门加速 + # 超级强力油门策略：根据当前速度动态调整油门大小 + if vehicle_speed < 3.0: # 低速阈值提高到3m/s，使用更大油门加速 + throttle = 0.8 + elif vehicle_speed < 6.0: # 中低速阈值提高到6m/s，适当减小油门 + throttle = 0.6 + elif vehicle_speed < 9.0: # 中高速阈值提高到9m/s，进一步减小油门 throttle = 0.4 - elif vehicle_speed < 5.0: # 中低速时，适当减小油门 + else: # 高速时，使用更高的维持油门 throttle = 0.3 - elif vehicle_speed < 7.0: # 中高速时，进一步减小油门 - throttle = 0.2 - else: # 高速时，使用最小油门维持速度 - throttle = 0.2 steer = base_steer # 使用基础转向角 @@ -629,7 +629,7 @@ def decide_avoidance_direction(self, detection_result, current_steer, vehicle_sp cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) # 绿色文本表示无障碍物 # 显示图像窗口 - cv2.imshow('第三视角 - 强力油门系统', display_image) + cv2.imshow('第三视角 - 超级强力油门系统', display_image) # 监听键盘输入 key = cv2.waitKey(1) & 0xFF From cf6be9d43326ca808e30da7b2e00db6e53ecb797 Mon Sep 17 00:00:00 2001 From: chen Date: Mon, 1 Dec 2025 09:31:01 +0800 Subject: [PATCH 17/25] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 842 ++++++++++++------------------ 1 file changed, 322 insertions(+), 520 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 4557e5233c..28e9d03a43 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -1,681 +1,483 @@ # -------------------------- # 1. 初始化CARLA连接和环境 # -------------------------- -# 导入必要的库 import carla import time import numpy as np import cv2 import math from collections import deque +import torch +import torch.nn as nn +import torch.nn.functional as F +import random +import os + +# 修复1: 简化的神经网络架构 +class SimpleDrivingNetwork(nn.Module): + """ + 简化的驾驶网络 - 更适合实时控制 + """ + + def __init__(self): + super(SimpleDrivingNetwork, self).__init__() + + # 图像处理分支 (简化) + self.conv_layers = nn.Sequential( + nn.Conv2d(3, 8, kernel_size=5, stride=2), + nn.ReLU(), + nn.Conv2d(8, 16, kernel_size=5, stride=2), + nn.ReLU(), + nn.Conv2d(16, 32, kernel_size=3, stride=2), + nn.ReLU(), + nn.AdaptiveAvgPool2d((4, 4)) + ) + + # 状态信息维度: 速度 + 转向历史 + state_dim = 4 + + # 融合层 + self.fc_layers = nn.Sequential( + nn.Linear(32 * 4 * 4 + state_dim, 64), + nn.ReLU(), + nn.Linear(64, 32), + nn.ReLU(), + nn.Linear(32, 3) # [throttle, brake, steer] + ) + + def forward(self, image, state): + # 处理图像 + visual_features = self.conv_layers(image) + visual_features = visual_features.view(visual_features.size(0), -1) + + # 融合特征 + combined = torch.cat([visual_features, state], dim=1) + + # 输出控制 + control = self.fc_layers(combined) + throttle_brake = torch.sigmoid(control[:, :2]) + steer = torch.tanh(control[:, 2:]) + + return torch.cat([throttle_brake, steer], dim=1) + + +# 修复2: 改进的神经网络控制器 +class ImprovedNeuralController: + def __init__(self): + self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') + print(f"使用设备: {self.device}") + + # 使用简化网络 + self.model = SimpleDrivingNetwork().to(self.device) + self.model.eval() + + # 控制历史，用于平滑 + self.control_history = deque(maxlen=5) + + # 修复3: 更保守的初始控制 + self.last_throttle = 0.3 + self.last_brake = 0.0 + self.last_steer = 0.0 + + def preprocess_image(self, image): + """修复图像预处理""" + if image is None: + # 返回黑色图像 + return torch.zeros((1, 3, 120, 160), device=self.device) + + try: + # 调整图像尺寸，减少计算量 + small_img = cv2.resize(image, (160, 120)) + img_tensor = torch.from_numpy(small_img).float().to(self.device) + img_tensor = img_tensor.permute(2, 0, 1).unsqueeze(0) / 255.0 + return img_tensor + except Exception as e: + print(f"图像预处理错误: {e}") + return torch.zeros((1, 3, 120, 160), device=self.device) + + def preprocess_state(self, speed, steer_history): + """修复状态预处理""" + state_data = [ + speed / 20.0, # 归一化速度 + steer_history[-1] if steer_history else 0.0, # 最近转向 + steer_history[-2] if len(steer_history) > 1 else 0.0, # 前一次转向 + np.mean(steer_history) if steer_history else 0.0 # 平均转向 + ] + return torch.tensor(state_data, device=self.device).unsqueeze(0) + + def get_control(self, image, speed, steer_history): + """修复控制生成逻辑""" + try: + with torch.no_grad(): + # 预处理 + img_tensor = self.preprocess_image(image) + state_tensor = self.preprocess_state(speed, steer_history) + + # 神经网络推理 + control_output = self.model(img_tensor, state_tensor) + + # 提取控制指令 + throttle = control_output[0, 0].item() + brake = control_output[0, 1].item() + steer = control_output[0, 2].item() + + # 修复4: 添加安全限制 + throttle = max(0.0, min(0.8, throttle)) # 限制最大油门 + brake = max(0.0, min(0.5, brake)) # 限制最大刹车 + steer = max(-0.5, min(0.5, steer)) # 限制转向幅度 + + return throttle, brake, steer + + except Exception as e: + print(f"神经网络控制错误: {e}") + # 返回安全默认值 + return 0.3, 0.0, 0.0 + + +# 修复5: 传统控制器作为备份 +class TraditionalController: + """可靠的传统控制逻辑""" + + def __init__(self, world): + self.world = world + self.map = world.get_map() + self.waypoint_distance = 10.0 + self.last_waypoint = None + + def get_control(self, vehicle): + """基于路点的传统控制""" + # 获取车辆状态 + transform = vehicle.get_transform() + location = vehicle.get_location() + velocity = vehicle.get_velocity() + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) + + # 获取路点 + waypoint = self.map.get_waypoint(location, project_to_road=True) + next_waypoints = waypoint.next(self.waypoint_distance) + + if not next_waypoints: + # 如果没有找到路点，尝试获取当前路点 + next_waypoints = [waypoint] + + target_waypoint = next_waypoints[0] + self.last_waypoint = target_waypoint + + # 计算转向 + vehicle_yaw = math.radians(transform.rotation.yaw) + target_loc = target_waypoint.transform.location + + dx = target_loc.x - location.x + dy = target_loc.y - location.y + + local_x = dx * math.cos(vehicle_yaw) + dy * math.sin(vehicle_yaw) + local_y = -dx * math.sin(vehicle_yaw) + dy * math.cos(vehicle_yaw) + + if abs(local_x) < 0.1: + steer = 0.0 + else: + angle = math.atan2(local_y, local_x) + steer = np.clip(angle / math.radians(45), -1.0, 1.0) + + # 速度控制 + if speed < 5.0: # 18 km/h + throttle = 0.6 + brake = 0.0 + elif speed < 10.0: # 36 km/h + throttle = 0.3 + brake = 0.0 + else: + throttle = 0.1 + brake = 0.1 + + return throttle, brake, steer + + +# CARLA初始化部分保持不变... # 连接到本地CARLA服务器，端口2000 client = carla.Client('localhost', 2000) -# 设置超时时间为15秒 client.set_timeout(15.0) -# 加载名为'Town01'的地图 world = client.load_world('Town01') # 获取并设置世界的运行参数 settings = world.get_settings() -# 启用同步模式，这意味着仿真将等待客户端的每一个tick信号 settings.synchronous_mode = True -# 设置固定的时间步长，单位为秒 settings.fixed_delta_seconds = 0.1 -# 启用子步进，用于更精确的物理模拟 -settings.substepping = True -# 设置子步进的最大时间步长 -settings.max_substep_delta_time = 0.01 -# 设置最大子步进次数 -settings.max_substeps = 10 -# 应用这些设置 world.apply_settings(settings) # 定义天气参数 weather = carla.WeatherParameters( - cloudiness=30.0, # 云量 - precipitation=0.0, # 降雨量 - sun_altitude_angle=70.0 # 太阳高度角 + cloudiness=30.0, + precipitation=0.0, + sun_altitude_angle=70.0 ) -# 应用天气设置 world.set_weather(weather) -# 获取地图对象 +# 获取地图和出生点 map = world.get_map() -# 获取地图中所有可用的出生点 spawn_points = map.get_spawn_points() if not spawn_points: - raise Exception("No spawn points available") # 如果没有找到出生点则抛出异常 + raise Exception("No spawn points available") -# 选择一个更好的出生点（索引为10的点） -spawn_point = spawn_points[10] # 选择更靠前的出生点 +# 选择更合适的出生点 +spawn_point = spawn_points[10] -# -------------------------- -# 2. 生成车辆和障碍物 -# -------------------------- -# 获取蓝图库，包含所有可生成的actor的蓝图 +# 生成车辆 blueprint_library = world.get_blueprint_library() - -# 主车辆（红色特斯拉Model3） -# 查找特斯拉Model3的车辆蓝图 vehicle_bp = blueprint_library.find('vehicle.tesla.model3') -# 设置车辆颜色为红色 (RGB) vehicle_bp.set_attribute('color', '255,0,0') -# 在指定的出生点生成主车辆 vehicle = world.spawn_actor(vehicle_bp, spawn_point) + if not vehicle: raise Exception("无法生成主车辆") -# 禁用车辆的自动驾驶模式 + vehicle.set_autopilot(False) -# 确保车辆的物理模拟是开启的 vehicle.set_simulate_physics(True) print(f"车辆生成在位置: {spawn_point.location}") # 生成障碍物车辆 -obstacle_count = 3 # 计划生成3个障碍物 +obstacle_count = 3 for i in range(obstacle_count): if i >= len(spawn_points): break - # 过滤出所有车辆类型的蓝图 other_vehicles = blueprint_library.filter('vehicle.*') - # 随机选择一个车辆蓝图 other_vehicle_bp = np.random.choice(other_vehicles) - # 选择一个与主车辆不同的出生点 spawn_idx = (i + 15) % len(spawn_points) - # 尝试在指定位置生成障碍物车辆 other_vehicle = world.try_spawn_actor(other_vehicle_bp, spawn_points[spawn_idx]) if other_vehicle: - # 为障碍物车辆启用自动驾驶 other_vehicle.set_autopilot(True) -# -------------------------- -# 3. 配置传感器 -# -------------------------- -# 配置第三视角RGB相机 -# 查找RGB相机的传感器蓝图 +# 配置传感器（简化配置） third_camera_bp = blueprint_library.find('sensor.camera.rgb') -# 设置相机图像宽度 third_camera_bp.set_attribute('image_size_x', '640') -# 设置相机图像高度 third_camera_bp.set_attribute('image_size_y', '480') -# 设置相机视场角 third_camera_bp.set_attribute('fov', '110') -# 定义相机相对于车辆的变换（位置和旋转） third_camera_transform = carla.Transform( - carla.Location(x=-5.0, y=0.0, z=3.0), # 在车后5米，高3米处 - carla.Rotation(pitch=-15.0) # 向下倾斜15度 + carla.Location(x=-5.0, y=0.0, z=3.0), + carla.Rotation(pitch=-15.0) ) -# 生成相机传感器并将其附加到主车辆上 third_camera = world.spawn_actor(third_camera_bp, third_camera_transform, attach_to=vehicle) -# 配置激光雷达 (LiDAR) -# 查找激光雷达的传感器蓝图 -lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') -# 设置激光雷达的通道数（线数） -lidar_bp.set_attribute('channels', '32') -# 设置激光雷达的最大探测范围 -lidar_bp.set_attribute('range', '50') -# 设置激光雷达的每秒点数 -lidar_bp.set_attribute('points_per_second', '100000') -# 设置激光雷达的旋转频率 -lidar_bp.set_attribute('rotation_frequency', '10') -# 设置激光雷达的上视场角 -lidar_bp.set_attribute('upper_fov', '15') -# 设置激光雷达的下视场角 -lidar_bp.set_attribute('lower_fov', '-25') -# 定义激光雷达相对于车辆的变换 -lidar_transform = carla.Transform(carla.Location(x=0.0, z=2.5)) # 在车顶中心，高2.5米处 -# 生成激光雷达传感器并将其附加到主车辆上 -lidar = world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle) +front_camera_bp = blueprint_library.find('sensor.camera.rgb') +front_camera_bp.set_attribute('image_size_x', '640') +front_camera_bp.set_attribute('image_size_y', '480') +front_camera_bp.set_attribute('fov', '90') +front_camera_transform = carla.Transform( + carla.Location(x=2.0, y=0.0, z=1.5), + carla.Rotation(pitch=0.0) +) +front_camera = world.spawn_actor(front_camera_bp, front_camera_transform, attach_to=vehicle) + +# 传感器数据存储 +third_image = None +front_image = None -# -------------------------- -# 4. 传感器数据处理 -# -------------------------- -# 全局变量，用于存储来自传感器的最新数据 -third_image = None # 存储相机图像 -lidar_data = None # 存储激光雷达点云 -# 相机数据回调函数 def third_camera_callback(image): - # 使用global关键字修改全局变量 global third_image - # 将原始图像数据（CARLA的Image对象）转换为numpy数组 array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - # 重塑数组形状以匹配图像的尺寸 (高度, 宽度, 4通道RGBA) array = np.reshape(array, (image.height, image.width, 4)) - # 提取前3个通道（RGB）并存储 third_image = array[:, :, :3] -# 激光雷达数据回调函数 -def lidar_callback(point_cloud): - # 使用global关键字修改全局变量 - global lidar_data - # 将原始点云数据转换为numpy数组，每个点由4个32位浮点数表示 (x, y, z, intensity) - data = np.frombuffer(point_cloud.raw_data, dtype=np.dtype('f4')) - # 重塑数组形状，每行代表一个点 (x, y, z, intensity) - data = np.reshape(data, (int(data.shape[0] / 4), 4)) - # 存储处理后的点云数据 - lidar_data = data - -# 开始监听传感器数据，设置回调函数 -third_camera.listen(third_camera_callback) -lidar.listen(lidar_callback) - -# 等待2秒，确保传感器有足够的时间进行初始化并返回第一帧数据 -time.sleep(2.0) # 增加等待时间确保传感器初始化 - -# -------------------------- -# 5. 路径规划与导航逻辑 -# -------------------------- -def get_next_waypoint(vehicle_location, distance=8.0): - """ - 获取车辆前方指定距离的路点 - :param vehicle_location: 车辆当前的位置 - :param distance: 希望获取的下一个路点距离当前车辆的距离 - :return: 下一个目标路点 (carla.Waypoint) - """ - # 获取车辆当前位置所在的路点，并将位置投影到道路中心线上 - waypoint = map.get_waypoint(vehicle_location, project_to_road=True) - - # 尝试获取当前路点前方distance米处的路点 - next_waypoints = waypoint.next(distance) - if next_waypoints: - return next_waypoints[0] # 返回第一个找到的路点 - - # 如果当前路点在路口(junction)，进行特殊处理 - if waypoint.is_junction: - # 尝试在同一条道路上寻找下一个路点 - for wp in waypoint.next(distance): - if wp.road_id == waypoint.road_id: - return wp - - # 如果找不到同一条路的路点，则尝试变道 - # 检查是否可以向右变道 - if waypoint.lane_change & carla.LaneChange.Right: - right_way = waypoint.get_right_lane() - if right_way: - return right_way.next(distance)[0] - # 检查是否可以向左变道 - elif waypoint.lane_change & carla.LaneChange.Left: - left_way = waypoint.get_left_lane() - if left_way: - return left_way.next(distance)[0] - - # 如果以上方法都失败，返回当前路点 - return waypoint - -def calculate_steering_angle(vehicle_transform, target_waypoint): - """ - 计算到达目标路点所需的转向角 - :param vehicle_transform: 车辆当前的变换（位置和旋转） - :param target_waypoint: 目标路点 - :return: 归一化的转向指令 (-1.0 到 1.0 之间) - """ - # 获取车辆当前位置和目标路点位置 - vehicle_location = vehicle_transform.location - target_location = target_waypoint.transform.location - - # 将车辆的偏航角（yaw）从度转换为弧度 - vehicle_yaw = math.radians(vehicle_transform.rotation.yaw) - - # 计算目标位置相对于车辆位置的向量 (dx, dy) - dx = target_location.x - vehicle_location.x - dy = target_location.y - vehicle_location.y - # 将目标位置向量从世界坐标系转换到车辆局部坐标系 - # 这使得我们可以更容易地判断目标在车辆的左、右还是正前方 - local_x = dx * math.cos(vehicle_yaw) + dy * math.sin(vehicle_yaw) - local_y = -dx * math.sin(vehicle_yaw) + dy * math.cos(vehicle_yaw) - - # 如果目标在车辆正后方（或非常接近），返回0转向角以避免除以零 - if abs(local_x) < 0.1: - return 0.0 - - # 计算目标方向与车辆当前朝向的夹角 - angle = math.atan2(local_y, local_x) - - # 定义最大转向角（例如60度），并将计算出的角度归一化到[-1, 1]范围 - max_angle = math.radians(60) - steering = angle / max_angle - - # 使用np.clip确保转向值在有效范围内 - return np.clip(steering, -1.0, 1.0) - -# -------------------------- -# 6. 优化避障控制逻辑 -# -------------------------- -class ObstacleAvoidance: - """ - 障碍物检测与避障决策类 - 封装了使用LiDAR数据进行障碍物检测和生成避障指令的逻辑 - """ - def __init__(self): - """初始化避障控制器""" - # 用于存储障碍物检测历史，防止频繁切换决策 - self.obstacle_history = deque(maxlen=10) - # 紧急制动标志 - self.emergency_brake = False - # 记录上一次的避障方向，用于决策的连续性 - self.last_avoid_direction = 0 # -1: 左, 0: 无, 1: 右 - - def detect_obstacles(self, lidar_data, vehicle_speed): - """ - 修复版的障碍物检测算法 - 处理LiDAR点云数据，检测前方障碍物并分析其位置 - :param lidar_data: 原始的LiDAR点云数据 (numpy array) - :param vehicle_speed: 车辆当前速度 (m/s) - :return: 包含障碍物检测结果的字典 - """ - # 如果没有LiDAR数据，返回默认结果（无障碍物） - if lidar_data is None: - return self._get_default_detection_result() - - # 如果点云为空，返回默认结果 - if len(lidar_data) == 0: - return self._get_default_detection_result() +def front_camera_callback(image): + global front_image + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + front_image = array[:, :, :3] - try: - # 1. 地面过滤：移除高度低于阈值的点，减少地面点的干扰 - ground_threshold = -0.5 # 地面阈值，单位为米 - valid_mask = lidar_data[:, 2] > ground_threshold # z坐标大于阈值的点被认为是有效的 - valid_points = lidar_data[valid_mask] - - if len(valid_points) == 0: - return self._get_default_detection_result() - - # 2. 计算每个有效点相对于车辆的距离和角度 - # 计算x-y平面上的距离 - distances = np.sqrt(valid_points[:, 0] ** 2 + valid_points[:, 1] ** 2) - # 计算与车辆x轴正方向的夹角（弧度） - angles = np.arctan2(valid_points[:, 1], valid_points[:, 0]) - - # 3. 定义感兴趣区域（ROI）：只关注车辆正前方的区域 - front_angle_range = np.radians(75) # 前方75度的范围 - # 过滤出在角度范围内且距离大于1米（避免过近的噪声点）的点 - front_mask = (np.abs(angles) <= front_angle_range) & (distances > 1.0) - - front_points = valid_points[front_mask] - front_distances = distances[front_mask] - front_angles = angles[front_mask] - - if len(front_points) == 0: - return self._get_default_detection_result() - - # 4. 分区域检测：将前方区域分为近、中、远三个区域 - near_zone = front_distances < 8.0 # 近区：0-8米 - mid_zone = (front_distances >= 8.0) & (front_distances < 20.0) # 中区：8-20米 - far_zone = (front_distances >= 20.0) & (front_distances < 35.0) # 远区：20-35米 - - # 5. 紧急制动检测：检测近区中非常接近的障碍物 - emergency_points = front_points[near_zone & (front_distances < 4.0)] # 4米内的点 - self.emergency_brake = len(emergency_points) > 10 # 如果超过10个点，则触发紧急制动 - - # 6. 计算最近障碍物的距离和角度 - min_distance = np.min(front_distances) - min_idx = np.argmin(front_distances) - obstacle_angle = front_angles[min_idx] if len(front_angles) > min_idx else 0.0 - - # 7. 分左右区域分析障碍物分布 - left_points_distances = front_distances[front_angles > 0] # 车辆左侧的点（角度为正） - right_points_distances = front_distances[front_angles < 0] # 车辆右侧的点（角度为负） - - # 计算左右两侧最近障碍物的距离 - left_min = np.min(left_points_distances) if len(left_points_distances) > 0 else float('inf') - right_min = np.min(right_points_distances) if len(right_points_distances) > 0 else float('inf') - - # 8. 计算左右两侧的自由空间（无障碍物的区域大小） - safe_threshold = 15.0 # 认为15米外是安全的 - # 统计左右两侧距离大于安全阈值的点的数量，作为自由空间的度量 - left_free = np.sum(left_points_distances > safe_threshold) if len(left_points_distances) > 0 else 1000 - right_free = np.sum(right_points_distances > safe_threshold) if len(right_points_distances) > 0 else 1000 - - # 9. 判断是否检测到障碍物的综合条件 - obstacle_detected = (np.sum(near_zone) > 5 or # 近区有超过5个点 - np.sum(mid_zone) > 10 or # 中区有超过10个点 - min_distance < 12.0) # 最近障碍物距离小于12米 - - # 返回包含所有检测信息的字典 - return { - 'obstacle_detected': obstacle_detected, - 'min_distance': min_distance, - 'left_clearance': left_min, - 'right_clearance': right_min, - 'left_free_space': left_free, - 'right_free_space': right_free, - 'obstacle_angle': obstacle_angle, - 'emergency_brake': self.emergency_brake - } - except Exception as e: - # 如果检测过程中发生错误，打印错误信息并返回默认结果 - print(f"障碍物检测错误: {e}") - return self._get_default_detection_result() - - def _get_default_detection_result(self): - """ - 返回默认的检测结果（无障碍物） - 这是一个辅助函数，用于在没有数据或出错时提供一个安全的默认值 - :return: 默认的检测结果字典 - """ - return { - 'obstacle_detected': False, - 'min_distance': 30.0, - 'left_clearance': float('inf'), - 'right_clearance': float('inf'), - 'left_free_space': 1000, - 'right_free_space': 1000, - 'obstacle_angle': 0.0, - 'emergency_brake': False - } - - def decide_avoidance_direction(self, detection_result, current_steer, vehicle_speed): - """ - 避障决策逻辑 - 根据障碍物检测结果，决定车辆应采取的避障动作（转向和刹车） - :param detection_result: 障碍物检测结果字典 - :param current_steer: 当前的转向值 - :param vehicle_speed: 当前车辆速度 - :return: (避障转向值, 避障刹车值, 是否紧急制动) - """ - # 如果没有检测到障碍物，重置状态并返回无动作 - if not detection_result['obstacle_detected']: - self.last_avoid_direction = 0 - return 0, 0, False - - # 从检测结果中提取关键信息 - min_dist = detection_result['min_distance'] - left_clear = detection_result['left_clearance'] - right_clear = detection_result['right_clearance'] - left_free = detection_result['left_free_space'] - right_free = detection_result['right_free_space'] - obstacle_angle = detection_result['obstacle_angle'] - - # 如果需要紧急制动，返回最大刹车力度 - if detection_result['emergency_brake']: - return 0, 1.0, True # (转向, 刹车, 紧急制动标志) - - # 1. 动态安全距离：速度越快，需要的安全距离越大 - safety_margin = max(3.0, vehicle_speed * 0.5) - - # 2. 计算左右两侧的安全得分，用于决策避障方向 - # 得分越高，表示该方向越安全 - left_score = (left_clear - safety_margin) + (left_free * 0.1) - right_score = (right_clear - safety_margin) + (right_free * 0.1) - - # 3. 加入历史决策的权重，防止决策频繁跳动（ hysteresis 机制） - if self.last_avoid_direction != 0: - if self.last_avoid_direction == 1: # 上一次是向右避障 - right_score += 2.0 # 给右侧加分 - else: # 上一次是向左避障 - left_score += 2.0 # 给左侧加分 - - # 4. 初始化避障控制量 - avoid_steer = 0.0 # 避障所需的转向 - avoid_brake = 0.0 # 避障所需的刹车 - - # 5. 根据与障碍物的距离决定是否需要刹车 - if min_dist < safety_margin + 2.0: - avoid_brake = 0.3 + (safety_margin - min_dist) * 0.1 # 距离越近，刹车力度越大 - - # 6. 根据安全得分决定避障方向 - if right_score > left_score + 1.0: # 右侧明显更安全 - avoid_steer = -0.5 # 向右转向（CARLA中，负转向值为右转） - self.last_avoid_direction = 1 # 记录这次是向右避障 - elif left_score > right_score + 1.0: # 左侧明显更安全 - avoid_steer = 0.5 # 向左转向（正转向值为左转） - self.last_avoid_direction = -1 # 记录这次是向左避障 - else: # 两侧安全性相近，根据障碍物位置决定 - if obstacle_angle > 0: # 障碍物在左侧 - avoid_steer = -0.4 # 向右避让 - self.last_avoid_direction = 1 - else: # 障碍物在右侧或正前方 - avoid_steer = 0.4 # 向左避让 - self.last_avoid_direction = -1 - - # 返回最终的避障决策 - return avoid_steer, avoid_brake, False +third_camera.listen(third_camera_callback) +front_camera.listen(front_camera_callback) -# -------------------------- -# 7. 主控制循环 -# -------------------------- -# 初始化避障控制器实例 -obstacle_avoidance = ObstacleAvoidance() +time.sleep(2.0) -# 初始化控制状态变量 -throttle = 1.0 # 油门值 (0.0 到 1.0)，初始设为最大 -steer = 0.0 # 转向值 (-1.0 到 1.0) -brake = 0.0 # 刹车值 (0.0 到 1.0) -waypoint_distance = 8.0 # 目标路点距离 +# 修复6: 初始化控制器 +nn_controller = ImprovedNeuralController() +traditional_controller = TraditionalController(world) -# 获取车辆初始位置的下一个路点 -vehicle_location = vehicle.get_location() -waypoint = get_next_waypoint(vehicle_location, waypoint_distance) +# 控制变量 +throttle = 0.3 # 更保守的初始油门 +steer = 0.0 +brake = 0.0 +NEURAL_NETWORK_MODE = False # 默认使用传统控制，更稳定 -# 使用滑动窗口对控制信号进行平滑滤波，减少控制抖动 -steer_filter = deque(maxlen=3) # 存储最近3次的转向值 -throttle_filter = deque(maxlen=2) # 存储最近2次的油门值 +# 转向历史，用于平滑 +steer_history = deque(maxlen=10) print("初始化车辆状态...") -# 再次确保车辆物理引擎开启 vehicle.set_simulate_physics(True) -# 应用一个初始的强力启动控制，让车辆动起来 -print("应用强力启动控制...") +# 修复7: 更温和的启动控制 +print("应用启动控制...") vehicle.apply_control(carla.VehicleControl( - throttle=1.0, # 最大油门 + throttle=0.5, # 降低初始油门 steer=0.0, brake=0.0, hand_brake=False )) try: - print("自动驾驶系统启动（超级强力油门版本）") - print("控制键: q-退出, w-加速, s-减速, a-左转向, d-右转向, r-重置方向, 空格-紧急制动") + print("自动驾驶系统启动 - 初始模式: 传统控制") + print("控制键: q-退出, m-切换控制模式, r-重置车辆, t-传统模式, n-神经网络模式") - frame_count = 0 # 帧计数器 - stuck_count = 0 # 车辆卡住状态计数器 - last_position = vehicle.get_location() # 记录上一帧的位置，用于检测是否卡住 + frame_count = 0 + stuck_count = 0 + last_position = vehicle.get_location() + success_count = 0 # 成功运行计数器 - # 主循环，持续运行直到用户退出 + # 主循环 while True: - # 触发仿真世界进行一次步进 world.tick() frame_count += 1 - # 获取车辆当前的状态信息 + # 获取车辆状态 vehicle_transform = vehicle.get_transform() vehicle_location = vehicle.get_location() vehicle_velocity = vehicle.get_velocity() - # 计算车辆当前的速度 (m/s) vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2) - # 每帧打印车辆状态信息 print( - f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 位置=({vehicle_location.x:.1f}, {vehicle_location.y:.1f})") + f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}") - # 检测车辆是否卡住（速度为零或移动极慢） + # 修复8: 改进的卡住检测 current_position = vehicle_location distance_moved = current_position.distance(last_position) - is_almost_stopped = vehicle_speed < 0.1 # 新增：判断车辆是否几乎静止 - if distance_moved < 0.1 and is_almost_stopped: # 同时满足位置不动和速度为零 + + # 更精确的卡住检测 + is_moving = distance_moved > 0.2 or vehicle_speed > 1.0 + if not is_moving: stuck_count += 1 else: stuck_count = 0 + success_count += 1 # 成功运行一帧 + + last_position = current_position - last_position = current_position # 更新上一帧位置 + # 修复9: 更智能的卡住恢复 + if stuck_count > 15: # 1.5秒后认为卡住 + print("检测到车辆卡住，执行恢复程序...") - # 如果车辆卡住超过10帧，尝试强力脱困 - if stuck_count > 10: - print("车辆卡住，尝试超级强力脱困...") - # 记录当前转向角，用于脱困时保持方向 - current_steer = steer - # 先短暂倒车，保持当前转向角 + # 先完全停止 vehicle.apply_control(carla.VehicleControl( - throttle=0.0, - steer=current_steer, # 保持当前转向 - brake=1.0, - hand_brake=False, - reverse=True + throttle=0.0, steer=0.0, brake=1.0, hand_brake=True )) time.sleep(0.5) - # 然后向前猛冲，保持相同转向角 + + # 然后尝试不同方向的脱困 + recovery_steer = random.choice([-0.5, 0.5]) # 随机选择方向 vehicle.apply_control(carla.VehicleControl( - throttle=1.0, - steer=current_steer, # 保持当前转向 - brake=0.0, - hand_brake=False, - reverse=False + throttle=0.8, steer=recovery_steer, brake=0.0, hand_brake=False )) - stuck_count = 0 # 重置卡住计数器 + time.sleep(1.0) - # 更新目标路点：如果车辆接近当前路点，则获取下一个 - current_distance = vehicle_location.distance(waypoint.transform.location) - if current_distance < 4.0: # 如果距离目标路点小于4米 - waypoint = get_next_waypoint(vehicle_location, waypoint_distance) # 获取新的目标路点 + stuck_count = 0 + success_count = 0 - # 计算基础转向角：基于当前路点的导航转向 - base_steer = calculate_steering_angle(vehicle_transform, waypoint) + # 每成功运行100帧显示一次状态 + if success_count % 100 == 0: + print(f"已成功运行 {success_count} 帧") - # 使用LiDAR数据检测障碍物并生成避障指令 - detection_result = obstacle_avoidance.detect_obstacles(lidar_data, vehicle_speed) - avoid_steer, avoid_brake, emergency_brake = obstacle_avoidance.decide_avoidance_direction( - detection_result, steer, vehicle_speed - ) + # 控制逻辑 + if NEURAL_NETWORK_MODE: + # 神经网络控制 + nn_throttle, nn_brake, nn_steer = nn_controller.get_control( + front_image, vehicle_speed, steer_history + ) - # 综合控制输出 - 简化逻辑，专注于让车动起来 - if emergency_brake: - # 紧急制动状态 - throttle = 0.2 - brake = 0.1 - steer = base_steer * 0.1 - print("!!! 紧急制动 !!!") - elif detection_result['obstacle_detected']: - # 检测到障碍物，应用避障控制 - brake = avoid_brake - # 动态调整油门：距离越近，油门越小，以获得更好的操控性 - obstacle_distance_factor = max(0.2, min(1.0, detection_result['min_distance'] / 15.0)) - throttle = 0.5 * obstacle_distance_factor # 基础油门提高到0.5，更猛的避障油门 - throttle = 0.6 # 避障时也保持更高油门 - steer = avoid_steer * 0.1 + base_steer * 0.2 - print("!!! 检测到障碍物，准备避障 !!!") - elif detection_result['obstacle_detected']: - # （注：此处原代码逻辑有重复，第二个elif条件与第一个相同，可能是笔误。 - # 它永远不会被执行。保留原样以符合用户"代码不要改变"的要求。） - brake = avoid_brake - throttle = 0.4 # 避障时也保持更高油门 - steer = avoid_steer * 0.8 + base_steer * 0.2 - print(f"避障中 - 距离:{detection_result['min_distance']:.1f}m") - else: - # 正常行驶状态，无障碍物 - brake = 0.0 - steer = base_steer * 0.5 - - # 超级强力油门策略：根据当前速度动态调整油门大小 - if vehicle_speed < 3.0: # 低速阈值提高到3m/s，使用更大油门加速 - throttle = 0.8 - elif vehicle_speed < 6.0: # 中低速阈值提高到6m/s，适当减小油门 - throttle = 0.6 - elif vehicle_speed < 9.0: # 中高速阈值提高到9m/s，进一步减小油门 - throttle = 0.4 - else: # 高速时，使用更高的维持油门 - throttle = 0.3 + # 修复10: 更激进的控制平滑 + throttle = 0.3 * throttle + 0.7 * nn_throttle + brake = 0.3 * brake + 0.7 * nn_brake + steer = 0.2 * steer + 0.8 * nn_steer - steer = base_steer # 使用基础转向角 + # 记录转向历史 + steer_history.append(steer) - # 应用控制信号平滑滤波 - steer_filter.append(steer) - throttle_filter.append(throttle) - - # 计算滤波后的控制信号 - smoothed_steer = np.mean(steer_filter) - smoothed_throttle = np.mean(throttle_filter) + else: + # 传统控制 - 更稳定 + throttle, brake, steer = traditional_controller.get_control(vehicle) + steer_history.append(steer) - # 构造最终的车辆控制命令 + # 应用控制 control = carla.VehicleControl( - throttle=smoothed_throttle, - steer=smoothed_steer, + throttle=throttle, + steer=steer, brake=brake, hand_brake=False, reverse=False ) - # 打印即将应用的控制命令 - print(f"控制输出: 油门={control.throttle:.2f}, 刹车={control.brake:.2f}, 转向={control.steer:.2f}") - # 将控制命令应用到车辆上 vehicle.apply_control(control) - # 可视化显示：如果相机图像可用 + # 显示和输入处理 if third_image is not None: - # 创建图像副本，避免修改原始数据 display_image = third_image.copy() - # 在图像上绘制车辆速度信息 + + # 显示信息 cv2.putText(display_image, f"Speed: {vehicle_speed * 3.6:.1f} km/h", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - # 在图像上绘制油门值 - cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 60), + cv2.putText(display_image, f"Mode: {'Neural' if NEURAL_NETWORK_MODE else 'Traditional'}", + (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - # 在图像上绘制转向值 - cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 90), + cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + cv2.putText(display_image, f"Brake: {brake:.2f}", (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - # 根据障碍物检测结果，在图像上绘制不同的状态文本 - if detection_result['obstacle_detected']: - status_text = f"OBSTACLE: {detection_result['min_distance']:.1f}m" - cv2.putText(display_image, status_text, (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) # 红色文本表示有障碍物 - else: - cv2.putText(display_image, "CLEAR", (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) # 绿色文本表示无障碍物 + # 卡住警告 + if stuck_count > 5: + cv2.putText(display_image, "STUCK DETECTED!", (10, 180), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - # 显示图像窗口 - cv2.imshow('第三视角 - 超级强力油门系统', display_image) + cv2.imshow('自动驾驶系统 - 修复版', display_image) - # 监听键盘输入 key = cv2.waitKey(1) & 0xFF if key == ord('q'): - break # 按下'q'键退出程序 - elif key == ord('w'): - throttle = min(1.0, throttle + 0.1) # 按下'w'键增加油门 - elif key == ord('s'): - throttle = max(0.0, throttle - 0.1) # 按下's'键减少油门 - elif key == ord('a'): - steer = max(-1.0, steer - 0.1) # 按下'a'键向左转 - elif key == ord('d'): - steer = min(1.0, steer + 0.1) # 按下'd'键向右转 + break + elif key == ord('m'): + NEURAL_NETWORK_MODE = not NEURAL_NETWORK_MODE + print(f"切换到{'神经网络' if NEURAL_NETWORK_MODE else '传统'}控制模式") + elif key == ord('t'): + NEURAL_NETWORK_MODE = False + print("切换到传统控制模式") + elif key == ord('n'): + NEURAL_NETWORK_MODE = True + print("切换到神经网络控制模式") elif key == ord('r'): - steer = 0.0 # 按下'r'键重置转向 - elif key == ord(' '): - brake = 1.0 # 按下空格键紧急制动 - throttle = 0.0 + # 重置车辆 + vehicle.set_transform(spawn_point) + throttle = 0.3 + steer = 0.0 + brake = 1.0 + stuck_count = 0 + success_count = 0 + steer_history.clear() - # 短暂休眠，降低CPU占用率（在同步模式下，这个sleep的影响不大，但仍是个好习惯） time.sleep(0.01) except KeyboardInterrupt: - # 如果用户按下Ctrl+C，捕获中断信号并打印信息 print("系统已停止") except Exception as e: - # 如果程序运行中发生其他错误，打印错误信息和堆栈跟踪 print(f"系统错误: {e}") import traceback + traceback.print_exc() finally: - # 程序退出前的清理工作 print("正在清理资源...") - # 停止传感器数据监听 third_camera.stop() - lidar.stop() + front_camera.stop() - # 销毁所有生成的车辆和传感器actor + # 销毁actor for actor in world.get_actors(): if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'): actor.destroy() - # 恢复世界设置为异步模式，方便下次运行 + # 恢复设置 settings.synchronous_mode = False world.apply_settings(settings) - # 关闭所有OpenCV创建的窗口 cv2.destroyAllWindows() print("资源清理完成") \ No newline at end of file From aeb3661b2c88ce5c747907459f889987e36cdf0a Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 2 Dec 2025 09:12:42 +0800 Subject: [PATCH 18/25] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=B0=8F=E8=BD=A6?= =?UTF-8?q?=E5=92=8Cnpc=E5=8F=91=E7=94=9F=E7=A2=B0=E6=92=9E=E9=97=AE?= =?UTF-8?q?=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/README.md | 2 + src/car_navigation_system/main.py | 433 +++++++++++++++++++++++++--- 2 files changed, 393 insertions(+), 42 deletions(-) diff --git a/src/car_navigation_system/README.md b/src/car_navigation_system/README.md index 171c7bf327..4580a6a70c 100644 --- a/src/car_navigation_system/README.md +++ b/src/car_navigation_system/README.md @@ -19,6 +19,8 @@ pip install carla numpy opencv-python matplotlib pip install setuptools==40.2.0 pip insatll wheel + pip insatll torch + pip install random ``` ## 快速启动 diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 28e9d03a43..08181021ad 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -14,10 +14,10 @@ import os -# 修复1: 简化的神经网络架构 +# 修复1: 简化的神经网络架构（增加障碍物检测通道） class SimpleDrivingNetwork(nn.Module): """ - 简化的驾驶网络 - 更适合实时控制 + 简化的驾驶网络 - 增加障碍物感知 """ def __init__(self): @@ -34,12 +34,15 @@ def __init__(self): nn.AdaptiveAvgPool2d((4, 4)) ) - # 状态信息维度: 速度 + 转向历史 - state_dim = 4 + # 状态信息维度: 速度 + 转向历史 + 障碍物信息 + state_dim = 7 # 增加障碍物相关维度 # 融合层 self.fc_layers = nn.Sequential( - nn.Linear(32 * 4 * 4 + state_dim, 64), + nn.Linear(32 * 4 * 4 + state_dim, 128), # 增加网络容量 + nn.ReLU(), + nn.Dropout(0.1), # 添加dropout防止过拟合 + nn.Linear(128, 64), nn.ReLU(), nn.Linear(64, 32), nn.ReLU(), @@ -62,9 +65,144 @@ def forward(self, image, state): return torch.cat([throttle_brake, steer], dim=1) -# 修复2: 改进的神经网络控制器 +# 新增：障碍物检测器类 +class ObstacleDetector: + def __init__(self, world, vehicle, max_distance=50.0): + self.world = world + self.vehicle = vehicle + self.max_distance = max_distance + self.blueprint_library = world.get_blueprint_library() + self.last_obstacle_info = { + 'has_obstacle': False, + 'distance': float('inf'), + 'relative_angle': 0.0, + 'obstacle_type': None + } + + def get_obstacle_info(self): + """检测前方障碍物信息""" + try: + vehicle_transform = self.vehicle.get_transform() + vehicle_location = vehicle_transform.location + vehicle_rotation = vehicle_transform.rotation + + # 获取车辆前方的向量 + forward_vector = vehicle_transform.get_forward_vector() + + # 获取世界中的所有车辆（排除自身） + all_vehicles = self.world.get_actors().filter('vehicle.*') + + min_distance = float('inf') + closest_obstacle = None + relative_angle = 0.0 + + for other_vehicle in all_vehicles: + if other_vehicle.id == self.vehicle.id: + continue + + other_location = other_vehicle.get_location() + + # 计算距离 + distance = vehicle_location.distance(other_location) + + if distance > self.max_distance: + continue + + # 计算相对位置向量 + relative_vector = carla.Location( + other_location.x - vehicle_location.x, + other_location.y - vehicle_location.y, + 0 + ) + + # 计算角度（车辆前方与障碍物方向的夹角） + forward_2d = carla.Vector3D(forward_vector.x, forward_vector.y, 0) + relative_2d = carla.Vector3D(relative_vector.x, relative_vector.y, 0) + + # 归一化向量 + forward_2d_norm = math.sqrt(forward_2d.x ** 2 + forward_2d.y ** 2) + relative_2d_norm = math.sqrt(relative_2d.x ** 2 + relative_2d.y ** 2) + + if forward_2d_norm > 0 and relative_2d_norm > 0: + dot_product = forward_2d.x * relative_2d.x + forward_2d.y * relative_2d.y + cos_angle = dot_product / (forward_2d_norm * relative_2d_norm) + cos_angle = max(-1.0, min(1.0, cos_angle)) # 限制范围 + angle = math.acos(cos_angle) + + # 转换为角度 + angle_deg = math.degrees(angle) + + # 只考虑前方±60度范围内的障碍物 + if angle_deg <= 60 and distance < min_distance: + min_distance = distance + closest_obstacle = other_vehicle + relative_angle = angle_deg if relative_2d.y >= 0 else -angle_deg + + if closest_obstacle is not None and min_distance < self.max_distance: + self.last_obstacle_info = { + 'has_obstacle': True, + 'distance': min_distance, + 'relative_angle': relative_angle, + 'obstacle_type': closest_obstacle.type_id, + 'obstacle_speed': self.get_vehicle_speed(closest_obstacle) + } + else: + self.last_obstacle_info = { + 'has_obstacle': False, + 'distance': float('inf'), + 'relative_angle': 0.0, + 'obstacle_type': None, + 'obstacle_speed': 0.0 + } + + return self.last_obstacle_info + + except Exception as e: + print(f"障碍物检测错误: {e}") + return self.last_obstacle_info + + def get_vehicle_speed(self, vehicle): + """获取车辆速度""" + velocity = vehicle.get_velocity() + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) + return speed * 3.6 # 转换为km/h + + def visualize_obstacles(self, image, vehicle_transform): + """在图像上可视化障碍物检测结果""" + if not self.last_obstacle_info['has_obstacle']: + return image + + height, width = image.shape[:2] + + # 计算障碍物在图像中的位置（简化投影） + distance = self.last_obstacle_info['distance'] + angle = self.last_obstacle_info['relative_angle'] + + # 归一化角度到图像坐标 + x_pos = int(width / 2 + (angle / 60) * (width / 2)) + + # 根据距离计算大小和颜色 + if distance < 10: + color = (0, 0, 255) # 红色，很近 + radius = 15 + elif distance < 20: + color = (0, 165, 255) # 橙色 + radius = 10 + else: + color = (0, 255, 255) # 黄色 + radius = 5 + + # 绘制障碍物指示器 + cv2.circle(image, (x_pos, int(height * 0.8)), radius, color, -1) + cv2.putText(image, f"{distance:.1f}m", (x_pos - 20, int(height * 0.8) - 20), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2) + + return image + + +# 修复2: 改进的神经网络控制器（整合障碍物检测） class ImprovedNeuralController: - def __init__(self): + def __init__(self, obstacle_detector=None): self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') print(f"使用设备: {self.device}") @@ -75,11 +213,19 @@ def __init__(self): # 控制历史，用于平滑 self.control_history = deque(maxlen=5) + # 障碍物检测器 + self.obstacle_detector = obstacle_detector + # 修复3: 更保守的初始控制 self.last_throttle = 0.3 self.last_brake = 0.0 self.last_steer = 0.0 + # 避障参数 + self.emergency_brake_distance = 5.0 # 紧急刹车距离 + self.safe_following_distance = 8.0 # 安全跟车距离 + self.obstacle_avoidance_steer = 0.0 + def preprocess_image(self, image): """修复图像预处理""" if image is None: @@ -96,23 +242,74 @@ def preprocess_image(self, image): print(f"图像预处理错误: {e}") return torch.zeros((1, 3, 120, 160), device=self.device) - def preprocess_state(self, speed, steer_history): - """修复状态预处理""" + def preprocess_state(self, speed, steer_history, obstacle_info): + """修复状态预处理，加入障碍物信息""" + has_obstacle = 1.0 if obstacle_info['has_obstacle'] else 0.0 + normalized_distance = min(obstacle_info['distance'] / 50.0, 1.0) if obstacle_info['has_obstacle'] else 1.0 + normalized_angle = obstacle_info['relative_angle'] / 60.0 if obstacle_info['has_obstacle'] else 0.0 + state_data = [ speed / 20.0, # 归一化速度 steer_history[-1] if steer_history else 0.0, # 最近转向 steer_history[-2] if len(steer_history) > 1 else 0.0, # 前一次转向 - np.mean(steer_history) if steer_history else 0.0 # 平均转向 + np.mean(steer_history) if steer_history else 0.0, # 平均转向 + has_obstacle, # 是否有障碍物 + normalized_distance, # 归一化距离 + normalized_angle # 归一化角度 ] return torch.tensor(state_data, device=self.device).unsqueeze(0) - def get_control(self, image, speed, steer_history): - """修复控制生成逻辑""" + def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): + """应用避障逻辑""" + if not obstacle_info['has_obstacle']: + return throttle, brake, steer + + distance = obstacle_info['distance'] + angle = obstacle_info['relative_angle'] + + # 紧急情况：前方有近距离障碍物 + if distance < self.emergency_brake_distance: + print(f"紧急刹车！距离障碍物: {distance:.1f}m") + return 0.0, 1.0, steer # 全力刹车 + + # 中距离障碍物：减速并准备转向 + elif distance < self.safe_following_distance: + # 计算安全速度比例 + safe_speed_ratio = (distance - 2.0) / (self.safe_following_distance - 2.0) + safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio)) + + # 如果当前速度过高，减速 + if speed > 5.0 * safe_speed_ratio: + throttle = 0.0 + brake = 0.3 + + # 如果障碍物在正前方，尝试轻微转向避开 + if abs(angle) < 10: # 正前方±10度内 + # 根据障碍物距离决定转向幅度 + avoid_steer = 0.5 if angle >= 0 else -0.5 + # 平滑转向 + steer = 0.7 * steer + 0.3 * avoid_steer + + # 远距离障碍物：轻微调整 + elif distance < 20.0: + # 轻微减速 + if speed > 10.0: + throttle *= 0.8 + + # 如果障碍物在正前方，轻微转向 + if abs(angle) < 15: + avoid_steer = 0.2 if angle >= 0 else -0.2 + steer = 0.8 * steer + 0.2 * avoid_steer + + return throttle, brake, steer + + def get_control(self, image, speed, steer_history, obstacle_info): + """修复控制生成逻辑，加入避障""" try: with torch.no_grad(): # 预处理 img_tensor = self.preprocess_image(image) - state_tensor = self.preprocess_state(speed, steer_history) + state_tensor = self.preprocess_state(speed, steer_history, obstacle_info) # 神经网络推理 control_output = self.model(img_tensor, state_tensor) @@ -127,6 +324,11 @@ def get_control(self, image, speed, steer_history): brake = max(0.0, min(0.5, brake)) # 限制最大刹车 steer = max(-0.5, min(0.5, steer)) # 限制转向幅度 + # 应用避障逻辑 + throttle, brake, steer = self.apply_obstacle_avoidance( + throttle, brake, steer, obstacle_info, speed + ) + return throttle, brake, steer except Exception as e: @@ -135,23 +337,81 @@ def get_control(self, image, speed, steer_history): return 0.3, 0.0, 0.0 -# 修复5: 传统控制器作为备份 +# 修复5: 传统控制器作为备份（整合障碍物检测） class TraditionalController: """可靠的传统控制逻辑""" - def __init__(self, world): + def __init__(self, world, obstacle_detector=None): self.world = world self.map = world.get_map() self.waypoint_distance = 10.0 self.last_waypoint = None + self.obstacle_detector = obstacle_detector + self.emergency_brake_distance = 6.0 + self.safe_following_distance = 10.0 + + def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info): + """传统控制器的避障逻辑""" + if not obstacle_info['has_obstacle']: + return throttle, brake, steer + + distance = obstacle_info['distance'] + angle = obstacle_info['relative_angle'] + vehicle_speed = math.sqrt(vehicle.get_velocity().x ** 2 + + vehicle.get_velocity().y ** 2 + + vehicle.get_velocity().z ** 2) * 3.6 # km/h + + # 紧急刹车 + if distance < self.emergency_brake_distance: + print(f"传统控制：紧急刹车！距离: {distance:.1f}m") + return 0.0, 1.0, 0.0 + + # 减速跟随 + elif distance < self.safe_following_distance: + # 计算所需的安全距离（基于速度） + required_distance = max(5.0, vehicle_speed * 0.3) # 0.3秒车距 + + if distance < required_distance: + # 距离太近，减速 + if vehicle_speed > 10: + throttle = 0.0 + brake = 0.4 + else: + throttle = 0.1 + brake = 0.0 + + # 如果障碍物在正前方，尝试变道 + if abs(angle) < 15: + # 获取当前车道和相邻车道 + location = vehicle.get_location() + waypoint = self.map.get_waypoint(location) + + # 尝试获取左侧车道 + left_lane = waypoint.get_left_lane() + right_lane = waypoint.get_right_lane() + + if left_lane and left_lane.lane_type == carla.LaneType.Driving: + steer = -0.3 # 向左变道 + elif right_lane and right_lane.lane_type == carla.LaneType.Driving: + steer = 0.3 # 向右变道 + else: + # 没有可用的相邻车道，轻微转向避开 + steer = 0.2 if angle >= 0 else -0.2 + + return throttle, brake, steer def get_control(self, vehicle): - """基于路点的传统控制""" + """基于路点的传统控制，整合避障""" # 获取车辆状态 transform = vehicle.get_transform() location = vehicle.get_location() velocity = vehicle.get_velocity() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) * 3.6 # km/h + + # 获取障碍物信息 + obstacle_info = None + if self.obstacle_detector: + obstacle_info = self.obstacle_detector.get_obstacle_info() # 获取路点 waypoint = self.map.get_waypoint(location, project_to_road=True) @@ -180,21 +440,64 @@ def get_control(self, vehicle): angle = math.atan2(local_y, local_x) steer = np.clip(angle / math.radians(45), -1.0, 1.0) - # 速度控制 - if speed < 5.0: # 18 km/h - throttle = 0.6 - brake = 0.0 - elif speed < 10.0: # 36 km/h - throttle = 0.3 - brake = 0.0 + # 速度控制（基于障碍物距离调整） + throttle = 0.0 + brake = 0.0 + + if obstacle_info and obstacle_info['has_obstacle']: + distance = obstacle_info['distance'] + + # 根据障碍物距离调整速度 + if distance < 15: + if speed > 20: + throttle = 0.0 + brake = 0.3 + elif speed > 10: + throttle = 0.1 + brake = 0.0 + else: + throttle = 0.3 + brake = 0.0 + elif distance < 30: + if speed > 30: + throttle = 0.0 + brake = 0.1 + else: + throttle = 0.4 + brake = 0.0 + else: + # 没有近距离障碍物，正常行驶 + if speed < 20: + throttle = 0.6 + brake = 0.0 + elif speed < 40: + throttle = 0.4 + brake = 0.0 + else: + throttle = 0.2 + brake = 0.1 else: - throttle = 0.1 - brake = 0.1 + # 没有障碍物，正常行驶 + if speed < 20: + throttle = 0.6 + brake = 0.0 + elif speed < 40: + throttle = 0.4 + brake = 0.0 + else: + throttle = 0.2 + brake = 0.1 + + # 应用避障逻辑 + if obstacle_info: + throttle, brake, steer = self.apply_obstacle_avoidance( + throttle, brake, steer, vehicle, obstacle_info + ) return throttle, brake, steer -# CARLA初始化部分保持不变... +# CARLA初始化部分... # 连接到本地CARLA服务器，端口2000 client = carla.Client('localhost', 2000) client.set_timeout(15.0) @@ -294,9 +597,13 @@ def front_camera_callback(image): time.sleep(2.0) -# 修复6: 初始化控制器 -nn_controller = ImprovedNeuralController() -traditional_controller = TraditionalController(world) +# 新增：初始化障碍物检测器 +print("初始化障碍物检测器...") +obstacle_detector = ObstacleDetector(world, vehicle, max_distance=50.0) + +# 修复6: 初始化控制器（传入障碍物检测器） +nn_controller = ImprovedNeuralController(obstacle_detector) +traditional_controller = TraditionalController(world, obstacle_detector) # 控制变量 throttle = 0.3 # 更保守的初始油门 @@ -307,6 +614,9 @@ def front_camera_callback(image): # 转向历史，用于平滑 steer_history = deque(maxlen=10) +# 障碍物信息历史 +obstacle_history = deque(maxlen=5) + print("初始化车辆状态...") vehicle.set_simulate_physics(True) @@ -327,6 +637,8 @@ def front_camera_callback(image): stuck_count = 0 last_position = vehicle.get_location() success_count = 0 # 成功运行计数器 + collision_count = 0 # 碰撞计数器 + last_collision_time = 0 # 上次碰撞时间 # 主循环 while True: @@ -339,8 +651,19 @@ def front_camera_callback(image): vehicle_velocity = vehicle.get_velocity() vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2) - print( - f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}") + # 检测障碍物 + obstacle_info = obstacle_detector.get_obstacle_info() + obstacle_history.append(obstacle_info) + + # 显示障碍物信息 + if obstacle_info['has_obstacle']: + print(f"障碍物检测: 距离={obstacle_info['distance']:.1f}m, " + f"角度={obstacle_info['relative_angle']:.1f}°, " + f"类型={obstacle_info['obstacle_type']}") + + print(f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, " + f"模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}, " + f"障碍物={'有' if obstacle_info['has_obstacle'] else '无'}") # 修复8: 改进的卡住检测 current_position = vehicle_location @@ -366,12 +689,21 @@ def front_camera_callback(image): )) time.sleep(0.5) - # 然后尝试不同方向的脱困 - recovery_steer = random.choice([-0.5, 0.5]) # 随机选择方向 - vehicle.apply_control(carla.VehicleControl( - throttle=0.8, steer=recovery_steer, brake=0.0, hand_brake=False - )) - time.sleep(1.0) + # 检查是否有前方障碍物 + if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 10: + print("前方有障碍物，尝试倒车...") + # 倒车 + vehicle.apply_control(carla.VehicleControl( + throttle=0.0, steer=0.0, brake=0.0, reverse=True + )) + time.sleep(1.0) + else: + # 然后尝试不同方向的脱困 + recovery_steer = random.choice([-0.5, 0.5]) # 随机选择方向 + vehicle.apply_control(carla.VehicleControl( + throttle=0.8, steer=recovery_steer, brake=0.0, hand_brake=False + )) + time.sleep(1.0) stuck_count = 0 success_count = 0 @@ -382,9 +714,9 @@ def front_camera_callback(image): # 控制逻辑 if NEURAL_NETWORK_MODE: - # 神经网络控制 + # 神经网络控制（传入障碍物信息） nn_throttle, nn_brake, nn_steer = nn_controller.get_control( - front_image, vehicle_speed, steer_history + front_image, vehicle_speed, steer_history, obstacle_info ) # 修复10: 更激进的控制平滑 @@ -415,6 +747,9 @@ def front_camera_callback(image): if third_image is not None: display_image = third_image.copy() + # 可视化障碍物检测结果 + display_image = obstacle_detector.visualize_obstacles(display_image, vehicle_transform) + # 显示信息 cv2.putText(display_image, f"Speed: {vehicle_speed * 3.6:.1f} km/h", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) @@ -427,12 +762,23 @@ def front_camera_callback(image): cv2.putText(display_image, f"Brake: {brake:.2f}", (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + # 显示障碍物信息 + if obstacle_info['has_obstacle']: + cv2.putText(display_image, f"Obstacle: {obstacle_info['distance']:.1f}m", (10, 180), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) + else: + cv2.putText(display_image, "Obstacle: None", (10, 180), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + # 卡住警告 if stuck_count > 5: - cv2.putText(display_image, "STUCK DETECTED!", (10, 180), + cv2.putText(display_image, "STUCK DETECTED!", (10, 210), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - cv2.imshow('自动驾驶系统 - 修复版', display_image) + # 安全区域标记 + cv2.rectangle(display_image, (240, 360), (400, 480), (0, 255, 0), 2) # 前方安全区域 + + cv2.imshow('自动驾驶系统 - 带障碍物检测', display_image) key = cv2.waitKey(1) & 0xFF if key == ord('q'): @@ -454,7 +800,10 @@ def front_camera_callback(image): brake = 1.0 stuck_count = 0 success_count = 0 + collision_count = 0 steer_history.clear() + obstacle_history.clear() + print("车辆已重置") time.sleep(0.01) From c17b31f75d113ead1ef0e03b4bcc10a96b757a97 Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 16 Dec 2025 09:20:21 +0800 Subject: [PATCH 19/25] =?UTF-8?q?=E6=94=B9=E8=BF=9B=E9=81=BF=E9=9A=9C?= =?UTF-8?q?=E9=80=BB=E8=BE=91=EF=BC=8C=E9=81=BF=E5=85=8D=E8=BF=87=E5=BA=A6?= =?UTF-8?q?=E8=BD=AC=E5=90=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 38 +++++++++++++++++++------------ 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 08181021ad..c18eeeb990 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -259,8 +259,9 @@ def preprocess_state(self, speed, steer_history, obstacle_info): ] return torch.tensor(state_data, device=self.device).unsqueeze(0) + # 在 ImprovedNeuralController 类的 apply_obstacle_avoidance 方法中，修改以下部分： def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): - """应用避障逻辑""" + """应用避障逻辑 - 优化版本""" if not obstacle_info['has_obstacle']: return throttle, brake, steer @@ -270,36 +271,43 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed) # 紧急情况：前方有近距离障碍物 if distance < self.emergency_brake_distance: print(f"紧急刹车！距离障碍物: {distance:.1f}m") - return 0.0, 1.0, steer # 全力刹车 + return 0.0, 1.0, 0.0 # 紧急情况下保持直行，只刹车 # 中距离障碍物：减速并准备转向 elif distance < self.safe_following_distance: # 计算安全速度比例 - safe_speed_ratio = (distance - 2.0) / (self.safe_following_distance - 2.0) + safe_speed_ratio = (distance - 3.0) / (self.safe_following_distance - 3.0) safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio)) # 如果当前速度过高，减速 - if speed > 5.0 * safe_speed_ratio: + target_speed = 15.0 * safe_speed_ratio # 目标速度最大15km/h + current_speed_kmh = speed * 3.6 + + if current_speed_kmh > target_speed: throttle = 0.0 - brake = 0.3 + brake = 0.4 * ((current_speed_kmh - target_speed) / current_speed_kmh) + else: + throttle = 0.3 * safe_speed_ratio + brake = 0.0 # 如果障碍物在正前方，尝试轻微转向避开 - if abs(angle) < 10: # 正前方±10度内 + if abs(angle) < 15: # 正前方±15度内 # 根据障碍物距离决定转向幅度 - avoid_steer = 0.5 if angle >= 0 else -0.5 - # 平滑转向 - steer = 0.7 * steer + 0.3 * avoid_steer + avoid_factor = max(0, 1.0 - distance / self.safe_following_distance) + avoid_steer = 0.3 * avoid_factor if angle >= 0 else -0.3 * avoid_factor + # 平滑转向 - 保持更多原始转向 + steer = 0.8 * steer + 0.2 * avoid_steer # 远距离障碍物：轻微调整 - elif distance < 20.0: + elif distance < 25.0: # 轻微减速 - if speed > 10.0: - throttle *= 0.8 + if speed > 8.0: + throttle *= 0.7 # 如果障碍物在正前方，轻微转向 - if abs(angle) < 15: - avoid_steer = 0.2 if angle >= 0 else -0.2 - steer = 0.8 * steer + 0.2 * avoid_steer + if abs(angle) < 20: + avoid_steer = 0.15 if angle >= 0 else -0.15 + steer = 0.9 * steer + 0.1 * avoid_steer return throttle, brake, steer From c412e750db3a254cc83b89ccb490571a5ca0366b Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 16 Dec 2025 10:15:39 +0800 Subject: [PATCH 20/25] =?UTF-8?q?=E6=94=B9=E8=BF=9B=E9=81=BF=E9=9A=9C?= =?UTF-8?q?=E9=80=BB=E8=BE=91=EF=BC=8C=E9=81=BF=E5=85=8D=E8=BF=87=E5=BA=A6?= =?UTF-8?q?=E8=BD=AC=E5=90=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 198 +++--------------------------- 1 file changed, 14 insertions(+), 184 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index b817e6c9d6..1160a755fd 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -14,16 +14,10 @@ import os - # 修复1: 简化的神经网络架构（增加障碍物检测通道） class SimpleDrivingNetwork(nn.Module): """ 简化的驾驶网络 - 增加障碍物感知 -======= -# 修复1: 简化的神经网络架构 -class SimpleDrivingNetwork(nn.Module): - """ - """ def __init__(self): @@ -40,7 +34,6 @@ def __init__(self): nn.AdaptiveAvgPool2d((4, 4)) ) - # 状态信息维度: 速度 + 转向历史 + 障碍物信息 state_dim = 7 # 增加障碍物相关维度 @@ -50,14 +43,6 @@ def __init__(self): nn.ReLU(), nn.Dropout(0.1), # 添加dropout防止过拟合 nn.Linear(128, 64), - - # 状态信息维度: 速度 + 转向历史 - state_dim = 4 - - # 融合层 - self.fc_layers = nn.Sequential( - nn.Linear(32 * 4 * 4 + state_dim, 64), - nn.ReLU(), nn.Linear(64, 32), nn.ReLU(), @@ -80,7 +65,6 @@ def forward(self, image, state): return torch.cat([throttle_brake, steer], dim=1) - # 新增：障碍物检测器类 class ObstacleDetector: def __init__(self, world, vehicle, max_distance=50.0): @@ -219,11 +203,6 @@ def visualize_obstacles(self, image, vehicle_transform): # 修复2: 改进的神经网络控制器（整合障碍物检测） class ImprovedNeuralController: def __init__(self, obstacle_detector=None): - -# 修复2: 改进的神经网络控制器 -class ImprovedNeuralController: - def __init__(self): - self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') print(f"使用设备: {self.device}") @@ -234,7 +213,6 @@ def __init__(self): # 控制历史，用于平滑 self.control_history = deque(maxlen=5) - # 障碍物检测器 self.obstacle_detector = obstacle_detector @@ -243,13 +221,11 @@ def __init__(self): self.last_brake = 0.0 self.last_steer = 0.0 - # 避障参数 self.emergency_brake_distance = 5.0 # 紧急刹车距离 self.safe_following_distance = 8.0 # 安全跟车距离 self.obstacle_avoidance_steer = 0.0 - def preprocess_image(self, image): """修复图像预处理""" if image is None: @@ -266,21 +242,16 @@ def preprocess_image(self, image): print(f"图像预处理错误: {e}") return torch.zeros((1, 3, 120, 160), device=self.device) - def preprocess_state(self, speed, steer_history, obstacle_info): """修复状态预处理，加入障碍物信息""" has_obstacle = 1.0 if obstacle_info['has_obstacle'] else 0.0 normalized_distance = min(obstacle_info['distance'] / 50.0, 1.0) if obstacle_info['has_obstacle'] else 1.0 normalized_angle = obstacle_info['relative_angle'] / 60.0 if obstacle_info['has_obstacle'] else 0.0 - def preprocess_state(self, speed, steer_history): - """修复状态预处理""" - state_data = [ speed / 20.0, # 归一化速度 steer_history[-1] if steer_history else 0.0, # 最近转向 steer_history[-2] if len(steer_history) > 1 else 0.0, # 前一次转向 - np.mean(steer_history) if steer_history else 0.0, # 平均转向 has_obstacle, # 是否有障碍物 normalized_distance, # 归一化距离 @@ -342,24 +313,12 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed) def get_control(self, image, speed, steer_history, obstacle_info): """修复控制生成逻辑，加入避障""" - - np.mean(steer_history) if steer_history else 0.0 # 平均转向 - ] - return torch.tensor(state_data, device=self.device).unsqueeze(0) - - def get_control(self, image, speed, steer_history): - """修复控制生成逻辑""" - try: with torch.no_grad(): # 预处理 img_tensor = self.preprocess_image(image) - state_tensor = self.preprocess_state(speed, steer_history, obstacle_info) - state_tensor = self.preprocess_state(speed, steer_history) - - # 神经网络推理 control_output = self.model(img_tensor, state_tensor) @@ -373,13 +332,11 @@ def get_control(self, image, speed, steer_history): brake = max(0.0, min(0.5, brake)) # 限制最大刹车 steer = max(-0.5, min(0.5, steer)) # 限制转向幅度 - # 应用避障逻辑 throttle, brake, steer = self.apply_obstacle_avoidance( throttle, brake, steer, obstacle_info, speed ) - return throttle, brake, steer except Exception as e: @@ -388,30 +345,22 @@ def get_control(self, image, speed, steer_history): return 0.3, 0.0, 0.0 - # 修复5: 传统控制器作为备份（整合障碍物检测） class TraditionalController: """可靠的传统控制逻辑""" def __init__(self, world, obstacle_detector=None): - -# 修复5: 传统控制器作为备份 -class TraditionalController: - """可靠的传统控制逻辑""" - - def __init__(self, world): - self.world = world self.map = world.get_map() self.waypoint_distance = 10.0 self.last_waypoint = None - self.obstacle_detector = obstacle_detector self.emergency_brake_distance = 6.0 self.safe_following_distance = 10.0 + # 在 TraditionalController 类的 apply_obstacle_avoidance 方法中，修改以下部分： def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info): - """传统控制器的避障逻辑""" + """传统控制器的避障逻辑 - 优化版本""" if not obstacle_info['has_obstacle']: return throttle, brake, steer @@ -429,49 +378,45 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_inf # 减速跟随 elif distance < self.safe_following_distance: # 计算所需的安全距离（基于速度） - required_distance = max(5.0, vehicle_speed * 0.3) # 0.3秒车距 + required_distance = max(5.0, vehicle_speed * 0.4) # 增加到0.4秒车距 if distance < required_distance: # 距离太近，减速 + speed_ratio = distance / required_distance if vehicle_speed > 10: throttle = 0.0 - brake = 0.4 + brake = 0.6 * (1.0 - speed_ratio) else: - throttle = 0.1 + throttle = 0.2 * speed_ratio brake = 0.0 # 如果障碍物在正前方，尝试变道 - if abs(angle) < 15: - # 获取当前车道和相邻车道 + if abs(angle) < 20: # 放宽角度范围 location = vehicle.get_location() waypoint = self.map.get_waypoint(location) - # 尝试获取左侧车道 + # 检查相邻车道是否可用 left_lane = waypoint.get_left_lane() right_lane = waypoint.get_right_lane() + # 优先选择转向较小的方向 if left_lane and left_lane.lane_type == carla.LaneType.Driving: - steer = -0.3 # 向左变道 + # 检查左侧是否有足够空间 + steer = -0.25 elif right_lane and right_lane.lane_type == carla.LaneType.Driving: - steer = 0.3 # 向右变道 + steer = 0.25 else: - # 没有可用的相邻车道，轻微转向避开 - steer = 0.2 if angle >= 0 else -0.2 + # 没有可用的相邻车道，保持车道轻微避开 + steer = 0.15 if angle >= 0 else -0.15 return throttle, brake, steer def get_control(self, vehicle): """基于路点的传统控制，整合避障""" - - - def get_control(self, vehicle): - """基于路点的传统控制""" - # 获取车辆状态 transform = vehicle.get_transform() location = vehicle.get_location() velocity = vehicle.get_velocity() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) * 3.6 # km/h # 获取障碍物信息 @@ -479,9 +424,6 @@ def get_control(self, vehicle): if self.obstacle_detector: obstacle_info = self.obstacle_detector.get_obstacle_info() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) - - # 获取路点 waypoint = self.map.get_waypoint(location, project_to_road=True) next_waypoints = waypoint.next(self.waypoint_distance) @@ -509,7 +451,6 @@ def get_control(self, vehicle): angle = math.atan2(local_y, local_x) steer = np.clip(angle / math.radians(45), -1.0, 1.0) - # 速度控制（基于障碍物距离调整） throttle = 0.0 brake = 0.0 @@ -564,24 +505,10 @@ def get_control(self, vehicle): throttle, brake, steer, vehicle, obstacle_info ) - # 速度控制 - if speed < 5.0: # 18 km/h - throttle = 0.6 - brake = 0.0 - elif speed < 10.0: # 36 km/h - throttle = 0.3 - brake = 0.0 - else: - throttle = 0.1 - brake = 0.1 - - return throttle, brake, steer - # CARLA初始化部分... -# CARLA初始化部分保持不变... # 连接到本地CARLA服务器，端口2000 client = carla.Client('localhost', 2000) client.set_timeout(15.0) @@ -669,7 +596,6 @@ def third_camera_callback(image): third_image = array[:, :, :3] - def front_camera_callback(image): global front_image array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) @@ -702,33 +628,6 @@ def front_camera_callback(image): # 障碍物信息历史 obstacle_history = deque(maxlen=5) - -def front_camera_callback(image): - global front_image - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - front_image = array[:, :, :3] - - -third_camera.listen(third_camera_callback) -front_camera.listen(front_camera_callback) - -time.sleep(2.0) - -# 修复6: 初始化控制器 -nn_controller = ImprovedNeuralController() -traditional_controller = TraditionalController(world) - -# 控制变量 -throttle = 0.3 # 更保守的初始油门 -steer = 0.0 -brake = 0.0 -NEURAL_NETWORK_MODE = False # 默认使用传统控制，更稳定 - -# 转向历史，用于平滑 -steer_history = deque(maxlen=10) - - print("初始化车辆状态...") vehicle.set_simulate_physics(True) @@ -749,11 +648,9 @@ def front_camera_callback(image): stuck_count = 0 last_position = vehicle.get_location() success_count = 0 # 成功运行计数器 - collision_count = 0 # 碰撞计数器 last_collision_time = 0 # 上次碰撞时间 - # 主循环 while True: world.tick() @@ -765,7 +662,6 @@ def front_camera_callback(image): vehicle_velocity = vehicle.get_velocity() vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2) - # 检测障碍物 obstacle_info = obstacle_detector.get_obstacle_info() obstacle_history.append(obstacle_info) @@ -780,11 +676,6 @@ def front_camera_callback(image): f"模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}, " f"障碍物={'有' if obstacle_info['has_obstacle'] else '无'}") - - print( - f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}") - - # 修复8: 改进的卡住检测 current_position = vehicle_location distance_moved = current_position.distance(last_position) @@ -797,15 +688,6 @@ def front_camera_callback(image): stuck_count = 0 success_count += 1 # 成功运行一帧 - - last_position = current_position - - # 修复9: 更智能的卡住恢复 - if stuck_count > 15: # 1.5秒后认为卡住 - print("检测到车辆卡住，执行恢复程序...") - - - last_position = current_position # 修复9: 更智能的卡住恢复 @@ -818,7 +700,6 @@ def front_camera_callback(image): )) time.sleep(0.5) - # 检查是否有前方障碍物 if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 10: print("前方有障碍物，尝试倒车...") @@ -857,37 +738,6 @@ def front_camera_callback(image): # 记录转向历史 steer_history.append(steer) - - - # 然后尝试不同方向的脱困 - recovery_steer = random.choice([-0.5, 0.5]) # 随机选择方向 - vehicle.apply_control(carla.VehicleControl( - throttle=0.8, steer=recovery_steer, brake=0.0, hand_brake=False - )) - time.sleep(1.0) - - stuck_count = 0 - success_count = 0 - - # 每成功运行100帧显示一次状态 - if success_count % 100 == 0: - print(f"已成功运行 {success_count} 帧") - - # 控制逻辑 - if NEURAL_NETWORK_MODE: - # 神经网络控制 - nn_throttle, nn_brake, nn_steer = nn_controller.get_control( - front_image, vehicle_speed, steer_history - ) - - # 修复10: 更激进的控制平滑 - throttle = 0.3 * throttle + 0.7 * nn_throttle - brake = 0.3 * brake + 0.7 * nn_brake - steer = 0.2 * steer + 0.8 * nn_steer - - # 记录转向历史 - steer_history.append(steer) - else: # 传统控制 - 更稳定 throttle, brake, steer = traditional_controller.get_control(vehicle) @@ -908,7 +758,6 @@ def front_camera_callback(image): if third_image is not None: display_image = third_image.copy() - # 可视化障碍物检测结果 display_image = obstacle_detector.visualize_obstacles(display_image, vehicle_transform) @@ -918,7 +767,6 @@ def front_camera_callback(image): cv2.putText(display_image, f"Mode: {'Neural' if NEURAL_NETWORK_MODE else 'Traditional'}", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) @@ -943,20 +791,6 @@ def front_camera_callback(image): cv2.imshow('自动驾驶系统 - 带障碍物检测', display_image) - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Brake: {brake:.2f}", (10, 150), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - - # 卡住警告 - if stuck_count > 5: - cv2.putText(display_image, "STUCK DETECTED!", (10, 180), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - - cv2.imshow('自动驾驶系统 - 修复版', display_image) - - key = cv2.waitKey(1) & 0xFF if key == ord('q'): break @@ -977,15 +811,11 @@ def front_camera_callback(image): brake = 1.0 stuck_count = 0 success_count = 0 - collision_count = 0 steer_history.clear() obstacle_history.clear() print("车辆已重置") - steer_history.clear() - - time.sleep(0.01) except KeyboardInterrupt: From 0b3c2c05209e1e1b03b83e79a9ed6ddc17006188 Mon Sep 17 00:00:00 2001 From: chen Date: Wed, 17 Dec 2025 21:22:50 +0800 Subject: [PATCH 21/25] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=B0=8F=E8=BD=A6?= =?UTF-8?q?=E8=A2=ABnpc=E5=8D=A1=E4=BD=8F=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 125 ++++++++++++++++++++++++++---- 1 file changed, 112 insertions(+), 13 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 18f2d9df55..38cf91dbbc 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -259,7 +259,6 @@ def preprocess_state(self, speed, steer_history, obstacle_info): ] return torch.tensor(state_data, device=self.device).unsqueeze(0) - # 在 ImprovedNeuralController 类的 apply_obstacle_avoidance 方法中，修改以下部分： def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): """应用避障逻辑 - 优化版本""" @@ -344,7 +343,6 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed) avoid_steer = 0.2 if angle >= 0 else -0.2 steer = 0.8 * steer + 0.2 * avoid_steer - return throttle, brake, steer def get_control(self, image, speed, steer_history, obstacle_info): @@ -587,17 +585,78 @@ def get_control(self, vehicle): print(f"车辆生成在位置: {spawn_point.location}") -# 生成障碍物车辆 +# 改进的NPC车辆生成和设置 +print("生成NPC车辆...") obstacle_count = 3 -for i in range(obstacle_count): - if i >= len(spawn_points): - break - other_vehicles = blueprint_library.filter('vehicle.*') - other_vehicle_bp = np.random.choice(other_vehicles) - spawn_idx = (i + 15) % len(spawn_points) - other_vehicle = world.try_spawn_actor(other_vehicle_bp, spawn_points[spawn_idx]) - if other_vehicle: - other_vehicle.set_autopilot(True) +npc_vehicles = [] # 存储NPC车辆以便后续管理 + +# 获取所有可用的车辆蓝图 +vehicle_blueprints = blueprint_library.filter('vehicle.*') + +# 选择远离主车辆的出生点（避免直接堵塞） +valid_spawn_points = [] +main_spawn_location = spawn_point.location + +for point in spawn_points: + distance = main_spawn_location.distance(point.location) + # 选择距离主车辆50米以上的出生点，避免直接碰撞 + if distance > 50.0: + valid_spawn_points.append(point) + if len(valid_spawn_points) >= obstacle_count: + break + +# 如果找不到足够远的点，使用所有点 +if len(valid_spawn_points) < obstacle_count: + print("警告：找不到足够远的出生点，使用所有可用点") + valid_spawn_points = spawn_points[:obstacle_count] + +# 生成NPC车辆 +for i in range(min(obstacle_count, len(valid_spawn_points))): + try: + # 随机选择车辆类型（排除特斯拉，使场景更丰富） + available_blueprints = [bp for bp in vehicle_blueprints if 'tesla' not in bp.id] + if not available_blueprints: + available_blueprints = vehicle_blueprints + + npc_bp = random.choice(available_blueprints) + + # 设置车辆颜色 + if npc_bp.has_attribute('color'): + colors = npc_bp.get_attribute('color').recommended_values + if colors: + npc_bp.set_attribute('color', random.choice(colors)) + + # 生成车辆 + npc_vehicle = world.try_spawn_actor(npc_bp, valid_spawn_points[i]) + + if npc_vehicle: + # 启用自动驾驶模式并设置速度限制 + npc_vehicle.set_autopilot(True) + + # 设置NPC车辆的速度限制（让它们能正常行驶） + traffic_manager = client.get_trafficmanager() + traffic_manager.set_global_distance_to_leading_vehicle(2.5) # 设置跟车距离 + traffic_manager.set_random_device_seed(12345) # 设置随机种子确保一致性 + + # 为每个NPC车辆设置不同的速度限制（避免所有车速度相同导致堵塞） + target_speed = random.uniform(30.0, 50.0) # 30-50 km/h + traffic_manager.vehicle_percentage_speed_difference(npc_vehicle, random.uniform(-10, 10)) + + # 设置NPC车辆的驾驶行为（更安全、更智能） + traffic_manager.auto_lane_change(npc_vehicle, True) # 允许自动变道 + traffic_manager.distance_to_leading_vehicle(npc_vehicle, 3.0) # 设置跟车距离 + traffic_manager.collision_detection(npc_vehicle, world, True) # 启用碰撞检测 + + npc_vehicles.append(npc_vehicle) + print(f"生成NPC车辆 {i + 1}: {npc_bp.id} 在位置 {valid_spawn_points[i].location}") + + # 短暂暂停，避免生成时的碰撞 + time.sleep(0.1) + + except Exception as e: + print(f"生成NPC车辆 {i + 1} 时出错: {e}") + +print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") # 配置传感器（简化配置） third_camera_bp = blueprint_library.find('sensor.camera.rgb') @@ -676,9 +735,40 @@ def front_camera_callback(image): hand_brake=False )) + +# 添加NPC车辆管理函数 +def check_and_reset_stuck_npcs(): + """检查并重置卡住的NPC车辆""" + for npc in npc_vehicles: + try: + npc_speed = math.sqrt( + npc.get_velocity().x ** 2 + npc.get_velocity().y ** 2 + npc.get_velocity().z ** 2) * 3.6 + # 如果NPC车辆速度过低（小于1km/h）且没有碰撞，可能是卡住了 + if npc_speed < 1.0: + print(f"检测到NPC车辆 {npc.id} 可能卡住，尝试重置...") + # 获取当前位置 + current_transform = npc.get_transform() + # 寻找最近的可用出生点 + closest_point = None + min_distance = float('inf') + for point in spawn_points: + distance = point.location.distance(current_transform.location) + if distance < min_distance and distance > 10.0: # 避免重置到太近的位置 + min_distance = distance + closest_point = point + + if closest_point: + npc.set_transform(closest_point) + npc.set_autopilot(True) + print(f"重置NPC车辆 {npc.id} 到新位置") + except: + pass + + try: print("自动驾驶系统启动 - 初始模式: 传统控制") print("控制键: q-退出, m-切换控制模式, r-重置车辆, t-传统模式, n-神经网络模式") + print(f"当前有 {len(npc_vehicles)} 辆NPC车辆在运行") frame_count = 0 stuck_count = 0 @@ -686,12 +776,18 @@ def front_camera_callback(image): success_count = 0 # 成功运行计数器 collision_count = 0 # 碰撞计数器 last_collision_time = 0 # 上次碰撞时间 + last_npc_check_time = 0 # 上次检查NPC的时间 # 主循环 while True: world.tick() frame_count += 1 + # 定期检查NPC车辆状态（每100帧检查一次） + if frame_count - last_npc_check_time > 100: + check_and_reset_stuck_npcs() + last_npc_check_time = frame_count + # 获取车辆状态 vehicle_transform = vehicle.get_transform() vehicle_location = vehicle.get_location() @@ -870,7 +966,10 @@ def front_camera_callback(image): # 销毁actor for actor in world.get_actors(): if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'): - actor.destroy() + try: + actor.destroy() + except: + pass # 恢复设置 settings.synchronous_mode = False From 27420a4ae08d023a4c4dab0a692a1046b55efe89 Mon Sep 17 00:00:00 2001 From: chen Date: Wed, 17 Dec 2025 21:45:55 +0800 Subject: [PATCH 22/25] =?UTF-8?q?=E5=88=A0=E9=99=A4=E4=BA=86=E9=87=8D?= =?UTF-8?q?=E5=A4=8D=E7=9A=84=20apply=5Fobstacle=5Favoidance=20=E6=96=B9?= =?UTF-8?q?=E6=B3=95=E5=AE=9A=E4=B9=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 38 ------------------------------- 1 file changed, 38 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 38cf91dbbc..bce974b7f0 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -259,12 +259,8 @@ def preprocess_state(self, speed, steer_history, obstacle_info): ] return torch.tensor(state_data, device=self.device).unsqueeze(0) - # 在 ImprovedNeuralController 类的 apply_obstacle_avoidance 方法中，修改以下部分： def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): """应用避障逻辑 - 优化版本""" - - def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): - if not obstacle_info['has_obstacle']: return throttle, brake, steer @@ -274,16 +270,11 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed) # 紧急情况：前方有近距离障碍物 if distance < self.emergency_brake_distance: print(f"紧急刹车！距离障碍物: {distance:.1f}m") - return 0.0, 1.0, 0.0 # 紧急情况下保持直行，只刹车 - return 0.0, 1.0, steer # 全力刹车 - - # 中距离障碍物：减速并准备转向 elif distance < self.safe_following_distance: # 计算安全速度比例 - safe_speed_ratio = (distance - 3.0) / (self.safe_following_distance - 3.0) safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio)) @@ -317,32 +308,6 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed) avoid_steer = 0.15 if angle >= 0 else -0.15 steer = 0.9 * steer + 0.1 * avoid_steer - safe_speed_ratio = (distance - 2.0) / (self.safe_following_distance - 2.0) - safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio)) - - # 如果当前速度过高，减速 - if speed > 5.0 * safe_speed_ratio: - throttle = 0.0 - brake = 0.3 - - # 如果障碍物在正前方，尝试轻微转向避开 - if abs(angle) < 10: # 正前方±10度内 - # 根据障碍物距离决定转向幅度 - avoid_steer = 0.5 if angle >= 0 else -0.5 - # 平滑转向 - steer = 0.7 * steer + 0.3 * avoid_steer - - # 远距离障碍物：轻微调整 - elif distance < 20.0: - # 轻微减速 - if speed > 10.0: - throttle *= 0.8 - - # 如果障碍物在正前方，轻微转向 - if abs(angle) < 15: - avoid_steer = 0.2 if angle >= 0 else -0.2 - steer = 0.8 * steer + 0.2 * avoid_steer - return throttle, brake, steer def get_control(self, image, speed, steer_history, obstacle_info): @@ -392,7 +357,6 @@ def __init__(self, world, obstacle_detector=None): self.emergency_brake_distance = 6.0 self.safe_following_distance = 10.0 - # 在 TraditionalController 类的 apply_obstacle_avoidance 方法中，修改以下部分： def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info): """传统控制器的避障逻辑 - 优化版本""" if not obstacle_info['has_obstacle']: @@ -435,12 +399,10 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_inf # 优先选择转向较小的方向 if left_lane and left_lane.lane_type == carla.LaneType.Driving: - # 检查左侧是否有足够空间 steer = -0.25 elif right_lane and right_lane.lane_type == carla.LaneType.Driving: steer = 0.25 else: - # 没有可用的相邻车道，保持车道轻微避开 steer = 0.15 if angle >= 0 else -0.15 return throttle, brake, steer From 6df2a6dc238a16ebb8a14dc911d19095909c65c1 Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 23 Dec 2025 22:46:16 +0800 Subject: [PATCH 23/25] =?UTF-8?q?=E6=A0=B9=E6=8D=AE=E4=BB=A3=E7=A0=81?= =?UTF-8?q?=EF=BC=8C=E6=9B=B4=E6=94=B9README.MD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/README.md | 37 ++++++++++++++++++++--------- 1 file changed, 26 insertions(+), 11 deletions(-) diff --git a/src/car_navigation_system/README.md b/src/car_navigation_system/README.md index 4580a6a70c..7912e8cb97 100644 --- a/src/car_navigation_system/README.md +++ b/src/car_navigation_system/README.md @@ -1,11 +1,12 @@ # 多模态 CARLA 导航避障系统 ## 项目简介 -本项目基于 CARLA 模拟器与神经网络技术，实现了具备多传感器融合能力的智能车辆导航避障系统。系统集成前视摄像头、第三视角摄像头与激光雷达（LiDAR），通过多模态数据感知环境，结合智能避障算法，实现车辆自主行驶与障碍物规避功能。 +本项目基于 CARLA 模拟器与神经网络技术，实现了具备多传感器融合能力的智能车辆导航避障系统。系统集成前视摄像头、第三视角摄像头与障碍物检测模块，通过多模态数据感知环境，结合神经网络与传统控制算法，实现车辆自主行驶与障碍物规避功能。 ## 核心功能 -- 多传感器融合感知：集成 RGB 摄像头（前视 + 第三视角）与 32 线激光雷达，全面获取环境信息 -- 智能避障算法：基于激光雷达点云数据，分区域检测障碍物，动态选择最优避障方向（左 / 右） -- 可视化监控：实时显示前视画面、第三视角跟随画面、LiDAR 鸟瞰图，叠加关键行驶数据 -- 人机交互控制：支持键盘手动干预，兼容自动 / 手动切换 +- 多模态感知系统：集成 RGB 摄像头（前视 + 第三视角）与障碍物检测器，全面获取环境信息 +- 智能避障算法：基于改进神经网络控制器与传统控制器双模式，动态检测并规避前方障碍物 +- 实时可视化：实时显示第三视角画面，叠加障碍物检测结果、车速、控制状态等关键信息 +- 双模式控制：支持神经网络模式与传统控制模式切换，可根据环境选择最优控制策略 +- 智能恢复机制：内置车辆卡住检测与自动恢复系统，保障行驶稳定性 ## 环境配置 - 操作系统：Windows 10/11 或 Ubuntu 20.04/22.04 - Python 版本：3.7 @@ -51,12 +52,26 @@ - a 手动左转向 - d 手动右转向 - r 重置转向角度（回正） - ## 系统架构 1. 环境初始化模块 -连接 CARLA 服务器（默认 localhost:2000）。 -加载 Town01 地图，设置同步模式（固定时间步长 0.05s）。 -配置天气参数（多云、无降水、太阳高度角）。 +- 连接 CARLA 服务器（默认 localhost:2000） +- 加载 Town01 地图，设置同步模式（固定时间步长 0.1s） +- 配置天气参数（云量30%、无降水、太阳高度角70°） 2. 智能体生成模块 -主车辆：特斯拉 Model3（红色），关闭自动驾驶，由自定义算法控制。 -障碍物车辆：随机生成 6 辆不同类型车辆，开启自动驾驶，分散分布在地图中。 +- 主车辆：特斯拉 Model3（红色），关闭自动驾驶，由自定义控制算法控制 +- NPC车辆：随机生成3辆不同类型车辆，开启自动驾驶，分散在50米外起始位置 +3. 传感器系统 +- 前视摄像头：90°视野，分辨率640×480，用于神经网络输入 +- 第三视角摄像头：110°视野，分辨率640×480，用于可视化监控 +- 障碍物检测器：实时检测前方±60°范围内50米内障碍物 +4. 控制系统 +- 神经网络控制器：基于改进的SimpleDrivingNetwork，融合图像与障碍物信息 +- 传统控制器：基于路点跟踪的经典控制算法，稳定性更高 +- 障碍物避障逻辑：紧急刹车、减速跟随、转向避让多级策略 +5. 可视化与监控 +- 实时显示第三视角画面 +- 可视化障碍物位置与距离 +- 显示车速、控制模式、油门/刹车/转向值 +- 安全区域标记与卡住警告 +## 联系方式 +邮箱：2985835251@qq.com \ No newline at end of file From 313898dd374d79a57ceff0c669329c0aa5b6f91e Mon Sep 17 00:00:00 2001 From: chen Date: Sat, 27 Dec 2025 15:33:55 +0800 Subject: [PATCH 24/25] =?UTF-8?q?=E9=80=9A=E8=BF=87=E7=AE=80=E5=8C=96?= =?UTF-8?q?=E7=B3=BB=E7=BB=9F=E6=9E=B6=E6=9E=84=EF=BC=8C=E6=8F=90=E9=AB=98?= =?UTF-8?q?=E7=A8=B3=E5=AE=9A=E6=80=A7=E5=92=8C=E5=8F=AF=E9=9D=A0=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 1201 +++++++++-------------------- 1 file changed, 344 insertions(+), 857 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index bce974b7f0..7d02bb9fc7 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -1,940 +1,427 @@ # -------------------------- -# 1. 初始化CARLA连接和环境 +# 简化修复版：确保车辆正确生成 # -------------------------- + import carla import time import numpy as np import cv2 import math from collections import deque -import torch -import torch.nn as nn -import torch.nn.functional as F import random -import os - - -# 修复1: 简化的神经网络架构（增加障碍物检测通道） -class SimpleDrivingNetwork(nn.Module): - """ - 简化的驾驶网络 - 增加障碍物感知 - """ - - def __init__(self): - super(SimpleDrivingNetwork, self).__init__() - - # 图像处理分支 (简化) - self.conv_layers = nn.Sequential( - nn.Conv2d(3, 8, kernel_size=5, stride=2), - nn.ReLU(), - nn.Conv2d(8, 16, kernel_size=5, stride=2), - nn.ReLU(), - nn.Conv2d(16, 32, kernel_size=3, stride=2), - nn.ReLU(), - nn.AdaptiveAvgPool2d((4, 4)) - ) - - # 状态信息维度: 速度 + 转向历史 + 障碍物信息 - state_dim = 7 # 增加障碍物相关维度 - - # 融合层 - self.fc_layers = nn.Sequential( - nn.Linear(32 * 4 * 4 + state_dim, 128), # 增加网络容量 - nn.ReLU(), - nn.Dropout(0.1), # 添加dropout防止过拟合 - nn.Linear(128, 64), - nn.ReLU(), - nn.Linear(64, 32), - nn.ReLU(), - nn.Linear(32, 3) # [throttle, brake, steer] - ) - def forward(self, image, state): - # 处理图像 - visual_features = self.conv_layers(image) - visual_features = visual_features.view(visual_features.size(0), -1) - # 融合特征 - combined = torch.cat([visual_features, state], dim=1) +class SimpleController: + """简单但可靠的控制逻辑""" - # 输出控制 - control = self.fc_layers(combined) - throttle_brake = torch.sigmoid(control[:, :2]) - steer = torch.tanh(control[:, 2:]) - - return torch.cat([throttle_brake, steer], dim=1) - - -# 新增：障碍物检测器类 -class ObstacleDetector: - def __init__(self, world, vehicle, max_distance=50.0): + def __init__(self, world, vehicle): self.world = world self.vehicle = vehicle - self.max_distance = max_distance - self.blueprint_library = world.get_blueprint_library() - self.last_obstacle_info = { - 'has_obstacle': False, - 'distance': float('inf'), - 'relative_angle': 0.0, - 'obstacle_type': None - } - - def get_obstacle_info(self): - """检测前方障碍物信息""" - try: - vehicle_transform = self.vehicle.get_transform() - vehicle_location = vehicle_transform.location - vehicle_rotation = vehicle_transform.rotation + self.map = world.get_map() + self.target_speed = 30.0 # km/h + self.waypoint_distance = 5.0 + self.last_waypoint = None - # 获取车辆前方的向量 - forward_vector = vehicle_transform.get_forward_vector() + def get_control(self): + """基于路点的简单控制""" + # 获取车辆状态 + location = self.vehicle.get_location() + transform = self.vehicle.get_transform() + velocity = self.vehicle.get_velocity() - # 获取世界中的所有车辆（排除自身） - all_vehicles = self.world.get_actors().filter('vehicle.*') + # 计算速度 + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 # km/h - min_distance = float('inf') - closest_obstacle = None - relative_angle = 0.0 + # 获取路点 + waypoint = self.map.get_waypoint(location, project_to_road=True) - for other_vehicle in all_vehicles: - if other_vehicle.id == self.vehicle.id: - continue + if not waypoint: + # 如果没有找到路点，返回保守控制 + return 0.3, 0.0, 0.0 - other_location = other_vehicle.get_location() + # 获取下一个路点 + next_waypoints = waypoint.next(self.waypoint_distance) - # 计算距离 - distance = vehicle_location.distance(other_location) + if not next_waypoints: + # 如果没有下一个路点，使用当前路点 + target_waypoint = waypoint + else: + target_waypoint = next_waypoints[0] - if distance > self.max_distance: - continue + self.last_waypoint = target_waypoint - # 计算相对位置向量 - relative_vector = carla.Location( - other_location.x - vehicle_location.x, - other_location.y - vehicle_location.y, - 0 - ) + # 计算转向 + vehicle_yaw = math.radians(transform.rotation.yaw) + target_loc = target_waypoint.transform.location - # 计算角度（车辆前方与障碍物方向的夹角） - forward_2d = carla.Vector3D(forward_vector.x, forward_vector.y, 0) - relative_2d = carla.Vector3D(relative_vector.x, relative_vector.y, 0) - - # 归一化向量 - forward_2d_norm = math.sqrt(forward_2d.x ** 2 + forward_2d.y ** 2) - relative_2d_norm = math.sqrt(relative_2d.x ** 2 + relative_2d.y ** 2) - - if forward_2d_norm > 0 and relative_2d_norm > 0: - dot_product = forward_2d.x * relative_2d.x + forward_2d.y * relative_2d.y - cos_angle = dot_product / (forward_2d_norm * relative_2d_norm) - cos_angle = max(-1.0, min(1.0, cos_angle)) # 限制范围 - angle = math.acos(cos_angle) - - # 转换为角度 - angle_deg = math.degrees(angle) - - # 只考虑前方±60度范围内的障碍物 - if angle_deg <= 60 and distance < min_distance: - min_distance = distance - closest_obstacle = other_vehicle - relative_angle = angle_deg if relative_2d.y >= 0 else -angle_deg - - if closest_obstacle is not None and min_distance < self.max_distance: - self.last_obstacle_info = { - 'has_obstacle': True, - 'distance': min_distance, - 'relative_angle': relative_angle, - 'obstacle_type': closest_obstacle.type_id, - 'obstacle_speed': self.get_vehicle_speed(closest_obstacle) - } - else: - self.last_obstacle_info = { - 'has_obstacle': False, - 'distance': float('inf'), - 'relative_angle': 0.0, - 'obstacle_type': None, - 'obstacle_speed': 0.0 - } + # 计算相对位置 + dx = target_loc.x - location.x + dy = target_loc.y - location.y - return self.last_obstacle_info + local_x = dx * math.cos(vehicle_yaw) + dy * math.sin(vehicle_yaw) + local_y = -dx * math.sin(vehicle_yaw) + dy * math.cos(vehicle_yaw) - except Exception as e: - print(f"障碍物检测错误: {e}") - return self.last_obstacle_info - - def get_vehicle_speed(self, vehicle): - """获取车辆速度""" - velocity = vehicle.get_velocity() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) - return speed * 3.6 # 转换为km/h - - def visualize_obstacles(self, image, vehicle_transform): - """在图像上可视化障碍物检测结果""" - if not self.last_obstacle_info['has_obstacle']: - return image - - height, width = image.shape[:2] - - # 计算障碍物在图像中的位置（简化投影） - distance = self.last_obstacle_info['distance'] - angle = self.last_obstacle_info['relative_angle'] - - # 归一化角度到图像坐标 - x_pos = int(width / 2 + (angle / 60) * (width / 2)) - - # 根据距离计算大小和颜色 - if distance < 10: - color = (0, 0, 255) # 红色，很近 - radius = 15 - elif distance < 20: - color = (0, 165, 255) # 橙色 - radius = 10 + if abs(local_x) < 0.1: + steer = 0.0 else: - color = (0, 255, 255) # 黄色 - radius = 5 + angle = math.atan2(local_y, local_x) + steer = max(-0.5, min(0.5, angle / 1.0)) - # 绘制障碍物指示器 - cv2.circle(image, (x_pos, int(height * 0.8)), radius, color, -1) - cv2.putText(image, f"{distance:.1f}m", (x_pos - 20, int(height * 0.8) - 20), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2) + # 速度控制 + if speed < self.target_speed * 0.8: + throttle, brake = 0.6, 0.0 + elif speed > self.target_speed * 1.2: + throttle, brake = 0.0, 0.3 + else: + throttle, brake = 0.3, 0.0 - return image + return throttle, brake, steer -# 修复2: 改进的神经网络控制器（整合障碍物检测） -class ImprovedNeuralController: - def __init__(self, obstacle_detector=None): - self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') - print(f"使用设备: {self.device}") +class SimpleDrivingSystem: + def __init__(self): + self.client = None + self.world = None + self.vehicle = None + self.camera = None + self.controller = None + self.camera_image = None - # 使用简化网络 - self.model = SimpleDrivingNetwork().to(self.device) - self.model.eval() + def connect(self): + """连接到CARLA服务器""" + print("正在连接到CARLA服务器...") - # 控制历史，用于平滑 - self.control_history = deque(maxlen=5) + try: + # 尝试多种连接方式 + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(10.0) - # 障碍物检测器 - self.obstacle_detector = obstacle_detector + # 检查可用地图 + available_maps = self.client.get_available_maps() + print(f"可用地图: {available_maps}") - # 修复3: 更保守的初始控制 - self.last_throttle = 0.3 - self.last_brake = 0.0 - self.last_steer = 0.0 + # 加载地图 + self.world = self.client.load_world('Town01') + print("地图加载成功") - # 避障参数 - self.emergency_brake_distance = 5.0 # 紧急刹车距离 - self.safe_following_distance = 8.0 # 安全跟车距离 - self.obstacle_avoidance_steer = 0.0 + # 设置同步模式 + settings = self.world.get_settings() + settings.synchronous_mode = False # 先使用异步模式确保连接 + settings.fixed_delta_seconds = None + self.world.apply_settings(settings) - def preprocess_image(self, image): - """修复图像预处理""" - if image is None: - # 返回黑色图像 - return torch.zeros((1, 3, 120, 160), device=self.device) + print("连接成功！") + return True - try: - # 调整图像尺寸，减少计算量 - small_img = cv2.resize(image, (160, 120)) - img_tensor = torch.from_numpy(small_img).float().to(self.device) - img_tensor = img_tensor.permute(2, 0, 1).unsqueeze(0) / 255.0 - return img_tensor except Exception as e: - print(f"图像预处理错误: {e}") - return torch.zeros((1, 3, 120, 160), device=self.device) - - def preprocess_state(self, speed, steer_history, obstacle_info): - """修复状态预处理，加入障碍物信息""" - has_obstacle = 1.0 if obstacle_info['has_obstacle'] else 0.0 - normalized_distance = min(obstacle_info['distance'] / 50.0, 1.0) if obstacle_info['has_obstacle'] else 1.0 - normalized_angle = obstacle_info['relative_angle'] / 60.0 if obstacle_info['has_obstacle'] else 0.0 - - state_data = [ - speed / 20.0, # 归一化速度 - steer_history[-1] if steer_history else 0.0, # 最近转向 - steer_history[-2] if len(steer_history) > 1 else 0.0, # 前一次转向 - np.mean(steer_history) if steer_history else 0.0, # 平均转向 - has_obstacle, # 是否有障碍物 - normalized_distance, # 归一化距离 - normalized_angle # 归一化角度 - ] - return torch.tensor(state_data, device=self.device).unsqueeze(0) - - def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): - """应用避障逻辑 - 优化版本""" - if not obstacle_info['has_obstacle']: - return throttle, brake, steer - - distance = obstacle_info['distance'] - angle = obstacle_info['relative_angle'] - - # 紧急情况：前方有近距离障碍物 - if distance < self.emergency_brake_distance: - print(f"紧急刹车！距离障碍物: {distance:.1f}m") - return 0.0, 1.0, 0.0 # 紧急情况下保持直行，只刹车 - - # 中距离障碍物：减速并准备转向 - elif distance < self.safe_following_distance: - # 计算安全速度比例 - safe_speed_ratio = (distance - 3.0) / (self.safe_following_distance - 3.0) - safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio)) - - # 如果当前速度过高，减速 - target_speed = 15.0 * safe_speed_ratio # 目标速度最大15km/h - current_speed_kmh = speed * 3.6 - - if current_speed_kmh > target_speed: - throttle = 0.0 - brake = 0.4 * ((current_speed_kmh - target_speed) / current_speed_kmh) - else: - throttle = 0.3 * safe_speed_ratio - brake = 0.0 - - # 如果障碍物在正前方，尝试轻微转向避开 - if abs(angle) < 15: # 正前方±15度内 - # 根据障碍物距离决定转向幅度 - avoid_factor = max(0, 1.0 - distance / self.safe_following_distance) - avoid_steer = 0.3 * avoid_factor if angle >= 0 else -0.3 * avoid_factor - # 平滑转向 - 保持更多原始转向 - steer = 0.8 * steer + 0.2 * avoid_steer - - # 远距离障碍物：轻微调整 - elif distance < 25.0: - # 轻微减速 - if speed > 8.0: - throttle *= 0.7 - - # 如果障碍物在正前方，轻微转向 - if abs(angle) < 20: - avoid_steer = 0.15 if angle >= 0 else -0.15 - steer = 0.9 * steer + 0.1 * avoid_steer + print(f"连接失败: {e}") + print("请确保:") + print("1. CARLA服务器正在运行") + print("2. 服务器端口为2000") + print("3. 地图Town01可用") + return False - return throttle, brake, steer + def spawn_vehicle(self): + """生成车辆 - 简化版本""" + print("正在生成车辆...") - def get_control(self, image, speed, steer_history, obstacle_info): - """修复控制生成逻辑，加入避障""" try: - with torch.no_grad(): - # 预处理 - img_tensor = self.preprocess_image(image) - state_tensor = self.preprocess_state(speed, steer_history, obstacle_info) - - # 神经网络推理 - control_output = self.model(img_tensor, state_tensor) - - # 提取控制指令 - throttle = control_output[0, 0].item() - brake = control_output[0, 1].item() - steer = control_output[0, 2].item() - - # 修复4: 添加安全限制 - throttle = max(0.0, min(0.8, throttle)) # 限制最大油门 - brake = max(0.0, min(0.5, brake)) # 限制最大刹车 - steer = max(-0.5, min(0.5, steer)) # 限制转向幅度 - - # 应用避障逻辑 - throttle, brake, steer = self.apply_obstacle_avoidance( - throttle, brake, steer, obstacle_info, speed - ) + # 获取蓝图库 + blueprint_library = self.world.get_blueprint_library() - return throttle, brake, steer + # 选择车辆蓝图 + vehicle_bp = blueprint_library.find('vehicle.tesla.model3') + if not vehicle_bp: + print("未找到特斯拉蓝图，尝试其他车辆...") + vehicle_bp = blueprint_library.filter('vehicle.*')[0] - except Exception as e: - print(f"神经网络控制错误: {e}") - # 返回安全默认值 - return 0.3, 0.0, 0.0 + vehicle_bp.set_attribute('color', '255,0,0') # 红色 + # 获取出生点 + spawn_points = self.world.get_map().get_spawn_points() + print(f"找到 {len(spawn_points)} 个出生点") -# 修复5: 传统控制器作为备份（整合障碍物检测） -class TraditionalController: - """可靠的传统控制逻辑""" - - def __init__(self, world, obstacle_detector=None): - self.world = world - self.map = world.get_map() - self.waypoint_distance = 10.0 - self.last_waypoint = None - self.obstacle_detector = obstacle_detector - self.emergency_brake_distance = 6.0 - self.safe_following_distance = 10.0 - - def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info): - """传统控制器的避障逻辑 - 优化版本""" - if not obstacle_info['has_obstacle']: - return throttle, brake, steer - - distance = obstacle_info['distance'] - angle = obstacle_info['relative_angle'] - vehicle_speed = math.sqrt(vehicle.get_velocity().x ** 2 + - vehicle.get_velocity().y ** 2 + - vehicle.get_velocity().z ** 2) * 3.6 # km/h - - # 紧急刹车 - if distance < self.emergency_brake_distance: - print(f"传统控制：紧急刹车！距离: {distance:.1f}m") - return 0.0, 1.0, 0.0 - - # 减速跟随 - elif distance < self.safe_following_distance: - # 计算所需的安全距离（基于速度） - required_distance = max(5.0, vehicle_speed * 0.4) # 增加到0.4秒车距 - - if distance < required_distance: - # 距离太近，减速 - speed_ratio = distance / required_distance - if vehicle_speed > 10: - throttle = 0.0 - brake = 0.6 * (1.0 - speed_ratio) - else: - throttle = 0.2 * speed_ratio - brake = 0.0 - - # 如果障碍物在正前方，尝试变道 - if abs(angle) < 20: # 放宽角度范围 - location = vehicle.get_location() - waypoint = self.map.get_waypoint(location) - - # 检查相邻车道是否可用 - left_lane = waypoint.get_left_lane() - right_lane = waypoint.get_right_lane() - - # 优先选择转向较小的方向 - if left_lane and left_lane.lane_type == carla.LaneType.Driving: - steer = -0.25 - elif right_lane and right_lane.lane_type == carla.LaneType.Driving: - steer = 0.25 - else: - steer = 0.15 if angle >= 0 else -0.15 + if not spawn_points: + print("没有可用的出生点！") + return False - return throttle, brake, steer + # 选择第一个出生点 + spawn_point = spawn_points[0] - def get_control(self, vehicle): - """基于路点的传统控制，整合避障""" - # 获取车辆状态 - transform = vehicle.get_transform() - location = vehicle.get_location() - velocity = vehicle.get_velocity() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) * 3.6 # km/h + # 尝试生成车辆 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) - # 获取障碍物信息 - obstacle_info = None - if self.obstacle_detector: - obstacle_info = self.obstacle_detector.get_obstacle_info() + if not self.vehicle: + print("无法生成车辆，尝试清理现有车辆...") + # 清理现有车辆 + for actor in self.world.get_actors().filter('vehicle.*'): + actor.destroy() + time.sleep(0.5) - # 获取路点 - waypoint = self.map.get_waypoint(location, project_to_road=True) - next_waypoints = waypoint.next(self.waypoint_distance) + # 再次尝试 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) - if not next_waypoints: - # 如果没有找到路点，尝试获取当前路点 - next_waypoints = [waypoint] + if self.vehicle: + print(f"车辆生成成功！ID: {self.vehicle.id}") + print(f"位置: {spawn_point.location}") - target_waypoint = next_waypoints[0] - self.last_waypoint = target_waypoint + # 禁用自动驾驶 + self.vehicle.set_autopilot(False) - # 计算转向 - vehicle_yaw = math.radians(transform.rotation.yaw) - target_loc = target_waypoint.transform.location + return True + else: + print("车辆生成失败") + return False - dx = target_loc.x - location.x - dy = target_loc.y - location.y + except Exception as e: + print(f"生成车辆时出错: {e}") + return False - local_x = dx * math.cos(vehicle_yaw) + dy * math.sin(vehicle_yaw) - local_y = -dx * math.sin(vehicle_yaw) + dy * math.cos(vehicle_yaw) + def setup_camera(self): + """设置相机""" + print("正在设置相机...") - if abs(local_x) < 0.1: - steer = 0.0 - else: - angle = math.atan2(local_y, local_x) - steer = np.clip(angle / math.radians(45), -1.0, 1.0) - - # 速度控制（基于障碍物距离调整） - throttle = 0.0 - brake = 0.0 - - if obstacle_info and obstacle_info['has_obstacle']: - distance = obstacle_info['distance'] - - # 根据障碍物距离调整速度 - if distance < 15: - if speed > 20: - throttle = 0.0 - brake = 0.3 - elif speed > 10: - throttle = 0.1 - brake = 0.0 - else: - throttle = 0.3 - brake = 0.0 - elif distance < 30: - if speed > 30: - throttle = 0.0 - brake = 0.1 - else: - throttle = 0.4 - brake = 0.0 - else: - # 没有近距离障碍物，正常行驶 - if speed < 20: - throttle = 0.6 - brake = 0.0 - elif speed < 40: - throttle = 0.4 - brake = 0.0 - else: - throttle = 0.2 - brake = 0.1 - else: - # 没有障碍物，正常行驶 - if speed < 20: - throttle = 0.6 - brake = 0.0 - elif speed < 40: - throttle = 0.4 - brake = 0.0 - else: - throttle = 0.2 - brake = 0.1 + try: + blueprint_library = self.world.get_blueprint_library() + camera_bp = blueprint_library.find('sensor.camera.rgb') + + # 设置相机属性 + camera_bp.set_attribute('image_size_x', '640') + camera_bp.set_attribute('image_size_y', '480') + camera_bp.set_attribute('fov', '90') + + # 相机位置（车辆后方） + camera_transform = carla.Transform( + carla.Location(x=-8.0, z=6.0), # 在车辆后方上方 + carla.Rotation(pitch=-20.0) # 向下看 + ) - # 应用避障逻辑 - if obstacle_info: - throttle, brake, steer = self.apply_obstacle_avoidance( - throttle, brake, steer, vehicle, obstacle_info + # 生成相机 + self.camera = self.world.spawn_actor( + camera_bp, camera_transform, attach_to=self.vehicle ) - return throttle, brake, steer + # 设置回调函数 + self.camera.listen(lambda image: self.camera_callback(image)) + + print("相机设置成功") + return True + + except Exception as e: + print(f"设置相机时出错: {e}") + return False + def camera_callback(self, image): + """相机数据回调""" + try: + # 转换图像数据 + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + self.camera_image = array[:, :, :3] # RGB通道 + except: + pass -# CARLA初始化部分... -# 连接到本地CARLA服务器，端口2000 -client = carla.Client('localhost', 2000) -client.set_timeout(15.0) -world = client.load_world('Town01') - -# 获取并设置世界的运行参数 -settings = world.get_settings() -settings.synchronous_mode = True -settings.fixed_delta_seconds = 0.1 -world.apply_settings(settings) - -# 定义天气参数 -weather = carla.WeatherParameters( - cloudiness=30.0, - precipitation=0.0, - sun_altitude_angle=70.0 -) -world.set_weather(weather) - -# 获取地图和出生点 -map = world.get_map() -spawn_points = map.get_spawn_points() -if not spawn_points: - raise Exception("No spawn points available") - -# 选择更合适的出生点 -spawn_point = spawn_points[10] - -# 生成车辆 -blueprint_library = world.get_blueprint_library() -vehicle_bp = blueprint_library.find('vehicle.tesla.model3') -vehicle_bp.set_attribute('color', '255,0,0') -vehicle = world.spawn_actor(vehicle_bp, spawn_point) - -if not vehicle: - raise Exception("无法生成主车辆") - -vehicle.set_autopilot(False) -vehicle.set_simulate_physics(True) - -print(f"车辆生成在位置: {spawn_point.location}") - -# 改进的NPC车辆生成和设置 -print("生成NPC车辆...") -obstacle_count = 3 -npc_vehicles = [] # 存储NPC车辆以便后续管理 - -# 获取所有可用的车辆蓝图 -vehicle_blueprints = blueprint_library.filter('vehicle.*') - -# 选择远离主车辆的出生点（避免直接堵塞） -valid_spawn_points = [] -main_spawn_location = spawn_point.location - -for point in spawn_points: - distance = main_spawn_location.distance(point.location) - # 选择距离主车辆50米以上的出生点，避免直接碰撞 - if distance > 50.0: - valid_spawn_points.append(point) - if len(valid_spawn_points) >= obstacle_count: - break - -# 如果找不到足够远的点，使用所有点 -if len(valid_spawn_points) < obstacle_count: - print("警告：找不到足够远的出生点，使用所有可用点") - valid_spawn_points = spawn_points[:obstacle_count] - -# 生成NPC车辆 -for i in range(min(obstacle_count, len(valid_spawn_points))): - try: - # 随机选择车辆类型（排除特斯拉，使场景更丰富） - available_blueprints = [bp for bp in vehicle_blueprints if 'tesla' not in bp.id] - if not available_blueprints: - available_blueprints = vehicle_blueprints - - npc_bp = random.choice(available_blueprints) - - # 设置车辆颜色 - if npc_bp.has_attribute('color'): - colors = npc_bp.get_attribute('color').recommended_values - if colors: - npc_bp.set_attribute('color', random.choice(colors)) + def setup_controller(self): + """设置控制器""" + self.controller = SimpleController(self.world, self.vehicle) + print("控制器设置完成") + + def run(self): + """主运行循环""" + print("\n" + "=" * 50) + print("简化自动驾驶系统") + print("=" * 50) + + # 连接服务器 + if not self.connect(): + return # 生成车辆 - npc_vehicle = world.try_spawn_actor(npc_bp, valid_spawn_points[i]) + if not self.spawn_vehicle(): + return + + # 设置相机 + if not self.setup_camera(): + # 即使相机失败也继续运行 + print("警告：相机设置失败，继续运行...") + + # 设置控制器 + self.setup_controller() + + # 等待一会儿让系统稳定 + print("系统初始化中...") + time.sleep(2.0) + + # 设置天气 + weather = carla.WeatherParameters( + cloudiness=30.0, + precipitation=0.0, + sun_altitude_angle=70.0 + ) + self.world.set_weather(weather) - if npc_vehicle: - # 启用自动驾驶模式并设置速度限制 - npc_vehicle.set_autopilot(True) - - # 设置NPC车辆的速度限制（让它们能正常行驶） - traffic_manager = client.get_trafficmanager() - traffic_manager.set_global_distance_to_leading_vehicle(2.5) # 设置跟车距离 - traffic_manager.set_random_device_seed(12345) # 设置随机种子确保一致性 + # 生成一些NPC车辆 + self.spawn_npc_vehicles(2) + + print("\n系统准备就绪！") + print("控制指令:") + print(" q - 退出程序") + print(" r - 重置车辆") + print(" s - 紧急停止") + print("\n开始自动驾驶...\n") + + frame_count = 0 + running = True - # 为每个NPC车辆设置不同的速度限制（避免所有车速度相同导致堵塞） - target_speed = random.uniform(30.0, 50.0) # 30-50 km/h - traffic_manager.vehicle_percentage_speed_difference(npc_vehicle, random.uniform(-10, 10)) - - # 设置NPC车辆的驾驶行为（更安全、更智能） - traffic_manager.auto_lane_change(npc_vehicle, True) # 允许自动变道 - traffic_manager.distance_to_leading_vehicle(npc_vehicle, 3.0) # 设置跟车距离 - traffic_manager.collision_detection(npc_vehicle, world, True) # 启用碰撞检测 - - npc_vehicles.append(npc_vehicle) - print(f"生成NPC车辆 {i + 1}: {npc_bp.id} 在位置 {valid_spawn_points[i].location}") - - # 短暂暂停，避免生成时的碰撞 - time.sleep(0.1) - - except Exception as e: - print(f"生成NPC车辆 {i + 1} 时出错: {e}") - -print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") - -# 配置传感器（简化配置） -third_camera_bp = blueprint_library.find('sensor.camera.rgb') -third_camera_bp.set_attribute('image_size_x', '640') -third_camera_bp.set_attribute('image_size_y', '480') -third_camera_bp.set_attribute('fov', '110') -third_camera_transform = carla.Transform( - carla.Location(x=-5.0, y=0.0, z=3.0), - carla.Rotation(pitch=-15.0) -) -third_camera = world.spawn_actor(third_camera_bp, third_camera_transform, attach_to=vehicle) - -front_camera_bp = blueprint_library.find('sensor.camera.rgb') -front_camera_bp.set_attribute('image_size_x', '640') -front_camera_bp.set_attribute('image_size_y', '480') -front_camera_bp.set_attribute('fov', '90') -front_camera_transform = carla.Transform( - carla.Location(x=2.0, y=0.0, z=1.5), - carla.Rotation(pitch=0.0) -) -front_camera = world.spawn_actor(front_camera_bp, front_camera_transform, attach_to=vehicle) - -# 传感器数据存储 -third_image = None -front_image = None - - -def third_camera_callback(image): - global third_image - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - third_image = array[:, :, :3] - - -def front_camera_callback(image): - global front_image - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - front_image = array[:, :, :3] - - -third_camera.listen(third_camera_callback) -front_camera.listen(front_camera_callback) - -time.sleep(2.0) - -# 新增：初始化障碍物检测器 -print("初始化障碍物检测器...") -obstacle_detector = ObstacleDetector(world, vehicle, max_distance=50.0) - -# 修复6: 初始化控制器（传入障碍物检测器） -nn_controller = ImprovedNeuralController(obstacle_detector) -traditional_controller = TraditionalController(world, obstacle_detector) - -# 控制变量 -throttle = 0.3 # 更保守的初始油门 -steer = 0.0 -brake = 0.0 -NEURAL_NETWORK_MODE = False # 默认使用传统控制，更稳定 - -# 转向历史，用于平滑 -steer_history = deque(maxlen=10) - -# 障碍物信息历史 -obstacle_history = deque(maxlen=5) - -print("初始化车辆状态...") -vehicle.set_simulate_physics(True) - -# 修复7: 更温和的启动控制 -print("应用启动控制...") -vehicle.apply_control(carla.VehicleControl( - throttle=0.5, # 降低初始油门 - steer=0.0, - brake=0.0, - hand_brake=False -)) - - -# 添加NPC车辆管理函数 -def check_and_reset_stuck_npcs(): - """检查并重置卡住的NPC车辆""" - for npc in npc_vehicles: try: - npc_speed = math.sqrt( - npc.get_velocity().x ** 2 + npc.get_velocity().y ** 2 + npc.get_velocity().z ** 2) * 3.6 - # 如果NPC车辆速度过低（小于1km/h）且没有碰撞，可能是卡住了 - if npc_speed < 1.0: - print(f"检测到NPC车辆 {npc.id} 可能卡住，尝试重置...") - # 获取当前位置 - current_transform = npc.get_transform() - # 寻找最近的可用出生点 - closest_point = None - min_distance = float('inf') - for point in spawn_points: - distance = point.location.distance(current_transform.location) - if distance < min_distance and distance > 10.0: # 避免重置到太近的位置 - min_distance = distance - closest_point = point - - if closest_point: - npc.set_transform(closest_point) - npc.set_autopilot(True) - print(f"重置NPC车辆 {npc.id} 到新位置") - except: - pass + while running: + # 获取车辆状态 + velocity = self.vehicle.get_velocity() + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 + + # 获取控制指令 + throttle, brake, steer = self.controller.get_control() + + # 应用控制 + control = carla.VehicleControl( + throttle=float(throttle), + brake=float(brake), + steer=float(steer), + hand_brake=False, + reverse=False + ) + self.vehicle.apply_control(control) + + # 更新显示 + if self.camera_image is not None: + display_img = self.camera_image.copy() + + # 添加状态信息 + cv2.putText(display_img, f"Speed: {speed:.1f} km/h", + (20, 40), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) + cv2.putText(display_img, f"Throttle: {throttle:.2f}", + (20, 80), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) + cv2.putText(display_img, f"Steer: {steer:.2f}", + (20, 120), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) + cv2.putText(display_img, f"Frame: {frame_count}", + (20, 160), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) + + cv2.imshow('Autonomous Driving - Simple Version', display_img) + + # 处理按键 + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + print("正在退出...") + running = False + elif key == ord('r'): + self.reset_vehicle() + elif key == ord('s'): + # 紧急停止 + self.vehicle.apply_control(carla.VehicleControl( + throttle=0.0, brake=1.0, hand_brake=True + )) + print("紧急停止") + + frame_count += 1 + + # 每100帧显示一次状态 + if frame_count % 100 == 0: + print(f"运行中... 帧数: {frame_count}, 速度: {speed:.1f} km/h") + + time.sleep(0.05) + + except KeyboardInterrupt: + print("\n用户中断") + except Exception as e: + print(f"运行错误: {e}") + finally: + self.cleanup() + def spawn_npc_vehicles(self, count=2): + """生成NPC车辆（简化）""" + print(f"正在生成 {count} 辆NPC车辆...") -try: - print("自动驾驶系统启动 - 初始模式: 传统控制") - print("控制键: q-退出, m-切换控制模式, r-重置车辆, t-传统模式, n-神经网络模式") - print(f"当前有 {len(npc_vehicles)} 辆NPC车辆在运行") + try: + blueprint_library = self.world.get_blueprint_library() + spawn_points = self.world.get_map().get_spawn_points() - frame_count = 0 - stuck_count = 0 - last_position = vehicle.get_location() - success_count = 0 # 成功运行计数器 - collision_count = 0 # 碰撞计数器 - last_collision_time = 0 # 上次碰撞时间 - last_npc_check_time = 0 # 上次检查NPC的时间 + npc_vehicles = [] - # 主循环 - while True: - world.tick() - frame_count += 1 + for i in range(min(count, len(spawn_points))): + # 跳过主车辆的出生点 + if i == 0: + continue - # 定期检查NPC车辆状态（每100帧检查一次） - if frame_count - last_npc_check_time > 100: - check_and_reset_stuck_npcs() - last_npc_check_time = frame_count + try: + # 随机选择车辆类型 + vehicle_bps = list(blueprint_library.filter('vehicle.*')) + if vehicle_bps: + vehicle_bp = random.choice(vehicle_bps) - # 获取车辆状态 - vehicle_transform = vehicle.get_transform() - vehicle_location = vehicle.get_location() - vehicle_velocity = vehicle.get_velocity() - vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2) - - # 检测障碍物 - obstacle_info = obstacle_detector.get_obstacle_info() - obstacle_history.append(obstacle_info) - - # 显示障碍物信息 - if obstacle_info['has_obstacle']: - print(f"障碍物检测: 距离={obstacle_info['distance']:.1f}m, " - f"角度={obstacle_info['relative_angle']:.1f}°, " - f"类型={obstacle_info['obstacle_type']}") - - print(f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, " - f"模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}, " - f"障碍物={'有' if obstacle_info['has_obstacle'] else '无'}") - - # 修复8: 改进的卡住检测 - current_position = vehicle_location - distance_moved = current_position.distance(last_position) - - # 更精确的卡住检测 - is_moving = distance_moved > 0.2 or vehicle_speed > 1.0 - if not is_moving: - stuck_count += 1 - else: - stuck_count = 0 - success_count += 1 # 成功运行一帧 + # 生成NPC + npc = self.world.try_spawn_actor(vehicle_bp, spawn_points[i]) - last_position = current_position + if npc: + npc.set_autopilot(True) + npc_vehicles.append(npc) + print(f"生成NPC车辆 {len(npc_vehicles)}") + except: + pass - # 修复9: 更智能的卡住恢复 - if stuck_count > 15: # 1.5秒后认为卡住 - print("检测到车辆卡住，执行恢复程序...") + print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") - # 先完全停止 - vehicle.apply_control(carla.VehicleControl( - throttle=0.0, steer=0.0, brake=1.0, hand_brake=True - )) - time.sleep(0.5) + except Exception as e: + print(f"生成NPC车辆时出错: {e}") - # 检查是否有前方障碍物 - if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 10: - print("前方有障碍物，尝试倒车...") - # 倒车 - vehicle.apply_control(carla.VehicleControl( - throttle=0.0, steer=0.0, brake=0.0, reverse=True - )) - time.sleep(1.0) - else: - # 然后尝试不同方向的脱困 - recovery_steer = random.choice([-0.5, 0.5]) # 随机选择方向 - vehicle.apply_control(carla.VehicleControl( - throttle=0.8, steer=recovery_steer, brake=0.0, hand_brake=False - )) - time.sleep(1.0) - - stuck_count = 0 - success_count = 0 - - # 每成功运行100帧显示一次状态 - if success_count % 100 == 0: - print(f"已成功运行 {success_count} 帧") - - # 控制逻辑 - if NEURAL_NETWORK_MODE: - # 神经网络控制（传入障碍物信息） - nn_throttle, nn_brake, nn_steer = nn_controller.get_control( - front_image, vehicle_speed, steer_history, obstacle_info - ) + def reset_vehicle(self): + """重置车辆位置""" + print("重置车辆...") - # 修复10: 更激进的控制平滑 - throttle = 0.3 * throttle + 0.7 * nn_throttle - brake = 0.3 * brake + 0.7 * nn_brake - steer = 0.2 * steer + 0.8 * nn_steer + spawn_points = self.world.get_map().get_spawn_points() + if spawn_points: + new_spawn_point = random.choice(spawn_points) + self.vehicle.set_transform(new_spawn_point) + print(f"车辆已重置到新位置: {new_spawn_point.location}") - # 记录转向历史 - steer_history.append(steer) + # 等待重置完成 + time.sleep(0.5) - else: - # 传统控制 - 更稳定 - throttle, brake, steer = traditional_controller.get_control(vehicle) - steer_history.append(steer) - - # 应用控制 - control = carla.VehicleControl( - throttle=throttle, - steer=steer, - brake=brake, - hand_brake=False, - reverse=False - ) + def cleanup(self): + """清理资源""" + print("\n正在清理资源...") - vehicle.apply_control(control) - - # 显示和输入处理 - if third_image is not None: - display_image = third_image.copy() - - # 可视化障碍物检测结果 - display_image = obstacle_detector.visualize_obstacles(display_image, vehicle_transform) - - # 显示信息 - cv2.putText(display_image, f"Speed: {vehicle_speed * 3.6:.1f} km/h", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Mode: {'Neural' if NEURAL_NETWORK_MODE else 'Traditional'}", - (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Brake: {brake:.2f}", (10, 150), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - - # 显示障碍物信息 - if obstacle_info['has_obstacle']: - cv2.putText(display_image, f"Obstacle: {obstacle_info['distance']:.1f}m", (10, 180), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - else: - cv2.putText(display_image, "Obstacle: None", (10, 180), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - - # 卡住警告 - if stuck_count > 5: - cv2.putText(display_image, "STUCK DETECTED!", (10, 210), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - - # 安全区域标记 - cv2.rectangle(display_image, (240, 360), (400, 480), (0, 255, 0), 2) # 前方安全区域 - - cv2.imshow('自动驾驶系统 - 带障碍物检测', display_image) - - key = cv2.waitKey(1) & 0xFF - if key == ord('q'): - break - elif key == ord('m'): - NEURAL_NETWORK_MODE = not NEURAL_NETWORK_MODE - print(f"切换到{'神经网络' if NEURAL_NETWORK_MODE else '传统'}控制模式") - elif key == ord('t'): - NEURAL_NETWORK_MODE = False - print("切换到传统控制模式") - elif key == ord('n'): - NEURAL_NETWORK_MODE = True - print("切换到神经网络控制模式") - elif key == ord('r'): - # 重置车辆 - vehicle.set_transform(spawn_point) - throttle = 0.3 - steer = 0.0 - brake = 1.0 - stuck_count = 0 - success_count = 0 - collision_count = 0 - steer_history.clear() - obstacle_history.clear() - print("车辆已重置") - - time.sleep(0.01) - -except KeyboardInterrupt: - print("系统已停止") -except Exception as e: - print(f"系统错误: {e}") - import traceback - - traceback.print_exc() - -finally: - print("正在清理资源...") - third_camera.stop() - front_camera.stop() - - # 销毁actor - for actor in world.get_actors(): - if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'): + if self.camera: + try: + self.camera.stop() + self.camera.destroy() + except: + pass + + if self.vehicle: try: - actor.destroy() + self.vehicle.destroy() except: pass - # 恢复设置 - settings.synchronous_mode = False - world.apply_settings(settings) - cv2.destroyAllWindows() - print("资源清理完成") \ No newline at end of file + # 等待销毁完成 + time.sleep(1.0) + + cv2.destroyAllWindows() + print("清理完成") + + +def main(): + """主函数""" + print("自动驾驶系统 - 简化版本") + print("确保CARLA服务器正在运行...") + + system = SimpleDrivingSystem() + system.run() + + +if __name__ == "__main__": + main() \ No newline at end of file From 0cff48109fd3f507b11346b6fa6ae58434b098a5 Mon Sep 17 00:00:00 2001 From: chen Date: Sat, 27 Dec 2025 15:54:34 +0800 Subject: [PATCH 25/25] =?UTF-8?q?=E9=80=9A=E8=BF=87=E7=AE=80=E5=8C=96?= =?UTF-8?q?=E7=B3=BB=E7=BB=9F=E6=9E=B6=E6=9E=84=EF=BC=8C=E6=8F=90=E9=AB=98?= =?UTF-8?q?=E7=A8=B3=E5=AE=9A=E6=80=A7=E5=92=8C=E5=8F=AF=E9=9D=A0=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 810 +----------------------------- 1 file changed, 1 insertion(+), 809 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index fb2e10b2fd..7d02bb9fc7 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -10,58 +10,6 @@ from collections import deque import random -import os - - -# 修复1: 简化的神经网络架构（增加障碍物检测通道） -class SimpleDrivingNetwork(nn.Module): - """ - 简化的驾驶网络 - 增加障碍物感知 - """ - - def __init__(self): - super(SimpleDrivingNetwork, self).__init__() - - # 图像处理分支 (简化) - self.conv_layers = nn.Sequential( - nn.Conv2d(3, 8, kernel_size=5, stride=2), - nn.ReLU(), - nn.Conv2d(8, 16, kernel_size=5, stride=2), - nn.ReLU(), - nn.Conv2d(16, 32, kernel_size=3, stride=2), - nn.ReLU(), - nn.AdaptiveAvgPool2d((4, 4)) - ) - - # 状态信息维度: 速度 + 转向历史 + 障碍物信息 - state_dim = 7 # 增加障碍物相关维度 - - # 融合层 - self.fc_layers = nn.Sequential( - nn.Linear(32 * 4 * 4 + state_dim, 128), # 增加网络容量 - nn.ReLU(), - nn.Dropout(0.1), # 添加dropout防止过拟合 - nn.Linear(128, 64), - nn.ReLU(), - nn.Linear(64, 32), - nn.ReLU(), - nn.Linear(32, 3) # [throttle, brake, steer] - ) - - def forward(self, image, state): - # 处理图像 - visual_features = self.conv_layers(image) - visual_features = visual_features.view(visual_features.size(0), -1) - - # 融合特征 - combined = torch.cat([visual_features, state], dim=1) - - # 输出控制 - control = self.fc_layers(combined) - throttle_brake = torch.sigmoid(control[:, :2]) - steer = torch.tanh(control[:, 2:]) - - class SimpleController: """简单但可靠的控制逻辑""" @@ -69,299 +17,11 @@ class SimpleController: def __init__(self, world, vehicle): self.world = world self.vehicle = vehicle - -# 新增：障碍物检测器类 -class ObstacleDetector: - def __init__(self, world, vehicle, max_distance=50.0): - self.world = world - self.vehicle = vehicle - self.max_distance = max_distance - self.blueprint_library = world.get_blueprint_library() - self.last_obstacle_info = { - 'has_obstacle': False, - 'distance': float('inf'), - 'relative_angle': 0.0, - 'obstacle_type': None - } - - def get_obstacle_info(self): - """检测前方障碍物信息""" - try: - vehicle_transform = self.vehicle.get_transform() - vehicle_location = vehicle_transform.location - vehicle_rotation = vehicle_transform.rotation - - # 获取车辆前方的向量 - forward_vector = vehicle_transform.get_forward_vector() - - # 获取世界中的所有车辆（排除自身） - all_vehicles = self.world.get_actors().filter('vehicle.*') - - min_distance = float('inf') - closest_obstacle = None - relative_angle = 0.0 - - for other_vehicle in all_vehicles: - if other_vehicle.id == self.vehicle.id: - continue - - other_location = other_vehicle.get_location() - - # 计算距离 - distance = vehicle_location.distance(other_location) - - if distance > self.max_distance: - continue - - # 计算相对位置向量 - relative_vector = carla.Location( - other_location.x - vehicle_location.x, - other_location.y - vehicle_location.y, - 0 - ) - - # 计算角度（车辆前方与障碍物方向的夹角） - forward_2d = carla.Vector3D(forward_vector.x, forward_vector.y, 0) - relative_2d = carla.Vector3D(relative_vector.x, relative_vector.y, 0) - - # 归一化向量 - forward_2d_norm = math.sqrt(forward_2d.x ** 2 + forward_2d.y ** 2) - relative_2d_norm = math.sqrt(relative_2d.x ** 2 + relative_2d.y ** 2) - - if forward_2d_norm > 0 and relative_2d_norm > 0: - dot_product = forward_2d.x * relative_2d.x + forward_2d.y * relative_2d.y - cos_angle = dot_product / (forward_2d_norm * relative_2d_norm) - cos_angle = max(-1.0, min(1.0, cos_angle)) # 限制范围 - angle = math.acos(cos_angle) - - # 转换为角度 - angle_deg = math.degrees(angle) - - # 只考虑前方±60度范围内的障碍物 - if angle_deg <= 60 and distance < min_distance: - min_distance = distance - closest_obstacle = other_vehicle - relative_angle = angle_deg if relative_2d.y >= 0 else -angle_deg - - if closest_obstacle is not None and min_distance < self.max_distance: - self.last_obstacle_info = { - 'has_obstacle': True, - 'distance': min_distance, - 'relative_angle': relative_angle, - 'obstacle_type': closest_obstacle.type_id, - 'obstacle_speed': self.get_vehicle_speed(closest_obstacle) - } - else: - self.last_obstacle_info = { - 'has_obstacle': False, - 'distance': float('inf'), - 'relative_angle': 0.0, - 'obstacle_type': None, - 'obstacle_speed': 0.0 - } - - return self.last_obstacle_info - - except Exception as e: - print(f"障碍物检测错误: {e}") - return self.last_obstacle_info - - def get_vehicle_speed(self, vehicle): - """获取车辆速度""" - velocity = vehicle.get_velocity() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) - return speed * 3.6 # 转换为km/h - - def visualize_obstacles(self, image, vehicle_transform): - """在图像上可视化障碍物检测结果""" - if not self.last_obstacle_info['has_obstacle']: - return image - - height, width = image.shape[:2] - - # 计算障碍物在图像中的位置（简化投影） - distance = self.last_obstacle_info['distance'] - angle = self.last_obstacle_info['relative_angle'] - - # 归一化角度到图像坐标 - x_pos = int(width / 2 + (angle / 60) * (width / 2)) - - # 根据距离计算大小和颜色 - if distance < 10: - color = (0, 0, 255) # 红色，很近 - radius = 15 - elif distance < 20: - color = (0, 165, 255) # 橙色 - radius = 10 - else: - color = (0, 255, 255) # 黄色 - radius = 5 - - # 绘制障碍物指示器 - cv2.circle(image, (x_pos, int(height * 0.8)), radius, color, -1) - cv2.putText(image, f"{distance:.1f}m", (x_pos - 20, int(height * 0.8) - 20), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2) - - return image - - -# 修复2: 改进的神经网络控制器（整合障碍物检测） -class ImprovedNeuralController: - def __init__(self, obstacle_detector=None): - self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') - print(f"使用设备: {self.device}") - - # 使用简化网络 - self.model = SimpleDrivingNetwork().to(self.device) - self.model.eval() - - # 控制历史，用于平滑 - self.control_history = deque(maxlen=5) - - # 障碍物检测器 - self.obstacle_detector = obstacle_detector - - # 修复3: 更保守的初始控制 - self.last_throttle = 0.3 - self.last_brake = 0.0 - self.last_steer = 0.0 - - # 避障参数 - self.emergency_brake_distance = 5.0 # 紧急刹车距离 - self.safe_following_distance = 8.0 # 安全跟车距离 - self.obstacle_avoidance_steer = 0.0 - - def preprocess_image(self, image): - """修复图像预处理""" - if image is None: - # 返回黑色图像 - return torch.zeros((1, 3, 120, 160), device=self.device) - - try: - # 调整图像尺寸，减少计算量 - small_img = cv2.resize(image, (160, 120)) - img_tensor = torch.from_numpy(small_img).float().to(self.device) - img_tensor = img_tensor.permute(2, 0, 1).unsqueeze(0) / 255.0 - return img_tensor - except Exception as e: - print(f"图像预处理错误: {e}") - return torch.zeros((1, 3, 120, 160), device=self.device) - - def preprocess_state(self, speed, steer_history, obstacle_info): - """修复状态预处理，加入障碍物信息""" - has_obstacle = 1.0 if obstacle_info['has_obstacle'] else 0.0 - normalized_distance = min(obstacle_info['distance'] / 50.0, 1.0) if obstacle_info['has_obstacle'] else 1.0 - normalized_angle = obstacle_info['relative_angle'] / 60.0 if obstacle_info['has_obstacle'] else 0.0 - - state_data = [ - speed / 20.0, # 归一化速度 - steer_history[-1] if steer_history else 0.0, # 最近转向 - steer_history[-2] if len(steer_history) > 1 else 0.0, # 前一次转向 - np.mean(steer_history) if steer_history else 0.0, # 平均转向 - has_obstacle, # 是否有障碍物 - normalized_distance, # 归一化距离 - normalized_angle # 归一化角度 - ] - return torch.tensor(state_data, device=self.device).unsqueeze(0) - - def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): - """应用避障逻辑 - 优化版本""" - if not obstacle_info['has_obstacle']: - return throttle, brake, steer - - distance = obstacle_info['distance'] - angle = obstacle_info['relative_angle'] - - # 紧急情况：前方有近距离障碍物 - if distance < self.emergency_brake_distance: - print(f"紧急刹车！距离障碍物: {distance:.1f}m") - return 0.0, 1.0, 0.0 # 紧急情况下保持直行，只刹车 - - # 中距离障碍物：减速并准备转向 - elif distance < self.safe_following_distance: - # 计算安全速度比例 - safe_speed_ratio = (distance - 3.0) / (self.safe_following_distance - 3.0) - safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio)) - - # 如果当前速度过高，减速 - target_speed = 15.0 * safe_speed_ratio # 目标速度最大15km/h - current_speed_kmh = speed * 3.6 - - if current_speed_kmh > target_speed: - throttle = 0.0 - brake = 0.4 * ((current_speed_kmh - target_speed) / current_speed_kmh) - else: - throttle = 0.3 * safe_speed_ratio - brake = 0.0 - - # 如果障碍物在正前方，尝试轻微转向避开 - if abs(angle) < 15: # 正前方±15度内 - # 根据障碍物距离决定转向幅度 - avoid_factor = max(0, 1.0 - distance / self.safe_following_distance) - avoid_steer = 0.3 * avoid_factor if angle >= 0 else -0.3 * avoid_factor - # 平滑转向 - 保持更多原始转向 - steer = 0.8 * steer + 0.2 * avoid_steer - - # 远距离障碍物：轻微调整 - elif distance < 25.0: - # 轻微减速 - if speed > 8.0: - throttle *= 0.7 - - # 如果障碍物在正前方，轻微转向 - if abs(angle) < 20: - avoid_steer = 0.15 if angle >= 0 else -0.15 - steer = 0.9 * steer + 0.1 * avoid_steer - - return throttle, brake, steer - - def get_control(self, image, speed, steer_history, obstacle_info): - """修复控制生成逻辑，加入避障""" - try: - with torch.no_grad(): - # 预处理 - img_tensor = self.preprocess_image(image) - state_tensor = self.preprocess_state(speed, steer_history, obstacle_info) - - # 神经网络推理 - control_output = self.model(img_tensor, state_tensor) - - # 提取控制指令 - throttle = control_output[0, 0].item() - brake = control_output[0, 1].item() - steer = control_output[0, 2].item() - - # 修复4: 添加安全限制 - throttle = max(0.0, min(0.8, throttle)) # 限制最大油门 - brake = max(0.0, min(0.5, brake)) # 限制最大刹车 - steer = max(-0.5, min(0.5, steer)) # 限制转向幅度 - - # 应用避障逻辑 - throttle, brake, steer = self.apply_obstacle_avoidance( - throttle, brake, steer, obstacle_info, speed - ) - - return throttle, brake, steer - - except Exception as e: - print(f"神经网络控制错误: {e}") - # 返回安全默认值 - return 0.3, 0.0, 0.0 - - -# 修复5: 传统控制器作为备份（整合障碍物检测） -class TraditionalController: - """可靠的传统控制逻辑""" - - def __init__(self, world, obstacle_detector=None): - self.world = world - self.map = world.get_map() self.target_speed = 30.0 # km/h self.waypoint_distance = 5.0 self.last_waypoint = None - def get_control(self): """基于路点的简单控制""" # 获取车辆状态 @@ -372,74 +32,6 @@ def get_control(self): # 计算速度 speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 # km/h - self.obstacle_detector = obstacle_detector - self.emergency_brake_distance = 6.0 - self.safe_following_distance = 10.0 - - def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info): - """传统控制器的避障逻辑 - 优化版本""" - if not obstacle_info['has_obstacle']: - return throttle, brake, steer - - distance = obstacle_info['distance'] - angle = obstacle_info['relative_angle'] - vehicle_speed = math.sqrt(vehicle.get_velocity().x ** 2 + - vehicle.get_velocity().y ** 2 + - vehicle.get_velocity().z ** 2) * 3.6 # km/h - - # 紧急刹车 - if distance < self.emergency_brake_distance: - print(f"传统控制：紧急刹车！距离: {distance:.1f}m") - return 0.0, 1.0, 0.0 - - # 减速跟随 - elif distance < self.safe_following_distance: - # 计算所需的安全距离（基于速度） - required_distance = max(5.0, vehicle_speed * 0.4) # 增加到0.4秒车距 - - if distance < required_distance: - # 距离太近，减速 - speed_ratio = distance / required_distance - if vehicle_speed > 10: - throttle = 0.0 - brake = 0.6 * (1.0 - speed_ratio) - else: - throttle = 0.2 * speed_ratio - brake = 0.0 - - # 如果障碍物在正前方，尝试变道 - if abs(angle) < 20: # 放宽角度范围 - location = vehicle.get_location() - waypoint = self.map.get_waypoint(location) - - # 检查相邻车道是否可用 - left_lane = waypoint.get_left_lane() - right_lane = waypoint.get_right_lane() - - # 优先选择转向较小的方向 - if left_lane and left_lane.lane_type == carla.LaneType.Driving: - steer = -0.25 - elif right_lane and right_lane.lane_type == carla.LaneType.Driving: - steer = 0.25 - else: - steer = 0.15 if angle >= 0 else -0.15 - - return throttle, brake, steer - - def get_control(self, vehicle): - """基于路点的传统控制，整合避障""" - # 获取车辆状态 - transform = vehicle.get_transform() - location = vehicle.get_location() - velocity = vehicle.get_velocity() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) * 3.6 # km/h - - # 获取障碍物信息 - obstacle_info = None - if self.obstacle_detector: - obstacle_info = self.obstacle_detector.get_obstacle_info() - - # 获取路点 waypoint = self.map.get_waypoint(location, project_to_road=True) @@ -473,7 +65,6 @@ def get_control(self, vehicle): steer = 0.0 else: angle = math.atan2(local_y, local_x) - steer = max(-0.5, min(0.5, angle / 1.0)) # 速度控制 @@ -500,182 +91,6 @@ def connect(self): """连接到CARLA服务器""" print("正在连接到CARLA服务器...") - steer = np.clip(angle / math.radians(45), -1.0, 1.0) - - # 速度控制（基于障碍物距离调整） - throttle = 0.0 - brake = 0.0 - - if obstacle_info and obstacle_info['has_obstacle']: - distance = obstacle_info['distance'] - - # 根据障碍物距离调整速度 - if distance < 15: - if speed > 20: - throttle = 0.0 - brake = 0.3 - elif speed > 10: - throttle = 0.1 - brake = 0.0 - else: - throttle = 0.3 - brake = 0.0 - elif distance < 30: - if speed > 30: - throttle = 0.0 - brake = 0.1 - else: - throttle = 0.4 - brake = 0.0 - else: - # 没有近距离障碍物，正常行驶 - if speed < 20: - throttle = 0.6 - brake = 0.0 - elif speed < 40: - throttle = 0.4 - brake = 0.0 - else: - throttle = 0.2 - brake = 0.1 - else: - # 没有障碍物，正常行驶 - if speed < 20: - throttle = 0.6 - brake = 0.0 - elif speed < 40: - throttle = 0.4 - brake = 0.0 - else: - throttle = 0.2 - brake = 0.1 - - # 应用避障逻辑 - if obstacle_info: - throttle, brake, steer = self.apply_obstacle_avoidance( - throttle, brake, steer, vehicle, obstacle_info - ) - - return throttle, brake, steer - - -# CARLA初始化部分... -# 连接到本地CARLA服务器，端口2000 -client = carla.Client('localhost', 2000) -client.set_timeout(15.0) -world = client.load_world('Town01') - -# 获取并设置世界的运行参数 -settings = world.get_settings() -settings.synchronous_mode = True -settings.fixed_delta_seconds = 0.1 -world.apply_settings(settings) - -# 定义天气参数 -weather = carla.WeatherParameters( - cloudiness=30.0, - precipitation=0.0, - sun_altitude_angle=70.0 -) -world.set_weather(weather) - -# 获取地图和出生点 -map = world.get_map() -spawn_points = map.get_spawn_points() -if not spawn_points: - raise Exception("No spawn points available") - -# 选择更合适的出生点 -spawn_point = spawn_points[10] - -# 生成车辆 -blueprint_library = world.get_blueprint_library() -vehicle_bp = blueprint_library.find('vehicle.tesla.model3') -vehicle_bp.set_attribute('color', '255,0,0') -vehicle = world.spawn_actor(vehicle_bp, spawn_point) - -if not vehicle: - raise Exception("无法生成主车辆") - -vehicle.set_autopilot(False) -vehicle.set_simulate_physics(True) - -print(f"车辆生成在位置: {spawn_point.location}") - -# 改进的NPC车辆生成和设置 -print("生成NPC车辆...") -obstacle_count = 3 -npc_vehicles = [] # 存储NPC车辆以便后续管理 - -# 获取所有可用的车辆蓝图 -vehicle_blueprints = blueprint_library.filter('vehicle.*') - -# 选择远离主车辆的出生点（避免直接堵塞） -valid_spawn_points = [] -main_spawn_location = spawn_point.location - -for point in spawn_points: - distance = main_spawn_location.distance(point.location) - # 选择距离主车辆50米以上的出生点，避免直接碰撞 - if distance > 50.0: - valid_spawn_points.append(point) - if len(valid_spawn_points) >= obstacle_count: - break - -# 如果找不到足够远的点，使用所有点 -if len(valid_spawn_points) < obstacle_count: - print("警告：找不到足够远的出生点，使用所有可用点") - valid_spawn_points = spawn_points[:obstacle_count] - -# 生成NPC车辆 -for i in range(min(obstacle_count, len(valid_spawn_points))): - try: - # 随机选择车辆类型（排除特斯拉，使场景更丰富） - available_blueprints = [bp for bp in vehicle_blueprints if 'tesla' not in bp.id] - if not available_blueprints: - available_blueprints = vehicle_blueprints - - npc_bp = random.choice(available_blueprints) - - # 设置车辆颜色 - if npc_bp.has_attribute('color'): - colors = npc_bp.get_attribute('color').recommended_values - if colors: - npc_bp.set_attribute('color', random.choice(colors)) - - # 生成车辆 - npc_vehicle = world.try_spawn_actor(npc_bp, valid_spawn_points[i]) - - if npc_vehicle: - # 启用自动驾驶模式并设置速度限制 - npc_vehicle.set_autopilot(True) - - # 设置NPC车辆的速度限制（让它们能正常行驶） - traffic_manager = client.get_trafficmanager() - traffic_manager.set_global_distance_to_leading_vehicle(2.5) # 设置跟车距离 - traffic_manager.set_random_device_seed(12345) # 设置随机种子确保一致性 - - # 为每个NPC车辆设置不同的速度限制（避免所有车速度相同导致堵塞） - target_speed = random.uniform(30.0, 50.0) # 30-50 km/h - traffic_manager.vehicle_percentage_speed_difference(npc_vehicle, random.uniform(-10, 10)) - - # 设置NPC车辆的驾驶行为（更安全、更智能） - traffic_manager.auto_lane_change(npc_vehicle, True) # 允许自动变道 - traffic_manager.distance_to_leading_vehicle(npc_vehicle, 3.0) # 设置跟车距离 - traffic_manager.collision_detection(npc_vehicle, world, True) # 启用碰撞检测 - - npc_vehicles.append(npc_vehicle) - print(f"生成NPC车辆 {i + 1}: {npc_bp.id} 在位置 {valid_spawn_points[i].location}") - - # 短暂暂停，避免生成时的碰撞 - time.sleep(0.1) - - except Exception as e: - print(f"生成NPC车辆 {i + 1} 时出错: {e}") - -print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") - - try: # 尝试多种连接方式 self.client = carla.Client('localhost', 2000) @@ -706,13 +121,6 @@ def connect(self): print("3. 地图Town01可用") return False -def front_camera_callback(image): - global front_image - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - front_image = array[:, :, :3] - - def spawn_vehicle(self): """生成车辆 - 简化版本""" print("正在生成车辆...") @@ -727,7 +135,6 @@ def spawn_vehicle(self): print("未找到特斯拉蓝图，尝试其他车辆...") vehicle_bp = blueprint_library.filter('vehicle.*')[0] - vehicle_bp.set_attribute('color', '255,0,0') # 红色 # 获取出生点 @@ -766,15 +173,10 @@ def spawn_vehicle(self): print("车辆生成失败") return False -print("初始化车辆状态...") -vehicle.set_simulate_physics(True) - - except Exception as e: print(f"生成车辆时出错: {e}") return False - def setup_camera(self): """设置相机""" print("正在设置相机...") @@ -802,54 +204,9 @@ def setup_camera(self): # 设置回调函数 self.camera.listen(lambda image: self.camera_callback(image)) - -# 添加NPC车辆管理函数 -def check_and_reset_stuck_npcs(): - """检查并重置卡住的NPC车辆""" - for npc in npc_vehicles: - try: - npc_speed = math.sqrt( - npc.get_velocity().x ** 2 + npc.get_velocity().y ** 2 + npc.get_velocity().z ** 2) * 3.6 - # 如果NPC车辆速度过低（小于1km/h）且没有碰撞，可能是卡住了 - if npc_speed < 1.0: - print(f"检测到NPC车辆 {npc.id} 可能卡住，尝试重置...") - # 获取当前位置 - current_transform = npc.get_transform() - # 寻找最近的可用出生点 - closest_point = None - min_distance = float('inf') - for point in spawn_points: - distance = point.location.distance(current_transform.location) - if distance < min_distance and distance > 10.0: # 避免重置到太近的位置 - min_distance = distance - closest_point = point - - if closest_point: - npc.set_transform(closest_point) - npc.set_autopilot(True) - print(f"重置NPC车辆 {npc.id} 到新位置") - except: - pass - - -try: - print("自动驾驶系统启动 - 初始模式: 传统控制") - print("控制键: q-退出, m-切换控制模式, r-重置车辆, t-传统模式, n-神经网络模式") - print(f"当前有 {len(npc_vehicles)} 辆NPC车辆在运行") - - frame_count = 0 - stuck_count = 0 - last_position = vehicle.get_location() - success_count = 0 # 成功运行计数器 - collision_count = 0 # 碰撞计数器 - last_collision_time = 0 # 上次碰撞时间 - last_npc_check_time = 0 # 上次检查NPC的时间 - - print("相机设置成功") return True - except Exception as e: print(f"设置相机时出错: {e}") return False @@ -906,22 +263,6 @@ def run(self): # 生成一些NPC车辆 self.spawn_npc_vehicles(2) - # 定期检查NPC车辆状态（每100帧检查一次） - if frame_count - last_npc_check_time > 100: - check_and_reset_stuck_npcs() - last_npc_check_time = frame_count - - # 获取车辆状态 - vehicle_transform = vehicle.get_transform() - vehicle_location = vehicle.get_location() - vehicle_velocity = vehicle.get_velocity() - vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2) - - # 检测障碍物 - obstacle_info = obstacle_detector.get_obstacle_info() - obstacle_history.append(obstacle_info) - - print("\n系统准备就绪！") print("控制指令:") print(" q - 退出程序") @@ -1010,17 +351,11 @@ def spawn_npc_vehicles(self, count=2): npc_vehicles = [] - # 修复8: 改进的卡住检测 - current_position = vehicle_location - distance_moved = current_position.distance(last_position) - - for i in range(min(count, len(spawn_points))): # 跳过主车辆的出生点 if i == 0: continue - try: # 随机选择车辆类型 vehicle_bps = list(blueprint_library.filter('vehicle.*')) @@ -1046,9 +381,6 @@ def reset_vehicle(self): """重置车辆位置""" print("重置车辆...") - last_position = current_position - - spawn_points = self.world.get_map().get_spawn_points() if spawn_points: new_spawn_point = random.choice(spawn_points) @@ -1069,37 +401,6 @@ def cleanup(self): except: pass - # 检查是否有前方障碍物 - if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 10: - print("前方有障碍物，尝试倒车...") - # 倒车 - vehicle.apply_control(carla.VehicleControl( - throttle=0.0, steer=0.0, brake=0.0, reverse=True - )) - time.sleep(1.0) - else: - # 然后尝试不同方向的脱困 - recovery_steer = random.choice([-0.5, 0.5]) # 随机选择方向 - vehicle.apply_control(carla.VehicleControl( - throttle=0.8, steer=recovery_steer, brake=0.0, hand_brake=False - )) - time.sleep(1.0) - - stuck_count = 0 - success_count = 0 - - # 每成功运行100帧显示一次状态 - if success_count % 100 == 0: - print(f"已成功运行 {success_count} 帧") - - # 控制逻辑 - if NEURAL_NETWORK_MODE: - # 神经网络控制（传入障碍物信息） - nn_throttle, nn_brake, nn_steer = nn_controller.get_control( - front_image, vehicle_speed, steer_history, obstacle_info - ) - - if self.vehicle: try: self.vehicle.destroy() @@ -1109,7 +410,6 @@ def cleanup(self): # 等待销毁完成 time.sleep(1.0) - cv2.destroyAllWindows() print("清理完成") @@ -1124,112 +424,4 @@ def main(): if __name__ == "__main__": - main() - - else: - # 传统控制 - 更稳定 - throttle, brake, steer = traditional_controller.get_control(vehicle) - steer_history.append(steer) - - # 应用控制 - control = carla.VehicleControl( - throttle=throttle, - steer=steer, - brake=brake, - hand_brake=False, - reverse=False - ) - - vehicle.apply_control(control) - - # 显示和输入处理 - if third_image is not None: - display_image = third_image.copy() - - # 可视化障碍物检测结果 - display_image = obstacle_detector.visualize_obstacles(display_image, vehicle_transform) - - # 显示信息 - cv2.putText(display_image, f"Speed: {vehicle_speed * 3.6:.1f} km/h", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Mode: {'Neural' if NEURAL_NETWORK_MODE else 'Traditional'}", - (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Brake: {brake:.2f}", (10, 150), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - - # 显示障碍物信息 - if obstacle_info['has_obstacle']: - cv2.putText(display_image, f"Obstacle: {obstacle_info['distance']:.1f}m", (10, 180), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - else: - cv2.putText(display_image, "Obstacle: None", (10, 180), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - - # 卡住警告 - if stuck_count > 5: - cv2.putText(display_image, "STUCK DETECTED!", (10, 210), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - - # 安全区域标记 - cv2.rectangle(display_image, (240, 360), (400, 480), (0, 255, 0), 2) # 前方安全区域 - - cv2.imshow('自动驾驶系统 - 带障碍物检测', display_image) - - key = cv2.waitKey(1) & 0xFF - if key == ord('q'): - break - elif key == ord('m'): - NEURAL_NETWORK_MODE = not NEURAL_NETWORK_MODE - print(f"切换到{'神经网络' if NEURAL_NETWORK_MODE else '传统'}控制模式") - elif key == ord('t'): - NEURAL_NETWORK_MODE = False - print("切换到传统控制模式") - elif key == ord('n'): - NEURAL_NETWORK_MODE = True - print("切换到神经网络控制模式") - elif key == ord('r'): - # 重置车辆 - vehicle.set_transform(spawn_point) - throttle = 0.3 - steer = 0.0 - brake = 1.0 - stuck_count = 0 - success_count = 0 - collision_count = 0 - steer_history.clear() - obstacle_history.clear() - print("车辆已重置") - - time.sleep(0.01) - -except KeyboardInterrupt: - print("系统已停止") -except Exception as e: - print(f"系统错误: {e}") - import traceback - - traceback.print_exc() - -finally: - print("正在清理资源...") - third_camera.stop() - front_camera.stop() - - # 销毁actor - for actor in world.get_actors(): - if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'): - try: - actor.destroy() - except: - pass - - # 恢复设置 - settings.synchronous_mode = False - world.apply_settings(settings) - cv2.destroyAllWindows() - print("资源清理完成") - + main() \ No newline at end of file