简介这份资源是人工势场法路径规划的改进版实现面向机器人导航、自动驾驶与服务机器人方向的开发者及研究者重点解决传统势场法中目标不可达与局部极小值等典型缺陷。压缩包共5个文件均为m格式的MATLAB脚本整体约4KB涵盖势函数计算、斥力与引力分解、角度求解及主流程调用等模块便于直接运行与二次修改。已有744人学习下载说明其在路径规划入门与算法改进中具有一定参考价值。通过改进势场函数资源有效缓解了目标不可达问题并在静态环境下实现避碰操作读者可据此理解吸引场与排斥场的构建方式、权重参数调整思路以及多层或适应性势场的改进策略进而掌握从数学模型到仿真验证的完整流程为实际项目中的避障与寻路提供可复用的代码基础与排错参考。1. 人工势场法改进版为什么你的机器人在走廊尽头原地打转做过移动机器人避碰的同行大概率都经历过这个场景机器人在空旷区域跑得好好的一进窄走廊或者凹形障碍区就开始在某个位置来回抖动或者干脆停在离目标点两米远的地方不动了。你打开日志一看引力算出来指向目标斥力算出来指向反方向两个力刚好抵消合力为零——这就是经典人工势场法最出名的局部极小值问题。人工势场法的思路其实非常符合直觉把目标点设成一个“引力源”把障碍物设成“斥力源”机器人的运动方向由所有势场力的合力决定。势函数设计得好机器人就像一颗滚下山坡的球自然滑向目标。但问题在于真实环境不是光滑的山坡而是充满了“山谷”和“鞍点”。改进人工势场法要解决的核心问题就三个局部极小值逃逸、目标不可达、动态障碍物避碰。这篇内容适合正在做移动机器人路径规划、无人机避障或者AGV调度系统的工程师我会把势函数怎么改、参数怎么调、代码怎么写、坑在哪里按能直接复现的方式讲清楚。2. 势函数怎么设计从经典公式到改进思路2.1 经典引力势与斥力势的数学形式经典人工势场法中机器人位置记为 $q$目标点记为 $q_{goal}$障碍物记为 $q_{obs}$。引力势函数最常见的形式是二次型$$U_{att}(q) \frac{1}{2} k_{att} \cdot \rho^2(q, q_{goal})$$其中 $k_{att}$ 是引力增益系数$\rho(q, q_{goal})$ 是机器人到目标的欧氏距离。对势函数求负梯度就得到引力$$F_{att}(q) -\nabla U_{att}(q) k_{att} \cdot \rho(q, q_{goal})$$斥力势函数通常只在障碍物影响范围内生效$$U_{rep}(q) \begin{cases} \frac{1}{2} k_{rep} \left(\frac{1}{\rho(q,q_{obs})} - \frac{1}{\rho_0}\right)^2 \rho(q,q_{obs}) \leq \rho_0 \ 0 \rho(q,q_{obs}) \rho_0 \end{cases}$$$k_{rep}$ 是斥力增益$\rho_0$ 是障碍物影响半径。斥力是对斥力势求负梯度得到的方向从障碍物指向机器人。这套公式的问题在于当机器人、障碍物、目标三点共线且障碍物在中间时引力和斥力方向相反合力可能为零。更隐蔽的情况是多个障碍物的斥力矢量合成后刚好抵消引力机器人就卡住了。2.2 改进势函数引入距离修正因子最常见的改进思路是在斥力势函数中乘以一个与目标距离相关的因子让机器人在靠近目标时斥力相对减弱同时让势函数形状在目标附近更“陡峭”减少平坦区域。改进后的斥力势函数$$U_{rep}(q) \begin{cases} \frac{1}{2} k_{rep} \left(\frac{1}{\rho(q,q_{obs})} - \frac{1}{\rho_0}\right)^2 \cdot \rho^n(q, q_{goal}) \rho(q,q_{obs}) \leq \rho_0 \ 0 \rho(q,q_{obs}) \rho_0 \end{cases}$$$n$ 通常取 1 或 2。当 $n1$ 时斥力势在目标附近会自然减小因为 $\rho(q, q_{goal})$ 趋近于零。这样即使障碍物离目标很近机器人也不会因为斥力过大而无法到达目标。对应的斥力表达式需要对 $q$ 求负梯度分解为两个分量$$F_{rep}(q) F_{rep1} F_{rep2}$$$$F_{rep1} k_{rep} \left(\frac{1}{\rho(q,q_{obs})} - \frac{1}{\rho_0}\right) \cdot \frac{\rho^n(q,q_{goal})}{\rho^2(q,q_{obs})} \cdot \hat{e}_{obs\to robot}$$$$F_{rep2} -\frac{n}{2} k_{rep} \left(\frac{1}{\rho(q,q_{obs})} - \frac{1}{\rho_0}\right)^2 \cdot \rho^{n-1}(q,q_{goal}) \cdot \hat{e}_{robot\to goal}$$$F_{rep1}$ 是传统的排斥分量$F_{rep2}$ 是指向目标方向的“牵引修正分量”。这个 $F_{rep2}$ 很关键——它在机器人被斥力推离障碍物的同时额外给了一个朝目标方向的力能有效缓解目标不可达问题。2.3 参数选取与量纲匹配参数不能拍脑袋定量纲要匹配。引力增益 $k_{att}$ 和斥力增益 $k_{rep}$ 的单位不同前者对应加速度/距离后者对应加速度·距离。实际调参时我一般这样做参数典型范围作用调参方向$k_{att}$0.5 ~ 5.0决定朝目标的“拉力”强度太小走不动太大容易冲过目标$k_{rep}$0.1 ~ 2.0决定避障的“推力”强度太小撞障碍太大抖动剧烈$\rho_0$0.5 ~ 3.0 m障碍物影响半径至少大于机器人半径安全裕度$n$1 或 2距离修正因子指数$n1$ 平滑$n2$ 目标附近斥力衰减更快一个实用的起步配置机器人半径 0.3m$\rho_0 1.0$m$k_{att} 1.0$$k_{rep} 0.5$$n 1$。先在这个基础上跑仿真观察轨迹再微调。注意$k_{rep}$ 和 $\rho_0$ 是耦合的。增大 $\rho_0$ 相当于扩大了障碍物的“势力范围”此时 $k_{rep}$ 要相应减小否则机器人在远处就开始剧烈避让轨迹会非常保守。3. 局部极小值逃逸三个可落地的工程方案3.1 方案一虚拟目标点法当检测到机器人合力接近零且未到达目标时在机器人当前位置附近生成一个临时虚拟目标点打破力平衡。虚拟目标点的选取原则是垂直于“机器人—目标”连线方向偏向障碍物较少的一侧。import numpy as np def virtual_target(robot_pos, goal_pos, obstacles, robot_radius0.3): 当检测到局部极小值时生成虚拟目标点。 robot_pos: (2,) 机器人当前位置 goal_pos: (2,) 真实目标位置 obstacles: list of (2,) 障碍物位置 # 计算机器人到目标的方向 dir_to_goal goal_pos - robot_pos dist_to_goal np.linalg.norm(dir_to_goal) if dist_to_goal 1e-6: return goal_pos dir_to_goal dir_to_goal / dist_to_goal # 垂直方向 perp np.array([-dir_to_goal[1], dir_to_goal[0]]) # 评估左右两侧的障碍物密度 left_count 0 right_count 0 for obs in obstacles: vec obs - robot_pos proj np.dot(vec, perp) if proj 0: left_count 1 else: right_count 1 # 选择障碍物较少的一侧 if left_count right_count: direction perp else: direction -perp # 虚拟目标点距离机器人约 1.5 倍障碍物影响半径 virtual_goal robot_pos direction * 1.5 return virtual_goal这段代码的逻辑是先判断机器人到目标的单位方向再取垂直方向作为候选逃逸方向。然后统计左右两侧障碍物的数量往少的那边走。虚拟目标点的距离取 1.5 倍障碍物影响半径太近起不到逃逸效果太远会让机器人偏离目标方向太多。参数说明robot_radius用于后续安全检查实际生成虚拟目标点时还要确保该点不在障碍物内部。如果虚拟目标点落在障碍物影响范围内需要沿垂直方向继续搜索直到找到自由区域。3.2 方案二模拟退火扰动在合力接近零时给机器人的运动方向叠加一个随机扰动扰动幅度随时间衰减。这个方法实现简单但需要设置“退火温度”和“冷却速率”。import numpy as np class AnnealingPerturbation: def __init__(self, T00.5, alpha0.95, min_T0.01): self.T T0 # 初始温度 self.alpha alpha # 冷却系数 self.min_T min_T # 最低温度 def perturb(self, force, stuck_flag): force: (2,) 原始合力 stuck_flag: bool 是否检测到局部极小 if not stuck_flag: # 未卡住时缓慢恢复温度 self.T min(self.T / self.alpha, 0.5) return force # 卡住时施加随机扰动 noise np.random.randn(2) * self.T new_force force noise # 冷却 self.T max(self.T * self.alpha, self.min_T) return new_force逻辑说明stuck_flag由外部检测器给出通常判断条件是合力模长小于阈值如 0.01且到目标距离大于阈值如 0.5m。扰动幅度 $T$ 初始 0.5每次卡住后乘以 0.95 冷却最低 0.01。未卡住时温度缓慢恢复保证下次卡住时仍有逃逸能力。这个方法的坑在于如果冷却太快机器人可能还没逃出极小值区域扰动就消失了冷却太慢机器人会在目标附近反复抖动。我一般把冷却系数设在 0.9~0.97 之间根据控制周期调整。3.3 方案三沿墙走行为切换这是最接近实际工程的做法检测到局部极小值后切换到“沿墙走”模式沿着障碍物边界运动直到满足“离开条件”再切回势场法。沿墙走的实现依赖障碍物距离传感器激光雷达或超声波阵列。核心逻辑是保持机器人一侧与障碍物保持固定距离沿着障碍物轮廓移动。离开条件通常有两个一是机器人到目标的距离比进入沿墙走时更近二是机器人已经绕过了障碍物的“凸起”部分。def wall_following_step(robot_pos, robot_yaw, laser_ranges, laser_angles, follow_sideleft, target_dist0.5): 单步沿墙走控制。 laser_ranges: 各角度激光测距值 laser_angles: 对应角度弧度 follow_side: left 或 right target_dist: 期望与墙的距离 # 找到跟随侧最近障碍物的距离和角度 if follow_side left: mask laser_angles 0 else: mask laser_angles 0 side_ranges laser_ranges[mask] side_angles laser_angles[mask] if len(side_ranges) 0: return 0.0, 0.3 # 没有障碍物直行 min_idx np.argmin(side_ranges) min_dist side_ranges[min_idx] min_angle side_angles[min_idx] # 比例控制器距离误差 - 角速度 dist_error min_dist - target_dist angular_z -1.5 * dist_error # 负号取决于跟随侧 # 前向速度前方障碍物远则快近则慢 front_mask np.abs(laser_angles) 0.5 front_dist np.min(laser_ranges[front_mask]) if np.any(front_mask) else 3.0 linear_x 0.3 * min(1.0, front_dist / 1.0) return linear_x, angular_z逻辑说明dist_error为正表示离墙太远需要转向靠近为负表示太近需要转开。比例系数 1.5 是经验值太大容易震荡太小响应慢。前向速度根据前方障碍物距离缩放保证安全。沿墙走的坑在于“离开条件”的设计。如果条件太宽松机器人可能刚切回势场法又立刻陷入同一个极小值如果太严格机器人会沿着墙走很远路径质量差。我一般用“到目标距离比进入时减少 10% 且合力方向与目标方向夹角小于 60 度”作为离开条件。4. 动态障碍物避碰势场法的时间维度扩展4.1 相对速度斥力场静态势场法假设障碍物不动但实际场景中障碍物可能在移动。把障碍物的速度考虑进来构造相对速度斥力场$$U_{rep}^{dyn}(q, \dot{q}) \begin{cases} \frac{1}{2} k_{rep} \left(\frac{1}{\rho(q,q_{obs})} - \frac{1}{\rho_0}\right)^2 \cdot \frac{1}{1 e^{-\alpha \cdot v_{rel}}} \rho \leq \rho_0 \ 0 \rho \rho_0 \end{cases}$$其中 $v_{rel}$ 是机器人与障碍物的相对速度在“障碍物→机器人”方向上的分量。当障碍物朝机器人运动时$v_{rel}$ 为正斥力增强当障碍物远离时斥力减弱。def dynamic_repulsive_force(robot_pos, robot_vel, obs_pos, obs_vel, k_rep0.5, rho01.0, alpha2.0): 计算动态障碍物的斥力。 robot_vel, obs_vel: (2,) 速度向量 vec_obs_to_robot robot_pos - obs_pos dist np.linalg.norm(vec_obs_to_robot) if dist rho0 or dist 1e-6: return np.zeros(2) unit_vec vec_obs_to_robot / dist # 相对速度在障碍物-机器人方向的分量 rel_vel robot_vel - obs_vel v_rel np.dot(rel_vel, unit_vec) # Sigmoid 调制因子 modulation 1.0 / (1.0 np.exp(-alpha * v_rel)) # 基础斥力大小 magnitude k_rep * (1.0/dist - 1.0/rho0) * (1.0/dist**2) * modulation return magnitude * unit_vec参数说明alpha控制速度调制的敏感度典型值 1.0~3.0。alpha越大障碍物速度对斥力的影响越剧烈。v_rel的计算要注意方向——是相对速度在障碍物指向机器人方向上的投影不是相对速度的模长。4.2 预测窗口与避碰时机动态避碰不能等障碍物进入 $\rho_0$ 才反应需要提前预测。常用做法是计算“碰撞锥”如果机器人和障碍物保持当前速度在未来 $T_{pred}$ 时间内最小距离小于安全阈值就提前施加避让力。def collision_cone_check(robot_pos, robot_vel, obs_pos, obs_vel, safe_dist0.5, t_pred2.0, dt0.1): 预测未来 t_pred 秒内是否会发生危险接近。 返回 (is_danger, min_dist, time_to_min) min_dist float(inf) time_to_min 0.0 t 0.0 while t t_pred: future_robot robot_pos robot_vel * t future_obs obs_pos obs_vel * t d np.linalg.norm(future_robot - future_obs) if d min_dist: min_dist d time_to_min t t dt is_danger min_dist safe_dist return is_danger, min_dist, time_to_min逻辑说明以 0.1s 为步长预测未来 2s 内机器人和障碍物的位置找最小距离。如果最小距离小于安全阈值就触发避让。避让力的方向垂直于相对速度方向偏向远离障碍物的一侧。这个方法的计算量随预测步数线性增长实际部署时可以用解析法求最小距离二次函数极值比离散采样快很多。另外预测窗口 $T_{pred}$ 要大于机器人从当前速度减速到零所需的时间否则来不及避让。5. 避坑与排查调参和部署中最容易翻车的五个点5.1 机器人到达目标附近开始剧烈抖动现象机器人在距离目标 0.3~0.5m 处来回震荡无法稳定停止。原因引力与目标距离成正比距离趋近于零时引力也趋近于零但斥力可能仍然存在如果目标附近有障碍物。更常见的原因是离散控制周期下机器人一步跨过了目标点下一步引力方向反转形成极限环。解决在目标附近设置“到达死区”当距离小于阈值如 0.2m时直接输出零速度。同时把引力改为饱和形式当距离大于某值时引力恒定避免远处引力过大导致冲过头。5.2 窄通道内机器人左右摇摆现象机器人在窄走廊中前进时左右两侧墙壁的斥力交替占优机器人走 S 形。原因两侧障碍物距离相近时斥力方向对距离变化非常敏感。机器人稍微偏左右侧斥力减小、左侧斥力增大把它推回中间但推过头又偏向右侧形成振荡。解决对斥力做低通滤波或者对左右两侧斥力做对称化处理。另一个办法是降低 $k_{rep}$ 同时增大 $\rho_0$让斥力变化更平缓。如果走廊宽度已知可以在走廊内切换到“沿走廊中线”的专用控制器。5.3 动态障碍物“擦肩而过”时机器人急停现象行人从机器人侧面快速经过时机器人突然急停或猛打方向。原因相对速度调制因子在 $v_{rel}$ 接近零时处于 Sigmoid 的线性区微小的速度变化会导致斥力大幅波动。另外如果障碍物速度估计有噪声$v_{rel}$ 会抖动。解决对障碍物速度做卡尔曼滤波或滑动平均。把 Sigmoid 的 $\alpha$ 调小让调制曲线更平缓。设置最小避让速度避免急停。5.4 势场参数在仿真中好用实车效果差现象仿真中轨迹平滑实车上机器人反应迟钝或过冲。原因仿真通常假设理想传感器和瞬时响应实车有控制延迟、传感器噪声、轮子打滑。仿真中的 $k_{att}$ 和 $k_{rep}$ 直接搬到实车往往偏大。解决实车调试时先把所有增益减半确认能走再逐步加。控制周期从 10ms 到 100ms 都试一遍找到延迟和响应速度的平衡点。传感器噪声大的话对距离测量做中值滤波。5.5 多个障碍物时斥力合成方向异常现象机器人面对多个障碍物时避让方向明显不合理甚至朝障碍物撞过去。原因斥力矢量合成时如果两个障碍物距离相近但方向相反合成斥力可能指向机器人后方或侧面。另外如果某个障碍物距离极近其斥力模长会非常大完全主导合力方向。解决对斥力模长做上限截断避免单个障碍物斥力过大。或者采用“最近障碍物优先”策略只计算最近几个障碍物的斥力忽略远处的。更稳健的做法是把势场法和速度障碍法结合用速度空间搜索代替力合成。6. 从仿真到实车验证势场法改进效果的三个技巧6.1 用相图判断局部极小值在二维平面上画出机器人的“位置—合力方向”相图可以直观看到哪些区域是陷阱。具体做法是在环境中均匀撒点每个点计算势场合力用箭头表示方向。如果某个区域箭头形成闭合环或者指向一个不动点那就是局部极小值区域。import numpy as np import matplotlib.pyplot as plt def plot_force_field(goal, obstacles, k_att1.0, k_rep0.5, rho01.0, x_range(-5, 5), y_range(-5, 5), resolution30): 绘制势场力方向图用于识别局部极小值区域。 xs np.linspace(x_range[0], x_range[1], resolution) ys np.linspace(y_range[0], y_range[1], resolution) X, Y np.meshgrid(xs, ys) U np.zeros_like(X) V np.zeros_like(Y) for i in range(resolution): for j in range(resolution): pos np.array([X[i,j], Y[i,j]]) # 引力 vec_goal goal - pos dist_goal np.linalg.norm(vec_goal) if dist_goal 1e-6: f_att k_att * vec_goal else: f_att np.zeros(2) # 斥力 f_rep np.zeros(2) for obs in obstacles: vec_obs pos - obs d np.linalg.norm(vec_obs) if d rho0 and d 1e-6: f_rep k_rep * (1.0/d - 1.0/rho0) * (1.0/d**2) * (vec_obs/d) f_total f_att f_rep U[i,j] f_total[0] V[i,j] f_total[1] plt.figure(figsize(8, 8)) plt.quiver(X, Y, U, V, alpha0.6) plt.scatter(goal[0], goal[1], cg, s100, labelGoal) for obs in obstacles: plt.scatter(obs[0], obs[1], cr, s80, labelObstacle) plt.legend() plt.axis(equal) plt.show()这段代码把环境离散化后逐点计算合力用箭头画出来。如果看到箭头在某个区域“打转”或者指向一个非目标点那里就是局部极小值。改进势函数后重新画一遍对比箭头模式是否改善。6.2 用轨迹曲率评估平滑度实车验证时记录机器人轨迹计算曲率 $\kappa \frac{|xy - yx|}{(x^2 y^2)^{3/2}}$。曲率突变的地方对应机器人急转弯或抖动。改进势场法的目标之一就是让曲率变化更连续。我一般会对比改进前后的曲率均值和最大值。如果改进后均值下降但最大值反而上升说明某些局部区域出现了新的震荡需要针对性调参。6.3 用“最小距离曲线”验证避碰安全性记录机器人与最近障碍物的距离随时间的变化曲线。安全阈值设为机器人半径加 0.1m 裕度。如果曲线在某个时刻逼近阈值说明避碰响应太慢或者斥力增益不够。改进势场法后这条曲线应该整体上移且波动更小。一个我踩过的坑只看最终是否碰撞是不够的有些轨迹虽然没撞但最小距离只有 0.05m实车稍微有点定位误差就撞了。所以最小距离曲线比碰撞标志更能反映安全裕度。6.4 我自己的调试习惯每次改完势函数我会先跑三组仿真空旷环境到目标、单障碍物绕行、窄走廊通过。三组都过了再上实车。实车第一轮只跑低速0.2m/s用手持急停开关守着。确认低速没问题后再逐步提速。参数记录我习惯用表格每次只改一个参数记录轨迹和最小距离。改了两个参数以上出了问题根本不知道是哪个引起的。这个习惯帮我省了很多后悔药。希望帮到你。本文还有配套的精品资源点击获取
