1. 项目概述一次从数据到决策的实战演练2022年的MathorCup高校数学建模挑战赛B题题目是“自动泊车系统的路径规划与决策”。这个题目一出来当时就在我们参赛圈子里引起了不小的讨论。它不像一些纯理论推导的题目而是把一个非常前沿且接地气的工业问题——自动泊车直接抛给了我们。核心任务很明确给你一个包含静态障碍物比如柱子、墙和动态障碍物比如其他正在移动的车辆的停车场环境你需要为一辆具备完整运动学模型的小车规划出一条从起点到指定泊车位的安全、高效路径并且要做出合理的决策比如何时等待、何时超车、何时调整姿态。这不仅仅是一道数学题它是一次完整的“数据建模-算法设计-仿真验证”的微型项目实战。你需要处理环境感知信息离散化的栅格地图、理解车辆的运动约束阿克曼转向几何、设计搜索或优化算法来寻找路径最后还要用合理的指标评价你的方案。对于参赛学生而言这是一个绝佳的机会将控制理论、运筹学、计算机科学和人工智能的知识进行融合应用。我作为当年的参与者和后来的指导者复盘这道题发现其价值远超一次比赛它清晰地勾勒出了一个经典机器人路径规划问题的全貌非常值得深入拆解。无论你是正在备赛的同学还是对机器人、自动驾驶算法感兴趣的开发者理解这道题的解题思路都能为你打开一扇通往智能决策系统的大门。2. 核心问题拆解与建模思路面对“自动泊车路径规划”这样一个复杂问题直接上手编码是行不通的。第一步也是最重要的一步是将模糊的自然语言描述转化为精确的、可计算的数学模型。这需要我们像剥洋葱一样逐层分解问题。2.1 环境表示从连续世界到离散可计算空间真实的停车场是连续的但计算机擅长处理离散问题。因此我们首先要对环境进行“数字化”。B题通常提供停车场平面图、障碍物位置等信息。最常用且有效的方法是栅格法。我们将整个停车场区域划分为均匀的网格比如10cm×10cm一个格子。每个格子有一个状态0自由可通行、1静态障碍物、或者一个概率值表示存在动态障碍物的可能性。这样连续的车辆位置x, y就可以近似为某个栅格的中心坐标。这种方法的优势在于直观且与许多搜索算法如A*天然契合。但缺点也很明显精度与计算开销的矛盾。格子划分太细搜索空间爆炸格子太粗规划出的路径可能“擦着”障碍物过去实际无法执行。注意在建模时必须进行“膨胀”处理。即不仅将障碍物本身占用的栅格标记为不可通行还要将其周围一定距离至少为车辆外接圆半径的栅格也标记为不可通行。这相当于为车辆增加了一个安全边界是保证路径物理可行性的关键一步在实际机器人应用中也是标准操作。2.2 车辆模型阿克曼转向与运动约束你不能把车当成一个可以任意方向移动的点。小车的转向是“阿克曼”几何模型这意味着它的转弯半径是有限的不能原地掉头轨迹是连续的圆弧和直线的组合。在数学上我们常用刚体运动学来描述。一个简化的状态量可以定义为 (x, y, θ)即位置和航向角。控制量通常是 (v, φ)即速度和前轮转角。其运动方程是非线性的。在路径规划中我们常常需要基于这个模型去生成或验证路径的可行性。例如RRT快速探索随机树算法在扩展节点时就需要调用车辆模型来生成一段可行的局部轨迹而不是简单地连接两个点。2.3 问题定义多目标优化下的决策路径规划的目标不是找到“任意一条”路而是在多重约束下找到“最优”或“较优”的路。这构成了一个多目标优化问题首要目标安全性。路径必须与所有静态障碍物保持安全距离并且能有效应对动态障碍物预测其轨迹或规划出保守的避让策略。核心目标路径长度。通常希望路径尽可能短以减少行驶时间和能耗。关键目标平滑性与舒适性。路径应由缓变的曲线构成避免急转弯和方向突变这关系到控制的难易度和乘坐体验。现实目标决策可行性。在遇到动态障碍物时模型需要做出“等待”、“绕行”或“加速通过”的决策。这需要引入时间维度和简单的行为决策逻辑。因此我们的数学模型最终要转化成一个在运动学和环境约束下的多目标优化问题。评价函数成本函数的设计至关重要它通常是上述多个目标的加权和例如总成本 路径长度 α × 转弯惩罚 β × 接近障碍物惩罚。3. 核心算法选型与深度解析有了清晰的模型接下来就是选择“武器”——算法。B题没有限制算法这既是自由也是挑战。我们需要根据问题特点在经典算法和现代算法之间做出权衡。3.1 全局规划器A* 算法及其变种对于已知的静态地图A* 算法几乎是全局路径规划的首选入门算法。它本质上是Dijkstra算法的启发式改进通过引入一个到终点的估计成本启发函数如曼哈顿距离、欧几里得距离极大地提高了搜索效率。在泊车场景中应用A*有几个技术细节节点定义节点不能只是(x, y)位置必须包含航向角θ即状态为(x, y, θ)。因为对于车辆而言不同的朝向意味着完全不同的可达状态。邻接点生成不能简单地向四个或八个方向移动。需要根据车辆的运动学模型向前、向后模拟出几条不同转向角下的短轨迹这些轨迹的终点就是当前节点的“邻接点”。这被称为状态格栅或运动基元。启发函数设计欧几里得距离是常用的但对于有朝向要求的泊车终点启发函数需要同时惩罚位置和角度的偏差。A*的优点是能保证找到最优解在给定的离散化和运动基元下但缺点是在高维状态空间x, y, θ中搜索效率依然可能成为瓶颈且难以直接处理动态障碍物。3.2 局部规划与动态避障动态窗口法DWA对于动态障碍物纯粹的全局规划需要频繁重规划开销大。这时就需要局部实时规划器DWA是一个经典且有效的选择。DWA的思路非常符合直觉在每一个控制周期比如0.1秒采样在车辆当前可达的速度和转向角范围内采样多组(v, ω)控制指令ω是角速度。模拟对每一组指令依据车辆运动学模型向前模拟未来一段短时间如1-2秒的轨迹。评价用设计好的评价函数给每一条模拟轨迹打分。函数通常包括朝向目标的程度、与障碍物的距离、当前速度大小等。执行选择得分最高的那组(v, ω)指令发送给车辆执行。DWA的优点在于反应快速能很好地处理未知或动态的障碍物。在B题中可以将其作为底层执行器接收来自上层全局规划器的粗略路径点然后负责跟踪路径并实时避让动态车辆。3.3 高级算法尝试RRT与Hybrid A*对于复杂的狭小泊车位A*可能因为离散化精度问题而找不到解。这时可以考虑更高级的算法。RRT快速探索随机树这是一种概率完备的算法。它从起点开始随机向空间采样并尝试将树上最近的节点以可行运动连接到采样点。它特别擅长解决高维、有复杂约束的路径规划问题比如倒车入库。它的变种RRT*还能渐进优化路径。在Matlab或Python中都有成熟的工具箱可以调用。Hybrid A混合A**这是A与连续运动规划的混合体。它像A一样在离散的网格上搜索但每个节点都关联着一段由连续车辆模型生成的可行轨迹并且节点扩展是在连续空间中进行碰撞检测。Hybrid A*是业界解决车辆运动规划的标杆算法之一它能生成既安全又平滑、且运动学可行的路径。实操心得在比赛有限的时间内我建议采用“分层规划”架构。上层用A或Hybrid A在静态地图上规划一条粗略的全局路径忽略动态障碍物或将其视为静态做保守规划。下层用DWA**进行局部跟踪和动态避障。这种架构鲁棒性强且模块清晰便于编程实现和调试。千万不要试图用一个算法解决所有问题。4. 完整求解流程与实现细节下面我将结合当年解题的经验梳理一个从数据到仿真验证的完整实现流程。我们以Python为主要工具环境进行说明。4.1 步骤一环境建模与数据预处理首先需要解析题目提供的停车场地图数据可能是图片坐标也可能是矩阵数据。import numpy as np import matplotlib.pyplot as plt def create_grid_map(map_data, resolution0.1, inflation_radius0.3): 根据原始数据创建膨胀后的栅格地图。 map_data: 二维数组0表示空闲1表示障碍物。 resolution: 每个栅格的边长米。 inflation_radius: 膨胀半径米通常 车辆半径。 # 1. 基础栅格地图 grid_map np.array(map_data) # 2. 膨胀处理 from scipy.ndimage import binary_dilation # 计算膨胀需要覆盖的栅格数 inflate_cells int(np.ceil(inflation_radius / resolution)) # 创建结构元素圆形或方形 structure np.ones((2*inflate_cells1, 2*inflate_cells1)) inflated_map binary_dilation(grid_map, structurestructure) # 3. 将膨胀后的障碍物区域标记为1不可通行 occupied_grid inflated_map.astype(int) return occupied_grid, resolution # 假设读取了地图数据 # original_map load_map(parking_lot.png) # grid_map, res create_grid_map(original_map, resolution0.1, inflation_radius0.5)这一步的输出是一个二维矩阵其中1代表不可通行区域0代表自由区域。务必可视化检查膨胀效果确保安全通道的宽度足够车辆通过。4.2 步骤二基于Hybrid A*的全局路径规划实现这里给出一个高度简化的Hybrid A*核心框架重点在于理解其状态节点和扩展过程。class Node: def __init__(self, x, y, theta, g0, h0, parentNone): self.x x # 连续x坐标 self.y y # 连续y坐标 self.theta theta # 连续航向角弧度 self.g g # 从起点到当前节点的实际成本 self.h h # 到终点的启发成本 self.f g h # 总成本 self.parent parent # 父节点用于回溯路径 self.path [] # 从父节点到本节点的运动轨迹片段离散点列表 def heuristic(node, goal): 启发函数结合距离和角度偏差 dx goal.x - node.x dy goal.y - node.y dist_cost np.sqrt(dx*dx dy*dy) # 角度偏差成本归一化处理 angle_diff abs(np.arctan2(dy, dx) - node.theta) angle_diff min(angle_diff, 2*np.pi - angle_diff) angle_cost angle_diff / np.pi # 归一化到[0,1] return dist_cost 0.5 * angle_cost # 权重可调 def expand_node(current_node, goal, grid_map, resolution): 扩展当前节点生成一系列运动学可行的后继节点 successors [] # 定义一组离散的控制指令速度v正为前进负为后退转向角phi # 车辆参数轴距L L 2.5 # 示例轴距 for v in [1.0, -0.5]: # 前进和低速后退 for phi in np.linspace(-np.pi/4, np.pi/4, 5): # 转向角范围 # 1. 运动学模型模拟一段轨迹离散积分 sim_time 1.0 # 模拟1秒 dt 0.1 x, y, theta current_node.x, current_node.y, current_node.theta path_segment [] for _ in range(int(sim_time/dt)): # 阿克曼转向模型 x v * np.cos(theta) * dt y v * np.sin(theta) * dt theta (v / L) * np.tan(phi) * dt path_segment.append((x, y, theta)) # 2. 碰撞检测将连续坐标映射到栅格 grid_x int(x / resolution) grid_y int(y / resolution) if grid_map[grid_y, grid_x] 1: # 碰撞 break else: # 模拟完成未碰撞创建新节点 new_node Node(x, y, theta, parentcurrent_node) new_node.path path_segment new_node.g current_node.g sim_time # 成本可以用时间或路径长 new_node.h heuristic(new_node, goal) new_node.f new_node.g new_node.h successors.append(new_node) return successors def hybrid_a_star(start, goal, grid_map, resolution): 简化的Hybrid A*主循环 open_set PriorityQueue() start_node Node(start[0], start[1], start[2]) start_node.h heuristic(start_node, goal) start_node.f start_node.h open_set.put((start_node.f, start_node)) closed_set set() # 用于记录访问过的状态可离散化后比较 while not open_set.empty(): _, current open_set.get() # 到达目标判定考虑位置和朝向容差 if (abs(current.x - goal.x) 0.2 and abs(current.y - goal.y) 0.2 and abs(current.theta - goal.theta) np.pi/12): # 回溯路径 path [] node current while node: path.append((node.x, node.y, node.theta)) node node.parent return path[::-1] # 反转从起点到终点 # 状态标识用于判断是否已访问 state_key (round(current.x/resolution), round(current.y/resolution), round(current.theta/(np.pi/12))) if state_key in closed_set: continue closed_set.add(state_key) # 扩展当前节点 for successor in expand_node(current, goal, grid_map, resolution): open_set.put((successor.f, successor)) return None # 未找到路径这段代码勾勒了Hybrid A的核心在连续空间采样用车辆模型生成轨迹在离散的栅格地图上进行碰撞检测并利用A的框架进行搜索。实际比赛中你需要优化启发函数、设计更合理的运动基元、并实现高效的状态剪枝。4.3 步骤三动态窗口法DWA局部跟踪与避障当全局路径生成后我们得到一系列路径点。DWA负责跟踪这些点并避开动态障碍物。def dynamic_window_approach(robot_state, global_path, dynamic_obstacles, config): robot_state: [x, y, theta, v, omega] global_path: 全局路径点列表 [(x1,y1), (x2,y2), ...] dynamic_obstacles: 动态障碍物列表每个为[x, y, vx, vy, radius] config: 机器人参数和DWA参数配置字典 # 0. 找到当前最近的全局目标点lookahead distance lookahead_dist 1.5 target_point get_lookahead_point(robot_state, global_path, lookahead_dist) # 1. 生成动态窗口 # 基于当前速度和加速度限制计算下一时刻可达的速度范围 v_min max(config[min_speed], robot_state[3] - config[max_accel] * config[dt]) v_max min(config[max_speed], robot_state[3] config[max_accel] * config[dt]) omega_min max(-config[max_yaw_rate], robot_state[4] - config[max_delta_yaw] * config[dt]) omega_max min(config[max_yaw_rate], robot_state[4] config[max_delta_yaw] * config[dt]) best_score -float(inf) best_control [robot_state[3], robot_state[4]] # 默认保持原状态 # 2. 在窗口内采样 for v in np.linspace(v_min, v_max, config[v_samples]): for omega in np.linspace(omega_min, omega_max, config[omega_samples]): # 3. 轨迹预测 traj predict_trajectory(robot_state, v, omega, config[predict_time], config[dt]) # 4. 评价函数计算 # 4.1 目标朝向得分 heading_score calc_heading_score(traj, target_point) # 4.2 距离得分远离障碍物 dist_score calc_obstacle_distance_score(traj, dynamic_obstacles, config) # 4.3 速度得分鼓励合理速度 velocity_score v / config[max_speed] # 4.4 路径跟随得分 path_score calc_path_alignment_score(traj, global_path) # 加权总分 total_score (config[alpha] * heading_score config[beta] * dist_score config[gamma] * velocity_score config[delta] * path_score) # 5. 选择最优 if total_score best_score and dist_score config[safe_threshold]: best_score total_score best_control [v, omega] return best_control def predict_trajectory(state, v, omega, predict_time, dt): 根据当前状态和控制量预测未来一段时间的轨迹 traj [state[:3]] # 只记录位置和朝向 current_state state.copy() for _ in range(int(predict_time/dt)): # 简化的差分驱动模型与阿克曼模型在低速下近似 current_state[0] v * np.cos(current_state[2]) * dt # x current_state[1] v * np.sin(current_state[2]) * dt # y current_state[2] omega * dt # theta traj.append(current_state[:3].copy()) return trajDWA的核心在于评价函数的设计。权重参数alpha, beta, gamma, delta需要仔细调节。例如在靠近动态障碍物时应增大beta距离得分的权重让机器人更倾向于保守避让在开阔区域跟踪路径时则增大alpha朝向得分和delta路径得分的权重。4.4 步骤四决策逻辑与仿真集成最后我们需要一个主循环来集成全局规划和局部规划并处理“等待”等决策逻辑。def main_simulation_loop(): # 初始化 global_path hybrid_a_star(start_pose, goal_pose, static_grid_map, resolution) robot_state start_pose [0, 0] # 初始速度、角速度为0 dynamic_obs_list [] # 动态障碍物列表每个时间步更新 for t in np.arange(0, max_time, sim_dt): # 1. 更新动态障碍物状态根据题目给定的运动模型 dynamic_obs_list update_dynamic_obstacles(t, dynamic_obs_list) # 2. 决策层判断是否需要全局重规划或执行等待 need_replan check_collision_risk(robot_state, global_path, dynamic_obs_list, threshold2.0) if need_replan and is_blocked_long_term(dynamic_obs_list): # 如果风险高且阻塞是长期的则触发全局重规划将动态障碍物临时视为静态 print(fTime {t}: 动态障碍物长期阻塞触发全局重规划。) # 可以尝试规划一条不同的路线或者直接进入等待状态 # 简单策略将当前最近动态障碍物的位置膨胀后加入静态地图重新调用Hybrid A* # new_static_map fuse_dynamic_obstacle(static_grid_map, dynamic_obs_list) # global_path hybrid_a_star(robot_state[:3], goal_pose, new_static_map, resolution) # 更简单的策略发送零速度指令等待 control [0.0, 0.0] else: # 3. 正常情况DWA局部规划 control dynamic_window_approach(robot_state, global_path, dynamic_obs_list, dwa_config) # 4. 更新机器人状态根据车辆运动学模型 robot_state update_robot_state(robot_state, control, sim_dt) # 5. 检查是否到达目标 if reached_goal(robot_state, goal_pose): print(任务完成) break # 6. 可视化用于调试 visualize_state(robot_state, global_path, dynamic_obs_list, t)这个主循环体现了“感知-决策-控制”的基本框架。决策逻辑check_collision_risk和is_blocked_long_term是体现智能的地方。例如可以预测动态障碍物未来几秒的轨迹如果预测会与当前路径相交且障碍物速度较慢则判定为“长期阻塞”触发等待或绕行决策。5. 常见问题、调试技巧与方案优化在实际编程和调试过程中一定会遇到各种问题。以下是一些典型的“坑”和解决思路。5.1 路径搜索失败或效率低下问题表现Hybrid A*长时间跑不出结果或者规划出的路径非常奇怪绕大远路。排查思路检查碰撞检测这是最常见的原因。确保膨胀半径设置正确并且碰撞检测函数准确无误。可以单独写一个测试函数手动给定几个位置看碰撞检测结果是否符合预期。调整启发函数权重如果启发函数h(n)的权重过低搜索会退化成Dijkstra盲目搜索如果过高虽然快但可能找不到最优解甚至找不到可行解。可以尝试调整启发函数中距离成本和角度成本的权重比例。优化运动基元expand_node函数中生成的后继轨迹运动基元至关重要。基元太少搜索空间覆盖不全基元太多计算爆炸。需要根据车辆的最小转弯半径和典型场景来设计。例如泊车场景需要包含“大角度转向倒车”的基元。状态离散化粒度在closed_set中判断状态是否访问过时对连续状态(x, y, θ)的离散化粒度即resolution和角度分档要合适。太粗会错过很多有效状态太细会导致closed_set过大搜索缓慢。实操技巧可视化是王道。在算法运行时实时绘制出搜索树open_set和closed_set中的节点、当前扩展的轨迹、障碍物地图。这能帮你直观地看到算法“卡”在了哪里。5.2 DWA局部规划震荡或撞墙问题表现机器人跟踪路径时左右摇摆或者在靠近障碍物时决策犹豫甚至撞上静态障碍物。排查思路评价函数参数调优DWA的性能极度依赖评价函数中各项的权重alpha, beta, gamma, delta。这是一个多目标权衡的过程。建议先固定其他权重单独调节beta障碍物距离项观察机器人在静态障碍物前的行为。目标是找到一个临界值能让机器人在安全距离外平滑绕行。动态窗口范围设置v_max、omega_max、max_accel等参数需要匹配机器人的物理特性。如果最大角速度设置过大可能导致规划出的轨迹曲率变化剧烈控制无法跟踪。预测时间predict_time这个参数非常关键。时间太短机器人“目光短浅”容易陷入局部最优比如对着一个移动的障碍物直冲过去时间太长计算负担重且预测的轨迹可能不准确。一般设置为机器人能在该时间内停下或做出显著机动的时间比如1-3秒。目标点选取get_lookahead_point函数中前瞻距离lookahead_dist的选择影响跟踪的平滑性和超前性。距离太短跟踪紧密但容易对路径噪声敏感距离太长跟踪平滑但可能对急弯反应迟钝。可以将其设计为与速度正相关的动态值。实操技巧分模块测试。先在一个空旷无人的地图上只测试DWA跟踪一条直线或圆弧的能力调通后再加入障碍物。对于动态障碍物先用一个匀速直线运动的模拟障碍物测试避障逻辑。5.3 决策逻辑不智能问题表现机器人要么一味等待导致超时要么鲁莽前行导致碰撞风险。优化方案引入速度障碍物法VO思想进行预测与其简单判断当前距离不如预测动态障碍物未来一段时间的位置。可以假设动态障碍物保持当前速度匀速运动计算出机器人与它的最近会遇点CPA和最短会遇时间TCPA。如果CPA小于安全距离且TCPA很小则判定为高风险需紧急避让或制动。设计有限状态机FSM将机器人的行为模式化。例如可以定义几种状态CRUISING巡航跟踪、OVERTAKING超车、YIELDING礼让等待、EMERGENCY_STOP紧急停止。状态之间的转换由感知到的风险如TCPA、距离触发。这样逻辑更清晰也更容易调试。设置不同的安全阈值根据场景设置多层级的风险阈值。例如警戒距离黄色预警开始轻微调整路径、危险距离红色预警触发主动避让或减速、碰撞距离立即紧急停车。不同阈值对应不同的DWA评价函数权重或不同的行为状态。5.4 仿真与真实差距问题根源比赛是在理想仿真环境下进行忽略了车辆动力学延迟、传感器噪声、控制误差等。提升方案为了让方案更“鲁棒”可以在仿真中主动引入一些扰动来测试。控制延迟在update_robot_state函数中可以模拟一个低通滤波器或简单的延迟环节让执行的控制指令比规划指令慢一拍。定位噪声在机器人的真实状态robot_state上叠加高斯噪声再提供给DWA算法作为输入模拟不完美的定位。感知噪声在动态障碍物的观测位置和速度上也添加噪声。 一个能在有噪声和延迟的仿真中稳定运行的方案其得分和评价会更高。最后在比赛提交论文时除了阐述算法一定要用清晰的图表展示你的结果比如画出最终的成功路径、展示DWA在不同场景下的决策过程用轨迹簇和得分热图、给出关键参数的敏感性分析比如改变权重如何影响路径长度和安全性。通过可视化和量化分析能让你的解决方案显得更加扎实和可信。这道B题是一个完美的练手项目它几乎涵盖了智能车辆决策规划的每一个核心环节吃透它你对这个领域的理解会上一个大台阶。