CN112147599A - 一种基于样条函数的连续时间上3d激光雷达和惯性传感器外参标定方法 - Google Patents
一种基于样条函数的连续时间上3d激光雷达和惯性传感器外参标定方法 Download PDFInfo
- Publication number
- CN112147599A CN112147599A CN201910571677.3A CN201910571677A CN112147599A CN 112147599 A CN112147599 A CN 112147599A CN 201910571677 A CN201910571677 A CN 201910571677A CN 112147599 A CN112147599 A CN 112147599A
- Authority
- CN
- China
- Prior art keywords
- inertial sensor
- point
- laser
- plane
- spline
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Manufacturing & Machinery (AREA)
- Traffic Control Systems (AREA)
- Navigation (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明提供了一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定方法,提供了一种基于连续时间的标定3D激光雷达和惯性传感器外参的方法。该方法分为两个阶段,第一阶段为基于已知环境地图,对标定数据中的3D激光点云进行预处理,第二阶段为将高频率惯性传感器的运动轨迹用样条函数建模为连续轨迹,基于样条函数得到任意时刻惯性传感器的位姿,引入3D激光雷达和惯性传感器之间的外参,约束激光点,构建优化问题进行求解。本方法利用样条函数对惯性传感器的轨迹连续化,有效解决传感器间测量频率不同所带来的紧耦合的困难。本方法对环境要求较低,只需有一个空旷的墙角即可。本发明的方法得到的标定结果可直接应用到机器人导航、无人驾驶等任务中,为多传感器数据融合提供了基础条件。
Description
技术领域
本发明涉及多传感器标定领域,特别涉及3D激光雷达和惯性传感器外参标定的技术领域。
背景技术
近年来无人驾驶领域发展较好,很重要的一个因素是激光雷达的普及。而激光雷达的线束越来越高,价格越来越低,测距能力越来越强,这些变化都将推动无人驾驶行业尽快落地。一般来说无人车上会装备许多互补的传感器,例如激光雷达、相机、毫米波雷达、惯性传感器等。而多传感器数据融合前提是传感器之间的内参、外参都有一个比较好的标定结果。相机间的标定、相机和惯性传感器之间的标定、相机和激光雷达之间的标定的工作有许多,但直接标定激光雷达和惯性传感器之间的外参的工作较少。
标定惯性传感器与其他传感器之间的外参时,为了充分激励惯性传感器每一个轴,通常会上下左右前后剧烈运动、摇晃。而激光雷达采集数据的方式是旋转内部电机上的激光测距单元进行测距,如果在采集激光数据时发生运动,又不对输出的数据进行运动补偿,则测距数据会出错,甚至会产生严重的撕裂。若激光雷达在采集数据时本体有运动,需要对测距数据进行运动补偿,每个数据点都需要视为一次单独测量。
目前常用的补偿方法是帧间线性插值,即匀速假设,而在标定场景中,传感器需要剧烈晃动,显然匀速假设无法成立,线性插值无法应用。
发明内容
本发明所要解决的技术问题是提供一种误差小的基于样条函数的激光雷达与惯性传感器外参标定方法,为此,本发明采用以下技术方案:
一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定***,所述标定***先预处理3D激光数据,再将高频率惯性传感器的运动轨迹用样条函数建模为连续轨迹,基于样条函数得到任意时刻惯性传感器的位姿,引入3D激光雷达和惯性传感器之间的外参,约束激光点,构建优化问题进行求解。
所述办法包括以下步骤:
步骤一:将装有3D激光雷达和惯性传感器的设备,在墙角前后左右上下各个角度充分运动、旋转,激励惯性传感器的每一轴,得到标定的数据;
步骤二:标定环境已事先建模为高精度点云地图,分别提取墙角3个平面点云,进行平面拟合得到平面参数;
步骤三:对步骤二得到每一帧激光点云,与环境地图匹配,得到该帧点云相对于环境地图的位姿;每个点根据匹配结果转换到环境地图坐标系,计算每个点与拟合平面的垂直距离,根据一定阈值,判断该点是否属于拟合平面,每个点增加一个属性,即属于哪个平面或不属于任何平面;
步骤四:根据步骤三得到的每一帧激光点云的位姿,及惯性传感器和激光雷达传感器之间的先验外参,计算对应时刻惯性传感器的位姿,和惯性传感器的测量值一起,约束样条,建立惯性传感器的先验连续轨迹;
步骤五:基于样条函数得到任意时刻惯性传感器的位姿,引入3D激光雷达和惯性传感器之间的外参,对3D激光的每个测量点进行运行补偿;通过环境约束,约束3D激光测量点的位置,构建一个优化问题,同时优化样条参数和传感器之间的外参,利用高斯牛顿法进行求解。
进一步地,所述标定方法标定的传感器类型是3D激光雷达和具有加速度计和角速度计的惯性传感器,3D激光雷达的测量频率为10Hz,惯性传感器的测量频率为400Hz。
进一步地,所述标定方法的标定环境是室内,且有一个墙角的三个面平滑无凸起,且墙角周围基本无物品放置。
进一步地,所述步骤二中的高精度点云地图的建模方法为:将一个单线激光雷达固定在步进电机上,步进电机匀速旋转,单线激光每一个测量点的位置可通过步进电机的旋转速度精确确定,旋转几圈后,得到高精度的环境点云地图;
在步骤二中,利用基于RANSAC的平面提取方法,在点云地图中提取三个墙角平面点云,三个平面Pi(i=0,1,2)分别用一个法向量ni和原点距离平面距离wi确定。
进一步地,在所述步骤四中,要求所选择的样条函数可微、可导方便,且局部参数变化不影响整条轨迹,故选择B样条;根据激光雷达与惯性传感器之间的先验外参将每一帧激光位姿转换为对应时刻惯性传感器的位姿利用惯性传感器的10Hz位姿和400Hz的测量数据,约束B样条参数,得到惯性传感器的连续轨迹,即其中表示旋转量的连续B样条函数,t(t)表示平移量的连续B样条函数,具体可写作Φ(t)是样条基函数,cθ和ct是样条参数;所述惯性传感器的轨迹用6条6阶B样条表示,即5次函数表示轨迹,对时间求2次导之后,即加速度曲线为3次函数;加速度计和角速度计的偏置随时间缓慢变换,用3阶B样条曲线表示。
进一步地,在所述步骤五中,根据惯性传感器的连续轨迹和传感器间的先验外参,得到激光雷达任意帧任意点的位姿,同时结合步骤三中得到的每一点的平面属性,用平面约束激光点位姿,其经过运动补偿的激光点到平面距离可建模为对应目标函数为R表示雷达传感器测量噪声的协方差矩阵。
其中和分别表示加速度计和角速度计的测量噪声协方差矩阵,gw表示重力加速度,ba和bω表示加速度计和角速度计的偏置;由于加速度计和角速度计的偏置变换缓慢,将其建模为零均值的高斯过程噪声,其协方差分别为Qω和Qa,偏置的目标函数写作:
B样条是一种表示连续时间轨迹的有效方法,其可微、可导的特性和局部性的特点,可以快速有效的将传感器轨迹建模连续时间下的轨迹。本***基于B样条,将传感器轨迹连续化,有效解决激光雷达每个点的运动补偿问题,最终建立优化问题,优化样条参数和传感器间外参。
本发明与现有技术相比,具有以下优点:相比于匀速假设,用线性插值补偿每个激光测量点的方法,本方法将传感器轨迹连续化,且充分利用传感器测量信息约束轨迹,能够得到任意频率的位姿信息,对每个激光点运动进行补偿;同时利用平面对激光点进行约束,引入传感器间外参,充分利用B样条参数的稀疏性,快速有效求解样条参数和传感器之间的外参。该方法标定的结果,可广泛应用于自动驾驶、机器人导航领域中的多传感器融合中。
附图说明
图1是本发明一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定***的流程图。
图2是本发明的实验环境图。
图3是本发明的环境地图。
图4是本发明3个平面的提取和激光点云数据增加平面属性可视化图。
图5是本发明实际试验结果,包括多次标定结果的均值和方差,真值从CAD模型中测量得到。
具体实施方式
下面结合附图,对本发明的实施作进一步详细的描述,但本发明并不局限于具体实施方式。
图1是一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定***的流程图,包括前面的处理流程的五个步骤。
标定环境如图2所示。环境地图已知,可利用本发明所述单线激光雷达加步进电机方案得到,也可通过其他建图方法获得。基于RANSAC平面提取方法,在已知环境地图中提取墙角的三个平面点云Pi(i=0,1,2),并计算拟合3个平面点云,得到平面参数,分别为法向量ni和原点距离平面距离wi。平面提取结果可参考图3。
将装有3D激光雷达和惯性传感器的设备,在墙角前后左右上下各个角度充分运动、旋转,激励惯性传感器的每一轴,采集标定数据。标定数据中每一帧激光点云在已知的环境地图中进行重定位,得到每一帧激光点云的位姿同时给每个点添加一个平面属性,表征属于那个平面,根据点到面公式 若点到面的距离小于一定阈值,则认为该点属于平面Pi,不属于任何一个平面的点,将被舍弃,某一典型激光帧数据处理结果如图4所示。
手动测量激光传感器和惯性传感器间外参,得到一个先验外参信息根据惯性传感器测量数据αq和传感器间先验外参和每一帧激光点云位姿初始化惯性传感器连续轨迹。对惯性传感器连续轨迹求导可得到加速度的连续轨迹α(t)和角速度的连续轨迹ω(t);加速度计和角速度计在tq时刻的测量值分别为αq和在加速度的连续轨迹和角速度的连续轨迹的对应时刻tq处采样,得到预测值α(tq)和ω(tq),由此建立惯性传感器的测量目标函数Ja和Jω。根据惯性传感器的连续轨迹和先验外参信息得到激光雷达任意帧任意点的位姿,同时结合每一点的平面属性,用平面约束激光点位姿,其经过运动补偿的激光点到平面距离可建模为对应目标函数为 最终构建总体目标函数J,优化求解样条参数和传感器间外参。
最终标定如图1所示的激光和惯性传感器的外参结果如图5所示。图5包含多次标定结果的平均值和标准差,真值通过CAD模型测量得到。标定误差平移量在2厘米以下,旋转量在2度以下。
以上所述仅是本发明优选实施方式,应当指出,对于本技术领域的普通技术人员,在不脱离本发明构思的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明保护范围内。
Claims (9)
1.一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定方法,其特征在于,所述标定方法分为两阶段:第一阶段为3D激光数据预处理,第二阶段为将高频率惯性传感器的运动轨迹用样条函数建模为连续轨迹,基于样条函数得到任意时刻惯性传感器的位姿,引入3D激光雷达和惯性传感器之间的外参,约束激光点,构建优化问题进行求解。
2.如权利要求1所述的一种基于样条函数的连续时间上3D激光雷达和惯性传感器外参标定方法,其特征在于,所述方法包括以下步骤:
步骤一:将装有3D激光雷达和惯性传感器的设备,在墙角前后左右上下各个角度充分运动、旋转,激励惯性传感器的每一轴,得到标定的数据;
步骤二:标定环境已事先建模为高精度点云地图,分别提取墙角3个平面点云,进行平面拟合得到平面参数;
步骤三:对步骤二得到每一帧激光点云,与环境地图匹配,得到该帧点云相对于环境地图的位姿;每个点根据匹配结果转换到环境地图坐标系,计算每个点与拟合平面的垂直距离,根据一定阈值,判断该点是否属于拟合平面,每个点增加一个属性,即属于哪个平面或不属于任何平面;
步骤四:根据步骤三得到的每一帧激光点云的位姿,及惯性传感器和激光雷达传感器之间的先验外参,计算对应时刻惯性传感器的位姿,和惯性传感器的测量值一起,约束样条,建立惯性传感器的先验连续轨迹;
步骤五:基于样条函数得到任意时刻惯性传感器的位姿,引入3D激光雷达和惯性传感器之间的外参,对3D激光的每个测量点进行运行补偿;通过环境约束,约束3D激光测量点的位置,构建一个优化问题,同时优化样条参数和传感器之间的外参,利用高斯牛顿法进行求解。
3.根据权利要求1所述的标定方法,其特征在于,所述标定方法标定的传感器类型是3D激光雷达和具有加速度计和角速度计的惯性传感器,3D激光雷达的测量频率为10Hz,惯性传感器的测量频率为400Hz。
4.根据权利要求1所述的标定方法,其特征在于,所述标定方法的标定环境是室内,且有一个墙角的三个面平滑无凸起,且墙角周围基本无物品放置。
5.根据权利要求2所述的标定方法,其特征在于,所述步骤二中的高精度点云地图的建模方法为:将一个单线激光雷达固定在步进电机上,步进电机匀速旋转,单线激光每一个测量点的位置可通过步进电机的旋转速度精确确定,旋转几圈后,得到高精度的环境点云地图;
在步骤二中,利用基于RANSAC的平面提取方法,在点云地图中提取三个墙角平面点云,三个平面Pi(i=0,1,2)分别用一个法向量ni和原点距离平面距离wi确定。
7.根据权利要求2所述的标定方法,其特征在于,在所述步骤四中,要求所选择的样条函数可微、可导方便,且局部参数变化不影响整条轨迹,故选择B样条;根据激光雷达与惯性传感器之间的先验外参将每一帧激光位姿转换为对应时刻惯性传感器的位姿利用惯性传感器的10Hz位姿和400Hz的测量数据,约束B样条参数,得到惯性传感器的连续轨迹,即其中表示旋转量的连续B样条函数,t(t)表示平移量的连续B样条函数,具体可写作Φ(t)是样条基函数,cθ和ct是样条参数;所述惯性传感器的轨迹用6条6阶B样条表示,即5次函数表示轨迹,对时间求2次导之后,即加速度曲线为3次函数;加速度计和角速度计的偏置随时间缓慢变换,用3阶B样条曲线表示。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910571677.3A CN112147599B (zh) | 2019-06-28 | 2019-06-28 | 基于样条函数的3d激光雷达和惯性传感器外参标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910571677.3A CN112147599B (zh) | 2019-06-28 | 2019-06-28 | 基于样条函数的3d激光雷达和惯性传感器外参标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112147599A true CN112147599A (zh) | 2020-12-29 |
CN112147599B CN112147599B (zh) | 2022-05-20 |
Family
ID=73869001
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910571677.3A Active CN112147599B (zh) | 2019-06-28 | 2019-06-28 | 基于样条函数的3d激光雷达和惯性传感器外参标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112147599B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113759349A (zh) * | 2021-09-22 | 2021-12-07 | 阿波罗智能技术(北京)有限公司 | 激光雷达与定位设备的标定方法、设备及自动驾驶车辆 |
CN113790738A (zh) * | 2021-08-13 | 2021-12-14 | 上海智能网联汽车技术中心有限公司 | 一种基于智能云台imu的数据补偿方法 |
CN114518111A (zh) * | 2022-03-11 | 2022-05-20 | 六安智梭无人车科技有限公司 | 一种激光雷达与惯性测量单元标定方法及*** |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110596683A (zh) * | 2019-10-25 | 2019-12-20 | 中山大学 | 一种多组激光雷达外参标定***及其方法 |
CN112444798A (zh) * | 2020-11-27 | 2021-03-05 | 杭州易现先进科技有限公司 | 多传感器设备时空外参标定方法、装置以及计算机设备 |
-
2019
- 2019-06-28 CN CN201910571677.3A patent/CN112147599B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110596683A (zh) * | 2019-10-25 | 2019-12-20 | 中山大学 | 一种多组激光雷达外参标定***及其方法 |
CN112444798A (zh) * | 2020-11-27 | 2021-03-05 | 杭州易现先进科技有限公司 | 多传感器设备时空外参标定方法、装置以及计算机设备 |
Non-Patent Citations (4)
Title |
---|
张海花 等: "改进的动调式陀螺测斜***校准方法研究", 《测井技术》 * |
谢丽丽: "三维成像***中投影镜头畸变的标定与修正研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 * |
陆志东 等: "高精度惯性导航***重力补偿方法", 《空科学技术》 * |
马芳武等: "无人驾驶车辆单目视觉里程计的研究进展", 《吉林大学学报(工学版)》 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113790738A (zh) * | 2021-08-13 | 2021-12-14 | 上海智能网联汽车技术中心有限公司 | 一种基于智能云台imu的数据补偿方法 |
CN113759349A (zh) * | 2021-09-22 | 2021-12-07 | 阿波罗智能技术(北京)有限公司 | 激光雷达与定位设备的标定方法、设备及自动驾驶车辆 |
CN113759349B (zh) * | 2021-09-22 | 2022-10-04 | 阿波罗智能技术(北京)有限公司 | 激光雷达与定位设备的标定方法、设备及自动驾驶车辆 |
CN114518111A (zh) * | 2022-03-11 | 2022-05-20 | 六安智梭无人车科技有限公司 | 一种激光雷达与惯性测量单元标定方法及*** |
Also Published As
Publication number | Publication date |
---|---|
CN112147599B (zh) | 2022-05-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110706279B (zh) | 基于全局地图与多传感器信息融合的全程位姿估计方法 | |
CN109709801B (zh) | 一种基于激光雷达的室内无人机定位***及方法 | |
CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
CN112147599B (zh) | 基于样条函数的3d激光雷达和惯性传感器外参标定方法 | |
CN103411621B (zh) | 一种面向室内移动机器人的光流场视觉/ins组合导航方法 | |
CN114323033B (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
CN108759822B (zh) | 一种移动机器人3d定位*** | |
CN109737968B (zh) | 基于二维LiDAR和智能手机的室内融合定位方法 | |
CN112781594A (zh) | 基于imu耦合的激光雷达迭代最近点改进算法 | |
CN110209180A (zh) | 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法 | |
Karam et al. | Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping | |
Hinzmann et al. | Flexible stereo: constrained, non-rigid, wide-baseline stereo vision for fixed-wing aerial platforms | |
CN115902930A (zh) | 一种面向船舶检测的无人机室内建图与定位方法 | |
Chen et al. | 3D LiDAR-GPS/IMU calibration based on hand-eye calibration model for unmanned vehicle | |
Peng et al. | Vehicle odometry with camera-lidar-IMU information fusion and factor-graph optimization | |
CN114397642A (zh) | 一种基于图优化的三维激光雷达与imu外参标定方法 | |
Li et al. | Aerial-triangulation aided boresight calibration for a low-cost UAV-LiDAR system | |
CN115046543A (zh) | 一种基于多传感器的组合导航方法及*** | |
CN112945233A (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
Wang et al. | Pose and velocity estimation algorithm for UAV in visual landing | |
CN115900697B (zh) | 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆 | |
CN117075158A (zh) | 基于激光雷达的无人变形运动平台的位姿估计方法及*** | |
CN116929343A (zh) | 位姿估计方法、相关设备及存储介质 | |
CN104075710A (zh) | 一种基于航迹预测的机动扩展目标轴向姿态实时估计方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |