CN117876504A - 应用于agv的激光雷达外参标定方法及装置 - Google Patents

Abstract

本申请提供一种应用于AGV的激光雷达外参标定方法及装置，涉及计算机视觉技术领域，方法包括：根据AGV的激光雷达发送的初始点云数据，计算激光雷达的目标参数并构建第一矩阵；将第一矩阵与初始点云数据的坐标点相乘，得到第一点云数据，根据第一点云数据构建第二矩阵；将第二矩阵与第一点云数据的坐标点相乘，得到第二点云数据，计算初始点云数据和目标点云数据的质心坐标差并构建第三矩阵；通过第三矩阵对第二点云数据进行齐次变换，生成第三点云数据并构建第四矩阵；结合第一、第二、第三及第四矩阵，生成激光雷达相对AGV的坐标系的六自由度信息。本申请具有的技术效果是：当多个激光雷达需要进行外参标定时，减少激光雷达外参标定时间。

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附图说明

图1是本申请实施例提供的一种应用于AGV的激光雷达外参标定方法的流程示意图；

图2是本申请实施例提供的一种应用于AGV的激光雷达外参标定装置的结构示意图；

图3是本申请实施例提供的一种电子设备的结构示意图。

附图标记说明：1000、电子设备；1001、处理器；1002、通信总线；1003、用户接口；1004、网络接口；1005、存储器。

具体实施方式

为了使本技术领域的人员更好地理解本说明书中的技术方案，下面将结合本说明书实施例中的附图，对本说明书实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。

在本申请实施例的描述中，“示性的”、“例如”或者“举例来说”等词用于表示作例子、例证或说明。本申请实施例中被描述为“示性的”、“例如”或者“举例来说”的任何实施例或设计方案不应被解释为比其它实施例或设计方案更优选或更具优势。确切而言，使用“示性的”、“例如”或者“举例来说”等词旨在以具体方式呈现相关概念。

图1是本申请实施例提供的一种应用于AGV的激光雷达外参标定方法的流程示意图。应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行；除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行；并且图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

本申请公开了一种应用于AGV的激光雷达外参标定方法，如图1所示，该方法包括S101-S105。

S101，获取目标位置的AGV的激光雷达发送的初始点云数据，根据初始点云数据，计算激光雷达的目标参数，并根据目标参数构建第一矩阵。

在一个示例中，在开始之前，需要需要将安装了激光雷达的AGV停在靠墙位置，这个位置需要确保激光雷达可以扫描到墙面和地面特征，以获取包含墙面和地面点云数据。然后在AGV的运动中心放置一个十字光标作为标记，记录该标记点在地面上的坐标(x，y，z)，作为后续计算AGV坐标系原点的依据。

在获得包含墙面和地面特征的点云数据后，需要人工调整此AGV上激光雷达的外参参数，将外参调整到最精准的状态。这里的外参参数包括激光雷达的三维位置和三维方向。人工调整外参需要依靠专业知识和经验，通过不断细微调整达到最佳状态。

将调整完毕的精准外参的参数状态保存下来，作为目标状态。之后在对其他AGV上的激光雷达进行外参标定时，都以这个目标状态作为标准，通过匹配的方式自动计算外参参数。

之所以要首先手动调整一个最精准的目标状态，是为了保证后续所有AGV的标定效果。手动调整一次虽然会花费时间，但可以为所有的自动标定提供一个精准的标准参考目标，从而提高标定的整体自动化程度和精准度。采用目标匹配的标定方式，可以避免多次手动调整，大大节省标定时间，也可以使每个AGV的标定精度达到与目标状态一致。需要说明的是，由于不同AGV上安装的激光雷达的位置和数量不同，例如：有的激光雷达倒装在AGV上，有的AGV上存在多个激光雷达，为了适应这些情况，在计算外参时需要考虑激光雷达的具体安装位置和方向。对于倒装的情况，需要进行坐标变换来调整点云;对于多个激光雷达，需要分别标定每个激光雷达的外参。但本方案的总体思路是一致的，即采用目标匹配的方式，使用预先手动调整好的目标状态作为标准，自动计算出不同安装情况下的精准外参。这种方法既适用于标准安装，也可扩展到非标准安装的情况，从而提高了标定的适用范围和鲁棒性。

初始点云数据是指在标定过程中，AGV上的激光雷达首先扫描获得的原始点云数据。该点云数据中包含了激光雷达在目标位置扫描到的全部三维空间点信息。其中会包含AGV正前方墙面上的点以及下方地面的点，也就是点云数据中会包含墙面特征和地面特征。这些特征对计算激光雷达的安装位置和方向非常重要。

初始点云数据既包含点的三维坐标信息(x，y，z)，也包含点的激光反射强度信息。坐标信息可以用于计算空间距离、法向量等，反射强度可以用于区分墙面、地面等不同特征。

获取初始点云的数据是标定过程的第一步。只有获取包含墙面和地面特征的点云数据，才能按照技术方案提取特征点，计算目标参数，构建变换矩阵，进而获得激光雷达的外参。

具体实现时，首先控制AGV移动到预定的标定位置，该位置需要使激光雷达能够扫描到地面和墙面特征。保持AGV静止后，开启激光雷达获取初始点云数据。接着对初始点云数据进行处理，通过设定地面约束条件，提取出满足条件的点作为地面特征点，即θ<45°且median(z)>min(zout)+ε。其中θ表示点云在激光雷达坐标系下x轴与候选地面法向量的夹角，median(z)表示全部点云z值的中值，zout表示不在候选地面法向量上的点的z值集合，ε为容差。然后，根据地面特征点构建平面方程，求解方程得到目标法向量(A，B，C)。将法向量代入相关公式，计算出激光雷达的离地高度/>、俯仰角/>和横滚角/>，即目标参数。最后，根据目标参数构建矩阵/>，即第一矩阵。其中，第一矩阵为：。

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

在一个示例中，为了从初始点云数据中准确提取出地面点，需要制定合适的地面提取约束条件。首先考虑到激光雷达安装相对地面角度不宜过大，一般来说，设置法向量与x轴夹角θ小于45°作为约束。另外，地面点的z值特征是整个点云中z值较大的一部分，因此设置中值大于非地面点z的最小值加容差ε作为约束。

根据这两个地面提取约束，可以遍历初始点云中的全部点，提取出满足两个条件的点作为地面点。具体方法是计算每个点对应的法向量，求取法向量与x轴夹角；同时取全部点的z值中值median(z)和不在当前法向量方向的点的z值最小值min(zout)；比较每个点的θ与45°大小，以及median(z)与min(zout)+ε大小，即可判断是否满足地面点条件，最终提取出所有满足条件的点作为地面特征点。

提取出地面特征点的目的是为了构建地面方程，进而计算出目标参数中的离地高度、俯仰角和横滚角。通过地面特征点拟合地面方程Ax+By+Cz+D=0，求解方程得到法向量(A，B，C)，带入相关公式计算出理想的目标参数。

这样在提取地面点时设置约束条件，可以提高地面点提取的准确性，进而提高基于地面点的目标参数计算准确性，使后续变换矩阵构建更加可靠，有助于最终标定结果的精度。

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

在一个示例中，在提取出地面特征点后，需要根据这些点来计算出离地高度、俯仰角和横滚角这三个目标参数。

首先，利用地面特征点拟合出地面方程Ax+By+Cz+D=0。这里A、B、C、D为待求系数，x、y、z为每个地面特征点在激光雷达坐标系下的坐标。通过最小二乘法可以解出A、B、C、D的值，得到地面方程。

接下来，需要从地面方程出发求解出目标法向量(A，B，C)。有了法向量后，将其代入三个预先建立的计算公式，可以得到理想的三个目标参数。

其中，离地高度计算公式为，带入法向量求/>。俯仰角计算公式为/>，横滚角计算公式为/>，带入法向量可以求出/>和/>。

通过拟合地面方程，解出法向量，再代入公式计算目标参数，这样可以避免直接从点云中计算带来的误差。利用地面特征点进行多步推导计算，可以提高目标参数的计算准确度，为后续的变换矩阵构建提供精确的控制参数。

S102，将第一矩阵与初始点云数据的坐标点算术相乘，得到第一点云数据，根据第一点云数据构建第二矩阵。

在一个示例中，在获取了第一矩阵后，将其与初始点云数据的各个坐标点进行算术相乘，得到第一点云数据。这样做的目的是利用第一矩阵/>对初始点云数据进行初步的坐标变换，使其外参得到一个粗调整的结果。

首先需要解析出第一矩阵中的旋转变换部分，包含有俯仰角/>、横滚角/>和偏航角/>。然后将每个坐标点(x，y，z)进行坐标旋转变换，得到调整后的坐标值(x1，y1，z1)。叠加第一矩阵中的平移变换部分，包含有离地高度，就可以算出第一点云数据。

接下来，需要根据处理后的第一点云数据来构建第二矩阵。具体是先移除第一点云数据中的地面点，剩余的点即为墙面点。然后利用这些墙面点构建墙面方程，求解方程得到墙面法向量，并计算偏航角/>。最后将偏航角/>代入构建第二矩阵/>。其中，。

这样顺序进行两次坐标变换的目的是实现外参的粗调整，通过初始粗调整逐步接近外参的理想值，为后续的精确调整打下基础，提高鲁棒性。进行两次粗调整可以使点云外参的精度更高。

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

在一个示例，在得到去除地面点后的第一点云数据后，需要基于其中的墙面特征点来计算偏航角并构建第二矩阵。

首先从第一点云数据中去除掉地面特征点，剩余的点即为墙面特征点。然后利用这些墙面特征点拟合墙面方程，其中、/>、为每个墙面点的坐标，E、F、G、H为待求系数。采用最小二乘法可以解出墙面方程。

接下来从墙面方程出发求解墙面法向量(E，F，G)，将法向量带入预设的偏航角计算公式来求取偏航角/>。

得到偏航角后，将其代入矩阵运算公式，可以构建出第二矩阵/>。/>包含了偏航角变换所需的运算内容。

这样通过先提取墙面点，再拟合墙面方程求解法向量，最后计算偏航角并构建，目的是为了引入偏航角这一坐标变换，使第一点云向目标位置外参更进一步逼近，为后续的叠加变换做准备。

S103，将第二矩阵与第一点云数据的坐标点算术相乘，得到第二点云数据，计算初始点云数据和目标位置对应的目标点云数据的质心坐标差，根据质心坐标差构建第三矩阵。

在一个示例中，在计算出第二矩阵后，需要继续对第一点云数据进行坐标变换。具体是将/>与第一点云数据中的各点坐标相乘，得到第二点云数据。这样进一步调整了点云数据的坐标，使其外参更加接近理想状态。

接下来需要计算初始点云数据和目标位置对应的目标点云数据之间在xy平面上的质心坐标差，包含x方向距离差和y方向距离差/>。这是为了进一步改善点云在xy平面上的位置差异。

计算初始点云数据的质心坐标和目标位置对应的目标点云数据的质心坐标/>的差，得到质心坐标差/>，其中，/>，，/>；根据质心坐标差，构建第三矩阵。其中，第三矩阵为：。

进行质心坐标建立矩阵并作用的目的是为了进一步提高点云数据的匹配精度。此前进行的是角度变换，现在进行平移变换，结合运动的六自由度对点云位置进行调整，有助于提高最后标定结果的准确度。通过多步调整，逐步去除点云与目标位置的差异，为后续精调匹配打下基础。

S104，通过第三矩阵对第二点云数据进行齐次变换，生成第三点云数据，根据初始点云数据和第三点云数据构建第四矩阵。

在一个示例中，在得到第三点云数据后，需要根据初始点云数据和调整后的第三点云数据来构建第四矩阵，进行精调。

具体包括：选取第三点云数据作为目标点云，初始点云数据为源点云，利用迭代最近点(ICP)算法进行点云匹配。ICP算法的思想是通过不断迭代优化的方式，在目标点云和源点云之间找到最优的旋转矩阵R和平移矩阵T，使配准误差最小。

在每次迭代中，计算源点云中的每个点在目标点云中的最近邻点，然后计算目标函数即误差函数E，包含了R和T。不断优化R和T，使E最小，最终得到最小误差时的R和T即为最优解。根据ICP算法求解得到的最终旋转矩阵R和平移矩阵T，构建第四矩阵。/>中包含了R和T，可以对Cloud3实现精确的坐标变换，使其达到与初始点云数据完全重合的效果。

进行ICP精调的目的是实现点云与目标位置的高精度配准，通过迭代不断优化的方式，可以达到外参的理想标定效果。多步骤分级调整有利于提高鲁棒性，避免单次标定的误差。

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

在一个示例中，为了使第三点云数据高精度匹配初始点云数据，需要使用迭代最近点(ICP)算法进行精确配准。

具体做法是先取第三点云数据作为目标点云，初始点云数据为源点云。然后在两组点云中迭代寻找最近的点对，计算其旋转矩阵R和平移向量T，并带入误差函数E(R，T)。其中E(R，T)表示了两点云配准的优化目标，通过不断迭代优化R和T以使E最小。

每次迭代中，从初始点云数据中取点p，在第三点云数据中找到最近邻点q，计算q和之间的距离误差。不断优化R和T以减小所有点对的误差平方和，直到优化过程收敛，最终得到最小E时的R和T作为最优配准结果。

得到最优的R和T后，将其代入矩阵运算公式，可以构建出第四矩阵。/>中包含了R和T，可以实现初始点云数据到第三点云数据的精确配准变换。

这样通过迭代最近点算法可以有效实现两组点云的高精度配准，避免了直接计算变换的误差。得到的第四矩阵包含了精确的旋转和平移信息，为点云的精准标定奠定基础。

S105，结合第一矩阵、第二矩阵、第三矩阵及第四矩阵，生成激光雷达相对AGV的坐标系的平移自由度信息和旋转自由度信息。

在一个示例中，在得到四个矩阵后，需要将它们结合起来，获得激光雷达相对于AGV坐标系的六自由度外参信息，包括平移量X、Y、Z和旋转量Roll、Pitch、Yaw。其中，X为激光雷达在AGV坐标系下X轴方向上的平移量，Y为激光雷达在AGV坐标系下Y轴方向上的平移量，Z为激光雷达在AGV坐标系下Z轴方向上的平移量，Roll为激光雷达绕X轴的旋转角度，Pitch为激光雷达绕Y轴的旋转角度，Yaw为激光雷达绕Z轴的旋转角度。

具体方法是将四个矩阵按顺序进行乘法运算，得到目标矩阵M。矩阵M包含了从初始点云数据到理想目标点云的全部坐标变换信息。然后对矩阵M进行求解，可以获得：X=M(0，3)、Y=M(1，3)、Z=M(2，3)；

Pitch=arcsin(-M(2，0))；

Roll=arcsin(M(2，1)/(cos(Pitch)))；

Yaw=arcsin(M(1，0)/(cos(Pitch)))。上述求解过程即获得了激光雷达安装的平移量X、Y、Z以及旋转量Roll、Pitch、Yaw，完成了整个坐标系的标定。

基于上述方法，本申请还公开了一种应用于AGV的激光雷达外参标定装置，如图2所示，图2是本申请实施例提供的一种应用于AGV的激光雷达外参标定装置的结构示意图。

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

需要说明的是：上述实施例提供的装置在实现其功能时，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将设备的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。另外，上述实施例提供的装置和方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。

请参见图3，为本申请实施例提供了一种电子设备的结构示意图。如图3所示，所述电子设备1000可以包括：至少一个处理器1001，至少一个网络接口1004，用户接口1003，存储器1005，至少一个通信总线1002。

其中，通信总线1002用于实现这些组件之间的连接通信。

其中，用户接口1003可以包括显示屏（Display）、摄像头（Camera），可选用户接口1003还可以包括标准的有线接口、无线接口。

其中，网络接口1004可选的可以包括标准的有线接口、无线接口（如WI-FI接口）。

其中，处理器1001可以包括一个或者多个处理核心。处理器1001利用各种接口和线路连接整个服务器内的各个部分，通过运行或执行存储在存储器1005内的指令、程序、代码集或指令集，以及调用存储在存储器1005内的数据，执行服务器的各种功能和处理数据。可选的，处理器1001可以采用数字信号处理（Digital Signal Processing，DSP）、现场可编程门阵列（Field-Programmable Gate Array，FPGA）、可编程逻辑阵列（ProgrammableLogic Array，PLA）中的至少一种硬件形式来实现。处理器1001可集成中央处理器（CentralProcessing Unit，CPU）、图像处理器（Graphics Processing Unit，GPU）和调制解调器等中的一种或几种的组合。其中，CPU主要处理操作系统、用户界面和应用程序等；GPU用于负责显示屏所需要显示的内容的渲染和绘制；调制解调器用于处理无线通信。可以理解的是，上述调制解调器也可以不集成到处理器1001中，单独通过一块芯片进行实现。

其中，存储器1005可以包括随机存储器（Random Access Memory，RAM），也可以包括只读存储器（Read-Only Memory）。可选的，该存储器1005包括非瞬时性计算机可读介质（non-transitory computer-readable storage medium）。存储器1005可用于存储指令、程序、代码、代码集或指令集。存储器1005可包括存储程序区和存储数据区，其中，存储程序区可存储用于实现操作系统的指令、用于至少一个功能的指令（比如触控功能、声音播放功能、图像播放功能等）、用于实现上述各个方法实施例的指令等；存储数据区可存储上面各个方法实施例中涉及的数据等。存储器1005可选的还可以是至少一个位于远离前述处理器1001的存储装置。如图3所示，作为一种计算机存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及一种应用于AGV的激光雷达外参标定方法的应用程序。

在图3所示的电子设备1000中，用户接口1003主要用于为用户提供输入的接口，获取用户输入的数据；而处理器1001可以用于调用存储器1005中存储一种应用于AGV的激光雷达外参标定方法的应用程序，当由一个或多个处理器执行时，使得电子设备执行如上述实施例中一个或多个所述的方法。

一种电子设备可读存储介质，所述电子设备可读存储介质存储有指令。当由一个或多个处理器执行时，使得电子设备执行如上述实施例中一个或多个所述的方法。

需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本申请并不受所描述的动作顺序的限制，因为依据本申请，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本申请所必需的。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所披露的装置，可通过其他的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些服务接口，装置或单元的间接耦合或通信连接，可以是电性或其他的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储器中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干指令用以使得一台计算机设备（可为个人计算机、服务器或者网络设备等）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储器包括：U盘、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述者，仅为本公开的示例性实施例，不能以此限定本公开的范围。即但凡依本公开教导所作的等效变化与修饰，皆仍属本公开涵盖的范围内。本领域技术人员在考虑说明书及实践这里的公开后，将容易想到本公开的其它实施方案。本申请旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未记载的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的范围和精神由权利要求限定。

Claims (10)

1.一种应用于AGV的激光雷达外参标定方法，其特征在于，所述方法包括：

获取目标位置的AGV的激光雷达发送的初始点云数据，根据所述初始点云数据，计算所述激光雷达的目标参数，并根据所述目标参数构建第一矩阵；

将所述第一矩阵与所述初始点云数据的坐标点算术相乘，得到第一点云数据，根据所述第一点云数据构建第二矩阵；

将所述第二矩阵与所述第一点云数据的坐标点算术相乘，得到第二点云数据，计算所述初始点云数据和所述目标位置对应的目标点云数据的质心坐标差，根据所述质心坐标差构建第三矩阵；

通过所述第三矩阵对所述第二点云数据进行齐次变换，生成第三点云数据，根据所述初始点云数据和所述第三点云数据构建第四矩阵；

结合所述第一矩阵、所述第二矩阵、所述第三矩阵及所述第四矩阵，生成所述激光雷达相对所述AGV的坐标系的平移自由度信息和旋转自由度信息。

2.根据权利要求1所述的应用于AGV的激光雷达外参标定方法，其特征在于，所述根据所述初始点云数据，计算所述激光雷达的目标参数，包括：

获取地面约束条件，所述地面约束条件包括：

且/>，其中，/>为在激光雷达坐标系下的x轴与候选地面法向量间的夹角，/>函数为对初始点云数据中的全部点云数据点的z值取中值，z值为初始点云数据中的点云数据点在激光雷达坐标系下的z轴坐标值，/>函数为取中的最小值，/>为不在候选地面法向量方向上的点云数据点的z值集合，/>为容差值；

根据所述地面约束条件，从所述初始点云数据中筛选出满足所述地面约束条件的点云数据点作为地面特征点；

根据所述地面特征点，计算所述激光雷达的目标参数。

3.根据权利要求2所述的应用于AGV的激光雷达外参标定方法，00其特征在于，所述目标参数包括离地高度、俯仰角及横滚角，所述根据所述地面特征点，计算所述激光雷达的目标参数，包括：

根据所述地面特征点，构建平面方程，所述平面方程为：，其中，A、B、C、D均为方程系数，x为地面特征点在激光雷达坐标系下的x坐标，y为地面特征点在激光雷达坐标系下的y坐标，z为地面特征点在激光雷达坐标系下的z坐标；

求解所述平面方程，得到目标法向量；

将所述目标法向量代入第一方程，得到所述离地高度，所述第一方程为：，其中，/>为所述离地高度；

将所述目标法向量代入第二方程，得到所述俯仰角，所述第二方程为：，其中，/>为所述俯仰角；

将所述目标法向量代入第三方程，得到所述横滚角，所述第三方程为：，其中，/>为所述横滚角。

4.根据权利要求2所述的应用于AGV的激光雷达外参标定方法，其特征在于，所述根据所述第一点云数据构建第二矩阵，包括：

去除所述第一点云数据中的所述地面特征点，得到墙面特征点；

根据所述墙面特征点，计算所述激光雷达的偏航角；

所述根据所述墙面特征点，计算所述激光雷达的偏航角，包括：

根据所述墙面特征点，构建墙面方程，所述墙面方程为：，其中，E、F、G、H均为方程系数，/>为墙面特征点在激光雷达坐标系下的x坐标，/>为墙面特征点在激光雷达坐标系下的y坐标，/>为墙面特征点在激光雷达坐标系下的z坐标；

求解所述墙面方程，得到墙面法向量；

将所述墙面法向量代入第四方程，得到所述偏航角，所述第四方程为：，/>为所述偏航角，/>均为方程系数；

根据所述偏航角，构建第二矩阵。

5.根据权利要求1所述的应用于AGV的激光雷达外参标定方法，其特征在于，所述计算所述初始点云数据和所述目标位置对应的目标点云数据的质心坐标差，根据所述质心坐标差构建第三矩阵，包括：

计算初始点云数据的质心坐标和所述目标位置对应的目标点云数据的质心坐标/>的差，得到质心坐标差/>，其中，，/>，/>；

根据所述质心坐标差，构建第三矩阵。

6.根据权利要求1所述的应用于AGV的激光雷达外参标定方法，其特征在于，所述根据所述初始点云数据和所述第三点云数据构建第四矩阵，包括：

利用迭代最近点算法，将所述第三点云数据作为目标点云，将所述初始点云数据作为源点云，匹配所述目标点云与所述源点云之间的旋转矩阵和平移矩阵；

将所述旋转矩阵和所述平移矩阵代入误差函数中迭代，当所述误差函数最小时，得到目标旋转矩阵和目标平移向量，所述误差函数为：，其中，/>为误差函数，/>为旋转矩阵，为平移向量，n为匹配点对数的数量，/>为所述初始点云数据中的第i个点云，/>为所述第三点云数据中与/>对应的最近点；

根据所述目标旋转矩阵和所述目标平移向量，构建第四矩阵。

7.根据权利要求1所述的应用于AGV的激光雷达外参标定方法，其特征在于，所述结合所述第一矩阵、所述第二矩阵、所述第三矩阵及所述第四矩阵，生成所述激光雷达相对所述AGV的坐标系的平移自由度信息和旋转自由度信息，包括：

将所述第一矩阵、所述第二矩阵、所述第三矩阵及所述第四矩阵依次进行算术相乘，得到目标矩阵；

求解所述目标矩阵，生成所述激光雷达相对于所述AGV的坐标系的平移自由度信息和旋转自由度信息，其中，所述平移自由度信息包括：X、Y、Z，所述旋转自由度信息包括：Roll、Pitch、Yaw，其中，X为激光雷达在AGV坐标系下X轴方向上的平移量，Y为激光雷达在AGV坐标系下Y轴方向上的平移量，Z为激光雷达在AGV坐标系下Z轴方向上的平移量，Roll为激光雷达绕X轴的旋转角度，Pitch为激光雷达绕Y轴的旋转角度，Yaw为激光雷达绕Z轴的旋转角度。

8.一种应用于AGV的激光雷达外参标定装置，其特征在于，所述装置包括：获取模块、第一构建模块、第二构建模块、第三构建模块及生成模块；其中，

所述获取模块，用于获取目标位置AGV的激光雷达发送的初始点云数据，根据所述初始点云数据，计算所述激光雷达的目标参数，并根据所述目标参数，构建第一矩阵；

所述第一构建模块，用于将所述第一矩阵与所述初始点云数据的坐标点算术相乘，得到第一点云数据，根据所述第一点云数据，构建第二矩阵；

所述第二构建模块，用于将所述第二矩阵与所述第一点云数据的坐标点算术相乘，得到第二点云数据，计算所述初始点云数据和目标点云数据的质心坐标差，根据所述质心坐标差，构建第三矩阵；

所述第三构建模块，用于通过所述第三矩阵对所述第二点云数据进行齐次变换，生成第三点云数据，根据所述初始点云数据和所述第三点云数据之间的位置关系，构建第四矩阵；

所述生成模块，用于结合所述第一矩阵、所述第二矩阵、所述第三矩阵及所述第四矩阵，生成所述激光雷达相对所述AGV的坐标系的平移自由度信息和旋转自由度信息。

9.一种电子设备，其特征在于，包括处理器、存储器、用户接口及网络接口，所述存储器用于存储指令，所述用户接口和网络接口用于给其他设备通信，所述处理器用于执行所述存储器中存储的指令，以使所述电子设备执行如权利要求1-7任意一项所述的方法。

10.一种计算机可读存储介质，其特征在于，存储有能够被处理器加载并执行如权利要求1-7任意一项所述的方法的计算机程序。