Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
123 changes: 67 additions & 56 deletions src/Smart_car/obstacle_avoidance.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,53 +13,58 @@

# ======================== 枚举定义 ========================
class CollisionRiskLevel(Enum):
"""碰撞风险等级"""
NONE = 0 # 无风险
LOW = 1 # 低风险(预警)
MEDIUM = 2 # 中风险(减速)
HIGH = 3 # 高风险(紧急制动)
"""碰撞风险等级枚举:用于标识当前与前方障碍物的碰撞危险程度"""
NONE = 0 # 无风险:障碍物距离较远或相对速度低,无需任何干预
LOW = 1 # 低风险(预警):存在潜在碰撞隐患,需提醒驾驶员注意
MEDIUM = 2 # 中风险(减速):碰撞概率上升,需车辆自动减速降低风险
HIGH = 3 # 高风险(紧急制动):碰撞即将发生,需全力制动避免事故

class ControlCommand(Enum):
"""车辆控制指令"""
NORMAL = 0 # 正常行驶
WARNING = 1 # 预警(声光提示)
DECELERATE = 2 # 减速
EMERGENCY_STOP = 3 # 紧急制动
"""车辆控制指令枚举:根据碰撞风险等级对应生成的车辆执行指令"""
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: 障碍物类型(行人/车辆/障碍物)
障碍物对象初始化
:param distance: 障碍物距离(米),正值表示在本车前方
:param relative_speed: 相对速度(m/s),正值表示本车与障碍物正在相互靠近
:param obstacle_type: 障碍物类型(行人/车辆/固定障碍物等)
"""
self.distance = max(0.0, distance) # 距离非负
self.distance = max(0.0, distance) # 距离非负:避免感知数据异常导致的无效负数距离
self.relative_speed = relative_speed
self.obstacle_type = obstacle_type
self.update_time = time.time() # 感知数据更新时间
self.update_time = time.time() # 感知数据更新时间戳:用于判断数据是否失效

def update(self, distance: float, relative_speed: float):
"""更新障碍物感知数据"""
"""
更新障碍物感知数据:感知模块周期性刷新数据时调用
:param distance: 最新检测到的障碍物距离(米)
:param relative_speed: 最新检测到的本车与障碍物相对速度(m/s)
"""
self.distance = max(0.0, distance)
self.relative_speed = relative_speed
self.update_time = time.time()

class CollisionPreventionSystem:
"""碰撞预防系统核心类"""
"""碰撞预防系统核心类:整合感知、风险评估、指令生成与控制执行的全流程"""
def __init__(self):
self.current_speed = 0.0 # 车辆当前速度(m/s)
self.obstacle = None # 感知到的前方障碍物
self.risk_level = CollisionRiskLevel.NONE
self.control_command = ControlCommand.NORMAL
self.current_speed = 0.0 # 车辆当前行驶速度(m/s)
self.obstacle = None # 感知到的前方障碍物对象,初始为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: 障碍物类型
:param obstacle_type: 障碍物类型(行人/车辆/固定障碍物等)
"""
if self.obstacle is None:
self.obstacle = Obstacle(obstacle_distance, obstacle_relative_speed, obstacle_type)
Expand All @@ -68,15 +73,18 @@ def perception_update(self, obstacle_distance: float, obstacle_relative_speed: f

def calculate_ttc(self) -> float:
"""
计算碰撞时间(Time To Collision, TTC)
:return: TTC值(秒),无穷大表示无碰撞风险
计算碰撞时间(Time To Collision, TTC):预测本车与障碍物发生碰撞的剩余时间
:return: TTC值(秒),无穷大(float('inf'))表示无碰撞风险(无障碍物或障碍物远离)
"""
if self.obstacle is None or self.obstacle.relative_speed <= 0:
return float('inf') # 相对速度≤0,无碰撞风险
return self.obstacle.distance / self.obstacle.relative_speed
return float('inf') # 相对速度≤0:障碍物与本车相互远离或静止,无碰撞风险
return self.obstacle.distance / self.obstacle.relative_speed # TTC核心计算公式

def evaluate_risk(self):
"""评估碰撞风险等级"""
"""
评估碰撞风险等级:基于TTC值和障碍物距离双指标,综合判定碰撞危险程度
判定逻辑兼顾时间维度(TTC)和空间维度(距离),提升风险判断准确性
"""
ttc = self.calculate_ttc()
distance = self.obstacle.distance if self.obstacle else float('inf')

Expand All @@ -89,7 +97,10 @@ def evaluate_risk(self):
self.risk_level = CollisionRiskLevel.HIGH

def generate_control_command(self) -> ControlCommand:
"""根据风险等级生成控制指令"""
"""
根据风险等级生成控制指令:建立风险等级与车辆控制动作的一一对应关系
:return: 对应的车辆控制指令(ControlCommand枚举类型)
"""
if self.risk_level == CollisionRiskLevel.NONE:
self.control_command = ControlCommand.NORMAL
elif self.risk_level == CollisionRiskLevel.LOW:
Expand All @@ -102,57 +113,57 @@ def generate_control_command(self) -> ControlCommand:

def execute_control(self, command: ControlCommand) -> float:
"""
执行车辆控制指令,返回控制后的车速
:param command: 控制指令
:return: 调整后的车速(m/s)
执行车辆控制指令,根据指令调整车辆车速,并返回控制后的最新车速
:param command: 待执行的车辆控制指令(ControlCommand枚举类型)
:return: 调整后的车辆当前车速(m/s)
"""
delta_time = 0.1 # 控制周期(秒),模拟实时控制
delta_time = 0.1 # 控制周期(秒),模拟车载系统实时控制的100ms刷新间隔

if command == ControlCommand.NORMAL:
# 正常行驶,维持当前速度(可扩展加速逻辑)
# 正常行驶,维持当前速度(可扩展后续加速逻辑)
pass
elif command == ControlCommand.WARNING:
# 仅预警,不调整车速(声光提示驾驶员)
# 仅预警,不调整车速(通过声光设备提示驾驶员注意前方障碍物)
print("[预警] 检测到前方障碍物,请注意!")
elif command == ControlCommand.DECELERATE:
# 减速:中等减速度(最大减速度的50%)
# 减速:采用中等减速度(最大减速度的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)
:param obstacle_type: 障碍物类型(行人/车辆/固定障碍物等)
:param current_speed: 车辆当前行驶速度(m/s)
"""
# 1. 更新车辆当前速度
# 1. 更新车辆当前速度:同步本车最新行驶状态
self.current_speed = current_speed

# 2. 更新感知数据
# 2. 更新感知数据:同步前方障碍物的最新检测信息
self.perception_update(obstacle_distance, obstacle_relative_speed, obstacle_type)

# 3. 风险评估
# 3. 风险评估:基于最新感知数据判定碰撞风险等级
self.evaluate_risk()

# 4. 生成控制指令
# 4. 生成控制指令:根据风险等级匹配对应的车辆控制动作
command = self.generate_control_command()

# 5. 执行控制
# 5. 执行控制:执行控制指令并调整车辆车速
new_speed = self.execute_control(command)

# 6. 输出状态信息
# 6. 输出状态信息:打印系统当前各项关键参数,便于调试和监控
print(f"\n=== 系统状态 ===")
print(f"障碍物类型:{obstacle_type}")
print(f"障碍物距离:{self.obstacle.distance:.2f} 米")
Expand All @@ -164,22 +175,22 @@ def run_cycle(self, obstacle_distance: float, obstacle_relative_speed: float, ob

# ======================== 测试用例 ========================
if __name__ == "__main__":
# 初始化碰撞预防系统
# 初始化碰撞预防系统实例:创建系统核心对象
cps = CollisionPreventionSystem()

# 模拟不同场景的运行周期
# 模拟不同场景的运行周期:覆盖无风险、低风险、中风险、高风险四种典型行驶场景
test_scenarios = [
# 场景1:无风险(远距离、低相对速度)
# 场景1:无风险(远距离、低相对速度,无需任何干预)
{"distance": 10.0, "relative_speed": 1.0, "type": "车辆", "speed": 15.0},
# 场景2:低风险(预警)
# 场景2:低风险(预警)(距离接近安全阈值、相对速度适中,需提醒驾驶员)
{"distance": 6.0, "relative_speed": 3.0, "type": "行人", "speed": 10.0},
# 场景3:中风险(减速)
# 场景3:中风险(减速)(距离接近紧急阈值、相对速度较高,需自动减速)
{"distance": 3.0, "relative_speed": 4.0, "type": "障碍物", "speed": 8.0},
# 场景4:高风险(紧急制动)
# 场景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(
Expand All @@ -188,4 +199,4 @@ def run_cycle(self, obstacle_distance: float, obstacle_relative_speed: float, ob
obstacle_type=scenario["type"],
current_speed=scenario["speed"]
)
time.sleep(0.5) # 模拟时间间隔
time.sleep(0.5) # 模拟时间间隔:模拟不同场景之间的时间流逝,更贴近实际运行场景
72 changes: 72 additions & 0 deletions src/Smart_car/overload.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
import time
import random

class UnmannedVehicleOverloadWarningSystem:
"""无人车超载预警系统"""
def __init__(self, max_load=1000):
# 初始化系统参数
self.max_load = max_load # 车辆最大载重(单位:kg,可自定义)
self.current_load = 0 # 当前载重
self.warning_level = 0 # 预警等级:0-正常,1-轻度过载,2-中度过载,3-严重超载

def simulate_weight_collection(self):
"""模拟无人车重量采集(模拟传感器数据,可替换为真实硬件接口)"""
# 模拟载重变化:每次在当前基础上小幅波动(模拟乘客/货物上下车)
weight_change = random.randint(-50, 100)
self.current_load = max(0, self.current_load + weight_change) # 载重不能为负数
return self.current_load

def judge_overload(self):
"""超载判断逻辑,划分预警等级"""
load_ratio = self.current_load / self.max_load # 载重占比

if load_ratio <= 0.8:
self.warning_level = 0 # 正常:载重≤80%最大载重
return "正常", "绿色", "当前载重未超出安全范围,无需处理"
elif 0.8 < load_ratio <= 0.95:
self.warning_level = 1 # 轻度过载:80%<载重≤95%
return "轻度过载预警", "黄色", "当前载重接近上限,建议停止加载"
elif 0.95 < load_ratio <= 1.1:
self.warning_level = 2 # 中度过载:95%<载重≤110%
return "中度过载警告", "橙色", "当前载重已超出安全上限,立即停止运行并卸载部分货物"
else:
self.warning_level = 3 # 严重超载:载重>110%
return "严重超载警报", "红色", "极度危险!立即紧急制动,联系工作人员处理"

def display_warning(self, status, color, desc):
"""可视化输出预警信息(模拟车载终端显示)"""
print("=" * 50)
print(f"【无人车超载预警系统 - 实时监测】")
print(f"当前时间:{time.strftime('%Y-%m-%d %H:%M:%S')}")
print(f"最大载重:{self.max_load} kg")
print(f"当前载重:{self.current_load:.2f} kg")
print(f"载重占比:{self.current_load/self.max_load*100:.2f}%")
print(f"预警状态:【{status}】({color})")
print(f"处理建议:{desc}")
print("=" * 50)
print()

def run(self, monitor_times=10):
"""运行系统,持续监测载重并输出预警"""
print("无人车超载预警系统已启动...")
print(f"开始持续监测(共监测{monitor_times}次,每次间隔2秒)\n")

for i in range(monitor_times):
# 1. 采集当前载重(模拟)
self.simulate_weight_collection()
# 2. 判断超载等级
status, color, desc = self.judge_overload()
# 3. 显示预警信息
self.display_warning(status, color, desc)
# 4. 严重超载时紧急停止
if self.warning_level == 3:
print("❗❗❗ 检测到严重超载,系统紧急停止运行! ❗❗")
break
# 5. 间隔2秒进行下一次监测
time.sleep(2)

if __name__ == "__main__":
# 初始化系统:设置无人车最大载重为1000kg(可根据需求修改)
overload_system = UnmannedVehicleOverloadWarningSystem(max_load=1000)
# 运行系统,持续监测10次(可修改监测次数)
overload_system.run(monitor_times=10)
Loading
Loading