系列文章目录
文章目录
3.5.IGE-LIO 基于强度加强的惯性雷达里程计
原文：链接
代码：github
摘要—同步定位与地图构建（SLAM）在移动机器人状态估计中起着重要作用。大多数流行的激光雷达SLAM（L-SLAM）方法仅从环境的几何结构中提取特征点，这在退化场景中可能导致定位不准确。在本文中，我们提出了一种新的激光雷达强度梯度增强型紧密耦合激光雷达-惯性里程计（IGE-LIO）框架。该框架提出了一种新的激光雷达强度梯度特征提取方法，用于精确的姿态估计，克服了L-SLAM在退化环境中面临的挑战。在计算每个激光雷达点的强度梯度后，我们动态地提取强度边缘点（IEPs）。此外，我们从纹理信息中提取几何平面点（GPPs）和几何边缘点（GEPs）。然后，对每种类型的特征点进行误差分析，并设计加权函数以纠正测量噪声，并减轻特征提取过程中引入的额外不确定性带来的偏差。随后，通过结合点到平面和点到边缘关联的残差，构建了一个迭代扩展卡尔曼滤波器（IEKF）框架。最后，在室内、室外和激光雷达退化场景中进行了广泛的实验。结果表明，与现有的几何方法相比，我们提出的方法在鲁棒性和准确性方面有了显著提高，特别是在激光雷达退化场景中。
主要贡献：
-
一种稳健的紧密耦合激光雷达几何和纹理信息与IMU的LIO框架，用于姿态估计。该框架可以为旋转激光雷达和固态激光雷达实现最优结果。
-
提出了一种新的特征提取方法，不仅提取GPPs和GEPs，还计算每个激光雷达点的强度梯度，无需校准强度值，并使用动态阈值动态选择IEPs。
-
为了区分和权衡每个特征点在姿态优化中的质量，我们引入了一种新的加权函数，联合融合几何和强度信息，用于GPPs、GEPs和IEPs。
A 离群点剔除
-
考虑到设备的尺寸和激光雷达的类型，我们排除距离激光雷达0.5 - 150米范围之外的点。
-
我们设置了强度值的离群点剔除阈值：当图像中典型范围0到255内的像素值小于15或大于240时进行剔除。尽管这些点从强度特征点的提取中被排除，但它们仍用于几何特征点的提取。
扫描二维码关注公众号，回复： 17549985 查看本文章
B 相邻点选择
为了确保计算出的激光雷达点的强度梯度更准确，首先选择激光雷达点的相邻点是至关重要的。
仅依靠扫描线无法提取每个点在多个方向上的相邻点集，导致最终的强度极值点（IEPs）仅从单一方向的边缘提取。我们使用强度图像来选择其他方向的相邻点。如图2所示，激光雷达点 p j \mathbf{p}_j pj 被转换为图像坐标 ( u , v ) (u, v) (u,v)：
φ = arctan ( y , x ) ; θ = arctan ( z , x 2 + y 2 ) u = W φ fov ( φ right − φ ) ; v = H θ fov ( θ up − θ ) \varphi = \arctan(y, x); \quad \theta = \arctan(z, \sqrt{x^{2} + y^{2}}) \\ u = \frac{W}{\varphi^{\text{fov}}}(\varphi^{\text{right}} - \varphi); \quad v = \frac{H}{\theta^{\text{fov}}}(\theta^{\text{up}} - \theta) φ=arctan(y,x);θ=arctan(z,x2+y2)u=φfovW(φright−φ);v=θfovH(θup−θ)
其中 ( R , θ , φ ) (R, \theta, \varphi) (R,θ,φ) 表示球坐标。 W W W 和 H H H 分别表示图像的宽度和高度。 φ fov = φ right − φ left \varphi^{\text{fov}} = \varphi^{\text{right}} - \varphi^{\text{left}} φfov=φright−φleft 表示激光雷达的水平视场角（FoV）， θ fov = θ up − θ down \theta^{\text{fov}} = \theta^{\text{up}} - \theta^{\text{down}} θfov=θup−θdown 表示激光雷达的垂直视场角。对于旋转式激光雷达，点 p j \mathbf{p}_j pj 的扫描线 s j s_j sj 可以表示垂直坐标，即 v = s j v = s_j v=sj。此外，水平视场角通常为360°，表示为 φ fov = 2 π \varphi^{\text{fov}} = 2\pi φfov=2π， φ right = π \varphi^{\text{right}} = \pi φright=π。像素值由强度值 I i j I_{ij} Iij 定义。
位于两个强度不同的物体边界上的激光雷达点，在一侧与另一侧之间存在较大的强度差异。如果在点 p j \mathbf{p}_j pj 的一定距离内有足够的有效点，我们选择并表示两组相邻点： P ‾ M \overline{\mathcal{P}}_M PM 表示在点 p j \mathbf{p}_j pj 一侧有 M M M 个点的相邻点集， P ‾ N \overline{\mathcal{P}}_N PN 表示在另一侧有 N N N 个点的相邻点集。此外， P ‾ M \overline{\mathcal{P}}_M PM 表示与点 p j \mathbf{p}_j pj 强度差异较大的相邻点集，而 P ‾ N \overline{\mathcal{P}}_N PN 表示与点 p j \mathbf{p}_j pj 强度相近的相邻点集。使用强度图像时，激光雷达点由相应的非零像素表示。由于旋转式激光雷达的垂直分辨率较低，只需与前一行的相邻非零像素进行比较即可，即 M = 1 M = 1 M=1 且 N = 0 N = 0 N=0，如图3(a)所示。
主流的固态激光雷达的扫描线与其他扫描线相交，能够从多个方向的边缘提取相邻点集。因此，每个点只需确定其在扫描线上的相邻点集 P ‾ M \overline{\mathcal{P}}_M PM 和 P ‾ N \overline{\mathcal{P}}_N PN，如图3©所示。
图3. 强度极值点（IEPs）提取示意图。(a)和(b)适用于旋转式激光雷达。©和(d)适用于固态激光雷达。绿色点和红色点具有不同的强度值。白色点表示空洞。黄色点和黑色点分别表示提取的强度极值点和虚拟的强度极值点。
C. 特征提取
1) 几何平面点（GPPs）和几何边缘点（GEPs）
使用与LOAM类似的方法来提取几何特征点，每个激光雷达点的平滑度计算如下：
c j = 1 ∣ N ∣ ⋅ ∥ p j ∥ ∥ ∑ p i ∈ P j , i ≠ j ( p i − p j ) ∥ c_j = \frac{1}{|N| \cdot \|\mathbf{p}_j\|} \left\|\sum_{\mathbf{p}_i \in \mathcal{P}_j, i \neq j} (\mathbf{p}_i - \mathbf{p}_j)\right\| cj=∣N∣⋅∥pj∥1
pi∈Pj,i=j∑(pi−pj)
其中 p j \mathbf{p}_j pj 表示激光雷达坐标系（ L L L 系）中的第 j j j 个点， P j \mathcal{P}_j Pj 是 p j \mathbf{p}_j pj 周围的点集， N N N 是 P j \mathcal{P}_j Pj 集合中的点数。
点的分类基于它们的平滑度，平滑度较高的点被分类为边缘特征点，平滑度较低的点被分类为平面特征点。最后，我们得到两组几何特征点：对于当前扫描， P GPP \mathcal{P}_{\text{GPP}} PGPP 表示几何平面点集， P GEP \mathcal{P}_{\text{GEP}} PGEP 表示几何边缘点集。
2) 强度极值点（IEPs）
所提出的方法基于强度梯度从边界提取强度极值点。如果一个点的强度值较大，其周围点的强度值可能会有较大的梯度波动，这使得该点更有可能被初步识别为强度极值点。该方法采用动态阈值来减少这种情况的发生。给定激光雷达点 p j \mathbf{p}_j pj 的当前强度值 η j \eta_j ηj，我们可以得到：
λ j M = f M ( η j ) = min ( C M , V M × η j ) λ j N = f N ( η j ) = max ( C N , V N × η j ) \begin{align*} \lambda_j^M &= \mathbf{f}_M(\eta_j) = \min(\mathbf{C}_M, \mathbf{V}_M \times \eta_j)\\ \lambda_j^N &= \mathbf{f}_N(\eta_j) = \max(\mathbf{C}_N, \mathbf{V}_N \times \eta_j) \end{align*} λjMλjN=fM(ηj)=min(CM,VM×ηj)=fN(ηj)=max(CN,VN×ηj)
其中 λ j M \lambda_j^M λjM 和 λ j N \lambda_j^N λjN 是用于分别确定 P ‾ M \overline{\mathcal{P}}_M PM 和 P ‾ N \overline{\mathcal{P}}_N PN 中的点是否为点 p j \mathbf{p}_j pj 的有效相邻点的阈值。 C M \mathbf{C}_M CM 和 C N \mathbf{C}_N CN 是常数， V M \mathbf{V}_M VM 和 V N \mathbf{V}_N VN 是相应的线斜率，见图4。
如果 P ‾ N \overline{\mathcal{P}}_N PN 中每个点与 p j \mathbf{p}_j pj 之间的强度差的绝对值小于 λ j N \lambda_j^N λjN，并且 P ‾ M \overline{\mathcal{P}}_M PM 中每个点与 p j \mathbf{p}_j pj 之间的强度差的绝对值大于 λ j M \lambda_j^M λjM，那么点 p j \mathbf{p}_j pj 的强度梯度可以计算为：
G j = ∣ 1 N + 1 ( η j + ∑ p ˇ m M ∈ P ˇ M η m ) − 1 M ∑ p ˇ n N ∈ P ˇ N η n ∣ G_j = \left|\frac{1}{N + 1} \left(\eta_j + \sum_{\check{\mathbf{p}}_m^M \in \check{\mathcal{P}}_M} \eta_m\right) - \frac{1}{M} \sum_{\check{\mathbf{p}}_n^N \in \check{\mathcal{P}}_N} \eta_n\right| Gj=
N+11
ηj+pˇmM∈PˇM∑ηm
−M1pˇnN∈PˇN∑ηn
其中 p ˇ m M \check{\mathbf{p}}_m^M pˇmM 和 p ˇ n N \check{\mathbf{p}}_n^N pˇnN 分别表示 P ˇ M \check{\mathcal{P}}_M PˇM 和 P ˇ N \check{\mathcal{P}}_N PˇN 中的第 m m m 个和第 n n n 个点。 G j > G max G_j > G_{\max} Gj>Gmax 的点 p j \mathbf{p}_j pj 被视为强度极值点。被视为强度极值点（IEP）的候选点，其中 G max G_{\max} Gmax 是设定的强度梯度阈值。最终提取的强度极值点通过以下公式确定：
p ‾ j = 1 2 ( p ˇ 0 M + p j ) \overline{\mathbf{p}}_j = \frac{1}{2}(\check{\mathbf{p}}_0^M + \mathbf{p}_j) pj=21(pˇ0M+pj)
其中 p ˇ 0 M \check{\mathbf{p}}_0^M pˇ0M 表示 P ˇ M \check{\mathcal{P}}_M PˇM 中最接近 p j \mathbf{p}_j pj 的点。如图3(b)和(d)所示，提取的黄色特征点偏离了虚拟黑线。
在处理完所有点后，得到当前帧最终提取的强度极值点集，记为 P IEP \mathcal{P}_{\text{IEP}} PIEP。随后，将这三种类型的特征点组合成一个新的特征点集，记为 P ˇ = { P GPP , P GEP , P IEP } \check{\mathcal{P}} = \{\mathcal{P}_{\text{GPP}}, \mathcal{P}_{\text{GEP}}, \mathcal{P}_{\text{IEP}}\} Pˇ={
PGPP,PGEP,PIEP}。
B. 状态估计
1) 反向传播
通过反向传播，我们可以得到 t j t_j tj 和 t k t_k tk 之间IMU的相对位姿，记为 k T I = ( k R I , k p I ) ^k\mathbf{T}_I = (^k\mathbf{R}_I, ^k\mathbf{p}_I) kTI=(kRI,kpI)。在这种情况下，第 j j j 个点 L p ˘ j ^L\breve{\mathbf{p}}_j Lp˘j 被投影到扫描的末尾：
k L p ˘ j = I T k − 1 I T j L T I k − 1 L p ˘ j ^L_k\breve{\mathbf{p}}_j = {}^I\mathbf{T}_k^{-1}{}^I\mathbf{T}_j {}^L\mathbf{T}_I^{k - 1}{}^L\breve{\mathbf{p}}_j kLp˘j=ITk−1ITjLTIk−1Lp˘j
其中 L p ˘ j ∈ P ˇ ^L\breve{\mathbf{p}}_j \in \check{\mathcal{P}} Lp˘j∈Pˇ， L T I k − 1 ^L\mathbf{T}_I^{k - 1} LTIk−1 表示上一次优化后得到的外参。在本文中，为了简化，将 k L p ˘ j ^L_k\breve{\mathbf{p}}_j kLp˘j 记为 L p ‾ j ^L\overline{\mathbf{p}}_j Lpj。
2) 误差分析
所提出的方法根据特征点的几何和强度信息为特征点设计单独的加权函数，并使用权值来量化点的质量，在点优化中给予高质量的特征点更大的权重。此外，我们通过参考sigmoid函数来设计加权函数。权值 w P w_P wP 随着正因子 x p x_p xp 的增加而增加，而权值 w N w_N wN 随着负因子 x n x_n xn 的增加而减小：
w P = λ 1 1.0 + e − λ 2 x p , w P ∈ ( 1 2 λ 1 , λ 1 ) w N = λ 1 2.0 − e − λ 2 x n , w N ∈ ( 1 2 λ 1 , λ 1 ) \begin{align*} w_P &= \frac{\lambda_1}{1.0 + e^{-\lambda_2x_p}}, & w_P &\in \left(\frac{1}{2}\lambda_1, \lambda_1\right)\\ w_N &= \frac{\lambda_1}{2.0 - e^{-\lambda_2x_n}}, & w_N &\in \left(\frac{1}{2}\lambda_1, \lambda_1\right) \end{align*} wPwN=1.0+e−λ2xpλ1,=2.0−e−λ2xnλ1,wPwN∈(21λ1,λ1)∈(21λ1,λ1)
其中 λ 1 \lambda_1 λ1 和 λ 2 \lambda_2 λ2 是超参数； λ 1 \lambda_1 λ1 设置为2，以将权值约束在通常的1 - 2范围内， λ 2 \lambda_2 λ2 可以控制因子变化时权值的变化率，通常设置为1。
根据平滑度的定义，点 L p ˘ j ^L\breve{\mathbf{p}}_j Lp˘j 的平滑度值 c j c_j cj 越小，表明它来自平面的可能性越高。考虑强度信息，令 P j \mathcal{P}_j Pj 表示由几何平面点（GPP）及其周围平面点组成的点集。具有几乎相同强度的平面点比具有不规则强度分布的点更有可能属于同一平面。因此，使用与文献[16]中相同的方法计算 P j \mathcal{P}_j Pj 中点的强度分布，这可以为评估平面点的质量提供加权信息：
η ˉ = 1 N ∑ L p ˘ i ∈ P j η i ; σ η 2 = 1 N − 1 ∑ L p ˘ i ∈ P j ( η i − η ˉ ) 2 \bar{\eta} = \frac{1}{N} \sum_{^L\breve{\mathbf{p}}_i \in \mathcal{P}_j} \eta_i; \sigma_{\eta}^{2} = \frac{1}{N - 1} \sum_{^L\breve{\mathbf{p}}_i \in \mathcal{P}_j} (\eta_i - \bar{\eta})^2 ηˉ=N1Lp˘i∈Pj∑ηi;ση2=N−11Lp˘i∈Pj∑(ηi−ηˉ)2
其中 η i \eta_i ηi 表示点 L p ˘ i ∈ P j ^L\breve{\mathbf{p}}_i \in \mathcal{P}_j Lp˘i∈Pj 的强度值， η ˉ \bar{\eta} ηˉ 表示相邻点集的平均强度， σ η 2 \sigma_{\eta}^{2} ση2 表示强度分布的方差。
由于平滑度对几何平面点（GPPs）的影响较大，与平滑度相关的参数 λ 2 \lambda_2 λ2 设置为2。因此，在平滑度和强度分布的综合影响下，几何平面点 w j GPP w_j^{\text{GPP}} wjGPP 的权重可以反映平面点的质量：
w j GPP = 2.0 2.0 − e − 2 σ η × 2.0 2.0 − e − c j w_j^{\text{GPP}} = \frac{2.0}{2.0 - e^{-2\sigma_{\eta}}} \times \frac{2.0}{2.0 - e^{-c_j}} wjGPP=2.0−e−2ση2.0×2.0−e−cj2.0
几何边缘点（GEPs）的提取是基于使用几何信息评估点 L p ˘ j ^L\breve{\mathbf{p}}_j Lp˘j 与其周围点之间的差异，以提取具有高平滑度的点。几何边缘点权重的公式为：
w j GEP = 2.0 2.0 − e − c j w_j^{\text{GEP}} = \frac{2.0}{2.0 - e^{-c_j}} wjGEP=2.0−e−cj2.0
真实的强度极值点（IEPs）可能存在于连接原始点 L p ˘ j ^L\breve{\mathbf{p}}_j Lp˘j 和 p ˇ 0 M \check{\mathbf{p}}_0^M pˇ0M 的直线上的任何位置。随着两点之间的距离减小，该中点接近具有更高置信度的虚拟点。此外，强度极值点的强度梯度越大，这些强度极值点位于具有显著纹理差异的真实边缘上的可能性就越高。因此，强度极值点权重 w j IEP w_j^{\text{IEP}} wjIEP 的公式为：
w j IEP = 2.0 1.0 + e − G j / 255 Δ l w_j^{\text{IEP}} = \frac{2.0}{1.0 + e^{-\frac{G_j / 255}{\Delta l}}} wjIEP=1.0+e−ΔlGj/2552.0
其中 Δ l \Delta l Δl 表示点 L p ˘ j ^L\breve{\mathbf{p}}_j Lp˘j 和 p ˇ 0 M \check{\mathbf{p}}_0^M pˇ0M 之间的距离。
为了获得更准确可靠的权重，考虑了点到激光雷达的距离的影响，加权函数为：
w j l = 2.0 1.0 + e − l j w_j^l = \frac{2.0}{1.0 + e^{-l_j}} wjl=1.0+e−lj2.0
其中 l j l_j lj 表示从 L p ˘ j ^L\breve{\mathbf{p}}_j Lp˘j 到激光雷达坐标系的距离。 w j l w_j^l wjl 是 L p ˘ j ^L\breve{\mathbf{p}}_j Lp˘j 在特征提取期间的权重。因此， L p ˘ j ^L\breve{\mathbf{p}}_j Lp˘j 的最终权重 w j w_j wj 通过 w j = w j l × w j ′ w_j = w_j^l \times w_j' wj=wjl×wj′ 计算。所设计的加权函数将最终权重值限制在一定范围内。特征点的权重越高，表示质量越高。
3) 残差计算
几何边缘点和强度极值点的处理方式类似于构建残差。假设当前迭代次数为 k k k，在当前迭代估计 x ^ k ′ \hat{\mathbf{x}}_k' x^k′ 处计算状态。 P ˇ \check{\mathcal{P}} Pˇ 中的每个特征点 L p ˘ j ′ ^L\breve{\mathbf{p}}_j' Lp˘j′ 可以从激光雷达坐标系投影到全局坐标系：
G p ˘ j ′ = G T ^ I k I T L L p ˘ j ′ ^G\breve{\mathbf{p}}_j' = {}^G\hat{\mathbf{T}}_I^k {}^I\mathbf{T}_L {}^L\breve{\mathbf{p}}_j' Gp˘j′=GT^IkITLLp˘j′
对于每个激光雷达特征点，假设其最近邻点拟合的平面或边缘代表真实的环境平面或边缘。因此，特征点与全局坐标系中最近平面或边缘在地图中的距离（如图5所示），通过在 x ^ k ′ \hat{\mathbf{x}}_k' x^k′ 处对测量值(4)进行一阶近似得到：
0 = h 1 ( x k , L n j ) ≈ h 1 ( x ^ k ′ , 0 ) + H 1 k x ~ k + v j = z j k + H 1 k x ~ k + v j \begin{align*} 0 &= \mathbf{h}_1(\mathbf{x}_k, {}^L\mathbf{n}_j) \approx \mathbf{h}_1(\hat{\mathbf{x}}_k', \mathbf{0}) + \mathbf{H}_1^k \widetilde{\mathbf{x}}_k + \mathbf{v}_j\\ &= \mathbf{z}_j^k + \mathbf{H}_1^k \widetilde{\mathbf{x}}_k + \mathbf{v}_j \end{align*} 0=h1(xk,Lnj)≈h1(x^k′,0)+H1kx
k+vj=zjk+H1kx
k+vj
其中 H 1 k \mathbf{H}_1^k H1k 是 h 1 ( x ^ k ′ ⊞ x ~ k , z j k ) \mathbf{h}_1(\hat{\mathbf{x}}_k' \boxplus \widetilde{\mathbf{x}}_k, \mathbf{z}_j^k) h1(x^k′⊞x
k,zjk) 关于 x ~ k \widetilde{\mathbf{x}}_k x
k 的雅可比矩阵， v j ∼ N ( 0 , R j ) \mathbf{v}_j \sim \mathbf{N}(0, \mathbf{R}_j) vj∼N(0,Rj) 表示来自原始测量噪声 L n j ^L\mathbf{n}_j Lnj 的噪声。然而，需要根据计算出的权重更新激光雷达测量噪声：
R ^ j = R j w j \hat{\mathbf{R}}_j = \frac{\mathbf{R}_j}{w_j} R^j=wjRj
3.6. Intensity- SLAM 针对大规模环境的强度辅助定位与建图
原文：链接
代码：github
摘要：同时定位与建图（SLAM）是根据来自激光探测与测距（LiDAR）、相机等传感器的观测，来估计机器人位置并重建环境的任务，在自动驾驶、无人机配送等机器人应用中广泛使用。传统基于LiDAR的SLAM算法主要利用场景的几何特征，却忽略了强度信息。近期一些基于深度学习的SLAM算法考虑强度特征，以端到端方式训练位姿估计网络，但数据收集工作量大，且在非训练环境中的泛化能力不明。本文将强度特征引入SLAM系统，提出一种全新的SLAM框架，同时利用几何和强度特征。该SLAM包含基于强度的前端里程计估计和基于强度的后端优化。通过在户外自动驾驶和室内仓库机器人操作场景进行大量实验，结果表明，此方法优于现有的纯几何LiDAR SLAM方法。
关键要点：
- 提出新框架：提出一种同时利用强度和几何信息进行定位估计的新型SLAM框架，涉及前端里程计估计和后端因子图优化，且该方法开源。公式表示可能涉及到各类坐标转换、优化计算等相关公式，文中未完全展开。
- 强度图与代价：提议构建强度图展示强度分布，在现有纯几何LiDAR SLAM中引入强度代价以估计机器人位置。
- 实验评估：对该方法进行全面评估，在仓库和自动驾驶场景测试，结果显示在多种场景中优于现有纯几何方法。
图1展示了所提出的方法在KITTI序列00上的定位和建图示例。定位结果显示在右下角，其中真实轨迹用红色绘制，估计轨迹用绿色绘制。与仅基于几何的方法相比，我们的方法实现了更高的定位精度。
A. 强度校准
激光雷达（LiDAR）发射激光束，并测量反射信号的到达时间以及能量。物体在传感器坐标系中的位置由发射角度和距离（即到达时间）确定。强度值由从反射信号接收到的能量与发射的激光功率之比确定。接收功率 P r P_r Pr 的物理原理可根据文献[21]确定为：
P r = P e D r 2 ρ 4 R 2 η s y s η a t m cos α (1) P_r = \frac{P_e D_r^2 \rho}{4R^2} \eta_{sys} \eta_{atm} \cos \alpha \tag{1} Pr=4R2PeDr2ρηsysηatmcosα(1)
其中， P e P_e Pe 是发射的激光束功率， D r D_r Dr 是接收器孔径直径， η s y s \eta_{sys} ηsys 是系统传输因子， η a t m \eta_{atm} ηatm 是大气传输因子， α \alpha α 是物体表面与激光束之间的入射角， ρ \rho ρ 是物体表面的材料反射率。
具体而言，距离测量值 R R R 和入射角 α \alpha α 是外部参数，而 η s y s \eta_{sys} ηsys 和 η a t m \eta_{atm} ηatm 是常数参数。因此，根据文献[21]，强度测量值 I I I 为：
I = P r P e = D r 2 ρ 4 R 2 η s y s η a t m cos α = ρ a l l cos α R 2 \begin{align*} I &= \frac{P_r}{P_e} = \frac{D_r^2 \rho}{4R^2} \eta_{sys} \eta_{atm} \cos \alpha\\ &= \frac{\rho_{all} \cos \alpha}{R^2} \tag{2} \end{align*} I=PePr=4R2Dr2ρηsysηatmcosα=R2ρallcosα(2)
其中 ρ a l l \rho_{all} ρall 是一个常数。因此，表面反射率 ρ \rho ρ 仅与入射角 α \alpha α 和测量距离 R R R 相关：
ρ ∝ I R 2 cos α (3) \rho \propto \frac{IR^2}{\cos \alpha} \tag{3} ρ∝cosαIR2(3)
对于激光雷达扫描，距离很容易测量。因此，可以通过分析局部法向量来估算入射角 α \alpha α。对于每个点 p ∈ R 3 \mathbf{p} \in \mathbb{R}^3 p∈R3，我们可以找到最近的两个点 p 1 \mathbf{p}_1 p1 和 p 2 \mathbf{p}_2 p2，使得局部表面法向量 n \mathbf{n} n 可以表示为：
n = ( p − p 1 ) × ( p − p 2 ) ∣ p − p 1 ∣ ⋅ ∣ p − p 2 ∣ (4) \mathbf{n} = \frac{(\mathbf{p} - \mathbf{p}_1) \times (\mathbf{p} - \mathbf{p}_2)}{|\mathbf{p} - \mathbf{p}_1| \cdot |\mathbf{p} - \mathbf{p}_2|} \tag{4} n=∣p−p1∣⋅∣p−p2∣(p−p1)×(p−p2)(4)
其中 × \times × 是叉积。入射角为：
cos α = p T ⋅ n ∣ p ∣ (5) \cos \alpha = \frac{\mathbf{p}^T \cdot \mathbf{n}}{|\mathbf{p}|} \tag{5} cosα=∣p∣pT⋅n(5)
因此，我们可以从具有几何读数 P P P 和强度读数 I I I 的原始激光雷达扫描中推导出校准后的强度扫描 I ~ \tilde{I} I~。具体来说，由于强度部分由距离测量值校准，因此我们仅对入射角进行校准。此外，低强度值也会导致较低的信噪比（SNR），这也会降低距离测量的准确性，如文献[27]所述。在实际应用中，低强度的点将被忽略。
图2为所提出的强度SLAM（Intensity - SLAM）系统概述。在前端，通过分析几何和强度特征来估计里程计；在后端，利用强度扫描上下文进行回环检测和全局位姿图优化。
B. 关键点选择与特征提取
本文并非仅使用几何特征，而是同时利用基于几何和强度信息的特征。校准后的强度信息包含环境的反射率分布，能够揭示不同物体的分布情况。因此，强度信息也有助于识别多个帧中的相同特征。
对于点云集合 P \mathcal{P} P 中的每个点 p i \mathbf{p}_i pi 及其强度值 η ~ i \tilde{\eta}_i η~i（ η ~ i \tilde{\eta}_i η~i 属于强度集合 I ~ \tilde{\mathcal{I}} I~） ，搜索其在点云集合 P \mathcal{P} P 中的邻近点集 N i \mathcal{N}_i Ni ，并按以下公式计算局部距离分布 σ i g \sigma_i^g σig 和强度分布 σ i I \sigma_i^I σiI：
σ i g = 1 ∣ N i ∣ ⋅ ∑ p j ∈ N i ( ∣ p i − p j ∣ ) σ i I = 1 ∣ N i ∣ ⋅ ∑ p j ∈ N i ( ∣ η ~ i − η ~ j ∣ ) \begin{align*} \sigma_i^g &= \frac{1}{|\mathcal{N}_i|} \cdot \sum_{\mathbf{p}_j \in \mathcal{N}_i} (|\mathbf{p}_i - \mathbf{p}_j|)\\ \sigma_i^I &= \frac{1}{|\mathcal{N}_i|} \cdot \sum_{\mathbf{p}_j \in \mathcal{N}_i} (|\tilde{\eta}_i - \tilde{\eta}_j|) \tag{6} \end{align*} σigσiI=∣Ni∣1⋅pj∈Ni∑(∣pi−pj∣)=∣Ni∣1⋅pj∈Ni∑(∣η~i−η~j∣)(6)
其中 ∣ N ∣ |\mathcal{N}| ∣N∣ 表示点的数量。特征基于加权求和进行选择：
σ i = w g ⋅ σ i g + w I ⋅ σ i I (7) \sigma_i = w^g \cdot \sigma_i^g + w^I \cdot \sigma_i^I \tag{7} σi=wg⋅σig+wI⋅σiI(7)
其中 w g w^g wg 和 w I w^I wI 分别是几何分布和强度分布的权重。需注意，对于像墙壁这样的平坦表面，平滑度值较小；而对于角点或边缘点，平滑度值较大。因此，在每次扫描中，从 σ \sigma σ 值较大的点中选择边缘特征 P E ∈ P \mathcal{P}_{\mathcal{E}} \in \mathcal{P} PE∈P ，从 σ \sigma σ 值较小的点中选择平面特征 P S ∈ P \mathcal{P}_{\mathcal{S}} \in \mathcal{P} PS∈P 。
C. 强度图构建
强度图 M \mathcal{M} M 包含周围环境的反射率分布。对于大多数仅基于几何的SLAM，地图通过占据栅格[28]或八叉树地图[29]来维护和更新。三维空间被分割成栅格单元，每个单元由一个概率函数表示。类似的思路可用于构建和更新强度图。这里不使用概率函数，而是使用强度观测值 I ( η k ∣ z 1 : t ) \mathcal{I}(\eta_k|z_{1:t}) I(ηk∣z1:t) 来表示每个栅格单元 m i m_i mi[26]。具体来说，对于在时间 t t t 对栅格单元的每次观测，表面反射率可按以下方式更新：
M ( m i ∣ z 1 : t ) = M ( m i ∣ z 1 : t − 1 ) + η m i − M ( m i ∣ z 1 : t − 1 ) n m i (8) \mathcal{M}(m_i|z_{1:t}) = \mathcal{M}(m_i|z_{1:t - 1}) + \frac{\eta_{m_i} - \mathcal{M}(m_i|z_{1:t - 1})}{n_{m_i}} \tag{8} M(mi∣z1:t)=M(mi∣z1:t−1)+nmiηmi−M(mi∣z1:t−1)(8)
其中 M ( m i ∣ z 1 : t ) \mathcal{M}(m_i|z_{1:t}) M(mi∣z1:t) 是当前的强度观测值， n m i n_{m_i} nmi 是在单元 m i m_i mi 上的总观测次数。需要注意的是，如果栅格单元中没有物体，由于没有信号反射，强度标记为0。
D. 扫描与地图匹配
激光里程计的任务是估计当前帧到全局地图的变换矩阵 T ∈ S E ( 3 ) \mathbf{T} \in SE(3) T∈SE(3)。最优估计可通过最小化几何误差和强度误差来实现。
1) 几何残差
与LOAM[7]类似，几何误差通过将当前边缘和平面特征与全局地图进行匹配来计算。这可以通过最小化点到边缘和点到平面的残差来实现。
给定一个边缘特征点 p i ∈ P E \mathbf{p}_i \in \mathcal{P}_{\mathcal{E}} pi∈PE 以及变换后的点 p ˘ i = T p i \breve{\mathbf{p}}_i = \mathbf{T}\mathbf{p}_i p˘i=Tpi，我们可以从全局地图中搜索两个最近的点 p i 1 \mathbf{p}_i^1 pi1 和 p i 2 \mathbf{p}_i^2 pi2。点到边缘的残差定义为：
f E ( p ˘ i ) = ( p ˘ i − p i 1 ) × ( p ˘ i − p i 2 ) ∣ p i 1 − p i 2 ∣ (9) f_{\mathcal{E}}(\breve{\mathbf{p}}_i) = \frac{(\breve{\mathbf{p}}_i - \mathbf{p}_i^1) \times (\breve{\mathbf{p}}_i - \mathbf{p}_i^2)}{|\mathbf{p}_i^1 - \mathbf{p}_i^2|} \tag{9} fE(p˘i)=∣pi1−pi2∣(p˘i−pi1)×(p˘i−pi2)(9)
类似地，给定一个平面特征点 p i ∈ P S \mathbf{p}_i \in \mathcal{P}_{\mathcal{S}} pi∈PS 以及变换后的点 p ˘ i \breve{\mathbf{p}}_i p˘i，我们可以从全局地图中搜索三个最近的点 p i 1 \mathbf{p}_i^1 pi1， p i 2 \mathbf{p}_i^2 pi2 和 p i 3 \mathbf{p}_i^3 pi3。点到平面的残差定义为：
f S ( p ˘ i ) = ( p ˘ i − p i 1 ) T ⋅ ( p i 1 − p i 2 ) × ( p i 1 − p i 3 ) ∣ ( p i 1 − p i 2 ) × ( p i 1 − p i 3 ) ∣ (10) f_{\mathcal{S}}(\breve{\mathbf{p}}_i) = (\breve{\mathbf{p}}_i - \mathbf{p}_i^1)^T \cdot \frac{(\mathbf{p}_i^1 - \mathbf{p}_i^2) \times (\mathbf{p}_i^1 - \mathbf{p}_i^3)}{|(\mathbf{p}_i^1 - \mathbf{p}_i^2) \times (\mathbf{p}_i^1 - \mathbf{p}_i^3)|} \tag{10} fS(p˘i)=(p˘i−pi1)T⋅∣(pi1−pi2)×(pi1−pi3)∣(pi1−pi2)×(pi1−pi3)(10)
2) 强度残差
强度残差通过将特征与强度图进行匹配来计算。这可以通过最小化当前点 p i \mathbf{p}_i pi（包括边缘特征和平面特征）与强度图中变换后的点 p ˘ i \breve{\mathbf{p}}_i p˘i 之间的强度残差来实现：
f I ( p ˘ i ) = η i − M ( p ˘ i ) (11) f_{\mathcal{I}}(\breve{\mathbf{p}}_i) = \eta_i - \mathcal{M}(\breve{\mathbf{p}}_i) \tag{11} fI(p˘i)=ηi−M(p˘i)(11)
其中 M ( p ˘ i ) \mathcal{M}(\breve{\mathbf{p}}_i) M(p˘i) 是强度图 M \mathcal{M} M 中的点强度值。
为了在强度图中引入强度信息，虽然用最近的栅格单元来表示强度更直接，但在栅格分辨率较低的大规模建图中，强度信息不太准确。对于每个变换后的点 p ˘ i = [ x i , y i , z i ] T \breve{\mathbf{p}}_i = [x_i, y_i, z_i]^T p˘i=[xi,yi,zi]T，我们可以找到周围的八个栅格单元。设 M 1 ( p ˘ i ) , M 2 ( p ˘ i ) , … , M 8 ( p ˘ i ) \mathbf{M}_1(\breve{\mathbf{p}}_i), \mathbf{M}_2(\breve{\mathbf{p}}_i), \ldots, \mathbf{M}_8(\breve{\mathbf{p}}_i) M1(p˘i),M2(p˘i),…,M8(p˘i) 为这些栅格单元的强度测量值。设单元1的中心位置为 p 1 = [ x 1 , y 1 , z 1 ] T \mathbf{p}_1 = [x_1, y_1, z_1]^T p1=[x1,y1,z1]T，单元8（离原点最远）的中心位置为 p 2 = [ x 2 , y 2 , z 2 ] T \mathbf{p}_2 = [x_2, y_2, z_2]^T p2=[x2,y2,z2]T，其中 x 2 − x 1 x_2 - x_1 x2−x1， y 2 − y 1 y_2 - y_1 y2−y1 和 z 2 − z 1 z_2 - z_1 z2−z1 分别是每个栅格单元的宽度、高度和深度。目标点的强度估计通过线性插值计算：
M 12 ( p ˘ i ) = x i − x 1 x 2 − x 1 ⋅ M 1 ( p ˘ i ) + x 2 − x i x 2 − x 1 ⋅ M 2 ( p ˘ i ) \mathcal{M}_{12}(\breve{\mathbf{p}}_i) = \frac{x_i - x_1}{x_2 - x_1} \cdot \mathcal{M}_1(\breve{\mathbf{p}}_i) + \frac{x_2 - x_i}{x_2 - x_1} \cdot \mathcal{M}_2(\breve{\mathbf{p}}_i) M12(p˘i)=x2−x1xi−x1⋅M1(p˘i)+x2−x1x2−xi⋅M2(p˘i)
类似地，我们可以推导出 M 34 \mathcal{M}_{34} M34， M 56 \mathcal{M}_{56} M56 和 M 78 \mathcal{M}_{78} M78。强度值估计如下：
M 9 ( p ^ i ) = y i − y 1 y 2 − y 1 ⋅ M 12 ( p ^ i ) + y i − y 1 y 2 − y 1 ⋅ M 34 ( p ^ i ) M 10 ( p ^ i ) = y i − y 1 y 2 − y 1 ⋅ M 56 ( p ^ i ) + y i − y 1 y 2 − y 1 ⋅ M 78 ( p ^ i ) M ( p ^ i ) = z i − z 1 z 2 − z 1 ⋅ M 9 ( p ^ i ) + z i − z 1 z 2 − z 1 ⋅ M 10 ( p ^ i ) \begin{align*} \mathcal{M}_9(\hat{\mathbf{p}}_i) &= \frac{y_i - y_1}{y_2 - y_1} \cdot \mathcal{M}_{12}(\hat{\mathbf{p}}_i) + \frac{y_i - y_1}{y_2 - y_1} \cdot \mathcal{M}_{34}(\hat{\mathbf{p}}_i)\\ \mathcal{M}_{10}(\hat{\mathbf{p}}_i) &= \frac{y_i - y_1}{y_2 - y_1} \cdot \mathcal{M}_{56}(\hat{\mathbf{p}}_i) + \frac{y_i - y_1}{y_2 - y_1} \cdot \mathcal{M}_{78}(\hat{\mathbf{p}}_i)\\ \mathcal{M}(\hat{\mathbf{p}}_i) &= \frac{z_i - z_1}{z_2 - z_1} \cdot \mathcal{M}_9(\hat{\mathbf{p}}_i) + \frac{z_i - z_1}{z_2 - z_1} \cdot \mathcal{M}_{10}(\hat{\mathbf{p}}_i) \end{align*} M9(p^i)M10(p^i)M(p^i)=y2−y1yi−y1⋅M12(p^i)+y2−y1yi−y1⋅M34(p^i)=y2−y1yi−y1⋅M56(p^i)+y2−y1yi−y1⋅M78(p^i)=z2−z1zi−z1⋅M9(p^i)+z2−z1zi−z1⋅M10(p^i)
3) 位姿估计
最终的位姿可以通过最小化几何残差和强度残差来估计：
T ∗ = arg min T ∗ ∑ p i ∈ P E ∣ f E ( p ^ i ) ∣ + ∑ p i ∈ P S ∣ f S ( p ^ i ) ∣ + ∑ p i ∈ P ∣ f I ( p ^ i ) ∣ (13) \mathbf{T}^*=\arg \min_{\mathbf{T}^*} \sum_{\mathbf{p}_i \in \mathcal{P}_{\mathcal{E}}} |f_{\mathcal{E}}(\hat{\mathbf{p}}_i)| + \sum_{\mathbf{p}_i \in \mathcal{P}_{\mathcal{S}}} |f_{\mathcal{S}}(\hat{\mathbf{p}}_i)| + \sum_{\mathbf{p}_i \in \mathcal{P}} |f_{\mathcal{I}}(\hat{\mathbf{p}}_i)| \tag{13} T∗=argT∗minpi∈PE∑∣fE(p^i)∣+pi∈PS∑∣fS(p^i)∣+pi∈P∑∣fI(p^i)∣(13)
可以使用列文伯格 - 马夸尔特（Levenberg - Marquardt）算法[30]来求解。注意，初始位姿对齐是通过假设恒定的角速度和线速度来估计的，与假设相同的变换矩阵相比，这可以提高收敛速度。
E. 回环检测与全局优化
回环检测的目标是从历史数据中识别出再次访问的场景。对于仅前端的SLAM系统，由于测量噪声而产生漂移是不可避免的。为了减少定位漂移，后端SLAM通过识别相同的地方来检测回环。我们使用帧选择来减少检索时间，因为计算回环检测的成本通常很高。关键帧根据以下标准选择：(1) 机器人的位移是显著的，即大于一个预定义的阈值；(2) 旋转角度变化显著；(3) 经过的时间超过了一定的周期。对于大规模环境，设置更高的阈值以降低计算成本。所有关键帧都存储到后端维护的位姿图中。
对于每个关键帧，我们使用强度扫描上下文（Intensity Scan Context, ISC）[9] 将当前帧提取为一个全局描述符。与仅基于几何的描述符（如LiDAR3D - COM[31]、NBLD[32]和Scan Context[33]）相比，ISC在识别回环时对旋转变化具有鲁棒性。ISC是一个二维矩阵，通过将方位角和径向方向均匀地划分为 N s N_s Ns 个扇区和 N r N_r Nr 个环来计算。每个子空间由该区域内点的最大强度表示。
给定一个关键帧，我们可以从强度和几何信息中提取ISC描述符 Ω \Omega Ω。为了比较一个高质量的ISC描述符 Ω q \Omega^q Ωq 和一个候选ISC描述符 Ω c \Omega^c Ωc，设 v i q \mathbf{v}_i^q viq 和 v i c \mathbf{v}_i^c vic 分别为 Ω q \Omega^q Ωq 和 Ω c \Omega^c Ωc 的第 i i i 列。相似性得分 φ ( Ω q , Ω c ) \varphi(\Omega^q, \Omega^c) φ(Ωq,Ωc) 通过取每个扇区的平均余弦距离得到：
φ ( Ω q , Ω c ) = 1 N s ∑ i = 0 N s − 1 ( v i q ⋅ v i c ∥ v i q ∥ ⋅ ∥ v i c ∥ ) \varphi(\Omega^q, \Omega^c) = \frac{1}{N_s} \sum_{i = 0}^{N_s - 1} \left(\frac{\mathbf{v}_i^q \cdot \mathbf{v}_i^c}{\|\mathbf{v}_i^q\| \cdot \|\mathbf{v}_i^c\|}\right) φ(Ωq,Ωc)=Ns1i=0∑Ns−1(∥viq∥⋅∥vic∥viq⋅vic)
可以通过对 φ ( Ω q , Ω c ) \varphi(\Omega^q, \Omega^c) φ(Ωq,Ωc) 设置阈值来确定回环。
回环检测能够有效地识别回环对。然而，误检测可能导致位姿图优化失败。为了防止误报，使用几何一致性验证来检查候选帧的相似性。对于候选回环帧，我们从全局地图中搜索附近的线和平面信息。对于当前帧，提取边缘和平面特征，并通过最小化点到边缘和点到平面的距离与相应的全局线和平面进行匹配。当两帧不相关时，距离总和通常很高。因此，可以通过设置阈值来过滤误报。
通过识别出再次访问的地方，我们可以在两帧之间添加边，并且可以应用全局优化来纠正漂移[34]。