一种激光雷达自标定方法、系统、设备及存储介质与流程

本技术实施例涉及自动驾驶，特别涉及一种激光雷达自标定方法、系统、设备及存储介质。

背景技术：

1、传感器标定是传感器应用的前提。标定的质量极大程度上影响了自动驾驶感知、定位、决策、规划、控制等模块。任何传感器在制造、安装之后都需要通过特定方法进行标定，以保证传感器符合设计指标，并尽可能提高测量值的准确性。传感器在安装到自动驾驶汽车上之后，需要对其安装位置进行标定；同时，在车辆行驶过程中，由于震动等原因，会导致传感器位置与原位置产生偏离，因此有必要每隔一定的时间对传感器进行校准。

2、激光雷达由于分辨率高，直接获得三维信息，抗干扰能力强，是目前自动驾驶车辆上常配备的一种传感器。激光雷达整车安装角度发生偏移主要由两方面引起：1)安装激光雷达下线时受到工艺和安装工人操作的影响；2)激光雷达在长时间使用过程中会受到碰撞，挤压，颠簸等因素影响。如何修正这个偏移角度对于使用激光雷达至关重要。

3、现有的激光雷达在线标定方法多数集中于多雷达之间的外参的标定，或雷达与其他种类传感器之间的外参标定。对于激光雷达与车体之间的标定，当前主要使用的方法依赖于精准的安装方式，在初始安装时标定激光雷达和车体坐标系的外参。外参的平移部分可通过激光测距仪等测量工具测得，且对最终输出的结果精度影响较小。外参中的旋转部分可通过提取标定间墙面和地面的点云，估计地平面，墙面的法向量来获得。这些方法都依赖于特定的标定间和标定工具，不能解决外参在使用过程中出现变化，或初始标定不准需要校正的情况。

技术实现思路

1、本技术实施例提供一种激光雷达自标定方法、系统、设备及存储介质，能够在车辆运行过程中自动标定激光雷达到车辆的三个旋转角，同时保证标定精度。

2、为解决上述技术问题，第一方面，本技术实施例提供一种激光雷达自标定方法，包括以下步骤：对于激光雷达采集到的一帧点云，拟合得到地平面方程；基于地平面方程，得到地面的法向量方向；并基于所述法向量方向与z轴方向的夹角，计算翻滚角和俯仰角；获取激光雷达轨迹，并基于所述激光雷达轨迹，得到各个时刻车辆前进方向与激光雷达向前的方向的差值；基于所述差值，得到偏置角。

3、在一些示例性实施例中，采用迭代的随机抽样一致方法拟合出地平面方程，包括：步骤s101、从点云中随机选三个点，若三个点能构成三角形，且三角形的面积超过面积阈值，则计算所述三个点构成的平面方程；步骤s102、遍历点云中所有点，计算点到所述三个点构成的平面的距离；根据所述距离重新划分内点和外点；若所述距离小于距离阈值，则点为内点；统计点云中内点的数量；步骤s103、重复步骤s101和步骤s102，直至点云中内点的比例超过比例阈值，停止重复，并将点云中内点的比例超过比例阈值时的平面作为地平面初值；步骤s104、根据所述地平面初值，重新计算内点；使用奇异值分解重新拟合所述初始时刻的平面，得到地平面方程。

4、在一些示例性实施例中，采用最低点表示方法拟合出地平面方程，包括：将雷达的测量点表示为p(x，y，z，i)，雷达的测量点包括点的三维坐标和反射强度；将所有的测量点按照z值从小到大进行排序；从z值最小的点开始遍历，假设当前点的索引为i，点的总数为n；若第i个点与第i+k*n个点的z值相差小于预先设定的阈值，表明i点可能为地平面的最低点，用pl来表示；其中，k为比例系数，取0.001；删除测量点中所有比pl的z值更小的点；从z值最小的点开始，统计一定数量的点的z值平均值，作为初始地面高度；基于初始地面高度，得到内点；所述内点与初始地面高度接近；采用随机抽样一致方法迭代拟合平面，得到地平面方程。

5、在一些示例性实施例中，基于地平面方程，得到地面的法向量方向；并基于所述法向量方向与z轴方向的夹角，计算翻滚角和俯仰角；地平面方程表示为：

6、ax+by+cz+d＝0

7、地面的法向量方向表示为：

8、

9、其中，dp为平面的截距；

10、基于法向量与z轴方向的夹角，计算翻滚角和俯仰角，如下式所示：

11、

12、

13、

14、

15、

16、其中，θ为法向量与z轴方向的夹角，roll表示翻滚角，pitch表示俯仰角。

17、在一些示例性实施例中，在计算得到翻滚角和俯仰角之后，在获取激光雷达轨迹之前，还包括：根据平面的截距在z轴方向的投影，计算激光雷达的高度；激光雷达的高度表示为：

18、

19、其中，h为激光雷达的高度，dp为平面的截距，c为地面的法向量方向的z轴坐标值。

20、在一些示例性实施例中，在计算得到翻滚角和俯仰角之后，将点云旋转至雷达坐标系的z轴与地面z轴一致；获取激光雷达轨迹，并基于所述激光雷达轨迹，得到各个时刻车辆前进方向与激光雷达向前的方向的差值，包括：采用同步定位与地图构建算法，估计出各个时刻的激光雷达6自由度位姿；将每个时刻的位姿ti表示为ti＝{xi，yi，zi，φi，θi，ψi}；在获得激光雷达每个时刻的位姿后，采用b样条插值法从离散的x，y位置拟合得到x，y相对于时间的连续函数；基于x，y相对于时间的连续函数，得到x，y相对于时间的导数；基于x，y相对于时间的导数，计算得到车辆前进的速度方向；基于车辆前进的速度方向与激光雷达向前的方向，得到车辆前进的速度方向与激光雷达向前的方向之间的差值；x，y相对于时间的连续函数分别表示为：

21、x(t)＝bspline({xi}，{ti})

22、y(t)＝bspline({yi}，{ti})

23、其中，x，y相对于时间的导数表示为：

24、

25、其中，x′i，y′i分别为x，y相对于时间t的导数，x′i，y′i分别表示车辆在x，y方向的分速度方向；车辆前进的速度方向表示为：

26、

27、其中，为车辆前进的速度方向，车辆前进的速度方向为车辆坐标系向前的方向；位姿ti＝{xi，yi，zi，φi，θi，ψi}中位姿的角度ψi为激光雷达向前的方向。

28、在一些示例性实施例中，在得到各个时刻车辆前进方向与激光雷达向前的方向的差值之后，基于所述差值，得到偏置角；偏置角的计算公式表示为：

29、

30、其中，s为过滤后的数据点集，所述过滤后的数据点集通过去除车辆运动速度小于预设速度的数据以及车辆行驶过程中轨迹曲率大于预设轨迹曲率的数据得到。

31、第二方面，本技术实施例还提供了一种激光雷达自标定系统，包括依次连接的地平面方程获取模块以及计算模块；地平面方程获取模块用于对激光雷达采集到的一帧点云，拟合得到地平面方程；计算模块用于根据地平面方程，得到地面的法向量方向；并基于所述法向量方向与z轴方向的夹角，计算翻滚角和俯仰角；获取激光雷达轨迹，并基于所述激光雷达轨迹，得到各个时刻车辆前进方向与激光雷达向前的方向的差值；基于所述差值，得到偏置角。

32、另外，本技术还提供了一种电子设备，包括：至少一个处理器；以及，与至少一个处理器通信连接的存储器；其中，存储器存储有可被至少一个处理器执行的指令，指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述激光雷达自标定方法。

33、另外，本技术还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时实现上述激光雷达自标定方法。

34、本技术实施例提供的技术方案至少具有以下优点：

35、本技术实施例提供一种激光雷达自标定方法、系统、设备及存储介质，该方法包括以下步骤：对于激光雷达采集到的一帧点云，拟合得到地平面方程；基于地平面方程，得到地面的法向量方向；并基于法向量方向与z轴方向的夹角，计算翻滚角和俯仰角；获取激光雷达轨迹，并基于激光雷达轨迹，得到各个时刻车辆前进方向与激光雷达向前的方向的差值；基于差值，得到偏置角。

36、本技术提出一种全新的激光雷达自标定算法，该算法首先拟合出地平面方程以标定翻滚角(roll角)和俯仰角(pitch角)；用激光同步定位与地图构建算法得到激光雷达轨迹，获得各个时刻车辆前进方向与雷达向前方向的差值，标定偏置角(yaw角)。本技术提供的激光雷达自标定方法，能够在车辆运行过程中自动标定激光雷达到车辆的三个旋转角，标定精度达到了0.3°以内；同时该方法避免了精确的安装要求，可以修正安装不当导致的误差，不需要专门的标定间，只需要在道路上正常行驶就可以完成自标定。