zoukankan      html  css  js  c++  java
  • 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);

    结果如下:

    原图:

    人工势场:

    规划路径:

    不过该方法有时候会陷入局部最优,比如下面这个势场:

    规划路径:

  • 相关阅读:
    ThinkPHP 3.2.2 实现持久登录 ( 记住我 )
    Java实现 LeetCode 20 有效的括号
    Java实现 LeetCode 20 有效的括号
    Java实现 LeetCode 19删除链表的倒数第N个节点
    Java实现 LeetCode 19删除链表的倒数第N个节点
    Java实现 LeetCode 19删除链表的倒数第N个节点
    Java实现 LeetCode 18 四数之和
    Java实现 LeetCode 18 四数之和
    Java实现 LeetCode 18 四数之和
    Java实现 LeetCode 17 电话号码的字母组合
  • 原文地址:https://www.cnblogs.com/tiandsp/p/12270897.html
Copyright © 2011-2022 走看看