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);
}
离散网格配置
corbo::DiscretizationGridInterface::Ptr MpcController::configureGrid(const rclcpp_lifecycle::LifecycleNode::SharedPtr& nh)
{
FiniteDifferencesGridSE2::Ptr grid;
if (variable_grid)
{
FiniteDifferencesVariableGridSE2::Ptr grid = std::make_shared<FiniteDifferencesVariableGridSE2>();
}
else
{
grid = std::make_shared<FiniteDifferencesGridSE2>();
}
std::string collocation_method = "forward_differences";
if (collocation_method == "forward_differences")
{
grid->setFiniteDifferencesCollocationMethod(std::make_shared<ForwardDiffCollocationSE2>()); //_fd_eval
}
if (cost_integration_method == "left_sum")
{
grid->setCostIntegrationRule(FullDiscretizationGridBaseSE2::CostIntegrationRule::LeftSum); //_cost_integration
}
}
求解器配置
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，对应_functions.stage_cost，指针类型：QuadraticFormCostSE2
ocp->setStageCost(std::make_shared<QuadraticFormCostSE2>(Q, R, integral_form, lsq_solver));
//终端cost，对应_functions.final_stage_cost，指针类型：QuadraticFinalStateCostSE2
ocp->setFinalStageCost(std::make_shared<QuadraticFinalStateCostSE2>(Qf, lsq_solver));
//不等式约束，对应_functions.stage_inequalities，指针类型：StageInequalitySE2
_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); //这里参考点只取了目标点一个点，也就是直接追踪裁剪后的最后一个点，这种做法是有问题的
corbo::ZeroReference uref(_dynamics->getInputDimension());
//当前点，目标点，参考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，这里sref和singal_target和uint都为nullptr，xinit是_x_seq_init
_ocp->compute(x, xref, uref, sref, t, i == 0, signal_target, xinit, uinit, ns); //求解器进行计算
//后续进入strucured_optimal_control_problem.cpp
最优化问题求解接口
structured_optimal_control_problem.cpp
//当前点，参考点，参考U，nullptr，离散时间，第一次，nullptr，_x_seq_init，nullptr
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);
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);
}
离散网格更新
full_discretization_grid_base_se2.cpp
corbo::GridUpdateResult FullDiscretizationGridBaseSE2::update(const Eigen::VectorXd& x0, ReferenceTrajectoryInterface& xref,
ReferenceTrajectoryInterface& uref, NlpFunctions& nlp_fun, OptimizationEdgeSet& edges,
SystemDynamicsInterface::Ptr dynamics, bool new_run, const corbo::Time& t,
ReferenceTrajectoryInterface* sref, const Eigen::VectorXd* prev_u, double prev_u_dt,
ReferenceTrajectoryInterface* xinit, ReferenceTrajectoryInterface* uinit)
{
//设置_x_seq为参考轨迹，_u_seq为参考控制，一般为0
initializeSequences(x0, xref.getReferenceCached(n - 1), xinit ? *xinit : xref, uinit ? *uinit : uref, nlp_fun);
if (new_run || result.updated()) // new run -> new t
{
//更新各个边的状态
result.edges_updated =
nlp_fun.update(getN(), t.toSec(), xref, uref, sref, hasSingleDt(), x0, {getDt()}, this); // returns true if edge dimensions changed
}
if (result.updated()) // vertices or eges updated
{
createEdges(nlp_fun, edges, dynamics); //构建实际超图
result.edges_updated = true; // now we definitely updated the edgeset
}
}
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); //将更新的值传入顶点里面，供grid使用
if (_solver->_cache_first_order_derivatives) precompute1stOrderDerivatives();
}
obj_value = _problem->computeValueObjective(); //计算目标函数cost
}
//计算约束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);
}
