粒子扫描匹配优化更新粒子权重scanMatch方法gridslamprocessor.cpp深度解析1.函数签名与作用inlinevoidGridSlamProcessor::scanMatch(constdouble*plainReading)作用对每个粒子执行扫描匹配优化修正其位姿估计并计算观测似然用于更新粒子权重。输入plainReading: 激光扫描数据数组距离值核心流程for each particle: 1. 使用 optimize() 优化位姿 2. 如果优化成功接受新位姿 3. 计算似然和得分 4. 更新粒子权重 5. 计算活跃区域为地图更新做准备2.逐行代码分析2.1 初始化变量doublesumScore0;for(ParticleVector::iterator itm_particles.begin();it!m_particles.end();it){OrientedPoint corrected;// 优化后的位姿doublescore,l,s;// 得分、对数似然、匹配得分变量说明corrected: 存储优化后的位姿[score](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h#L32-L32): 扫描匹配的总得分用于评估优化质量l: 对数似然log-likelihood用于更新粒子权重[s](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_main/ugv_bringup/ugv_bringup/base_ctrl.py#L0-L0): 匹配得分matched points 的加权和sumScore: 所有粒子的平均得分用于调试2.2 位姿优化optimize()scorem_matcher.optimize(corrected,it-map,it-pose,plainReading);这是最关键的一步调用 ScanMatcher 的优化算法。optimize() 算法详解// scanmatcher.cpp:336doubleScanMatcher::optimize(OrientedPointpnew,// 输出优化后的位姿constScanMatcherMapmap,// 参考地图constOrientedPointinit,// 初始位姿运动模型预测constdouble*readings// 激光扫描数据)const{doublebestScore-1;OrientedPoint currentPoseinit;// 计算初始位姿的得分doublecurrentScorescore(map,currentPose,readings);// 搜索步长doubleadeltam_optAngularDelta;// 角度步长默认 0.05 raddoubleldeltam_optLinearDelta;// 线性步长默认 0.05 munsignedintrefinement0;// 细化次数// 7 个搜索方向enumMove{Front,Back,Left,Right,TurnLeft,TurnRight,Done};intc_iterations0;do{// 如果当前得分没有提升缩小步长细化搜索if(bestScorecurrentScore){refinement;adelta*0.5;// 角度步长减半ldelta*0.5;// 线性步长减半}bestScorecurrentScore;OrientedPoint bestLocalPosecurrentPose;OrientedPoint localPosecurrentPose;Move moveFront;// 内层循环尝试 6 个方向的移动do{localPosecurrentPose;switch(move){caseFront:localPose.xldelta;// 向前moveBack;break;caseBack:localPose.x-ldelta;// 向后moveLeft;break;caseLeft:localPose.y-ldelta;// 向左moveRight;break;caseRight:localPose.yldelta;// 向右moveTurnLeft;break;caseTurnLeft:localPose.thetaadelta;// 左转moveTurnRight;break;caseTurnRight:localPose.theta-adelta;// 右转moveDone;break;default:;}// 里程计可靠性加权可选doubleodo_gain1;if(m_angularOdometryReliability0.){doubledthinit.theta-localPose.theta;dthatan2(sin(dth),cos(dth));dth*dth;odo_gain*exp(-m_angularOdometryReliability*dth);}if(m_linearOdometryReliability0.){doubledxinit.x-localPose.x;doubledyinit.y-localPose.y;doubledrhodx*dxdy*dy;odo_gain*exp(-m_linearOdometryReliability*drho);}// 计算新位姿的得分doublelocalScoreodo_gain*score(map,localPose,readings);// 如果得分更高更新最优位姿if(localScorecurrentScore){currentScorelocalScore;bestLocalPoselocalPose;}c_iterations;}while(move!Done);// 更新当前位姿为局部最优currentPosebestLocalPose;// 继续优化直到// 1. 得分不再提升或// 2. 达到最大细化次数}while(currentScorebestScore||refinementm_optRecursiveIterations);pnewcurrentPose;returnbestScore;}算法原理坐标下降法Coordinate Descent核心思想从初始位姿开始依次尝试 6 个方向的小步长移动选择得分最高的方向如果没有改进缩小步长细化重复直到收敛或达到最大迭代次数可视化第 1 轮粗搜索: 步长: ldelta0.05m, adelta0.05rad ↑ T.L | ← L · R → (· 当前位置) | ↓ T.R F/B 尝试 6 个方向找到最佳方向 第 2 轮细化: 步长: ldelta0.025m, adelta0.025rad 在最佳方向附近继续搜索 ... 直到步长足够小或达到最大迭代次数score() 函数计算匹配得分doubleScanMatcher::score(constScanMatcherMapmap,constOrientedPointp,constdouble*readings)const{// 将激光端点投影到地图上// 检查每个端点是否落在占用栅格上// 返回匹配点的数量和质量doubles0;for(each laser beam){Point phittransformToEndPoint(p,reading[i],angle[i]);IntPoint iphitmap.world2map(phit);// 在核窗口内搜索最佳匹配for(xx-kernelSize to kernelSize)for(yy-kernelSize to kernelSize){constPointAccumulatorcellmap.cell(iphit(xx,yy));if(cell.occupancythreshold){// 是障碍物Point muphit-cell.mean();// 距离最近障碍物的偏移sexp(-|mu|²/gaussianSigma);// 高斯评分}}}returns;}物理意义得分越高表示激光扫描与地图的对齐越好通过高斯核函数平滑评分避免硬阈值2.3 判断优化是否成功if(scorem_minimumScore){it-posecorrected;// 接受优化后的位姿}else{if(m_infoStream){m_infoStreamScan Matching Failed, using odometry.std::endl;m_infoStreamlp:m_lastPartPosestd::endl;m_infoStreamop:m_odoPosestd::endl;}// 保留运动模型预测的位姿不更新}逻辑如果score m_minimumScore默认 0.0接受优化结果否则回退到纯里程计位姿为什么需要这个检查在某些情况下如长廊环境、特征稀少扫描匹配可能失败此时强行接受错误的优化结果会导致地图漂移保守策略宁可不用扫描匹配也不接受错误结果2.4 计算似然和得分m_matcher.likelihoodAndScore(s,l,it-map,it-pose,plainReading);sumScorescore;it-weightl;it-weightSuml;likelihoodAndScore() 详解// scanmatcher.h:190inlineunsignedintScanMatcher::likelihoodAndScore(doubles,// 输出匹配得分doublel,// 输出对数似然constScanMatcherMapmap,// 参考地图constOrientedPointp,// 粒子位姿constdouble*readings// 激光扫描)const{l0;// 初始化对数似然s0;// 初始化得分// 步骤 1: 将激光坐标系转换到世界坐标系constdouble*anglem_laserAnglesm_initialBeamsSkip;OrientedPoint lpp;// 考虑激光雷达相对于机器人的安装位置lp.xcos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;lp.ysin(p.theta)*m_laserPose.xcos(p.theta)*m_laserPose.y;lp.thetam_laserPose.theta;// 未击中时的对数似然惩罚项doublenoHitnullLikelihood/(m_likelihoodSigma);unsignedintskip0;unsignedintc0;// 匹配点计数// 自由空间判定阈值doublefreeDeltamap.getDelta()*m_freeCellRatio;// 步骤 2: 遍历每束激光for(constdouble*rreadingsm_initialBeamsSkip;rreadingsm_laserBeams;r,angle){skip;skipskipm_likelihoodSkip?0:skip;if(*rm_usableRange)continue;// 超出范围跳过if(skip)continue;// 跳过的光束// 步骤 3: 计算激光端点的世界坐标Point phitlp;phit.x*r*cos(lp.theta*angle);phit.y*r*sin(lp.theta*angle);IntPoint iphitmap.world2map(phit);// 步骤 4: 计算自由空间点端点前方一点Point pfreelp;pfree.x(*r-freeDelta)*cos(lp.theta*angle);pfree.y(*r-freeDelta)*sin(lp.theta*angle);pfreepfree-phit;IntPoint ipfreemap.world2map(pfree);boolfoundfalse;PointbestMu(0.,0.);// 步骤 5: 在核窗口内搜索最佳匹配for(intxx-m_kernelSize;xxm_kernelSize;xx)for(intyy-m_kernelSize;yym_kernelSize;yy){IntPoint priphitIntPoint(xx,yy);IntPoint pfpripfree;constPointAccumulatorcellmap.cell(pr);// 端点所在栅格constPointAccumulatorfcellmap.cell(pf);// 自由空间栅格// 条件端点是障碍物且前方是自由空间if(((double)cellm_fullnessThreshold)((double)fcellm_fullnessThreshold)){Point muphit-cell.mean();// 端点到栅格中心的偏移if(!found){bestMumu;foundtrue;}else{// 选择距离最近的栅格bestMu(mu*mu)(bestMu*bestMu)?mu:bestMu;}}}// 步骤 6: 累加得分if(found){// 高斯评分距离越近得分越高sexp(-1./m_gaussianSigma*bestMu*bestMu);c;}// 步骤 7: 累加对数似然if(!skip){doublef(-1./m_likelihoodSigma)*(bestMu*bestMu);l(found)?f:noHit;// 找到匹配用 f否则用 noHit}}returnc;// 返回匹配点数量}关键概念解析(1) 激光坐标变换// 激光雷达通常不在机器人中心需要补偿OrientedPoint lpp;lp.xcos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;lp.ysin(p.theta)*m_laserPose.xcos(p.theta)*m_laserPose.y;lp.thetam_laserPose.theta;示例机器人位姿: (5, 3, π/4) 激光安装位置: (0.2, 0, 0) // 前方 20cm 激光世界坐标: lp.x 5 cos(π/4)*0.2 - sin(π/4)*0 5.14 lp.y 3 sin(π/4)*0.2 cos(π/4)*0 3.14 lp.theta π/4 0 π/4(2) 自由空间判定Point pfreelp;pfree.x(*r-freeDelta)*cos(lp.theta*angle);pfree.y(*r-freeDelta)*sin(lp.theta*angle);物理意义激光端点应该是障碍物端点前方一小段距离应该是自由空间这确保了激光束终止在障碍物表面而不是穿过障碍物示意图机器人 o ----激光束---- [自由空间] [端点/障碍物] ^^^^^^^^^^ ^^^^^^^^^^^^ pfree phit(3) 核窗口搜索for(intxx-m_kernelSize;xxm_kernelSize;xx)for(intyy-m_kernelSize;yym_kernelSize;yy){IntPoint priphitIntPoint(xx,yy);// 检查 pr 是否是障碍物pripfree 是否是自由空间}作用在端点周围的小窗口内搜索最佳匹配容忍一定的定位误差和地图噪声m_kernelSize通常为 1 或 23×3 或 5×5 窗口(4) 对数似然计算doublef(-1./m_likelihoodSigma)*(bestMu*bestMu);l(found)?f:noHit;数学原理假设观测误差服从高斯分布 N(0, σ²) 似然函数 P(z|x,m) ∝ exp(-|z - h(x,m)|² / (2σ²)) 对数似然 log P(z|x,m) -|z - h(x,m)|² / (2σ²) C 其中 z: 实际观测激光端点 h(x,m): 预测观测地图上的障碍物位置 |z - h(x,m)|: bestMu残差 σ²: m_likelihoodSigma未击中的处理doublenoHitnullLikelihood/(m_likelihoodSigma);[nullLikelihood](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h#L39-L39) 是一个负常数如 -1.0如果激光束没有找到匹配给予一个固定的惩罚避免对数似然趋向负无穷2.5 更新粒子权重it-weightl;// 累加对数似然it-weightSuml;// 累加到总权重用于选择最佳粒子为什么累加而不是替换GMapping 使用累积权重来表示整条轨迹的质量[weight](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/gridfastslam/gfsreader.h#L76-L76) Σ log P(z_t | x_t, m)从 t0 到当前时刻轨迹越长权重越小因为不断累加负值后续归一化在 [normalize()](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx#L40-L66) 中会通过 softmax 归一化w_iexp(gain*(weight_i-weight_max))2.6 计算活跃区域// 为后续的地图更新做准备m_matcher.invalidateActiveArea();m_matcher.computeActiveArea(it-map,it-pose,plainReading);作用标记哪些地图栅格会被当前激光扫描更新在重采样时只复制这些活跃的 patches写时复制大幅减少内存拷贝开销流程图computeActiveArea(): ├─ 遍历每束激光 ├─ 计算激光束穿过的栅格 ├─ 计算端点周围的栅格 └─ 将这些栅格所在的 patches 标记为active 后续 registerScan(): ├─ 只复制 active patches └─ 其他 patches 共享内存2.7 输出调试信息if(m_infoStream)m_infoStreamAverage Scan Matching ScoresumScore/m_particles.size()std::endl;用途监控扫描匹配的平均质量如果分数持续很低可能需要调整参数如增加粒子数、调整噪声模型3.完整流程图scanMatch(plainReading) │ ├─ for each particle: │ │ │ ├─ 1. optimize() - 位姿优化 │ │ ├─ 从运动模型预测的位姿开始 │ │ ├─ 尝试 6 个方向的小步长移动 │ │ ├─ 选择得分最高的方向 │ │ ├─ 如果没有改进缩小步长 │ │ └─ 重复直到收敛 │ │ │ ├─ 2. 判断优化是否成功 │ │ ├─ if score minimumScore: 接受优化位姿 │ │ └─ else: 保留里程计位姿 │ │ │ ├─ 3. likelihoodAndScore() - 计算似然 │ │ ├─ 转换激光坐标到世界坐标系 │ │ ├─ 对每束激光 │ │ │ ├─ 计算端点和自由空间点 │ │ │ ├─ 在核窗口内搜索最佳匹配 │ │ │ ├─ 累加高斯得分 s │ │ │ └─ 累加对数似然 l │ │ └─ 返回匹配点数量 │ │ │ ├─ 4. 更新粒子权重 │ │ ├─ weight l │ │ └─ weightSum l │ │ │ └─ 5. computeActiveArea() - 标记活跃区域 │ └─ 为后续地图更新做准备 │ └─ 输出平均得分调试4.关键参数说明参数默认值作用m_optAngularDelta0.05 rad角度搜索步长m_optLinearDelta0.05 m线性搜索步长m_optRecursiveIterations3最大细化次数m_minimumScore0.0最小可接受得分m_gaussianSigma0.05高斯核方差评分用m_likelihoodSigma0.1似然计算方差m_kernelSize1搜索窗口半径3×3m_likelihoodSkip0跳过的激光束加速计算m_fullnessThreshold0.5占用概率阈值5.性能优化技巧(1) 跳束采样skipskipm_likelihoodSkip?0:skip;if(skip)continue;不是所有激光束都参与似然计算每隔m_likelihoodSkip束计算一次大幅减少计算量对精度影响很小(2) 早期终止if(*rm_usableRange)continue;超出可用范围的激光束直接跳过避免处理不可靠的远距离测量(3) 核窗口限制for(intxx-m_kernelSize;xxm_kernelSize;xx)只在局部小窗口内搜索避免全局搜索的高计算复杂度6.常见问题与调试Q1: 扫描匹配频繁失败怎么办症状Scan Matching Failed, using odometry.可能原因环境特征太少长廊、空旷房间运动模型噪声太大初始位姿偏离太远m_minimumScore设置过高解决方法降低m_minimumScore如设为 -100减小运动模型噪声[srr](file://gmapping/openslam_gmapping/gui/gsp_thread.h#L151-L151), [stt](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/gui/gsp_thread.h#L154-L154) 等增加粒子数量Q2: 地图漂移严重可能原因扫描匹配陷入局部最优里程计误差累积解决方法增加m_optRecursiveIterations更多细化迭代减小初始步长m_optLinearDelta,m_optAngularDelta使用更精确的里程计Q3: 计算速度太慢优化方法增加m_likelihoodSkip如设为 2 或 3减小m_kernelSize设为 0 或 1减少粒子数量降低激光分辨率下采样7.总结[scanMatch](file://gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h#L314-L314) 是 GMapping 算法的核心优化引擎负责✅位姿修正: 通过扫描匹配纠正里程计误差✅权重计算: 评估每个粒子的观测似然✅地图准备: 标记活跃区域为高效更新做准备算法特点坐标下降法: 简单有效适合实时应用鲁棒性: 通过得分阈值避免错误优化效率: 核窗口搜索、跳束采样等优化技巧精度: 高斯评分、自由空间验证提高匹配质量这就是 [scanMatch](file://gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h#L314-L314) 函数的完整实现分析它是理解 GMapping 如何实现高精度 SLAM 的关键。
gmapping算法源码实现分析（四）
发布时间：2026/5/25 1:40:09
粒子扫描匹配优化更新粒子权重scanMatch方法gridslamprocessor.cpp深度解析1.函数签名与作用inlinevoidGridSlamProcessor::scanMatch(constdouble*plainReading)作用对每个粒子执行扫描匹配优化修正其位姿估计并计算观测似然用于更新粒子权重。输入plainReading: 激光扫描数据数组距离值核心流程for each particle: 1. 使用 optimize() 优化位姿 2. 如果优化成功接受新位姿 3. 计算似然和得分 4. 更新粒子权重 5. 计算活跃区域为地图更新做准备2.逐行代码分析2.1 初始化变量doublesumScore0;for(ParticleVector::iterator itm_particles.begin();it!m_particles.end();it){OrientedPoint corrected;// 优化后的位姿doublescore,l,s;// 得分、对数似然、匹配得分变量说明corrected: 存储优化后的位姿[score](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h#L32-L32): 扫描匹配的总得分用于评估优化质量l: 对数似然log-likelihood用于更新粒子权重[s](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_main/ugv_bringup/ugv_bringup/base_ctrl.py#L0-L0): 匹配得分matched points 的加权和sumScore: 所有粒子的平均得分用于调试2.2 位姿优化optimize()scorem_matcher.optimize(corrected,it-map,it-pose,plainReading);这是最关键的一步调用 ScanMatcher 的优化算法。optimize() 算法详解// scanmatcher.cpp:336doubleScanMatcher::optimize(OrientedPointpnew,// 输出优化后的位姿constScanMatcherMapmap,// 参考地图constOrientedPointinit,// 初始位姿运动模型预测constdouble*readings// 激光扫描数据)const{doublebestScore-1;OrientedPoint currentPoseinit;// 计算初始位姿的得分doublecurrentScorescore(map,currentPose,readings);// 搜索步长doubleadeltam_optAngularDelta;// 角度步长默认 0.05 raddoubleldeltam_optLinearDelta;// 线性步长默认 0.05 munsignedintrefinement0;// 细化次数// 7 个搜索方向enumMove{Front,Back,Left,Right,TurnLeft,TurnRight,Done};intc_iterations0;do{// 如果当前得分没有提升缩小步长细化搜索if(bestScorecurrentScore){refinement;adelta*0.5;// 角度步长减半ldelta*0.5;// 线性步长减半}bestScorecurrentScore;OrientedPoint bestLocalPosecurrentPose;OrientedPoint localPosecurrentPose;Move moveFront;// 内层循环尝试 6 个方向的移动do{localPosecurrentPose;switch(move){caseFront:localPose.xldelta;// 向前moveBack;break;caseBack:localPose.x-ldelta;// 向后moveLeft;break;caseLeft:localPose.y-ldelta;// 向左moveRight;break;caseRight:localPose.yldelta;// 向右moveTurnLeft;break;caseTurnLeft:localPose.thetaadelta;// 左转moveTurnRight;break;caseTurnRight:localPose.theta-adelta;// 右转moveDone;break;default:;}// 里程计可靠性加权可选doubleodo_gain1;if(m_angularOdometryReliability0.){doubledthinit.theta-localPose.theta;dthatan2(sin(dth),cos(dth));dth*dth;odo_gain*exp(-m_angularOdometryReliability*dth);}if(m_linearOdometryReliability0.){doubledxinit.x-localPose.x;doubledyinit.y-localPose.y;doubledrhodx*dxdy*dy;odo_gain*exp(-m_linearOdometryReliability*drho);}// 计算新位姿的得分doublelocalScoreodo_gain*score(map,localPose,readings);// 如果得分更高更新最优位姿if(localScorecurrentScore){currentScorelocalScore;bestLocalPoselocalPose;}c_iterations;}while(move!Done);// 更新当前位姿为局部最优currentPosebestLocalPose;// 继续优化直到// 1. 得分不再提升或// 2. 达到最大细化次数}while(currentScorebestScore||refinementm_optRecursiveIterations);pnewcurrentPose;returnbestScore;}算法原理坐标下降法Coordinate Descent核心思想从初始位姿开始依次尝试 6 个方向的小步长移动选择得分最高的方向如果没有改进缩小步长细化重复直到收敛或达到最大迭代次数可视化第 1 轮粗搜索: 步长: ldelta0.05m, adelta0.05rad ↑ T.L | ← L · R → (· 当前位置) | ↓ T.R F/B 尝试 6 个方向找到最佳方向 第 2 轮细化: 步长: ldelta0.025m, adelta0.025rad 在最佳方向附近继续搜索 ... 直到步长足够小或达到最大迭代次数score() 函数计算匹配得分doubleScanMatcher::score(constScanMatcherMapmap,constOrientedPointp,constdouble*readings)const{// 将激光端点投影到地图上// 检查每个端点是否落在占用栅格上// 返回匹配点的数量和质量doubles0;for(each laser beam){Point phittransformToEndPoint(p,reading[i],angle[i]);IntPoint iphitmap.world2map(phit);// 在核窗口内搜索最佳匹配for(xx-kernelSize to kernelSize)for(yy-kernelSize to kernelSize){constPointAccumulatorcellmap.cell(iphit(xx,yy));if(cell.occupancythreshold){// 是障碍物Point muphit-cell.mean();// 距离最近障碍物的偏移sexp(-|mu|²/gaussianSigma);// 高斯评分}}}returns;}物理意义得分越高表示激光扫描与地图的对齐越好通过高斯核函数平滑评分避免硬阈值2.3 判断优化是否成功if(scorem_minimumScore){it-posecorrected;// 接受优化后的位姿}else{if(m_infoStream){m_infoStreamScan Matching Failed, using odometry.std::endl;m_infoStreamlp:m_lastPartPosestd::endl;m_infoStreamop:m_odoPosestd::endl;}// 保留运动模型预测的位姿不更新}逻辑如果score m_minimumScore默认 0.0接受优化结果否则回退到纯里程计位姿为什么需要这个检查在某些情况下如长廊环境、特征稀少扫描匹配可能失败此时强行接受错误的优化结果会导致地图漂移保守策略宁可不用扫描匹配也不接受错误结果2.4 计算似然和得分m_matcher.likelihoodAndScore(s,l,it-map,it-pose,plainReading);sumScorescore;it-weightl;it-weightSuml;likelihoodAndScore() 详解// scanmatcher.h:190inlineunsignedintScanMatcher::likelihoodAndScore(doubles,// 输出匹配得分doublel,// 输出对数似然constScanMatcherMapmap,// 参考地图constOrientedPointp,// 粒子位姿constdouble*readings// 激光扫描)const{l0;// 初始化对数似然s0;// 初始化得分// 步骤 1: 将激光坐标系转换到世界坐标系constdouble*anglem_laserAnglesm_initialBeamsSkip;OrientedPoint lpp;// 考虑激光雷达相对于机器人的安装位置lp.xcos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;lp.ysin(p.theta)*m_laserPose.xcos(p.theta)*m_laserPose.y;lp.thetam_laserPose.theta;// 未击中时的对数似然惩罚项doublenoHitnullLikelihood/(m_likelihoodSigma);unsignedintskip0;unsignedintc0;// 匹配点计数// 自由空间判定阈值doublefreeDeltamap.getDelta()*m_freeCellRatio;// 步骤 2: 遍历每束激光for(constdouble*rreadingsm_initialBeamsSkip;rreadingsm_laserBeams;r,angle){skip;skipskipm_likelihoodSkip?0:skip;if(*rm_usableRange)continue;// 超出范围跳过if(skip)continue;// 跳过的光束// 步骤 3: 计算激光端点的世界坐标Point phitlp;phit.x*r*cos(lp.theta*angle);phit.y*r*sin(lp.theta*angle);IntPoint iphitmap.world2map(phit);// 步骤 4: 计算自由空间点端点前方一点Point pfreelp;pfree.x(*r-freeDelta)*cos(lp.theta*angle);pfree.y(*r-freeDelta)*sin(lp.theta*angle);pfreepfree-phit;IntPoint ipfreemap.world2map(pfree);boolfoundfalse;PointbestMu(0.,0.);// 步骤 5: 在核窗口内搜索最佳匹配for(intxx-m_kernelSize;xxm_kernelSize;xx)for(intyy-m_kernelSize;yym_kernelSize;yy){IntPoint priphitIntPoint(xx,yy);IntPoint pfpripfree;constPointAccumulatorcellmap.cell(pr);// 端点所在栅格constPointAccumulatorfcellmap.cell(pf);// 自由空间栅格// 条件端点是障碍物且前方是自由空间if(((double)cellm_fullnessThreshold)((double)fcellm_fullnessThreshold)){Point muphit-cell.mean();// 端点到栅格中心的偏移if(!found){bestMumu;foundtrue;}else{// 选择距离最近的栅格bestMu(mu*mu)(bestMu*bestMu)?mu:bestMu;}}}// 步骤 6: 累加得分if(found){// 高斯评分距离越近得分越高sexp(-1./m_gaussianSigma*bestMu*bestMu);c;}// 步骤 7: 累加对数似然if(!skip){doublef(-1./m_likelihoodSigma)*(bestMu*bestMu);l(found)?f:noHit;// 找到匹配用 f否则用 noHit}}returnc;// 返回匹配点数量}关键概念解析(1) 激光坐标变换// 激光雷达通常不在机器人中心需要补偿OrientedPoint lpp;lp.xcos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;lp.ysin(p.theta)*m_laserPose.xcos(p.theta)*m_laserPose.y;lp.thetam_laserPose.theta;示例机器人位姿: (5, 3, π/4) 激光安装位置: (0.2, 0, 0) // 前方 20cm 激光世界坐标: lp.x 5 cos(π/4)*0.2 - sin(π/4)*0 5.14 lp.y 3 sin(π/4)*0.2 cos(π/4)*0 3.14 lp.theta π/4 0 π/4(2) 自由空间判定Point pfreelp;pfree.x(*r-freeDelta)*cos(lp.theta*angle);pfree.y(*r-freeDelta)*sin(lp.theta*angle);物理意义激光端点应该是障碍物端点前方一小段距离应该是自由空间这确保了激光束终止在障碍物表面而不是穿过障碍物示意图机器人 o ----激光束---- [自由空间] [端点/障碍物] ^^^^^^^^^^ ^^^^^^^^^^^^ pfree phit(3) 核窗口搜索for(intxx-m_kernelSize;xxm_kernelSize;xx)for(intyy-m_kernelSize;yym_kernelSize;yy){IntPoint priphitIntPoint(xx,yy);// 检查 pr 是否是障碍物pripfree 是否是自由空间}作用在端点周围的小窗口内搜索最佳匹配容忍一定的定位误差和地图噪声m_kernelSize通常为 1 或 23×3 或 5×5 窗口(4) 对数似然计算doublef(-1./m_likelihoodSigma)*(bestMu*bestMu);l(found)?f:noHit;数学原理假设观测误差服从高斯分布 N(0, σ²) 似然函数 P(z|x,m) ∝ exp(-|z - h(x,m)|² / (2σ²)) 对数似然 log P(z|x,m) -|z - h(x,m)|² / (2σ²) C 其中 z: 实际观测激光端点 h(x,m): 预测观测地图上的障碍物位置 |z - h(x,m)|: bestMu残差 σ²: m_likelihoodSigma未击中的处理doublenoHitnullLikelihood/(m_likelihoodSigma);[nullLikelihood](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h#L39-L39) 是一个负常数如 -1.0如果激光束没有找到匹配给予一个固定的惩罚避免对数似然趋向负无穷2.5 更新粒子权重it-weightl;// 累加对数似然it-weightSuml;// 累加到总权重用于选择最佳粒子为什么累加而不是替换GMapping 使用累积权重来表示整条轨迹的质量[weight](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/gridfastslam/gfsreader.h#L76-L76) Σ log P(z_t | x_t, m)从 t0 到当前时刻轨迹越长权重越小因为不断累加负值后续归一化在 [normalize()](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx#L40-L66) 中会通过 softmax 归一化w_iexp(gain*(weight_i-weight_max))2.6 计算活跃区域// 为后续的地图更新做准备m_matcher.invalidateActiveArea();m_matcher.computeActiveArea(it-map,it-pose,plainReading);作用标记哪些地图栅格会被当前激光扫描更新在重采样时只复制这些活跃的 patches写时复制大幅减少内存拷贝开销流程图computeActiveArea(): ├─ 遍历每束激光 ├─ 计算激光束穿过的栅格 ├─ 计算端点周围的栅格 └─ 将这些栅格所在的 patches 标记为active 后续 registerScan(): ├─ 只复制 active patches └─ 其他 patches 共享内存2.7 输出调试信息if(m_infoStream)m_infoStreamAverage Scan Matching ScoresumScore/m_particles.size()std::endl;用途监控扫描匹配的平均质量如果分数持续很低可能需要调整参数如增加粒子数、调整噪声模型3.完整流程图scanMatch(plainReading) │ ├─ for each particle: │ │ │ ├─ 1. optimize() - 位姿优化 │ │ ├─ 从运动模型预测的位姿开始 │ │ ├─ 尝试 6 个方向的小步长移动 │ │ ├─ 选择得分最高的方向 │ │ ├─ 如果没有改进缩小步长 │ │ └─ 重复直到收敛 │ │ │ ├─ 2. 判断优化是否成功 │ │ ├─ if score minimumScore: 接受优化位姿 │ │ └─ else: 保留里程计位姿 │ │ │ ├─ 3. likelihoodAndScore() - 计算似然 │ │ ├─ 转换激光坐标到世界坐标系 │ │ ├─ 对每束激光 │ │ │ ├─ 计算端点和自由空间点 │ │ │ ├─ 在核窗口内搜索最佳匹配 │ │ │ ├─ 累加高斯得分 s │ │ │ └─ 累加对数似然 l │ │ └─ 返回匹配点数量 │ │ │ ├─ 4. 更新粒子权重 │ │ ├─ weight l │ │ └─ weightSum l │ │ │ └─ 5. computeActiveArea() - 标记活跃区域 │ └─ 为后续地图更新做准备 │ └─ 输出平均得分调试4.关键参数说明参数默认值作用m_optAngularDelta0.05 rad角度搜索步长m_optLinearDelta0.05 m线性搜索步长m_optRecursiveIterations3最大细化次数m_minimumScore0.0最小可接受得分m_gaussianSigma0.05高斯核方差评分用m_likelihoodSigma0.1似然计算方差m_kernelSize1搜索窗口半径3×3m_likelihoodSkip0跳过的激光束加速计算m_fullnessThreshold0.5占用概率阈值5.性能优化技巧(1) 跳束采样skipskipm_likelihoodSkip?0:skip;if(skip)continue;不是所有激光束都参与似然计算每隔m_likelihoodSkip束计算一次大幅减少计算量对精度影响很小(2) 早期终止if(*rm_usableRange)continue;超出可用范围的激光束直接跳过避免处理不可靠的远距离测量(3) 核窗口限制for(intxx-m_kernelSize;xxm_kernelSize;xx)只在局部小窗口内搜索避免全局搜索的高计算复杂度6.常见问题与调试Q1: 扫描匹配频繁失败怎么办症状Scan Matching Failed, using odometry.可能原因环境特征太少长廊、空旷房间运动模型噪声太大初始位姿偏离太远m_minimumScore设置过高解决方法降低m_minimumScore如设为 -100减小运动模型噪声[srr](file://gmapping/openslam_gmapping/gui/gsp_thread.h#L151-L151), [stt](file:///home/lxb/ros2_ws/ugv_ws/src/ugv_else/gmapping/openslam_gmapping/gui/gsp_thread.h#L154-L154) 等增加粒子数量Q2: 地图漂移严重可能原因扫描匹配陷入局部最优里程计误差累积解决方法增加m_optRecursiveIterations更多细化迭代减小初始步长m_optLinearDelta,m_optAngularDelta使用更精确的里程计Q3: 计算速度太慢优化方法增加m_likelihoodSkip如设为 2 或 3减小m_kernelSize设为 0 或 1减少粒子数量降低激光分辨率下采样7.总结[scanMatch](file://gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h#L314-L314) 是 GMapping 算法的核心优化引擎负责✅位姿修正: 通过扫描匹配纠正里程计误差✅权重计算: 评估每个粒子的观测似然✅地图准备: 标记活跃区域为高效更新做准备算法特点坐标下降法: 简单有效适合实时应用鲁棒性: 通过得分阈值避免错误优化效率: 核窗口搜索、跳束采样等优化技巧精度: 高斯评分、自由空间验证提高匹配质量这就是 [scanMatch](file://gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h#L314-L314) 函数的完整实现分析它是理解 GMapping 如何实现高精度 SLAM 的关键。
