diff --git a/src/unmannedcar_MPC/RADME.md b/src/unmannedcar_MPC/RADME.md
index 3f7d1cc7a7..ff4da0f760 100644
--- a/src/unmannedcar_MPC/RADME.md
+++ b/src/unmannedcar_MPC/RADME.md
@@ -1,14 +1,110 @@
-这是一个基于CARLA模拟器的主动式车道保持辅助系统项目，它就像一个虚拟的智能驾驶员，能够通过摄像头观察道路情况，自动控制方向盘使车辆稳定行驶在车道中心。
+readme.py - 项目说明文档
 
-整个系统的运作始于一个摄像头，它安装在虚拟车辆的前方，持续捕捉前方的道路图像。这些彩色图像被送入一个视觉处理程序，首先将图像从常见的RGB色彩空间转换到HLS空间，这是因为车道线在这种色彩模式下更容易被识别。程序会过滤出白色和黄色的像素，这些能就是车道线。接着，通过一种叫做透视变换的技术，把图像转换成鸟瞰图，仿佛我们从天空俯视道路一样，这样能更准确地判断车道线的弯曲程度和车辆的位置。然后，系统采用滑动窗口的方法，从图像底部开始，像爬楼梯一样一层层向上寻找属于车道线的像素点，再用一条光滑的曲线去拟合这些点，从而得到两条清晰的车道边界。最后，计算出车辆中心点与这两条边界中心线的横向距离，这个距离就是车辆偏离车道中心的误差。
+运行此文件查看完整项目说明
 
-环境安装：python -m venv venv
+README\_TEXT = """
 
-. venv/bin/activate
+一、项目概述
 
-pip install --upgrade pip
+这是一个基于Carla仿真平台和MPC控制器的自动驾驶系统，包含完整的驾驶评估
 
-pip install -r requirements.txt
+和导航功能。
 
-结果：生成一辆小车，按照预定轨迹提前改变路径，实现控制预测与转弯。
+二、主要功能
+
+1\. MPC路径跟踪控制
+
+2\. 智能速度控制（自动加速/刹车）
+
+3\. 驾驶评分系统（5个维度实时评分）
+
+4\. 碰撞预警系统
+
+5\. 航点导航系统
+
+6\. 完整的可视化界面
+
+三、新增功能说明
+
+1\. 航点导航系统
+
+   - 实时跟踪车辆航点进度
+
+   - 显示当前航点序号和距离
+
+   - 航点进度条可视化
+
+   - 屏幕顶部航点状态指示器
+
+2\. 驾驶评分系统
+
+   - 综合评分（0-100分，A/B/C/D等级）
+
+   - 五个评分维度：速度稳定性、转向平滑度、刹车使用、路径跟踪、安全性
+
+   - 评分趋势图表
+
+3\. 其他显示功能
+
+   - 速度显示（带颜色编码）
+
+   - 刹车状态显示
+
+   - 转向角度显示
+
+   - 油门/刹车输入显示
+
+   - 碰撞警告显示
+
+   - 帧率信息显示
+
+四、文件结构
+
+├── config.py          # 配置文件
+
+├── main.py            # 主程序（包含所有控制逻辑）
+
+├── drawer.py          # 绘图显示模块
+
+├── mpc.py             # MPC控制器
+
+├── readme.py          # 项目说明文档
+
+└── requirements.txt   # 依赖包列表
+
+五、运行方法
+
+1\. 启动Carla服务器：./CarlaUE4.sh
+
+2\. 运行项目：python main.py
+
+3\. 查看说明：python readme.py
+
+六、配置说明
+
+在config.py中可以修改：
+
+\- 服务器地址和地图
+
+\- 屏幕分辨率和帧率
+
+\- 相机视野角度
+
+七、依赖包
+
+\- carla
+
+\- pygame
+
+\- numpy
+
+\- scipy
+
+"""
+
+
+
+if \_\_name\_\_ == "\_\_main\_\_":
+
+    print(README\_TEXT)
 
diff --git a/src/unmannedcar_MPC/drawer.py b/src/unmannedcar_MPC/drawer.py
index e5c118a84e..1b8a890195 100644
--- a/src/unmannedcar_MPC/drawer.py
+++ b/src/unmannedcar_MPC/drawer.py
@@ -1,8 +1,11 @@
+#!/usr/bin/env python3
+
 import carla
 import config as Config
 import numpy as np
 import math
 import time
+import random
 
 
 class PyGameDrawer():
@@ -26,6 +29,11 @@ def __init__(self, main):
         # 通用信息字体
         self.info_font = self.pygame.freetype.SysFont('Arial', 16)
 
+        # 驾驶辅助线相关字体
+        self.assist_font = self.pygame.freetype.SysFont('Arial', 18)
+        self.assist_font_small = self.pygame.freetype.SysFont('Arial', 14)
+        self.radar_font = self.pygame.freetype.SysFont('Arial', 12)
+
         # 初始化时间
         self.start_time = time.time()
         self.frame_count = 0  # 帧计数器
@@ -797,6 +805,416 @@ def draw_waypoint_indicators(self, waypoints, current_index):
                 title_font.render_to(self.main.surface, (screen_width // 2 - 40, indicator_y - 40), nav_title,
                                      (150, 200, 255))
 
+    # 显示驾驶辅助线
+    def display_driving_assist_lines(self, vehicle_location, vehicle_transform, steer_angle, path=None):
+        """在屏幕上显示驾驶辅助线和预期路径"""
+        screen_width = Config.PYGAME_WIDTH
+        screen_height = Config.PYGAME_HEIGHT
+
+        # 设置显示位置（屏幕中央）
+        center_x = screen_width // 2
+        center_y = screen_height // 2
+
+        # 1. 绘制车辆中心参考线
+        # 绘制垂直中心线
+        self.pygame.draw.line(
+            self.main.surface,
+            (0, 255, 0, 100),  # 半透明绿色
+            (center_x, center_y - 50),
+            (center_x, center_y + 150),
+            1
+        )
+
+        # 绘制水平中心线
+        self.pygame.draw.line(
+            self.main.surface,
+            (0, 255, 0, 100),  # 半透明绿色
+            (center_x - 100, center_y),
+            (center_x + 100, center_y),
+            1
+        )
+
+        # 2. 绘制转向辅助线
+        # 根据转向角度计算辅助线的方向和长度
+        turn_radius = 200  # 基础转弯半径
+        if abs(steer_angle) > 0.01:  # 有转向时
+            # 计算转向曲率
+            curvature = steer_angle * 2.0
+            arc_radius = int(turn_radius / (abs(curvature) + 0.1))
+
+            # 计算弧线的起点、终点和中心
+            if steer_angle > 0:  # 右转
+                center_offset = arc_radius
+                arc_color = (255, 100, 0, 150)  # 橙色
+            else:  # 左转
+                center_offset = -arc_radius
+                arc_color = (255, 200, 0, 150)  # 黄色
+
+            # 绘制转向弧线
+            arc_center_x = center_x + center_offset
+            arc_center_y = center_y + 100
+
+            # 计算弧线角度范围
+            start_angle = 180 if steer_angle > 0 else 0
+            end_angle = 0 if steer_angle > 0 else 180
+
+            # 绘制弧线
+            self.pygame.draw.arc(
+                self.main.surface,
+                arc_color,
+                (arc_center_x - arc_radius, arc_center_y - arc_radius,
+                 arc_radius * 2, arc_radius * 2),
+                math.radians(start_angle),
+                math.radians(end_angle),
+                3
+            )
+
+            # 绘制转向方向指示箭头
+            arrow_length = 30
+            if steer_angle > 0:  # 右转箭头
+                arrow_points = [
+                    (center_x + 150, center_y + 50),
+                    (center_x + 150 - arrow_length, center_y + 50 - arrow_length // 2),
+                    (center_x + 150 - arrow_length, center_y + 50 + arrow_length // 2)
+                ]
+            else:  # 左转箭头
+                arrow_points = [
+                    (center_x - 150, center_y + 50),
+                    (center_x - 150 + arrow_length, center_y + 50 - arrow_length // 2),
+                    (center_x - 150 + arrow_length, center_y + 50 + arrow_length // 2)
+                ]
+
+            self.pygame.draw.polygon(
+                self.main.surface,
+                arc_color,
+                arrow_points
+            )
+
+            # 显示转向半径
+            radius_text = f"R: {arc_radius / 10:.1f}m"
+            radius_font = self.pygame.freetype.SysFont('Arial', 14)
+            radius_font.render_to(
+                self.main.surface,
+                (arc_center_x - 30, arc_center_y - arc_radius - 20),
+                radius_text,
+                arc_color
+            )
+
+        # 3. 绘制安全距离参考线
+        # 基于速度的安全距离（假设1秒反应距离）
+        speed_m_s = 0  # 如果没有速度信息，默认为0
+        if hasattr(self.main, 'ego'):
+            velocity = self.main.ego.get_velocity()
+            speed_m_s = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2)
+
+        safe_distance = speed_m_s * 1.5  # 1.5秒的安全距离
+
+        # 绘制安全距离线（红色）
+        safe_line_y = center_y + 100 - int(safe_distance * 5)  # 缩放因子
+        if safe_line_y > center_y - 100:  # 确保在屏幕内
+            self.pygame.draw.line(
+                self.main.surface,
+                (255, 0, 0, 100),  # 半透明红色
+                (center_x - 80, safe_line_y),
+                (center_x + 80, safe_line_y),
+                2
+            )
+
+            # 标注安全距离
+            safe_text = f"Safe: {safe_distance:.1f}m"
+            safe_font = self.pygame.freetype.SysFont('Arial', 12)
+            safe_font.render_to(
+                self.main.surface,
+                (center_x + 90, safe_line_y - 10),
+                safe_text,
+                (255, 0, 0)
+            )
+
+        # 4. 绘制车道保持辅助线
+        # 绘制车道边界线（蓝色虚线）
+        lane_width = 80  # 车道宽度
+        left_lane_x = center_x - lane_width
+        right_lane_x = center_x + lane_width
+
+        # 绘制左车道线（蓝色虚线）
+        for i in range(0, 200, 10):
+            if i % 20 < 10:  # 创建虚线效果
+                self.pygame.draw.line(
+                    self.main.surface,
+                    (100, 100, 255, 150),  # 半透明蓝色
+                    (left_lane_x, center_y + i),
+                    (left_lane_x, center_y + i + 5),
+                    2
+                )
+
+        # 绘制右车道线（蓝色虚线）
+        for i in range(0, 200, 10):
+            if i % 20 < 10:  # 创建虚线效果
+                self.pygame.draw.line(
+                    self.main.surface,
+                    (100, 100, 255, 150),  # 半透明蓝色
+                    (right_lane_x, center_y + i),
+                    (right_lane_x, center_y + i + 5),
+                    2
+                )
+
+        # 5. 如果提供了路径，绘制预期路径
+        if path and len(path) > 1:
+            # 转换路径点到屏幕坐标
+            path_points = []
+            for i, location in enumerate(path):
+                if i >= 10:  # 只显示前10个路径点
+                    break
+
+                # 将世界坐标转换为屏幕坐标
+                cam_loc = self.__w_locs_2_camera_locs([location])[0]
+
+                # 只添加在屏幕内的点
+                if (0 <= cam_loc[0] <= screen_width and
+                        0 <= cam_loc[1] <= screen_height):
+                    path_points.append(cam_loc)
+
+            # 绘制路径线（绿色虚线）
+            if len(path_points) > 1:
+                for i in range(len(path_points) - 1):
+                    # 创建渐变颜色：近处明亮，远处暗淡
+                    alpha = int(255 * (1.0 - i / len(path_points)))
+                    color = (0, 255, 0, alpha)
+
+                    # 绘制虚线
+                    if i % 2 == 0:
+                        self.pygame.draw.line(
+                            self.main.surface,
+                            color,
+                            path_points[i],
+                            path_points[i + 1],
+                            2
+                        )
+
+                # 在最后一个路径点上绘制标记
+                if path_points:
+                    last_point = path_points[-1]
+                    self.pygame.draw.circle(
+                        self.main.surface,
+                        (255, 255, 0),  # 黄色
+                        last_point,
+                        5
+                    )
+
+        # 6. 绘制车辆当前位置指示器
+        # 在屏幕中心绘制一个车辆图标
+        vehicle_color = (0, 200, 255)  # 青色
+
+        # 绘制车辆主体（矩形）
+        vehicle_rect = self.pygame.Rect(center_x - 15, center_y - 25, 30, 50)
+        self.pygame.draw.rect(
+            self.main.surface,
+            vehicle_color,
+            vehicle_rect,
+            2
+        )
+
+        # 绘制车辆前进方向箭头
+        arrow_length = 40
+        self.pygame.draw.line(
+            self.main.surface,
+            (255, 255, 0),  # 黄色箭头
+            (center_x, center_y),
+            (center_x, center_y - arrow_length),
+            3
+        )
+
+        # 绘制箭头头部
+        arrow_head = [
+            (center_x, center_y - arrow_length),
+            (center_x - 5, center_y - arrow_length + 10),
+            (center_x + 5, center_y - arrow_length + 10)
+        ]
+        self.pygame.draw.polygon(
+            self.main.surface,
+            (255, 255, 0),
+            arrow_head
+        )
+
+        # 7. 显示辅助系统状态
+        assist_font = self.pygame.freetype.SysFont('Arial', 16)
+        assist_text = "DRIVING ASSIST"
+        assist_font.render_to(
+            self.main.surface,
+            (center_x - 50, center_y - 80),
+            assist_text,
+            (200, 200, 255)
+        )
+
+        # 根据转向角度显示转向辅助状态
+        if abs(steer_angle) > 0.1:
+            turn_status = "TURNING"
+            turn_color = (255, 200, 0)
+        else:
+            turn_status = "STRAIGHT"
+            turn_color = (0, 255, 0)
+
+        status_font = self.pygame.freetype.SysFont('Arial', 14)
+        status_font.render_to(
+            self.main.surface,
+            (center_x - 40, center_y - 60),
+            turn_status,
+            turn_color
+        )
+
+    # 显示简单雷达图（检测周围环境）
+    def display_simple_radar(self, vehicle_location, obstacles=None):
+        """在屏幕右下角显示简单的雷达图，显示周围环境"""
+        screen_width = Config.PYGAME_WIDTH
+        screen_height = Config.PYGAME_HEIGHT
+
+        # 雷达图位置和大小
+        radar_x = screen_width - 180
+        radar_y = screen_height - 180
+        radar_radius = 70
+
+        # 绘制雷达图背景（圆形）
+        self.pygame.draw.circle(
+            self.main.surface,
+            (20, 20, 40),  # 深蓝色背景
+            (radar_x, radar_y),
+            radar_radius
+        )
+
+        # 绘制雷达图网格
+        # 同心圆
+        for i in range(1, 4):
+            radius = int(radar_radius * i / 4)
+            self.pygame.draw.circle(
+                self.main.surface,
+                (50, 50, 80),  # 网格颜色
+                (radar_x, radar_y),
+                radius,
+                1
+            )
+
+        # 十字线
+        self.pygame.draw.line(
+            self.main.surface,
+            (50, 50, 80),
+            (radar_x - radar_radius, radar_y),
+            (radar_x + radar_radius, radar_y),
+            1
+        )
+        self.pygame.draw.line(
+            self.main.surface,
+            (50, 50, 80),
+            (radar_x, radar_y - radar_radius),
+            (radar_x, radar_y + radar_radius),
+            1
+        )
+
+        # 绘制方向指示
+        # 前方（上）
+        self.pygame.draw.line(
+            self.main.surface,
+            (100, 100, 200),
+            (radar_x, radar_y - radar_radius + 5),
+            (radar_x, radar_y - radar_radius + 15),
+            2
+        )
+
+        # 绘制车辆位置（中心点）
+        self.pygame.draw.circle(
+            self.main.surface,
+            (0, 255, 0),  # 绿色表示车辆
+            (radar_x, radar_y),
+            4
+        )
+
+        # 如果提供了障碍物信息，绘制障碍物
+        if obstacles:
+            for obstacle in obstacles:
+                # 获取障碍物信息
+                if isinstance(obstacle, dict):
+                    # 从main.py传递的障碍物字典
+                    distance = obstacle.get('distance', 0)
+                    angle = obstacle.get('angle', 0)
+
+                    # 限制距离在雷达范围内
+                    normalized_distance = min(distance / 50.0, 1.0)  # 假设最大检测距离50米
+
+                    # 计算障碍物在雷达图上的位置
+                    obstacle_radius = int(radar_radius * normalized_distance)
+                    obstacle_angle = math.radians(angle)
+
+                    obstacle_x = radar_x + int(obstacle_radius * math.sin(obstacle_angle))
+                    obstacle_y = radar_y - int(obstacle_radius * math.cos(obstacle_angle))
+
+                    # 根据障碍物类型设置颜色
+                    obstacle_type = obstacle.get('type', 'unknown')
+                    if obstacle_type == 'vehicle':
+                        color = (255, 100, 100)  # 红色表示车辆
+                    elif obstacle_type == 'static':
+                        color = (200, 200, 100)  # 黄色表示静态障碍物
+                    else:
+                        color = (150, 150, 150)  # 灰色表示未知障碍物
+
+                    # 绘制障碍物点
+                    self.pygame.draw.circle(
+                        self.main.surface,
+                        color,
+                        (obstacle_x, obstacle_y),
+                        4
+                    )
+
+                    # 如果障碍物很近，添加警告效果
+                    if distance < 10.0:
+                        pulse_radius = 4 + 2 * math.sin(self.frame_count * 0.2)
+                        self.pygame.draw.circle(
+                            self.main.surface,
+                            (255, 0, 0, 100),
+                            (obstacle_x, obstacle_y),
+                            int(pulse_radius),
+                            1
+                        )
+
+        # 绘制雷达扫描线（旋转效果）
+        scan_angle = (self.frame_count * 2) % 360  # 根据帧数旋转
+        scan_end_x = radar_x + int(radar_radius * math.sin(math.radians(scan_angle)))
+        scan_end_y = radar_y - int(radar_radius * math.cos(math.radians(scan_angle)))  # 注意：屏幕y轴向下为正
+
+        # 绘制扫描线
+        self.pygame.draw.line(
+            self.main.surface,
+            (0, 255, 0, 100),  # 半透明绿色
+            (radar_x, radar_y),
+            (scan_end_x, scan_end_y),
+            1
+        )
+
+        # 绘制雷达图标题
+        radar_font = self.pygame.freetype.SysFont('Arial', 14)
+        radar_font.render_to(
+            self.main.surface,
+            (radar_x - 40, radar_y - radar_radius - 20),
+            "RADAR",
+            (100, 200, 255)
+        )
+
+        # 添加图例
+        legend_font = self.pygame.freetype.SysFont('Arial', 10)
+        legend_font.render_to(
+            self.main.surface,
+            (radar_x - radar_radius, radar_y + radar_radius + 10),
+            "● Vehicle   ● Static",
+            (150, 150, 150)
+        )
+
+        # 显示检测范围
+        range_text = "Range: 50m"
+        legend_font.render_to(
+            self.main.surface,
+            (radar_x - radar_radius, radar_y + radar_radius + 25),
+            range_text,
+            (100, 100, 200)
+        )
+
     @staticmethod
     def get_location_bbox(location, camera):
         bb_cords = np.array([[0, 0, 0, 1]])
diff --git a/src/unmannedcar_MPC/main.py b/src/unmannedcar_MPC/main.py
index 02b89db5eb..d0c3a0c9b2 100644
--- a/src/unmannedcar_MPC/main.py
+++ b/src/unmannedcar_MPC/main.py
@@ -59,6 +59,15 @@ def __init__(self):
         self.waypoint_reached_count = 0  # 已到达航点计数
         self.waypoint_progress = 0.0  # 航点进度（0-1）
 
+        # 障碍物检测系统
+        self.obstacles = []  # 障碍物列表
+        self.obstacle_detection_range = 50.0  # 障碍物检测范围（米）
+
+        # 驾驶辅助系统状态
+        self.lane_assist_active = True  # 车道保持辅助状态
+        self.adaptive_cruise_active = True  # 自适应巡航状态
+        self.collision_avoidance_active = True  # 碰撞避免状态
+
         # start game loop
         self.game.game_loop(self.world, self.on_tick)
 
@@ -80,6 +89,9 @@ def on_tick(self):
         # 更新航点信息
         self.update_waypoint_navigation(path)
 
+        # 检测障碍物
+        self.detect_obstacles()
+
         # get forward speed
         velocity = self.ego.get_velocity()
         speed_m_s = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2)
@@ -128,6 +140,15 @@ def on_tick(self):
             else:
                 speed_error = current_speed_kmh - self.target_speed_kmh
 
+            # 根据前方障碍物调整速度
+            if self.obstacles:
+                # 如果有障碍物，进一步降低目标速度
+                min_obstacle_distance = min(self.obstacles, key=lambda x: x['distance'])['distance']
+                if min_obstacle_distance < 20.0:  # 20米内有障碍物
+                    safety_factor = min_obstacle_distance / 20.0  # 距离越近，因子越小
+                    self.target_speed_kmh = min(self.target_speed_kmh, 30.0 * safety_factor)
+                    speed_error = current_speed_kmh - self.target_speed_kmh
+
             if speed_error > 5:  # 超过目标速度5km/h时强力刹车
                 control.throttle = 0.0
                 self.brake_force = min(self.brake_force + 0.1, 1.0)  # 快速增加刹车力度
@@ -205,14 +226,32 @@ def on_tick(self):
             self.waypoint_progress
         )
 
+        # 显示驾驶辅助线和雷达图
+        self.drawer.display_driving_assist_lines(
+            self.ego.get_location(),
+            self.ego.get_transform(),
+            self.steer_angle,
+            path  # 传入路径用于绘制预期路径
+        )
+
+        # 显示雷达图（传入检测到的障碍物）
+        self.drawer.display_simple_radar(self.ego.get_location(), self.obstacles)
+
     def check_collision_warning(self, path, speed_kmh, steer_angle):
         """检测可能的碰撞风险"""
         # 基于转向角度和速度的简单碰撞检测
         speed_factor = speed_kmh / 100.0  # 速度越快，风险越高
         steer_factor = abs(steer_angle)  # 转向角度越大，风险越高
 
+        # 考虑障碍物距离
+        obstacle_factor = 0.0
+        if self.obstacles:
+            min_obstacle_distance = min(self.obstacles, key=lambda x: x['distance'])['distance']
+            if min_obstacle_distance < 10.0:
+                obstacle_factor = 1.0 - (min_obstacle_distance / 10.0)
+
         # 计算碰撞风险
-        collision_risk = speed_factor * (1.0 + steer_factor * 3)
+        collision_risk = speed_factor * (1.0 + steer_factor * 3) + obstacle_factor * 0.5
 
         # 检查是否超过阈值
         warning_threshold = 0.5
@@ -228,6 +267,8 @@ def check_collision_warning(self, path, speed_kmh, steer_angle):
         if self.collision_warning != was_warning:
             if self.collision_warning:
                 print(f"碰撞警告激活！速度: {speed_kmh:.1f} km/h, 转向: {steer_angle:.3f}, 风险: {collision_risk:.2f}")
+                if self.obstacles:
+                    print(f"  最近障碍物距离: {min(self.obstacles, key=lambda x: x['distance'])['distance']:.1f}米")
             else:
                 print("碰撞警告解除")
 
@@ -265,6 +306,8 @@ def calculate_driving_score(self, current_speed, steer_angle, brake_force, path)
             safety_penalty += 30  # 碰撞警告扣分
         if brake_force > 0.5:
             safety_penalty += 20  # 紧急刹车扣分
+        if self.obstacles and min(self.obstacles, key=lambda x: x['distance'])['distance'] < 5.0:
+            safety_penalty += 30  # 距离障碍物太近扣分
         safety = max(0, 100 - safety_penalty)
 
         # 保存各项评分因子
@@ -353,6 +396,112 @@ def update_waypoint_navigation(self, path):
         if len(self.waypoint_positions) > 0:
             self.waypoint_progress = self.current_waypoint_index / len(self.waypoint_positions)
 
+    def detect_obstacles(self):
+        """检测车辆周围的障碍物"""
+        # 清空障碍物列表
+        self.obstacles = []
+
+        # 获取车辆位置和朝向
+        vehicle_location = self.ego.get_location()
+        vehicle_transform = self.ego.get_transform()
+        vehicle_rotation = vehicle_transform.rotation
+
+        # 获取车辆前方的航点作为参考方向
+        wp = self.map.get_waypoint(vehicle_location)
+
+        # 检测周围的车辆
+        vehicle_list = self.world.get_actors().filter('vehicle.*')
+
+        for vehicle in vehicle_list:
+            # 排除自车
+            if vehicle.id == self.ego.id:
+                continue
+
+            # 计算车辆距离
+            other_location = vehicle.get_location()
+            distance = vehicle_location.distance(other_location)
+
+            # 只检测一定范围内的车辆
+            if distance < self.obstacle_detection_range:
+                # 计算相对方向
+                dx = other_location.x - vehicle_location.x
+                dy = other_location.y - vehicle_location.y
+
+                # 计算相对于车辆前进方向的角度
+                # 这里简化处理，使用航点方向作为参考
+                forward_vector = vehicle_transform.get_forward_vector()
+                relative_vector = carla.Vector3D(dx, dy, 0)
+
+                # 计算点积和叉积
+                dot_product = forward_vector.x * relative_vector.x + forward_vector.y * relative_vector.y
+                cross_product = forward_vector.x * relative_vector.y - forward_vector.y * relative_vector.x
+
+                # 计算角度（弧度）
+                angle = math.atan2(cross_product, dot_product)
+
+                # 转换为度
+                angle_deg = math.degrees(angle)
+
+                # 只考虑前方±60度范围内的障碍物
+                if abs(angle_deg) < 60:
+                    # 计算相对速度（简化）
+                    other_velocity = vehicle.get_velocity()
+                    relative_speed = math.sqrt(
+                        (other_velocity.x - self.ego.get_velocity().x) ** 2 +
+                        (other_velocity.y - self.ego.get_velocity().y) ** 2
+                    )
+
+                    # 添加到障碍物列表
+                    self.obstacles.append({
+                        'location': other_location,
+                        'distance': distance,
+                        'angle': angle_deg,
+                        'relative_speed': relative_speed,
+                        'type': 'vehicle'
+                    })
+
+        # 检测静态障碍物（简化：使用地图中的建筑）
+        # 这里简化处理，实际上应该使用传感器数据
+        if self.frame_count % 30 == 0:  # 每30帧检测一次静态障碍物
+            # 随机添加一些模拟的静态障碍物用于演示
+            import random
+            for i in range(random.randint(0, 3)):
+                # 在车辆前方随机位置添加模拟障碍物
+                angle = random.uniform(-45, 45)
+                distance = random.uniform(10, 40)
+
+                # 计算障碍物位置
+                angle_rad = math.radians(angle)
+                obstacle_x = vehicle_location.x + distance * math.cos(angle_rad + math.radians(vehicle_rotation.yaw))
+                obstacle_y = vehicle_location.y + distance * math.sin(angle_rad + math.radians(vehicle_rotation.yaw))
+                obstacle_z = vehicle_location.z
+
+                obstacle_location = carla.Location(x=obstacle_x, y=obstacle_y, z=obstacle_z)
+
+                # 添加到障碍物列表
+                self.obstacles.append({
+                    'location': obstacle_location,
+                    'distance': distance,
+                    'angle': angle,
+                    'relative_speed': 0.0,
+                    'type': 'static'
+                })
+
+        # 按距离排序
+        self.obstacles.sort(key=lambda x: x['distance'])
+
+        # 保留最近的5个障碍物
+        if len(self.obstacles) > 5:
+            self.obstacles = self.obstacles[:5]
+
+        # 每100帧输出一次障碍物信息
+        if self.frame_count % 100 == 0 and self.obstacles:
+            print(f"\n=== 障碍物检测报告 (帧 {self.frame_count}) ===")
+            for i, obstacle in enumerate(self.obstacles):
+                print(f"障碍物 {i + 1}: 距离={obstacle['distance']:.1f}米, 角度={obstacle['angle']:.1f}°, "
+                      f"类型={obstacle['type']}, 相对速度={obstacle['relative_speed']:.1f} m/s")
+            print("=" * 40)
+
 
 if __name__ == '__main__':
     Main()
\ No newline at end of file