人工势场法
文章目录
- 前言
- 一、人工势场法
- 二、简要理解
- 1.示例
- 2.代码
- 总结
前言
路径规划是移动机器人领域的一个重要组成部分，传统的路径规划代表算法包括 A*算法、Dijkstra 算法、人工势场法以及仿生学的蚁群算法。人工势场法是机器人路径规划算法中一种简单有效的方法。人工势场法的基本思想是在移动机器人的工作环境中构造一个人工势场，势场中包括斥力极和吸引极,不希望机器人进入的区域和障碍物定义为斥力极,目标及建议机器人进入的区域定义为引力极,使得在该势场中的移动机器人受到其目标位姿引力场和障碍物周围斥力场的共同作用,朝目标前进。
提示：以下是本篇文章正文内容，下面案例可供参考
一、人工势场法
人工势场法（Artificial Potential Field）简称 APF，属于局部路径规划中常使用的
一种方法。将传统力学中“场”的概念引入该方法，假设让智能体在这种虚拟力场下
进行运动。如何设计引力场影响着该算法的性能，同时存在容易陷入局部极小点的局
限性。
二、简要理解
1.示例：
概念图如图 所示，人工势场就像构建了一个吸铁石，包括引力场和斥力场。黑色为障碍物，箭头为智能体要运动的方向，目标点为“Goal”。智能体按照箭头的方向到达“Goal”，目标点像有着“吸引力”一样吸引着智能体靠近。而在障碍物附近，智能体逆着箭头的方向，好像对智能体产生“排斥力”。智能体前进的方向就是这两种力的合力的方向。
机器人在工作空间中的位置为X=[x，y]T通常的目标函数势场函数（引力场函数）为：
上面公式部分我借鉴了别人的博客，地址：https://me.csdn.net/weixin_43010548
2.代码：
下面方法也是一种路径规划算法，不过障碍物过多的时候建立势场可能比较耗时，而且容易陷入局部最优。
算法流程如下：
1. 对于栅格场景中每一个像素分别计算到终点的距离，距离越大，则对该像素赋值越大，结束得到引力场。
2. 对于栅格场景中每一个像素分别计算到所有障碍物的距离，距离越大，则对该像素赋值越小，结束得到斥力场。
3. 引力场和斥力场相加得到总人工势场。
4. 得到人工势场后，从起始位置用梯度下降或者邻域搜索法找到一条路径到结束点。
这里建立势场只用到了障碍物最外层边界，如果障碍物所有像素全部用来计算，太过于耗时了。路径寻找用了邻域搜索。
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);
该处使用的url网络请求的数据。
总结
代码摘自：https://www.cnblogs.com/tiandsp/p/12270897.html