基于改进图优化的激光SLAM算法_颜普.pdf

2 0 2 3年7月第4 7卷第4期安徽大学学报(自然科学版)J o u r n a l o fA n h u iU n i v e r s i t y(N a t u r a lS c i e n c eE d i t i o n)J u l y2 0 2 3V o l.4 7N o.4d o i:1 0.3 9 6 9/j.i s s n.1 0 0 0-2 1 6 2.2 0 2 3.0 4.0 0 5收稿日期:2 0 2 1-1 2-1 7基金项目:国家自然科学基金资助项目(6 1 9 0 1 0 0 6);安徽省自然科学基金青年项目(1 9 0 8 0 8 5 Q F 2 8 1);安徽省高校协同创新项目(G X X T-2 0 1 9-0 0 7)作者简介:颜 普(1 9 8 6-),男,安徽亳州人,安徽建筑大学讲师,E-m a i l:y a n p u 8 1 8 81 6 3.c o m.基于改进图优化的激光S L AM算法颜 普1,2,曹昊宇2,雷为好2(1.安徽建筑大学 智能建筑与建筑节能安徽省重点实验室,安徽 合肥2 3 0 0 2 2;2.安徽建筑大学 电子与信息工程学院,安徽 合肥2 3 0 6 0 1)摘 要:近 些 年 来,移 动 机 器 人 受 到 了 社 会 各 界 的 广 泛 关 注,其 中S L AM(s i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g)技术是移动机器人领域的研究热点.针对基于图优化框架下的激光S L AM算法对激光雷达运动畸变处理不够完善的情况,提出一种处理激光雷达数据的改进方案,调整点云数据中每一帧的时间戳与使用连续两帧之间的时间戳差值代替雷达驱动发出的固定的时间差值,解决了雷达数据反序、扫描时频率不稳定等问题.将基于粒子滤波的激光S L AM算法与基于图优化的激光S L AM算法、改进后的图优化激光S L AM算法进行对比实验,观察所建地图的精度与效果.经过多次实验表明改进的图优化激光S L AM算法显著提升了建图效果.关键词:移动机器人;激光S L AM;图优化;激光雷达运动畸变;粒子滤波中图分类号:T P 2 4 9;T P 3 9 9 文献标志码:A 文章编号:1 0 0 0-2 1 6 2(2 0 2 3)0 4-0 0 3 5-0 7L a s e rS L AMa l g o r i t h mb a s e do n i m p r o v e dg r a p ho p t i m i z a t i o nYANP u1,2,C AO H a o y u2,L E IW e i h a o2(1.A n h u iP r o v i n c i a lK e yL a b o r a t o r yo f I n t e l l i g e n tB u i l d i n ga n dB u i l d i n gE n e r g yC o n s e r v a t i o n,A n h u i J i a n z h uU n i v e r s i t y,H e f e i 2 3 0 0 2 2,C h i n a;2.C o l l e g eo fE l e c t r o n i ca n dI n f o r m a t i o nE n g i n e e r i n g,A n h u i J i a n z h uU n i v e r s i t y,H e f e i 2 3 0 6 0 1,C h i n a)A b s t r a c t:I nr e c e n ty e a r s,m o b i l er o b o t sh a v ea t t r a c t e dw i d ea t t e n t i o nf r o ma l ls e c t o r so fs o c i e t y,a m o n g w h i c h S L AM(s i m u l t a n e o u sl o c a l i z a t i o na n d m a p p i n g)t e c h n o l o g yi sar e s e a r c hh o t s p o t i nt h ef i e l do fm o b i l er o b o t s.D u et ot h el a s e rS L AMa l g o r i t h mb a s e do ng r a p ho p t i m i z a t i o ni n a d e q u a t eh a n d l e dl i d a r m o t i o nd i s t o r t i o n,a ni m p r o v e ds c h e m ef o rh a n d l i n gl i d a rd a t aw a sp r o p o s e d.T h e i m p r o v e ds c h e m ea d j u s t e dt h ep o i n t c l o u dd a t a i ne a c hf r a m eo ft h et i m e s t a m pa n dt h e nu s e dt h et i m e s t a m po ft h ed i f f e r e n c eb e t w e e nt h et w oc o n s e c u t i v ef r a m e si n s t e a do fr a d a rd r i v e ri nf i x e dt i m ev a l u e.O u rs c h e m es o l v e dt h ep r o b l e m ss u c ha s r e v e r s eo r d e ro f l i d a rd a t aa n d f r e q u e n c yd e v i a t i o nd u r i n gs c a n n i n g.As e r i e so f c o m p a r a t i v ee x p e r i m e n t sw e r ec a r r i e do u t f o r t h e l a s e rS L AMa l g o r i t h mb a s e do np a r t i c l ef i l t e r,t h e l a s e rS L AMa l g o r i t h mb a s e do ng r a p ho p t i m i z a t i o na n d t h e l a s e rS L AMa l g o r i t h mb a s e do ni m p r o v e dg r a p ho p t i m i z a t i o n t oo b s e r v et h ea c c u r a c ya n de f f e c to ft h eb u i l tm a p.S e v e r a l e x p e r i m e n t ss h o w e dt h a tt h ei m p r o v e dg r a p ho p t i m i z a t i o nl a s e rS L AM a l g o r i t h ms i g n i f i c a n t l y i m p r o v e s t h em a p p i n ge f f e c t.K e y w o r d s:m o b i l er o b o t;l a s e rS L AM;g r a p ho p t i m i z a t i o n;m o t i o nd i s t o r t i o no fl i d a r;p a r t i c l e f i l t e r近些年来随着人工智能的迅速发展,人们对便捷的生活以及智能化生产的需求越来越高,在这种需求的驱使下,机器人行业快速发展,而移动机器人是众多机器人种类中的一种,是许多企业与高校的研究热点.在研究移动机器人的技术中,S L AM(s i m u l t a n e o u s l o c a l i z a t i o na n dm a p p i n g)是其中一个重要分支.目前应用在移动机器人上的S L AM算法主要分为激光S L AM与视觉S L AM这两种.在视觉S L AM中比较流行的算法有O R B(o r i e n t e df a s ta n db r i e f)-S L AM1,L S D(l a r g es c a l ed i r e c tm o n o c u l a r)-S L AM2等,它们的基本想法是通过相机扫描出的图像构建3维环境.其优势在于环境特征信息更加丰富以及成本较低等.但存在运算负荷大、构建的地图难以直接用于导航与传感器等缺点.而激光S L AM的发展时间较长,其优点在于建图与定位的效果更稳定,所建地图可以直接用于导航等.基于激光的S L AM算法大体上可以分为两种不同的框架.一种是基于滤波器的激光S L AM算法3,另一种是基于图优化的激光S L AM算法3.基于图优化的激光S L AM算法的优点在于它可以消除长时间的误差、所建地图更加精确等.但是在目前主流的图优化框架中,对激光雷达的运动畸变的处理存在着无法解决雷达数据反序、雷达运行时频率不稳定等问题.论文首先对基于图优化的激光S L AM算法展开研究,然后对算法中对激光雷达运动畸变的处理方案进行改进.1 基于图优化的激光S L AM算法基于图优化的激光S L AM算法基本框架如图1所示.前端主要解决位姿图创建的问题,其中数据关联分为顺序匹配与回环检测.顺序匹配的作用是解决一段时间内的传感器数据的帧间匹配与相对位姿估计的问题;而回环检测会对当前位姿与过去已访问位姿进行匹配,解决其相对位姿估计问题.后端会对前端所构建的位姿图进行优化,得到最优的位姿图序列.图优化S L AM实际可以看作是一个最优解问题.构建一个位姿序列x=x0,x1,xi,xj,.调整各节点间的位姿,使得节点之间的里程计预测值fi j(x)与传感器观测值zi之间的误差ei j(x)最小,其中x代表机器人的位置,i和j代表了位姿图中相邻的两个节点.误差ei j(x)表达式为公式(1)所示.假设误差服从高斯分布,则观测值平方的误差如公式(2)所示.其中i是误差所对应的信息矩阵.非线性最小二乘的目标函数如公式(3)所示,位姿最优解的具体计算流程如图2所示.ei j(x)=fi j(x)-zi j,(1)Ei(x)=ei(x)Tiei(x),(2)F(x)=Ni=1Ei(x)=Ni=1ei(x)Tiei(x).(3)在如图2所示的图优化计算过程中,可以使用S D(s t e e p e s td e s c e n t),GN(G a u s s-N e w t o n)与LM(L e v e n b e r g-M a r q u a r d t)方法对目标函数m i nF(x)或ei(x)进行优化计算.首先给定一初始值x0,再对目标函数进行线性展开得到Ji(x)矩阵与H矩阵.其中Ji(x)为J a c o b i a n矩阵,H为H e s s e矩阵,它们都具有稀疏性.可以通过稀疏矩阵的特性对构建的线性方程组进行快速求解,并不断更新解,迭代直至收敛为止.图1 图优化框架图2 图优化计算过程63安徽大学学报(自然科学版)第4 7卷2 改进的图优化激光S L AM算法目前,在基于图优化框架的二维激光S L AM中,比较有代表性的有c a r t o g r a p h e r5与h e c t o r5.而