CN109932706A - 一种无人装卸物流装备系统激光雷达标定系统及标定方法 - Google Patents

Abstract

本发明公开了一种无人装卸物流装备系统激光雷达标定系统及标定方法，该标定方法包括以下步骤：使所述激光雷达在龙门桁架上指定的位置原地旋转，获取激光雷达的扫描数据，即获得激光雷达坐标系S₁下的周围环境数据；操作机械臂获得机械坐标系S₂下标定板中心点坐标；利用改进的RANSAC算法提取激光雷达坐标系S₁下标定板中心点坐标；计算激光雷达坐标系S₁和机械坐标系S₂之间的旋转矩阵R和平移矩阵T。本发明公开的标定系统及标定方法能实现大场景下激光雷达的标定，且标定效率高，标定速度快。

Description

一种无人装卸物流装备系统激光雷达标定系统及标定方法

技术领域

本发明涉及激光探测领域，特别涉及一种无人装卸物流装备系统激光雷达标定系统及标定方法。

背景技术

激光雷达是以发射激光光束来探测目标的位置、速度等特征量的雷达系统。在工业环境下，利用激光雷达可以有效获取空间周围信息。为了实现对目标物体精确定位，需对激光雷达进行精确的标定，从而提高雷达的测距精度以及确保激光雷达在实际工作中的可靠性。

但由于目前市场上的激光雷达单圈扫描采集的点较为稀疏，在无人装卸场景下，通过传统方法标定得到的精度较低，无法满足实际应用需求，因此本发明提出一种基于龙门桁架的无人装卸物流装备系统激光雷达标定方法，以适应在工业环境下的激光雷达标定，使得标定精度更高，效率更快。

发明内容

为解决上述技术问题，本发明提供了一种无人装卸物流装备系统激光雷达标定系统及标定方法，以达到能够实现大场景下激光雷达的标定，并提高激光雷达的标定精度的目的。

为达到上述目的，本发明的技术方案如下：

一种无人装卸物流装备系统激光雷达标定系统，包括位于龙门桁架上的激光雷达和机械臂，所述龙门桁架下方设置标定板，所述激光雷达分别通过数据传输单元和调节单元与上位机连接，所述调节单元包括角度编码器和带动激光雷达旋转的转台，所述激光雷达安装于所述转台上，所述上位机通过机械控制单元与所述机械臂连接，所述机械臂包括可在龙门桁架上移动的吸盘和固定于吸盘下表面的十字激光器。

一种无人装卸物流装备系统激光雷达标定方法，采用上述的标定系统，包括以下步骤：

(1)使所述激光雷达在龙门桁架上指定的位置原地旋转，获取激光雷达的扫描数据，即获得激光雷达坐标系S₁下的周围环境数据；

(2)操作机械臂获得机械坐标系S₂下标定板中心点坐标；

(3)利用改进的RANSAC算法提取激光雷达坐标系S₁下标定板中心点坐标；

(4)计算激光雷达坐标系S₁和机械坐标系S₂之间的旋转矩阵R和平移矩阵T。

进一步的技术方案中，所述步骤(1)中，上位机通过角度编码器发出脉冲信号，驱动转台转动，带动激光雷达转动，所述激光雷达旋转181度为一个数据采集周期。

进一步的技术方案中，所述步骤(1)中，获取的扫描数据包括空间中任意一点在激光雷达坐标系S₁下的坐标值以及回光强度。

进一步的技术方案中，所述步骤(2)中，上位机通过控制龙门桁架带动机械臂移动到标定板上方，使十字激光器对准标定板中心，获取机械坐标系S₂下标定板中心点坐标。

进一步的技术方案中，所述步骤(3)的具体方法如下：

Step101：利用PCL点云库中的直通滤波方法将除有效装载区域之外的点过滤掉，以减小计算复杂度；

Step102：随机选取三点，利用PCL库中的RANSAC算法，初始化平面参数，计算剩余点数与总数比值是否小于阈值，若小于，则合并平面，算法结束；若不小于，则进行Step103；

Step103：在剩余点集中随机抽取三点，判断所选取的三点是否共线，若共线，则重新选取三点；若不共线，则计算这三点所组成的平面参数；

Step104：计算剩余点集中所有点到该平面的距离，若距离小于阈值l_n,则标记为局内点加入当前平面点集M，并判断剩余点是否判断完毕，若剩余点判断完毕，则进入Step105；若未判断完毕，则继续循环Step104；

Step105：判断局内点个数是否大于阈值T_n，若大于，则进行Step106；若不大于，则判断迭代次数是否小于K，若不小于K，则认为未检测到平面特征，算法结束；否则，继续循环Step103～Step105，直至满足条件后，继续Step106；

Step106：计算原始三点法矢之间的两两夹角，判断法矢之间夹角是否小于阈值δ_n；若小于，对这些局内点利用特征值法精确计算平面参数；否则，继续循环Step102～Step106；

Step107：利用Step106中计算得到的平面参数，计算每个点到该面的距离，重新更新点集M；

Step108：判断点集M中的点数是否大于T_n，若大于，则完成当前平面提取，从剩余点集中删掉当前平面局内点，继续循环Step102～Step107，直至剩余点集中点的个数小于设定阈值t_n或已提取到指定数量的平面，结束循环，进入Step109；否则，继续循环Step103～Step107；

Step109：判断各平面法向量之间的夹角是否小于阈值T_σ以及原点到平面距离差值是否小于阈值T_od，若两条件同时满足，则合并两平面，并重新计算平面参数；

Step1010：计算标定板平面的中心点坐标。

更进一步的技术方案中，所述Step102～Step104中，设随机选取的三点坐标为A(x₁，y₁，z₁)、B(x₂，y₂，z₂)、C(x₃，y₃，z₃)。判断随机选取的三点是否共线的公式为：

p＝0.5*(AB+AC+BC)；

P＝p*(p-AB)*(p-AC)*(p-BC)；

若P＝0；则认为随机选取的三点共线；

计算随机选取的三点组成的平面参数的公式为：

a＝(y₂-y₁)(z₃-z₁)-(z₂-z₁)(y₃-y₁)

b＝(z₂-z)(x₃-x₁)-(x₂-x₁)(z₃-z₁)

c＝(y₃-y₁)(x₂-x₁)-(y₂-y₁)(x₃-x₁)

d＝-(ax₁+by₁+cz₁)

计算剩余点集中任意点P(x_p，y_p，z_p)到平面的公式为：

更进一步的技术方案中，所述Step1010中，设每个标定板平面有n个点，每个点坐标表示为x(x_i，y_i，z_i)，则计算标定板平面中心点坐标的公式为：

进一步的技术方案中，所述步骤(4)的具体计算方法如下：

Step201：在空间中选取任意两点P₁和P₂，向量V＝P₁P₂在坐标系S₁和坐标系S₂中的分别为v₁、v₂，v₁、v₂称为一对同名向量，则有v₂＝Rv₁；

坐标系的旋转采用Rodrigues法描述：即采用一个向量来表示绕坐标轴的旋转，向量长度表示旋转角度，设此向量为u＝(u_x，u_y，u_z)，则坐标系的旋转矩阵可表示为：

则有v₂-v₁＝S(v₂+v₁)，设由i组同名向量，i＞2，同理v_2i-v_1i＝S(v_2i+v_i1)；

Step202：令A_i＝v_2i+v_1i，D_i＝v_2i-v_1i，则有：

将上式化简为Au＝D；

Step203：利用最小二乘法求解：

u＝(A^TA)^-1A^TD；

代入(1)式，即可求得旋转矩阵R；

Step204：设任意空间点p_k，k＝1，2，3，...，点坐标向量在两个坐标系下分别表示为V_1k＝OP_1k、V_2k＝OP_2k，则平移矩阵T由下式求解；

通过上述技术方案，本发明提供的无人装卸物流装备系统激光雷达标定系统及标定方法通过设置调节单元，可实现激光雷达发射的激光光束的闭环接收，获取激光雷达的测量数据，激光雷达旋转扫描周围环境，即可得到含有标定板在内的测量数据；通过设定角度编码器，实现对激光雷达扫描速度的控制；本方案仅需在标定开始前进行调节单元中激光雷达、转台、角度编码器的安装，标定过程中，调节单元无需重复安装，仅需预先设定角度编码器的参数，即可获取激光雷达周围360度的测量数据，实现标定过程的自动化。不需要定制专门的标定板(如含有高精度镂空圆的标定板、涂有防反射材料的标定板)，且标定板的位置随机，无需刻意精确的摆放标定板，为标定过程节省了时间，大大地提高了标定效率；且标定板制作简单，制作成本低，可节省标定成本。

本发明公开了的无人装卸物流装备系统激光雷达标定方法，是在激光雷达运动后，开始采集多圈点云数据，并利用改进的RANSAC算法提取出标定板中心点位置；再通过人为操纵机械臂，可以获得机械坐标系下的标定板的中心点位置；最后，激光雷达坐标系S₁下标定板的中心点位置和机械坐标系S₂下标定板的中心点位置一一对应，求解出两个坐标系之间的旋转矩阵R和平移矩阵T，并基于统计学的方法，以统计直方图的方式得出最终结果，可有效的减少随机误差，提高精度。利用多圈扫描的数据进行激光雷达坐标系下标定板中心点的提取，可以提高提取精度，提高激光雷达标定的精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。

图1为本发明实施例所公开的激光雷达标定系统装置结构示意图；

图2为本发明实施例所公开的激光雷达标定系统组成示意图；

图3为本发明实施例所公开的激光雷达标定方法流程示意图；

图4为本发明实施例所公开的改进的RANSAC算法流程示意图；

图5为本发明实施例所公开的求解旋转矩阵R和平移矩阵T算法流程图。

图中，1、激光雷达；2、转台；3、龙门桁架；4、机械臂；5、标定板；6、吸盘；7、十字激光器。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

本发明提供了一种无人装卸物流装备系统激光雷达标定系统及标定方法，能解决大场景下激光雷达的标定问题，提高激光雷达标定的效率和精度。

如图3所示为本发明实施例的激光雷达标定的流程图，包括Step1-Step4。结合图2的系统组成图，对本发明的实施例进行详细介绍。

Step1：使激光雷达转动，获取激光雷达的扫描数据，即获得激光雷达坐标系S₁下的周围环境数据。

图1所示为本发明实施例的激光雷达标定系统装置结构，激光雷达1与转台2一起组合安装并固定在龙门桁架3上；在激光雷达1的旋转过程中，龙门桁架3、机械臂4和标定板5均保持不动，上位机发出指令给调节单元，调节单元中的角度编码器发出脉冲信号，控制转台2转动，从而带动激光雷达1转动，以获取周围环境的所有数据，获取的扫描数据包括空间中任意一点在激光雷达坐标系S₁下的坐标值以及回光强度，其中包括标定板5的位置数据。获得的数据通过数据传输单元上传给上位机。

本发明实施例中，转台2的转动速率为一秒一度。

本发明实施例中，激光雷达1转动181度为一个采样周期；采取多圈扫描的方式将数据保存下来，以增加点云数据的稠密性，提高标定精度。

Step2：操作机械臂4获得机械坐标系S₂下标定板中心点坐标。

机械臂4及吸盘6安装在龙门桁架3上，由上位机操作，通过机械控制单元控制机械臂4到达指定位置，并由吸盘6右上角的十字激光器7发出的十字激光对准每一个标定板中心点，以获取准确位置；

本发明实施例中共有15个标定板5；

当操作机械臂4获取机械坐标系S₂下标定板5中心点坐标时，激光雷达1是处于非工作状态的；

本发明实施例中，十字激光器7仅用于对准标定板5的中心点，不具备数据传输功能，由于其安装在吸盘6的右上角(近似于安装在机械坐标系的原点位置)，故当十字激光器对准标定板中心时，可认为是机械坐标系S₂移动到了标定板的中心位置，此时，通过上位机与机械控制单元的通讯机制可记录下机械臂及吸盘的位置，反馈给上位机，即可获得机械坐标系S₂下标定板的中心位置。

本发明实施例中，标定板5表面贴有防反光贴纸，以减少光强度的变化对采集数据的影响。

在标定板5表面的防反光贴纸上印有十字形状，以提高机械臂4及吸盘6移动时，十字激光器与标定板中心点的对准精度，减小测量误差。

Step3：利用改进的RANSAC算法提取激光雷达坐标系S₁下标定板中心点；

图4所示为本发明实施例中的改进的RANSAC算法提取标定板中心点的流程图，包括：

Step301：利用PCL点云库中的直通滤波方法将除有效装载区域之外的点过滤掉，以减小计算复杂度；

Step302：随机选取三点，利用PCL库中的RANSAC算法，初始化平面参数，计算剩余点数与总数比值是否小于阈值，若小于，则合并平面，算法结束；若不小于，则进行Step3；

Step303：在剩余点集中随机抽取三点，判断所选取的三点是否共线，若共线，则重新选取三点；若不共线，则计算这三点所组成的平面参数；

本发明实施例中，设随机选取的三点坐标为A(x₁，y₁，z₁)、B(x₂，y₂，z₂)、C(x₃，y₃，z₃)。判断随机选取的三点是否共线的公式为：

p＝0.5*(AB+AC+BC)

P＝p*(p-AB)*(p-AC)*(p-BC)

若P＝0；则认为随机选取的三点共线。

本发明实施例中，设随机选取的三点坐标为A(x₁，y₁，z₁)、B(x₂，y₂，z₂)、C(x₃，y₃，z₃)。则计算平面参数的公式为：

a＝(y₂-y₁)(z₃-z₁)-(z₂-z₁)(y₃-y₁)

b＝(z₂-z)(x₃-x₁)-(x₂-x₁)(z₃-z₁)

c＝(y₃-y₁)(x₂-x₁)-(y₂-y₁)(x₃-x₁)

d＝-(ax₁+by₁+cz₁)

Step304：计算剩余点集中所有点到该平面的距离，若距离小于阈值l_n，则标记为局内点加入当前平面点集M，并判断剩余点是否判断完毕，若剩余点判断完毕，则进入Step305；若未判断完毕，则继续循环Step304；

本发明实施例中，点P(x_p，y_p，z_p)到平面的公式为：

Step305：判断局内点个数是否大于阈值T_n，若大于，则进行Step306；若不大于，则判断迭代次数是否小于K，若不小于K，则认为未检测到平面特征，算法结束；否则，继续循环Step303～Step305直至满足条件后，继续Step306；

Step306：计算原始三点法矢之间的两两夹角，判断法矢之间夹角是否小于阈值δ_n；若小于，对这些局内点利用特征值法精确计算平面参数；否则，继续循环Step302～Step306；

Step307：利用Step306中计算得到的平面参数，计算每个点到该面的距离，重新更新点集M；

Step308：判断点集M中的点数是否大于T_n，若大于，则完成当前平面提取，从剩余点集中删掉当前平面局内点，继续循环Step302～Step307，直至剩余点集中点的个数小于设定阈值t_n或以提取到指定数量的平面，结束循环，进入Step309；否则，继续循环Step303～Step307；

Step309：判断各平面法向量之间的夹角是否小于阈值T_σ以及原点到平面距离差值是否小于阈值T_od，若两条件同时满足，则合并两平面，并重新计算平面参数。

Step3010：计算标定板平面的中心点坐标；

本发明实施例中，设每个标定板平面有n个点，每个点坐标表示为x(x_i，y_i，z_i)，则计算标定板平面中心点坐标的公式为

Step4：计算激光雷达坐标系S₁和机械坐标系S₂之间的旋转矩阵R和平移矩阵T。

图5所示为本发明实施例中的旋转矩阵R和平移矩阵T的求解流程图，包括：

Step401：在空间中选取任意两点P₁和P₂，向量V＝P₁P₂在坐标系S₁和坐标系S₂中的分别为v₁、v₂，v₁、v₂称为一对同名向量，则有v₂＝Rv₁。

本发明实施例中，坐标系的旋转采用Rodrigues法描述：即采用一个向量来表示绕坐标轴的旋转，向量长度表示旋转角度，设此向量为u＝(u_x，u_y，u_z)，则坐标系的旋转矩阵可表示为：

进一步地，则有v₂-v₁＝S(v₂+v₁)。设由i(i＞2)组同名向量，同理v_2i-v_1i＝S(v_2i+v_i1)。

Step402：令A_i＝v_2i+v_1i，D_i＝v_2i-v_1i，则有：

进一步地，将上式化简为Au＝D。

Step403：利用最小二乘法求解：

u＝(A^TA)^-1A^TD；

代入(1)式，即可求得旋转矩阵R。

Step404：设任意空间点p_k，k＝1，2，3，...，点坐标向量在两个坐标系下分别表示为V_1k＝OP_1k、V_2k＝OP_2k。则平移矩阵T可由下式求解；

Step405：在所有标志点中选择任意的点组合，并重复Step401～Step404，计算得到多组旋转矩阵和平移矩阵；

Step406：将得到的多组旋转矩阵和旋转矩阵利用直方图的方式统计出来，选取频率最高的旋转矩阵和平移矩阵作为最终的激光雷达外参矩阵。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (9)

1.一种无人装卸物流装备系统激光雷达标定系统，其特征在于，包括位于龙门桁架上的激光雷达和机械臂，所述龙门桁架下方设置标定板，所述激光雷达分别通过数据传输单元和调节单元与上位机连接，所述调节单元包括角度编码器和带动激光雷达旋转的转台，所述激光雷达安装于所述转台上，所述上位机通过机械控制单元与所述机械臂连接，所述机械臂包括可在龙门桁架上移动的吸盘和固定于吸盘下表面的十字激光器。

2.一种无人装卸物流装备系统激光雷达标定方法，采用如权利要求1所述的标定系统，其特征在于，包括以下步骤：

(1)使所述激光雷达在龙门桁架上指定的位置原地旋转，获取激光雷达的扫描数据，即获得激光雷达坐标系S₁下的周围环境数据；

(2)操作机械臂获得机械坐标系S₂下标定板中心点坐标；

(3)利用改进的RANSAC算法提取激光雷达坐标系S₁下标定板中心点坐标；

(4)计算激光雷达坐标系S₁和机械坐标系S₂之间的旋转矩阵R和平移矩阵T。

3.根据权利要求2所述的一种无人装卸物流装备系统激光雷达标定方法，其特征在于，所述步骤(1)中，上位机通过角度编码器发出脉冲信号，驱动转台转动，带动激光雷达转动，所述激光雷达旋转181度为一个数据采集周期。

4.根据权利要求2所述的一种无人装卸物流装备系统激光雷达标定方法，其特征在于，所述步骤(1)中，获取的扫描数据包括空间中任意一点在激光雷达坐标系S₁下的坐标值以及回光强度。

5.根据权利要求2所述的一种无人装卸物流装备系统激光雷达标定方法，其特征在于，所述步骤(2)中，上位机通过控制龙门桁架带动机械臂移动到标定板上方，使十字激光器对准标定板中心，获取机械坐标系S₂下标定板中心点坐标。

6.根据权利要求2所述的一种无人装卸物流装备系统激光雷达标定方法，其特征在于，所述步骤(3)的具体方法如下：

Step101：利用PCL点云库中的直通滤波方法将除有效装载区域之外的点过滤掉，以减小计算复杂度；

Step102：随机选取三点，利用PCL库中的RANSAC算法，初始化平面参数，计算剩余点数与总数比值是否小于阈值，若小于，则合并平面，算法结束；若不小于，则进行Step103；

Step103：在剩余点集中随机抽取三点，判断所选取的三点是否共线，若共线，则重新选取三点；若不共线，则计算这三点所组成的平面参数；

Step104：计算剩余点集中所有点到该平面的距离，若距离小于阈值l_n，则标记为局内点加入当前平面点集M，并判断剩余点是否判断完毕，若剩余点判断完毕，则进入Step105；若未判断完毕，则继续循环Step104；

Step105：判断局内点个数是否大于阈值T_n，若大于，则进行Step106；若不大于，则判断迭代次数是否小于K，若不小于K，则认为未检测到平面特征，算法结束；否则，继续循环Step103～Step105，直至满足条件后，继续Step106；

Step106：计算原始三点法矢之间的两两夹角，判断法矢之间夹角是否小于阈值δ_n；若小于，对这些局内点利用特征值法精确计算平面参数；否则，继续循环Step102～Step106；

Step107：利用Step106中计算得到的平面参数，计算每个点到该面的距离，重新更新点集M；

Step108：判断点集M中的点数是否大于T_n，若大于，则完成当前平面提取，从剩余点集中删掉当前平面局内点，继续循环Step102～Step107，直至剩余点集中点的个数小于设定阈值t_n或已提取到指定数量的平面，结束循环，进入Step109；否则，继续循环Step103～Step107；

Step109：判断各平面法向量之间的夹角是否小于阈值T_σ以及原点到平面距离差值是否小于阈值T_od，若两条件同时满足，则合并两平面，并重新计算平面参数；

Step1010：计算标定板平面的中心点坐标。

7.根据权利要求6所述的一种无人装卸物流装备系统激光雷达标定方法，其特征在于，所述Step102～Step104中，设随机选取的三点坐标为A(x₁，y₁，z₁)、B(x₂，y₂，z₂)、C(x₃，y₃，z₃)。判断随机选取的三点是否共线的公式为：

p＝0.5*(AB+AC+BC)；

P＝p*(p-AB)*(p-AC)*(p-BC)；

若P＝0；则认为随机选取的三点共线；

计算随机选取的三点组成的平面参数的公式为：

a＝(y₂-y₁)(z₃-z₁)-(z₂-z₁)(y₃-y₁)

b＝(z₂-z)(x₃-x₁)-(x₂-x₁)(z₃-z₁)

c＝(y₃-y₁)(x₂-x₁)-(y₂-y₁)(x₃-x₁)

d＝-(ax₁+by₁+cz₁)

计算剩余点集中任意点P(x_p，y_p，z_p)到平面的公式为：

8.根据权利要求6所述的一种无人装卸物流装备系统激光雷达标定方法，其特征在于，所述Step1010中，设每个标定板平面有n个点，每个点坐标表示为x(x_i，y_i，z_i)，则计算标定板平面中心点坐标的公式为：

9.根据权利要求2所述的一种无人装卸物流装备系统激光雷达标定方法，其特征在于，所述步骤(4)的具体计算方法如下：

Step201：在空间中选取任意两点P₁和P₂，向量V＝P₁P₂在坐标系S₁和坐标系S₂中的分别为v₁、v₂，v₁、v₂称为一对同名向量，则有v₂＝Rv₁；

坐标系的旋转采用Rodrigues法描述：即采用一个向量来表示绕坐标轴的旋转，向量长度表示旋转角度，设此向量为u＝(u_x，u_y，u_z)，则坐标系的旋转矩阵可表示为：

则有v₂-v₁＝S(v₂+v₁)，设由i组同名向量，i＞2，同理v_2i-v_1i＝S(v_2i+v_i1)；

Step202：令A_i＝v_2i+v_1i，D_i＝v_2i-v_1i，则有：

将上式化简为Au＝D；

Step203：利用最小二乘法求解：

u＝(A^TA)^-1A^TD；

代入(1)式，即可求得旋转矩阵R；

Step204：设任意空间点p_k，k＝1，2，3，...，点坐标向量在两个坐标系下分别表示为V_1k＝OP_1k、V_2k＝OP_2k，则平移矩阵T由下式求解；