应用于AGV的激光雷达外参标定方法及装置与流程

本技术涉及计算机视觉，具体涉及一种应用于agv的激光雷达外参标定方法及装置。

背景技术：

1、随着自动导航车辆的广泛应用，其安全性和导航定位精度的要求日益提高。为满足这一要求，在自动导航车辆尤其是自动引导车(automated guidedvehicle，agv)上通常会安装激光雷达，以实现对环境的感知。激光雷达安装的位置和方向会直接影响感知效果，因此必须对激光雷达进行外参标定，以得到激光雷达的外参。外参的确定是为了使激光雷达获取的点云数据可以准确转换到导航车辆的坐标系统下，这是实现导航车辆的定位和避障等功能所必须的。

2、目前，传统的激光雷达的外参标定方法通常依赖于精密的机械装置，通过机械装置改变激光雷达的位姿进行标定，然而，由于这种传统的标定方法操作复杂，导致外参标定的时间较长，当存在多个激光雷达需要进行外参标定时，会耗费大量时间成本。

技术实现思路

1、本技术提供一种应用于agv的激光雷达外参标定方法及装置，用于当多个激光雷达需要进行外参标定时，减少激光雷达外参标定时间。

2、第一方面，本技术提供了一种应用于agv的激光雷达外参标定方法，所述方法包括：获取目标位置的agv的激光雷达发送的初始点云数据，根据所述初始点云数据，计算所述激光雷达的目标参数，并根据所述目标参数构建第一矩阵；将所述第一矩阵与所述初始点云数据的坐标点算术相乘，得到第一点云数据，根据所述第一点云数据构建第二矩阵；将所述第二矩阵与所述第一点云数据的坐标点算术相乘，得到第二点云数据，计算所述初始点云数据和所述目标位置对应的目标点云数据的质心坐标差，根据所述质心坐标差构建第三矩阵；通过所述第三矩阵对所述第二点云数据进行齐次变换，生成第三点云数据，根据所述初始点云数据和所述第三点云数据构建第四矩阵；结合所述第一矩阵、所述第二矩阵、所述第三矩阵及所述第四矩阵，生成所述激光雷达相对所述agv的坐标系的平移自由度信息和旋转自由度信息。

3、通过采用上述技术方案，计算目标参数矩阵对初始点云进行初步配准，提高后续计算的鲁棒性。然后，依次构建第二、第三矩阵对点云位置和角度进行校准，逐步优化平移和旋转变换，减少积累误差。采用迭代最近点算法计算第四矩阵，实现高精度的点云配准。最后，多个矩阵运算获得目标矩阵，直接转换为六自由度外参结果。整个方法分步骤实现点云渐进配准，避免单步骤标定误差积累，提高了计算准确率。相较于手工标定，该技术方案实现了快速自动批量标定，大幅提升效率，降低了自动导航车辆系统集成难度，有效改善了感知定位和环境构建质量。当多个激光雷达需要进行外参标定时，减少激光雷达外参标定时间。

4、可选的，所述根据所述初始点云数据，计算所述激光雷达的目标参数，包括：获取地面约束条件，所述地面约束条件包括：且，其中，为在激光雷达坐标系下的x轴与候选地面法向量间的夹角，函数为对初始点云数据中的全部点云数据点的z值取中值，z值为初始点云数据中的点云数据点在激光雷达坐标系下的z轴坐标值，函数为取中的最小值，为不在候选地面法向量方向上的点云数据点的z值集合，为容差值；根据所述地面约束条件，从所述初始点云数据中筛选出满足所述地面约束条件的点云数据点作为地面特征点；根据所述地面特征点，计算所述激光雷达的目标参数。

5、通过采用上述技术方案，设定点到水平面法向量角度和点云数据点在激光雷达坐标系下的z轴坐标值，合理限定了地面点的法向和高程分布范围。然后依据这两个约束条件筛选满足要求的点作为地面特征点，过滤掉大量非地面点。最后，利用地面特征点拟合平面和计算目标参数，有效减少了非地面点对结果的干扰。基于地面特征点计算目标参数，能有效提高计算精度。

6、可选的，所述目标参数包括离地高度、俯仰角及横滚角，所述根据所述地面特征点，计算所述激光雷达的目标参数，包括：根据所述地面特征点，构建平面方程，所述平面方程为：，其中，a、b、c、d均为方程系数，x为地面特征点在激光雷达坐标系下的x坐标，y为地面特征点在激光雷达坐标系下的y坐标，z为地面特征点在激光雷达坐标系下的z坐标；求解所述平面方程，得到目标法向量；将所述目标法向量代入第一方程，得到所述离地高度，所述第一方程为：，其中，为所述离地高度；将所述目标法向量代入第二方程，得到所述俯仰角，所述第二方程为：，其中，为所述俯仰角；将所述目标法向量代入第三方程，得到所述横滚角，所述第三方程为：，其中，为所述横滚角。

7、通过采用上述技术方案，依据地面特征点拟合地面方程，解出法向量，避免了直接基于点云计算法向量的误差。然后，根据第一公式，利用法向量计算离地高度；据第二、三公式，计算出俯仰角和横滚角。实现了法向量的精确解算。法向量代入预设公式，可直接获得理想的目标参数。高精度目标参数的计算，奠定了后续变换矩阵构建的基础。

8、可选的，所述根据所述第一点云数据构建第二矩阵，包括：去除所述第一点云数据中的所述地面特征点，得到墙面特征点；根据所述墙面特征点，计算所述激光雷达的偏航角；所述根据所述墙面特征点，计算所述激光雷达的偏航角，包括：根据所述墙面特征点，构建墙面方程，所述墙面方程为：，其中，e、f、g、h均为方程系数，为墙面特征点在激光雷达坐标系下的x坐标，为墙面特征点在激光雷达坐标系下的y坐标，为墙面特征点在激光雷达坐标系下的z坐标；求解所述墙面方程，得到墙面法向量；将所述墙面法向量代入第四方程，得到所述偏航角，所述第四方程为：，为所述偏航角，均为方程系数；根据所述偏航角，构建第二矩阵。

9、通过采用上述技术方案，从去除地面点后的第一点云中提取墙面特征点。然后根据墙面特征点拟合墙面方程，并求解出墙面法向量。代入预设的偏航角计算公式，精确求解偏航角。最后将求得的偏航角应用于矩阵构建。通过墙面方程的建立和求解，实现了偏航角的准确解算。准确的偏航角可构建精确的第二矩阵，从而提高第一点云到目标点云在水平面内的配准效果。提高了偏航角以及第二矩阵的计算鲁棒性，减少了误差的累积。

10、可选的，所述计算所述初始点云数据和所述目标位置对应的目标点云数据的质心坐标差，根据所述质心坐标差构建第三矩阵，包括：计算初始点云数据的质心坐标和所述目标位置对应的目标点云数据的质心坐标的差，得到质心坐标差，其中，，，；根据所述质心坐标差，构建第三矩阵。

11、通过采用上述技术方案，通过矩阵运算编码平移信息，避免了直接计算平移向量带来的误差，计算质心坐标差可以有效消除点云在xy平面内的位置偏差，分步计算矩阵，降低单步计算带来的误差积累。通过矩阵运算编码平移信息，计算质心坐标差实现点云中心重合，避免直接计算带来的误差，分步计算降低单步误差积累，使点云与目标在xy平面和z轴方向达到高精度配准，为后续旋转校准奠定基础。

12、可选的，所述根据所述初始点云数据和所述第三点云数据构建第四矩阵，包括：利用迭代最近点算法，将所述第三点云数据作为目标点云，将所述初始点云数据作为源点云，匹配所述目标点云与所述源点云之间的旋转矩阵和平移矩阵；将所述旋转矩阵和所述平移矩阵代入误差函数中迭代，当所述误差函数最小时，得到目标旋转矩阵和目标平移向量，所述误差函数为：，其中，为误差函数，为旋转矩阵，为平移向量，n为匹配点对数的数量，为所述初始点云数据中的第i个点云，为所述第三点云数据中与对应的最近点；根据所述目标旋转矩阵和所述目标平移向量，构建第四矩阵。

13、通过采用上述技术方案，设置第三点云为目标点云，初始点云为源点云，迭代计算两者间的旋转矩阵和平移向量。然后构建误差函数，通过不断迭代优化旋转矩阵和平移向量，使误差函数最小化，求解出最优目标旋转矩阵和平移向量。最后，将其应用到第四矩阵的构建中。相较于直接计算变换，迭代最近点算法实现了两组点云的高精度配准。得到的第四矩阵中包含了两点云间精确的旋转和平移信息，是实现外参标定的关键。分步骤迭代计算点云配准，避免了一次性计算带来的误差积累，显著提高了计算鲁棒性，大幅提升了外参的计算精度。

14、可选的，所述结合所述第一矩阵、所述第二矩阵、所述第三矩阵及所述第四矩阵，生成所述激光雷达相对所述agv的坐标系的平移自由度信息和旋转自由度信息，包括：将所述第一矩阵、所述第二矩阵、所述第三矩阵及所述第四矩阵依次进行算术相乘，得到目标矩阵；求解所述目标矩阵，生成所述激光雷达相对于所述agv的坐标系的平移自由度信息和旋转自由度信息，其中，所述平移自由度信息包括：x、y、z，所述旋转自由度信息包括：roll、pitch、yaw，其中，x为激光雷达在agv坐标系下x轴方向上的平移量，y为激光雷达在agv坐标系下y轴方向上的平移量，z为激光雷达在agv坐标系下z轴方向上的平移量，roll为激光雷达绕x轴的旋转角度，pitch为激光雷达绕y轴的旋转角度，yaw为激光雷达绕z轴的旋转角度。

15、通过采用上述技术方案，该计算过程通过矩阵运算和解析，获取描述激光雷达外参的六度自由度信息，完成了坐标系标定参数的准确输出。将四个编码变换信息的矩阵进行顺序相乘，得到目标矩阵。然后，解析目标矩阵，依据提前设定的解析公式，计算出平移向量和旋转角。避免了直接计算带来的误差。矩阵编码和运算保证了信息准确传递，解析公式的设计考虑了输出需求，实现了外参的准确获取。充分利用了四个矩阵包含的变换信息，通过运算解析直接输出外参，整体流程合理，精度高，对外参标定性能提升显著。

16、第二方面，本技术提供一种应用于agv的激光雷达外参标定装置，所述装置包括：获取模块、第一构建模块、第二构建模块、第三构建模块及生成模块；其中，所述获取模块，用于获取目标位置agv的激光雷达发送的初始点云数据，根据所述初始点云数据，计算所述激光雷达的目标参数，并根据所述目标参数，构建第一矩阵；所述第一构建模块，用于将所述第一矩阵与所述初始点云数据的坐标点算术相乘，得到第一点云数据，根据所述第一点云数据，构建第二矩阵；所述第二构建模块，用于将所述第二矩阵与所述第一点云数据的坐标点算术相乘，得到第二点云数据，计算所述初始点云数据和目标点云数据的质心坐标差，根据所述质心坐标差，构建第三矩阵；所述第三构建模块，用于通过所述第三矩阵对所述第二点云数据进行齐次变换，生成第三点云数据，根据所述初始点云数据和所述第三点云数据之间的位置关系，构建第四矩阵；所述生成模块，用于结合所述第一矩阵、所述第二矩阵、所述第三矩阵及所述第四矩阵，生成所述激光雷达相对所述agv的坐标系的平移自由度信息和旋转自由度信息。

17、通过采用上述技术方案，计算目标参数矩阵对初始点云进行初步配准，提高后续计算的鲁棒性。然后，依次构建第二、第三矩阵对点云位置和角度进行校准，逐步优化平移和旋转变换，减少积累误差。采用迭代最近点算法计算第四矩阵，实现高精度的点云配准。最后，多个矩阵运算获得目标矩阵，直接转换为六自由度外参结果。整个方法分步骤实现点云渐进配准，避免单步骤标定误差积累，提高了计算准确率。相较于手工标定，该技术方案实现了快速自动批量标定，大幅提升效率，降低了自动导航车辆系统集成难度，有效改善了感知定位和环境构建质量。当有多个激光雷达需要标定时，减少时间标定成本，满足应用需求。

18、第三方面，本技术提供一种电子设备，采用如下技术方案：包括处理器、存储器、用户接口及网络接口，所述存储器用于存储指令，所述用户接口和网络接口用于给其他设备通信，所述处理器用于执行所述存储器中存储的指令，以使所述电子设备执行如上述任一种应用于agv的激光雷达外参标定方法的计算机程序。

19、第四方面，本技术提供一种计算机可读存储介质，采用如下技术方案：存储有能够被处理器加载并执行上述任一种应用于agv的激光雷达外参标定方法的计算机程序。

20、综上所述，本技术包括以下至少一种有益技术效果：

21、1. 当多个激光雷达需要进行外参标定时，减少激光雷达外参标定时间；

22、2.将四个编码变换信息的矩阵进行顺序相乘，得到目标矩阵。然后，解析目标矩阵，依据提前设定的解析公式，计算出平移向量和旋转角。避免了直接计算带来的误差。矩阵编码和运算保证了信息准确传递，解析公式的设计考虑了输出需求，实现了外参的准确获取。充分利用了四个矩阵包含的变换信息，通过运算解析直接输出外参，整体流程合理，精度高，对外参标定性能提升显著。