CN110873883B - 融合激光雷达和imu的定位方法、介质、终端和装置 - Google Patents
融合激光雷达和imu的定位方法、介质、终端和装置 Download PDFInfo
- Publication number
- CN110873883B CN110873883B CN201911205995.4A CN201911205995A CN110873883B CN 110873883 B CN110873883 B CN 110873883B CN 201911205995 A CN201911205995 A CN 201911205995A CN 110873883 B CN110873883 B CN 110873883B
- Authority
- CN
- China
- Prior art keywords
- imu
- laser
- coordinate system
- laser radar
- relation
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 238000005259 measurement Methods 0.000 claims abstract description 48
- 238000012937 correction Methods 0.000 claims abstract description 35
- 230000009466 transformation Effects 0.000 claims abstract description 25
- 238000005457 optimization Methods 0.000 claims abstract description 21
- 238000009434 installation Methods 0.000 claims abstract description 17
- 238000012545 processing Methods 0.000 claims abstract description 14
- 230000004927 fusion Effects 0.000 claims description 25
- 230000003287 optical effect Effects 0.000 claims description 19
- 238000004590 computer program Methods 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 11
- 238000004422 calculation algorithm Methods 0.000 claims description 10
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 238000006073 displacement reaction Methods 0.000 claims description 7
- 230000008859 change Effects 0.000 claims description 5
- 230000010354 integration Effects 0.000 claims description 2
- 230000001131 transforming effect Effects 0.000 claims description 2
- 230000006870 function Effects 0.000 description 37
- 230000008569 process Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 6
- 230000008878 coupling Effects 0.000 description 4
- 238000010168 coupling process Methods 0.000 description 4
- 238000005859 coupling reaction Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000002604 ultrasonography Methods 0.000 description 1
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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明公开一种融合激光雷达和IMU的定位方法、介质、终端和装置,基于激光雷达传感器和惯性测量单元IMU的互补性,首先对获得的激光数据进行坐标变换处理,然后在已知激光传感器和IMU传感器安装关系的前提下,利用激光传感器对IMU进行必要的校正,然后利用两种传感器的测量值构建以定位变量为未知量的损失函数,最后通过最小二乘法对损失函数求解获得优化求解后的定位变量,即机器人的位姿数据,从而在不失高效性的前提下提高定位的稳定性、准确性,减少定位失败的可能。
Description
【技术领域】
本发明涉及导航定位领域,尤其涉及一种融合激光雷达和IMU的定位方法、介质、终端和装置。
【背景技术】
定位技术对于运动的机器人来说无疑是十分重要的基础,许多传感器都可以达到辅助定位的目的,例如,GPS,IMU(惯性测量元件),编码器,激光雷达,超声波,摄像头,WIFI,蓝牙,外部追踪等等。为了提高稳定性,传感器的混合使用在机器人领域也是极为常见。本发明,为了在室内缺乏GPS信号的情况下,达到长时间精准而稳定高效的计算平面机器人的位置,融合使用了二维激光雷达以及IMU两种传感器。
目前,平面运动机器人的研究与应用中已经大量使用激光雷达辅助机器的导航,考虑到成本,一般二维激光雷达使用较多,二维激光雷达只能扫描一个平面,其在工作是会在当前位置发出激光并接收反射光束,解析得到距离信息,而后激光发射器会转过一个角度分辨率对应的角度,再次重复这个过程。激光雷达的在一般情况下具有测距精准的优点,但其毕竟是一种受环境影响很大的传感器,在某些场景下,激光传感器的测量误差会急剧增大,从而造成定位失败。而IMU则是一种高频,并且几乎不依赖外界环境的瞬时传感器,但是由于工艺上客观的原因,IMU的测量偏差也不可避免。一般IMU传感器厂商也不会为每个产品提供偏差值,IMU的测量值为加速度和角速率,其在耦合以及一重、二重积分后,位置和姿态上的偏差也将会变得无法忽略。
【发明内容】
本发明提供了一种融合激光雷达和IMU的定位方法、介质、终端和装置,解决了以上所述的技术问题。
本发明解决上述技术问题的技术方案如下:一种融合激光雷达和IMU的定位方法,包括以下步骤:
步骤1,获取二维激光雷达扫描生成的相邻两帧点云,并将点云的极坐标转换为直角坐标;
步骤2,将坐标转换后的点云作为特征点,并基于光流法追踪所述特征点生成相邻两帧点云图像之间的变换关系;
步骤3,获取二维激光雷达和惯性测量单元IMU的安装关系,基于所述安装关系和所述变换关系对惯性测量单元进行偏差校正;
步骤4,融合二维激光雷达和偏差校正后惯性测量单元IMU的测量值,生成机器人的位姿数据。
在一个优选实施方式中,相邻两帧点云图像之间的变换关系为:
其中bk和bk+1表示相邻两帧点云图像,L表示激光坐标系,d表示扫描点距离激光雷达的距离,θ表示扫描角,x、y分别表示点云的直角坐标。
在一个优选实施方式中,对于平面机器人,二维激光雷达和惯性测量单元IMU的安装关系为:
其中为激光传感器坐标系与IMU坐标系的旋转关系,/>为激光传感器坐标系与IMU坐标系的位移关系,且激光传感器坐标系与IMU坐标系的XY平面重合:
在一个优选实施方式中,所述惯性测量单元的陀螺仪偏差校正函数为:
采用最小二乘法对所述陀螺仪偏差校正函数进行求解生成陀螺仪偏差bw。
在一个优选实施方式中,所述惯性测量单元的加速度计偏差校正函数为:
采用最小二乘法对所述加速度计偏差校正函数进行求解生成加速度计偏差ba。
在一个优选实施方式中,所述步骤4具体为:利用二维激光雷达和偏差校正后惯性测量单元的测量值构建以机器人定位变量为未知量的目标优化函数,通过最小二乘法对所述目标优化函数求解生成机器人的位姿数据,所述目标优化函数如下:
其中rB为偏差校正后IMU的测量值与机器人定位变量的损失函数,具体为:
rL为二维激光雷达的测量值与机器人定位变量的损失函数,具体为:
c(i)表示通过图像光流算法求得的不同帧激光间特征点的对应关系。
本发明实施例的第二方面提供了一种计算机可读存储介质,存储有计算机程序,所述计算机程序被处理器执行时,实现以上所述的融合激光雷达和IMU的定位方法。
本发明实施例的第三方面提供了一种融合激光雷达和IMU的定位终端,包括所述的计算机可读存储介质和处理器,所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述融合激光雷达和IMU的定位方法的步骤。
本发明实施例的第四方面提供了一种融合激光雷达和IMU的定位装置,包括点云数据处理模块、光流追踪模块、偏差校正模块和融合模块,
所述点云数据处理模块用于获取二维激光雷达扫描生成的相邻两帧点云,并将点云的极坐标转换为直角坐标;
所述光流追踪模块用于将坐标转换后的点云作为特征点,并基于光流法追踪所述特征点生成相邻两帧点云图像之间的变换关系;
所述偏差校正模块用于获取二维激光雷达和惯性测量单元IMU的安装关系,基于所述安装关系和所述变换关系对惯性测量单元进行偏差校正;
所述融合模块用于融合二维激光雷达和偏差校正后惯性测量单元IMU的测量值,生成机器人的位姿数据。
在一个优选实施方式中,所述融合模块具体用于利用二维激光雷达和偏差校正后惯性测量单元的测量值构建以机器人定位变量为未知量的目标优化函数,通过最小二乘法对所述目标优化函数求解生成机器人的位姿数据,所述目标优化函数如下:
其中rB为偏差校正后IMU的测量值与机器人定位变量的损失函数,具体为:
rL为二维激光雷达的测量值与机器人定位变量的损失函数,具体为:
c(i)表示通过图像光流算法求得的不同帧激光间特征点的对应关系。
本发明基于激光雷达传感器和惯性测量单元IMU的互补性,提出一种面向平面机器人融合二维激光雷达和IMU传感器的方法,首先对获得的激光数据进行坐标变换处理,然后在已知激光传感器和IMU传感器安装关系的前提下,利用激光传感器对IMU进行必要的校正,然后利用两种传感器的测量值构建以定位变量为未知量的损失函数,最后通过最小二乘法对损失函数求解获得优化求解后的定位变量,从而在不失高效性的前提下提高定位的稳定性、准确性,减少定位失败的可能。
为使发明的上述目的、特征和优点能更明显易懂,下文特举本发明较佳实施例,并配合所附附图,作详细说明如下。
【附图说明】
为了更清楚地说明本发明实施例的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本发明的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他相关的附图。
图1是实施例1提供的融合激光雷达和IMU的定位方法的流程示意图;
图2是实施例2提供的融合激光雷达和IMU的定位装置的结构示意图;
图3是实施例3提供的融合激光雷达和IMU的定位终端的结构示意图;
图4为采用光流算法对转换后的激光特征点进行跟踪的示意图;
图5为平面机器人中二维激光雷达与IMU的在俯视下的安装示意图。
【具体实施方式】
为了使本发明的目的、技术方案和有益技术效果更加清晰明白,以下结合附图和具体实施方式,对本发明进行进一步详细说明。应当理解的是,本说明书中描述的具体实施方式仅仅是为了解释本发明,并不是为了限定本发明。
图1是本发明实施例1提供的一种融合激光雷达和IMU的定位方法的流程示意图,如图1所示,方法包括以下步骤:
步骤1,获取二维激光雷达扫描生成的相邻两帧点云,并将点云的极坐标转换为直角坐标;
步骤2,将坐标转换后的点云作为特征点,并基于光流法追踪所述特征点生成相邻两帧点云图像之间的变换关系;
步骤3,获取二维激光雷达和惯性测量单元IMU的安装关系,基于所述安装关系和所述变换关系对惯性测量单元进行偏差校正;
步骤4,融合二维激光雷达和偏差校正后惯性测量单元IMU的测量值,生成机器人的位姿数据。
上述优选实施例基于激光雷达传感器和惯性测量单元IMU的互补性,提出一种面向平面机器人融合二维激光雷达和IMU传感器的方法,首先对获得的激光数据进行坐标变换处理,然后在已知激光传感器和IMU传感器安装关系的前提下,利用激光传感器对IMU进行必要的校正,然后利用两种传感器的测量值构建以定位变量为未知量的损失函数,通过对损失函数求解获得优化求解后的定位变量,从而在不失高效性的前提下提高定位的稳定性、准确性,减少定位失败的可能。
以下对上述实施例的步骤进行详细说明。
二维激光雷达扫描一个平面时会发出激光光束并接收反射光束,通过解析得到距离信息后,雷达转过一个角度分辨率对应的角度再次重复这个过程,从而得到一系列的扫描数据,包括距离以及反射光强等。受制于机械结构上的限制,激光雷达通常会有一部分盲区。一次二维激光雷达扫描获得一帧点云数据,通常如公式所示:
laser_scan=[l1 l2 L li L lN] (1)
其中i∈[1,N],Ii表示第i次发射激光测距的结果,N为电机转一圈,激光的发射次数。根据使用激光雷达传感器的特性不同,N可能会从几十到几千。由激光雷达扫描原理可知,激光数据通常为极坐标表示。为了将其作为图像处理,首先通过公式(2)将极坐标投影到笛卡尔坐标系,
其中d表示扫描点距离激光雷达的距离,θr表示激光扫描一圈转过的角度。坐标转换后的激光数据可以直接当做特征点使用,代替了图像算法中极为耗时的特征检测过程。
根据公式(2)将激光转为点特征后,则可根据光流算法追踪两帧点云图像中的特征点,如图4所示,得出两帧点云图像(设定两帧分别记为bk和bk+1)之间的变换关系:
其中,为两帧点云从bk到bk+1的位移变换,/>为两帧点云间的角度变换,其中左上标L为激光坐标系。
这里利用图像的光流算法进行初始位置与姿态的求解,极大的减小了系统的开发难度,并且不失高效和准确性。
然后获取IMU和激光雷达传感器的安装关系。对于平面机器人,二者的安装关系可简化为图5所示,从激光坐标系变换到IMU坐标系可记为其中/>为激光坐标系与IMU坐标系的旋转关系,/>为两个激光坐标系与IMU坐标系的位移关系,B为IMU坐标系,也是机体(body)坐标系,L为激光坐标系。
假设激光和IMU的XY平面重合,则:
在有了激光的计算结果(3)以及激光与IMU的坐标变换(4)后,就可以对IMU的偏差进行校正,该IMU偏差在机器人开机运行阶段为常量。惯性测量单元IMU一般具有陀螺仪以及加速计两个部分,本实施例对其分别进行校正,公式(5)展示了t时刻的角速率测量值与角速率真值ωt、角速率偏差bw以及噪声nw之间的关系。
为了求得bw,我们根据公式(6)(7),求得在激光bk和bk+1帧之间,IMU绕Z轴旋转的角度为和/>以及变化值/>
则
这里tk和tk+1分别表示K帧和K+1帧对应时刻。而由公式2中计算同样可得到角度变化那么取得初始化阶段所有的帧K∈B,B是指初始化阶段所有激光帧的集合,如{0,1,…k,k+1,…},可得到一个损失函数:
通过最小二乘法,对公式(8)进行求解,即可得到陀螺仪偏差bw。上述方程的具体求解过程属于本技术领域的常规技术手段,在此不进行详细说明。
类似的,根据加速度,速度和位移的关系(9)(10)(11)联系公式(3),也可以得到关于加速度计偏差ba的损失函数:
其中表示t时刻的加速度测量值,at表示t时刻的加速度真值,bat表示t时刻的加速度偏差,na表示噪声。公式(10)为k+1时刻速度的求解方法,/>为k+1时刻imu的速度,为k时刻imu的速度,公式(11)为k+1时刻位置的求解方法,/>为k+1时刻imu的位置,为k时刻imu的位置,△t为k时刻到k+1时刻的间隔时间。
因为本实施例对应的平面上运动的真实物体,因此也就是初始速度为0。通过对方程(8)和(12)的求解,即可得到IMU的偏差值估计,初始化自此完成。
在对IMU偏差求解后,本实施例可以融合IMU和激光两种传感器的测量值,进行稳定的定位。如公式(13)所示,χ为以激光从N(N>=0)到M(M>=N)帧之间的状态值x(位置与角度/>),其也是我们最终要求的值:
χ=[xN,x1,L,xM] (13)
通过IMU的测量值与待求的状态值构建IMU的损失函数如下所示:
同样根据二维激光雷达的测量值与待求的状态值构建二维激光雷达的损失函数如下所示:
此处c(i)表示不同帧激光间特征点的对应关系,其由图像光流算法求得,而在此处只需要对特征进行光流跟踪即可,并不需要如初始化时需要对位姿进行求解,从而进一步减少了计算的复杂性。而且,为了减小激光匹配中的异常值对于定位方程的影响,本实施例还使用了huber norm函数:
最终,本实施例以从N到M帧之间两种传感器的损失函数和为目标优化函数(18),对其求解即可得到我们需要的状态值,即机器人的待求位置与角度,从而达到最终的目标。
其中为第k帧到k+1帧之间的imu的测量值,χ为待优化的变量(也就是我们求解的目标),其具体可见公式(15),后半部分为公式(16),其中/>为激光的测量值,/>和/>左上标xy为激光平面,其通过转换矩阵/>变换到imu坐标系下,再联立j和k帧的待求状态值,即构建了一个待优化的目标值,其中/>为协方差矩阵,可以由经验得来,设定为一个常量矩阵,也可以在优化过程中根据雅可比矩阵演变。其中(i,bk,bj)∈C为k帧激光中第i个特征点与第j帧中的c(i)帧存在对应关系,所有存在对应关系的特征即构成集合C,其中/>为激光第i个特征点在j和k之间的协方差矩阵,也可以根据激光传感器的特性,设定为常量矩阵。
应理解,上述实施例中各步骤的序号的大小并不意味着执行顺序的先后,各过程的执行顺序应以其功能和内在逻辑确定,而不应对本发明实施例的实施过程构成任何限定。
本发明实施例还提供了一种计算机可读存储介质,存储有计算机程序,所述计算机程序被处理器执行时,实现以上所述的融合激光雷达和IMU的定位方法。
图2是本发明实施例2提供的一种融合激光雷达和IMU的定位装置的结构示意图,如图2所示,包括点云数据处理模块100、光流追踪模块200、偏差校正模块300和融合模块400,
所述点云数据处理模块100用于获取二维激光雷达扫描生成的相邻两帧点云,并将点云的极坐标转换为直角坐标;
所述光流追踪模块200用于将坐标转换后的点云作为特征点,并基于光流法追踪所述特征点生成相邻两帧点云图像之间的变换关系;
所述偏差校正模块300用于获取二维激光雷达和惯性测量单元IMU的安装关系,基于所述安装关系和所述变换关系对惯性测量单元进行偏差校正;
所述融合模块400用于融合二维激光雷达和偏差校正后惯性测量单元IMU的测量值,生成机器人的位姿数据。
在一个优选实施方式中,所述融合模块400具体用于利用二维激光雷达和偏差校正后惯性测量单元的测量值构建以机器人定位变量为未知量的目标优化函数,通过最小二乘法对所述目标优化函数求解生成机器人的位姿数据,所述目标优化函数如下:
其中rB为偏差校正后IMU的测量值与机器人定位变量的损失函数,具体为:
rL为二维激光雷达的测量值与机器人定位变量的损失函数,具体为:
c(i)表示通过图像光流算法求得的不同帧激光间特征点的对应关系。
本发明实施例还提供了一种融合激光雷达和IMU的定位终端,包括所述的计算机可读存储介质和处理器,所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述融合激光雷达和IMU的定位方法的步骤。图3是本发明实施例3提供的融合激光雷达和IMU的定位终端的结构示意图,如图3所示,该实施例的融合激光雷达和IMU的定位终端8包括:处理器80、可读存储介质81以及存储在所述可读存储介质81中并可在所述处理器80上运行的计算机程序82。所述处理器80执行所述计算机程序82时实现上述各个方法实施例中的步骤,例如图1所示的步骤1至步骤4。或者,所述处理器80执行所述计算机程序82时实现上述各装置实施例中各模块的功能,例如图2所示模块100至400的功能。
示例性的,所述计算机程序82可以被分割成一个或多个模块,所述一个或者多个模块被存储在所述可读存储介质81中,并由所述处理器80执行,以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段,该指令段用于描述所述计算机程序82在所述融合激光雷达和IMU的定位终端8中的执行过程。
所述融合激光雷达和IMU的定位终端8可包括,但不仅限于,处理器80、可读存储介质81。本领域技术人员可以理解,图3仅仅是融合激光雷达和IMU的定位终端8的示例,并不构成对融合激光雷达和IMU的定位终端8的限定,可以包括比图示更多或更少的部件,或者组合某些部件,或者不同的部件,例如所述融合激光雷达和IMU的定位终端还可以包括电源管理模块、运算处理模块、输入输出设备、网络接入设备、总线等。
所称处理器80可以是中央处理单元(Central Processing Unit,CPU),还可以是其他通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现成可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述可读存储介质81可以是所述融合激光雷达和IMU的定位终端8的内部存储单元,例如融合激光雷达和IMU的定位终端8的硬盘或内存。所述可读存储介质81也可以是所述融合激光雷达和IMU的定位终端8的外部存储设备,例如所述融合激光雷达和IMU的定位终端8上配备的插接式硬盘,智能存储卡(Smart Media Card,SMC),安全数字(SecureDigital,SD)卡,闪存卡(Flash Card)等。进一步地,所述可读存储介质81还可以既包括所述融合激光雷达和IMU的定位终端8的内部存储单元也包括外部存储设备。所述可读存储介质81用于存储所述计算机程序以及所述融合激光雷达和IMU的定位终端所需的其他程序和数据。所述可读存储介质81还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到,为了描述的方便和简洁,仅以上述各功能单元、模块的划分进行举例说明,实际应用中,可以根据需要而将上述功能分配由不同的功能单元、模块完成,即将所述装置的内部结构划分成不同的功能单元或模块,以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中,上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。另外,各功能单元、模块的具体名称也只是为了便于相互区分,并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
在上述实施例中,对各个实施例的描述都各有侧重,某个实施例中没有详述或记载的部分,可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到,结合本文中所公开的实施例描述的各示例的单元及方法步骤,能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。
在本发明所提供的实施例中,应该理解到,所揭露的装置/终端设备和方法,可以通过其它的方式实现。例如,以上所描述的装置/终端设备实施例仅仅是示意性的,例如,所述模块或单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口,装置或单元的间接耦合或通讯连接,可以是电性,机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外,在本发明各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
本发明并不仅仅限于说明书和实施方式中所描述,因此对于熟悉领域的人员而言可容易地实现另外的优点和修改,故在不背离权利要求及等同范围所限定的一般概念的精神和范围的情况下,本发明并不限于特定的细节、代表性的设备和这里示出与描述的图示示例。
Claims (6)
1.一种融合激光雷达和IMU的定位方法,其特征在于,包括以下步骤:
步骤1,获取二维激光雷达扫描生成的相邻两帧点云,并将点云的极坐标转换为直角坐标;
步骤2,将坐标转换后的点云作为特征点,并基于光流法追踪所述特征点生成相邻两帧点云图像之间的变换关系;
步骤3,获取二维激光雷达和惯性测量单元IMU的安装关系,基于所述安装关系和所述变换关系对惯性测量单元进行偏差校正;
步骤4,融合二维激光雷达和偏差校正后惯性测量单元IMU的测量值,生成机器人的位姿数据;
相邻两帧点云图像之间的变换关系为:
其中bk和bk+1表示相邻两帧点云图像,L表示激光坐标系,d表示扫描点距离激光雷达的距离,θ表示扫描角,x、y分别表示点云的直角坐标;
对于平面机器人,二维激光雷达和惯性测量单元IMU的安装关系为:
其中为激光传感器坐标系与IMU坐标系的旋转关系,/>为激光传感器坐标系与IMU坐标系的位移关系,且激光传感器坐标系与IMU坐标系的XY平面重合:
所述惯性测量单元的陀螺仪偏差校正函数为:
其中,为IMU坐标系中旋转角度/>和/>的变化值,/>和/>均为IMU坐标系中绕Z轴旋转的角度;K为点云图像取得初始化阶段的第K帧,B则为初始化阶段所有激光帧的集合;
采用最小二乘法对所述陀螺仪偏差校正函数进行求解生成陀螺仪偏差bw;
所述惯性测量单元的加速度计偏差校正函数为:
其中,表示t时刻的加速度测量值,at表示t时刻的加速度真值,bat表示t时刻的加速度偏差,na表示噪声,/>为k+1时刻imu的速度,/>为k时刻imu的速度,/>为k+1时刻imu的位置,/>为k时刻imu的位置,△tk为k时刻到k+1时刻的间隔时间;
采用最小二乘法对所述加速度计偏差校正函数进行求解生成加速度计偏差ba。
2.根据权利要求1所述的融合激光雷达和IMU的定位方法,其特征在于,所述步骤4具体为:利用二维激光雷达和偏差校正后惯性测量单元的测量值构建以机器人定位变量为未知量的目标优化函数,通过最小二乘法对所述目标优化函数求解生成机器人的位姿数据,所述目标优化函数如下:
其中rB为偏差校正后IMU的测量值与机器人定位变量的损失函数,具体为:
rL为二维激光雷达的测量值与机器人定位变量的损失函数,具体为:
c(i)表示通过图像光流算法求得的不同帧激光间特征点的对应关系;χ为以激光从N(N>=0)到M(M>=N)帧之间的状态值x,x包括位置与角度/>(i,bk,bj)∈C为k帧激光中第i个特征点与第j帧中的c(i)帧存在对应关系,所有存在对应关系的特征即构成集合C,其中为激光第i个特征点在j和k之间的协方差矩阵;其中/>为第k帧到k+1帧之间的imu的测量值;/>和/>左上标xy为激光平面,其通过转换矩阵/>变换到imu坐标系下;为激光第i个特征点在j和k之间的协方差矩阵;
激光传感器坐标系变换到IMU坐标系可记为其中/>为激光坐标系与IMU坐标系的旋转关系,/>为两个激光坐标系与IMU坐标系的位移关系。
3.一种计算机可读存储介质,其特征在于,存储有计算机程序,所述计算机程序被处理器执行时,实现权利要求1-2任一项所述的融合激光雷达和IMU的定位方法。
4.一种融合激光雷达和IMU的定位终端,其特征在于,包括权利要求3所述的计算机可读存储介质和处理器,所述处理器执行所述计算机可读存储介质上的计算机程序时实现如权利要求1-2任一项所述融合激光雷达和IMU的定位方法的步骤。
5.一种融合激光雷达和IMU的定位装置,其特征在于,包括点云数据处理模块、光流追踪模块、偏差校正模块和融合模块,
所述点云数据处理模块用于获取二维激光雷达扫描生成的相邻两帧点云,并将点云的极坐标转换为直角坐标;
所述光流追踪模块用于将坐标转换后的点云作为特征点,并基于光流法追踪所述特征点生成相邻两帧点云图像之间的变换关系;
所述偏差校正模块用于获取二维激光雷达和惯性测量单元IMU的安装关系,基于所述安装关系和所述变换关系对惯性测量单元进行偏差校正;
所述融合模块用于融合二维激光雷达和偏差校正后惯性测量单元IMU的测量值,生成机器人的位姿数据;
其中,相邻两帧点云图像之间的变换关系为:
其中bk和bk+1表示相邻两帧点云图像,L表示激光坐标系,d表示扫描点距离激光雷达的距离,θ表示扫描角,x、y分别表示点云的直角坐标;
对于平面机器人,二维激光雷达和惯性测量单元IMU的安装关系为:
其中为激光传感器坐标系与IMU坐标系的旋转关系,/>为激光传感器坐标系与IMU坐标系的位移关系,且激光传感器坐标系与IMU坐标系的XY平面重合:
所述惯性测量单元的陀螺仪偏差校正函数为:
其中,为IMU坐标系中旋转角度/>和/>的变化值,/>和/>均为IMU坐标系中绕Z轴旋转的角度;K为点云图像取得初始化阶段的第K帧,B则为初始化阶段所有激光帧的集合;
采用最小二乘法对所述陀螺仪偏差校正函数进行求解生成陀螺仪偏差bw;
所述惯性测量单元的加速度计偏差校正函数为:
其中,表示t时刻的加速度测量值,at表示t时刻的加速度真值,bat表示t时刻的加速度偏差,na表示噪声,/>为k+1时刻imu的速度,/>为k时刻imu的速度,/>为k+1时刻imu的位置,/>为k时刻imu的位置,△tk为k时刻到k+1时刻的间隔时间;
采用最小二乘法对所述加速度计偏差校正函数进行求解生成加速度计偏差ba。
6.根据权利要求5所述的融合激光雷达和IMU的定位装置,其特征在于,所述融合模块具体用于利用二维激光雷达和偏差校正后惯性测量单元的测量值构建以机器人定位变量为未知量的目标优化函数,通过最小二乘法对所述目标优化函数求解生成机器人的位姿数据,所述目标优化函数如下:
其中rB为偏差校正后IMU的测量值与机器人定位变量的损失函数,具体为:
rL为二维激光雷达的测量值与机器人定位变量的损失函数,具体为:
c(i)表示通过图像光流算法求得的不同帧激光间特征点的对应关系;
χ为以激光从N(N>=0)到M(M>=N)帧之间的状态值x,x包括位置与角度/>(i,bk,bj)∈C为k帧激光中第i个特征点与第j帧中的c(i)帧存在对应关系,所有存在对应关系的特征即构成集合C,其中/>为激光第i个特征点在j和k之间的协方差矩阵;其中/>为第k帧到k+1帧之间的imu的测量值;/>和/>左上标xy为激光平面,其通过转换矩阵变换到imu坐标系下;/>为激光第i个特征点在j和k之间的协方差矩阵;
激光传感器坐标系变换到IMU坐标系可记为其中/>为激光坐标系与IMU坐标系的旋转关系,/>为两个激光坐标系与IMU坐标系的位移关系。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911205995.4A CN110873883B (zh) | 2019-11-29 | 2019-11-29 | 融合激光雷达和imu的定位方法、介质、终端和装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911205995.4A CN110873883B (zh) | 2019-11-29 | 2019-11-29 | 融合激光雷达和imu的定位方法、介质、终端和装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110873883A CN110873883A (zh) | 2020-03-10 |
CN110873883B true CN110873883B (zh) | 2023-08-29 |
Family
ID=69717339
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911205995.4A Active CN110873883B (zh) | 2019-11-29 | 2019-11-29 | 融合激光雷达和imu的定位方法、介质、终端和装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110873883B (zh) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111947649A (zh) * | 2020-06-21 | 2020-11-17 | 珠海市一微半导体有限公司 | 基于数据融合的机器人定位方法、芯片以及机器人 |
CN112304307B (zh) * | 2020-09-15 | 2024-09-06 | 浙江大华技术股份有限公司 | 一种基于多传感器融合的定位方法、装置和存储介质 |
CN112924955B (zh) * | 2021-01-29 | 2022-12-16 | 同济大学 | 一种路侧激光雷达点云坐标动态修正方法 |
CN112882961B (zh) * | 2021-03-31 | 2022-04-08 | 郑州信大捷安信息技术股份有限公司 | 一种列车轨迹纠偏算法性能测试方法 |
CN112985415B (zh) * | 2021-04-15 | 2021-08-10 | 武汉光谷信息技术股份有限公司 | 一种室内定位方法及系统 |
CN113790738A (zh) * | 2021-08-13 | 2021-12-14 | 上海智能网联汽车技术中心有限公司 | 一种基于智能云台imu的数据补偿方法 |
CN114166219B (zh) * | 2021-12-01 | 2024-06-14 | 珠海一微半导体股份有限公司 | 一种校正惯导误差的方法、芯片及机器人 |
CN114136316B (zh) * | 2021-12-01 | 2024-08-02 | 珠海一微半导体股份有限公司 | 基于点云特征点的惯导误差消除方法、芯片及机器人 |
CN115307646B (zh) * | 2022-10-08 | 2023-03-24 | 浙江光珀智能科技有限公司 | 一种多传感器融合的机器人定位方法、系统及装置 |
CN117554937B (zh) * | 2024-01-08 | 2024-04-26 | 安徽中科星驰自动驾驶技术有限公司 | 一种误差可控的激光雷达和组合惯导外参标定方法和系统 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102800127A (zh) * | 2012-07-18 | 2012-11-28 | 清华大学 | 一种基于光流优化的三维重建方法及装置 |
US9043069B1 (en) * | 2012-11-07 | 2015-05-26 | Google Inc. | Methods and systems for scan matching approaches for vehicle heading estimation |
WO2018205119A1 (zh) * | 2017-05-09 | 2018-11-15 | 深圳市速腾聚创科技有限公司 | 基于激光雷达扫描的路沿检测方法和系统 |
CN108932736A (zh) * | 2018-05-30 | 2018-12-04 | 南昌大学 | 二维激光雷达点云数据处理方法以及动态机器人位姿校准方法 |
EP3410153A1 (en) * | 2017-06-02 | 2018-12-05 | Airbus Defence and Space, S.A. | Portable device and method for locating components inside a closed environment, preferably inside an aircraft |
CN109597095A (zh) * | 2018-11-12 | 2019-04-09 | 北京大学 | 背包式三维激光扫描与立体成像组合系统及数据获取方法 |
CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
CN110389369A (zh) * | 2019-07-30 | 2019-10-29 | 南京林业大学 | 基于rtk-gps和移动二维激光扫描的冠层点云获取方法 |
-
2019
- 2019-11-29 CN CN201911205995.4A patent/CN110873883B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102800127A (zh) * | 2012-07-18 | 2012-11-28 | 清华大学 | 一种基于光流优化的三维重建方法及装置 |
US9043069B1 (en) * | 2012-11-07 | 2015-05-26 | Google Inc. | Methods and systems for scan matching approaches for vehicle heading estimation |
WO2018205119A1 (zh) * | 2017-05-09 | 2018-11-15 | 深圳市速腾聚创科技有限公司 | 基于激光雷达扫描的路沿检测方法和系统 |
EP3410153A1 (en) * | 2017-06-02 | 2018-12-05 | Airbus Defence and Space, S.A. | Portable device and method for locating components inside a closed environment, preferably inside an aircraft |
CN108932736A (zh) * | 2018-05-30 | 2018-12-04 | 南昌大学 | 二维激光雷达点云数据处理方法以及动态机器人位姿校准方法 |
CN109597095A (zh) * | 2018-11-12 | 2019-04-09 | 北京大学 | 背包式三维激光扫描与立体成像组合系统及数据获取方法 |
CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
CN110389369A (zh) * | 2019-07-30 | 2019-10-29 | 南京林业大学 | 基于rtk-gps和移动二维激光扫描的冠层点云获取方法 |
Non-Patent Citations (1)
Title |
---|
多功能巡检救援机器人系统设计及定位导航;汪名飞 等;《机械设计与制造》;20181130;第233-236页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110873883A (zh) | 2020-03-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110873883B (zh) | 融合激光雷达和imu的定位方法、介质、终端和装置 | |
CN111427061A (zh) | 一种机器人建图方法、装置,机器人及存储介质 | |
US11279045B2 (en) | Robot pose estimation method and apparatus and robot using the same | |
CN111121754A (zh) | 移动机器人定位导航方法、装置、移动机器人及存储介质 | |
CN110470333B (zh) | 传感器参数的标定方法及装置、存储介质和电子装置 | |
CN110880189A (zh) | 联合标定方法及其联合标定装置和电子设备 | |
CN114612348B (zh) | 激光点云运动畸变校正方法、装置、电子设备及存储介质 | |
CN113587934B (zh) | 一种机器人、室内定位方法、装置和可读存储介质 | |
CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
CN112146682B (zh) | 智能汽车的传感器标定方法、装置、电子设备及介质 | |
WO2022179094A1 (zh) | 车载激光雷达外参数联合标定方法、系统、介质及设备 | |
CN114296057A (zh) | 一种计算测距系统相对外参的方法、装置和存储介质 | |
WO2024001649A1 (zh) | 机器人定位方法、装置和计算可读存储介质 | |
CN115200572B (zh) | 三维点云地图构建方法、装置、电子设备及存储介质 | |
CN116736266B (zh) | 一种用于扫描激光雷达的码盘角度方法 | |
CN115291197A (zh) | 激光雷达的标定参数检测方法、装置、车辆及存储介质 | |
CN113759384A (zh) | 一种传感器位姿转换关系确定方法、装置、设备和介质 | |
CN116952229A (zh) | 无人机定位方法、装置、系统和存储介质 | |
CN113093759A (zh) | 基于多传感器信息融合的机器人编队构造方法及系统 | |
Yang et al. | Simultaneous estimation of ego-motion and vehicle distance by using a monocular camera | |
CN112154480B (zh) | 可移动平台的定位方法、装置、可移动平台及存储介质 | |
CN111351487A (zh) | 多传感器的时钟同步方法、装置及计算设备 | |
CN116047481A (zh) | 矫正点云数据畸变方法、装置、设备及存储介质 | |
CN112987010B (zh) | 一种用于机器人的多雷达建图的系统及方法 | |
CN114185020A (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 |