cartographer 常用的类型|激光SLAMCartographer源码解读

Rigid3类型在rigid_transform.h，很有学习价值

1
2
3
4
5
Vector translation_;
Quaternion rotation_;
// using Vector = Eigen::Matrix<FloatType, 3, 1>;
// using Quaternion = Eigen::Quaternion<FloatType>;
// using AngleAxis = Eigen::AngleAxis<FloatType>;

Rigid3d类型实质是包含了一个3行1列的Eigen矩阵，一个Eigen::Quaternion，可以直接用 cout
值得注意的运算符：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// 参见 SLAM 十四讲
Rigid3 inverse() const
{
  const Quaternion rotation = rotation_.conjugate();
  const Vector translation = -(rotation * translation_);
  return Rigid3(translation, rotation);
}

template <typename FloatType>
Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
                            const Rigid3<FloatType>& rhs) {
  return Rigid3<FloatType>(
      lhs.rotation() * rhs.translation() + lhs.translation(),
      (lhs.rotation() * rhs.rotation()).normalized());
}

OptimizationProblem2D类的成员

struct NodeId，成员只有int trajectory_id; 和 int node_index;，后者是从0开始的

struct SubmapId，成员只有int trajectory_id; 和 int submap_index;，后者是从0开始的

class MapById是对std::map的一个封装，其实是模板<typename IdType, typename DataType>，前者可以是NodeId 或者 SubmapId

// Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
struct NodeSpec2D
{
  common::Time time;
  transform::Rigid2d local_pose_2d;
  transform::Rigid2d global_pose_2d;
  Eigen::Quaterniond gravity_alignment;
};

struct SubmapSpec2D
{
  transform::Rigid2d global_pose;
};
// optimization_problem_->submap_data()  就是 SubmapSpec2D
MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;

data_是类PoseGraph2D中的成员变量，类型 PoseGraphData

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
struct PoseGraphData
{
  /* struct InternalSubmapData
  {
  std::shared_ptr<const Submap> submap;
  SubmapState state = SubmapState::kNoConstraintSearch;

  // IDs of the nodes that were inserted into this map together with
  // constraints for them. They are not to be matched again when this submap
  // becomes 'kFinished'.
    std::set<NodeId>  node_ids;
  }; */

  // Submaps get assigned an ID and state as soon as they are seen, even
  // before they take part in the background computations.
  MapById<SubmapId, InternalSubmapData> submap_data;

  // Global submap poses currently used for displaying data.
  MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
  MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;

  // Data that are currently being shown.
  MapById<NodeId, TrajectoryNode> trajectory_nodes;

  // Global landmark poses with all observations.
  std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
      landmark_nodes;
  // How our various trajectories are related.
  TrajectoryConnectivityState trajectory_connectivity_state;
  int num_trajectory_nodes = 0;
  std::map<int, InternalTrajectoryState> trajectories_state;

  // Set of all initial trajectory poses.
  std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;

  std::vector<PoseGraphInterface::Constraint> constraints;
};

1
2
3
4
5
6
7
8
9
struct InternalSubmapData
{
  std::shared_ptr<const Submap> submap;
  SubmapState state = SubmapState::kNoConstraintSearch;
  // IDs of the nodes that were inserted into this map together with
  // constraints for them. They are not to be matched again when this submap
  // becomes 'kFinished'.
  std::set<NodeId> node_ids;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
struct Constraint {
  struct Pose {
    transform::Rigid3d zbar_ij;
    double translation_weight;
    double rotation_weight;
  };
  SubmapId submap_id;  // 'i' in the paper.
  NodeId node_id;      // 'j' in the paper.
  // Pose of the node 'j' relative to submap 'i'.
  Pose pose;
/*  Differentiates between intra-submap (where node 'j' was inserted into
  submap 'i') and inter-submap constraints (where node 'j' was not inserted
  into submap 'i').*/
  enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
class OptimizationProblem2D
    : public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
                                          transform::Rigid2d>
{
public:
    void Solve(
      const std::vector<Constraint>& constraints,
      const std::map<int, PoseGraphInterface::TrajectoryState>&
          trajectories_state,
      const std::map<std::string, LandmarkNode>& landmark_nodes) override;
    // 省略很多函数
private:
  optimization::proto::OptimizationProblemOptions  options_;
  MapById<NodeId, NodeSpec2D>  node_data_;
  MapById<SubmapId, SubmapSpec2D> submap_data_;
  sensor::MapByTime<sensor::ImuData> imu_data_;
  sensor::MapByTime<sensor::OdometryData> odometry_data_;
  std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;

  sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
  std::map<std::string, transform::Rigid3d> landmark_data_;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
// 结果、时间、函数
struct WorkItem
{
  enum class Result {
    kDoNotRunOptimization,
    kRunOptimization,
  };

  std::chrono::steady_clock::time_point time;
  std::function<Result()> task;
};

using WorkQueue = std::deque<WorkItem>;

ConstraintBuilder2D::Result 其实是 vector<Constraint>

PoseExtrapolator

1
2
3
std::deque<TimedPose> timed_pose_queue_;
std::deque<sensor::ImuData> imu_data_;
std::deque<sensor::OdometryData> odometry_data_;
cartographer里凡是变量名包含queue的，类型都是deque

插入子图

sensor::RangeData

local坐标系下的

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
// Rays begin at 'origin'. 'returns' are the points where obstructions were
// detected. 'misses' are points in the direction of rays for which no return
// was detected, and were inserted at a configured distance
// It is assumed that between the 'origin' and 'misses' is free space
struct RangeData {
  Eigen::Vector3f  origin;
  PointCloud  returns;
  PointCloud  misses;
};

using PointCloud = std::vector<RangefinderPoint>;
// Stores 3D position of a point observed by a rangefinder sensor.
struct RangefinderPoint
{
	Eigen::Vector3f position;
};

TimedRangeData

1
2
3
4
5
struct TimedPointCloudData {
  common::Time time;
  Eigen::Vector3f origin;
  TimedPointCloud ranges;
};
1
2
3
4
5
6
7
8
9
struct TimedPointCloudOriginData {
  struct RangeMeasurement {
    TimedRangefinderPoint point_time;
    size_t origin_index;
  };
  common::Time time;
  std::vector<Eigen::Vector3f> origins;
  std::vector<RangeMeasurement> ranges;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
// Like 'RangeData', but with 'TimedPointClouds'.
struct TimedRangeData {
  Eigen::Vector3f origin;
  TimedPointCloud returns;
  TimedPointCloud misses;
};

using TimedPointCloud = std::vector<TimedRangefinderPoint>;

struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time;
};
1
2
3
4
5
6
7
8
9
10
11
struct SubmapPose
{
	int version;
	transform::Rigid3d  pose;
};

struct SubmapData
{
	std::shared_ptr<const Submap> submap;
	transform::Rigid3d  pose;
};

LocalTrajectoryBuilder2D

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
struct MatchingResult
{
    common::Time time;    // 扫描匹配发生的时间
    // tracking 坐标系下的位姿(Local SLAM坐标系下的位姿), 需要乘以
    // tracking frame in map的转换(Local SLAM坐标系到Global SLAM坐标系的转换)才能转到全局坐标系
    transform::Rigid3d local_pose;
    // tracking 坐标系下的点云（即LocalSLAM坐标系下的点云）
    sensor::RangeData range_data_in_local;
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;
};

struct InsertionResult
{
    // Node数据   插入的节点数据，TrajectoryNode::Data包含了处理之后的点云数据
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    //该Node数据插入的两个submap
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
};

struct TrajectoryNode
{
  struct Data
  {
    common::Time  time;
    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond  gravity_alignment;
    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud  filtered_gravity_aligned_point_cloud;
    // 省略 3D.

    // The node pose in the local SLAM frame.
    transform::Rigid3d   local_pose;
  };
  common::Time time() const { return constant_data->time; }
  // This must be a shared_ptr. If the data is used for visualization while the
  // node is being trimmed, it must survive until all use finishes.
  std::shared_ptr<const Data> constant_data;
  // The node pose in the global SLAM frame.
  transform::Rigid3d  global_pose;
};

MatchingResult里有一个local_pose，InsertionResult的constant_data成员里也有一个local_pose， 这两个是相同的，赋值用的都是下一帧位姿的观测值。不过在后端里用的是InsertionResult的local_pose，源码在PoseGraph2D::AddNode

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// An individual submap, which has a 'local_pose' in the local map frame, 
// keeps track of how many range data were inserted into it
// and sets 'insertion_finished' when the map no longer changes 
// and is ready for loop closing
class Submap
{
public:
	Submap(const transform::Rigid3d& local_submap_pose)
	  : local_pose_(local_submap_pose) {}

private:
	// Pose of this submap in the local map frame
	const transform::Rigid3d  local_pose_;
	// Number of RangeData inserted.
	int num_range_data_ = 0;
	bool insertion_finished_ = false;
};

常用函数

1
2
3
4
5
6
7
8
9
10
11
12
// 对点云中每一个点做变换， 左乘 transform
PointCloud TransformPointCloud(const PointCloud&  point_cloud,
                       const transform::Rigid3f&  transform)
{
	PointCloud result;
	result.reserve(point_cloud.size()  );
	for (const RangefinderPoint& point : point_cloud)
	{
		result.emplace_back(transform * point);
	}
	return result;
}
1
2
3
4
5
6
7
8
9
RangeData TransformRangeData(const RangeData& range_data,
                             const transform::Rigid3f& transform)
{
  return RangeData{
      transform * range_data.origin,
      TransformPointCloud(range_data.returns, transform),
      TransformPointCloud(range_data.misses, transform),
  };
}

Trajectory

enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };

前端 2. 传感器数据的同步及融合-RangeDataCollator|激光SLAMCartographer源码解读
如果只用一个雷达，可以不看这段

激光雷达点云数据均为作为主要输入，使用时无需考虑具体几个传感器和类型，可认为是一个雷达产生的点云数据。但实际cartographer通过RangeDataCollator类将多种传感器进行了融合，并进行了时间同步，最后形成对应的时间戳，pose和点云集合。在真正使用时通过畸变校准，并构建scan match和插入submap的传感器数据rangedata。

有多个雷达的点云信息，它们的各个点可能时间会重合，因此需要将所有雷达的点云信息进行时间的整理，保证所有点的时间是单调的。多个雷达数据合并到一起，它们的原点可能不一样，因此要保存各自的原点。

头文件部分：

1
2
3
4
5
6
7
8
9
10
11
12
  // 插入集合
sensor::TimedPointCloudOriginData AddRangeData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data);

private:
  // 融合
  sensor::TimedPointCloudOriginData CropAndMerge();
  // 期望处理传感器类型清单
  const std::set<std::string> expected_sensor_ids_;
  // 同步和融合后集合，每种传感器至多一帧点云数据
  std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;

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
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& timed_point_cloud_data)
{
  // 检测该点云数据 sensor_id 是否在期望的sensor_ids里面
  CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
  // TODO(gaschler): These two cases can probably be one.
  // 此传感器类型数据已有
  if (id_to_pending_data_.count(sensor_id) != 0) 
  {
    current_start_ = current_end_;
    // When we have two messages of the same sensor, move forward the older of
    // the two (do not send out current).
    /* 如果当前融合时间段内融合结果中已包含同一ID的传感器数据，则应采用最新的点云数据进行替换
    但是结束的时间戳仍然采用原来的时间刻度，开始进行数据截取合并CropAndMerge()并返回 */
    // std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
    current_end_ = id_to_pending_data_.at(sensor_id).time;
    auto result = CropAndMerge();
    id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
    return result;
  }
  id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
  // 若现在收到的数据类型未全，即期望收到种类未全，直接退出，无需融合
  if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
    return {};
  }
  current_start_ = current_end_;
  // We have messages from all sensors, move forward to oldest.
  common::Time oldest_timestamp = common::Time::max();
  // 找传感器数据中最早的时间戳
  for (const auto& pair : id_to_pending_data_) {
    oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
  }
    /*  current_end_是下次融合的开始时间，是本次融合的最后时间刻度
  但其实是此次融合所有传感器中最早的时间戳*/
  current_end_ = oldest_timestamp;
  return CropAndMerge();
}

进一步处理在CropAndMerge，这个函数十分复杂。 简单理解如果有一个传感器频率较高，已经来过一帧并进行了缓存，另外一个未来，
这个传感器又来一帧，则先进行截取合并送出结果（实际上就是上帧缓存的点云直接发出），
然后将新来的一帧替换换来的buffer。

参考: cartographer 代码思想解读 RangeDataCollator

前端 4. Ceres scan matcher|激光SLAMCartographer源码解读

原理

在把一帧的scan插入到子图之前，scan 相对子图的位姿是由 ceres scan matcher 优化获得的。由于这是一个实时的局部优化，需要一个好的初始位姿估计。前面的real time CSM就是把位姿估计器传来的预测值更新为一个好的初值，如果没有real time CSM，就还用位姿估计器传来的预测值作初值。Ceres Scan Matcher以初值作为先验，并找到最佳的点，该点就是通过scan match获得的在子地图中的位置，实现方式是 interpolating the submap and sub-pixel aligning the scan. 前端的两个scan matcher其实都是 scan to map 问题，让当前观测和已知环境最匹配

scan matcher 负责在地图中找到一个scan pose，使得在该位姿下所有 hit 点在 local map中的占用概率之和最大，于是转化为一个最小二乘问题：

式中的 就是我们需要求的位姿，它将scan坐标系中的点 (hk)全部转化到 Submap坐标系下， 让点 (hk)最大概率地匹配到Submap。 由于栅格中原本存储的就是空闲的概率，其实从栅格查出来的就是 。 相对地，real time scan match是占据概率之和最大，其实二者本质差不多，只是CSM换成最小二乘形式了，为了更精确。

在代码里实现的时候，又加上了旋转和平移的残差，防止点云匹配的结果和初值差太多。其实只要初值够好，这两个残差可以去掉。

这种方式的速度很快，计算复杂度高，鲁棒性好，但不能修正比子图分辨率大很多的误差。如果传感器和时间戳是合理的，只使用Ceres Scan Matcher通常是最好的选择。

Msmooth

Msmooth是一个将local子图中的各点概率进行平滑处理的函数，能够提供比栅格分辨率更好的精度。实质是双三次插值算法，该函数的输出结果为(0, 1)以内的数，实际返回了Submap中的占用概率，如果趋近1，那是很好了。

提高Scan matcher的频率可以获得更好优化结果，但计算量加大；或者用IMU来估计 scan match 中的旋转角度

ceres匹配对初值要求相当高，匹配后的结果会考虑其与初始位置偏差进行权重化，说明 cartographer认为其匹配后的值应该与初值相差不大。 一旦初值离真实值过远，很容易陷入局部最优解，导致优化失败。这也是为什么ceres对线速度与角速度有限制。

1
2
3
4
5
6
7
8
9
auto pose_observation = absl::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(pose_prediction.translation(), 
          initial_ceres_pose,
         filtered_gravity_aligned_point_cloud,
         *matching_submap->grid(),    pose_observation.get(),
         &summary);
// 省略:  统计残差， 观测值 - 预测值
return  pose_observation;

Match 函数

CeresScanMatcher2D类主要函数只有Match：根据初值，将点云hit点和栅格对齐，返回观测值

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
/*
 input:
 1. 上一个 scan 的位姿 target_translation
 2. 经过 RealTimeCorrelativeScanMatcher2D 处理过的
     当前的 scan 的位姿的初始估计  initial_pose_estimate
 3. 当前 scan 点云（2D）  point_cloud
 4. local map 概率分布栅格图   grid

 output:  更新的位姿估计  pose_estimate
*/
void CeresScanMatcher2D::Match(
   const Eigen::Vector2d&   target_translation,
   const transform::Rigid2d&  initial_pose_estimate,
   const sensor::PointCloud&  point_cloud,
   const Grid2D&  grid,
   transform::Rigid2d* const  pose_estimate,
   ceres::Solver::Summary* const  summary) const
{
	// 优化的变量（3维）
  double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
                                   initial_pose_estimate.translation().y(),
                                   initial_pose_estimate.rotation().angle() };
  ceres::Problem  problem;
  CHECK_GT(options_.occupied_space_weight(), 0.);
    // 点云匹配残差块
  problem.AddResidualBlock(
      CreateOccupiedSpaceCostFunction2D(
          options_.occupied_space_weight() /
              std::sqrt(static_cast<double>(point_cloud.size())),
          point_cloud,   grid ),
      nullptr /* loss function */, ceres_pose_estimate );

    // 省略 GridType::TSDF
  CHECK_GT(options_.translation_weight(), 0.);
  // ceres 的套路都在函数里定义，很简单   AutoDiff
  problem.AddResidualBlock(
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.translation_weight(), target_translation),
      nullptr, ceres_pose_estimate);

  CHECK_GT(options_.rotation_weight(), 0.);
  // ceres 的套路都在函数里定义，很简单
  problem.AddResidualBlock(
      RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.rotation_weight(), ceres_pose_estimate[2]),
      nullptr, ceres_pose_estimate);
   // ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
  ceres::Solve(ceres_solver_options_, &problem, summary);

  *pose_estimate = transform::Rigid2d(
      {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}

ceres scan matcher 将上面的最小二乘问题分解成了三个代价函数：点云与栅格的匹配残差、平移的残差、角度的残差。其中，点云匹配的权重总和为1，平移的权重和旋转的权重都是手动给的。

三个对应的核函数都是 nullptr， ceres的linear_solver_type是DENSE_QR

TranslationDeltaCostFunctor2D

两个Cost Function的实现，就是保证平移和旋转最小的代价函数

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
// Computes the cost of translating 'pose' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor2D {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const double scaling_factor, const Eigen::Vector2d& target_translation)
  {
    return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor2D,
                             2 /* residuals */,
                             3 /* pose variables */>(
        new TranslationDeltaCostFunctor2D(scaling_factor, target_translation));
  }
  template <typename T>
  bool operator()(const T* const pose, T* residual) const {
    residual[0] = scaling_factor_ * (pose[0] - x_);
    residual[1] = scaling_factor_ * (pose[1] - y_);
    return true;
  }
 private:
  // Constructs a new TranslationDeltaCostFunctor2D  from the given
  // 'target_translation' (x, y).
  explicit TranslationDeltaCostFunctor2D(
      const double scaling_factor, const Eigen::Vector2d& target_translation)
      : scaling_factor_(scaling_factor),
        x_(target_translation.x()),
        y_(target_translation.y()) {}
  // 省略: 禁止拷贝构造函数 和 赋值运算符
  const double scaling_factor_;
  const double x_;
  const double y_;
};

这个代价函数比较简单，都是套路，RotationDeltaCostFunctor2D也是同理

  • 平移量的残差计算，当前位姿逼近目标位姿 (位姿估计器输出的预测值xy)。目标函数
    residual[0] = scaling_factor_ * (pose[0] - x_); residual[1] = scaling_factor_ * (pose[1] - y_);

  • 逼近目标角度，目标函数scaling_factor_ * (pose[2] - angle_)

  • 这里是逼近之前的位姿，所以要求先验位姿比较准确，经试验发现ceres输出的位姿和预测值相差极小，疑似存在问题，可以将这两个ceres优化去掉，但地图匹配必须保留

统计残差

观测值 - 预测值

1
2
3
4
5
6
7
8
9
10
11
12
13
if (pose_observation)
{
    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
    const double residual_distance =
        (pose_observation->translation() - pose_prediction.translation())
            .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance);

    const double residual_angle =
        std::abs(pose_observation->rotation().angle() -
                 pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle);
}

参考：
无处不在的小土
jiajiading的分析
叶落寒蝉的分析
Ceres的使用

前端 3. Real Time Correlative Scan Matcher|激光SLAMCartographer源码解读

AddAccumulatedRangeData

现在进入AddAccumulatedRangeData函数，返回类型为 std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>

所用参数为

  • const common::Time time     当前同步时间
  • const sensor::RangeData& gravity_aligned_range_data     经过重力修正后的点云returns和miss
  • const transform::Rigid3d& gravity_alignment,     重力向量
  • const absl::optional<common::Duration>& sensor_duration

先看开头，我交换了部分代码顺序，并不影响。这里实际是获得scanMatch的两个参数

1
2
3
4
5
6
7
8
9
10
11
if (gravity_aligned_range_data.returns.empty())
{
  LOG(WARNING) << "Dropped empty horizontal range data.";
  return nullptr;
}
const sensor::PointCloud&  filtered_gravity_aligned_point_cloud =
sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
          .Filter(gravity_aligned_range_data.returns);

if (filtered_gravity_aligned_point_cloud.empty())
    return nullptr;

这一段就是流程图的Adaptive Voxel Filter环节，解释参考(九) Voxel Filter和Adaptive Voxel Filter

然后是估计位姿部分

1
2
3
4
5
const transform::Rigid3d  non_gravity_aligned_pose_prediction =
  extrapolator_->ExtrapolatePose(time);
  // 经过重力方向计算投影后的2d位置
const transform::Rigid2d   pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse() );

返回的pose_prediction和上面的filtered_gravity_aligned_point_cloud一起进入 Scan Mathching 环节，这个在流程图上很明显。

表面上只是重力对齐以后再处理，其实这里面还为后续的匹配与查找减小了查找范围，因为* gravity_alignment.inverse()这一乘使得与上一帧数据的角度变化缩小了（基本是同一角度），所以 为后面的ScanMatch减小了角度的查找范围。

在后面再把角度乘回来：

1
2
3
4
5
pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

transform::Rigid3d pose_estimate =
     transform::Embed3D(*pose_estimate_2d) * gravity_alignment;

这样就实现了角度的估计，gravity_alignment上下是同一数值，只是为了减少计算量，点云数据也按这个gravity_alignment做了处理

scan match

1
2
3
4
5
6
7
8
9
// local map frame <--- gravity-aligned frame
// 求 Scan 插入Submap的最优 Pose
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
    ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
if (pose_estimate_2d == nullptr)
{
  LOG(WARNING) << "Scan matching failed.";
  return nullptr;
}

这里的ScanMatch包含了real time correlative scan matcher 和 ceres scan match两部分，前者为后者提供好的初值

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
// 输出是对该帧传感器数据的最优pose
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    const common::Time time,  const transform::Rigid2d&  pose_prediction,
    const sensor::PointCloud&  filtered_gravity_aligned_point_cloud)
{
  // 如果还没有子图，返回 PoseExtrapolator 的预测值
  if (active_submaps_.submaps().empty() )
    return absl::make_unique<transform::Rigid2d>(pose_prediction);

  // 永远是头部的子图做匹配   SCAN-to-MAP
  std::shared_ptr<const Submap2D> matching_submap =  
      active_submaps_.submaps().front();

  transform::Rigid2d initial_ceres_pose = pose_prediction;

  // 函数最终更新 initial_ceres_pose，返回匹配的最高分数
  if (options_.use_online_correlative_scan_matching())
  {
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction,   filtered_gravity_aligned_point_cloud,
        // Grid2D类型
        *matching_submap->grid(),  &initial_ceres_pose );
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }
  // ceres scan matcher 部分
}

Real Time Correlative Scan Matcher

这里借用了karto的做法，即本质是暴力匹配，而hector是用了高斯牛顿法和多分辨率地图。但是karto还是用了多分辨率地图，而cartographer这里没有。

它使用类似于在回环检测中激光与子地图匹配的方式，然后最好的匹配用作Ceres Scan Matcher的先验。这个scan matcher非常消耗资源，但它在特征丰富的环境中是很鲁棒。

我们要找到机器人位姿的后验概率分布，使用贝叶斯公式得到

运动模型是已知的多元高斯分布， 算法只研究观测模型。观测模型难以计算，一般具有多极值。 雷达观测数据 里包含了很多激光点数据，假设他们是完全独立的

将一帧观测数据的概率拆分成了当前帧的每一个点的概率，在计算这个概率之前，我们需要对数据帧以及地图进行栅格化。

位姿估计器传来的预测值不适合做初值，真正初值是在它附近。

实时相关性scan match 以预测值为中心，在搜索窗口内枚举出所有候选位姿，并生成每个位姿对应的点云，也就是说按照 把当前观测 投影到栅格地图 m (t 时刻的激光数据与 t-1 时刻的地图匹配， scan to map)，然后分别计算每个点云中hit点在栅格地图的CellIndex，并计算所有hit点的占据概率之和，概率最大值对应的位姿就是我们需要的。代表着在这个位姿下，激光点得映射到占据栅格中的可能性最大，当前观测与已知环境最为一致，前后帧匹配成功。 scan match非常重要，如果结果位姿很差，建图就会失败，后端怎么调整也没用

搜索的精度是栅格地图的分辨率

这个scan matcher可以修改做重定位，不过搜索窗口扩大很多

  1. 暴力搜索
    我们要计算 ，这就需要一个三层for循环 计算每一个位姿，还有第四个循环，也就是遍历点云中每个点。 计算量巨大，因为每一个位姿都要重新投影，需要计算sin和cos函数

  2. 计算2D切片
    把三层循环交换顺序，最外层遍历 ，这样(x, y)的循环只有加法。

  3. 多分辨率搜索
    首先选择一个相对低的分辨率对 map 和 scan 进行栅格化，每个栅格上保留高分辨率情况下的该区域的最高概率，这样在匹配时就不会错过正确的位姿估计
    低分辨率找到最佳匹配后再在该位置进行高分辨率的栅格化，并设定相对较小的 search window 进行计算。

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
// 更新 pose_estimate， 返回最高的分数
double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d&  initial_pose_estimate,
    const sensor::PointCloud&  point_cloud, 
    const Grid2D&  grid,
    transform::Rigid2d*  pose_estimate  ) const
{
  CHECK(pose_estimate != nullptr);
  // 预测位姿的旋转量
  const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
   /*  左乘 initial_rotation， 将当前点云转换为
  初始角度坐标系下点云，这里只有 旋转  */
  const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
      point_cloud,
      transform::Rigid3f::Rotation(Eigen::AngleAxisf(
          initial_rotation.cast<float>().angle(), 
          Eigen::Vector3f::UnitZ()  )   )
      );

  const SearchParameters search_parameters(
      options_.linear_search_window(),  options_.angular_search_window(),
      rotated_point_cloud,   grid.limits().resolution()  );
	// 获取搜索窗口下机器人朝向各个方向角时的点云数据

  // 先通过角度切片，然后在xy方向上暴力搜索，为了减少计算量
  // 切片数目 num_scans , 按照搜索角度/步长，以初始值为基础，生成一些列的旋转后的点云
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);

     // DiscreteScan2D 就是 std::vector<Eigen::Array2i>
     /* 把切片后的旋转点云坐标，左乘initial_pose_estimate.translation
       转化到地图坐标，这里只有 平移
    最终通过GetCellIndex返回一系列的  激光点return所在的cell index*/
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(),   rotated_scans,
      Eigen::Translation2f( initial_pose_estimate.translation().x(),
      initial_pose_estimate.translation().y()  )
      );
   /*  获得candidates, size是 search_parameters.num_scans * 
  (linear_bounds.max_x - inear_bounds.min_x) * (linear_bounds.max_y - inear_bounds.min_y)*/

    /*  这里是num_scans循环，x和y循环，没有 theta循环了
  成员是 scan_index, x_index_offset(逐步增加), y_index_offset(逐步增加), 
  search_parameters意思其实是 每个激光点附近的矩形区域*/
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);

  // 计算当前帧在theta、x、y三层窗口中，每个状态在grid地图中的匹配的评分
  // search_parameters 实际没用
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
  // 取最好的候选
  const Candidate2D&   best_candidate =
      *std::max_element(candidates.begin(), candidates.end());

  // 最后的结果是预测值加候选位姿
  *pose_estimate = transform::Rigid2d(
      {initial_pose_estimate.translation().x() + best_candidate.x,
       initial_pose_estimate.translation().y() + best_candidate.y },
       // 前端观测，所以右乘
      initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation) );

  return best_candidate.score;
}
  1. 获取位姿估计器传来的初值的旋转分量，将激光点云左乘旋转分量，即先做旋转变换
  2. 根据搜索窗口x、y、yaw的大小计算 num_scans和LinearBounds， x y搜索的num_linear_perturbations
  3. 切片旋转：GenerateRotatedScans函数，其实就是theta循环，将角度进行序列化(初始角度跟search_window的大小相关，按步长增加)，点云依次左乘角度对应的旋转向量(旋转轴为z轴)，即可生成num_scans段点云
  4. 把点云中所有点做变换：左乘初值的平移部分，根据变换后的坐标，找出它在栅格地图中的CellIndex并保存
  5. 根据搜索范围，生成候选者，数量是25*num_scans
  • 根据保存的索引队列，从栅格地图中获取最大置信度，这就用到了ProbabilityGrid类，根据每个点的CellIndex获取占据概率，最终对分数进行加权处理
  • 求得每个候选位置的score，score最高的为最佳位置。

候选者是针对scan_index (0~num_scans)所做的x y方向的offset，生成所有候选者的过程其实就是num_scans和x y三个循环

Match 函数的计算量很大 。 试验发现最终的分数，大部分在0.6~0.9

SearchParameters

根据lua参数，确定后面的切片步长angular_perturbation_step_size，切片数num_scans，搜索区域

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
SearchParameters::SearchParameters(
               const double linear_search_window,
               const double angular_search_window,
               const sensor::PointCloud& point_cloud,
               const double resolution)
    : resolution(resolution)
{
  // We set this value to something on the order of 
  // resolution to make sure that the std::acos() below is defined.
  float max_scan_range = 3.f * resolution;
  for (const sensor::RangefinderPoint& point : point_cloud) {
    const float range = point.position.head<2>().norm();
    max_scan_range = std::max(range, max_scan_range);
  }
  const double kSafetyMargin = 1. - 1e-3;
  angular_perturbation_step_size =
      kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
                                         (2. * common::Pow2(max_scan_range)));
  // 这两者的计算不同
  num_angular_perturbations =
      std::ceil(angular_search_window / angular_perturbation_step_size);
  const int num_linear_perturbations =
      std::ceil(linear_search_window / resolution);
  // 切片数目，由于resolution不修改，实际取决于 max_scan_range 和 angular_search_window
  num_scans = 2 * num_angular_perturbations + 1;
  /*  Linear search window in pixel offsets; bounds are inclusive.
  struct LinearBounds {
    int min_x;
    int max_x;
    int min_y;
    int max_y;
  };
  std::vector<LinearBounds> linear_bounds;   Per rotated scans  */
  linear_bounds.reserve(num_scans);
  // 对同一帧scan, linear_bounds的元素都相同，不同帧就不同了
  for (int i = 0; i != num_scans; ++i) {
    linear_bounds.push_back(
        LinearBounds{-num_linear_perturbations, num_linear_perturbations,
                     -num_linear_perturbations, num_linear_perturbations} );
  }
}

GenerateRotatedScans 切片旋转

生成序列化旋转的多段点云

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
std::vector<sensor::PointCloud> GenerateRotatedScans(
    const sensor::PointCloud&  point_cloud,
    const SearchParameters&  search_parameters)
{
  std::vector<sensor::PointCloud> rotated_scans;
  rotated_scans.reserve(search_parameters.num_scans);
  // 旋转角度步长
  double delta_theta = -search_parameters.num_angular_perturbations *
                       search_parameters.angular_perturbation_step_size;
  //  个数 num_scans
  for (int scan_index = 0; scan_index < search_parameters.num_scans;
       ++scan_index,
       delta_theta += search_parameters.angular_perturbation_step_size)
  {
    // 左乘旋转向量，旋转轴为z轴
    rotated_scans.push_back(sensor::TransformPointCloud(
        point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()) ) )  );
  }
  return rotated_scans;
}

ScoreCandidates

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
void ScoreCandidates(
    const Grid2D& grid,  const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters&  search_parameters,
    std::vector<Candidate2D>*  const candidates) const
{
  for (Candidate2D& candidate : *candidates)
  {
    candidate.score = ComputeCandidateScore(
        static_cast<const ProbabilityGrid&>(grid),
        discrete_scans[candidate.scan_index],
        candidate.x_index_offset,
        candidate.y_index_offset);
    break;
      // 省略 GridType::TSDF
      // 进一步做权重处理
    candidate.score *=
        std::exp(
          -common::Pow2(
            std::hypot(candidate.x, candidate.y) *
            options_.translation_delta_cost_weight() 
                        +
            std::abs(candidate.orientation) *
            options_.rotation_delta_cost_weight()
                       )
          );
  }
}


// 其中用到的函数， 计算每个候选的得分
float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
             const DiscreteScan2D&  discrete_scan,
             int x_index_offset,  int y_index_offset)
{
  float candidate_score = 0.f;
  // 这里就用到前面获取的x y 索引队列
  for (const Eigen::Array2i& xy_index : discrete_scan)
  {
    const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
                                           xy_index.y() + y_index_offset  );
    // Returns probability of the cell with  'cell_index'
    // 这个就是占据概率
    const float probability =
        probability_grid.GetProbability(proposed_xy_index);
    candidate_score += probability;
  }
  // 返回的是一帧点云所有激光点得分的平均值
  candidate_score /= static_cast<float>(discrete_scan.size());
  CHECK_GT(candidate_score, 0.f);
  return candidate_score;
}

参考： RealTimeCorrelativeScanMatcher2D

机器人到障碍物的距离 calculateDistance|路径规划TEB算法

在TEB中的footprint模型 中提到calculateDistance函数计算机器人到障碍物的 Euclidean 距离，用于图优化环节。

以最简单的Point类型为例

1
2
3
4
5
virtual double calculateDistance(const PoseSE2& current_pose, 
    const Obstacle* obstacle) const
{
	return obstacle->getMinimumDistance(current_pose.position());
}

Obstacle是个抽象类，它的派生类有
1
2
3
4
5
PointObstacle
CircularObstacle
PillObstacle
LineObstacle
PolygonObstacle

显然不同的障碍类型对函数有不同实现，这里又涉及到costmap_converter和Homotopy，这就更复杂了

createGraph() —— PointObstacle::checkLineIntersection —— PointObstacle::checkCollision

TEB中的footprint模型|路径规划TEB算法

TEB算法中的footprint模型是单独构建的，而没有直接调用通用代价地图中的 footprint，二者footprint模型不同，但在TEB中二者都有使用。

TEB的footprint用于图优化，costmap的 footprint用于 `feasibility check`环节

LayeredCostmap类的成员函数： double getInscribedRadius() { return inscribed_radius_; }

对于polygon轮廓，两种footprint模型的内接半径相同， TEB的模型没有返回外接半径的函数

在TebLocalPlannerROS::initialize中定义: RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh);，然后进行模型验证validateFootprints

在planner初始化时，也就是在TebOptimalPlanner::initialize中赋值给robot_model_.

RobotFootprintModelPtr 其实就是 BaseRobotFootprintModel类型的 boost::shared_ptr

很显然BaseRobotFootprintModel是个抽象类，定义了机器人footprint/contour models的接口，根据yaml设置的类型而定义相应的类，这个类只用于optimization，代价地图的footprint只用于检查路径的可行性

  • Point 和 circular模型的footprints要求算力最少。

  • two-circles模型要求的是两次 distance calculations

  • line模型增加了一些 simple case checking, 但是算力在大多数情况仍要求不高。

  • polygon模型对每一个edge都要求 distance check，所以计算时间主要取决于 edges和vertices 的数量

也就是说这几种模型的算力要求依次增大。

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
RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh)
{
  std::string model_name; 
  if (!nh.getParam("footprint_model/type", model_name))
  {
	// 如果未指定类型，则使用点类型
    return boost::make_shared<PointRobotFootprint>();
  }
  // point  
  if (model_name.compare("point") == 0)
  {
      ROS_INFO("Footprint model 'point' loaded for trajectory optimization.");
      return  boost::make_shared<PointRobotFootprint>();
  }
  // circular   省略
  if (model_name.compare("circular") == 0)
  // line  省略
  if (model_name.compare("line") == 0)
  // two circles 省略
  if (model_name.compare("two_circles") == 0)

  // polygon  只对 Point2dContainer vertices_; 赋值
  if (model_name.compare("polygon") == 0)
  {
    // check parameters
    XmlRpc::XmlRpcValue footprint_xmlrpc;
    if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc) )
    {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for 
      	trajectory optimization, since param '" << nh.getNamespace() << 
      	"/footprint_model/vertices' does not exist. Using point-model instead");
        return boost::make_shared<PointRobotFootprint>();
    }
    // get vertices
    if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
      try
      {
        Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, 
        	"/footprint_model/vertices");
        ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization.");
        return boost::make_shared<PolygonRobotFootprint>(polygon);
      } 
      catch(const std::exception& ex)
      {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: "
        	<< ex.what() << ". Using point-model instead.");
        return boost::make_shared<PointRobotFootprint>();
      }
    }
    else
    {
      ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, 
      	since param '" << nh.getNamespace() << "/footprint_model/vertices' does not 
      	define an array of coordinates. Using point-model instead.");
      return boost::make_shared<PointRobotFootprint>();
    }
  }
  return boost::make_shared<PointRobotFootprint>();
}

validateFootprints

1
2
3
4
5
6
7
// TebLocalPlannerROS::initialize中的调用
validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, 
          cfg_.obstacles.min_obstacle_dist);

void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, 
    double costmap_inscribed_radius, double min_obst_dist)
{}

函数就一句ROS_WARN_COND，要求 TEB优化所用的机器人内接半径 + 参数 min_obstacle_dist > costmap中的内接半径(包括footprint_padding) ，否则Infeasible optimziation results会频繁出现

这里就是设置为点类型的缺陷之处，第1参数个会是0，但是如果min_obstacle_dist超过costmap中的内接半径，会无法走窄通道。


BaseRobotFootprintModel的派生类实现了calculateDistance函数，计算机器人到障碍物的 Euclidean距离，用于图优化环节。以最简单的Point类型为例

1
2
3
4
5
virtual double calculateDistance(const PoseSE2& current_pose, 
    const Obstacle* obstacle) const
{
	return obstacle->getMinimumDistance(current_pose.position());
}

至于细节，需要单独一篇文章分析

变量robot_model_在之后用于图优化时的一系列函数：
AddEdgesObstacles
AddEdgesObstaclesLegacy
AddEdgesDynamicObstacles
AddEdgesVelocityObstacleRatio，也就是所有和障碍相关的约束

以及可视化中的publishInfeasibleRobotPose

(二) costmap converter基本机制|路径规划TEB算法costmap converter

配置使用的是costmap_converter::CostmapToLinesDBSRANSAC，类继承关系:

1
class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons

这个机制太复杂了，还没有搞清楚

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 workerCallback(const ros::TimerEvent&)
{
  updateCostmap2D();
  compute();
}


void CostmapToPolygonsDBSMCCH::updateCostmap2D()
{
	occupied_cells_.clear();
	if (!costmap_->getMutex())
	{
		ROS_ERROR("Cannot update costmap since the mutex pointer is null");
		return;
	}
	int idx = 0;
	costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
	    
	// get indices of obstacle cells
	for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
	{
	for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
	{
	  int value = costmap_->getCost(i,j);
	  if(value >= costmap_2d::LETHAL_OBSTACLE)
	  {
	    double x, y;
	    costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
	    occupied_cells_.push_back( KeyPoint( x, y ) );
	  }
	  ++idx;
	}
	}
}

(一) TEB中的costmap converter源码部分|路径规划TEB算法costmap converter

TEB默认情况下不使用Costmap Converter。事实上，此插件可以在复杂场景下极大提高运算效率，尤其是处理激光雷达分散的测量数据时，因为将障碍物视为系列孤立点效率极低。

使用costmap_converter转换障碍，结果Rviz没有显示出来，同时报警

首先确认安装sudo apt-get install -y ros-visualization-msgs，一般安装后就行了

初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
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();
}

配置使用的是costmap_converter::CostmapToLinesDBSRANSAC

computeVelocityCommands 函数中的调用

1
2
3
4
if (costmap_converter_)
  updateObstacleContainerWithCostmapConverter();
else
  updateObstacleContainerWithCostmap();
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
void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter()
{
  if (!costmap_converter_)
    return;
    
  //Get obstacles from costmap converter
  costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles();
  if (!obstacles)
    return;

  for (std::size_t i=0; i<obstacles->obstacles.size(); ++i)
  {
    const costmap_converter::ObstacleMsg* obstacle = &obstacles->obstacles.at(i);
    const geometry_msgs::Polygon* polygon = &obstacle->polygon;

    if (polygon->points.size()==1 && obstacle->radius > 0) // Circle
    {
      obstacles_.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius)));
    }
    else if (polygon->points.size()==1) // Point
    {
      obstacles_.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y)));
    }
    else if (polygon->points.size()==2) // Line
    {
      obstacles_.push_back(ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y,
                                                        polygon->points[1].x, polygon->points[1].y )));
    }
    else if (polygon->points.size()>2) // Real polygon
    {
        PolygonObstacle* polyobst = new PolygonObstacle;
        for (std::size_t j=0; j<polygon->points.size(); ++j)
        {
            polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y);
        }
        polyobst->finalizePolygon();
        obstacles_.push_back(ObstaclePtr(polyobst));
    }

    // Set velocity, if obstacle is moving
    if(!obstacles_.empty())
      obstacles_.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation);
  }
}
TEB所有参数含义|路径规划TEB算法

局部代价地图更新频率应当不低于planner和costmap_conveter的转换频率

话题

  • global_plan (nav_msgs/Path)

  • local_plan (nav_msgs/Path) 局部路径，主要是可视化目的

  • teb_poses (geometry_msgs/PoseArray) 当前本地计划的离散姿态列表(SE2)，主要用于可视化目的， 个数是 teb.sizePoses() ，每个位姿点其实就是teb.Pose(i)

这两个话题本质都是一样的，发布在TebVisualization::publishLocalPlanAndPoses

  • teb_markers (visualization_msgs/Marker) teb_local_planner通过具有不同名称空间的标记来提供规划场景的其他信息。 Namespaces PointObstacles and PolyObstacles: visualize all point and polygon obstacles that are currently considered during optimization. Namespace TebContainer: Visualize all found and optimized trajectories that rest in alternative topologies (only if parallel planning is enabled). Some more information is published such as the optimization footprint model

话题teb_markers_array 和 eb_markers是绑定的，不过不必关注。

话题的发布在TebVisualization::publishRobotFootprintModel

  • teb_feedback (teb_local_planner/FeedbackMsg)
    反馈消息包含计划的轨迹，包括速度轮廓和时间信息以及障碍物列表。主要用于评估和调试。必须启用参数publish_feedback

话题的发布在TebVisualization::publishFeedbackMessage

参数

某些参数不会加载到参数服务器，需要用户在yaml里设置，这是源码决定的

速度

  • max_vel_x: 1.0

  • max_vel_x_backwards: 0.5 最大倒车速度。将此速度设置为0或者负数将导致错误。禁止倒车应在penalty部分将前向行驶的权重设置得极高。 参阅优化参数weight_kinematics_forward_drive

  • acc_lim_x: 0.5 最大线加速度，m²/s

  • max_vel_theta: 1 最大转向角速度，作用在车辆高速行驶时，防止高重心转弯过快翻车

  • acc_lim_theta: 0.3 角加速度限制可以避免车轮打滑，车辆失控。对于低速平稳运行的车辆可以不约束此两项，将二者设为一个足够大的值即可

  • max_vel_y: 0.2

  • acc_lim_y: 0.5

以上几个最大值约束会用于saturateVelocity函数

以下参数仅适用于汽车型机器人

  • min_turning_radius: 0.0 最小转弯半径，对于差动机器人为0，代表可以原地转弯。源码在TebLocalPlannerROS::convertTransRotVelToSteeringAngle， 转弯半径应在低速时生效. 把这个值稍微调大一点，可以防止车采用过分的差速转向策略，而导致速度非常缓慢。 转向半径肯定在一米以内，可以按需设置，转向半径如果大了，U形弯就会外道入弯，能比较轻松过弯，不过小弯道的时候也会贴外侧，这样会比较浪费时间，但是如果小了，小弯会贴内，节省时间，但是U形弯就会内侧入弯，那就有可能过不了弯了。
    这个参数让TEB更适合阿克曼模型。为避免规划出不可能实现的移动路径，请动手测量实际车辆的转弯半径。此参数事实上约束了舵机的最大转角。有些车辆转向性能不佳，前轮实际转过角度小于舵机角度，则应当给指令转角乘上一增益后再控制舵机，否则车辆将总是不能实现设置的最小转弯半径。请注意此项应当与最大角速度配合设置。

  • cmd_angle_instead_rotvel: false # not used, is differential 是否将收到的角速度消息转换为操作上的角度变化。设置成True时，话题cmd_vel.angular.z内的数据是舵机角度。

cmd_angle_instead_rotvel 和 min_turning_radius不能同时为0，否则报警 You are mixing a carlike and a diffdrive robot

  • wheelbase: 默认0.0，目前是1，只有在cmd_angle_instead_rotvel为true时才有效

轮廓

  • footprint_model: 可用的有 point, circular, two_circles, line, polygon. point是最简单的，所需的计算时间最少，但准确度也最低；polygon多边形模型最复杂，完全考虑到机器人的轮廓形状，计算准确度最高。circular类型把机器人建模为一个简单的圆，半径为/footprint_model/radius，计算距离和point相似，不同点是每次调用函数时机器人半径会被加入到参数min_obstacle_dist。对圆形轮廓，可以因此选择point类型，然后将半径加到最小障碍物距离来替换，减小计算量

  • is_footprint_dynamic: 目前False，是否footprint为动态的. If true, 在检查trajectory feasibility之前更新footprint

GoalTolerance

  • xy_goal_tolerance: 0.1
  • yaw_goal_tolerance: 0.05
  • free_goal_vel: False 自由目标速度。设为False时，车辆到达终点时的目标速度为0。缺少目标速度约束将导致车辆“全速冲线”，即使前方有一堵墙也是如此 (因为撞墙的时刻不在规划器考虑范围内了)

Trajectory

  • teb_autosize: True 优化期间允许改变轨迹的时域长度，也就是改变 dt_ref

  • dt_ref: 0.3 局部路径规划的解析度; Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)。 TEB通过状态搜索树寻找最优路径，而dt_ref则是最优路径上的两个相邻姿态（即位置、速度、航向信息，可通过TEB可视化在rivz中看到）的默认距离。 此距离不固定，规划器自动根据速度大小调整这一距离，速度越大，相邻距离自然越大，较小的值理论上可提供更高精度。
    从autoResize函数可以看到，当相邻时间差距离(TimeDiff(i) )和dt_ref的差超过正负dt_hysteresis时，规划器将改变这一距离。 增大到0.4，teb_pose个数大大减少，车速明显加快，多数接近最大速度

  • dt_hysteresis: 0.1 允许改变的时域解析度的浮动范围， 一般为 dt_ref 的 10% 左右

  • min_samples: 3，必须大于2。 源码在initTrajectoryToGoal和updateAndPruneTEB以及autoResize

  • max_samples: 500. 源码在optimizeTEB中的autoResize

  • allow_init_with_backwards_motion: 默认true，设置为false. 只影响初始化，而不是路径优化的结果。 源码在initTrajectoryToGoal，只有使用的TebOptimalPlanner::plan第一个参数是initial_plan才有效

  • force_reinit_new_goal_dist: 1.0 源码在TebOptimalPlanner::plan的热启动阶段，判断局部目标(transformed_plan最后一个元素)与teb_poses最后一个元素之差的范数是否达到阈值，其实是判断局部目标是否太远了。如果小于阈值则调用updateAndPruneTEB

  • global_plan_overwrite_orientation: True 覆盖全局路径中的临时局部路径点的朝向，有些全局规划器不考虑局部子目标的朝向，因此在这里校正。将未来一段的规划的姿态角的平均值作为估计的局部子目标的姿态。源码estimateLocalGoalOrientation和initTrajectoryToGoal

  • global_plan_viapoint_sep: 0.3 # 正数才启用。调整全局路径上选取的航迹点的间隔，应根据机器人的尺寸大小调整.

  • max_global_plan_lookahead_dist: 1.5 最大向前看距离. 指定要进行优化的全局计划子集的最大长度 (累积欧几里得距离) 。然后，实际长度由本地成本图大小和此最大界限的逻辑结合确定。设置为零或负数以停用此限制。 此距离 1.应随车辆最大速度的增大而增大 2.不应超过激光雷达等传感器的可靠测量范围 3. 不应超过局部代价地图的大小，即不能要求TEB对局部耗费地图以外的部分进行规划。

在TebLocalPlannerROS::transformGlobalPlan()函数中被使用，决定局部规划初始轨迹的最大长度，实际调试发现此参数无需过大，因为局部轨迹在每个控制周期都被更新，实际执行的指令仅是轨迹上第一个点的速度值，这里设置为1.5即可， 过长也可能导致优化结果无法有效收敛

  • global_plan_prune_distance: 0.6 在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用，因为全局路径是从全局起始点到全局目标点的一条轨迹，而初始的局部路径仅是从机器人当前位置到局部目标点的一小段路径，全局路径裁剪其中一部分即局部路径，该参数决定了从机器人当前位置的后面一定距离开始裁剪

  • force_reinit_new_goal_angular: 0.78 原理同上，不过是角度，弧度制

    1
    2
    3
    4
    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 )

  • feasibility_check_no_poses: 3 在判断生成的轨迹是否冲突时使用，此时设置为3，即从轨迹起点开始逐个检查轨迹上的3个点，若3个点均不发生碰撞，则认为本次轨迹有效。若小于0则检查所有路径点。由于teb优化并非硬约束，这里相当于是轨迹生成之后的一层保障， 这个参数根据机器人的速度和环境复杂程度调整，否则极有可能出现在狭窄环境中走走停停的情况，甚至可以减小到1或0 源码在 isTrajectoryFeasible。 如果isTrajectoryFeasible函数检查的局部路径点太多，有的会超出局部代价地图，结果函数一直返回false

  • min_resolution_collision_check_angular: 默认不加载，需要用户赋值，比如 3.14159265。代价地图的检查碰撞所用的最小角分辨率。如果未达到要求，将增加 intermediate samples. 源码在TebOptimalPlanner::isTrajectoryFeasible

  • control_look_ahead_poses 非默认参数，需要用户赋值，比如 2。 ROS Wiki对这个参数的解释其实是min_resolution_collision_check_angular

  • exact_arc_length: false. 一般用true. 如果为true，则在速度，加速度和转弯速率计算中使用精确的弧长(计算三角函数，会增加cpu负担)，否则使用两个位姿点的直线距离做近似。

可视化

  • publish_feedback: false. 发布包含完整轨迹和活动障碍物的反馈，在可视化部分，对应TebOptimalPlanner::visualize()

  • visualize_with_time_as_z_axis_scale: 局部路径的z坐标赋值，源码在publishLocalPlanAndPoses， TebVisualization::publishObstacles

恢复措施

恢复措施可以尝试将快要撞到障碍或在空隙间不走的机器人或路径规划错误的机器人恢复至正常状态。TEB实现了由move_base规定的振荡恢复方法。 但是，实测中控制器将高频率振荡速度指令(>10Hz)，用户应当评估自己的电动机能否承受。 (可在 move_base 配置中关闭)

  • 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

  • oscillation_recovery: 默认true. Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards

  • oscillation_recovery_min_duration: 10 在这个时间内，是否再次发生FailureDetector检测的振荡，调用在configureBackupModes中

在小车遇到空隙不向前走时，容易出现下面的信息，然后小车来回振荡，有时能导航成功

1
2
3
4
5
[ WARN] Clearing costmap to unstuck robot (3.00 m).
[ WARN] TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] Rotate recovery behavior started.   # 开始振荡  RotateRecovery::runBehavior()
[ INFO] TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).

  • oscillation_filter_duration: 10 failure_detector_中buffer容器的大小是oscillation_filter_duration * controller_frequency)

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

  • oscillation_omega_eps: (0,1)内的 normalized 角速度的平均值，默认0.1

Obstacles

  • min_obstacle_dist: 0.05 最小障碍物距离，用于限制机器人与障碍物的最小距离，默认0.5。若设置了车辆的轮廓，则不建议使用此参数。使用车辆轮廓配置footprint_model配合膨胀距离即可实现较好效果。障碍物惩罚权重很高，TEB不会违背这一最小距离约束，因此会把可以通过的缝隙视作不能通过，并终止运行。
    增大min_obstacle_dist可以防止机器人离墙太近，但是对于狭窄通道的情况，反而应该设置的很小。 可以增大机器人轮廓，但减小min_obs_dist，提高防撞性，又保证过窄通道

实际还配合obstacle_association_force_inclusion_factor 和 obstacle_association_cutoff_factor两个参数生效， 参考TebOptimalPlanner::AddEdgesObstacles()函数

  • obstacle_association_force_inclusion_factor: 10.0 the obstacles that will be taken into account are those closer than min_obstacle_dist * factor, if legacy is false

  • obstacle_association_cutoff_factor: 40.0 the obstacles that are further than min_obstacle_dist * factor will not be taken into account, if legacy is false

距离小于min_obstacle_dist * obstacle_association_force_inclusion_factor值的障碍物点，被强制加入优化框架中，距离大于min_obstacle_dist * obstacle_association_cutoff_factor的障碍物点被直接抛弃不再考虑，然后在剩余的障碍物点中计算机器人左侧最小距离和右侧最小距离。这三个参数的设置非常重要，需要根据机器人的外形尺寸小心调整，否则极易出现狭窄空间机器人无法通过或优化不收敛的情况。

  • inflation_dist: 默认0.0 障碍物膨胀距离。这个值必须大于min_obstacle_dist才有效。源码在AddEdgesObstacles. 此膨胀只是降低通过这些区域的优先级，不应当用此距离迫使车辆远离障碍物。障碍物周边的缓冲区，零惩罚代价，缓冲区会导致规划器减速，源码在AddEdgesObstacles，另外参考weight_inflation

  • include_costmap_obstacles: True 这个不是True就不用干了

  • costmap_obstacles_behind_robot_dist: 1.0 # distance at which obstacles behind the robot are taken into account

  • legacy_obstacle_association: false 源码在buildGraph

  • obstacle_poses_affected: 25 对于每一个障碍物（点、线、多边形），距其最近的poses都会被定位出来。参数obstacle_poses_affected个相邻的最近poses都会被考虑进来。略微影响障碍物周围轨迹的平滑度。 如果legacy_obstacle_association是false，则不启用

costmap_converter

  • costmap_converter_plugin: 可取值为costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::CostmapToPolygonsDBSConcaveHull, “” 空字符串表示不启用

目前所用为CostmapToLinesDBSRANSAC"，头文件costmap_to_lines_ransac.h，最终的基类是BaseCostmapToPolygons

决定是否使用costmap_converter插件，原始costmap中障碍物全部以点来表示，计算机器人到障碍物的距离实际需要计算机器人到每一个障碍物点的距离，当环境非常复杂时计算代价会非常大。costmap_converter插件的作用是将障碍物预先表示成线段或多边形的形式，可以在一定程度上减轻后续计算距离的压力，尤其是处理激光雷达分散的测量数据时。因为将障碍物视为系列孤立点效率极低。 “costmap_converter::CostmapToPolygonsDBSMCCH”是一个较为精确的方法，它将环境转换为非凸多边形；在将障碍物距离加入g2o优化框架中 (障碍物距离是目标函数之一，描述为超图的边)

  • costmap_converter_spin_thread: True
  • costmap_converter_rate: 10

Optimization

除了撞击障碍物不被允许外，其余的约束在没有可行方案时可被打破（如规划出事实上不可行的转弯半径）

  • no_inner_iterations: 5 图优化optimizer的迭代次数

  • no_outer_iterations: 4 每次外部循环迭代都会根据所需的时间分辨率dt_ref自动调整轨迹的大小，并调用内部优化器

  • penalty_epsilon 0.1 一次性改变所有的惩罚项，为惩罚函数增加一个小的安全余量，以实现硬约束近似. 例如为速度的约束提供一个缓冲的效果，在到达速度限制前会产生一定的惩罚，让其提前减速达到缓冲的效果
    penalty_epsilon=0.1.png
    penalty_epsilon=0.5.png
    0.5时报警.png



如果选择过高的值，会影响最终的信息矩阵，可能会出现不收敛的情况

  • weight_max_vel_x 2 满足最大允许平移速度的优化权重，还有最大角速度权重，以及加速度，角加速度权重，在整个运动过程中以主要以高速还是低速运行，则是看这些权重的分配
  • weight_max_vel_theta (double，默认值：1.0) 满足最大允许角速度的优化权重
  • weight_acc_lim_x (double，默认值：1.0) 满足最大允许平移加速度的优化权重

  • weight_acc_lim_theta (double，默认值：1.0) 满足最大允许角加速度的优化重量

  • weight_multiplier

  • weight_obstacle 默认值：50.0. 优化权重以保持与障碍物的最小距离。可以增大至几百，让机器人提前转向，避免卡死

  • obstacle_cost_exponent 无默认值，目前设置为0.65. 源码在EdgeObstacle::computeError和EdgeInflatedObstacle::computeError，判断是否为1，同时min_obstacle_dist > 0才有效。用于更新g2o的误差函数

  • weight_kinematics_nh 默认值：1000. 用于满足非完整运动学的优化权重. 该参数必须很高，因为运动学方程构成了一个等式约束，即使值1000也不意味着由于与其他成本相比较小的“原始”成本值而导致的矩阵条件不佳

  • weight_kinematics_forward_drive 默认值：1，改为1000。 迫使机器人仅选择前进方向 (正向速度) 。较小的权重，仍然会允许向后行驶。我们无法限制优化器的搜索空间为正的速度，因为优化器不支持硬约束，只能大幅提高weight_kinematics_forward_drive以避免后退速度，但是值太大又会减小收敛速度。不管权重多高，无法规划出前进的路径时，还是可能出现停滞或者倒车。

  • weight_optimaltime 最短时间权重。提高至5时，车辆在直道上线速度明显加快，而且越大越能接近max_vel_x，并靠近路径边缘沿切线过弯。 Optimization weight for contracting the trajectory w.r.t transition time

  • weight_inflation 默认0.1， Optimization weight for the inflation penalty，应当很小，源码在AddEdgesObstacles的信息矩阵里

  • weight_dynamic_obstacle Optimization weight for satisfying a minimum separation from dynamic obstacles

  • weight_dynamic_obstacle_inflation Optimization weight for the inflation penalty of dynamic obstacles (should be small)

  • weight_adapt_factor 2. 源码只有optimizeTEB的最后。 每个外部TEB迭代中，一些特殊权重 (当前为weight_obstacle) 会通过此因子重复缩放 (weight_new = weight_old * factor) 。迭代增加权重而不是设置较大的先验值会导致底层优化问题的更好数值条件。

  • optimization_verbose: 默认false. 图优化的过程是否显示，可以用于调试

    1
    2
    iteration= 1	 chi2= 0.458692	 time= 9.3481e-05	 cumTime= 0.000284497	 edges= 49	 schur= 0
    lambda= 5.804560	 levenbergIter= 2

并行规划

  • enable_homotopy_class_planning (bool，默认：true) 激活并行规划(占用更多的CPU资源) 若在单一路径上运行，则没有必要时用此功能

2021-06-11_174533.png
当enable_homotopy_class_planning = False，规划的局部路径会陷入局部最小值。因为路径向上面障碍物或下面障碍物偏，总体的代价值都会增加（时间成本），只有往中间走总体代价值才最低。但此时路径invalid，中间缺少路径点，此时机器人卡死了。
2021-06-11_174540.png
设置True，teb会同时搜寻多条路径并选取一条更可行的，feasibility check环节会抛弃不可行的路径

  • enable_multithreading (bool，默认：true) 激活多个线程以计划不同线程中的每个轨迹

  • max_number_classes (int，默认值：4，设置为2) 指定要考虑的最大不同轨迹数(限制计算工作量)，影响CPU

  • selection_cost_hysteresis (double，默认值：1.0) 指定新候选人必须具有多少轨迹成本才能选择之前的轨迹 (如果new_cost < old_cost * factor，则进行选择

  • selection_prefer_initial_plan 默认0.95 Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan

  • selection_obst_cost_scale (double，默认值：100.0) 仅选择“最佳”候选者的障碍物成本术语的额外缩放

  • selection_viapoint_cost_scale (double，默认值：1.0) 仅为了选择“最佳”候选者而对通孔成本条款进行额外的缩放

  • selection_alternative_time_cost (bool，默认值：false) 如果为true，则将时间成本 (时间差平方的总和) 替换为总过渡时间 (时间差之和)

  • simple_exploration 默认false. 如果为true，不同的 trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal.

  • roadmap_graph_no_samples (int，默认值：15) 指定为创建roadmap graph而生成的样本数，前提是simple_exploration为false

  • roadmap_graph_area_width (double，默认值：6) 在起点和目标之间的矩形区域中采样随机关键点/航路点。以米为单位指定该区域的宽度

  • delete_detours_backwards 默认true. planner will discard the plans detouring backwards with respect to the best plan

  • h_signature_prescaler (double，默认值：1.0) 标度内部参数 (H-signature) ，用于区分同伦类。警告：仅当您在本地成本图中观察到障碍物过多的问题时，才减小此参数，请勿将其选得太低，否则障碍物无法彼此区分 (0.2 < 值 <= 1)

  • h_signature_threshold (double，默认值：0.1) 如果实部和复杂部的差都低于指定的阈值，则假定两个H签名相等

  • obstacle_heading_threshold (double，默认值：1.0) 在障碍物航向和目标航向之间指定标量乘积的值，以便将障碍物)考虑在内进行探索

  • visualize_hc_graph (bool，默认：false) 如果为true，在rviz里可看到优化使用的graph

  • viapoints_all_candidates (bool，默认：true) 如果为true，则将不同拓扑的所有轨迹附加到该组通孔点，否则，仅将与初始/全局计划共享相同拓扑的轨迹与它们连接 (对test_optim_node无效)

  • switching_blocking_period (double，默认值：0.0) 指定允许切换到新的等效类之前需要终止的持续时间 (以秒为单位)

提高性能的调试

  • 使用Costmap Converter
  • 减小no_outer_iterations no_inner_iterations
  • 减小局部地图大小
  • 增大dt_ref obstacle_poses_affected
  • 关闭多路径并行规划 (效果非常显著)
  • 降低 max_lookahead_distance (一般)

如何提高线速度

增大max_vel_x, dt_ref, weight_max_vel_x。如果还不见效就增大 weight_optimaltime

TEB参数设置如下:

1
2
3
4
5
6
dt_ref: 0.5
max_vel_x: 3.0
max_vel_x_backwards: 0.02
max_vel_y: 0.0
acc_lim_x: 0.8
weight_optimaltime: 30

实际的速度最大到2.7，速度曲线如下：
速度实测.png

加大weight_optimaltime到45，最大速度超过了2.8，但修改dt_ref的效果不大了，可见weight_optimaltime提高最大速度的效果比dt_ref更好

尽量避免后退

要求TEB的速度命令只有前进命令是不可能的，因为优化框架不支持硬约束，轻微违反约束是可能的。 应当显著加大weight_kinematics_forward_drive，但是权重太大又会降低收敛速度(这对任何权重都是如此) 。将 max_vel_x_backwards减少到0(或0.02)，至少减少到与参数penalty_epsilon相同。

解读 Costmap2DROS 的构造函数|路径规划代价地图

ROS源码里有个costmap_2d_node.cpp，编译成可执行文件costmap_2d_node，注意它的链接库：

1
2
3
4
5
6
target_link_libraries(costmap_2d_node
    costmap_2d
    ${PCL_LIBRARIES}
    ${Boost_LIBRARIES}
    ${catkin_LIBRARIES}
)

它的main函数非常简单，实际就是Costmap2DROS的构造函数
1
2
3
4
5
6
7
8
9
10
11
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "costmap_node");
  tf::TransformListener tf(ros::Duration(10));
  costmap_2d::Costmap2DROS lcr("costmap", tf);
  ros::spin();
  return (0);
}

获取参数值和tf变换

首先是一些参数的获取。循环等待直到获得机器人底盘坐标系和global系之间的坐标转换。
并获取rolling_window、track_unknown_space、always_send_full_costmap的参数，默认为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
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
Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :
    layered_costmap_(NULL),
    name_(name),  // 局部还是全局代价地图
    tf_(tf),
    transform_tolerance_(0.3),
    map_update_thread_shutdown_(false),
    stop_updates_(false),
    initialized_(true),
    stopped_(false),
    robot_stopped_(false),
    map_update_thread_(NULL),
    last_publish_(0),
    plugin_loader_("costmap_2d", "costmap_2d::Layer"),
    publisher_(NULL),
    dsrv_(NULL),
    footprint_padding_(0.0)
{
  //初始化old pose
  old_pose_.setIdentity();
  old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));
  // 如果是全局代价地图，这里name是global_costmap
  ros::NodeHandle private_nh("~/" + name);
  ros::NodeHandle g_nh;

  //获取tf前缀
  ros::NodeHandle prefix_nh;
  std::string tf_prefix = tf::getPrefixParam(prefix_nh);

  //两个坐标系，全局和局部的参数是不同的
  private_nh.param("global_frame", global_frame_, std::string("/map"));
  private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));

  //确保基于tf前缀正确设置了坐标系
  //tf::resolve或者TransformListener::resolve的作用就是使用tf_prefix参数将frame_name解析为frame_id
  global_frame_ = tf::resolve(tf_prefix, global_frame_);
  robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);

  ros::Time last_error = ros::Time::now();
  std::string tf_error;
  
  //确保机器人global_frame_和robot_base_frame_之间的有效转换, 否则一直停留在这里阻塞
  // 这里已经写死为 0.1，有需要可以改变
  while (ros::ok()
      && !tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), 
        ros::Duration(0.1), ros::Duration(0.01), &tf_error))
  {
    ros::spinOnce();
    // 如果５秒内未成功tf转换，报警
    if (last_error + ros::Duration(5.0) < ros::Time::now())
    {
      ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
               robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
      last_error = ros::Time::now();
    }
    // The error string will accumulate and errors will typically be the same, so the last
    // will do for the warning above. Reset the string here to avoid accumulation.
    tf_error.clear();
  }
  //检测是否需要“窗口滚动”，从参数服务器获取以下参数
  bool rolling_window, track_unknown_space, always_send_full_costmap;
  private_nh.param("rolling_window", rolling_window, false);
  private_nh.param("track_unknown_space", track_unknown_space, false);
  private_nh.param("always_send_full_costmap", always_send_full_costmap, false);

有时出现了下面的报错

这就是因为上面的0.1太小了

LayeredCostmap类的对象

layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

接下来创建一个LayeredCostmap类实例layered_costmap_，作为规划器使用的master costmap，并通过它管理各层地图。

LayeredCostmap类是Costmap2DROS的类成员，它是master costmap，也能够管理各层地图，因为它 含有指向各层子地图的指针 std::vector<boost::shared_ptr<Layer> > plugins_，能够调用子地图的类方法，开启子地图的更新，各层子地图最后都会合并到master costmap，提供给规划器的使用。

它的构造函数只有:

1
2
3
4
5
6
7
8
9
10
11
// 如果追踪未知信息的cell，默认false
if (track_unknown)
	costmap_.setDefaultValue(255);
else
	costmap_.setDefaultValue(0);

// 枚举值的含义对应如下：
/*	NO_INFORMATION = 255;
	LETHAL_OBSTACLE = 254;
	INSCRIBED_INFLATED_OBSTACLE = 253;
	FREE_SPACE = 0;    */

LayeredCostmap含有类成员Costmap2D costmap_，这个类就是底层地图，用于记录地图数据。

继续Costmap2DROS的构造函数代码：

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
// 查看参数服务器里的costmap是否有plugins参数，如果没有，则重置为旧参数
if (!private_nh.hasParam("plugins"))
{
    resetOldParameters(private_nh);
}
// 如果加载了plugins，循环将每个plugin即每层子地图添加进layered_costmap_类的
// 指针组对象中，并对每层地图调用其 initialize 类方法，初始化各层地图。
if (private_nh.hasParam("plugins"))
{
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("Using plugin \"%s\"", pname.c_str());

      // plugin_loader_类型是　pluginlib::ClassLoader<Layer>，ROS插件机制
      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
      // 添加到容器  plugins_
      layered_costmap_->addPlugin(plugin);
      // 执行 Layer::initialize， 它向各层地图通知主地图的存在，并调用
      // oninitialize方法  （Layer类中的虚函数，被定义在各层地图中）
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
}

这里得到的插件即为各层子地图。每层子地图使用Pluginlib（ROS插件机制）来实例化，各个层可以被独立的编译，且允许使用C++接口对Costmap做出任意的改变。

footprint话题

订阅footprint话题，回调函数为setUnpaddedRobotFootprintPolygon。当话题上收到footprint时，回调函数会将接收到的footprint根据参数footprint_padding_的值进行膨胀，得到“膨胀”后的padded_footprint_，传递给各级地图。

查看节点可知，/move_base/local_costmap/footprint和/move_base/global_costmap/footprint两个话题的发布订阅者都是move_base

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
std::string topic_param, topic;
if (!private_nh.searchParam("footprint_topic", topic_param))
{
  topic_param = "footprint_topic";
}
private_nh.param(topic_param, topic, std::string("footprint"));
footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);

if (!private_nh.searchParam("published_footprint_topic", topic_param))
{
  topic_param = "published_footprint";
}
private_nh.param(topic_param, topic, std::string("oriented_footprint"));
footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);

setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));


发布代价地图

创建地图的发布器实例,Costmap2DPublisher类是用于地图发布的封装类,在函数mapUpdateLoop中调用

1
2
3
4
5
6
7
8
9
10
11
12
//发布Costmap2D
publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap);

//创建地图更新线程的控制量
stop_updates_ = false;
initialized_ = true;
stopped_ = false;

//创建一个timer去检测机器人是否在移动
robot_stopped_ = false;
//回调函数movementCB通过比较前后两个pose的差来判断机器人是否在移动
timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);

开启动态参数配置，它的回调函数Costmap2DROS::reconfigureCB会在节点启动时运行一次，加载.cfg文件的配置参数。这个函数给对应参数赋值，并创建了一个Costmap2DROS::mapUpdateLoop函数的线程，即地图更新线程。

1
2
3
4
5
6
//开启参数动态配置
  dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
  //回调函数reconfigureCB 除了对一些类成员的配置值做赋值以外，还会开启一个更新map的线程 
  dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,_2);
  dsrv_->setCallback(cb);
}

这里的代码有点复杂了，mapUpdateLoop函数循环调用UpdateMap函数，更新地图。并以publish_cycle为周期，发布更新后的地图。

UpdateMap首先调用类内getRobotPose函数，通过tf转换，将机器人底盘系的坐标转换到global系，得到机器人的位姿。然后通过layered_costmap_调用LayeredCostmap类的updateMap函数，更新地图。

流程如下
costmap的初始化流程.png

整个costmap初始化，最关键和复杂的就是各层的加载和更新地图