Cartographer定位模式下在低性能处理器上的参数配置

纯定位文件配置

cartographer纯定位模式下的配置文件hxzh_backpack_2d_localization.lua

include "hxzh_backpack_2d.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options

在已有的trajectory 0 轨迹上新建了一条trajectory 1 的轨迹，该轨迹上即实时执行前端匹配，也同时执行后端优化，后端优化会与trajectory 0已有的submap进行匹配，确定当前激光帧的准确位姿。在trajectory 1轨迹上只同时维护3个submap，这样不会造成内存堆积，可以一直持续定位，而且精度很高，前提是有一张质量较好的地图。但是Cartographer运行时对计算力要求较高，如果当前cpu计算能力较差时，或者定位时生成node与submap之间的约束不能够及时计算优化，则会进入排队优化状态，这样，生成约束的速度大于处理约束的速度，造成约束队列越来越来多，影响机器人正常定位状态。终端会有如下打印
“There are too many remaining work items in queue：101”
后面跟的数字越大，表示积压的未及时处理的约束越多。为了能够使Cartographer在较低性能的处理器上同样能够满足实时定位跟踪，我们可以采取如下一些策略。

从local_slam降低计算量

纯定位模式下的local_slam与建图模式下相同，在触发optimize_every_n_nodes时与已有地图进行匹配进行矫正，可以理解为Cartographer纯定位模式是一种特殊的建图模式，只是保留了最近的三个submap，之前的将其删除，由于此机制的存在，使用cartographer定位时如果走出地图边缘外，依然可以通过local slam定位一段距离。
为了实现性能较差的处理器上实时定位，关于cartographer纯定位模式下减少计算量，在localslam中可以通过配置参数减少node 的生成，根据实际情况适当增加下面两个数值

 TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2
 TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(1.)

因为在纯定位过程中不需要如此高频率的生成node，只要保证位姿不丢就可以，具体参数定义可以查看cartographer/mapping/internal/motion_filter.cc尤其是max_angle_radians数值，默认1°的话，理论上旋转一圈就会生成360个node，计算量很大。此外可以根据雷达特性，在能够满足精度要求的前提下适当降低rangefinder_sampling_ratio和 max_range ，同样可以减少计算量。

从pose graph中降低计算量

Cartographer纯定位时会新建一条轨迹trajectory 1，在新轨迹上生成的node回去尝试和所有已完成的submap进行约束构建，优化后不断的矫正位姿，此时如果约束过少，会使得后续定位过程产生的偏移不能及时矫正，约束太多，会有过多重复，cpu负担增加，导致无法实时计算优化，影响定位精度。
为了减少纯定位模式下pose graph的计算量，可以从减少构建node与submap的约束量中入手，可以调整如下参数：

POSE_GRAPH.constraint_buidler.sampling_ratio = 0.3
POSE_GRAPH.constraint_buidler.max_constaint_distance = 15

对于参数sampling_ratio是一个采样频率，具体实现可以看cartographer/cartographer/common/fixed_ratio_sampler.cc中。

bool FixedRatioSampler::Pulse() {
 
    ++num_pulses_;
 
    if (static_cast < double > (num_samples) / num_pulses < ratio_) {
 
        ++num_samples_;
 
        return true;
 
    }
 
    return false;
 
}

其可以简单理解为，取值为1时，全部采样；取值为0.5时，一半采样；取值为0.33时，三分之一采样；取值为0.1时，十分之一采样，以此类推。所以该值越小，采样频率越少，对于pose graph中，需要计算的有效约束也会越少。对于参数max_constaint_distance，可以理解为当前node与距离多远的submap构建约束，如果建图场景比较小，可以适当减小该参数，提出距离较远的约束。上述是通过修改参数适应低性能处理器，如果为了继续降低计算量，在代码中可以修改如下部分，cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc中注释如下代码（注意：只能是在纯定位模式下，建图模式需要开启）。

/*if (newly_finished_submap) {
    const SubmapId newly_finished_submap_id = submap_ids.front();
    for (const auto& node_id_data : optimization_problem_->node_data()) {
        const NodeId& node_id = node_id_data.id;
        if (newly_finished_submap_node_ids.count(node_id) == 0) {
            ​    ComputeConstraint(node_id, newly_finished_submap_id);
        }
    }
}*/

注释后，新轨迹上的每生成一个新submap不会与之前的node做约束优化，会大大减少计算量。

从trimmer机制降低计算量

Trimmer是一个删除子图的操作，其具体参数在cartographer/configuration_files/pose_graph.lua中

-- overlapping_submaps_trimmer_2d = {
 
-- fresh_submaps_count = 1, --相同区域最多保留几个子图
 
-- min_covered_area = 2,--子图未被覆盖的面积小于该数值将被删除
 
-- min_added_submaps_count = 5,--最少增加几个子图之后才进行覆盖区域的计算和Trimmer
 
-- },

这段被注释的部分，如果放开就会发现，建图时重复走一条路，submap会不连续，很多相似的被删除了。上述参数主要是调整参数某数submap是否达到被删除的阈值。具体实现可以查看cartographer/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc中，可以简单理解为，删除与最新的1个submaps覆盖后剩余栅格小于2个的子图submap。
如果建图时在一个地方来回行走多次，使用Trimmer参数建图后，纯定位时在一个区域搜索的子图数量会减少，可以在不影响建图的情况下合理开启删减子图的操作。

总结

本文主要针对于Cartographer纯定位模式如何降低计算量，使其能够在性能较低的处理器上实现实时定位，从local slam 、pose graph 、trimmer 等方面展开讨论，可以根据机器人实际情况，合理配置，最终表现出比较良好的定位效果。当然还有更多的优化空间，需要更为深入的研究和思考

转至链接: link.