MCTS 自动驾驶泊车规划实战Python 实现 4 步核心算法与碰撞检测泊车场景是自动驾驶技术中最具挑战性的场景之一。狭窄的空间、复杂的障碍物分布以及对精确控制的严苛要求使得传统规划算法往往难以胜任。蒙特卡洛树搜索MCTS凭借其出色的探索-利用平衡能力成为解决这一难题的有力工具。本文将带你从零实现一个完整的MCTS泊车规划器涵盖算法核心四步骤和关键的碰撞检测模块。1. 环境搭建与问题建模在开始编码前我们需要明确泊车问题的数学表达。假设车辆位于一个二维平面状态可以表示为s(x,y,θ)其中(x,y)是后轴中心坐标θ是航向角。控制输入u(δ,d)δ是前轮转角d是行驶距离正数前进负数后退。首先安装必要依赖pip install numpy matplotlib shapely定义车辆运动学模型自行车模型import numpy as np class VehicleModel: def __init__(self, wheelbase2.8): self.wheelbase wheelbase # 轴距 def move(self, state, control): 状态转移函数 Args: state: (x,y,theta) control: (steer_angle, distance) Returns: new_state: (x,y,theta) x, y, theta state delta, d control if abs(delta) 1e-5: # 直行特殊情况 new_x x d * np.cos(theta) new_y y d * np.sin(theta) new_theta theta else: radius self.wheelbase / np.tan(delta) beta d / radius new_x x radius * (np.sin(theta beta) - np.sin(theta)) new_y y - radius * (np.cos(theta beta) - np.cos(theta)) new_theta (theta beta) % (2*np.pi) return np.array([new_x, new_y, new_theta])2. MCTS 核心四步实现MCTS通过四个阶段的循环迭代逐步构建搜索树2.1 节点选择Selection使用UCTUpper Confidence Bound for Trees算法平衡探索与利用class MCTSNode: def __init__(self, state, parentNone): self.state state # 车辆状态 self.parent parent # 父节点 self.children [] # 子节点列表 self.visits 0 # 访问次数 self.total_reward 0 # 累计奖励 self.untried_actions self.get_possible_actions() # 未尝试动作 def uct_value(self, exploration_weight1.4): 计算UCT值 if self.visits 0: return float(inf) return (self.total_reward / self.visits) exploration_weight * np.sqrt(np.log(self.parent.visits) / self.visits) def select_child(self): 选择UCT值最大的子节点 return max(self.children, keylambda c: c.uct_value())2.2 节点扩展Expansion当遇到未完全探索的节点时进行扩展def expand(self): 扩展新节点 action self.untried_actions.pop() next_state self.env.move(self.state, action) child_node MCTSNode(next_state, parentself) self.children.append(child_node) return child_node2.3 模拟Simulation从新节点开始进行随机rolloutdef simulate(self, max_depth50): 随机模拟直到终止条件 current_state self.state.copy() reward 0 depth 0 while depth max_depth: if self.env.is_terminal(current_state): reward self.env.calculate_reward(current_state) break action self.random_action() current_state self.env.move(current_state, action) depth 1 return reward2.4 反向传播Backpropagation将模拟结果反向传播更新路径上的节点统计量def backpropagate(self, reward): 反向传播奖励值 self.visits 1 self.total_reward reward if self.parent: self.parent.backpropagate(reward)3. 碰撞检测模块实现精确的碰撞检测是泊车规划的安全保障。我们使用Shapely库进行几何碰撞检测from shapely.geometry import Polygon, Point class CollisionChecker: def __init__(self, obstacles): Args: obstacles: 障碍物列表每个障碍物用多边形表示 self.obstacles [Polygon(obs) for obs in obstacles] def get_vehicle_shape(self, state): 获取车辆占据区域矩形近似 x, y, theta state # 车辆轮廓四个角点相对于后轴中心 corners_local np.array([ [3.0, 0.8], # 前右 [3.0, -0.8], # 前左 [-1.0, -0.8], # 后左 [-1.0, 0.8] # 后右 ]) # 坐标变换到全局坐标系 rotation np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)] ]) corners_global np.dot(corners_local, rotation.T) [x, y] return Polygon(corners_global) def check_collision(self, state): 检查给定状态是否碰撞 vehicle self.get_vehicle_shape(state) return any(vehicle.intersects(obs) for obs in self.obstacles)4. 完整算法集成与优化将各模块整合成完整系统并添加并行化等优化class MCTSParkingPlanner: def __init__(self, env, iterations1000, max_depth20): self.env env self.iterations iterations self.max_depth max_depth def plan(self, initial_state): root MCTSNode(initial_state) for _ in range(self.iterations): node root # 1. Selection while node.untried_actions [] and node.children ! []: node node.select_child() # 2. Expansion if node.untried_actions ! []: node node.expand() # 3. Simulation reward node.simulate(max_depthself.max_depth) # 4. Backpropagation node.backpropagate(reward) # 返回访问次数最多的动作 best_child max(root.children, keylambda c: c.visits) return self.env.get_action(root.state, best_child.state)性能优化技巧动作空间离散化将连续控制空间离散为有限动作集def get_possible_actions(self): 生成离散动作空间 steer_angles np.linspace(-np.pi/4, np.pi/4, 5) # 5个转向角度 distances [-1.0, 1.0] # 前进或后退 return [(δ, d) for δ in steer_angles for d in distances]启发式奖励函数设计综合考虑距离、朝向和安全的奖励def calculate_reward(self, state): 计算状态奖励 # 目标距离惩罚 dist_penalty 0.1 * np.linalg.norm(state[:2] - self.goal[:2]) # 朝向偏差惩罚 angle_diff min(abs(state[2] - self.goal[2]), 2*np.pi - abs(state[2] - self.goal[2])) angle_penalty 0.05 * angle_diff # 碰撞惩罚 collision_penalty 10.0 if self.collision_checker.check_collision(state) else 0 # 到达目标奖励 goal_reward 100.0 if self.is_terminal(state) else 0 return goal_reward - dist_penalty - angle_penalty - collision_penalty并行化模拟使用多进程加速rolloutfrom concurrent.futures import ProcessPoolExecutor def parallel_simulate(self, node, n_simulations10): 并行执行多个模拟 with ProcessPoolExecutor() as executor: futures [executor.submit(node.simulate) for _ in range(n_simulations)] rewards [f.result() for f in futures] return np.mean(rewards)5. 可视化与调试可视化是算法调试的重要工具我们使用matplotlib实现def plot_scenario(self, stateNone, pathNone): 绘制泊车场景 plt.figure(figsize(10, 6)) # 绘制障碍物 for obs in self.obstacles: plt.fill(*zip(*obs), r, alpha0.5) # 绘制停车位 plt.plot([self.goal[0]-2, self.goal[0]2], [self.goal[1], self.goal[1]], g--, linewidth2) # 绘制车辆 if state is not None: vehicle self.collision_checker.get_vehicle_shape(state) plt.fill(*zip(*vehicle.exterior.coords), b, alpha0.6) # 绘制路径 if path is not None: xs, ys zip(*[(s[0], s[1]) for s in path]) plt.plot(xs, ys, b-o, markersize3) plt.axis(equal) plt.grid(True) plt.title(Autonomous Parking Scenario) plt.show()典型调试技巧包括检查搜索树扩展是否合理验证奖励函数权重设置分析碰撞检测准确性监控算法收敛速度6. 实战案例垂直泊车让我们看一个具体案例。假设停车位位于(10,5)车辆初始位置在(5,5)# 创建环境 obstacles [ [(8,3), (8,7), (12,7), (12,3)], # 右侧车辆 [(6,2), (6,4), (4,4), (4,2)] # 左侧障碍物 ] env ParkingEnv(goal[10,5,np.pi/2], obstaclesobstacles) # 初始状态 init_state np.array([5, 5, 0]) # 运行规划器 planner MCTSParkingPlanner(env, iterations2000) path [init_state] current_state init_state.copy() for _ in range(50): # 最大步数 action planner.plan(current_state) current_state env.move(current_state, action) path.append(current_state) if env.is_terminal(current_state): break # 可视化结果 env.plot_scenario(pathpath)这个实现展示了MCTS如何通过不断探索和利用逐步找到最优泊车路径。在实际应用中你可能需要根据具体场景调整参数如迭代次数、探索权重等。完整代码还需要添加异常处理、日志记录等工程化组件才能用于实际系统。