CN112285714A - 一种基于多传感器的障碍物速度融合方法和装置 - Google Patents

一种基于多传感器的障碍物速度融合方法和装置 Download PDF

Info

Publication number
CN112285714A
CN112285714A CN202010935568.8A CN202010935568A CN112285714A CN 112285714 A CN112285714 A CN 112285714A CN 202010935568 A CN202010935568 A CN 202010935568A CN 112285714 A CN112285714 A CN 112285714A
Authority
CN
China
Prior art keywords
obstacle
speed
vehicle
data
transverse
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.)
Pending
Application number
CN202010935568.8A
Other languages
English (en)
Inventor
史院平
韩志华
杨福威
王冠男
张旭
王启亮
黄文欢
张宇
李超
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou Zhitu Technology Co Ltd
Original Assignee
Suzhou Zhitu Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Suzhou Zhitu Technology Co Ltd filed Critical Suzhou Zhitu Technology Co Ltd
Priority to CN202010935568.8A priority Critical patent/CN112285714A/zh
Publication of CN112285714A publication Critical patent/CN112285714A/zh
Pending legal-status Critical Current

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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/52—Discriminating between fixed and moving objects or between objects moving at different speeds
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
    • G01S13/723—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar by using numerical data
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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
    • 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
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G01S2013/932—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles using own vehicle data, e.g. ground speed, steering wheel direction
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G01S2013/9323—Alternative operation using light waves

Abstract

本发明是一种基于多传感器的障碍物速度融合方法和装置，该方法包括：获取自车车身信息、车辆运行周围环境中的激光点云数据和毫米波雷达的障碍物数据；分别对激光点云和毫米波雷达的数据进行处理，得到障碍物的速度及其置信度；基于障碍物历史轨迹信息和自车车身信息，判断障碍物相对自车的运动趋势，采取不同方法融合障碍物的速度；基于自车车身信息确定障碍物的绝对速度。本发明充分利用了传感器的自身特性，结合自动驾驶应用场景，为自动驾驶车辆的障碍物感知提供了一种较准确的速度，保障自动驾驶车辆的安全和稳定运行。

Description

一种基于多传感器的障碍物速度融合方法和装置
技术领域
本发明涉及智能驾驶技术领域，具体涉及一种基于多传感器的障碍物速度融合方法和装置。
背景技术
自动驾驶的安全控制离不开准确的感知，而感知依赖于传感器的特性。激光雷达可以精准地实现障碍物相对自车的边缘轮廓感知，但也因为检测不到障碍物的另外一半边缘，从而导致其几何中心位置可能发生较大变化，从而导致测量的速度发生突变；毫米波雷达对障碍物的位置和速度都可以提供有效的感知，但也因为毫米波雷达自身硬件特性限制，其横向距离和位置可能存在较大变化，从而导致测量的速度不准确。因此，需要有效融合不同传感器的速度，提高障碍物速度的准确性，来保障自动驾驶的安全和平稳运行。
目前，现有的自动驾驶感知在对障碍物速度的检测方法中，一般依赖于毫米波雷达或者激光雷达单一传感器的数据，这些方法容易存在较大误差，尤其在自车转弯或者与障碍物有相对横向运动的时候，不能准确地反映障碍物的真实速度。
专利CN2017105384317-用于确定障碍物速度的方法和装置，其主要根据障碍物信息融合列表中是否包括激光雷达或毫米波雷达障碍物标识符，从而相应的采用绝对速度以及位置信息更新激光雷达或毫米波雷达障碍物标识符对应的历史速度信息和历史位置信息，但没有考虑到激光雷达或毫米波雷达传感器测量速度的特性，尤其是毫米波雷达对于非同向或相向而行的车辆，其横向距离和位置可能存在较大变化，导致测量的速度不准确，同时也会给融合速度滤波的输入带来波动性，从而影响融合速度的准确性。
发明内容
本发明的目的在于克服现有技术存在的问题，提供一种基于多传感器的障碍物速度融合方法和装置。
为实现上述技术目的，达到上述技术效果，本发明通过以下技术方案实现：
一种基于多传感器的障碍物速度融合方法，该方法包括：
步骤101）获取自车车身信息、车辆运行周围环境中的激光点云数据和毫米波雷达的障碍物数据；
步骤102）分别对激光点云和毫米波雷达的数据进行处理，得到障碍物的速度及其置信度；
步骤103）基于障碍物历史轨迹信息和自车车身信息，判断障碍物相对自车的运动趋势，采取不同方法融合障碍物的速度；
步骤104）基于自车车身信息确定障碍物的绝对速度。
进一步的，所述步骤102）中：
对毫米波雷达的数据进行处理时，选择对毫米波雷达的速度进行跟踪处理，计算得到较为准确的毫米波雷达障碍物的横纵向速度，并分别计算其横纵向速度的置信度；
对激光点云的数据进行处理，得到激光雷达障碍物的位置，并根据位置对时间微分求得速度，分别计算其横纵向速度的置信度。
进一步的，对所述激光点云的数据进行处理时，选择对激光点云的数据进行聚类处理和跟踪，求得激光雷达障碍物的速度，具体包括：
对激光点云进行聚类，计算聚类后障碍物的几何重心，作为障碍物的位置，对该位置进行跟踪处理，得到准确的激光雷达障碍物的位置；
对激光雷达障碍物的位置与时间进行微分求得速度，对速度进行跟踪处理，得到准确的激光雷达障碍物的速度，并分别计算其横纵向速度的置信度。
进一步的，所述步骤103）中：
障碍物，为传感器测量的障碍物与融合列表中障碍物进行关联匹配上的障碍物；
障碍物历史轨迹信息，为融合列表中保存的障碍物的位置、速度、类别和形状大小信息；
自车车身信息，至少包括自车的速度、航向角、航向角速度信息；
根据障碍物历史轨迹信息，计算其速度的方向角与自车的航向角的差异，判断障碍物相对自车的运动趋势；
根据障碍物相对自车的运动趋势，采取不同的方法融合障碍物的速度。
进一步的，所述判断障碍物相对自车的运动趋势的方法如下：
首先，保存自车的航向角历史帧数据，对数据进行过滤平滑，计算出自车当前航向角与历史帧航向角差异，若差异超过设定的阈值，则判断自车相对障碍物在转弯状态，障碍物与自车同时转弯情形也作此处理；
其次，保存障碍物历史轨迹信息，并判断与自车的相对运动状态，障碍物相对自车速度方向为theta，取x和y分别为相对自车的位置，其计算公式为：theta=arcsin(y/sqrt(x*x+y*y))，如果theta>PI/2.0,并且theta<=PI,则theta=PI-diff_angle;如果theta>PI,并且theta<=3*PI/2.0,则theta=diff_angle-PI;如果theta>3*PI/2.0,并且theta<=2*PI,则theta=2*PI-theta，其中PI为圆周率;
最后，判断theta是否超过设定的阈值，若超过，则判断障碍物与自车在相对转弯状态，否则，判断障碍物与自车在同向或者相向状态。
进一步的，采取不同的方法融合障碍物的速度，包括：
根据障碍物相对自车的运动趋势，分为障碍物与自车运动是同向或者相向，以及除此之外的情形；
当障碍物与自车运动是同向或者相向时，优先选择毫米波雷达的纵向速度以及激光雷达的横向速度作为测量值，与预测值进行加权融合；
当障碍物与自车运动为上述之外的情形时，优先选择激光雷达的纵向速度和横向速度作为测量值，与预测值进行加权融合；
更新历史列表上障碍物速度的滤波器，以及历史轨迹信息。
进一步的，选择毫米波雷达的纵向速度以及激光雷达的横向速度作为测量值，与预测值进行加权融合时，包括：
a.判断毫米波雷达的纵向速度置信度是否符合要求，若符合，选择毫米波雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；
b.若上述a中不符合，则接着判断激光雷达的纵向速度置信度是否符合要求，若符合，则选择激光雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；若不符合，则根据历史帧纵向速度值预测当前帧的纵向速度值，作为最终障碍物的纵向速度；
c.接着判断激光雷达的横向速度置信度是否符合要求，若符合，选择激光雷达的横向速度作为障碍物的测量值与预测值加权后得到最终障碍物的横向速度；若不符合，则选择历史帧的横向速度值预测当前帧的横向速度值，作为最终障碍物的横向速度。
进一步的，选择激光雷达的纵向速度和横向速度作为测量值，与预测值进行加权融合时，包括：
a.判断激光雷达的纵向速度置信度是否符合要求，若符合，选择激光雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；
b.若上述a不符合，则判断毫米波雷达的纵向速度置信度是否符合要求，若符合，则选择毫米波雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；若不符合，则选择历史帧的纵向速度值，预测当前帧的纵向速度值，作为最终障碍物的纵向速度；
c.判断激光雷达的横向速度置信度是否符合要求，若符合，选择激光雷达的横向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的横向速度；若不符合，则选择历史帧的横向速度值预测当前帧的横向速度值，作为最终障碍物的横向速度。
一种基于多传感器的障碍物速度融合装置，该装置包括：
数据获取模块，用于获取自车车身信息、激光点云和毫米波雷达障碍物数据；
传感器数据处理模块，用于传感器标定、毫米波雷达的障碍物数据处理、激光点云的数据处理；
关联匹配模块，为传感器测量障碍物与融合列表中障碍物进行关联匹配计算，作为数据融合的基础；
数据融合模块，用于障碍物的位置、形状、类别和速度的融合；
数据维护和发布模块，用于更新匹配和未匹配障碍物的信息，发布障碍物的数据；
自动驾驶车辆通过处理器分别连接数据获取模块、传感器数据处理模块、关联匹配模块、数据融合模块、数据维护和发布模块。
进一步的，所述数据获取模块包括：
获取车身信息子模块，用于获取自车的车速、航向角、航向角速度和档位信息；
获取激光点云信息子模块，用于获取所有激光雷达的点云信息；
获取毫米波雷达障碍物数据子模块，用于获取毫米波雷达的障碍物数据；
所述传感器数据处理模块，包括：
传感器标定子模块，用于将传感器标定到同一车体坐标系下；
毫米波雷达数据处理子模块，用于对毫米波雷达的障碍物的位置和速度进行跟踪处理，并分别计算得到其横纵向速度的置信度；
激光点云的数据处理子模块，用于对激光点云进行聚类，得到障碍物的几何重心位置，对其重心位置进行跟踪处理，根据前后帧重心位置差计算得到激光雷达障碍物的速度，并对其速度进行跟踪处理，分别计算得到其横纵向速度的置信度；
所述关联匹配模块，包括：
距离矩阵计算子模块，用于计算障碍物之间的距离矩阵；
关联距离阈值设置子模块，用于根据不同传感器以及类别，动态设置不同的关联距离阈值；
匹配序列对计算子模块，根据距离矩阵计算子模块和关联距离阈值设置子模块，进行匹配序列对计算，确定最终的匹配关系；
所述数据融合模块，包括：
位置融合子模块，用于确定所述障碍物的准确位置；
速度融合子模块，用于将传感器的速度测量值和跟踪列表中相应的速度预测值进行加权，并基于车身信息，确定障碍物的准确速度；
形状融合子模块，用于确定障碍物的凸包点；
类别融合子模块，用于确定障碍物的类别；
所述数据维护和发布模块，包括：
数据维护子模块，为更新匹配的障碍物信息，对未匹配的跟踪列表中的障碍物进行预测和删除，对未匹配的传感器测量的障碍物有条件地创建为新的跟踪对象；
数据发布子模块，为对障碍物数据进行后处理，确定障碍物是否可发布，从而将障碍物数据发布出去。
本发明的有益效果是:
1、本发明对激光进行聚类后，求取的是几何重心，相较目前大多数聚类后求取的几何中心，有效减小形状对于位置的影响，再对位置进行跟踪处理，有效保证激光雷达障碍物位置数据的稳定性，确保激光雷达障碍物速度计算的准确性。
2、相较于传感器输入的速度，进行了数据的合理性判断，排除异常不合理或者异常跳变的数据，确保了输入数据的准确性。
3、相较目前单一应用某一种传感器的速度，判断了障碍物相对自车的运动趋势，考虑了不同传感器在不同场景下的探测性能及其局限性，综合融合障碍物的横纵向速度，并对横纵向速度进行跟踪处理，保证横纵向速度的准确性。
4、本发明方法在进行速度融合之前，充分考虑了各传感器的优缺点和运行场景，确保了障碍物的速度准确，保障了自动驾驶车辆的平稳和安全运行。
附图说明
图1为本发明实施例提供的速度融合流程框图；
图2为本发明实施例提供的速度融合详细流程图；
图3为本发明实施例提供的装置简图；
图4为本发明实施例提供的装置详细模块图；
图5为本发明实施例提供的车辆坐标系示意图。
具体实施方式
下面将参考附图并结合实施例，来详细说明本发明。
如图1、图2和图5所示，一种基于多传感器的障碍物速度融合方法，该方法包括：
步骤101）获取自车车身信息、车辆运行周围环境中的激光点云数据和毫米波雷达的障碍物数据；
步骤102）分别对激光点云和毫米波雷达的数据进行处理，得到障碍物的速度及其置信度；
步骤103）基于障碍物历史轨迹信息和自车车身信息，判断障碍物相对自车的运动趋势，采取不同方法融合障碍物的速度；
步骤104）基于自车车身信息确定障碍物的绝对速度。
所述步骤102）中：
对毫米波雷达的数据进行处理时，选择对毫米波雷达的速度进行跟踪处理，计算得到较为准确的毫米波雷达障碍物的横纵向速度，并分别计算其横纵向速度的置信度；速度置信度计算如下：
S=sqrt(((x1-x)^2+(x2-x)^2+....(xn-x)^2)/(n-1))，
如上式，首先分别计算横纵向标准差，同时保存历史帧横纵向速度值，默认设置速度置信度为1，如果当前标准差s超过一定阈值，并且当前的速度值与历史帧存在较大波动，则设置信度为0。
对激光点云的数据进行处理，得到激光雷达障碍物的位置，并根据位置对时间微分求得速度，分别计算其横纵向速度的置信度。
对所述激光点云的数据进行处理时，选择对激光点云的数据进行聚类处理和跟踪，求得激光雷达障碍物的速度，具体包括：
对激光点云进行聚类，计算聚类后障碍物的几何重心，作为障碍物的位置，对该位置进行跟踪处理，得到准确的激光雷达障碍物的位置；聚类后障碍物的几何重心具体计算如下：
x=Σxi/n,y=Σyi/n,z=Σzi/n，
其中，Σxi为激光聚类后的凸包多边形顶点的x坐标值的求和，同理Σyi和Σzi分别是激光聚类后的凸包多边形顶点的y坐标值和z坐标值的求和；
对激光雷达障碍物的位置与时间进行微分求得速度，对速度进行跟踪处理，得到准确的激光雷达障碍物的速度，并分别计算其横纵向速度的置信度，速度置信度的计算方法同上。
所述步骤103）中：
障碍物，为传感器测量的障碍物与融合列表中障碍物进行关联匹配上的障碍物；
障碍物历史轨迹信息，为融合列表中保存的障碍物的位置、速度、类别和形状大小信息；
自车车身信息，至少包括自车的速度、航向角、航向角速度信息；
根据障碍物历史轨迹信息，计算其速度的方向角与自车的航向角的差异，判断障碍物相对自车的运动趋势；
根据障碍物相对自车的运动趋势，采取不同的方法融合障碍物的速度。
所述判断障碍物相对自车的运动趋势的方法如下：
首先，保存自车的航向角历史帧数据，对数据进行过滤平滑，计算出自车当前航向角与历史帧航向角差异，若差异超过设定的阈值，则判断自车相对障碍物在转弯状态，障碍物与自车同时转弯情形也作此处理；
其次，保存障碍物历史轨迹信息，并判断与自车的相对运动状态，障碍物相对自车速度方向为theta，取x和y分别为相对自车的位置，其计算公式为：theta=arcsin(y/sqrt(x*x+y*y))，如果theta>PI/2.0,并且theta<=PI,则theta=PI-diff_angle;如果theta>PI,并且theta<=3*PI/2.0,则theta=diff_angle-PI;如果theta>3*PI/2.0,并且theta<=2*PI,则theta=2*PI-theta，其中PI为圆周率，可取3.1415926;
最后，判断theta是否超过设定的阈值，若超过，则判断障碍物与自车在相对转弯状态，否则，判断障碍物与自车在同向或者相向状态。
采取不同的方法融合障碍物的速度，包括：
根据障碍物相对自车的运动趋势，分为障碍物与自车运动是同向或者相向，以及除此之外的情形；
当障碍物与自车运动是同向或者相向时，优先选择毫米波雷达的纵向速度以及激光雷达的横向速度作为测量值，与预测值进行加权融合；
当障碍物与自车运动为上述之外的情形时，优先选择激光雷达的纵向速度和横向速度作为测量值，与预测值进行加权融合；
更新历史列表上障碍物速度的滤波器，以及历史轨迹信息。
选择毫米波雷达的纵向速度以及激光雷达的横向速度作为测量值，与预测值进行加权融合时，包括：
a.判断毫米波雷达的纵向速度置信度是否符合要求，若符合，选择毫米波雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；
b.若上述a中不符合，则接着判断激光雷达的纵向速度置信度是否符合要求，若符合，则选择激光雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；若不符合，则根据历史帧纵向速度值预测当前帧的纵向速度值，作为最终障碍物的纵向速度；
c.接着判断激光雷达的横向速度置信度是否符合要求，若符合，选择激光雷达的横向速度作为障碍物的测量值与预测值加权后得到最终障碍物的横向速度；若不符合，则选择历史帧的横向速度值预测当前帧的横向速度值，作为最终障碍物的横向速度。
选择激光雷达的纵向速度和横向速度作为测量值，与预测值进行加权融合时，包括：
a.判断激光雷达的纵向速度置信度是否符合要求，若符合，选择激光雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；
b.若上述a不符合，则判断毫米波雷达的纵向速度置信度是否符合要求，若符合，则选择毫米波雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；若不符合，则选择历史帧的纵向速度值，预测当前帧的纵向速度值，作为最终障碍物的纵向速度；
c.判断激光雷达的横向速度置信度是否符合要求，若符合，选择激光雷达的横向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的横向速度；若不符合，则选择历史帧的横向速度值预测当前帧的横向速度值，作为最终障碍物的横向速度；
而后，更新历史列表上述障碍物速度的滤波器，以及历史轨迹信息。
在步骤104）中，自动驾驶车辆在运行时，需要准确感知周围场景中的障碍物的绝对速度，以做出不同的路径规划和控制策略，保障车辆的安全和平稳运行。
如图3、图4和图5所示，一种基于多传感器的障碍物速度融合装置，该装置包括：
数据获取模块，用于获取自车车身信息、激光点云和毫米波雷达障碍物数据；
传感器数据处理模块，用于传感器标定、毫米波雷达的障碍物数据处理、激光点云的数据处理；
关联匹配模块，为传感器测量障碍物与融合列表中障碍物进行关联匹配计算，作为数据融合的基础；
数据融合模块，用于障碍物的位置、形状、类别和速度的融合；
数据维护和发布模块，用于更新匹配和未匹配障碍物的信息，发布障碍物的数据；
自动驾驶车辆通过处理器分别连接数据获取模块、传感器数据处理模块、关联匹配模块、数据融合模块、数据维护和发布模块。
所述数据获取模块包括：
获取车身信息子模块，用于获取自车的车速、航向角、航向角速度和档位信息；
获取激光点云信息子模块，用于获取所有激光雷达的点云信息；
获取毫米波雷达障碍物数据子模块，用于获取毫米波雷达的障碍物数据；
所述传感器数据处理模块，包括：
传感器标定子模块，用于将传感器标定到同一车体坐标系下；
毫米波雷达数据处理子模块，用于对毫米波雷达的障碍物的位置和速度进行跟踪处理，并分别计算得到其横纵向速度的置信度；
激光点云的数据处理子模块，用于对激光点云进行聚类，得到障碍物的几何重心位置，对其重心位置进行跟踪处理，根据前后帧重心位置差计算得到激光雷达障碍物的速度，并对其速度进行跟踪处理，分别计算得到其横纵向速度的置信度；
所述关联匹配模块，包括：
距离矩阵计算子模块，用于计算障碍物之间的距离矩阵；
关联距离阈值设置子模块，用于根据不同传感器以及类别，动态设置不同的关联距离阈值；
匹配序列对计算子模块，根据距离矩阵计算子模块和关联距离阈值设置子模块，进行匹配序列对计算，确定最终的匹配关系；
所述数据融合模块，包括：
位置融合子模块，用于确定所述障碍物的准确位置；
速度融合子模块，用于将传感器的速度测量值和跟踪列表中相应的速度预测值进行加权，并基于车身信息，确定障碍物的准确速度；
形状融合子模块，用于确定障碍物的凸包点；
类别融合子模块，用于确定障碍物的类别；
所述数据维护和发布模块，包括：
数据维护子模块，为更新匹配的障碍物信息，对未匹配的跟踪列表中的障碍物进行预测和删除，对未匹配的传感器测量的障碍物有条件地创建为新的跟踪对象；
数据发布子模块，为对障碍物数据进行后处理，确定障碍物是否可发布，从而将障碍物数据发布出去。
本发明原理
本发明对激光进行聚类后，求取的是几何重心，相较目前大多数聚类后求取的几何中心，有效减小形状对于位置的影响；再对位置进行跟踪处理，有效保证激光雷达障碍物位置数据的稳定性，确保激光雷达障碍物速度计算的准确性。
相较于传感器输入的速度，进行了数据的合理性判断，排除异常不合理或者异常跳变的数据，确保了输入数据的准确性。
再者，相较目前单一应用某一种传感器的速度，计算了障碍物相对自车的运动方向，考虑了不同传感器在不同场景下的探测性能及其局限性，综合融合障碍物的横纵向速度，并对横纵向速度进行跟踪处理，保证横纵向速度的准确性。
本发明方法在进行速度融合之前，充分考虑了各传感器的优缺点和运行场景，确保了障碍物的速度准确，保障了自动驾驶车辆的平稳和安全运行。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于多传感器的障碍物速度融合方法，其特征在于，该方法包括：
步骤101)获取自车车身信息、车辆运行周围环境中的激光点云数据和毫米波雷达的障碍物数据；
步骤102)分别对激光点云和毫米波雷达的数据进行处理，得到障碍物的速度及其置信度；
步骤103)基于障碍物历史轨迹信息和自车车身信息，判断障碍物相对自车的运动趋势，采取不同方法融合障碍物的速度；
步骤104)基于自车车身信息确定障碍物的绝对速度。
2.根据权利要求1所述的基于多传感器的障碍物速度融合方法，其特征在于，所述步骤102)中：
对毫米波雷达的数据进行处理时，选择对毫米波雷达的速度进行跟踪处理，计算得到较为准确的毫米波雷达障碍物的横纵向速度，并分别计算其横纵向速度的置信度；
对激光点云的数据进行处理，得到激光雷达障碍物的位置，并根据位置对时间微分求得速度，分别计算其横纵向速度的置信度。
3.根据权利要求2所述的基于多传感器的障碍物速度融合方法，其特征在于，对所述激光点云的数据进行处理时，选择对激光点云的数据进行聚类处理和跟踪，求得激光雷达障碍物的速度，具体包括：
对激光点云进行聚类，计算聚类后障碍物的几何重心，作为障碍物的位置，对该位置进行跟踪处理，得到准确的激光雷达障碍物的位置；
对激光雷达障碍物的位置与时间进行微分求得速度，对速度进行跟踪处理，得到准确的激光雷达障碍物的速度，并分别计算其横纵向速度的置信度。
4.根据权利要求1所述的基于多传感器的障碍物速度融合方法，其特征在于，所述步骤103)中：
障碍物，为传感器测量的障碍物与融合列表中障碍物进行关联匹配上的障碍物；
障碍物历史轨迹信息，为融合列表中保存的障碍物的位置、速度、类别和形状大小信息；
自车车身信息，至少包括自车的速度、航向角、航向角速度信息；
根据障碍物历史轨迹信息，计算其速度的方向角与自车的航向角的差异，判断障碍物相对自车的运动趋势；
根据障碍物相对自车的运动趋势，采取不同的方法融合障碍物的速度。
5.根据权利要求4所述的基于多传感器的障碍物速度融合方法，其特征在于，所述判断障碍物相对自车的运动趋势的方法如下：
首先，保存自车的航向角历史帧数据，对数据进行过滤平滑，计算出自车当前航向角与历史帧航向角差异，若差异超过设定的阈值，则判断自车相对障碍物在转弯状态，障碍物与自车同时转弯情形也作此处理；
其次，保存障碍物历史轨迹信息，并判断与自车的相对运动状态，障碍物相对自车速度方向为theta，取x和y分别为相对自车的位置，其计算公式为：theta＝arcsin(y/sqrt(x*x+y*y))，如果theta>PI/2.0,并且theta<＝PI,则theta＝PI-diff_angle；如果theta>PI,并且theta<＝3*PI/2.0,则theta＝diff_angle-PI；如果theta>3*PI/2.0,并且theta<＝2*PI,则theta＝2*PI-theta，其中PI为圆周率；
最后，判断theta是否超过设定的阈值，若超过，则判断障碍物与自车在相对转弯状态，否则，判断障碍物与自车在同向或者相向状态。
6.根据权利要求4所述的基于多传感器的障碍物速度融合方法，其特征在于，采取不同的方法融合障碍物的速度，包括：
根据障碍物相对自车的运动趋势，分为障碍物与自车运动是同向或者相向，以及除此之外的情形；
当障碍物与自车运动是同向或者相向时，优先选择毫米波雷达的纵向速度以及激光雷达的横向速度作为测量值，与预测值进行加权融合；
当障碍物与自车运动为上述之外的情形时，优先选择激光雷达的纵向速度和横向速度作为测量值，与预测值进行加权融合；
更新历史列表上障碍物速度的滤波器，以及历史轨迹信息。
7.根据权利要求6所述的基于多传感器的障碍物速度融合方法，其特征在于，选择毫米波雷达的纵向速度以及激光雷达的横向速度作为测量值，与预测值进行加权融合时，包括：
a.判断毫米波雷达的纵向速度置信度是否符合要求，若符合，选择毫米波雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；
b.若上述a中不符合，则接着判断激光雷达的纵向速度置信度是否符合要求，若符合，则选择激光雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；若不符合，则根据历史帧纵向速度值预测当前帧的纵向速度值，作为最终障碍物的纵向速度；
c.接着判断激光雷达的横向速度置信度是否符合要求，若符合，选择激光雷达的横向速度作为障碍物的测量值与预测值加权后得到最终障碍物的横向速度；若不符合，则选择历史帧的横向速度值预测当前帧的横向速度值，作为最终障碍物的横向速度。
8.根据权利要求6所述的基于多传感器的障碍物速度融合方法，其特征在于，选择激光雷达的纵向速度和横向速度作为测量值，与预测值进行加权融合时，包括：
a.判断激光雷达的纵向速度置信度是否符合要求，若符合，选择激光雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；
b.若上述a不符合，则判断毫米波雷达的纵向速度置信度是否符合要求，若符合，则选择毫米波雷达的纵向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的纵向速度；若不符合，则选择历史帧的纵向速度值，预测当前帧的纵向速度值，作为最终障碍物的纵向速度；
c.判断激光雷达的横向速度置信度是否符合要求，若符合，选择激光雷达的横向速度作为障碍物的测量值与预测值加权后，得到最终障碍物的横向速度；若不符合，则选择历史帧的横向速度值预测当前帧的横向速度值，作为最终障碍物的横向速度。
9.一种基于多传感器的障碍物速度融合装置，其特征在于，该装置包括：
数据获取模块，用于获取自车车身信息、激光点云和毫米波雷达障碍物数据；
传感器数据处理模块，用于传感器标定、毫米波雷达的障碍物数据处理、激光点云的数据处理；
关联匹配模块，为传感器测量障碍物与融合列表中障碍物进行关联匹配计算，作为数据融合的基础；
数据融合模块，用于障碍物的位置、形状、类别和速度的融合；
数据维护和发布模块，用于更新匹配和未匹配障碍物的信息，发布障碍物的数据；
自动驾驶车辆通过处理器分别连接数据获取模块、传感器数据处理模块、关联匹配模块、数据融合模块、数据维护和发布模块。
10.根据权利要求9所述的基于多传感器的障碍物速度融合装置，其特征在于，所述数据获取模块包括：
获取车身信息子模块，用于获取自车的车速、航向角、航向角速度和档位信息；
获取激光点云信息子模块，用于获取所有激光雷达的点云信息；
获取毫米波雷达障碍物数据子模块，用于获取毫米波雷达的障碍物数据；
所述传感器数据处理模块，包括：
传感器标定子模块，用于将传感器标定到同一车体坐标系下；
毫米波雷达数据处理子模块，用于对毫米波雷达的障碍物的位置和速度进行跟踪处理，并分别计算得到其横纵向速度的置信度；
激光点云的数据处理子模块，用于对激光点云进行聚类，得到障碍物的几何重心位置，对其重心位置进行跟踪处理，根据前后帧重心位置差计算得到激光雷达障碍物的速度，并对其速度进行跟踪处理，分别计算得到其横纵向速度的置信度；
所述关联匹配模块，包括：
距离矩阵计算子模块，用于计算障碍物之间的距离矩阵；
关联距离阈值设置子模块，用于根据不同传感器以及类别，动态设置不同的关联距离阈值；
匹配序列对计算子模块，根据距离矩阵计算子模块和关联距离阈值设置子模块，进行匹配序列对计算，确定最终的匹配关系；
所述数据融合模块，包括：
位置融合子模块，用于确定所述障碍物的准确位置；
速度融合子模块，用于将传感器的速度测量值和跟踪列表中相应的速度预测值进行加权，并基于车身信息，确定障碍物的准确速度；
形状融合子模块，用于确定障碍物的凸包点；
类别融合子模块，用于确定障碍物的类别；
所述数据维护和发布模块，包括：
数据维护子模块，为更新匹配的障碍物信息，对未匹配的跟踪列表中的障碍物进行预测和删除，对未匹配的传感器测量的障碍物有条件地创建为新的跟踪对象；
数据发布子模块，为对障碍物数据进行后处理，确定障碍物是否可发布，从而将障碍物数据发布出去。
CN202010935568.8A 2020-09-08 2020-09-08 一种基于多传感器的障碍物速度融合方法和装置 Pending CN112285714A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010935568.8A CN112285714A (zh) 2020-09-08 2020-09-08 一种基于多传感器的障碍物速度融合方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010935568.8A CN112285714A (zh) 2020-09-08 2020-09-08 一种基于多传感器的障碍物速度融合方法和装置

Publications (1)

Publication Number Publication Date
CN112285714A true CN112285714A (zh) 2021-01-29

Family

ID=74420351

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010935568.8A Pending CN112285714A (zh) 2020-09-08 2020-09-08 一种基于多传感器的障碍物速度融合方法和装置

Country Status (1)

Country Link
CN (1) CN112285714A (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113442915A (zh) * 2021-08-17 2021-09-28 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) 一种自动避障天线
CN113514806A (zh) * 2021-04-02 2021-10-19 苏州挚途科技有限公司 自动驾驶过程中障碍物确定方法、装置及电子设备
CN113650607A (zh) * 2021-07-20 2021-11-16 江铃汽车股份有限公司 一种低速场景自动驾驶方法、系统及汽车
CN113741480A (zh) * 2021-09-16 2021-12-03 中科南京软件技术研究院 一种基于动态障碍物提取与代价地图相结合的避障方法
CN114170274A (zh) * 2022-02-11 2022-03-11 北京宏景智驾科技有限公司 目标跟踪方法和装置、电子设备和存储介质
CN114999228A (zh) * 2022-06-01 2022-09-02 北京易航远智科技有限公司 一种自动驾驶车在恶劣天气防碰撞方法
CN115311852A (zh) * 2022-07-12 2022-11-08 重庆长安汽车股份有限公司 交通拥堵的预测方法、系统、车辆及存储介质
WO2022242111A1 (zh) * 2021-05-21 2022-11-24 阿波罗智能技术(北京)有限公司 障碍物检测方法以及装置
WO2023050679A1 (zh) * 2021-09-30 2023-04-06 上海商汤智能科技有限公司 一种障碍物的检测方法、装置、计算机设备、存储介质、计算机程序及计算机程序产品

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6061015A (en) * 1998-01-14 2000-05-09 Honda Giken Kogyo Kabushiki Kaisha Vehicle obstacle detecting system
CN108764373A (zh) * 2018-06-08 2018-11-06 北京领骏科技有限公司 一种用于自动驾驶中的传感器数据过滤及融合方法
CN109212530A (zh) * 2017-07-04 2019-01-15 百度在线网络技术（北京）有限公司 用于确定障碍物速度的方法和装置
CN109298415A (zh) * 2018-11-20 2019-02-01 中车株洲电力机车有限公司 一种轨道和道路障碍物检测方法
CN109709961A (zh) * 2018-12-28 2019-05-03 百度在线网络技术（北京）有限公司 道路障碍物检测方法、装置及自动驾驶汽车
CN110239535A (zh) * 2019-07-03 2019-09-17 国唐汽车有限公司 一种基于多传感器融合的弯道主动避撞控制方法
CN110673599A (zh) * 2019-09-29 2020-01-10 北京邮电大学 基于传感器网络的自动驾驶车辆环境感知系统
CN110764108A (zh) * 2019-11-05 2020-02-07 畅加风行(苏州)智能科技有限公司 一种用于港口自动驾驶场景的障碍物检测方法及装置

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6061015A (en) * 1998-01-14 2000-05-09 Honda Giken Kogyo Kabushiki Kaisha Vehicle obstacle detecting system
CN109212530A (zh) * 2017-07-04 2019-01-15 百度在线网络技术（北京）有限公司 用于确定障碍物速度的方法和装置
CN108764373A (zh) * 2018-06-08 2018-11-06 北京领骏科技有限公司 一种用于自动驾驶中的传感器数据过滤及融合方法
CN109298415A (zh) * 2018-11-20 2019-02-01 中车株洲电力机车有限公司 一种轨道和道路障碍物检测方法
CN109709961A (zh) * 2018-12-28 2019-05-03 百度在线网络技术（北京）有限公司 道路障碍物检测方法、装置及自动驾驶汽车
CN110239535A (zh) * 2019-07-03 2019-09-17 国唐汽车有限公司 一种基于多传感器融合的弯道主动避撞控制方法
CN110673599A (zh) * 2019-09-29 2020-01-10 北京邮电大学 基于传感器网络的自动驾驶车辆环境感知系统
CN110764108A (zh) * 2019-11-05 2020-02-07 畅加风行(苏州)智能科技有限公司 一种用于港口自动驾驶场景的障碍物检测方法及装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
辛煜;梁华为;梅涛;黄如林;杜明博;王智灵;陈佳佳;赵盼;: "基于激光传感器的无人驾驶汽车动态障碍物检测及表示方法", 机器人, no. 06, pages 654 - 661 *

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113514806A (zh) * 2021-04-02 2021-10-19 苏州挚途科技有限公司 自动驾驶过程中障碍物确定方法、装置及电子设备
CN113514806B (zh) * 2021-04-02 2023-12-19 苏州挚途科技有限公司 自动驾驶过程中障碍物确定方法、装置及电子设备
WO2022242111A1 (zh) * 2021-05-21 2022-11-24 阿波罗智能技术(北京)有限公司 障碍物检测方法以及装置
CN113650607A (zh) * 2021-07-20 2021-11-16 江铃汽车股份有限公司 一种低速场景自动驾驶方法、系统及汽车
CN113442915A (zh) * 2021-08-17 2021-09-28 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) 一种自动避障天线
CN113442915B (zh) * 2021-08-17 2022-07-15 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) 一种自动避障天线
CN113741480A (zh) * 2021-09-16 2021-12-03 中科南京软件技术研究院 一种基于动态障碍物提取与代价地图相结合的避障方法
WO2023050679A1 (zh) * 2021-09-30 2023-04-06 上海商汤智能科技有限公司 一种障碍物的检测方法、装置、计算机设备、存储介质、计算机程序及计算机程序产品
CN114170274A (zh) * 2022-02-11 2022-03-11 北京宏景智驾科技有限公司 目标跟踪方法和装置、电子设备和存储介质
CN114999228A (zh) * 2022-06-01 2022-09-02 北京易航远智科技有限公司 一种自动驾驶车在恶劣天气防碰撞方法
CN114999228B (zh) * 2022-06-01 2022-12-16 北京易航远智科技有限公司 一种自动驾驶车在恶劣天气防碰撞方法
CN115311852A (zh) * 2022-07-12 2022-11-08 重庆长安汽车股份有限公司 交通拥堵的预测方法、系统、车辆及存储介质

Similar Documents

Publication Publication Date Title
CN112285714A (zh) 一种基于多传感器的障碍物速度融合方法和装置
US9975550B2 (en) Movement trajectory predicting device and movement trajectory predicting method
WO2019218861A1 (zh) 一种行车道路的估计方法以及行车道路估计系统
CN110214106B (zh) 可操作用于确定车道的一部分的位置的装置
CN111578894B (zh) 一种确定障碍物航向角的方法及装置
JP2004508627A (ja) 経路予測システムおよび方法
CN110929796B (zh) 基于多源传感器决策层数据融合方法、系统及存储介质
CN109696676B (zh) 一种有效障碍物目标确定方法、装置和车辆
WO2021056499A1 (zh) 数据处理方法、设备和可移动平台
CN115993597A (zh) 一种视觉雷达感知融合方法及终端设备
EP4024974A1 (en) Data processing method and apparatus, chip system, and medium
US6956959B2 (en) Apparatus for recognizing environment
CN115923839A (zh) 一种车辆路径规划方法
CN114475593B (zh) 行驶轨迹预测方法、车辆及计算机可读存储介质
US11080562B1 (en) Key point recognition with uncertainty measurement
CN113432615B (zh) 基于多传感器融合可驾驶区域的检测方法、系统和车辆
CN115223131A (zh) 一种自适应巡航的跟随目标车辆检测方法、装置及汽车
EP3467545A1 (en) Object classification
JP2022550762A (ja) 衝突認識手段を用いた動力車両の周辺領域内にある離れたターゲット車両を追跡するための方法
KR20200133122A (ko) 차량 충돌 방지 장치 및 방법
CN111959482A (zh) 自主驾驶装置及方法
Kim et al. Fusing lidar, radar, and camera using extended Kalman filter for estimating the forward position of vehicles
CN115285128A (zh) 一种危险目标的确定方法、装置、设备及存储介质
CN114670851A (zh) 基于寻优跟踪算法的辅助驾驶系统、方法、终端及介质
CN114282776A (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