基于可视图法与路径平滑的机器人避障路径规划MATLAB实现

1. 项目概述从一道经典赛题到机器人路径规划的实战演练2012年高教社杯全国大学生数学建模竞赛的D题“机器人避障问题”即便过去了十多年依然是许多数学建模爱好者、机器人路径规划初学者乃至相关领域研究生反复研习的经典案例。这道题的魅力在于它用一个非常具体且直观的场景——让一个机器人在一个充满圆形障碍物的平面区域内从起点绕过障碍物走到终点——封装了计算几何、优化理论、图论和智能算法等多个领域的核心思想。当年参赛的队伍需要综合运用这些知识建立数学模型并最终通过编程求解出一条最优或近似最优的无碰撞路径。今天我们抛开紧张的竞赛氛围以一名过来人和技术实践者的视角重新拆解这道题。我会结合当年的优秀论文思路但更侧重于分享如何用MATLAB将模型“落地”把纸上的公式和算法变成屏幕上清晰可视的轨迹。无论你是正在备赛的学生还是对路径规划感兴趣的工程师这篇文章都将带你深入问题的内核理解其建模的巧思与编程实现的细节最终获得一套可以直接运行、修改和扩展的代码框架。2. 问题核心与建模思路拆解2.1 问题场景还原与抽象题目描述了一个长宽各为100单位的正方形平面区域。区域内散布着若干个已知位置和半径的圆形障碍物2012年题为12个。机器人被简化为一个点即质点模型从指定的起点O(0,0)出发需要安全到达指定的终点A。所谓“安全”意味着机器人的运动路径不能进入任何障碍物的内部可以相切。机器人的移动速度是固定的因此路径最短等价于时间最优。此外题目还增加了一个关键约束机器人在拐弯时即路径方向发生改变的点必须满足一个最小转弯半径的限制。这意味着路径不能是简单的折线在拐点处需要用一段圆弧进行平滑过渡。这短短的描述实际上构建了一个标准的带运动约束的最短路径规划问题。我们需要在二维欧几里得空间中寻找一条从起点到终点的连续曲线这条曲线必须避开所有圆形障碍物并且其曲率处处不超过机器人最小转弯半径所允许的最大值在直线段曲率为0在圆弧段曲率为固定值。抽象之后问题的输入是起点、终点坐标所有障碍物的圆心坐标和半径机器人的最小转弯半径。输出是一条满足约束的路径以及该路径的总长度。2.2 核心建模策略从“可视图法”到“路径平滑”当年优秀的论文普遍采用了分层建模的策略这也是解决此类复杂约束优化问题的经典思路。2.2.1 第一阶段构建无碰撞的节点网络首先忽略机器人的转弯约束只考虑避障。一个非常有效的方法是可视图法。其核心思想是最短路径必然由一系列直线段组成这些直线段的端点只能是起点、终点以及障碍物的边界点通常是切点。因此我们可以将起点、终点以及每个障碍物上选取的若干关键点如与其它障碍物或起终点连线相切的点作为图的“节点”。如果两个节点之间的连线不穿过任何障碍物内部则在它们之间添加一条“边”边的权重就是两节点间的欧氏距离。这样我们就把连续的路径规划问题转化为了在一个离散图上的最短路径搜索问题可以使用Dijkstra或A*等经典算法高效求解。这一步得到的是一条由直线段组成的折线路径它保证了无碰撞但完全不符合转弯半径约束。2.2.2 第二阶段引入转弯约束与路径平滑得到折线路径后我们需要在每一个拐点即路径方向改变的点处用一段满足最小转弯半径的圆弧来替代尖锐的角。具体方法是在拐点处作与两条相邻路径线段相切、半径为给定最小转弯半径的圆弧。计算这段圆弧的圆心、起点和终点并用它替换原来的拐角。平滑处理后整条路径就变成了“直线-圆弧-直线-圆弧…”的组合。这里有一个关键计算如何确定圆弧的起点和终点即切点这需要解算几何关系。通常从拐点沿着两条线段分别向内后退一段距离这两个后退点就是切点后退的距离与转弯半径和拐角的角度有关。平滑后需要重新验证整条路径特别是新加入的圆弧段是否仍然与所有障碍物无碰撞。有时为了满足转弯半径而添加的圆弧可能会“撞”上附近的障碍物这就需要调整第一阶段生成的折线路径或者尝试在障碍物上选择不同的关键点来构建网络。2.2.3 第三阶段模型优化与全局搜索上述两阶段方法是一种启发式策略不一定能得到全局最优解。更优的模型会尝试将避障和转弯约束同时考虑进去。例如将机器人的可行区域自由空间根据障碍物和转弯约束进行复杂的几何描述或者采用非线性优化的方法将路径参数化如用一系列样条曲线的控制点表示以路径长度为优化目标以避障和曲率约束为不等式条件进行求解。这类方法理论上更精确但计算复杂对初值敏感在当年的竞赛环境中实现难度较大。因此许多获奖论文采用的是第一种分层策略的改进版并通过在障碍物上选取更多的关键点如每隔一定角度取一个边界点来丰富节点网络以期逼近全局最优。注意在构建可视图时判断一条线段是否与障碍物相交是高频操作。直接计算线段到圆心的距离再与半径比较是效率较高的方法。避免使用复杂的多边形相交检测因为圆形判断相对简单。3. MATLAB实现详解从算法到代码理论清晰后我们进入实战环节。我将分模块讲解如何用MATLAB实现上述两阶段策略并提供可运行的代码片段。我们假设机器人最小转弯半径为10单位障碍物数据存储在一个矩阵obstacles中每行格式为[x_center, y_center, radius]。3.1 数据准备与可视化基础任何几何问题的编程第一步永远是可视化。它能帮助我们直观地理解问题、调试代码。% 1. 定义场景和障碍物 (以2012年部分数据为例) area [0, 100, 0, 100]; % 区域 [xmin, xmax, ymin, ymax] start [0, 0]; target [100, 100]; turn_radius 10; % 假设有3个障碍物用于示例 obstacles [20, 30, 10; 60, 40, 15; 80, 70, 12]; % 2. 绘制场景 figure(1); clf; hold on; axis equal; axis(area); rectangle(Position, [area(1), area(3), area(2)-area(1), area(4)-area(3)], EdgeColor, k); % 绘制障碍物 for i 1:size(obstacles, 1) viscircles(obstacles(i, 1:2), obstacles(i, 3), Color, r, LineWidth, 1.5); end % 绘制起点和终点 plot(start(1), start(2), go, MarkerSize, 10, MarkerFaceColor, g); plot(target(1), target(2), ro, MarkerSize, 10, MarkerFaceColor, r); legend(, 障碍物, 起点 O, 终点 A); title(机器人避障问题场景图); xlabel(X坐标); ylabel(Y坐标); grid on;这段代码创建了一个基本的场景图。viscircles函数需要Image Processing Toolbox如果没有可以用rectangle(Position, [x-r, y-r, 2*r, 2*r], Curvature, [1,1])替代。3.2 可视图构建与最短路径搜索这是最核心的模块。我们首先需要生成节点集合然后构建邻接矩阵。% 3. 生成节点关键点 % 节点包括起点、终点、每个障碍物边界上的若干点。 % 简单策略在每个障碍物上每隔30度取一个边界点。 node_list [start; target]; % 初始化节点列表 theta_res pi/6; % 30度 for i 1:size(obstacles, 1) cx obstacles(i, 1); cy obstacles(i, 2); r obstacles(i, 3); for theta 0:theta_res:2*pi-theta_res % 避免重复0和2pi node_x cx r * cos(theta); node_y cy r * sin(theta); % 确保节点在区域内 if node_x area(1) node_x area(2) node_y area(3) node_y area(4) node_list [node_list; [node_x, node_y]]; end end end % 4. 构建邻接矩阵可视图 num_nodes size(node_list, 1); adj_matrix inf(num_nodes, num_nodes); % 初始化无穷大 for i 1:num_nodes for j i1:num_nodes % 避免重复和无向图对称 p1 node_list(i, :); p2 node_list(j, :); % 检查线段p1-p2是否与任何障碍物相交 collision false; for k 1:size(obstacles, 1) if lineCircleCollision(p1, p2, obstacles(k, :)) collision true; break; end end % 如果无碰撞则计算距离并赋值 if ~collision dist norm(p1 - p2); adj_matrix(i, j) dist; adj_matrix(j, i) dist; % 无向图 end end end % 5. 使用Dijkstra算法寻找最短路径 [shortest_dist, path_idx] dijkstra(adj_matrix, 1, 2); % 假设起点是第1个节点终点是第2个 path_nodes node_list(path_idx, :); % 折线路径的节点序列 % 6. 绘制折线路径 figure(1); plot(path_nodes(:,1), path_nodes(:,2), b--o, LineWidth, 1.5, MarkerSize, 4); legend(, 障碍物, 起点 O, 终点 A, 折线路径);这里我们假设有两个自定义函数lineCircleCollision用于判断线段与圆是否相交dijkstra实现了Dijkstra算法。它们的实现如下function collision lineCircleCollision(p1, p2, circle) % p1, p2: 线段端点 [x1,y1; x2,y2] % circle: 障碍物 [cx, cy, r] cx circle(1); cy circle(2); r circle(3); % 向量化计算 ac [cx - p1(1), cy - p1(2)]; ab [p2(1) - p1(1), p2(2) - p1(2)]; % 计算线段上距离圆心最近的点投影 ab_squared sum(ab.^2); if ab_squared 0 % p1和p2重合 t 0; else t max(0, min(1, dot(ac, ab) / ab_squared)); end closest_point p1 t * ab; % 计算最近距离 dist_sq sum((closest_point - [cx, cy]).^2); % 如果距离小于等于半径且最近点在线段上t在[0,1]则碰撞 collision (dist_sq r^2); end function [dist, path] dijkstra(adj_matrix, start_idx, end_idx) num_nodes size(adj_matrix, 1); dist inf(1, num_nodes); visited false(1, num_nodes); prev zeros(1, num_nodes); dist(start_idx) 0; for i 1:num_nodes % 找到未访问节点中距离最小的 min_dist inf; u -1; for v 1:num_nodes if ~visited(v) dist(v) min_dist min_dist dist(v); u v; end end if u -1 || u end_idx break; end visited(u) true; % 更新邻居节点距离 for v 1:num_nodes if ~visited(v) adj_matrix(u, v) inf alt dist(u) adj_matrix(u, v); if alt dist(v) dist(v) alt; prev(v) u; end end end end % 回溯路径 path []; u end_idx; if prev(u) ~ 0 || u start_idx while u ~ 0 path [u, path]; u prev(u); end end dist dist(end_idx); end实操心得在生成障碍物边界点时采样密度theta_res需要权衡。密度太高节点数爆炸图搜索变慢密度太低可能错过关键切线路径导致结果远离最优。一个实用的技巧是可以先以较低密度如90度生成网络找到一条粗略路径后在路径附近的障碍物上增加采样点进行局部细化搜索。3.3 路径平滑直线转“直线-圆弧”得到折线路径path_nodes后我们需要对每个内拐点非起点终点进行平滑处理。% 7. 路径平滑处理 smoothed_path []; % 用于存储平滑后的路径点序列 for i 1:length(path_idx) if i 1 % 起点直接加入 smoothed_path [smoothed_path; path_nodes(i, :)]; elseif i length(path_idx) % 终点直接加入 smoothed_path [smoothed_path; path_nodes(i, :)]; else % 处理拐点 i p_prev path_nodes(i-1, :); % 前一个点 p_curr path_nodes(i, :); % 当前拐点 p_next path_nodes(i1, :); % 后一个点 % 计算向量 v_in p_curr - p_prev; v_out p_next - p_curr; % 单位化 u_in v_in / norm(v_in); u_out v_out / norm(v_out); % 计算拐角角度内角 alpha acos(dot(u_in, u_out)); % 计算需要后退的距离d d turn_radius / tan(alpha / 2); % 检查后退距离是否超过线段长度 if d norm(v_in) || d norm(v_out) warning(在点 %d 处转弯半径要求超过了线段长度。可能需要调整路径。, i); smoothed_path [smoothed_path; p_curr]; % 无法平滑保留原拐点 continue; end % 计算两个切点 T1 和 T2 T1 p_curr - u_in * d; T2 p_curr u_out * d; % 计算圆弧圆心 O_c % 角平分线方向 bisector_dir (u_in u_out); bisector_dir bisector_dir / norm(bisector_dir); % 圆心在角平分线上距离拐点 L turn_radius / sin(alpha/2) L turn_radius / sin(alpha/2); O_c p_curr bisector_dir * L; % 计算圆弧的起始角和终止角相对于圆心 angle_start atan2(T1(2) - O_c(2), T1(1) - O_c(1)); angle_end atan2(T2(2) - O_c(2), T2(1) - O_c(1)); % 确保圆弧方向正确从小角到大角 if dot([-u_in(2), u_in(1)], u_out) 0 % 判断转向叉积的z分量 % 逆时针 if angle_end angle_start angle_end angle_end 2*pi; end else % 顺时针 if angle_start angle_end angle_start angle_start 2*pi; end end % 生成圆弧上的点 theta_arc linspace(angle_start, angle_end, 20); % 用20个点近似圆弧 arc_points [O_c(1) turn_radius * cos(theta_arc), ... O_c(2) turn_radius * sin(theta_arc)]; % 将切点T1、圆弧点、切点T2加入路径 % 注意上一个线段的终点应该已经接近T1这里为了精确我们加入T1 smoothed_path [smoothed_path; T1]; smoothed_path [smoothed_path; arc_points]; smoothed_path(end1, :) T2; % 确保T2是下一个直线段的起点 end end % 8. 绘制平滑后的路径 figure(1); plot(smoothed_path(:,1), smoothed_path(:,2), m-, LineWidth, 2.5); legend(, 障碍物, 起点 O, 终点 A, 折线路径, 平滑路径 (直线-圆弧)); title(机器人避障路径规划结果); % 9. 计算并显示路径长度 path_length 0; for i 2:length(smoothed_path) path_length path_length norm(smoothed_path(i, :) - smoothed_path(i-1, :)); end fprintf(平滑路径总长度: %.4f\n, path_length);这段代码实现了在每个内拐点处用圆弧替换。关键几何关系是从拐点沿着两条线段分别向内后退距离d R / tan(α/2)得到两个切点这两个切点与圆心连线垂直于各自的路径线段。圆心位于拐角的角平分线上距离拐点L R / sin(α/2)。3.4 碰撞检测复查与路径优化平滑后的路径尤其是新加入的圆弧段必须重新进行碰撞检测。% 10. 平滑路径的碰撞复查 collision_detected false; for i 1:size(obstacles, 1) circle obstacles(i, :); for j 2:length(smoothed_path) p1 smoothed_path(j-1, :); p2 smoothed_path(j, :); % 对于路径的每一小段直线或圆弧的一部分检查是否与障碍物相交 % 这里简化处理将平滑路径的连续点连线视为小线段进行检查 % 更精确的做法应对圆弧段进行采样检测 if lineCircleCollision(p1, p2, circle) fprintf(警告平滑路径段 (%d-%d) 与障碍物 %d 发生碰撞\n, j-1, j, i); collision_detected true; % 在图上标出碰撞点 plot([p1(1), p2(1)], [p1(2), p2(2)], r:, LineWidth, 3); end end end if ~collision_detected fprintf(平滑路径碰撞复查通过。\n); else fprintf(平滑路径存在碰撞需要调整。\n); % 调整策略可能包括增大转弯半径如果允许、重新选择可视图的节点、在碰撞点附近进行局部路径重规划。 end如果复查发现碰撞说明当前方案不可行。常见的调整策略有局部节点调整回到可视图构建阶段在发生碰撞的圆弧附近的障碍物上增加或移动关键节点生成一条稍微绕行的折线再尝试平滑。优化平滑方式不一定在所有拐点都使用最大转弯半径即最小转弯圆弧。可以尝试使用更大的转弯半径如果机器人能力允许或者采用贝塞尔曲线、样条曲线等曲率连续变化的平滑方式能提供更大的灵活性来避开障碍物。采用不同的全局规划器如果问题复杂可视图法可能陷入局部最优。可以考虑采用随机采样算法如快速随机扩展树RRT或其变种RRT*它们在复杂障碍物环境中具有更好的概率完备性。4. 常见问题、优化技巧与扩展思考在实际实现和调试过程中你会遇到各种各样的问题。下面是我总结的一些典型问题及其解决方案。4.1 算法效率与精度问题问题1节点太多导致图搜索极慢。当障碍物较多或边界点采样过密时节点数N会很大邻接矩阵的构建和Dijkstra算法的复杂度将达到O(N²)或O(N² log N)计算时间无法接受。解决技巧稀疏化采样不要在所有障碍物的所有方向均匀采样。可以先进行粗略规划然后在初步路径附近的障碍物上进行密集采样远离路径的障碍物少采样或不采样。使用A*算法Dijkstra是盲目搜索A*算法通过引入启发式函数如到终点的欧氏距离可以大幅减少搜索范围。MATLAB中也可以尝试graph和shortestpath函数它们通常经过优化。近似最近邻搜索在构建图时对于每个节点只连接距离最近的K个无碰撞节点而不是所有节点将完全图变为K近邻图。问题2平滑后的圆弧与障碍物发生碰撞。这是两阶段法的主要缺陷。解决技巧碰撞预测与规避在平滑阶段不是直接计算几何圆弧而是将“避障”作为约束条件加入。例如可以建立一个优化问题寻找两个切点的位置在两条线段上滑动使得圆弧半径满足要求且不与任何障碍物相交。这可以用数值优化方法如fmincon求解。采用样条平滑使用二次或三次均匀B样条连接路径点。通过优化样条控制点的位置可以同时最小化路径长度并满足曲率约束近似转弯半径和避障约束。这种方法更优雅但建模和求解更复杂。问题3路径不是全局最优。可视图法得到的路径严重依赖于选取的节点。解决技巧关键切线点对于圆形障碍物最短路径往往由障碍物之间的公切线或障碍物与起终点之间的切线组成。与其均匀采样不如精确计算这些切线点作为节点。两个圆之间有4条公切线外外公切线2条内外公切线2条计算这些切点并加入节点集能极大提高找到最优路径的概率。混合算法先用RRT或PRM概率路线图快速生成一条可行路径然后以此路径为参考在其周围狭窄的“走廊”内进行可视图的精细采样和搜索兼顾了全局探索和局部优化。4.2 MATLAB编程实现细节问题4判断线段与圆相交的函数lineCircleCollision在端点靠近圆时可能误判。我们实现的函数检查的是线段上离圆心最近的点到圆心的距离。当线段的一个端点非常靠近圆但线段本身并未穿过圆时也可能被判为碰撞因为最近点就是那个端点。解决技巧这是一个正确的实现。在路径规划中我们要求路径不能进入障碍物内部。因此路径点节点落在障碍物边界上是允许的相切但进入内部则不允许。我们的函数当距离小于等于半径时判为碰撞这意味着端点落在圆上距离等于半径也被认为是碰撞。如果题目允许相切可以将判断条件改为dist_sq r^2仅当严格小于半径时才碰撞。务必根据题目要求明确这一点。问题5平滑路径时后退距离d可能大于线段长度。当拐角非常尖锐α很小或线段很短时计算出的d可能超过前一段或后一段线段的长度导致切点不在线段上几何关系不成立。解决技巧代码中已经加入了检查。如果发生这种情况说明在这个拐点处无法用给定半径的圆弧平滑。此时有两种选择1) 放弃对这个拐点的平滑保留尖角如果机器人允许原地转向2) 回到上游调整折线路径让这个拐角变得平缓一些例如在附近增加一个中间节点。4.3 模型扩展与思考原题是一个经典的模型。在实际科研或工程中可以从多个维度进行扩展机器人模型扩展将质点模型扩展为有尺寸的圆形或矩形。此时障碍物需要相应地“膨胀”Minkowski和规划问题转化为在“膨胀”后的障碍物环境中为质点寻找路径。动态环境如果障碍物是移动的问题就变成了动态路径规划。需要考虑速度障碍法VO、动态窗口法DWA或结合预测的模型预测控制MPC。不确定性考虑机器人的定位、控制可能存在误差障碍物位置也可能不精确。这时需要引入鲁棒规划或随机规划的概念比如在障碍物周围增加安全缓冲区或者规划一条对误差不敏感的最优路径。多目标优化除了路径最短可能还需要考虑能耗最小、平滑度最高、远离危险区域等多个目标。这需要引入多目标优化算法如NSGA-II来求解帕累托最优解集。回顾整个实现过程从问题抽象、模型建立到代码编写最深的体会是将复杂的连续空间问题通过巧妙的几何转化可视图离散化为图搜索问题是解决许多路径规划问题的钥匙。而两阶段法先找折线再平滑虽然可能不是数学上最严谨的但其思路清晰、实现相对简单、效果直观非常适合作为学习和竞赛的起点。我提供的MATLAB代码框架已经实现了这个核心流程你可以通过调整节点生成策略、改进平滑算法、引入更高效的搜索算法来不断提升其性能。编程实现时务必重视可视化图形是调试几何算法最强大的工具。最后路径规划没有银弹针对具体问题的场景和约束对通用算法进行适配和改造才是真正的价值所在。