%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%???全空間人工勢場說明???%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear?all
clc

x_goal=100;????y_goal=100;????%?小車目標點
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%????????????相關參數????????????%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k_att=1;???????k_rep=20;????%?勢場參數?（?引力系數?斥力系數?）
L0=100;?????????????????????%?L0為障礙物區域可對機器人的運動產生影響的最大距離
LO_min=2;???????????????????%?L0_min為沿墻行走的安全距離
U_max=5000;????FF_max=200;
obs_maxNum=4;???????????????%?矩形障礙物的最大數目

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%??????讀取障礙物、繪制障礙物????%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DataDir?????=[‘.\dataRect‘];??????????????????????%?directory?of?data
DataOrgName?=[‘\dataObs\obs11.txt‘];??????????????%?File?of?Obstacle
copyfile（[?DataDir?DataOrgName?]?‘.\dataRect\temp\obs.txt‘）;
load?-ascii?.\dataRect\temp\obs.txt;
delete（‘.\dataRect\temp\obs.txt‘）;
for?i=1:obs_maxNum
????x_q（i）=obs（i1）;????y_q（i）=obs（i2）;????x_L（i）=obs（i3）;????y_w（i）=obs（i4）;????theta（i）=obs（i5）;
end
Rect_x=zeros（obs_maxNum5）;??Rect_y=zeros（obs_maxNum5）;
for?i=1:obs_maxNum??????????????????????????%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%???????????????
????x1_（i）=x_q（i）;?????????????????????????????y1_（i）=y_q（i）;
????x2_（i）=x_q（i）+x_L（i）*cos（theta（i））;????????y2_（i）=y_q（i）+x_L（i）*sin（theta（i））;
????x3_（i）=x_q（i）+x_L（i）*cos（theta（i））+y_w（i）*cos（pi/2+theta（i））;
????y3_（i）=y_q（i）+x_L（i）*sin（theta（i））+y_w（i）*sin（pi/2+theta（i））;
????x4_（i）=x_q（i）+y_w（i）*cos（pi/2+theta（i））;???y4_（i）=y_q（i）+y_w（i）*sin（pi/2+theta（i））;
????Rect_x（i:）=[x1_（i）?x2_（i）?x3_（i）?x4_（i）?x1_（i）];??
????Rect_y（i:）=[y1_（i）?y2_（i）?y3_（i）?y4_（i）?y1_（i）];
end

x_min=0.0;??x_max=100.0;??y_min=0.0;???y_max=100.0;?????????%?繪制點陣區域
point_NumX=100;????point_NumY=100;??????????????????????????%?繪制點陣密度
result_x=zeros（point_NumX+11）;??result_y=zeros（point_NumY+11）;
result_U=zeros（point_NumY+1point_NumX+1）;??result_sF=zeros（point_NumY+1point_NumX+1）;
result_Dx=zeros（point_NumX+11）;??result_Dy=zeros（point_NumY+11）;
for?point_i=1:（point_NumX+1）
????x=x_min+（x_max-x_min）/point_NumX*（point_i-1）;
	for?point_j=1:（point_NumY+1）
????????y=y_min+（y_max-y_min）/point_NumY*（point_j-1）;????????%?繪制點陣

????????%?機器人與障礙物碰撞情況
????????obs_flag=zeros（1obs_maxNum）;
????????for?obs_i=1:obs_maxNum?
????????????if?inpolygon（xyRect_x（obs_i:）Rect_y（obs_i:））?>?0????%?點（xy）在矩形障礙物內（有碰撞發生）
????????????????obs_flag（i）=1;???????????????????????????%?機器人撞到第?i?個障礙物
????????????end
????????end
????????%?引力場計算
????????L_goal=sqrt（（x-x_goal）^2+（y-y_goal）^2）;??????????%?機器人到目標位置的距離
????????if?L_goal<1e-10
????????????L_goal=1e-10;????????????????????????????????%?防止除零錯誤發生
????????end
????????U_att=1/2*k_att*L_goal^2;????????????????????????%?機器人引力勢場
????????F_att=-k_att*[（x-x_goal）（y-y_goal）]

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????7182??2014-03-21?15:04??新建文件夾\BUG_Potential.m

?????文件??????20901??2013-05-31?10:13??新建文件夾\Bug_Rect_3.m

?????文件???????3148??2013-06-27?15:34??新建文件夾\Bug_Rect_Plot.m

?????文件???????4325??2012-12-06?19:47??新建文件夾\calculate_obs1.m

?????文件???????2881??2014-03-21?15:12??新建文件夾\Point_to_Rect.m

?????文件???????9915??2013-06-27?17:21??新建文件夾\try_Bug_Rect_Plot.m

?????文件???????9920??2013-06-27?15:55??新建文件夾\try_Bug_Rect_Plot2.m

?????目錄??????????0??2014-05-05?08:59??新建文件夾

-----------?---------??----------?-----??----

????????????????58272????????????????????8