一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定方法技术

本发明专利技术提供了一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定方法，提供了一种基于连续时间的标定3D激光雷达和惯性传感器外参的方法。该方法分为两个阶段，第一阶段为基于已知环境地图，对标定数据中的3D激光点云进行预处理，第二阶段为将高频率惯性传感器的运动轨迹用样条函数建模为连续轨迹，基于样条函数得到任意时刻惯性传感器的位姿，引入3D激光雷达和惯性传感器之间的外参，约束激光点，构建优化问题进行求解。本方法利用样条函数对惯性传感器的轨迹连续化，有效解决传感器间测量频率不同所带来的紧耦合的困难。本方法对环境要求较低，只需有一个空旷的墙角即可。本发明专利技术的方法得到的标定结果可直接应用到机器人导航、无人驾驶等任务中，为多传感器数据融合提供了基础条件。

【技术实现步骤摘要】
一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定方法
本专利技术涉及多传感器标定领域，特别涉及3D激光雷达和惯性传感器外参标定的

技术介绍
近年来无人驾驶领域发展较好，很重要的一个因素是激光雷达的普及。而激光雷达的线束越来越高，价格越来越低，测距能力越来越强，这些变化都将推动无人驾驶行业尽快落地。一般来说无人车上会装备许多互补的传感器，例如激光雷达、相机、毫米波雷达、惯性传感器等。而多传感器数据融合前提是传感器之间的内参、外参都有一个比较好的标定结果。相机间的标定、相机和惯性传感器之间的标定、相机和激光雷达之间的标定的工作有许多，但直接标定激光雷达和惯性传感器之间的外参的工作较少。标定惯性传感器与其他传感器之间的外参时，为了充分激励惯性传感器每一个轴，通常会上下左右前后剧烈运动、摇晃。而激光雷达采集数据的方式是旋转内部电机上的激光测距单元进行测距，如果在采集激光数据时发生运动，又不对输出的数据进行运动补偿，则测距数据会出错，甚至会产生严重的撕裂。若激光雷达在采集数据时本体有运动，需要对测距数据进行运动补偿，每个数据点都需要视为一次单独测量。目前常用的补偿方法是帧间线性插值，即匀速假设，而在标定场景中，传感器需要剧烈晃动，显然匀速假设无法成立，线性插值无法应用。
技术实现思路
本专利技术所要解决的技术问题是提供一种误差小的基于样条函数的激光雷达与惯性传感器外参标定方法，为此，本专利技术采用以下技术方案：一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定系统，所述标定系统先预处理3D激光数据，再将高频率惯性传感器的运动轨迹用样条函数建模为连续轨迹，基于样条函数得到任意时刻惯性传感器的位姿，引入3D激光雷达和惯性传感器之间的外参，约束激光点，构建优化问题进行求解。所述办法包括以下步骤：步骤一：将装有3D激光雷达和惯性传感器的设备，在墙角前后左右上下各个角度充分运动、旋转，激励惯性传感器的每一轴，得到标定的数据；步骤二：标定环境已事先建模为高精度点云地图，分别提取墙角3个平面点云，进行平面拟合得到平面参数；步骤三：对步骤二得到每一帧激光点云，与环境地图匹配，得到该帧点云相对于环境地图的位姿；每个点根据匹配结果转换到环境地图坐标系，计算每个点与拟合平面的垂直距离，根据一定阈值，判断该点是否属于拟合平面，每个点增加一个属性，即属于哪个平面或不属于任何平面；步骤四：根据步骤三得到的每一帧激光点云的位姿，及惯性传感器和激光雷达传感器之间的先验外参，计算对应时刻惯性传感器的位姿，和惯性传感器的测量值一起，约束样条，建立惯性传感器的先验连续轨迹；步骤五：基于样条函数得到任意时刻惯性传感器的位姿，引入3D激光雷达和惯性传感器之间的外参，对3D激光的每个测量点进行运行补偿；通过环境约束，约束3D激光测量点的位置，构建一个优化问题，同时优化样条参数和传感器之间的外参，利用高斯牛顿法进行求解。进一步地，所述标定方法标定的传感器类型是3D激光雷达和具有加速度计和角速度计的惯性传感器，3D激光雷达的测量频率为10Hz，惯性传感器的测量频率为400Hz。进一步地，所述标定方法的标定环境是室内，且有一个墙角的三个面平滑无凸起，且墙角周围基本无物品放置。进一步地，所述步骤二中的高精度点云地图的建模方法为：将一个单线激光雷达固定在步进电机上，步进电机匀速旋转，单线激光每一个测量点的位置可通过步进电机的旋转速度精确确定，旋转几圈后，得到高精度的环境点云地图；在步骤二中，利用基于RANSAC的平面提取方法，在点云地图中提取三个墙角平面点云，三个平面Pi(i＝0,1,2)分别用一个法向量ni和原点距离平面距离wi确定。进一步地，在所述步骤三中，Lxj是第k帧激光点云中的一点，第k帧激光点云与环境地图匹配得到位姿为根据点到面公式若点到面的距离小于一定阈值，则认为该点属于平面Pi，不属于任何一个平面的点，将被舍弃。进一步地，在所述步骤四中，要求所选择的样条函数可微、可导方便，且局部参数变化不影响整条轨迹，故选择B样条；根据激光雷达与惯性传感器之间的先验外参将每一帧激光位姿转换为对应时刻惯性传感器的位姿利用惯性传感器的10Hz位姿和400Hz的测量数据，约束B样条参数，得到惯性传感器的连续轨迹，即其中表示旋转量的连续B样条函数，t(t)表示平移量的连续B样条函数，具体可写作Φ(t)是样条基函数，cθ和ct是样条参数；所述惯性传感器的轨迹用6条6阶B样条表示，即5次函数表示轨迹，对时间求2次导之后，即加速度曲线为3次函数；加速度计和角速度计的偏置随时间缓慢变换，用3阶B样条曲线表示。进一步地，在所述步骤五中，根据惯性传感器的连续轨迹和传感器间的先验外参，得到激光雷达任意帧任意点的位姿，同时结合步骤三中得到的每一点的平面属性，用平面约束激光点位姿，其经过运动补偿的激光点到平面距离可建模为对应目标函数为R表示雷达传感器测量噪声的协方差矩阵。进一步地，在所述步骤五中，惯性传感器连续轨迹求导可得到加速度的连续轨迹和角速度的连续轨迹，分别为：函数S(·)是将对应的参数转换为旋转矩阵，假设加速度计和角速度计在tq时刻的测量值分别为αq和由此建立惯性传感器的测量目标函数：其中和分别表示加速度计和角速度计的测量噪声协方差矩阵，gw表示重力加速度，ba和bω表示加速度计和角速度计的偏置；由于加速度计和角速度计的偏置变换缓慢，将其建模为零均值的高斯过程噪声，其协方差分别为Qω和Qa，偏置的目标函数写作：最终的目标函数为用高斯牛顿法进行优化求解。B样条是一种表示连续时间轨迹的有效方法，其可微、可导的特性和局部性的特点，可以快速有效的将传感器轨迹建模连续时间下的轨迹。本系统基于B样条，将传感器轨迹连续化，有效解决激光雷达每个点的运动补偿问题，最终建立优化问题，优化样条参数和传感器间外参。本专利技术与现有技术相比，具有以下优点：相比于匀速假设，用线性插值补偿每个激光测量点的方法，本方法将传感器轨迹连续化，且充分利用传感器测量信息约束轨迹，能够得到任意频率的位姿信息，对每个激光点运动进行补偿；同时利用平面对激光点进行约束，引入传感器间外参，充分利用B样条参数的稀疏性，快速有效求解样条参数和传感器之间的外参。该方法标定的结果，可广泛应用于自动驾驶、机器人导航领域中的多传感器融合中。附图说明图1是本专利技术一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定系统的流程图。图2是本专利技术的实验环境图。图3是本专利技术的环境地图。图4是本专利技术3个平面的提取和激光点云数据增加平面属性可视化图。图5是本专利技术实际试验结果，包括多次标定结果的均值和方差，真值从CAD模型中测量得到。具体实施方式下面结合附图，对本专利技术的实施作进一步详细的描述，但本专利技术并不局限于具体实施本文档来自技高网...

【技术保护点】
1.一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定方法，其特征在于，所述标定方法分为两阶段：第一阶段为3D激光数据预处理，第二阶段为将高频率惯性传感器的运动轨迹用样条函数建模为连续轨迹，基于样条函数得到任意时刻惯性传感器的位姿，引入3D激光雷达和惯性传感器之间的外参，约束激光点，构建优化问题进行求解。/n

【技术特征摘要】
1.一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定方法，其特征在于，所述标定方法分为两阶段：第一阶段为3D激光数据预处理，第二阶段为将高频率惯性传感器的运动轨迹用样条函数建模为连续轨迹，基于样条函数得到任意时刻惯性传感器的位姿，引入3D激光雷达和惯性传感器之间的外参，约束激光点，构建优化问题进行求解。


2.如权利要求1所述的一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定方法，其特征在于，所述方法包括以下步骤：
步骤一：将装有3D激光雷达和惯性传感器的设备，在墙角前后左右上下各个角度充分运动、旋转，激励惯性传感器的每一轴，得到标定的数据；
步骤二：标定环境已事先建模为高精度点云地图，分别提取墙角3个平面点云，进行平面拟合得到平面参数；
步骤三：对步骤二得到每一帧激光点云，与环境地图匹配，得到该帧点云相对于环境地图的位姿；每个点根据匹配结果转换到环境地图坐标系，计算每个点与拟合平面的垂直距离，根据一定阈值，判断该点是否属于拟合平面，每个点增加一个属性，即属于哪个平面或不属于任何平面；
步骤四：根据步骤三得到的每一帧激光点云的位姿，及惯性传感器和激光雷达传感器之间的先验外参，计算对应时刻惯性传感器的位姿，和惯性传感器的测量值一起，约束样条，建立惯性传感器的先验连续轨迹；
步骤五：基于样条函数得到任意时刻惯性传感器的位姿，引入3D激光雷达和惯性传感器之间的外参，对3D激光的每个测量点进行运行补偿；通过环境约束，约束3D激光测量点的位置，构建一个优化问题，同时优化样条参数和传感器之间的外参，利用高斯牛顿法进行求解。


3.根据权利要求1所述的标定方法，其特征在于，所述标定方法标定的传感器类型是3D激光雷达和具有加速度计和角速度计的惯性传感器，3D激光雷达的测量频率为10Hz，惯性传感器的测量频率为400Hz。


4.根据权利要求1所述的标定方法，其特征在于，所述标定方法的标定环境是室内，且有一个墙角的三个面平滑无凸起，且墙角周围基本无物品放置。


5.根据权利要求2所述的标定方法，其特征在于，所述步骤二中的高精度点云地图的建模方法为：将一个单线激光雷达固定在步进电机上，步进电机匀速旋转，单线激光每一个测量点的位置可通过步进电机的旋转速度精确确定，旋转几圈后，得到高精度的环境点云地图；
在步骤二中，利用基于RANSAC的平面提取方法，在点云地图中提取三个...

【专利技术属性】
技术研发人员：刘勇，吕佳俊，徐晋鸿，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江;33