【ACADO 使用】纵向控制的简单应用

纵向控制的简单应用（考虑延时delay=0.6s）。 

#include <acado_toolkit.hpp>
#include <acado_gnuplot.hpp>

int main(){
    USING_NAMESPACE_ACADO

    DifferentialState s,v,a;
    Control u_a;
    Parameter T;
    double Ts = 0.6;

    DifferentialEquation f(0.0,T);
    f << dot(s) == v;
    f << dot(v) == a;
    f << dot(a) == (u_a - a)/Ts;

    Function h;
    h << u_a;

    DMatrix Q(1,1);
    Q(0,0) = 1.0;

    DVector r(1);
    r.setAll(0.0);

    OCP ocp(0.0,T,50);
    ocp.minimizeLSQ(Q,h,r);

    ocp.subjectTo(f);
    ocp.subjectTo(AT_START, s == 0.0);
    ocp.subjectTo(AT_START, v == 5.0);
    ocp.subjectTo(AT_START, a == 0.0);

    ocp.subjectTo(AT_END, s == 10.0);
    ocp.subjectTo(AT_END, v == 0.0);
    ocp.subjectTo(AT_END, a == 0.0);

    ocp.subjectTo(0 <= v <= 5);
    ocp.subjectTo(-4 <= a <= 1);
    ocp.subjectTo(-2<= u_a <= 2);
    ocp.subjectTo(0.0 <= T <= 8.0);

    OptimizationAlgorithm algorithm(ocp);

    GnuplotWindow window;
    window.addSubplot(s, "Distance s");
    window.addSubplot(v,"velocity v");
    window.addSubplot(a,"acc a");
    window.addSubplot(u_a,"control u_a");
    algorithm << window;
    algorithm.solve();

    return 0;
    
}

效果如下：