田联房,陈浩权,杜启亮+,郑灿涛
(1.华南理工大学 自动化科学与工程学院,广东 广州 510640;2.华南理工大学自主系统与网络控制教育部重点实验室,广东 广州 510640)
目前行人检测与跟踪的方案主要有普通摄像头[1]、红外摄像头[2,3]、激光雷达[4-6]等。由于摄像头容易受到光线、视场大小、拍摄角度等影响,只适用于照明稳定,拍摄位置相对固定的应用场景。红外方案工作范围远且不受可见光线影响,即使在较暗,甚至全黑的环境中也可以正常工作,但受技术限制,红外图像存在信噪比低、对比度低、分辨率低以及噪声多等问题,且成本较高,一般只用于光线较暗的场景。因激光雷达具有不受光线影响、精度高、成本低等优点,目前智能服务机器人一般会配备二维的激光雷达。由于只有二维平面信息,且当距离较远时分辨率较低,使得基于二维激光雷达的行人检测难度很高。
为此Leigh A等[5]选择了15种跟人腿相关的特征,使用随机决策森林训练了一个分类器进行分类检测。Lucas Beyer等[6]提出利用卷积神经网络训练分类器来对轮椅以及人腿进行分类。该算法由于不需要人为选择特征,一定程度上能更好地检测行人,但卷积神经网络对运算资源需求极高,不适合需要控制成本的应用场合。
由于二维信息不足,复杂的背景容易对行人检测和跟踪产生干扰,比如靠近墙角的椅子腿,即使以上两种复杂的分类器仍然会误判为人的腿部。为减小复杂背景的干扰,魏丽等[7]通过对激光雷达数据进行鲁棒主成份分析,可以将短时间内连续多个扫描周期的激光雷达数据分解成背景与运动目标。该方法可以很好地应用在走廊等相对空旷的环境,但无法应对行人静止以及环境拥挤的情况。
针对上述基于激光雷达的行人检测、跟踪存在的背景干扰问题,本文提出先构建环境地图,再利用似然域模型的思想来分割当前激光雷达数据帧中前景与背景,然后只对分割出来的前景进行行人检测,并利用改进的卡尔曼滤波器实现移动机器人在复杂背景下对行人的跟踪与跟随。
本文使用的实验平台为一个轮式机器人,如图1所示,左右轮子都带有测距编码器。在距离地面37 cm处安装了一个单线的Flash Lidar F4激光雷达,扫描角度360°,角分辨率0.5°,帧率10FPS。
图1 实验硬件平台
如图2所示,在完成环境地图构建后,先利用蒙特卡洛定位确定机器人位姿。如果定位失败则直接进行数据聚类,否则进行似然域背景差分,只对提取到的前景对应的激光雷达数据进行数据聚类,避免复杂背景对后续行人检测与跟踪的干扰。最后根据一定的规则对行人跟踪器输出的跟踪目标进行判断,如存在目标行人,则让机器人对目标行人进行自动跟随,否则结束进入下一个扫描帧的循环。
图2 基于似然域背景差分的行人检测和跟踪流程
机器人同时定位与建图(simultaneous localization and mapping,SLAM)问题[8,9]旨在实现机器人从未知环境的未知地点出发,在运动过程中定位自身位姿,再根据机器人位姿构建地图,从而达到同时自定位与建图的目的。
GMapping算法具有不依赖激光雷达性能、不需要大量计算资源、建图精确等优点,故本文选用其来构建环境地图。图3为图1实验平台在一个办公室内运行GMapping算法后得到的栅格地图。图中白色部分表示该区域不被占据;黑色部分则表示该区域被障碍占据;灰色则是状态不明,一般是被障碍隔挡无法探测或未建图区域;数字对应背景差分实验位置。
图3 GMapping构建的栅格地图
蒙特卡洛定位[10]是在地图信息已知的情况下,机器人利用里程计信息和激光雷达数据确定自身位姿的一种算法。该算法会先根据一定的均值和方差,呈正态分布地初始化一个粒子群,然后利用里程计数据和机器人的运动模型更新粒子群中所有粒子的位姿,接着通过计算在对应位姿下激光雷达数据与地图的契合程度来获得粒子的重要性权重,最后根据一定的规则进行粒子群重采样,并给出权重最大的位姿作为后验位姿。实际使用中一般会在重采样步骤中增加随机粒子来使机器人从全局定位失效以及陷入局部最优解中恢复。
似然域模型是一种传感器测量模型[10],其考虑了传感器固有的不确定性,并为传感器测量噪声建立模型来获得测量过程的概率。其一般用于在已知地图与测量值时,估计机器人不同位姿的概率。本文基于似然域模型的思想,提出一种在已知地图与机器人位姿时,根据似然度大小进行分割的背景差分方法。
似然域模型定义了一个条件概率分布p(zt|xt,m),其中zt是t时刻的测量,xt=(xyθ)T是机器人的位姿,m是环境地图。而对于激光雷达一类传感器,其测量束不止一个,在满足每个测量束之间噪声独立分布的假设时
(1)
似然域模型简单地将最大距离读数丢弃,将每个非最大距离的测量束的端点坐标通过下面的式子转换到地图坐标系
(2)
式中: (xk,sensyk,sens)T表示激光雷达传感器在机器人坐标系中的位置,θk,sens表示测量束相对于机器人航向的角度。
在得到每个测量束端点的全局坐标后,就可以计算该测量束扫到了地图障碍的似然。在计算似然时,考虑两种噪声和不确定性来源:
(1)测量噪声
找到端点坐标 (xk,sensyk,sens)T到地图m上的障碍的最短的欧氏距离dist,认为该测量束探测到地图障碍的似然服从以0为中心的高斯分布
(3)
(2)无法解释的随机测量
认为该测量束探测到地图障碍的似然服从一个平均分布,其中zmax是最大测量距离
(4)
综合以上两个考虑,可以加权得到对应传感器测量束探测到地图中物体的似然,其中zhit和zrand是权重
(5)
如图4所示,似然度的大小表征着该测量束探测到背景的概率,其与测量束端点到地图障碍的最短距离dist有关。
图4 dist与,m)的关系
同时为了避免机器人定位不准确时造成前景提取错误,首先计算总体测量的似然度p(zt|xt,m),仅当该似然度大于一个固定阈值theta_p,才进行前景提取。否则不提取前景,使用全部激光雷达数据进行行人的检测和跟踪。
虽然人腿相关的可提取特征非常多,但是对多种环境的实验结果表明,仅有少数特征对检测器的贡献较大,使用太多对检测器贡献不大的特征反而会增加过拟合的风险。同时,考虑到二维激光雷达信息的不足,仅识别一只腿容易误检。因此,本文提出一种基于行人双腿模型的行人检测算法。
首先选用层次聚类算法[11]对激光雷达数据进行聚类,然后根据行人腿部特点,将激光点数在5个~30个之间的簇作为行人腿部候选点集。
如图5所示,左右两个点集代表数据聚类之后的两个不同的簇。Dk代表对应簇的首尾元素的距离,Lk代表对应簇的总长度。检测器最终给出行人双腿模型的中间位置作为行人坐标。检测器具体流程如图6所示。
图5 行人双腿模型
图6 行人双腿模型检测流程
由于行人的运动和机器人本身的运动都存在噪声,只依赖行人检测得到的结果难以得到较好的跟随效果。文献[12]提出了一种采用最小欧氏距离的目标匹配方法来对多行人进行卡尔曼跟踪,在对12段手扶电梯出入口视频序列进行跟踪实验后,结果表明,多行人跟踪的总调和均值达到96.1%,能够稳定地跟踪多个行人。二维栅格地图一定程度上跟图片一样,且本文应用场景与手扶电梯相似,背景都相对固定,都存在不确定数量行人的靠近、相互遮挡、远离等,所以本文在文献[12]提出的跟踪算法基础上稍作改进后应用到行人跟踪中。
文献[12]中的跟踪强度公式仅考虑了检测成功与检测失败两种情况,没有考虑目标长时间被遮挡的情况。针对非目标行人在经过机器人旁边只能探测到一条腿以及经过目标行人旁边完全被遮挡,无法检测到的情况,如果直接采用文献[12]中的跟踪强度公式,会使得非目标行人的跟踪强度迅速降低,导致目标跟踪失败。本文在文献[12]提出的多行人卡尔曼跟踪算法的基础上,修改了跟踪强度的计算公式如下
(6)
在非目标行人丢失时,利用最近50次的跟踪信息判断是否出现了上述两种被遮挡情况,如没有出现则将hide置0,认为行人走出检测范围,对目标跟踪强度进行较大幅度的削减;否则置1,认为行人只是被遮挡,缓慢降低跟踪强度等待目标走出遮挡区域。
本文在实验中发现,改进后的算法能够利用跟踪有效减少目标被遮挡情况下的漏检,对于突然闯进、离开都有较好的适应性,能够满足本文的需要。
在本文中,选择出现在机器人正前方30 cm以内的行人作为跟随目标,一直到目标行人对应跟踪器的跟踪强度减小到一定程度或者人为设定放弃跟随。
如图7所示,获得目标行人在全局坐标系下的坐标 (xp,yp) 以及机器人自身的坐标 (xr,yr) 后,在机器人与目标行人的连线上,以距离行人位置0.2 m处为终点,指向行人为目标方向设定机器人的目标位姿 (xg,yg,yawg)
drp=sqrt((xp-xr)2+(yp-yr)2)
(7)
drt=drp-0.2
(8)
xg=xr+drt*((xp-xr)/drp)
(9)
yg=yr+drt*((yp-yr)/drp)
(10)
yawg=arctan((yp-yr),(xp-xr))
(11)
将目标位姿发布到机器人操作系统(robot operating system,ROS),借助ROS的navigation功能完成机器人自动跟随。
图7 跟随目标
基于第1节所介绍的实验平台,本文使用C++语言在ROS操作系统下实现了以上所述基于激光雷达的似然域背景差分行人检测、跟踪与跟随。算法运行在CPU为Intel i7-3610QE的工控机上,在不使用GPU加速的情况下,共统计了380帧正常工作中的有效激光雷达数据,实现了实时跟踪多行人以及目标行人跟随。
如图8所示,机器人分别位于图3中的1-3号位置,图中黑色轮廓为地图原有障碍,障碍上的白色细点为似然域分割出来的激光雷达背景数据,白色较粗且有黑边包裹的点为似然域方法提取出来的激光雷达前景数据,中间黑色的圆为机器人所在位置,并且图中有标出前景中实验者的位置。
图8 似然域背景差分结果
从地图3个位置的实验结果可以看到,如果激光雷达数据比较靠近地图原有障碍,均被表示成背景。只有当激光雷达数据比较远离地图原有障碍,似然度较低时,才会被当成前景。表明本文提出的基于似然域模型的背景差分可以有效地将激光雷达数据中的前景和背景区分开来。
本文利用ROS上公开的人腿检测实现分别对原始激光雷达数据以及利用似然域背景差分提取到的前景进行了行人检测实验。如图9(a)所示,机器人位于位置1处,行人位于一个表面较窄的柜子旁边。图9(b)是在图9(a)情况下直接采用全部激光雷达数据进行检测得到的结果,行人左腿与呈直角型的柜子被误检成一个行人。图9(c)是在图9(a)情况下采用似然域背景差分提取到的前景数据进行检测得到的结果,作为背景的柜子数据不参与检测,行人被正确检测。如图9(d)所示,机器人位于位置2处,一张椅子位于一个办公桌前。图9(e)是在图9(d)情况下采用全部数据进行检测得到的结果,椅子与柜子分别被当成两条腿误检成行人。图9(f)是在图9(d)情况下采用前景数据检测得到,排除了背景的干扰,椅子未被误检。如图9(g)所示,机器人位于位置3处,行人靠近一个柜子的边沿。图9(h)和图9(i)分别是在图9(g)情况下利用全部数据以及利用前景数据检测得到的结果。在利用全部数据时,行人左腿与柜子被误检成一个行人。而在仅利用前景进行检测时,行人被正确检测。
图9 似然域背景差分对复杂背景干扰的抑制
实验结果表明,进行似然域背景差分后,背景部分激光雷达数据不参与后续行人的检测和跟踪,可以一定程度上降低复杂背景对行人检测的干扰。
为了验证本文提出的背景差分与行人检测算法,在图3中的1-3号位置分别进行了单人、双人、拄拐杖单人3项检测,其中双人实验中两个人并排走动,距离不超过50 cm。
对于每项检测,行人在距离激光雷达50 cm到150 cm之间走动,共有3个穿长裤及4个穿短裤的行人参与检测。本文利用采集到的有效帧分别在有无背景分割情况下,利用本文提出的检测算法进行检测,记为方法1;利用ROS上公开的实现进行检测,记为方法2。如图10所示,图10(a)~图10(c)分别为位置1-3的3次实验。3项实验的统计结果见表1,P为准确率(Precision),R为召回率(Recall)。行人在距离激光雷达50 cm到150 cm之间移动,约占激光雷达数据的1/4,考虑到激光雷达数据聚类的连续性,假定每帧数据有4个样本,仅对正样本进行标注,除去正样本数即每个有效帧的负样本数。
图10 不同位置的行人检测结果
表1 行人检测结果
检测结果显示本文提出的检测算法和方法2在有背景分割的情况下准确率比没有背景分割的情况分别提高了3.49%、3.64%,召回率分别提高了0.37%、0.43%,表明对激光雷达数据进行似然域背景差分能够有效降低复杂背景对行人检测的影响。
在同等条件下,本文提出的检测算法比方法2准确率平均提高0.535%,召回率平均提高0.1%,具有更好的检测效果。
如图11所示,黑色五角星和黑色圆形分别标识出了机器人与目标行人的起点和终点,起点和终点间的黑色粗线是机器人的轨迹、白色粗线是目标行人检测与跟踪后得到的坐标。为了检验本文提出的检测算法与跟踪算法以及跟随策略的稳定性,有一非目标行人在目标行人出现后出现,然后与目标行人并行,最后超越目标行人并与目标行人交叉运动。非目标行人的运动轨迹由白色箭头标识出,轨迹上断续的灰色粗线为非目标行人检测得到的坐标。目标行人与非目标行人的步行速度约0.3 m/s。
图11 行人跟踪与跟随图例
由图11可以看到,非目标行人与机器人并行时只能检测到一条腿,以及相交时被目标行人遮挡,会导致非目标行人检测失败,轨迹出现断续,但本文改进的跟踪器很好地对非目标行人进行了跟踪,始终没有丢失该非目标行人。目标行人的白色轨迹线较为连续平滑,且与代表非目标行人的灰色轨迹线相交时也没有出现错误,表明本文提出的行人检测与跟踪算法能够有效地对多行人进行检测与跟踪。机器人与目标行人轨迹基本重合,表明机器人能够较好地跟随目标行人。
跟踪过程共采集到380帧有效激光雷达数据,机器人移动速度如图12所示,机器人与目标行人间距如图13所示。在第180帧附近,目标行人与非目标行人交叉出现,且正在转弯,导致目标行人与机器人距离小于设定安全距离0.6 m,机器人停止移动。在第0帧到第150帧以及第230帧到第380帧期间,机器人与行人的间距平稳,机器人速度在短暂停顿重新规划路径后也能快速启动到平稳的速度。表明本文提出的跟随策略能使机器人对目标行人进行平稳跟随。
图12 机器人移动速度
图13 机器人与目标行人间距
在有背景分割情况下,对380帧有效数据分别使用方法1和方法2进行了行人检测实验。检测时间如图14所示,其中方法1的平均检测时间为3.426 ms,方法2的平均检测时间为5.014 ms。表明本文提出的行人检测算法能有效降低行人检测时间。
图14 两种方法的行人检测时间
针对基于激光雷达的移动机器人,本文提出一种先对环境建图,再利用似然域模型提取前景数据来降低复杂背景干扰的行人检测、跟踪、跟随方法。实验结果表明,本方案能够有效提取激光雷达的前景数据,一定程度上降低了复杂背景对行人检测、跟踪的干扰。对于前景数据选用层次聚类算法进行聚类后,采用本文提出的行人检测方法进行多行人的检测,然后利用本文改进的跟踪算法对多行人进行有效跟踪,最后采用本文提出的自动跟随策略实现了移动机器人在已知地图环境下对目标行人的有效跟随。整个系统实时性高,不受复杂背景干扰,但对于背景变化较快场合的适应性有待进一步研究。