一种移动载体上激光雷达同时建图和内参标定方法

1.本发明涉及自动驾驶汽车领域，尤其涉及一种移动载体上激光雷达同时建图和内参标定方法。
背景技术：
：：2.激光雷达具有直接测量三维位置、探测范围远、受光照干扰小等优点。在无人车和机器人的定位、建图、感知等任务中发挥了重要作用。3.激光雷达内部一般包含多通道激光收发电路，使用电机带动收发模块或者镜片运动，实现较大范围的扫描。受光电器件非线性、各通道间差异、机械误差等因素影响，厂家需要对每颗激光雷达进行内参标定，才能达到较好的测量精度。4.激光雷达安装在自动驾驶车辆上后，内参受长期振动、冷热循环、器件老化等因素影响缓慢漂移，如果仍然使用出厂时标定的内参，点云精度会下降。现有的内参标定方法主要包括：5.1、监督式标定：用标定板等装置搭建专门的标定场景，使用高精度扫描仪获取场景的三维模型真值。用待标定的激光雷达扫描上述场景，优化雷达的内参和在场景中的位姿，使其点云尽可能落到真值模型的面上。此类方法具有标定速度快、内参精度高等优势，但标定厂房的搭建和建模成本高昂，且让用户定期将车辆送回厂房标定是不现实的。6.2、非监督式标定：放松了对标定场景的要求，不需要场景真值模型，并假设场景是局部平整的。先用待标定的激光雷达在场景中多个位置静态采集数据，建立场景的第一个点云模型。再将上述激光雷达放回场景中心，以多个倾斜角静态采集数据，建立场景的第二个点云模型。优化激光雷达的内参使两点云中平面尽可能重合。7.上述非监督标定方法一定程度上克服了场地和成本的限制，但在实践中仍存在以下问题：8.1、场景模型由多帧点云配准而来，配准误差降低了最终的标定精度。9.2、对于参数量较大的内参模型，单一场景提供的约束可能不足，导致标定结果错误。10.3、对于固定在车辆上的激光雷达，难以通过改变倾斜角的方式采集数据。11.4、采集每帧点云前，必须先停稳载体，实际操作效率不高。技术实现要素：12.针对现有技术不足，本发明提出了一种移动载体上激光雷达同时建图和内参标定方法，包括如下步骤：13.s101：根据待标定激光雷达的机械和光学结构，设计修正测距偏差和测角偏差的内参模型；14.s102：将待标定激光雷达和惯性测量单元(inertialmeasurementunit，imu)刚性固定在载体上，采集载体在道路上行驶过程中激光雷达的原始测量值和imu运动数据；15.s103：读取一帧激光雷达的原始测量值，读取s101中内参模型的参数，补偿测量偏差，生成一帧点云；16.s104：补偿s103中点云帧的运动畸变，计算点云帧相对于场景点云地图的位姿，将点云帧融合入场景点云地图；17.s105：搜索s104所述场景点云地图中每个点的k近邻，计算每个点的法向量和局部平整度；18.s106：每间隔预设的帧数，对s104所述场景点云地图随机下采样，产生两个子点云地图，计算它们的平面一致性代价，并执行下一步；如果没有积累足够的新帧，则退回s103继续处理下一帧；19.s107：求s106中平面一致性代价对内参模型中内参的偏导，使用梯度下降法更新内参模型中的内参数值；20.s108：删去s104所述场景点云地图中距离当前激光雷达位置超出预设距离的点，衰减s107中更新内参数值的步长；21.s109：退回s103继续处理下一帧，直至采集的所有数据用完，输出所述内参模型的参数作为最终标定结果。22.进一步的，步骤s101中，待标定激光雷达为n线的机械扫描激光雷达，其内参模型如下：23.标定前第i束激光的测距值记为第i束激光的俯仰角记为第i束激光相对于转子的方向角记为转子编码器输出的方向角记为24.使用查找表补偿测距偏差；所述查找表记为dtable，保存了n束激光在k种距离下的测距偏差值；距离点数k常用值为8至16，在激光雷达量程内指数分布，如1m、2m、4m…128m、256m；查表操作记为通过插值生成任意下的测距偏差值；标定后的测距值25.使用补偿第i束激光的俯仰角，标定后的俯仰角26.使用补偿第i束激光相对转子的方向角，标定后相对转子的方向角27.使用二次谐波模型补偿转子编码器的测角误差，标定后转子的方向角使用二次谐波模型补偿转子编码器的测角误差，标定后转子的方向角28.若用户没有给定上述内参模型中参数的初始值，将所有参数初始化为0。29.进一步的，步骤s102中，载体为使用轮子或履带或机械足驱动自身行进的车辆或机器人；激光雷达原始测量值包括激光测距值、激光出射角和时间戳；imu运动数据至少包括三轴加速度和三轴角速度。上述数据采集是不间断的，采集过程中载体在持续行进。30.进一步的，步骤s103中，使用s101中建立的内参模型和保存的内参数值，补偿激光雷达原始测量值的偏差；将补偿后的测量值[di,θi,φi,φr]转换为空间直角坐标系中点云的公式如下：[0031][0032]约定激光雷达中心为原点，前方为x轴，左方为y轴，上方为z轴。部分型号的激光雷达如velodynehdl-64，每束激光出射位置的差异较大，使用horizoffset和vertoffset补偿激光出射位置在水平和高度方向上的偏移。[0033]进一步的，步骤s104中，补偿点云帧运动畸变的方法是：[0034]使用imu运动数据推算采集此帧过程中激光雷达的位姿，按点云时间戳对上述位姿插值，得到采集每个点时的激光雷达的位姿；[0035]计算上述位姿相对于参考位姿的变换，对每个点的坐标施加上述变换；[0036]其中，参考位姿常选择点云帧起始或中间或结尾时刻激光雷达的位姿；[0037]计算点云帧相对于场景点云地图的位姿的方法是：[0038]使用加权的点对面icp算法将点云帧和场景点云地图配准得到激光雷达的位姿，把点云配准得到的激光雷达的位姿和imu推算得到的激光雷达的位姿输入卡尔曼滤波器，得到滤波后的激光雷达的位姿并更新imu漂移参数。[0039]进一步的，步骤s105中，使用kd-tree或八叉树或体素哈希表对点云进行组织，在此基础上查找某点的k近邻，k的常用值为8至16；计算某点法向量的方法是:[0040]计算其k近邻坐标的均值，将k近邻坐标减去均值后计算协方差矩阵，对该矩阵进行特征值分解，该点的法向量为最小特征值对应的特征向量；若最小特征值小于阈值，则局部平整度定为1，否则局部平整度为阈值除以最小特征值。[0041]进一步的，步骤s106中，帧数间隔取值为50至200；[0042]随机下采样的方法是，遍历场景点云地图中的点，按预设概率执行{放入子点云a，放入子点云b，直接跳过}中的一种操作；[0043]平面一致性代价c＝σ|dot(pa-pb,nbkb)|，上式中pa为点云a中的点，pb为点云b中pa的最近邻点，nb为pb的法向量，kb为pb对应的权重，dot为向量点乘，竖线代表绝对值。[0044]进一步的，步骤s107中，偏导矩阵j＝d(c+r)/dp，上式中c为平面一致性代价，p为内参，r为正则项；[0045]内参数值更新方式为p＝p-αj，其中α为步长；根据标定场景质量调节正则化强度和内参更新步长。[0046]相比于现有的激光雷达内参标定方法，本方法具有如下优势：[0047]1、本方法在载体行驶时连续的采集数据，对路径精度和载体姿态无严格要求。相比于需要在预设位置和姿态下静止采集数据的方法，降低了操作难度，提高了采集效率，尤其适合标定已固定在车辆或机器人上的激光雷达。[0048]2、本方法采集点云的视点(viewpoint)集中在载体行驶路径上，相邻点云帧的内容较为接近，降低了点云配准的难度。本方法还使用了传感器融合、面元估计等技术，可构建较为精细的场景点云地图，也有利于提高最终的标定精度。[0049]3、本方法迭代的构建场景点云地图片段和更新激光雷达内参数值，每个场景片段的平面信息都参与了内参数值的更新，降低了随机噪声的干扰，提高了内参标定的精确性，单个场景片段平面不够丰富时也可以进行标定。附图说明[0050]图1为本发明提出的移动载体上激光雷达同时建图和内参标定方法的流程图；[0051]图2为本发明使用的内参模型的示意图；[0052]图3为本发明使用的加权点对面icp的流程图。具体实施方式[0053]为了更为具体地描述本发明，下面结合附图和具体实施方式对本发明的技术方案作进一步详细描述。[0054]激光雷达向周围环境发射激光并接收物体反射的激光，通过测量激光的飞行时间(time-of-flight，tof)确定被测物体的距离，通过测量激光的出射角度，确定被测物体的角度。测距值、测角值、时间戳常被称为激光雷达的原始测量值，本发明设计的内参模型主要修正激光雷达的测距偏差和测角偏差。[0055]一般来说，激光雷达使用晶体振荡器产生基准时钟，使用硬件逻辑控制激光的发射次序和时刻(firingsequencetiming)。此种定时方案偏移低、抖动小、老化慢，因此激光雷达时间戳的误差可以忽略。[0056]出于成本考虑，一些激光雷达厂家只标定了部分内参，或者使用了过度简化的内参模型。即使厂家进行了精细标定，激光雷达安装在自动驾驶车辆上后，内参受长期振动、冷热循环、器件老化等因素影响仍会缓慢漂移。因此设计完善的内参模型和易用的标定方法，对于提高和保持激光雷达数据的精度是十分必要的。[0057]激光雷达的内参标定方法主要有两类，监督式标定和非监督式标定。监督式标定需要用标定板等装置搭建专门的标定场景，使用高精度扫描仪获取场景的三维模型真值。用待标定的激光雷达扫描上述场景，优化雷达的内参和在场景中的位姿，使其点云尽可能落到真值模型的面上。此类方法具有标定速度快、内参精度高等优势，但标定厂房的搭建和建模成本高昂，且让用户定期将车辆送回厂房标定是不现实的。[0058]非监督式标定放松了对标定场景的要求，不需要场景真值模型，并假设场景是局部平整的。一种典型的方法先用待标定的激光雷达在场景中多个位置静态采集数据，建立场景的第一个点云模型。再将上述激光雷达放回场景中心，以多个倾斜角静态采集数据，建立场景的第二个点云模型。优化激光雷达的内参使两点云中平面尽可能重合。[0059]上述非监督标定方法一定程度上克服了场地和成本的限制，但在实践中仍存在较多问题：[0060]易用性方面，现有方法要求静态采集数据，车辆完成一组向前行驶、减速停稳、静态采集的动作需要5秒左右，数据采集效率不高。此外现有方法要求改变激光雷达的倾角，因为激光雷达已固定在车辆上，需要车辆行驶到不同坡度的斜坡上，变相提高了对场地的要求。[0061]准确性方面，非监督标定使用的场景模型由多帧点云配准而来，配准误差制约着标定精度。此外对于参数量较大的内参模型，单一场景提供的约束可能不足，导致标定结果错误。增加数据采集点和摆放倾角的数量可以缓解上述问题，但是增加了标定流程的复杂度。[0062]为提升移动载体上激光雷达内参标定的易用性和准确性，本发明提出了一种移动载体上激光雷达同时建图和内参标定方法，图1给出了本发明的方法流程图。本发明根据激光雷达的工作原理和物理结构建立了修正测距偏差和测角偏差的内参模型，在载体行驶时连续的采集环境点云数据和运动数据，迭代的构建地图片段和更新内参数值，最终得到较为精确的激光雷达内参和场景地图。本发明使用运动补偿、面元估计、传感器融合等技术降低建图误差，通过优化地图片段中面元在多帧观测下的一致性来更新内参数值。[0063]本部分以32线机械扫描式激光雷达velodynevlp-32c为例展开介绍，但方法同样适用于其他型号。vlp-32c的详细资料可参见https://velodynelidar.com/products/ultra-puck/。[0064]下面介绍本发明提出的移动载体上激光雷达同时建图和内参标定方法的具体步骤：[0065]如图1所示，步骤s101中，设计激光雷达的内参模型。测距偏差的来源包括温度变化、器件老化、对焦误差、激光收发电路的非线性等，例如近处和远处物体返回的光强相差几个数量级，大动态范围下接收电路的信号变形、时延改变。又因为vlp-32c每通道的激光收发电路是独立的，使用关于激光线号i和原始测距值的查找表dtable描述测距偏差。查找表保存了32束激光在15种距离下的测距偏差值，距离点在2m至256m范围内指数分布。查表操作记为通过插值生成任意下的测距偏差值。标定后的测距值下的测距偏差值。标定后的测距值[0066]vlp-32c的测角值包含两部分。第一部分是激光束相对于转子的俯仰角和方向角激光收发电路和镜头刚性固定在转子上，和的偏差主要来源于装配误差，且随着激光雷达老化缓慢漂移。使用补偿第i束激光的俯仰角，标定后的俯仰角使用补偿第i束激光相对转子的方向角，标定后相对转子的方向角[0067]第二部分是转子相对于底座的方向角vlp-32c使用磁编码器测量其偏差来源包括磁感元件安装的偏心和倾斜、磁感元件轴灵敏度差异、外界磁场干扰等。二次谐波模型可以较好的拟合上述偏差，标定后转子的方向角[0068]图2给出了上述内参模型的示意图。用户可以给定上述内参模型中参数的初始值，没有给定的默认为0。[0069]步骤s102中，实验汽车在道路上行驶，采集传感器数据。车顶上安装了激光雷达、imu、gnss接收机，使用定制的金属结构件将传感器可靠的固定在一起，使用专门的硬件控制器实现传感器时间同步。激光雷达帧率为10hz，imu数据速率为2khz，gnss数据速率为5hz。在某园区内部道路行驶，最快速度约10m/s，道路旁建筑较为密集。[0070]值得注意的是，本方法适用的载体并不限于汽车。本方法中gnss数据不是必须的，但有助于提高精度和效率。本方法可从自然场景中筛选面特征，但选择平面丰富、干扰较少的场景有助于提高精度和效率。本方法要求imu和激光雷达刚性连接，使用金属结构件将imu固定在激光雷达底座附近，可获得较高的刚度，降低行驶中连接形变带来的误差。[0071]值得注意的是，采集到的点云密度和行驶速度成反比，当点云密度低于一定限度后，点云配准的误差会急剧增加，导致后续的建图和标定精度达不到要求。当点云密度高于一定限度后，点云配准的精度基本不再提升，继续降低车速只会增加采集数据的耗时。较适合vlp-32c的车速范围是2m/s至10m/s，对于更高线数的激光雷达，可相应的提高行驶速度。[0072]步骤s103中，使用s101中建立的内参模型和保存的内参数值，补偿激光雷达原始测量值的偏差。将补偿后的测量值[di,θi,φi,φr]转换为空间直角坐标系(激光雷达坐标系)中点云的公式如下：[0073][0074]约定激光雷达中心为原点，前方为x轴，左方为y轴，上方为z轴，中心点坐标和前方(φr＝0)的定义可在用户手册中找到。上式中horizoffset和vertoffset代表激光出射位置在水平和高度方向上的偏移，由于vlp-32c内部所有激光束共用一枚透镜准直后发射，即存在唯一的光学中心，horizoffset和vertoffset可忽略。[0075]步骤s104中，补偿s103中点云帧的运动畸变，计算点云帧相对于场景点云地图的位姿，将点云帧融合入场景点云地图。点云帧中每个点的采集时刻相差较大，尽管激光雷达测距一次耗时很短，但是需要扫描机械操纵激光束指向不同角度，将结果累积起来才能得到一帧点云，例如vlp-32c完成一圈扫描需要100ms。汽车行驶时激光雷达位姿不断变化，不同时刻采集的点参考的坐标系不一致，直观上看点云出现变形和撕裂，这就是激光雷达点云的运动畸变。运动畸变导致点云无法直接对应场景的三维结构，车辆快速直行或转向时，运动畸变可达米级，因此必须对运动畸变进行补偿，才能构建较精确的环境点云地图。[0076]补偿运动畸变的基本原理是，计算采集点云帧过程中激光雷达的位姿，对每个点施加当时的激光雷达坐标系至参考坐标系的变换，变换后所有点统一至参考坐标系下。汽车行驶中位姿变化是宽带的，使用imu数据估算宽带运动精度较高且计算开销较小。[0077]约定第一帧点云结尾时刻imu所在位置为原点，建立东北天坐标系(enu)。imu测得自身(sensorframe)的角速度和加速度记为第k帧点云结尾时刻imu相对enu系的姿态、速度、位置记为第k帧点云结尾时刻imu角速度和加速度的偏置记为点p为第k+1帧点云内任意一点，其时间戳为k+p。[0078]按下面的公式推算k+p时刻imu相对enu系的姿态、加速度、速度、位移。其中s(ω)为ω的反对称矩阵，当地g值可用纬度和海拔精确计算。[0079][0080]实际工程中，imu坐标系和激光雷达坐标系不会恰好重合，使用描述激光雷达坐标系至imu坐标系的变换。则点p对应的激光雷达位姿激光雷达坐标系至imu坐标系的变换。则点p对应的激光雷达位姿[0081]本例将点p从k+p时刻的激光雷达坐标系，变换至k+1帧结尾的激光雷达坐标系下，得到补偿运动畸变后的点[0082]接下来使用加权的点对面icp算法将k+1帧点云和场景点云地图配准，该算法对第k+1帧点云相对场景点云地图的位姿进行迭代优化，使匹配代价c最小化，算法的详细流程如图3所示。[0083][0084]上式中p为第k+1帧点云中的点，使用将p变换至场景点云地图坐标系下，变换后的点记为q为场景点云地图中距离p′最近的点，nq为q对应的法向量，kq为q对应的权重，dot为向量点乘。[0085]解析求解十分困难，因为p和q的配对关系(correspondence)是关于的不连续函数，icp使用迭代法求其近似解，具体步骤如下：[0086]使用当前的对p进行变换，搜索配对的q。假设p和q的配对关系不变，计算使匹配代价最小。循环迭代，直至数值上收敛或超出最大迭代次数。[0087]imu运动推算短时的精度很高，但误差会随着时间快速累积，需结合其他传感器进行纠正。点云配准得到的位姿漂移较小，但在环境特征不足或干扰物体较多时可能匹配错误。gnss可直接测定绝对位置，消除建图的累积误差，但随机噪声较大。本例中使用卡尔曼滤波器对imu推算结果、点云配准位姿、gnss位置进行融合，得到较为精确的k+1帧结尾时刻的状态量[0088]开始建图前需要初始化上述状态量，不正确的初值会导致地图起始部分精度降低，甚至无法建图。其中可按最近一次imu内参标定结果设置，可通过基于imu的航姿算法估计，可通过配准相邻的点云帧估计，在本例的坐标约定下为[0089]最后按照卡尔曼滤波器估计的对k+1帧点云进行坐标变换，将变换后的点云帧合入场景点云地图。实际道路环境中，汽车可能会因为等待绿灯、避让行人而暂时停车，此时扫描到的点云帧几乎是重复的。重复的点云无法提供新的场景信息，只会降低场景点云地图的均匀性。本例使用当前点云帧至场景点云地图中最后一帧的位移，来评价当前点云帧的重复性，若位移《0.1m则不合入场景点云地图。[0090]步骤s105中，搜索s104所述场景点云地图中每个点的k近邻，计算每个点的法向量和局部平整度。点云可以看作真实场景的采样，求点云法向量的实质是还原场景中的平面信息(也称为面元)，在诸多点云处理算法中起着重要作用。求某点法向量的方法是使用其k近邻进行局部平面拟合，空间中平面方程和拟合残差可写为：[0091][0092][0093]上述平面拟合问题存在高效的解析解法。计算其k近邻坐标的均值，将k近邻坐标减去均值后计算协方差矩阵，对该矩阵进行特征值分解，最小特征值对应的特征向量即为法向量，最小特征值即为拟合残差。[0094]拟合所用的点数k常取8至16，点数过少时易受噪声干扰出现拟合错误，点数过多则会抹平细小的三维结构，也增大了计算负担。为了加快k近邻搜索，通常使用空间数据结构对点云进行组织，例如kd-tree、八叉树、体素哈希表。[0095]实际点云地图中，靠近激光雷达且与激光束大致垂直的表面上点非常密集，远离激光雷达且与激光束大致平行的表面上点非常稀疏，固定k值无法适应点密度的大幅变化，而自适应算法复杂度较高。记场景点云地图为m，本方法对m进行体素平均下采样(gridaveragedownsample)得mds，mds包含非空体素内的点数和平均坐标。在mds中查找m的k近邻，k近邻坐标按点数加权后计算均值和协方差矩阵。即用下采样限制点云的最高密度后，再进行固定k近邻平面拟合。[0096]受点云密度和精度的限制，结构复杂的物体上，例如枝叶和草丛，计算的法向量错误较多。本方法使用平面拟合残差评价对应法向量的质量，若最小特征值小于阈值，局部平整度为1，否则局部平整度为阈值除以最小特征值。建图和标定时点的权重正比于局部平整度,降低不可靠法向量的影响。[0097]步骤s106中，对s104所述场景点云地图随机下采样，产生两个子点云地图，计算它们的平面一致性代价。车辆行驶时，场景中同一平面会被多条激光束从多个位置和角度重复观测，使用多帧点云构建地图后，内参误差导致的几何不一致会显现出来，本方法通过优化平面一致性代价来推算激光雷达内参。[0098]该步骤通常每隔50至200帧执行一次，因为只有当地图几何结构出现明显变化时，才能提供新的标定信息。如果地图中没有积累足够的新帧，则退回s103继续处理下一帧。随机下采样的方法是，遍历场景点云地图中的点，按预设概率执行{放入子点云a，放入子点云b，直接跳过}中的一种操作。平面一致性代价c＝σ|dot(pa-pb,nbkb)|，上式中pa为点云a中的点，pb为点云b中pa的最近邻点，nb为pb的法向量，kb为pb对应的权重，dot为向量点乘，竖线为绝对值。[0099]步骤s107中，求s106中平面一致性代价对s101中内参的偏导，使用梯度下降法更新s101中内参数值。偏导矩阵j＝d(c+r)/dp，上式中c为平面一致性代价，p为内参，r为正则项。内参数值更新方式为p＝p-αj，其中α为步长。当场景中平面不够丰富时，提高正则系数有助于抑制过拟合导致的标定误差。若场景包含大量不同位置和角度的平面，可以降低正则系数。[0100]步骤s108中，删去s104所述场景点云地图中距离当前激光雷达位置过远的点，衰减s107中更新内参数值的步长。远离激光雷达的点再次回到视野内的概率较低，若点的距离超出激光雷达量程的1.1倍，将其删去以节约内存和计算资源。标定开始阶段，使用较大的步长加快收敛速度，之后逐渐衰减步长，抑制随机噪声的干扰，得到较为精确的内参数值。本例使用的衰减曲线为对数曲线。[0101]步骤s109中，若s102中采集的数据已用完，输出s101中内参作为最终标定结果。否则退回s103继续处理下一帧。若单次采集的数据不够丰富，或希望进一步提高标定精度，可以在多个路段采集多个数据集，并多次运行本方法，将前一次标定的结果作为后一次标定的初值，并适当降低参数更新的步长。[0102]以上仅为本发明具体实施方式，不能以此来限定本发明的范围，本
技术领域：
：内的一般技术人员根据本创作所作的均等变化，以及本领域内技术人员熟知的改变，都应仍属本发明涵盖的范围。当前第1页12当前第1页12
技术特征：
1.一种移动载体上激光雷达同时建图和内参标定方法，其特征在于，包括如下步骤：s101：根据待标定激光雷达的机械和光学结构，设计修正测距偏差和测角偏差的内参模型；s102：将待标定激光雷达和惯性测量单元刚性固定在载体上，采集载体在道路上行驶过程中激光雷达的原始测量值和惯性测量单元的运动数据；s103：读取一帧激光雷达的原始测量值，读取s101中内参模型的参数，补偿测量偏差，生成一帧点云；s104：补偿s103中点云帧的运动畸变，计算点云帧相对于场景点云地图的位姿，将点云帧融合入场景点云地图；s105：搜索s104所述场景点云地图中每个点的k近邻，计算每个点的法向量和局部平整度；s106：每间隔预设的帧数，对s104所述场景点云地图随机下采样，产生两个子点云地图，计算它们的平面一致性代价，并执行下一步；如果没有积累足够的新帧，则退回s103继续处理下一帧；s107：求s106中平面一致性代价对内参模型中内参的偏导，使用梯度下降法更新内参模型中的内参数值；s108：删去s104所述场景点云地图中距离当前激光雷达位置超出预设距离的点，衰减s107中更新内参数值的步长；s109：退回s103继续处理下一帧，直至采集的所有数据用完，输出所述内参模型的参数作为最终标定结果。2.根据权利要求1所述的移动载体上激光雷达同时建图和内参标定方法，其特征在于，步骤s101中，待标定激光雷达为n线的机械扫描激光雷达，其内参模型如下：标定前第i束激光的测距值记为第i束激光的俯仰角记为第i束激光相对于转子的方向角记为转子编码器输出的方向角记为使用查找表补偿测距偏差；所述查找表记为dtable，保存了n束激光在k种距离下的测距偏差值；查表操作记为通过插值生成任意下的测距偏差值；标定后的测距值使用补偿第i束激光的俯仰角，标定后的俯仰角使用补偿第i束激光相对转子的方向角，标定后相对转子的方向角使用二次谐波模型补偿转子编码器的测角误差，标定后转子的方向角用二次谐波模型补偿转子编码器的测角误差，标定后转子的方向角若用户没有给定上述内参模型中参数的初始值，将所有参数初始化为0。3.根据权利要求1所述的移动载体上激光雷达同时建图和内参标定方法，其特征在于，步骤s102中，载体为使用轮子或履带或机械足驱动自身行进的车辆或机器人；激光雷达原始测量值包括激光测距值、激光出射角和时间戳；惯性测量单元的运动数据至少包括三轴加速度和三轴角速度。4.根据权利要求2所述的移动载体上激光雷达同时建图和内参标定方法，其特征在于，
步骤s103中，使用s101中建立的内参模型和保存的内参数值，补偿激光雷达原始测量值的偏差；将补偿后的测量值[d
i
,θ
i
,φ
i
,φ
r
]转换为空间直角坐标系中点云的公式如下：约定激光雷达中心为原点，前方为x轴，左方为y轴，上方为z轴，上式中horizoffset和vertoffset代表激光出射位置在水平和高度方向上的偏移。5.根据权利要求1所述的移动载体上激光雷达同时建图和内参标定方法，其特征在于，步骤s104中，补偿点云帧运动畸变的方法是：使用惯性测量单元的运动数据推算采集此帧过程中激光雷达的位姿，按点云时间戳对上述位姿插值，得到采集每个点时的激光雷达的位姿；计算上述位姿相对于参考位姿的变换，对每个点的坐标施加上述变换；其中，参考位姿常选择点云帧起始或中间或结尾时刻激光雷达的位姿；计算点云帧相对于场景点云地图的位姿的方法是：使用加权的点对面icp算法将点云帧和场景点云地图配准得到激光雷达的位姿，把点云配准得到的激光雷达的位姿和惯性测量单元推算得到的激光雷达的位姿输入卡尔曼滤波器，得到滤波后的激光雷达的位姿并更新惯性测量单元漂移参数。6.根据权利要求1所述的移动载体上激光雷达同时建图和内参标定方法，其特征在于，步骤s105中，使用kd-tree或八叉树或体素哈希表对点云进行组织，在此基础上查找某点的k近邻；计算某点法向量的方法是:计算其k近邻坐标的均值，将k近邻坐标减去均值后计算协方差矩阵，对该矩阵进行特征值分解，该点的法向量为最小特征值对应的特征向量；若最小特征值小于阈值，则局部平整度定为1，否则局部平整度为阈值除以最小特征值。7.根据权利要求1所述的移动载体上激光雷达同时建图和内参标定方法，其特征在于，步骤s106中，帧数间隔取值为50至200；随机下采样的方法是，遍历场景点云地图中的点，按预设概率执行{放入子点云a，放入子点云b，直接跳过}中的一种操作；平面一致性代价c＝σ|dot(p
a-p
b
,n
b
k
b
)|，上式中p
a
为点云a中的点，p
b
为点云b中p
a
的最近邻点，n
b
为p
b
的法向量，k
b
为p
b
对应的权重，dot为向量点乘，竖线代表绝对值。8.根据权利要求1所述的移动载体上激光雷达同时建图和内参标定方法，其特征在于，步骤s107中，偏导矩阵j＝d(c+r)/dp，上式中c为平面一致性代价，p为内参，r为正则项；内参数值更新方式为p＝p-αj，其中α为步长；根据标定场景质量调节正则化强度和内参更新步长。

技术总结
本发明公开了一种移动载体上激光雷达同时建图和内参标定方法。本发明根据激光雷达的工作原理和物理结构建立了修正测距偏差和测角偏差的内参模型，在载体行驶时连续的采集环境点云数据和运动数据，迭代的构建地图片段和更新内参数值，最终得到较为精确的激光雷达内参和场景地图。本发明使用运动补偿、面元估计、传感器融合等技术降低建图误差，通过优化地图片段中面元在多帧观测下的一致性来更新内参数值。数值。数值。


技术研发人员：刘希尧 杜歆
受保护的技术使用者：浙江大学
技术研发日：2022.12.13
技术公布日：2023/7/13