基于改进EKF的激光和视觉SLAM融合算法.pdf
《基于改进EKF的激光和视觉SLAM融合算法.pdf》由会员分享,可在线阅读,更多相关《基于改进EKF的激光和视觉SLAM融合算法.pdf(8页珍藏版)》请在咨信网上搜索。
1、JOURNAL OF NORTH UNIVERSITY OF CHINA(NATURAL SCIENCE EDITION)(Sum No.211)(总第2 11期)2023No.5Vol.44中北大学学报(自然科学版)2023年第44卷第5期文章编号:16 7 3-319 3(2 0 2 3)0 5-0 536-0 8基于改进EKF的激光和视觉SLAM融合算法黄永琦,秦品乐,曾建潮,柴锐,赵鹏程,温馨(中北大学大数据学院,山西太原0 30 0 51)摘要:角点特征在机器人同步定位与建图(SimultaneousLocalization andMapping,SLA M)系统中具有关键性的作用。
2、然而,由于环境差异、机器人运动距离和传感器的影响,导致现有测量方法的角点估计误差较大。本文在原有使用扩展卡尔曼滤波(ExtendedKalmanFilter,EK F)融合激光和视觉SLAM数据的基础上,引入多新息理论,提出了多新息改进EKF融合激光和视觉SLAM数据算法。由于多新息理论能有效利用历史时刻的数据,使系统在原先只使用当前时刻数据的情况下,扩展为能够利用之前多个时刻的有效数据。因此,利用多新息理论改进EKF,可以充分利用之前时刻由角特征和垂线特征融合成的角点结果,从而提升角点估计精度和建图结果。实验结果表明,在室内坏境中,本文方法在迭代次数2 0 次和10 0 次时平均误差分别为0
3、.0 2 6 8 和0.0 10 9,相较于未改进EKF方法,角点估计的精度平均提升了33.9%。关键词:同时定位与建图构建(SLAM);多传感器融合;多新息理论;扩展卡尔曼滤波中图分类号:TP394.1文献标识码:Adoi:10.3969/j.issn.1673-3193.2023.05.008aserandvisualsLAMEusionorithmBasedonImprovedEKFHUANG Yongqi,QIN Pinle,ZENG Jianchao,CHAI Rui,ZHAO Pengcheng,WEN Xin(School of Data Science and Technolo
4、gy,North University of China,Taiyuan 030051,China)Abstract:The corner point feature plays a key role in the simultaneous localization and mapping(SLAM)system of the robot.However,due to the influence of environmental differences,robot mov-ing distance and sensors,the existing methods have the proble
5、m of large corner point estimation error.The multi-neo-information theory was introduced,and a multi-neo-information improved EKF fusion la-ser and visual SLAM data algorithm was proposed based on the original use of extended kalman filter(EKF)to fuse laser and visual SLAM data.Since the multi-neo-i
6、nformation theory could effectively usethe data of historical moments,the system,which originally only used the data of the current moment,was expanded to use the data of the previous multiple moments.Therefore,the use of multi-neo-infor-mation theory to improve EKF could make full use of the corner
7、 point results fused by the corner fea-tures and perpendicular features at the previous moment,so as to improve the accuracy of corner pointestimation and mapping results.The experimental results show that in the indoor environment,the av-erage errors of the proposed method are 0.026 8 and 0.010 9 a
8、t 20 iterations and 100 iterations,respec-tively,and the accuracy of corner point estimation is improved by 33.9%compared with the unimprovedEKF method.Key words:simultaneous location and mapping(SLAM);multi-sensor fusion;multi-innovation theory;extended Kalman filter(EKF)收稿日期:2 0 2 3-0 2-0 2基金项目:山西
9、省重点研发项目(2 0 18 0 3D31212一1);山西省 揭榜挂帅 重大专项(2 0 2 10 10 10 10 10 18)作者简介:黄永琦(19 9 7 一),男,硕士生,主要从事计算机视觉的研究。通信作者:秦品乐(19 7 8 一),男,教授,博士,主要从事机器视觉、大数据的研究。537基于改进EKF的激光和视觉SLAM融合算法(黄永琦等)(总第2 11期)0引言同步定位与建图(SimultaneousLocalizationandMapping,SLA M)主要用于解决移动机器人在未知环境中运行时连续构建周围环境地图并同时估计自身在地图中位置的问题。SLAM起源于19 8 6 年
10、,用于描述地标和操作几何的不确定性关系 2,在每次观察地标之后进行更新,且地图上不同地标位置的估计之间须有高度的相关性,随后发展出了激光SLAM、视觉SLAM和多模态融合SLAM算法。激光SLAM可以有效提取到特征点的深度信息,视觉SLAM则可以利用丰富的纹理信息。然而,激光SLAM在环境特征不明显、动态环境等条件下表现不佳,并且由于其重定位能力较差,导致追踪丢失后工作状态很难恢复 3;视觉SLAM在无纹理或者光照较弱的环境下表现较差,而且在距离未知的情况下,单目相机具有需要初始化、尺度不确定、尺度漂移等缺点 4。为了弥补各传感器的不足,激光数据和视觉数据的融合成了热门研究课题 5针对单模态存
11、在不足导致角点估计精度不高的问题,Graeter等 6)提出使用束调整(BundleAdjustment,BA)将激光高精度测量深度的优势和相机提取特征的优势结合起来进行融合。Hwang等 7 提出了一种基于扩展卡尔曼滤波(Ex-tended Kalman Filter,EK F)的 SLAM方法,该方法使用最近邻数据进行数据关联,使用EKF估计移动机器人的位姿和构建地图。针对建图过程中角点偏差较高的问题,Mu等 8 提出利用相关扫描匹配方法实现2 DLiDAR点云与3DRGB-D点云的匹配,该方法降低了建图过程中的偏差。针对角点数据融合过程中噪声统计不准确的问题,Chan等 9 提出了使用轨
12、迹匹配将激光和单目相机的视觉SLAM数据融合在一起,并通过使用金字塔滤波器将图像进行降采样,过滤高频噪声。针对融合后角点估计精度不高的问题,Kuberlka等 10 提出使用RTS为EKF作为基准,融合激光和视觉SLAM的角点数据。该方法可以精确估计机器人的位姿,提高融合后的精度。针对在特征环境较少的情况下,传统的激光SLAM算法容易低估角点数据的问题,Junior等 11利用EKF将车轮里程计和IMU的数据融合到LeGO-LOAM算法中,该方法降低了噪声估计,提高了融合精度。针对角点数据融合过程中,融合的角点数据不完善导致生成地图不全的问题,Liu等 12 对使用深度相机获得的原始数据先进行
13、预处理,再进行融合,该方法可以有效地完善角点数据,提高建图结果。针对机器人在建图过程中角点数据丢失的问题,Zhang等 13提出了基于激光与视觉传感器加权异步融合的机器人同步定位与建图算法,充分利用了视觉传感器中的余数据,提高了建图中数据融合的精度,减少了迭代次数,具有较好的鲁棒性。Zuo等 14提出利用EKF融合激光雷达和图像数据中提取的视觉特征,充分利用数据特征来提高系统鲁棒性。然而,这些融合算法仍存在一些问题:1)融合后角点估计精度不高;2)若当前时刻数据出现偏差,则会导致建图结果产生偏差。本文基于EKF融合激光和视觉SLAM数据,提出了一种结合多新息理论改进的EKF融合激光和视觉SLA
14、M数据的算法。多新息理论可以有效利用历史时刻的状态信息。为了提高融合后角点估计的精度,利用多新息理论改进EKF,可以充分利用之前时刻由角特征和垂线特征融合成的角点结果,来优化当前时刻的数据。通过实验验证,该方法可以提高融合后角点估计精度,获得精确的建图结果。1相关工作1.1基于EKF的激光和视觉SLAM数据融合算法在三维结构环境中,为了融合激光数据和视觉数据,首先从激光传感器中提取角点特征,从单目视觉传感器中提取垂线特征,然后通过EKF将提取到的两个特征进行融合 15。在机器人坐标系B中,从激光数据提取的角点特征为XH-xByBOJT(1)极坐标系统中的角点特征为系统中的角点特征为cB=LpB
15、oBpBJT。(2)将当前第i时刻的状态矢量定义为u,根据垂线提取方法,单目视觉中的垂线特征向量为T1=(u o(3)式中:I表示相机坐标系C中的垂线特征。定义IB为机器人坐标系B中的垂线特征,将其表示为1P=ay?T=BiCIs,(4)538中北大学学报(自然科学版)2023年第5期式中:BBC表示相机坐标系C到机器人坐标系B的旋转矩阵。垂线特征向量在机器人坐标系B下的方位角为op=artanB(5)B。将激光提取的角特征c和单目视觉提取的垂线特征,通过EKF进行融合。EKF包括预测、观测和更新3个阶段,基于EKF的SLAM通过3个阶段的不断迭代,获取了机器人定位和特征定位。融合方程为h=h
16、(cB)=oB,H=101,Z=0,K=Q(c,HT(HQ(c)HTB=c-K(Z-h),B=(I-KH)Q2B)(6)式中:是!的协方差矩阵;Q是c的协方差矩阵;Q是的协方差矩阵,表示为传感器融合后预测的角特征状态;cBi是从激光数据中提取的角特征。详细的伪代码如表1所示。表1基于EKF的融合伪代码Tab.1 Fusion pseudocode based on EKF算法1:基于EKF的激光和视觉SLAM数据融合输入:角点特征向量XB,垂线特征向量1输出:融合后预测的角特征状态QA1fori=l;in;i+/n表示机器人运行时长2机器人坐标系下角点特征转换:XBc;3机器人坐标系下垂线特征
17、转换:1一1;B4计算在机器人坐标系下垂线特征向量:e9=arctanB5计算EKF融合的扩展卡尔曼增益:K=QBHT(H QH6计算融合后角特征状态:Q=(I-KH)QB7将融合后的角特征压人到角特征状态集栈中:QA=push(Q8return角特征状态集QA。1.2多新息理论常见的辨识算法一般都是单新息辨识方法 16。单新息只用单步新息对参数进行修正,标量系统的说明为y(r)=pT()o+v(r),(7)式中:y()表示系统的输人;T()表示由系统输数据和输出数据组成的向量;是最终需要辨识的参数向量;v()表示均值为零的系统随机噪声。因为是需要辨识的参数向量,所以可利用最小二乘辩识方法推出
18、,即(r)=(r-1)+L(r)e(),(8)式中:e()为标量单新息;L()为算法增益向量。标量单新息e()表示为e(r)=y(r)-T()(l)ER,(9)式中:()为时刻的参数估计,通过(一1)递推出来,在递推的过程中依靠增益向量L()与标量新息e()的乘积来对一1时刻的辨识参数进行修正。因此,多新息其实是将标量单新息进行迭代。将式(8)中的标量单新息e()推广为向量形式,并根据式(7)进行拓展。首先将增益向量L()拓展为增益矩阵(p,),接着将标量单新息e()拓展为单新息向量E(p,)=e(),e(一l),e(r一p十1)。因此,多新息辨识算法可被表示为()=(-1)+r(p,)E(p
19、,),(10)式中:增益矩阵(p,)ERxp;单新息向量E(p,)ER,p 1时的作用表示为新息长度,当p等于1时,则向标量新息退化。详细推导和证明多新息理论的过程可参考文献 17。通过EKF算法进行激光和视觉数据融合,可以实现较好的建图结果。然而,SLAM系统的非线性很强,仅利用当前时刻数据融合很容易造成误差累积,进而导致EKF滤波发散,产生精度不高等问题 18。利用历史时刻数据可以减少误差累积,提高融合精度,因此,利用多新息理论改进EKF进行融合是本文的研究重点。539基于改进EKF的激光和视觉SLAM融合算法(黄永琦等)(总第2 11期)2基于改进EKF的激光和视觉SLAM娄数据融合算法
20、由于标准的EKF算法 19 通过前一个时刻系统的状态来预测当前系统的状态,没有有效利用历史时刻的状态信息,本文引人了多新息理论来优化EKF201。多新息辨识 2 1-2 可以有效利用历史时刻的状态信息,从而提高参数估计精度。因此,本文使用多新息优化后的EKF来对激光SLAM和视觉SLAM的数据进行融合。EKF假设传感器噪声信息服从高斯分布,因此,使用系统状态X和协方差矩阵P来进行描述,描述方式为X(+1)=f(X(),(),v(),(11)式中:X()表示时刻的系统状态;()表示系统控制输人矢量;v()表示均值为零的高斯白噪声;v()的转换矩阵为Q(),可将其表述为E(v()=O,(12)E(
21、v(r)v(r)T)=Q()。(13)EKF分为预测、观测和更新3个阶段。在预测过程中,将十1时刻的系统状态表述为X(十1|),系统的观测信息为(十1)。在观测阶段,将系统的观测模型描述为z(r+1)=h(X(x+1)+w(+1),(14)式中:Z(十1)为观测阶段得到的数据;w(十1)为均值为零的高斯白噪声。在更新阶段,通过观测信息Z(k 十1)对系统在k十1时刻的状态进行更新。更新方程中标量新息e()可表示为e(r)=yr=Z-h(X)。(15)将标量新息e()扩展为新息矩阵E(p,),同时将系统观测数据Z()扩展为Z(,),测量噪声v()扩展为V(p,),扩展滤波增益矩阵为K(p,r),
22、它们的表示方式为e()Ze(-1)Z,-1E(p,)=,Z(p,)=Le(一p+1)Z一p+1KK,-1V-1V(p,)=,K(p,)=K+1P+1(16)式中:力为新息长度。将式(15)代人标准EKF融合激光和视觉SLAM数据方法中,可以扩展为c=C+L(),L2(),.,L,()JE(,)=cEi=L,()e(-i+1),(17)式中:是时刻激光数据中提取的角点特征,也是一1时刻提取的角点最佳值;L()(i=l,2,p)表示卡尔曼增益系矩阵;E(p,)表示由多个时刻的标量新息构成的标量新息矩阵。在式(16)的基础上进行扩展,得到多新息EKF融合激光和视觉SLAM数据算法的基本公式,为K;=
23、Q(c,HT(H,Q(c,HT.2cP=cP+K(p,i)E(p,i),Q=(I-K,H,)Q()。(18)B详细的伪代码如表2 所示。表2 基于改进EKF融合伪代码Tab.2Fusion pseudocode based on improved EKF算法2:基于改进EKF的激光和视觉SLAM数据融合输入:角点特征向量XB,垂线特征向量1输出:改进EKF后角特征状态集QA1fori=l;in;i+/n表示机器人运行时长2机器人坐标系下角点特征转换:XB一c;3机器人坐标系下垂线特征转换:11;4利用标量新息e(i)扩展为新息矩阵E=(p,i);K;Ki-15计算多新息改进的扩展卡尔曼增益:K
24、(p,i)=:K;-p+16计算多新息改进EKF融合后角特征状态:QB=(I-K,H,)QB7将融合后的角特征压人到角特征状态集栈中:QA=push(Q8return改进EKF后角特征状态集QA。中北大学学报(自然科学版)5402023年第5期3实验与分析本文使用TurtleBot3机器人作为实验平台,机器人上装载了思岚A2M8激光雷达、RealsenseD435相机和里程计,在室内和室外环境中对激光和视觉数据进行实验。3.1系统运行轨迹图为了验证加人多新息EKF融合激光和视觉SLAM数据的精度提升,分别对ORB-SLAM2算法、基于EKF和基于多新息EKF进行融合做实验。室内外场景的区别在于
25、光照、纹理信息和场景复杂程度的不同。因此,本文搭建了室内和室外仿真实验场景,如图1和图2 所示。图1和图2 从左往右分别为场景仿真图、场景实拍图和本文方法与其他3种方法角点精度对比的轨迹图。图中的4条线分别表示地标真实测量数据、使用ORB-SLAM2生成的轨迹数据、使用EKF融合后生成的轨迹数据及使用多新息扩展卡尔曼滤波(MI-EKF)融合后生成的轨迹数据。由于室内场景特征点较少及光照变化影响,导致ORB-SLAM2在连续拐弯的时候测量角点精度较低,生成的轨迹图误差较大。从图1和图2 可以看出,多新息EKF在轨迹角点处的精度有了很明显的提升,而且在整个建图的过程中有效利用了历史时刻的状态信息,
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 基于 改进 EKF 激光 视觉 SLAM 融合 算法
1、咨信平台为文档C2C交易模式,即用户上传的文档直接被用户下载,收益归上传人(含作者)所有;本站仅是提供信息存储空间和展示预览,仅对用户上传内容的表现方式做保护处理,对上载内容不做任何修改或编辑。所展示的作品文档包括内容和图片全部来源于网络用户和作者上传投稿,我们不确定上传用户享有完全著作权,根据《信息网络传播权保护条例》,如果侵犯了您的版权、权益或隐私,请联系我们,核实后会尽快下架及时删除,并可随时和客服了解处理情况,尊重保护知识产权我们共同努力。
2、文档的总页数、文档格式和文档大小以系统显示为准(内容中显示的页数不一定正确),网站客服只以系统显示的页数、文件格式、文档大小作为仲裁依据,平台无法对文档的真实性、完整性、权威性、准确性、专业性及其观点立场做任何保证或承诺,下载前须认真查看,确认无误后再购买,务必慎重购买;若有违法违纪将进行移交司法处理,若涉侵权平台将进行基本处罚并下架。
3、本站所有内容均由用户上传,付费前请自行鉴别,如您付费,意味着您已接受本站规则且自行承担风险,本站不进行额外附加服务,虚拟产品一经售出概不退款(未进行购买下载可退充值款),文档一经付费(服务费)、不意味着购买了该文档的版权,仅供个人/单位学习、研究之用,不得用于商业用途,未经授权,严禁复制、发行、汇编、翻译或者网络传播等,侵权必究。
4、如你看到网页展示的文档有www.zixin.com.cn水印,是因预览和防盗链等技术需要对页面进行转换压缩成图而已,我们并不对上传的文档进行任何编辑或修改,文档下载后都不会有水印标识(原文档上传前个别存留的除外),下载后原文更清晰;试题试卷类文档,如果标题没有明确说明有答案则都视为没有答案,请知晓;PPT和DOC文档可被视为“模板”,允许上传人保留章节、目录结构的情况下删减部份的内容;PDF文档不管是原文档转换或图片扫描而得,本站不作要求视为允许,下载前自行私信或留言给上传者【自信****多点】。
5、本文档所展示的图片、画像、字体、音乐的版权可能需版权方额外授权,请谨慎使用;网站提供的党政主题相关内容(国旗、国徽、党徽--等)目的在于配合国家政策宣传,仅限个人学习分享使用,禁止用于任何广告和商用目的。
6、文档遇到问题,请及时私信或留言给本站上传会员【自信****多点】,需本站解决可联系【 微信客服】、【 QQ客服】,若有其他问题请点击或扫码反馈【 服务填表】;文档侵犯商业秘密、侵犯著作权、侵犯人身权等,请点击“【 版权申诉】”(推荐),意见反馈和侵权处理邮箱:1219186828@qq.com;也可以拔打客服电话:4008-655-100;投诉/维权电话:4009-655-100。