MATLAB非线性可视化（引3）多摆模型
MATLAB非线性可视化（引3）多摆模型
matlabaihaozhe
学matlab编程就关注matlab爱好者！
%双摆
%4阶RK求解
clear
clc
close all
%输入
N=2;%双摆
m=1;
l=1;
g=9.8;
Input=[N,m,l,g];
%初始条件和时间设置
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,:);
%计算出关节坐标
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间隔为常数的算法
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];
%初始条件和时间设置
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,:);
%计算出关节坐标
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间隔为常数的算法
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上发布