一种多组多线激光雷达的自动标定算法的制作方法

本发明涉及一种多组多线激光雷达的自动标定算法。
背景技术：
：在基于多线激光雷达扫描的三维重建与SLAM应用中，多传感器的数据融合往往能给算法带来更好的鲁棒性与精度，同时能抓取更多的环境三维细节信息，为进一步处理带来更完善的空间数据。但是，多传感器系统中，传感器系统均存在自身的局部坐标，为了统一坐标系，我们必须要进行传感器之间的坐标系标定，找到传感器之间的三维坐标变换关系。为了解决此问题，人们提出了许多的方法和技术。最早采用的是手动测量以及用外部仪器测量等物理方法来寻找雷达传感器之间的坐标关系，这些方法较为简单，但是操作比较复杂且精度较低。为了提高精度，人们研究了采用特制的靶标来帮助获取雷达之间的共视图，从而进行更精确标定的相关技术。靶标标定法在一定程度上提高了标定的精度，但是需要特制带有特定几何特征的靶标，操作较为复杂，特别地，在比较稀疏的单帧雷达点云中，很难在雷达的共视数据中找到完全对应的靶标点。因此，如何精确地标定三维激光雷达之间的坐标成为了一个亟待解决的关键技术。技术实现要素：本发明的目的在于克服上述现有技术的不足，提供一种多组多线激光雷达的自动标定算法。为实现上述目的，本发明采用以下技术方案：一种多组多线激光雷达的自动标定算法，包括以下步骤：S1、基于激光雷达A与激光雷达B的位置关系，估计激光雷达A与激光雷达B的坐标变换关系初值Tguess；S2、利用地图构建算法对激光雷达A所采集的点云数据进行局部子地图M的构建；S3、获取激光雷达A在地图构建算法中时间0-N的运动轨迹激光雷达A在n时刻的点云数据以及激光雷达B在n时刻的点云数据基于数据同步和轨迹同步的假设，将点云数据通过与Tguess变换到时刻n在局部子地图M中的位置，并通过最近邻点云搜索算法找到其在局部子地图M中的附近点，则有，式中，ApproxNearestNeighbors(*)为最近邻点云搜索算法；S4、基于环境一致性约束，求解激光雷达A与激光雷达B的多组标定关系具体为，对0-N时刻中的每组点云分别求取与的距离最小值，则有，......则，优选地，还包括步骤S5、根据随机采样一致性规则滤除异常样本点,则有最后取均值求出Tcali。优选地，步骤S1中激光雷达A与激光雷达B的坐标变换关系初值Tguess为手工测量获得的粗略估计值。优选地，步骤S2中构建局部子地图M的地图构建算法为基于Generalized-ICP的帧间匹配SLAM算法。采用上述技术方案后，本发明与
背景技术：
相比，具有如下优点：基于激光雷达本身获取的点云数据来产生一个概率三维模型，利用该模型本身的约束找到传感器之间的坐标变换关系统计模型，并采用随机采样一致性规则消除配准过程中的歧义性问题，获得精确的坐标变换，完成多组多线雷达的自动标定。附图说明图1为双线雷达三维扫描系统；图2为图1中雷达A的坐标系表示；图3为实施例1中公视点手动配准方法的雷达及靶标布置示意图；图4为实施例1中公视点手动配准方法获取的点云数据示意图图5为实施例2中雷达A与雷达B的自动标定示意图；图6为实施例2中自动标定算法流程框图；图7为实施例2中第一组扫描数据的矩阵曲线；图8为实施例2中第二组扫描数据的矩阵曲线。具体实施方式为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。如图1所示的是一种获取构建地图所需点云数据的双线雷达三维扫描系统，由于单颗雷达的视野较窄，因而通过两颗雷达进行交叉安装，来同步获取更完整的三维数据。其中，雷达A置于水平上方，雷达B以45°角安装于水平雷达的下方。如图2所示的是图1中雷达A的坐标系表示。在上述系统中，激光雷达A与激光雷达B均存在自身的局部坐标，为了统一坐标系，我们必须要进行传感器之间的坐标系标定，找到传感器之间的三维坐标变换关系。实施例1采用手动标定方式进行激光雷达A与激光雷达B的标定。如图3所示，采用公视点手动配准方法，对雷达公视区域进行方形孔靶标放置，由于雷达公视区域较小，为了保证靶标采样点数的密集，我们设置靶标孔大小为200x200mm，并将靶标A与B分别放置在系统左右边大约一米位置。如图4所示，得到两组点云。由图4可以看到，多线雷达的点云非常稀疏，难以在两组点云公视的方块孔靶标中找到对应点，所以我们采用RANSAC拟合(RANSAC拟合属于现有技术，在此不做赘述)的方式，从中拟合出两个方形，在方形角点上找到对应点进行坐标变换，最后计算的变换矩阵就是两组点云的标定矩阵。使用靶标点，来对手动标定得到的估计标定矩阵进行误差分析，(计算手动标定后的靶标对应点之间的平均位移距离及旋转角度距离和总体最小均方误差)能够得到X,Y,Z，Roll，Yaw，Pitch的误差及均方误差，两组实验误差如下：TABLE1SceneIDX_error(mm)Y_error(mm)Z_error(mm)Root_mean_error(mm)S115.89620.2717.37726.795S221.96023.38910.25733.682TABLE2SceneIDRoll_error(Radian)Yaw_error(Radian)Pitch_error(Radian)Root_mean_error(Radian)S10.0530.0600.0820.114S20.0660.0130.0960.116实施例2采用多组多线激光雷达自动标定算法对图1所示的双线雷达三维扫描系统进行自动标定。将系统放置在任意结构化场景中进行数据采集，本实施例使用走廊环境，两组场景。每组场景1次行走，总共2组数据，每组数据分别采集550帧点云数据。如图5所示，采用地图构建算法对雷达A采集到的点云数据进行短时建图，获得子地图M，雷达B的数据经过初值旋转和同步位姿，变换到雷达A的地图对应位置附近，而后进行自动标定与修正。具体步骤如图6所示：S1、基于激光雷达A与激光雷达B的位置关系，估计激光雷达A与激光雷达B的坐标变换关系初值Tguess。坐标变换关系初值Tguess为手工测量或其它方式获得的估计值。S2、利用地图构建算法对激光雷达A所采集的点云数据进行局部子地图M的构建。本实施例中采用的地图构建算法为基于Generalized-ICP的帧间匹配SLAM算法。S3、获取激光雷达A在地图构建算法中时间0-N的运动轨迹激光雷达A在n时刻的点云数据以及激光雷达B在n时刻的点云数据基于数据同步(双雷达的同步数据获取中，外部的环境在雷达共视图中的公共区域是同构的)及轨迹同步(双雷达的同步数据获取中，两个雷达的运动轨迹在同一个时间内是相同的)的假设，将点云数据通过与Tguess变换到时刻n在局部子地图M中的位置，并通过最近邻点云搜索算法找到其在局部子地图M中的附近点，则有，式中，ApproxNearestNeighbors(*)为最近邻点云搜索算法；S4、基于环境一致性约束，求解激光雷达A与激光雷达B的多组标定关系具体为，对0-N时刻中的每组点云分别求取与的距离最小值，则有，......则，由于本实施例采用的是ICP算法，ICP算法在没有明显特征(比如走廊环境)的单一环境的时候容易陷入局部极小值，导致优化后的解不是最优解而是陷入局部极小值的次优解，最终导致配准算法的失败。因而，在步骤S4求解Tcali之后，还包括步骤S5。S5、根据随机采样一致性规则滤除异常样本点，则有最终在滤除异常样本点以后使用均值估计最终标定矩阵。如图7与图8所示的分别是2组数据矩阵曲线，其中，横坐标为550帧点云数据，纵坐标为X、Y、Z、Roll、Pitch以及Yaw六组数据(分别对应图中从上到下的六条曲线)，这六组数据整体均趋向于一个常数，图中所示的波动点即ICP配准失败的异常点。如此，滤除所述异常样本点后，即得到了两组估计矩阵，使用实施例1实验环境中的靶标点来对自动标定得到的估计标定矩阵进行误差分析(计算手动标定后的靶标对应点之间的平均位移距离及旋转角度距离和总体最小均方误差)能够得到X,Y,Z，Roll，Yaw，Pitch的误差及均方误差，两组实验误差如下：TABLE3SceneIDX_error(mm)Y_error(mm)Z_error(mm)Root_mean_error(mm)S16.3725.9433.1299.258S27.8775.2766.13411.291TABLE4SceneIDRoll_error(Radian)Yaw_error(Radian)Pitch_error(Radian)Root_mean_error(Radian)S10.0180.0220.0370.046S20.0290.0260.0240.045可以看出，实施例2所采用的自动标定算法，其位移标定精度与姿态标定精度均由于实施例1中的传统手工靶标标定精度，显示出了优异的性能。实施例2中的自动标定算法只需要根据激光雷达传感器的同步数据，就能自动标定雷达之间的坐标关系，高效快捷，并且本方法能推广到多雷达系统的标定中。以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本
技术领域：
的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。当前第1页1 2 3