FA_规划和控制(PC)-人工势场法(APF)

FA：formulas and algorithm,PC：planning and control，APF：artificial potential field

一、人工势场法（APF）核心概念

人工势场法（Artificial Potential Field, APF）是一种模拟物理中 “势场” 概念的路径规划算法，核心思想是：

目标点：对机器人产生引力（Attractive Potential），拉着机器人向目标移动； 障碍物：对机器人产生斥力（Repulsive Potential），推着机器人远离障碍物； 机器人的运动方向由引力和斥力的合力决定，最终沿着 “势场梯度下降” 的方向找到一条无碰撞的路径。

可以类比成：你（机器人）在一个山谷里，目标点是山谷的最低点（引力），障碍物是山谷里的石头（斥力），你会沿着 “往最低点走、同时避开石头” 的方向移动。

1. 核心数学模型

（1）引力场与引力

引力场通常采用二次函数（目标点附近平滑，距离越远引力越大）：Uatt​(q)=21​ξ⋅∥q−qgoal​∥2引力是引力场的负梯度（梯度方向是势场上升方向，负梯度是下降方向）：Fatt​(q)=−∇Uatt​(q)=ξ⋅(qgoal​−q)其中：

ξ：引力增益系数（控制引力大小）； q=(x,y)：机器人当前位置； qgoal​=(xgoal​,ygoal​)：目标点位置； ∥⋅∥：欧几里得距离。

（2）斥力场与斥力

斥力场采用分段函数（仅在障碍物一定范围内生效，避免全局影响）：
$
Urep​(q)={21​η⋅(∥q−qobs​∥1​−ρ0​1​)2,0,​∥q−qobs​∥≤ρ0​∥q−qobs​∥>ρ0​​
斥力是斥力场的负梯度：
Frep​(q)=−∇Urep​(q)={η⋅(∥q−qobs​∥1​−ρ0​1​)⋅∥q−qobs​∥21​⋅∥q−qobs​∥q−qobs​​,0,​∥q−qobs​∥≤ρ0​∥q−qobs​∥>ρ0​​
其中：

η：斥力增益系数； qobs​=(xobs​,yobs​)：障碍物位置； ρ0​：障碍物影响半径（超出则无斥力）。

（3）总势场与总受力

总势场 = 引力场 + 斥力场：
Utotal​(q)=Uatt​(q)+∑i=1n​Urep,i​(q)
总受力 = 引力 + 所有障碍物的斥力：
Ftotal​(q)=Fatt​(q)+∑i=1n​Frep,i​(q)

二、APF的应用场景

APF算法因实现简单、实时性好，广泛应用于：

• 移动机器人路径规划：如AGV（自动导引车）、无人机、扫地机器人的局部路径规划；
• 自动驾驶：低速场景下的避障（如园区无人车、泊车辅助）；
• 机械臂运动规划：关节空间/笛卡尔空间的避障路径生成；
• 游戏AI：游戏角色的避障移动（如NPC避开障碍物到达目标点）。

局限性：易陷入局部最小值（机器人被引力和斥力平衡“卡住”，无法到达目标），需额外策略优化（如添加随机扰动、动态调整势场参数）。

三、APF的操作步骤

以二维平面机器人路径规划为例，核心步骤如下：

1、初始化参数：

• 机器人起始位置q s t a r t q_{start}qstart​​、目标位置q g o a l q_{goal}qgoal​​；
• 障碍物位置列表Q o b s ​ = [ q o b s 1 ​ , q o b s 2 ​ , . . . ] Q_{obs}​=[q_{obs1}​,q_{obs2}​,...]Qobs​​=[qobs1​​,qobs2​​,...]；
• 引力系数ξ ξξ、斥力系数η ηη、障碍物影响半径ρ 0 ρ_0ρ0​​；
• 步长δ δδ（机器人每次移动的距离）、目标阈值ϵ ϵϵ（距离目标小于ϵ ϵϵ则停止）。

2、迭代计算路径：

• 计算当前位置的引力F a t t F_{att}Fatt​​；
• 计算所有障碍物的斥力F r e p , i F_{rep,i}Frep,i​​，求和得到总斥力；
• 计算总受力F t o t a l F_{total}Ftotal​​，归一化后作为移动方向；
• 机器人沿总受力方向移动一步：q n e w ​ = q c u r r e n t ​ + δ ⋅ F t o t a l ∥ F t o t a l ​∥ q_{new}​=q_{current}​+δ⋅\frac{F_{total}}{∥F_{total}​∥}qnew​​=qcurrent​​+δ⋅∥Ftotal​​∥Ftotal​​​​；
• 记录新位置到路径列表；
• 检查是否到达目标（∥ q c u r r e n t ​ − q g o a l ​∥ < ϵ ∥q_{current}​−q_{goal}​∥<ϵ∥qcurrent​​−qgoal​​∥<ϵ），若到达则停止，否则重复迭代。
  输出路径：将迭代过程中记录的位置列表作为最终路径。

四、APF代码实现（Python）

以下是完整的二维APF路径规划代码，包含可视化功能，可直接运行：

importnumpy as npimportmatplotlib.pyplot as plt# 定义人工势场法类class APFPlanner: def __init__(self):# 核心参数（可根据场景调整）self.xi=0.8# 引力增益系数self.eta=50.0# 斥力增益系数self.rho0=1.0# 障碍物影响半径self.delta=0.05# 移动步长self.epsilon=0.1# 目标阈值（距离小于该值则到达）self.max_iter=1000# 最大迭代次数（防止死循环）# 计算引力def attractive_force(self, current_pos, goal_pos):# current_pos: 机器人当前位置 (x,y)# goal_pos: 目标位置 (x,y)direction=goal_pos - current_pos distance=np.linalg.norm(direction)# 引力 = xi * (目标位置 - 当前位置)force=self.xi * directionreturnforce# 计算单个障碍物的斥力def repulsive_force(self, current_pos, obs_pos):# current_pos: 机器人当前位置 (x,y)# obs_pos: 单个障碍物位置 (x,y)direction=current_pos - obs_pos# 斥力方向：远离障碍物distance=np.linalg.norm(direction)ifdistance>self.rho0:# 超出影响半径，斥力为0returnnp.array([0.0,0.0])else:# 斥力计算公式rep_force=self.eta *(1/distance -1/self.rho0)*(1/distance**2)*(direction / distance)returnrep_force# 规划路径def plan(self, start_pos, goal_pos, obs_list):# start_pos: 起始位置 (x,y)# goal_pos: 目标位置 (x,y)# obs_list: 障碍物列表 [[x1,y1], [x2,y2], ...]current_pos=np.array(start_pos,dtype=np.float64)path=[current_pos.copy()]# 记录路径for_inrange(self.max_iter):# 1. 计算引力f_att=self.attractive_force(current_pos, goal_pos)# 2. 计算总斥力（遍历所有障碍物）f_rep_total=np.array([0.0,0.0])forobsinobs_list: obs_pos=np.array(obs)f_rep=self.repulsive_force(current_pos, obs_pos)f_rep_total+=f_rep# 3. 总受力f_total=f_att + f_rep_total f_total_norm=np.linalg.norm(f_total)# 防止受力为0（陷入局部最小值）iff_total_norm<1e-6: print("警告：受力为0，可能陷入局部最小值！")# 添加微小随机扰动，跳出局部最小值f_total=np.array([np.random.uniform(-0.1,0.1), np.random.uniform(-0.1,0.1)])f_total_norm=np.linalg.norm(f_total)# 4. 移动一步（归一化方向 + 步长）current_pos+=self.delta *(f_total / f_total_norm)# 5. 记录路径path.append(current_pos.copy())# 6. 检查是否到达目标ifnp.linalg.norm(current_pos - goal_pos)<self.epsilon: print("到达目标！")breakreturnnp.array(path)# -------------------------- 测试代码 --------------------------if__name__=="__main__":# 1. 初始化规划器apf=APFPlanner()# 2. 设置起始点、目标点、障碍物start=(0.0,0.0)# 起始位置goal=(5.0,5.0)# 目标位置obstacles=[# 障碍物列表(2.0,2.0),(3.0,4.0),(1.0,3.5)]# 3. 规划路径path=apf.plan(start, goal, obstacles)# 4. 可视化结果plt.figure(figsize=(8,8))# 绘制障碍物forobsinobstacles: circle=plt.Circle(obs, apf.rho0,color='r',alpha=0.3,label='Obstacle'ifobs==obstacles[0]else"")plt.gca().add_patch(circle)# 绘制路径plt.plot(path[:,0], path[:,1],'b-',linewidth=2,label='APF Path')# 绘制起始点和目标点plt.scatter(start[0], start[1],color='green',s=100,label='Start')plt.scatter(goal[0], goal[1],color='red',s=100,label='Goal')# 图表设置plt.xlabel('X (m)')plt.ylabel('Y (m)')plt.title('Artificial Potential Field (APF) Path Planning')plt.legend()plt.grid(True)plt.axis('equal')plt.show()

代码关键部分解释

• 参数初始化：xi（引力）和eta（斥力）是核心可调参数——xi过小会导致机器人向目标移动慢，eta过大会导致斥力过强（机器人远离障碍物过远），需根据场景调试；
• 引力计算：直接按公式实现，方向始终指向目标点；
• 斥力计算：判断机器人与障碍物的距离，超出rho0则斥力为0，否则按公式计算；
• 路径迭代：每次计算总受力后归一化方向，移动固定步长，直到到达目标或达到最大迭代次数；
• 局部最小值处理：添加随机扰动，避免机器人因引力和斥力平衡“卡住”；
• 可视化：用matplotlib绘制路径、障碍物、起始/目标点，直观展示规划结果。

运行前置条件

安装依赖库：

pipinstallnumpy matplotlib

直接运行代码，会弹出可视化窗口，显示规划的路径。

五、结束语

• 核心原理：APF通过引力（拉向目标）和斥力（推离障碍物）的合力引导机器人移动，本质是势场梯度下降；
• 关键步骤：初始化参数→迭代计算引力/斥力→沿合力移动→到达目标停止；
• 使用要点：需调试xi（引力）、eta（斥力）、rho0（障碍物影响半径）等参数，同时注意处理局部最小值问题；
• 适用场景：实时性要求高、环境简单的局部路径规划（如AGV、无人机避障）。