(七) Teb Optimal的路径规划
1
2
3
4
5
6
7
8
9
10
11
12
13
// 禁止配置参数的更改 during the following optimization step
boost::mutex::scoped_lock cfg_lock(cfg_.configMutex() );
bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
if (!success)
{
planner_->clearPlanner(); // force reinitialization for next time
ROS_WARN("teb_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;
return false;
}

clearPlanner()

不会停止规划,而是强制下次重新初始化planner,要停止继续规划,只能return。 如果本次导航失败或路径不好,出现一大堆TebPose缠绕,在下一次导航时,它们可能还在,要消除就需要clearPlanner();
下次导航前,需要clearPlanner.png


实际中用的是Homotopy Planner,但是它太复杂了,我们先看TebOptimal Planner,前者取的其实是 best Teb Optimal Planner
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
bool TebOptimalPlanner::plan(
const std::vector<geometry_msgs::PoseStamped>& initial_plan,
const geometry_msgs::Twist* start_vel,
bool free_goal_vel )
{
ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
// 每次收到新目标时执行,然后pose size都是3
if (!teb_.isInit() )
{
teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x,
cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
// 0 个 intermediate samples, dt=1 --> autoResize will add more samples
// before first optimization
}
else // 热启动
{
PoseSE2 start_(initial_plan.front().pose);
PoseSE2 goal_(initial_plan.back().pose);
if (teb_.sizePoses()>0
&& ( goal_.position() - teb_.BackPose().position()).norm() <
cfg_->trajectory.force_reinit_new_goal_dist
&& fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) <
cfg_->trajectory.force_reinit_new_goal_angular ) // actual warm start!

teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples);

else // goal is too far away from BackPose, reinit相当于 isInit()为false
{
// 当导航较远的goal时,则首先clear轨迹序列,然后重新起点终点插值
teb_.clearTimedElasticBand();
teb_.initTrajectoryToGoal( start, goal, 0, cfg_->robot.max_vel_x,
cfg_->trajectory.min_samples,
cfg_->trajectory.allow_init_with_backwards_motion );
}
}
// 当前从里程计获取的速度
if (start_vel)
// 赋值给 std::pair<bool, geometry_msgs::Twist> vel_start_;
setVelocityStart(*start_vel); // start pose的速度
if (free_goal_vel) // 一般为 false
setVelocityGoalFree(); // 这是唯一调用此函数的地方,不看了
else
// reactivate and 使用之前设置的速度 ( 如果未修改则是0 )
vel_goal_.first = true;

return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
}

isInit()检查trajectory是否初始化: bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty();}

1
2
3
4
5
6
7
8
9
10
11
12
13
// 清空 timediff_vec_ 和 pose_vec_, 之后 isInit返回 false
void TimedElasticBand::clearTimedElasticBand()
{
for (PoseSequence::iterator pose_it = pose_vec_.begin();
pose_it != pose_vec_.end(); ++pose_it)
delete *pose_it;
pose_vec_.clear();

for (TimeDiffSequence::iterator dt_it = timediff_vec_.begin();
dt_it != timediff_vec_.end(); ++dt_it )
delete *dt_it;
timediff_vec_.clear();
}

关于变量pose_vec_timediff_vec_,看下篇文章


plan 函数涉及参数 max_vel_x, min_samples, force_reinit_new_goal_dist, allow_init_with_backwards_motion, free_goal_vel

TebOptimalPlanner::plan()主要是根据输入的初始路径初始化或更新时间弹性带(轨迹)的初始状态,设置了轨迹起始点以及速度加速度的约束, 最后调用 optimizeTEB() 函数:这里是核心中的核心,最关键的就是建图buildGraph和优化OptimizeGraph两个步骤

将轨迹优化问题构建成了一个g2o图优化问题,并通过g2o中关于大规模稀疏矩阵的优化算法解决,涉及到构建超图(hyper-graph),简单来说 将机器人位姿和时间间隔描述为节点(vertex),目标函数以及约束函数作为边(edge)。 graph中,每一个约束都为一条边,并且每条边允许连接的节点的数目是不受限制的。

建图是将路径的一系列约束加进去,比如与障碍物保持一定距离,速度、加速度限制、时间最小约束、最小转弯半径约束、旋转方向约束等,这些都转换成了代价函数放在g2o中,g2o优化时会使这些代价函数达到最小。

TebOptimalPlanner 的初始化

TebLocalPlannerROS::initialize有这样一段

1
2
3
4
5
6
7
8
9
10
if (cfg_.hcp.enable_homotopy_class_planning)
{
// 省略
}
else
{
planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_,
&obstacles_, robot_model, visualization_, &via_points_) );
ROS_INFO("Parallel planning in distinctive topologies disabled.");
}

TebOptimalPlanner的构造函数就一句initialize(cfg, obstacles, robot_model, visual, via_points);,实际是:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
void TebOptimalPlanner::initialize(const TebConfig& cfg, 
ObstContainer* obstacles,
RobotFootprintModelPtr robot_model,
TebVisualizationPtr visual,
const ViaPointContainer* via_points)
{
// init optimizer (set solver and block ordering settings)
optimizer_ = initOptimizer();
cfg_ = &cfg;
obstacles_ = obstacles;
robot_model_ = robot_model;
via_points_ = via_points;
cost_ = HUGE_VAL;
prefer_rotdir_ = RotType::none;
setVisualization(visual);
// 在上面的 setVelocityStart 里赋值
vel_start_.first = true;
vel_start_.second.linear.x = 0;
vel_start_.second.linear.y = 0;
vel_start_.second.angular.z = 0;

vel_goal_.first = true;
vel_goal_.second.linear.x = 0;
vel_goal_.second.linear.y = 0;
vel_goal_.second.angular.z = 0;
initialized_ = true;
}

initOptimizer()函数在(十) optimizeTEB 2 图优化的过程


(六) 静态障碍和自定义障碍

对应的片段:

1
2
3
4
5
6
7
8
9
10
11
12
// clear currently existing obstacles
obstacles_.clear(); // 初始化时 obstacles_.reserve(500);
// 用代价地图信息更新 obstacle container 或者
// 用costmap_converter提供的polygons
if (costmap_converter_)
updateObstacleContainerWithCostmapConverter();
else
updateObstacleContainerWithCostmap();

// 考虑custom obstacles (must be called after other updates,
// since the container is not cleared )
updateObstacleContainerWithCustomObstacles();

obstacles_来源于两部分:

  1. 来自代价地图updateObstacleContainerWithCostmap() 或者 CostmapConverter 的updateObstacleContainerWithCostmapConverter()

  2. 来自用户发布的障碍物消息updateObstacleContainerWithCustomObstacles()

分别由参数include_costmap_obstaclescostmap_converter_plugin决定,还有动态障碍include_dynamic_obstacles。 之后又对应buildGraph中的AddEdgesObstaclesAddEdgesDynamicObstacles

其实目前用的是costmap_converter

updateObstacleContainerWithCostmap

获得当前朝向的单位向量

1
2
3
4
Eigen::Vector2d  PoseSE2::orientationUnitVec() const
{
return Eigen::Vector2d(std::cos(_theta), std::sin(_theta) );
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
// Add costmap obstacles if desired
if (cfg_.obstacles.include_costmap_obstacles)
{
Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec();

for (unsigned int i=0; i<costmap_->getSizeInCellsX()-1; ++i)
{
for (unsigned int j=0; j<costmap_->getSizeInCellsY()-1; ++j)
{
if (costmap_->getCost(i,j) == costmap_2d::LETHAL_OBSTACLE)
{
Eigen::Vector2d obs;
costmap_->mapToWorld(i,j, obs.coeffRef(0), obs.coeffRef(1));

// 检查障碍是否 not far behind the robot
Eigen::Vector2d obs_dir = obs - robot_pose_.position();
// if(obs_dir.norm() < 1.5) //改变最大速度约束
// cfg_.robot.max_vel_x = 0.3;
// 如果norm值过大 或者 obs_dir和robot_orient的点乘积小于0, 不算障碍
if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_.obstacles.costmap_obstacles_behind_robot_dist )
continue;
// 点障碍
obstacles_.push_back(ObstaclePtr(new PointObstacle(obs)));
} } } }

这里是参数include_costmap_obstacles唯一出现的地方。 参数costmap_obstacles_behind_robot_dist默认是1.
函数根据costmap中的障碍物信息更新障碍物容器。在这里,可以找到前方距离障碍物的最短距离。然后根据该距离值调整max_vel_x,这样优化时最大线速度的约束会随着障碍物的远近而调整,在一定程度上会改善避障效果。

自定义障碍

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles()
{
// Add custom obstacles obtained via message
boost::mutex::scoped_lock l(custom_obst_mutex_);
// 类型 costmap_converter::ObstacleArrayMsg
if (!custom_obstacle_msg_.obstacles.empty())
{
// We only use the global header to specify the obstacle coordinate system
// instead of individual ones
Eigen::Affine3d obstacle_to_map_eig;
try
{
tf::StampedTransform obstacle_to_map;
tf_->waitForTransform(global_frame_, ros::Time(0),
custom_obstacle_msg_.header.frame_id, ros::Time(0),
custom_obstacle_msg_.header.frame_id, ros::Duration(0.5) );
tf_->lookupTransform(global_frame_, ros::Time(0),
custom_obstacle_msg_.header.frame_id, ros::Time(0),
custom_obstacle_msg_.header.frame_id, obstacle_to_map);
tf::transformTFToEigen(obstacle_to_map, obstacle_to_map_eig);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
obstacle_to_map_eig.setIdentity();
}
for (size_t i=0; i<custom_obstacle_msg_.obstacles.size(); ++i)
{
if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 &&
custom_obstacle_msg_.obstacles.at(i).radius > 0 ) // circle
{
Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
obstacles_.push_back(ObstaclePtr(new CircularObstacle( (obstacle_to_map_eig * pos).head(2), custom_obstacle_msg_.obstacles.at(i).radius) ) );
}
else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 ) // point
{
Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
obstacles_.push_back(ObstaclePtr(new PointObstacle( (obstacle_to_map_eig * pos).head(2) )));
}
else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 2 ) // line
{
Eigen::Vector3d line_start( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
Eigen::Vector3d line_end( custom_obstacle_msg_.obstacles.at(i).polygon.points.back().x,
custom_obstacle_msg_.obstacles.at(i).polygon.points.back().y,
custom_obstacle_msg_.obstacles.at(i).polygon.points.back().z );
obstacles_.push_back(ObstaclePtr(new LineObstacle( (obstacle_to_map_eig * line_start).head(2),
(obstacle_to_map_eig * line_end).head(2) ) ) );
}
else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.empty())
{
ROS_WARN("Invalid custom obstacle received. List of polygon vertices is empty. Skipping...");
continue;
}
else // polygon
{
PolygonObstacle* polyobst = new PolygonObstacle;
for (size_t j=0; j<custom_obstacle_msg_.obstacles.at(i).polygon.points.size(); ++j)
{
Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points[j].x,
custom_obstacle_msg_.obstacles.at(i).polygon.points[j].y,
custom_obstacle_msg_.obstacles.at(i).polygon.points[j].z );
polyobst->pushBackVertex( (obstacle_to_map_eig * pos).head(2) );
}
polyobst->finalizePolygon();
obstacles_.push_back(ObstaclePtr(polyobst));
}
// Set velocity, if obstacle is moving
if(!obstacles_.empty())
{
obstacles_.back()->setCentroidVelocity(custom_obstacle_msg_.obstacles[i].velocities, custom_obstacle_msg_.obstacles[i].orientation);
obstacles_.back()->setDynamic();
}
}
}
}

消息costmap_converter/ObstacleArrayMsg定义:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
std_msgs/Header header    # frame_id 为map
costmap_converter/ObstacleMsg[] obstacles

# costmap_converter/ObstacleMsg 的成员如下
std_msgs/Header header # frame_id 为map
# Obstacle footprint (polygon descriptions)
geometry_msgs/Polygon polygon
# radius for circular/point obstacles
float64 radius
# Obstacle ID
# Specify IDs in order to provide (temporal) relationships
# between obstacles among multiple messages.
int64 id
# Individual orientation (centroid)
geometry_msgs/Quaternion orientation
# Individual velocities (centroid)
geometry_msgs/TwistWithCovariance velocities

Obstacle定义在obstacle.h,派生类有PointObstacle, CircularObstacle, LineObstacle,PolygonObstacle

1
2
typedef boost::shared_ptr<Obstacle> ObstaclePtr;
typedef std::vector<ObstaclePtr> ObstContainer;


(三) 从全局路径获得 transform_plan

MoveBase::executeCycle() —-> computeVelocityCommands,就是从这个函数开始调用TEB算法,下面转入teb_local_planner_ros.cpp文件中。

开始代码:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
// check if plugin initialized,省略
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
cmd_vel.angular.z = 0;
goal_reached_ = false;
// 修改源码
if(!rotate_to_path_ && new_goal_)
{
double start_angle = tf::getYaw(path_start_orientation_);
tf::Stamped<tf::Pose> robot_pose;
costmap_ros_->getRobotPose(robot_pose);
double robot_angle = tf::getYaw(robot_pose.getRotation());

bool result = pointToPath(start_angle, robot_angle, cmd_vel);
if(result)
{
rotate_to_path_ = true;
return true;
}
return true;
}

// Get robot pose in global frame(map坐标系),转为PoseSE2类型
tf::Stamped<tf::Pose> robot_pose;
costmap_ros_->getRobotPose(robot_pose);
robot_pose_ = PoseSE2(robot_pose);

// Get robot velocity from odom
tf::Stamped<tf::Pose> robot_vel_tf;
odom_helper_.getRobotVel(robot_vel_tf);
robot_vel_.linear.x = robot_vel_tf.getOrigin().getX();
robot_vel_.linear.y = robot_vel_tf.getOrigin().getY();
robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation());

// prune global plan to cut off parts of the past (spatially before the robot)
pruneGlobalPlan(*tf_, robot_pose, global_plan_);

// Transform global plan to the frame of interest (w.r.t. the local costmap)
std::vector<geometry_msgs::PoseStamped> transformed_plan;
int goal_idx;
tf::StampedTransform tf_plan_to_global;
if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.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");
return false;
}

// update via-points container 这里是false,不用看了
if (!custom_via_points_active_)
updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);

// check if global goal is reached
tf::Stamped<tf::Pose> global_goal;
tf::poseStampedMsgToTF(global_plan_.back(), global_goal);
global_goal.setData( tf_plan_to_global * global_goal );
double dx = global_goal.getOrigin().getX() - robot_pose_.x();
double dy = global_goal.getOrigin().getY() - robot_pose_.y();
double delta_orient = g2o::normalize_theta( tf::getYaw(global_goal.getRotation()) - robot_pose_.theta() );
// 是否达到目标位置
if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance)
position_reanched_ = true;

// 修改源码
if(position_reached_)
{
double goal_angle = tf::getYaw(goal_orientation_);
double robot_angle = tf::getYaw(robot_pose.getRotation());

bool result = pointToPath(goal_angle, robot_angle, cmd_vel);
if(result)
{
goal_reached_ = true;
return true;
}
return true;
}

if(!goal_reached_)
{
if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance
&& fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance
&& (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0))
{
goal_reached_ = true;
return true;
}
}
// check if we should enter any backup mode and apply settings
configureBackupModes(transformed_plan, goal_idx);

// Return false if the transformed global plan is empty
if (transformed_plan.empty())
{
ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");
return false;
}

  • pruneGlobalPlan: 截取全局路径的一部分作为局部规划的初始轨迹,主要是把全局路径中机器人已经经过的位姿删去。取决于参数 dist_behind_robot,机器人身后要保留的距离,单位米,默认1,不能太小,尤其不能小于地图的cellsize。 在调用前后,global_plan_常常不发生变化,所以不必太关注。

  • transformGlobalPlan: 根据local costmap地图大小,将此地图内的全局规划器的规划保存到transformed_plan中。transformed_plan所在的坐标系仍然为global_frames

global_plan_对应的就是话题/move_base/TebLocalPlannerROS/global_plan,它的size和成员一般在规划两三次后就会变,size一般是逐步变小,transformed_plan也跟随变化。

transformGlobalPlan函数之后,transformed_plan开始和global_plan_相等,transformed_plan逐渐取global_plan_的后面部分,此时goal_idx == global_plan.size() - 1。 有时候transformed_planglobal_plan_中间部分,此时goal_idx != global_plan.size() - 1

这三者会出现以下几种情况:

  1. global_plan_ size: 49, transformed_plan size:49, goal_idx: 48 : transformed_plan是从global_plan第1个点开始(id=0), 二者完全相同goal_idx对应transformed_planglobal_plan_最后一个点

  2. global_plan_ size: 102, transformed_plan size:69, goal_idx: 68: transformed_plan是从global_plan第1个点开始(id=0), 但这次 transformed_plan 取得是 global_plan 前半部分goal_idx对应transformed_plan最后一个点,

  3. global_plan_ size: 49, transformed_plan size:45, goal_idx: 48: transformed_plan是从global_plan第5个点开始(id=4),取global_plan后半部分goal_idx对应transformed_planglobal_plan_最后一个点

  4. global_plan size: 98, transformed_plan size:69, goal_idx: 73: transformed_plan是从global_plan第6个点开始(id=5), 也就是取中间部分goal_idx对应transformed_plan最后一个点。 goal_idx != transformed_plan.size()-1

goal_idx 是 global_plan 中的id,它对应的点一定和 transformed_plan 最后一个点相同,所以后面才可以赋值给goal_pointrobot_goal_


(二) 算法的初始化

MoveBase中加载局部路径规划器

1
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
{
if(!initialized_)
{
// create Node Handle with name of plugin (as used in move_base for loading)
ros::NodeHandle nh("~/" + name);

// get parameters of TebConfig via the nodehandle and override the default config
cfg_.loadRosParamFromNodeHandle(nh); // TebConfig cfg_;
// nh.param("trapped_threshold", trapped_threshold_, 20);
nh.param("filter_back_velocity", filter_back_velocity_, true);
// nh.param("setting_acc", setting_acc_, 1.5);

obstacles_.reserve(500); // ObstContainer obstacles_;

// 在TebVisualization构造函数里,注册话题 global_plan, local_plan,teb_poses, teb_markers, teb_feedback
visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_));

//创建robot footprint/contour model for optimization,对5种类型分别处理
// 点类型最简单,返回新建的PointRobotFootprint共享指针
// 对于polygon类型,只对 Point2dContainer vertices_;赋值
RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh);

// 根据参数,选择创建哪种 planner
if (cfg_.hcp.enable_homotopy_class_planning)
{
planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
ROS_INFO("Parallel planning in distinctive topologies enabled.");
}
else
{
planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
ROS_INFO("Parallel planning in distinctive topologies disabled.");
}
// init other variables
tf_ = tf; // 在 move_base_node.cpp中最初定义
// 这个就是 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
costmap_ros_ = costmap_ros;
// locking should be done in MoveBase
costmap_ = costmap_ros_->getCostmap();

// 创建 CostmapModel 共享指针
costmap_model_ = boost::make_shared<base_local_planner::CostmapModel>(*costmap_);

// 就是return global_frame, 实际是 "map"
global_frame_ = costmap_ros_->getGlobalFrameID();
cfg_.map_frame = global_frame_; // 更新cf_的成员

// 代价地图的local frame, 实际是 "base_footprint"
robot_base_frame_ = costmap_ros_->getBaseFrameID();

//Initialize a costmap to polygon converter
if (!cfg_.obstacles.costmap_converter_plugin.empty())
{
try
{
costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.obstacles.costmap_converter_plugin);
std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin);
// replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace
boost::replace_all(converter_name, "::", "/");
costmap_converter_->setOdomTopic(cfg_.odom_topic);
costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name));
costmap_converter_->setCostmap2D(costmap_);

costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread);
ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded.");
}
catch(pluginlib::PluginlibException& ex)
{
ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treated as point obstacles. Error message: %s", ex.what());
costmap_converter_.reset();
}
}
else
ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.");

// 类型是 vector<geometry_msgs::Point>
footprint_spec_ = costmap_ros_->getRobotFootprint();
costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
// 更新cf_的成员
cfg_.robot.robot_inscribed_radius = robot_inscribed_radius_;
cfg_.robot.robot_circumscribed_radius = robot_circumscribed_radius;

// init the odom helper to receive the robot's velocity from odom messages
odom_helper_.setOdomTopic(cfg_.odom_topic);

// setup dynamic reconfigure
dynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(nh);
dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2);
dynamic_recfg_->setCallback(cb);

// validate optimization footprint and costmap footprint
// 对于point类型,第1个参数是0,要求最好第1项+第3项 > 第2项
// 否则 Infeasible optimziation results might occur frequently
validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_,
cfg_.obstacles.min_obstacle_dist);

// custom obstacles的回调函数,话题 /move_base/TebLocalPlannerROS/obstacles
custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);

// custom via-points的回调,话题 /move_base/TebLocalPlannerROS/via_points
via_points_sub_ = nh.subscribe("via_points", 1, &TebLocalPlannerROS::customViaPointsCB, this);
// queue_follow 所用,话题 /rectangle_distance
front_dist_sub_ = nh.subscribe("/rectangle_distance", 1, &TebLocalPlannerROS::frontDistanceCB, this);
// 新添加
footprint_sub_ = nh.subscribe("/footprint", 10, &TebLocalPlannerROS::footprintCB, this);


ros::NodeHandle nh_move_base("~");
double controller_frequency = 5;
// 实际配置是 30
nh_move_base.param("controller_frequency", controller_frequency, controller_frequency);
// initialize failure detector,Detect if the robot got stucked
failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency));
// 初始化,返回一个很大的数
min_distance_custom_obstacle = std::numeric_limits<double>::max();

// set initialized flag
initialized_ = true;
ROS_DEBUG("teb_local_planner plugin initialized.");
}
else
ROS_WARN("teb_local_planner has already been initialized, doing nothing.");
}

关于RobotFootprintModelPtr,看TEB中的footprint模型

calculateMinAndMaxDistances函数和RobotFootprintModelPtr说明同时使用了两种footprint模型


(五) 局部子目标的朝向覆盖
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
// 局部的子目标 (transformed plan最后一个元素)
tf::Stamped<tf::Pose> goal_point;
tf::poseStampedMsgToTF(transformed_plan.back(), goal_point);
robot_goal_.x() = goal_point.getOrigin().getX();
robot_goal_.y() = goal_point.getOrigin().getY();

if (cfg_.trajectory.global_plan_overwrite_orientation)
{
robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point,
goal_idx, tf_plan_to_global);
// overwrite/update goal orientation of the transformed plan with the actual goal
// (enable using the plan as initialization)
transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta());
}
else
robot_goal_.theta() = tf::getYaw(goal_point.getRotation());

// plan only contains the goal, 一般在导航的最后出现size是1的情况
// 用实际的机器人位姿更新 transformed plan 的起点(allows using the plan as initial trajectory)
if (transformed_plan.size()==1)
{ // 插入起点,但不初始化
transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped() );
}
tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // 把起点更新为 机器人当前位姿

参数global_plan_overwrite_orientation为True时,执行estimateLocalGoalOrientation() 将未来一段的规划的姿态角的平均值返回为估计的局部目标姿态

transformed_plan.back()就是所谓的 local goal ,但是遇到需要终点转弯时,local goal的朝向不是最终的goal的朝向,仍然是规划的


swap内存

解决编译时内存不足c++: internal compiler error: Killed (program cc1plus)

1
2
3
4
5
6
7
8
9
10
11
# 创建分区文件, 大小 3G
sudo dd if=/dev/zero of=/swapfile bs=1M count=3000

sudo mkswap /swapfile

sudo swapon /swapfile

# After compiling, you may wish to

sudo swapoff /swapfile
sudo rm /swapfile

其实swap空间就是把硬盘上的空间用来当内存使用,是一种折中的处理方法。


(一) 理论和安装

理论

time elastic band(时间橡皮筋)从elastic band算法(1993年提出)优化而来。TEB的初始轨迹是全局路径传来的transformed_plan,中间插入N个控制橡皮筋形状的路径点(机器人位姿),在点与点之间定义时间差,转换为明确依赖于时间的轨迹,从而实现对机器人的实时控制。由于其模块化的形式,该方法可以扩展目标函数和约束。

TEB的创新在于考虑了动态约束和增加时间信息,如有限的机器人速度和加速度,将规划问题用加权多目标优化框架表示,也是一个bundle adjustment问题。在实时应用中,在线对全局路径进行大规模的计算往往是不可行的,TEB大多数目标函数是局部(local)的,因为它们依赖于一些邻近的中间 configurations。TEB的这种局部性会生成一个稀疏系统矩阵,才可以实现快速有效的大规模优化。作者写第一篇论文时(2012),把约束表示为一个分段连续、可微的代价函数,到了下一篇论文(2013),改成了使用g2o的图优化方法(利文伯格法)。

路径(Path)与轨迹(Trajectory)区别在于,轨迹包含了时间信息。所以TEB算法里老用Trajectory这个词。

各类目标函数

目标函数的梯度可以理解为作用在弹性带上的外力。

TEB的目标函数分为两类:

  • 约束:例如 速度、加速度限制
  • 目标:例如 最快、最短路径或者到障碍的距离

目标函数依赖的参数只取决于邻近位姿的子集。比如:

  • 速度(加速度)约束取决于两个(三个)连续的位姿和一个(两个)时间差。
  • 障碍物的清除和在中间路线上进行导航,只影响单个位姿和它的k个最近的邻居(实际上大约3-5)。
  • 遵守机器人的非完整运动学约束涉及到了两个相邻的位姿,这两个位姿位于一个常曲率的普通圆弧上

但是最快和最短路径是例外,显然是因为它们需要全局地依赖于所有的参数。

TEB同时考虑原始路径的via-point约束和避开障碍。这两个目标函数相似,不同之处在于via-point吸引路径,而障碍物排斥路径。

约束用到了几个顶点(待优化变量),对应的就是几元边。 比如速度约束用到了两个位姿和一个时间差,是三元边。 加速度约束就是五元边。 障碍约束和via-point就是一元边。非完整运动学约束是二元边。
图的构建过程

为什么可以用非线性优化的框架?

平时所说的图优化的最小二乘形式是这样的:
图优化的最小二乘形式

TEB所优化的目标函数是这样的:
TEB的目标函数
我们先把上面的F(x)看成 , 其中

图优化中,顶点是待估计的参数,观测数据构成了边。 TEB的待估计参数就是 ,也就是位姿和时间差的组合。 边表示目标函数 ,并连接多个顶点。

这个图的顶点vertexs是橡皮筋的状态(机器人姿态+时间),起点和终点当然是固定的,图的边是我们自己定义的优化目标函数。默认使用利文伯格法,优化器是g2o::CSparse

g2o框架在批处理模式下优化了TEB,因此每次迭代都会生成一个新的图。在一个机器人控制周期内对轨迹修改步骤进行多次迭代(四个循环包括5次利文伯格的迭代)。 g2o框架提供了两种基于Cholesky decomposition 的求解器: CHOLMODCSparse。默认是CSParse

对优化后的TEB进行验证,检查是否违反了硬约束,如果违反,在这种情况下机器人要么停止,要么重新调用运动规划器。

TEB每次迭代的平均运行时间依赖于TEB的维数,TEB的维数随位姿数(configurations)线性增长。对于10000多个states,对应大约2500种configurations。

Homotopy Planner

TEB中实现了两种规划器,一个就是普通的TebOptimalPlanner,另一个是HomotopyClassPlanner,HomotopyClassPlanner是一种同时优化多个轨迹的方法,由于目标函数的非凸性(非凸函数)会生成一系列最优的候选轨迹,最终在备选局部解的集合中寻求总体最佳候选轨迹,对应论文:《Integrated online trajectory planning and optimization in distinctive topologies》

optimizer本身只能找到局部最优轨迹,有时找到的路径invalid,所以一般都是用HomotopyClassPlanner。HomotopyClassPlanner类像是多个TebOptimalPlanner类实例的组合。HomotopyClassPlanner类中也会实例化一个由全局规划器生成的路径作为参考的对象。除此之外,它还会使用probabilistic roadmap (PRM) methods在障碍物周边采样一些keypoints,将这些keypoints连接起来,去除方向没有朝向目标点的连接和与障碍物重叠的连接。这样就形成了一个网络,然后将起始点和终点接入到这个网络。如下图所示:

使用Depth First Search(深度优先方法)搜索所有可行的路径。将这些路径作为参考,实例化多个TebOptimalPlanner类的实例。采用多线程并行优化,得到多条优化后的路径。将这些路径进行可行性分析,选出代价值最小的最优路径。不得不说HomotopyClassPlanner类里的方法是一个鲁棒性和可靠性更高的方法。


安装

编译teb时报错,或者在没有编译TEB的计算机上通过.so运行TEB时会报错
加载TEB算法时报警.png
libteb_local_planner.so 缺链接库.png
实际的链接情况.png

因为没有安装g2o和ceres,不过不需要自己安装g2o。 先运行sudo apt-get install ros-melodic-teb-local-planner,这样会在/opt/ros里安装g2o,然后把g2o的so文件复制到/usr/local/lib

1
2
sudo cp /opt/ros/melodic/lib/libg2o* /usr/local/lib
sudo cp -r /opt/ros/melodic/include/g2o /usr/local/include

如果自己之前安装的g2o的相关共享库没有清除干净,解决方法为:

  • 删除/usr/local/include/g2o,指令为 sudo rm -rf /usr/local/include/g2o
  • 删除/usr/local/lib下有关libg2o*.so的库文件,先进入目录/usr/local/lib,然后挨个(可多个同时)删除: `sudo rm -rf libg2o.so libg2o_.so libg2o_*.so`

参考:
对ROS局部运动规划器Teb的理解
Trajectory modification considering dynamic constraints of autonomous robots
Efficient Trajectory Optimization using a Sparse Model—使用稀疏模型对有效轨迹进行优化


AMCL的自动全局定位
Welcome to my blog, enter password to read.
Read more
点积和范数

L2范数 squareNorm(),等价于计算vector的自身点积,norm()返回squareNorm的开方根。

这些操作应用于matrix,norm() 会返回Frobenius或Hilbert-Schmidt范数。

如果你想使用其他Lp范数,可以使用lpNorm< p >()方法。p可以取Infinity,表示L∞范数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
int main()
{
VectorXf v(2);
MatrixXf m(2,2), n(2,2);

v << -1,
2;

m << 1,-2,
-3,4;
cout << "v.squaredNorm() = " << v.squaredNorm() << endl;
cout << "v.norm() = " << v.norm() << endl;
cout << "v.lpNorm<1>() = " << v.lpNorm<1>() << endl;
cout << "v.lpNorm<Infinity>() = " << v.lpNorm<Infinity>() << endl;
cout << endl;
cout << "m.squaredNorm() = " << m.squaredNorm() << endl;
cout << "m.norm() = " << m.norm() << endl;
cout << "m.lpNorm<1>() = " << m.lpNorm<1>() << endl;
cout << "m.lpNorm<Infinity>() = " << m.lpNorm<Infinity>() << endl;
}


single orientation layer
abstract Welcome to my blog, enter password to read.
Read more