本文是对上一篇的补充
TimedElasticBand在timed_elastic_band.h，此类定义了一条时变弹性带的轨迹模型，只有两个protected成员1
2
3
4
5
6
7
8
9
10//Internal container storing the sequence of optimzable pose vertices
PoseSequence pose_vec_;
// Internal container storing the sequence of optimzable timediff vertices
TimeDiffSequence timediff_vec_;
// Container of poses that represent the spatial(空间的) part of the trajectory
typedef std::vector<VertexPose*> PoseSequence;
// Container of time differences that define the temporal(时间的) of the trajectory
typedef std::vector<VertexTimeDiff*> TimeDiffSequence;
类VertexPose是g2o类型，在g2o_types\vertex_pose.h，保存和封装SE2 pose (position and orientation) into a vertex(图顶点)，用于g2o的图优化。 类VertexTimeDiff是g2o类型，在g2o_types\vertex_timediff.h，保存和封装时间差deltaT为一个vertex，用于g2o的图优化.
在TEB初始化的过程中，将起点的Pose与终点的Pose所在的顶点设为固定的，使得g2o不对这两个Pose进行优化。
初始化某一个TEB时，其路线简单的由起点和终点的连线组成。在这条直线上均匀采样点作为待优化的顶点。采样的步长由参数min_samples决定。而 timediff 顶点的初始值为 步长除以参数max_vel_x。
每有一个pose顶点就产生一个time_diff顶点，time_diff顶点实际上是每两个Pose之间所需要的时间。
初始化轨迹 initTrajectoryToGoal
以指定的plan(或者起点和终点)进行插值产生的轨迹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
81bool TimedElasticBand::initTrajectoryToGoal(
const std::vector<geometry_msgs::PoseStamped>& plan,
double max_vel_x, double max_vel_theta, bool estimate_orient,
int min_samples, bool guess_backwards_motion )
{
if (!isInit()) // timediff_vec_ pose_vec_ 都是空
{
PoseSE2 start(plan.front().pose);
PoseSE2 goal(plan.back().pose);
// double dt = 0.1;
addPose(start); // 向 pose_vec_ 添加 起点 此时的 BackPose()是 start
//StartConf 称为固定约束，第一个参数是 index
setPoseVertexFixed(0,true); // pose_vec_.at(index)->setFixed(status);
bool backwards = false;
// 参数 guess_backwards_motion 在这里
// check if the goal is behind the start pose (w.r.t. start orientation)
if ( guess_backwards_motion &&
( goal.position()-start.position()).dot(start.orientationUnitVec()) < 0)
backwards = true;
// TODO: dt ~ max_vel_x_backwards for backwards motions
// 取自全局路径点
for (int i=1; i<(int)plan.size()-1; ++i)
{
double yaw;
if (estimate_orient)
{
// get yaw from the orientation of the distance vector
// between pose_{i+1} and pose_{i}
double dx = plan[i+1].pose.position.x - plan[i].pose.position.x;
double dy = plan[i+1].pose.position.y - plan[i].pose.position.y;
yaw = std::atan2(dy,dx);
if (backwards)
yaw = g2o::normalize_theta(yaw+M_PI);
}
else
{
yaw = tf::getYaw(plan[i].pose.orientation);
}
PoseSE2 intermediate_pose(plan[i].pose.position.x, plan[i].pose.position.y, yaw);
/* 根据最大线速度和角速度，估计从 BackPose 到 intermediate_pose所花的时间，
假设匀速运动。也就是说这是估算了一个最小值 */
double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
// intermediate_pose添加到 pose_vec_, 约束false, 它成为BackPose()
// dt 添加到 timediff_vec_ , 约束false
addPoseAndTimeDiff(intermediate_pose, dt);
}
// number of samples 小于参数 min_samples, insert manually
// -1 的意思是不包含goal point，它在之后添加
if ( sizePoses() < min_samples-1 )
{
ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than
specified by min_samples. Forcing the insertion of more samples ...");
while (sizePoses() < min_samples-1)
{
// 策略: interpolate between the current pose and the goal
// 只处理了 BackPose() 到 goal 这一段，而且是逐步二分
PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
// let the optimier correct the timestep (TODO: better initialization
addPoseAndTimeDiff( intermediate_pose, dt );
}
}
// Now add final state with given orientation
double dt = estimateDeltaT(BackPose(), goal, max_vel_x, max_vel_theta);
// goal 也插入 pose_vec_, 成为 BackPose()
addPoseAndTimeDiff(goal, dt);
// GoalConf 也成为了固定约束
setPoseVertexFixed(sizePoses()-1,true);
}
else // size!=0
{
ROS_WARN("Cannot init TEB between given configuration and goal, because
TEB vectors are not empty or TEB is already initialized (call this function
before adding states yourself)!");
ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d",
(unsigned int) sizePoses(), (unsigned int) sizeTimeDiffs());
return false;
}
return true;
}
- 设置起点并且固定（不允许被优化）。
- 设置到目标一条直线上平均间隔的点作为初始位姿。
- 设置初始的时间间隔序列。
- 设置终点并固定（不允许被优化）。
注意： dt的单位是秒，不是时间戳，是路径点之间的时间差。所有点都插入 pose_vec_，但是除起点外的timestep都插入 timediff_vec_
热启动 updateAndPruneTEB
更新和改变轨迹，即根据现在的位置更新起点（vector中的第一个点），改变终点。调用 teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples);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// new_start new_goal 的根源都在 transformed_plan
void TimedElasticBand::updateAndPruneTEB(boost::optional<const PoseSE2&> new_start,
boost::optional<const PoseSE2&> new_goal, int min_samples)
{
// first and simple approach: change only start confs(and virtual start conf for inital velocity)
// TEST if optimizer can handle this "hard" placement
if ( new_start && sizePoses()>0 )
{
// find nearest state (using L2-norm) in order to prune the trajectory
// (remove already passed states)
double dist_cache = (new_start->position()- Pose(0).position()).norm();
double dist;
// satisfy min_samples, otherwise max 10 samples
int lookahead = std::min<int>( sizePoses()-min_samples, 10);
int nearest_idx = 0;
for (int i = 1; i<=lookahead; ++i)
{
dist = (new_start->position()- Pose(i).position() ).norm();
if (dist < dist_cache)
{
dist_cache = dist;
nearest_idx = i;
}
else break;
}
// prune trajectory at the beginning (and extrapolate
//sequences at the end if the horizon is fixed )
if (nearest_idx>0)
{
// nearest_idx is equal to the number of samples to be removed (since it counts from 0 )
// WARNING delete starting at pose 1, and overwrite the original pose(0) with
// new_start, since Pose(0) is fixed during optimization!
// delete first states such that the closest state is the new first one
deletePoses(1, nearest_idx);
deleteTimeDiffs(1, nearest_idx); // delete corresponding time differences
}
// update start
Pose(0) = *new_start;
}
if (new_goal && sizePoses()>0)
{
BackPose() = *new_goal;
}
};
当机器人位置变化时，初始轨迹是删去已经走过的，保留后续的。
作用：
- 取当前位置为起点，删去已经走过的轨迹，并重新设定终点。
- 对于位姿与时间序列，删减去新起点之前的位姿和时间，也就是删除两个容器中旧的元素
- 将新的终点加入序列
