%%?function?apf_3
%?人工勢(shì)場(chǎng)法進(jìn)行水下機(jī)器人路徑規(guī)劃，考慮體積范圍
%?計(jì)算勢(shì)函數(shù)，畫出運(yùn)動(dòng)軌跡圖像

%?作者：李欣
%?單位：上海海事大學(xué)水下機(jī)器人與智能系統(tǒng)實(shí)驗(yàn)室
%?Date:?2008-10-30
%?Modified:?2010-1-5?2014-11-12?2018-3-5
%?Shanghai?China

close?all;?

%?設(shè)置工作區(qū)域
xmin?=?[0;?0];??
xmax?=?[50;50];

%?Maximum?number
Nsteps?=?600;

%設(shè)置機(jī)器人的參數(shù)%
%?選定方向上機(jī)器人運(yùn)動(dòng)步長(zhǎng)參數(shù)
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?
??
%?設(shè)置目標(biāo)（Goal/Target）位置坐標(biāo)
P_Goal=[25;?25];
obstacles?=?[6?20?11?16?18?19?;6?16?17?14?11.9?19];
Mat?=?size（obstacles）;?%障礙物點(diǎn)數(shù)
obNum?=?Mat（12）;
nt?=?20;?%?Tar運(yùn)動(dòng)步數(shù)
nr?=?20;?%?Ro的速度，決定能否跟的上
x1?=?1;
y1?=?1;
g?=?1;
h?=?0;
distrt?=?0;?%?計(jì)算距離，終止條件
distro?=?0*ones（2obNum）;?%?計(jì)算距離，避讓臨界
t?=?0;
na?=?0;

%?設(shè)置機(jī)器

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????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