融合NDT的2D激光数据扫描匹配遗传算法

2021-03-26穆莉莉何世政姚潘涛祁娜娜

传感器与微系统 2021年3期

穆莉莉, 陈凯, 何世政, 姚潘涛, 祁娜娜

(安徽理工大学机械工程学院,安徽淮南 232001)

0 引言

移动机器人在连续运动过程中需要求解各个时刻的位姿,在同步定位与环境建图 (simultaneous location and mapping,SLAM)领域通常采用激光测距仪作为外部传感器,利用扫描匹配方法求解出连续的激光帧之间的相对位姿变换。常采用正态分布变换(normal distribution transform,NDT)方法解决激光扫描匹配问题[1],通过建立分段连续可微的概率分布,无需建立激光点之间明确的对应关系。然而在初始位姿误差较大的情况下使用NDT方法求解容易陷入局部极值,从而导致结果错误[2]。遗传算法(genetic algorithm,GA)作为一种全局优化方法,具有极强的搜索能力[3]。近年来,有许多学者尝试将遗传算法引入到激光扫描配准问题中来,并取得了一些进展[4～10]。Tom等人[4]采用地图一致性和紧凑性作为适应度判断,逐步细分种群进行全局搜索,得到了较为准确的结果,但该方法计算代价大、速度慢,无法满足实时性要求；Kristijan等人[5]提出GLASM(genetic look-up based algorithm for scan matching)遗传方法,将参考帧激光栅格化建立查找表,通过查表的方式对种群进行适应度计算,能够快速求解出位姿变换,该方法在精度上有一定损失,且初始位姿误差大时匹配效果差；Feng D J等人[6]提出遗传粒子滤波算法求解高度非线性、非高斯的SLAM扫描匹配问题；陈焕等人[7]提出的遗传迭代最近点扫描匹配算法(generalized ICP,GICP)将改进的迭代最近点(iterative closest point,ICP)关联匹配规则作为适应度计算标准进行求解,能在里程计误差较大或者机器人绑架情况下快速收敛,一定程度上解决了扫描匹配算法中任意的配准问题。总体来说,基于遗传算法的激光扫描匹配算法目前仍存在速度慢、计算代价大、匹配效果不佳等问题。

本文提出一种融合NDT的改进遗传算法,采用正态分布变换作为适应度计算标准,同时将参考帧栅格进一步细分,建立查找表加速求解速度,利用遗传算法不断迭代求解出最优位姿变换。

1 基于NDT算法的激光数据扫描匹配

设参考扫描Sref={p1,p2,…,pn},pi={xi,yi}∈R2是n

典型的NDT算法分为两步：

1)构建NDTcell

将Sref划分为N个cell,统计cellj(j=1,2,…,N)里面的激光点数量,然后对于激光点数n大于3的cellj通过式(1)和式(2)计算其均值qj和方差Σj

(1)

(2)

2)最小化评价函数

(3)

优化函数定义为所有点的得分总和

(4)

NDT采用牛顿法来实现极小化过程,牛顿法通常用Jacobian矩阵和Hessian矩阵代替函数本身和梯度向量。由于不需要对score函数线性化,因此可以直接求解Hessian矩阵。

2 融合NDT的2D激光数据扫描匹配遗传算法

遗传算法借鉴自然选择过程和适者生存原则,具有全局搜索能力。本文提出融合NDT的2D激光数据扫描匹配遗传(genetic algorithms for scanning matching of 2D laser data fused with NDT,NGASM)算法,将NDT方法引入遗传算法作为适应度评价,在全局空间进行搜索,提高了求解精度的同时避免了算法陷入局部极值,同时将NDT 进一步细分建立查找表以避免遗传算法进化时反复计算个体适应度消耗大量时间,加快了求解速度。

2.1 种群编码

将二维平面中机器人的运动用平移和旋转变量表示为T(x,y,θ),初始值由里程计获取,通过在初始值附近的搜索空间内,求取修正值ΔT(Δx,Δy,Δθ)修正误差,采用二进制格雷码编码,编码长度为24,均匀随机产生N个个体样本初始化种群。

2.2 适应度评价

对于如图1(a)所示的参考帧Sref,首先划分cell,统计cell里面的激光点数,然后根据式(1)和式(2)计算对应的均值qj和方差Σj,得到该cell的PDF。进一步,将cell进行栅格细分,构造查找表,如图1(b)所示。表中每个单元表示二维平面上的一个激光扫描点,落到相应的cellj中就用对应的均值qj和方差Σj利用式(3)计算其得分值,查找表的整体尺寸稍大于激光传感器范围以覆盖所有可能的激光点。将Scur变换到Sref坐标系下,生成S′cur,对于S′cur中的每一个点,在查找表中直接查找其得分值,作为该点的适应度值,显然,个体适应度的值score定义为S′cur中每个点在栅格查找表中的得分之和。

本文采用标准NDT即固定大小尺寸进行划分cell,栅格大小取0.5～2 m。为了解决固定尺寸划分激光点过于分散的问题,同时采用了K均值聚类算法对激光点进行聚类划分的方法,然后进行比对分析,其中采用固定尺寸划分cell的NGASM简称为FNGASM,采用K均值聚类划分cell的NGASM简称为KNGASM。如图1(c)和图1(d)所示,分别是固定尺寸划分cell和K均值聚类划分cell的NDT图,图中越亮的地方表示的分值越高。

图1 NDT适应度查找表

当直接将S′cur中的激光点代入式(3)计算时,需要消耗大量时间,而且遗传算法中进行个体适应度计算时每个个体都要进行求解,所以算法的实时性得不到满足。本文提出建立查找表,只需在创建查找表时计算一遍得分值,遗传算法所有个体的得分值直接到表中进行查找,节约了大量时间。地图离散化带来的一个缺点是精度下降,可以通过缩小查找表栅格尺寸来一定程度上抵消这种现象,但随之而来的是创建查找表时间的增加以及内存的增长,所以要在两者之间权衡。

2.3 选择、交叉和变异

选择操作首先求出每个个体适应度占总适应度的比例λi,设立阈值λb,直接淘汰λi<λb的M个个体,然后再在λi>λb的个体中按比例依概率随机复制生成M个个体补足种群。

交叉操作采用均匀交叉,pc是交叉率,随机生成交叉概率pr,如果pr>pc,对选中的父本进行交叉；变异操作直接翻转二进制某一位置值,设定一个极小的变异率pm,以降低突变对种群多样性产生的不良影响。

2.4 终止准则

假设种群的数量是N,对于种群中的每个个体 ,都有对应的里程计数据(xi,yi,θi),通过下面的公式求得所有基因的里程计数据均值(xmean,ymean)以及所有个体的里程计数据的方差

(5)

(6)

当方差cov小于某常量C时 ,算法达到收敛。

融合NDT的2D激光数据扫描匹配遗传算法的流程图如图2所示。

图2 NGASM流程

3 实验结果与分析

为了对NGASM算法扫描匹配效果进行验证,选取标准的DNT算法以及同样创建查找表的GLASM算法进行对比,使用MATLAB软件进行实验。以下实验数据均是在Intel Core—i5—3200M 2.6 GHz双核处理器平台的Windows系统上获取。实验中查找表栅格大小l=0.05 m,固定尺寸的NDT cell的尺寸L=1 m,种群的数量N=50,交叉概率pr=0.9,变异概率pm=0.001。实验环境如图3所示,随机选取了5个位姿。

图3 MATLAB实验环境

3.1 耗时分析

如表1所示,GLASM仅需0.012 s即创建了查找表,FNGASM花费了0.406 s来创建查找表,是GLASM的3.38倍,而KNGASM在创建表格消耗时间就达到秒(s)级,用时2.128 0 s,主要是对激光点进行聚类时不断迭代消耗大量时间。在进行适应度评价时,GLASM和NGASM均直接在查找表中查找,但由于GLASM需要查找每个激光点栅格值以及其四周的栅格值,而NGASM直接查找激光点得分值,NGASM耗费时间比GLASM减少42.8 %。表中完成配准对GLASM和NGASM两种遗传算法来说是进化10次,NDT完成配准仅需1.766 0 s,遗传算法在实时性方面优势不足,GLASM进化10次就耗费了5.092 s,NGASM由于快速查表节省时间,进化10次后两种不同方式划分cell的NGASM就分别比GLASM少耗时1.39,0.721 s,随着进化次数的增加,NGASM相比GLASM在实时性方面优势更加明显。

表1 不同激光扫描匹配所需的时间 s

3.2 精度分析

如图3所示,机器人经过随机生成的5个点,求解位姿,误差结果如表2所示。三种算法都能达到很高的精度,从MDE指标来看,其中GLASM和NDT两种算法精度接近,GLASM的0.029 25小于NDT的0.3142,表明在平移误差方面GLASM的精度要高,NGASM精度最高,其中效果最好的KNGASM比误差最大的NDT精度提高了21.6 %。MRE反映了旋转误差,NGASM同样误差最小,KNGASM比NDT精度提高了27.3 %。综合表2数据,NGASM无论是在平移误差还是在旋转误差方面,均提高了机器人的定位精度,其中KNGASM由于考虑了激光点的分布位置,在精度方面得到了进一步提升。