CN110244284B - 一种用于多线激光雷达和gps\ins标定的标定板及其方法 - Google Patents

Abstract

本发明公开了一种用于多线激光雷达和GPS\INS标定的标定板及其方法，标定板正面中心嵌有GPS测量天线，标定板背面安装有定位模块与数据传输模块。方法包括：系统保持静止，获取一组时间同步的雷达坐标{Pi}和大地坐标{Pj}；使用RANSAC算法获得旋转矩阵Rs和平移矩阵Ts；使用{Rs，Ts}结合GPS\INS得到旋转矩阵R₀和平移矩阵T₀；系统保持运动，获取一组时间同步的雷达坐标{Pn}和大地坐标{Pm}，同时记录站心坐标系的姿态和大地坐标；根据上述坐标并结合初值{R₀,T₀}，计算{Pn}在大地坐标系下的坐标{Pn}’，构建目标函数；通过SVD求解超定方程得到优化后的旋转矩阵R和平移矩阵T。本发明的优点在于可以在室外环境中，为多线激光雷达和GPS\INS提供与现有技术相比更加精确的标定结果。

Description

一种用于多线激光雷达和GPS\INS标定的标定板及其方法

技术领域

本发明涉及机器人视觉与多传感器融合领域，具体涉及一种用于多线激光雷达和GPS\INS标定的标定板及其方法。

背景技术

随着自动化领域的不断发展，越来愈多的传感器被安装在各种智能系统上，多传感器的融合逐渐成为了研究热点。在传感器中激光雷达的扫描范围可达100米以上，且单帧的点云数据输出较为稀疏，在场景建模一类的应用中，通常需要将多帧点云数据进行拼接，这时则需要知道每帧点云数据之间的相对外参，如果是在一些导航之类的应用中，还需要知道点云数据的绝对坐标。在以GPS\INS组合导航模块为定位核心的系统中，以基准站所处的位置为原点，东北天为XYZ轴的坐标系称为大地坐标系，GPS\INS模块自身的坐标系称为站心坐标系，其中站心坐标系到大地坐标系的外参关系可以直接由GPS\INS模块给出。由于在系统中通常各个传感器与GPS\INS模块之间为刚性连接，当完成雷达坐标系与站心坐标系之间的相对外参标定后，则可得到各帧点云数据在大地坐标系下的绝对坐标，从而满足各种应用的需求。

目前的激光雷达和GPS\INS标定算法中，通常是针对其中的IMU模块和激光雷达之间进行标定，由IMU进行积分得到站心坐标系之间的姿态变换，而雷达坐标系之间的姿态变换通常使用ICP算法进行点云配准。这种类型的方法过于依赖周边环境，在室内效果较好，在室外由于点云约束不强，导致标定结果较差。

发明内容

本发明为了克服现有标定算法精度较低的不足，提出了一种用于多线激光雷达和GPS\INS标定的标定板及其方法。

本发明的上述技术目的是通过以下技术方案得以实现的：

一种用于多线激光雷达和GPS\INS标定的标定板，其特征在于，所述标定板正面中心嵌有GPS测量天线，所述标定板背面安装有定位模块与数据传输模块。

一种用于多线激光雷达和GPS\INS标定的方法，其特征在于，包括如下步骤：

步骤一：系统保持静止，获取一组时间同步的标定板中心雷达坐标{Pi}和大地坐标{Pj}；

步骤二：使用RANSAC算法获得雷达坐标系到大地坐标系的旋转矩阵Rs和平移矩阵Ts；

步骤三：使用{Rs，Ts}结合GPS\INS给出的系统姿态和大地坐标得到雷达坐标系到站心坐标系的旋转矩阵R₀和平移矩阵T₀；

步骤四：系统保持运动，获取一组时间同步的标定板中心雷达坐标{Pn}和大地坐标{Pm}，同时记录站心坐标系的姿态和大地坐标；

步骤五：根据站心坐标系姿态和大地坐标并结合初值{R₀,T₀}，可以计算{Pn}在大地坐标系下的坐标{Pn}’，构建目标函数E=min||{Pn}’ - {Pm}||；

步骤六：通过SVD求解超定方程得到优化后的雷达坐标系到站心坐标系的旋转矩阵R和平移矩阵T。

优选的，所述步骤一中获取一组时间同步的标定板中心雷达坐标{Pi}和大地坐标{Pj}具体步骤如下：

(1)使用RANSAC算法拟合近地面，得到近地面方程后，根据点到面的距离公式和距离阈值，将点云分为地面点与非地面点；

(2)利用欧氏距离作为判据对非地面点进行聚类；

(3)去除点数小于阈值的聚类，对剩下聚类利用RANSAC算法拟合平面，去除距离平面大于阈值的点；

(4)对各个聚类使用PCA算法投影至XY平面，令所有点的z坐标为0，则可形成多组二维点集；

(5)对各个二维点集使用旋转卡尺算法，得到点集的最小外接矩形；

(6)将各个外接矩形的长宽与标定板长宽进行匹配，求取匹配成功的矩形中心坐标，与上一记录中心坐标比较，如果角度差或距离差大于阈值，则记录为雷达坐标Pi；

(7)通过标定板数据传输模块获取时间同步后的标定板中心大地坐标Pj。

优选的，所述步骤四中获取一组时间同步的标定板中心雷达坐标{Pn}和大地坐标{Pm}的具体步骤与步骤一相同。

优选的，所述（2）具体步骤如下：

①将点云使用KDTree结构保存为kd；

②从点云中随机选取一点P，并建立队列D；

③以某一距离阈值Td，在kd中寻找到队列首的点距离小于Td的点，将所有的未曾寻找到的点加入到队列中；

④使用BFS算法，重复S102-3，直至不能寻找到新的点，将所有寻找到的点划分为一个聚类；

⑤重复S102-2~S102-4，直到点云中所有的点都被划分为某一聚类。

优选的，所述（4）中PCA投影的详细步骤如下：

1)首先求取聚类C点云的质心，；

2)将聚类C所有点减去质心，；

3)将所有点构成一个维的矩阵，则有；

4)求解的特征向量与特征值；

5)将特征向量以一定顺序排序组成一个的矩阵，则矩阵即为将聚类点云投影至XY平面的投影矩阵。

本发明技术方案的有益效果在于设计了一种用于多线激光雷达和GPS\INS标定的标定板及其方法，可以在室外环境中，为多线激光雷达和GPS\INS提供与现有技术相比更加精确的标定结果。

附图说明

图1为本发明实施例提供的一种用于多线激光雷达和GPS\INS标定的标定板结构示意图；

图2为本发明实施例提供的一种系统架构示意图；

图3为本发明实施例提供的激光雷达和GPS\INS标定的方法流程图；

图4为本发明实施例提供的标定板中心雷达坐标提取流程图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明，本实施例不构成对本发明的限制。

如图1所示为本发明设计的一种用于多线激光雷达和GPS\INS标定的标定板，标定板1正面内嵌有GPS测量天线2，测量天线2的虚拟中心大致位于标定板1正平面上。模块3为RTK定位模块，可通过差分定位技术输出厘米级的标定板1中心大地坐标。模块4为数据传输模块，负责将标定板1中心大地坐标传输到系统核心处理器配合其它数据进行标定。

如图2所示为本发明在港口自动化场景中的一种实例，其中基准站1负责将采集的载波相位发给流动站，其坐标系A称为大地坐标系。激光雷达2安装于集装箱吊具下部，从周围环境中获取点云数据，其坐标系B称为雷达坐标系。模块3为GPS\INS模块，负责提供系统实时在大地坐标系下的位置与姿态，其坐标系称为站心坐标系。其中激光雷达与GPS\INS模块为刚性连接，对其进行的标定相当于求雷达坐标系与站心坐标系之间的空间变换矩阵。

所述激光雷达和GPS\INS标定的方法流程图如图3所示，标定详细部分包括以下步骤：

利用PPS信号和GPRMC数据结合插值算法对激光雷达数据点进行授时，部分激光雷达带有授时系统，只需要将GPRMC数据和PPS信号输入到雷达模块中，能直接输出授时后的点云数据。如果雷达不带有授时系统，则需要在PPS信号与GPRMC信号到来时，对激光雷达当前Scan进行授时，其后的Scan授时根据雷达扫描周期进行插值计算。

步骤一：让系统保持静止，人拿着标定板正面朝向雷达，以雷达中心为原点，保持在雷达探测范围内，围绕其行走一周，逐帧对点云数据进行下列处理：

S101.使用RANSAC算法拟合近地面，得到近地面方程后，根据点到面的距离公式和距离阈值，将点云分为地面点与非地面点；

S102.利用欧氏距离作为判据对非地面点进行聚类，详细步骤如下：

S102-1.将点云使用KDTree结构保存为kd；

S102-2.从点云中随机选取一点P，并建立队列D；

S102-3.以某一距离阈值Td，在kd中寻找到队列首的点距离小于Td的点，将所有的未曾寻找到的点加入到队列中；

S102-4.使用BFS算法，重复S102-3，直至不能寻找到新的点，将所有寻找到的点划分为一个聚类；

S102-5.重复S102-2~S102-4，直到点云中所有的点都被划分为某一聚类；

S103.去除点数小于阈值的聚类，对剩下聚类利用RANSAC算法拟合平面，去除距离平面大于阈值的点；

S104.对各个聚类使用PCA算法投影至XY平面，令所有点的z坐标为0，则可形成多组二维点集，PCA投影的详细步骤如下：

S104-1.首先求取聚类C点云的质心，；

S104-2.将聚类C所有点减去质心，；

S104-3.将所有点构成一个维的矩阵，则有；

S104-4.求解的特征向量与特征值；

S104-5.将特征向量以一定顺序排序组成一个的矩阵，则矩阵即为将聚类点云投影至XY平面的投影矩阵。

S105.对各个二维点集使用旋转卡尺算法，得到点集的最小外接矩形；

S106.将各个外接矩形的长宽与标定板长宽进行匹配，求取匹配成功的矩形中心坐标，与上一记录中心坐标比较，如果角度差或距离差大于阈值，则记录为雷达坐标Pi；

S107. 通过标定板数据传输模块获取时间同步后的标定板中心大地坐标Pj；

步骤二：利用上述步骤获取的一组点集{Pi, Pj}，求取点集的流程图如图4所示。通过求解空间相似变换方程和RANSAC算法，得到静止状态的雷达坐标系到大地坐标系的旋转矩阵Rs和平移矩阵Ts；

步骤三：结合静止状态的GPS\INS给出的站心姿态Rs，Ps，Ys和大地坐标Xs，Ys，Zs计算得到雷达坐标系到站心坐标系的旋转矩阵和平移矩阵作为预测初值。

步骤四：获得预测初值后，让吊具保持缓速，执行变幅、回转等运动，人拿着标定板在雷达探测范围内移动，逐帧对点云数据进行下列处理：

S401.以帧首的点云数据为当前帧的雷达坐标系，记录帧首时间,利用时间同步后的IMU数据结合与对当前帧点云数据逐Scan进行位置校正与角度校正，获取校正后的点云数据；

S402.同S101~S107获得一对标定板中心的雷达坐标Pn和大地坐标Pm；

S403.记录对应时刻的GPS\INS给出的站心姿态R，P，Y和大地坐标X，Y，Z；

步骤五：利用上述步骤获取的一组点集{Pn ，Pm}，根据对应时刻的站心姿态和大地坐标并结合初值{，}，可以计算{Pn}在大地坐标系下的坐标{Pn}’，构建目标函数E=min||{Pn}’- {Pm}||。

步骤六：通过SVD求解超定方程得到优化后的雷达坐标系到站心坐标系的旋转矩阵R和平移矩阵T，至此标定流程结束。

以上所述的以港口自动化场景为例的具体实施例仅仅是为了说明本发明的技术方案，不能被用于限定本发明的具体实施范围，凡依照本发明的范围在不同系统上的实施、变化或改进，均应该属于本专利的涵盖范围之内。

Claims (5)

1.一种用于多线激光雷达和GPS\INS标定的方法，其特征在于，用于多线激光雷达和GPS\INS标定方法的标定板正面中心嵌有GPS测量天线，所述标定板背面安装有定位模块与数据传输模块；

标定方法包括如下步骤：

步骤一：系统保持静止，获取一组时间同步的标定板中心雷达坐标{Pi}和大地坐标{Pj}；

步骤二：使用RANSAC算法获得雷达坐标系到大地坐标系的旋转矩阵Rs和平移矩阵Ts；

步骤三：使用{Rs，Ts}结合GPS\INS给出的系统姿态和大地坐标得到雷达坐标系到站心坐标系的旋转矩阵R₀和平移矩阵T₀；

步骤四：系统保持运动，获取一组时间同步的标定板中心雷达坐标{Pn}和大地坐标{Pm}，同时记录站心坐标系的姿态和大地坐标；

步骤五：根据站心坐标系姿态和大地坐标并结合初值{R₀,T₀}，可以计算{Pn}在大地坐标系下的坐标{Pn}’，构建目标函数E＝min||{Pn}’-{Pm}||；

步骤六：通过SVD求解超定方程得到优化后的雷达坐标系到站心坐标系的旋转矩阵R和平移矩阵T。

2.根据权利要求1所述的一种用于多线激光雷达和GPS\INS标定的方法，其特征在于：所述步骤一中获取一组时间同步的标定板中心雷达坐标{Pi}和大地坐标{Pj}具体步骤如下：

(1)使用RANSAC算法拟合近地面，得到近地面方程后，根据点到面的距离公式和距离阈值，将点云分为地面点与非地面点；

(2)利用欧氏距离作为判据对非地面点进行聚类；

(3)去除点数小于阈值的聚类，对剩下聚类利用RANSAC算法拟合平面，去除距离平面大于阈值的点；

(4)对各个聚类使用PCA算法投影至XY平面，令所有点的z坐标为0，则可形成多组二维点集；

(5)对各个二维点集使用旋转卡尺算法，得到点集的最小外接矩形；

(6)将各个外接矩形的长宽与标定板长宽进行匹配，求取匹配成功的矩形中心坐标，与上一记录中心坐标比较，如果角度差或距离差大于阈值，则记录为雷达坐标Pi；

(7)通过标定板数据传输模块获取时间同步后的标定板中心大地坐标Pj。

3.根据权利要求2所述的一种用于多线激光雷达和GPS\INS标定的方法，其特征在于：所述步骤四中获取一组时间同步的标定板中心雷达坐标{Pn}和大地坐标{Pm}的具体步骤与步骤一相同。

4.根据权利要求2所述的一种用于多线激光雷达和GPS\INS标定的方法，其特征在于：所述(2)具体步骤如下：

①将点云使用KDTree结构保存为kd；

②从点云中随机选取一点P，并建立队列D；

③以某一距离阈值Td，在kd中寻找到队列首的点距离小于Td的点，将所有的未曾寻找到的点加入到队列中；

④使用BFS算法，重复②和③，直至不能寻找到新的点，将所有寻找到的点划分为一个聚类；

⑤重复②～④，直到点云中所有的点都被划分为某一聚类。

5.根据权利要求2所述的一种用于多线激光雷达和GPS\INS标定的方法，其特征在于：所述(4)中PCA投影的详细步骤如下：

1)首先求取聚类C点云的质心P_m，

2)将聚类C所有点减去质心P_m，P′_i＝P_i-P_m；

3)将所有点构成一个3×n维的矩阵M，则有Cov＝MM^T；

4)求解Cov的特征向量与特征值；

5)将特征向量以一定顺序排序组成一个3×3的矩阵M_pca，则矩阵M_pca即为将聚类点云投影至XY平面的投影矩阵。

Images