基于3D激光雷达的无人水面艇海上目标检测

李小毛,张鑫,王文涛,瞿栋,祝川

(上海大学机电工程与自动化学院,上海 200072)

基于3D激光雷达的无人水面艇海上目标检测

李小毛,张鑫,王文涛,瞿栋,祝川

(上海大学机电工程与自动化学院,上海 200072)

无人水面艇(unmanned surface vehicle,USV)实现自主导航避障需要实时感知周围的环境信息,检测出威胁无人水面艇航行的障碍物,而3D激光雷达在无人系统障碍检测中起重要作用.提出了一种基于3D激光雷达的障碍检测算法,即将一个周期内的3D激光点云投影到2.5D栅格地图中,对障碍物进行聚类分割；对栅格中的原始点云数据进行特征提取,表示为椭圆障碍物.而多帧激光数据采用最近邻数据关联识别出其中的动态障碍物,并用卡尔曼滤波实时跟踪.基于电子海图的仿真激光数据验证了该方法在无人水面艇障碍检测中的有效性.

无人水面艇;激光雷达;障碍检测;栅格地图;椭圆表示

无人水面艇(unmanned surface vehicle,USV)是一种智能化的小型水面平台,可以代替有人水面艇在特定水域完成一些繁琐的、复杂的甚至危险的任务.如果要在多变的、复杂的海况下安全自主地完成各项任务,则无人水面艇必须具备良好的环境感知能力,其中准确稳定的障碍物检测能力是无人水面艇实现自主导航避障的前提和关键技术.

无人水面艇障碍物检测法按所用传感器的不同可以分为被动检测法和主动检测法[1].被动检测法是指利用单目视觉或立体视觉传感器感知环境信息的方法,这种方法是文献[2-5]中最常见的障碍物检测方法,其主要特点是可以获得丰富的颜色特征,通常是利用边缘检测算法提取出海天交界线,然后在海天交界线下方搜索感兴趣的目标特征.但是,单目视觉检测法往往难以获得目标精确的深度信息,且图像极易受光照变化的影响;而立体视觉检测法数据量较大,较难实现实时处理的需求.主动检测法是利用激光雷达、海事雷达等距离传感器检测无人水面艇周围的障碍物.海事雷达主要用于远距离的海上目标检测与跟踪,但是通常只能提供目标的位置和速度信息,无法识别出目标的形状特征,且雷达在近距离范围内存在一定盲区.Almeida等[6]、Blaich等[7]和Hermann等[8]分别提出了基于雷达的障碍检测方法,较好地实现了无人水面艇远距离的目标检测.

激光雷达根据其扫描激光束的数量分为2D激光雷达和3D激光雷达.由于测量精度高,抗干扰能力强,故激光雷达非常适用于无人水面艇中远距离的障碍检测.Peng等[9]提出了一种基于2D激光雷达的障碍检测算法(障碍物表示为圆形),由于传感器的限制,其检测距离只有50 m,且在无人水面艇剧烈晃动时会出现丢失目标的情况.

使用3D激光雷达可以较好地解决上述问题.3D激光雷达检测距离远,水平、垂直视场大,为此被广泛应用于无人水面艇、无人车等无人平台的目标检测和环境感知中.3D激光雷达的数据表示方法有点云表示法、基于特征的表示法和基于栅格的表示法[10],其中基于栅格的表示法将空间离散成较小的栅格单元(cell),每个栅格中存储了相应位置的传感器数据.该方法计算效率高,易于实施且不依赖于预先定义的特征,故非常适用于对实时性要求较高的无人系统.基于栅格的表示法按照栅格划分的维度又可以分为2D栅格地图、2.5D栅格地图和3D体素栅格,其中2.5D栅格地图是2D栅格地图在第3维度上的扩展,将激光点云数据投影到栅格中后,只保留其最大最小高度值表示栅格的高度,这样相当于对3D点云数据作了降维处理,这种栅格地图可以较好地处理悬挂结构.

本工作提出了一种基于3D激光雷达和2.5D栅格地图的障碍检测与跟踪方法,本方法不同于常规的栅格表示法,其主要表现为在障碍物的特征提取和动态障碍物判定时,不直接对栅格进行处理,而是提取出每个栅格中保存的原始激光点云,根据海上目标的形状特点表示为椭圆障碍物,并对椭圆特征进行最近邻关联和卡尔曼滤波跟踪.本方法在基于电子海图的仿真界面中取得了较好的效果.

1 传感器配置

本工作使用的激光雷达是Velodyne 16线激光雷达(见图1).该激光雷达不同于同种类型的32线或64线激光雷达,是一款混合固态激光雷达(其旋转部件在雷达内部),受外界环境干扰小,非常适用于恶劣的海上环境.

图1 Velodyne 16线激光雷达Fig.1 Velodyne 16-layers Lidar

Velodyne 16线激光雷达是一个360°全方位激光雷达,其有效探测距离为100 m,垂直视场为30°,在垂直方向共有16条扫描线,每秒可以获取30万个激光点云数据.除了能获得目标的距离方位信息外,该雷达还可以获得目标的反射强度信息,这些信息对于杂波的滤除和目标的分类具有重要作用.Velodyne 16线激光雷达的详细技术参数如表1所示.

表1 Velodyne 16线激光雷达技术参数Table 1 Specifcation of Velodyne 16-layers Lidar

值得一提的是,为了获得360°全方位的目标信息,最好将Velodyne 16线激光雷达安装在无人水面艇的最高处,且在使用激光点云数据时首先将其从激光坐标系转换到船体坐标系,具体的转换关系根据激光雷达的实际安装位置而定.

2 激光雷达数据预处理

2.1 坐标转换和无效点云滤除

3D激光雷达的点云数据大多是以球坐标形式(ρ,α,ω)存储的.为了方便起见,首先将点云转换为3D笛卡尔坐标系形式(x,y,z).坐标转换所用的几何关系如下:

式中,ρ,α,ω分别为球坐标中的球径、方位角和高度角.

平静的水面几乎不会产生激光回波[11],但激光点云中仍然有很多无效数据,这些无效数据主要有无人水面艇本身反射的激光点、有效范围外的点和环境中返回的杂波点,如海浪的激光回波等.对于第一类无效点云,可以指定一个激光点云的有效范围,将该范围外的点云滤除.如距离激光雷达较近的点很可能是无人水面艇本身的反射回波,而激光有效检测范围外的点往往不可靠,故可以事先滤除掉;对于第二类无效点云,其反射强度通常较低,根据这一条件也即可滤除.

2.2 点云畸变校准

无人水面艇的运动和激光雷达的扫描延时会导致产生的激光点云发生一定程度的畸变[12],利用惯导数据可以较好地解决激光点云的畸变问题.

假设激光雷达的旋转频率为f,则可以计算出激光雷达旋转一定角度ϕ所用的时间为

然后从惯导数据中得到当前无人水面艇的横滚角α、俯仰角β和偏航角γ.为了校准每个激光点的位置,需要得到t时刻无人水面艇的平移矩阵和旋转矩阵.由于激光扫描频率很快,因此t

时刻无人水面艇的横滚角α(t)、俯仰角β(t)和偏航角γ(t)可以通过线性插值估计出来:

从而可得到旋转矩阵˜R(t).进一步地,为获得平移矩阵,假设无人水面艇沿全局坐标系的x轴方向运动,则x方向的基向量为

无人水面艇沿x方向的速度可以表示为

对速度进行积分,得到无人水面艇的航迹矢量x(t):

将初始条件代入式(6)可以求得平移矩阵T(t).最终根据点云的平移矩阵和旋转矩阵求得修正后的每个激光点的位置,即式中,pi表示第i个激光点的位置,p′i表示校正后激光点的位置.

3 2.5D栅格表示法

3.1 栅格投影

2.5D栅格地图又称为高度图[13],是无人车领域比较成熟的3D激光传感器栅格表示法[14].本工作的栅格表示基于该方法进行了一些改进.

首先,根据激光雷达的检测范围建立2D栅格地图,无人水面艇位于栅格地图的原点(见图2),其中R为激光的检测范围.栅格的尺寸选取不同于常用的栅格表示法,而是将无人水面艇可通行宽度w表示为栅格的边长,这样可以保证障碍物不会被过度分割.

接着,对一个周期内的激光点云进行遍历,根据每个点的坐标将其投影到相对应的栅格中,栅格中的点云计数器加1.3D激光点存于栅格中的双向链式数据结构中,该结构记录所有点云的最小高度.

可以将栅格按其点云数据的属性分为障碍栅格和非障碍栅格,判断为障碍栅格的条件有:①栅格中的激光点数不小于2;②所有点云的最小高度小于无人水面艇的可通行高度.前者是为了滤除点云中的孤立点,后者则是为了避免悬挂结构(如桥梁等)对无人水面艇行驶的影响.

图2 2D栅格地图Fig.2 2D grid map

3.2 障碍栅格聚类

对于障碍栅格聚类即障碍物点云的聚类,本工作采用的是图像处理领域的二值连通域区域标记算法,其本质是一种基于距离的聚类算法.

首先遍历栅格地图,找到第一个未标记的障碍栅格,将其标记为O1,接着在该障碍栅格的8邻域内寻找其他的障碍栅格,同样标记为O1,反复标记障碍栅格的邻域,直到该连通域内所有障碍栅格都被成功标记为止,记录该连通域内所有障碍栅格的位置.接着重复上述操作找到栅格地图中其余连通域,并标记.最终得到若干个连通区域即栅格地图中的若干个障碍物,这时可以将同一连通域中每个障碍栅格存储的激光点云合并,得到同一障碍物的所有激光点云.

为了加快连通域标记算法的速度,本工作采用了文献[15]中提出的一种快速二值图像连通域区域标记算法,该算法综合了线标记法和区域增长法的优点,即只需对二值图像扫描就可以依次标记出所有的连通区域,能大幅提高算法的计算效率.图3为障碍栅格聚类的结果,其中黑色栅格为障碍栅格,白色栅格为非障碍栅格,栅格中的数字为障碍栅格所属的连通域的标号,即栅格地图中障碍物的序号.

图3 障碍栅格聚类结果Fig.3 Clustering results of obstacle grid

4 椭圆特征提取

通常,无人车领域激光目标用一个封闭盒子[16]表示,考虑到海上的船只等障碍物的形状类似于椭圆形,故本工作考虑将激光检测到的障碍物用椭圆表示,并提取椭圆的特征参数.

为了得到椭圆障碍物的方向,本工作采用了一种主成分分析(principal component analysis,PCA)法,其本质是一种降维思路,该方法可以将m个变量转化为n个主成分(m≥n),这些主成分能够最大程度地反映出原始变量的信息,且每个主成分都是原始变量的线性组合.对于2D平面上的激光点云来说,其几何解释就是计算这些点云的主要和次要特征方向向量,即椭圆的长轴和短轴方向向量.

计算激光点云主成分的步骤如下.

(1)从3.2节所描述的连通域中取出所有的激光点云,记Xi和Yi分别为第i个激光点的x,y坐标.

(2)计算Xi和Yi的协方差矩阵.

(3)计算协方差矩阵的特征向量和特征值.

计算协方差矩阵的特征值λ1,λ2和对应的特征向量ζ1,ζ2,其中较大的特征值对应的特征向量为椭圆长轴方向向量,较小的特征值对应的特征向量为椭圆短轴方向向量(见图4).

图4 点云数据主成分Fig.4 Principal components of point cloud

得到点云的主成分方向向量后,将所有数据在这两个方向投影,得到边界点,可确定椭圆长轴的长度和短轴的长度,即能确定椭圆障碍物的唯一位置和形状.椭圆障碍物的特征参数主要有中心点位置、短半轴长度、长半轴长度和长轴方向等.对所有连通域内的激光点云进行上述相同的操作,便可以将栅格地图上所有的障碍物表示成椭圆,并得到其特征参数.

5 动态障碍物跟踪

针对单帧激光点云数据的处理,只能得到静态场景下某一时刻无人水面艇周围的障碍物.为了满足海上动态环境需求,需要关联多帧激光数据,提取出周围的动态障碍物信息,并对这些动态障碍物的位置和速度进行预测和跟踪.

考虑到海上目标较少,且分布比较分散,故本工作采用最近邻数据关联方法关联多帧激光数据中的障碍物,并用经典卡尔曼滤波预测动态目标的位置和速度.

最近邻数据关联算法是最简单且易实现的数据关联方法,其基本思想[17]是首先以量测值的预测位置为中心建立相关波门,如果落入相关波门内的量测值只有1个,则该值即为匹配值;如果有多个量测值落入相关波门内,此时则需要统计距离最小的量测值并将其作为匹配值:

式中,z为(k+1)时刻的量测值,︿z(k+1|k)为(k+1)时刻量测的预测值,S−1(k+1)为信息协方差.

利用最近邻关联和卡尔曼滤波跟踪动态障碍物的步骤如下.建立一个动态障碍物列表,将k时刻的所有障碍物加入该列表,每个障碍物有一个存在置信度.利用最近邻数据关联方法关联第(k+1)时刻的障碍物,关联结果有3种情况[18]:①障碍物在动态障碍物列表中,但(k+1)时刻没有找到匹配目标的障碍物,将其存在置信度减1;②在(k+1)时刻新出现而在动态障碍物列表中不存在的障碍物,将其加入到动态障碍物列表,存在置信度赋初值;③(k+1)时刻出现的障碍物与动态障碍物列表中的障碍物成功匹配,将其存在置信度加1,并利用卡尔曼滤波预测其位置和速度.另外,将速度小于1 m/s的障碍物视为静态障碍物,移出动态障碍物列表;当动态障碍物列表中某一障碍物的存在置信度小于特定值时,判定该障碍物已经消失,将其从动态障碍物列表中移除.

6 实验结果

6.1 实验设置

本算法的实验验证是在基于电子海图的仿真程序中进行的.本仿真程序可以模拟海上场景,为无人水面艇规划路径,并在无人水面艇周围设置若干个模拟障碍物,其中障碍物以球形表示.

图5 仿真程序中障碍物的设置Fig.5 Setup of obstacles in simulation program

栅格地图的范围设置为X(−100,100),Y(−100,100),其中无人水面艇的位置设置在栅格地图的中心点.栅格大小设置为5 m×5 m,激光数据的采集频率为5 Hz.

为了验证障碍物检测法在不同环境下的检测和跟踪效果,本工作建立了两个不同的场景:单个运动障碍物沿直线运动的场景和多个障碍物的场景(其中一部分是静态障碍物,其余是动态障碍物,动态障碍物沿任意曲线运动).

6.2 实验结果

图6为设置单个动态障碍物的检测和跟踪结果,图中绿色圆形表示激光的检测范围,其中障碍物的半径设置为6 m,速度为2 m/s,沿水平方向向右运动.由图6可以看出,障碍物检测法可以有效地检测出障碍物的位置,不会出现丢帧现象.障碍物中心的检测位置和实际位置始终有一个固定偏差(见图7),该偏差大约在3 m左右,这是由于激光雷达只能检测到物体局部表面,该误差可能根据障碍物的形状、方位的不同而发生改变,但是对无人水面艇的导航避障结果影响很小.

图6 单个障碍物检测和跟踪结果Fig.6 Detection and tracking result of single obstalce

图7 动态障碍物检测距离误差Fig.7 Detection distance errors of dynamic obstacles

图8为设置多个障碍物的检测和跟踪结果,图中设置两个静态障碍物和一个动态障碍物.两个静态障碍物的半径分别为10和6 m,动态障碍物的半径为7 m,沿任意曲线运动,速度为5 m/s.由图8可以看出,本算法可以有效地检测和跟踪随机动态目标.由于激光雷达检测的局部性,静止的障碍物在检测结果中也有运动的趋势,但是其位置变化很小,且速度通常会小于

设定的阈值,故可根据上述方法将其从动态障碍物列表中有效剔除.

图8 多个障碍物检测和跟踪结果Fig.8Detection and tracking results of multi-obstalces

7 结束语

本工作以无人水面艇的环境感知为背景,提出了一种基于多线激光雷达和2.5D栅格地图的海上目标检测算法.本算法利用3D激光点云数据可以较大地提高目标的检测效率,同时将点云数据的预处理投影到2D空间进行,可以有效地减少计算时间,提高算法的实时性.此外,对障碍物实施数据关联和卡尔曼滤波可以准确地获得障碍物的位置、速度大小和运动方向,为无人水面艇的自主导航、自主避障提供了数据支持.本算法在基于电子海图的仿真程序中显示出较好的效果.同时,本算法拟应用在上海大学“精海5号”无人水面艇上.

[1]LIu Z,ZHANG Y,Yu X,et al.Unmanned surface vehicles:an overview of developments and challenges[J].Annual Reviews in Control,2016,41:71-93.

[2]WANG H,WEI Z,WANG S,et al.Real-time obstacle detection for unmanned surface vehicle[C]//IEEE Defense Science Research Conference and Expo.2011:1-4.

[3]WANG H,MOu X,MOu W,et al.Vision based long range object detection and tracking for unmanned surface vehicle[C]//IEEE International Conference on Cybernetics and Intelligent Systems.2015:101-105.

[4]LI C,CAO Z,XIAO Y,et al.Fast object detection from unmanned surface vehicles via objectness and saliency[C]//IEEE Chinese Automation Congress.2015:500-505.

[5]KRIsTAN M,PERˇs J,STLIˇC V,et al.A graphical model for rapid obstacle image-map estimation from unmanned surface vehicles[C]//ACCV.2014:391-406.

[6]ALMEIDA C,FRANCO T,FERREIRA H,et al.Radar based collision detection developments on USV ROAZⅡ[C]//Oceans.2009:1-6.

[7]BLAICH M,SCHusTER M,REuTER J.Collision avoidance for vessels using a low-cost radar sensor[C]//Proc of the Ifac World Congress.2014:9673-9678.

[8]HERMANN D,GALEAZZI R,ANDERsEN J C,et al.Smart sensor based obstacle detection for highspeed unmanned surface vehicle[C]//Ifac Conference on Manoeuvring and Control of Marine Craft.2015:190-197.

[9]PENG Y,Qu D,ZHONG Y,et al.The obstacle detection and obstacle avoidance algorithm based on 2-D lidar[C]//IEEE International Conference on Information and Automation.2015:1648-1653.

[10]AsVADI A,PREMEBIDA C,PEIXOTO P,et al.3D lidar-based static and moving obstacle detection in driving environments[J].Robotics&Autonomous Systems,2016,83(S1):299-311.

[11]HALTERMAN R,BRuCH M.Velodyne hdl-64e lidar for unmanned surface vehicle obstacle detection[C]//SPIE Defense,Security,and Sensing.International Society for Optics and Photonics, 2010:76920D.

[12]HIMMELsBACH M,M¨uLLER A,LuETTEL T,et al.LIDAR-based 3D object perception[C]// Proceedings of 1st International Workshop on Cognition for Technical Systems.2008:1.

[13]DOuILLARD B,UNDERWOOD J,MELKuMYAN N,et al.Hybrid elevation maps:3D surface models for segmentation[C]//2010 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).2010:1532-1538.

[14]AsVADI A,PEIXOTO P,NuNEs U.Detection and tracking of moving objects using 2.5D motion grids[C]//IEEE International Conference on Intelligent Transportation Systems.2015:788-793.

[15]高红波,王卫星.一种二值图像连通区域标记的新算法[J].计算机应用,2007,27(11):2776-2777.

[16]CHO H,SEO Y W,KuMAR B V K V,et al.A multi-sensor fusion system for moving object detection and tracking in urban driving environments[C]//IEEE International Conference on Robotics and Automation.2014:1836-1843.

[17]何友,修建娟,关欣.雷达数据处理及应用[M].2版.北京:电子工业出版社,2009:120-121.

[18]辛煜,梁华为,梅涛,等.基于激光传感器的无人驾驶汽车动态障碍物检测及表示方法[J].机器人, 2014,36(6):654-661.

3D Lidar-based marine object detection for USV

LI Xiaomao,ZHANG Xin,WANG Wentao,QU Dong,ZHU Chuan
(School of Mechatronic Engineering and Automation,Shanghai University,Shanghai 200072,China)

To realize autonomous navigation and avoidance,it is necessary for an unmanned surface vehicle(USV)to perceive the surrounding environment in real time,and detect obstacles threatening its sailing.Three dimensional Lidar plays an important role in unmanned system obstacle detection.In this paper,an obstacle detection algorithm based on 3D Lidar is proposed.The 3D laser point cloud is projected onto a 2.5D grid map in one period.The obstacles are clustered and segmented.The elliptical feature of obstacles is extracted from the raw laser point.Dynamic obstacles are found and tracked using the nearest neighbor data association and a Kalman flter.The simulation data based on the electronic chart verifes efectiveness of the method in USV obstacle detection.

unmanned surface vehicle(USV);Lidar;obstacle detection;grid map;elliptical representation

TP 242.6

A

1007-2861(2017)01-0027-10

10.3969/j.issn.1007-2861.2016.07.022

2017-01-11

国家自然科学基金资助项目(61673254);上海市自然科学基金资助项目(13ZR1454300);上海市科委能力建设资助项目(14500500400)

李小毛(1981—),男,研究员,研究方向为图像处理、雷达数据处理、无人艇环境感知、导航和控制及其总体技术.E-mail:lixiaomao@shu.edu.cn