MATLAB非線性可視化（引3）多擺模型
MATLAB非線性可視化（引3）多擺模型
matlabaihaozhe
學(xué)matlab編程就關(guān)注matlab愛好者！
%雙擺
%4階RK求解
clear
clc
close all
%輸入
N=2;%雙擺
m=1;
l=1;
g=9.8;
Input=[N,m,l,g];
%初始條件和時間設(shè)置
y0=[pi/2;pi/2;0;0];%這里全部是弧度值。分別代表[擺1與垂面夾角，擺2與垂面夾角，擺1角動量，擺2角動量]
h=1e-2;
x0=0:h:20;
%代入到ODE求解器中
[y1,Output]=ODE_RK4_hyh(x0,h,y0,Input);
%提取出角度
tN=size(y1,2);
th1=y1(1,:);
th2=y1(2,:);
%計算出關(guān)節(jié)坐標
CX1_A=zeros(1,tN);
CX1_B=CX1_A+l*sin(th1);
CY1_A=zeros(1,tN);
CY1_B=CY1_A-l*cos(th1);
CX2_A=CX1_B;
CX2_B=CX2_A+l*sin(th2);
CY2_A=CY1_B;
CY2_B=CY2_A-l*cos(th2);
%繪圖
n=1;
figure()
set(gcf,’position’,[488 342 400 300])
for k=1:4:1160
clf
xlim([-2,2])
ylim([-2,2])
hold on
%繪制擺
plot([CX1_A(k),CX1_B(k)],[CY1_A(k),CY1_B(k)],’color’,’k’,’LineWidth’,1.5)
plot([CX2_A(k),CX2_B(k)],[CY2_A(k),CY2_B(k)],’color’,’k’,’LineWidth’,1.5)
%繪制軌線
if k>200
n=n+1;
end
Nm=k-n+1;
%軌跡1
F_color=[1,0,0];
F_color=F_color*0.6+[1,1,1]*0.4*0.999;
cdata=[linspace(1,F_color(1),Nm+1)’,linspace(1,F_color(2),Nm+1)’,linspace(1,F_color(3),Nm+1)’];
cdata=reshape(cdata,Nm+1,1,3);
if k>3
patch([CX1_B(n:k),NaN],[CY1_B(n:k),NaN],1:Nm+1,’EdgeColor’,’interp’,’Marker’,’none’,...
’MarkerFaceColor’,’flat’,’CData’,cdata,’LineWidth’,1.5);
end
%軌跡2
F_color=[0,0,1];
F_color=F_color*0.6+[1,1,1]*0.4*0.999;
cdata=[linspace(1,F_color(1),Nm+1)’,linspace(1,F_color(2),Nm+1)’,linspace(1,F_color(3),Nm+1)’];
cdata=reshape(cdata,Nm+1,1,3);
if k>3
patch([CX2_B(n:k),NaN],[CY2_B(n:k),NaN],1:Nm+1,’EdgeColor’,’interp’,’Marker’,’none’,...
’MarkerFaceColor’,’flat’,’CData’,cdata,’LineWidth’,1.5);
end
hold off
pause(0.05)
F=getframe(gca);
I=frame2im(F);
[I,map]=rgb2ind(I,20);
end
function [y,Output]=ODE_RK4_hyh(x,h,y0,Input)
%4階RK方法
%h間隔為常數(shù)的算法
y=zeros(size(y0,1),size(x,2));
y(:,1)=y0;
for ii=1:length(x)-1
yn=y(:,ii);
xn=x(ii);
K1=Fdydx(xn ,yn ,Input);
K2=Fdydx(xn+h/2,yn+h/2*K1,Input);
K3=Fdydx(xn+h/2,yn+h/2*K2,Input);
K4=Fdydx(xn+h ,yn+h*K3 ,Input);
y(:,ii+1)=yn+h/6*(K1+2*K2+2*K3+K4);
end
Output=[];
end
function dydx=Fdydx(x,y,Input)
%將原方程整理為dy/dx=F(y,x)的形式
%輸入Input整理
m=Input(2);
l=Input(3);
g=Input(4);
%輸入
th1=y(1);%角度1
th2=y(2);%角度2
pth1=y(3);%角動量1
pth2=y(4);%角動量2
%利用拉格朗日法得到的方程
M=l^2*m*(-16 + 9*cos(th1 - th2)^2);
dth1 = -6*(2*pth1 - 3*pth2*cos(th1 - th2))/M;
dth2 = -6*(8*pth2 - 3*pth1*cos(th1 - th2))/M;
dpth1=-0.5*l*m*(3*g*sin(th1)+dth1*dth2*l*sin(th1-th2));
dpth2=0.5*l*m*(dth1*dth2*l*sin(th1-th2)-g*sin(th2));
%整理輸出
dydx=zeros(4,1);
dydx(1)=dth1;
dydx(2)=dth2;
dydx(3)=dpth1;
dydx(4)=dpth2;
end
%三擺
%4階RK求解
clear
clc
close all
%輸入
N=3;%雙擺
m=1;
l=1;
g=9.8;
Input=[N,m,l,g];
%初始條件和時間設(shè)置
y0=[1/2*pi;1/2*pi;1/2*pi;0;0;0];%這里全部是弧度值。分別代表[擺1與垂面夾角，擺2與垂面夾角，擺1角動量，擺2角動量]
h=1e-2;
x0=0:h:20;
%代入到ODE求解器中
[y1,Output]=ODE_RK4_hyh(x0,h,y0,Input);
%提取出角度
tN=size(y1,2);
th1=y1(1,:);
th2=y1(2,:);
th3=y1(3,:);
%計算出關(guān)節(jié)坐標
CX1_A=zeros(1,tN);
CX1_B=CX1_A+l*sin(th1);
CY1_A=zeros(1,tN);
CY1_B=CY1_A-l*cos(th1);
CX2_A=CX1_B;
CX2_B=CX2_A+l*sin(th2);
CY2_A=CY1_B;
CY2_B=CY2_A-l*cos(th2);
CX3_A=CX2_B;
CX3_B=CX3_A+l*sin(th3);
CY3_A=CY2_B;
CY3_B=CY3_A-l*cos(th3);
%繪圖
n=1;
figure()
set(gcf,’position’,[488 342 400 300])
for k=1:4:1100
clf
xlim([-3,3])
ylim([-3,3])
hold on
plot([CX1_A(k),CX1_B(k)],[CY1_A(k),CY1_B(k)],’color’,’k’,’linewidth’,1)
plot([CX2_A(k),CX2_B(k)],[CY2_A(k),CY2_B(k)],’color’,’k’,’linewidth’,1)
plot([CX3_A(k),CX3_B(k)],[CY3_A(k),CY3_B(k)],’color’,’k’,’linewidth’,1)
hold off
%繪制軌線
if k>200
n=n+1;
end
Nm=k-n+1;
%軌跡1
F_color=[1,0,0];
F_color=F_color*0.6+[1,1,1]*0.4*0.999;
cdata=[linspace(1,F_color(1),Nm+1)’,linspace(1,F_color(2),Nm+1)’,linspace(1,F_color(3),Nm+1)’];
cdata=reshape(cdata,Nm+1,1,3);
if k>3
patch([CX1_B(n:k),NaN],[CY1_B(n:k),NaN],1:Nm+1,’EdgeColor’,’interp’,’Marker’,’none’,...
’MarkerFaceColor’,’flat’,’CData’,cdata,’LineWidth’,1);
end
%軌跡2
F_color=[0,0,1];
F_color=F_color*0.6+[1,1,1]*0.4*0.999;
cdata=[linspace(1,F_color(1),Nm+1)’,linspace(1,F_color(2),Nm+1)’,linspace(1,F_color(3),Nm+1)’];
cdata=reshape(cdata,Nm+1,1,3);
if k>3
patch([CX2_B(n:k),NaN],[CY2_B(n:k),NaN],1:Nm+1,’EdgeColor’,’interp’,’Marker’,’none’,...
’MarkerFaceColor’,’flat’,’CData’,cdata,’LineWidth’,1);
end
%軌跡3
F_color=[0,1,0];
F_color=F_color*0.6+[1,1,1]*0.4*0.999;
cdata=[linspace(1,F_color(1),Nm+1)’,linspace(1,F_color(2),Nm+1)’,linspace(1,F_color(3),Nm+1)’];
cdata=reshape(cdata,Nm+1,1,3);
if k>3
patch([CX3_B(n:k),NaN],[CY3_B(n:k),NaN],1:Nm+1,’EdgeColor’,’interp’,’Marker’,’none’,...
’MarkerFaceColor’,’flat’,’CData’,cdata,’LineWidth’,1);
end
pause(0.02)
end
function [y,Output]=ODE_RK4_hyh(x,h,y0,Input)
%4階RK方法
%h間隔為常數(shù)的算法
y=zeros(size(y0,1),size(x,2));
y(:,1)=y0;
for ii=1:length(x)-1
yn=y(:,ii);
xn=x(ii);
K1=Fdydx(xn ,yn ,Input);
K2=Fdydx(xn+h/2,yn+h/2*K1,Input);
K3=Fdydx(xn+h/2,yn+h/2*K2,Input);
K4=Fdydx(xn+h ,yn+h*K3 ,Input);
y(:,ii+1)=yn+h/6*(K1+2*K2+2*K3+K4);
end
Output=[];
end
function dydx=Fdydx(x,y,Input)
%將原方程整理為dy/dx=F(y,x)的形式
%三擺方程
%輸入Input整理
m=Input(2);
l=Input(3);
g=Input(4);
%輸入
th=y(1:3);%角度1
pth=y(4:6);%角動量1
%利用拉格朗日法得到的方程
M=l^2*m*(-169+81*cos(2*(th(1)-th(2)))-9*cos(2*(th(1)-th(3)))+45*cos(2*(th(2)-th(3))));
dth1=(6*(-23*pth(1)+9*cos(2*(th(2)-th(3)))*pth(1)+27*cos(th(1)-th(2))*pth(2)-9*cos(th(1)+th(2)-2*th(3))*pth(2)+21*cos(th(1)-th(3))*pth(3)-27*cos(th(1)-2*th(2)+th(3))*pth(3)))/M;
dth2=(6*(27*cos(th(1)-th(2))*pth(1)-9*cos(th(1)+th(2)-2*th(3))*pth(1)-47*pth(2)+9*cos(2*(th(1)-th(3)))*pth(2)-27*cos(2*th(1)-th(2)-th(3))*pth(3)+57*cos(th(2)-th(3))*pth(3)))/M;
dth3=(6*(21*cos(th(1)-th(3))*pth(1)-27*cos(th(1)-2*th(2)+th(3))*pth(1)-27*cos(2*th(1)-th(2)-th(3))*pth(2)+57*cos(th(2)-th(3))*pth(2)-143*pth(3)+81*cos(2*(th(1)-th(2)))*pth(3)))/M;
dth=[dth1;dth2;dth3];
dpth1=-0.5*l*m*(5*g*sin(th(1))+l*dth(1)*(3*dth(2)*sin(th(1)-th(2))+dth(3)*sin(th(1)-th(3))));
dpth2=-0.5*l*m*(-3*l*dth(1)*dth(2)*sin(th(1)-th(2))+3*g*sin(th(2))+l*dth(2)*dth(3)*sin(th(2)-th(3)));
dpth3=0.5*l*m*(l*dth(1)*dth(3)*sin(th(1)-th(3))+l*dth(2)*dth(3)*sin(th(2)-th(3))-g*sin(th(3)));
%整理輸出
dydx=zeros(6,1);
dydx(1)=dth1;
dydx(2)=dth2;
dydx(3)=dth3;
dydx(4)=dpth1;
dydx(5)=dpth2;
dydx(6)=dpth3;
end
參考資料：
[1]https://en.wikipedia.org/wiki/Double_pendulum
圖片來源：由 在Pixabay上發(fā)布