1. 项目概述与核心价值最近在带学生做数学建模竞赛的备赛训练发现“机器人避障问题”是一个经久不衰的经典赛题也是实际机器人导航与控制领域的核心问题。简单来说这个问题就是给定一个已知或部分已知的环境地图以及机器人的起点和目标点要求规划出一条从起点到终点的最优或可行路径同时确保机器人在移动过程中不会与障碍物发生碰撞。听起来是不是很像我们手机里的地图导航没错其底层逻辑是相通的只不过机器人对路径的“平滑度”、“安全性”和“动态适应性”要求更高不能像导航软件那样只告诉你“前方100米右转”就完事了。这个问题之所以重要是因为它直接关系到移动机器人、自动驾驶汽车、无人机、甚至仓库AGV小车能否安全、高效地完成任务。无论是扫地机器人绕开桌腿还是火星车在复杂地形上自主探索都离不开避障算法的支持。在数学建模竞赛中这类问题通常不会给你一个现成的算法库去调用而是要求你从最基础的几何、优化理论出发自己构建模型、设计算法并求解。这恰恰是锻炼我们抽象问题、建立数学模型和编程实现能力的绝佳场景。对于初学者可能会觉得无从下手而对于有经验的选手如何平衡路径的最优性如最短距离、最短时间、最低能耗与算法的复杂度、实时性才是真正的挑战。接下来我就结合自己多年的实战和教学经验把这个问题的“里子”和“面子”都拆开来讲透从问题理解到模型构建再到算法实现和代码调试手把手带你走一遍。2. 问题拆解与核心概念澄清在动手建模之前我们必须把问题边界和核心概念界定清楚。一个典型的“机器人避障问题”描述可能包含以下要素我们需要逐一解析其背后的数学含义和工程考量。2.1 环境表示地图如何数字化首先机器人所处的环境需要被计算机理解。常见的方式有两种栅格地图将环境划分为均匀的网格像棋盘一样。每个格子有一个状态空闲可通行、占用障碍物或未知。这是最直观的方法特别适合处理不规则障碍物。其数学本质是一个二维矩阵矩阵元素的值代表该位置的状态。优点是实现简单兼容性好缺点是分辨率固定内存消耗随环境增大而平方增长且路径只能是网格点的连线不够平滑。几何特征地图用基本的几何形状如多边形、圆形来描述障碍物。例如一个圆柱形柱子可以用一个圆来表示一张方桌可以用一个矩形来表示。这种方法非常精确内存占用小且便于进行精确的几何碰撞检测。在数学建模中如果题目给出了障碍物的精确坐标和形状比如“圆形障碍物圆心(2,3)半径1”通常就意味着我们应采用几何特征地图。注意选择哪种地图表示方式直接决定了后续路径搜索和碰撞检测算法的设计。竞赛题中如果障碍物形状规则且数量不多强烈推荐使用几何特征地图因为它能引出更优美、更考验数学功底的优化模型。2.2 机器人模型它是个“点”还是个“家伙”这是新手最容易忽略的关键点。机器人有大小和形状不能简单地被看作一个质点。点机器人模型这是最简单的假设即把机器人视为一个没有大小的点。这样避障问题就简化为为这个点规划一条不与障碍物区域相交的路径。实现方法是将障碍物区域按照机器人的轮廓进行“膨胀”。例如如果机器人是一个半径为r的圆形那么我们可以将每个障碍物的边界向外扩展r然后为点机器人规划路径。这个操作在几何上称为“Minkowski和”或“膨胀操作”。完整形状模型更真实的模型需要考虑机器人的实际形状圆形、矩形或多边形和朝向。这意味着在路径上的每一点我们都需要检查机器人以其特定形状和角度放置时是否与障碍物重叠。这大大增加了碰撞检测的复杂度。在多数数学建模竞赛中为了降低初赛难度通常会默认或暗示使用点机器人模型但会通过设置“安全距离”来模拟机器人的尺寸。我们需要仔细审题确认这一点。2.3 路径表示一连串点还是一条曲线规划出的路径需要被表示出来。折线路径由一系列连续的线段组成。这是大多数离散搜索算法如A*的直接输出。优点是生成简单但路径不够平滑机器人在顶点处需要停顿转向不符合实际运动控制需求。参数化曲线例如贝塞尔曲线、样条曲线。它能生成非常平滑的路径机器人可以以连续的速度和加速度行进。但这通常需要在折线路径的基础上进行后处理优化。对于建模竞赛通常要求输出一系列路径点的坐标这本质上就是折线路径。但高级的模型会考虑路径的光滑性并将其作为一个优化目标。2.4 优化目标什么才是“好”路径“避障”只是基本要求“最优”避障才是目标。常见的优化目标有最短路径长度最直观的目标即路径的总欧几里得距离最短。最短时间这需要结合机器人的运动学模型最大速度、加速度来考虑。一条更长的直线路径可能比一条短的、但弯弯绕绕的路径用时更短。最安全路径最大化路径与所有障碍物的最小距离。最平滑路径最小化路径的曲率或转向角的变化使运动更平稳能耗更低。很多时候这些目标是相互冲突的最短的路径可能贴着障碍物走很不安全。因此实际问题往往是一个多目标优化问题。在竞赛中常见的处理方法是将其转化为单目标问题例如以路径长度为主要目标同时约束路径与障碍物的距离必须大于某个安全阈值。3. 核心算法思想与模型构建理解了问题要素后我们就可以着手构建数学模型和选择算法了。这里我介绍两种最主流、也最适合数学建模竞赛的思路基于图搜索的方法和基于优化计算的方法。3.1 方法一基于图搜索的路径规划这种方法的核心思想是“离散化”和“搜索”。其步骤非常系统化步骤1环境离散化与构图如果使用栅格地图那么每个空闲网格的中心或顶点就可以看作图的一个“节点”。如果使用几何特征地图我们则需要人工或采用某种策略在自由空间非障碍物区域中撒点称为“采样点”这些点就是图的节点。接着我们需要定义节点之间的“边”。常见的策略是栅格八连通每个网格节点可以与周围8个方向的相邻网格节点连接。可见性图对于几何特征地图连接任意两个节点如果连接它们的线段不与任何障碍物相交则在这两个节点间添加一条边边的权重就是线段长度。这种方法生成的图包含了所有可能的“贴边”走的最短路径但缺点是边数可能非常多O(n²)。概率路图这是一种更高效的采样方法。在自由空间中随机撒大量点然后每个点尝试与其一定距离内的邻近点连接如果连线无碰撞则添加边。这样构建的图规模可控是处理复杂环境的常用方法。步骤2在图上执行搜索算法图构建好后起点和终点也被映射为图中的两个节点。剩下的问题就变成了经典的图论问题寻找两点间的最短路径。常用算法有Dijkstra算法保证找到最短路径但需要遍历所有节点速度较慢。A算法*Dijkstra的改进版利用一个“启发式函数”来预估当前节点到终点的代价从而优先搜索更有希望的节点效率高很多。启发式函数通常选用欧几里得距离或曼哈顿距离。这是竞赛中最推荐使用的搜索算法因为它高效、易实现且效果直观。步骤3路径后处理搜索得到的是由节点和边组成的折线。我们可能需要对其进行平滑化。一个简单有效的方法是“贪婪收缩”遍历路径上的每个中间点尝试连接它前面和后面的点如果这条新线段无碰撞则删除这个中间点从而拉直路径。实操心得在实现A算法时启发式函数的选择至关重要。欧几里得距离是最常用且往往最有效的。但要注意启发式函数绝对不能高估实际代价须满足“可采纳性”否则A无法保证找到最优解。在栅格地图中如果允许对角移动使用对角线距离作为启发函数会更准确。3.2 方法二基于优化的路径规划这种方法更侧重于“连续”和“最优”。它把路径规划直接表述为一个数学优化问题。模型构建 假设我们用一系列路径点P0, P1, P2, ..., Pn来表示路径其中P0是起点Pn是终点。我们的目标是最小化目标函数通常是路径总长度Sum(||Pi - P_{i-1}||)。满足约束条件避障约束对于路径上的每一段线段Pi P_{i1}以及每一个障碍物Obstacle_j都需要满足distance(线段 Pi P_{i1}, Obstacle_j) safe_distance。这个距离计算是几何问题对于圆形障碍物是点到圆心的距离减半径对于多边形障碍物则需要计算线段到多边形每条边的最短距离。边界约束所有路径点需在环境边界内。(可选) 平滑性约束例如限制相邻线段之间的转角不能太大。求解方法 这通常是一个非线性、非凸的优化问题直接求解非常困难。在竞赛中我们可以采用以下策略序列二次规划如果问题规模不大可以使用MATLAB的fmincon函数或Python的scipy.optimize.minimize来尝试求解。需要提供目标函数和约束函数的解析形式或数值计算方式。转化为非线性最小二乘如果我们把避障约束distance d_safe改写为max(d_safe - distance, 0)^2作为惩罚项加入目标函数就可以将约束优化问题转化为无约束优化问题然后用高斯-牛顿法、Levenberg-Marquardt算法等求解。这种方法更容易实现但惩罚权重的选择需要调参。智能优化算法当问题复杂度高时可以采用遗传算法、粒子群算法等。这些算法不要求梯度信息擅长在全局空间搜索但通常计算量大且不能保证找到最优解更适合作为对比方案或备用方案。注意事项基于优化的方法数学味更浓模型看起来更“高级”但对参赛者的数学建模和编程能力要求也更高。它非常容易陷入局部最优解比如规划出的路径卡在两个障碍物之间出不来。一个实用的技巧是用基于图搜索的方法如A的结果作为优化方法的初始猜测路径*。这样优化算法只需要在这个“还不错”的路径基础上进行微调和平滑成功率会大大提升。4. 关键环节实现与代码剖析这里我以一个经典的竞赛题目为例演示如何用Python实现一个基于几何地图和A*算法的避障路径规划。题目假设环境是一个100x100的平面内有若干个圆形障碍物机器人可视为一个点需要从起点(10,10)走到终点(90,90)并保持与障碍物边缘至少2个单位的距离。4.1 数据结构定义与碰撞检测这是整个项目的基石必须写得健壮。import math import heapq from typing import List, Tuple class Point: def __init__(self, x: float, y: float): self.x x self.y y def distance_to(self, other: Point) - float: return math.hypot(self.x - other.x, self.y - other.y) class CircleObstacle: def __init__(self, center: Point, radius: float): self.center center self.radius radius def distance_to_point(self, p: Point) - float: 计算点到圆周的最短距离正值在外负值在内 return self.center.distance_to(p) - self.radius def distance_to_segment(self, p1: Point, p2: Point) - float: 计算线段到圆周的最短距离。这是一个简化版精确计算需考虑点到线段垂足、端点等情况 # 计算线段向量 v Point(p2.x - p1.x, p2.y - p1.y) w Point(self.center.x - p1.x, self.center.y - p1.y) c1 v.x * w.x v.y * w.y # 点乘 w·v if c1 0: # 最近点是p1 return self.distance_to_point(p1) c2 v.x * v.x v.y * v.y # 点乘 v·v (线段长度的平方) if c2 c1: # 最近点是p2 return self.distance_to_point(p2) # 最近点在线段中间计算投影比例 b c1 / c2 projection Point(p1.x b * v.x, p1.y b * v.y) return self.distance_to_point(projection) def is_collision_free(p1: Point, p2: Point, obstacles: List[CircleObstacle], safe_margin: float) - bool: 检查线段p1p2是否与所有障碍物保持安全距离 for obs in obstacles: if obs.distance_to_segment(p1, p2) safe_margin: return False return True代码解读与避坑distance_to_segment函数是碰撞检测的核心。我们采用了计算线段到圆心距离再减去半径的方法。这里有一个常见的坑当圆心到线段的垂足不在线段上时最近点其实是线段的某个端点。上面的代码通过点乘c1和c2巧妙地处理了这三种情况。确保这个函数正确无误是整个规划算法可靠的前提。4.2 构建概率路图与A*搜索我们不使用固定的栅格而是采用更灵活的概率路图PRM来构建搜索图。def build_roadmap(bounds: Tuple[float, float, float, float], # (x_min, y_min, x_max, y_max) obstacles: List[CircleObstacle], start: Point, goal: Point, n_samples: int 300, connection_radius: float 15.0, safe_margin: float 2.0) - dict: 构建概率路图。 返回一个图结构用邻接表表示graph[node_id] List[(neighbor_id, edge_cost)] import random x_min, y_min, x_max, y_max bounds # 1. 采样点 nodes [start, goal] # 确保起点和终点在图中 node_ids {start: 0, goal: 1} for i in range(n_samples): while True: # 在边界内随机采样 x random.uniform(x_min, x_max) y random.uniform(y_min, y_max) p Point(x, y) # 检查采样点是否本身就在障碍物内考虑安全距离 collision False for obs in obstacles: if obs.distance_to_point(p) safe_margin: collision True break if not collision: nodes.append(p) node_ids[p] len(nodes) - 1 break # 2. 连接邻近节点 graph {i: [] for i in range(len(nodes))} for i in range(len(nodes)): for j in range(i 1, len(nodes)): if nodes[i].distance_to(nodes[j]) connection_radius: continue # 距离太远不连接 if is_collision_free(nodes[i], nodes[j], obstacles, safe_margin): cost nodes[i].distance_to(nodes[j]) graph[i].append((j, cost)) graph[j].append((i, cost)) # 无向图 return graph, nodes, node_ids def a_star_search(graph, nodes, start_id, goal_id): 标准的A*搜索算法实现 open_set [] heapq.heappush(open_set, (0, start_id)) # (f_cost, node_id) came_from {start_id: None} g_cost {start_id: 0} # 从起点到当前节点的实际代价 f_cost {start_id: nodes[start_id].distance_to(nodes[goal_id])} # 起点f_cost g h while open_set: current_f, current_id heapq.heappop(open_set) if current_id goal_id: # 重构路径 path [] while current_id is not None: path.append(nodes[current_id]) current_id came_from[current_id] return path[::-1] # 反转得到从起点到终点的路径 for neighbor_id, edge_cost in graph[current_id]: tentative_g_cost g_cost[current_id] edge_cost if neighbor_id not in g_cost or tentative_g_cost g_cost[neighbor_id]: # 找到一条到neighbor更优的路径 came_from[neighbor_id] current_id g_cost[neighbor_id] tentative_g_cost # 启发函数h使用欧几里得距离 h_cost nodes[neighbor_id].distance_to(nodes[goal_id]) f_cost[neighbor_id] tentative_g_cost h_cost heapq.heappush(open_set, (f_cost[neighbor_id], neighbor_id)) return None # 未找到路径4.3 路径后处理与可视化找到的路径可能有很多冗余拐点我们可以进行简单的平滑。def smooth_path(path: List[Point], obstacles: List[CircleObstacle], safe_margin: float) - List[Point]: 贪婪路径平滑尝试连接非相邻点以缩短路径 if len(path) 2: return path smoothed [path[0]] current_index 0 while current_index len(path) - 1: next_index len(path) - 1 # 从最远的终点开始尝试 while next_index current_index 1: if is_collision_free(path[current_index], path[next_index], obstacles, safe_margin): # 可以直达跳过中间点 smoothed.append(path[next_index]) current_index next_index break next_index - 1 else: # 没有找到可直达的点走到下一个点 smoothed.append(path[current_index 1]) current_index 1 return smoothed # 主程序流程示例 def main(): # 1. 定义环境 bounds (0, 0, 100, 100) obstacles [ CircleObstacle(Point(30, 40), 8), CircleObstacle(Point(60, 30), 6), CircleObstacle(Point(50, 70), 10), CircleObstacle(Point(80, 60), 7) ] start Point(10, 10) goal Point(90, 90) safe_margin 2.0 # 2. 构建路图并搜索 print(正在构建路图...) graph, nodes, node_ids build_roadmap(bounds, obstacles, start, goal, n_samples400, connection_radius20.0, safe_marginsafe_margin) print(f路图构建完成共 {len(nodes)} 个节点。) print(正在执行A*搜索...) raw_path a_star_search(graph, nodes, node_ids[start], node_ids[goal]) if raw_path is None: print(未找到可行路径尝试增加采样点数量或连接半径。) return # 3. 路径平滑 print(正在平滑路径...) smoothed_path smooth_path(raw_path, obstacles, safe_margin) # 4. 计算路径长度 def path_length(p): length 0.0 for i in range(len(p)-1): length p[i].distance_to(p[i1]) return length print(f原始路径长度: {path_length(raw_path):.2f}) print(f平滑后路径长度: {path_length(smoothed_path):.2f}) # 5. 可视化 (需要matplotlib) try: import matplotlib.pyplot as plt import matplotlib.patches as patches fig, ax plt.subplots(figsize(10, 10)) ax.set_xlim(bounds[0], bounds[2]) ax.set_ylim(bounds[1], bounds[3]) ax.set_aspect(equal) # 绘制障碍物带安全距离圈 for obs in obstacles: circle patches.Circle((obs.center.x, obs.center.y), obs.radius, colorgray, alpha0.5) ax.add_patch(circle) safety_circle patches.Circle((obs.center.x, obs.center.y), obs.radius safe_margin, colorred, fillFalse, linestyle--, linewidth0.8) ax.add_patch(safety_circle) # 绘制路图节点和边可选太多会杂乱 # for i, node in enumerate(nodes): # ax.plot(node.x, node.y, o, colorlightblue, markersize2) # for neighbor_id, _ in graph[i]: # neighbor nodes[neighbor_id] # ax.plot([node.x, neighbor.x], [node.y, neighbor.y], colorlightblue, linewidth0.2) # 绘制路径 raw_x [p.x for p in raw_path] raw_y [p.y for p in raw_path] ax.plot(raw_x, raw_y, b-o, linewidth2, markersize4, label原始A*路径) smooth_x [p.x for p in smoothed_path] smooth_y [p.y for p in smoothed_path] ax.plot(smooth_x, smooth_y, r-s, linewidth2, markersize6, label平滑后路径) # 起点终点 ax.plot(start.x, start.y, g*, markersize15, label起点) ax.plot(goal.x, goal.y, m*, markersize15, label终点) ax.legend() ax.grid(True, linestyle--, alpha0.7) ax.set_title(机器人避障路径规划 (PRM A*)) plt.show() except ImportError: print(未安装matplotlib无法可视化。请安装后重试。) # 输出路径点坐标 print(平滑路径点坐标) for i, p in enumerate(smoothed_path): print(f P{i}: ({p.x:.2f}, {p.y:.2f})) if __name__ __main__: main()5. 常见问题、调试技巧与模型优化在实际编程和调试过程中你肯定会遇到各种问题。下面是我总结的一些典型场景和解决思路。5.1 路径搜索失败“未找到路径”这是最常见的问题。原因1采样点不足或连接半径太小。PRM在空旷区域生成的节点太少或者节点之间无法连接导致图是不连通的起点和终点不在同一个连通分量里。排查将路图可视化取消上面代码中绘制节点和边的注释看看节点分布是否均匀起点和终点附近是否有节点它们是否与图的其他部分相连。解决增加n_samples如从300到800或增大connection_radius。也可以采用“桥测试”等更高级的采样策略在障碍物附近狭窄通道处增加采样密度。原因2安全距离设置过大。安全距离超过了某些通道的实际宽度导致理论上存在的路径被安全边界“堵死”。排查检查障碍物和安全距离的可视化图观察是否存在本可通过的狭窄区域被红色虚线安全边界完全覆盖。解决根据机器人实际物理尺寸和控制系统精度合理设置安全距离。在竞赛中如果题目未明确可以将其作为一个可调参数进行分析。原因3起点或终点被障碍物包围。初始化时未检查起点和终点是否在障碍物内。解决在程序开始时增加对起点和终点的碰撞检测如果无效直接报错。5.2 路径质量不佳绕远、不平滑、贴边绕远A*算法找到的是图上的最短路径但如果构图本身不好比如采样点分布不合理导致必须绕路路径就不会优。尝试增加采样点数量或改用可见性图如果障碍物不多后者能保证找到几何上的最短路径。不平滑A*输出的本来就是折线。后处理平滑是关键。除了上面提到的贪婪收缩还可以考虑使用梯度下降法对路径点坐标进行微调以最小化路径长度和曲率。贴边虽然满足了安全距离但路径紧贴安全边界飞行这在动态环境中很危险。可以在目标函数中增加一项“安全项”例如最大化路径点到最近障碍物的平均距离。在优化框架下这很容易实现。5.3 算法效率低下搜索速度慢图规模过大PRM中节点和边太多。可以设置每个节点连接的最大邻居数如最近10个而不是固定半径内的所有节点。碰撞检测耗时is_collision_free函数被调用次数极多每次尝试连接边时。其中distance_to_segment涉及开方运算。优化可以先进行快速粗略检测比如判断线段所在的外接矩形与障碍物的外接圆是否相交如果不相交则直接通过避免精确计算。空间换时间对于静态环境可以预先计算一个距离变换图Distance Transform Map查询任意位置到最近障碍物的距离变为O(1)操作但这适用于栅格地图。5.4 模型扩展与进阶思考在基本模型上我们可以引入更多现实因素让模型更丰满这在竞赛论文中是非常好的加分点。动态障碍物如果障碍物也在运动问题就变成了“动态避障”。这时路径规划需要结合时间维度。一个经典方法是使用“速度障碍法”或“动态窗口法”在机器人的速度空间中搜索既可达又安全的控制指令。非完整约束真实的汽车、差速驱动机器人不能横向移动有最小转弯半径限制。这要求路径必须满足曲率约束。此时单纯的几何路径可能不可行需要规划符合运动学模型的路径如Dubins曲线适用于汽车模型或Reeds-Shepp曲线。不确定性处理传感器有噪声机器人定位有误差。我们可以采用“鲁棒规划”或“随机规划”的方法要求路径在一定的位置不确定性下碰撞概率低于某个阈值。这通常需要更复杂的概率模型。多目标权衡正式论文中可以建立多目标优化模型使用帕累托前沿等概念来分析路径长度、安全性和平滑性之间的权衡关系并用图展示出来显得非常专业。调试这类算法的过程就像在解一个多维的谜题。我的经验是一定要可视化。把环境、障碍物、采样点、搜索图、最终路径都画出来。眼睛看到问题所在比盯着代码和数字冥思苦想要快十倍。先从简单的场景开始比如只有一个障碍物确保算法基础逻辑正确再逐步增加复杂度。最后别忘了在论文中清晰地阐述你的模型假设、算法选择理由、参数设置依据以及可视化结果分析这些才是打动评委的关键。