做无人机项目的时候我经常被问到路径规划到底该用哪种算法。A*、RRT、TEB这些听着可靠但在某些需要快速响应、算力受限的场景里我反而会先写一个人工势场算法Artificial Potential Field。它的思路特别直白目标点对无人机产生引力障碍物对无人机产生斥力无人机沿着合力方向走。就这么简单三五个公式就能说清楚Python实现也只要一两百行。这篇文章我会把人工势场算法从头到尾拆开讲包含数学建模、完整的Python代码、二维仿真验证方法以及从仿真走向真机时那些文档里不会写的坑。如果你正在入门无人机避障或者想快速验证某个避障想法这篇文章可以直接帮你跳过前面折腾的弯路。1. 为什么避障场景里我会先想到人工势场1.1 势场法想解决的问题无人机在飞行过程中避开障碍物本质上是一个“当前位置 期望运动方向”的实时决策问题。相比之下A*和RRT这类基于搜索的算法是在地图上找一条从起点到终点的全局路径它们更适合静态、已知的离线规划场景。而人工势场天然就是一个局部规划器它不关心整张地图只关心当前位置附近的障碍物和目标点产生的力。这种“局部性”在无人机上非常吃香。因为无人机挂载的传感器激光雷达、深度相机通常只能感知周围几米到几十米的环境你根本拿不到完整的全局地图。势场法正好只需要局部信息传感器扫到什么障碍物就往势场里塞什么障碍物点计算一次合力输出一个速度方向整个过程实时性非常高。1.2 和A*、RRT等算法的现实对比我经常被拿来对比的几种路径规划算法直接列个表看得更清楚算法计算方式全局/局部动态避障能力实现难度无人机适配性A*栅格搜索全局弱需重规划中需要建图RRT/RRT*随机采样全局弱需重规划中高需要建图TEB轨迹优化局部较强高常用于地面机器人人工势场力场计算局部强天然支持低适合机载实时运行这个表不是说要抛弃A或RRT而是说不同算法有各自的适用场景。实际工程里最常用的方案是“全局规划 局部避障”的组合A或者RRT先规划出一条大方向上的参考路径势场法负责在飞行过程中绕开突发障碍物。纯靠人工势场做全程规划万一进入复杂环境容易把自己困在局部极小值里这个后面我会专门展开讲。1.3 适合人工势场的具体场景根据我的经验这几个场景里人工势场特别出活室内或低空慢速飞行障碍物以静态为主飞行速度不高传感器帧率高但计算资源紧张比如用树莓派或者STM32做飞控外挂只需要局部避障不用规划全局路径上层已经有大致的航线做编队飞行中的机间避碰把其他无人机当成动态障碍物处理。第一版验证算法时也特别适合先写一个势场法因为代码量小、参数少、问题容易定位。不推荐用在狭窄通道、复杂迷宫里那个场景还是让RRT或者A*上场更靠谱。2. 势场法的数学建模三个公式撑起整套算法2.1 引力场把无人机吸向目标点引力场的核心思想是让无人机距离目标点越远收到的吸引力越大方向指向目标点。公式可以写成[ F_{att} -k_{att} \times (P - P_{goal}) ]其中 ( P ) 是无人机当前位置( P_{goal} ) 是目标点位置( k_{att} ) 是引力增益系数。这个公式用Python表达就是import numpy as np def attractive_force(current, goal, k_att1.0): return -k_att * (current - goal)注意这里 ( (P - P_{goal}) ) 是无人机指向目标点的向量加上负号之后力的方向就变成了由障碍物外部指向目标点也就是期望运动方向。为什么不用固定大小的引力而是让引力随距离变化这里有个控制层面的考虑。如果引力大小恒定无人机在距离目标点很远时会以一个恒定速度冲过去靠近目标点时依然不减速最后很难精确悬停在目标位置。而线性引力相当于一个比例控制器越靠近目标力越小天然带减速效果飞行过程会更平滑。2.2 斥力场让障碍物周围形成一堵“软墙”斥力场的作用距离是有限的只影响进入某个半径 ( d_0 ) 范围内的无人机。公式如下[ F_{rep} \begin{cases} k_{rep} \times \left( \frac{1}{d} - \frac{1}{d_0} \right) \times \frac{1}{d^2} \times \frac{P - P_{obs}}{d} d \leq d_0 \ 0 d d_0 \end{cases} ]其中 ( d ) 是无人机到障碍物的距离( P_{obs} ) 是障碍物位置。对着公式逐项拆解你会发现每一部分都有明确的目的( \left( \frac{1}{d} - \frac{1}{d_0} \right) )保证在 ( d d_0 ) 处斥力为0进入作用范围后距离越近斥力越大( \frac{1}{d^2} )这是整个斥力公式里最核心的“加速器”让斥力在靠近障碍物时呈几何级数增长形成一种“这堵墙越靠近越硬”的效果( \frac{P - P_{obs}}{d} )单位方向向量表示斥力始终从障碍物指向无人机也就是把无人机往外推。当一个区域里有多个障碍物时把每个障碍物产生的斥力向量累加得到的就是总斥力。def repulsive_force(current, obstacles, k_rep100.0, d02.0): F_rep np.zeros(2) for obs in obstacles: delta current - obs dist np.linalg.norm(delta) if dist 1e-3: dist 1e-3 if dist d0: F_rep k_rep * (1.0/dist - 1.0/d0) * (1.0/dist**2) * (delta / dist) return F_rep这里有个小细节当无人机几乎贴在障碍物上时( d ) 非常小( \frac{1}{d^2} ) 会非常大甚至溢出。所以在代码里加了一个dist 1e-3的保护避免除零或者数值爆炸。2.3 合力计算与步长归一化避障时的期望运动方向就是合力方向[ F_{total} F_{att} F_{rep} ]但这里有一个很容易踩的坑如果直接把合力的大小当作步长那么当无人机离目标很远时引力很大单步位移会非常大轨迹会变得很粗糙当无人机靠近目标时步长又太小收敛缓慢。所以实际使用时要把合力归一化只取方向然后乘以一个固定的步长def normalize(vector): norm np.linalg.norm(vector) if norm 1e-6: return np.zeros_like(vector) return vector / norm F_total F_att F_rep direction normalize(F_total) position position step_size * direction这一步等价于把势场法从“力控制”变成了“速度控制”。在飞控里你最终下发的往往也是期望速度而不是期望力所以这种处理方式更贴近无人机控制的实际接口。2.4 四个核心参数的物理意义和调参起点人工势场算法一共就四个关键参数但很多人调参调得一头雾水。这里给一份我常用的初值参考表参数物理意义初始参考值调参方向( k_{att} )引力增益决定无人机奔向目标的速度强弱1.0目标点附近振荡时减小( k_{rep} )斥力增益决定避障时的“反弹强度”100~1000碰撞风险高时增大( d_0 )斥力作用半径超出这个距离障碍物不产生影响2~3 m障碍物密集时适当减小( step_size )单步位移相当于速度控制中的单位速度0.1~0.3 m速度过快时减小调参逻辑也很简单先固定 ( k_{att} )只调 ( d_0 ) 让无人机能在期望距离内提前反应再逐渐增大 ( k_{rep} )直到不发生碰撞。如果目标点附近出现抖动通常是 ( k_{att} ) 相对 ( k_{rep} ) 太小导致斥力占主导需要增大 ( k_{att} ) 或者给斥力加一个距离衰减项。3. Python实现写一个能直接跑的PotentialFieldPlanner3.1 环境建模和数据结构在写代码之前先明确几个约定避免后面改来改去无人机模型简化为二维平面上的质点坐标用 ( (x, y) ) 表示高度保持不变障碍物用点集表示每个点是一个二维坐标实际项目中这些点来自激光雷达或者深度相机的点云起点、目标点、障碍物全部转成numpy数组这样后续向量运算不用写循环。3.2 完整实现代码下面是我常用的一个实现类结构清晰便于扩展import numpy as np class PotentialFieldPlanner: def __init__(self, k_att1.0, k_rep100.0, d02.0, step_size0.1, max_iter500, goal_threshold0.2): self.k_att k_att self.k_rep k_rep self.d0 d0 self.step_size step_size self.max_iter max_iter self.goal_threshold goal_threshold self.goal None self.obstacles [] self.path [] def set_goal(self, goal): self.goal np.array(goal, dtypefloat) def set_obstacles(self, obstacles): self.obstacles [np.array(obs, dtypefloat) for obs in obstacles] def _attractive_force(self, current): return -self.k_att * (current - self.goal) def _repulsive_force(self, current): F_rep np.zeros_like(current) for obs in self.obstacles: delta current - obs dist np.linalg.norm(delta) if dist 1e-3: dist 1e-3 if dist self.d0: F_rep self.k_rep * (1.0 / dist - 1.0 / self.d0) * (1.0 / (dist * dist)) * (delta / dist) return F_rep def plan(self, start): current np.array(start, dtypefloat) self.path [current.copy()] for _ in range(self.max_iter): F_att self._attractive_force(current) F_rep self._repulsive_force(current) F_total F_att F_rep norm np.linalg.norm(F_total) if norm 1e-9: break direction F_total / norm current current self.step_size * direction self.path.append(current.copy()) if np.linalg.norm(current - self.goal) self.goal_threshold: break return np.array(self.path)这段代码跑通之后你就拥有一个最基础的人工势场规划器了。调用方式如下planner PotentialFieldPlanner() planner.set_goal([10.0, 10.0]) planner.set_obstacles([[5.0, 5.0], [7.0, 4.5]]) path planner.plan([0.0, 0.0]) print(path)3.3 代码里的几个关键设计有几个点我特意做了取舍解释一下方便你自己修改第一个点是合力为零时直接停止迭代。当无人机、障碍物、目标点恰好共线且引力和斥力大小相等方向相反时飞机会停在原地不动这就是局部极小值。我在代码里用norm 1e-9把它暴露出来而不是让它死循环这样在调试时能立刻定位到问题。第二个点是goal_threshold这个参数。在真实无人机上因为传感器和定位都有噪声想让无人机精确落在目标点几乎不可能所以到达判断必须留一个半径范围。我在仿真里一般设0.2米真机上会设到0.5米左右。第三个点是所有障碍物都存成点集。你可能觉得障碍物应该用多边形表示更精确但在真机上把雷达点云直接喂给势场法是最省事、最通用的做法。后续想升级成多边形碰撞检测只需要修改_repulsive_force内部实现对外接口可以保持不变。3.4 可视化验证matplotlib画轨迹算法写完了光打印坐标看不出效果我一般会写一个简单的可视化脚本把轨迹、障碍物、起点终点一起画出来import matplotlib.pyplot as plt from matplotlib.patches import Circle def visualize_planning(path, start, goal, obstacles): plt.figure(figsize(8, 8)) plt.plot(path[:, 0], path[:, 1], b-, linewidth2, labelpath) plt.plot(start[0], start[1], go, markersize12, labelstart) plt.plot(goal[0], goal[1], r*, markersize18, labelgoal) for obs in obstacles: circle Circle(obs, radius0.4, colorblack, alpha0.3) plt.gca().add_patch(circle) plt.legend() plt.axis(equal) plt.grid(True, linestyle--, alpha0.6) plt.show()在二维仿真里这个脚本足够用了。到了真机阶段可以把这段可视化代码接到ROS的rviz或者plotjuggler上实时显示航迹。4. 仿真测试先让算法在电脑里飞起来4.1 基础场景验证第一个仿真场景我用最简单的配置起点在 ( (0,0) )目标在 ( (10,10) )中间放一个障碍物在 ( (5,5) )半径0.4米。用默认参数跑一遍planner PotentialFieldPlanner(k_att1.0, k_rep100.0, d02.0, step_size0.1) planner.set_goal([10.0, 10.0]) planner.set_obstacles([[5.0, 5.0]]) path planner.plan([0.0, 0.0]) visualize_planning(path, [0.0, 0.0], [10.0, 10.0], [[5.0, 5.0]])正常情况下轨迹会绕过 ( (5,5) ) 附近的障碍物画出一条平滑的弧线最终收敛到目标点。如果轨迹绕的弯太大了通常是 ( d_0 ) 设得太大如果轨迹几乎贴着障碍物走则是 ( k_{rep} ) 太小障碍物还没“来得及”把无人机推开无人机就已经靠近了。这里有一个判断技巧看轨迹进入障碍物影响范围时是否平滑。如果轨迹有折角说明步长太大缩小step_size到0.05再试如果轨迹一直朝目标方向冲到了障碍物面前才急转向说明 ( d_0 ) 太小提前量不够。4.2 参数敏感性实验人工势场算法看起来参数不多但它们之间的搭配很微妙。我做过一组实验固定 ( k_{att}1.0 )、step_size0.1只改 ( k_{rep} ) 和 ( d_0 )结果差异非常大( k_{rep} )( d_0 )现象502.0轨迹绕弯半径大比较安全但路径明显变长2002.0轨迹较直避障干脆但目标点附近轻微抖动2004.0无人机在很远处就开始绕行路径效率低下10001.0无人机冲得很近才急转弯有碰撞风险从这组实验可以看出( d_0 ) 直接影响“提前量”( k_{rep} ) 直接影响“绕行幅度”。我的建议是先把 ( d_0 ) 设成无人机机身半径的3~5倍再调 ( k_{rep} )不要一上来就两个参数一起改不然你根本分不清是哪个参数导致的问题。4.3 两个经典失效场景目标不可达与局部极小仿真阶段最重要的任务不是跑通理想场景而是把失效场景暴露出来。人工势场有两个天生的缺陷必须在仿真里提前预演。第一个是目标点附近有障碍物时无人机永远无法到达目标。原因是当无人机靠近目标点时如果目标点旁边的障碍物产生的斥力大于引力合力方向会把无人机推开形成“看得见目标但过不去”的局面。解决思路是在斥力公式里乘上一个与目标距离相关的衰减因子[ F_{rep_new} k_{rep} \times \left( \frac{1}{d} - \frac{1}{d_0} \right) \times \frac{1}{d^2} \times \frac{P - P_{obs}}{d} \times \left| P - P_{goal} \right|^n ]这个因子的含义是越靠近目标斥力越小让引力重新占据主导。这是我实际用过最有效的一个小改动( n ) 取2~3效果就很明显。第二个是局部极小值问题。当无人机、障碍物、目标点共线时引力和斥力可能正好抵消无人机悬停在半路不动。想验证这个现象把起点设在 ( (0,0) )目标在 ( (4,0) )障碍物放在 ( (2,0) ) 上你会发现轨迹在 ( (2,0) ) 之前就停住了。这时需要在合力为零时给无人机一个随机方向的扰动或者设计一个切线方向的力让它绕出去。这里我不打算给出过度复杂的改进方案因为你做仿真验证时最重要的就是确认“在什么条件下算法会失效”然后为每个失效场景准备对策。知道边界在哪里飞真机的时候才不会心慌。5. 从仿真到真机那些文档里不会写的坑5.1 无人机不是质点运动学约束与指令转换仿真里一个步长移动0.1米真机上可不能这么干。无人机有最大速度、最大加速度限制转弯也有滞后性。直接把势场法输出的方向乘以一个固定速度发给飞控飞机会因为惯性冲过头甚至撞上障碍物。常规做法是把势场法的输出当成期望速度向量再经过限速、限加速度处理之后转成位置控制指令或者速度控制指令下发。在PX4里通常走offboard模式通过SET_POSITION_TARGET_LOCAL_NEDMAVLink消息下发期望速度。ArduPilot类似只是在消息格式上略有差异。一个我踩过的坑是势场法在某个时刻输出的期望方向可能突变很大比如从左边突然变成右边。如果不加平滑滤波飞控会收到一个突兀的大角速度指令机身姿态会发生明显晃动。解决方法是给期望速度方向加一阶低通滤波或者对偏航角变化率做限幅这样飞行看起来会从容很多。5.2 传感器数据到障碍物模型的转换仿真里障碍物坐标是直接写死的真机上的障碍物来自传感器。激光雷达扫出来的是一堆离散点深度相机输出的是深度图。直接用原始点云算斥力会带来两个问题一是点太密计算量大实时性下降二是噪声点被当成障碍物导致势场抖动。我的处理流程是把点云先做栅格化把空间划分成固定大小的格栅每个格栅只保留一个代表点比如格栅中心点。然后对代表点做一次距离滤波去掉离无人机太远的点只保留 ( d_0 ) 范围内的障碍物点。如果机载算力足够还可以先把多个点云帧做时间融合减少单帧噪声的影响。这里有个细节栅格大小决定了避障的灵敏度和计算量的平衡。栅格太大障碍物边缘被磨平可能会把狭窄通道填满栅格太小计算量上去了但避障精度并没有显著提升。我用过30cm的栅格做过室内避障体验比较均衡。5.3 动态障碍物和算法进化的方向人工势场对动态障碍物的处理能力是它比A*更受欢迎的一个原因。处理思路很简单把动态障碍物当成一个正在高速靠近的斥力源在计算斥力时不只是看当前位置的距离还要考虑它的相对速度。如果障碍物朝无人机方向运动斥力要相应增强如果障碍物正在远离斥力则减弱。改进公式并不复杂关键是在set_obstacles传入障碍物时同时传入速度向量。计算斥力时判断相对速度方向def _repulsive_force_dynamic(self, current, obs_positions, obs_velocities, current_velocity): F_rep np.zeros_like(current) for obs_pos, obs_vel in zip(obs_positions, obs_velocities): delta current - obs_pos dist np.linalg.norm(delta) if dist 1e-3: dist 1e-3 # 相对速度在连线方向上的分量 rel_vel current_velocity - obs_vel v_radial np.dot(rel_vel, delta / dist) influence max(0.0, 1.0 v_radial) # 靠近时增强斥力 if dist self.d0: F_rep influence * self.k_rep * (1.0/dist - 1.0/self.d0) * (1.0/(dist*dist)) * (delta/dist) return F_rep这个改动的物理直觉是迎面飞来的障碍物斥力效果要放大同向飞行的障碍物斥力效果弱一些。如果你做编队飞行这个思路可以直接用在机间防碰撞上。5.4 下一步改进方向人工势场能做好局部避障但单靠它做完整的自主飞行是不够的。我建议你在实际项目中把势场法当作“局部避障模块”来用上层跑全局路径规划下层用势场法做实时规避。如果想继续深挖下面几个方向优先级很高在斥力公式里加入“相对速度项”把动态避障从想法落地到代码设计局部极小值逃逸状态机检测到合力为零或轨迹振荡时切换成预设的绕行行为把势场计算从二维扩展到三维注意三维势场的计算量会显著增加可能需要调整栅格大小和障碍物点的下采样策略把参数在线自适应根据无人机速度和障碍物密集程度实时调整 ( d_0 ) 和 ( k_{rep} )而不是固定死。我自己做这类项目的感受是人工势场算法的门槛低但天花板并不低。它最宝贵的地方在于提供了一种直观的物理隐喻让你能把避障问题想清楚再根据实际飞行中的表现一步步修正。参数永远是调出来的不是算出来的。希望这篇文章能让你在仿真和真机之间少走几个来回把精力花在真正有意义的问题上。