c++ - 使用 CppAD 时，IPOPT 不服从约束但不记录违规

Question

我正在尝试评估两个五阶多项式(x 和 y 位置各一个)的系数和时间，这两个多项式在将初始位置、速度和方向连接到所需的位置时将工作量和时间(目标函数)降至最低速度为 0 的最终位置和方向(等式约束)。这是代码:

#include <vector>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>

using CppAD::AD;

typedef struct {
    double x, y, theta, linear_velocity;
} Waypoint;

typedef std::vector<Waypoint> WaypointList;

struct TrajectoryConfig {
    //! gain on accumulated jerk term in cost function
    double Kj;
    //! gain on time term in cost function
    double Kt;
    //! gain on terminal velocity term in cost function
    double Kv;
};

class Trajectory {
 public:
     explicit Trajectory(TrajectoryConfig config);
     ~Trajectory();
     void updateConfigs(TrajectoryConfig config);
     void solve(WaypointList waypoints);
 private:
     //! solution vector
     std::vector<double> solution_;
     //! gain on accumulated jerk term in cost function
     double Kj_;
     //! gain on time term in cost function
     double Kt_;
     //! gain on terminal velocity term in cost function
     double Kv_;
};

/*
  Trajectory(TrajectoryConfig)

  class constructor.  Initializes class given configuration struct
*/
Trajectory::Trajectory(TrajectoryConfig config) {
    Kj_ = config.Kj;
    Kt_ = config.Kt;
    Kv_ = config.Kv;
}

Trajectory::~Trajectory() {
    std::cerr << "Trajectory Destructor!" << std::endl;
}

enum Indices { A0 = 0, A1, A2, A3, A4, A5, B0, B1, B2, B3, B4, B5, T };

class FGradEval {
 public:
     size_t M_;
     // gains on cost;
     double Kj_, Kt_;
     // constructor
     FGradEval(double Kj, double Kt) {
         M_ = 13;  // no. of parameters per trajectory segment: 2 x 6 coefficients + 1 time
         Kj_ = Kj;
         Kt_ = Kt;
     }

     typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
     void operator()(ADvector& fgrad, const ADvector& vars) { 
         fgrad[0] = 0;

         AD<double> accum_jerk;
         AD<double> a0, a1, a2, a3, a4, a5;
         AD<double> b0, b1, b2, b3, b4, b5;
         AD<double> T, T2, T3, T4, T5;
         AD<double> x, y, vx, vy;

         size_t offset = 1;

         a0 = vars[Indices::A0];
         a1 = vars[Indices::A1];
         a2 = vars[Indices::A2];
         a3 = vars[Indices::A3];
         a4 = vars[Indices::A4];
         a5 = vars[Indices::A5];
         b0 = vars[Indices::B0];
         b1 = vars[Indices::B1];
         b2 = vars[Indices::B2];
         b3 = vars[Indices::B3];
         b4 = vars[Indices::B4];
         b5 = vars[Indices::B5];
         T  = vars[Indices::T];
         T2 = T*T;
         T3 = T*T2;
         T4 = T*T3;
         T5 = T*T4;

         x    = a0 + a1*T + a2*T2 + a3*T3 + a4*T4 + a5*T5;
         y    = b0 + b1*T + b2*T2 + b3*T3 + b4*T4 + b5*T5;
         vx   = a1 + 2*a2*T + 3*a3*T2 + 4*b4*T3 + 5*a5*T4;
         vy   = b1 + 2*b2*T + 3*b3*T2 + 4*b4*T3 + 5*b5*T4;

         //! cost-terms
         //! accum_jerk is the analytic integral of int_0^T (jerk_x^2 + jerk_y^2) dt
         accum_jerk = 36 * T * (a3*a3 + b3*b3) + 144 * T2 * (a3*a4 + b3*b4) + T3 * (240*(a3*a5 + b3*b5) + 192*(a4*a4 + b4*b4))
             + 720 * T4 * (a4*a5 + b4*b5) + 720 * T5 * (a5*a5 + b5*b5);
         fgrad[0] += Kj_ * accum_jerk;
         fgrad[0] += Kt_ * T;

         //! initial equality constraints
         fgrad[offset]     = vars[Indices::A0];
         fgrad[1 + offset] = vars[Indices::B0];
         fgrad[2 + offset] = vars[Indices::A1];
         fgrad[3 + offset] = vars[Indices::B1];
         offset += 4;

         //! terminal inequality constraints
         fgrad[offset]     = x;
         fgrad[offset + 1] = y;
         fgrad[offset + 2] = vx;
         fgrad[offset + 3] = vy;
     }
};

void Trajectory::solve(WaypointList waypoints) {
    if (waypoints.size() != 2) {
        std::cerr << "Trajectory::solve - Function requires 2 waypoints." << std::endl;
        return;
    }

    //! status flag for solution
    bool ok;
    //! typedef for ipopt/cppad
    typedef CPPAD_TESTVECTOR(double) Dvector;
    //! no. of variables for optimization problem
    size_t n_vars = 13;
    //! no. of constraints
    size_t n_cons = 4 * 2;  // the start and final waypoint each contribute 4 constraints (x, y, theta, v) -> (x, y, vx, vy)
    //! create vector container for optimizer solution
    //! and initialize it to zero
    Dvector vars(n_vars);
    for (size_t i = 0; i < n_vars; i++) {
        vars[i] = 0;
    }

    //! set initial state (this will only determine the first two coefficients of the initial polynomials)
    double v = (fabs(waypoints[0].linear_velocity) < 1e-3)
        ? 1e-3 : waypoints[0].linear_velocity;
    vars[Indices::A0] = waypoints[0].x;
    vars[Indices::B0] = waypoints[0].y;
    vars[Indices::A1] = v * cos(waypoints[0].theta);
    vars[Indices::B1] = v * sin(waypoints[0].theta);
    vars[Indices::T] = 0;
    //! there are no explicit bounds on vars, so set to something large for the optimizer
    //! we could perhaps put bounds on the coeffs corresponding to acc, jerk, snap, ..
    Dvector vars_lb(n_vars);
    Dvector vars_ub(n_vars);

    for (size_t i = 0; i < n_vars; i++) {
        vars_lb[i] = -1e10;
        vars_ub[i] =  1e10;
    }

    //! time must be non-negative!
    vars_lb[Indices::T] = 0;

    //! set the bounds on the constraints
    Dvector cons_lb(n_cons);
    Dvector cons_ub(n_cons);

    //! offset term on index
    size_t offset = 0;

    //! initial equality constraint - we must start from where we are!
    cons_lb[0] = waypoints[0].x;
    cons_ub[0] = waypoints[0].x;

    cons_lb[1] = waypoints[0].y;
    cons_ub[1] = waypoints[0].y;

    cons_lb[2] = v * cos(waypoints[0].theta);
    cons_ub[2] = v * cos(waypoints[0].theta);

    cons_lb[3] = v * sin(waypoints[0].theta);
    cons_ub[3] = v * sin(waypoints[0].theta);

    offset += 4;

    //! terminal point
    cons_lb[offset] = waypoints[1].x;
    cons_ub[offset] = waypoints[1].x;

    cons_lb[offset + 1] = waypoints[1].y;
    cons_ub[offset + 1] = waypoints[1].y;

    cons_lb[offset + 2] = 1e-3 * cos(waypoints[1].theta);
    cons_ub[offset + 2] = 1e-3 * cos(waypoints[1].theta);

    cons_lb[offset + 3] = 1e-3 * sin(waypoints[1].theta);
    cons_ub[offset + 3] = 1e-3 * sin(waypoints[1].theta);

    //! create instance of objective function class
    FGradEval fg_eval(Kj_, Kt_);

    //! IPOPT INITIALIZATION
    std::string options;
    options += "Integer print_level  5\n";
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    options += "Integer max_iter         100\n";
    // options += "Numeric tol         1e-4\n";

    //! compute the solution
    CppAD::ipopt::solve_result<Dvector> solution;

    //! solve
    CppAD::ipopt::solve<Dvector, FGradEval>(
            options, vars, vars_lb, vars_ub, cons_lb, cons_ub, fg_eval, solution);

    //! check if the solver was successful
    ok = solution.status == CppAD::ipopt::solve_result<Dvector>::success;

    //! if the solver was unsuccessful, exit
    //! this case will be handled by calling method
    if (!ok) {
        std::cerr << "Trajectory::solve - Failed to find a solution!" << std::endl;
        return;
    }

    //! (DEBUG) output the final cost
    std::cout << "Final Cost: " << solution.obj_value << std::endl;

    //! populate output with argmin vector
    for (size_t i = 0; i < n_vars; i++) {
        solution_.push_back(solution.x[i]);
    }

    return;
}

我遇到问题的地方如下:

1. 初始等式约束(起始位置、速度和方向)得到支持，而终端速度约束则没有。该算法终止于正确的最终 (x,y,angle)，但速度不为零。我已经查看了代码，但我不明白为什么会遵守端点的位置和方向而速度不会。我怀疑我对等式约束的定义不是我想的那样。
2. 问题没有规律地收敛，但这似乎是一个相当简单的问题(见输出)

******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************

This is Ipopt version 3.11.9, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).

Number of nonzeros in equality constraint Jacobian...:       30
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:       23

Total number of variables............................:       13
                     variables with only lower bounds:        0
                variables with lower and upper bounds:       13
                     variables with only upper bounds:        0
Total number of equality constraints.................:        8
Total number of inequality constraints...............:        0
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:        0

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
   0  9.9999900e-03 1.00e+00 5.00e-04  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0
   1  5.9117705e-02 1.00e+00 1.20e+02  -1.0 5.36e+07    -  1.04e-05 7.63e-06f 18
   2  1.1927070e+00 1.00e+00 2.62e+06  -1.0 9.21e+05  -4.0 6.16e-15 2.29e-23H  1
   3  2.9689692e-01 1.00e+00 1.80e+05  -1.0 2.24e+13    -  1.83e-07 8.42e-10f 20
   4r 2.9689692e-01 1.00e+00 1.00e+03  -0.0 0.00e+00    -  0.00e+00 4.58e-07R 11
   5r 2.1005820e+01 9.99e-01 5.04e+02  -0.0 6.60e-02    -  9.90e-01 4.95e-01f  2
   6r 7.7118141e+04 9.08e-01 5.18e+03  -0.0 2.09e+00    -  4.21e-01 1.00e+00f  1
   7r 1.7923891e+04 7.82e-01 1.54e+03  -0.0 3.63e+00    -  9.90e-01 1.00e+00f  1
   8r 5.9690221e+03 5.41e-01 5.12e+02  -0.0 2.92e+00    -  9.90e-01 1.00e+00f  1
   9r 4.6855625e+03 5.54e-01 1.95e+02  -0.0 5.14e-01    -  9.92e-01 1.00e+00f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  10r 8.4901226e+03 5.55e-01 5.18e+01  -0.0 2.24e-01    -  1.00e+00 1.00e+00f  1

Number of Iterations....: 10

                                   (scaled)                 (unscaled)
Objective...............:   8.4901225582208808e+03    8.4901225582208808e+03
Dual infeasibility......:   6.3613117039244315e+06    6.3613117039244315e+06
Constraint violation....:   5.5503677023620179e-01    5.5503677023620179e-01
Complementarity.........:   9.9999982900301554e-01    9.9999982900301554e-01
Overall NLP error.......:   6.3613117039244315e+06    6.3613117039244315e+06


Number of objective function evaluations             = 43
Number of objective gradient evaluations             = 6
Number of equality constraint evaluations            = 71
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 12
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 10
Total CPU secs in IPOPT (w/o function evaluations)   =      0.006
Total CPU secs in NLP function evaluations           =      0.001

EXIT: Maximum Number of Iterations Exceeded.

我并不是专门为我的问题寻找答案。我希望得到一些关于为什么我的问题可能无法按预期工作的建议。具体来说，我的约束是否有意义，如定义的那样？变量初始化是否正确？

Answer 1

问题出在以下几行:

 x    = a0 + a1*T + a2*T2 + a3*T3 + a4*T4 + a5*T5;
 y    = b0 + b1*T + b2*T2 + b3*T3 + b4*T4 + b5*T5;
 vx   = a1 + 2*a2*T + 3*a3*T2 + 4*b4*T3 + 5*a5*T4;
 vy   = b1 + 2*b2*T + 3*b3*T2 + 4*b4*T3 + 5*b5*T4;

具体来说，

vx   = a1 + 2*a2*T + 3*a3*T2 + 4*b4*T3 + 5*a5*T4;

应该是

vx   = a1 + 2*a2*T + 3*a3*T2 + 4*a4*T3 + 5*a5*T4;

基于 a 到 x 坐标和 b 到 y 坐标的映射。

这解决了违反约束的问题。

关于收敛/可行性问题，我发现确保初始猜测在可行集中(遵守等式约束)解决了这个问题；在固定初始条件后，优化器性能的度量(inf_pr 和 inf_du 等...)要小得多。