我正在尝试评估两个五阶多项式的系数和时间（x和y位置各一个），它们在将初始位置，速度和方向连接到所需时最小化努力和时间（目标函数）最终位置和方向，0速度（等式约束）。这是代码：
$.ajax({
url: "http://127.0.0.1:5005/runNetwork",
method: "POST",
data: { foo : "bar" },
success: function(response) {
console.log(response);
},
error: function(response, error) {
console.log(response)
console.log(error);
}
});
我遇到问题的地方如下：
#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; }
我并不是特意找到问题的答案。我希望的是一些建议，为什么我的问题可能没有按预期工作。具体来说，我的约束是否有意义，如定义？变量初始化是否正确完成？
答案 0 :(得分：0)
问题在于以下几行：
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到x坐标和b到y坐标的映射。
这解决了违反约束的问题。
关于收敛/可行性的问题，我发现确保初始猜测在可行集（服从等式约束）中解决了这个问题;在确定初始条件后，优化器性能（inf_pr和inf_du等等）的测量值要小得多。