function?Theta?=?compute_angle（XjXgrrepn）
%X????????????當(dāng)前位姿
%Xgr??????????目標(biāo)點(diǎn)、障礙點(diǎn)位姿
%repn?????????障礙數(shù)目
%ret??????????目標(biāo)點(diǎn)、障礙點(diǎn)與X軸夾角
%?%?Xo?=?[-80?-80]
%?%?attk?=?1;?%計(jì)算引力需要的增益系數(shù)
%?%?repk?=?2;?%計(jì)算斥力的增益系數(shù)
%?%?Po?=?100;?%障礙影響距離，當(dāng)障礙和車的距離大于這個(gè)距離時(shí)，斥力為0，即不受該障礙的影響。也是自己設(shè)定。
%?%?repn?=?5;%障礙個(gè)數(shù)
%?%?step?=?10;?%步長(zhǎng)
%?%?J?=?200;?%迭代次數(shù)
for_Theta=zeros（repn+13）;
for?i?=?1?:?repn+1
??????deltax?=?Xgr（i1）-Xj（1）;
??????deltay?=?Xgr（i2）-Xj（2）;
??????deltaz?=?Xgr（i3）-Xj（3）;
??????r?=?sqrt（deltax^2?+?deltay^2+deltaz^2?）;?
????%%%%%%應(yīng)該是arccos（）????????????????????

????for_Theta（i1）?=?asin（deltax/r）;%asin（）為反正弦（）
????for_Theta（i2）?=?asin（deltay/r）;
????for_Theta（i3）?=?asin（deltaz/r）;
????Theta=for_Theta;
????

end

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????文件?????????843??2017-05-06?13:38??compute_angle.m
?????文件?????????978??2017-05-06?13:14??compute_attract.m
?????文件????????3140??2017-05-06?13:03??compute_repulsion.m
?????文件?????????788??2017-05-06?12:22??draw_sphere.m
?????文件????????5606??2017-11-10?20:42??multi_auv.m
?????文件????????5606??2016-11-20?19:11??multi_auv1.m
?????文件????????6362??2016-11-21?19:57??multi_auv2.m
?????文件????????6339??2016-11-21?19:56??multi_auv3.m
?????文件????????5195??2016-11-21?20:10??multi_auv4.m
?????文件?????????389??2016-11-21?20:34??obstacle.m