【路径规划】基于人工势场求解二维障碍路径规划问题matlab源码

1 简介

 在 1986 年，专家 Oussama Khatib 提出了实时性好、改进空间很大、应用领域也极其广泛的人工势场法去解决机器人避障及路径规划问题，人工势场法的工作原理是：将机器人所在的工作空间虚拟为充满引力势场和斥力势场的空间。目标点与机器人产生的相互作用场为引力势场，障碍物与机器人产生的相互作用场为斥力场，机器人在引力势场和斥力势场的合势场作用下，沿着势函数下降的方向运动，搜索无碰撞最优路径。具体的实现方法是对虚拟的引力场和斥力场分别求负梯度之后，可以得到引力和斥力的大小和方向，机器人在引力和斥力的合力作用下行进至目标点。基于人工势场法的机器人避障最大的特点就是边行走边利用传感器探测可行的避障路径，算法本身的特点就奠定了其相对于其他避障算法对未知环境的适应性较强的优势，并且人工势场法的计算量小、速度快、便于其他人理解，把抽象的机器人避障算法通过建模表现为受力分析、数学计算、向量求解等问题，便于改进和优化，避障轨迹平滑，缺点是会出现目标不可达，局部极小值两种问题，图 1.10 为人工势场法避障原理图，所以人工势场法非常适合应用于未知环境下对避障的实时性要求较高的避障问题求解中去。

2 部分代码

%% function apf_3

% 人工势场法进行水下机器人路径规划，考虑体积范围

% 计算势函数，画出运动轨迹图像

close all; 

% 设置工作区域

xmin = [0; 0];  

xmax = [50;50];

% Maximum number

Nsteps = 600;

%设置机器人的参数%

% 选定方向上机器人运动步长参数

lambda = 0.1;

Ns=30; 

r = 1; 

xs=0*ones(2,Ns); 

Jo(:,1)=0*ones(Ns,1); 

Jg(:,1)=0*ones(Ns,1); 

J(:,1)=0*ones(Ns,1); 

theta(:,1)=0*ones(Ns,1);

for m=2:Ns

    theta(m,1)=theta(m-1,1)+(pi/180)*(360/Ns); 

end 

  

% 设置目标(Goal/Target)位置坐标

P_Goal=[25; 25];

obstacles = [6 20 11 16 18 19 ;6 16 17 14 11.9 19];

Mat = size(obstacles); %障碍物点数

obNum = Mat(1,2);

nt = 20; % Tar运动步数

nr = 20; % Ro的速度，决定能否跟的上

x1 = 1;

y1 = 1;

g = 1;

h = 0;

distrt = 0; % 计算距离，终止条件

distro = 0*ones(2,obNum); % 计算距离，避让临界

t = 0;

na = 0;

% 设置机器人初始位置坐标

P_Ro=[5; 5]; 

w1 = 1; 

w2 = 5; 

P_Ro(:,2:Nsteps) = 0*ones(2,Nsteps-1);

% 画出势场

xx=0:35/100:35; 

yy=xx; 

% 计算障碍物势函数 

 for jj=1:length(xx) 

    for ii=1:length(yy) 

       op(ii,jj)=obstaclefunction([xx(jj);yy(ii)],w1,obstacles); 

    end 

 end 

% 计算目标势函数

 for jj=1:length(xx) 

    for ii=1:length(yy) 

        gp(ii,jj)=goalfunction([xx(jj);yy(ii)],P_Goal(:,1),w2); 

    end 

 end 

P_RoA = P_Ro(1,1);

P_RoB = P_Ro(2,1);

P_RoC = P_Ro(1,1);

P_RoD = P_Ro(2,1);

potential = gp - op;

figure;

plot(P_Goal(1,1),P_Goal(2,1),'+','MarkerSize',10);

hold on

plot(P_Goal(1,1),P_Goal(2,1),'o','MarkerSize',22);

xlabel('x'); 

ylabel('y'); 

plot(obstacles(1,:),obstacles(2,:),'o', 'MarkerEdgeColor','r','MarkerSize',10); 

contour(xx,yy,potential,90);

axis([0 35 0 35]); 

hold off

figure,

plot(P_Goal(1,1),P_Goal(2,1),'+','MarkerSize',10);

hold on

plot(P_Goal(1,1),P_Goal(2,1),'o','MarkerSize',22);

xlabel('x'); 

ylabel('y'); 

plot(obstacles(1,:),obstacles(2,:),'o','MarkerSize',22); 

plot(obstacles(1,:),obstacles(2,:),'o', 'MarkerEdgeColor','r','MarkerSize',10); 

 contour(xx,yy,op,20);

 axis([0 35 0 35]); 

hold off

figure, 

plot(P_Goal(1,1),P_Goal(2,1),'+','MarkerSize',10);

hold on

plot(P_Goal(1,1),P_Goal(2,1),'o','MarkerSize',22);

xlabel('x'); 

ylabel('y'); 

plot(obstacles(1,:),obstacles(2,:),'o','MarkerSize',22); 

plot(obstacles(1,:),obstacles(2,:),'o', 'MarkerEdgeColor','r','MarkerSize',10); 

 contour(xx,yy,gp,50);

 axis([0 35 0 35]); 

hold off

% Robot运动路径仿真过程

P_Goal_1 = P_Goal(:,1);

for k=1:Nsteps

    % 设定运动边界

    P_Ro(:,k) = min(P_Ro(:,k),xmax); 

    P_Ro(:,k) = max(P_Ro(:,k),xmin); 

    for m=1:Ns 

        xs(:,m) = [P_Ro(1,k)+r*cos(theta(m,1)); P_Ro(2,k)+r*sin(theta(m,1))];      

        % 计算是否进入障碍物的作用范围

        for t = 1:obNum

        distro(:,t) = xs(:,m) - obstacles(:,t);

        end

        sum1 = sum(distro.^2);

        if min(sum1) < 1.69 % 设定障碍物作用距离的平方值

           Jo(m,1) = 100;

        else

           Jo(m,1) = obstaclefunction(xs(:,m),w1,obstacles); 

        end

        Jg(m,1) = goalfunction(xs(:,m),P_Goal_1,w2); 

        J(m,1)= Jg(m,1) - Jo(m,1);

    end 

    

    [val,num] = max(J); 

    distrt = P_Ro(:,k) - P_Goal_1; 

    if sum(distrt.^2) > 0.5        

        P_Ro(:,k+1) = [P_Ro(1,k)+lambda*cos(theta(num,1)); P_Ro(2,k)+lambda*sin(theta(num,1))]; 

    else

        break;

    end

    P_RoA = P_Ro(1,k+1);

    P_RoB = P_Ro(2,k+1);

    P_RoC = P_Ro(1,1:k+1);

    P_RoD = P_Ro(2,1:k+1);

    Deltalambda=0.1*lambda*(2*rand-1); 

    Deltatheta=2*pi*(2*rand-1); 

    P_Ro(:,k+1)=[P_Ro(1,k+1)+Deltalambda*cos(theta(num,1)+Deltatheta); P_Ro(2,k+1)+Deltalambda*sin(theta(num,1)+Deltatheta)]; 

end

figure;

% 障碍物势场

plot(obstacles(1,:),obstacles(2,:),'o','MarkerSize',22);

hold on;

% 机器人运动路径

plot(P_Ro(1,1:k) ,P_Ro(2,1:k) ,'r-'); 

hold on

plot(P_Ro(1,1),P_Ro(2,1),'s', 'MarkerFaceColor','g','MarkerSize',10);

plot(P_Goal(1,1),P_Goal(2,1),'+','MarkerSize',10);

plot(P_Goal(1,1),P_Goal(2,1),'o','MarkerSize',22);

axis([0 35 0 35]); 

xlabel('x'); 

ylabel('y'); 

title('Robot''s path with obstacles'); 

hold off

3 仿真结果

4 参考文献

[1]邓学强, DENG, Xue-qiang, School, of, & Computer等. (2014). 基于改进人工势场法的移动机器人路径规划. 山东理工大学学报(自然科学版), 01(v.28;No.134), 42-45.