布料仿真先导1-双球摆动系统建模求解和ode45仿真以及遇到的问题

本来是为了做布料仿真的，看维基说，布料仿真其实是基于d簧-阻尼的小球模型，即把布料分为许多个小点之间互相连接，他们之间是通过d簧-阻尼模型来建立运动模型的，为了弄清楚原理，咱先从简单的两个小球的刚性连接开始仿真，再将刚性连接更换为d簧-阻尼连接，最后在多点仿真实现布料仿真。

        考虑到两个摆球之间的受力太多了，所以学习了一下拉格朗日的力学方法，通过能量的角度去列写双球的运动方程，其核心就是拉格朗日函数，即L = T - V动能减去势能，会用来列些方程就行了。它的本质是虚功原理。

以下是数学建模的推导过程：

        推导到这个地方我的运动方程就已经列些完成了，现在要从这两个二阶的线性微分方程组来得到两个小球运动的轨迹，即两个角度值随时间的变化而变化。

        我的第一个思路是尝试用欧拉法来做数值解，但是可能是我的程序写的有问题把，我把我的推导过程和程序也放在这里，如果有谁找到了问题，也可以告诉我，但是我自己用欧拉法去求数值解得时候是没有成功的。

        下面是我使用欧拉法求解的时候使用的matlab代码：

%% 双杆球系统微分方程坐标组求解
m1 = 1;
m2 = 1;
l1 = 0.7;
l2 = 0.5;

x1_1(1) = 20 * pi / 180;
x1_2(1) = 20 * pi / 180;
x2_1(1) = 5 * pi / 180;
x2_2(1) = 5 * pi / 180;
v1_1(1) = 0;
v1_2(1) = 0;
v2_1(1) = 0;
v2_2(1) = 0;

x1(1) = 20 * pi / 180;
x2(1) = 5 * pi / 180;
v1(1) = 0;
v2(1) = 0;
g = 10;
%列些微分方程
T = 5;
dt = 0.05;
A = (m1 + m2)*l1;
B = @(n1 , n2)(m2 * l2 * cos(n1 - n2));
C = @(n1 , n2)(m2 * l2 * sin(n1 - n2));
D = @(n1)((m1 + m2)*g*sin(n1));
E = @(n1 , n2)(l1 * cos(n1 - n2));
F = l2;
G = @(n1 , n2)(l1 * sin(n1 - n2));
H = @(n2)(-1 * g * sin(n2));

t = dt : dt : T;
for i = 1 : 1 : length(t)-1
    
    a1 = A; b1 = B(x1(i) , x2(i));c1 = C(x1(i) , x2(i));d1 = D(x1(i));
    e1 = E(x1(i) , x2(i));f1 = F; g1 = G(x1(i) , x2(i));h1 = H(x2(i));
    
    x1_1(i+1) = dt * v1(i) + x1(i);
    x2_1(i+1) = dt * v2(i) + x2(i);
    v1_1(i+1) = (dt*( c1*v2(i)^2 + d1 + (b1/f1)*(g1*v1(i)^2 + h1) )) / ((e1/f1)-a1) + v1(i);
    v2_1(i+1) = (dt*( c1*v2(i)^2 + d1 + (a1/e1)*(g1*v1(i)^2 + h1) )) / ((a1*f1)/e1 - b1) + v2(i);
    
    a1 = A; b1 = B(x1_1(i+1) , x2_1(i+1));c1 = C(x1_1(i+1) , x2_1(i+1));d1 = D(x1_1(i+1));
    e1 = E(x1_1(i+1) , x2_1(i+1));f1 = F; g1 = G(x1_1(i+1) , x2_1(i+1));h1 = H(x2_1(i+1));
    x1_2(i+1) = dt * v1_1(i+1) + x1(i);
    x2_2(i+1) = dt * v2_1(i+1) + x2(i);
    v1_2(i+1) = (dt*( c1*v2_1(i+1)^2 + d1 + (b1/f1)*(g1*v1_1(i+1)^2 + h1) )) / ((e1/f1)-a1) + v1(i);
    v2_2(i+1) = (dt*( c1*v2_1(i+1)^2 + d1 + (a1/e1)*(g1*v1_1(i+1)^2 + h1) )) / ((a1*f1)/e1 - b1) + v2(i);
    
    x1(i + 1) = 0.5 * (x1_1(i+1) + x1_2(i+1));
    x2(i + 1) = 0.5 * (x2_1(i+1) + x2_2(i+1));
    v1(i + 1) = 0.5 * (v1_1(i+1) + v1_2(i+1));
    v2(i + 1) = 0.5 * (v2_1(i+1) + v2_2(i+1));
end

figure;plot(t,x1);hold on;
plot(t,x2);
legend('phi1' , 'phi2');

figure;plot(v1);hold on;
plot(v2);
legend('phi1' , 'phi2');

x1 = x1 * 180 / pi;
x2 = x2 * 180 / pi;

h1 = figure;
for i = 1 : 1 : length(x1)
    set(0,'CurrentFigure',h1);
    scatter(0 , 0 , '*');hold on;
    x = [l1*sind(x1(i)) , l1*sind(x1(i)) + l2*sind(x2(i))];
    y = [-1*l1*cosd(x1(i)) , -l1*cosd(x1(i)) - l2*cosd(x2(i))];
    scatter(x , y);hold on;
    plot([0 x(1)] , [0 y(1)] , 'r');hold on;
    plot([x(1) x(2)] , [y(1) y(2)] , 'r');hold on;
    xlim([-5 5]);
    ylim([-5 5]);
    pause(0.1);
    hold off;
end

   由于使用欧拉发没有成功，那么就尝试第二种办法用ode45去求解这个问题，但是这里我也发现了一个问题，就是在matlab中，我最开始使用的是角度值去带入运算的，结果总是有异常，当更换为弧度值（程序中sind也更换为sin）看起来就是正常的了。以下是变换为ode45的函数列表过程

以下为实现代码

%基于ode45的双球摆动问题的数值解,注意要化为弧度在计算
close all;clearvars;clc;
dbstop if error;

global m1 m2  l1  l2  g;
m1 = 2;
m2 = 1;
l1 = 1;
l2 = 1;
g = 9.8;

[t , y] = ode45(@vdp1,[0 20],[70*pi/180 0 20*pi/180 0]);

x1 = y(: , 1) .* 180/pi;
x2 = y(: , 3) .* 180/pi;
h1 = figure;
for i = 1 : 1 : length(t)
    set(0,'CurrentFigure',h1);
    scatter(0 , 0 , '*');hold on;
    x = [l1*sind(x1(i)) , l1*sind(x1(i)) + l2*sind(x2(i))];
    y = [-1*l1*cosd(x1(i)) , -l1*cosd(x1(i)) - l2*cosd(x2(i))];
    scatter(x , y);hold on;
    plot([0 x(1)] , [0 y(1)] , 'r');hold on;
    plot([x(1) x(2)] , [y(1) y(2)] , 'r');hold on;
    xlim([-3 3]);
    ylim([-3 3]);
    pause(0.05);
    hold off;
end

function dydt = vdp1(t,y)   %y = [phi1 phi1' phi2 phi2']
    
    global m1  m2  l1  l2  g;
    dydt = zeros(4 , 1);
    dydt(1) = y(2);
    dydt(2) = ( (m1+m2)*g*sin(y(1)) + m2*l2*sin(y(1)-y(3))*y(4)^2 - ...
                ( m2*l2*cos(y(1)-y(3))*g*sin(y(3)) + ...
                  m2*l2*cos(y(1)-y(3))*l1*sin(y(1)-y(3))*y(2)^2 ) /l2 ) ...
               / ( m2*l2*cos(y(1)-y(3))/l2 - (m1+m2)*l1 ) ;
           
    dydt(3) = y(4);  
    dydt(4) =   ( l1*sin(y(1)-y(3))*y(2)^2 - g*sin(y(3)) + ...
                  (l1*cos(y(1)-y(3))) * ( (m1+m2)*g*sin(y(1)) + m2*l2*sin(y(1)-y(3))*y(4)^2 )/((m1+m2)*l1) ) /... 
                 (l2 - l1*cos(y(1)-y(3))*m2*l2*cos(y(1)-y(3))/(m1+m2)*l1);
             
end

下一步把两个球之间的刚性连接更换为d性连接，估计拉格朗日函数需要重新构造