Cartographer 前端 Local SLAM 详解（２）

Cartographer 前端 Local SLAM 详解

1. 前端概述

1.1 Local SLAM 的职责

核心任务:

• 实时位姿跟踪: 在 10-100Hz 的频率下估计机器人位姿
• 局部地图构建: 维护短期 Submaps (通常包含 45-90 次扫描)
• 数据预处理: 滤波、降采样、异常值剔除

与后端的关系:

前端输出 → 后端输入 ├─ Node (位姿节点) ├─ Submap (子图) └─ Constraint (局部约束) 

性能指标:

• 延迟: < 100ms (从接收点云到输出位姿)
• 精度: 局部漂移 < 1% 路径长度
• 鲁棒性: 处理传感器噪声、动态物体

1.2 数据流管线

RangeData (原始点云) ↓ [1] 预处理阶段 ├─ Voxel Filter (体素下采样) ├─ Adaptive Voxel Filter (自适应采样) └─ Range Filter (距离裁剪) ↓ [2] 位姿预测阶段 ├─ Pose Extrapolator (融合 IMU/Odometry) └─ 输出初始位姿估计 ↓ [3] 扫描匹配阶段 ├─ CSM (Correlative Scan Matcher) │ └─ 暴力搜索提供粗匹配 ├─ Ceres Scan Matcher │ └─ 非线性优化精修位姿 └─ 输出优化后位姿 ↓ [4] Submap 更新阶段 ├─ 射线投射 (Ray Tracing) ├─ 概率更新 (Bayesian Update) └─ Submap 状态管理 ↓ [5] 运动过滤阶段 ├─ Motion Filter (过滤微小运动) └─ 决定是否插入新 Node ↓ InsertionResult (输出给后端) 

2. 数据预处理

2.1 Voxel Filter (体素滤波器)

目的: 减少点云数量，降低计算负载。

算法原理:

// sensor/voxel_filter.cc PointCloud VoxelFilter::Filter(const PointCloud& point_cloud){// 1. 创建体素网格 (3D 哈希表) absl::flat_hash_map<VoxelKey, Eigen::Vector3f> voxel_map;for(const RangefinderPoint& point : point_cloud){// 2. 计算点所属的体素索引// key = floor(point / voxel_size)const VoxelKey key =ComputeVoxelKey(point.position, resolution_);// 3. 每个体素只保留一个点 (覆盖策略)// 可选策略: 中心点、质心、最近点 voxel_map[key]= point.position;}// 4. 提取滤波后的点云 PointCloud filtered; filtered.reserve(voxel_map.size());for(constauto&[key, point]: voxel_map){ filtered.push_back({point});}return filtered;}// 体素键计算 (整数网格坐标) VoxelKey ComputeVoxelKey(const Eigen::Vector3f& point,float resolution){return VoxelKey{.x =static_cast<int>(std::floor(point.x()/ resolution)),.y =static_cast<int>(std::floor(point.y()/ resolution)),.z =static_cast<int>(std::floor(point.z()/ resolution))};}

参数配置:

-- trajectory_builder_2d.lua TRAJECTORY_BUILDER_2D ={ voxel_filter_size =0.025,-- 2.5cm 体素尺寸-- 对于 40m 范围内的点云:-- 原始: ~10000 点-- 滤波后: ~2000 点 (减少 80%)}

性能分析:

• 时间复杂度: O(N)O(N)O(N) (哈希表插入)
• 空间复杂度: O(M)O(M)O(M) (M = 非空体素数)
• 数据压缩率: 典型 5:1 (取决于点云密度)

2.2 Adaptive Voxel Filter (自适应体素滤波器)

问题: 固定体素尺寸可能导致:

• 远距离点云过度稀疏 → 匹配失败
• 近距离点云仍过密 → 计算浪费

解决方案: 动态调整体素尺寸以保证最小点数。

算法实现:

// sensor/internal/voxel_filter.cc PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud){float current_resolution = max_length_;// 初始尺寸 (最粗)while(true){// 1. 使用当前分辨率滤波 PointCloud filtered =VoxelFilter::Filter( point_cloud, current_resolution);// 2. 检查点数是否满足要求if(filtered.size()>= min_num_points_){return filtered;// 满足条件，返回结果}// 3. 点数不足，减小体素尺寸 (增加密度) current_resolution *=0.5f;// 4. 防止无限循环 (最小尺寸限制)if(current_resolution < max_length_ *1e-6f){LOG(WARNING)<<"Cannot achieve min_num_points: "<< filtered.size()<<" < "<< min_num_points_;return filtered;}}}

参数配置:

TRAJECTORY_BUILDER_2D ={ adaptive_voxel_filter ={ max_length =0.5,-- 初始体素尺寸 (最粗) min_num_points =200,-- 最少保留点数 max_range =50.0,-- 最大有效距离},}

应用场景:

2.3 Range Filter (距离过滤器)

目的: 剔除无效测量点。

过滤规则:

// sensor/range_data.cc RangeData CropRangeData(const RangeData& range_data,constfloat min_range,constfloat max_range){ RangeData cropped;for(const RangefinderPoint& point : range_data.returns){constfloat range = point.position.norm();// 1. 过滤太近的点 (传感器盲区)if(range < min_range)continue;// 2. 过滤太远的点 (噪声大、精度低)if(range > max_range)continue;// 3. 过滤 NaN/Inf 值if(!std::isfinite(range))continue; cropped.returns.push_back(point);}return cropped;}

典型参数:

TRAJECTORY_BUILDER_2D ={ min_range =0.2,-- 20cm 以内为盲区 max_range =30.0,-- 30m 以外丢弃-- 原因: LiDAR 精度随距离下降-- σ_range ≈ 0.01m + 0.001 × distance}

3. 位姿预测 (Pose Extrapolator)

3.1 多传感器融合

目的: 在扫描匹配前提供初始位姿估计。

数据来源:

1. IMU (惯性测量单元):

• 提供: 角速度 ω\omegaω, 线加速度 aaa
• 优势: 高频 (200-1000Hz)，短期精度高
• 劣势: 存在零偏漂移

2. Odometry (轮式里程计):

• 提供: 位移增量 Δx,Δy,Δθ\Delta x, \Delta y, \Delta \thetaΔx,Δy,Δθ
• 优势: 长期稳定性好
• 劣势: 打滑误差、机械误差

融合策略:

// mapping/internal/pose_extrapolator.ccclassPoseExtrapolator{public:// 添加 IMU 数据voidAddImuData(const sensor::ImuData& imu_data){ imu_tracker_->AddImuData(imu_data);// 更新重力方向估计 (用于水平校正) gravity_direction_ = imu_tracker_->orientation()* Eigen::Vector3d::UnitZ();}// 添加里程计数据voidAddOdometryData(const sensor::OdometryData& odometry_data){// 计算里程计速度 (差分)if(timed_pose_queue_.empty()){ odometry_pose_ = odometry_data.pose;return;}constauto delta_pose = odometry_pose_.inverse()* odometry_data.pose;constdouble delta_time = common::ToSeconds(odometry_data.time - last_odometry_time_); linear_velocity_from_odometry_ = delta_pose.translation()/ delta_time; angular_velocity_from_odometry_ = transform::RotationQuaternionToAngleAxisVector( delta_pose.rotation())/ delta_time;}// 预测当前时刻位姿 transform::Rigid3d ExtrapolatePose(const common::Time time){constauto last_pose = timed_pose_queue_.back();constdouble extrapolation_duration = common::ToSeconds(time - last_pose.time);// 1. IMU 提供旋转增量const Eigen::Quaterniond rotation = imu_tracker_->orientation()* last_pose.pose.rotation().inverse()* last_pose.pose.rotation();// 2. 里程计提供平移增量const Eigen::Vector3d translation = last_pose.pose.translation()+ linear_velocity_from_odometry_ * extrapolation_duration;return transform::Rigid3d(translation, rotation);}private: std::unique_ptr<ImuTracker> imu_tracker_; std::deque<TimedPose> timed_pose_queue_;// 历史位姿队列 Eigen::Vector3d linear_velocity_from_odometry_; Eigen::Vector3d angular_velocity_from_odometry_;};

3.2 IMU 数据处理

重力对齐 (Gravity Alignment):

// mapping/internal/imu_tracker.ccvoidImuTracker::AddImuData(const sensor::ImuData& imu_data){constdouble delta_t = common::ToSeconds(imu_data.time - time_);// 1. 更新角速度积分 (旋转估计)const Eigen::Quaterniond rotation = transform::AngleAxisVectorToRotationQuaternion( imu_data.angular_velocity * delta_t); orientation_ =(orientation_ * rotation).normalized();// 2. 重力方向滤波 (低通滤波消除高频噪声)// α ∈ [0, 1]: 滤波系数，越小越平滑constdouble alpha =1.0- std::exp(-delta_t / gravity_time_constant_); Eigen::Vector3d gravity_in_tracking_frame = orientation_.conjugate()* imu_data.linear_acceleration; gravity_direction_ =(1.0- alpha)* gravity_direction_ + alpha * gravity_in_tracking_frame.normalized();// 3. 纠正旋转 (使 Z 轴对齐重力方向)const Eigen::Quaterniond gravity_alignment = Eigen::Quaterniond::FromTwoVectors( orientation_ * Eigen::Vector3d::UnitZ(), gravity_direction_); orientation_ = gravity_alignment * orientation_;}

数学原理:

qt+1=qt⋅exp⁡(Δt2ωt) \mathbf{q}_{t+1} = \mathbf{q}_t \cdot \exp\left(\frac{\Delta t}{2} \boldsymbol{\omega}_t\right) qt+1​=qt​⋅exp(2Δt​ωt​)

其中 ωt\boldsymbol{\omega}_tωt​ 为角速度，exp⁡(⋅)\exp(\cdot)exp(⋅) 为四元数指数映射。

4. 扫描匹配 (Scan Matching)

4.1 CSM (Correlative Scan Matcher) - 暴力搜索

目标: 在搜索窗口内找到最佳匹配位姿。

算法流程:

// mapping/scan_matching/real_time_correlative_scan_matcher_2d.ccboolRealTimeCorrelativeScanMatcher2D::Match(const transform::Rigid2d& initial_pose_estimate,const sensor::PointCloud& point_cloud,const Grid2D& grid, transform::Rigid2d* pose_estimate){// 1. 定义搜索参数const SearchParameters search_parameters( linear_search_window_,// ±0.1m angular_search_window_,// ±0.2rad (约 ±11°) point_cloud, grid.limits().resolution());// 2. 生成候选位姿 (离散化搜索空间)const std::vector<Candidate2D> candidates =GenerateExhaustiveSearchCandidates(search_parameters);// 候选数量计算:// N = (2 * linear_window / resolution + 1)^2 *// (2 * angular_window / angular_step + 1)// 典型: (21)^2 * (41) ≈ 18000 个候选// 3. 并行评分 (OpenMP 加速) std::vector<Candidate2D> scored_candidates =ComputeCandidatesScores(candidates, point_cloud, grid);// 4. 选择最佳候选const Candidate2D& best_candidate =*std::max_element( scored_candidates.begin(), scored_candidates.end(),[](const Candidate2D& a,const Candidate2D& b){return a.score < b.score;});// 5. 阈值检查if(best_candidate.score < min_score_){returnfalse;// 匹配失败}*pose_estimate = initial_pose_estimate * best_candidate.pose;returntrue;}

评分函数:

floatComputeCandidateScore(const Candidate2D& candidate,const sensor::PointCloud& point_cloud,const Grid2D& grid){float score =0.0f;int num_valid_points =0;for(const RangefinderPoint& point : point_cloud){// 1. 变换点云到候选位姿const Eigen::Vector2f transformed_point = candidate.pose.cast<float>()* point.position.head<2>();// 2. 查询占用概率constfloat probability = grid.GetProbability(grid.limits().GetCellIndex(transformed_point));// 3. 累加分数 (占用区域分数高)if(probability != kUnknownProbability){ score += probability;++num_valid_points;}}// 4. 归一化return num_valid_points >0? score / num_valid_points :0.0f;}

性能优化:

// 预计算点云旋转 (避免重复三角函数运算)classRotatingRangeScanMatcher{voidPrecomputeRotatedPointClouds(const sensor::PointCloud& point_cloud,const std::vector<float>& angles){ rotated_scans_.clear(); rotated_scans_.reserve(angles.size());for(constfloat angle : angles){const Eigen::Rotation2Df rotation(angle); sensor::PointCloud rotated; rotated.reserve(point_cloud.size());for(constauto& point : point_cloud){ rotated.push_back({rotation * point.position.head<2>()});} rotated_scans_.push_back(std::move(rotated));}} std::vector<sensor::PointCloud> rotated_scans_;};

4.2 Ceres Scan Matcher - 非线性优化

目标: 在 CSM 提供的初值基础上，精确求解位姿。

Cost Function 定义:

// mapping/scan_matching/ceres_scan_matcher_2d.ccclassOccupiedSpaceCostFunction2D{public:OccupiedSpaceCostFunction2D(constdouble scaling_factor,const sensor::PointCloud& point_cloud,const Grid2D& grid):scaling_factor_(scaling_factor),point_cloud_(point_cloud),grid_(grid){}template<typenameT>booloperator()(const T*const pose, T* residual)const{// pose = [x, y, theta]const Eigen::Matrix<T,2,1>translation(pose[0], pose[1]);const Eigen::Rotation2D<T>rotation(pose[2]);// 遍历所有点计算残差for(size_t i =0; i < point_cloud_.size();++i){// 1. 变换点云const Eigen::Matrix<T,2,1>point(T(point_cloud_[i].position.x()),T(point_cloud_[i].position.y()));const Eigen::Matrix<T,2,1> world_point = rotation * point + translation;// 2. 双线性插值查询占用概率const T probability =InterpolateProbability(world_point, grid_);// 3. 残差 = 1 - 占用概率 (被占用区域残差小) residual[i]= scaling_factor_ *(T(1.0)- probability);}returntrue;}private:// 双线性插值 (平滑梯度，有利于优化)template<typenameT>static T InterpolateProbability(const Eigen::Matrix<T,2,1>& point,const Grid2D& grid){const MapLimits& limits = grid.limits();const T x_index =(point.x()- limits.min().x())/ limits.resolution();const T y_index =(point.y()- limits.min().y())/ limits.resolution();// 获取四个邻近栅格constint x0 =static_cast<int>(std::floor(x_index));constint y0 =static_cast<int>(std::floor(y_index));const T p00 =T(grid.GetProbability(Eigen::Array2i(x0, y0)));const T p01 =T(grid.GetProbability(Eigen::Array2i(x0, y0 +1)));const T p10 =T(grid.GetProbability(Eigen::Array2i(x0 +1, y0)));const T p11 =T(grid.GetProbability(Eigen::Array2i(x0 +1, y0 +1)));// 双线性插值const T wx = x_index -T(x0);const T wy = y_index -T(y0);return(T(1.0)- wx)*(T(1.0)- wy)* p00 +(T(1.0)- wx)* wy * p01 + wx *(T(1.0)- wy)* p10 + wx * wy * p11;}constdouble scaling_factor_;const sensor::PointCloud& point_cloud_;const Grid2D& grid_;};

正则化项 (防止过拟合):

// 平移正则化classTranslationDeltaCostFunction2D{public:TranslationDeltaCostFunction2D(constdouble scaling_factor,const transform::Rigid2d& initial_pose):scaling_factor_(scaling_factor),x_(initial_pose.translation().x()),y_(initial_pose.translation().y()){}template<typenameT>booloperator()(const T*const pose, T* residual)const{// 惩罚远离初值的平移 residual[0]= scaling_factor_ *(pose[0]-T(x_)); residual[1]= scaling_factor_ *(pose[1]-T(y_));returntrue;}private:constdouble scaling_factor_;constdouble x_, y_;};// 旋转正则化classRotationDeltaCostFunction2D{public:RotationDeltaCostFunction2D(constdouble scaling_factor,constdouble initial_angle):scaling_factor_(scaling_factor),angle_(initial_angle){}template<typenameT>booloperator()(const T*const pose, T* residual)const{// 角度归一化到 [-π, π] residual[0]= scaling_factor_ *NormalizeAngle(pose[2]-T(angle_));returntrue;}private:constdouble scaling_factor_;constdouble angle_;};

构建优化问题:

voidCeresScanMatcher2D::Match(const transform::Rigid2d& initial_pose_estimate,const sensor::PointCloud& point_cloud,const Grid2D& grid, transform::Rigid2d* pose_estimate, ceres::Solver::Summary* summary){// 1. 初始化优化变量double pose[3]={ initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y(), initial_pose_estimate.rotation().angle()};// 2. 构建问题 ceres::Problem problem;// 2.1 添加占用空间约束 problem.AddResidualBlock(CreateOccupiedSpaceCostFunction2D( options_.occupied_space_weight(), point_cloud, grid),nullptr,// 损失函数 (或使用 HuberLoss) pose );// 2.2 添加平移正则化 problem.AddResidualBlock(CreateTranslationDeltaCostFunction2D( options_.translation_weight(), initial_pose_estimate),nullptr, pose );// 2.3 添加旋转正则化 problem.AddResidualBlock(CreateRotationDeltaCostFunction2D( options_.rotation_weight(), initial_pose_estimate.rotation().angle()),nullptr, pose );// 3. 配置求解器 ceres::Solver::Options solver_options; solver_options.max_num_iterations = options_.num_iterations(); solver_options.num_threads = options_.num_threads(); solver_options.linear_solver_type = ceres::DENSE_QR;// 4. 求解 ceres::Solve(solver_options,&problem, summary);// 5. 提取结果*pose_estimate = transform::Rigid2d({pose[0], pose[1]}, pose[2]);}

参数配置:

TRAJECTORY_BUILDER_2D.ceres_scan_matcher ={ occupied_space_weight =1.0,-- 占用空间约束权重 translation_weight =10.0,-- 平移正则化权重 rotation_weight =40.0,-- 旋转正则化权重 num_iterations =10,-- 最大迭代次数 num_threads =1,-- 线程数}

5. Submap 插入

5.1 射线投射 (Ray Tracing)

目的: 更新从传感器到击中点路径上的所有栅格。

Bresenham 算法实现:

// mapping/2d/ray_to_pixel_mask.cc std::vector<Eigen::Array2i>RayToPixelMask(const Eigen::Array2i& scaled_begin,const Eigen::Array2i& scaled_end,int subpixel_scale){ std::vector<Eigen::Array2i> pixel_mask;// Bresenham 直线算法int dx = std::abs(scaled_end.x()- scaled_begin.x());int dy = std::abs(scaled_end.y()- scaled_begin.y());int sx = scaled_begin.x()< scaled_end.x()?1:-1;int sy = scaled_begin.y()< scaled_end.y()?1:-1;int err = dx - dy; Eigen::Array2i current = scaled_begin;while(true){ pixel_mask.push_back(current / subpixel_scale);if(current == scaled_end)break;int e2 =2* err;if(e2 >-dy){ err -= dy; current.x()+= sx;}if(e2 < dx){ err += dx; current.y()+= sy;}}return pixel_mask;}

5.2 概率更新

贝叶斯更新公式:

P(m∣z1:t)=P(zt∣m)⋅P(m∣z1:t−1)P(zt) P(m|z_{1:t}) = \frac{P(z_t|m) \cdot P(m|z_{1:t-1})}{P(z_t)} P(m∣z1:t​)=P(zt​)P(zt​∣m)⋅P(m∣z1:t−1​)​

Log-Odds 表示 (数值稳定):

logit(p)=log⁡p1−p \text{logit}(p) = \log\frac{p}{1-p} logit(p)=log1−pp​

Lt=Lt−1+logit(P(zt∣m)) L_{t} = L_{t-1} + \text{logit}(P(z_t|m)) Lt​=Lt−1​+logit(P(zt​∣m))

代码实现:

// mapping/2d/probability_grid_range_data_inserter_2d.ccvoidProbabilityGridRangeDataInserter2D::Insert(const sensor::RangeData& range_data, GridInterface* grid){ ProbabilityGrid* probability_grid =static_cast<ProbabilityGrid*>(grid);// 1. 更新 Miss 栅格 (自由空间)for(const Eigen::Vector2f& miss : range_data.misses){const Eigen::Array2i cell_index = probability_grid->limits().GetCellIndex(miss);UpdateCell(cell_index, hit_table_[0],// Miss 概率对应的 log-odds probability_grid);}// 2. 更新 Hit 栅格 (障碍物)for(const Eigen::Vector2f& hit : range_data.returns){const Eigen::Array2i cell_index = probability_grid->limits().GetCellIndex(hit);UpdateCell(cell_index, hit_table_[1],// Hit 概率对应的 log-odds probability_grid);}}voidUpdateCell(const Eigen::Array2i& cell_index, uint16 update_value, ProbabilityGrid* grid){ uint16* cell = grid->mutable_value(cell_index);// Log-Odds 加法 (对应概率乘法)constint new_value =ClampToUint16(*cell + update_value - kUpdateMarker);*cell = new_value;}

5.3 Submap 生命周期管理

状态转换:

// mapping/internal/2d/local_trajectory_builder_2d.ccvoidLocalTrajectoryBuilder2D::AddRangeData(const sensor::RangeData& range_data){// 1. 获取当前活跃 Submap std::vector<std::shared_ptr<const Submap2D>> insertion_submaps = active_submaps_.submaps();// 2. 插入数据到 Submapfor(constauto& submap : insertion_submaps){ submap->InsertRangeData(range_data, range_data_inserter_.get());}// 3. 检查是否需要完成当前 Submapif(insertion_submaps.back()->num_range_data()>= options_.num_range_data()){ active_submaps_.FinishSubmap();// kActive → kFinished}}// mapping/internal/2d/active_submaps_2d.ccvoidActiveSubmaps2D::FinishSubmap(){// 1. 标记当前 Submap 为已完成 submaps_.back()->SetInsertionFinished();// 2. 创建新的活跃 SubmapAddSubmap(ComputeSubmapPose());}

6. 运动过滤 (Motion Filter)

目的: 减少冗余 Node，降低后端计算负载。

过滤策略:

// mapping/internal/motion_filter.ccboolMotionFilter::IsSimilar(const common::Time time,const transform::Rigid3d& pose){if(num_total_ ==0|| time - last_time_ > max_time_seconds_ ||(pose.translation()- last_pose_.translation()).norm()> max_distance_meters_ || transform::GetAngle(pose.rotation()* last_pose_.rotation().inverse())> max_angle_radians_){// 运动足够大，不过滤 last_time_ = time; last_pose_ = pose;++num_total_;returnfalse;}// 运动太小，过滤掉++num_total_;++num_different_;returntrue;}

参数配置:

TRAJECTORY_BUILDER_2D.motion_filter ={ max_time_seconds =5.0,-- 5秒后强制插入 max_distance_meters =0.2,-- 移动 20cm 插入 max_angle_radians =0.004,-- 旋转 0.23° 插入}

过滤率统计:

• 典型过滤率: 70-80% (10Hz LiDAR → 2-3Hz Node 插入)
• 后端计算量降低: ~60%

7. 前端性能分析

7.1 计算时间分布

实测数据 (Intel i7, 2D LiDAR @ 10Hz):

总耗时: ~50ms/frame ├─ 数据预处理: 8ms (16%) │ ├─ Voxel Filter: 5ms │ └─ Range Filter: 3ms │ ├─ 位姿预测: 2ms (4%) │ ├─ 扫描匹配: 35ms (70%) │ ├─ CSM: 25ms ← 瓶颈 │ └─ Ceres: 10ms │ └─ Submap 插入: 5ms (10%) ├─ Ray Tracing: 3ms └─ Probability Update: 2ms 

7.2 优化建议

1. 减少 CSM 搜索范围

-- 从 ±0.3m 减小到 ±0.1m TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher ={ linear_search_window =0.1,-- 默认 0.3 angular_search_window =0.15,-- 默认 0.2-- 加速: 50% → 节省 12ms}

2. 增大体素尺寸

TRAJECTORY_BUILDER_2D.voxel_filter_size =0.05-- 默认 0.025-- 点云减少 75% → CSM 加速 40%

3. 禁用 CSM (仅依赖 Ceres)

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching =false-- 风险: 鲁棒性下降 (需要高质量运动先验)

8. 前端失败案例分析

案例 1: 特征贫乏环境

现象: 长走廊、空旷场地匹配失败。

原因:

// 评分函数输出低分float score =ComputeCandidateScore(...);// score = 0.45 < min_score (0.6) → 匹配失败

解决方案:

• 降低 min_score 阈值 (风险: 误匹配增加)
• 添加角点检测器 (仅匹配高信息量点)

案例 2: 快速旋转

现象: 原地旋转时位姿跳变。

原因: Motion Filter 过滤了大量旋转数据。

解决方案:

TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians =0.001-- 降低阈值

案例 3: 动态物体干扰

现象: 人群中地图出现"幽灵墙"。

解决方案:

// 添加动态物体过滤器 (欧式聚类) sensor::PointCloud FilterDynamicObjects(const sensor::PointCloud& cloud){// 1. 欧式聚类 std::vector<Cluster> clusters =EuclideanClustering(cloud,0.3);// 2. 过滤移动簇 (速度 > 0.5m/s) sensor::PointCloud static_cloud;for(constauto& cluster : clusters){if(EstimateClusterVelocity(cluster)<0.5){ static_cloud.insert(static_cloud.end(), cluster.points.begin(), cluster.points.end());}}return static_cloud;}

9. 总结

前端核心算法:

1. CSM: 提供鲁棒的初值 (暴力搜索保证全局性)
2. Ceres: 精确优化 (双线性插值提供平滑梯度)
3. Submap: 限定匹配范围 (防止全局漂移)

关键参数:

适用场景:

• ✅ 结构化环境 (室内、走廊)
• ✅ 中低速运动 (< 2m/s)
• ⚠️ 高动态场景 (需额外滤波)
• ❌ 完全对称环境 (如空房间)