(十二) optimizeTEB 3 AddEdgesObstacles

类的定义

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
/**
* Edge defining the cost function for keeping a minimum distance from obstacles.

* The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes:
* \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$.
* \e dist2point denotes the minimum distance to the point obstacle.
* \e weight can be set using setInformation(). \n
* \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow()

* @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
* @remarks Do not forget to call setTebConfig() and setObstacle()
*/
class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose>
{
public:
EdgeObstacle()
{
_measurement = NULL;
}
/**
* @brief Set pointer to associated obstacle for the underlying cost function
* @param obstacle 2D position vector containing the position of the obstacle
*/
void setObstacle(const Obstacle* obstacle)
{
_measurement = obstacle;
}
/**
* @brief Set pointer to the robot model
* @param robot_model Robot model required for distance calculation
*/
void setRobotModel(const BaseRobotFootprintModel* robot_model)
{
robot_model_ = robot_model;
}
/**
* @brief Set all parameters at once
* @param cfg TebConfig class
* @param robot_model Robot model required for distance calculation
* @param obstacle 2D position vector containing the position of the obstacle
*/
void setParameters(const TebConfig& cfg,
const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
{
cfg_ = &cfg;
robot_model_ = robot_model;
_measurement = obstacle;
}
// cost function
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()" );
const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
/* inline double penaltyBoundFromBelow( const double& var,
const double& a, const double& epsilon)
{
if (var >= a+epsilon)
return 0.;
else
return (-var + (a+epsilon));
} */
double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);

// Original obstacle cost
_error[0] = penaltyBoundFromBelow( dist,
cfg_->obstacles.min_obstacle_dist,
cfg_->optim.penalty_epsilon );
if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
{
// Optional non-linear cost. Note the max cost (before weighting) is
// the same as the straight line version and that all other costs are
// below the straight line (for positive exponent), so it may be
// necessary to increase weight_obstacle and/or the inflation_weight
// when using larger exponents
_error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(
_error[0] / cfg_->obstacles.min_obstacle_dist,
cfg_->optim.obstacle_cost_exponent);
}
ROS_ASSERT_MSG(std::isfinite(_error[0]),
"EdgeObstacle::computeError() _error[0]=%f\n",
_error[0]);
}
protected:
const BaseRobotFootprintModel* robot_model_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

一元边,观测值维度为1,测量值类型为Obstacle基类,连接VertexPose顶点。存储了某个障碍物的中心点的三维位置,形状与顶点的位置

只将离某个Pose最近的最左边与最右边的两个Obstacle加入优化中。(因为优化路径不会使得路径相对于障碍物的位置关系发生改变)。同时,还设了一个阈值,凡是离该Pose距离低于某个距离的障碍物也一并加入考虑之中。

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
void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier)
{
if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr )
return;
// 判断 inflation_dist > min_obstacle_dist
bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;

Eigen::Matrix<double,1,1> information;
information.fill(cfg_->optim.weight_obstacle * weight_multiplier);

Eigen::Matrix<double,2,2> information_inflated;
information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier;
information_inflated(1,1) = cfg_->optim.weight_inflation;
information_inflated(0,1) = information_inflated(1,0) = 0;

auto iter_obstacle = obstacles_per_vertex_.begin();

auto create_edge = [inflated, &information, &information_inflated, this] (int index,
const Obstacle* obstacle) {
if (inflated)
{
// 一元边,观测值维度为2,类型为Obstacle基类,连接VertexPose顶点
EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle;
dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
// 信息矩阵为2x2对角阵,(0,0) = weight_obstacle, (1,1) = weight_inflation
dist_bandpt_obst->setInformation(information_inflated);
dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obstacle);
optimizer_->addEdge(dist_bandpt_obst);
}
else
{
// 一元边,观测值维度为1,测量值类型为Obstacle基类,连接VertexPose顶点
EdgeObstacle* dist_bandpt_obst = new EdgeObstacle;
dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
// 信息矩阵为 1x1 weight_obstacle * weight_multiplier
dist_bandpt_obst->setInformation(information);
dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obstacle);
optimizer_->addEdge(dist_bandpt_obst);
};
};

/* iterate all teb points, skipping the last and, if the
EdgeVelocityObstacleRatio edges should not be created, the first one too*/
const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0;
for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i)
{
double left_min_dist = std::numeric_limits<double>::max();
double right_min_dist = std::numeric_limits<double>::max();
ObstaclePtr left_obstacle;
ObstaclePtr right_obstacle;

const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec();

// iterate obstacles
for (const ObstaclePtr& obst : *obstacles_)
{
// we handle dynamic obstacles differently below
if(cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic())
continue;

// calculate distance to robot model
double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get());

// force considering obstacle if really close to the current pose
if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor)
{
iter_obstacle->push_back(obst);
continue;
}
// cut-off distance
if (dist > cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_cutoff_factor)
continue;

// determine side (left or right) and assign obstacle if closer than the previous one
if (cross2d(pose_orient, obst->getCentroid()) > 0) // left
{
if (dist < left_min_dist)
{
left_min_dist = dist;
left_obstacle = obst;
}
}
else
{
if (dist < right_min_dist)
{
right_min_dist = dist;
right_obstacle = obst;
}
}
}
// create obstacle edges
if (left_obstacle)
iter_obstacle->push_back(left_obstacle);
if (right_obstacle)
iter_obstacle->push_back(right_obstacle);

// continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges
if (i == 0)
{
++iter_obstacle;
continue;
}
// create obstacle edges
for (const ObstaclePtr obst : *iter_obstacle)
create_edge(i, obst.get());
++iter_obstacle;
}
}

误差函数的计算

两个类根据机器人的轮廓模型计算当前Pose与某个障碍物的距离dist,至于如何计算,属于另一个话题。 然后EdgeObstacle的error如下

1
error = (dist > min_obstacle_dist+ε) ? 0 : (min_obstacle_dist+ε) - dist

EdgeInflatedObstacle则是

1
2
3
4
# Original "straight line" obstacle cost. The max possible value
# before weighting is min_obstacle_dist,和 EdgeObstacle的相同
error[0] = (dist > min_obstacle_dist+ε) ? 0 : (min_obstacle_dist+ε) - dist
error[1] = (dist > inflation_dist) ? 0 : (inflation_dist) - dist

但是这两个类的computeError在error[0]赋值结束后又补充这样一段:

1
2
3
4
5
6
7
8
9
10
if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
{
/* Optional 非线性代价. Note the max cost (before weighting) is
the same as the straight line version and that all other costs are
below the straight line (for positive exponent), so it may be
necessary to increase weight_obstacle and/or the inflation_weight
when using larger exponents */
_error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(
_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);
}

涉及参数obstacle_cost_exponent,不明白这个公式是怎么来的


(十六) saturateVelocity
1
2
3
4
// 如果优化结果违反约束(由于是软约束,所以是可能的), Saturate 速度
saturateVelocity(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z,
cfg_.robot.max_vel_x, cfg_.robot.max_vel_y,
cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);

saturateVelocity

若规划出来的速度超过限制速度,则令其等于最大限制速度

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
// Limit translational velocity for forward driving
if (vx > max_vel_x)
vx = max_vel_x;

// limit strafing velocity
if (vy > max_vel_y)
vy = max_vel_y;
else if (vy < -max_vel_y)
vy = -max_vel_y;

if(vx>= -0.12 && vx< 0)
{
vx = -0.12;
}
else if(vx< -0.12)
{
planner_->clearPlanner();
}
// Limit angular velocity
if (omega > max_vel_theta)
omega = max_vel_theta;
else if (omega < -max_vel_theta)
omega = -max_vel_theta;

剩余部分也很简单

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// 对汽车机器人,convert rot-vel to steering angle if desired
// The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
// and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself.
if (cfg_.robot.cmd_angle_instead_rotvel)
{ // 目前是false,省略
}
// a feasible solution should be found, reset counter
no_infeasible_plans_ = 0;
// 保存上一个速度命令 (for recovery analysis etc.)
last_cmd_ = cmd_vel;

// Now visualize everything
planner_->visualize();
visualization_->publishObstacles(obstacles_);
visualization_->publishViaPoints(via_points_);
visualization_->publishGlobalPlan(global_plan_);
return true; // 函数 computeVelocityCommands 结束

可视化部分,见下一篇


(十五) 输出速度命令

调用是这样的:

1
2
3
4
5
6
7
8
9
10
11
12
// Get the velocity command for this sampling interval
if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z,
cfg_.trajectory.control_look_ahead_poses, robot_vel_.linear.x,
robot_vel_.angular.z) )
{
planner_->clearPlanner();
ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner...");
++no_infeasible_plans_; // increase number of infeasible solutions in a row
time_last_infeasible_plan_ = ros::Time::now();
last_cmd_ = cmd_vel;
return false;
}

getVelocityCommand

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
bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega, 
int look_ahead_poses, double current_vx, double current_vtheta) const
{
// TimedElasticBand teb_; //!< Actual trajectory object
if (teb_.sizePoses()<2)
{
ROS_ERROR("TebOptimalPlanner::getVelocityCommand():
The trajectory contains less than 2 poses. Make sure to init and
optimize/plan the trajectory fist." );
vx = 0;
vy = 0;
omega = 0;
return false;
}
// teb_.sizePoses() 在规划中一直很大,除非是接近终点时候
look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));
double dt = 0.0;
/* double& TimeDiff(int index)
{
ROS_ASSERT(index<sizeTimeDiffs());
return timediff_vec_.at(index)->dt();
} */
ROS_INFO("\033[45;37m before counter, look_ahead_poses: %f \033[0m", look_ahead_poses);
for(int counter = 0; counter < look_ahead_poses; ++counter)
{
dt += teb_.TimeDiff(counter);
// TODO: change to look-ahead time? Refine trajectory ?
if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses)
{
look_ahead_poses = counter + 1;
break;
}
}
if (dt<=0)
{
ROS_ERROR("getVelocityCommand() timediff<=0 is invalid!");
vx = 0; vy = 0; omega = 0;
return false;
}
// Get velocity from the first two configurations
extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);

double v_new = vx;
double k = (v_new - current_vx) * (v_new - current_vx) /
( (v_new - current_vx) * (v_new - current_vx)
+ (cfg_->robot.acc_lim_x * dt) * (cfg_->robot.acc_lim_x * dt) );
vx = current_vx + k * (v_new - current_vx);
return true;
}

参数control_look_ahead_poses增大,会让look_ahead_poses变量增大,dt也会增大
间接导致最终的look_ahead_poses也比较大。如果参数dt_ref也增大,那此二者就更大了。

这里涉及到的公式太复杂了,实在看不出为什么增大 control_look_ahead_poses 会让速度平滑了

extractVelocity

提取机器人的速度在本体坐标系下的表示

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
// pose2 是 teb_.Pose(look_ahead_poses)
void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2,
double dt, double& vx, double& vy, double& omega) const
{
if (dt == 0)
{
vx = 0; vy = 0; omega = 0;
return;
}
Eigen::Vector2d deltaS = pose2.position() - pose1.position();

if (cfg_->robot.max_vel_y == 0) // nonholonomic robot
{
Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) );
// translational velocity
double dir = deltaS.dot(conf1dir);
// 无后退的情况下,sign函数都返回 1
vx = (double) g2o::sign(dir) * deltaS.norm() / dt;
vy = 0;
}
else // holonomic robot ...... 省略

// rotational velocity
double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());
omega = orientdiff / dt;
}

前面两个参数的增大导致deltaS.norm()dt都增大,从这里直接看不出vx是增大还是减小了,实际运行后,通过日志可以看到vx整体增大了。


1
2
3
4
5
6
inline int sign(T x)
{
if (x > 0) return 1;
else if (x < 0) return -1;
else return 0;
}

(四)configureBackupModes 1 处理 goal_idx 和 transformed_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
void TebLocalPlannerROS::configureBackupModes(
std::vector<geometry_msgs::PoseStamped>& transformed_plan,
int& goal_idx)
{
ros::Time current_time = ros::Time::now();
// reduced horizon backup mode 上篇博客所列的第四种情况下

// we do not reduce if the goal is already selected , because the orientation
// might change -> can introduce oscillations
// no_infeasible_plans_ 改成 no_feasible_plans_ 比较合适
if (cfg_.recovery.shrink_horizon_backup &&
goal_idx < (int)transformed_plan.size()-1 &&
(no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).toSec() <
// keep short horizon for at least a few seconds
cfg_.recovery.shrink_horizon_min_duration ) )
{
ROS_INFO_COND(no_infeasible_plans_==1,
"Activating reduced horizon backup mode for at least %.2f sec (infeasible
trajectory detected).",
cfg_.recovery.shrink_horizon_min_duration);

// Shorten horizon if requested reduce to 50 percent:
int horizon_reduction = goal_idx/2;

if (no_infeasible_plans_ > 9)
{
ROS_INFO_COND(no_infeasible_plans_==10, "Infeasible trajectory detected
10 times in a row: further reducing horizon...");
horizon_reduction /= 2;
}
// we have a small overhead (过头) here, since we already transformed 50%
// more of the trajectory. But that's ok for now, since we do not
// need to make transformGlobalPlan more complex
// reduced horizon 应当很少发生
int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1;
goal_idx -= horizon_reduction;

// 从 transformed_plan 的 new_goal_idx 开始,一直删除到尾部
if (new_goal_idx_transformed_plan >0 && goal_idx >= 0)
transformed_plan.erase(transformed_plan.begin() + new_goal_idx_transformed_plan, transformed_plan.end());
else
goal_idx += horizon_reduction; // this should not happen, but safety first
}
// ......第二部分......
}
  • shrink_horizon_backup: true. 在自动检测到问题(plan not feasible)的情况下,允许planner临时缩小范围(50%). TEB将以更近的点作为规划目标,尝试重新规划出可行路径。调试时可关闭,以在可视化界面上观察原出错路径。另请参阅参数shrink_horizo​​n_min_duration

  • shrink_horizon_min_duration: 10,单位秒。 如果检测到不可行的轨迹,激活缩小的水平线后备模式,本参数为其最短持续时间。个人怀疑这个参数无意义,因为它更新值的地方也都有no_infeasible_plans_变量的更新,判断时,只根据no_infeasible_plans_即可,详见代码configureBackupModes


(四)configureBackupModes 2 FailureDetector和振荡恢复

configureBackupModes 第二部分

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
// detect and resolve oscillations
if (cfg_.recovery.oscillation_recovery)
{
double max_vel_theta;
// 车一般不能倒退,所以基本是 max_vel_x
double max_vel_current = last_cmd_.linear.x >= 0 ? cfg_.robot.max_vel_x : cfg_.robot.max_vel_x_backwards;
if (cfg_.robot.min_turning_radius!=0 && max_vel_current>0)
max_vel_theta = std::max( max_vel_current/std::abs(cfg_.robot.min_turning_radius), cfg_.robot.max_vel_theta );
else
max_vel_theta = cfg_.robot.max_vel_theta;

failure_detector_.update(last_cmd_, cfg_.robot.max_vel_x, cfg_.robot.max_vel_x_backwards, max_vel_theta,
cfg_.recovery.oscillation_v_eps, cfg_.recovery.oscillation_omega_eps);

bool oscillating = failure_detector_.isOscillating();
// 距离上次振荡过了多久,若小于配置值(10秒),说明最近也振荡了
bool recently_oscillated = (ros::Time::now()-time_last_oscillation_).toSec() < cfg_.recovery.oscillation_recovery_min_duration;
// 如果现在是振荡状态
if (oscillating)
{
if (!recently_oscillated) // 最近的状态不是振荡
{
// save current turning direction
// 以下其实都是对 last_preferred_rotdir_ 赋值
if (robot_vel_.angular.z > 0)
last_preferred_rotdir_ = RotType::left;
else
last_preferred_rotdir_ = RotType::right;
ROS_WARN("TebLocalPlannerROS: possible oscillation (of the robot
or its local plan) detected. Activating recovery strategy
(prefer current turning direction during optimization) ");
}
time_last_oscillation_ = ros::Time::now();
// 对 prefer_rotdir_ 赋值,最终影响 AddEdgesPreferRotDir
planner_->setPreferredTurningDir(last_preferred_rotdir_);
}
// 现在和最近都没有振荡 clear recovery behavior
else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none)
{
last_preferred_rotdir_ = RotType::none;
planner_->setPreferredTurningDir(last_preferred_rotdir_);
ROS_INFO("TebLocalPlannerROS: oscillation recovery disabled/expired.");
}
}

但是如果机器人确实stucked,最终会在move_base里Abort

setPreferredTurningDir

设置期望的初始转弯方向 (也就是惩罚相反的方向)。A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two solutions (in the same equivalence class!) with similar cost. 配置参数以调整惩罚的权重,Initial 说明惩罚只适用于trajectory的前几个位姿

参数dir可以是RotType::left, RotType::right, RotType::none

代码实际调用的是:

1
2
3
4
5
6
7
8
void HomotopyClassPlanner::setPreferredTurningDir(RotType  dir)
{
// set preferred turning dir for all TEBs
for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
{
(*it_teb)->setPreferredTurningDir(dir);
}
}

结果还是TebOptimalPlanner的同名成员函数,赋值而已

1
virtual void setPreferredTurningDir(RotType dir) {  prefer_rotdir_ = dir;  }

赋值完了,prefer_rotdir_会在AddEdgesPreferRotDir里产生影响

AddEdgesPreferRotDir

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
void TebOptimalPlanner::AddEdgesPreferRotDir()
{
/* roesmann 注释: these edges can result in odd predictions, in particular
we can observe a substantional mismatch between open- and closed-loop planning
leading to a poor control performance.
目前这个功能只用于振荡恢复,短期使用这个约束不会有重大影响
可能让机器人脱离振荡 */
if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir == 0)
return;
// 必须定义好转向
if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left)
{
ROS_WARN("TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation.");
return;
}
// create edge for satisfiying kinematic constraints
Eigen::Matrix<double,1,1> information_rotdir;
information_rotdir.fill(cfg_->optim.weight_prefer_rotdir);
// 目前只使用前三个旋转
for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i)
{
EdgePreferRotDir* rotdir_edge = new EdgePreferRotDir;
rotdir_edge->setVertex(0, teb_.PoseVertex(i));
rotdir_edge->setVertex(1, teb_.PoseVertex(i+1));
rotdir_edge->setInformation(information_rotdir);

if (prefer_rotdir_ == RotType::left)
rotdir_edge->preferLeft();
else if (prefer_rotdir_ == RotType::right)
rotdir_edge->preferRight();

optimizer_->addEdge(rotdir_edge);
}
}

调用时在configureBackupModes第二部分,那里是对prefer_rotdir_赋值,

FailureDetector

FailureDetector的代码在TEB的recovery_behaviors.cppFailureDetector检测机器人是否 stucked或者振荡,分析last N(buffer的大小)个速度命令,看这些线速度和角速度的平均值是否小于oscillation_v_epsoscillation_omega_eps

构造函数和析构全是空。

buffer

1
2
3
// Set buffer length (measurement history)
// length: number of measurements to be kept
void setBufferLength(int length) { buffer_.set_capacity(length); }

调用:

1
2
failure_detector_.setBufferLength(std::round(
cfg_.oscillation_filter_duration * controller_frequency ) );

默认是10*30,其实比较大,可以降低oscillation_filter_duration

1
2
// Clear buffer, 置为不振荡状态
void clear();

FailureDetector::update

先看调用,只有第一个参数是速度控制量,其他都是配置参数:

1
2
3
4
failure_detector_.update(last_cmd_, cfg_.robot.max_vel_x, cfg_.robot.max_vel_x_backwards, 
max_vel_theta, cfg_.recovery.oscillation_v_eps, cfg_.recovery.oscillation_omega_eps);

bool oscillating = failure_detector_.isOscillating();

源码其实也简单

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
void FailureDetector::update(const geometry_msgs::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps)
{
if (buffer_.capacity() == 0)
return;
VelMeasurement measurement;
/*
struct VelMeasurement
{
double v = 0;
double omega = 0;
}; */
measurement.v = twist.linear.x; // 不考虑 y 方向速度
measurement.omega = twist.angular.z;

if (measurement.v > 0 && v_max>0)
measurement.v /= v_max;
else if (measurement.v < 0 && v_backwards_max > 0)
measurement.v /= v_backwards_max;

if (omega_max > 0)
measurement.omega /= omega_max;

buffer_.push_back(measurement);
// immediately compute new state
detect(v_eps, omega_eps);
}

detect

  • v_eps: (0,1)内的normalized 线速度的平均值的阈值,默认0.1
  • omega_eps: (0,1)内的normalized 角速度的平均值,默认0.1

如果机器人stucked则返回true,否则false

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
bool detect(double v_eps, double omega_eps)
{
oscillating_ = false;
//至少有一半的速度数据才检测,也就是过一点时间才判断
if (buffer_.size() < buffer_.capacity()/2)
return false;
double n = (double)buffer_.size();
// 计算buffer内线速度和角速度的均值
double v_mean=0;
double omega_mean=0;
int omega_zero_crossings = 0;
for (int i=0; i < n; ++i)
{
v_mean += buffer_[i].v;
omega_mean += buffer_[i].omega;
if ( i>0 && g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) )
++omega_zero_crossings;
}
v_mean /= n;
omega_mean /= n;
if (std::abs(v_mean) < v_eps && std::abs(omega_mean) < omega_eps
&& omega_zero_crossings>1 )
{
oscillating_ = true;
}
return oscillating_;
}

参数

我用的参数值如下:

1
2
3
4
5
6
7
shrink_horizon_backup = true;
shrink_horizon_min_duration = 10: # 默认10
oscillation_recovery = true;
oscillation_v_eps = 0.1; # 线速度和角速度设置0.1已经足够
oscillation_omega_eps = 0.1;
oscillation_recovery_min_duration = 5;
oscillation_filter_duration = 6 # 默认10,这个大了会使buffer太大

要出现振荡,最直观的就是车左右摇摆或者车前后小步行走,因为代码中没有绝对值,否则朝一个方向小幅行走也算振荡了。实际中当然是不允许这种奇怪的行为


(十一) optimizeTEB 2 初始化g2o求解器和添加约束

g2o边和顶点的定义

所有边都在头文件base_teb_edges.h中,顶点就是g2o_types\vertex_timediff.hg2o_types\vertex_pose.h。两个顶点都比较简单,值得注意的是vertex_timediff.hsetToOriginImpl

1
2
3
4
virtual void setToOriginImpl()
{
_estimate = 0.1; // 不是0,不明白为什么
}

一元边:

1
2
template <int D, typename E, typename VertexXi>
class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>

  • EdgeViaPoint:以指定的经过点位置与待优化位姿(顶点)的误差作为目标。(保证轨迹会经过指定点)

  • EdgeObstacle:以障碍物与待优化位姿(顶点)的距离经过惩罚函数后的输出作为目标(保证离障碍物大于一定距离)

  • EdgeTimeOptimal:以时间间隔dt最小作为目标(保证时间最短轨迹)

二元边
二元边

1
2
template <int D, typename E, typename VertexXi, typename VertexXj>
class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>

主要是考虑了运动学的约束关系(车的相邻的两个位姿态必须落在一个圆弧上)。二元边的两个顶点分别是两个位姿,还限制了最小转弯半径。

多元边(构造函数中resize顶点的个数):

1
2
template <int D, typename E>
class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>

  • EdgeVelocity:以实际速度和限定最大速度经过惩罚函数的输出作为目标函数,顶点为相邻的位姿和时间间隔。(限制实际速度不超过最大,程序中引入了sigmoid函数决定速度的符号(根据论文内的说法,引入此函数是因为优化算法只能求解连续函数)

  • Edgevelocityholonomic:与EdgeVelocity的区别在于,ds是机器人本体坐标系下的,然后去求速度在本体坐标系下的表示。其余思路一致


1
2
3
class VertexPose : public g2o::BaseVertex<3, PoseSE2 >

class VertexTimeDiff : public g2o::BaseVertex<1, double>

初始化 optimizer

planner_初始化在if (cfg_.hcp.enable_homotopy_class_planning)的判断里,然后实际调用TebOptimalPlanner::initialize或者HomotopyClassPlanner::initialize,前者调用了TebOptimalPlanner::initOptimizer

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
boost::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer()
{
// Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
static boost::once_flag flag = BOOST_ONCE_INIT;
// 将为TEB定义的顶点和边注册到g2o::Factory
boost::call_once(&registerG2OTypes, flag);

// allocating the optimizer
boost::shared_ptr<g2o::SparseOptimizer> optimizer = boost::make_shared<g2o::SparseOptimizer>();
// linear solver utilized for optimization
// 针对稀疏矩阵,写死为 g2o::LinearSolverCSparse
// typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
TEBLinearSolver* linearSolver = new TEBLinearSolver(); // see typedef in optimization.h
linearSolver->setBlockOrdering(true);

// block solver utilized for optimization
// typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > TEBBlockSolver;
TEBBlockSolver* blockSolver = new TEBBlockSolver(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver =
new g2o::OptimizationAlgorithmLevenberg(blockSolver);

optimizer->setAlgorithm(solver);
optimizer->initMultiThreading(); // required for >Eigen 3.1

return optimizer;
}

注意算法求解器写死为 利文伯格法g2o::LinearSolverCSparse


构建超图 buildGraph

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
bool TebOptimalPlanner::buildGraph(double weight_multiplier)
{
// 构建图优化问题。若顶点和边非空,则 add
if (!optimizer_->edges().empty() || !optimizer_->vertices().empty())
{
ROS_WARN("Cannot build graph, because it is not empty. Call graphClear() ");
return false;
}
// 增加图的顶点,TimedElasticBand类型的路径点和dt为顶点
// 添加顺序会影响 稀疏矩阵的结构,进而影响优化效率
AddTEBVertices();
// add Edges (local cost functions)
// legacy_obstacle_association
/*已经修改了将轨迹姿势与优化障碍联系起来的策略(参见changelog)。
您可以通过将此参数设置为true来切换到 旧策略。
旧策略:对于每个障碍,找到最近的TEB姿势; 新策略:对于每个teb姿势,只找到“相关”的障碍 */
if (cfg_->obstacles.legacy_obstacle_association) // false
AddEdgesObstaclesLegacy(weight_multiplier);
else
AddEdgesObstacles(weight_multiplier); // 参数写死为 2

if (cfg_->obstacles.include_dynamic_obstacles)
AddEdgesDynamicObstacles();
/* error为其连接的Pose顶点的位置到这个Viapoint的距离的模长。
信息矩阵为1x1的矩阵,其值为 weight_viapoint
添加全局路径约束, 取决于参数 global_plan_viapoint_sep */
AddEdgesViaPoints();

// 对于TimedElasticBand类型中的位姿顶点序列-1条边,设置该边的顶点和权重矩阵,添加该边
AddEdgesVelocity();
AddEdgesAcceleration();
AddEdgesJerk(); // 实际不执行
// error直接就是连接的VertexTimeDiff的dt本身,信息矩阵为1x1矩阵,其值为weight_optimaltime
AddEdgesTimeOptimal(); //添加时间约束

if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
AddEdgesKinematicsDiffDrive(); // differential drive robot
else
AddEdgesKinematicsCarlike(); // carlike robot since the turning radius is bounded from below.

AddEdgesPreferRotDir(); // 实际不执行
return true;
}

添加 vertex

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
void TebOptimalPlanner::AddTEBVertices()
{
// add vertices to graph
ROS_DEBUG_COND(cfg_->optim.optimization_verbose, "Adding TEB vertices ...");
unsigned int id_counter = 0; // used for vertices ids
// std::vector<ObstContainer> obstacles_per_vertex_; 对应n-1 initial vertices
obstacles_per_vertex_.resize(teb_.sizePoses());
auto iter_obstacle = obstacles_per_vertex_.begin();
for (int i=0; i < teb_.sizePoses(); ++i)
{
// 两种 vertex 都要添加
teb_.PoseVertex(i)->setId(id_counter++);
optimizer_->addVertex(teb_.PoseVertex(i) );
if (teb_.sizeTimeDiffs()!=0 && i<teb_.sizeTimeDiffs() )
{
teb_.TimeDiffVertex(i)->setId(id_counter++);
optimizer_->addVertex(teb_.TimeDiffVertex(i) );
}
iter_obstacle->clear();
// 这里和 AddEdgesObstacles 有关
(iter_obstacle++)->reserve( obstacles_->size() );
}
}

这里的Vertex没有调用setEstimate函数,不设置初值。

添加约束

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::AddEdgesVelocity()
{
// 只看 non-holonomic robot
if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_theta==0)
return;

int n = teb_.sizePoses();
Eigen::Matrix<double,2,2> information;
// 对角线元素
information(0,0) = cfg_->optim.weight_max_vel_x;
information(1,1) = cfg_->optim.weight_max_vel_theta;
information(0,1) = 0.0;
information(1,0) = 0.0;
// 对所有的位姿,信息矩阵都是相同的
for (int i=0; i < n - 1; ++i)
{
EdgeVelocity* velocity_edge = new EdgeVelocity;
// 速度约束有三个顶点, 多元边
velocity_edge->setVertex(0,teb_.PoseVertex(i));
velocity_edge->setVertex(1,teb_.PoseVertex(i+1));
velocity_edge->setVertex(2,teb_.TimeDiffVertex(i));

velocity_edge->setInformation(information);
velocity_edge->setTebConfig(*cfg_);
optimizer_->addEdge(velocity_edge);
}
}

error有两项,分别是线速度与线速度线速度是否在设定好的区间内。

只要看懂了论文和g2o的应用套路,这里的添加的edge大部分都能看懂。比较复杂而且重要的是AddEdgesObstaclesAddEdgesViaPointsvia_point规定了轨迹应当经过这些点,否则会产生相应的cost。via_point边会与原规划的路径中与其距离最近的Pose顶点相连。

对于加速度约束,还要注意 vel_start_.firstvel_goal_.first,前者一般会添加,后者基本是false,信息矩阵一直是同一个。




1
2
3
4
5
6
7
8
9
10
11
void TebOptimalPlanner::clearGraph()
{
// clear optimizer states
if (optimizer_)
{
// neccessary, because optimizer->clear deletes pointer-targets
// therefore it deletes TEB states
optimizer_->vertices().clear();
optimizer_->clear();
}
}

局部规划可以让路径显示graph,在rviz中配置

参考:
TEB中的g2o问题的设置


(十四) 可行性 isTrajectoryFeasible
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
// Do not allow config changes during the following optimization step
boost::mutex::scoped_lock cfg_lock(cfg_.configMutex());
bool success = planner_->plan(robot_pose_, robot_goal_, &robot_vel_,
cfg_.goal_tolerance.free_goal_vel); // straight line init
if (!success)
{
planner_->clearPlanner(); // force reinitialization for next time
ROS_WARN("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;
}
// Check feasibility (but within the first few states only)
if(cfg_.robot.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);
// is a circular footprint
if( robot_circumscribed_radius / robot_inscribed_radius_ < 1.02 )
cfg_.obstacles.min_obstacle_dist = robot_circumscribed_radius;
// fit dynamic footprint
cfg_.robot.robot_inscribed_radius = robot_inscribed_radius_;
cfg_.robot.robot_circumscribed_radius = robot_circumscribed_radius;
}

isTrajectoryFeasible

接下来算法遍历前n个位姿(n就是参数feasibility_check_no_poses),从当前的机器人位姿开始,检查这几个位姿点是否有碰撞。为了检测是否发生碰撞,validation-model可能用于optimization的footprint更复杂

参数feasibility_check_no_poses不能太大,因为optimizer可能不完全收敛。小的障碍碰撞在未来可以获得纠正,所以设置太大导致的失败不一定是有效的。

狭窄环境中,调试要慎重。局部规划可能拒绝一个infeasible trajectory,但全局规划会认为全局路径是feasible,结果就是机器人stucked。 如果禁止了后退速度,会经常出现not feasible,尤其是在cluttered环境

源码中是这样调用的:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, 
robot_inscribed_radius_, robot_circumscribed_radius,
cfg_.trajectory.feasibility_check_no_poses);
if (!feasible)
{
cmd_vel.linear.x = 0; cmd_vel.linear.y = 0; cmd_vel.angular.z = 0;
// reset everything to start again with the initialization of new trajectories.
planner_->clearPlanner();
ROS_WARN("trajectory is not feasible. Resetting planner...");
++no_infeasible_plans_;
time_last_infeasible_plan_ = ros::Time::now();
last_cmd_ = cmd_vel;
return false;
}

如果用的是HomotopyClassPlanner,实际上还是对best TebOptimalPlanner验证轨迹可行性,即TebOptimalPlannerPtr best = findBestTeb();,实际用的是同名函数

  • costmapmodel: costmap model的指针, `costmap_model = boost::makeshared<
    base_local_planner::CostmapModel>(*costmap
    );`

  • footprintspec: 机器人世界坐标系的轮廓, `costmap_ros->getRobotFootprint();`

  • look_ahead_idx: 其实就是参数feasibility_check_no_poses,沿着trajectory需要验证的位姿点,
    如果是-1,那就验证整个trajectory。

这里的障碍物是以代价地图代表,而不是算法内部的ObstacleContainer

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
bool TebOptimalPlanner::isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model,
const std::vector<geometry_msgs::Point>& footprint_spec,
double inscribed_radius, double circumscribed_radius,
int look_ahead_idx )
{
if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses() )
look_ahead_idx = teb().sizePoses() - 1;

for (int i=0; i <= look_ahead_idx; ++i)
{
// 只考虑覆盖一个障碍的情况,撞了则直接 return
if ( costmap_model->footprintCost(teb().Pose(i).x(),
teb().Pose(i).y(),
teb().Pose(i).theta(),
footprint_spec, inscribed_radius, circumscribed_radius) == -1 )
{
if (visualization_)
visualization_->publishInfeasibleRobotPose(teb().Pose(i), *robot_model_);

return false;
}
// 检查两个位姿点的间距是否比机器人内接半径大;或者它们的偏航角差距大于
// 参数min_resolution_collision_check_angular
// 如果符合一种情况, 需要增加插值(临时位姿点)
// 如果两个位姿点被障碍物push away,那么它们的中心可能与障碍碰撞
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;
ROS_INFO("n_additional_samples: %d", n_additional_samples);
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;
}
}
}
}
}
return true;
}

实质上比较简单,判断路径feasible用的是base_local_planner::CostmapModelfootprintCost函数,位姿点间距大则增加插值。 详细参考文章CostmapModel 判断是否碰撞

有时会无法把footprint point转为map坐标系的坐标,因为它在局部代价地图之外。如果isTrajectoryFeasible函数检查的局部路径点太多,有的会超出局部代价地图,这样会一直返回false

小车撞障碍但又是 isTrajectoryFeasible

日志:

1
2
3
4
5
footprint cost in isTrajectoryFeasible: -1.000000
[17:17:26.883 - WARN] (): trajectory is not feasible. Resetting planner..
[17:17:26.940 - INFO] (): footprint cost in isTrajectoryFeasible: 0.00
[17:17:26.940 - INFO] (): footprint cost in isTrajectoryFeasible: 0.00
# 后面一直是 0

costmap_model->footprintCost如果是-1,就是撞了障碍,但只有第一次是-1,后面全是0. 撞一次就离开了障碍,自然是0. 之所以继续撞,是因为路径规划的开头,手动让机器人位姿先调整到transformed_plan的第一个元素的朝向,这二者没有关系。


(九) TimedElasticBand 类和优化之前的准备

本文是对上一篇的补充

TimedElasticBandtimed_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
81
bool 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;
}

  1. 设置起点并且固定(不允许被优化)。
  2. 设置到目标一条直线上平均间隔的点作为初始位姿。
  3. 设置初始的时间间隔序列。
  4. 设置终点并固定(不允许被优化)。

大致流程
注意: 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;
}
};

当机器人位置变化时,初始轨迹是删去已经走过的,保留后续的。

作用:

  1. 取当前位置为起点,删去已经走过的轨迹,并重新设定终点。
  2. 对于位姿与时间序列,删减去新起点之前的位姿和时间,也就是删除两个容器中旧的元素
  3. 将新的终点加入序列

(十) optimizeTEB 1 流程和 autoResize

optimizeTEB 的流程就是循环几次下列操作(多次图优化):

  1. teb_.autoResize() 在外循环里
  2. buildGraph函数;
  3. optimizeGraph 内循环就是g2o的optimize函数的参数
  4. computeCurrentCost函数(目前的图优化的花费,只在最后一次外循环时进行一次)
  5. clearGraph函数;
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::optimizeTEB( int  iterations_innerloop, 
int iterations_outerloop, bool compute_cost_afterwards,
double obst_cost_scale, double viapoint_cost_scale,
bool alternative_time_cost )
{
if (cfg_->optim.optimization_activate == false)
return false;

bool success = false;
optimized_ = false;
// ROS源码里是 1
double weight_multiplier = 2.0;
// TODO: we introduced the non-fast mode with the support of dynamic obstacles
// ( which leads to better results in terms of x-y-t homotopy planning )
// 但此模式尚未完全测试
// 目前保持legacy fast mode为默认
bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles; // false

for(int i=0; i<iterations_outerloop; ++i)
{
if (cfg_->trajectory.teb_autosize) // 设置为 true
{
teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis,
cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);
}
success = buildGraph(weight_multiplier);
if (!success)
{
clearGraph();
return false;
}
success = optimizeGraph(iterations_innerloop, false);
if (!success)
{
clearGraph();
return false;
}
optimized_ = true;
// compute cost vec only in the last iteration
if (compute_cost_afterwards && i==iterations_outerloop-1)
computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);

clearGraph();
// 根据目前配置,变成 4 了
weight_multiplier *= cfg_->optim.weight_adapt_factor;
}
return true;
}

optimization_activate

  1. 外部循环通过调用autoResize()来根据时间分辨率调整轨迹,应根据CPU的控制速率要求来定

  2. 内部循环调用optimizeGraph()进行优化,内部循环的时间经过实验2-6次足够

  3. 最后调用computeCurrentCost()计算轨迹花费,将图中所有边的error的平方和构成cost

autoResize

一般不要用快速模式,也就是要进行100次外循环

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
// iterate through all TEB states and add/remove states
void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis,
int min_samples, int max_samples, bool fast_mode)
{
bool modified = true;
// 确保不会get stuck in some oscillation, 因此最大100此循环
for (int rep = 0; rep < 100 && modified; ++rep)
{
modified = false;
// TimeDiff connects Point(i) with Point(i+1)
// TimeDiff return timediff_vec_.at(index)->dt();
for(int i=0; i < sizeTimeDiffs(); ++i)
{
if(TimeDiff(i) > dt_ref + dt_hysteresis && sizeTimeDiffs()<max_samples)
{
//ROS_DEBUG("teb_local_planner: autoResize() inserting new bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs() );
double newtime = 0.5 * TimeDiff(i);
TimeDiff(i) = newtime;
// 时间差太大,插入一个元素
insertPose(i+1, PoseSE2::average(Pose(i),Pose(i+1)) );
insertTimeDiff(i+1, newtime);

modified = true;
}
// only remove samples if size is larger than min_samples.
else if(TimeDiff(i) < dt_ref - dt_hysteresis && sizeTimeDiffs()>min_samples)
{
//ROS_DEBUG("teb_local_planner: autoResize() deleting bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs() );
if(i < ((int)sizeTimeDiffs()-1))
{
// 时间差太小,合并 i 到 i+1
TimeDiff(i+1) = TimeDiff(i+1) + TimeDiff(i);
deleteTimeDiff(i);
deletePose(i+1);
}
else
{ // last motion should be adjusted, shift time to the interval before
// 合并 i 到 i-1
TimeDiff(i-1) += TimeDiff(i);
deleteTimeDiff(i);
deletePose(i);
}

modified = true;
}
}
// 如果是快速模式则结束,也就是rep<100不起作用
if (fast_mode) break;
}
}

这里其实是动态对initTrajectoryToGoal中建立的弹性带进行调整,不能让位姿点和deltT太密集,也不能太稀疏。min_samplesmax_samples就是一组限制条件,dt_refdt_hysteresis是另一组


ROS自带的恢复行为

原理图

所有的recovery behaviors都必须实现nav_core::RecoveryBehavior定义的接口(纯虚函数)。目前有类ClearCostmapRecovery, MoveSlowAndClear, RotateRecovery RotateRecovery一般不用

每个恢复行为类主要实现两个函数,一个负责初始化,另一个负责执行恢复行为。

1
2
virtual void initialize()
virtual void runBehavior() = 0;

参数

恢复行为在move_base_params.yaml中定义,如果不定义,会使用默认的配置。MoveBase构造函数中的使用:

1
2
if(!loadRecoveryBehaviors(private_nh))
loadDefaultRecoveryBehaviors();

如果加载自己的恢复行为不成功,就加载系统默认的。 如果用默认参数,则是按顺序加载了①conservative_clear, ②rotate, ③aggressive_clear, ④rotate. ①/③都是ClearCostmapRecovery类,负责清理机器人一定范围外的costmap上的数据,区别在于③保留的范围更小,清理的更多。恢复行为是按照列表顺序调用的,当①②无效后,才会执行③,清理更多区域。 默认配置中,aggressive_reset清除的范围是4 * local_costmap/circumscribed_radius以外


我的配置是这样的:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
recovery_behaviors:
- name: 'conservative_reset'
type: 'clear_costmap_recovery/ClearCostmapRecovery'
- name: 'aggressive_reset'
type: 'clear_costmap_recovery/ClearCostmapRecovery'
conservative_reset:
reset_distance: 3.0
layer_names:
- obstacles

aggressive_reset:
reset_distance: 0.0
layer_names:
- obstacles

函数中的变量recovery_behaviors_插入的就是conservative_resetaggressive_reset

两个恢复行为都是ClearCostmapRecovery类,相关参数:

  • reset_distance (double, default: 3.0)指定距离机器人半径几米的区域之外的障碍在恢复代价地图为静态地图时候将被清除。
  • layer_names (std::vector, default:”obstacles”)哪一层的信息将会被清除


还有两个重要参数:

  • oscillation_distance (double, default: 0.5) 来回震荡的距离,超过一定的时间没有移动足够的距离就会进行recovery behavior。移动达到了这个距离,就会重置oscillation_timeout的计时器

  • oscillation_timeout (double, default: 0.0) 在执行恢复行为前允许的振荡时间,0代表无穷的时间

我的小车不允许有一些奇怪的行为,比如振荡。所以不用rotate recoveryoscillation_distance设置为0.05也是为了避免振荡

状态机

Movebase有三个状态:

  • PLANNING:全局规划中,尚未得到全局路径
  • CONTROLLING:全局规划完成,进入局部规划
  • CLEARING:恢复行为

当 ①全局规划失败 ②局部规划失败 ③机器人震荡时会进入到恢复行为。

三种情况对应的恢复行为起因

1
2
3
4
5
6
enum RecoveryTrigger
{
PLANNING_R, // 全局规划失败,即无法算出 plan
CONTROLLING_R, // 局部规划失败,从而无法得到速度命令
OSCILLATION_R // 机器人在不断抖动,长时间被困在一小片区域
};

恢复行为的状态机.png

日志:

1
2
3
4
5
6
7
move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
Clearing costmap to unstuck robot (3.000000m).

move_base_recovery,state machine is CLEARING, Executing behavior 1 of 2
Clearing costmap to unstuck robot (0.000000m).

Aborting because a valid plan could not be found. Even after executing all recovery behaviors

RotateRecovery::runBehavior()

只是在原地逆时针旋转180度,这样costmap就会自己更新,mark或者clear一点的值,有没有路是局部代价地图在转一圈过程中发现的。

这里的目标是转过180度,将当前角度与目标180的偏差记录在distleft中,并通过这个偏差产生角速度的控制量,具体为 `vel = sqrt(2 * acc_lim_th * distleft)` ,并且要确保这个速度在角速度阈值之内,发布这个角速度,并循环检测偏差是否满足tolerance范围,当符合时退出。

按默认情况,它先清理周围一定范围以外的costmap(障碍层),接下来重新执行规划,若不奏效,则旋转180度,再执行规划。交替两次后,已转过360度,若还是没能排除故障,则恢复行为失败,关闭Movebase规划器。

ClearCostmapRecovery::runBehavior()

它将距离机器人 一定距离的正方形区域外的 代价地图重置,理解不了看看参数reset_distance就明白了。 ClearCostmapRecovery自身不会让机器人运动

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
void ClearCostmapRecovery::runBehavior(){
// 省略:判断是否初始化,两个代价地图是否为空,否则return
// reset_distance_ 就是定义的参数
clear_costmap_log.warn("Clearing costmap to unstuck robot (%fm).", reset_distance_);
clear(global_costmap_);
clear(local_costmap_);
}

void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS* costmap)
{
std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();

tf::Stamped<tf::Pose> pose;
// 在代价地图的global坐标系中的坐标 x y
if(!costmap->getRobotPose(pose)){
ROS_ERROR("Cannot clear map because pose cannot be retrieved");
return;
}
double x = pose.getOrigin().x();
double y = pose.getOrigin().y();
// 遍历代价地图的每一层,转为costmap_2d::Layer,获取名称
for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp)
{
boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
std::string name = plugin->getName();
int slash = name.rfind('/');
if( slash != std::string::npos ){
name = name.substr(slash+1);
}
if(clearable_layers_.count(name)!=0){
boost::shared_ptr<costmap_2d::CostmapLayer> costmap;
costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin);
clearMap(costmap, x, y);
}
}
}

void ClearCostmapRecovery::clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap,
double pose_x, double pose_y)
{
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));

double start_point_x = pose_x - reset_distance_ / 2;
double start_point_y = pose_y - reset_distance_ / 2;
double end_point_x = start_point_x + reset_distance_;
double end_point_y = start_point_y + reset_distance_;

int start_x, start_y, end_x, end_y;
costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
// 获取字符格式的代价地图
unsigned char* grid = costmap->getCharMap();
for(int x=0; x<(int)costmap->getSizeInCellsX(); x++)
{
bool xrange = x>start_x && x<end_x;
for(int y=0; y<(int)costmap->getSizeInCellsY(); y++)
{
// 注意是不处理 reset_distance_正方形范围内的代价值
if(xrange && y>start_y && y<end_y)
continue;
int index = costmap->getIndex(x,y);
// 范围外的代价值重置为 NO_INFORMATION
if(grid[index]!=NO_INFORMATION){
grid[index] = NO_INFORMATION;
}
}
}
double ox = costmap->getOriginX(), oy = costmap->getOriginY();
double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();

costmap->addExtraBounds(ox, oy, ox + width, oy + height);
return;
}

MoveSlowAndClear

这个使用较少。 它实现了缓慢移动的恢复行为,清理 costmap后什么都不管,按照前进速度和转角速度走。从代码里可以看到,根据指定的距离,这是通过先清除全局 costmap 跟局部 costmap 一圈的 obstacle layer 的障碍,然后直接发指令实现的。由于只清除了 obstacle layer ,其实 static layer 的障碍还在,而且这里清不清除跟发指令关系不大,该撞上去的还是会撞的,相当于闭着眼睛往前走。

禁止所有恢复行为造成的问题

不如自己不定义,还会使用默认的参数,所以将下面代码去掉:

1
2
if(!loadRecoveryBehaviors(private_nh))
loadDefaultRecoveryBehaviors();

结果发现平时正常导航的情况居然出错了

1
2
3
4
found_legal is 0
[ERROR] Failed to get a plan.
[ INFO] Robot in controlling, but can't get new global plan
[ERROR] 17:51:42.074 (): Aborting because a valid plan could not be found. Even after executing all recovery behaviors

并不是每次的导航都会出错,这要看全局路径,全局路径部分的bool found_legal = planner_->calculatePotentials有时会返回0,也就是找不到全局路径,此时会触发恢复行为。所以如果全禁止,会直接 Abort

正常的日志是这样的:

1
2
3
4
5
6
7
8
9
10
Received move_base goal (x: 62.766, y: -1.474, yaw deg: -5, yaw rad: -0.09)
[ERROR] Failed to get a plan.
[ INFO] Robot in controlling, but can't get new global plan
[INFO] move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
[ WARN] Clearing costmap to unstuck robot (0.000000m).
[ERROR] Failed to get a plan.
[ INFO] Robot in controlling, but can't get new global plan
[ INFO] move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
[ WARN] Clearing costmap to unstuck robot (0.000000m).
[ INFO] 到达目标

也就是获取不到全局路径时,会很快执行一次恢复行为,clear costmap,然后又找到了路径,这个过程之快,几乎看不到。

参考: 导航recovery机制