CN108226951B - 一种基于激光传感器的快速运动障碍物实时跟踪方法 - Google Patents
一种基于激光传感器的快速运动障碍物实时跟踪方法 Download PDFInfo
- Publication number
- CN108226951B CN108226951B CN201711411184.0A CN201711411184A CN108226951B CN 108226951 B CN108226951 B CN 108226951B CN 201711411184 A CN201711411184 A CN 201711411184A CN 108226951 B CN108226951 B CN 108226951B
- Authority
- CN
- China
- Prior art keywords
- laser radar
- obstacle
- tracking
- laser
- fast moving
- 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
Images
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/66—Tracking systems using electromagnetic waves other than radio waves
Landscapes
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明提供了一种基于激光传感器的快速运动障碍物实时跟踪方法,其特征在于,包括将单线激光雷达安装在车身四周,多线激光雷达安装在车身两侧,三维激光雷达安装在车身顶部,建立以激光雷达为中心的坐标系,并对坐标系进行内参标定和外参标定;根据道路结构手动选择激光雷达扫描模式;本发明所述的一种基于激光传感器的快速运动障碍物实时跟踪方法具有计算量小、跟踪效果好,可以在激光跟踪领域中被广泛使用。
Description
技术领域
本发明属于自动驾驶领域,尤其是涉及一种基于激光传感器的快速运动障碍物实时跟踪方法。
背景技术
在电子信息化的当今社会,目标的检测与跟踪在军用领域和民用领域都发挥着非常重要的作用。在军事应用领域,精确的检测和跟踪敌方目标是精确打击对方的基础,也是军事实力的重要体现。在民用方面,检测与跟踪也广泛应用于自动驾驶、无人机、机器人等领域。
其中所运用的粒子滤波技术是在贝叶斯滤波框架下,通过动态搜寻出一组状态空间中随机的带有权重的样本点集合,对检测目标后验概率密度p(Xk|Z1:k)进行近似拟合,以样本点的加权平均值来替代概率密度函数的积分运算,从而获得目标动态运动状态的次优估计得一种算法。
如果在T时刻检测并跟踪到的车辆为快速运动车辆,则其真实状态和位置为跟踪到的假阳性结果。随着时间持续会衍生出更多的假阳性结果,这样会大大的造成不必要的计算负担,影响跟踪效果,甚至系统崩溃。
而且传统粒子滤波算法在检测和跟踪性能很大程度上取决于粒子的数量,特别是在真实世界的场景中,如果目标太小、检测环境嘈杂、障碍物存在遮挡隐蔽,以及物体的快速出现或者消失等情况下,这将直接导致计算量的指数级增加以及实时性跟踪性能的降低,往往导致跟踪的误检率也较高。
发明内容
有鉴于此,本发明旨在提出一种可以降低计算量、减小系统功耗,降低环境感知误检率,提高系统稳定性和安全性的基于激光传感器的快速运动障碍物实时跟踪方法。
为达到上述目的,本发明的技术方案是这样实现的:
一种基于激光传感器的快速运动障碍物实时跟踪方法,包括如下步骤:
(1)将激光雷达安装在车身上,以激光雷达的中心建立坐标系,并对坐标系进行内参标定和外参标定;
(2)通过激光雷达扫描自车周围,利用空间数据聚类算法组成障碍物;
(3)对步骤(2)所述的障碍物进行状态分类,包括初观测组和非初观测组;
(4)初次观测组的障碍物设置初始状态规则;
(5)对非初次观测组和已设置初始状态的初次观测组的障碍物均进行动态跟踪;
(6)生成障碍物信息列表。
进一步,步骤(1)所述的内参标定包括旋转校正角度、垂直校正角度、距离校正角度、垂直便宜因子以及水平便宜因子;外参标定包括:激光雷达坐标系相对于自车坐标系的旋转矩阵R和平移矩阵T。
进一步,步骤(1)所述的激光雷达为单线激光雷达或多线激光雷达或全向激光雷达;所述的单线激光雷达安装在车身四周,,多线激光雷达安装在车身两侧,所述的全向激光雷达安装在车身顶部。
进一步,步骤(2)所述的空间数据聚类算法为K-means算法,包括通过激光点云聚类,组成符合障碍物规则的高斯分布,判断为障碍物。
进一步,步骤(5)所述的初始状态规则包括结构化道路障碍物、非结构化道路两侧障碍物和非结构化道路中间障碍物。
进一步,所述的车身上还安装有摄像头、惯导设备和GPS接收器。
进一步,步骤(5)所述的动态跟踪,包括将摄像头所采集的周围环境信息和激光雷达所采集的障碍物信息通过PF粒子滤波模型进行融合关联,使用KF扩展卡尔曼滤波模型生成跟踪信息。
相对于现有技术,本发明所述的一种基于激光传感器的快速运动障碍物实时跟踪方法具有以下优势:
本发明对跟踪障碍物的初始状态进行优化,加入了障碍物跟踪的新参考状态及观测状态,作为一种盒式粒子改进滤波跟踪算法,通过对区间进行分别处理,在获取同等检测跟踪效果的同时,有效的降低计算量和算法的总耗时量,大大提高跟踪的实时性;本发明计算量小、跟踪效果好,可以在激光跟踪领域中被广泛使用。
附图说明
构成本发明的一部分的附图用来提供对本发明的进一步理解,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的不当限定。在附图中:
图1为本发明实施例所述的运行流程图。
具体实施方式
需要说明的是,在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
在本发明的描述中,需要理解的是,术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。此外,术语“第一”、“第二”等仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中,除非另有说明,“多个”的含义是两个或两个以上。
在本发明的描述中,需要说明的是,除非另有明确的规定和限定,术语“安装”、“相连”、“连接”应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或一体地连接;可以是机械连接,也可以是电连接;可以是直接相连,也可以通过中间媒介间接相连,可以是两个元件内部的连通。对于本领域的普通技术人员而言,可以通过具体情况理解上述术语在本发明中的具体含义。
下面将参考附图并结合实施例来详细说明本发明。
一种基于激光传感器的快速运动障碍物实时跟踪方法,包括如下步骤:
(1)将激光雷达安装在车身上,以激光雷达的中心建立坐标系,并对坐标系进行内参标定和外参标定;
(2)通过激光雷达扫描自车周围,利用空间数据聚类算法组成障碍物;
(3)对步骤(2)所述的障碍物进行状态分类,包括初观测组和非初观测组;
(4)初次观测组的障碍物设置初始状态规则;
(5)对非初次观测组和已设置初始状态的初次观测组的障碍物均进行动态跟踪;
(6)生成障碍物信息列表。
步骤(1)所述的内参标定包括旋转校正角度、垂直校正角度、距离校正角度、垂直便宜因子以及水平便宜因子;外参标定包括:激光雷达坐标系相对于自车坐标系的旋转矩阵R和平移矩阵T;所述的激光雷达为单线激光雷达或多线激光雷达或全向激光雷达;所述的单线激光雷达安装在车身四周,多线激光雷达安装在车身两侧,坐标系中心均为自车,所述的全向激光雷达安装在车身顶部,以全向激光雷达为坐标系中心。
步骤(2)所述的空间数据聚类算法为K-means算法,包括通过激光点云聚类,组成符合障碍物规则的高斯分布,判断为障碍物。
步骤(5)所述的初始状态规则包括结构化道路障碍物、非结构化道路两侧障碍物和非结构化道路中间障碍物;所述的动态跟踪,包括将摄像头所采集的周围环境信息和激光雷达所采集的障碍物信息通过PF粒子滤波模型进行融合关联,使用KF扩展卡尔曼滤波模型生成跟踪信息。
所述的车身上还安装有摄像头、惯导设备和GPS接收器。
具体的工作运行流程:
首先,将三维激光雷达安装在车辆上,以激光雷达的中心为其坐标系,需要对其进行内参标定和外参标定。内参标定包括旋转校正角度、垂直校正角度、距离校正角度、垂直便宜因子以及水平便宜因子。外参标定包括:激光雷达坐标系相对于自车坐标系的旋转矩阵R和平移矩阵T。
其中,所述的激光雷达,如果采用单线激光雷达,则其具有结构简单、功耗低、使用方便等优点,用于障碍物检测、道路路沿检测、路面检测。但其只有一根扫描线,所以也存在信息量少等缺点,导致很难获得完整障碍物的大小信息,以及在道路复杂情况下,车辆的遮挡也很难被检测跟踪到;如果采用的多线激光雷达,则其不仅可以实现单线雷达全部功能,还可以检测到障碍物的高度信息,但精度不足、系统复杂度加大,数据量增加且处理难度加大。如果采用全向激光雷达,与单线、多线激光雷达相比,其可视范围和环境信息的丰富性、细节上都有极大提升,能够获得尽可能大范围的探测,同时获得尽可能小的遮挡区域。此外,为了避免车顶对三维全向激光雷达的遮挡,三维全向激光雷达的安装应至少高于车顶0.5米。
然后,激光雷达扫描自车周围环境并识别和分类:
通过自车装载的激光雷达传感器不停扫描自车周围环境,通过空间数据聚类方法,把点云数据分割成不同的空间子集,根据一些常用的聚类算法,如K-means算法,进行点云聚类。将可以聚在一起的点云数据抽象成一个障碍物。根据模型的宽度,将其进行分类:如车辆、自行车、行人等。
此后,根据道路模型优化跟踪目标初始状态:
对于障碍物(如跟踪车辆),通过关联判断其是否为初次观测对象,如果不是,则根据卡尔曼滤波模型更新其状态(位置、速度、角速度、方向角);如果是初次观测到的对象,则根据自车所在的道路模型(如车辆所在车道、限速信息)等,给被跟踪车辆的初始状态设置为一定规则下的随机状态。以下为赋予初始状态的实施例:
最后,障碍物的动态跟踪:
获取目标的运动信息,对目标进行跟踪。采用常速度卡尔曼滤波模型,在过程误差协方差中考虑加速度,设状态变量为
X-[x,y,vx,vy,ψ,ω]T
其中,(x,y)表示跟踪目标的位置,((vx,vy)为跟踪目标的速度,ψ为跟踪目标的方向角,ω为跟踪目标的角速度。
卡尔曼滤波器的运动模型和观测模型分别为:
xk=Fkxk-1+wk-1
zk=Hkxk+vk
其中,Fk与Hk是已知系统矩阵和测量矩阵,wk-1和vk分别为均值为0,方差为Qk-1和Rk并相互独立的过程噪声和测量噪声。
xk表示为跟踪障碍物的位置、速度、加速度、角速度等信息。zk表示为在k时刻,各类型传感器的测量值。
卡尔曼滤波时间更新方程为:
测量更新方程为:
Pk+1|k+1=(I-Kk+1Hk+1)Pk+1|k
其中,K为卡尔曼滤波的增益矩阵。
本发明将一定规则加入到跟踪物体的状态,跟踪物体的观测状态,通过对区间进行分别处理,在获取同等检测跟踪效果的同时,有效的降低计算量和算法的总耗时量,大大提高跟踪的实时性。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (6)
1.一种基于激光传感器的快速运动障碍物实时跟踪方法,其特征在于,包括如下步骤:
(1)将激光雷达安装在车身上,以激光雷达的中心建立坐标系,并对坐标系进行内参标定和外参标定;
(2)通过激光雷达扫描白车周围,利用空间数据聚类算法组成障碍物;
(3)对步骤(2)所述的障碍物进行状态分类,包括初观测组和非初观测组;
(4)初次观测组的障碍物设置初始状态规则;
(5)对非初次观测组和已设置初始状态的初次观测组的障碍物均进行动态跟踪;
(6)生成障碍物信息列表;
步骤(5)所述的动态跟踪,包括将摄像头所采集的周围环境信息和激光雷达所采集的障碍物信息通过PF粒子滤波模型进行融合关联,使用KF扩展卡尔曼滤波模型生成跟踪信息;
动态跟踪的具体方法如下:
获取目标的运动信息,对日标进行跟踪;采用常速度卡尔曼滤波模型,在过程误差协方差中考虑加速度,设状态变量为
卡尔曼滤波器的运动模型和观测模型分别为:
xk=Fkxk-1+Wk-1
zk=Hkxk+Vk
其中,Fk与Hk为已知系统矩阵和测量矩阵,Wk-1和Vk分别为均值为0,方差为Qk-1和Rk并相互独立的过程噪声和测量噪声;
xk表示为跟踪障碍物的位置、速度、加速度、角速度信息;zk表示为在k时刻,各类型传感器的测量值;
卡尔曼滤波时间更新方程为:
测量更新方程为:
Pk+1|k+1=(/-Kk+1Hk+1)Pk+1|k
其中,k为卡尔曼滤波的增益矩阵。
2.根据权利要求1所述的一种基于激光传感器的快速运动障碍物实时跟踪方法,其特征在于:步骤(1)所述的内参标定包括旋转校正角度、垂直校正角度、距离校正角度、垂直便宜因子以及水平便宜因子;外参标定包括:激光雷达坐标系相对于车坐标系的旋转矩阵R和平移矩阵T。
3.根据权利要求1所述的一种基 于激光传感器的快速运动障碍物实时跟踪方法,其特征在:步骤(1)所述的激光雷达为单线激光雷达或多线激光雷达或全向激光雷达;所述的单线激光雷达安装在车身四周,多线激光雷达安装在车身两侧,所述的全向激光雷达安装在车身顶部。
4.根据权利要求1所述的一种基 于激光传感器的快速运动障碍物实时跟踪方法,特征在于:步骤(2)所述的空间数据聚类算法为k means- 算法,包括通过激光点云聚类,组成符合障碍物规则的高斯分布,判断为障碍物。
5.根据权利要求1所述的一种基 于激光传感器的快速运动障碍物实时跟踪方法,其特征在小:步骤(4)所述的初始状态规则包括结构化道路障碍物、非结构化道路两侧障碍物和非结构化道路中间障碍物。
6.根据权利要求1所述的一种基于激光传感器的快速运动障碍物实时跟踪方法,共特征在于:所述的车身上还安装有摄像头、惯导设备和GPS接收器。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711411184.0A CN108226951B (zh) | 2017-12-23 | 2017-12-23 | 一种基于激光传感器的快速运动障碍物实时跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711411184.0A CN108226951B (zh) | 2017-12-23 | 2017-12-23 | 一种基于激光传感器的快速运动障碍物实时跟踪方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108226951A CN108226951A (zh) | 2018-06-29 |
CN108226951B true CN108226951B (zh) | 2020-12-01 |
Family
ID=62648511
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711411184.0A Active CN108226951B (zh) | 2017-12-23 | 2017-12-23 | 一种基于激光传感器的快速运动障碍物实时跟踪方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108226951B (zh) |
Families Citing this family (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109254301A (zh) * | 2018-09-14 | 2019-01-22 | 杭州巨星科技股份有限公司 | 激光测距仪专用模组 |
CN109447161B (zh) * | 2018-10-31 | 2021-07-13 | 桂林电子科技大学 | 一种基于汽车雷达数据的实时增量与自适应聚类方法 |
TWI680898B (zh) * | 2018-11-23 | 2020-01-01 | 財團法人車輛研究測試中心 | 近距離障礙物之光達偵測裝置及其方法 |
CN109901193A (zh) * | 2018-12-03 | 2019-06-18 | 财团法人车辆研究测试中心 | 近距离障碍物的光达侦测装置及其方法 |
CN109582018A (zh) * | 2018-12-04 | 2019-04-05 | 黄昊 | 基于区块链的四维架构的智能驾驶方法、装置及系统 |
CN109597097B (zh) * | 2018-12-06 | 2023-04-18 | 北京主线科技有限公司 | 基于多线激光的扫描式障碍物检测方法 |
CN109858460B (zh) * | 2019-02-20 | 2022-06-10 | 重庆邮电大学 | 一种基于三维激光雷达的车道线检测方法 |
CN110068819B (zh) * | 2019-03-27 | 2021-08-31 | 东软睿驰汽车技术(沈阳)有限公司 | 一种提取障碍物位置信息的方法及装置 |
CN110007293B (zh) * | 2019-04-24 | 2021-11-02 | 禾多科技(北京)有限公司 | 场端多线束激光雷达的在线标定方法 |
CN110109144B (zh) * | 2019-04-30 | 2020-12-22 | 北京云迹科技有限公司 | 基于多线激光雷达的路肩检测方法及装置 |
CN110244322B (zh) * | 2019-06-28 | 2023-04-18 | 东南大学 | 基于多源传感器的路面施工机器人环境感知系统及方法 |
CN110320504B (zh) * | 2019-07-29 | 2021-05-18 | 浙江大学 | 一种基于激光雷达点云统计几何模型的非结构化道路检测方法 |
CN110481601B (zh) * | 2019-09-04 | 2022-03-08 | 深圳市镭神智能系统有限公司 | 一种轨道检测系统 |
CN110865394A (zh) * | 2019-09-24 | 2020-03-06 | 中国船舶重工集团公司第七0七研究所 | 一种基于激光雷达数据的目标分类系统及其数据处理方法 |
CN110909671B (zh) * | 2019-11-21 | 2020-09-29 | 大连理工大学 | 一种融合概率和高度信息的栅格地图障碍物检测方法 |
CN111308499A (zh) * | 2020-03-09 | 2020-06-19 | 中振同辂(江苏)机器人有限公司 | 一种基于多线激光雷达的障碍物检测方法 |
CN112327847A (zh) * | 2020-11-04 | 2021-02-05 | 北京石头世纪科技股份有限公司 | 一种绕行物体的方法、装置、介质和电子设备 |
CN112528778A (zh) * | 2020-11-27 | 2021-03-19 | 深兰科技(上海)有限公司 | 障碍物检测方法、装置、电子设备及存储介质 |
CN118243221B (zh) * | 2024-05-20 | 2024-08-09 | 国家海洋技术中心 | 一种光传感器的浸没因子测量装置及测量方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6187769B2 (ja) * | 2014-04-25 | 2017-08-30 | マツダ株式会社 | 追従走行制御装置 |
CN105741635A (zh) * | 2016-03-01 | 2016-07-06 | 武汉理工大学 | 一种多功能道路实验车平台 |
CN106291736A (zh) * | 2016-08-16 | 2017-01-04 | 张家港长安大学汽车工程研究院 | 无人驾驶汽车车道动态障碍物检测方法 |
CN107330925B (zh) * | 2017-05-11 | 2020-05-22 | 北京交通大学 | 一种基于激光雷达深度图像的多障碍物检测和跟踪方法 |
-
2017
- 2017-12-23 CN CN201711411184.0A patent/CN108226951B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN108226951A (zh) | 2018-06-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108226951B (zh) | 一种基于激光传感器的快速运动障碍物实时跟踪方法 | |
CN110208793B (zh) | 基于毫米波雷达的辅助驾驶系统、方法、终端和介质 | |
CN113490863B (zh) | 雷达辅助的单个图像三维深度重建 | |
US11494979B2 (en) | Bounding box estimation and lane vehicle association | |
CN107144839B (zh) | 通过传感器融合检测长对象 | |
US11527084B2 (en) | Method and system for generating a bird's eye view bounding box associated with an object | |
US11195028B2 (en) | Real-time simultaneous detection of lane marker and raised pavement marker for optimal estimation of multiple lane boundaries | |
US9255988B2 (en) | Object fusion system of multiple radar imaging sensors | |
CN108509972A (zh) | 一种基于毫米波和激光雷达的障碍物特征提取方法 | |
US11475678B2 (en) | Lane marker detection and lane instance recognition | |
US11544940B2 (en) | Hybrid lane estimation using both deep learning and computer vision | |
WO2020133217A1 (zh) | 连续障碍物检测方法、设备、系统及存储介质 | |
CN113189975B (zh) | 确定车辆的位置数据和/或运动数据的方法 | |
CN112781599B (zh) | 确定车辆的位置的方法 | |
CN114442101B (zh) | 基于成像毫米波雷达的车辆导航方法、装置、设备及介质 | |
US11709260B2 (en) | Data driven resolution function derivation | |
CN112597839B (zh) | 基于车载毫米波雷达的道路边界检测方法 | |
Lim et al. | Real-time forward collision warning system using nested Kalman filter for monocular camera | |
CN115151836A (zh) | 用于探测车辆周围环境中的移动对象的方法和机动车辆 | |
CN114419573A (zh) | 动态占据栅格估计方法及装置 | |
CN116022163A (zh) | 基于超局部子图的自动驾驶车辆扫描匹配和雷达姿态估计器 | |
TWI680898B (zh) | 近距離障礙物之光達偵測裝置及其方法 | |
Rabe et al. | Ego-lane estimation for downtown lane-level navigation | |
CN117250595B (zh) | 一种车载毫米波雷达金属井盖目标误报抑制方法 | |
CN115494506A (zh) | 基于毫米波雷达与视觉设备的非视距探测鬼探头预警方法 |
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 |