狭窄车位检测与自动垂直泊车路径规划混合A~*【附代码】

✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导，毕业论文、期刊论文经验交流。
✅ 专业定制毕设、代码
✅如需沟通交流，查看文章底部二维码

（1）基于激光雷达‑视觉空间几何法的融合车位检测：

首先对激光雷达点云进行地面分割，采用基于栅格高度差的分割方法去除地面点，对剩余障碍物点云做欧氏聚类获得车辆轮廓。同时，利用YOLOv5s视觉模型检测两侧停放车辆的目标框，提取车轮接地点和车身边缘线。在决策层，基于加权平均法将两传感器的检测结果进行融合，激光雷达提供精确的距离和方位角，视觉提供语义类别和侧面轮廓，权重依据传感器的有效检测范围和置信度动态调整。在输出层，通过空间几何方法计算目标车辆的角点位置，进而运用最小外接矩形拟合和扩展卡尔曼滤波定位自车位置，解算出车位长度、宽度和倾角。在白天和黄昏场景下的实车试验中，融合检测方法对狭窄车位长度的平均误差分别为0.0076 m和0.0091 m，检测准确率分别达到99.3%和99.2%，为高精度的泊车规划提供了可靠的环境模型。

（2）逆向扩展混合A*与起始点泊车区域优选：

提出一种逆向着道的混合A*路径规划方法，将规划起点设定为目标车位内部，目标点设定为泊车通道的某个等待位置，利用混合A*在逆向空间中搜索路径，之后反转节点序列得到正向泊车轨迹。该方式使得路径扩展从狭窄空间向宽敞空间生长，显著降低了在狭窄车位内部进行复杂前向搜索的计算量。在节点扩展时，状态空间维度为(x, y, θ)，动作空间为7种固定曲率的弧线，包括最大转向角内的均匀离散曲率和两段直线。为了进一步提高成功率，基于车位的宽度和通道尺寸构建起始区域可行性评估函数，以车位宽度减去车宽1.2倍作为边界，筛选出最优的泊车发起位置。100次随机垂直车位测试下，逆向混合A*的平均规划时间为0.38秒，路径长度比传统前向混合A*缩短15.6%，成功率从71%提升至93%，在5.0米宽狭窄车位中表现尤为突出。

（3）模型预测控制与单步多步连续位姿跟踪：

设计了一个基于运动学模型的模型预测控制器，用来跟踪逆向混合A*生成的泊车参考轨迹。预测时域设为40步，每步0.05秒，状态量为(x, y, θ)，控制量为速度和前轮转角的速度变化率。为了应对狭窄空间中的位姿误差累积，在MPC的成本函数中增加了相对于车位边界的软约束项，通过指数函数对接近边界的预测位姿施加高额惩罚。此外，采用双线性插值在轨迹节点之间进行平滑，并引入末端代价，确保车辆最终位姿的横纵向误差均小于0.03 m。在Carsim‑Simulink联合仿真中，针对单步泊车（一次性倒入）和多步泊车（揉库）分别进行验证，单步泊车最大横向误差0.021 m，航向角误差0.017 rad；多步泊车最大横向误差0.024 m，航向角误差0.022 rad，完全满足泊车精度的工程要求。"

"import numpy as np

import cvxpy as cp

# 逆向混合A*状态扩展（简化）

def hybrid_astar_expand(state, goal, obstacles):

actions = [(-0.5, 5.0), (0.0, 5.0), (0.5, 5.0)] # (曲率, 弧长)

successors = []

for k, l in actions:

x, y, theta = state

if k != 0:

r = 1.0/abs(k)

cx = x - np.sin(theta)*r; cy = y + np.cos(theta)*r

dtheta = l/r

x_new = cx + np.sin(theta+dtheta)*r

y_new = cy - np.cos(theta+dtheta)*r

theta_new = theta + dtheta

else:

x_new = x + l*np.cos(theta); y_new = y + l*np.sin(theta)

theta_new = theta

if not collision((x_new, y_new), obstacles):

successors.append((x_new, y_new, theta_new))

return successors

# 空间几何车位检测

def detect_parking_slot(vehicle_corners, self_pose):

# vehicle_corners: 两侧车辆角点

left_car = vehicle_corners[0]; right_car = vehicle_corners[1]

# 计算车位宽度和深度

width = np.linalg.norm(np.array(right_car[0])-np.array(left_car[1]))

depth = np.linalg.norm(np.array(left_car[0])-np.array(left_car[3]))

# 转换到自车坐标系

rel_angle = np.arctan2(right_car[0][1]-left_car[1][1], right_car[0][0]-left_car[1][0])

return width, depth, rel_angle

# MPC跟踪控制器

def mpc_control(x_ref, current_state, dt, N=40):

n_state = 3; n_ctrl = 2

x = cp.Variable((n_state, N+1)); u = cp.Variable((n_ctrl, N))

cost = 0; constraints = [x[:,0] == current_state]

for k in range(N):

# 运动学模型约束

A = np.eye(3); B = np.diag([dt, dt, dt]) # 简化

constraints += [x[:,k+1] == A@x[:,k] + B@u[:,k]]

cost += cp.sum_squares(x[:,k+1] - x_ref[:,k+1]) + 0.1*cp.sum_squares(u[:,k])

prob = cp.Problem(cp.Minimize(cost), constraints)

prob.solve()

return u[:,0].value

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇