🚗 智能车辆"耍赖式"安全停车系统
智能车辆"耍赖式"安全停车系统
🎯 项目概述
本系统采用"耍赖式"安全停车策略，在危险情况下优先选择靠边停车而非急刹，通过智能决策实现平稳、安全的停车体验。
🔥 痛点分析
传统紧急制动(Emergency Braking)存在三大问题：
- 急刹导致追尾：后车反应不及造成二次事故
- 乘客不适：急减速产生强烈推背感和眩晕
- 道路阻塞：车辆完全停止在车道中央，影响交通
🚀 核心创新
- ✅ 危险检测 → 评估靠边可行性 → 平滑变道 → 渐进减速 → 安全停车
- ✅ 全程无急刹，最大减速度控制在舒适范围内(≤3m/s²)
- ✅ 智能评估右侧空间，确保停车安全
📦 安装依赖
bash
pip install numpy matplotlib scipy
🎮 运行方式
bash
python main.py --scenario emergency_lane_change
📁 项目结构
smart_parking/
├── main.py # 主程序入口
├── vehicle.py # 车辆动力学模型
├── perception.py # 环境感知模块
├── decision.py # 决策引擎
├── motion_planner.py # 运动规划器
├── controller.py # 控制器
├── scenarios.py # 测试场景
└── utils.py # 工具函数
⚙️ 配置参数
修改 config.py 调整：
- 最大舒适减速度:
MAX_COMFORT_DECEL = 3.0 m/s² - 变道安全距离:
LANE_CHANGE_SAFE_DISTANCE = 2.0 m - 停车检测阈值:
EMERGENCY_THRESHOLD = 0.6
📖 使用说明
详见下方"使用说明"章节
📝 核心代码实现
- config.py - 配置文件
""" 智能停车系统配置文件 包含车辆参数、控制参数和安全阈值 """
from dataclasses import dataclass from typing import Tuple
@dataclass class VehicleConfig: """车辆物理参数""" length: float = 4.5 # 车长 (米) width: float = 1.8 # 车宽 (米) wheelbase: float = 2.7 # 轴距 (米) max_speed: float = 120.0 # 最高时速 (km/h) max_accel: float = 3.0 # 最大加速度 (m/s²) max_decel: float = 8.0 # 最大减速度 (m/s²) comfort_decel: float = 3.0 # 舒适减速度 (m/s²) ← 关键参数 steering_range: float = 0.5 # 最大转向角 (rad)
@dataclass class SafetyConfig: """安全参数配置""" # 危险检测阈值 (0-1, 1为最危险) emergency_threshold: float = 0.6 collision_time_threshold: float = 2.0 # 碰撞时间阈值 (秒)
ini
# 靠边停车参数
min_road_width: float = 6.0 # 最小可操作路宽
safe_shoulder_width: float = 1.5 # 安全路肩宽度
lane_change_safe_distance: float = 2.0 # 变道安全距离
# 停车参数
target_parking_speed: float = 0.5 # 目标停车速度
parking_completion_threshold: float = 0.1 # 停车完成判定@dataclass class ControlConfig: """控制参数""" dt: float = 0.02 # 控制周期 (秒) lookahead_distance: float = 20.0 # 前瞻距离 (米) kp_speed: float = 2.0 # 速度环P增益 ki_speed: float = 0.1 # 速度环I增益 kd_speed: float = 0.05 # 速度环D增益
全局配置实例
VEHICLE_CONFIG = VehicleConfig() SAFETY_CONFIG = SafetyConfig() CONTROL_CONFIG = ControlConfig()
车道定义 (相对于自车的横向位置)
LANE_CENTER = 0.0 # 当前车道中心 LEFT_LANE = -3.6 # 左侧车道中心 RIGHT_LANE = 3.6 # 右侧车道中心 SHOULDER_LEFT = -5.0 # 左路肩 SHOULDER_RIGHT = 5.0 # 右路肩
- vehicle.py - 车辆动力学模型
""" 车辆动力学模型 实现基于自行车模型的车辆运动学仿真 支持位置、速度、加速度、航向角的精确计算 """
import numpy as np from config import VEHICLE_CONFIG, CONTROL_CONFIG from typing import Tuple
class VehicleDynamics: """ 车辆动力学模型类
ini
基于自行车模型(Bicycle Model)，考虑：
- 纵向运动：加速/制动
- 横向运动：转向
- 航向变化：由前轮转角决定
核心公式：
- 纵向速度: v_new = v_old + a * dt
- 航向角变化率: dθ/dt = (v / L) * tan(δ)
- 位置更新: x += v*cos(θ)*dt, y += v*sin(θ)*dt
"""
def __init__(self, initial_state: np.ndarray = None):
"""
初始化车辆状态
Args:
initial_state: [x, y, v, θ, δ] 初始状态
- x, y: 位置坐标 (米)
- v: 速度 (m/s)
- θ: 航向角 (弧度)
- δ: 前轮转角 (弧度)
"""
if initial_state is None:
# 默认初始状态：原点、静止、朝正北
self.state = np.array([0.0, 0.0, 15.0, 0.0, 0.0]) # 15m/s ≈ 54km/h
else:
self.state = np.array(initial_state, dtype=np.float64)
# 历史轨迹记录
self.trajectory = [self.state.copy()]
# 车辆尺寸参数
self.length = VEHICLE_CONFIG.length
self.width = VEHICLE_CONFIG.width
self.wheelbase = VEHICLE_CONFIG.wheelbase
@property
def x(self) -> float:
"""获取X坐标"""
return self.state[0]
@property
def y(self) -> float:
"""获取Y坐标"""
return self.state[1]
@property
def velocity(self) -> float:
"""获取速度 (m/s)"""
return self.state[2]
@property
def heading(self) -> float:
"""获取航向角 (弧度)"""
return self.state[3]
@property
def steering_angle(self) -> float:
"""获取前轮转角 (弧度)"""
return self.state[4]
def get_corners(self) -> np.ndarray:
"""
计算车辆四个角点的坐标
用于碰撞检测和可视化
Returns:
4x2数组，每行是一个角的[x, y]坐标
"""
# 车辆中心点
cx, cy = self.x, self.y
# 航向角
theta = self.heading
# 半长和半宽
half_length = self.length / 2
half_width = self.width / 2
# 四个角的相对偏移（车身坐标系）
corners_rel = np.array([
[-half_length, -half_width], # 左下
[-half_length, half_width], # 右下
[half_length, half_width], # 右上
[half_length, -half_width], # 左上
])
# 旋转矩阵
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
rotation_matrix = np.array([
[cos_theta, -sin_theta],
[sin_theta, cos_theta]
])
# 转换到世界坐标系
corners_world = (rotation_matrix @ corners_rel.T).T + np.array([cx, cy])
return corners_world
def update(self, acceleration: float, steering_rate: float) -> np.ndarray:
"""
更新车辆状态
Args:
acceleration: 纵向加速度 (m/s²)，正值加速，负值制动
steering_rate: 前轮转角变化率 (rad/s)
Returns:
更新后的完整状态向量
物理模型详解：
------------------
1. 速度更新（欧拉积分）：
v_new = v_old + a * dt
2. 前轮转角更新：
δ_new = δ_old + ω_steer * dt
（限制转角在安全范围内）
3. 航向角更新：
基于自行车模型：dθ/dt = (v/L) * tan(δ)
4. 位置更新：
x_new = x_old + v * cos(θ) * dt
y_new = y_old + v * sin(θ) * dt
"""
dt = CONTROL_CONFIG.dt
# 解包当前状态
x, y, v, theta, delta = self.state
# ===== 1. 更新速度 =====
# 限制加速度范围
max_accel = VEHICLE_CONFIG.max_accel
max_decel = VEHICLE_CONFIG.max_decel
acceleration = np.clip(acceleration, -max_decel, max_accel)
v_new = v + acceleration * dt
# 防止速度为负
v_new = max(v_new, 0.0)
# ===== 2. 更新前轮转角 =====
# 限制转向角变化率
max_steering_rate = 0.5 # rad/s
steering_rate = np.clip(steering_rate, -max_steering_rate, max_steering_rate)
delta_new = delta + steering_rate * dt
# 限制最大转向角
max_delta = VEHICLE_CONFIG.steering_range
delta_new = np.clip(delta_new, -max_delta, max_delta)
# ===== 3. 更新航向角 =====
# 自行车模型：航向角变化率 = (速度/轴距) * tan(前轮转角)
if abs(v_new) > 0.1: # 速度足够大时才考虑转向
theta_dot = (v_new / self.wheelbase) * np.tan(delta_new)
else:
theta_dot = 0.0 # 低速时忽略转向对航向的影响
theta_new = theta + theta_dot * dt
# 归一化到 [-π, π]
theta_new = np.arctan2(np.sin(theta_new), np.cos(theta_new))
# ===== 4. 更新位置 =====
x_new = x + v_new * np.cos(theta_new) * dt
y_new = y + v_new * np.sin(theta_new) * dt
# ===== 5. 更新状态 =====
self.state = np.array([x_new, y_new, v_new, theta_new, delta_new])
self.trajectory.append(self.state.copy())
return self.state.copy()
def apply_control(self, control_input: dict) -> np.ndarray:
"""
应用控制指令
Args:
control_input: 控制字典
- 'acceleration': 加速度指令
- 'target_steering': 目标转向角（可选）
Returns:
更新后的状态
"""
acceleration = control_input.get('acceleration', 0.0)
if 'target_steering' in control_input:
# PID转向控制
current_delta = self.steering_angle
target_delta = control_input['target_steering']
error = target_delta - current_delta
steering_rate = 2.0 * error # 简单比例控制
else:
steering_rate = 0.0
return self.update(acceleration, steering_rate)
def get_state_dict(self) -> dict:
"""获取状态字典，便于JSON序列化"""
return {
'x': self.x,
'y': self.y,
'velocity': self.velocity,
'speed_kmh': self.velocity * 3.6,
'heading': self.heading,
'heading_deg': np.degrees(self.heading),
'steering_angle': self.steering_angle,
'steering_deg': np.degrees(self.steering_angle)
}
def __repr__(self) -> str:
return (f"Vehicle(x={self.x:.2f}, y={self.y:.2f}, "
f"v={self.velocity*3.6:.1f}km/h, θ={np.degrees(self.heading):.1f}°)")class ComfortableDecelerationProfile: """ 舒适减速曲线生成器
ini
生成符合人体工程学的减速曲线，避免急刹带来的不适感
采用S型曲线(S-curve)实现平滑过渡
"""
@staticmethod
def generate_profile(
initial_speed: float,
target_speed: float,
comfort_decel: float = None
) -> Tuple[np.ndarray, np.ndarray]:
"""
生成减速曲线
Args:
initial_speed: 初始速度 (m/s)
target_speed: 目标速度 (m/s)
comfort_decel: 舒适减速度 (m/s²)
Returns:
times: 时间序列
speeds: 对应时刻的速度序列
"""
if comfort_decel is None:
comfort_decel = VEHICLE_CONFIG.comfort_decel
# 计算理论制动时间
speed_diff = initial_speed - target_speed
if speed_diff <= 0:
return np.array([0]), np.array([initial_speed])
t_brake = speed_diff / comfort_decel
# 生成S型曲线的时间点
num_points = int(t_brake / CONTROL_CONFIG.dt) + 10
times = np.linspace(0, t_brake * 1.2, num_points)
speeds = np.zeros_like(times)
for i, t in enumerate(times):
if t < t_brake:
# S型曲线：使用sigmoid函数变体
# 在0.1*t_brake到0.9*t_brake之间完成主要减速
tau = t / t_brake
# 平滑S曲线
if tau < 0.1:
# 缓启动阶段
factor = 0.5 * (1 - np.cos(np.pi * tau / 0.1)) / 2
elif tau > 0.9:
# 缓结束阶段
factor = 0.5 * (1 + np.cos(np.pi * (tau - 0.9) / 0.1)) / 2 + 0.5
else:
# 主要减速阶段
factor = 0.5 + 0.5 * (tau - 0.5) * 2
speeds[i] = initial_speed - speed_diff * factor
else:
speeds[i] = target_speed
return times, speeds测试代码
if name == "main": print("=" * 50) print("车辆动力学模型测试") print("=" * 50)
python
# 创建车辆实例
vehicle = VehicleDynamics(initial_state=[0, 0, 20, 0, 0])
print(f"初始状态: {vehicle}")
# 模拟匀速行驶
print("\n--- 匀速行驶测试 ---")
for _ in range(50):
vehicle.update(acceleration=0.0, steering_rate=0.0)
print(f"匀速后: {vehicle}")
# 模拟舒适减速
print("\n--- 舒适减速测试 ---")
comfort_decel = VEHICLE_CONFIG.comfort_decel
for _ in range(100):
vehicle.update(acceleration=-comfort_decel, steering_rate=0.0)
print(f"减速后: {vehicle}")
print(f"当前速度: {vehicle.velocity * 3.6:.1f} km/h")
# 测试减速曲线
print("\n--- 减速曲线生成测试 ---")
times, speeds = ComfortableDecelerationProfile.generate_profile(
initial_speed=25.0, # 90 km/h
target_speed=5.0, # 18 km/h
comfort_decel=2.5
)
print(f"曲线点数: {len(times)}")
print(f"制动时间: {times[-1]:.2f}s")
print(f"速度从 {speeds[0]*3.6:.1f} km/h 降至 {speeds[-1]*3.6:.1f} km/h")- perception.py - 环境感知模块
""" 环境感知模块 模拟车载传感器数据，检测周围车辆、车道线和路肩 """
import numpy as np from typing import List, Dict, Optional, Tuple from dataclasses import dataclass from enum import Enum import math
class ObjectType(Enum): """感知对象类型""" VEHICLE = "vehicle" PEDESTRIAN = "pedestrian" CYCLIST = "cyclist" STATIC_OBSTACLE = "static"
@dataclass class DetectedObject: """检测到的物体""" obj_id: int obj_type: ObjectType x: float # 世界坐标系X y: float # 世界坐标系Y vx: float # X方向速度 vy: float # Y方向速度 length: float # 长度 width: float # 宽度 confidence: float # 检测置信度
@dataclass class LaneInfo: """车道信息""" lane_id: int center_x: float # 车道中心线X坐标 left_boundary: float # 左边界X right_boundary: float # 右边界X curvature: float # 曲率 confidence: float
@dataclass class RoadShoulder: """路肩信息""" left_shoulder_width: float # 左路肩宽度 right_shoulder_width: float # 右路肩宽度 shoulder_quality: float # 路肩质量 (0-1, 1为最佳)
class SensorFusion: """ 传感器融合类 模拟毫米波雷达、摄像头、激光雷达的数据融合 """
ini
def __init__(self, vehicle_width: float = 1.8):
"""
初始化传感器融合模块
Args:
vehicle_width: 自车宽度，用于计算安全边界
"""
self.vehicle_width = vehicle_width
self.detection_range = 150.0 # 检测范围 (米)
self.fov = 120.0 # 视场角 (度)
def detect_objects(
self,
ego_vehicle: 'VehicleDynamics',
scenario_data: dict
) -> List[DetectedObject]:
"""
检测周围物体
Args:
ego_vehicle: 自车状态
scenario_data: 场景数据，包含其他车辆信息
Returns:
检测到的物体列表
"""
detected_objects = []
ego_x, ego_y, ego_v, ego_heading = (
ego_vehicle.x, ego_vehicle.y,
ego_vehicle.velocity, ego_vehicle.heading
)
# 获取场景中的物体
objects_in_scenario = scenario_data.get('objects', [])
for obj_data in objects_in_scenario:
# 计算相对位置和速度
dx = obj_data['x'] - ego_x
dy = obj_data['y'] - ego_y
# 转换到自车坐标系
cos_h = math.cos(-ego_heading)
sin_h = math.sin(-ego_heading)
rel_x = dx * cos_h - dy * sin_h
rel_y = dx * sin_h + dy * cos_h
# 检查是否在检测范围内
distance = math.sqrt(rel_x**2 + rel_y**2)
if distance > self.detection_range:
continue
# 检查是否在视场角内
angle = math.degrees(math.atan2(rel_y, rel_x))
if abs(angle) > self.fov / 2:
continue
# 计算相对速度
rel_vx = obj_data['vx'] - ego_v * math.cos(ego_heading)
rel_vy = obj_data['vy'] - ego_v * math.sin(ego_heading)
# 计算检测置信度（距离越近，置信度越高）
confidence = max(0.5, 1.0 - distance / self.detection_range)
# 确定物体类型
obj_type_map = {
'car': ObjectType.VEHICLE,
'truck': ObjectType.VEHICLE,
'pedestrian': ObjectType.PEDESTRIAN,
'cyclist': ObjectType.CYCLIST,
'obstacle': ObjectType.STATIC_OBSTACLE
}
detected_obj = DetectedObject(
obj_id=obj_data['id'],
obj_type=obj_type_map.get(obj_data['type'], ObjectType.VEHICLE),
x=obj_data['x'],
y=obj_data['y'],
vx=rel_vx,
vy=rel_vy,
length=obj_data.get('length', 4.5),
width=obj_data.get('width', 1.8),
confidence=confidence
)
detected_objects.append(detected_obj)
return detected_objects
def detect_lanes(
self,
ego_vehicle: 'VehicleDynamics',
road_layout: dict
) -> List[LaneInfo]:
"""
检测车道线
Args:
ego_vehicle: 自车状态
road_layout: 道路布局数据
Returns:
车道信息列表
"""
lanes = []
ego_x, ego_y, ego_heading = (
ego_vehicle.x, ego_vehicle.y, ego_vehicle.heading
)
# 获取道路车道配置
lane_configs = road_layout.get('lanes', [])
for lane_id, lane_data in enumerate(lane_configs):
# 计算自车到车道中心的横向距离
lane_center = lane_data['center_x']
lane_width = lane_data['width']
# 在自车坐标系下计算
cos_h = math.cos(-ego_heading)
sin_h = math.sin(-ego_heading)
rel_lane_x = (lane_center - ego_x) * cos_h - (0 - ego_y) * sin_h
# 只返回自车所在及相邻车道
if abs(rel_lane_x) < 6.0: # 6米外不关注
lane_info = LaneInfo(
lane_id=lane_id,
center_x=lane_center,
left_boundary=lane_center - lane_width/2,
right_boundary=lane_center + lane_width/2,
curvature=lane_data.get('curvature', 0.0),
confidence=0.95
)
lanes.append(lane_info)
return sorted(lanes, key=lambda l: l.center_x)
def detect_shoulder(
self,
ego_vehicle: 'VehicleDynamics',
road_layout: dict
) -> RoadShoulder:
"""
检测路肩
Args:
ego_vehicle: 自车状态
road_layout: 道路布局数据
Returns:
路肩信息
"""
# 获取路肩宽度配置
left_shoulder = road_layout.get('left_shoulder_width', 0.0)
right_shoulder = road_layout.get('right_shoulder_width', 0.0)
# 评估路肩质量
def evaluate_shoulder_quality(width: float) -> float:
if width < 0.5:
return 0.0
elif width < 1.0:
return 0.3
elif width < 1.5:
return 0.6
else:
return 0.9
return RoadShoulder(
left_shoulder_width=left_shoulder,
right_shoulder_width=right_shoulder,
shoulder_quality=min(
evaluate_shoulder_quality(left_shoulder),
evaluate_shoulder_quality(right_shoulder)
)
)class EmergencyDetector: """ 紧急状况检测器
ini
核心功能：
1. 计算碰撞时间(TTC)
2. 评估危险等级
3. 识别可避让空间
"""
def __init__(self, safety_config: 'SafetyConfig' = None):
"""
初始化紧急检测器
Args:
safety_config: 安全配置参数
"""
self.safety_config = safety_config or SafetyConfig()
def calculate_ttc(
self,
ego_vehicle: 'VehicleDynamics',
detected_object: DetectedObject
) -> float:
"""
计算碰撞时间(Time to Collision)
使用相对运动学计算两车相撞所需时间
公式推导：
- 相对位置: P_rel = [rel_x, rel_y]
- 相对速度: V_rel = [rel_vx, rel_vy]
- 若V_rel ≠ 0，则 TTC = - (P_rel · V_rel) / |V_rel|²
当且仅当相对速度与相对位置的夹角<90°且最近距离小于阈值
Args:
ego_vehicle: 自车状态
detected_object: 检测到的物体
Returns:
TTC值（秒），-1表示不会碰撞
"""
# 获取相对位置和速度
rel_x, rel_y = detected_object.x - ego_vehicle.x, detected_object.y - ego_vehicle.y
rel_vx, rel_vy = detected_object.vx, detected_object.vy
# 相对速度大小
rel_speed_sq = rel_vx**2 + rel_vy**2
if rel_speed_sq < 0.01: # 相对静止
return -1.0
# 计算TTC
dot_product = rel_x * rel_vx + rel_y * rel_vy
if dot_product >= 0:
# 物体在远离或横向移动，不会碰撞
return -1.0
ttc = -dot_product / rel_speed_sq
# 验证碰撞可能性（检查最近距离）
closest_distance = self._calculate_closest_distance(
rel_x, rel_y, rel_vx, rel_vy, ttc
)
# 考虑车辆尺寸的安全距离
safety_margin = (ego_vehicle.length + detected_object.length) / 2 + 2.0
if closest_distance > safety_margin:利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！