人工势场法Matlab代码详解

一、简介

人工势场法是一种常用的路径规划方法，也被广泛应用于机器人导航、无人机飞行等领域。本文将通过一个Matlab代码示例，对人工势场法的实现进行分析和阐述。

二、代码实现

为了方便理解，我们先来看一下整个程序的框架：

function artificialPotentialField
clear all; clc; close all;
goal=[90 90];
q_start=[10 10];
boundary=[0 100 0 100];
obstacles= [30 30 10;
               50 50 10;
               70 70 10];
           
for i=1:5000
    % 确定机器人所处位置,这里采用了随机漫步的方式
    q_rand = ceil(rand(1,2)*100);
    dist=[];
    
    % 计算机器人到终点的距离
    for j=1:size(q_rand,1)
        dist(j)=magnitude(q_rand(j,:),goal);
    end
    
    % 取距离终点最近的点
    [val,id]=min(dist);
    q_nearest=q_rand(id,:);
    
    % 判断是否与障碍物相交
    if collisionDetect(q_nearest,obstacles)
        continue;
    end
    
    % 如果与终点距离小于5,结束搜索
    if (val<5)
         quiver(q_nearest(1),q_nearest(2),0,10,0,'filled','MaxHeadSize', 0.8); 
         hold all;
         break;
    end
    
    % 计算q_new
    x=q_nearest;
    y=goal;
    unitvector=(y-x)/norm(y-x);
    angle=atan2(unitvector(2),unitvector(1));
    mindis=10;
    dis=2*mindis;
    q_new=[];
    
    % 随机搜索区域范围内有没有最近距离的点
    for j=-pi/3:0.1:pi/3
        l=mindis*[cos(angle+j),sin(angle+j)];
        if l(1)boundary(2) || l(2)boundary(4)
            continue
        end

        if collisionDetect(l,obstacles)
            continue
        end
        
        if magnitude(l,goal)<dis
            dis=magnitude(l,goal);
            q_new=l;
        end
    end
    
    % 如果没有最近距离的点,在搜索范围内随机
    if norm(q_new)==0
        q_new=10*unitvector+q_nearest;
    end
    
    % 根据人工势场法计算机器人向目标前进的方向
    delta=0.5;
    eta=2;
    fgoal=(goal-q_new)/magnitude(goal-q_new)*delta;
    fobs=frep(q_new,obstacles,eta);
    fq=fgoal+fobs;
    q=q_new+fq;
    
    % 如果与障碍物相交,舍弃该点
    if collisionDetect(q,obstacles)
        continue
    end
    
    % 绘制机器人运动轨迹
    quiver(q_nearest(1),q_nearest(2),q(1)-q_nearest(1),q(2)-q_nearest(2),0,'filled','MaxHeadSize', 0.8); 
    hold all;
end

% 确定两点距离
function dist= magnitude(x,y)
dist= sqrt((y(1)-x(1))^2+(y(2)-x(2))^2);
end

% 计算避障向量
function fobs= frep(q,obstacles,eta)
n=size(obstacles,1);
fobs=[0,0];
for i=1:n
    p=obstacles(i,1:2);
    d=magnitude(p,q);
    if d<obstacles(i,3)
        fobs=fobs-(q-p)*eta*(1/d-1/obstacles(i,3))/(d^3);
    end        
end
fobs=fobs';
end

% 判断是否与障碍物相交
function collision= collisionDetect(q,obstacles)
collision=0;
for i=1:size(obstacles,1)
    if magnitude(q,obstacles(i,1:2))<obstacles(i,3)
        collision=1;
        return;
    end
end
end

三、代码详解

上面的程序是一个完整的人工势场法实践代码，接下来我们将对代码中的每个部分进行详细解释。

1. 初始化程序

在程序的第一行到第五行，我们清空了所有变量并关闭了所有图形界面。这样做的目的是为了保证每次程序的运行都是从零开始的，避免出现一些难以查找的问题。

function artificialPotentialField
clear all; clc; close all;

2. 设置目标点、机器人起点和边界

由于人工势场法是一种路径规划方法，所以我们需要事先设定目标点、机器人起点和边界。这些参数的设置需要根据实际情况进行调整，我们在这里设置了一个二维障碍物数组。

goal=[90 90];
q_start=[10 10];
boundary=[0 100 0 100];
obstacles= [30 30 10;
               50 50 10;
               70 70 10];

3. 随机搜索

随机搜索是人工势场法的一个重要部分，它用来确定机器人当前所处的位置。在这个实例中，我们采用了随机漫步的方式进行搜索，代码如下：

for i=1:5000
    q_rand = ceil(rand(1,2)*100);
    dist=[];
    for j=1:size(q_rand,1)
        dist(j)=magnitude(q_rand(j,:),goal);
    end
    [val,id]=min(dist);
    q_nearest=q_rand(id,:);
    if collisionDetect(q_nearest,obstacles)
        continue;
    end
    if (val<5)
         quiver(q_nearest(1),q_nearest(2),0,10,0,'filled','MaxHeadSize', 0.8); 
         hold all;
         break;
    end

在这里，我们通过随机漫步的方式找到了机器人的当前位置，并计算了机器人和目标点的距离。如果机器人和障碍物相交，我们就跳过这一步，再重新进行一次随机漫步，直到找到不与障碍物相交的点为止。如果机器人到目标点的距离小于5，我们就认为机器人到达了终点，直接绘制机器人运动轨迹并退出程序。

4. 计算q_new

计算q_new的过程与随机搜索类似，我们从q_nearest开始向目标点进行探索，最终找到一个最近距离的点q_new，代码如下：

x=q_nearest;
y=goal;
unitvector=(y-x)/norm(y-x);
angle=atan2(unitvector(2),unitvector(1));
mindis=10;
dis=2*mindis;
q_new=[];
for j=-pi/3:0.1:pi/3
    l=mindis*[cos(angle+j),sin(angle+j)];
    if l(1)boundary(2) || l(2)boundary(4)
        continue
    end
    if collisionDetect(l,obstacles)
        continue
    end
    if magnitude(l,goal)<dis
        dis=magnitude(l,goal);
        q_new=l;
    end
end
if norm(q_new)==0
    q_new=10*unitvector+q_nearest;
end

在这里，我们首先计算了目标点和当前点q_nearest的单位向量unitvector，然后设置了一个最小距离mindis。随后，我们定义了一个角度范围[-pi/3, pi/3]，通过遍历这个范围内的点来找到最近距离的点q_new。如果在随机探索范围内找不到最近距离的点，我们就通过机器人当前的前进方向来设置一个默认的q_new。

5. 人工势场法求解机器人运动方向

在人工势场法中，我们需要计算机器人的向目标点前进的方向fgoal和避障方向fobs，然后将两个方向相加得到机器人的运动方向fq。最后，通过将q_new和fq相加，得到机器人的新位置q，代码如下：

delta=0.5;
eta=2;
fgoal=(goal-q_new)/magnitude(goal-q_new)*delta;
fobs=frep(q_new,obstacles,eta);
fq=fgoal+fobs;
q=q_new+fq;

其中，delta和eta是两个重要的参数，用来控制机器人向目标点和避免障碍物的方向。在我们的代码中，delta=0.5，eta=2。

6. 碰撞检测

碰撞检测是人工势场法中不可或缺的一个部分，它可以帮助机器人避开障碍物。在我们的代码中，我们通过判断机器人是否与障碍物相交来进行碰撞检测，代码如下：

function collision= collisionDetect(q,obstacles)
collision=0;
for i=1:size(obstacles,1)
    if magnitude(q,obstacles(i,1:2))<obstacles(i,3)
        collision=1;
        return;
    end
end
end

如果机器人与障碍物相交，我们就返回1，否则返回0。

7. 避障向量计算

在计算避障方向fobs时，我们通过计算机器人与障碍物之间的距离，来调整机器人的运动方向。具体来说，我们用下面的公式来计算fobs：

fobs=(q-p)*eta(1/d-1/ro)/(d^3)

其中，q表示机器人的当前位置，p表示障碍物的位置，d表示机器人与障碍物之间的距离，ro表示障碍物的触发距离，eta表示控制避障力度的参数。

在我们的代码中，计算避障向量的函数如下：

function fobs= frep(q,obstacles,eta)
n=size(obstacles,1);
fobs=[0,0];
for i=1:n
    p=obstacles(i,1:2);
    d=magnitude(p,q);
    if d<obstacles(i,3)
        fobs=fobs-(q-p)*eta*(1/d-1/obstacles(i,3))/(d^3);
    end        
end
fobs=fobs';
end

四、总结

本文简要介绍了人工势场法的实现方法，并通过一个完整的Matlab代码示例，对人工势场法的各个方面进行了详细的介绍和阐述。通过本文的学习，相信读者已经可以理解人工势场法的基本原理和实现方式，并可以根据自己的需要进行代码的修改和运用。