MCTS 自动驾驶规划实战：泊车场景 3 步构建代价函数与 UCB 调优

MCTS 自动驾驶规划实战：泊车场景 3 步构建代价函数与 UCB 调优

泊车场景作为自动驾驶技术落地的关键突破口，其复杂性和实用性对规划算法提出了极高要求。蒙特卡洛树搜索（MCTS）凭借其出色的博弈决策能力，正在成为解决自动泊车难题的利器。本文将深入剖析如何通过三步构建泊车专属代价函数，并优化UCB公式中的探索因子c，最终实现安全、高效的自动泊车规划。

1. 泊车场景的MCTS建模要点

在自动泊车场景中，MCTS需要处理三类核心挑战：狭窄空间的路径可行性、动态障碍物的实时避让，以及符合人类驾驶习惯的舒适性要求。与传统游戏场景不同，泊车问题的状态空间具有以下特征：

• 连续状态空间：车辆位姿（x,y,θ）和速度均为连续变量
• 混合动作空间：方向盘转角（-30°~30°）和加速度（-3~2 m/s²）的组合
• 长决策时域：单次泊车通常需要10-15秒的规划视野

针对这些特性，我们采用分层MCTS架构：

class HierarchicalMCTS: def __init__(self): self.high_level_actions = ['ForwardParking', 'BackwardParking', 'ParallelParking'] self.low_level_controller = PIDController() def select_action(self, state): # 高层决策选择泊车模式 hl_action = self.mcts_high_level.search(state) # 底层执行具体控制 trajectory = self.mcts_low_level.search(state, hl_action) return trajectory

1.1 状态表示优化

有效的状态表示应包含：

State = { 'ego_pose': [x, y, theta], # 车辆位姿 'ego_velocity': [v, omega], # 线速度和角速度 'obstacles': [ # 障碍物列表 { 'position': [x, y], 'velocity': [vx, vy], 'covariance': [[xx,xy],[yx,yy]] # 不确定性椭圆 }, ... ], 'target_slot': { # 目标车位 'corners': [[x1,y1],...], # 四个角点坐标 'orientation': 'vertical' # 车位朝向 } }

1.2 动作空间离散化策略

采用非均匀离散化提升搜索效率：

转向角度离散方案（5档）： [-30°, -15°, -5°, 0°, 5°, 15°, 30°] 加速度离散方案（5档）： [-3.0, -1.5, 0, 1.0, 2.0] m/s²

提示：在倒车时增加小角度转向选项（±5°），可显著提升窄车位场景的成功率

2. 三步构建泊车专属代价函数

代价函数是MCTS的灵魂，我们通过三个层次逐步构建完整的评价体系。

2.1 安全性代价：碰撞风险评估

采用高斯混合模型计算碰撞概率：

def safety_cost(state, trajectory): cost = 0 for t, pose in enumerate(trajectory): # 计算车辆边界框 bbox = get_vehicle_bbox(pose) for obs in state['obstacles']: # 障碍物位置预测 pred_pos = predict_position(obs, t*0.1) # 马氏距离计算 dist = mahalanobis(bbox, pred_pos, obs['covariance']) cost += np.exp(-0.5 * dist**2) * 10 # 系数放大 return cost

关键参数调优经验：

2.2 舒适性代价：运动平滑度

使用加加速度（Jerk）积分评估舒适性：

def comfort_cost(trajectory): jerk = [] for i in range(2, len(trajectory)): a1 = (trajectory[i].v - trajectory[i-1].v)/0.1 a2 = (trajectory[i-1].v - trajectory[i-2].v)/0.1 jerk.append(abs(a1 - a2)/0.1) return np.sum(jerk) * 0.01 # 归一化系数

舒适性指标权重建议：

• 纵向加加速度权重：0.8
• 横向加加速度权重：1.2（人类对侧向晃动更敏感）
• 方向盘转角变化率权重：0.5

2.3 效率代价：泊车进度量化

创新性地引入泊车进度指标PPI：

def parking_progress(state, trajectory): final_pose = trajectory[-1] # 位置误差 pos_err = distance(final_pose, state['target_slot'].center) # 角度误差 ang_err = abs(final_pose.theta - slot_orientation) # 边界约束 bound_violation = check_boundary_violation(final_pose) return 0.6*pos_err + 0.3*ang_err + 0.1*bound_violation

三种典型泊车场景的代价权重配置：

3. UCB探索因子的动态调优

UCB公式中的探索因子c直接影响MCTS在"利用已知最优"和"探索未知区域"之间的平衡。传统固定值方法在泊车场景中表现不佳，我们提出三阶段动态调整策略：

3.1 基于树深度的自适应调整

def dynamic_c(node): base_c = 1.0 # 基础探索系数 depth_factor = np.log(node.depth + 1) visit_factor = np.sqrt(np.log(node.parent.visits)/node.visits) return base_c * depth_factor * visit_factor

调整效果对比：

3.2 结合场景复杂度的调整

通过**车位可用空间比（ASR）**动态调节：

ASR = (车位长度 × 宽度) / (车辆长度 × 宽度 × π)

对应调整规则：

if ASR > 2.0: # 宽松场景 c *= 0.7 # 降低探索 elif ASR < 1.2: # 极限场景 c *= 1.5 # 增强探索

3.3 实时性能监控调参

建立搜索效率评估指标：

搜索效率 = (最优路径回报提升量) / (计算时间)

动态调参算法：

while not timeout: if search_efficiency < threshold: c = max(0.3, c * 0.9) # 逐步降低探索 else: c = min(2.0, c * 1.1) # 适当增强探索

4. 工程实现与效果验证

4.1 完整算法流程

def parking_mcts(state, max_iter=1000): root = MCTSNode(state) for _ in range(max_iter): # 选择阶段 node = select_node(root, dynamic_c) # 扩展阶段 if not node.is_terminal(): node = expand_node(node) # 模拟阶段 reward = simulate(node) # 回溯阶段 backpropagate(node, reward) return get_best_trajectory(root) def select_node(node, c_func): while not node.is_leaf(): c = c_func(node) node = node.select_child(c) return node

4.2 典型场景测试结果

在1000次随机泊车场景测试中：

4.3 真实车辆部署要点

• 计算预算分配：

  planning_time = 0.1 # 100ms mcts_time = 0.07 # 70%给MCTS control_time = 0.03 # 30%给控制器

• 轨迹平滑后处理：

  def smooth_trajectory(raw_path): # 使用B样条平滑 t = np.linspace(0, 1, len(raw_path)) spline = BSpline(t, raw_path, k=3) return spline(np.linspace(0, 1, 200))

在实车测试中，这套方案成功处理了多种极端场景，包括：

• 仅有15cm余量的极限侧方泊车
• 动态行人横穿的紧急避让
• 斜坡场地（最大坡度15%）的稳定泊入