clear?all
%初始化車的參數(shù)
Xo=[0?0];%起點(diǎn)位置
k=15;%計算引力需要的增益系數(shù)
K=0;%初始化
m=5;%計算斥力的增益系數(shù)，都是自己設(shè)定的。
Po=2.5;%障礙影響距離，當(dāng)障礙和車的距離大于這個距離時，斥力為0，即不受該障礙的影響。也是自己設(shè)定。
n=7;%障礙個數(shù)
a=0.5;
l=0.1;%步長
J=200;%循環(huán)迭代次數(shù)
%如果不能實(shí)現(xiàn)預(yù)期目標(biāo)，可能也與初始的增益系數(shù)，Po設(shè)置的不合適有關(guān)。
%end
%給出障礙和目標(biāo)信息
Xsum=[10?10;1?1;3?3;4?4;3?6;6?2;5?5;8?8.5];%這個向量是（n+1）*2維，其中[10?10]是目標(biāo)位置，剩下的都是障礙的位置。
Xj=Xo;%j=1循環(huán)初始，將車的起始坐標(biāo)賦給Xj
%***************初始化結(jié)束，開始主體循環(huán)******************
for?j=1:J?%循環(huán)開始
Goal（j1）=Xj（1）;?%Goal是保存車走過的每個點(diǎn)的坐標(biāo)。剛開始先將起點(diǎn)放進(jìn)該向量。
Goal（j2）=Xj（2）;
%調(diào)用計算角度模塊
Theta=compute_angle（XjXsumn）;%Theta是計算出來的車和障礙，和目標(biāo)之間的與X軸之間的夾角，統(tǒng)一規(guī)定角度為逆時針方向，用這個模塊可以計算出來。

%調(diào)用計算引力模塊
Angle=Theta（1）;%Theta（1）是車和目標(biāo)之間的角度，目標(biāo)對車是引力。
angle_at=Theta（1）;%為了后續(xù)計算斥力在引力方向的分量賦值給angle_at
[FatxFaty]=compute_Attract（XjXsumkAngle0Pon）;?%計算出目標(biāo)對車的引力在xy方向的兩個分量值。
for?i=1:n
angle_re（i）=Theta（i+1）;%計算斥力用的角度，是個向量，因?yàn)橛衝個障礙，就有n個角度。
end

%調(diào)用計算斥力模塊
[FrerxxFreryyFataxxFata