matlab練習程序（人工勢場法）

該方法也是一種路徑規划算法，不過障礙物過多的時候建立勢場可能比較耗時，而且容易陷入局部最優。

算法流程如下：

1. 對於柵格場景中每一個像素分別計算到終點的距離，距離越大，則對該像素賦值越大，結束得到引力場。

2. 對於柵格場景中每一個像素分別計算到所有障礙物的距離，距離越大，則對該像素賦值越小，結束得到斥力場。

3. 引力場和斥力場相加得到總人工勢場。

4. 得到人工勢場后，從起始位置用梯度下降或者鄰域搜索法找到一條路徑到結束點。

這里建立勢場只用到了障礙物最外層邊界，如果障礙物所有像素全部用來計算，太過於耗時了。路徑尋找用了鄰域搜索。

matlab代碼如下：

clear all;
close all;
clc;

img = imread('map.png');
imshow(img);
[h,w] = size(img);

img1 = 255-(img-imerode(img,ones(3)));  %求邊界
figure;
imshow(img1);

p = ginput();
hold on;
plot(p(:,1),p(:,2),'r.')

grav = zeros(h*w,1);                    %引力場
repu = zeros(h*w,1);                    %斥力場

ind=zeros(h*w,2);
indobs=zeros(sum(sum((255-img1)>0)),2);
num = 1;
for i=1:h
    for j=1:w
        ind((i-1)*w+j,:) = [j i];
        if img1(i,j) ==0   
            indobs(num,:)=[j i];
            num = num + 1;
        end
        d = norm([j,i]-p(2,:));             %根據距離判斷，距離越小，值越小
        grav((i-1)*w+j) = 2*d;              %建立引力場,2是引力系數
    end
end

for i=1:length(indobs)
    t = ind - repmat(indobs(i,:),length(ind),1);    %根據距離判斷，距離越小，值越大
    repu = repu + 5./sqrt(t(:,1).^2+t(:,2).^2);       %建立斥力場,5是斥力系數
end

repu(repu>500) = 500;
re = grav + repu;                           %綜合場

imgre = reshape(re,[h,w])';

nei=[ -1 -1; -1 0; -1 1;
       0 -1; 0 0;  0 1;
       1 -1; 1 0;  1 1];
path=floor(p(1,:));
pre = [0 0];
while norm(path(end,:)-p(2,:))>2    
    pc = path(end,:);
    im = imgre(pc(end,2)-1:pc(end,2)+1,pc(end,1)-1:pc(end,1)+1);
    [~,ind] = min(reshape(im,9,1));         %八鄰域找最小值
    
    pre = path(end,:);   
    path = [path;path(end,:)+nei(ind,:)];
    
    if  norm(path(end,:)-pre) == 0 || ...           %超出邊界或陷入局部最優了
        path(end,1)==1 || path(end,2) ==1 || ...
        path(end,1)==w || path(end,2) ==h;
        break;
    end
end

hold on;
plot(path(:,1),path(:,2));
figure;
mesh(imgre);

結果如下：

原圖：

人工勢場：

規划路徑：

不過該方法有時候會陷入局部最優，比如下面這個勢場：

規划路徑：