From dc9ce57da3c9d34c34fc40306498265c5d762b0e Mon Sep 17 00:00:00 2001 From: yuboyyy <2814479226@qq.com> Date: Thu, 18 Dec 2025 15:55:50 +0800 Subject: [PATCH 1/7] 1 --- src/Smart_car/dianliangyuxianshi.py | 95 +++++++++++++++++++++++++++++ 1 file changed, 95 insertions(+) create mode 100644 src/Smart_car/dianliangyuxianshi.py diff --git a/src/Smart_car/dianliangyuxianshi.py b/src/Smart_car/dianliangyuxianshi.py new file mode 100644 index 0000000000..6475107b10 --- /dev/null +++ b/src/Smart_car/dianliangyuxianshi.py @@ -0,0 +1,95 @@ +import time +import random # 仅用于模拟硬件数据,实际场景删除 + + +class UnmannedVehicleBattery: + """无人车电池电量管理类""" + + def __init__(self): + # 电池参数配置(根据实际电池规格调整) + self.max_voltage = 12.6 # 满电电压(12V锂电池为例) + self.min_voltage = 10.0 # 欠压保护电压 + self.current_voltage = 0.0 # 当前电压 + self.battery_percent = 0.0 # 剩余电量百分比 + + def read_battery_voltage(self): + """ + 读取电池电压(模拟硬件采集) + 实际场景:替换为ADC读取/串口接收BMS数据/I2C通信等 + """ + # 模拟电压波动(范围:10.0~12.6V) + self.current_voltage = round(random.uniform(10.0, 12.6), 2) + # 实际硬件示例(以树莓派ADC为例): + # import adafruit_ads1x15.ads1115 as ADS + # from adafruit_ads1x15.analog_in import AnalogIn + # i2c = board.I2C() + # ads = ADS.ADS1115(i2c) + # chan = AnalogIn(ads, ADS.P0) + # self.current_voltage = chan.voltage * voltage_divider_ratio # 电压分压比 + + def calculate_battery_percent(self): + """计算剩余电量百分比""" + if self.current_voltage >= self.max_voltage: + self.battery_percent = 100.0 + elif self.current_voltage <= self.min_voltage: + self.battery_percent = 0.0 + else: + # 线性计算(实际可根据电池放电曲线优化) + self.battery_percent = round( + (self.current_voltage - self.min_voltage) / + (self.max_voltage - self.min_voltage) * 100, + 1 + ) + + def get_battery_status(self): + """判断电量状态""" + if self.battery_percent >= 95: + return "满电", "🟢" + elif 20 <= self.battery_percent < 95: + return "正常", "🟢" + elif 5 <= self.battery_percent < 20: + return "低电量", "🟡" + else: + return "紧急(请充电)", "🔴" + + def display_battery_info(self): + """可视化显示电量信息""" + # 清空控制台(可选) + # os.system('cls' if os.name == 'nt' else 'clear') + + # 电量条可视化 + bar_length = 20 + filled_length = int(bar_length * self.battery_percent // 100) + battery_bar = "█" * filled_length + "-" * (bar_length - filled_length) + + # 获取状态 + status, color = self.get_battery_status() + + # 打印信息 + print(f"\n=== 无人车电池状态 ===") + print(f"当前电压: {self.current_voltage}V") + print(f"剩余电量: |{battery_bar}| {self.battery_percent}%") + print(f"状态: {color} {status}") + + # 低电量告警 + if self.battery_percent < 5: + print("⚠️ 电量过低,立即停止作业并充电!") + + +def main(): + """主循环""" + battery = UnmannedVehicleBattery() + print("无人车电量监控系统启动...") + + try: + while True: + battery.read_battery_voltage() # 读取电压 + battery.calculate_battery_percent() # 计算电量 + battery.display_battery_info() # 显示信息 + time.sleep(1) # 1秒刷新一次 + except KeyboardInterrupt: + print("\n监控系统已退出") + + +if __name__ == "__main__": + main() \ No newline at end of file From dc3521a3d5958b0ce4b19be565ce899dd6de4365 Mon Sep 17 00:00:00 2001 From: yuboyyy <2814479226@qq.com> Date: Thu, 18 Dec 2025 15:56:17 +0800 Subject: [PATCH 2/7] 1 --- src/Smart_car/dianliangyuxianshi.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Smart_car/dianliangyuxianshi.py b/src/Smart_car/dianliangyuxianshi.py index 6475107b10..40b6b6f8f5 100644 --- a/src/Smart_car/dianliangyuxianshi.py +++ b/src/Smart_car/dianliangyuxianshi.py @@ -73,7 +73,7 @@ def display_battery_info(self): # 低电量告警 if self.battery_percent < 5: - print("⚠️ 电量过低,立即停止作业并充电!") + print("⚠️ 电量过低,立即停止作业并充电") def main(): From 28b2a2eee835d868c8119c6b82eeaedf92d2c0c4 Mon Sep 17 00:00:00 2001 From: yuboyyy <2814479226@qq.com> Date: Thu, 18 Dec 2025 19:46:33 +0800 Subject: [PATCH 3/7] 1 --- ...ngyuxianshi.py => autonomous vehicle battery level display.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/Smart_car/{dianliangyuxianshi.py => autonomous vehicle battery level display.py} (100%) diff --git a/src/Smart_car/dianliangyuxianshi.py b/src/Smart_car/autonomous vehicle battery level display.py similarity index 100% rename from src/Smart_car/dianliangyuxianshi.py rename to src/Smart_car/autonomous vehicle battery level display.py From 880ca4738b73e5facc54d21b7934701e5c724008 Mon Sep 17 00:00:00 2001 From: yuboyyy <2814479226@qq.com> Date: Thu, 18 Dec 2025 20:25:49 +0800 Subject: [PATCH 4/7] 1 --- ...vel display.py => autonomous_vehicle_battery_level_display.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/Smart_car/{autonomous vehicle battery level display.py => autonomous_vehicle_battery_level_display.py} (100%) diff --git a/src/Smart_car/autonomous vehicle battery level display.py b/src/Smart_car/autonomous_vehicle_battery_level_display.py similarity index 100% rename from src/Smart_car/autonomous vehicle battery level display.py rename to src/Smart_car/autonomous_vehicle_battery_level_display.py From 1636dad0a68b8f4302340b584db8e75f9f8d8096 Mon Sep 17 00:00:00 2001 From: yuboyyy <2814479226@qq.com> Date: Thu, 18 Dec 2025 21:29:20 +0800 Subject: [PATCH 5/7] 1 --- src/Smart_car/Speed_warning.py | 102 +++++++++++++++++++++++++++++++++ 1 file changed, 102 insertions(+) create mode 100644 src/Smart_car/Speed_warning.py diff --git a/src/Smart_car/Speed_warning.py b/src/Smart_car/Speed_warning.py new file mode 100644 index 0000000000..bb69dd8071 --- /dev/null +++ b/src/Smart_car/Speed_warning.py @@ -0,0 +1,102 @@ +import time +import serial # 需安装:pip install pyserial +import random # 仿真速度数据(真实场景替换为传感器读取) + +# ------------------- 配置参数 ------------------- +# 限速阈值(km/h) +SPEED_LIMIT = 20 +# 预警等级配置 +WARNING_LEVELS = { + "low": (SPEED_LIMIT, SPEED_LIMIT + 5), # 轻度超速:20-25km/h + "medium": (SPEED_LIMIT + 5, SPEED_LIMIT + 10), # 中度超速:25-30km/h + "high": (SPEED_LIMIT + 10, float('inf')) # 重度超速:>30km/h +} +# 串口配置(对接车载显示屏/报警器,真实场景启用) +SERIAL_PORT = "COM3" # Windows: COM3 / Linux: /dev/ttyUSB0 +BAUD_RATE = 9600 +ser = None + + +# ------------------- 初始化函数 ------------------- +def init_serial(): + """初始化串口(用于向硬件发送预警指令)""" + global ser + try: + ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1) + print(f"Serial port {SERIAL_PORT} initialized successfully") + except Exception as e: + print(f"Serial port init failed: {e}") + ser = None + + +# ------------------- 速度读取函数 ------------------- +def read_vehicle_speed(): + """ + 读取车辆速度(真实场景替换为传感器/总线数据) + 返回:当前速度(km/h) + """ + # 仿真:随机生成10-35km/h的速度(真实场景删除此段) + simulated_speed = random.uniform(10, 35) + return round(simulated_speed, 1) + + # 真实场景示例:从串口读取速度传感器数据 + # if ser and ser.in_waiting > 0: + # speed_data = ser.readline().decode('utf-8').strip() + # return float(speed_data) if speed_data else 0.0 + # return 0.0 + + +# ------------------- 超速预警核心函数 ------------------- +def speed_warning(current_speed): + """ + 超速预警判断与输出 + :param current_speed: 当前速度(km/h) + :return: 预警等级(None/low/medium/high)、预警信息(英文) + """ + if current_speed < SPEED_LIMIT: + return None, f"Current speed: {current_speed} km/h - Normal" + + # 判断预警等级 + warning_level = None + warning_msg = "" + if WARNING_LEVELS["low"][0] <= current_speed < WARNING_LEVELS["low"][1]: + warning_level = "low" + warning_msg = f"Speed Warning: {current_speed} km/h (Over limit by {current_speed - SPEED_LIMIT} km/h) - Slow down!" + elif WARNING_LEVELS["medium"][0] <= current_speed < WARNING_LEVELS["medium"][1]: + warning_level = "medium" + warning_msg = f"Over Speed Warning: {current_speed} km/h - Reduce speed immediately!" + elif current_speed >= WARNING_LEVELS["high"][0]: + warning_level = "high" + warning_msg = f"CRITICAL Over Speed Warning: {current_speed} km/h - STOP VEHICLE!" + + # 输出预警(控制台 + 串口/硬件) + print(f"[{time.strftime('%Y-%m-%d %H:%M:%S')}] {warning_msg}") + if ser: + ser.write(f"{warning_level}:{warning_msg}\n".encode('utf-8')) + + return warning_level, warning_msg + + +# ------------------- 主循环 ------------------- +def main(): + # 初始化串口(可选) + # init_serial() + + print(f"Autonomous Vehicle Speed Warning System Started | Speed Limit: {SPEED_LIMIT} km/h") + try: + while True: + # 读取当前速度 + current_speed = read_vehicle_speed() + # 触发超速预警 + speed_warning(current_speed) + # 1秒刷新一次(可根据传感器频率调整) + time.sleep(1) + except KeyboardInterrupt: + print("\nSystem stopped by user") + finally: + if ser: + ser.close() + + +if __name__ == "__main__": + main() \ No newline at end of file From 202a616c4fbff92dd333044fa05879960c0eec40 Mon Sep 17 00:00:00 2001 From: yuboyyy <2814479226@qq.com> Date: Fri, 19 Dec 2025 09:28:06 +0800 Subject: [PATCH 6/7] 1 --- src/Smart_car/obstacle_avoidance.py | 193 ++++++++++++++++++++++++++++ 1 file changed, 193 insertions(+) create mode 100644 src/Smart_car/obstacle_avoidance.py diff --git a/src/Smart_car/obstacle_avoidance.py b/src/Smart_car/obstacle_avoidance.py new file mode 100644 index 0000000000..1787cd840f --- /dev/null +++ b/src/Smart_car/obstacle_avoidance.py @@ -0,0 +1,193 @@ +import time +import random +import matplotlib.pyplot as plt +import matplotlib.animation as animation +from matplotlib.patches import Rectangle, Circle + +# 无人车状态常量 +SAFE_DISTANCE = 50 # 安全距离(厘米) +WARNING_DISTANCE = 30 # 警告距离(厘米) +DANGER_DISTANCE = 15 # 危险距离(厘米) +NORMAL_SPEED = 20 # 正常速度(km/h) +LOW_SPEED = 5 # 低速(km/h) +STOP_SPEED = 0 # 停车速度 + +# 可视化全局变量 +fig, (ax_scene, ax_plot) = plt.subplots(1, 2, figsize=(12, 5)) +distance_history = [] # 前方距离历史 +speed_history = [] # 车速历史 +time_history = [] # 时间轴 +car_pos = [5, 2.5] # 无人车初始位置(x,y) +obstacle_pos = [0, 0] # 障碍物位置 +car_direction = "forward" + + +class UnmannedCar: + def __init__(self): + self.speed = 0 + self.direction = "forward" + + def simulate_sensor(self, direction): + """模拟传感器测距(加入轻微固定偏移,让障碍物位置可预测)""" + if direction == "front": + # 模拟障碍物距离缓慢变化(更贴近实际) + base_dist = random.randint(10, 60) if len(distance_history) < 5 else distance_history[-1] + random.randint( + -5, 5) + distance = max(0, min(100, base_dist)) # 限制0-100cm + else: + distance = random.randint(20, 80) # 左右侧距离 + + # 更新障碍物位置(用于可视化) + global obstacle_pos + obstacle_pos = [car_pos[0] + distance / 10, car_pos[1]] # 缩放适配画布 + print(f"[{direction}] 传感器检测距离:{distance} cm") + return distance + + def adjust_speed(self, new_speed): + self.speed = new_speed + print(f"车速调整为:{self.speed} km/h") + + def adjust_direction(self, new_dir): + global car_direction + self.direction = new_dir + car_direction = new_dir + print(f"行驶方向调整为:{self.direction}") + + def collision_avoidance(self): + """核心避撞逻辑""" + front_dist = self.simulate_sensor("front") + + # 记录数据用于绘图 + distance_history.append(front_dist) + speed_history.append(self.speed) + time_history.append(len(time_history)) + + if front_dist > SAFE_DISTANCE: + self.adjust_speed(NORMAL_SPEED) + self.adjust_direction("forward") + + elif WARNING_DISTANCE < front_dist <= SAFE_DISTANCE: + print("⚠️ 前方接近障碍物,减速!") + self.adjust_speed(LOW_SPEED) + self.adjust_direction("forward") + + elif front_dist <= DANGER_DISTANCE: + print("🚨 前方紧急危险!立即停车!") + self.adjust_speed(STOP_SPEED) + self.adjust_direction("stop") + + left_dist = self.simulate_sensor("left") + right_dist = self.simulate_sensor("right") + + if left_dist > SAFE_DISTANCE: + print("🔄 左侧有空间,转向左侧避障") + self.adjust_direction("left") + self.adjust_speed(LOW_SPEED) + elif right_dist > SAFE_DISTANCE: + print("🔄 右侧有空间,转向右侧避障") + self.adjust_direction("right") + self.adjust_speed(LOW_SPEED) + else: + print("❌ 左右侧均有障碍物,无法避障,保持停车!") + + +# 初始化可视化场景 +def init_visualization(): + # 左侧:场景图(无人车+障碍物) + ax_scene.set_xlim(0, 15) + ax_scene.set_ylim(0, 5) + ax_scene.set_title("无人车避障场景模拟") + ax_scene.set_xlabel("位置 (cm/10)") + ax_scene.set_ylabel("位置 (cm/10)") + ax_scene.grid(True) + + # 右侧:数据曲线图 + ax_plot.set_xlim(0, 20) + ax_plot.set_ylim(0, max(NORMAL_SPEED + 5, SAFE_DISTANCE + 5)) + ax_plot.set_title("实时数据监控") + ax_plot.set_xlabel("检测次数") + ax_plot.set_ylabel("数值") + ax_plot.grid(True) + ax_plot.legend(["前方距离 (cm)", "车速 (km/h)"], loc="upper right") + return ax_scene, ax_plot + + +# 实时更新可视化 +def update_visualization(frame): + # 清空场景图 + ax_scene.clear() + ax_scene.set_xlim(0, 15) + ax_scene.set_ylim(0, 5) + ax_scene.set_title("无人车避障场景模拟") + ax_scene.set_xlabel("位置 (cm/10)") + ax_scene.set_ylabel("位置 (cm/10)") + ax_scene.grid(True) + + # 绘制无人车(矩形) + car_color = "green" if car_direction == "forward" else "yellow" if car_direction in ["left", "right"] else "red" + car = Rectangle((car_pos[0], car_pos[1] - 0.5), 1, 1, color=car_color, label="无人车") + ax_scene.add_patch(car) + + # 绘制障碍物(圆形) + obstacle = Circle(obstacle_pos, 0.3, color="black", label="障碍物") + ax_scene.add_patch(obstacle) + + # 绘制方向标识 + if car_direction == "left": + ax_scene.arrow(car_pos[0] + 0.5, car_pos[1], -0.3, 0, head_width=0.2, color="blue") + elif car_direction == "right": + ax_scene.arrow(car_pos[0] + 0.5, car_pos[1], 0.3, 0, head_width=0.2, color="blue") + elif car_direction == "forward": + ax_scene.arrow(car_pos[0] + 0.5, car_pos[1], 0.3, 0, head_width=0.2, color="blue") + + # 更新曲线图 + ax_plot.clear() + ax_plot.plot(time_history, distance_history, 'b-', label="前方距离 (cm)") + ax_plot.plot(time_history, speed_history, 'r-', label="车速 (km/h)") + # 绘制安全阈值线 + ax_plot.axhline(y=SAFE_DISTANCE, color='g', linestyle='--', label="安全距离") + ax_plot.axhline(y=WARNING_DISTANCE, color='y', linestyle='--', label="警告距离") + ax_plot.axhline(y=DANGER_DISTANCE, color='r', linestyle='--', label="危险距离") + ax_plot.set_xlim(max(0, len(time_history) - 20), len(time_history)) + ax_plot.set_ylim(0, max(NORMAL_SPEED + 5, SAFE_DISTANCE + 5)) + ax_plot.set_title("实时数据监控") + ax_plot.set_xlabel("检测次数") + ax_plot.set_ylabel("数值") + ax_plot.grid(True) + ax_plot.legend(loc="upper right") + + return ax_scene, ax_plot + + +# 主运行逻辑 +if __name__ == "__main__": + car = UnmannedCar() + init_visualization() + + # 启动动画更新(每1秒刷新一次,和传感器检测频率同步) + ani = animation.FuncAnimation(fig, update_visualization, interval=1000, blit=False) + + + # 启动无人车避障逻辑(后台运行) + def run_car(): + print("=== 无人车启动 ===") + try: + while True: + car.collision_avoidance() + time.sleep(1) + except KeyboardInterrupt: + print("\n=== 无人车停止 ===") + car.adjust_speed(STOP_SPEED) + car.adjust_direction("stop") + + + # 多线程运行(避免阻塞可视化) + import threading + + car_thread = threading.Thread(target=run_car) + car_thread.daemon = True + car_thread.start() + + # 显示可视化窗口 + plt.tight_layout() + plt.show() \ No newline at end of file From b951f8c8dcc406b4fa337752b877c6f3d616977d Mon Sep 17 00:00:00 2001 From: yuboyyy <2814479226@qq.com> Date: Fri, 19 Dec 2025 13:19:52 +0800 Subject: [PATCH 7/7] 1 --- src/Smart_car/obstacle_avoidance.py | 376 ++++++++++++++-------------- 1 file changed, 187 insertions(+), 189 deletions(-) diff --git a/src/Smart_car/obstacle_avoidance.py b/src/Smart_car/obstacle_avoidance.py index 1787cd840f..9fb55b685a 100644 --- a/src/Smart_car/obstacle_avoidance.py +++ b/src/Smart_car/obstacle_avoidance.py @@ -1,193 +1,191 @@ import time -import random -import matplotlib.pyplot as plt -import matplotlib.animation as animation -from matplotlib.patches import Rectangle, Circle - -# 无人车状态常量 -SAFE_DISTANCE = 50 # 安全距离(厘米) -WARNING_DISTANCE = 30 # 警告距离(厘米) -DANGER_DISTANCE = 15 # 危险距离(厘米) -NORMAL_SPEED = 20 # 正常速度(km/h) -LOW_SPEED = 5 # 低速(km/h) -STOP_SPEED = 0 # 停车速度 - -# 可视化全局变量 -fig, (ax_scene, ax_plot) = plt.subplots(1, 2, figsize=(12, 5)) -distance_history = [] # 前方距离历史 -speed_history = [] # 车速历史 -time_history = [] # 时间轴 -car_pos = [5, 2.5] # 无人车初始位置(x,y) -obstacle_pos = [0, 0] # 障碍物位置 -car_direction = "forward" - - -class UnmannedCar: +import math +from enum import Enum + +# ======================== 常量定义 ======================== +# 安全参数(可根据车型调整) +SAFE_DISTANCE = 5.0 # 安全距离(米),低于此值触发预警 +EMERGENCY_DISTANCE = 2.0 # 紧急制动距离(米) +MAX_DECELERATION = 8.0 # 最大减速度(m/s²),符合道路安全标准 +TTC_THRESHOLD_LOW = 3.0 # 低风险TTC阈值(秒) +TTC_THRESHOLD_HIGH = 1.5 # 高风险TTC阈值(秒) +VEHICLE_MAX_SPEED = 30.0 # 车辆最大速度(m/s)≈108km/h + +# ======================== 枚举定义 ======================== +class CollisionRiskLevel(Enum): + """碰撞风险等级""" + NONE = 0 # 无风险 + LOW = 1 # 低风险(预警) + MEDIUM = 2 # 中风险(减速) + HIGH = 3 # 高风险(紧急制动) + +class ControlCommand(Enum): + """车辆控制指令""" + NORMAL = 0 # 正常行驶 + WARNING = 1 # 预警(声光提示) + DECELERATE = 2 # 减速 + EMERGENCY_STOP = 3 # 紧急制动 + +# ======================== 核心类实现 ======================== +class Obstacle: + """障碍物类:模拟感知到的障碍物信息""" + def __init__(self, distance: float, relative_speed: float, obstacle_type: str): + """ + :param distance: 障碍物距离(米),正值表示前方 + :param relative_speed: 相对速度(m/s),正值表示靠近 + :param obstacle_type: 障碍物类型(行人/车辆/障碍物) + """ + self.distance = max(0.0, distance) # 距离非负 + self.relative_speed = relative_speed + self.obstacle_type = obstacle_type + self.update_time = time.time() # 感知数据更新时间 + + def update(self, distance: float, relative_speed: float): + """更新障碍物感知数据""" + self.distance = max(0.0, distance) + self.relative_speed = relative_speed + self.update_time = time.time() + +class CollisionPreventionSystem: + """碰撞预防系统核心类""" def __init__(self): - self.speed = 0 - self.direction = "forward" - - def simulate_sensor(self, direction): - """模拟传感器测距(加入轻微固定偏移,让障碍物位置可预测)""" - if direction == "front": - # 模拟障碍物距离缓慢变化(更贴近实际) - base_dist = random.randint(10, 60) if len(distance_history) < 5 else distance_history[-1] + random.randint( - -5, 5) - distance = max(0, min(100, base_dist)) # 限制0-100cm + self.current_speed = 0.0 # 车辆当前速度(m/s) + self.obstacle = None # 感知到的前方障碍物 + self.risk_level = CollisionRiskLevel.NONE + self.control_command = ControlCommand.NORMAL + + def perception_update(self, obstacle_distance: float, obstacle_relative_speed: float, obstacle_type: str): + """ + 更新感知模块数据 + :param obstacle_distance: 障碍物距离(米) + :param obstacle_relative_speed: 相对速度(m/s) + :param obstacle_type: 障碍物类型 + """ + if self.obstacle is None: + self.obstacle = Obstacle(obstacle_distance, obstacle_relative_speed, obstacle_type) else: - distance = random.randint(20, 80) # 左右侧距离 - - # 更新障碍物位置(用于可视化) - global obstacle_pos - obstacle_pos = [car_pos[0] + distance / 10, car_pos[1]] # 缩放适配画布 - print(f"[{direction}] 传感器检测距离:{distance} cm") - return distance - - def adjust_speed(self, new_speed): - self.speed = new_speed - print(f"车速调整为:{self.speed} km/h") - - def adjust_direction(self, new_dir): - global car_direction - self.direction = new_dir - car_direction = new_dir - print(f"行驶方向调整为:{self.direction}") - - def collision_avoidance(self): - """核心避撞逻辑""" - front_dist = self.simulate_sensor("front") - - # 记录数据用于绘图 - distance_history.append(front_dist) - speed_history.append(self.speed) - time_history.append(len(time_history)) - - if front_dist > SAFE_DISTANCE: - self.adjust_speed(NORMAL_SPEED) - self.adjust_direction("forward") - - elif WARNING_DISTANCE < front_dist <= SAFE_DISTANCE: - print("⚠️ 前方接近障碍物,减速!") - self.adjust_speed(LOW_SPEED) - self.adjust_direction("forward") - - elif front_dist <= DANGER_DISTANCE: - print("🚨 前方紧急危险!立即停车!") - self.adjust_speed(STOP_SPEED) - self.adjust_direction("stop") - - left_dist = self.simulate_sensor("left") - right_dist = self.simulate_sensor("right") - - if left_dist > SAFE_DISTANCE: - print("🔄 左侧有空间,转向左侧避障") - self.adjust_direction("left") - self.adjust_speed(LOW_SPEED) - elif right_dist > SAFE_DISTANCE: - print("🔄 右侧有空间,转向右侧避障") - self.adjust_direction("right") - self.adjust_speed(LOW_SPEED) - else: - print("❌ 左右侧均有障碍物,无法避障,保持停车!") - - -# 初始化可视化场景 -def init_visualization(): - # 左侧:场景图(无人车+障碍物) - ax_scene.set_xlim(0, 15) - ax_scene.set_ylim(0, 5) - ax_scene.set_title("无人车避障场景模拟") - ax_scene.set_xlabel("位置 (cm/10)") - ax_scene.set_ylabel("位置 (cm/10)") - ax_scene.grid(True) - - # 右侧:数据曲线图 - ax_plot.set_xlim(0, 20) - ax_plot.set_ylim(0, max(NORMAL_SPEED + 5, SAFE_DISTANCE + 5)) - ax_plot.set_title("实时数据监控") - ax_plot.set_xlabel("检测次数") - ax_plot.set_ylabel("数值") - ax_plot.grid(True) - ax_plot.legend(["前方距离 (cm)", "车速 (km/h)"], loc="upper right") - return ax_scene, ax_plot - - -# 实时更新可视化 -def update_visualization(frame): - # 清空场景图 - ax_scene.clear() - ax_scene.set_xlim(0, 15) - ax_scene.set_ylim(0, 5) - ax_scene.set_title("无人车避障场景模拟") - ax_scene.set_xlabel("位置 (cm/10)") - ax_scene.set_ylabel("位置 (cm/10)") - ax_scene.grid(True) - - # 绘制无人车(矩形) - car_color = "green" if car_direction == "forward" else "yellow" if car_direction in ["left", "right"] else "red" - car = Rectangle((car_pos[0], car_pos[1] - 0.5), 1, 1, color=car_color, label="无人车") - ax_scene.add_patch(car) - - # 绘制障碍物(圆形) - obstacle = Circle(obstacle_pos, 0.3, color="black", label="障碍物") - ax_scene.add_patch(obstacle) - - # 绘制方向标识 - if car_direction == "left": - ax_scene.arrow(car_pos[0] + 0.5, car_pos[1], -0.3, 0, head_width=0.2, color="blue") - elif car_direction == "right": - ax_scene.arrow(car_pos[0] + 0.5, car_pos[1], 0.3, 0, head_width=0.2, color="blue") - elif car_direction == "forward": - ax_scene.arrow(car_pos[0] + 0.5, car_pos[1], 0.3, 0, head_width=0.2, color="blue") - - # 更新曲线图 - ax_plot.clear() - ax_plot.plot(time_history, distance_history, 'b-', label="前方距离 (cm)") - ax_plot.plot(time_history, speed_history, 'r-', label="车速 (km/h)") - # 绘制安全阈值线 - ax_plot.axhline(y=SAFE_DISTANCE, color='g', linestyle='--', label="安全距离") - ax_plot.axhline(y=WARNING_DISTANCE, color='y', linestyle='--', label="警告距离") - ax_plot.axhline(y=DANGER_DISTANCE, color='r', linestyle='--', label="危险距离") - ax_plot.set_xlim(max(0, len(time_history) - 20), len(time_history)) - ax_plot.set_ylim(0, max(NORMAL_SPEED + 5, SAFE_DISTANCE + 5)) - ax_plot.set_title("实时数据监控") - ax_plot.set_xlabel("检测次数") - ax_plot.set_ylabel("数值") - ax_plot.grid(True) - ax_plot.legend(loc="upper right") - - return ax_scene, ax_plot - - -# 主运行逻辑 + self.obstacle.update(obstacle_distance, obstacle_relative_speed) + + def calculate_ttc(self) -> float: + """ + 计算碰撞时间(Time To Collision, TTC) + :return: TTC值(秒),无穷大表示无碰撞风险 + """ + if self.obstacle is None or self.obstacle.relative_speed <= 0: + return float('inf') # 相对速度≤0,无碰撞风险 + return self.obstacle.distance / self.obstacle.relative_speed + + def evaluate_risk(self): + """评估碰撞风险等级""" + ttc = self.calculate_ttc() + distance = self.obstacle.distance if self.obstacle else float('inf') + + # 风险等级判定逻辑 + if ttc >= TTC_THRESHOLD_LOW or distance >= SAFE_DISTANCE: + self.risk_level = CollisionRiskLevel.NONE + elif TTC_THRESHOLD_HIGH <= ttc < TTC_THRESHOLD_LOW or EMERGENCY_DISTANCE <= distance < SAFE_DISTANCE: + self.risk_level = CollisionRiskLevel.LOW if ttc >= TTC_THRESHOLD_HIGH else CollisionRiskLevel.MEDIUM + else: + self.risk_level = CollisionRiskLevel.HIGH + + def generate_control_command(self) -> ControlCommand: + """根据风险等级生成控制指令""" + if self.risk_level == CollisionRiskLevel.NONE: + self.control_command = ControlCommand.NORMAL + elif self.risk_level == CollisionRiskLevel.LOW: + self.control_command = ControlCommand.WARNING + elif self.risk_level == CollisionRiskLevel.MEDIUM: + self.control_command = ControlCommand.DECELERATE + else: + self.control_command = ControlCommand.EMERGENCY_STOP + return self.control_command + + def execute_control(self, command: ControlCommand) -> float: + """ + 执行车辆控制指令,返回控制后的车速 + :param command: 控制指令 + :return: 调整后的车速(m/s) + """ + delta_time = 0.1 # 控制周期(秒),模拟实时控制 + + if command == ControlCommand.NORMAL: + # 正常行驶,维持当前速度(可扩展加速逻辑) + pass + elif command == ControlCommand.WARNING: + # 仅预警,不调整车速(声光提示驾驶员) + print("[预警] 检测到前方障碍物,请注意!") + elif command == ControlCommand.DECELERATE: + # 减速:中等减速度(最大减速度的50%) + deceleration = MAX_DECELERATION * 0.5 + self.current_speed = max(0.0, self.current_speed - deceleration * delta_time) + print(f"[减速] 当前车速:{self.current_speed:.2f} m/s(原速度:{self.current_speed + deceleration * delta_time:.2f} m/s)") + elif command == ControlCommand.EMERGENCY_STOP: + # 紧急制动:最大减速度 + deceleration = MAX_DECELERATION + self.current_speed = max(0.0, self.current_speed - deceleration * delta_time) + print(f"[紧急制动] 当前车速:{self.current_speed:.2f} m/s(紧急制动中)") + + # 限制车速不超过最大值 + self.current_speed = min(self.current_speed, VEHICLE_MAX_SPEED) + return self.current_speed + + def run_cycle(self, obstacle_distance: float, obstacle_relative_speed: float, obstacle_type: str, current_speed: float): + """ + 碰撞预防系统单次运行周期 + :param obstacle_distance: 障碍物距离(米) + :param obstacle_relative_speed: 相对速度(m/s) + :param obstacle_type: 障碍物类型 + :param current_speed: 车辆当前速度(m/s) + """ + # 1. 更新车辆当前速度 + self.current_speed = current_speed + + # 2. 更新感知数据 + self.perception_update(obstacle_distance, obstacle_relative_speed, obstacle_type) + + # 3. 风险评估 + self.evaluate_risk() + + # 4. 生成控制指令 + command = self.generate_control_command() + + # 5. 执行控制 + new_speed = self.execute_control(command) + + # 6. 输出状态信息 + print(f"\n=== 系统状态 ===") + print(f"障碍物类型:{obstacle_type}") + print(f"障碍物距离:{self.obstacle.distance:.2f} 米") + print(f"相对速度:{self.obstacle.relative_speed:.2f} m/s") + print(f"碰撞时间(TTC):{self.calculate_ttc():.2f} 秒") + print(f"风险等级:{self.risk_level.name}") + print(f"控制指令:{command.name}") + print(f"当前车速:{new_speed:.2f} m/s") + +# ======================== 测试用例 ======================== if __name__ == "__main__": - car = UnmannedCar() - init_visualization() - - # 启动动画更新(每1秒刷新一次,和传感器检测频率同步) - ani = animation.FuncAnimation(fig, update_visualization, interval=1000, blit=False) - - - # 启动无人车避障逻辑(后台运行) - def run_car(): - print("=== 无人车启动 ===") - try: - while True: - car.collision_avoidance() - time.sleep(1) - except KeyboardInterrupt: - print("\n=== 无人车停止 ===") - car.adjust_speed(STOP_SPEED) - car.adjust_direction("stop") - - - # 多线程运行(避免阻塞可视化) - import threading - - car_thread = threading.Thread(target=run_car) - car_thread.daemon = True - car_thread.start() - - # 显示可视化窗口 - plt.tight_layout() - plt.show() \ No newline at end of file + # 初始化碰撞预防系统 + cps = CollisionPreventionSystem() + + # 模拟不同场景的运行周期 + test_scenarios = [ + # 场景1:无风险(远距离、低相对速度) + {"distance": 10.0, "relative_speed": 1.0, "type": "车辆", "speed": 15.0}, + # 场景2:低风险(预警) + {"distance": 6.0, "relative_speed": 3.0, "type": "行人", "speed": 10.0}, + # 场景3:中风险(减速) + {"distance": 3.0, "relative_speed": 4.0, "type": "障碍物", "speed": 8.0}, + # 场景4:高风险(紧急制动) + {"distance": 1.5, "relative_speed": 5.0, "type": "车辆", "speed": 6.0}, + ] + + # 运行测试场景 + for i, scenario in enumerate(test_scenarios): + print(f"\n==================== 测试场景 {i+1} ====================") + cps.run_cycle( + obstacle_distance=scenario["distance"], + obstacle_relative_speed=scenario["relative_speed"], + obstacle_type=scenario["type"], + current_speed=scenario["speed"] + ) + time.sleep(0.5) # 模拟时间间隔 \ No newline at end of file