用龙格库塔法解动力学方程组

报警告：在 t=3.840683e-02 处失败。在时间 t 处，步长大小必须降至所允许的最小值(1.110223e-16)以下，才能达到积分容差要求。

尝试过调小精度，换用ode23，但无效，怀疑可能是方程组中有一个方程的分母存在三角函数，导致异常，将y3限制到一个固定范围后不再报错，但是不收敛了，求解答

代码如下：

clear; clc; close all;

m0 = 0.016; ktheta = 0.0008; L0 = 0.02; phi0 = 0.6; N = 12; L1 = 0.02; L2 = 0.02;

theta10 = 0.875; m1 = 0.003; m2 = 0.003; k = 5; kh = 30; h = 0.03; mu = 0.3; g = 9.8; omega = 31.4; A = 0.0015

tspan = [0, 11.5];

y0 = [0; 0; 0.6; 0]; % 初始状态

options = odeset( 'RelTol', 1e-1, 'AbsTol', 1e-2, 'MaxStep', 1e-1);

[t, Y] = ode45(@(t,y) system_dynamics(t,y,m0,L0,mu,g,N,ktheta,phi0,A,omega), ...

tspan, y0,options);

x = Y(:, 1); phi = Y(:, 3);

figure;

plot(t, x, 'b', 'LineWidth', 1.2);

legend('x'); xlabel('t (s)'); ylabel(' (m)'); grid on;

% plot( t, phi, 'r', 'LineWidth', 1.2);

% legend('\phi'); xlabel('t (s)'); ylabel(' (rad)'); grid on;

function dy = system_dynamics(t,y,m0,L0,mu,g,N,ktheta,phi0,A,omega)

y1 = y(1); y2 = y(2); y3 = y(3); y4 = y(4);

P = - A*omega^2*sin(omega*t);

dy1 = y2;

dy4 = (ktheta*N*y3 - ktheta*phi0*N + L0*(L0*(y4)^2*cos(y3) - P)*cos(y3)*mu*m0 + m0*sin(y3) - g*L0*m0*(mu*cos(y3)+sin(y3))) / ((L0)^2*m0*mu*sin(y3)*cos(y3) + (L0)^2*m0*(sin(y3))^2+eps) ;

dy2 = (- L0*(y4)^2*cos(y3)*m0*mu + g*m0*mu + m0*mu*P + (L0*m0*mu*sin(y3)*dy4)) / m0;

dy3 = y4;

dy = [dy1; dy2; dy3; dy4];

end

警告: 在 t=3.840683e-02 处失败。在时间 t 处，步长大小必须降至所允许的最小值(1.110223e-16)以下，才能达到积分容差要求。

> In ode45 (line 360)

In one (line 12)

>>

[Dyuman Joshi]

I have vectorized the ode function, updated the solver to a stiff ode solver and plotted the output.

As you can see below, the solution values are rising rapidly around t = 3.84e-2. The discontinuity is most likely the reason why the solver is failing.

It would be helpful to have backgorund on the problem you are trying to solve, including the equations in symbolic form.

m0 = 0.016; ktheta = 0.0008; L0 = 0.02; phi0 = 0.6; N = 12; L1 = 0.02; L2 = 0.02;
theta10 = 0.875; m1 = 0.003; m2 = 0.003; k = 5; kh = 30; h = 0.03; mu = 0.3; g = 9.8; omega = 31.4; A = 0.0015;
tspan = [0, 11.5];
y0 = [0; 0; 0.6; 0];  % 初始状态

%Include output function plot
options = odeset( 'RelTol', 1e-1,  'AbsTol', 1e-2,  'MaxStep', 1e-1, 'OutputFcn', @odeplot);

%using a stiff ode solver
[t, Y] = ode23s(@(t,y) system_dynamics(t,y,m0,L0,mu,g,N,ktheta,phi0,A,omega), ...
               tspan, y0, options);  

Warning: Failure at t=3.840346e-02. Unable to meet integration tolerances without reducing the step size below the smallest value allowed (1.110223e-16) at time t.

%Change y-axis scale to log to visualize the steepness
ax = gca;
ax.YScale = 'log';

x = Y(:, 1); phi = Y(:, 3);

figure;
plot(t, x, 'b', 'LineWidth', 1.2);
legend('x'); xlabel('t (s)'); ylabel(' (m)'); grid on;

●

% plot( t, phi, 'r', 'LineWidth', 1.2);
% legend('\phi'); xlabel('t (s)'); ylabel(' (rad)'); grid on;
function dy = system_dynamics(t,y,m0,L0,mu,g,N,ktheta,phi0,A,omega)
    y1 = y(1); y2 = y(2); y3 = y(3); y4 = y(4);
    P = -A*omega^2*sin(omega*t);
    dy1 = y2;
    dy4 = (ktheta*N*y3 - ktheta*phi0*N + L0*(L0*(y4).^2.*cos(y3) - P).*cos(y3)*mu*m0 + m0*sin(y3) - g*L0*m0*(mu*cos(y3)+sin(y3))) ./ ((L0)^2*m0*mu*sin(y3).*cos(y3) + (L0)^2*m0*(sin(y3)).^2+eps)  ;
    dy2  = (- L0*(y4).^2.*cos(y3)*m0*mu + g*m0*mu + m0*mu*P + (L0*m0*mu*sin(y3).*dy4)) / m0;
    dy3 = y4;
        dy = [dy1; dy2; dy3; dy4];
end

[帅]

[帅]

[Dyuman Joshi]

Your code is correct w.r.t the equations you have written.

How is the stiffness of the legs to be considered? The given unit of stiffness doesn't corelate with Hooke's law.

How do you calculate f1 and f2? That too, two separate definitions. Also, how is that considered/related to other variables?

[帅]

方程组是通过拉格朗日第二方程推导得到的，包括f1与f2。在此案例情形下（case 1）有：m1=m2=k=kh=θ1=θ2=0；

机器人的腿是刚性的，腿与主体通过扭转刚度为ktheta的扭转弹簧连接。

公式如下：