一种用于激光-IMU外参标定的方法及系统与流程

本公开涉及机器人相关技术领域，具体的说，是涉及一种用于激光-imu外参标定的方法及系统。

背景技术：

本部分的陈述仅仅是提供了与本公开相关的背景技术信息，并不必然构成在先技术。

近年来，机器人行业已经提出了各种同时定位和建图的多传感器融合方法，这些技术都依赖于传感设备间精确地外部校准和同步。机器人在缺少地图信息和无gps环境中，例如室内，森林，以及矿井等。机器人的slam(同时定位和建图)技术承担了重要的作用。在同时定位和建图过程中，机器人位姿的求解的实时性和准确性直接决定了slam建图的效果。

惯性测量单元(简称为imu)、激光雷达和视觉相机都是常用的测算机器人位姿的设备。发明人发现，imu数据可以提高slam位姿求解的速度，但是imu的测量存在极大的误差难以消除；激光雷达通常会在每一帧扫描中收集一系列3d点，如果不对外参进行标定，在进行激光里程计定位或者slam建图过程中去表达运动方式时会发生测量畸变导致运动失真。激光雷达可以提高位姿求解的精度，但是计算过程过于耗费资源，实时性差。

技术实现要素：

本公开为了解决上述问题，提出了一种用于激光-imu外参标定的方法及系统，实现激光雷达(lidar)与imu紧耦合，对激光-imu外参进行准确标定，解决激光-imu外参标定中机械外参不易获得，人工测量误差较大，且测量比较麻烦的问题，在一定程度上相互弥补激光雷达(lidar)与imu的各自的缺点，可以提高slam方法的位姿求解精度和速度，本公开的外参标定方法可应用于激光雷达-imu两种传感器的组合系统。

为了实现上述目的，本公开采用如下技术方案：

一个或多个实施例提供了一种用于激光-imu外参标定的方法，包括如下步骤：

获取imu测量数据和激光雷达的测量数据；

对获得的imu测量数据进行imu预积分，计算获得imu下一时刻相对imu初始位姿的imu位姿变换估算值，根据估算值和下一时刻imu的实际测量值，获得数据关联的关联残差；

对激光雷达的测量数据进行处理，利用imu预积分将获得多个激光雷达点重投影到世界坐标系的投影坐标，计算每个激光雷达点到校准目标地图的重投影误差；

根据关联残差和计算获得的每个激光雷达点到校准目标地图的重投影误差，采用非线性最小二乘方法，迭代优化激光雷达-imu外参标定，获得外参标定结果。

一种用于激光-imu外参标定的系统，包括：

数据获取模块：用于获取imu测量数据和激光雷达的测量数据；

第一数据处理模块：用于对获得的imu测量数据进行imu预积分，计算获得imu下一时刻相对imu初始位姿的imu位姿变换估算值，根据估算值和下一时刻imu的实际测量值，获得数据关联的关联残差；

第二数据处理模块：用于对激光雷达的测量数据进行处理，利用imu预积分将获得多个激光雷达点重投影到世界坐标系的投影坐标，计算每个激光雷达点到校准目标地图的重投影误差；

外参标定结果计算模块：用于根据关联残差和计算获得的每个激光雷达点到校准目标地图的重投影误差，采用非线性最小二乘方法，迭代优化激光雷达-imu外参标定，获得外参标定结果。

一种电子设备，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成上述方法所述的步骤。

一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时，完成上述方法所述的步骤。

与现有技术相比，本公开的有益效果为：

本公开通过构建贝叶斯网，将m+1帧激光数据到来时imu的位姿数据与m帧时imu的位姿信息预测得到的m+1帧imu位姿信息相减，得到imu在m帧激光数据时刻的位姿残差，作为参数标定的约束条件，可以提高外参标定的准确性。

本公开通过利用激光坐标系和imu坐标系之间的重投影，确定激光雷达点的残差作为外参标定的约束条件。并利用每帧之间的imu预积分求得的相对位姿变换，避免了激光雷达激光点的累计误差。

本公开通过构建激光因子和imu因子之间的数据关系，利用非线性最小二乘法对多帧数据进行联合优化，避免了因为噪声和imu偏置等问题导致的数据模糊导致的方程无法求解等问题。通过数据关联和迭代多帧数据，将意外因素影响降低，使求解过程更具有鲁棒性，更接近真实值。

本公开利用imu预积分的思想解决激光雷达扫描中的运动失真，并且考虑了imu预积分在进行优化之前存在偏差bf和时间漂移δt，通过一阶展开法加入偏差修正，来提高外参标定优化的效果。

本公开的方法不用预先使用机械外参进行计算，避免了机械测量的繁琐复杂等问题。本公开的方法通过算法对外参进行计算优化，提高了外参测量的精度并免去了测量误差。

附图说明

构成本公开的一部分的说明书附图用来提供对本公开的进一步理解，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的限定。

图1是本公开实施例1的方法流程图；

图2是本公开实施例1的激光位姿变换表示及重投影误差示意图；

图3是本公开实施例1校准目标图像示例；

图4是本公开实施例1的激光雷达因子和imu因子之间的表示。

具体实施方式：

下面结合附图与实施例对本公开作进一步说明。

应该指出，以下详细说明都是示例性的，旨在对本公开提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本公开所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本公开的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。需要说明的是，在不冲突的情况下，本公开中的各个实施例及实施例中的特征可以相互组合。下面将结合附图对实施例进行详细描述。

技术术语解释：

imu：(英文：inertialmeasurementunit，简称imu)是测量物体三轴姿态角(或角速率)以及加速度的装置。

在一个或多个实施方式中公开的技术方案中，如图1所示，一种用于激光-imu外参标定的方法，包括如下步骤：

步骤1、获取imu测量数据和激光雷达的测量数据；

步骤2、对获得的imu测量数据进行imu预积分，计算获得imu下一时刻相对imu初始位姿的imu位姿变换估算值，根据估算值和下一时刻imu的实际测量值，获得数据关联的关联残差；

步骤3、对激光雷达的测量数据进行处理，利用imu预积分将获得多个激光雷达点重投影到世界坐标系的投影坐标，计算每个激光雷达点到校准目标地图的重投影误差；

步骤4、根据关联残差和计算获得的每个激光雷达点到校准目标地图的重投影误差，采用非线性最小二乘方法，迭代优化激光雷达-imu外参标定，获得外参标定结果。

外参标定结果包括imu到激光雷达的姿态差异rc和位置差异pc，imu的偏差(bias)，激光雷达每帧扫描中imu的旋转角度位置和速度以及两个传感器之间的时间漂移δt。本实施例的公式中，下标w表示固定的世界坐标系，上标m表示激光雷达的第m帧扫描。

imu测量数据可以包括角速率和线加速度，所述imu的移动轨迹包括移动速度和位姿，所述位姿包括imu的姿态和位置。

步骤2为imu数据的处理步骤，可以具体如下：

imu测量数据主要是imu的线加速度和角速度，单纯的imu数据无法应用于异构传感器标定的过程，需要将imu传感器数据通过imu预积分的方法进行解算，在imu坐标系下计算出imu轨迹，获得imu坐标点。

imu每个时刻都可实现自身主体加速度aq(q＝1，…，n)和角速度wq(q＝1，…，n)的测量，测量模型可以如下：

其中，是世界坐标系到imu坐标系下imu陀螺仪(gyroscope)输出的角速率实际测量值，bωwb(t)是全局坐标系下imu陀螺仪的真实值，是imu加速度计(accelerometer)输出的速度计实际测量值，wa(t)是世界坐标系下加速度计真实值，wg是世界坐标系下重力加速度，是世界坐标系到imu坐标系下的姿态，b^g(t)、b^a(t)是imu中陀螺仪和加速度计的偏差(bias)，η^g(t)和η^a(t)是高斯噪声。下标b表示imu坐标系，w表示世界坐标系。

运动学方程如下：

其中，rwb表示imu在imu坐标系到世界坐标系下的姿态，wv表示imu在世界坐标系下的速度，wp表示imu在世界坐标系下的位置。

假设激光扫描的时间间隔为δt，在区间[t，t+δt]，在t+δt位姿变换可以通过如下公式计算：

rwb(t+δt)＝rwb(t)exp(bωwb(t)δt)(6)

wv(t+δt)＝wv(t)+wa(t)δt(7)

其中，rwb(t)是t时刻imu坐标系到世界坐标系下的姿态，rwb(t+δt)是t+δt时刻的姿态更新；wv(t+δt)是t+δt时刻imu在世界坐标系下的速度；wp(t+δt)是t+δt时刻imu在世界坐标系下的位置。

根据公式(1)-(8)可得：

其中，r(t+δt)、v(t+δt)、p(t+δt)为考虑过重力加速度、imu加速度计和陀螺仪的偏差，以及高斯噪声后的姿态、速度和位置。t为当前时刻,t+δt时刻为下一时刻，r(t+δt)为t+δt时刻imu姿态，v(t+δt)为t+δt时刻imu的速度，p(t+δt)为t+δt时刻imu姿态的位置，r(t)为t时刻imu姿态,v(t)为t时刻imu的速度，p(t)为t时刻imu姿态的位置；b^g(t)、b^a(t)是imu中陀螺仪和加速度计的偏差，g为重力加速度，为imu角速度实际测量值η^gd(t)为imu角速度噪声为imu加速度实际测量值η^ad(t)为imu加速度噪声。

通过公式(9)-(11)由上个时刻t状态可以推算出下个时刻t+△t的状态是下一时刻的imu位姿变换估算值，所述姿态包括旋转角度r，速度v和位置p。可以获得imu初始位姿坐标系下，在δt时间内imu的移动轨迹为t+△t测量时刻相对imu初始位姿的imu位姿变换。

根据计算获得的imu位姿变换估算值以及下一时刻t+△t的实际imu测量值，计算数据关联的关联残差，作为优化的约束条件，表示如下：

其中，为imu在m帧激光扫描时imu的残差向量，包含姿态残差，速度残差，位置残差，是第m帧激光雷达数据到来时imu在世界坐标系下的位置，是第m+1帧激光雷达数据到来时imu在世界坐标系下的位置，是第m帧激光雷达数据到来时imu在世界坐标系下的姿态，δtm是第m帧激光雷达数据与上一扫描时刻的时间间隔，是第m帧激光雷达数据到来时imu在世界坐标系下的速度，g为重力加速度，是在世界坐标系下m帧激光和m+1帧激光之间的位置差值，是第m+1帧激光雷达数据到来时imu在世界坐标系下的速度，是在世界坐标系下m帧激光和m+1帧激光之间的速度差值，是在世界坐标系下m帧激光和m+1帧激光之间的旋转差值。

步骤2通过构建贝叶斯网，将m+1帧激光数据到来时imu的位姿数据与m帧时imu的位姿信息预测得到的m+1帧imu位姿信息相减，得到imu在m帧激光数据时刻的位姿残差，作为参数标定的约束条件，可以提高外参标定的准确性。

步骤3为激光雷达数据的处理步骤，可以具体如下：

步骤31、将第i帧激光雷达坐标系fⁱl中的点xi，投影到imu进行第i帧数据采集以imu原点为中心的坐标系fⁱi中，获得坐标系fⁱi中的坐标点；

步骤32、采用imu预积分将投影到fⁱi中的坐标点，投影到第一个激光坐标系f⁰l即为世界坐标系中，获得f⁰l坐标系下的坐标。

激光雷达扫描的每一个点，记录的数据都是根据激光发射和接收的时间差、激光强度差、激光发射的偏航角、俯仰角计算而得，激光雷达的坐标系是此时刻以激光雷达为原点的坐标系，但激光雷达在整个扫描过程中是运动的，可以以运动出发点为世界坐标系f⁰l。

用点到平面的距离定义激光雷达点到地图的重投影误差，用于表示激光雷达因子，如图2所示，图中r⁰w是第0帧激光雷达数据到来时imu在世界坐标系下的姿态,是第m帧激光雷达数据到来时imu在世界坐标系下的姿态,是第i帧激光雷达数据到来时imu在世界坐标系下的姿态,p⁰w是第0帧激光雷达数据到来时imu在世界坐标系下的位置,是第m帧激光雷达数据到来时imu在世界坐标系下的位置,pⁱw是第i帧激光雷达数据到来时imu在世界坐标系下的位置,f⁰i是第0帧激光雷达数据到来时imu坐标系,f^mi是第m帧激光雷达数据到来时imu坐标系,fⁱi是第i帧激光雷达数据到来时imu坐标系，f⁰l是第0帧激光雷达数据到来时激光坐标系,f^ml是第m帧激光雷达数据到来时激光坐标系,fⁱl是第i帧激光雷达数据到来时激光坐标系。

点xi表示第i帧激光雷达坐标系fⁱl中的点，利用集合s和imu预积分测量投影到第一个激光坐标系f⁰l中，可以如下：

首先，xi投影到imu坐标系fⁱi中，如下：

其中，xⁱi是xi投影到imu坐标系fⁱi中的坐标，rc是激光坐标系和imu坐标系之间的姿态差异，初值为单位矩阵；pc是激光坐标系和imu坐标系之间的位置差异，初值为0。

将(imu坐标系下的点)投影到世界坐标系f⁰l：

当tm≤ti≤tm+1，得到

其中，xⁱl0表示点xi在坐标系下的坐标。

xⁱl0点到平面的距离di即为第i帧激光雷达点到地图的重投影误差，可以作为imu测量数据和激光雷达的测量数据的关联参数，可以表示为:

其中，i＝1,2，...，n。

步骤3中，利用激光坐标系和imu坐标系之间的重投影，确定激光雷达点的残差作为外参标定的约束条件。并利用每帧之间的imu预积分求得的相对位姿变换，避免了激光雷达激光点的累计误差。

进一步的，imu预积分是预先处理的，但在进行优化之前存在偏差bf和时间漂移δt，可以通过一阶展开法加入偏差修正，来提高优化的效果。偏差和时间漂移如下：

既有

其中，bf为imu加速度计测量的偏置，为加速度计偏置的真实值，为偏置噪声；bω为imu陀螺仪的测量偏置，为陀螺仪偏置的真实值，为偏置噪声；δt为时间漂移的测量值，为时间漂移的真实值，为时间漂移噪声。

利用雅克比矩阵的推导可以求出待估计状态bf、bω和δt，并加入到步骤4的优化中进行求解。

步骤4中，根据关联残差和计算获得的每个激光雷达点到校准目标地图的重投影误差，采用非线性最小二乘方法，迭代优化激光雷达-imu外参标定，获得外参标定结果的方法，包括平面分割获得激光雷达和imu之间的位姿估计值和非线性优化的步骤：

数据关联的方法包括如下步骤：

步骤41、在第一帧激光扫描时通过ransac的平面拟合算法构成校准目标的目标平面p；

步骤42、采用近邻搜索将每次激光扫描的激光点云数据关联至目标平面p，使得激光雷达和imu之间的姿态差异和位置差异即公式(26)最小，获得激光雷达和imu之间的位姿估计值

假设传感器系统在第一次扫描期间是静态的。点-面数据关联是一个迭代过程，处理在两次连续扫描中检测到的平面的法向量。对于扫描之间的有限移动，通过一个简单的近邻搜索将相应的平面链接在一起。可以使用房间角落或建筑物作为校准目标，如图3所示。利用一组三个近正交平面的地图几何，在寻找最近邻点之前，我们通过减去法向量的质心来加强跟踪过程。

假设静止扫描，每个扫描具有点到平面关联允许粗略地估计激光雷达姿态。

该迭代过程将每个激光雷达点与图3所示的一个地图平面相关联，该实施例关联过程中将向量ni和ωi作为初始平面关联到xi的参数，ni是到平面p的单位法向量，ωi是到平面p的距离。假设每次扫描都有点到平面的关联，使得公式(26)最小的目标公式为：

其中，{k,l∈n|tk＝τm，tl+1＝τm+1}，通过公式26得到每一帧激光雷达在第一帧激光坐标系中的表示，可以粗略估计出激光雷达和imu之间的姿态差异和位置差异

非线性优化的步骤如下：

4-1、根据关联残差和计算获得的每个激光雷达点到校准目标地图的重投影误差，以及激光雷达和imu之间的位姿估计值构建激光雷达和imu之间的关联数据集合s；

4-2、采用非线性最小二乘的方法关联数据集合s，以关联残差和重投影误差的和最小为目标，迭代优化激光-imu之间外部参数，即迭代优化更新激光雷达和imu之间的位姿估计值包括姿差异估计值和位置差异估计值得到最终的外部参数标定值，即为最终激光雷达和imu之间的姿态差异rc和位置差异pc，以及对应的激光雷达对imu的偏差(bias)和时间漂移δt。

利用因子图将得到的激光雷达和imu之间的外参利用最大似然估计进行优化，如图4所示，s表示激光雷达和imu之间的关联数据，其中参数均由前项求出。非线性最小二乘的方法的优化目标函数为：

z表示之前的有效测量，f表示损失函数。在高斯噪声的假设下，将表示imu的测量残差，di表示激光雷达点到平面距离的残差。

损失函数f可以为：

通过非线性最小二乘计算使得(27)最小，得出我们期望的rc和pc两个参数，以及激光雷达对imu的偏差(bias)和时间漂移δt进行标定，实现激光雷达和imu之间的外参标定。

步骤4通过构建激光因子和imu因子之间的数据关系，利用非线性最小二乘法对多帧数据进行联合优化，避免了因为噪声和imu偏置等问题导致的数据模糊导致的方程无法求解等问题。通过数据关联和迭代多帧数据，将意外因素影响降低，使求解过程更具有鲁棒性，更接近真实值。

实施例2

本实施例提供一种用于激光-imu外参标定的系统，包括：

数据获取模块：用于获取imu测量数据和激光雷达的测量数据；

第一数据处理模块：用于对获得的imu测量数据进行imu预积分，计算获得imu下一时刻相对imu初始位姿的imu位姿变换估算值，根据估算值和下一时刻imu的实际测量值，获得数据关联的关联残差；

第二数据处理模块：用于对激光雷达的测量数据进行处理，利用imu预积分将获得多个激光雷达点重投影到世界坐标系的投影坐标，计算每个激光雷达点到校准目标地图的重投影误差；

外参标定结果计算模块：用于根据关联残差和计算获得的每个激光雷达点到校准目标地图的重投影误差，采用非线性最小二乘方法，迭代优化激光雷达-imu外参标定，获得外参标定结果。

实施例3

本实施例提供一种电子设备，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成实施例1的方法所述的步骤。

实施例4

本实施例提供一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时，完成实施例1的方法所述的步骤。

以上所述仅为本公开的优选实施例而已，并不用于限制本公开，对于本领域的技术人员来说，本公开可以有各种更改和变化。凡在本公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。

上述虽然结合附图对本公开的具体实施方式进行了描述，但并非对本公开保护范围的限制，所属领域技术人员应该明白，在本公开的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本公开的保护范围以内。