Решение дифференциальных уравнений с использованием bvp4c MATLAB - мостового крана

Question

Я решаю дифференциальные уравнения в MATLAB, используя bvp4c для школьного проекта. Задача состоит в том, чтобы спланировать траекторию для мостового крана с использованием противовзлома.

Проблема, с которой я столкнулся, заключается в том, что система не соблюдает заданные граничные условия (например, положение крана должно быть в интервале <0,1> , а результирующее положение не соответствует к этому интервалу и не встречает границ в конце). И самое интересное, что Angle не учитывает граничные условия (должен начинаться с 0 рад и начинаться со случайного значения).

Может кто-нибудь помочь мне решить эту проблему?

Мой код выглядит так:

function solveBVP
run parametry.m     % parameters of overhead crane (g, m0, m1, L1)

global tf_su g m0 m1 L1 sol
tf_su = 2;          % final time
options = bvpset('RelTol',1e-6,'NMax',1000,'Stats','on');
solinit=bvpinit(linspace(0,tf_su,1000),@odeInit,[0 0 0 0]);
sol = bvp4c(@myODE, @odeBC, solinit,options);

sol.th0=[sol.x;sol.y(1,:)]';
sol.th1=[sol.x;sol.y(2,:)]';
disp(sol.parameters);
close all
figure
hold on
plot(sol.x,sol.y(1,:),'Linewidth',1);
plot(sol.x,sol.y(2,:),'Linewidth',1);
hold off
grid on
grid minor
legend('Position','Angle')
xlabel('Time [s]')
ylabel('Postion [m] - Angle [rad]')
end

function dydt = myODE(t,y,p)
global tf_su g m0 m1 L1

% y(1) ... cart
% y(2) ... arm
% y(3) ... dcart
% y(4) ... darm

u=pi^2/tf_su^2*(p(2)+p(4))*cos(pi*t/tf_su);
for i = 2:5
    u=u-(i*pi/tf_su)^2*p(i-1)*cos(i*pi*t/tf_su);
end

    dydt = [y(3) y(4) u 0];

    dydt(3) = (m0*L1*sin(y(2))*y(4)^2 + m1*g*sin(y(2))*cos(y(2))) / (m0 + m1*sin(y(2))^2) + u ./ (m0 + m1*sin(y(2))^2);
    dydt(4) = ((m0+m1)*g*sin(y(2)) + m1*L1*y(4)^2*sin(y(2))*cos(y(4))) / (m0 + m1*L1*sin(y(2))^2) + u*cos(y(2)) ./ ((m0 + m1*sin(y(2))^2)*L1);
end

function res = odeBC(ya, yb, p)

res = [ya(1)
    ya(2)
    ya(3)
    ya(4)
    yb(1) - 1
    yb(2)
    yb(3)
    yb(4)
    ];
end

function v = odeInit(t)
global tf_su
    v=[0;
        -pi+pi*t/tf_su;
        0;
        pi/tf_su;
        ]';
end