基于四线激光雷达的校园道路的检测与跟踪.pdf

1 3 4 传感器与微系统( T r a n s d u c e r a n d Mi c r o s y s t e m T e c h n o l o g i e s ) 2 0 1 3年 第 3 2卷 第 9期 基 于四线激光雷达的校园道路 的检测与跟踪 杨 象军， 项志宇，刘济林 ( 浙江大学 信息科学与电子 工程 学系。 浙江 杭州 3 1 0 0 2 7 ) 摘要：道路跟踪是机器人视觉导航的一个重要任务。相比单线激光雷达， 四线激光雷达具有信息量大， 可靠性高的优点。对检测到的数据点进行直线拟合， 并融合 4条扫描线信息获得融合道边。在对路面点 进行平面拟合后， 可以测得较稳定的道边高度。同时在获得的候选直线段中， 进行岔 口检测， 能检测出 T 型和十字型岔口。根据车体的运动信息， 进一步构造卡尔曼滤波器对道边和岔口进行跟踪估计。校园环 境的实验结果表明： 该方法能够有效快速地检测和跟踪道边 ， 并能对岔口进行可靠地识别与跟踪。 关键词：四线激光雷达； 道边高度； 岔口检测； 卡尔曼滤波 中图分类号 ：T P 3 9 1 4 文献标识码 ：A 文章编号 ：1 0 0 0 - 9 7 8 7 ( 2 0 1 3 ) 0 9 -01 3 4 - 0 5 De t e c t i o n a n d t r a c k i n g 0 f c a mp us r o a d ba s e d 0 n t i l t l 1 1 1 0Ur- l i ne l aS er r ac I ar Y A N G X i a n g - j u n ， X I A N G Z h i y u ， L I U J i l i n ( D e p a r t me n t o f I n f o r ma t i o n S c i e n c e a n d E l e c t r o n i c E n g i n e e r i n g ， Z h e j i a n g U niv e r s i t y ， Ha n g z h o u 3 1 0 0 2 7 ， C h i n a ) Ab s t r a c t ：Ro a d t r a c k i n g i s a n i mp o r t a n t t a s k o f r o b o t v i s u a l n a v i g a t i o n C o mp a r e d wi t h s i n g l e l i n e l a s e r r a d a r ， f o u r l i n e l a s e r r a d a r h a s a d v a n t a g e s o f l a r g e a mo u n t o f i n f o r ma t i o n a n d h i g h r e l i a b i l i t y D e t e c t e d d a t a p o i n t s i s s t r a i g h t n i n e fi t t e d， a n d f u s e i n f o r ma t i o n o f f o u r s c a n l i n e s t o g e t f u s e d r o a d b o u n d a r y A f t e r p l a n a r fi t t i n g t h e r o a d s u r f a c e p o i nt s， t h e r o a d b o un da r y h e i g h t c a n b e me a s u r e d Amo n g t h e o bt a i ne d c a n di da t e s t r a i g h t l i n e s e g me nt s， d e t e c t t h e i n t e r s e c t i o n t o g e t t he T s h a p e d a n d c r o s s s h a p e d i n t e r s e c t i o n，Ac c o r d i ng to mo t i o n i nfo rm a t i o n o f t h e v e h i c l e， c o n s t r u c t Ka l ma n f i l t e r t o t r a c k a n d e s t i ma t e t h e r o a d b o u n d a r y a n d i n t e r s e c t i o n T h e e x p e r i me n t a l r e s u l t s i n c a mp u s e n v i r o n me nt s h o w t h a t t h i s me t ho d c a n d e t e c t a n d t r a c k a l o n g t h e r o a ds i d e e f f e c t i v e l y a n d q u i c kl y， a nd d i s t i n g u i s h a n d t r a c k r e l i a b l y the i n t e r s e c t i o n i n t he r o a d a s w e l 1 Ke y wo r d s ： f o u r - l i n e l a s e r r a d a r ；r o a d b o u n d a ry h e i g h t ；i n t e r s e c t i o n d e t e c t i o n；Ka l ma n fi l t e r i n g 0引 言 道路边沿和岔 口检测是 机器人实现 自主导航 的重要任 务之一。基于图像的道边信息检测由于图像的信息量大、 分辨率高等特点已得到广泛的应用。但是， 这类检测极易 受到外部环境变化的影响， 如光照、 阴影、 雨天等。而激光 雷达却不易受外部环境的影响， 不仅能在恶劣的天气条件 下工作， 满足全天候使用的要求 ， 而且探测范围远， 测距准 确， 功耗较小， 视野也较广。所以， 基于激光雷达的道边信 息检测开始受到大家的重视， 也有了较快的发展。 Wi j e s o m a W S等人 采用单线激光雷达结合扩展卡 尔曼滤波算法获取道边 ， 并 进行 跟踪 ， 取得 不错 的效果 。文 献 2 ， 3 中提出了根据前时刻连续有限状态和误差信息， 直接构造一组模糊 中心结合最大熵原理来对下一时刻状 态 和误差进行实 时预测 ， 用于提取并跟踪道边 ， 该 方法无需 构 收稿 日期 ： 2 0 1 3 - 0 2 - 0 1 造状态方程和观测方程 ， 计算简单。Ha n J a e h y n n等人 采 用数据点到断点间直线距离超过阈值最大为断点来分割线 段检 测 道边 ， 并 采 用 I P D A F 对 道边 进 行 跟 踪。X u Z e z h o n g 6 将数据点看成随机采样点， 分成若干个区域， 计 算该区域的随机密度来检测道边， 方法简单 ， 但计算量大， 要计算每个区域的协方差矩阵。道路边沿本身高度比较 小， 以上方法所采用的单线雷达易受地面起伏和车体俯仰 角的变化而漏检道边， 增大安装俯仰角能改善漏检， 但检测 距离缩小 ， 不利于机器人 的导航 ， 同时以上方法均未对岔 口 进行检测和跟踪， 也没有对道边高度信息进行提取。 本文使用 L U X四线激光雷达， 由4条角度间隔为0 8 。 的线激光构成。较单线雷达而言， L U X四线激光雷达检测 的范围广， 获取的数据量更大， 使得同时稳定检测道边和获 取远距离道边信息成为可能。 第 9期 杨象军， 等 ： 基于四线激光雷达的校园道路的检测与跟踪 1 3 5 1 道路检测与跟踪原理 道路与岔 口的检测 与跟踪主要步骤 ( 见 图 1 ) 如下 ： 1 ) 数据分割提取直线 ， 筛选直线并融合得到完整的道边线 段； 2 ) 提取地面点拟合地平面， 计算道边高度； 3 ) 在候选直 线中检测岔 口， 融合并提取道路信息； 4) 构造 函数， 对下 一 帧数据初步滤波， 拟合出道边与岔口线段 ， 融合后作为新 的观测 ； 5 ) 卡尔曼滤波， 获得道边和岔口的较好估计。 图 1 道路检 测与跟踪 的流程 图 Fi g 1 Fl o w c h a r t o f r o a d de t e c t i o n a n d t r a c ki n g 2 道边与岔 口检测 2 1数 据 分割 与提 取 道 边 假设小车出发前航向与道边平行。雷达检测到的是离 散的数据点， 需要从中分割 出道边、 地面和岔 口的有用信 息。本文采用简单快速的方法， 首先根据点到线的距离小 于阈值判断为共线点来拟合共线点为线段， 快速获得道路 轮廓。再根据以下道边特性 ， 从中初步提取出道边 ： 1 ) 道边大致平行小车的航向， 所以， 道边应与竖直方向 近似平行； 2 ) 左右2条道边应该近似平行， 且道边有一定的长度。 对雷达得到的4条扫描线数据 ， 分别进行道边提取处 理后可以得到4条道边。然后进行融合( 在 坐标上取平 均 ， 在 Y坐标上下端点取最小值， 上端点取最大值) ， 就可以 得到一条较完整的初步道边检测结果。 2 2获取道边的高度 地面数据点位于2条道边间， 分布方向与道边近似垂 直 ， 由此空 间关 系 ， 在 先前 检测 到 的道边 之 间所包 含 区域 内， 对每条激光扫描线提取地面数据候选点( 较上面的激光 线有可能扫描不到地面， 基本上至少会有 2条激光线扫描 到地面) 。四线激光雷达获取的数据点是三维坐标， 为避免 障碍物等影响， 对地面候选点采用随机抽样一致性( F a n d o m s a mp l e c o n s e n s u s ， R A N S A C ) 算法 J ， 迭代随机抽取 3个 点拟合平面， 最后得到的最大多数候选点满足的平 面则为 地平面。获得地平面后 ， 道边下端点到地面平面的距离即 为道边的高度。在实际情况 中， 最下一条激光扫描线， 即 图2 中虚线获得的道边高度最准确， 因为该扫描线的俯角 最大， 下端点受车体俯仰角变化的影响最小。 道边下端点 激光雷达 地 面 图 2道边高度检测原理 F i g 2 De t e c t i o n pr i n c i p l e o f r o a d - b o u n da r y h e i g h t 获取道边高度的意义在于从高度的角度进一步明确雷 达扫描到的是道路边缘的凸台， 而不是其他障碍物， 在岔口 检测 中十分重要 。 2 3岔 口的检 测 本文 只是针对较 为常见的 T型 口和十字型 口。在 图 3 所示 ， 上平线所对应的道边线提示了岔 口的存在， 在此称为 岔口线段。道边检测出后 ， 根据岔 口线段的一些特性对先 前取得的4组道路框架线段来提取岔口： 1 ) 岔口线段 的Y坐标离道边上端点的距离不小于道 宽 ； 2 ) 岔口线段的长度不能小于一定的阈值； 3 ) 岔口线段斜率应与道边近似垂直。 这样 ， 便可以获得 岔 口线 段 ， 若检 测不到 岔 口线段 ， 则 认为还未到岔口。4组数据获得的岔口线段分为 2个部分， 在原点坐标左端和原点坐标右端， 将这 2个部分分别融合 ( Y坐标取均值， 坐标上线段左端取最小值， 右端取最大 值) 得到岔口左右2个部分 ， 再判断 T型口还是十字型口。 若岔 口左右 2个部分距离不小于道宽， 即岔 口的竖直 朝向可通行， 则为十字型口； 否则， 是 T型口， 将岔口左右 2个部分融合起来得到完整的T型岔口。 至此， 则检测出道边与岔口， 用一组道路信息 B ， A B 表示。A 和 B 分别为左道边线段对应直线方程 - A Y-B =0的斜率与截距； 和 B ， 分别为右道边线段 对应直线方程 - A Y -B =0的斜率与截距 ； 4 和 B 分别 为岔口水平朝向线段对应直线方程 Y -A -B =0的斜率 与截距。获取岔口信息 ， 岔口中心点和2个朝向的道宽。 3 道边和岔 口跟踪 由道边和岔口检测取得一组道路信息可以构造 3条直 线 函数对下一 帧的雷达数 据进 行跟踪 ， 如 图 3中所示 的虚 线 。 设 A y 为在下一帧时小车前进的距离和航向 偏移， 通过机器人惯导信息获得， 则移动后所对应的直线方 程分别为： 上平线 Y= + t a n ( a r c t a n A ko + ) + 第 9期 杨象军 ， 等： 基于四线激光雷达的校园道路的检测与跟踪 1 3 7 ： 一 ；J x m f a 1 4组数据检测 的道边与岔 口 ( a ) r o a d - b o u n d a r y a n d in t e r s e c t i o n d e t e c t e d b y f o u r g r o u p s o f d a t a Fi g 8 0 0 O O 一0 0 一O 一 0 -0 1 6 1 4 1 2 l O 8 6 4 2 0 i i f l f 一1 O一5 0 5 1 0 x m ( b ) 融合后获得的完整道边与岔 口 ( b ) c o mp l e t e r o a d - b o u n d a r y a n d i n t er s e c t i o n af t e r f us i on 图7 T宇型 交叉 口的检测 F i g 7 T - t y p e i n t e r s e c t i o n d e t e c t i o n 图 8拟合平面与数据点的三维坐标 c o o r d i n a t e s o f fit t i n g p l a n e a n d da t a p o i n t s 道边高度， 对同一道边检测 4次获得的平均值与方差 ， 如 表 1 。左右道边实际高度为 1 7 c m， 检测的左右道边高度平 均值也比较接近实际高度。 表 1 道边高度 Ta b I Ro a d - b o u n da r y h e i g h t m ( a ) 左道边下端点 ( a ) l e ft r o a d - b o u n d a r y e n d p o in t l 7 O l 75 l 8 0 l 85 l 0 1 95 2 UO 2 U 5 2 1 0 y m ( b ) 右道边下端点 f b 1 n g h t r o a d - b o u n d a r y e n d p o i n t 图 9 道边各端点在 Y - Z方 向的坐标关系 Fi g 9 Co o r di n a t e r e l a t i o n s o f r o a d- b o u nd a ry e a c h e nd p o i n t i n t he y - Z d i r e c t i m 2 0 1 5 1 0 5 0 由图2可知， 雷达安装高度高于道边， 检测到的道边各 下端点在 Y - Z坐标上应近似共线。这可以在道边检测中作 为判断道边的一个依据， 对于提高道边岔口检测的准确性有 一 定的帮助。如图9 ， 左右道边检测到的各下端点近似共线。 4 3对 交 叉 口的跟 踪 图 1 0中3条细线即为构造直线对数据初步滤波， 提取 道边和岔 口的数据并拟合直线得到道边与岔 口线段， 如 图 l l 。图 l 2则为卡尔曼滤波后获得完整的道边与岔 口。 利用 3条构造函数对道路进行跟踪， 简单快速地提取道边 、 和岔口数据， 进而获取道边和岔口线段； 再对融合后的道边 和岔口线段进行卡尔曼滤波获得完整道边和岔 口， 跟踪速 一 一 度快 ， 而且准确 ， 满足实时性要求。 4 4多帧跟踪结果融合 对 校园里的一条小道 ( 图 1 3 ) 进行 连续检测 跟踪 ， 获得 图 1 4， 圆点为岔口， 虚线指示岔口朝向。检测跟踪效果基本 与道路实际情况吻合， 进一步证明构造函数对道边与岔口 进行检测跟踪实时性好， 且跟踪效果稳定。 一 ： 。 l l 5 0 5 x m ( a 】 最低激光线的数据 x m ( b ) 第二低激光线的数据 x m x m ( c ) 第二高激光线的数据 ( d ) 最高激光线的数据 ( c ) t h e s e c o n d h i g h l a s e r li n e d a t a ( d ) h i g h e s t l a s e r l i n e d a ta 图 l 0 3条构造直线 Fi g 1 0 Th r e e s t r u c t ur a l l i n e s x m x m ( a ) 最低激光线跟踪结果 (b ) 第二低激光线跟踪结果 ( a ) tr a c k i n g r e s u l t o f l o we s t ( b ) t r a c k i n g r e s u l t o f th e s e c o n d l a s e r l in e l o w l a s e r l i n e ( C ) 第二高激光线跟踪结果 ( c ) tra c kin g r e s u l t o f s e c o n d x m ( d ) 最高激光线跟踪结果 ( d ) tr a c k i n g r e s u l t o f the h i【g l1 l a s e r l i n e h iig h c s t l a s e r li n e 图 1 1 各激光线提取的道边 与岔 口 Fi g 1 1 Ro ad d o u n da r y a nd i n t e r s e c t i o n o f e a c h l a s e r l i n e 1 3 8 传 感 器 与 微 系 统 第 3 2卷 1 8 1 6 1 4 1 2 1 0 8 6 4 2 0 ； ； ： l i - ； 一 j ! i 一 一 鼍 寸 一 一 咔 牵一 l l 一 ：_ 丰 一 ： 王 i =。： ： 毒 ： ： ： j r 。 r r r 一 4 3 2 1 0 1 2 3 x m 图 1 2 跟踪到的道边与岔口 F i g 1 2 T r a c k e d r o a d - b o u n d a r y a n d i n t e r s e c t i o n 一 4 -2 0 2 4 4 2 U Z 4 x m x m ( a ) 原始数据 ( b ) 检测结果 ( a ) o r i g in a l d a t a ( b ) d e t e c t i o n r e s u l t 图 1 4 最终的道边和岔 口 F i g 1 4 Fi n a l r o a d- b o u n da r y a nd i n t e r s e c t i o n 5 结束语 本文基于四线激光雷达来检测跟踪道边与岔 口， 充分 利用4条激光得到的数据， 准确地检测到道边和岔口以及 道边高度， 并用一组包含道边线段和岔 口线段的斜率和截 距的数据来表示道路的特征， 用这组数据构建直线函数对 下一帧数据进行卡尔曼滤波来跟踪估计道边和岔 口， 实现 了可靠 的识别 与跟踪 。 参考文献 ： 1 Wi j e s o m a W S， K oda g o d a K R S， B ala s u ri y a A u n a P R 0 a d b o u n d a r y d e t e c t i o n a n d t r a c k i n g u s i n g l a d a r s e n s i n g J I E E E T r a n s a c t i o n s o n R o b o t i c s a n d A u t o ma t i o n ， 2 0 0 4， 2 0 ( 3) ： 4 5 6 4 6 4 2 L i u P e t e r X， Me n g Ma x Q H O n l i n e d a t a - d ri v e n f u z z y c l u s t e ri n g w i t h a p p l i c a t i o n s t o r e a l - t i me r o b o t ic t r a c k i n g J I E E E T r a n s a c t i o n s o n F u z z y S y s t e m， 2 0 0 4， 1 2 ( 4 ) ： 5 1 6- 5 2 3 陈得宝 ， 赵春霞 ， 张浩 峰 ， 等 基 于 2维激 光测距仪 的快速路 边检测 J 中国图象图形 学报， 2 0 0 7 ， 1 2 ( 9) ： 1 6 0 4-1 6 0 9 Ha n J a e h y u n， Ki m Do n g c h u l ， L e e Mi n c h a e， e t a 1 En h a n c e d r o a d - b o u n d a w a n d o b s t a c l e d e t e c t i o n u s i n g a d o wn w a r d l o o k i n g l id a r s e n s o r J I E E E T r ans a c t i o n s o n V e h i c u l a r T e c h n o l o g y ， 2 0 1 2， 6 1 ( 3 ) ： 9 7 1 -9 8 5 Mus i c k i D， Ev an s R，S t a n k o v i e S I n t e g r a t e d p r o b a b i l i s t i c d a t a a s s o c i a t i o n J I E E E T r a n s a c t i o n s o n A u t o ma t i c C o n t r o l ， 1 9 9 4， 3 9 ( 6 )： 1 2 3 7 -1 2 4 1 X u Z e z h o n g L a s e r r a n g e fi n d e r b a s e d r o a d f o l l o w i n g C I E E E I n t e r n a t i o n a l C o n f e r e n c e o n Me c h a t r o n i c s a n d Au t o ma t i o n， 2 0 0 5： 7 1 3 -71 7 F i s e h l e r M A， B o l l e s R C Ra n d o m s a mp l e c o n s e n s u s ： A pa r a d i g m f o r mo d e l fi t t i n g wi t h a p p l i c a t i o n s t o i ma g e a l l a l y s i s a n d a u t o ma t e d c a r t o gra p h y J C o m mu n i c a t io n o f t h e A C M， 1 9 8 1 ， 2 4 ： 3 8 1 3 9 5 8 K i r c h n e r A， He i n r i e h T Mo d e l - b a s e d d e t e c t i o n o f r o a d b o u n d a r i e s w i t h U l a s e r s c a n n e r C I E E E I n t e rna t i o n a l C o n f e r e n c e o n I n - t e l l i g e n t Ve h i c l e s ， S t u t t g a r t ， Ge r ma n y， 1 9 9 8： 9 3 -9 8 9 A u f r e r e R， Me n z C， T h o r p e C M u l t i p l e s e n s o r f u s i o n f o r d e t e c t i n g l o c a t i o n o f c u r b s ， w a l l s ， a n d b a r r i e r s c P r o c e e d i n g s o f t h e I EEE I n t e l l i g e n t Ve h i c l e s S y mp o s i u m， 20 0 3： 1 2 6-1 31 1 O D a r ms M， K o m a r M a n d L u e k e S M a p - b a s e d r o a d b o u n d a r y e s t i m a t i o n C P r o c e e d i n g s o f t h e I E E E I n t e l l i g e n t V e h i c l e s S y m - p o s i u m ， 2 0 1 0： 6 0 9-6 1 4 1 1 K o d a g o d a K R S ， G e S S ， Wi j e s o m a W S ， e t a 1 I MM P D A F a p p r o a c h f o r r o a d - b o u n d a t r a c k i n g J I E E E T r a n s a c t i o n s o n V e h i c u l ar T e c h n o l o g y ， 2 0 0 7， 5 6 ( 2 )： 4 7 8-4 8 6 1 2 P e t e r s o n K， Z i g l a r J ， R y b s k i P E F a s t f e a t u r e d e t e c t i o n a n d s t o - c h a s t i c p a r a me t e r e s t i ma t i o n o f r o a d s h a p e u s i n g mu l t i p l e L I D A R c 3 I E E E I n t e r n a t i o n al C o n f e r e n c e o n I n t e l l i g e n t R o b o t s a n d S y s t e ms， 2 0 0 8： 6 1 2-6 1 9 1 3 P h i l i p p L i n d n e r ， E r i c R i c h t e r ， G e r d Wa n ie l i k ， e t a 1 Mu l t i c h a n n e l l i d a r p r o c e s s i n g fo r l a n e d e t e c t i o n a n d e s t i m a t i o n c P ro c e e - d i n g s o f t h e 1 2 t h I n t e r n a t i o n al I E EE C o n f e r e n c e o n I n t e l l i g e n t Tr a n s p o r t a t i o n S y s t e ms ， 2 0 0 9： 1-6 作者简介 ： 杨 象军( 1 9 8 8一) ， 男 ， 浙江温州人 ， 硕士研究生 ， 主要 研究方 向 为智能车的 自主导航 。 t 。 ； ； t； 、 t ( 上接第 1 2 8页) 3 刘志平 ， 康 宜华， 武新军 储罐底板漏磁检测传感器设计 J 无损检测 ， 2 0 0 4， 2 6 ( 1 2 ) ： 6 1 2-6 1 5 4 胡军辉 ， 王友钊 Wi n d o w s C E设备驱动 程序开发 J 计 算机 工程 ， 2 0 0 6， 3 2 ( 1 6 ) ： 4 1- 4 3 5 华清 远见嵌 入式 培训 中心 Wi n d o w s C E嵌入 式开 发标 准教 P p 程 M 修订版 北京 ： 人 民邮电出版社 ， 2 0 1 0 6 刘力阳 ， 张金成 基 于 A R M& Wi n C E的无纸记 录仪 J 仪 表 技术与传感器 ， 2 0 1 1 ( 8 ) ： 2 8-3 0 作者简介 ： 于卫刚( 1 9 8 6一) ， 男 ， 陕西咸 阳人， 工学硕士 ， 研究方 向为嵌入 式系统与应用。 j