激光雷达移动测量系统的快速标定方法与流程

本发明涉及移动测量领域，特别是一种激光雷达移动测量系统的快速标定方法。

背景技术：

移动测量系统作为一种采集空间三维信息的技术手段，在数字城市、无人驾驶、地形图测绘等领域有着广泛的应用。移动测量系统包含有多种传感器，采集到的数据有惯性导航系统(以下简称ins)数据、卫星导航系统(以下简称gnss)数据、激光雷达数据、里程编码器数据、影像数据等。如何获得传感器之间精确的相对位置和姿态关系并将多种传感器数据融合处理，是得到高精度点云的关键问题。激光雷达移动测量系统标定主要是为了获得激光雷达与惯性导航系统之间的相对位置和姿态关系。相对位置和姿态关系用t、r来表示，其中，t代表相对位置矢量，由三维坐标系三个方向的平移量(tx，ty，tz)构成；r代表姿态旋转矩阵，由三个旋转角(航向角yaw，俯仰角pitch，翻滚角roll)的三角函数来构成。故标定参数实际是三个方向的平移量(tx，ty，tz)和三个旋转角(yaw，pitch，roll)总共6个参数。张海啸，钟若飞，孙海丽.顾及平面特征的车载激光扫描系统外参数标定法，提出通过采集不同车行方向的同一区域的点云数据，提取平面特征数据并进行平面特征数据的自动化配准，对多个不同角度的平面特征的共同检校，实现三维空间中不同车行方向采集的相同地物点云的重合，最终完成系统外参数的标定的方法。试验结果显示，该方法对车载激光扫描系统外参数的标定实现了自动化，减少人工参与且达到了较高的检校精度。但是该方案仍存在计算量大，标定速度较慢的问题，每次标定需要花费大量的时间。官云兰，程效军，施贵刚等，提出一种稳健的点云数据平面拟合方法，该法以特征值法为基础，通过利用一定的准则删除点云数据中的粗差或异常值，从而获得稳健的平面参数估计值.在实验中，分别利用最小二乘法、特征值法和该稳健特征值法对点云数据进行拟合，结果显示该法能克服异常值的影响，得到可靠的平面参数估值，具有稳健性。闫利，刘华，陈长军，曹亮等提出了一种无地面控制点的车载激光扫描系统外标定方法，该方法利用车载激光扫描系统对同一地物多次扫描的激光点云需重合作为约束条件，使用lm(levenberg-marguardt)最优化算法解算标定参数，使用该方法对车载激光扫描系统进行了外标定，并用实测控制点验证了标定后系统的定位精度。但是上述的方案均涉及较多参数的运算，效率较低。中国专利文献cn110221275a记载了一种激光雷达与相机之间的标定方法和装置，通过获取旋转矢量和平移矢量计算图像与点云之间的重合度，该方案需要设定一个标定板，操作较为麻烦，而且受场地空间的限制，也难以获得较高精度的标定参数。

技术实现要素：

本发明所要解决的技术问题是提供一种激光雷达移动测量系统的标定方法，在符合，能够无需设立额外的标定装置，并且能够快速对激光雷达系统进行标定。

为解决上述技术问题，本发明所采用的技术方案是：一种激光雷达移动测量系统的快速标定方法，包括以下步骤：

s1、将具有惯性导航系统、卫星导航系统和激光扫描雷达系统的移动测量系统在具有形状规则的建筑物的十字路口多次采集激光雷达数据；

s2、根据惯性导航系统、卫星导航系统和激光扫描雷达系统采集的数据联合解算点云；

将基于激光雷达坐标系的原始点云转换为基于载体坐标系的成果点云；

在转换过程中调整姿态角，分别根据翻滚角、航向角和俯仰角调整载体坐标系的点云，补偿各次采集的点云的姿态角误差；

通过以上步骤实现激光雷达移动测量系统的快速标定。

优选的方案中，在步骤s1中，移动测量系统在十字路口至少采集为三次，其中两次为往返采集行走轨迹，其中至少两次的采集行走轨迹互相垂直。

优选的方案中，从原始点云转换到成果点云的坐标转换过程为：

pi＝rpr+t；公式1

pb＝rbipi；公式2

pw＝rwbpb+twb；公式3

其中，pr为原始点云坐标，为3维列向量；

pr＝(xr，yr，zr)^t；公式4

pi为惯性导航系统坐标系中的点云坐标，t、r为标定参数，分别表示激光雷达坐标系相对于惯性导航系统坐标的位置和姿态，其中t为3维列向量；

t＝(tx，ty，tz)^t；公式5

r为3×3原始姿态矩阵；

公式6

其中，roll表示翻滚角，yaw表示航向角，pitch表示俯仰角；

pb为载体坐标系中的点云坐标，rbi为惯性导航系统坐标系在载体坐标系中的姿态矩阵，载体坐标系的坐标原点和ins坐标系的坐标原点重合，所惯性导航系统坐标系转换到载体坐标系没有位置向量；

pw为世界坐标系中的点云坐标，rwb为载体坐标系在世界坐标系中的姿态矩阵，twb为载体坐标系在世界坐标系中的位置向量，rwb和twb由卫星导航系统和惯性导航系统提供。

优选的方案中，调整载体坐标系的姿态角：

pb′＝δrrbi(rpr+t)；公式7

pb′为调整后的载体坐标系坐标，δr为调整的姿态角矩阵；

此处的3维列向量t，由结构设计或测量得到，解算时t的误差予以忽略；

世界坐标系的点云坐标：

pw′＝rwbpb′+twb；公式8

pw′为调整后的世界坐标系点云坐标。

优选的方案中，姿态角调整的顺序为首先调整翻滚角，然后调整航向角或俯仰角。

优选的方案中，翻滚角的调整步骤为：

取同一条道路往返采集的点云进行矩形裁切，得到道路横截面；

在两次采集的点云，靠近道路横截面两端中心对称的位置分别截取一个窗口；

分别调整两次采集的点云的翻滚角，直至窗口内两次采集的点云的误差小于阈值，完成对翻滚角的调整。

优选的方案中，翻滚角具体的调整步骤为：

s100、设定一个道路宽度值w，设定一个窗口大小s，分别取距离道路中心左右w位置的载体坐标系局部点云，局部点云的范围由窗口大小s来定义；

将局部点云命名为m1、n1、m2、n2；

s101、对窗口内的局部点云分别进行平面拟合，并统计残差和中误差，剔除残差大于2倍中误差的噪点；

重新对n1、n2剔除噪点后的点进行平面拟合，得到平面方程。

a1x+b1y+c1z+d1＝0；公式9

a2x+b2y+c2z+d2＝0；公式10

求m1、m2剔除噪点后的中心点坐标pm1、pn2；

求pm1到n1拟合平面的距离d1，求pm2到n2拟合平面的距离d2，距离计算按照公式来进行：

根据距离之差δd＝|d1-d2|调整翻滚角，当翻滚角没有误差时，往返点云平行或重合，δd＝0。

优选的方案中，调整过程中，首先确定翻滚角的调整量δr的正负号，设定一个调整步距a，对翻滚角分别调整a和-a，即分别设置δr＝a和δr＝-a，计算得到新的标定参数，重新计算局部点云的坐标，再计算两次调整后的δd，取δd变小相对应的调整量，确定翻滚角调整量的方向；

根据确定后的调整量正、负号继续对δr进行累计设置；如果调整量为正号，则继续设置δr＝a×i；如果调整量为负号，则继续设置δr＝-a×i；i表示按照步距调整的次数；

计算得到新的标定参数，然后重新计算局部点云的坐标，并计算调整后的δd；直到δd小于设定的阈值θ后，停止对翻滚角的调整。

优选的方案中，保持翻滚角的调整量δr不变，调整载体坐标系的航向角；

取同一条道路往返采集的，与行驶方向平行的建筑物立面的顶部和底部的局部点云，采用与翻滚角调整相同的方法调整至局部点云平行或重合。

优选的方案中，保持翻滚角的调整量δr和航向角的调整量不变，调整载体坐标系的俯仰角，选取道路相互垂直方向的两份点云，选择建筑物立面顶部和底部的局部点云，采用与翻滚角调整相同的方法调整至局部点云重合。

本发明提供的一种激光雷达移动测量系统的快速标定方法，通过采用以上的方案，不需要建立高精度控制场，仅需对实际现场进行采集，不需要人工选取大量控制点，所用的计算方法较为简便快速，实现起来较为简单，而且能够人工和自动相结合，确保标定参数的正确性。通过将坐标系统一到载体坐标系，在载体坐标系中进行角度标定，能适应任意的激光雷达安装角度。通过分析对载体坐标系的三个姿态角进行调整时世界坐标系中点云数据的变化规律，将三个姿态角进行分步标定，忽略三个平移量参数的误差，避免了同时考虑平移量与旋转角误差进行解算时两类参数的相关性的影响，提高标定参数结果的稳定性。通常情况下，标定参数中的平移量可以利用结构设计保证或直接测量而获得，误差可以达到2厘米以下，甚至可达到毫米级，而移动测量的整体精度一般要求为5厘米，因此平移量参数的误差可忽略。在姿态角调整过程中，通过对测量目标不同位置的局部点云截取，配合不同行走采集行走轨迹点云图像的选择，大幅提高了调整精度和速度。本发明的方法适应范围较广，解算精度高速度快，适用于车载、机载、背负式移动测量系统。

附图说明

下面结合附图和实施例对本发明作进一步说明：

图1为本发明中激光雷达移动测量系统的结构示意图。

图2为本发明中采集现场和采集行走轨迹的俯视示意图。

图3为本发明中用于翻滚角调整的点云截取位置示意图。

图4为本发明中道路横截面裁切点云示意图。

图5为本发明中道路横截面局部点云选取示意图。

图6为本发明中翻滚角调整后的道路横截面点云示意图。

图7为本发明中用于航向角调整的建筑物立面点云截取位置示意图。

图8a为本发明中建筑物立面点云裁切的示意图。

图8b为本发明中建筑物立面点云裁切的局部放大示意图。

图9a为本发明中建筑物立面点云裁切位置的主视图。

图9b为本发明中建筑物立面点云裁切位置的侧视图。

图10为本发明中航向角调整后的建筑物立面点云示意图。

图11为本发明中用于俯仰角调整的建筑物立面位置点云的俯视图。

图12为本发明中用于俯仰角调整的建筑物立面位置点云的侧视图。

图13为本发明中用于俯仰角调整后的建筑物立面点云的侧视图。

图14a为本发明中标定前的建筑物立面点云示意图。

图14b为本发明中标定后的建筑物立面点云示意图。

图中，卫星导航系统1，惯性导航系统2，载体坐标系21，定位定姿系统3，激光雷达系统4，激光雷达坐标系41，载体5，十字路口6，采集行走轨迹7，建筑物8，翻滚角采集横截面9，建筑物立面采集位置10。

具体实施方式

如图1～3、7中，一种激光雷达移动测量系统的快速标定方法，包括以下步骤：

s1、将具有惯性导航系统2、卫星导航系统1和激光扫描雷达系统4的移动测量系统在具有形状规则的建筑物的十字路口6多次采集激光雷达数据；

s2、根据惯性导航系统、卫星导航系统和激光扫描雷达系统4采集的数据联合解算点云；

将基于激光雷达坐标系41的原始点云转换为基于载体坐标系21的成果点云；

在转换过程中调整姿态角，分别根据翻滚角、航向角和俯仰角调整载体坐标系21的点云，补偿各次采集的点云的姿态角误差；

通过以上步骤实现激光雷达移动测量系统的快速标定。

优选的方案中如图2中，在步骤s1中，移动测量系统在十字路口6至少采集为三次，其中至少两次为往返的采集行走轨迹7，其中至少两次的采集行走轨迹7互相垂直。本发明的方案通过三次采集行走轨迹7的操作，即可完成整个标定工作，与现有技术的方案相比，大幅降低工作强度。

优选的方案中，从原始点云转换到成果点云的坐标转换过程为：

pi＝rpr+t；公式1该步骤为将激光雷达坐标系41的原始点云坐标系通过姿态矩阵和位置向量的运算转换为基于惯性导航系统2坐标系的位置和姿态。

pb＝rbipi；公式2该步骤为将基于惯性导航系统2坐标系的位置和姿态转换为基于载体坐标系21的位置和姿态。其中载体坐标系21与基于惯性导航系统2坐标系的坐标原点重合，因此，公式2中没有位置向量参数。

pw＝rwbpb+twb；公式3该步骤是将载体坐标系21转换为世界坐标系。

其中，pr为原始点云坐标，为3维列向量；

pr＝(xr，yr，zr)^t；公式4

pi为惯性导航系统坐标系中的点云坐标，t、r为标定参数，t表示激光雷达坐标系41相对于惯性导航系统坐标的位置参数，r表示激光雷达坐标系41相对于惯性导航系统坐标的姿态参数，其中t为3维列向量；

t＝(tx，ty，tz)^t；公式5

r为3×3原始姿态矩阵；

其中，roll表示翻滚角，yaw表示航向角，pitch表示俯仰角；

pb为载体坐标系中的点云坐标，rbi为惯性导航系统坐标系在载体坐标系中的姿态矩阵，载体坐标系的坐标原点和ins坐标系的坐标原点重合，所惯性导航系统坐标系转换到载体坐标系没有位置向量；

pw为世界坐标系中的点云坐标，rwb为载体坐标系在世界坐标系中的姿态矩阵，twb为载体坐标系在世界坐标系中的位置向量，rwb和twb由卫星导航系统1和惯性导航系统2提供。

优选的方案中，调整载体坐标系的姿态角：

如果对载体坐标系21进行姿态角调整，则：

pb′＝δr×pb；

其中，pb′为调整后的载体坐标系21坐标，δr为调整的姿态角矩阵。

调整前，激光雷达坐标系转换到载体坐标系21的公式为：

pb＝rbi(rpr+t)；

调整后，按照调整前的公式结构，t′、r′为调整后的标定参数；

pb′＝rbi(r′pr+t′)；

同时，

pb′＝δrrbi(rpr+t)；公式7

则

rbi(r′pr+t′)＝δrrbi(rpr+t)；

则

当调整载体坐标系21姿态角时，标定参数，即激光雷达坐标系41相对于惯性导航系统2的坐标系的位置和姿态，通过以上公式计算。由于激光扫描雷达系统4距离ins坐标系原点较近，在小角度调整的情况下，t′与t非常接近毫米级。此处的3维列向量t，由结构设计或测量得到，解算时t的误差予以忽略；

标定后，世界坐标系中的点云坐标按照以下公式计算：

pw′＝rwbpb′+twb；公式8

pw′为调整后的世界坐标系点云坐标。rwb为载体坐标系在世界坐标系中的姿态矩阵，twb为载体坐标系在世界坐标系中的位置向量。

优选的方案中，姿态角调整的顺序为首先调整翻滚角，然后调整航向角或俯仰角。

优选的方案如图3中，翻滚角的调整步骤为：

取同一条道路往返采集的点云进行矩形裁切，例如对图3中两条水平的采集行走轨迹7采集的点云的翻滚角采集横截面9，得到道路横截面；

在两次采集的点云，靠近道路横截面两端中心对称的位置分别截取一个窗口；如图4中所示。

分别调整两次采集的点云的翻滚角，直至窗口内两次采集的点云的误差小于阈值，完成对翻滚角的调整。如图8中所示。通过对道路横截面两端中心对称的位置的窗口点云的采集，能够确保获取较为明显的差异特征，并能够大幅减少运算量。

优选的方案如图4～8中，翻滚角具体的调整步骤为：

s100、设定一个道路宽度值w，w的单位为米，设定一个窗口大小s，s的单位为平方米，s可以是矩形或者圆形，分别取距离道路中心左右w位置的载体坐标系21局部点云，局部点云的范围由窗口大小s来定义；

将两次采集的局部点云分别命名为m1、n1、m2、n2；

s101、对窗口内的局部点云分别进行平面拟合，并统计残差和中误差，剔除残差大于2倍中误差的噪点；

重新对n1、n2剔除噪点后的点进行平面拟合，得到平面方程。

a1x+b1y+c1z+d1＝0；公式9

a2x+b2y+c2z+d2＝0；公式10

求m1、m2剔除噪点后的中心点坐标pm1、pn2；

求pm1到n1拟合平面的距离d1，求pm2到n2拟合平面的距离d2，距离计算按照公式来进行：

公式11的分子部分并没有取绝对值，而是保留正负号，以正负号可以用来判断点在平面的哪一侧。

根据距离之差δd＝|d1-d2|调整翻滚角，当翻滚角没有误差时，往返采集行走轨迹7的点云平行或重合，δd＝0。

优选的方案中，调整过程中，首先确定翻滚角的调整量δr的正负号，设定一个调整步距a，对翻滚角分别调整a和-a，即分别设置δr＝a和δr＝-a，计算得到新的标定参数，重新计算局部点云的坐标，再计算两次调整后的δd，取δd变小相对应的调整量，确定翻滚角调整量的方向；

根据确定后的调整量正、负号继续对δr进行累计设置；如果调整量为正号，则继续设置δr＝a×i；如果调整量为负号，则继续设置δr＝-a×i；i表示按照步距调整的次数；

计算得到新的标定参数，然后重新计算局部点云的坐标，并计算调整后的δd；直到δd小于设定的阈值θ后，停止对翻滚角的调整。由此得到翻滚角的标定参数。

优选的方案如图7中，保持翻滚角的调整量δr不变，调整载体坐标系21的航向角；

取同一条道路采集行走轨迹7往返采集的，与行驶方向平行的建筑物立面的顶部和底部的局部点云，例如图7中两条水平的采集行走轨迹7采集的建筑物立面靠近采集行走轨迹7一侧的局部点云，并取该局部点云顶部和底部的局部点云，采用与翻滚角调整相同的方法调整至局部点云平行或重合。

也是先取顶部和底部的局部点云，分别进行平面拟合，并统计残差和中误差，剔除残差大于2倍中误差的噪点；重新对剔除噪点后的点进行平面拟合，得到平面方程。首先确定航向角的调整量δr的正负号。根据确定后的航向角调整量正、负号继续对δr进行累计设置，根据步距调整的次数；计算得到新的标定参数，然后重新计算局部点云的坐标，并计算调整后的δd；直到δd小于设定的阈值θ后，停止对翻滚角的调整。调整的过程示例如图8～10中所示。

优选的方案如图11～13中，保持翻滚角的调整量δr和航向角的调整量不变，调整载体坐标系21的俯仰角，选取道路相互垂直方向的两份点云，选择建筑物立面顶部和底部的局部点云，采用与翻滚角调整相同的方法调整至局部点云重合。

需要说明的，本例中以翻滚角、航向角和俯仰角的顺序进行了说明，而实际上，采用翻滚角、俯仰角和航向角的顺序进行调整也是可行的，属于本方案的等同方案。

当三个姿态角都调整完成后，得到最终的标定参数。重新融合解算点云，将十字路口的四份点云同时加载显示，可以观察到同名地物的重合度良好。本发明的方法精度符合设计要求，且结算迅速，实用性强。

对三个姿态角进行调整时，可手动截取所需的局部点云；也可以通过训练后的人工智能自动截取所需的局部点云。本发明仅需截取特定位置的局部点云，在确保标定精度的前提下，大幅减少了计算量。

对三个姿态角进行调整时，可以通过训练后的人工智能自动调整，自动判断重复采集的点云是否平行；也可手动调整，人工判断重复采集的点云是否平行；灵活性较高。

上述的实施例仅为本发明的优选技术方案，而不应视为对于本发明的限制，本申请中的实施例及实施例中的特征在不冲突的情况下，可以相互任意组合。本发明的保护范围应以权利要求记载的技术方案，包括权利要求记载的技术方案中技术特征的等同替换方案为保护范围。即在此范围内的等同替换改进，也在本发明的保护范围之内。