智能车库自动泊车系统
一、实际应用场景描述
某商业综合体地下停车场，高峰期车位紧张，传统人工寻找车位耗时耗力。现需开发一套智能系统，实现：
- 车辆进入车库后自动巡航
- 实时检测并识别空闲车位
- 规划最优路径自动泊入
- 记录车位坐标信息
系统运行环境：
- 硬件：配备激光雷达+摄像头的自动驾驶测试车
- 软件：Python 3.8+，ROS Noetic，OpenCV 4.5
- 场景：100m×50m标准地下车库，含200个标准车位
二、引入痛点
- 效率低下：传统寻位平均耗时8-12分钟/次
- 资源浪费：30%燃油消耗在寻位过程
- 体验差：驾驶员需全程专注，易产生疲劳
- 管理难：无法实时监控车位状态
- 安全隐患：狭窄空间易发生刮擦事故
三、核心逻辑讲解
graph TD A[车辆入库] --> B(环境感知层) B --> C{车位检测} C -->|有空位| D[路径规划] C -->|无空位| E[继续巡航] D --> F[运动控制] F --> G[自动泊车] G --> H[记录坐标] H --> I[任务完成]
关键技术模块
- 车位检测：基于YOLOv5的车位占用识别
- SLAM定位：Cartographer算法构建地图
- 路径规划：A*算法+DWA动态窗口法
- 运动控制：PID轨迹跟踪控制器
四、代码模块化实现
项目结构
smart_parking_system/ ├── config/ │ └── parking_config.yaml ├── src/ │ ├── perception/ │ │ ├── init.py │ │ ├── lane_detector.py │ │ └── parking_spot_detector.py │ ├── localization/ │ │ ├── init.py │ │ └── slam_localizer.py │ ├── planning/ │ │ ├── init.py │ │ ├── path_planner.py │ │ └── trajectory_generator.py │ ├── control/ │ │ ├── init.py │ │ └── pid_controller.py │ └── main.py ├── README.md └── requirements.txt
核心代码实现
- 车位检测器 (perception/parking_spot_detector.py)
""" 车位检测模块 - 基于YOLOv5的智能车位识别 功能：实时检测车位占用状态，返回空闲车位坐标 """
import cv2 import numpy as np import torch from typing import List, Tuple, Dict
class ParkingSpotDetector: """智能车位检测器"""
def __init__(self, model_path: str = "models/yolov5s.pt", conf_threshold: float = 0.6):
"""
初始化检测器
Args:
model_path: YOLOv5模型路径
conf_threshold: 置信度阈值
"""
self.conf_threshold = conf_threshold
# 加载预训练模型
self.model = torch.hub.load('ultralytics/yolov5', 'custom', path=model_path)
self.model.conf = conf_threshold
# 车位尺寸参数（单位：米）
self.spot_width = 2.5
self.spot_height = 5.0
# 相机内参（示例值，需实际标定）
self.camera_matrix = np.array([
[800, 0, 640],
[0, 800, 360],
[0, 0, 1]
])
self.dist_coeffs = np.zeros((4, 1))
def detect_spots(self, frame: np.ndarray) -> List[Dict]:
"""
检测图像中的车位状态
Args:
frame: 输入图像帧(BGR格式)
Returns:
车位信息列表，每个元素包含：
{
'id': 车位编号,
'bbox': [x1,y1,x2,y2],
'occupied': True/False,
'confidence': 置信度,
'world_coords': (x,y,z)世界坐标
}
"""
results = self.model(frame)
detections = results.pandas().xyxy[0].to_dict(orient='records')
spots = []
spot_id = 0
for det in detections:
if det['name'] == 'parking_spot':
x1, y1, x2, y2 = int(det['xmin']), int(det['ymin']), \
int(det['xmax']), int(det['ymax'])
# 截取车位ROI区域进行细粒度分类
roi = frame[y1:y2, x1:x2]
occupied = self._check_occupancy(roi)
# 像素坐标转世界坐标
world_coord = self._pixel_to_world(x1 + (x2-x1)//2, y2)
spots.append({
'id': spot_id,
'bbox': [x1, y1, x2, y2],
'occupied': occupied,
'confidence': det['confidence'],
'world_coords': world_coord
})
spot_id += 1
return spots
def _check_occupancy(self, roi: np.ndarray, threshold: float = 0.15) -> bool:
"""
判断车位是否被占用
Args:
roi: 车位区域图像
threshold: 占用判定阈值
Returns:
True表示被占用，False表示空闲
"""
# 转换为灰度图并进行高斯模糊
gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
blurred = cv2.GaussianBlur(gray, (5, 5), 0)
# 边缘检测
edges = cv2.Canny(blurred, 50, 150)
# 计算边缘密度作为占用指标
edge_density = np.sum(edges > 0) / edges.size
# 添加颜色特征分析（红色代表占用物体）
hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
red_mask = cv2.inRange(hsv, (0, 70, 50), (10, 255, 255))
red_ratio = np.sum(red_mask > 0) / red_mask.size
# 综合判定
occupancy_score = edge_density * 0.7 + red_ratio * 0.3
return occupancy_score > threshold
def _pixel_to_world(self, pixel_x: int, pixel_y: int) -> Tuple[float, float, float]:
"""
将像素坐标转换为世界坐标
Args:
pixel_x: 像素x坐标
pixel_y: 像素y坐标
Returns:
(x, y, z)世界坐标系下的位置
"""
# 简化处理：实际应用中需结合深度信息
focal_length = self.camera_matrix[0, 0]
principal_point = self.camera_matrix[0, 2]
# 假设摄像头安装高度为1.5米，俯仰角15度
camera_height = 1.5
pitch_angle = np.radians(15)
# 计算距离
distance = (camera_height * focal_length) / (pixel_y - principal_point) / np.cos(pitch_angle)
# 计算世界坐标
world_x = (pixel_x - principal_point) * distance / focal_length
world_y = distance * np.sin(pitch_angle)
world_z = 0 # 地面高度
return (round(world_x, 2), round(world_y, 2), world_z)
def visualize_detections(self, frame: np.ndarray, spots: List[Dict]) -> np.ndarray:
"""
可视化检测结果
Args:
frame: 原始图像
spots: 车位检测结果
Returns:
标注后的图像
"""
output = frame.copy()
for spot in spots:
x1, y1, x2, y2 = spot['bbox']
color = (0, 0, 255) if spot['occupied'] else (0, 255, 0)
label = f"ID:{spot['id']} {'Occupied' if spot['occupied'] else 'Free'}"
cv2.rectangle(output, (x1, y1), (x2, y2), color, 2)
cv2.putText(output, label, (x1, y1-10),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
# 绘制世界坐标
wx, wy, wz = spot['world_coords']
coord_text = f"({wx:.1f}, {wy:.1f})m"
cv2.putText(output, coord_text, (x1, y2+20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 2)
return output
2. SLAM定位模块 (localization/slam_localizer.py)
""" SLAM定位模块 - 基于Cartographer的实时定位 功能：构建车库地图并实现车辆精确定位 """
import rospy import tf2_ros import numpy as np from geometry_msgs.msg import PoseStamped, TransformStamped from nav_msgs.msg import OccupancyGrid, Odometry
class SlamLocalizer: """SLAM定位器"""
def __init__(self):
"""初始化SLAM定位器"""
rospy.init_node('slam_localizer', anonymous=True)
# TF变换监听器
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# 发布器
self.pose_pub = rospy.Publisher('/vehicle_pose', PoseStamped, queue_size=10)
self.map_pub = rospy.Publisher('/parking_map', OccupancyGrid, queue_size=1)
# 车辆当前位姿
self.current_pose = None
self.current_velocity = np.zeros(3) # [vx, vy, vtheta]
# 车位数据库
self.parking_spots = {}
# 订阅里程计信息
rospy.Subscriber('/odom', Odometry, self.odom_callback)
rospy.loginfo("SLAM Localizer initialized")
def odom_callback(self, msg: Odometry):
"""
里程计回调函数
Args:
msg: 里程计消息
"""
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
# 四元数转欧拉角
euler = self._quaternion_to_euler(
orientation.x, orientation.y, orientation.z, orientation.w
)
self.current_pose = {
'x': position.x,
'y': position.y,
'theta': euler[2], # yaw角
'timestamp': rospy.Time.now().to_sec()
}
velocity = msg.twist.twist.linear
angular = msg.twist.twist.angular
self.current_velocity = [velocity.x, velocity.y, angular.z]
# 发布当前位姿
self._publish_pose()
def _quaternion_to_euler(self, x: float, y: float, z: float, w: float) -> Tuple[float, float, float]:
"""
四元数转欧拉角
Returns:
(roll, pitch, yaw) 弧度制
"""
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll = np.arctan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch = np.arcsin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw = np.arctan2(t3, t4)
return (roll, pitch, yaw)
def _publish_pose(self):
"""发布车辆位姿"""
if self.current_pose is None:
return
pose_msg = PoseStamped()
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = "map"
pose_msg.pose.position.x = self.current_pose['x']
pose_msg.pose.position.y = self.current_pose['y']
pose_msg.pose.position.z = 0.0
# 欧拉角转四元数
q = self._euler_to_quaternion(0, 0, self.current_pose['theta'])
pose_msg.pose.orientation.x = q[0]
pose_msg.pose.orientation.y = q[1]
pose_msg.pose.orientation.z = q[2]
pose_msg.pose.orientation.w = q[3]
self.pose_pub.publish(pose_msg)
def _euler_to_quaternion(self, roll: float, pitch: float, yaw: float) -> Tuple[float, float, float, float]:
"""
欧拉角转四元数
Returns:
(x, y, z, w) 四元数
"""
cy = np.cos(yaw * 0.5)
sy = np.sin(yaw * 0.5)
cp = np.cos(pitch * 0.5)
sp = np.sin(pitch * 0.5)
cr = np.cos(roll * 0.5)
sr = np.sin(roll * 0.5)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
return (x, y, z, w)
def get_current_pose(self) -> Dict:
"""
获取当前车辆位姿
Returns:
位姿字典 {'x': float, 'y': float, 'theta': float}
"""
return self.current_pose
def update_parking_spots(self, spots: List[Dict]):
"""
更新车位数据库
Args:
spots: 车位检测结果列表
"""
for spot in spots:
spot_id = spot['id']
world_coords = spot['world_coords']
self.parking_spots[spot_id] = {
'coordinates': world_coords,
'occupied': spot['occupied'],
'last_update': rospy.Time.now().to_sec(),
'bbox': spot['bbox']
}
rospy.loginfo(f"Updated parking spot database: {len(self.parking_spots)} spots")
def get_free_spots(self) -> List[Dict]:
"""
获取所有空闲车位
Returns:
空闲车位列表
"""
free_spots = [
{'id': sid, **data}
for sid, data in self.parking_spots.items()
if not data['occupied']
]
return sorted(free_spots, key=lambda x: x['coordinates'][0]) # 按x坐标排序
def find_nearest_free_spot(self) -> Dict:
"""
找到最近的空闲车位
Returns:
最近空闲车位信息，若无则返回None
"""
free_spots = self.get_free_spots()
if not free_spots:
return None
current_pose = self.get_current_pose()
if current_pose is None:
return free_spots[0]
# 计算距离
def distance(spot):
dx = spot['coordinates'][0] - current_pose['x']
dy = spot['coordinates'][1] - current_pose['y']
return np.sqrt(dx**2 + dy**2)
nearest = min(free_spots, key=distance)
return nearest
3. 路径规划模块 (planning/path_planner.py)
""" 路径规划模块 - A*算法+DWA动态窗口法 功能：规划从当前位置到目标车位的最优路径 """
import heapq import numpy as np from typing import List, Tuple, Optional from dataclasses import dataclass
@dataclass class Node: """路径搜索节点""" x: float y: float theta: float # 航向角 g: float = 0 # 实际代价 h: float = 0 # 启发式代价 parent: 'Node' = None
@property
def f(self) -> float:
"""总代价"""
return self.g + self.h
def __lt__(self, other):
return self.f < other.f
class PathPlanner: """智能路径规划器"""
def __init__(self, map_resolution: float = 0.1, grid_size: int = 500):
"""
初始化路径规划器
Args:
map_resolution: 地图分辨率(m/格)
grid_size: 网格大小
"""
self.resolution = map_resolution
self.grid_size = grid_size
# 车辆参数
self.vehicle_length = 4.5 # 车长(m)
self.vehicle_width = 2.0 # 车宽(m)
self.wheel_base = 2.8 # 轴距(m)
# 运动约束
self.max_speed = 2.0 # 最大速度(m/s)
self.max_accel = 1.0 # 最大加速度(m/s²)
self.max_steer = np.radians(30) # 最大转向角(rad)
# 障碍物地图
self.obstacle_map = np.zeros((grid_size, grid_size), dtype=np.uint8)
def set_obstacles(self, obstacles: List[Tuple[float, float, float, float]]):
"""
设置障碍物（矩形区域）
Args:
obstacles: [(x1, y1, x2, y2), ...] 障碍物边界框
"""
self.obstacle_map.fill(0)
for obs in obstacles:
x1, y1, x2, y2 = obs
# 转换为网格坐标
gx1 = int((x1 / self.resolution) + self.grid_size // 2)
gy1 = int((y1 / self.resolution) + self.grid_size // 2)
gx2 = int((x2 / self.resolution) + self.grid_size // 2)
gy2 = int((y2 / self.resolution) + self.grid_size // 2)
# 确保在边界内
gx1 = max(0, min(gx1, self.grid_size-1))
gy1 = max(0, min(gy1, self.grid_size-1))
gx2 = max(0, min(gx2, self.grid_size-1))
gy2 = max(0, min(gy2, self.grid_size-1))
self.obstacle_map[gy1:gy2+1, gx1:gx2+1] = 1
def world_to_grid(self, x: float, y: float) -> Tuple[int, int]:
"""
世界坐标转网格坐标
Returns:
(grid_x, grid_y)
"""
gx = int((x / self.resolution) + self.grid_size // 2)
gy = int((y / self.resolution) + self.grid_size // 2)
return gx, gy
def grid_to_world(self, gx: int, gy: int) -> Tuple[float, float]:
"""
网格坐标转世界坐标
Returns:
(x, y)
"""
x = (gx - self.grid_size // 2) * self.resolution
y = (gy - self.grid_size // 2) * self.resolution
return x, y
def heuristic(self, node: Node, goal: Node) -> float:
"""
启发式函数 - 欧氏距离
Returns:
估计代价
"""
return np.sqrt((node.x - goal.x)**2 + (node.y - goal.y)**2)
def is_collision(self, x: float, y: float, theta: float) -> bool:
"""
检查位姿是否碰撞
Args:
x, y: 位置
theta: 航向角
Returns:
True表示碰撞
"""
# 车辆四个角的相对位置
corners_rel = [
(-self.vehicle_length/2, -self.vehicle_width/2),
(self.vehicle_length/2, -self.vehicle_width/2),
(self.vehicle_length/2, self.vehicle_width/2),
(-self.vehicle_length/2, self.vehicle_width/2)
]
cos_t = np.cos(theta)
sin_t = np.sin(theta)
for cx, cy in corners_rel:
# 旋转变换
rx = cx * cos_t - cy * sin_t
ry = cx * sin_t + cy * cos_t
# 世界坐标
wx = x + rx
wy = y + ry
# 网格坐标
gx, gy = self.world_to_grid(wx, wy)
# 边界检查
if gx < 0 or gx >= self.grid_size or gy < 0 or gy >= self.grid_size:
return True
if self.obstacle_map[gy, gx] == 1:
return True
return False
def astar_search(self, start: Tuple[float, float, float],
goal: Tuple[float, float, float]) -> Optional[List[Tuple[float, float, float]]]:
"""
A*路径搜索算法
Args:
start: (x, y, theta) 起点
goal: (x, y, theta) 终点
Returns:
路径点列表 [(x1,y1,θ1), (x2,y2,θ2), ...]，失败返回None
"""
start_node = Node(start[0], start[1], start[2])
goal_node = Node(goal[0], goal[1], goal[2])
open_set = []
heapq.heappush(open_set, start_node)
closed_set = set()
# 离散化角度用于哈希
angle_res = np.radians(15)
while open_set:
current = heapq.heappop(open_set)
# 到达目标
if self._is_goal_reached(current, goal_node):
return self._reconstruct_path(current)
# 角度离散化
angle_key = int(np.round(current.theta / angle_res)) % (int(2*np.pi/angle_res))
state_key = (int(current.x/self.resolution),
int(current.y/self.resolution),
angle_key)
if state_key in closed_set:
continue
closed_set.add(state_key)
# 扩展邻居节点
for neighbor in self._get_neighbors(current):
n_angle_key = int(np.round(neighbor.theta / angle_res)) % (int(2*np.pi/angle_res))
n_state_key = (int(neighbor.x/self.resolution),
int(neighbor.y/self.resolution),
n_angle_key)
if n_state_key in closed_set:
continue
if self.is_collision(neighbor.x, neighbor.y, neighbor.theta):
continue
neighbor.h = self.heuristic(neighbor, goal_node)
heapq.heappush(open_set, neighbor)
return None # 无可行路径
def _is_goal_reached(self, current: Node, goal: Node, tolerance: float = 0.5) -> bool:
"""
判断是否到达目标
Returns:
True表示到达
"""
dist = np.sqrt((current.x - goal.x)**2 + (current.y - goal.y)**2)
angle_diff = abs(self._normalize_angle(current.theta - goal.theta))
return dist < tolerance and angle_diff < np.radians(10)
def _normalize_angle(self, angle: float) -> float:
"""角度归一化到[-π, π]"""
while angle > np.pi:
angle -= 2 * np.pi
while angle < -np.pi:
angle += 2 * np.pi
return angle
def _get_neighbors(self, node: Node) -> List[Node]:
"""
获取相邻节点
Returns:
相邻节点列表
"""
neighbors = []
# 控制输入
dt = 0.5 # 时间步长
dtheta_list = [-self.max_steer, 0, self.max_steer]
speed_list = [0, self.max_speed * 0.5, self.max_speed]
for dtheta in dtheta_list:
for speed in speed_list:
if speed == 0 and dtheta == 0:
continue
# 运动学模型
new_theta = self._normalize_angle(node.theta + dtheta)
new_x = node.x + speed * np.cos(new_theta) * dt
new_y = node.y + speed * np.sin(new_theta) * dt
# 计算代价
move_cost = speed * dt
steer_cost = abs(dtheta) * 0.1
new_g = node.g + move_cost + steer_cost
neighbor = Node(new_x, new_y, new_theta, g=new_g, parent=node)
neighbors.appe
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
