From 5fd1d8dd337e91583d7fd9bce150d01cded24230 Mon Sep 17 00:00:00 2001 From: chen Date: Tue, 4 Nov 2025 08:31:43 +0800 Subject: [PATCH 01/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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)