CN106842231B - 一种道路边界检测及跟踪方法 - Google Patents
一种道路边界检测及跟踪方法 Download PDFInfo
- Publication number
- CN106842231B CN106842231B CN201610981078.5A CN201610981078A CN106842231B CN 106842231 B CN106842231 B CN 106842231B CN 201610981078 A CN201610981078 A CN 201610981078A CN 106842231 B CN106842231 B CN 106842231B
- Authority
- CN
- China
- Prior art keywords
- point
- road
- boundary
- scanning element
- laser radar
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
Abstract
本发明公开了一种道路边界检测及跟踪方法,包括:利用雷达扫描获取无人车周围环境的雷达扫描点的极坐标数据并转换成直角坐标;将雷达点映射到极坐标网格中,根据网格中延伸顶点的高度提取地面点;利用同一扫描线下近邻扫描点在道路边界处径向距离和高度突变的性质以及结合道路延伸方向提取道路边界点;采用随机采样一致性方法进行滤波,采用最小二乘法对滤波后的道路边界点进行拟合;采用卡尔曼滤波器进行跟踪;本发明能够在道路上有障碍物的情况下有效的提取实际场景中道路边界,同时卡尔曼滤波器的使用,增强了边界检测的准确性和可靠性,方法中没有复杂的计算,因此具有很好的实时性,可以广泛的应用于无人驾驶的导航模块中。
Description
技术领域
本发明涉及智能车辆环境感知技术领域,尤其涉及一种道路边界检测及跟踪方法。
背景技术
自无人车诞生以来,自主导航技术就吸引了众多学者和研究机构的目光,但目前仍然是一个难度很大的课题,其中道路边界检测与跟踪是这个领域非常重要的研究内容,也是这个领域的核心技术之一。在复杂的城市环境中,道路边界可以限制车辆的可通行区域,为无人驾驶汽车的自主导航和路径规划提供了丰富的信息;在建筑物、树木等遮挡导致GPS信号接收受限的城市区域,道路的边界也可以作为一种非常好的特征用于车辆位置估计;此外,在目标检测系统中,道路边界可以限制搜索区域,减少了不必要的干扰,在降低计算量的同时也显著的提高的检测精度。此外,在没有车道线的道路环境下,可以依赖可靠道路边界检测与跟踪技术实现无人车的车道跟踪与保持。
根据采用传感器的不同,出现了很多不同的道路边界检测与跟踪技术,主要有基于相机(单目或立体视觉)、2D或3D激光雷达的。
因为相机获取的信息量大,视野较宽、成本低等优点,基于视觉的道路边界的检测与跟踪技术应用较广泛。Seibert等人利用单目相机检测道路边界,并利用道路的边界和纹理信息,结合机器学习的方法对道路边界进行检测;Siegemund等人利用深度视觉信息检测道路的直线边缘和曲线边缘,结合条件随机场的方法将数据分为道路部分和人行道部分。但是相机拍摄的图像极易受到外部环境的干扰,如光照、树荫、下雨等都可能造成相机拍摄失败,在背景条件比较复杂的环境下,道路边界很容易和其它目标混在一起,此外,利用立体视觉检测道路边界需要计算两幅图像的视差,这样会增大计算量,难以满足无人车对实时性的要求。
随着雷迖技术的发展,研究人员开始采用激光雷达来替代或辅助摄像机。雷达不受光照、阴影等外界影响,不仅能够在恶劣天气条件下正常工作,而且探测范围广、测量距离远,测量精度高,因此在道路边界检测领域得到了广泛的使用,Wijesoma等人利用2D雷达结合卡尔曼滤波实现对道路边缘的检测与跟踪。但是,2D激光雷达每次扫描只能获取一个扫描线,所以当道路结构比较复杂时,这些基于2D雷达检测算法很容易受到观测噪声的影响。较2D雷达,3D激光雷达探测的范围较广,数据密度大、精度高,可以检测到更准确的道路边界信息同时抑制噪声的干扰,保证智能车能够安全的行驶在道路上,不会发生碰撞路边的情况,近年来被广泛应用到无人驾驶车辆的环境感知系统中。
三维激光雷达虽然获取数据速度快、点云密集、场景目标丰富,但其获取的数据具有海量特征,这对处理车载三维雷达数据的算法提出了更高的要求。Moosmann.提出一种方法将3D雷达数据映射到2D障碍物地图上,然后通过图像处理的方法提取特征,利用马尔科夫随机场进一步提取道路边缘,该方法可以很好地用于路径规划,但是由于处理时间太长无法满足实时性的要求,并且当车道中有行人、车辆等障碍物时会造成道路边界误检,此外,仅仅依靠检测,难以保障检测结果的稳定性和可靠性。
发明内容
针对上述现有技术中存在的缺陷与不足,本发明的目的在于,提供了一种基于三维激光雷达的道路边界检测与跟踪方法,包括以下步骤:
步骤1,以三维激光雷达的中心为坐标原点,过原点os且以三维激光雷达的垂直轴线方向为z轴,过原点os且平行于所述三维激光雷达横向切面指向无人车前进的方向为y轴,过原点且垂直于y轴和z轴所构成的平面指向无人车前进方向右侧的方向为x轴,建立三维激光雷达的直角坐标系osxyz;
获取多个三维激光雷达扫描点的极坐标数据,并将三维激光雷达扫描点的极坐标数据转换到三维激光雷达的直角坐标系osxyz下;
步骤2,将转换到三维激光雷达的直角坐标系osxyz下的三维激光雷达扫描点映射到M*N的2.5维极坐标网格中,根据极坐标网格中延伸顶点的高度以及延伸顶点与网格中最低点的高度差提取所有的三维激光雷达扫描点中的地面点;
步骤3,对所述提取的地面点,分别提取地面点中道路两旁的边界点;
步骤4,对所述提取的道路两旁的边界点,采用随机采样一致性方法对道路两旁的边界点进行滤波得到滤波后的道路两旁的边界点,并采用最小二乘法对分别对滤波后的道路两旁的边界点进行拟合,即得到道路边界线;
步骤5,利用卡尔曼滤波算法对所述的道路边界线进行跟踪。
进一步地,通过式(1)将步骤1中所述的三维激光雷达扫描点的极坐标转换到无人车的雷达直角坐标系下:
xi=γisin(αi)
yi=γicos(αi)cos(βi)
zi=γicos(αi)cos(βi) (1)
γi=j
其中,i为三维激光雷达的扫描点的序号,j为三维激光雷达的扫描线的序号,i和j取自然数,γi为三维激光雷达扫描点的极坐标数据,αi为第i个扫描点的激光束相对于osyz平面的角度偏移量,βi为第i个扫描点的激光束相对于osxy平面的角度偏移量,xi为扫描点的x轴坐标,yi为扫描点的Y轴坐标,zi为扫描点的Z轴坐标,ri为第i个扫描点所在的扫描线的序号。
进一步地,步骤2中所述的将转换到三维激光雷达的直角坐标系osxyz下的三维激光雷达扫描点映射到M*N的2.5维极坐标网格中,根据极坐标网格中延伸顶点的高度以及延伸顶点与网格中最低点的高度差提取所有的三维激光雷达扫描点中的地面扫描点,包括以下步骤:
步骤21,以三维激光雷达原点os为极点,以三维激光雷达直角坐标系所在的osxy平面为极平面,将极平面划分为M*N的2.5维极坐标网格图,网格的径向长度为T1,网格所在扇区的圆形角为360/M,然后将所有三维激光雷达的扫描点映射到对应的极坐标网格中;
步骤22,对位于同一个极坐标网格中所有的扫描点,按照极坐标网格中扫描点的高度从小到大进行排序,计算所有相邻扫描点之间的高度差;
步骤23,提取极坐标网格中的延伸顶点和最低点;
步骤24,如果极坐标网格中的延伸顶点高度小于给定阈值T2并且延伸顶点与最低点之间的高度差小于阈值T3,则将该网格中所有高度在延伸顶点及其一下的扫描点标记为地面扫描点;
步骤25,更换极坐标网格,重复执行步骤22至24,直至提取完所有极坐标网格中的地面扫描点。
进一步地,步骤23中所述的提取极坐标网格中的延伸顶点和最低点包括以下步骤:
步骤231,在极坐标网格中,从第i个点开始,如果第i个和第i+1个点之间的高度差大于阈值T4,这将第i个点作为该极坐标网格中的障碍物在垂直方向上的延伸顶点,结束搜索;
步骤232,如果第i个扫描点和第i+1个扫描点之间的高度差小于阈值T并且第i+1个点是网格中的最后一个点,则将第i+1个扫描点作为该网格中障碍物在垂直方向上的延伸顶点,结束搜索;
步骤233,如果第i个扫描点和第i+1个扫描点之间的高度差小于等于阈值T4,并且第i+1个扫描点不是极坐标网格中最后一个点,则i=i+1;否则,重复步骤231至步骤233。
进一步地,步骤3中所述的根据三维激光雷达同一扫描线下,近邻扫描点在道路边界处径向距离突变的性质分别提取地面扫描点中道路两旁的边界点,包括以下步骤:
步骤31,选定三维激光雷达的任一条扫描线,提取该扫描线中所有扫描点的径向距离和高度值;
步骤32,对该扫描线中的第i个扫描点,在同一扫描线上提取该点的m邻域中的最大径向距离和最小径向距离以及该邻域中的最大高度和最小高度,并计算它们的径向差值和最大高度差值作为该扫描点i的特征值;
步骤33,从离无人车最近的第i条扫描线开始,以y轴为中心轴分别从y正半轴开始,分别向x正半轴和负半轴方向搜索前k个径向特征值大于阈值T5和高度阈值大于T6的的扫描点,作为该扫描线中位于车辆前方道路的边界点;
步骤34,根据检测的左右道路边界宽度、左右边界检测点的中心相对前一扫描线边界中心的横向偏移预测第i+1条描线中位于道路中心的点坐标;
步骤35,对第i+1条扫描线,根据预测的道路中心点为中心,分别向x正半轴和x负半轴方向搜索第i+1条扫描线中的道路边界点;
步骤36,改变扫描线,依次提取无人车前方的道路边界点;
步骤37,采用和提取无人车前方道路边界点相同的方法沿着y负半轴依次提取无人车后方的所有道路边界点。
进一步地,步骤34中所述的根据检测的左右道路边界宽度、左右边界检测点的中心相对前一扫描线边界中心的横向偏移预测第i+1条描线中位于道路中心的点坐标包括:
步骤341,以检测到的第一个左右边界点作为道路的左右边界计算道路的宽度以及道路中心点;
步骤342,如果道路宽度大于等于标准车道宽度,计算当前扫描线与前一个扫描线的中心位置偏移,将计算的当前扫描线位于道路中心的点,并结合中心位置偏移量作为下一个扫描线中位于道路中心的点位置;
步骤343,如果道路宽度小于标准道路宽度,则根据前两条扫描线中位于道路中心的点以及中心位置偏移预测下一条扫描线中位于道路中心的点的位置。
进一步地,步骤4中对所述的提取的道路边界点,采用随机采样一致性方法对非道路边界干扰点进行滤波,包括:
步骤41,随机从检测的道路左边界点中任意选择两个点,得到通过该点的直线;
步骤42,用得到的直线模型去测试其他观测点,由点到直线的距离确定观测点是否为局内点或者为局外点;
步骤43,如果局内点足够多,并且局内点多于原有“最佳”直线的局内点,那么将这次迭代后的所有局外点作为干扰点进行滤除;
步骤44,更换道路右边界点,4-1至4-3,滤除右边界点中的干扰点。
进一步地,通过式(2)对步骤4中所述的采用最小二乘法对分别对两边的道路边界点进行拟合:
其中n为所述滤波之后的道路左边界或右边界点的总个数,ao,a1为多项式系数。
进一步地,步骤5中所述的利用卡尔曼滤波算法对所述的道路边界线进行跟踪,包括:
步骤51,判断是否检测到道路的边界;
步骤52,如果没有检测到车道线,则根据上一次最终的道路边界的位置利用卡尔曼滤波器对当前道路边界进行预测,预测结果就作为算法最终输出的道路边界线的位置;
步骤53,如果检测到道路边界线,计算当前检测的边界线与上一帧中最后获得的边界线之间斜率差以及截距差;如果斜率差小于等于阈值T7并且截距差小于等于阈值T8,则将当前检测的道路边界线线的位置作为测量值递给卡尔曼滤波器,利用卡尔曼滤波器对道路边界线进行一次预测,结合本次的测量值更新道路边界线的位置,更新结果就作为算法最终输出的道路边界线的位置。如果夹角超过阈值T7或者截距差大于阈值T8,则根据上一次最终的道路边界的位置利用卡尔曼滤波器对当前道路边界进行预测,预测结果就作为算法最终输出的道路边界线的位置。
与现有技术相比,本发明具有以下技术效果:
1.本发明首先采用基于延伸顶点的方法快速精确的提取雷达点云中的地面扫描点,排除了道路上存在其它障碍物时对道路边界检测造成的干扰,提高了检测的精度;
2.本发明根据雷达同一扫描线下近邻扫描点在道路边界处径向距离和高度突变的性质,采样近邻扫描点径向差和高度差的方法提取两边的道路边界点,可靠性高,并且算法简单便于程序实现,并且在提取道路边界点的过程中结合了道路延伸方向的特征,在提取道路边界点时可以很好的适应道路曲率的变化;
3.本发明在检测的基础上,增加了跟踪算法,保证了算法的稳定性和可靠性,实验证明,该算法具有较强的环境适应性,在转弯、上坡、下坡和单侧数据丢失的情况下均有良好的检测效,非常适用于在没有车道线的路况下实现无人车的车道保持功能。
附图说明
图1为本发明方法的主要步骤流程图;
图2为本发明实施例中激光雷达的一帧原始数据在直角坐标系下的表示;
图3为本发明实施例中依照本发明从雷达原始数据中提取的地面结果图;
图4为本发明实施例中依照本发明提取的道路边界结果图;
图5为本发明实施例中依照本发明得到的车道边界线拟合的结果图;
图6为本发明实施例中依照本发明得到的车道边界线跟踪的结果图。
具体实施方式
下面结合附图和实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施,给出了详细的实施方式和具体的操作过程,但本发明的保护范围不限于下述的实施例。
一种道路边界检测及跟踪方法,该方法采用了三维激光雷达对道路边界进行检测,但本方法不只适用于某一款激光雷达传感器,但为了具体描述三维激光雷达这类传感器的工作方式及数据格式,我们采用我们的无人驾驶平台上安装的三维激光雷达VelodyneHDL-32E为例,对本方法进行分析,该传感器共由32个激光发射单元组成一列,其垂直视场角为-30.67°~10.67°,垂直角度分辨率为1.33°,并且以每秒10Hz的频率进行水平旋转,每秒可产生约70万个点,可以提供车辆周围360°视场角100m范围内的环境信息。我们指定雷达旋转一周采集的数据作为一帧,雷达输出的数据是以球体坐标的方式表示的,该球体坐标以雷达为中心,每个雷达点有5个参数:垂直发射角α、水平发射角β、点到雷达中心的距离γ、反射强度Intensity、GPS时间帧信息。
如图1所示,本实施例有如下步骤:
步骤1,以三维激光雷达的中心为坐标原点,建立三维激光雷达的直角坐标系osxyz,获取无人车周围环境的三维激光雷达扫描点的极坐标数据并转换到雷达传感器的直角坐标系osxyz下;
具体的,基于三维激光雷达的探测范围取决于三维激光雷达在无人驾驶平台上的安装位置,为了尽可能的减少遮挡和增大激光雷达的探测大范围,本实施例中将HDL-32E三维激光雷达安装在我们无人驾驶车的车顶位置,并且雷达的轴线垂直与地面;
具体的,为了便于数据处理,将获取的三维激光雷达扫描点的极坐标数据转换到以三维激光雷达中心为原点的雷达传感器的直角坐标系下,三维激光雷达的直角坐标系osxyz是以所述激光雷达中心为坐标原点os,过原点且以三维激光雷达的垂直轴线方向为z轴,过原点且平行于所述三维激光雷达横向切面指向无人车前进的方向为y轴,过原点且垂直于y轴和z轴所构成的平面指向车辆右侧的方向为x轴;
具体的,将三维激光雷达扫描点的极坐标转换到无人车的三维激光雷达直角坐标系osxyz下,根据式(一)进行转换:
其中,i为三维激光雷达的扫描点的序号,j为三维激光雷达的扫描线的序号,γi为三维激光雷达扫描点的极坐标数据,αi为第i个扫描点的激光束相对于osyz平面的角度偏移量,βi为第i个扫描点的激光束相对于osxy平面的角度偏移量,xi为扫描点的x轴坐标,yi为扫描点的Y轴坐标,zi为扫描点的Z轴坐标,ri为第i个扫描点所在的扫描线的序号。
步骤2,当道路中存在行人、车辆时,会遮挡道路边界,增加道路边界检测的难度,由于道路边界属于地面的一部分,因此本发明采用了一种基于延伸顶点的快速有效的地面提取方法,将地面提取出来,只在地面点中检测边界,消除了其他障碍物对道路边界的遮挡的,降低了数据量,同时也提高了道路边界检测的精度,如图3所示的去除地面上的其他障碍物后的结果图,采用的方法是将传感器直角坐标系下的三维雷达扫描点映射到M*N的2.5维极坐标网格中,根据网格中延伸顶点的高度以及延伸顶点与网格中最低点的高度差提取所有的地面点;
具体的步骤包括:
2-1以三维激光雷达原点os为极点,以三维激光雷达直角坐标系的所在的osxy平面为极平面,将极平面划分为M*N的2.5维极坐标网格图,网格的径向长度为T1,网格所在扇区的圆形角为360/M,然后将所有的三维激光雷达的三维扫描点映射到对应的极坐标网格中;
2-2对位于同一个网格中所有的点,按照网格中点的高度从小到大进行排序,计算所有相邻点之间的高度差;
2-3提取网格中的延伸顶点和最低点;
2-4如果网格中的延伸顶点高度小于给定阈值T2并且延伸顶点与最低点之间的高度差小于阈值T3,则将该网格中所有高度在延伸顶点及其一下的点标记地面点;
2-5更换极坐标网格,重复执行2-2至2-4,提取所有网格中的地面点;
具体地,HDL-32E三维激光雷达的感知范围为70米左右,扫描精度为2cm,为了满足车辆导航的要求的同时提高实时性,本实施例中保留车辆周围50米范围内的数据,根据该雷达的性能,本实施例中T1取0.2米,网格所在扇区的圆心角取0.5度,所有M取值为360/0.5=720,N为50/0.2=250;
具体地,三维激光雷达据地面的安装高度为1.73米,根据雷达、道路边沿相对地面的高度和实验结果,本实施例中T2取值为-1.5米,T3取值为0.3米;
步骤2-3中,提取网格中的延伸顶点,具体方法为:
1)在极坐标网格中,从第i个点开始,如果第i个和第i+1个点之间的高度差大于阈值T4,这将第i个点作为该极坐标网格中的障碍物在垂直方向上的延伸顶点,结束搜索;
2)如果第i个扫描点和第i+1个扫描点之间的高度差小于阈值T并且第i+1个点是网格中的最后一个点,则将第i+1个扫描点作为该网格中障碍物在垂直方向上的延伸顶点,结束搜索;
3)如果第i个扫描点和第i+1个扫描点之间的高度差小于等于阈值T4,并且第i+1个扫描点不是网格中最后一个点,则i=i+1,重复步骤1)至3)继续搜索;
具体的,根据雷达的垂直分辨率和多次的实验结果,本实施例中T4取0.25米;
步骤3,对所述提取的地面点,根据雷达同一扫描线下,近邻点在道路边界处径向距离和高度突变的性质结合道路延伸方向分别提取道路两旁的边界点;
所述步骤3的具体方法包括:
3-1从HDL-32E三维激光雷达第1至32条扫描线中选定第i条扫描线,提取每条扫描线中所有扫描点的径向距离和高度值;
3-2对该扫描线中的第i个扫描点,在同一扫描线上提取该点的m邻域中径向距离最大和最小的扫描点,并计算它们的径向差和近邻点中的最大高度差值作为该扫描点i的特征值,具体的,本实施例中,取i的二邻域扫描点,即m取2;
3-3从离车最近的第i条扫描线开始,以y轴为中心轴分别从y正半轴开始,分别向x正半轴和负半轴方向搜索前k个径向特征值大于阈值T5和高度阈值大于T6的的扫描点,作为该扫描线中位于车辆前方道路的边界点;本实施例中采取从第3条扫描线开始,径向差阈值T5取0.1,高度差T6取值0.15;
3-4根据检测的左右道路边界宽度、左右边界检测点的中心相对前一扫描线边界中心的横向偏移预测第i+1条描线中位于道路中心的点坐标;
3-5对第i+1条扫描线,根据预测的道路中心点为中心,分别向x正半轴和x负半轴方向搜索第i+1条扫描线中的道路边界点;
3-6改变扫描线,依次提取车辆前方的道路边界点;
3-7采用相同的方法沿着y负半轴依次提取车辆后方的所有道路边界点;
具体的,所述步骤3-4中预测第i+1条扫描线中位于道路中心的点的坐标,包括:
3-41以检测到的第一个左右边界点作为道路的左右边界计算道路的宽度以及道路中心点;
3-42如果道路宽度大于等于标准车道宽度,计算当前扫描线与前一个扫描线的的中心位置偏移,将计算的当前扫描线位于道路中心的点,并结合中心位置偏移量作为下一个扫描线中位于道路中心的点位置
3-43如果道路宽度小于标准道路宽度,则根据前两条扫描线中位于道路中心的点以及中心位置偏移预测下一条扫描线中位于道路中心的点的位置;步骤4,在上面道路边界点提取过程中,由于杂草及干扰的存在,会引入一些非道路边界干扰点的存在,因此,本发明采用随机采样一致性方法对非道路边界干扰点进行滤波,这样可以最大限度的保留扫描到的道路边界上的点,同时将一些散乱点,如扫描到的花草等滤除。此外,根据雷达的扫描数据特点,扫描到道路边沿上的点一般都是呈线性分布,因此,本文采用最小二乘法对聚类产生的数据点进行拟合,得出被扫描的道路边沿表面轮廓。
具体的,对所述提取的道路边界点,采用随机采样一致性方法对非道路边界干扰点进行滤波方法包括:
4-1随机从检测的道路左边界点中任意选择两个点,得到通过该点的直线;
4-2用得到的直线模型去测试其他观测点,由点到直线的距离确定观测点是否为局内点或者为局外点;
4-3如果局内点足够多,并且局内点多于原有“最佳”直线的局内点,那么将这次迭代后的所有局外点作为干扰点进行滤除;
4-4更换道路右边界点,4-1至4-3,滤除又边界点中的干扰点;
具体的,步骤4中采用最小二乘法对两边的道路边界点分别进行一次方程拟合,拟合公式具体为:
其中n为所述滤波之后的道路左边界或右边界点的总个数,ao,a1为多项式系数,拟合直线的高度值取边界点的高度均值。
步骤5,在实际的道路上,不可能所有地方都存在明显的道路边界,我们希望在没有道路边界的时候,也能做出一个对于当前实际道路边界位置的猜测,为实现这个目标,本发明在最后使用了一个卡尔曼滤波器对识别的道路边界线进行滤波跟踪。这样做有两个好处,一是在无法识别出道路边界的时候,也能给出道路边界可能的位置。第二个就是,可以对识别出来的道路边界进行平滑。路面的实际情况千变万化,由于复杂的环境干扰,识别出来的道路边界线的位置可能会在小范围内摆动。对检测的道路边界线进行滤波跟踪后,我们就可以得到一个足够稳定的道路边界线预测位置。
具体的,步骤5中使用卡尔曼滤波算法对检测的道路左右边界线进行跟踪,对左右边界线分别进行以下步骤:
5-1判断是否检测到道路的边界情况;
5-2如果没有检测到车道线,则根据上一次最终的道路边界的位置利用卡尔曼滤波器对当前道路边界进行预测,预测结果就作为算法最终输出的道路边界线的位置。
5-3如果检测到道路边界线,计算当前检测的边界线与上一帧中最后获得的边界线之间斜率差以及截距差;如果斜率差小于等于阈值T7并且截距差小于等于阈值T8,则将当前检测的道路边界线线的位置作为测量值递给卡尔曼滤波器,利用卡尔曼滤波器对道路边界线进行一次预测,结合本次的测量值更新道路边界线的位置,更新结果就作为算法最终输出的道路边界线的位置。如果夹角超过阈值T7或者截距差大于阈值T8,则根据上一次最终的道路边界的位置利用卡尔曼滤波器对当前道路边界进行预测,预测结果就作为算法最终输出的道路边界线的位置。
具体的,由于道路的边界不会发生突变,所以前一帧得到的最后的道路边界与当前帧检测的道路边界之间角度和横向距离变化不会太大,通过实验发现,当T7取为30度横向距离变化取1.0时可以获得较理想的结果,因此,本实施例中T7取30,T8取1.0;
具体的,由于卡尔曼滤波器是一个线性滤波器,使用车道边界线的两端点的x坐标来表示边界线的变化更直观,所以,本发明使用每条边界线两个端点的x坐标来表示道路边界线,一共有4个变量,作为测量值输入给卡尔曼滤波器。
Claims (8)
1.一种道路边界检测及跟踪方法,该方法采用安装在无人车上的三维激光雷达对道路边界进行检测,其特征在于,包括以下步骤:
步骤1,以三维激光雷达的中心为坐标原点,过原点os且以三维激光雷达的垂直轴线方向为z轴,过原点os且平行于所述三维激光雷达横向切面指向无人车前进的方向为y轴,过原点且垂直于y轴和z轴所构成的平面指向无人车前进方向右侧的方向为x轴,建立三维激光雷达的直角坐标系osxyz;
获取多个三维激光雷达扫描点的极坐标数据,并将三维激光雷达扫描点的极坐标数据转换到三维激光雷达的直角坐标系osxyz下;
步骤2,将转换到三维激光雷达的直角坐标系osxyz下的三维激光雷达扫描点映射到M*N的2.5维极坐标网格中,根据极坐标网格中延伸顶点的高度以及延伸顶点与网格中最低点的高度差提取所有的三维激光雷达扫描点中的地面点;包括以下步骤:
步骤21,以三维激光雷达原点os为极点,以三维激光雷达直角坐标系所在的osxy平面为极平面,将极平面划分为M*N的2.5维极坐标网格图,网格的径向长度为T1,网格所在扇区的圆形角为360/M,然后将所有三维激光雷达的扫描点映射到对应的极坐标网格中;
步骤22,对位于同一个极坐标网格中所有的扫描点,按照极坐标网格中扫描点的高度从小到大进行排序,计算所有相邻扫描点之间的高度差;
步骤23,提取极坐标网格中的延伸顶点和最低点;
步骤24,如果极坐标网格中的延伸顶点高度小于给定阈值T2并且延伸顶点与最低点之间的高度差小于阈值T3,则将该网格中所有高度在延伸顶点及其以下的扫描点标记为地面扫描点;
步骤25,更换极坐标网格,重复执行步骤22至24,直至提取完所有极坐标网格中的地面扫描点;
步骤3,对所述提取的所有的三维激光雷达扫描点中的地面点,分别提取地面点中道路两旁的边界点;
步骤4,对所述提取的地面点中道路两旁的边界点,采用随机采样一致性方法对非道路边界干扰点进行滤波得到滤波后的道路两旁的边界点,并采用最小二乘法分别对滤波后的道路两旁的边界点进行拟合,即得到道路边界线;
步骤5,利用卡尔曼滤波算法对所述的道路边界线进行跟踪。
2.如权利要求1所述的道路边界检测及跟踪方法,其特征在于,通过式(1)将步骤1中所述的三维激光雷达扫描点的极坐标转换到无人车的雷达直角坐标系下:
其中,i为三维激光雷达的扫描点的序号,j为三维激光雷达的扫描线的序号,i和j取自然数,γi为三维激光雷达扫描点的极坐标数据,αi为第i个扫描点的激光束相对于osyz平面的角度偏移量,βi为第i个扫描点的激光束相对于osxy平面的角度偏移量,xi为扫描点的x轴坐标,yi为扫描点的Y轴坐标,zi为扫描点的Z轴坐标,ri为第i个扫描点所在的扫描线的序号。
3.如权利要求1所述的道路边界检测及跟踪方法,其特征在于,步骤23中所述的提取极坐标网格中的延伸顶点和最低点包括以下步骤:
步骤231,在极坐标网格中,从第i个点开始,如果第i个和第i+1个点之间的高度差大于阈值T4,则将第i个点作为该极坐标网格中的障碍物在垂直方向上的延伸顶点,结束搜索;
步骤232,如果第i个扫描点和第i+1个扫描点之间的高度差小于阈值T并且第i+1个点是网格中的最后一个点,则将第i+1个扫描点作为该网格中障碍物在垂直方向上的延伸顶点,结束搜索;
步骤233,如果第i个扫描点和第i+1个扫描点之间的高度差小于等于阈值T4,并且第i+1个扫描点不是极坐标网格中最后一个点,则i=i+1;否则,重复步骤231至步骤233。
4.如权利要求1所述的道路边界检测及跟踪方法,其特征在于,步骤3中对所述提取的所有的三维激光雷达扫描点中的地面点,分别提取地面点中道路两旁的边界点,包括以下步骤:
步骤31,选定三维激光雷达的任一条扫描线,提取该扫描线中所有扫描点的径向距离和高度值;
步骤32,对该扫描线中的第i个扫描点,在同一扫描线上提取该点的m邻域中的最大径向距离和最小径向距离以及该邻域中的最大高度和最小高度,并计算它们的径向差值和最大高度差值作为该扫描点i的特征值;
步骤33,从离无人车最近的第i条扫描线开始,以y轴为中心轴分别从y正半轴开始,分别向x正半轴和负半轴方向搜索前k个径向特征值大于阈值T5和高度阈值大于T6的的扫描点,作为该扫描线中位于车辆前方道路的边界点;
步骤34,根据检测的左右道路边界宽度、左右边界检测点的中心相对前一扫描线边界中心的横向偏移预测第i+1条描线中位于道路中心的点坐标;
步骤35,对第i+1条扫描线,根据预测的道路中心点为中心,分别向x正半轴和x负半轴方向搜索第i+1条扫描线中的道路边界点;
步骤36,改变扫描线,依次提取无人车前方的道路边界点;
步骤37,采用和提取无人车前方道路边界点相同的方法沿着y负半轴依次提取无人车后方的所有道路边界点。
5.如权利要求4所述的道路边界检测及跟踪方法,其特征在于,步骤34中所述的根据检测的左右道路边界宽度、左右边界检测点的中心相对前一扫描线边界中心的横向偏移预测第i+1条描线中位于道路中心的点坐标包括:
步骤341,以检测到的第一个左右边界点作为道路的左右边界计算道路的宽度以及道路中心点;
步骤342,如果道路宽度大于等于标准车道宽度,计算当前扫描线与前一个扫描线的中心位置偏移,将计算的当前扫描线位于道路中心的点,并结合中心位置偏移量作为下一个扫描线中位于道路中心的点位置;
步骤343,如果道路宽度小于标准道路宽度,则根据前两条扫描线中位于道路中心的点以及中心位置偏移预测下一条扫描线中位于道路中心的点的位置。
6.如权利要求1所述的道路边界检测及跟踪方法,其特征在于,步骤4中所述的提取的地面点中道路两旁的边界点,采用随机采样一致性方法对非道路边界干扰点进行滤波,包括:
步骤41,随机从检测的道路左边界点中任意选择两个点,得到通过该点的直线;
步骤42,用得到的直线模型去测试其他观测点,由点到直线的距离确定观测点是否为局内点或者为局外点;
步骤43,如果局内点足够多,并且局内点多于原有“最佳”直线的局内点,那么将这次迭代后的所有局外点作为干扰点进行滤除;
步骤44,更换道路右边界点,步骤41-43,滤除右边界点中的干扰点。
7.如权利要求1所述的道路边界检测及跟踪方法,其特征在于,通过式(2)对步骤4中所述的采用最小二乘法分别对滤波后的道路两旁的边界点进行拟合:
其中n为滤波后的道路左边界或右边界点的总个数,ao,a1为多项式系数。
8.如权利要求1所述的道路边界检测及跟踪方法,其特征在于,步骤5中所述的利用卡尔曼滤波算法对所述的道路边界线进行跟踪,包括:
步骤51,判断是否检测到道路的边界;
步骤52,如果没有检测到道路边界线,则根据上一次最终的道路边界的位置利用卡尔曼滤波器对当前道路边界进行预测,预测结果就作为算法最终输出的道路边界线的位置;
步骤53,如果检测到道路边界线,计算当前检测的边界线与上一帧中最后获得的边界线之间斜率差以及截距差;如果斜率差小于等于阈值T7并且截距差小于等于阈值T8,则将当前检测的道路边界线线的位置作为测量值递给卡尔曼滤波器,利用卡尔曼滤波器对道路边界线进行一次预测,结合本次的测量值更新道路边界线的位置,更新结果就作为算法最终输出的道路边界线的位置,如果斜率差超过阈值T7或者截距差大于阈值T8,则根据上一次最终的道路边界的位置利用卡尔曼滤波器对当前道路边界进行预测,预测结果就作为算法最终输出的道路边界线的位置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610981078.5A CN106842231B (zh) | 2016-11-08 | 2016-11-08 | 一种道路边界检测及跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610981078.5A CN106842231B (zh) | 2016-11-08 | 2016-11-08 | 一种道路边界检测及跟踪方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106842231A CN106842231A (zh) | 2017-06-13 |
CN106842231B true CN106842231B (zh) | 2019-03-22 |
Family
ID=59146194
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610981078.5A Active CN106842231B (zh) | 2016-11-08 | 2016-11-08 | 一种道路边界检测及跟踪方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106842231B (zh) |
Families Citing this family (50)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109144052B (zh) * | 2017-07-07 | 2021-12-28 | 奥特埃克斯股份有限公司 | 用于自动驾驶车辆的导航系统及其方法 |
CN107563314B (zh) * | 2017-08-18 | 2020-01-14 | 电子科技大学 | 一种基于平行坐标系的车道线检测方法 |
CN107677268B (zh) * | 2017-09-30 | 2020-11-06 | 福建农林大学 | 车载式道路几何线性信息自动测量装置及方法 |
US10788831B2 (en) * | 2017-10-06 | 2020-09-29 | Wipro Limited | Method and device for identifying center of a path for navigation of autonomous vehicles |
CN108573272B (zh) * | 2017-12-15 | 2021-10-29 | 蔚来(安徽)控股有限公司 | 车道拟合方法 |
CN110033482A (zh) * | 2018-01-11 | 2019-07-19 | 沈阳美行科技有限公司 | 一种基于激光点云的路沿识别方法和装置 |
CN108280840B (zh) * | 2018-01-11 | 2021-09-03 | 武汉理工大学 | 一种基于三维激光雷达的道路实时分割方法 |
CN110068834B (zh) * | 2018-01-24 | 2023-04-07 | 北京京东尚科信息技术有限公司 | 一种路沿检测方法和装置 |
CN108427124B (zh) * | 2018-02-02 | 2020-05-12 | 北京智行者科技有限公司 | 一种多线激光雷达地面点分离方法及装置、车辆 |
JP2019159380A (ja) * | 2018-03-07 | 2019-09-19 | 株式会社デンソー | 物体検知装置、物体検知方法およびプログラム |
CN108519867B (zh) * | 2018-04-12 | 2022-08-19 | 长沙景美集成电路设计有限公司 | Gpu中一种实现三角形反走样的装置和方法 |
CN108898672A (zh) * | 2018-04-27 | 2018-11-27 | 厦门维斯云景信息科技有限公司 | 一种制作三维高清道路图车道线的半自动点云方法 |
CN109579859B (zh) * | 2018-05-10 | 2019-11-22 | 北京建筑大学 | 一种高精度导航地图高程数据处理方法及装置 |
CN108898139B (zh) * | 2018-06-04 | 2022-06-10 | 上海大学 | 一种下雨环境下的激光雷达数据抗干扰处理方法及其实验装置 |
CN108957475A (zh) | 2018-06-26 | 2018-12-07 | 东软集团股份有限公司 | 一种道路边界检测方法及装置 |
CN108958246A (zh) * | 2018-06-29 | 2018-12-07 | 长安大学 | 无人车在U-Turn场景的轨迹跟踪控制方法 |
CN110378175B (zh) * | 2018-08-16 | 2022-09-30 | 北京京东叁佰陆拾度电子商务有限公司 | 道路边沿的识别方法和装置 |
CN109188448B (zh) * | 2018-09-07 | 2020-03-06 | 百度在线网络技术(北京)有限公司 | 点云非地面点过滤方法、装置及存储介质 |
CN109635641B (zh) * | 2018-11-01 | 2020-06-09 | 百度在线网络技术(北京)有限公司 | 道路边界线的确定方法、装置、设备和存储介质 |
CN111273314A (zh) * | 2018-11-16 | 2020-06-12 | 北京四维图新科技股份有限公司 | 点云数据处理方法、装置和存储介质 |
WO2020102987A1 (zh) * | 2018-11-20 | 2020-05-28 | 深圳大学 | 智能辅助驾驶方法及系统 |
CN109684921B (zh) * | 2018-11-20 | 2022-05-27 | 吉林大学 | 一种基于三维激光雷达的道路边界检测与跟踪方法 |
CN109670455A (zh) * | 2018-12-21 | 2019-04-23 | 联创汽车电子有限公司 | 计算机视觉车道线检测系统及其检测方法 |
CN109613544B (zh) * | 2018-12-26 | 2022-11-11 | 长安大学 | 一种基于激光雷达的高速公路视距检测方法 |
CN109752701B (zh) * | 2019-01-18 | 2023-08-04 | 中南大学 | 一种基于激光点云的道路边沿检测方法 |
CN109858460B (zh) * | 2019-02-20 | 2022-06-10 | 重庆邮电大学 | 一种基于三维激光雷达的车道线检测方法 |
CN110109146B (zh) * | 2019-04-30 | 2021-05-14 | 北京云迹科技有限公司 | 基于多线激光雷达的路面检测方法及装置 |
DE102019112413A1 (de) * | 2019-05-13 | 2020-11-19 | Bayerische Motoren Werke Aktiengesellschaft | Verfahren und vorrichtung zur multi-sensor-datenfusion für automatisierte und autonome fahrzeuge |
CN110261866B (zh) * | 2019-06-12 | 2021-10-29 | 重庆交通大学 | 一种基于路沿光阵的道路宽度几何检测方法和系统 |
CN110276293B (zh) * | 2019-06-20 | 2021-07-27 | 百度在线网络技术(北京)有限公司 | 车道线检测方法、装置、电子设备及存储介质 |
CN110363182B (zh) * | 2019-07-24 | 2021-06-18 | 北京信息科技大学 | 基于深度学习的车道线检测方法 |
CN112219206A (zh) * | 2019-07-25 | 2021-01-12 | 北京航迹科技有限公司 | 用于确定位姿的系统和方法 |
CN110696826B (zh) * | 2019-10-09 | 2022-04-01 | 北京百度网讯科技有限公司 | 用于控制车辆的方法和装置 |
CN110736999B (zh) * | 2019-10-24 | 2021-11-02 | 北京交通大学 | 基于激光雷达的铁路道岔检测方法 |
CN110781891B (zh) * | 2019-11-28 | 2023-01-31 | 吉林大学 | 一种基于激光雷达传感器的识别车辆可行驶区域的方法 |
CN110989613A (zh) * | 2019-12-18 | 2020-04-10 | 北京新能源汽车技术创新中心有限公司 | 车辆定位方法、装置、电子设备及存储介质 |
CN110986990B (zh) * | 2019-12-25 | 2023-03-28 | 西安主函数智能科技有限公司 | 封闭环境无人驾驶工程车路径规划方法及系统 |
CN111208533A (zh) * | 2020-01-09 | 2020-05-29 | 上海工程技术大学 | 一种基于激光雷达的实时地面检测方法 |
CN110928320B (zh) * | 2020-02-10 | 2020-08-04 | 上海高仙自动化科技发展有限公司 | 路径生成方法及生成装置、智能机器人及存储介质 |
CN111290396A (zh) * | 2020-03-12 | 2020-06-16 | 上海圭目机器人有限公司 | 一种管道检测无人船自动控制方法 |
WO2021207954A1 (zh) * | 2020-04-15 | 2021-10-21 | 华为技术有限公司 | 一种目标识别的方法和装置 |
CN111380461A (zh) * | 2020-04-21 | 2020-07-07 | 南京理工大学 | 一种电缆卷绕状态检测系统 |
EP4141736A4 (en) * | 2020-04-28 | 2023-06-21 | Huawei Technologies Co., Ltd. | LANE KEEPING METHOD AND APPARATUS |
CN112034482A (zh) * | 2020-08-24 | 2020-12-04 | 北京航天发射技术研究所 | 一种道路边界实时提取及测量方法和装置 |
CN112240772B (zh) * | 2020-12-16 | 2021-09-28 | 北京赛目科技有限公司 | 一种车道线生成方法及装置 |
CN112862844B (zh) * | 2021-02-20 | 2024-01-05 | 园测信息科技股份有限公司 | 基于车载点云数据的道路边界交互式提取方法 |
CN113581184B (zh) * | 2021-08-25 | 2023-04-07 | 京东鲲鹏(江苏)科技有限公司 | 一种最大可通行区域的确定方法、装置、设备和介质 |
CN113741523B (zh) * | 2021-09-08 | 2024-03-19 | 北京航空航天大学 | 一种基于边界和采样的混合无人机自主探测方法 |
CN115840227B (zh) * | 2023-02-27 | 2023-07-04 | 福思(杭州)智能科技有限公司 | 道路边缘的检测方法及装置 |
CN117351449B (zh) * | 2023-12-04 | 2024-02-09 | 上海几何伙伴智能驾驶有限公司 | 基于极坐标加权的道路可通行区域边界优化方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103500338A (zh) * | 2013-10-16 | 2014-01-08 | 厦门大学 | 基于车载激光扫描点云的道路斑马线自动提取方法 |
CN104331878A (zh) * | 2014-10-13 | 2015-02-04 | 上海交通大学 | 基于机器视觉的道路跟踪方法 |
-
2016
- 2016-11-08 CN CN201610981078.5A patent/CN106842231B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103500338A (zh) * | 2013-10-16 | 2014-01-08 | 厦门大学 | 基于车载激光扫描点云的道路斑马线自动提取方法 |
CN104331878A (zh) * | 2014-10-13 | 2015-02-04 | 上海交通大学 | 基于机器视觉的道路跟踪方法 |
Non-Patent Citations (2)
Title |
---|
一种智能汽车的实时道路边缘检测算法;朱学葵 等;《北京联合大学学报》;20151031;第29卷(第4期);第1-7页 |
地面无人车辆越野环境多要素合成可通行区域检测;肖强;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20160315;第13-24、28、41-45页 |
Also Published As
Publication number | Publication date |
---|---|
CN106842231A (zh) | 2017-06-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106842231B (zh) | 一种道路边界检测及跟踪方法 | |
CN109144072A (zh) | 一种基于三维激光的机器人智能避障方法 | |
CN105955258B (zh) | 基于Kinect传感器信息融合的机器人全局栅格地图构建方法 | |
CN110084272B (zh) | 一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法 | |
CN106570454B (zh) | 基于移动激光扫描的行人交通参数提取方法 | |
CN105404844B (zh) | 一种基于多线激光雷达的道路边界检测方法 | |
CN106401643B (zh) | 基于激光点云的隧道超欠挖检测方法 | |
Schlichting et al. | Localization using automotive laser scanners and local pattern matching | |
CN106780524A (zh) | 一种三维点云道路边界自动提取方法 | |
Wang et al. | Intelligent vehicle self-localization based on double-layer features and multilayer LIDAR | |
CN106199558A (zh) | 障碍物快速检测方法 | |
CN107831777A (zh) | 一种飞行器自主避障系统、方法及飞行器 | |
CN108647646A (zh) | 基于低线束雷达的低矮障碍物的优化检测方法及装置 | |
CN113792699B (zh) | 一种基于语义点云的对象级快速场景识别方法 | |
CN112801022A (zh) | 一种无人驾驶矿卡作业区道路边界快速检测更新的方法 | |
CN104536009A (zh) | 一种激光红外复合的地面建筑物识别及导航方法 | |
CN103176185A (zh) | 用于检测道路障碍物的方法及系统 | |
CN112346463B (zh) | 一种基于速度采样的无人车路径规划方法 | |
Mueller et al. | GIS-based topological robot localization through LIDAR crossroad detection | |
CN111721279A (zh) | 一种适用于输电巡检工作的末端路径导航方法 | |
Qian et al. | Hy-Seg: A hybrid method for ground segmentation using point clouds | |
CN109241855A (zh) | 基于立体视觉的智能车辆可行驶区域探测方法 | |
Jiang et al. | Effective and robust corrugated beam guardrail detection based on mobile laser scanning data | |
Fang et al. | Extraction 3D road boundaries from mobile laser scanning point clouds | |
Gaha et al. | A new LiDAR-based approach for poles and distribution lines detection and modelling |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |