激光雷达和IMU系统的时空联合标定方法技术方案

本申请涉及激光雷达和IMU系统的时空联合标定方法，包括如下步骤：利用B样条基函数对IMU运动姿态的连续时间下轨迹进行建模，得到IMU相对于世界坐标系的运动轨迹；利用正态分布变换点云匹配算法估计激光雷达的运动，得到激光雷达相对于初始位置的运动轨迹；以局部轨迹之间的Hausdorff距离为代价函数，结合手眼标定方法求解激光雷达的坐标系与IMU的坐标系之间时空关系的初始值；利用激光雷达与IMU的相对位姿减小激光雷达的点云数据畸变；根据IMU预积分和激光雷达点云的点、线、面特征，构建相应的非线性优化目标函数，结合校正后的激光雷达的点云数据和时空关系的初始值，在非线性图优化框架下对目标函数进行优化，并完成激光雷达和IMU的相对位姿标定。光雷达和IMU的相对位姿标定。光雷达和IMU的相对位姿标定。

【技术实现步骤摘要】
激光雷达和IMU系统的时空联合标定方法


[0001]本申请属于机器人
，涉及一种多传感器配置的导航系统时空标定方法。

技术介绍

[0002]随着社会的发展，机器人在各种环境下应用的越来越普及。传感器在机器人的各种感知中发挥着中十分重要的作用，就像是人的器官一样。多传感器融合是解决复杂环境下机器人自主环境感知与导航的重要方式，其可以充分利用不同传感器的优点，相互补偿，实现相较于单一传感器更为精准与鲁棒的感知与定位性能，因此近年来受到了广泛的关注，如在无人驾驶汽车、野外机器人、高精度地图构建、目标跟踪等。
[0003]现有的多传感器融合技术主要分为基于外部设备的标定方法和自标定的方法。基于外部设备的标定方法主要是利用外部传感器、转台、标定板，通过这些外部设备提供的先验信息去估计传感器的空间变换参数，该方法的精度更多取决于外部校准设备，同时该方法对外部设备的精度要求很高，且校准麻烦，受到时间地点限制，无法大规模的使用，限制的应用范围。
[0004]另一种自校准方法由于不需要外部设备，所以也是目前最先进和应用最广泛的一种方法。该方法利用传感器采集的数据生成运动轨迹进行优化，但是也存在传感器数据的不同步等需要优化的问题，时间偏移也会影响到校准的精度。同时由于机器人自主运动所面临的环境是非常复杂多变的，而大多传感器会因为时间的积累产生偏移现象，所以一个多传感器时空自主在线标定方法是迫切需要的。

技术实现思路

[0005]本申请的目的是提供激光雷达和IMU系统的时空联合标定方法，能够提高激光雷达和IMU(Inertial Measurement Unit，即惯性测量单元)系统的标定精度和效率。
[0006]本申请的一种激光雷达和IMU系统的时空联合标定方法，包括如下步骤：
[0007]S1、利用B样条基函数对IMU运动姿态的连续时间下轨迹进行建模，得到IMU相对于世界坐标系的运动轨迹；
[0008]S2、利用正态分布变换点云匹配算法估计激光雷达的运动，得到激光雷达相对于初始位置的运动轨迹；
[0009]S3、以局部轨迹之间的Hausdorff距离为代价函数，结合手眼标定方法求解激光雷达的坐标系与IMU的坐标系之间时空关系的初始值；
[0010]S4、利用激光雷达与IMU的相对位姿减小激光雷达的点云数据畸变；
[0011]S5、根据IMU预积分和激光雷达点云的点、线、面特征，构建相应的非线性优化目标函数，结合校正后的激光雷达的点云数据和时空关系的初始值，在非线性图优化框架下对目标函数进行优化，并完成激光雷达和IMU的相对位姿标定。
[0012]本技术方案的方法能够实现激光雷达和IMU系统中高精度标定的要求，以满足机器人系统、无人驾驶汽车系统等定位和测绘高精度的要求。
[0013]应当理解的是，以上的一般描述和后文的细节描述仅是示例性的，本专利技术并不受限制。
附图说明
[0014]此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本专利技术的实施例，并与说明书一起用于解释本专利技术的原理。
[0015]图1是本申请一实施例中对轨迹进行分段的原理示意图。
具体实施方式
[0016]这里将详细地对示例性实施例进行说明，以下示例性实施例中所描述的实施方式并不代表与本专利技术相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所记载的、本专利技术的一些方面相一致的系统和方法的例子。
[0017]本申请实施例提供一种激光雷达和IMU系统的时空联合标定方法，包括如下步骤：
[0018]步骤S1、利用B样条基函数对IMU运动姿态的连续时间下轨迹进行建模，得到IMU相对于世界坐标系的运动轨迹，其具体过程如下：
[0019]当IMU的运动轨迹的控制点均匀分布时，采用以下矩阵形式的B
‑
spline基函数进行建模，其公式为：
[0020][0021]其中，d为B
‑
spline基函数的阶数，即t∈[t
i
,t
i+1
)区间内的连续运动轨迹p(t)，仅由时间t
i
,t
i+1
,
…
,t
i+d
对应的控制点p
i
,p
i+1
,
…
,p
i+d
决定，u
T
＝[1,u,
…
,u
d
]，u＝(t
‑
t
i
)/(t
i+1
‑
t
i
)，表示样条矩阵M
(d+1)
的第j列；
[0022]因此，运动轨迹的累积可表示为：
[0023][0024]对旋转群组SO(3)空间中的均匀分布单元四元数控制点q
i
进行建模:
[0025][0026]其中Exp(
·
)表示李代数到李群的映射，Log(
·
)表示李群到李代数的映射。
[0027]本文采用三次样条基函数对运动轨迹进行建模，其中样条矩阵和累积样条矩阵可表示为:
[0028][0029]步骤S2、利用正态分布变换(NDT)点云匹配算法估计激光雷达的运动，得到激光雷达相对于初始位置的运动轨迹；
[0030]具体的是采用正态分布变换(NDT)点云匹配算法进行估计，根据激光雷达的运动轨迹上各点的时间戳，得到激光雷达的相对于初始位置的相对运动轨迹
L
Γ
L
。
[0031]步骤S3、以局部轨迹之间的Hausdorff距离(豪斯多夫距离)为代价函数，结合手眼标定方法求解激光雷达的雷达坐标系与IMU的IMU坐标系之间的时空关系的初始值；
[0032]由于Hausdorff距离可以描述两个离散轨迹之间的相似性，下式表示为Hausdorff距离，解决了激光雷达的运动轨迹
I
Γ
L
(q)和MU的运动轨迹
I
Γ
I
(q)之间的相似性：
[0033]H
x
＝max(h(x
i
,xj),h(x
j
,x
i
))
[0034][0035][0036]其中H表示轨迹之间的相似度。
[0037]为了消除里程计估计的失效区域，如图1所示对轨迹进行分段，并分别计算轨迹段内的Hausdorff距离。如果轨迹之间的相似度小于设定的阈值，则可以使用估计的轨迹进行初始参数标定。
[0038]上述获得的IMU运动姿态的连续时间运动轨迹
I
Γ
I
(t)和激光雷达的相对运动轨迹
L
Γ
L
，对两个传感器轨迹上各对应的时间戳进行变换，构造非线性最小二乘问题：
[0039][0040]对本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.激光雷达和IMU系统的时空联合标定方法，其特征在于，包括如下步骤：S1、利用B样条基函数对IMU运动姿态的连续时间下轨迹进行建模，得到IMU相对于世界坐标系的运动轨迹；S2、利用正态分布变换点云匹配算法估计激光雷达的运动，得到激光雷达相对于初始位置的运动轨迹；S3、以局部轨迹之间的Hausdorff距离为代价函数，结合手眼标定方法求解激光雷达的坐标系与IMU的坐标系之间时空关系的初始值；S4、利用激光雷达与IMU的相对位姿减小激光雷达的点云数据畸变；S5、根据IMU预积分和激光雷达点云的点、线、面特征，构建相应的非线性优化目标函数，结合校正后的激光雷达的点云数据和时空关系的初始值，在非线性图优化框架下对目标函数进行优化，并完成激光雷达和IMU的相对位姿标定。2.如权利要求1所述的激光雷达和IMU系统的时空联合标定方法，其特征在于，所述步骤S1包括：S11、在IMU的运动轨迹的控制点均匀分布时，采用以下矩阵形式的B
‑
spline基函数进行建模，其公式为：其中，d为B
‑
spline基函数的阶数，即t∈[t
i
,t
i+1
)区间内的连续运动轨迹p(t)，仅由时间t
i
,t
i+1
,
…
,t
i+d
对应的控制点p
i
,p
i+1
,
…
,p
i+d
决定，u
T
＝[1,u,
…
,u
d
]，u＝(t
‑
t
i
)/(t
i+1
‑
t
i
)，表示样条矩阵M
(d+1)
的第j列；S12、运动轨迹的累积可表示为：S13、对SO(3)空间中的均匀分布单元四元数控制点q
i
进行建模:其中Exp(
·
)表示李代数到李群的映射，Log(
·
)表示李群到李代数的映射；S14、采用三次样条基函数对运动轨迹进行建模，其中样条矩阵和累积样条矩阵可表示为:3.如权利要求1所述的激光雷达和IMU系统的时空联合标定方法，其特征在于，所述步骤S2包括：
采用正态分布变换点云匹配算法进行估计，根据激光雷达的运动轨迹上各点的时间戳，得到激光雷达的相对运动轨迹
L
Γ
L
。4.如权利要求1所述的激光雷达和IMU系统的时空联合标定方法，其特征在于，所述步骤S3包括：S31、利用Hausdorff距离描述激光雷达的运动轨迹
I
Γ
L
(q)和IMU的运动轨迹
I
Γ
I
(q)之间的相似性，下式表示为Hausdorff距离：H
x
＝max(h(x
i
,x
j
),h(x
j
,x
i
))))其中H表示轨迹之间的相似度；S32、IMU运动姿态的连续时间运动轨迹
I
Γ
I
(t)和激光雷达的相对运动轨迹
L
Γ
L
，对两个轨迹上各对应的时间戳进行变换，构造非线性最小二乘问题：S33、对满足要求的所有轨迹点，用上述公式计算代价，利用Levenberg
‑
Marquardt非线性优化算法求解上述非线性最小二乘优化问题，得到时空关系的初始参数q
LtoI
和Δt；S34、得到初始参数后，利用预积分运算对IMU测量进行积分，得到激光雷达帧数据采集时刻的运动，对激光雷达数据采集的运动失真进行相应补偿，得到最终的时空关系的初始值。5.如权利要求4所述的激光雷达和I...

【专利技术属性】
技术研发人员：修贤超，杨世泽，李云辉，苗中华，周哲，石沄艳，
申请(专利权)人：上海大学，
类型：发明
国别省市：