diff --git a/mpc_local_planner/src/controller.cpp b/mpc_local_planner/src/controller.cpp index dd42a17..b8524a6 100644 --- a/mpc_local_planner/src/controller.cpp +++ b/mpc_local_planner/src/controller.cpp @@ -59,9 +59,9 @@ bool Controller::configure(ros::NodeHandle& nh, const teb_local_planner::ObstCon teb_local_planner::RobotFootprintModelPtr robot_model, const std::vector& via_points) { - //创建机器模型 + // 创建机器人动力学模型 _dynamics = configureRobotDynamics(nh); - if (!_dynamics) return false; // we may need state and control dimensions to check other parameters + if (!_dynamics) return false; // 我们可能需要状态和控制维度来检查其他参数 //离散网络，比如多重打靶法。参考点，输入，状态，等变量也会存放在grid里面，会实时更新。而且grid也继承了顶点传入到超图问题构建中 _grid = configureGrid(nh); //求解器 @@ -74,23 +74,23 @@ bool Controller::configure(ros::NodeHandle& nh, const teb_local_planner::ObstCon nh.param("controller/outer_ocp_iterations", outer_ocp_iterations, outer_ocp_iterations); setNumOcpIterations(outer_ocp_iterations); - // further goal opions + // 进一步的目标选项 nh.param("controller/force_reinit_new_goal_dist", _force_reinit_new_goal_dist, _force_reinit_new_goal_dist); nh.param("controller/force_reinit_new_goal_angular", _force_reinit_new_goal_angular, _force_reinit_new_goal_angular); nh.param("controller/allow_init_with_backward_motion", _guess_backwards_motion, _guess_backwards_motion); nh.param("controller/force_reinit_num_steps", _force_reinit_num_steps, _force_reinit_num_steps); - // custom feedback: + // 自定义反馈 nh.param("controller/prefer_x_feedback", _prefer_x_feedback, _prefer_x_feedback); _x_feedback_sub = nh.subscribe("state_feedback", 1, &Controller::stateFeedbackCallback, this); - // result publisher: + // 结果发布 _ocp_result_pub = nh.advertise("ocp_result", 100); nh.param("controller/publish_ocp_results", _publish_ocp_results, _publish_ocp_results); nh.param("controller/print_cpu_time", _print_cpu_time, _print_cpu_time); - setAutoUpdatePreviousControl(false); // we want to update the previous control value manually + setAutoUpdatePreviousControl(false); // 我们希望手动更新之前的控制值 if (_ocp->initialize()) ROS_INFO("OCP initialized."); @@ -111,14 +111,30 @@ bool Controller::step(const Controller::PoseSE2& start, const Controller::PoseSE return step(initial_plan, vel, dt, t, u_seq, x_seq); } +/** + * @brief 执行控制器的一步 + * + * 该函数根据初始计划、当前速度、时间步长和当前时间执行控制器的一步操作，生成控制输入序列和状态序列。 + * + * @param initial_plan 初始路径规划，由多个几何消息PoseStamped组成 + * @param vel 当前速度（Twist） + * @param dt 时间步长（秒） + * @param t 当前时间（ROS时间） + * @param u_seq 输出控制输入序列（corbo::TimeSeries::Ptr） + * @param x_seq 输出状态序列（corbo::TimeSeries::Ptr） + * @return true 如果步骤成功执行 + * @return false 如果步骤执行失败 + */ bool Controller::step(const std::vector& initial_plan, const geometry_msgs::Twist& vel, double dt, ros::Time t, corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq) { + // 检查控制器配置是否完整 if (!_dynamics || !_grid || !_structured_ocp) { ROS_ERROR("Controller must be configured before invoking step()."); return false; } + // 检查初始计划至少包含两个姿态 if (initial_plan.size() < 2) { ROS_ERROR("Controller::step(): initial plan must contain at least two poses."); @@ -128,10 +144,11 @@ bool Controller::step(const std::vector& initial_pla PoseSE2 start(initial_plan.front().pose); PoseSE2 goal(initial_plan.back().pose); + // 获取目标姿态的稳定状态 Eigen::VectorXd xf(_dynamics->getStateDimension()); _dynamics->getSteadyStateFromPoseSE2(goal, xf); - // retrieve or estimate current state + // 检索或估计当前状态 Eigen::VectorXd x(_dynamics->getStateDimension()); // check for new measurements bool new_x = false; @@ -140,28 +157,28 @@ bool Controller::step(const std::vector& initial_pla new_x = _recent_x_feedback.size() > 0 && (t - _recent_x_time).toSec() < 2.0 * dt; if (new_x) x = _recent_x_feedback; } - if (!new_x && (!_x_ts || _x_ts->isEmpty() || !_x_ts->getValuesInterpolate(dt, x))) // predict with previous state sequence + if (!new_x && (!_x_ts || _x_ts->isEmpty() || !_x_ts->getValuesInterpolate(dt, x))) // 使用之前的状态序列预测 { - _dynamics->getSteadyStateFromPoseSE2(start, x); // otherwise initialize steady state + _dynamics->getSteadyStateFromPoseSE2(start, x); // 否则初始化稳定状态 } if (!new_x || !_prefer_x_feedback) { - // Merge state feedback with odometry feedback if desired. + // 如果需要，将状态反馈与里程反馈合并 // Note, some models like unicycle overwrite the full state by odom feedback unless _prefer_x_measurement is set to true. _dynamics->mergeStateFeedbackAndOdomFeedback(start, vel, x); } - // now check goal + // 检查目标 if (_force_reinit_num_steps > 0 && _ocp_seq % _force_reinit_num_steps == 0) _grid->clear(); if (!_grid->isEmpty() && ((goal.position() - _last_goal.position()).norm() > _force_reinit_new_goal_dist || std::abs(normalize_theta(goal.theta() - _last_goal.theta())) > _force_reinit_new_goal_angular)) { - // goal pose diverges a lot from the previous one, so force reinit + // 如果目标姿态与之前的姿态差异较大，则强制重新初始化 _grid->clear(); } if (_grid->isEmpty()) { - // generate custom initialization based on initial_plan + // 基于初始计划生成自定义初始化 // check if the goal is behind the start pose (w.r.t. start orientation) bool backward = _guess_backwards_motion && (goal.position() - start.position()).dot(start.orientationUnitVec()) < 0; generateInitialStateTrajectory(x, xf, initial_plan, backward); @@ -169,11 +186,12 @@ bool Controller::step(const std::vector& initial_pla corbo::Time time(t.toSec()); _x_seq_init.setTimeFromStart(time); - corbo::StaticReference xref(xf); // currently, we only support point-to-point transitions in ros + corbo::StaticReference xref(xf); // 目前，我们仅支持在ros中进行点到点的转换 corbo::ZeroReference uref(_dynamics->getInputDimension()); + // 执行预测控制器的一步 _ocp_successful = PredictiveController::step(x, xref, uref, corbo::Duration(dt), time, u_seq, x_seq, nullptr, nullptr, &_x_seq_init); - // publish results if desired + // 如果需要，发布结果 if (_publish_ocp_results) publishOptimalControlResult(); // TODO(roesmann): we could also pass time t from above ROS_INFO_STREAM_COND(_print_cpu_time, "Cpu time: " << _statistics.step_time.toSec() * 1000.0 << " ms."); ++_ocp_seq; @@ -225,34 +243,45 @@ void Controller::publishOptimalControlResult() void Controller::reset() { PredictiveController::reset(); } +/** + * @brief 配置离散网格 + * + * 该函数根据参数服务器中的配置，选择并创建适当的离散网格。 + * + * @param nh ROS节点句柄，用于获取参数 + * @return corbo::DiscretizationGridInterface::Ptr 返回创建的离散网格指针 + */ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::NodeHandle& nh) { + // 如果动力学模型未配置，返回空指针 if (!_dynamics) return {}; + // 默认网格类型为 "fd_grid" std::string grid_type = "fd_grid"; nh.param("grid/type", grid_type, grid_type); if (grid_type == "fd_grid") { FiniteDifferencesGridSE2::Ptr grid; - + // 配置可变网格 bool variable_grid = true; nh.param("grid/variable_grid/enable", variable_grid, variable_grid); if (variable_grid) { FiniteDifferencesVariableGridSE2::Ptr var_grid = std::make_shared(); - + // 从参数服务器中获取最小时间步长和最大时间步长 double min_dt = 0.0; nh.param("grid/variable_grid/min_dt", min_dt, min_dt); double max_dt = 10.0; nh.param("grid/variable_grid/max_dt", max_dt, max_dt); var_grid->setDtBounds(min_dt, max_dt); - + // 从参数服务器中获取是否启用网格适应，默认值为 true bool grid_adaptation = true; nh.param("grid/variable_grid/grid_adaptation/enable", grid_adaptation, grid_adaptation); if (grid_adaptation) { + // 从参数服务器中获取最大网格大小、时间步长滞后比率和最小网格大小 int max_grid_size = 50; nh.param("grid/variable_grid/grid_adaptation/max_grid_size", max_grid_size, max_grid_size); double dt_hyst_ratio = 0.1; @@ -273,15 +302,17 @@ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::Nod { grid = std::make_shared(); } - // common grid parameters + + // 公共网格参数 + // 从参数服务器中获取网格参考大小，默认值为 20 int grid_size_ref = 20; nh.param("grid/grid_size_ref", grid_size_ref, grid_size_ref); grid->setNRef(grid_size_ref); - + // 从参数服务器中获取时间步长参考值，默认值为 0.3 double dt_ref = 0.3; nh.param("grid/dt_ref", dt_ref, dt_ref); grid->setDtRef(dt_ref); - + // 从参数服务器中获取状态固定向量，默认值为 {true, true, true} std::vector xf_fixed = {true, true, true}; nh.param("grid/xf_fixed", xf_fixed, xf_fixed); if ((int)xf_fixed.size() != _dynamics->getStateDimension()) @@ -293,11 +324,11 @@ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::Nod Eigen::Matrix xf_fixed_eigen(xf_fixed.size()); // we cannot use Eigen::Map as vector does not provide raw access for (int i = 0; i < (int)xf_fixed.size(); ++i) xf_fixed_eigen[i] = xf_fixed[i]; grid->setXfFixed(xf_fixed_eigen); - + // 从参数服务器中获取是否启用热启动，默认值为 true bool warm_start = true; nh.param("grid/warm_start", warm_start, warm_start); grid->setWarmStart(warm_start); - + // 从参数服务器中获取配点方法，默认值为 "forward_differences" std::string collocation_method = "forward_differences"; nh.param("grid/collocation_method", collocation_method, collocation_method); @@ -318,6 +349,7 @@ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::Nod ROS_ERROR_STREAM("Unknown collocation method '" << collocation_method << "' specified. Falling back to default..."); } + // 从参数服务器中获取代价积分方法，默认值为 "left_sum" std::string cost_integration_method = "left_sum"; nh.param("grid/cost_integration_method", cost_integration_method, cost_integration_method); @@ -344,21 +376,33 @@ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::Nod return {}; } +/** + * @brief 配置机器人动力学模型 + * + * 该函数根据参数服务器中的配置，选择并创建适当的机器人动力学模型。 + * + * @param nh ROS节点句柄，用于获取参数 + * @return RobotDynamicsInterface::Ptr 返回创建的机器人动力学模型指针 + */ RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHandle& nh) { + // 默认机器人类型为 "unicycle"（单轮车模型） _robot_type = "unicycle"; nh.param("robot/type", _robot_type, _robot_type); if (_robot_type == "unicycle") { + // 如果机器人类型为 "unicycle"，返回 UnicycleModel 实例 return std::make_shared(); } else if (_robot_type == "simple_car") { + // 如果机器人类型为 "simple_car"（简单汽车模型） double wheelbase = 0.5; nh.param("robot/simple_car/wheelbase", wheelbase, wheelbase); bool front_wheel_driving = false; nh.param("robot/simple_car/front_wheel_driving", front_wheel_driving, front_wheel_driving); + // 如果前轮驱动，返回 SimpleCarFrontWheelDrivingModel 实例 if (front_wheel_driving) return std::make_shared(wheelbase); else @@ -366,6 +410,7 @@ RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHa } else if (_robot_type == "kinematic_bicycle_vel_input") { + // 如果机器人类型为 "kinematic_bicycle_vel_input"（运动学自行车模型，速度输入） double length_rear = 1.0; nh.param("robot/kinematic_bicycle_vel_input/length_rear", length_rear, length_rear); double length_front = 1.0; @@ -374,32 +419,44 @@ RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHa } else { + // 未知的机器人类型，输出错误信息 ROS_ERROR_STREAM("Unknown robot type '" << _robot_type << "' specified."); } return {}; } +/** + * @brief 配置非线性规划（NLP）求解器 + * + * 该函数根据参数服务器中的配置，选择并创建适当的NLP求解器。 + * + * @param nh ROS节点句柄，用于获取参数 + * @return corbo::NlpSolverInterface::Ptr 返回创建的NLP求解器指针 + */ corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle& nh) { - + // 从参数服务器中获取求解器类型，默认值为 "ipopt" std::string solver_type = "ipopt"; nh.param("solver/type", solver_type, solver_type); if (solver_type == "ipopt") { + // 配置Ipopt求解器 corbo::SolverIpopt::Ptr solver = std::make_shared(); - solver->initialize(); // requried for setting parameters afterward + solver->initialize(); // 初始化以便之后设置参数 + // 从参数服务器中获取Ipopt求解器的迭代次数，默认值为100 int iterations = 100; nh.param("solver/ipopt/iterations", iterations, iterations); solver->setIterations(iterations); + // 从参数服务器中获取Ipopt求解器的最大CPU时间，默认值为-1.0 double max_cpu_time = -1.0; nh.param("solver/ipopt/max_cpu_time", max_cpu_time, max_cpu_time); solver->setMaxCpuTime(max_cpu_time); - // now check for additional ipopt options + // 设置其他Ipopt选项 std::map numeric_options; nh.param("solver/ipopt/ipopt_numeric_options", numeric_options, numeric_options); for (const auto& item : numeric_options) @@ -441,12 +498,15 @@ corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle // } else if (solver_type == "lsq_lm") { + // 配置Levenberg-Marquardt求解器 corbo::LevenbergMarquardtSparse::Ptr solver = std::make_shared(); + // 从参数服务器中获取Levenberg-Marquardt求解器的迭代次数，默认值为10 int iterations = 10; nh.param("solver/lsq_lm/iterations", iterations, iterations); solver->setIterations(iterations); + // 从参数服务器中获取初始惩罚权重 double weight_init_eq = 2; nh.param("solver/lsq_lm/weight_init_eq", weight_init_eq, weight_init_eq); double weight_init_ineq = 2; @@ -456,6 +516,7 @@ corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle solver->setPenaltyWeights(weight_init_eq, weight_init_ineq, weight_init_bounds); + // 从参数服务器中获取权重适应因子 double weight_adapt_factor_eq = 1; nh.param("solver/lsq_lm/weight_adapt_factor_eq", weight_adapt_factor_eq, weight_adapt_factor_eq); double weight_adapt_factor_ineq = 1; @@ -463,6 +524,7 @@ corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle double weight_adapt_factor_bounds = 1; nh.param("solver/lsq_lm/weight_adapt_factor_bounds", weight_adapt_factor_bounds, weight_adapt_factor_bounds); + // 从参数服务器中获取最大权重适应值 double weight_adapt_max_eq = 500; nh.param("solver/lsq_lm/weight_adapt_max_eq", weight_adapt_max_eq, weight_adapt_max_eq); double weight_adapt_max_ineq = 500; @@ -483,17 +545,30 @@ corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle return {}; } +/** + * @brief 配置结构化最优控制问题（OCP） + * + * 该函数根据参数服务器中的配置，选择并创建适当的OCP。 + * + * @param nh ROS节点句柄，用于获取参数 + * @param obstacles 障碍物容器 + * @param robot_model 机器人轮廓模型 + * @param via_points 中间目标点 + * @return corbo::StructuredOptimalControlProblem::Ptr 返回创建的OCP指针 + */ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles, teb_local_planner::RobotFootprintModelPtr robot_model, const std::vector& via_points) { + // 创建超图优化问题 corbo::BaseHyperGraphOptimizationProblem::Ptr hg = std::make_shared(); - + // 创建结构化最优控制问题实例 corbo::StructuredOptimalControlProblem::Ptr ocp = std::make_shared(_grid, _dynamics, hg, _solver); - const int x_dim = _dynamics->getStateDimension(); - const int u_dim = _dynamics->getInputDimension(); + const int x_dim = _dynamics->getStateDimension(); // 状态维度 + const int u_dim = _dynamics->getInputDimension(); // 控制输入维度 + // 根据机器人类型配置控制输入的上下界 if (_robot_type == "unicycle") { double max_vel_x = 0.4; @@ -551,6 +626,7 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros:: return {}; } + // 配置阶段成本（stage cost） std::string objective_type = "minimum_time"; nh.param("planning/objective/type", objective_type, objective_type); bool lsq_solver = _solver->isLsqSolver(); @@ -643,6 +719,7 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros:: return {}; } + // 配置终端成本（terminal cost） std::string terminal_cost = "none"; nh.param("planning/terminal_cost/type", terminal_cost, terminal_cost); @@ -676,6 +753,7 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros:: return {}; } + // 配置终端约束（terminal constraint） std::string terminal_constraint = "none"; nh.param("planning/terminal_constraint/type", terminal_constraint, terminal_constraint); @@ -711,28 +789,28 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros:: return {}; } + // 配置阶段不等式约束（stage inequality constraint） _inequality_constraint = std::make_shared(); _inequality_constraint->setObstacleVector(obstacles); _inequality_constraint->setRobotFootprintModel(robot_model); - // configure collision avoidance - + // 配置碰撞避免参数 double min_obstacle_dist = 0.5; - nh.param("collision_avoidance/min_obstacle_dist", min_obstacle_dist, min_obstacle_dist); + nh.param("collision/min_obstacle_dist", min_obstacle_dist, min_obstacle_dist); _inequality_constraint->setMinimumDistance(min_obstacle_dist); bool enable_dynamic_obstacles = false; - nh.param("collision_avoidance/enable_dynamic_obstacles", enable_dynamic_obstacles, enable_dynamic_obstacles); + nh.param("collision/enable_dynamic_obstacles", enable_dynamic_obstacles, enable_dynamic_obstacles); _inequality_constraint->setEnableDynamicObstacles(enable_dynamic_obstacles); double force_inclusion_dist = 0.5; - nh.param("collision_avoidance/force_inclusion_dist", force_inclusion_dist, force_inclusion_dist); + nh.param("collision/force_inclusion_dist", force_inclusion_dist, force_inclusion_dist); double cutoff_dist = 2; - nh.param("collision_avoidance/cutoff_dist", cutoff_dist, cutoff_dist); + nh.param("collision/cutoff_dist", cutoff_dist, cutoff_dist); _inequality_constraint->setObstacleFilterParameters(force_inclusion_dist, cutoff_dist); // configure control deviation bounds - + if (_robot_type == "unicycle") { double acc_lim_x = 0.0; @@ -879,7 +957,6 @@ bool Controller::isPoseTrajectoryFeasible(base_local_planner::CostmapModel* cost ROS_ERROR("isPoseTrajectoriyFeasible is currently only implemented for fd grids"); return true; } - ROS_INFO(" look_ahead_idx:%d",look_ahead_idx); if (look_ahead_idx < 0 || look_ahead_idx >= _grid->getN()) look_ahead_idx = _grid->getN() - 1; for (int i = 0; i <= look_ahead_idx; ++i) diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index fc65870..9e8939c 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -351,7 +351,7 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt _robot_vel.linear.y = robot_vel_tf.pose.position.y; _robot_vel.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation); - // 修剪全局计划，剪掉机器人之前的部分 + // 修剪全局计划，剪掉机器人之前的部分，从global的头部开始查找，直到距离机器人小于global_plan_prune_distance pruneGlobalPlan(*_tf, robot_pose, _global_plan, _params.global_plan_prune_distance); // Convert To Local frame @@ -366,9 +366,9 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt return mbf_msgs::ExePathResult::INTERNAL_ERROR; } - // 更新路径点容器 + // 更新路径点容器，根据global_plan_viapoint_sep间隔距离，向_viapoint中插入路径点 if (!_custom_via_points_active) updateViaPointsContainer(transformed_plan, _params.global_plan_viapoint_sep); - + // 检查是否到达全局目标 geometry_msgs::PoseStamped global_goal; tf2::doTransform(_global_plan.back(), global_goal, tf_plan_to_global); @@ -561,6 +561,7 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCostmap() // 检查障碍物是否在机器人前方（例如，不远离机器人） Eigen::Vector2d obs_dir = obs - _robot_pose.position(); + // 如果障礙物在機器人後方，而且距離大於參數設置的距離，則忽略 if (obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > _params.costmap_obstacles_behind_robot_dist) continue; _obstacles.push_back(ObstaclePtr(new PointObstacle(obs))); @@ -704,7 +705,7 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCustomObstacles() * 该函数根据转换后的计划和最小间隔距离更新路径点容器。 * * @param transformed_plan 转换后的全局计划。 - * @param min_separation 路径点之间的最小间隔距离。 + * @param min_separation 路径点之间的最小间隔距离。默认为-1,表示不使用路径点。 */ void MpcLocalPlannerROS::updateViaPointsContainer(const std::vector& transformed_plan, double min_separation) { @@ -847,7 +848,7 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st double sq_dist_threshold = dist_threshold * dist_threshold; double sq_dist = 1e10; - // 我们需要循环直到找到一个距离机器人一定距离内的计划点 + // 我们需要循环直到找到一个距离机器人最近的路径点 for (int j = 0; j < (int)global_plan.size(); ++j) { double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x; @@ -866,7 +867,7 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st double plan_length = 0; // 检查沿计划的累计欧几里得距离 - // 现在我们将转换，直到点超出我们的距离阈值 + // 现在我们将转换，直到点超出我们的距离阈值，从距离机器人的最近的点开始，直到距离满足max_plan_length while (i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length <= 0 || plan_length <= max_plan_length)) { const geometry_msgs::PoseStamped& pose = global_plan[i];