diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index f9d8ddb25d..18f2d9df55 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -14,16 +14,10 @@ import os - # 修复1: 简化的神经网络架构（增加障碍物检测通道） class SimpleDrivingNetwork(nn.Module): """ 简化的驾驶网络 - 增加障碍物感知 -======= -# 修复1: 简化的神经网络架构 -class SimpleDrivingNetwork(nn.Module): - """ - """ def __init__(self): @@ -40,7 +34,6 @@ def __init__(self): nn.AdaptiveAvgPool2d((4, 4)) ) - # 状态信息维度: 速度 + 转向历史 + 障碍物信息 state_dim = 7 # 增加障碍物相关维度 @@ -50,14 +43,6 @@ def __init__(self): nn.ReLU(), nn.Dropout(0.1), # 添加dropout防止过拟合 nn.Linear(128, 64), - - # 状态信息维度: 速度 + 转向历史 - state_dim = 4 - - # 融合层 - self.fc_layers = nn.Sequential( - nn.Linear(32 * 4 * 4 + state_dim, 64), - nn.ReLU(), nn.Linear(64, 32), nn.ReLU(), @@ -80,7 +65,6 @@ def forward(self, image, state): return torch.cat([throttle_brake, steer], dim=1) - # 新增：障碍物检测器类 class ObstacleDetector: def __init__(self, world, vehicle, max_distance=50.0): @@ -219,11 +203,6 @@ def visualize_obstacles(self, image, vehicle_transform): # 修复2: 改进的神经网络控制器（整合障碍物检测） class ImprovedNeuralController: def __init__(self, obstacle_detector=None): - -# 修复2: 改进的神经网络控制器 -class ImprovedNeuralController: - def __init__(self): - self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') print(f"使用设备: {self.device}") @@ -234,7 +213,6 @@ def __init__(self): # 控制历史，用于平滑 self.control_history = deque(maxlen=5) - # 障碍物检测器 self.obstacle_detector = obstacle_detector @@ -243,13 +221,11 @@ def __init__(self): self.last_brake = 0.0 self.last_steer = 0.0 - # 避障参数 self.emergency_brake_distance = 5.0 # 紧急刹车距离 self.safe_following_distance = 8.0 # 安全跟车距离 self.obstacle_avoidance_steer = 0.0 - def preprocess_image(self, image): """修复图像预处理""" if image is None: @@ -266,21 +242,16 @@ def preprocess_image(self, image): print(f"图像预处理错误: {e}") return torch.zeros((1, 3, 120, 160), device=self.device) - def preprocess_state(self, speed, steer_history, obstacle_info): """修复状态预处理，加入障碍物信息""" has_obstacle = 1.0 if obstacle_info['has_obstacle'] else 0.0 normalized_distance = min(obstacle_info['distance'] / 50.0, 1.0) if obstacle_info['has_obstacle'] else 1.0 normalized_angle = obstacle_info['relative_angle'] / 60.0 if obstacle_info['has_obstacle'] else 0.0 - def preprocess_state(self, speed, steer_history): - """修复状态预处理""" - state_data = [ speed / 20.0, # 归一化速度 steer_history[-1] if steer_history else 0.0, # 最近转向 steer_history[-2] if len(steer_history) > 1 else 0.0, # 前一次转向 - np.mean(steer_history) if steer_history else 0.0, # 平均转向 has_obstacle, # 是否有障碍物 normalized_distance, # 归一化距离 @@ -288,8 +259,13 @@ def preprocess_state(self, speed, steer_history): ] return torch.tensor(state_data, device=self.device).unsqueeze(0) + + # 在 ImprovedNeuralController 类的 apply_obstacle_avoidance 方法中，修改以下部分： + def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): + """应用避障逻辑 - 优化版本""" + def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): - """应用避障逻辑""" + if not obstacle_info['has_obstacle']: return throttle, brake, steer @@ -299,11 +275,49 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed) # 紧急情况：前方有近距离障碍物 if distance < self.emergency_brake_distance: print(f"紧急刹车！距离障碍物: {distance:.1f}m") + + return 0.0, 1.0, 0.0 # 紧急情况下保持直行，只刹车 + return 0.0, 1.0, steer # 全力刹车 + # 中距离障碍物：减速并准备转向 elif distance < self.safe_following_distance: # 计算安全速度比例 + + safe_speed_ratio = (distance - 3.0) / (self.safe_following_distance - 3.0) + safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio)) + + # 如果当前速度过高，减速 + target_speed = 15.0 * safe_speed_ratio # 目标速度最大15km/h + current_speed_kmh = speed * 3.6 + + if current_speed_kmh > target_speed: + throttle = 0.0 + brake = 0.4 * ((current_speed_kmh - target_speed) / current_speed_kmh) + else: + throttle = 0.3 * safe_speed_ratio + brake = 0.0 + + # 如果障碍物在正前方，尝试轻微转向避开 + if abs(angle) < 15: # 正前方±15度内 + # 根据障碍物距离决定转向幅度 + avoid_factor = max(0, 1.0 - distance / self.safe_following_distance) + avoid_steer = 0.3 * avoid_factor if angle >= 0 else -0.3 * avoid_factor + # 平滑转向 - 保持更多原始转向 + steer = 0.8 * steer + 0.2 * avoid_steer + + # 远距离障碍物：轻微调整 + elif distance < 25.0: + # 轻微减速 + if speed > 8.0: + throttle *= 0.7 + + # 如果障碍物在正前方，轻微转向 + if abs(angle) < 20: + avoid_steer = 0.15 if angle >= 0 else -0.15 + steer = 0.9 * steer + 0.1 * avoid_steer + safe_speed_ratio = (distance - 2.0) / (self.safe_following_distance - 2.0) safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio)) @@ -330,28 +344,17 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed) avoid_steer = 0.2 if angle >= 0 else -0.2 steer = 0.8 * steer + 0.2 * avoid_steer + return throttle, brake, steer def get_control(self, image, speed, steer_history, obstacle_info): """修复控制生成逻辑，加入避障""" - - np.mean(steer_history) if steer_history else 0.0 # 平均转向 - ] - return torch.tensor(state_data, device=self.device).unsqueeze(0) - - def get_control(self, image, speed, steer_history): - """修复控制生成逻辑""" - try: with torch.no_grad(): # 预处理 img_tensor = self.preprocess_image(image) - state_tensor = self.preprocess_state(speed, steer_history, obstacle_info) - state_tensor = self.preprocess_state(speed, steer_history) - - # 神经网络推理 control_output = self.model(img_tensor, state_tensor) @@ -365,13 +368,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: @@ -380,30 +381,22 @@ def get_control(self, image, speed, steer_history): return 0.3, 0.0, 0.0 - # 修复5: 传统控制器作为备份（整合障碍物检测） class TraditionalController: """可靠的传统控制逻辑""" def __init__(self, world, obstacle_detector=None): - -# 修复5: 传统控制器作为备份 -class TraditionalController: - """可靠的传统控制逻辑""" - - def __init__(self, world): - self.world = world self.map = world.get_map() self.waypoint_distance = 10.0 self.last_waypoint = None - self.obstacle_detector = obstacle_detector self.emergency_brake_distance = 6.0 self.safe_following_distance = 10.0 + # 在 TraditionalController 类的 apply_obstacle_avoidance 方法中，修改以下部分： def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info): - """传统控制器的避障逻辑""" + """传统控制器的避障逻辑 - 优化版本""" if not obstacle_info['has_obstacle']: return throttle, brake, steer @@ -421,49 +414,45 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_inf # 减速跟随 elif distance < self.safe_following_distance: # 计算所需的安全距离（基于速度） - required_distance = max(5.0, vehicle_speed * 0.3) # 0.3秒车距 + required_distance = max(5.0, vehicle_speed * 0.4) # 增加到0.4秒车距 if distance < required_distance: # 距离太近，减速 + speed_ratio = distance / required_distance if vehicle_speed > 10: throttle = 0.0 - brake = 0.4 + brake = 0.6 * (1.0 - speed_ratio) else: - throttle = 0.1 + throttle = 0.2 * speed_ratio brake = 0.0 # 如果障碍物在正前方，尝试变道 - if abs(angle) < 15: - # 获取当前车道和相邻车道 + if abs(angle) < 20: # 放宽角度范围 location = vehicle.get_location() waypoint = self.map.get_waypoint(location) - # 尝试获取左侧车道 + # 检查相邻车道是否可用 left_lane = waypoint.get_left_lane() right_lane = waypoint.get_right_lane() + # 优先选择转向较小的方向 if left_lane and left_lane.lane_type == carla.LaneType.Driving: - steer = -0.3 # 向左变道 + # 检查左侧是否有足够空间 + steer = -0.25 elif right_lane and right_lane.lane_type == carla.LaneType.Driving: - steer = 0.3 # 向右变道 + steer = 0.25 else: - # 没有可用的相邻车道，轻微转向避开 - steer = 0.2 if angle >= 0 else -0.2 + # 没有可用的相邻车道，保持车道轻微避开 + steer = 0.15 if angle >= 0 else -0.15 return throttle, brake, steer def get_control(self, vehicle): """基于路点的传统控制，整合避障""" - - - def get_control(self, vehicle): - """基于路点的传统控制""" - # 获取车辆状态 transform = vehicle.get_transform() location = vehicle.get_location() velocity = vehicle.get_velocity() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) * 3.6 # km/h # 获取障碍物信息 @@ -471,9 +460,6 @@ def get_control(self, vehicle): if self.obstacle_detector: obstacle_info = self.obstacle_detector.get_obstacle_info() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) - - # 获取路点 waypoint = self.map.get_waypoint(location, project_to_road=True) next_waypoints = waypoint.next(self.waypoint_distance) @@ -501,7 +487,6 @@ def get_control(self, vehicle): angle = math.atan2(local_y, local_x) steer = np.clip(angle / math.radians(45), -1.0, 1.0) - # 速度控制（基于障碍物距离调整） throttle = 0.0 brake = 0.0 @@ -556,24 +541,10 @@ def get_control(self, vehicle): throttle, brake, steer, vehicle, obstacle_info ) - # 速度控制 - if speed < 5.0: # 18 km/h - throttle = 0.6 - brake = 0.0 - elif speed < 10.0: # 36 km/h - throttle = 0.3 - brake = 0.0 - else: - throttle = 0.1 - brake = 0.1 - - return throttle, brake, steer - # CARLA初始化部分... -# CARLA初始化部分保持不变... # 连接到本地CARLA服务器，端口2000 client = carla.Client('localhost', 2000) client.set_timeout(15.0) @@ -661,7 +632,6 @@ def third_camera_callback(image): third_image = array[:, :, :3] - def front_camera_callback(image): global front_image array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) @@ -694,33 +664,6 @@ def front_camera_callback(image): # 障碍物信息历史 obstacle_history = deque(maxlen=5) - -def front_camera_callback(image): - global front_image - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - front_image = array[:, :, :3] - - -third_camera.listen(third_camera_callback) -front_camera.listen(front_camera_callback) - -time.sleep(2.0) - -# 修复6: 初始化控制器 -nn_controller = ImprovedNeuralController() -traditional_controller = TraditionalController(world) - -# 控制变量 -throttle = 0.3 # 更保守的初始油门 -steer = 0.0 -brake = 0.0 -NEURAL_NETWORK_MODE = False # 默认使用传统控制，更稳定 - -# 转向历史，用于平滑 -steer_history = deque(maxlen=10) - - print("初始化车辆状态...") vehicle.set_simulate_physics(True) @@ -741,11 +684,9 @@ def front_camera_callback(image): stuck_count = 0 last_position = vehicle.get_location() success_count = 0 # 成功运行计数器 - collision_count = 0 # 碰撞计数器 last_collision_time = 0 # 上次碰撞时间 - # 主循环 while True: world.tick() @@ -757,7 +698,6 @@ def front_camera_callback(image): vehicle_velocity = vehicle.get_velocity() vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2) - # 检测障碍物 obstacle_info = obstacle_detector.get_obstacle_info() obstacle_history.append(obstacle_info) @@ -772,11 +712,6 @@ def front_camera_callback(image): f"模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}, " f"障碍物={'有' if obstacle_info['has_obstacle'] else '无'}") - - print( - f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}") - - # 修复8: 改进的卡住检测 current_position = vehicle_location distance_moved = current_position.distance(last_position) @@ -789,15 +724,6 @@ def front_camera_callback(image): stuck_count = 0 success_count += 1 # 成功运行一帧 - - last_position = current_position - - # 修复9: 更智能的卡住恢复 - if stuck_count > 15: # 1.5秒后认为卡住 - print("检测到车辆卡住，执行恢复程序...") - - - last_position = current_position # 修复9: 更智能的卡住恢复 @@ -810,7 +736,6 @@ def front_camera_callback(image): )) time.sleep(0.5) - # 检查是否有前方障碍物 if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 10: print("前方有障碍物，尝试倒车...") @@ -849,37 +774,6 @@ def front_camera_callback(image): # 记录转向历史 steer_history.append(steer) - - - # 然后尝试不同方向的脱困 - recovery_steer = random.choice([-0.5, 0.5]) # 随机选择方向 - vehicle.apply_control(carla.VehicleControl( - throttle=0.8, steer=recovery_steer, brake=0.0, hand_brake=False - )) - time.sleep(1.0) - - stuck_count = 0 - success_count = 0 - - # 每成功运行100帧显示一次状态 - if success_count % 100 == 0: - print(f"已成功运行 {success_count} 帧") - - # 控制逻辑 - if NEURAL_NETWORK_MODE: - # 神经网络控制 - nn_throttle, nn_brake, nn_steer = nn_controller.get_control( - front_image, vehicle_speed, steer_history - ) - - # 修复10: 更激进的控制平滑 - throttle = 0.3 * throttle + 0.7 * nn_throttle - brake = 0.3 * brake + 0.7 * nn_brake - steer = 0.2 * steer + 0.8 * nn_steer - - # 记录转向历史 - steer_history.append(steer) - else: # 传统控制 - 更稳定 throttle, brake, steer = traditional_controller.get_control(vehicle) @@ -900,7 +794,6 @@ def front_camera_callback(image): if third_image is not None: display_image = third_image.copy() - # 可视化障碍物检测结果 display_image = obstacle_detector.visualize_obstacles(display_image, vehicle_transform) @@ -910,7 +803,6 @@ def front_camera_callback(image): cv2.putText(display_image, f"Mode: {'Neural' if NEURAL_NETWORK_MODE else 'Traditional'}", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) @@ -935,20 +827,6 @@ def front_camera_callback(image): cv2.imshow('自动驾驶系统 - 带障碍物检测', display_image) - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(display_image, f"Brake: {brake:.2f}", (10, 150), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - - # 卡住警告 - if stuck_count > 5: - cv2.putText(display_image, "STUCK DETECTED!", (10, 180), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - - cv2.imshow('自动驾驶系统 - 修复版', display_image) - - key = cv2.waitKey(1) & 0xFF if key == ord('q'): break @@ -969,15 +847,11 @@ def front_camera_callback(image): brake = 1.0 stuck_count = 0 success_count = 0 - collision_count = 0 steer_history.clear() obstacle_history.clear() print("车辆已重置") - steer_history.clear() - - time.sleep(0.01) except KeyboardInterrupt: