简介：一份针对机器人避障算法学习与验证的MATLAB仿真代码，面向机器人技术初学者、在校学生及算法研究者，可用于理解并复现经典避障策略的完整实现流程。代码包为zip压缩格式，整体仅3KB，内含1个m文件，以函数化脚本方式呈现，覆盖环境建模、传感器感知、路径规划与控制律设计等核心环节，便于直接运行与二次修改。已有5875人下载学习，适合作为课程设计、毕业设计或科研预研阶段的辅助工具。文件中带有详细注释，可引导读者逐步梳理避障逻辑，并可通过调整障碍物坐标、传感器参数或转向阈值等变量，直观观察MATLAB仿真图上机器人轨迹的变化，从而加深对安全距离检测、局部路径调整等经典算法的理解。对于希望快速上手机器人导航实验、验证算法可行性或扩展自身避障方案的读者而言，这份紧凑而完整的代码样例具有较高的参考价值。
1. 机器人避障仿真：一个你改两行参数就能看结果的MATLAB项目
早些年我在某个实验室调一台小车，明明写好了避障逻辑，车还是直愣愣往墙上撞。翻了一晚上代码才发现，问题不在传感器，而在路径规划那一段的返回值单位搞错了。那是我第一次意识到：避障算法这种带空间概念的东西，光看日志调参根本是玄学，必须先在仿真里把轨迹画出来。这套机器人避障算法的MATLAB仿真代码，就是干这个用的——它把人工势场法的完整流程封装成几个函数，从构建地图、计算引力斥力，到画出一条绕开障碍物的平滑路径，全部能跑。你不需要有真车，也不需要接硬件，把文件夹拖进MATLAB改几个参数，就能看到机器人从起点走到终点、并且自动绕开中间障碍物的全过程。适合刚接触路径规划的研究生，也适合想快速验证某个避障思路的工程师。
2. 技术选型与项目结构：为什么拿MATLAB做路径规划仿真
2.1 为什么选MATLAB而不是写Python仿真
做路径规划仿真，Python当然也能干，而且开源库不少。但如果你在高校或做算法验证的部门待过，就会发现避障这个场景用MATLAB有几个实实在在的好处。
第一是矩阵操作自然。避障算法的输入输出基本都是坐标向量、障碍物列表、距离矩阵，MATLAB写这些不用考虑类型声明，一个数组就是一张地图。第二是可视化调试极快：plot、scatter、rectangle这些画图函数随手一调，轨迹立刻画出来，改完参数重跑一下就有新图，这比Python引一堆matplotlib配置要顺手得多。第三是很多机器人课程的配套代码本来就是MATLAB写的，拿这套避障代码做底子改，对接成本低。
当然MATLAB的劣势也明显：部署麻烦、不开源。但路径规划这个阶段本来就不追求部署，重点是算法验证，所以选MATLAB是合理的。
2.2 项目目录结构与函数划分
拿到压缩包解压后，目录结构是这样：
robot_avoidance/ ├── main.m % 主入口：初始化参数、设置障碍物、调用主循环 ├── apf_core.m % 人工势场法主循环：迭代计算合力并更新位置 ├── calc_attraction.m % 计算目标点的引力 ├── calc_repulsion.m % 计算每个障碍物的斥力 ├── obstacle_map.m % 生成障碍物位置与尺寸，返回障碍物列表 ├── plot_trajectory.m % 绘制地图、障碍物、轨迹与终点 └── params.md % 参数说明文档核心循环在apf_core.m里，它把整个逻辑串起来：初始化位置→循环里算引力、算斥力、算合力→更新位置→直到到达终点或超出最大迭代次数。calc_attraction.m和calc_repulsion.m是底层物理函数，分别返回一个力向量。plot_trajectory.m则负责把每一步位置画出来，形成一条路径轨迹。
这个拆分的思路值得学习：主循环不画图，画图函数不计算力。你在改参数的时候根本不碰其他文件，改动面小，不容易把功能改坏。
2.3 全局变量藏在哪
这套代码的全局变量主要集中在main.m的头部，牵引力系数、斥力系数、影响半径、步长等都在一个参数区里集中定义。这里建议不要用global关键字，而是用结构体或直接作为参数传入函数。
% main.m 开头 params.k_att = 1.0; % 引力系数 params.k_rep = 2.0; % 斥力系数 params.d0 = 1.5; % 斥力影响半径 params.step = 0.1; % 每步移动距离 params.max_iter = 500; % 最大迭代次数 params.threshold = 0.3; % 到达终点的判定阈值为什么用结构体而不是全局变量？因为调试的时候，你会频繁改参数看效果，用结构体传参可以把它挂在plot_trajectory里一起显示，随时能看到当前是哪组参数在跑。全局变量的话，改一次还要查一遍有没有被别的地方覆盖，踩过这个坑的人都知道。
明白了资源怎么组织之后，下一步就看核心算法到底怎么写的。
3. 避障算法核心代码拆解：人工势场法的引力与斥力平衡
3.1 整体运行流程
人工势场法的思想很直观：目标点对机器人产生“引力”，拉着机器人往终点走；障碍物对机器人产生“斥力”，把它推开。每一步机器人朝合力方向移动一小段，循环若干次，一条绕过障碍物的路径就出来了。
整体流程分为四步：
- 准备地图：定义起点、终点、障碍物位置与尺寸
- 初始化参数：确定引力系数、斥力系数、影响半径
- 迭代循环：每轮计算当前点所受合力，沿合力方向走一步
- 终止判断：到达目标附近或超出最大迭代次数则停止
3.2 引力计算函数
引力是目标点对机器人的吸引力，距离越远力越大，方向指向目标点。这段代码返回的是力的向量。
function F_att = calc_attraction(pos, goal, k_att) % 输入：当前位置、目标位置、引力系数 % 输出：引力向量 [fx, fy] dist = norm(goal - pos); if dist < 1e-6 F_att = [0, 0]; return; end F_att = k_att * (goal - pos) / dist; end这里是关键点：goal - pos得到的是从当前位置指向目标的向量，除以dist后得到一个单位向量，再乘上系数k_att。也就是说，引力大小只由k_att决定，方向始终指向目标点。用dist < 1e-6做保护，是为了避免机器人恰好到达目标点时出现除零错误。
参数说明：k_att一般设1~3之间。太大，机器人容易横冲直撞；太小，机器人会走得很慢甚至陷在障碍物附近出不来。
3.3 斥力计算函数
斥力是障碍物对机器人的排斥力，只在机器人和障碍物距离小于d0时才起作用，距离越近斥力越强，避障算法里这个函数最值得细看。
function F_rep = calc_repulsion(pos, obstacle, k_rep, d0) % 输入：当前位置、单个障碍物坐标、斥力系数、影响半径 % 输出：斥力向量 [fx, fy] dist = norm(pos - obstacle); if dist > d0 F_rep = [0, 0]; return; end if dist < 0.2 dist = 0.2; % 防止无限大的斥力 end F_rep = k_rep * (1/dist - 1/d0) * (pos - obstacle) / (dist^2); end公式的逻辑是：当距离大于d0时，默认障碍物根本不影响机器人，直接返回零向量；当距离小于d0时，斥力大小按(1/dist - 1/d0)计算，这个值在障碍物边缘趋近0，在机器人贴脸时趋近无穷大，正好符合“靠近才推、越近推力越大”的设计。方向是pos - obstacle，也就是障碍物指向机器人，往外推。
注意代码里有个防抖逻辑：dist < 0.2时强制把它拉回0.2，避免因为距离无限趋近零导致斥力爆炸、数值溢出。这个细节在真实调试里很常见，不写的话跑着跑着轨迹就飞出去了。
3.4 主循环与位置更新
主循环在apf_core.m里，它把引力和所有斥力叠加，得到合力，再沿合力方向走一步。多障碍物场景下，机器人收到的斥力是每个障碍物斥力的向量和。
function path = apf_core(start_pos, goal, obstacles, params) pos = start_pos; path = pos; for iter = 1:params.max_iter % 引力 F_att = calc_attraction(pos, goal, params.k_att); % 斥力：累加每个障碍物的贡献 F_rep = [0, 0]; for obs = obstacles' F_block = calc_repulsion(pos, obs', params.k_rep, params.d0); F_rep = F_rep + F_block; end F_total = F_att + F_rep; % 沿合力方向移动一小步 if norm(F_total) > 1e-3 pos = pos + params.step * F_total / norm(F_total); else % 合力接近零，说明可能陷入局部极小值，添加微扰动 pos = pos + params.step * [0.1, 0.1]; end path = [path; pos]; % 到达目标判断 if norm(pos - goal) < params.threshold break; end end end主循环里有两处值得注意。第一，斥力用for循环逐个障碍物累加，障碍物数量多了以后性能会下降，但作为仿真完全够用；真觉得慢，可以提前剔除距离过远的障碍物，不过那种优化在这套代码里暂时不需要。第二，合力接近零的情况特别常见——机器人在两个障碍物之间被左右拉力顶住，或者障碍物与目标点在一条直线上时，引力和斥力正好相互抵消，这就是人工势场法著名的局部极小值问题。
代码在检测到合力接近零时给出一个微小的扰动向量[0.1, 0.1]，让机器人向斜上方挪一下再继续算。这个“抖一下”的手段能救回一部分局部极小值，但不保证全部救活，后面避坑章节再展开讲。
3.5 流程图替身：位置更新的几何含义
如果非要用一句话描述主循环的几何过程：每一步机器人在当前位置画一个方向箭头——引力指向终点、斥力指向远离障碍物，把它们相加得到新的向量，然后沿这个方向走固定步长。
这也就是为什么路径往往是弧形或者折线的组合：在远离障碍物的地方，引力主导，轨迹笔直朝目标；靠近障碍物时，斥力逐渐增大，轨迹被“挤”到一侧，绕过之后再被引力拉回直线。
步长参数params.step决定了轨迹的平滑度和迭代次数：步长越小，轨迹越精细，但到达终点需要的迭代次数越多；步长太大，则可能直接跳过目标点，在目标附近来回震荡。这是调参时最先要感受的一个参数。
代码层面的东西讲透了，接下来把完整跑起来，看效果。
4. 运行与参数调优：从文件夹到第一条平滑路径
4.1 跑通这个仿真只需要三步
第一步，打开MATLAB，把整个robot_avoidance文件夹拖进当前目录或右键添加至路径。第二步，在命令行窗口输入main回车，或者在编辑器中直接点击“运行”。第三步，弹出的图像窗口会依次出现地图、障碍物、目标点，然后一条轨迹从起点逐步延伸出去，绕开障碍物后停在目标点附近。
% 命令行运行方式 >> cd D:\robot_avoidance >> main如果使用的是较新版本的MATLAB（比如R2020a以后），文件夹里的.m文件不需要额外配置就能直接运行，但如果你的工作目录不在工程目录下，别忘了cd过去或者用“设置路径”功能把文件夹加入搜索路径。这个坑我在机器上翻过一次车——明明代码没错，一直提示“未定义函数或变量 main”，十有八九是路径没加。
运行成功后，图像窗口里应该会显示这样的内容：
- 起点：一个绿色圆点，位置默认为坐标
(0, 0) - 目标点：一个红色星形或方块，默认在
(10, 10) - 障碍物：灰色圆形或者矩形，数量在
obstacle_map.m里定义 - 机器人路径：一条蓝色折线，从起点开始平滑地绕过障碍物
4.2 主要参数怎么调，改完会有什么变化
参数集中在main.m开头，调完直接重新运行即可，不用改其他文件。下面这张表列出每个参数的作用和合理范围，是这套代码里最值得反复调整的：
| 参数名 | 含义 | 推荐范围 | 调大后的影响 | 调小后的影响 |
|---|---|---|---|---|
| k_att | 引力系数 | 1~3 | 轨迹更直，更倾向于冲目标 | 轨迹更绕，靠近障碍物时反应更强 |
| k_rep | 斥力系数 | 1~5 | 绕障更果断但路径抖动 | 绕障半径变小，可能擦到障碍物 |
| d0 | 斥力影响半径 | 1~3 | 提前绕障，路径更早偏离 | 靠近障碍物才有反应，容易撞 |
| step | 每步移动距离 | 0.05~0.2 | 收敛快但轨迹粗糙 | 轨迹平滑但迭代次数增加 |
| threshold | 到达判定距离 | 0.1~0.5 | 提前停，可能没到终点 | 过度逼近目标，迭代更多 |
一个血泪教训是：刚开始调参时，我习惯把k_rep拉到10以上，以为斥力越大越安全。结果机器人反而在障碍物前面反复震荡，轨迹像锯齿一样。原因是斥力太大会让机器人一靠近障碍物就被弹飞，弹到引力主导范围后又冲回来，这样反复拉锯，路径极不稳定。
合理的做法是让k_rep保持在2~4的数倍于k_att之间，形成“引力和斥力势均力敌但斥力略占上风”的格局。
4.3 换一张地图：修改障碍物布局
默认地图里预置了几个障碍物，在obstacle_map.m里维护。你想换成自己的地图，直接改这个函数的返回矩阵就行。
function obstacles = obstacle_map() % 每行一个障碍物：[x坐标, y坐标] obstacles = [ 3.0, 3.0; 5.0, 6.0; 7.0, 4.0; 4.0, 8.0; 8.0, 7.0 ]; end注意：这套代码目前的障碍物被当作点来处理，画图时用圆圈表示。如果你要仿真的是矩形障碍，比如墙壁，那还需要在calc_repulsion里把障碍物从“点”改成“矩形区域”，计算机器人与矩形的最短距离，这个改造后半部分再细说。
4.4 不同避障算法的扩展空间
这套代码的核心是人工势场法，但不代表你只能用它。apf_core.m的主循环结构其实非常模块化：你把计算引力和斥力的方式换成其他策略，就能得到不同的避障算法。
换VFH（向量场直方图法）的思路是：把障碍物信息转化为极坐标直方图，选择没有障碍物的扇形区域方向作为机器人的前进方向。具体到代码上，就是在apf_core里用histogram对障碍物角度做统计，再替换掉原来的合力计算逻辑。
换DWA（动态窗口法）的思路是：在速度采样空间里评估每个候选速度组合（线速度+角速度），选择评价函数得分最高的一组作为下一步运动。calc_attraction和calc_repulsion这两个函数可以保留作为评价函数的一部分，用来给每个候选速度打分。
这种“替换算法核心而保留仿真框架”的开发方式，比从零搭建可视化环境要省很多时间，这些改造技巧到最后一章具体展开。
跑通只是第一步，仿真跑多了之后，各种“翻车现场”才是真正让你理解算法的老师。
5. 排错与避坑指南：路径规划仿真的五个常见翻车现场
5.1 机器人卡在障碍物与目标点之间不动了
现象：机器人路径走到某个位置就停住，轨迹不再向外延伸，命令行窗口里也没有报错，程序一直空转。这在仿真里表现得很直观：起点到障碍物的路径正常，之后的位置点全部变成同一个点。
原因：这是人工势场法最经典的局部极小值问题——当障碍物、机器人、目标点三者近似共线时，灶力与引力方向相反、大小近似相等，合力趋近零。主循环里判断合力过小会给一个固定扰动，但这个扰动可能不够，机器人推一下又退回原地。
解决：在apf_core里把扰动改成随机方向，或者直接加入“历史轨迹记忆”，检测到连续多步位置变化小于阈值时，强制让机器人沿垂直于当前合力方向的侧向移动一小段。最有效的方案是把threshold调低一些，让程序在未达目标前不会误以为已经到达终点。
5.2 轨迹绕路绕得莫名其妙
现象：障碍物明明在左边，机器人却绕了右边一大圈，路径长度明显超过直觉估计。
原因：斥力影响半径d0设置过大，机器人在距离障碍物很远的地方就已经感受到斥力，原始直线路径被过早破坏，走出一条冗余的弧线。我在调参时把d0设成5，结果整个路径全偏离了。
解决：把d0逐步调小到1~2之间，观察轨迹变化。另一个原因是障碍物的斥力叠加导致靠得近的两个障碍物之间形成了“斥力墙”，机器人无法通过。解决方式是把障碍物间距控制在d0的两倍以上，或者对密集障碍物做合并处理。
5.3 路径终点离目标点始终差一截
现象：迭代全部跑完，机器人停在目标点旁边，但距离始终大于你期望的精度。不管怎么提高max_iter，距离不再缩小。
原因：threshold参数设得太大，比如设成1.0，机器人还没真正走到目标点附近就触发了到达判定。程序认为“差不多到了”，于是提前跳出主循环。
解决：把threshold调到0.2以下，同时把step调小到0.05附近。注意两者必须配合，如果step是0.5而threshold是0.2，机器人一跨步就跨过目标点，永远没法真正进入判定范围。
5.4 障碍物与轨迹穿模：画出来的路径直接穿过障碍物
现象：轨迹曲线毫无疑问地穿过了一个障碍物圆圈，算法仿佛对障碍物视而不见。
原因：最常见的原因是障碍物坐标写错。obstacle_map.m返回的是点坐标，画图时viscircles把点画成半径固定的圆。多个障碍物之间如果距离太近，圆与圆重叠，而算法只对“中心点”算斥力，忽略了圆本身的面积。于是路径可以穿过圆与圆之间的缝隙，看起来就像穿模。
解决：在calc_repulsion里把障碍物中心与机器人距离减去障碍物半径，再把减完后的值代入斥力公式。这样机器人在靠近圆边缘时就会被推开，而不是到圆心附近才反应。
5.5 老版本MATLAB报错：函数名冲突
现象：运行main.m时报错“无法将输入参数转换为 double 型数据”，或者直接提示某行语法错误。
原因：如果MATLAB版本比较老（R2019b之前），部分新语法（比如结构体字段直接作为函数参数传递）可能存在兼容性问题。另外obstacle_map这类函数名如果与工具箱自带函数重名，也会有冲突。
解决：先将主脚本里所有结构体参数展开成普通变量传参，比如把params.k_att改成单独变量k_att传入函数。如果仍然报错，去命令行输入which obstacle_map -all，检查该名称是否被其他文件占用。曾经在我的机器上，这个坑折腾了整整一个下午，最终发现是另一个实验文件夹里有个同名函数被自动加入路径了。
这些踩坑经历其实都把问题指向同一个方向：人工势场法的物理直觉简单，但实现细节处处有门道。理解了这些门道之后，你就可以拿这套代码做二次开发了。
6. 二次开发思路：从静态障碍到动态环境的三步改造
6.1 给障碍物加上速度：静态避障升级成动态避障
人工势场法的经典短板是处理不了动态障碍物。改造思路不复杂：把障碍物坐标从常量改成随时间变化的量。
在obstacle_map.m里加一个obstacles_update(t)函数，每个循环用新坐标更新calc_repulsion的输入。障碍物的运动可以写成简单直线运动，比如obs_pos = obs_init + obs_velocity * t。这样机器人需要不断根据新的障碍物位置重规划路径。
有个细节必须注意：动态环境下，步长step要调小，否则机器人每步运动太大，容易直接冲进障碍物运动路径里。
6.2 把人工势场法的目标陷入问题用历史轨迹修正
在apf_core.m里维护一个pos_history变量，记录最近5步位置。每次更新前检查最近5步位置的最大位移是否小于一个阈值，如果是就判定为卡住，手动设置一个垂直于当前运动方向的临时引力，把机器人拉出去。
if norm(pos - pos_history(end-4)) < 0.01 % 检测到停滞：构造一个垂直于当前合力的摆脱力 F_escape = [-F_total(2), F_total(1)]; pos = pos + params.step * F_escape / norm(F_escape); end这个改造对绕多障碍物场景特别有用，等于给算法加了一个“后悔药”——发现自己卡死在原地时，强制横向挪一步再继续。
6.3 验证方法：量化你的改造效果
每次改完算法，别只用眼睛看轨迹。加一个简单的验证脚本，统计三项指标：轨迹总长度、迭代次数、与障碍物的最小距离。轨迹长度越短说明路径越高效，迭代次数越少说明收敛越快，最小距离衡量安全性。
验证脚本在main.m循环结束后加几行计算即可：
path_length = sum(sqrt(sum(diff(path).^2, 2))); min_distance = inf; for p = path' for obs = obstacles' min_distance = min(min_distance, norm(p' - obs')); end end这三项指标可以作为你每次改参数后的量化基准。从那以后，我每次拿到新的避障代码，都会先跑一遍原始版本记录这三项指标，再开始动刀改算法，这样就能知道每次修改到底是变好了还是变差了。希望这个习惯，也能帮你在调参和二次开发的路上少走一些弯路。
本文还有配套的精品资源，点击获取
