基于激光雷达回波强度的反光板位置拟合方法

方德浩 许 万 陈幼平 余磊涛 朱 力

1. 湖北工业大学机械工程学院，武汉，430212

2. 华中科技大学机械科学与工程学院，武汉，430074

0 引言

自动导引车(automated guided vehicle, AGV)是企业物流系统中的重要设备。AGV系统中的定位即确定AGV在给定地图环境中的全局坐标系位姿。在解决该问题时[1]，2D激光雷达因其高精度而常作为主要传感器。以激光雷达作为传感器的定位方式主要可分为基于特征的地图定位[2-3]、基于栅格的地图定位[4-5]。基于反光板的AGV定位属于特征地图定位[6]。在以反光板为地图特征的定位系统中，对反光板进行位置拟合为该定位系统的重要环节，对定位的稳定性、精度起到至关重要的作用。

反光板位置拟合主要分两个步骤：①对原始扫描数据的分割；②对分割后点集的拟合。在对原始数据进行分割的问题上，DIETMAYER等[7]提出的DIET算法通过设置两点间距离的阈值来实现原始扫描数据点的分割；ZHOU等[8]利用改进的基于点距离的分割(point-distance-based segmentation，PDBS) 法，使跨越数据头与数据尾的环境特征在分割后保持为同一特征；PREMEBIDA等[9]提出的自适应断点检测 (adaptive breakpoint detector，ABD)算法通过设置距离及角度的阈值进行数据分割；梁雄等[10]采用改进ABD算法对原ABD算法进行二次检测；BORGES 等[11]利用基于卡尔曼断点检测器进行断点检测；ZHAO等[12]利用基于预测的特征提取 (prediction-based feature extraction，PFE)，通过预测的方式进行数据分割；吴波[13]利用截断阈值法对扫描数据进行分割。对于分割后点集的拟合问题，刘之舟[14]利用回波强度重心法进行位置拟合；祖爽等[15]利用平均值法进行位置拟合；YUEN等[16]利用霍夫变换提取拟合位置；CHAUDHURI[17]利用最小二乘法 (least squares method，LSM)获取拟合圆心位置；刘珂等[18]利用多约束下最小二乘法实现分割后点集位置的拟合。上述分割算法、拟合算法都能够达到较好的效果，但分割算法在面对实际环境中产生的各种错误数据时大多会产生失效拟合，且拟合精度较低，难以满足定位算法对反光板位置拟合精度的需要。

随着传感器技术发展，激光雷达利用回波波形积分得到回波强度[19-20]，利用该值来辅助反光板进行位置拟合可提升拟合效果。笔者提出一种基于回波强度的反光板位置拟合方法，以圆柱形反光板为地图特征，首先对原始数据进行滤波处理，结合高反光材料产生的较大回波强度[21]与反光板几何特征进行数据分割，并结合回波强度对分割后数据进行反光板圆心拟合。

1 基本原理

1.1 基于反光板的激光定位系统

在基于反光板的激光雷达定位系统中，需要预先根据环境中布置的反光板搭建定位地图。图1所示为定位算法流程，其中，扫描数据分割及分割后数据拟合属于反光板位置拟合，其主要作用是将原始扫描数据转化为反光板极坐标。反光板位置匹配是将反光板极坐标与定位地图中反光板数据相匹配[22-23]，最后通过卡尔曼滤波、粒子滤波等算法进行激光雷达位姿计算[24-25]。

图1 基于反光板的2D激光雷达定位流程

1.2 回波强度特性分析

激光雷达发射激光信号到目标，经目标散射后再传回。激光雷达根据飞行时间 (time of flight，TOF)测量该目标的距离，同时根据接收信号计算回波强度。回波强度的计算公式为[19]

(1)

其中，Ei为激光雷达发射信号强度；ηsys为系统参数；ηatm为大气影响因子；Dr为激光雷达接收孔径；f(θi,φi,θs,φs)为二向性反射率分布函数(bidirectional reflectance distribution function，BRDF)；θi为激光入射角；φi为入射方位角；θs为激光出射角，激光雷达中θi=θs；φs为后向散射方位角；Rd为目标与激光雷达系统之间的距离。实验中，实验材料系数

(2)

因式(2)等号右侧的各个物理量保持不变，故式(1)可化简为

(3)

(1)从实验数据中提取距激光雷达约8.3 m处反光板上的所有扫描点。因扫描点均匀分布于反光板表面，故可用序号代替入射角度，绘制扫描点回波强度曲线图(图2)。

图2 反光板扫描点回波强度曲线

(2)从实验数据中提取不同反光板的点集，以各点集上最大回波强度点为垂直入射点(入射角θi=θs=0°)，绘制各垂直入射点的距离-回波强度曲线(图3)。

图3 垂直入射点的回波强度-距离曲线

同时，分别使用余弦函数、幂函数对实验(1)、(2)中的实际曲线进行拟合，得到图2、图3中的拟合曲线，可以看出，扫描点回波强度变化符合式(3)的规律。

2 基于回波强度的反光板位置拟合

2.1 扫描数据分割

2.1.1扫描数据预处理

首先对激光雷达原始扫描数据进行预处理，从激光雷达中获取的原始扫描数据为

D′scan={P′i}i=1，2，…，7200

(4)

P′i=(ρ′i,θ′i,E′s,i)

(5)

其中，D′scan为包含N(N=7200)个扫描点的一帧原始扫描数据。扫描点P′i由扫描距离ρ′i、扫描角度θ′i和回波强度E′s,i表示。反光板位置拟合前，需要针对下列情况进行滤波预处理：①若原始扫描数据中的某些孤立点与其相邻点的距离较大，则这些点被认为是测量噪声点。②当扫描点实际测量距离大于激光雷达最大测量距离时，激光雷达距离测量值设为错误值-1，这些扫描点将不会被使用。

针对上述情况，设计如下预处理算法。

输入: 原始扫描数据D′scan={P′i}

初始化：N=7200，最大允许突变量ρerror=0.3，k=1

fori=1 toNdo

ifP′i为唯一孤立点//P′i与周边点的距离均大于ρerror

elseP′i为非唯一孤立点//P′i与相邻点的距离小于ρerror即(|ρi-ρi-1|≤ρerror‖|ρi-ρi+1|≤ρerror)&&|ρi-ρi±1|≥ρerror

elseP′i为连续点//不做处理；

end for

输出: 预处理完成后的扫描数据Dscan={Pi}

使用滤波算法对原始扫描数据进行预处理前后的结果如图4所示。预处理不仅舍弃了超过激光雷达使用距离的扫描点，且使受测量噪声影响的部分扫描点得到修正。

2.1.2扫描数据分割算法

预处理后，需要对扫描数据进行分割，以得到不同反光板对应的点集。因周边环境与反光板的k′存在较大差异，故可通过式(1)得到的回波强度区分反光板与周边环境。由于测量时环境噪声的影响，若仅采用回波强度进行分割预处理，则扫描数据会错误地将一个反光板点集分割为多个反光板点集，对后续算法产生较大影响。因此本文在将回波强度作为扫描数据分割依据的基础上，把PDBS算法作为辅助分割判据，从而有效避免因错误数据造成的拟合失效。

(a) 预处理前

首先针对不同距离下的反光板进行扫描实验，选取反光板点集中的边缘点作为该反光板的截断阈值点，得到图5。

图5 边缘点距离-回波强度曲线

从图5可以看出，随着距离增大，边缘点回波强度变化较大，单一截断阈值难以满足要求，故需设置阈值截断函数对各距离下的截断阈值进行选取。以回波强度为应变量，距离作为自变量，使用MATLAB工具进行拟合，得到回波强度拟合曲线：

(6)

式中，a、b为拟合函数参数。

将回波强度拟合曲线沿Y轴向下平移c个单位得到截断函数曲线，其方程为

(7)

将扫描点的距离带入上述截断函数求得当前距离的回波强度截断值，若扫描点回波强度大于该截断值，则表示该点为反光板点。当受环境因素影响，导致部分扫描点回波强度小于该点截断值时，反光板点集中会产生环境点，出现点集内部分割，导致拟合失效。因此本文融合回波强度分割法与PDBS算法进行数据分割，该融合分割算法伪代码如下。

输入: 预处理完成后的扫描数据Dscan={Pi}

初始化：N=7200，e=0，当前待分割点集队列Gm，当前分割完成反光板队列Gall

forj= 1 toNdo

if (Es,j

then Break;

end for

fori=jtoNdo

ifEs,i>Es(ρi)&&Gm≠∅

thenPstart=Pi并且将Pi置于队列Gm中，令e=0

elseEs,i>Es(ρi) &&D(Pi,Pstart)≤2R//D为点Pi、Pstart间的欧氏距离，R为反光板半径

then 将Pi置于队列Gm中，令e=0

else thene=e+1

ife≥3

then 将队列Gm放入Gall中，并令i=0,Gm≠∅;

end for

fori=1 tojdo 执行上述算法的6～18行

输出:当前分割完成反光板队列Gall={G1,G2,…,GN}

上述伪代码1～5行的主要作用是使一帧数据以小回波强度点为起始点，从而保证跨越数据头与数据尾的特征在分割后仍为同一特征。融合分割算法的主要目的是将属于反光板的扫描数据点提取出来，并分割成多组数据集Gall={G1,G2, …,GN}，其中，Gm={P1,P2, …,Pi} (m=1，2，…，N)为属于某一反光板的扫描点集合。

2.2 圆柱形反光板拟合

图6 激光雷达扫描点分布示意图

执行融合分割算法后，需要对Gall中的点集Gm进行拟合计算。图6为激光雷达扫描圆柱形反光板的示意图，其中，O1O为实际距离，Pi为扫描点。通常，Gm中的扫描点在圆柱形反光板上呈现沿O1O的非对称分布。这会导致在不同观测位姿时，反光板拟合位置发生变化，AGV动态运行状态下的拟合精度难以保证。

图7 激光雷达实际扫描点分布图

为避免上述情况，根据图2中反光板内部点回波强度变化曲线，本文提出一种结合回波强度的拟合算法。以回波强度为权值，对反光板点集Gm中各点的角度进行加权来减小因扫描点Pi在圆柱形反光板上分布不均而造成的拟合误差。设Pf为反光板拟合位置，ρf为拟合距离(极径)，θf为拟合角度(极角)，则有Pf=(ρf,θf)。

3 实验与分析

3.1 实验平台

本实验中激光雷达为倍加福公司生产的R2000HD，其有效输出距离为0.1～30 m，输出回波强度为32～4095；实验中使用的反光板材料为3M公司生产的V级白色钻石级反光膜。该反光材料为全棱镜结构，理论反射率达100%；移动平台使用的是江淮重工三自由度堆垛式叉车。

实验环境为AGV工厂测试环境，实验环境四周墙壁为彩钢瓦，在墙壁上安装半径55 mm的圆形反光板，实验地面为平坦水泥地面。实验场景、叉车尺寸以及激光雷达安装位置如图8所示。

(a) 实验场景

3.2 实验与结果分析

本文设计的反光板位置拟合方法主要用于AGV激光定位系统，作为激光定位的重要步骤之一，该方法的拟合精度及数据容错能力显得尤为重要。为了验证本文算法的数据容错能力及拟合精度，设计了以下实验。

3.2.1数据容错能力实验

表1所示的原始扫描数据包含错误的数据帧，第120号数据的回波强度显著小于其周围扫描点回波强度，疑似为错误数据。

为验证本文融合分割算法的数据容错能力，使用基于回波强度的分割法与本文融合分割算法对表1中的数据进行分割处理，得到图9、图10，其中不同颜色的点来自不同的拟合点集。

表1 错误数据帧部分信息

图9 基于回波强度的分割法效果图

图10 融合分割法效果图

对比图9、图10可知，使用基于回波强度的分割法会在错误数据(120号数据)处将表1中的数据分割为两组点集Gm、Gm+1，因点集数量等于后续将拟合的反光板数量，故该方法将会在这个位置的一个反光板拟合“两个”反光板，对后续算法产生巨大影响。融合分割算法加入PDBS分割判据，保证了待分割点集Gm前后的连续性和该位置反光板的唯一性，提高了数据容错能力。图11所示为其余错误数据帧的拟合情况。

(a) 错误数据帧一

3.2.2静态拟合精度实验

为验证本文拟合算法的静态精度，在采集的原始扫描数据中，取距激光雷达8389.1 mm的反光板扫描数据为样本数据。分别使用霍夫变换法、最小二乘法(使用ceres库求解)、本文算法进行拟合计算，结果如图12、图13所示。表2所示为不同算法的距离标准差σρ、角度标准差σθ、距离极差Rρ、角度极差Rθ。由图12、图13以及表2可看出，本文算法拟合精度高、波动幅度小、稳定性好。

图12 拟合算法距离误差对比图

图13 拟合算法角度误差对比图

表2 拟合算法精度对比表

为获得各距离下本文算法的静态拟合精度，在采集的原始扫描数据中选取不同距离的反光板进行多组重复试验，记录其距离和角度的误差，如图14、图15所示。选用距离误差、角度误差的标准差为精度标准，表3为多组不同距离反光板的拟合精度。从图14、图15和表3可以看出，随距离的增大，拟合精度并没有明显下降，本文方法具有波动幅度小、稳定性好等特性。

表3 不同距离下的拟合精度

3.2.3动态拟合精度实验

AGV通常处于动态运行状态，为验证本文算法在动态运行下的拟合精度，使AGV从(0,0)点沿x轴正向117.4392°行驶8.9 m，车速为AGV正常行驶车速0.5 m/s，环境中两反光板直角坐标分别为(-1.5818 m,0.5524 m)、(-0.6693 m,8.7974 m)，通过两反光板的拟合数据推算得到反光板的间距(以此来表示动态拟合精度)。绘制间距随扫描帧数变化的曲线，如图16所示，其中，两反光板的实际间距为9.39 m。

图14 不同距离反光板角度拟合误差图

图15 不同距离反光板距离拟合误差图

图16 两反光板间距离曲线

从350帧实验数据可得两反光板之间距离误差的标准差为2.9 mm，极差为15.1 mm。动态拟合精度相对于静态拟合精度并未明显降低，观测位姿变化造成的实际拟合位置变化较小，故本文算法在动态运行环境下也具有较高的拟合精度。

4 结论

不同算法对相同错误数据处理效果的对比证明了本文算法具有一定的容错能力；使用不同算法分别对原始扫描数据进行拟合，证明了本文算法较高的拟合精度；对动态运行下的拟合精度统计，证明了本算法可以在AGV各种运行状态下均保持较高的拟合精度。综上所述，本文提出的算法可以满足基于反光板的激光雷达定位系统中对反光板位置拟合要求，具有较高的实用性。