人工勢場法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代碼示例，對人工勢場法的各個方面進行了詳細的介紹和闡述。通過本文的學習，相信讀者已經可以理解人工勢場法的基本原理和實現方式，並可以根據自己的需要進行代碼的修改和運用。