首页 > 其他分享> > 2021-11-14

2021-11-14

2021-11-14 10:33:42 作者：互联网

三维人工势场路径规划
代码：
main：
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) + lcos(Theta_F(1));
% P(2,i+1) = P(2,i) + lcos(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

function [Theta] = compute_angel(P,T,n,Theta,t)
%UNTITLED4 此处显示有关此函数的摘要
% 此处显示详细说明
for i = 1:n+1
deltaX(i) = T(1,i)-P(1);
deltaY(i) = T(2,i)-P(2);
deltaZ(i) = T(3,i)-P(3);
r(i) = sqrt(deltaX(i)^2+deltaY(i)2+deltaZ(i)^2);

if deltaX(i)>0
    Theta(1,t,i) = acos(deltaX(i)/r(i));
else
    Theta(1,t,i) = pi-acos(deltaX(i)/r(i));
end

if deltaY(i)>0
    Theta(2,t,i) = acos(deltaY(i)/r(i));
else
    Theta(2,t,i) = pi-acos(deltaY(i)/r(i));
end

if deltaZ(i)>0
    Theta(3,t,i) = acos(deltaZ(i)/r(i));
else
    Theta(3,t,i) = pi-acos(deltaZ(i)/r(i));
end

end
end

function [F] = compute_Attract(P,T,theta,k)
%UNTITLED6 此处显示有关此函数的摘要
% 此处显示详细说明
r = sqrt((T(1)-P(1))^{2+(T(2)-P(2))}2+(T(3)-P(3))^2);
F(1) = krcos(theta(1));
F(2) = krcos(theta(2));
F(3) = krcos(theta(3));
end

function [F_a,F_r] = compute_repulsion(P,O,T,theta_r,theta_a,m,po)
%UNTITLED8 此处显示有关此函数的摘要
% 此处显示详细说明
r = sqrt((O(1)-P(1))^{2+(O(2)-P(2))}2+(O(3)-P(3))^2);%当前位置与障碍的距离
rr = sqrt((T(1)-P(1))^{2+(T(2)-P(2))}2+(T(3)-P(3))^2);%当前位置与目标的距离
F_a = zeros(3,1);
F_r = zeros(3,1);

if r>po
F_a = zeros(3,1);%x y z方向吸力的分量
F_r = zeros(3,1);%x y z方向斥力的分量
else

if r<po/2
    F_aa = 0.5*m*((1/r-1/po)^2)*(rr^0.5)/2;
    F_rr = m*(1/r-1/po)*(1/r^2)*(rr^0.5);
    
    F_a(1,1) = F_aa*cos(theta_a(1));
    F_a(2,1) = F_aa*cos(theta_a(2));
    F_a(3,1) = F_aa*cos(theta_a(3));
    
    F_r(1,1) = -F_rr*cos(theta_r(1));
    F_r(2,1) = F_rr*cos(theta_r(2));
    F_r(3,1) = F_rr*cos(theta_r(3));
    
else% Po/2<rre(i)<Po
    F_aa = m*((1/r-1/po)^2)*rr;
    F_rr = m*(1/r-1/po)*(1/r^2)*(rr^2);
    
    F_a(1,1) = F_aa*cos(theta_a(1));
    F_a(2,1) = F_aa*cos(theta_a(2));
    F_a(3,1) = F_aa*cos(theta_a(3));
    
    F_r(1,1) = F_rr*cos(theta_r(1));
    F_r(2,1) = F_rr*cos(theta_r(2));
    F_r(3,1) = F_rr*cos(theta_r(3));
    
end

end

close all
clear all

tic
%%
%==========================================================================
%初始化
%==========================================================================
P0 = [0;0;0];%起点位置
n = 7;%障碍个数
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]’;%目标点与障碍位置第一列是目标点
%%
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’);
for i=1:5
po = i;%障碍影响距离
[P,K]=shi(po,Tar,n,P0,Obs);
%==========================================================================
%画图
%==========================================================================
switch i
case 1
plot3(P(1,1:K),P(2,1:K),P(3,1:K),’.b’);
case 2
plot3(P(1,1:K),P(2,1:K),P(3,1:K),’.r’);
case 3
plot3(P(1,1:K),P(2,1:K),P(3,1:K),’.g’);
case 4
plot3(P(1,1:K),P(2,1:K),P(3,1:K),’.m’);

otherwise
    plot3(P(1,1:K),P(2,1:K),P(3,1:K),'.k');

end
end
toc

function[ P,K]=shi(po,Tar,n,P0,Obs)
k = 15;%引力增益系数
m = 10;%斥力增益系数
% po = 1;%障碍影响距离
l = 0.05;%每次采样的移动步长
t = 2000;%循环次数
K = 0;
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) + lcos(Theta_F(1));
% P(2,i+1) = P(2,i) + lcos(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

仿真结果

设置不同的障碍影响距离](https://www.icode9.com/i/ll/?i=921838d6da8146bebb050ba2b43c67a7.jpg?,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBAcG93ZXJwYW5n,size_18,color_FFFFFF,t_70,g_se,x_16#pic_center)
在这里插入图片描述

标签：11,10,end,14,zeros,2021,FF,Theta,Obs
来源： https://blog.csdn.net/qq_22199637/article/details/121314585