本文主要介绍ROS中Navigation导航框架中MPC局部路径规划器mpc_local_planner的使用方法,并对源码进行解读,梳理其规划流程等,具体包含MPC模型预测控制算法简介、mpc_local_planner使用方法、mpc_local_planner源码解读与规划流程梳理三部分内容。
-
一、MPC模型预测控制算法简介
-
MPC(Model Predictive Control)是一种先进的控制策略,其核心思想是使用系统数学模型来预测系统在某一未来时间段内的表现来进行优化控制,并通过求解优化问题来计算最优的控制输入,多用于数位控制,所以分析时常采用离散型的状态空间表达形式X_{k+1}=AX_{k}+BU_{k}
MPC的设计和实施包含三个步骤,在k时刻第一步要做的是,估计/测量出系统当前状态。
第二步是基于u_{k}、u_{k+1}、u_{k+2}、u_{k+3} … u_{k+n}来做最优化控制,即在k时刻施加一个输入u_{k},根据我们的模型来预测系统k ~ k+1时间段内的表现,然后基于预测的k+1时刻的表现,再施加一个输入u_{k+1}、再根据我们的模型来预测系统k+1 ~ k+2时间段内的表现,以此类推,直至预测完我们设定的预测区间,比如k~k+3时间段。则输入u_{k}、u_{k+1}、u_{k+2}称为控制区间,在这个控制区间,u_{k}、u_{k+1}、u_{k+2}的选择就是一个最优化的问题。来使得设定的目标函数最小。
第三步是非常重要的一步,也是MPC当中非常有技巧性的一步,通过上面的计算。我们其实得到了u_{k}、u_{k+1}、u_{k+2}这三个输入,但是在实施的时候,并不是把这三个数值输入都实施下去,而是在K时刻只实施u_{k}这一个在k时刻计算出来的最优化结果。
在实施的时候只取u_{k}这一个输入非常重要,这是因为我们预测的模型很难完美的描述现实的系统,而且,在现实的系统当中可能会有扰动,就是说我们预测出来的实施u_{k}后,它的输出Y_{k}与现实当中真正执行完u_{k}后的Y_{k}是有偏差的,这就要求当我们实施u_{k}之后,在k+1时刻要把上面的三步重新执行一遍,即把整个的预测区间(control horizon)和控制区间(predictive horizon)向右移动一个时刻。这个过程就称为滚动优化控制(receiving horizon control)。
MPC的优点包括处理多变量、多约束系统,适应动态环境和提供优化性能。然而,它的计算复杂度较高,适用于需要高精度控制的应用。
MPC模型预测控制算法的详细介绍可见 :
-
二、mpc_local_planner使用方法
-
ROS现有开源MPC模型预测控制算法的局部路径规划器插件中,最受欢迎的是mpc_local_planner功能包,它与我们比较熟悉的teb_local_planner出自同一研究所(多特蒙德大学-控制理论与系统工程研究所),所以在流程及上有很多相似之处,mpc_local_planner使用步骤如下:
1、下载并将mpc_local_planner功能包放到ROS工作空间的src文件夹下
2、环境配置,在终端执行以下指令安装所需依赖和环境
rosdep install mpc_local_planner
若rosdep不能正常使用,可以参考我之前写的博客,链接如下:
《ROS Noetic版本 rosdep找不到命令 不能使用的解决方法》
3、使用catkin_make对mpc_local_planner功能包进行编译
4、可根据需要执行以下语句中的一个或多个(同一时间执行一个即可),来使用功能包自带的示例,对功能包是否能够正常工作,并可对其性能进行测试
可以执行以下语句进行阿克曼模型小车的demo的测试
roslaunch mpc_local_planner_examples carlike_minimum_time.launch
-
动态效果演示如下:
注:↓↓↓↓↓点击以下图片可查看动态演示图↓↓↓↓↓
-
注:↓↓↓↓↓点击以下图片可查看动态演示图↓↓↓↓↓
-
-
可以执行以下三条语句中任意一条进行差速模型小车的demo的测试
roslaunch mpc_local_planner_examples diff_drive_minimum_time.launch
roslaunch mpc_local_planner_examples diff_drive_quadratic_form.launch
roslaunch mpc_local_planner_examples diff_drive_minimum_time_costmap_conversion.launch
-
动态效果演示如下:
-
注:↓↓↓↓↓点击以下图片可查看动态演示图↓↓↓↓↓
-
-
执行任意上述launch文件对功能包是否能够正常工作进行测试后,若能正常进行局部路径规划,则可以进一步将该功能包运用在我们自己的机器人上,继续进行以下操作
5、在启动move_base的launch文件中,配置局部路径规划器插件为mpc_local_planner/MpcLocalPlannerROS,并根据机器人的实际情况,设定参数clearing_rotation_allowed的值来设定在规划时是否允许机器人旋转,比如对于阿克曼结构的类车型机器人,不具备原地旋转能力,该参数设为flase,如下所示
<param name="base_local_planner" value="mpc_local_planner/MpcLocalPlannerROS" />
<param name="controller_frequency" value="5" />
<param name="controller_patiente" value="15.0"/>
<param name="clearing_rotation_allowed" value="false" /> <!-- carlike robot is not able to rotate in place -->
6、在上述move_base节点配置中调用mpc_local_planner的参数配置文件mpc_local_planner_params.yaml,如下图所示,需要注意的是文件路径可能要根据实际情况进行修改。
-
7、进行效果测试,并根据测试效果对参数进行调节
-
注:↓↓↓↓↓点击以下图片可查看动态演示图↓↓↓↓↓
-
三、mpc_local_planner源码解读与规划流程梳理
-
在我之前的文章《ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理》中已经介绍过Move_base节点调用局部路径规划器插件的接口函数是computeVelocityCommands,接下来,我们就从这个函数入手梳理一下mpc_local_planner功能包的工作流程。
文章链接:
《ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理》
-
1、computeVelocityCommands函数
-
从局部路径规划器的接口函数computeVelocityCommands开始:
uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity,geometry_msgs::TwistStamped& cmd_vel, std::string& message)
(1)该函数中首先检查了该路径规划器是否初始化、并定义了一些变量等,然后读取了机器人当前的位姿和速度信息。
(2)调用pruneGlobalPlan函数,对全局路径进行修剪
由于机器人是实时运动的,所以需要对已经走过的全局路径裁减处理,其实现思路是从全局路径规划容器的起点开始一个个遍历,计算其与机器人当前点的距离差,直到找到距离小于我们设定的阈值的点,将该点之前的全局路径点全部删除,这样就可以将位于机器人后方距离超过一定阈值的点删除掉。该过程涉及到参数配置文件中的参数 global_plan_prune_distance
pruneGlobalPlan(*_tf, robot_pose, _global_plan, _params.global_plan_prune_distance);
(3)将全局路径转换到局部代价地图的坐标系下,并从中选择合适的点作为局部路径点
调用transformGlobalPlan函数,通过tf 树将全局路径信息转换到局部代价地图的坐标系下,值得注意的是,在这个函数中并不仅仅进行了坐标系的变换,还对全局路径信息点进行了一些处理,从中选出合适的点作为局部路径规划点。 其具体过程是依次对全局路径进行坐标变换,同时计算其与当前位置的距离,并统计已转换的全局路径点之间的距离和(即已转换的全局路径的长度),直到超过设定的长度阈值或者与当前点的距离大于设定的距离阈值,则丢弃剩余的路径点。 其中,长度阈值由配置文件中参数max_global_plan_lookahead_dist 决定,距离阈值由局部代价地图的大小width和height以及系数0.85有关
if (!transformGlobalPlan(*_tf, _global_plan, robot_pose, *_costmap, _global_frame, _params.max_global_plan_lookahead_dist, transformed_plan, &goal_idx, &tf_plan_to_global))
{
ROS_WARN("Could not transform the global plan to the frame of the controller");
message = "Could not transform the global plan to the frame of the controller";
return mbf_msgs::ExePathResult::INTERNAL_ERROR;
}
(4)对得到的局部路径规划点进行进一步的筛选
调用updateViaPointsContainer函数,对上一步得到的局部路径规划点进行进一步的筛选,其实现思路是,循环遍历局部路径规划点,从中选出距离上一个被选中的点的距离大于设定的阈值的点(局部路径规划点中的第一个点为本步筛选中第一个被选中的点)
if (!_custom_via_points_active) updateViaPointsContainer(transformed_plan,_params.global_plan_viapoint_sep);
这样处理可以将上一步得到的局部路径点稀疏化,从而减少局部路径点的个数,阈值由配置文件中的参数global_plan_viapoint_sep 决定,若该参数设置为负数,则表示跳过本步处理,不进行筛选
第(2)~(4)步的动态演示如下(与TEB的处理过程相同):
-
-
(5)判断机器人是否到达目标点
得到局部路径的状态点后,先检查是否已经到达了全局目标点,具体要检查以下内容:
(1)当前点距离目标点的距离是否小于由参数配置文件设定的阈值xy_goal_tolerance
(2)当前点的姿态角与目标姿态角是否小于由外参数配置文件设定的阈值yaw_goal_tolerance
if (std::abs(std::sqrt(dx * dx + dy * dy)) < _params.xy_goal_tolerance && std::abs(delta_orient) < _params.yaw_goal_tolerance)
{
_goal_reached = true;
return mbf_msgs::ExePathResult::SUCCESS;
}
(6)调整局部目标点的角度
根据外部参数 global_plan_overwrite_orientation决定是否调整局部目标点的角度(也就是经过上述处理后的路径的最后一个点的角度),调整是通过调用estimateLocalGoalOrientation函数来实现的
if (_params.global_plan_overwrite_orientation)
{
_robot_goal.theta() = estimateLocalGoalOrientation(_global_plan, transformed_plan.back(), goal_idx, tf_plan_to_global);
// overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)
tf2::Quaternion q;
q.setRPY(0, 0, _robot_goal.theta());
tf2::convert(q, transformed_plan.back().pose.orientation);
}
else
{
_robot_goal.theta() = tf2::getYaw(transformed_plan.back().pose.orientation);
}
当需要修改时,会判断当前局部路径规划的终点与全局路径规划的终点是否接近,如果两个点比较接近(就是同一个点或者下一个点就是终点)则直接使用当前局部路径规划中终点的方向作为规划的方向。如果当前局部路径规划的终点跟全局目标点之间还间隔很多个点的话,会使用当前局部路径的终点以及向后两个点的角度进行平滑处理,得到一个新的角度值。
当不需要修改时,采用局部目标点的原有角度。
(7)更新局部路径的起点
上一步完成了局部规划的目标点的确认,这一步要完成局部规划的起始点的确认,,前面虽然根据全局路径规划中截取了局部路径规划的点,但是这个容器的起点不一定是是机器人当前时刻所处的点。
if (transformed_plan.size() == 1) // plan only contains the goal
{
transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized)
}
transformed_plan.front() = robot_pose; // update start
如果路径中只包含一个目标点,即没有中间的路径点,程序会在路径的起始点处插入机器人当前的姿态。这是为了将路径用作初始化轨迹,以确保路径规划从机器人当前位置开始。如果路径包含多个点,则使用机器人当前位姿点覆盖更新容器中的第一个路径点。
(8)更新障碍物信息
根据配置文件中参数 costmap_converter决定如何更新障碍物信息,若该参数不为空,则调用costmap_converter插件进行地图信息更新,若为空直接使用costmap的信息进行更新。
分别对应下面那个函数:
updateObstacleContainerWithCostmapConverter()
updateObstacleContainerWithCostmap()
若使用costmap_converter插件进行更新,则在updateObstacleContainerWithCostmapConverter函数中调用了getObstacles()函数从代价地图转换器获得障碍物,并对障碍物根据其形状(圆、点、线、多边形)进行分类,将其转换成相应的障碍物类型,然后将障碍物坐标存储到obstacles_容器中,针对移动的障碍物还会设定其速度。
若使用costmap的信息进行更新,根据外部参数 include_costmap_obstacles决定是否考虑costmap中的障碍物,若考虑则遍历整个代价地图,对于每一个代价地图单元格,如果该单元格的代价为LETHAL_OBSTACLE(代价值为100),则将其视为障碍物,并检测该障碍物是否足够引人注意(例如,离机器人不远),如果障碍物在机器人后面且距离机器人的距离超过了外部参数决定的阈值 costmap_obstacles_behind_robot_dist,则忽略该障碍物。最后将得到的障碍物添加到障碍物容器中。
除此之外,还要从ROS消息中获取障碍物信息,然后将这些障碍物添加到当前的障碍物容器中。该过程通过函数updateObstacleContainerWithCustomObstacles完成
// also consider custom obstacles (must be called after other updates, since the container is not cleared)
// 还要考虑custom障碍(必须在其他更新之后调用,因为障碍容器没有被清除)
updateObstacleContainerWithCustomObstacles();
消息来源于:
custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);
(9)使用 _u_seq 中的先前控制输入作为优化控制问题的输入
若_u_seq存在且不为空,则通过_controller 对象获取了一个优化控制问题(Optimal Control Problem)的实例。通过调用 setPreviousControlInput 方法,将控制输入设置为 _u_seq 中的第一个元素(索引为 0)并传递了一个时间步长 dt 作为参数。这表示使用 _u_seq 中的先前控制输入作为优化控制问题的输入,以便在下一步的规划中考虑到之前的控制决策。
double dt = 1.0 / _params.controller_frequency;
if (_u_seq && !_u_seq->isEmpty()) _controller.getOptimalControlProblem()->setPreviousControlInput(_u_seq->getValuesMap(0), dt);
其中,时间dt由参数 controller_frequency 的倒数决定
至此,规划前的准备工作完成,下面正式开始规划进行局部路径的规划和控制
(10)进行局部路径规划
执行路径规划,调用 _controller.step() 函数来执行实际的路径规划计算。函数会接收转换后的路径、机器人的当前速度、时间间隔等作为输入,然后计算出机器人下一步的速度命令。
success = _controller.step(transformed_plan, _robot_vel, dt, t, _u_seq, _x_seq);
注: _controller.step() 函数的详细介绍见下文中的第2小节:“2、_controller.step()函数”
(11)检查路径规划是否成功
在路径规划完成后,接着会检查路径规划是否成功。如果路径规划失败,通常是因为没有找到可行的路径,程序会记录一个警告信息。然后,程序会重置 _controller,以便下一次的路径规划。同时,会增加计数器 _no_infeasible_plans,用于跟踪连续不可行规划的次数。如果路径规划成功,程序会继续执行后续的步骤。
if (!success)
{
_controller.reset(); // force reinitialization for next time
ROS_WARN("mpc_local_planner was not able to obtain a local plan for the current setting.");
++_no_infeasible_plans; // increase number of infeasible solutions in a row
_time_last_infeasible_plan = ros::Time::now();
_last_cmd = cmd_vel.twist;
message = "mpc_local_planner was not able to obtain a local plan";
return mbf_msgs::ExePathResult::NO_VALID_CMD;
}
…
(12)检查规划路径的可行性
可行性检查通常包括考虑机器人的轮廓、碰撞检测以及动力学约束等方面。如果规划出的轨迹不可行,程序会记录一个警告信息,并返回相应的错误代码。同时,程序会重置 _controller,以准备下一次的规划。
if (_params.is_footprint_dynamic)
{
// Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
_footprint_spec = _costmap_ros->getRobotFootprint();
costmap_2d::calculateMinAndMaxDistances(_footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius);
}
bool feasible = _controller.isPoseTrajectoryFeasible(_costmap_model.get(), _footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius,
_params.collision_check_min_resolution_angular, _params.collision_check_no_poses);
if (!feasible)
{
cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
// now we reset everything to start again with the initialization of new trajectories.
_controller.reset(); // force reinitialization for next time
ROS_WARN("MpcLocalPlannerROS: trajectory is not feasible. Resetting planner...");
++_no_infeasible_plans; // increase number of infeasible solutions in a row
_time_last_infeasible_plan = ros::Time::now();
_last_cmd = cmd_vel.twist;
message = "mpc_local_planner trajectory is not feasible";
return mbf_msgs::ExePathResult::NO_VALID_CMD;
}
…
(13)计算控制机器人运动的速度指令
从 _controller 获取机器人的速度命令,包括线性和角速度,它们将用于控制机器人的运动。速度命令的计算依赖于上面局部路径规划的结果和机器人的当前状态。
if (!_u_seq || !_controller.getRobotDynamics()->getTwistFromControl(_u_seq->getValuesMap(0), cmd_vel.twist))
{
_controller.reset();
ROS_WARN("MpcLocalPlannerROS: velocity command invalid. Resetting controller...");
++_no_infeasible_plans; // increase number of infeasible solutions in a row
_time_last_infeasible_plan = ros::Time::now();
_last_cmd = cmd_vel.twist;
message = "mpc_local_planner velocity command invalid";
return mbf_msgs::ExePathResult::NO_VALID_CMD;
}
…
(14)存储控制指令
将上一次有效的速度命令存储在 _last_cmd 中,以备后续参考。这对于分析路径规划的效果、机器人行为等非常有用。
_last_cmd = cmd_vel.twist;
_time_last_cmd = ros::Time::now();
..
(15)发布可视化信息
最后,程序会发布各种信息,用于可视化或监测路径规划的效果。这包括本地路径、障碍物信息、全局路径、途经点、机器人的轮廓模型等。可视化信息可以帮助我们了解机器人的行为和路径规划的效果,以及发现潜在的问题或改进点。
_publisher.publishLocalPlan(*_x_seq);
_publisher.publishObstacles(_obstacles);
_publisher.publishGlobalPlan(_global_plan);
_publisher.publishViaPoints(_via_points);
_publisher.publishRobotFootprintModel(_robot_pose, *_robot_model);
-
2、_controller.step()函数
-
上面我们对mpc_local_planner局部路径规划器的规划流程,也就是computeVelocityCommands函数中的内容,进行了概括总结,其中调用了_controller.step()函数来执行具体的路径规划,接下来,我们就对该函数的流程展开介绍
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)
(1). 配置检查
检查是否已正确配置了控制器。如果未正确配置,则会记录错误信息并返回 false,表示无法执行路径规划。配置包括检查是否已设置动力学模型 _dynamics、网格 _grid 和结构化优化控制问题 _structured_ocp。
if (!_dynamics || !_grid || !_structured_ocp)
{
ROS_ERROR("Controller must be configured before invoking step().");
return false;
}
(2)初始路径检查
代码检查初始路径(initial_plan)是否包含至少两个姿态点。初始路径通常是全局路径规划器生成的路径,必须包含起始点和目标点。
if (initial_plan.size() < 2)
{
ROS_ERROR("Controller::step(): initial plan must contain at least two poses.");
return false;
}
(3)提取起始和目标姿态
代码提取了初始路径的第一个和最后一个姿态点,分别用 start和 goal表示。这些姿态点将用于路径规划和目标设定。
PoseSE2 start(initial_plan.front().pose);
PoseSE2 goal(initial_plan.back().pose);
(4)目标状态格式转换
调用 _dynamics->getSteadyStateFromPoseSE2 将PoseSE2格式的变量goal转换为Eigen::VectorXd格式的变量xf
Eigen::VectorXd xf(_dynamics->getStateDimension());
_dynamics->getSteadyStateFromPoseSE2(goal, xf);
(5)初始化当前状态
getStateDimension()返回的值为3
Eigen::VectorXd x(_dynamics->getStateDimension());
(6)合并当前状态的测量和预测值
若 _recent_x_feedback 容器中有数据,且距离最近一次状态更新的时间是否小于 2.0 * dt 秒,则表示有新的状态数据可用。将_recent_x_feedback的值赋给当前状态x,如果没有新的状态数据可用,且(没有时间序列数据 _x_ts,或者该时间序列为空,或者无法在时间间隔 dt 内插值得到状态 x ),则直接将PoseSE2格式的变量start转换为 Eigen::VectorXd格式的变量 x。此外,如果需要,将状态反馈与里程计反馈合并。
bool new_x = false;
// 这里的{}用于限定互斥锁的作用范围,以确保互斥锁仅在需要时锁住。
{
//创建一个名为 lock 的 std::lock_guard 对象,它使用 名为_x_feedback_mutex的 互斥锁。
// 这个锁的作用是确保在同一时刻只有一个线程可以访问被锁住的代码块
std::lock_guard<std::mutex> lock(_x_feedback_mutex);
// _recent_x_feedback的值在另一个线程中进行更新,在本线程中获取其值,使用互锁可以保证不会同时对_recent_x_feedback执行读写操作
// 这行代码用于判断是否有新的状态数据可用。它包含两个条件:
// _recent_x_feedback.size() > 0:检查 _recent_x_feedback 容器中是否有数据。
//(t - _recent_x_time).toSec() < 2.0 * dt:检查距离最近一次状态更新的时间是否小于 2.0 * dt 秒,其中 t 表示当前时间,_recent_x_time 表示最近一次状态数据更新的时间,而 dt 表示时间步长。
//如果这两个条件都满足,将 new_x 设置为 true,表示有新的状态数据可用。
new_x = _recent_x_feedback.size() > 0 && (t - _recent_x_time).toSec() < 2.0 * dt;
// 如果 new_x 的值为 true,即有新的状态数据可用,程序会将 _recent_x_feedback 复制给 x,从而更新机器人的当前状态。
if (new_x) x = _recent_x_feedback;
}
// 如果没有新的状态数据可用,且(没有时间序列数据 _x_ts,或者该时间序列为空,或者无法在时间间隔 dt 内插值得到状态 x ),
// 则执行条件中的程序,直接将PoseSE2格式的变量start转换为 Eigen::VectorXd格式的变量 x
if (!new_x && (!_x_ts || _x_ts->isEmpty() || !_x_ts->getValuesInterpolate(dt, x))) // predict with previous state sequence
{
_dynamics->getSteadyStateFromPoseSE2(start, x); // otherwise initialize steady state
}
if (!new_x || !_prefer_x_feedback)
{
//如果需要,将状态反馈与里程计反馈合并。
//注意,一些模型(如独轮车)会通过odom反馈覆盖整个状态,除非_preferr_x_measurement设置为true。
_dynamics->mergeStateFeedbackAndOdomFeedback(start, vel, x);
}
(7)检查是否需要重新初始化
代码检查是否需要重新初始化路径规划。如果目标与上一个目标之间的距离或角度变化大于阈值,将清除路径规划数据 _grid。这是为了确保机器人能够适应新的目标或路径。
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();
}
(8)生成初始状态轨迹
如果路径规划数据为空(_grid->isEmpty()),则代码会生成初始状态轨迹。初始状态轨迹是一组状态的序列,从起始状态 x`开始,以目标状态 xf 结束。这个过程通常包括路径规划算法,以生成适当的状态序列。
先检查目标点是否在当前点的后方,并估计是否需要执行后退运动,参数_guess_backwards_motion是是否启用该功能的开关。开启该功能时,计算机器人的初始位置到目标位置的矢量与初始方向的点积,如果点积小于0,表示矢量与方向相反,因此机器人可能需要执行反向运动。
然后, 将给定的初始路径initial_plan,添加时间序列信息以及姿态信息,从而转换为初始轨迹,并采用线性差值对相邻两个轨迹点之间的轨迹进行差值,生成的轨迹存放在controller类的变量_x_seq_init中
if (_grid->isEmpty())
{
// 检查目标点是否在当前点的后方,并估计是否需要执行后退运动,参数_guess_backwards_motion是是否启用该功能的开关
// 开启该功能时,计算机器人的初始位置到目标位置的矢量与初始方向的点积,如果点积小于0,表示矢量与方向相反,因此机器人可能需要执行反向运动。
bool backward = _guess_backwards_motion && (goal.position() - start.position()).dot(start.orientationUnitVec()) < 0;
// 将给定的初始路径initial_plan,添加时间序列信息以及姿态信息,从而转换为初始轨迹,并采用线性差值对相邻两个轨迹点之间的轨迹进行差值
// 生成的轨迹存放在controller类的变量_x_seq_init中
generateInitialStateTrajectory(x, xf, initial_plan, backward);
}
(9)路径规划执行
代码调用 PredictiveController::step 函数来执行路径规划。函数接收当前状态 x、目标状态 xref、控制输入 uref以及时间步长等信息,计算出下一步的状态 x_seq 和控制输入 u_seq。如果路径规划成功,返回 true,否则返回 false。
_ocp_successful = PredictiveController::step(x, xref, uref, corbo::Duration(dt), time, u_seq, x_seq, nullptr, nullptr, &_x_seq_init);
注: PredictiveController::step() 函数的详细介绍见下文中的第3小节:“Predictive Controller::step() 函数”
(10)发布结果
如果 _publish_ocp_results 参数设置为真,代码会发布路径规划的结果。 这包括机器人的状态轨迹、速度命令等信息,用于监测和可视化路径规划的效果。
if (_publish_ocp_results) publishOptimalControlResult(); // TODO(roesmann): we could also pass time t from above
(11)计时信息
如果 _print_cpu_time 参数设置为真,代码会记录 CPU 执行时间的信息,用于性能监测。
ROS_INFO_STREAM_COND(_print_cpu_time, "Cpu time: " << _statistics.step_time.toSec() * 1000.0 << " ms.");
(12)更新序列计数器
代码递增 _ocp_seq,以跟踪执行的路径规划次数。
++_ocp_seq;
(13)更新上一个目标
代码记录当前的目标 goal 作为下一次的参考目标。
_last_goal = goal;
最后,代码返回路径规划是否成功的布尔值 _ocp_successful。
return _ocp_successful;
-
3、PredictiveController::step()函数
-
上面介绍的_controller.step()函数中调用了PredictiveController::step 函数来执行具体的MPC模型预测过程。函数接收当前状态 x、目标状态 xref、控制输入 uref以及时间步长等信息,计算出下一步的状态 x_seq 和控制输入 u_seq。如果计算成功,返回 true,否则返回 false。
注:该函数的源码位于control_box_rst功能包中,而非mpc_local_planner功能包
bool PredictiveController::step(const ControllerInterface::StateVector& x, ReferenceTrajectoryInterface& xref, ReferenceTrajectoryInterface& uref,
const Duration& dt, const Time& t, TimeSeries::Ptr u_sequence, TimeSeries::Ptr x_sequence,
SignalTargetInterface* signal_target, ReferenceTrajectoryInterface* sref, ReferenceTrajectoryInterface* xinit,
ReferenceTrajectoryInterface* uinit, const std::string& ns)
(1).检查控制器是否已经初始化。如果未初始化,则尝试进行初始化。如果初始化失败,返回 false,表示计算步骤无法继续。
if (!_initialized)
{
if (!initialize(x, xref, uref, dt, t, sref)) return false;
}
(2).创建一个名为 u的控制向量,用于存储计算得到的控制输入。
ControlVector u;
(3).检查是否存在优化控制问题(Optimal Control Problem,OCP)对象。如果不存在,返回 false,表示计算步骤无法继续。
if (!_ocp) return false;
(4)检查是否启用了自动更新前一控制输入的选项。如果启用了自动更新前一控制输入的选项,设置前一控制输入的时间步长为 dt(以秒为单位)。
if (_auto_update_prev_control) _ocp->setPreviousControlInputDt(dt.toSec());
(5) 创建一个名为 success 的布尔变量,用于跟踪计算步骤是否成功。 获取当前时间,用于计算计算步骤的执行时间。
// 创建一个名为 success 的布尔变量,用于跟踪计算步骤是否成功。
bool success = false;
// 获取当前时间,用于计算计算步骤的执行时间。
Time t_pre = Time::now();
(6) 执行最优控制问题的计算,在循环中多次执行优化控制问题的计算,_num_ocp_iterations 表示要执行的优化迭代次数。在每次迭代中,调用 _ocp->compute 方法来计算最优的控制输入。 x, xref, uref, sref, t, i == 0, signal_target, xinit, uinit, ns 等参数是用于优化计算的输入参数。
for (int i = 0; i < _num_ocp_iterations; ++i) success = _ocp->compute(x, xref, uref, sref, t, i == 0, signal_target, xinit, uinit, ns);
(7)计算并记录执行计算步骤所花费的时间。
_statistics.step_time = Time::now() - t_pre;
(8) 获取第一个优化控制输入,并将结果存储在 u
中。如果启用了自动更新前一控制输入的选项,使用计算得到的控制输入 u 来更新前一控制输入。
success = success && _ocp->getFirstControlInput(u);
// 如果启用了自动更新前一控制输入的选项,使用计算得到的控制输入 u 来更新前一控制输入。
if (_auto_update_prev_control) _ocp->setPreviousControlInput(u, dt.toSec()); // cache control input for next step call
(9)获取时间序列数据,包括状态序列和控制输入序列,并存储在 x_sequence 和 u_sequence 中。
_ocp->getTimeSeries(x_sequence, u_sequence);
_x_ts = x_sequence; //更新内部状态变量 _x_ts,将其设置为状态序列 x_sequence。
_u_ts = u_sequence; //更新内部状态变量 _u_ts,将其设置为控制输入序列 u_sequence。
(10)返回 success,指示计算步骤是否成功完成。如果成功,返回 true,否则返回 false。
原创文章作者:慕羽★。如若转载,请注明出处:古月居 https://www.guyuehome.com/45097
为你推荐
给作者打赏
您当前积分:0
评论(0)
您还未登录,请登录后发表或查看评论