本发明涉及激光雷达技术领域，具体涉及一种激光雷达与倾角传感器的外参信息标定方法。
背景技术：
激光雷达是一种可以精确探测物体位置的传感器，激光雷达通过对目标物体发射激光信号，根据从目标物体反射回来的信号时间差来计算激光雷达与目标物体间的距离，然后根据发射激光的角度来确定目标物体和发射器的角度，从而得出目标物体与激光发射器的相对位置。激光雷达由于具备高精度和高分辨率等测量优势，已被广泛应用于各个行业。目前基于激光雷达获得的点云数据，通过点云匹配的方法可以估计目标的三维位姿信息。
由于高精度的倾角传感器具有体积小、功耗低、稳定性高和抗干扰能力强等优点，为此，可以通过在激光传感器和目标上安装倾角传感器，利用倾角传感器提供额外的角度约束信息，提高目标位姿估计的精度。
其中，为了实现激光雷达和倾角传感器的两者信息融合，需要精确标定激光传感器和倾角传感器的外参信息。
当前，激光雷达和惯性测量器件之间的外参标定方法已较为成熟。具体地，激光雷达和惯性测量器件绑定到一起运动，激光雷达通过点云匹配得到位姿信息和惯性器件加速度和角速度积分得到的位姿信息具有相关性，通过上述信息利用优化算法可以求解两者之间的外参信息。但是，由于倾角传感器的输出仅包含与水平面的夹角，现有的外参标定方法并不能用于激光雷达和倾角传感器的外参信息标定。
技术实现要素：
为解决上述现有技术中存在的技术问题，本发明提供一种激光雷达与倾角传感器的外参信息标定方法。
为此，本发明公开了一种激光雷达与倾角传感器的外参信息标定方法，包括：
将激光雷达和倾角传感器固定连接；
将激光雷达和倾角传感器固定放置在设定的标定场地内，记录倾角传感器的输出数据；
在标定场地内放置一个标定辅助设备，并对标定辅助设备进行调水平；
在标定场地内放置一个激光雷达标定板，并在标定板平面内设置若干个视觉标志点，若干个视觉标志点中至少存在三个不共线的视觉标志点；
利用标定辅助设备对准标定板的视觉标志点，测量视觉标志点的位置信息，根据视觉标志点的位置信息估计标定板平面的法向量；
利用激光雷达在标定板上采集不少于两条不重合直线的点云数据，根据点云数据估计标定板平面的法向量；
移动标定板若干次，获取若干个标定板平面的法向量组，每个法向量组包括一个基于位置信息的法向量和一个基于点云数据的法向量；
调整激光雷达和倾角传感器在标定场地内的放置位置，获取多组数据组，每个数据组包括一个倾角传感器的输出数据和若干个标定板平面的法向量组；
根据多组数据组确定激光雷达与倾角传感器的外参信息。
在一些可选的实施方式中，所述标定辅助设备为全站仪。
在一些可选的实施方式中，所述根据多组数据组确定激光雷达与倾角传感器的外参信息，包括：
选取多组数据组中的一组数据组；
根据选取的一组数据组，确定倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转关系；
根据选取的一组数据组，确定标定辅助设备世界坐标系与激光雷达坐标系之间的旋转关系；
基于倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转关系和标定辅助设备世界坐标系与激光雷达坐标系之间的旋转关系，确定倾角传感器坐标系与激光雷达坐标系之间的旋转关系；
根据倾角传感器坐标系与激光雷达坐标系之间的旋转关系确定倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵求解方程组；
依次选取多组数据组中的剩余数据组，确定每个数据组对应的倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵求解方程组，利用多个旋转矩阵求解方程组确定倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵。
在一些可选的实施方式中，设定：选取的数据组为第m组，世界坐标系绕z轴、y轴和x轴旋转αm、βm和γ′m得到倾角传感器坐标系；
倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转关系表述为：
在一些可选的实施方式中，设定：第m组数据组中世界坐标系与激光雷达坐标系之间的平移矩阵忽略不计；
标定辅助设备世界坐标系与激光雷达坐标系之间的旋转关系表述为：
在一些可选的实施方式中，旋转矩阵
设定：
对公式
选取若干个法向量组中至少三个不平行的法向量组，获取每个法向量组对应的三个方程组；
利用获取的所有方程组，求解得到旋转矩阵
在一些可选的实施方式中，还包括构造正交约束为：
构造最优函数为：
ρ表示惩罚因子；
在求解旋转矩阵
在一些可选的实施方式中，倾角传感器坐标系与激光雷达坐标系之间的旋转关系表述为：
旋转矩阵
在一些可选的实施方式中，倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵求解方程组通过以下方式确定：
设定：
对转换公式
利用m组数据组得到所有旋转矩阵求解方程组
在一些可选的实施方式中，还包括构造正交约束为：
构造最优函数为：
ρ表示惩罚因子；
在求解旋转矩阵
本发明技术方案的主要优点如下：
本发明的激光雷达与倾角传感器的外参信息标定方法通过利用如全站仪的标定辅助设备和标定板，能够实现激光雷达与倾角传感器的外参信息标定，标定精度高，适用范围广。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一实施例的激光雷达与倾角传感器的外参信息标定方法的流程图；
图2为本发明一实施例的倾角传感器坐标系与水平面坐标系的关系示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例及相应的附图对本发明技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
以下结合附图，详细说明本发明实施例提供的技术方案。
如附图1所示，本发明一实施例提供了一种激光雷达与倾角传感器的外参信息标定方法，该方法包括：
将激光雷达和倾角传感器固定连接；
将激光雷达和倾角传感器固定放置在设定的标定场地内，记录倾角传感器的输出数据；
在标定场地内放置一个标定辅助设备，并对标定辅助设备进行调水平；
在标定场地内放置一个激光雷达标定板，并在标定板平面内设置若干个视觉标志点，若干个视觉标志点中至少存在三个不共线的视觉标志点；
利用标定辅助设备对准标定板的视觉标志点，测量视觉标志点的位置信息，根据视觉标志点的位置信息估计标定板平面的法向量；
利用激光雷达在标定板上采集不少于两条不重合直线的点云数据，根据点云数据估计标定板平面的法向量；
移动标定板若干次，获取若干个标定板平面的法向量组，每个法向量组包括一个基于位置信息的法向量和一个基于点云数据的法向量；
调整激光雷达和倾角传感器在标定场地内的放置位置，获取多组数据组，每个数据组包括一个倾角传感器的输出数据和若干个标定板平面的法向量组；
根据多组数据组确定激光雷达与倾角传感器的外参信息。
以下对本发明一实施例提供的激光雷达与倾角传感器的外参信息标定方法的各个步骤进行具体说明。
s1，将激光雷达和倾角传感器固定连接；
具体地，在进行外参信息标定的过程中，将激光雷达和倾角传感器固定连接，以保证两者之间的相对位姿关系不会发生变化。
在实际应用过程中，尽可能地保证激光雷达和倾角传感器安装在同一水平面内或者平行的水平面内，两者之间的旋转角变化尽可能小，以提高标定的精度。
s2，将激光雷达和倾角传感器固定放置在设定的标定场地内，记录倾角传感器的输出数据；
具体地，将激光雷达和倾角传感器放置在设定的标定场地内，并保证激光雷达和倾角传感器在后续的外参信息标定过程中保持恒定，而后记录倾角传感器的输出数据。
本发明一实施例中，采集倾角传感器的多组输出数据，取多组输出数据的平均值作为输出数据的最终值，以滤除倾角传感器的高斯误差。
倾角传感器的输出数据包括倾角传感器x轴和y轴的输出数据，如附图2所示，本发明一实施例中，设定倾角传感器坐标系f(s)中x轴和y轴的输出数据分别为γ和β。x轴的输出数据γ表示倾角传感器x轴与水平面的夹角，y轴的输出数据β表示倾角传感器y轴与水平面的夹角。
s3，在标定场地内放置一个标定辅助设备，并对标定辅助设备进行调水平；
本发明一实施例中，标定辅助设备可以采用全站仪。
具体地，在设定的标定场地内放置一个全站仪，并将全站仪调水平，以建立一个世界坐标系f(w)。其中，该世界坐标系的z轴垂直于水平面向上或向下，x轴和y轴在水平面内，以保证利用全站仪估计的标定板平面的法向量能够表示在倾角传感器坐标系，本发明一实施例中设定世界坐标系的z轴垂直于水平面向上。
当然，标定辅助设备也可以采用其他设备，只能够辅助建立一个z轴垂直于水平面、x轴和y轴在水平面内的世界坐标系，同时利用该标定辅助设备能够识别标定板。
s4，在标定场地内放置一个激光雷达标定板，并在标定板平面内设置若干个视觉标志点，若干个视觉标志点中至少存在三个不共线的视觉标志点；
由于同一平面内不共线的三点能够确定该平面，为此，本发明一实施例中，将激光雷达标定板放置在激光雷达视野范围内，并在标定板平面内设置至少包括三个不共线的视觉标志点的若干个视觉标志点，以满足后续标定使用需求。
此外，标定板的平整度需要在设定精度范围内，以尽可能保证标定板平面内各处的法向量的一致。其中，设定精度范围根据实际标定精度要求进行设置。
s5，利用标定辅助设备对准标定板的视觉标志点，测量视觉标志点的位置信息，根据视觉标志点的位置信息估计标定板平面的法向量；
具体地，使全站仪对准标定板的视觉标志点，利用全站仪测量多个视觉标志点的位置，根据测量的多个视觉标志点的位置信息，利用随机采样一致性(ransac)算法或最小二乘算法估计出标定板平面的法向量wn。
假设标定板平面为ax by cz d＝0，平面法向量为
s6，利用激光雷达在标定板上采集不少于两条不重合直线的点云数据，根据点云数据估计标定板平面的法向量；
利用激光雷达在标定板上采集不少于两条不重合直线的点云数据，利用两条不重合直线的三维点云数据确定激光雷达坐标系f(l)下标定板平面的法向量ln。
具体地，与步骤s5中法向量的求解过程相同，使用激光雷达获得的三维点云替代视觉标志点，获取相应的视觉标志点的位置数据，根据位置数据利用随机采样一致性(ransac)算法或最小二乘算法估计出标定板平面的法向量ln。
s7，移动标定板若干次，获取若干个标定板平面的法向量组，每个法向量组包括一个基于点云数据的法向量和一个基于位置信息的法向量；
具体地，每移动一次标定板，确定一个世界坐标系f(w)下标定板平面的法向量wn和一个激光雷达坐标系f(l)下标定板平面的法向量ln，移动标定板k(k≥3)次，获取k个标定板平面的法向量组(wni,lni),i＝1,2,...,k，wni和lni表示第i个法向量组中世界坐标系f(w)下标定板平面的法向量和激光雷达坐标系f(l)下标定板平面的法向量。
s8，调整激光雷达和倾角传感器在标定场地内的放置位置，获取多组数据组，每个数据组包括一个倾角传感器的输出数据和若干个标定板平面的法向量组；
具体地，每调整一次激光雷达和倾角传感器在标定场地内的放置位置，记录一次倾角传感器的输出数据，并重复步骤s5-s7，获取包括一个倾角传感器的输出数据和k个标定板平面的法向量组的数据组，调整激光雷达和倾角传感器在标定场地内的放置位置m(m≥3)次，获取m组数据组
s9，根据多组数据组确定激光雷达与倾角传感器的外参信息；
激光雷达和倾角传感器的外参信息包括旋转矩阵
具体地，本发明一实施例中，根据多组数据组确定激光雷达与倾角传感器的外参信息，包括以下步骤s91-s96：
s91，选取多组数据组中的一组数据组；
从m组数据组
s92，根据选取的一组数据组，确定倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转关系；
第m组数据组中，倾角传感器的x轴和y轴的输出数据分别为γm和βm，设定：世界坐标系f(w)绕z轴、y轴和x轴旋转αm、βm和
根据倾角传感器坐标系和世界坐标系的变换关系的推导过程知，倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转关系可表述为：
倾角传感器坐标系到世界坐标系的变换关系的推导过程如下：
倾角传感器测量过程中倾角传感器坐标系的参考面为水平面，双轴倾角传感器两个轴(x轴和y轴)输出值为自身x轴和y轴与参考水平面间的夹角γ和β，参考水平面上定义的坐标系f(g)和倾角传感器坐标系f(s)都过原点o。
如附图2所示，从倾角传感器x轴上一点f做参考水平面的垂线，与参考水平面相交于e点，oe直线为坐标系f(g)的x1轴(β＝0时，x1轴和x轴重合)；在参考水平面内做垂直于x1轴的垂线od，od直线为坐标系f(g)的y1轴。其中，x1轴与x轴的夹角与倾角传感器的x轴输出相同，均为β，y1轴与y轴的夹角为γ′。坐标系f(g)首先绕y1轴旋转β，然后绕变换后的坐标系x轴旋转γ′，可以得到倾角传感器坐标系f(s)，故有：
可知，为了求解旋转矩阵，需要确定γ′。
过倾角传感器y轴上一点a做垂直于参考水平面的垂线，与参考水平面相交于b点；同时过点a做垂直于y1轴的垂线垂直于d点(γ＝0时，y1轴和y轴重合)。
当γ＝0时，y1轴和y轴重合，有γ′＝γ＝0。
当β＝0时，x1轴和x轴重合，y轴与参考水平面的垂线同y轴与y1轴垂线重合，γ′＝γ。
证明：过y轴做y1轴垂线ad。x1轴和x轴，x1轴垂直于y1轴与y轴张成的平面oad，则ad同时垂直于x1轴与y1轴，则ad为参考水平面的垂线，故有γ′＝γ。
当γ≠0,β≠0时，
证明：由于od⊥ad，od⊥ab，则od⊥平面abd；由于od⊥fe，od⊥oe，则od⊥平面ofe；因此，有平面ofe∥平面abd。由于db与oe均位于参考水平面内，且均与参考水平面内的直线od垂直，则有db∥oe。
过a点可以做x轴的平行线am，其与db延长线必相交，设定交点为c，故有：△foe∽△acb，am⊥oa，am⊥od，am⊥平面oad，am⊥ad，∠cad＝pi/2.0。
根据上述几何关系可知，∠bad＝β，∠dao＝90-γ′，∠bao＝90-γ，根据三余弦定理可知cos(90-γ)＝cosβcos(90-γ′)，故可以得到：
由于当γ＝0时，
设定世界坐标系f(w)绕z轴旋转α可以使得世界坐标系f(w)与坐标系f(g)方向一致，则有：
因此，从倾角传感器坐标系f(s)与世界坐标系f(w)之间的变换关系可表示为：
s93，根据选取的一组数据组，确定标定辅助设备世界坐标系与激光雷达坐标系之间的旋转关系；
设定：第m组数据组中，标定辅助设备世界坐标系f(w)与激光雷达坐标系f(l)之间的平移矩阵忽略不计；则标定辅助设备世界坐标系与激光雷达坐标系之间的旋转关系可表述为：
进一步地，在已知若干个法向量组
设定
在第m组数据组中，每个标定板平面的法向量组都可以得到三个对应的方程组。由于旋转矩阵
具体地，上述的方程组可以表示为如下的矩阵形式：
amem-bm＝0
根据上述内容可知，am和bm为已知参数，通过奇异值分解可以求解参数em，即确定旋转矩阵
进一步地，由于上述方程组没有考虑旋转矩阵
具体地，构造正交约束为：
构造最优函数为：
其中，ρ表示惩罚因子。
由于em的初始值已经通过方程组求解得到，基于上述构造的正交约束和最优函数，可以进一步通过优化算法以得到em的精确值，从而提高旋转矩阵
进一步地，若求解出来的矩阵
s94，基于倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转关系和标定辅助设备世界坐标系与激光雷达坐标系之间的旋转关系，确定倾角传感器坐标系与激光雷达坐标系之间的旋转关系；
由于倾角传感器与激光雷达固定连接，即倾角传感器坐标系与激光雷达坐标系两者固连，因此仅标定两者的旋转矩阵即可完成激光雷达与倾角传感器的外参信息标定。
本发明一实施例中，倾角传感器坐标系与激光雷达坐标系的旋转关系可表述为：
由于倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转矩阵为
根据上述旋转矩阵
s95，根据倾角传感器坐标系与激光雷达坐标系之间的旋转关系确定倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵求解方程组；
设定
s96，依次选取多组数据组中的剩余数据组，确定每个数据组对应的倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵求解方程组，利用多个旋转矩阵求解方程组确定倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵。
由于每组数据组都可以得到三个旋转矩阵求解方程组，利用m组数据组则可以得到3m个旋转矩阵求解方程组。由于m大于等于3，旋转矩阵
具体地，旋转矩阵求解方程组可以表示为如下的矩阵形式：
aslesl-bsl＝0
根据上述内容可知，asl和bsl为已知参数，通过奇异值分解可以求解参数esl，即确定旋转矩阵
由于上述旋转矩阵求解方程组没有考虑旋转矩阵
具体地，构造正交约束为：
构造最优函数为：
其中，ρ表示惩罚因子。
由于esl的初始值已经通过方程组求解得到，基于上述构造的正交约束和最优函数，可以进一步通过优化算法以得到esl的精确值，从而提高旋转矩阵
进一步地，若求解出来的旋转矩阵
本发明一实施例提供的激光雷达与倾角传感器的外参信息标定方法通过利用如全站仪的标定辅助设备和标定板，能够实现激光雷达与倾角传感器的外参信息标定，标定精度高，适用范围广。
需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。此外，本文中“前”、“后”、“左”、“右”、“上”、“下”均以附图中表示的放置状态为参照。
最后应说明的是：以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
1.一种激光雷达与倾角传感器的外参信息标定方法，其特征在于，包括：
将激光雷达和倾角传感器固定连接；
将激光雷达和倾角传感器固定放置在设定的标定场地内，记录倾角传感器的输出数据；
在标定场地内放置一个标定辅助设备，并对标定辅助设备进行调水平；
在标定场地内放置一个激光雷达标定板，并在标定板平面内设置若干个视觉标志点，若干个视觉标志点中至少存在三个不共线的视觉标志点；
利用标定辅助设备对准标定板的视觉标志点，测量视觉标志点的位置信息，根据视觉标志点的位置信息估计标定板平面的法向量；
利用激光雷达在标定板上采集不少于两条不重合直线的点云数据，根据点云数据估计标定板平面的法向量；
移动标定板若干次，获取若干个标定板平面的法向量组，每个法向量组包括一个基于位置信息的法向量和一个基于点云数据的法向量；
调整激光雷达和倾角传感器在标定场地内的放置位置，获取多组数据组，每个数据组包括一个倾角传感器的输出数据和若干个标定板平面的法向量组；
根据多组数据组确定激光雷达与倾角传感器的外参信息。
2.根据权利要求1所述的激光雷达与倾角传感器的外参信息标定方法，其特征在于，所述标定辅助设备为全站仪。
3.根据权利要求1或2所述的激光雷达与倾角传感器的外参信息标定方法，其特征在于，所述根据多组数据组确定激光雷达与倾角传感器的外参信息，包括：
选取多组数据组中的一组数据组；
根据选取的一组数据组，确定倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转关系；
根据选取的一组数据组，确定标定辅助设备世界坐标系与激光雷达坐标系之间的旋转关系；
基于倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转关系和标定辅助设备世界坐标系与激光雷达坐标系之间的旋转关系，确定倾角传感器坐标系与激光雷达坐标系之间的旋转关系；
根据倾角传感器坐标系与激光雷达坐标系之间的旋转关系确定倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵求解方程组；
依次选取多组数据组中的剩余数据组，确定每个数据组对应的倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵求解方程组，利用多个旋转矩阵求解方程组确定倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵。
4.根据权利要求3所述的激光雷达与倾角传感器的外参信息标定方法，其特征在于，设定：选取的数据组为第m组，世界坐标系绕z轴、y轴和x轴旋转αm、βm和γ′m得到倾角传感器坐标系；
倾角传感器坐标系与标定辅助设备世界坐标系之间的旋转关系表述为：
5.根据权利要求4所述的激光雷达与倾角传感器的外参信息标定方法，其特征在于，设定：第m组数据组中世界坐标系与激光雷达坐标系之间的平移矩阵忽略不计；
标定辅助设备世界坐标系与激光雷达坐标系之间的旋转关系表述为：
6.根据权利要求5所述的激光雷达与倾角传感器的外参信息标定方法，其特征在于，旋转矩阵
设定：
对公式
选取若干个法向量组中至少三个不平行的法向量组，获取每个法向量组对应的三个方程组；
利用获取的所有方程组，求解得到旋转矩阵
7.根据权利要求6所述的激光雷达与倾角传感器的外参信息标定方法，其特征在于，还包括构造正交约束为：
构造最优函数为：
ρ表示惩罚因子；
在求解旋转矩阵
8.根据权利要求3或7所述的激光雷达与倾角传感器的外参信息标定方法，其特征在于，倾角传感器坐标系与激光雷达坐标系之间的旋转关系表述为：
旋转矩阵
9.根据权利要求8所述的激光雷达与倾角传感器的外参信息标定方法，其特征在于，倾角传感器坐标系与激光雷达坐标系之间的旋转矩阵求解方程组通过以下方式确定：
设定：
对转换公式
利用m组数据组得到所有旋转矩阵求解方程组
10.根据权利要求9所述的激光雷达与倾角传感器的外参信息标定方法，其特征在于，还包括构造正交约束为：
构造最优函数为：
ρ表示惩罚因子；
在求解旋转矩阵
