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);
}
  • 11
    点赞
  • 9
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
### 回答1: mpc_local_planner是一种基于模型预测控制的本地路径规划器,主要用于自主驾驶车辆的路径规划。以下是mpc_local_planner的使用教程: 1. 安装ROS和mpc_local_planner 首先需要安装ROS和mpc_local_planner。可以通过以下命令安装: ``` sudo apt-get install ros-kinetic-mpc-local-planner ``` 2. 配置参数 在使用mpc_local_planner之前,需要对其进行参数配置。可以通过修改launch文件中的参数来进行配置。主要需要配置的参数包括: - robot_radius:机器人半径 - max_vel_x:机器人最大线速度 - min_vel_x:机器人最小线速度 - max_vel_theta:机器人最大角速度 - min_vel_theta:机器人最小角速度 - acc_lim_x:机器人线加速度限制 - acc_lim_theta:机器人角加速度限制 - sim_time:模拟时间 - sim_granularity:模拟粒度 - angular_sim_granularity:角度模拟粒度 3. 启动mpc_local_planner 启动mpc_local_planner需要使用roslaunch命令。可以通过以下命令启动: ``` roslaunch mpc_local_planner mpc_local_planner.launch ``` 启动后,mpc_local_planner会订阅机器人的位姿信息和全局路径信息,并发布机器人的速度控制信息。 4. 可视化 可以使用rviz来可视化mpc_local_planner的运行情况。可以通过以下命令启动rviz: ``` rosrun rviz rviz ``` 在rviz中,需要添加以下显示: - RobotModel:机器人模型 - Path:全局路径 - Local Plan:局部路径 通过以上步骤,就可以使用mpc_local_planner进行路径规划了。 ### 回答2: mpc_local_planner是一种基于模型预测控制的局部路径规划器,可以在ROS系统上运行。它适用于机器人或自动驾驶车辆在未知环境或障碍物密集区域进行路径规划,并能考虑车辆动力学、环境条件等因素,使得路径更加稳定和安全。 使用mpc_local_planner需要先进行安装,具体操作可以参考ROS官方网站的安装指南。安装成功后,在ROS工作空间中创建一个新的包,并在package.xml文件中添加依赖,安装所需的依赖库。 首先,需要在代码中引入mpc_local_planner相关的头文件,并定义一个全局变量planner,类型为MPCPlannerROS(注意,此处需要使用MPCPlannerROS而不是MPCPlanner,因为它将ROS系统与MPC控制器结合在一起): #include <mpc_local_planner/mpc_planner_ros.h> MPCPlannerROS planner; 接下来,需要在ROS节点中初始化planner: ros::NodeHandle private_nh("~"); planner.initialize(private_nh); 然后,需要在主循环中调用planner的updatePlan方法,实时更新路径规划结果: while (ros::ok()) { // 获取机器人的位姿信息和局部地图信息,传递给planner.updatePlan()进行路径规划 planner.updatePlan(current_pose, local_costmap, local_map); } 最后,需要在ROS节点中添加一个订阅者,用于接收机器人的位姿信息、传感器数据等信息,以便更新路径规划: ros::Subscriber pose_sub = nh.subscribe("/amcl_pose", 10, poseCallback); 其中poseCallback为回调函数,用于获取机器人的位姿信息,并调用planner的setPlan方法,将目标路径传递给planner: void poseCallback(const geometry_msgs::PoseStamped& pose) { // 获取机器人当前的位姿信息,并调用planner的setPlan()方法传递目标路径 tf::Pose robot_pose; tf::poseMsgToTF(pose.pose, robot_pose); planner.setPlan(robot_pose); } mpc_local_planner的配置参数非常丰富,可以通过在ROS参数服务器中设置参数来调整路径规划的结果。例如,可以设置MPC控制器的时间步长、预测时间、约束条件等参数,或者调整局部地图的分辨率、占用阈值等参数,以达到更好的效果。 总之,mpc_local_planner是一种非常高效、灵活的路径规划器,能够适应各种环境和条件,并能根据实时传感器数据进行实时更新。虽然使用起来有一定的复杂度,但是掌握一定的使用方法和技巧,仍然可以在实际应用中发挥出很大的作用。 ### 回答3: MPC_local_planner是一种移动底盘路径规划器,通过模型预测控制的方法计算出机器人在未来一定时间内前进方向以及速度,从而达到对路径实时控制的目的,在ROS中常被用来作为机器人底层行走模块的路径规划器。 MPC_local_planner的使用教程如下: 1、安装MPC_local_planner 可以通过ROS论坛或者github下载源码进行安装,同时还需要安装和设置相关依赖包。 2、配置MPC_local_planner参数 MPC_local_planner的机器人行动学模型等参数都可在代码源文件中进行设置,主要包括机器人速度,角度等相关参数。 3、设置起始位置和目标点 在进行路径规划前,需要设置机器人的起始位置和目标点。 4、调用MPC_local_planner的API 在设置好相关参数和起始位置/目标点之后,可以调用MPC_local_planner提供的API进行路径规划,同时可以通过回调函数对路径进行相关调整和修改。 5、执行路径规划并进行路径跟踪 在完成路径规划之后,就可以根据生成的路径进行移动底层控制,实现机器人的路径跟踪。 总之,使用MPC_local_planner进行路径规划时要注意对参数的设置和路径的调整等,需要对ROS等相关操作系统有一定的掌握和理解,同时还要有一定的机器人运动学基础,才能够更好地完成路径规划的操作。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值