激光SLAM之Gmapping（2）算法分析解读

激光SLAM之Gmapping（2）算法分析解读

  Gmapping的程序框架是依托Open_slam，该框架主要分成slam_gmapping和openslam_gmapping。在slam_gmapping可以从Lasercallback出发，作为整个框架的起点，Lasercallback函数在slam_gmapping.cpp文件中。

主要函数及调用关系

废话不多说，先上函数调用图

①InitMapper函数

  如果是首次调用lasercallback函数，则进入InitMapper。函数在同样的.cpp文件中。激光雷达测得数据数据，是在激光雷达为坐标系的数据。在InitMapper设置一个比激光雷达Z轴上高一个单位的一个点，用这个点来判断激光雷达是否发生了倾斜。接下来就是根据激光雷达的安装位置（正反安放）。初始化并设置一些参数。

②addScan函数

  成功将测量值加入之后，在Lasercallback下面就是两个坐标系的变换。在addscan函数是主要函数的。在该函数了成功获取到里程计位姿后，根据激光雷达的安装方式，对角度进行修改,然后将ROS的激光雷达采集的信息转换成gmapping能看懂的格式,设置和激光数据时间戳匹配的机器人的位姿。调用processscan函数。

③Processscan函数

  processscan函数在gridslamprocessor.cpp中，首先获取当前的位姿，然后在从里程计运动模型获取位姿，这里调用了drawFromMotion函数，这个函数在motionmodel.cpp中，drawFromMotion函数中的sample函数是形参作为方差，均值为0的高斯分布。sample函数是数值分析所近似生成的高斯分布，具体的函数实现在stat.cpp中。当前位姿与上一次位姿做差，计算做累计角度偏差和位移偏差。利用激光雷达测得距离做得分处理。非首帧调用scanMatch，upDateTreeWeight ,resample。首帧则调用invalidActiveArea, computeActiveArea, registerScan.

④scanMach

  该函数在gridslamprocessor.hxx中，对粒子的最优位姿进行计算：optimize，该函数在scanmacher.cpp；计算粒子最优位姿后，重新计算粒子的权重；粒子的权重由粒子的似然表示的，计算出来最优的位姿后，进行了地图的扩充。Optimize首先计算当前位置的得分，调用score函数，score函数在scanmacher.h。当前得分与上一次的得分差，要减少搜索步长，得到周围的方向里面最好的一个位姿和对应的得分。返回最优额位置和得分。计算当前的位姿和初始位姿的区别，区别越大增益越小。增益的计算odo_gain*=exp(-m_angularOdometryReliability*dth)，-m_angularOdometryReliability为角度里程计的依赖，就是相信传感器传来的数据程度。dth为角度变化量。线性距离也是这样的。计算当前位姿的角度和初始角度的区别，如果里程计比较可靠的话，那么进行匹配的时候就需要对离初始位姿比较远的位姿施加惩罚得分=增益得分。

⑤Score

  score函数该函数先将激光雷达的坐标转换到世界坐标，先转换到机器人的坐标系，然后转到世界坐标。激光雷达击中到某一点，沿着激光方向的前一个点必是未击中的，得到击中点的栅格和前一个点的栅格。如果搜索出最优最可能被这个激光束击中的点，计算得分并返回该得分。

⑥Likelihoodandscore

  likelihoodandscore与score其类似。回到scanmatch函数，invalidateActiveArea和computeActiveArea，UpdateTreeweight。UpdateTreeweight在gridslamprocessor_tree.cpp。这里主要是更新权重，在这里调用normalize，然后调用resetTree把所有的粒子的所有轨迹清零，最后调用propagateweights。在propgateweight更新归一化的粒子权重。因为每一个粒子的路径都是从叶子节点开始的，得到了叶子节点，就得到了路径，所以叶子节点的子节点累计权重就等于每个粒子的权重。

⑦Resample

  该函数在gridslamprocessor.hxx。首先是备份老的粒子的轨迹，即保留叶子的节点。然后是需要重采样还是不需要重采样，如果不需要重采样，则权值不变。只为轨迹创建一个新的节点，每个粒子更新地图。当有效值小于阈值的时候需要重采样，通过resampleIndexes提取到需要删除的粒子。删除粒子后，保留当前的粒子并在保存的粒子的节点里新增一个节点。删除要删除粒子的节点，保留的粒子进行数据更新，将每个粒子的设置同一个权重。最后更新一下地图。

⑧InitMapper

  该函数是激光雷达测到数据是在激光雷达坐标系下测的，在Init-Mapper设置一个比激光雷达高一米的点用来判断激光雷达是否倾斜，根据激光雷达排放位置对角度进行调整。设置参数和初始化参数。

算法结构

输入：$S_{t-1}$前一时刻的粒子集信息
     $Z_t$最新时刻的观测数据
     $U_t$最新的里程计数据
求解：$S_t$当前时刻的粒子集信息
    初始化$X{V}_t=$初始化$X{V}_t$
For  所有$S_{t-1}^i\in S_{t-1}$执行
    提取相关位姿信息$X_{t-1}^i$，权值信息$W_{t-1}^i$，对应地图信息$m_{t-1}^i$
//执行扫描匹配
    $x{v}_t^i=x{v}_{t-1}^i⨁u_t$根据控制信息估计移动机器人位姿信息
    $x{\hat{v}}_t^i=argmaxp(x\mid m_{t-1}^i,z_t,x{v}_t^i)$ 通过估计出的位姿，结合当前观测信息与前一时刻的地图进行扫描匹配，求出匹配程度最高的位姿信息。
if   $x{\hat{v}}_t^i=failurethen$ 如果扫描匹配估计失败，则执行
   $x{v}_t^i\sim \rho (x{v}_t\mid x{v}_{t-1}^i,u_t)$根据控制信息分布粒子集
   $w_t^i=w_{t-1}^i\cdot p(z_t\mid m_{t-1}^i,x{v}_t^i)$然后相应更新粒子权值
else 否则
//在$x{\hat{v}}_t^i$附近采样
for  $k=1,\cdots ,K$执行
   x v k ∼ { x v j ∣ ∣ x v i − x v ^ i ∣ < Δ } xv_k \sim \{xv_j\mid |xv_i-x\hat{v}^i|<\Delta\} xvk​∼{xvj​∣∣xvi​−xv^i∣<Δ}在$xv$的估计值附近进行采样，估计位姿
end for    循环终止
//计算高斯分布
   ${\mu }_t^i=(0,0,0{)}^T$初始化位姿均值
   ${\eta }^i=0$初始化权值
for  all   x j ∈ { x 1 , … , x k } x_j\in\{x_1,\dots,x_k\} xj​∈{x1​,…,xk​} do对所有粒子执行如下操作
   ${\mu }_t^i={\mu }_t^i+x_j\cdot p(z_t\mid m_{t-1}^i,x_j)\cdot p(x_t\mid x_{t-1}^i,u_t)$计算位姿
   ${\eta }_t^i={\eta }_t^i+p(z_t\mid m_{t-1}^i,x_j)\cdot p(x_t\mid x_{t-1}^i,u_t)$ 计算权值
end for
  ${\mu }_t^i={\mu }_t^i/{\eta }_t^i$ 求加权
  ${\sum }_t^i=0$ 初始化协方差
for all   x j ∈ { x 1 , ⋯   , x k } x_j \in\{x_1,\cdots,x_k\} xj​∈{x1​,⋯,xk​} 执行
  ${\sum }_t^i={\sum }_t^i+(x{v}_j-{\mu }^i)(x{v}_j-{\mu }^i{)}^T\cdot p(z_t\mid m_{t-1}^i,x_j)\cdot p(x_t\mid x_{t-1}^i,u_t)$ 计算协方差
end for
  ${\sum }_t^i={\sum }_t^i/{\eta }_t^i$ 求加权平均协方差
//根据所得的均值和协方差采样新的位姿估计
  $x{v}_t^i\sim N({\mu }_t^i,{\sum }_t^i)$ 根据正态分布采样新位姿
//更新重要性权重
  $w_t^i=w_{t-1}^i\cdot {\eta }_t^i$
end if
//更新地图
  $m_t^i=integrateScan(m_{t-1}^i,x{v}_t^i,z_t)$ 根据新获取的位姿信息，结合之前的地图和最新的观测值更新地图
//更新粒子集
   S t = S t ∪ { ( x v t i , w t i , m t i ) } St=St \cup\{(xv^i_t,w^i_t,m^i_t)\} St=St∪{(xvti​,wti​,mti​)} 更新粒子集的位姿信息，权值信息和地图信息
end for
  $Neff=\frac{1}{{\sum }_{i=1}^N({\tilde{w}}^t{)}^2}$ 通过将权值归一化后，求取有效粒子数
if  $Neff<T$
then  若有效粒子低于阈值
   $St=resampling(St)$ 执行重采样
end if

参数解读

参数	意义	类型
tf(tf/tfMessage)	需要进行激光，基准和测距的相关框架转换
scan(sensor_msgs/LaserScan)	激光扫描从中创建地图
map_metadata(nav_msgs/MapMetaData)	从此主题获取地图数据，将其锁定并定期更新
map(nav_msgs/OccupancyGrid)	从此主题获取地图数据，将其锁定并定期更新
entropy(std_msgs/Float64)	机器人姿态分布的熵的估计值（较高的值表示较大的不确定性）
dynamic_map(nav_msgs/GetMap)	调用此服务以获取地图数据
inverted_laser	（改为使用变换数据）激光是正面朝上（扫描按CCW排序）还是上下颠倒（扫描按顺序排列）？	string，默认“false”
throttle_scans	处理	int,默认：“1”
base_frame	框架连接到移动基座	string,默认：“base_link"
map_frame	框架附加到地图	string，默认：“map”
odom_frame	框架连接到里程计系统	string，默认：“odom”
map_update_interval	在更新地图之间的时间长度（以秒为单位）。降低该数量更频繁地更新占用网格，代价是更大的计算负荷	float，默认值：5.0
maxUrange	激光的最大可用范围。光束被裁剪为该值。	float，默认值：80.0
sigma	贪心终点匹配使用的sigma	float，默认值：0.05
kernelSize	内核中要查找一个对应关系	int，default：1
Istep	翻译的优化步骤	float，default：0.05
astep	旋转的优化步骤	float，默认值： 0.05
iteration	扫描匹配器的迭代次数	int， default：5
Isigma	用于计算的波束可能性的sigma	float，默认值：0.075
ogain	在评估可能性时使用的增益，用于平滑重采样效果	float，默认值：3.0
Iskip	每次扫描中要跳过的光束数	int，默认值：0
minimumScore	考虑扫描结果的最小分数匹配好。当使用具有有限范围（例如5m）的激光扫描仪时，可以避免在大的开放空间中跳跃姿态估计。分数上升到600+，例如当遇到跳跃估计问题时，尝试50	float，default：0.05
srr	作为平移函数的平移误差（rho/rho）	float，默认值：0.1
srt	作为旋转函数的平移误差（rho/theta）	float，默认值：0.2
str	作为平移函数的旋转中的测量误差（θ/ rho）	float，默认值：0.1
stt	作为旋转函数的旋转中的测量误差（θ/ θ）	float，默认值：0.2
linearUpdate	每次机器人翻译远处时，处理一次扫描	float，default：0.5
angularUpdate	每次机器人旋转这么远处理一次扫描	float，default：0.5
temporalUpdate	如果上次扫描处理时间早于更新时间（秒），则处理扫描。小于零的值将关闭基于时间的更新	float，default：-0.1
resampleThreshold	基于Neff的重采样阈值	float，default：0.5
particles	过滤器中的粒子	int，默认值：30
xmin	初始地图大小X最小值	float，默认值：-100.0
ymin	初始地图大小Y最小值	float，默认值：-100.0
xmax	初始地图大小X最大值	float，默认值：100.0
ymax	初始地图大小Y最大值	float，默认值：100.0
delta	地图分辨率	float，默认值：0.05
IIsamplerange	可能性的平移采样范围	float，默认值：0.01
Iisamplestep	可能性的平移采样步骤	float，默认值：0.01
Iasamplerange	角度采样范围的可能性	float，默认值：0.005
Iasamplestep	角度采样步骤的可能性	float，默认值：0.005
transform_publish_period	转换发布之间的时间长度（以秒为单位）	float，默认值：0.05
occ_thersh	gmapping的占用值得阈值。具有较大占用率的单元被认为是占用的（即，在所得的sensor_msgs/LaserScan中设置为100）。	float，默认值：0.25
maxRange	传感器最大范围。如果传感器范围内没有障碍物的区域在地图中显示为自由空间，则设置maxUrange<实际传感器的最大范围<=maxRang。	float