第一章主要分析了teb算法的准备条件，包括获取当前位姿与速度、对全局路径的裁减以获取局部路径等以及局部地图的获取等。第二章主要讲述了在获取前置条件后，如何根据前置条件进行位姿优化，teb的路径优化主要是调用了g2o优化算法，以全局路径的点以及理论上点与点之间的运动时间作为g2o的优化顶点，以距离障碍物的距离、机器人最大运动速度、点与点之间的最短路径等约束条件作为g2o的边对局部路径规划的姿态进行了优化。这一章则是主要看一下算法怎么从优化后的点位到运动输出之间的关系。
假设算法已经获得了g2o优化后的结果，则算法在发布速度前会经历以下几步：
1、判断结果是否发散
if (planner_->hasDiverged())
{
cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
planner_->clearPlanner();
ROS_WARN_THROTTLE(1.0, "TebLocalPlannerROS: the trajectory has diverged. Resetting planner...");
++no_infeasible_plans_;
time_last_infeasible_plan_ = ros::Time::now();
last_cmd_ = cmd_vel.twist;
return mbf_msgs::ExePathResult::NO_VALID_CMD;
}
在获取g2o优化的位姿结果后，算法第一步是对结果进行了收敛性判断。判断的依据应该是：判断局部路径规划的最后一个点的优化前后的曼哈顿距离是否在满足的阈值内，如果不满足则说明优化的结果发散了。
2、是否动态更新footprint
if(cfg_.robot.is_footprint_dynamic)
{
footprint_spec_ = costmap_ros_->getRobotFootprint();
costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
}
这个过程是由参数cfg_.robot.is_footprint_dynamic决定的，机器人的footprint也就是机器人的模型一般来说是固定的，footprint的获取会在teb的初始化函数中获取一次，如果不改变的话后面算法会一直使用这个footprint，但是如果说机器人的模型会动态变化的话这会在这个位置进行一次更新。
3、轨迹可行性判读
bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);
前面全局路径规划得到一条路径，这条路径是确认可以到达终点的，但是因为这里的局部路径规划优化后的轨迹会与全局路径规划有所区别，同时还有上面的可能存在的机器人模型的变化，所以对于优化后的轨迹我们需要重新判断它的可行性。
if (i<look_ahead_idx)
{
double delta_rot = g2o::normalize_theta(g2o::normalize_theta(teb().Pose(i+1).theta()) -
g2o::normalize_theta(teb().Pose(i).theta()));
Eigen::Vector2d delta_dist = teb().Pose(i+1).position()-teb().Pose(i).position();
if(fabs(delta_rot) > cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius)
{
int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular),
std::ceil(delta_dist.norm() / inscribed_radius)) - 1;
PoseSE2 intermediate_pose = teb().Pose(i);
for(int step = 0; step < n_additional_samples; ++step)
{
intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0);
intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() +
delta_rot / (n_additional_samples + 1.0));
if ( costmap_model->footprintCost(intermediate_pose.x(), intermediate_pose.y(), intermediate_pose.theta(),
footprint_spec, inscribed_radius, circumscribed_radius) == -1 )
{
if (visualization_)
{
visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_);
}
return false;
}
}
}
}
判断的方式也很简单：
首先对于局部路径规划中的每个点，将机器人模型投影到这个点上去判断这个点是否会跟障碍物i模型相重合，如果存在重合就说明这条路径有问题需要重新规划。
其次，判断局部路径上每个点之间的距离以及朝向角度差，这个距离不能超过机器人的长度，如果超过了机器人的长度则点上的模型不能完全覆盖机器人路径，可能会存在说两个姿态点都是没有问题的但是点与点之间存在障碍物的问题，这样仅仅对点进行判断是不能完全保证路径可行的。对于这个问题的处理方式也很简单，对路径按照机器人长度进行差值，直到模型覆盖整条路径为止。角度的问题也是一样的。
4、计算速度
if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.trajectory.control_look_ahead_poses))
{
planner_->clearPlanner();
ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner...");
++no_infeasible_plans_;
time_last_infeasible_plan_ = ros::Time::now();
last_cmd_ = cmd_vel.twist;
message = "teb_local_planner velocity command invalid";
return mbf_msgs::ExePathResult::NO_VALID_CMD;
}
重头戏！！！！！teb的用处是什么？？？不就是为了求机器人的速度吗？弯弯绕绕走了这么久终于算速度了。点进去看一下。
emmm,其实还是比较简单的哈。注意到传入了一个参数：cfg_.trajectory.control_look_ahead_poses。这个参数是用来判断机器人使用前向第几个姿态点作为速度计算的终点。这里有个细节，就是对于靠近终点附近的点位，剩余点的数量可能会少于这个参数，那就直接用终点了。然后我们就有了一个起点一个终点。要怎么计算运动过去的速度呢？往下看：
Eigen::Vector2d deltaS = pose2.position() - pose1.position();
if (cfg_->robot.max_vel_y == 0)
{
Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) );
double dir = deltaS.dot(conf1dir);
vx = (double) g2o::sign(dir) * deltaS.norm()/dt;
vy = 0;
}
else
{
double cos_theta1 = std::cos(pose1.theta());
double sin_theta1 = std::sin(pose1.theta());
double p1_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
double p1_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
vx = p1_dx / dt;
vy = p1_dy / dt;
}
double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());
omega = orientdiff/dt;
首先deltaS 里面存储的是xy两个方向的距离，dir获取了机器人需要的方向，计算方式使用的是xy方向的距离乘以角度的三角函数。g2o::sign(dir)函数对速度进行归一化，当dir>0返回1，dir<0返回-1。然后具体的速度大小则是距离/时间
这里的时间是来自于点与点之间的理论时间和。在前面运动优化部分有计算过每个位姿之间的理论运动时间。例如这里如果要取前向第五个点，那么时间就是前面计算过的点1-2-3-4-5的时间和，距离就是点1与点5的距离差：
for(int counter = 0; counter < look_ahead_poses; ++counter)
{
dt += teb_.TimeDiff(counter);
if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses)
{
look_ahead_poses = counter + 1;
break;
}
}
同样的，角速度也是两点之间的角度差/时间和。
5、速度限定
saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z,
cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_trans, cfg_.robot.max_vel_theta,
cfg_.robot.max_vel_x_backwards);
这里就是速度进行最后的判断了，上述计算出来的速度是否满足算法设定的最大值，如果超过这个值的话会被限定到范围内。
6、非原地转向车辆的角速度修正
if (cfg_.robot.cmd_angle_instead_rotvel)
{
cmd_vel.twist.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.twist.linear.x, cmd_vel.twist.angular.z,
cfg_.robot.wheelbase, 0.95*cfg_.robot.min_turning_radius);
if (!std::isfinite(cmd_vel.twist.angular.z))
{
cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
last_cmd_ = cmd_vel.twist;
planner_->clearPlanner();
ROS_WARN("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner...");
++no_infeasible_plans_;
time_last_infeasible_plan_ = ros::Time::now();
message = "teb_local_planner steering angle is not finite";
return mbf_msgs::ExePathResult::NO_VALID_CMD;
}
}
这个函数是只针对类似于我们小汽车之类的无法原地转向的机器人的，则需要设定最小转弯半径。这里暂时就不展开看了。
7、发布信息
last_cmd_ = cmd_vel.twist;
planner_->visualize();
visualization_->publishObstacles(obstacles_);
visualization_->publishViaPoints(via_points_);
visualization_->publishGlobalPlan(global_plan_);
return mbf_msgs::ExePathResult::SUCCESS;
最后，算法会发布机器人的速度以及轨迹，到此，算法的整体流程就完成了。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
