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

本文主要是介绍轨迹规划 | 图解分析人工势场算法APF(附ROS C++/Python/Matlab仿真)，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

0 专栏介绍

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

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

1 传统避障方法缺陷

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

在这里插入图片描述

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

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

2 APF基本原理

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

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

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

在这里插入图片描述

常见的引力势函数为

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

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

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

常见的斥力势函数

$\boldsymbol{U}_d=\begin{cases} \frac{1}{2}\lambda _d\left( \frac{1}{d}-\frac{1}{d_0} \right) ^2, d\leqslant d_0\\ 0 , d>d_0\\\end{cases}$

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

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

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

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

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

$\boldsymbol{F}=\boldsymbol{F}_g+\sum{\boldsymbol{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 stepEigen::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 windowif (!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));endfor i=1:obstacleNum% distance between current pose and obstacle_idi = calDistance(obstacle(i, :), curPos');if di <= d0Fd(i, :) = lambdaD / sqrt(di) * (1 / di - 1 / d0) *  ...[(curPos(1) - obstacle(i, 1))    ...(curPos(2) - obstacle(i, 2))];endendFall = Fg + sum(Fd', 2);nextPos = Fall + curPos;
end