人工势场路径规划-matlab代码

Posted 2021-05-16 墨翰

一、人工势场算法原理

人工势场法是广泛应用于机器人、智能车领域中的一种路径规划算法，其原理是将智能车在行驶环境中的运动转化为智能车在人为设定的抽象势场中的运动，抽象势场由引力、斥力两大势场组成。 将引力势场和斥力势场进行叠加即为合力势场，智能车在合力势场的作用下行驶，行驶方向即为势能下降的方向。

二、matlab代码实现效果（圆形障碍物附代码）

步长未调整，所以路径并不光滑

clc
clear
close all
%% 输入障碍物坐标、起始点、目标点,图1是示意图
ob=[25 27];
q_start=[1 1];
q_end=[49 49];
Z1=[25 27];
Z1_R=5;
%% 传统人工势场
k=2;                                           %引力增益系数
m=3;                                          %斥力增益系数
L=1;                                          %机器人步长
P0=20;                                           %障碍物的影响距离
t_max=1000;                              %最大搜索步数
deta=5;                                    %停止搜索条件
n=size(ob,1);                               %n是障碍数目
Q=q_start;                                   %将第一步放入路径中
for i=1:t_max
    disp(['迭代次数',num2str(i),'迭代进度',num2str(i/t_max*100),'%'])
    X=Q(end,:);                             %X是当前点坐标，第i步
    R=sqrt((X(1)-q_end(1,1))^2+(X(2)-q_end(1,2))^2);                    %路径点和目标的距离
    U1=q_end-X;
    F_Y=k*R*U1/sqrt(U1(1)^2+U1(2)^2);                                            %计算引力F_Y
   
    F_C=[0,0];
    for j=1:n                                                                                               %所有斥力的总合F_C
       F_C=F_C+F_c{j};        
    end
    F_Z=F_Y+F_C;                                                                              %计算合力，即前进方向
    X_new=X+F_Z/sqrt(F_Z(1)^2+F_Z(2)^2)*L;                                  %沿合力行走步长L
    Q=[Q;X_new];                                                                                    %将新节点加入路径Q中
    if sqrt((q_end(1)-X_new(1))^2+(q_end(2)-X_new(2))^2)<deta     %新节点与终点距离小于步长L即跳出
        Q=[Q;q_end];
        plot(Q(:,1),Q(:,2),'-r');
        break
    end
    plot(q_start(:,1),q_start(:,2),'.b','MarkerSize',20);
    hold on
    plot(q_end(:,1),q_end(:,2),'.g','MarkerSize',20);
    hold on
    plot(ob(:,1),ob(:,2),'.k','MarkerSize',20);
    hold on
    plot(Q(:,1),Q(:,2),'-r');
    pause(0.1)
    r=Z1_R;%设置半径为20
    theta=0:pi/100:2*pi;%以pi/100为圆心角画圆
    x=Z1(1)+r*cos(theta);%圆心横坐标为40
    y=Z1(2)+r*sin(theta); %圆心纵坐标为40
    plot(x,y,'k:');%画圆
end
%% 画出最终路径
plot(Z1(:,1),Z1(:,2),':b')
hold on

Z1=[Z1;Z1(end,:)];
set(legend,'Position',[0.6958 0.7151 0.2071 0.2095])
title('传统人工势场算法')
axis([0,50,0,50])

三、matlab代码实现效果（矩形障碍物）

代码https://download.csdn.net/download/qq_44152192/18636203