路径规划算法3 改进的人工势场法（Matlab）

���录

传统人工势场

引力势场

斥力势场

 合力势场

 传统人工势场法存在的问题

 改进的人工势场函数

 Matlab代码实现

---

参考链接：

[1]朱伟达. 基于改进型人工势场法的车辆避障路径规划研究[D]. 江苏大学, 2017.

1986年Khatib首先提出人工势场法，并将其应用在机器人避障领域。该方法的基本思想是在障碍物周围构建障碍物斥力势场，在目标点周围构建引力势场，类似于物理学中的电磁场。被控对象在这两种势场组成的复合场中受到斥力作用和引力作用，斥力和引力的合力指引着被控对象的运动，搜索无碰的避障路径。

这种方法结构简单，便于低层的实时控制，在实时避障和平滑的轨迹控制方面，得到了广泛应用，其不足在于存在局部最优解，目标不可达、与障碍物碰撞等现象。

传统人工势场

引力势场

斥力势场

决定障碍物斥力势场的因素是汽车与障碍物间的距离，当汽车未进入障碍物的影响范围时，其受到的势能值为零；在汽车进入障碍物的影响范围后，两者之间的距离越大，汽车受到的势能值就越小，距离越小，汽车受到的势能值就越大。

 合力势场

  + = 

 传统人工势场法存在的问题

 改进的人工势场函数

 

 Matlab代码实现

main主函数

%main主函数%created by MW%date: 2023/2/28%func: Artificial Potential Field（APF） avoiding obstacle clc;clear; %% 创建并绘制障碍物obstacle = [2 5;            3 8;            7 9;            4 2;            6 6;            9 3];figure(1);scatter(obstacle(:,1),obstacle(:,2),'r')axis equal;%% 创建并绘制初始位置和目标位置start = [0 0];goal = [10 10];hold on;scatter(start(1),start(2),'filled','g');scatter(goal(1),goal(2),'filled','g'); %% 图片标注text(start(1),start(2),'起点');text(goal(1),goal(2),'终点');grid on;axis equal;axis([0 10 0 10]);xlabel('x');ylabel('y');title('人工势场法生成避障路径')%% 生成并绘制避障路径path = APF2D(start, goal, obstacle);hold on;plot(path(:,1), path(:,2),'LineWidth',1,'Color','b');

人工势场法主函数：

function path = APF2D(start,goal,obstacle)% Artificial Potential Field（APF）avoiding obstacle path%%APF参数初始化%如果不能实现预期目标，可能也与初始的增益系数，Po设置的不合适有关。att = 35;%引力增益系数req = 10;%斥力增益系数p0 = 5;%障碍物产生影响的最大距离，当障碍与移动目标之间距离大于Po时，斥力为0。step = 2;%步长maxIter = 200;%最大循环迭代次数n = length(obstacle(:,1));%障碍物个数path = start;%路径初始化newNode = start;for i = 1:maxIter    %% 引力计算    V_att = goal - newNode;%路径点到目标点的向量    r_att = sqrt(V_att(1)^2 + V_att(2)^2);%路径点到目标点的欧氏距离    P_att = att * V_att;%引力        %% 斥力计算    %改进的人工势场法，将斥力分散一部分到引力方向。通过添加随机扰动r_att^n实现，r_att为路径点到目标点的欧氏距离，本文n取2。    V_req = zeros(n,2);    for j =1:n        V_req(j,:) = [obstacle(j,1) - newNode(1), obstacle(j,2) - newNode(2)];%路径点到各个障碍物的向量        r_req(j) = sqrt(V_req(j,1)* V_req(j,1) + V_req(j,2)* V_req(j,2));%路径点到各个障碍物的欧氏距离    end       P_req = 0;    for k = 1:n        if r_req(k)