%%?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