ACADO最优控制程序转CasADi实现结果不符，求代码错误排查

我使用ACADO编写了一个优化问题，运行结果符合预期，完整实现代码如下：

x0 = [4.99098,13.5041,-0.481149,-0.0124761
5.1939,13.37,-0.3891,0.0136
5.3879,13.2902,-0.3957,-0.1045
5.5818,13.2059,-0.4298,-0.2737
5.7758,13.1097,-0.4952,-0.4026
6.1313,12.8784,-0.658,-0.4009
6.2929,12.7443,-0.7241,-0.3075
6.4545,12.5939,-0.7699,-0.17
6.7434,12.3066,-0.7854,0
7.0303,12.0197,-0.7854,0
7.3152,11.7348,-0.7854,0
7.602,11.4439,-0.8175,-0.2784
7.7434,11.2832,-0.8852,-0.4152
7.9758,10.9506,-1.0325,-0.3547
8.1677,10.586,-1.1298,-0.1944
8.2586,10.3853,-1.1591,-0.1207
8.4167,10.0096,-1.1772,0.0458
8.5029,9.8076,-1.1534,0.1866
8.6797,9.4439,-1.0842,0.1742
8.8826,9.0803,-1.0491,0.0151
8.9868,8.8985,-1.0544,-0.0744
9.1828,8.5348,-1.1063,-0.2042
9.358,8.151,-1.1731,-0.1031
9.4433,7.9449,-1.1822,-0.0824
9.5265,7.7348,-1.2094,-0.1909
9.6011,7.5247,-1.251,-0.2359
9.6655,7.3146,-1.2953,-0.2257
9.7205,7.1045,-1.3323,-0.1678
9.7688,6.8944,-1.3548,-0.0747
9.8141,6.6843,-1.3585,0.0344];

x0 = x0';

x_init = [4.99098,13.5041,-0.481149,-0.0124761];
x_final = [9.8141,6.6843,-1.3585,0.0344];

x_r = size(x0);
N = x_r(2)-1;

BEGIN_ACADO;                          
    
    acadoSet('problemname', 'ocp_car'); 
    DifferentialState x  y th phi;
    Control v  w;
    
    f = acado.DifferentialEquation();
    f.add(dot(x) == cos(th)*v);
    f.add(dot(y) == sin(th)*v);
    f.add(dot(th) == tan(phi)*v/0.68);
    f.add(dot(phi) == w);
    
    ocp = acado.OCP(0.0, 5, N);        
                                        
    ocp.minimizeLagrangeTerm(v*v+w*w);      
    ocp.subjectTo( f );              
  
    ocp.subjectTo( 'AT_START', x == x_init(1));  
    ocp.subjectTo(  'AT_START', y == x_init(2));    
    ocp.subjectTo(  'AT_START', th == x_init(3));
    ocp.subjectTo( 'AT_START', phi == x_init(4));
    
    ocp.subjectTo( 'AT_END', x == x_final(1));
    ocp.subjectTo(  'AT_END', y == x_final(2));
    ocp.subjectTo(  'AT_END', th == x_final(3));
    ocp.subjectTo( 'AT_END', phi == x_final(4));
    
    algo = acado.OptimizationAlgorithm(ocp); 
    algo.set('INTEGRATOR_TOLERANCE', 1e-5 ); 
    algo.set('DISCRETIZATION_TYPE', 'MULTIPLE_SHOOTING');
    
END_ACADO;

out = ocp_car_RUN();

figure;
hold on
scatter(out.STATES(:,2), out.STATES(:,3), 'r');
scatter(x0(1,:), x0(2,:), 'b');

CasADi实现代码 #

我尝试用CasADi搭建相同的优化问题，实现代码如下：

opti = casadi.Opti();
X = opti.variable(4, N+1);
U = opti.variable(2, N); 

obj = U(1,:)*U(1,:)' + U(2,:)*U(2,:)';
opti.minimize(obj(1,1));

f = @(x,u) [u(1)*cos(x(3)); u(1)*sin(x(3)); u(1)*tan(x(4))/0.68; u(2)];
dt = 0.670;
for k=1:N 
   k1 = f(X(:,k),         U(:,k));
   k2 = f(X(:,k) + dt/2*k1, U(:,k));
   k3 = f(X(:,k) + dt/2*k2, U(:,k));
   k4 = f(X(:,k) + dt*k3,   U(:,k));
   x_next = X(:,k) + dt/6*(k1+2*k2+2*k3+k4);
   opti.subject_to(X(:,k+1)==x_next);
end

opti.subject_to(X(:,1) == x_init');
opti.subject_to(X(:,end) == x_final'); 
opti.set_initial(X, x0);

opti.solver('ipopt');
sol = opti.solve();

hold on;
solved_val=sol.value(X)';
scatter(x0(1,:), x0(2,:), 'b');
scatter(solved_val(1,:), solved_val(2,:), 'r');

疑问 #

我不确定CasADi中的建模方式是否正确，若有错误欢迎指出。

内容的提问来源于stack exchange，提问作者GPrathap