我正在尝试从潜在领域中提取数据。我有一个势场示例代码,在 MATLAB 中使用 meshgrid。我已经粘贴了下面的代码。
我接下来要做的是将此字段“转换”为二维图形,收集/绘制等高线,并沿等高线放置数据点,即。我想收集沿每个轮廓的 XY 点列表(可能是均匀分布的),使数据易于在其他软件等中传输或操作。
最终没有透露太多关于我的研究的想法认为我需要以这种方式为多个 2D 切片建立所有数据点,以便我拥有这些点的密集 3D 字段,例如。一个由球体组成的星座,其大小取决于它所在的等高线的电位。
例如,如果这是我的等值线,我希望沿着等高线填充点/圆,在电荷/障碍物附近有更陡峭梯度的较小的紧密排列的点,以及外围较大的点,填满空间。
close all
clear all
clc
xf = 0:0.1:12;
yf = 0:0.1:12;
[X Y] = meshgrid(xf,yf);
Goal=[10;10];
Obs1=[3;3];
Obs2=[9;9];
KG = 10;
Ko = 20;
rG = sqrt((Goal(1)-X).^2+(Goal(2)-Y).^2);
VG = KG*rG; %PF of Goal
ro1 = sqrt((Obs1(1)-X).^2+(Obs1(2)-Y).^2);
Vo1 = Ko./ro1; %PF of Obstacle 1
ro2 = sqrt((Obs2(1)-X).^2+(Obs2(2)-Y).^2);
Vo2 = Ko./ro2; %PF of Obstacle 2
figure
mesh(xf,yf,VG+Vo1+Vo2)
axis([0 12 0 12 0 200])
xlabel('x')
ylabel('y')
zlabel('V')
H4_PotentialNavigation
H4_PlatoonRobots
function [t,x]=H4_PotentialNavigation()
clc
time = [0 300];
x0 = [0;0;0];
Goal = [10;10];
Obs1 = [3;3];
Obs2 = [9;9];
KG = 30;
Ko = 30;
[t x] = ode23(@vehicle,time,x0)
figure
plot(x(:,1),x(:,2),'r',Goal(1),Goal(2),'o',Obs1(1),Obs1(2),'x',Obs2(1),Obs2(2),'x')
axis([0 12 0 12])
function dx=vehicle(t,x)
rG = sqrt((Goal(1)-x(1))^2+(Goal(2)-x(2))^2);
FGx = KG*(Goal(1)-x(1))/rG;
FGy = KG*(Goal(2)-x(2))/rG;
ro1 = sqrt((Obs1(1)-x(1))^2+(Obs1(2)-x(2))^2);
Fo1x = -Ko*(Obs1(1)-x(1))/ro1^3;
Fo1y = -Ko*(Obs1(2)-x(2))/ro1^3;
ro2 = sqrt((Obs2(1)-x(1))^2+(Obs2(2)-x(2))^2);
Fo2x = -Ko*(Obs2(1)-x(1))/ro2^3;
Fo2y = -Ko*(Obs2(2)-x(2))/ro2^3;
Fx = (FGx+Fo1x+Fo2x);
Fy = (FGy+Fo1y+Fo2y);
alpha = atan(Fy/Fx);
v = 1;
L = 2;
if rG < 0.05 % Vehicle @ goal - stop
v = 0;
end
K = 2;
ph = K*(alpha-x(3));
dx = [v*cos(ph)*cos(x(3));
v*cos(ph)*sin(x(3));
v*sin(ph)/L];
end
end
function [t,x] = H4_PlatoonRobots()
clc
time = [0 300];
x0 = [0;0;0;0;0;0;0;0;0;0;0;0];
Goal = [10;10];
Obs1 = [3;3];
Obs2 = [9;9];
KG = 30;
Ko = 30;
[t x] = ode23(@vehicles,time,x0)
figure
plot (x(:,1),x(:,3),'b',x(:,5),x(:,7),'r',x(:,9),x(:,11),'g',...
Goal(1),Goal(2),'o',Obs1(1),Obs1(2),'x',Obs2(1),Obs2(2),'x')
axis([0 12 0 12])
function dx = vehicles(t,x)
Goal = [10;10];
Obs1=[3;3];
Obs2=[9;9];
KG=20;
Ko=30;
Kij=20;
KLi=100;
KF=10;
rD=0.5; % Separation from Leader
rG = sqrt((Goal(1)-x(1))^2+(Goal(2)-x(3))^2); % Leader and Goal
FGx = KG*(Goal(1)-x(1))/max([rG 0.01]); % Division by Zero catch
FGy = KG*(Goal(2)-x(3))/max([rG 0.01]);
ro1 = sqrt((Obs1(1)-x(1))^2+(Obs1(2)-x(3))^2);
Fo1x = -Ko*(Obs1(1)-x(1))/ro1^3;
Fo1y = -Ko*(Obs1(2)-x(2))/ro1^3;
ro2 = sqrt((Obs2(1)-x(1))^2+(Obs2(2)-x(3))^2);
Fo2x = -Ko*(Obs2(1)-x(1))/ro2^3;
Fo2y = -Ko*(Obs2(2)-x(2))/ro2^3;
FrLx = -KF*x(2);
FrLy = -KF*x(4);
Fx = (FGx + Fo1x + Fo2x + FrLx);
Fy = (FGy + Fo1y + Fo2y + FrLy);
xL = [0 1; 0 0]*[x(1);x(2)]+[0;1]*Fx; % Leader Dynamics
yL = [0 1; 0 0]*[x(3);x(4)]+[0;1]*Fy;
r1o1 = sqrt((Obs1(1)-x(5))^2+(Obs1(2)-x(7))^2); % Follower 1 and Obs1
F1o1x = -Ko*(Obs1(1)-x(5))/r1o1^3;
F1o1y = -Ko*(Obs1(2)-x(7))/r1o1^3;
r1o2 = sqrt((Obs2(1)-x(5))^2+(Obs2(2)-x(7))^2); % Follower 1 and Obs2
F1o2x = -Ko*(Obs2(1)-x(5))/r1o2^3;
F1o2y = -Ko*(Obs2(2)-x(7))/r1o2^3;
rL1 = sqrt((x(1)-x(5))^2+(x(3)-x(7))^2); % Follower 1 and Leader
FL1x = KLi *(rL1-rD)*(x(1)-x(5))/max([rL1 0.01]);
FL1y = KLi *(rL1-rD)*(x(3)-x(7))/max([rL1 0.01]);
r21 = sqrt((x(9)-x(5))^2+(x(11)-x(7))^2); % Follower 1 and Follower 2
F21x = -2* Kij *(x(9)-x(5))/max([r21^4 0.01]);
F21y = -2* Kij *(x(11)-x(7))/max([r21^4 0.01]);
Fr1x = -KF*x(6);
Fr1y = -KF*x(8);
F1x = (F1o1x+F1o2x+FL1x+F21x+Fr1x);
F1y = (F1o1y+F1o2y+FL1y+F21y+Fr1y);
x1 = [0 1; 0 0]*[x(5);x(6)]+[0;1]*F1x; % Follower 1 Dynamics
y1 = [0 1; 0 0]*[x(7);x(8)]+[0;1]*F1y;
r2o1 = sqrt((Obs1(1)-x(9))^2+(Obs1(2)-x(11))^2); % Follower 2 and Obs1
F2o1x = -Ko*(Obs1(1)-x(9))/r2o1^3;
F2o1y = -Ko*(Obs1(2)-x(11))/r2o1^3;
r2o2 = sqrt((Obs2(1)-x(9))^2+(Obs2(2)-x(11))^2); % Follower 2 and Obs2
F2o2x = -Ko*(Obs2(1)-x(9))/r2o2^3;
F2o2y = -Ko*(Obs2(2)-x(11))/r2o2^3;
rL2 = sqrt((x(1)-x(9))^2+(x(3)-x(11))^2); % Follower 2 and Leader
FL2x = KLi *(rL2-rD)*(x(1)-x(9))/max([rL2 0.01]);
FL2y = KLi *(rL2-rD)*(x(3)-x(11))/max([rL2 0.01]);
r12 = sqrt((x(5)-x(9))^2+(x(7)-x(11))^2); % Follower 2 and Follower 1
F12x = -2* Kij *(x(5)-x(9))/max([r12^4 0.01]);
F12y = -2* Kij *(x(7)-x(11))/max([r12^4 0.01]);
Fr2x = -KF*x(10);
Fr2y = -KF*x(12);
F2x = (F2o1x+F2o2x+FL2x+F12x+Fr2x);
F2y = (F2o1y+F2o2y+FL2y+F12y+Fr2y);
x2 = [0 1; 0 0]*[x(9);x(10)]+[0;1]*F2x; % Follower 2 Dynamics
y2 = [0 1; 0 0]*[x(11);x(12)]+[0;1]*F2y;
dx = [xL(1);xL(2);yL(1);yL(2);x1(1);x1(2);y1(1);y1(2);x2(1);x2(2);...
y2(1);y2(2)];
if x(2) == 0 && x(4) == 0 % avoid local minima
w = 0.001*randn(2,1);
dx = dx + [0;w(1);0;w(2);0;0;0;0;0;0;0;0];
end
if r12 == 0 % Seperate followers from initial condition
w = 0.001*randn(4,1);
dx = dx + [0;0;0;0;0;w(1);0;w(2);0;w(3);0;w(4)];
end
end
end
