基于ROI的NDT重定位激光雷达视场设置方法
期刊：MDPI
单位：山东理工大学
摘要: 在高精度地图自动导航的重定位图中，激光雷达布放和视场选择起到检测车辆相对位置和位姿的作用。当激光雷达视场被遮挡或激光雷达位置错位时，很容易导致重定位丢失或重定位精度低。针对遮挡视场过大时NDT重定位误差大、位置丢失的问题，提出一种基于高精度地图正态分布变换（NDT）重定位的激光雷达布局与视场选择方法。为了模拟真实的放置环境以及激光雷达被障碍物遮挡的情况，采用ROI算法对激光雷达点云进行切割，得到不同尺寸的激光雷达点云数据。切点云数据首先被下采样，然后重新定位。 NDT重定位的下采样点被记录为有效匹配点。使用 RMSE 值和有效匹配点优化 LiDAR 点云数据的方向和角度设置。结果表明，在路况复杂的城市场景中，单位角度内前后匹配点数量多于左右匹配点数量。 NDT重定位算法的匹配点越多，重定位精度越高。增加前后激光雷达视野可防止重新定位丢失。通过增加左右激光雷达视场可以提高重定位精度。
关键词：正态分布变换；高精度地图；自动导航
1. 简介
在过去的二十年里，迁移在汽车驾驶应用中发挥了至关重要的作用。重定位用于根据构建的地图获取车辆的全球位置和位姿。利用全球导航卫星系统（GNSS）[1-3]、惯性测量单元（IMU）[4,5]、摄像头、激光雷达和其他传感传感器，各种重新定位方法已成功解决此类问题。 GNSS 和 IMU 的集成是解决这些问题的常见解决方案[6-8]。然而，GNSS 很容易出现信号干扰或丢失的情况，例如在城市建筑中。当前的解决方案是使用里程信息来补偿 GNSS 测量结果。然而，在自动驾驶相关应用中的性能仍然普遍不足。相机因其固有的高信息内容、低成本和小尺寸而成为最有吸引力的重定位传感器之一[9-11]。视觉重定位利用摄像头提供的大量信息来估计机器人位置。由于在GPS信号较弱的情况下需要准确可靠的定位，郑志勇提出了一种新的陆地车辆定位视觉测量框架[12]。然而，对于户外大规模搬迁来说，确保其稳健运行仍然非常具有挑战性，尤其是在不断变化的环境和恶劣的天气条件下。 Kaixin Yang提出了一种由语义信息和概率数据关联组成的协调定位策略，提高了动态交通环境中SLAM（同步定位和建图）的准确性[13]。然而，城市中的许多墙壁和窗户都是由玻璃或乙烯基制成的，在白天很容易暴露在光线下。
解决城市问题的一个越来越流行的解决方案是完全停止使用 GNSS 和相机测量，而依靠激光雷达来测量环境的 3D 扫描。与相机和GNSS相比，激光雷达具有更好的穿透性和抗干扰特性。 LiDAR 使用距离测量来检测物体和表面，距离测量是根据反射光脉冲的飞行时间计算得出的[14-16]。此外，激光雷达系统获取的数据量比相机少。因此，激光雷达传感器经常用于定位和物体检测。然而，大多数采用现有点云地图的基于激光雷达的定位解决方案都假设道路场景相对恒定，因此新建筑、路边植被、物体变化造成的部分遮挡可能会严重损害其鲁棒性。因此，一个有趣但悬而未决的问题是激光雷达是否可以用于大规模变化环境中的鲁棒重定位。
在大规模、动态的城市环境中，基于高精度地图的正态分布变换（NDT）算法是汽车驾驶应用中广泛使用的主要重定位算法之一[17,18]。与 SLAM 算法相比，NDT 算法对于大型点云更有效。 NDT配准算法耗时且稳定，与初始值相关性很小，并且在初始误差较大时能够很好地进行修正。名古屋大学开源的Autoware算法主要采用NDT算法。 Autoware算法利用NDT匹配配准算法来获取自身在LiDAR点云地图中的位置和位置信息。然而，对于环境的显着几何变化或过于意外或动态的物体，NDT 算法并不稳健[19,20]。这些缺点严重影响了基于高精度地图的扫描匹配的性能。 NDT重定位算法可以处理一些环境变化，例如物体的意外遮挡（LiDAR传感器被树叶或其他车辆部分遮挡），这在城市中是不可避免的。当环境与地图存在差异时，无损检测的定位精度会降低。在实际应用中，多采用多激光雷达进行定位。激光雷达的位置直接决定了激光雷达的视场角。激光雷达视场的不同视角直接影响车辆的定位精度。许多研究并未具体评估不同位置的点云遮挡与基于高精度地图的无创检测性能之间的关系。
2. 算法原理
2.1. Loam
LOAM（Lidar odometry and Mapping in Real Time）是 Ji Zhu 等人提出的一种基于 3D LiDAR 的高精度实时定位与建图算法。 [21,22]。 LOAM框架的核心分为两部分，高频里程计和低频映射，如图1所示。
图1 LOAM算法的系统结构图。
里程计通过高频、少量的点云进行扫描匹配，估计两帧之间的运动关系，并将结果输出给建图算法；使用扫描地图匹配方法 [23,24]，映射以 1 Hz 的频率将未失真的点云与地图进行匹配和对齐。最后，综合两种算法创建的姿态变换，得到LiDAR的变换输出，其对地图的姿态约为10 Hz。我们首先从 LiDAR 云中提取特征点 pk。
我们选择锐边和平表面块上的特征点。令 i 为 pk 中的一个点，i ∈ pk 并令 S 为 LiDAR 扫描仪在同一扫描中返回的 i 的连续点的集合。这定义了一个评估局部表面平滑度的术语：
本文根据上述公式计算点的曲率。实际中，我们只需要比较一个点的曲率，就可以在一个点周围五个点之差的平方坐标中求出该点的曲率。这样，我们就可以求出每个点的曲率c，通过比较曲率，我们可以选择曲率较大的边缘点和曲率较小的平面点。为了防止特征点聚类，将每个扫描点云分为四部分，其中选择曲率最大的两个点作为边缘点，选择曲率最小的四个点作为平面点。在选择点时，我们要避免选择已经选择的点周围的点或激光雷达线接近平行平面的点，这些点通常被认为是不可靠的，因为它们无法随时看到。我们还希望避免可能的模糊点。
里程计算法可估计 LiDAR 在扫描过程中的运动。令 tk 为扫描 k 的开始时间。在每次扫描结束时，扫描期间感知的点云 pk 被重新投影到时间戳 tk+1。我们将重新投影的点云表示为 _ pk。在下一次扫描期间，k + 1, _ pk 与新接收的点云 pk+1 一起使用来估计 LiDAR 的运动。接下来就是寻找对应关系，即匹配两个点云的特征点； pk+1 的角点与 _ pk 的角线重合，pk+1 的平面点与 _ pk 的平面重合。有了点到线和点到面的对应关系，我们就可以计算出点到线和点到面的距离：
然后我们就可以求出该平面点到对应平面的距离：
LiDAR 运动在扫描过程中以恒定的角速度和线速度进行建模。这使我们能够在扫描内线性插值不同时间接收到的点的姿态变换。如果我们让 t 为当前时间戳，并回想一下 tk+1 是扫描 k + 1 的开始时间，则线性插值公式为
为了获得本帧数据中的点与前一帧数据中的点的对应关系，我们使用旋转矩阵R和平移量T
由于旋转矩阵的求导非常复杂，因此通过Rodrigues公式将旋转矩阵R展开如下：
这使得推导旋转矩阵变得容易。
现在我们有了点到线和点到面的距离，就可以得到优化的误差函数：
f 中的每条线代表一个特征点。下一个要求是求解雅可比矩阵。最后采用LM方法进行优化：
由于上一步的解是根据本地激光雷达观测坐标系TL得到的结果，因此它解决了相邻帧之间的变换。然而，为了同时进行定位和建图，需要解决全局坐标系TW下的变换。因此，当我们获得相邻几帧的姿态变换信息时，需要将其与全局地图进行匹配，并将其添加到全局地图中。
最后，通过使用rviz等软件，将激光雷达里程计解决方案获得的姿态信息与建图获得的信息进行转换和整合。
2.2. NDT重定位算法
为了识别LiDAR在离线地图中的位置，我们将LiDAR扫描的点云与离线地图的点云进行比较。在重定位过程中，LiDAR 扫描的点云可能与离线地图的点云不同，这可能是因为 LiDAR 视野被遮挡，或者是因为车辆仅使用了部分 LiDAR 点云。对于有偏差的地图重定位，我们使用NDT对齐算法，该算法不比较两个点云之间的差异，而是将参考点云地图转换为多维变量的正态分布[25-27]。如果变换参数能够使两组 LiDAR 数据良好匹配，则参考系统中变换点的概率密度会很大。因此，可以考虑采用优化方法来找到使两组LiDAR点云数据最匹配时概率密度之和最大化的变换参数。
第一步是将3D离线点云图网格化，使用小立方体划分扫描点的整个空间，对于每个网格，根据网格内的点计算其概率密度函数。这可以描述为：
其中 → μ 为离线地图网格正态分布的均值，m 表示离线地图网格中的点数，q 表示离线地图网格中的第 q 个点，y → q=1,... ,m 为离线地图网格中所有扫描点，Σ 表示离线地图网格的协方差矩阵。网格的概率密度函数可以描述为：
使用正态分布来表示离散的离线点云图有很多好处。这种分块、平滑的表示是连续可导的，每个晶格的概率密度函数可以被认为是局部表面的近似，它不仅描述了表面在空间中的位置，而且还包含有关表面的方向和平滑度的信息。表面。
使用 NDT 对齐时，目标是找到当前 LiDAR 扫描的位姿，以最大化当前扫描点位于离线地图表面上的可能性。然后我们需要优化的参数是当前扫描的 LiDAR 点云的变换（旋转、平移等），我们使用变换参数 → h 来描述。当前扫描是点云 X = {→x 1, . 。 。 , →x n}，给定扫描点集合 X 和变换参数 → h ，使得空间变换函数 T(→ h , →x q) 表示使用位姿变换 → h 来移动点 →x q ，组合与之前的一组状态密度函数（每个网格的概率密度函数），那么最佳变换参数 → h 应该是最大化似然函数的位姿变换：
那么，最大化似然也相当于最小化负对数似然 − log θ；
然后使用优化算法来调整变换参数 → h 以最小化这种负对数似然。 YNDT算法使用牛顿法进行参数优化。这里概率密度函数 f (→x ) 不必服从正态分布；任何反映扫描表面结构信息并对异常扫描点具有鲁棒性的概率密度函数就足够了。
2.3.点云数据预处理
x轴正方向为激光雷达的前方，y轴正方向为激光雷达的左侧，如图2所示。通过提取不同的激光雷达点云区域ROI（感兴趣区域），在前、后、左、右预留不同大小的激光雷达点云区域，模拟激光雷达视场不同程度的变化。可以描绘 ROI 以进行进一步处理。每个区域的LiDAR点云可以描述为：
LiDAR点云区域的前、后、左、右分别用ff(α)、fb(α)、fl(α)、fr(α)表示。激光雷达点云内部的点用(a,b)表示，α为切割激光雷达点云的视角，β为0°到90°，如图3所示。
体素下采样根据输入点云数据创建 3D 体素网格（将体素网格视为空间 3D 立方体的集合）[28-31]。然后在每个体素（3D立方体）内，体素中的其他点通过体素中所有点的重心来近似，使得体素中的所有点都由一个重心点表示。体素下采样根据输入点云数据创建 3D 体素网格（将体素网格视为空间 3D 立方体的集合）。然后，在每个体素（3D 立方体）内，使用体素的重心来近似体素中的所有点，以揭示体素中的其他点，以便体素中的所有点都由单个重心点表示。从而减少了点的数量，提高了匹配速度，点云的形状特征基本保持不变，并且保留了空间结构信息。体素网格选择越大，采样的点云越小，处理速度越快；然而，原始点云会过于模糊。较小的体素网格选择将产生相反的效果。同时，需要记录解采样后不同LiDAR点云数据的点数，如图4所示。
3. KITTI数据集测试
为了测试LiDAR不同视场对NDT重定位算法的影响，我们使用全长864.831 m、持续时间117 s的KITTI数据集。测试平台是一辆配备 Velodyne HDL-64E 的车辆。所有实验均在该平台上进行。车辆的平均速度约为2.5 m/s。所有评估实验均在配备 AMD R7-4800H 处理器、16 GB RAM 和单个 NVIDIA GeForce GTX1650ti GPU 的计算机上运行。 Velodyne HDL-64E 是直接安装在移动底盘上方的 64 线数字激光雷达，具有 360° 水平视场、5–15 Hz 旋转速度、26.8° 垂直视场（+2° 至 -24.8°） ，垂直角分辨率0.4°，水平角分辨率0.08°，点云计数高达130万点/秒，最大范围100m，测距精度±2cm。如图5所示，利用loam算法对KITTI数据集构建了高精度地图。 NDT匹配点是原始点云数据经过ROI处理和下采样后的点，如图6所示。由于体素下采样后原始点云密度不均匀，每个方向的NDT匹配点数量为也不规则，因此各个方向的无损检测匹配点的定位精度是不同的。本文代码设置的下采样因子为3.0。变换epsilon为0.05，步长为0.1，分辨率为2.0，最大迭代次数为30。本文采用均方根误差（RMSE）来衡量定位精度。 RMSE 是预测值与真实值之间的偏差的平方与观测值数量之比的平方。
如表1所示，比较了不同方向90°、180°、270°激光雷达点云重定位轨迹得到的RMSE。可见，在同一方向上，激光雷达的视角越大，NDT算法的匹配点越有效，定位精度越高，误差越小。当激光雷达左右视场角为90度时，无法重新定位激光雷达。当前后激光雷达视场角为90°时，可以重新定位。当激光雷达四个方向视场角放大时，正面和背面的精度提升较大，而左右两侧的精度提升较小。
为了进一步探讨LiDAR匹配点云方位和NDT匹配点数量对点云中定位精度和权重的影响，将定位轨迹漂移的影响在中间90°扩展到270° ° 位于左侧和右侧。实验以30度不同角度进行。如表2所示，通过比较激光雷达左右两侧NDT匹配点的数量可以看出，由于正面和后面的NDT匹配点云数量较多，并且在90°左右°~270°，1°时，NDT前后匹配点数量的比例对定位轨迹的精度起着决定性作用。同时也证明NDT匹配点越多，定位精度越高，漂移程度越小。正常驾驶模式下，由于雷达点云前后的点云比较丰富，因此建议多使用前后点云。
在无损检测匹配点数量大致的情况下，只需要较小的激光雷达视场角即可使用之前的无损检测匹配点。正面NDT匹配点也存在较大误差。这体现了前后端NDT匹配点的鲁棒性。
4.城市数据集测试
配备64线激光雷达的履带式车辆在真实环境中使用，如图7所示。这里使用的激光雷达是Ouster OS1-64，测量范围为150 m，精度为±2 cm ，垂直视角30°，水平视角360°，垂直角度分辨率0.52°，水平角度分辨率0.09°，旋转速率10Hz。此次测试是在特征明显的复杂城市环境中进行的。如图8所示，履带的左右两侧有弯路、直路，且有明显的大型障碍物。数据集中的道路长度为1秒，持续时间为23秒。所有评估实验均在配备 AMD R7-4800H 处理器、16 GB RAM 和单个 NVIDIA GeForce GTX1650ti GPU 的计算机上运行。
从图9和图10可以看出，当NDT匹配点数量小于100时，重定位误差会急剧增大，当NDT匹配点数量小于50时，极易丢失定位。每帧NDT匹配数的理论值设置为100。由于NDT匹配点的数量为3109，因此NDT匹配点的数量为249,2189。因此，无损检测有效匹配数不能低于总数的12.4%，且单帧NDT有效匹配不能少于100个。然而，路段的复杂性（有直线路段和弯路路段）导致NDT匹配点相对于点云帧体积的波动。根据实际实验，得到了可靠的范围，为14.7%~16.3%，如表4所示。
从图9和图10可以看出，每组数据都有4个拐点，变化较大。如图11和图12所示，随着车辆转弯，两侧激光雷达效果匹配点数量急剧减少，导致重定位精度下降，如图13和图14所示。右侧变大，有效匹配点的数量急剧增加，从而导致更高的重定位精度，如图15和图16所示。
如图17和图18所示，在正常的直线段中，障碍物分布不均匀且距离较近。轻松缩小左右LiDAR视场视觉的影响，导致NDT有效匹配点数量在50到100之间，导致重定位精度降低，甚至丢失位置。在正常直线段，由于前后激光雷达数据大部分为地面点，重定位精度受到严重影响。左右激光雷达点多为有效特征点，特征丰富。如图19和图20所示，在直线段，正面和背面激光雷达数据的定位精度较差。如图21和图22所示，左右两侧LiDAR数据的定位效果良好。
5.结论
本文提出了一种基于高精度地图NDT重定位的LiDAR布局和视场选择方法，以解决视场过大被遮挡时NDT重定位误差大和位置丢失的问题。为了模拟真实的放置环境和遮挡的LiDAR，采用ROI算法对LiDAR点云进行切割，得到不同尺寸的LiDAR点云数据。一、切入点云数据被下采样，然后重新定位。 NDT重定位的下采样点被记录为有效匹配点。使用 RMSE 值和有效匹配点来优化 LiDAR 点云数据的方向和角度设置。结果表明，在路况复杂的城市场景中，单位角度内前后匹配点数量多于左右匹配点数量。 NDT重定位算法的匹配点越多，重定位精度越高。增加前后 LiDAR 视野可防止重新定位丢失。单帧激光雷达数据的有效匹配点大于设定阈值。通过增加左右激光雷达的视场，可以提高重定位精度。您还需要与两侧的障碍物保持安全距离。未来的研究计划是改进NDT算法，以加快其处理速度，提高定位精度，并使其能够在激光雷达数据稀疏的情况下进行重新定位。标签：ROI,Based,定位,Relocation,LiDAR,点云,匹配,NDT,激光雷达 From： https://www.cnblogs.com/Gaowaly/p/18542405