人工势场法避障（三维）

close all clear all tic %% %========================================================================== %初始化 %========================================================================== P0 = [0;0;0];%起点位置 k = 15;%引力增益系数 m = 10;%斥力增益系数 po = 1;%障碍影响距离 n = 7;%障碍个数 l = 0.05;%每次采样的移动步长 t = 2000;%循环次数 K = 0; Tar = [10;10;10];%%目标点坐标 Obs = [10 10 10;1 1.2 1;3 2.5 3;4 4.5 4;3 6 3;6 2 6;6 6 6;8 8.5 8]';%目标点与障碍位置 第一列是目标点 P = zeros(3,t);%位置矩阵 F = zeros(3,t);%x y z方向上的力储存矩阵 F_a = zeros(3,t);%引力 FF_a = zeros(3,n); FF_r = zeros(3,n); Theta = zeros(3,t,n+1);%当前位置与目标的连线与x y z 轴的夹角 P(:,1) = P0; %% %========================================================================== %计算引力与斥力 %========================================================================== for i = 1:t %i = 1; %计算当前位置与目标的连线与x y z 轴的夹角 %当前位置向量 目标与障碍向量 目标数 Theta = compute_angel(P(:,i),Obs,n,Theta,i);%得到的夹角统一角度为逆时针方向 %计算目标点对对象的引力 F_aa = compute_Attract(P(:,i),Obs(:,1),Theta(:,i,1),k); F_a(:,i) = F_aa'; %计算障碍对对象的斥力 for j = 1:n [FF_a(:,j),FF_r(:,j)] = compute_repulsion(P(:,i),Obs(:,j+1),Obs(:,1),Theta(:,i,j+1),Theta(:,i,1),m,po); end %计算合力 for j = 1:n F(:,i) = FF_a(:,j)+FF_r(:,j)+F(:,i); end F(:,i) = F_a(:,i)+F(:,i); %计算下一步位置 w = F(:,i)./sqrt(F(1,i)^2+F(2,i)^2+F(3,i)^2); P(:,i+1) = P(:,i)+l*w; % Theta_F = compute_angel_F(F(:,i)); % % P(1,i+1) = P(1,i) + l*cos(Theta_F(1)); % P(2,i+1) = P(2,i) + l*cos(Theta_F(2)); % P(3,i+1) = P(3,i) + l*cos(Theta_F(3)); %判断是否达到目标点 if((P(1,i+1)-Tar(1))>0)&((P(2,i+1)-Tar(2))>0)&((P(3,i+1)-Tar(3))>0) K = i; break; end end %% %========================================================================== %画图 %========================================================================== figure(1); plot3(Obs(1,2:8),Obs(2,2:8),Obs(3,2:8),'o');hold on; plot3(Obs(1,1),Obs(2,1),Obs(3,1),'O'); plot3(P(1,1:K),P(2,1:K),P(3,1:K),'.b'); toc