查找等高线,在所述线上填充点(在 MATLAB 中)

计算科学 有限元 matlab 数值建模
2021-12-11 16:29:00

我正在尝试从潜在领域中提取数据。我有一个势场示例代码,在 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
0个回答
没有发现任何回复~