Loam_livox 论文阅读

最新推荐文章于 2022-12-16 21:25:20 发布

磊磊哈哈

最新推荐文章于 2022-12-16 21:25:20 发布

阅读量367

点赞数 1

原文链接：https://editor.csdn.net/md?articleId=115306945

版权

激光slam 学习专栏收录该内容

28 篇文章 43 订阅

订阅专栏

本文介绍了固态激光雷达（LiDAR）在特征提取和运动补偿方面的方法，包括点云处理中的特征点提取、去除不可靠点、帧内运动补偿。同时，针对动态物体，提出了动态物体滤波策略，通过迭代优化和残差检查来排除运动影响，从而提高定位和建图的准确性。

摘要由CSDN通过智能技术生成

摘要

相较于传统机械式旋转雷达，固态激光雷达的视场角固定,每帧特征点少，容易意识特征匹配退化，受到局部最小值影响（移动的物体）
livox的非重复扫描：扫描路径不重复
运动畸变：连续工作的激光在不同时间的取样造成
雷达的特征边、平面、VFH 垂直直方图

点云提取

距离
$D(P)=\sqrt{x^2+y^2+z^2}$
水平夹角
$\phi(P)=\tan^{-1}(\sqrt{(y^2+z^2)/x^2})$
垂直夹角
$\phi(P)=atan2(y,z)/PI*180\degree$
强度
$R : 激光反射强度$
$I(P)=R/D(P)^2$
激光与平面的夹角

见上图： $P_b$ 与局部平面的夹角
$P_{ac}=P_a-P_c$
$\theta(P_b)=cos^{-1}\frac{P_{ac}P_b}{|P_{ac}||P_b|}$
移除不可靠的点
（1）移除视场角边缘点，防止进行特征提取是不可靠（对于MID40：视场角：38.4,剔除 $\theta(P_b)>17\degree$ ）
（2）移除具有过大或过小的激光反射强度,(MID $I(P)\leq 7×10^{-3},I(P)\geq 1×10^{-1}$ )
强度过大：电路失真/饱和，距离失真强度多小：噪声影响大
（3）剔除入射角在 $0或\pi$ 附近的，见上图点f，在取平均值时，测量值受大值影响较大，不能代表区域的特性。（MID， $\theta(P)\leq 6\degree,\theta(P)\geq 175\degree$ ）
（4）隐藏点：比如点 $e$ ,在边特征提取时会导致问题，如何确定隐藏点：
$|P_e-P_d|\geq 0.1|P_e| 且|P_e |>|P_d|$
特征提取
由于特征点过少、FOV限制点，使用反射强度作为第4维变量，当一个点与周围点具有不同的反射强度值时，认为这个点时边缘点特征（材料变更导致），可以解决推行性变化，比如门和窗。

边到边的残差方程构建

边到边
$\xi_k:$ 当前坐标系边缘点计
$\xi_m:$ map坐标系边缘点
$p_{1,..,5}$ :每一个 $\xi_k$ 中的点在 $\xi_m$ 中找到其最临近的5个点（PCL kd树最临近搜索，并采用并行加速）,假设有个一点 $P_l\in \xi_k$ ,为了找到其在map中的临近点，需要将其映射到map:公式如下:

$R_k,T_k$ :当前帧最后一个点的位姿
$P_w=R_kP_l+T_k$
使用点到直线距离构建残差方程优化位姿
为了确保 $P_w$ 相邻的5个点在一条直线上，计算其均值 $u$ 和协防差 $\sum$ ,并且对协防差进行矩阵分解，如果最大的的特征值是第二大值的3倍，认为这5个点在一条直线上，之后构建残差方程如下(在P到直线距离)
$r_e2c=\frac{|P_w-P_5|×|P_w-P_1|}{|P_5-P_1|}$
4 . 面到面的残差
点 $P_w$ 为平面上的某点，在map上得到与其最临近的5个点，计算方差矩阵 $\sum$ ,求解特征值，最小的特征值小于第二个小特征值三倍时，认为存在一个平面，构建点到面的最小距离：
$r_{p2p}=\frac{(P_w-P_1)^T((P_3-P_5)×(P_3-P_1))}{|(P_3-P_5)×(P_3-P_1)|}$

帧内运动补偿

线性插值
$T_k,R_k:$ 当前帧位姿
$T_{k-1},R_{k-1}$ 前一帧位姿
存在如下关系:
$R_k=R_{k-1}R_{k-1}^k,T_k=R_{k-1}T_{k-1}^k+T_{k-1}$
对于 $t\in[t_{k-1},t_k]$ ,令 $s=\frac{t-t_{k-1}}{t_{k}-t_{k-1}}$ ,t时间的位姿为:
$\theta:标量$
$w : 单位向量（旋转向量）$
$\hat w:w的斜对称矩阵（w×）$
$R^t_{k-1}=e^{\hat w\theta s},T^t_{k-1}=sT^k_{k-1}$
根据罗德里德旋转公式可以得到:
$R_{k-1}^t=I+\hat w\sin(s\theta)+\hat w^2(1-\cos(s\theta))$
只需要计算cos与sin值就可以得到每一点相对于当前帧的位姿：
$P_w(t)=R_tP_t+T_t\\ R_t=R_{k-1}R_{k-1}^t,T_t=R_{k-1}T^T_{k-1}+T_{k-1}$

动态物体滤波

在每一次迭代时，重新查找每一个特征点的最邻点，并且重新构建两个残差，并执行少量的位姿优化迭代操作，最后得到残差，移除其中残差超过20%的值，最后进行位姿优化。
迭代循环
（1） $P_w=R_kP_l+T_k$
（2） $r_e2c=\frac{|P_w-P_5|×|P_w-P_1|}{|P_5-P_1|}$
（3） $r_{p2p}=\frac{(P_w-P_1)^T((P_3-P_5)×(P_3-P_1))}{|(P_3-P_5)×(P_3-P_1)|}$

#输入 	当前帧的边缘点集ξk和平面点集Pk,
#	 	地图点的边缘点集ξm和平面点集Pm
#		雷达前一帧的位姿Rk-1,Tk-1
#输出 	当前雷达的位姿R_k,T_k

开始：
for 位姿迭代不收敛 do
	for 对于点Pl 属于ξk
	计算残差（1）
	找到其ξm上对应的5个点P1-P5
		if(P1-P5) 在一条直线上
			计算直线之间残差（2）
	for 属于Pk的点Pl
		计算残差（1）
		在Pm上找到5个最临近点P1-P5
		if(P1-P5) 在一个平面上
			计算面与面之间的距离（3）
	进行位姿优化迭代2次
	重新计算残差re2c和rp2p,移除其中大于残差20%的值
	for 迭代最大次数 do
		if 非线性迭代不收点 then
			break
end