mpc_local_planner详解

控制器配置

Controller::configure()
{
  //创建机器模型
  _dynamics = std::make_shared<UnicycleModel>();
  //离散网络，比如多重打靶法。参考点，输入，状态，等变量也会存放在grid里面，会实时更新。而且grid也继承了顶点传入到超图问题构建中
  _grid   = configureGrid(nh);  
  //求解器
  _solver = configureSolver(nh);
  //最优化问题构建, _dynamics，_grid，_solver这三个指针也会传入到最优化问题类里
  _structured_ocp = configureOcp(nh, obstacles, robot_model, via_points);
 
}

求解器配置

configureSolver()
{
  //SolverIpopt这个类里面会新建2个结构:
  //_ipopt_nlp = new IpoptWrapper(this);  
  //_ipopt_app = IpoptApplicationFactory(); 
 
  //IpoptWrapper类是Ipopt的结构壳子，在nlp_solver_ipopt_wrapper.cpp中，里面主要是求解器的接口
  //这个类只是壳子，具体的问题实现在optimaization里面的问题类里，在configureOcp()里面，就创建了一个HyperGraphOptimizationProblemEdgeBased超图最优问题
  //get_nlp_info()  变量和约束信息
  //eval_f() 目标函数
  //eval_jac_g() 雅可比矩阵
  //eval_h()  海森矩阵   
 
  //IpoptApplicationFactory是ipopt的标准用法，创建一个IPOPT应用程序：
  //ApplicationReturnStatus status;
  //status = _ipopt_app->Initialize();
  // 设置优化参数
  //_ipopt_app->Options()->SetNumericValue("tol", 1e-9); //最小迭阈值
  //_ipopt_app->Options()->SetStringValue("mu_strategy", "adaptive");
  //_ipopt_app->OptimizeTNLP(_ipopt_nlp);
  //_ipopt_nlp->eval_f();  //获取
 
  corbo::SolverIpopt::Ptr solver = std::make_shared<corbo::SolverIpopt>();
  solver->initialize();
  solver->setIterations(iterations); //迭代次数
  solver->setMaxCpuTime(max_cpu_time);//最大计算时间
  solver->setIpoptOptionNumeric();  //对应SetNumericValue() 最小迭代阈值
  solver->setIpoptOptionString();  //对应SetStringValue
}

最优化问题构造

Controller::configureOcp()
{
//构建一个超图最优化问题框架，
corbo::BaseHyperGraphOptimizationProblem::Ptr hg = std::make_shared<corbo::HyperGraphOptimizationProblemEdgeBased>();
 
//构建一个最优控制问题框架,相当于在上面的框架上再套一层
corbo::StructuredOptimalControlProblem::Ptr ocp = std::make_shared<corbo::StructuredOptimalControlProblem>(_grid, _dynamics, hg, _solver);
 
 
//控制输入边界
ocp->setControlBounds(Eigen::Vector2d(-max_vel_x_backwards, -max_vel_theta), Eigen::Vector2d(max_vel_x, max_vel_theta));
//二次型目标函数cost
ocp->setStageCost(std::make_shared<QuadraticFormCostSE2>(Q, R, integral_form, lsq_solver));
//终端cost
ocp->setFinalStageCost(std::make_shared<QuadraticFinalStateCostSE2>(Qf, lsq_solver));
 
_inequality_constraint = std::make_shared<StageInequalitySE2>();
//障碍物不等式约束
_inequality_constraint->setObstacleVector(obstacles);
//footprint不等式约束
_inequality_constraint->setRobotFootprintModel(robot_model);
//设置障碍物最小距离
_inequality_constraint->setMinimumDistance(min_obstacle_dist);
//是否开启动态障碍物
_inequality_constraint->setEnableDynamicObstacles(enable_dynamic_obstacles);
//障碍物过滤
_inequality_constraint->setObstacleFilterParameters(force_inclusion_dist, cutoff_dist);
//加速度约束
Eigen::Vector2d ud_lb(-dec_lim_x, -acc_lim_theta);
Eigen::Vector2d ud_ub(acc_lim_x, acc_lim_theta);
_inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub);
 
//不等式约束传入最优控制器里
ocp->setStageInequalityConstraint(_inequality_constraint);
 
}

迭代过程

//由computeVelocityCommands过来
bool Controller::step(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist& vel, double dt, ros::Time t,
                      corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq)
{
  _dynamics->getSteadyStateFromPoseSE2(goal, xf); //目标点转为eigen格式
  //起始点根据状态反馈，来选择是用传入的start点，还是用反馈的状态点，还是用odom点。
  _dynamics->getSteadyStateFromPoseSE2(start, x);
  if(如果目标与上一个目标之间的距离或角度变化大于阈值，将清除路径规划数据 _grid。这是为了确保机器人能够适应新的目标或路径)
  {
    _grid->clear();
  }
  if (_grid->isEmpty())  //网格路径是否是空
  {
    bool backward = _guess_backwards_motion && (goal.position() - start.position()).dot(start.orientationUnitVec()) < 0;  //是否需要倒车
    //添加时间序列信息以及姿态信息，从而转换为初始轨迹，并采用线性差值对相邻两个轨迹点之间的轨迹进行差值，生成的轨迹存放在controller类的变量_x_seq_init中
    generateInitialStateTrajectory(x, xf, initial_plan, backward);  //生成参考轨迹
  }
 
  corbo::StaticReference xref(xf);  //这里参考点只取了目标点一个点，状态cost是每个预测点和目标点的偏差？
  //当前点，目标点，参考U，时间，离散时间，输入队列，状态队列
  _ocp_successful = PredictiveController::step(x, xref, uref, corbo::Duration(dt), time, u_seq, x_seq, nullptr, nullptr, &_x_seq_init);  //求解问题
 
  这里会进入predictive_controller.cpp里面的step
  _ocp->compute(x, xref, uref, sref, t, i == 0, signal_target, xinit, uinit, ns);  //求解器进行计算
  //后续进入strucured_optimal_control_problem.cpp

最优化问题求解接口

structured_optimal_control_problem.cpp

bool StructuredOptimalControlProblem::compute(const StateVector& x, ReferenceTrajectoryInterface& xref, ReferenceTrajectoryInterface& uref,
                                              ReferenceTrajectoryInterface* sref, const Time& t, bool new_run, SignalTargetInterface* signal_target,
                                              ReferenceTrajectoryInterface* xinit, ReferenceTrajectoryInterface* uinit, const std::string& ns)
{
  GridUpdateResult grid_udpate_result =
    _grid->update(x, xref, uref, _functions, *_edges, _dynamics, new_run, t, sref, &_u_prev, _u_prev_dt, xinit, uinit);  //状态更新至grid
  if (grid_udpate_result.vertices_updated)
  {
     _optim_prob->precomputeVertexQuantities();
  }
  if (grid_udpate_result.updated())
  {
     _optim_prob->precomputeEdgeQuantities();
  }  
  _solver->solve(*_optim_prob, grid_udpate_result.updated(), new_run, &_objective_value);
}

ipopt接口

nlp_solver_ipopt.cpp

SolverStatus SolverIpopt::solve(OptimizationProblemInterface& problem, bool new_structure, bool new_run, double* obj_value)
{
    _ipopt_nlp->setOptimizationProblem(problem);  //将问题指针传入nlp，nlp里面的计算过程全在problem里面
    //如果是第一次，先计算雅可比矩阵，海森矩阵
    if (new_structure)
    {
        _nnz_jac_constraints = problem.computeCombinedSparseJacobiansNNZ(false, true, true);
 
        problem.computeSparseHessiansNNZ(_nnz_hes_obj, _nnz_hes_eq, _nnz_hes_ineq, true);
        _nnz_h_lagrangian = _nnz_hes_obj + _nnz_hes_eq + _nnz_hes_ineq;
 
        _lambda_cache.resize(problem.getEqualityDimension() + problem.getInequalityDimension());
        _lambda_cache.setZero();
 
        _zl_cache.resize(problem.getParameterDimension());
        _zl_cache.setZero();
 
        _zu_cache.resize(problem.getParameterDimension());
        _zu_cache.setZero();
 
        // set max number of iterations
        _ipopt_app->Options()->SetIntegerValue("max_iter", _iterations);  // max_cpu_time // TODO(roesmann) parameter for number of iterations
    }
 
    if (_max_cpu_time > 0)
        _ipopt_app->Options()->SetNumericValue("max_cpu_time", _max_cpu_time);
    else if (_max_cpu_time == 0)
        _ipopt_app->Options()->SetNumericValue("max_cpu_time", 10e6);
 
    Ipopt::ApplicationReturnStatus ipopt_status;
    if (new_structure)
        ipopt_status = _ipopt_app->OptimizeTNLP(_ipopt_nlp);  //执行优化
    else
        ipopt_status = _ipopt_app->ReOptimizeTNLP(_ipopt_nlp);
 
    if (obj_value) *obj_value = _last_obj_value;
 
    return convertIpoptToNlpSolverStatus(ipopt_status);
}

ipoptWrapper接口

//计算目标函数
eval_f() 
{
  if (new_x)
    {
        Eigen::Map<const Eigen::VectorXd> x_map(x, n);
        _problem->setParameterVector(x_map);
        if (_solver->_cache_first_order_derivatives) precompute1stOrderDerivatives();
    }
 
    obj_value = _problem->computeValueObjective();
}
 
//计算约束g(x)
eval_g()
{
  if (new_x)
    {
        Eigen::Map<const Eigen::VectorXd> x_map(x, n);
        _problem->setParameterVector(x_map);
        if (_solver->_cache_first_order_derivatives) precompute1stOrderDerivatives();
    }
  Eigen::Map<Eigen::VectorXd> g_map(g, m);
  _problem->computeValuesEquality(g_map.head(_problem->getEqualityDimension()));
  _problem->computeValuesInequality(g_map.tail(_problem->getInequalityDimension()));
}
 
//计算雅可比
eval_jac_g()
{
  if (new_x)
  {
     Eigen::Map<const Eigen::VectorXd> x_map(x, n);
     _problem->setParameterVector(x_map);
     if (_solver->_cache_first_order_derivatives) precompute1stOrderDerivatives();
  }
  Eigen::Map<Eigen::VectorXd> values_map(values, nele_jac);
  if (_solver->_cache_first_order_derivatives)
      values_map = _solver->_jac_constr_cache;
  else
      _problem->computeCombinedSparseJacobiansValues(values_map, false, true, true);
}