qs=[00]‘;
qf=[4050]‘;?????%起始點和目標點
d=3;???????????????%步長
N1?=?1;???????????%T1中的點的個數
T1（:N1）?=?qs;

r=3;
nr?=?10;
n_rand?=?0;
flag?=?0;
z_real1=zeros（22*nr）;
theta?=?0:pi/200:2*pi;?????%畫障礙物圓以及留出一定閾值后的障礙區域
for?i6=1:nr???????????????????????%生成10個障礙物
????x0（1i6）=randi（4011）;?
????y0（1i6）=randi（5011）;?
????circ_yx（:i6）?=?[x0（1i6）y0（1i6）]‘;
????xr（i6:）?=?x0（1i6）+r*cos（theta）;??????????
????yr（i6:）?=?y0（1i6）+r*sin（theta）;
????plot（xr（i6:）yr（i6:）‘r‘）;
????hold?on;
end

for?i0?=?1:30
????x_rand?=?randi（4011）;
????y_rand?=?randi（5011）;
????n_rand?=?n_rand+1;
????plot（x_randy_rand‘ko‘）;
????hold?on;
????z_rand（:n_rand）?=?[x_randy_rand]‘;???%在空間中生成隨機點
????for?i7?=?1:nr
????????if?（norm（circ_yx（:i7）-qf）?<=r）
????????????flag?=?flag+1;
????????else
????????end
????????if?（flag?==?1）
????????????break;
????????end
????end
?????if?（flag?==?1）
????????????break;
?????end
????nj?=?0;
????nT1?=?size（T12）;?????????????????%T1中點的個數
????z_near（:1）?=?T1（:1）;
????for?i1?=?1:nT1
????????if?（norm（z_near（:1）?-?z_rand（:n_rand））>norm（T1（:i1）-z_rand（:n_rand）））
????????????z_near（:1）?=?T1（:i1）;
????????else
????????end
????end
????%plot（z_near（11）z_near（21）‘b+‘）;
????%hold?on;
????k=（y_rand-z_near（21））/（x_rand-z_near（11））;
????b=y_rand-k*x_rand;
????if?（z_near（11）<=x_rand）
????????x=z_near（11）:0.01:x_rand;
????else
????????x=x_rand:0.01:z_near（11）;
????end
????y=k*x+b;???????%起始點與隨機點的連線
????plot（xy‘b‘）;
????hold?on;
????for?i2=1:nr
????????[xjyj]=solve（‘yj=k*xj+b‘‘（xj-x0）^2+（yj-y0）^2=r^2‘‘xj‘‘yj‘）;??????????%求障礙物與連線的交點
?????????xj=eval（xj）;
?????????yj=eval（yj）;
?????????x_real=real（xj）;
?????????y_real=real（yj）;
?????????x_img=imag（xj）;?????????%交點的虛部
?????????y_img=imag（yj）;
??????????if?（x_img（1i2）==0）????%連線與障礙物有交點
??????????????if?（（（x_real（1i2）>x_rand）?&?（x_real（2i2）x_rand）））
??????????????????nj=nj+1;???????????%交點個數
??????????????????if?（（（x_real（1i2）-z_near（11））^2+（y_real（1i2）-z_near（21））^2）<=（（（x_real（2i2）-z_near（11））^2+（y_real（2i2）-z_near（21））^2）））
????????????????????????z_real1（1nj）=x_real（1i2）;
????????????????????????z_real1（2nj）=y_real（1i2）;
??????????????????else
???????????????????????z_real1（1nj）=x_real（2i2）;
???????????????????????z_real1（2nj）=y_real（2i2）;
??????????????????end
??????????????else
??????????????????if?（z_near（11）<=x_rand）
??????????????????????if?（（x_real（1i2）<=x_rand）?&?（x_real（2i2）<=x_rand）?&?（x_real（1i2）>=z_near（11））?&?（x_real（2i2）>=z_near（11）））
???????????????????????????nj=nj+2;
???????????????????????????????if?（x_real（1i2）>x_real（2i2））
?????????????????????????????????????z_real1（1nj-1）=x_real（2i2）;
?????????????????????????????????????z_real1（2nj-1）=y_real（2i2）;
?????????????????????????????????????z_real1（1nj）=x_real（1i2）;
?????????????????????????????????????z_real1（2nj）=y_real（1i2）;
?????????????????