轨迹规划 | 图解分析人工势场算法APF(附ROS C++/Python/Matlab仿真)

0 专栏介绍

附C++/Python/Matlab全套代码课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 传统避障方法缺陷

传统的避障方法通常基于几何或图形算法，缺乏对环境动态性和实时性的适应能力。例如，环境在实时操作中可能会出现移动障碍物、临时障碍物等情况，传统方法需要对全局障碍重新建模，产生巨大的计算开销。

与之相对，人工势场法(Artificial Potential Field, APF)基于机器人与障碍物之间的相互作用来生成路径，因此不需要进行全局路径搜索，避免了大量的计算开销。这使得人工势场法能够实现快速的响应和实时的避障能力，非常适合需要快速决策和动态调整路径的应用场景。此外，通过合理设计势场函数和调节参数，可以灵活地处理复杂环境中的多障碍物情况，增加了系统的鲁棒性和可扩展性。

接下来详细介绍人工势场算法的基本原理。

2 APF基本原理

人工势场法的基本思想是在机器人运动环境中创建一个势场$\mathbf{U}$，该势场分为两部分：

• 由目标位置产生的指向目标的引力场${\mathbf{U}}_g$；
• 由障碍物产生的远离障碍物的斥力场${\mathbf{U}}_d$

通过障碍物的斥力场和目标位置的引力场共同作用形成一个虚拟的人工势场，再搜索一条势函数下降的方向，寻找一条无碰撞的最优路径，其原理如图所示。

常见的引力势函数为

$${\mathbf{U}}_g=\frac{1}{2}{\lambda }_g{\left(\mathbf{x}-{\mathbf{x}}_g\right)}^T\left(\mathbf{x}-{\mathbf{x}}_g\right)$$

其中$\mathbf{x}$为机器人所处的位置向量，${\mathbf{x}}_g$为目标位置向量。引力势函数的负梯度即为引力函数

$${\mathbf{F}}_g=-{\nabla }_{\mathbf{x}}{\mathbf{U}}_g=-{\lambda }_g\left(\mathbf{x}-{\mathbf{x}}_g\right)$$

常见的斥力势函数

$${\mathbf{U}}_d=\{\begin{array}{l}\frac{1}{2}{\lambda }_d{\left(\frac{1}{d}-\frac{1}{d_0}\right)}^2,d⩽d_0\\ 0,d>d_0\end{array}$$

其中$d$为当前机器人距离障碍物的最小距离，$d_0$为斥力场作用限，常见的距离度量为欧式距离

$$d=\sqrt{{\left(\mathbf{x}-{\mathbf{x}}_d\right)}^T\left(\mathbf{x}-{\mathbf{x}}_d\right)}$$

斥力势函数的负梯度即为斥力函数

$${\mathbf{F}}_d=-{\nabla }_{\mathbf{x}}{\mathbf{U}}_d=\{\begin{array}{l}{\lambda }_d\left(\frac{1}{d}-\frac{1}{d_0}\right)\frac{1}{d^2}{\nabla }_{\mathbf{x}}d,d⩽d_0\\ 0,d>d_0\end{array}$$

在连续地图中，${\nabla }_{\mathbf{x}}d$可以由距离函数微分导出；在栅格地图中，${\nabla }_{\mathbf{x}}d$可以由栅格代价差分近似。基于引力和斥力，机器人受到的总力为

$$\mathbf{F}={\mathbf{F}}_g+\sum {\mathbf{F}}_d$$

人工势场法的算法流程如表所示。

3 人工势场可视化

人工势场可视化如下所示，颜色越深说明势垒越高

4 仿真实现

4.1 ROS C++实现

核心代码如下所示

bool APFPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
  ...

  // compute the tatget pose and force at the current step
  Eigen::Vector2d attr_force, rep_force, net_force;
  rep_force = getRepulsiveForce();
  while (plan_index_ < global_plan_.size())
  {
    target_ps_ = global_plan_[plan_index_];
    attr_force = getAttractiveForce(target_ps_);
    net_force = zeta_ * attr_force + eta_ * rep_force;

    ...

    if (std::hypot(b_x_d, b_y_d) > p_window_)
      break;

    ++plan_index_;
  }

  // smoothing the net force with historical net forces in the smoothing window
  if (!hist_nf_.empty() && hist_nf_.size() >= s_window_)
    hist_nf_.pop_front();
  hist_nf_.push_back(net_force);
  net_force = Eigen::Vector2d(0.0, 0.0);
  for (int i = 0; i < hist_nf_.size(); ++i)
    net_force += hist_nf_[i];
  net_force /= hist_nf_.size();

  // set the smoothed new_v
  ...

  // set the desired angle and the angle error
  ...

  // position reached
  ...

  return true;
}

4.2 Python实现

(待补充)

4.3 Matlab实现

function nextPos = obstacleAvoid(curPos, goal, obstacle, d0, weight)
	lambdaG = 1;
	lambdaD = weight;
	obstacleNum = size(obstacle, 1);
	Fg = -lambdaG * (curPos - goal);
	Fd = zeros(obstacleNum, 2);
	
    function d = calDistance(p1, p2)
        d = sqrt(sum((p1 - p2).^2));
    end
	
	for i=1:obstacleNum
	   % distance between current pose and obstacle_i
	   di = calDistance(obstacle(i, :), curPos');
	   if di <= d0
	       Fd(i, :) = lambdaD / sqrt(di) * (1 / di - 1 / d0) *  ...
	                  [(curPos(1) - obstacle(i, 1))    ...
	                   (curPos(2) - obstacle(i, 2))];
	   end
	end
	    
	Fall = Fg + sum(Fd', 2);
	nextPos = Fall + curPos;
end

完整工程代码请联系下方博主名片获取

---

更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系