MATLAB Code of Artificial Potencial Field Method for Robot Path Planning

MATLAB Code of Artificial Potencial Field Method for Robot Path Planning

%% function apf_3
% 人工势场法进行水下机器人路径规划，考虑体积范围
% 计算势函数，画出运动轨迹图像

% 作者：李欣
% 单位：上海海事大学水下机器人与智能系统实验室
% Date: 2008-10-30
% Modified: 2010-1-5 2014-11-12 2018-3-5
% Shanghai China

close all; 

% 设置工作区域
xmin = [0; 0];  
xmax = [50;50];

% Maximum number
Nsteps = 600;

%设置机器人的参数%
% 选定方向上机器人运动步长参数
lambda = 0.1;
Ns=30; 
r = 1; 
xs=0*ones（2Ns）; 
Jo（:1）=0*ones（Ns1）; 
Jg（:1）=0*ones（Ns1）; 
J（:1）=0*ones（Ns1）; 
theta（:1）=0*ones（Ns1）;

for m=2:Ns
    theta（m1）=theta（m-11）+（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（12）;
nt = 20; % Tar运动步数
nr = 20; % Ro的速度，决定能否跟的上
x1 = 1;
y1 = 1;
g = 1;
h = 0;
distrt = 0; % 计算距离，终止条件
distro = 0*ones（2obNum）; % 计算距离，避让临界
t = 0;
na = 0;

% 设置机器

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-03-20 06:15  APF_Code-master\
     文件         280  2018-03-20 06:15  APF_Code-master\README.md
     文件      126388  2018-03-20 06:15  APF_Code-master\apf-1.png
     文件       14545  2018-03-20 06:15  APF_Code-master\apf-2.png
     文件       87738  2018-03-20 06:15  APF_Code-master\apf.png
     文件        4076  2018-03-20 06:15  APF_Code-master\apf_basic.m
     文件         475  2018-03-20 06:15  APF_Code-master\goalfunction.m
     文件         568  2018-03-20 06:15  APF_Code-master\obstaclefunction.m