CN112084937A - 一种动态车辆检测方法 - Google Patents

一种动态车辆检测方法 Download PDF

Info

Publication number
CN112084937A
CN112084937A CN202010935148.XA CN202010935148A CN112084937A CN 112084937 A CN112084937 A CN 112084937A CN 202010935148 A CN202010935148 A CN 202010935148A CN 112084937 A CN112084937 A CN 112084937A
Authority
CN
China
Prior art keywords
dynamic
point cloud
particle
target
vehicle
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.)
Granted
Application number
CN202010935148.XA
Other languages
English (en)
Other versions
CN112084937B (zh
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.)
Tsinghua University
Original Assignee
Tsinghua University
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 Tsinghua University filed Critical Tsinghua University
Priority to CN202010935148.XA priority Critical patent/CN112084937B/zh
Publication of CN112084937A publication Critical patent/CN112084937A/zh
Application granted granted Critical
Publication of CN112084937B publication Critical patent/CN112084937B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G06V20/58—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads
    • G06V20/584—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads of vehicle lights or traffic lights
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V2201/00—Indexing scheme relating to image or video recognition or understanding
    • G06V2201/08—Detecting or categorising vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Multimedia (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Biomedical Technology (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Evolutionary Biology (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种动态车辆检测方法，该方法包括：步骤421，根据动态车辆的位姿初值，查看是否有关联动态目标，设置有关联动态目标和无关联动态目标时的粒子群初始化参数；步骤422，计算粒子的适应度函数值；步骤423，将粒子位置对应适应度函数值与上一次循环结果进行比较，将更大适应度函数值及粒子位置赋值为该粒子个体最大适应值及最佳位置，选取最大适应度函数值对应位置为全局最佳位置；步骤424，利用粒子个体和全局最佳位置，更新粒子速度和位置；步骤425，返回步骤422，直到满足最大循环次数或最佳位置变化小于预设阈值，输出全局最佳粒子即为优化后的位姿估计结果。本发明方法能够提升检测性能，提高正确检测数量，并能有效降低检测虚警数量。

Description

一种动态车辆检测方法
技术领域
本发明涉及智能交通系统安全驾驶技术领域，特别是关于一种动态车辆检测方法。
背景技术
近年来，无人驾驶技术成为人工智能领域的研究热点，照相机、激光雷达以及毫米波雷达等一系列传感器为无人驾驶车辆提供了所需环境信息，其中激光雷达凭借高精度与高分辨率成为无人驾驶常用传感器之一。车辆通常所处的城市道路中主要包括两类障碍物：静态障碍物与动态障碍物。静态障碍物检测在同步定位与地图构建(SimultaneousLocalization and Mapping,SLAM)领域占据重要地位，车辆可生成环境地图并进行自车定位；同时，动态目标检测与跟踪(Detection and Tracking of Moving Objects)技术也是无人驾驶感知的基础性技术之一，真实场景中动态目标每一时刻所处位置更加随机，这使得动态车辆检测更加困难。
目前，主流的车辆检测方法主要包括基于训练的方法和基于模型的方法。基于训练的方法又包括了传统机器学习方法和深度学习方法，前者主要思路为特征提取和分类器检测，后者主要利用深度神经网络对点云自适应学习。基于训练自适应学习的方法可以获得更高的检测精度，且通常来说可提供的数据量越多，获取的检测精度越高。但真实场景复杂多变，难以提供包含所有可能障碍物的充足训练数据，导致面对未经训练的目标难以正确检出。同时，基于训练的方法对处理器性能也提出了更高的要求。
基于模型的方法是另一种有效的车辆检测方法，通过构建目标测量模型，设计模型拟合函数，判断原始点云数据与测量模型拟合程度，进而利用一些约束条件判断目标所属类型。该方法不需大量训练数据支撑，对场景适应性更强。
发明内容
本发明的目的在于提供一种动态车辆检测方法来克服或减轻现有技术的上述缺陷中的至少一个。
为实现上述目的，本发明提供一种动态车辆检测方法，该方法包括：
步骤1，预处理点云；
步骤2，检测动态目标；
步骤3，从动态目标中识别出动态车辆；
步骤4，检测动态车辆，其具体包括：
步骤41，计算动态车辆的位姿初值；
步骤42，利用粒子群优化算法估计动态车辆位姿，其具体包括：
步骤421，根据动态车辆的位姿初值，在其位置附近一定范围内进行点云聚类搜索，查看是否有关联动态目标，并设置有关联动态目标和无关联动态目标时的粒子群初始化参数；
步骤422，将步骤3输出的动态车辆的点云聚类投影到水平x-y平面上，利用下式(1)和式(2)计算各粒子的适应度函数值；
Figure BDA0002671659570000021
Figure BDA0002671659570000022
其中，fS为点云聚类在位置S＝(xs,ys,θs)下的适应度函数，(xi,yi)为点云聚类中任一点，λi为归一化常数，
Figure BDA0002671659570000023
为积分归一化常数，βk表示经验系数，k＝0,...,3表示各积分区域，
Figure BDA0002671659570000024
表示位置S＝(xs,ys,θs)处车辆模型的各积分区域，gi(x,y)为点云聚类(xi,yi)在空间(x,y)位置处的二维正态分布，σ2为测量噪声方差；
步骤423，将各粒子位置对应适应度函数值与上一次循环结果进行比较，将更大的适应度函数值及对应粒子位置赋值为该粒子个体的最大适应值及最佳位置，并选取所有粒子中最大的适应度函数值对应位置为全局最佳位置；
步骤424，利用粒子个体和全局最佳位置，更新粒子的速度和位置；
步骤425，返回步骤422，直到满足最大循环次数或者最佳位置变化小于预设阈值，输出全局最佳粒子即为优化位姿结果。
进一步地，步骤424利用下式(3)和式(4)更新粒子的速度和位置：
vk＝wvk-1+c1r1(pbest-pk-1)+c2r2(gbest-pk-1) (3)
pk＝pk-1+vk (4)
式中，pbest为个体最佳粒子位置，gbest为全局最佳粒子位置，vk-1、vk分别为上一时刻和当前时刻粒子的速度，pk-1和pk分别为上一时刻和当前时刻粒子位置，w为惯性因子，c1和c2为加速常数，r1和r2为两随机数。
进一步地，步骤2具体包括：
步骤21，将点云聚类投影到水平面极坐标网格图中，在点云投影所覆盖的各径向扇形区域中，将距离网格图中心最近的网格状态设置为“占用”状态，“占用”状态的网格所在圆周与距离网格图中心径向最远的有点云投影的网格所在圆周之间的网格的状态设置为“遮挡”状态，没有点云投影的网格的状态设置为“自由”状态；
步骤22，统一相邻两帧点云的坐标系，并将该相邻两帧点云的网格图中的网格状态进行差分运算，计算每个点云聚类在相邻两帧点云中状态改变的网格数量，若状态改变的网格数量大于阈值，则该点云聚类设定为动态目标。
进一步地，步骤3包括：判断动态目标在连续三帧点云中的速度和方向角变化是否均在预设阈值内，如果是，则该动态目标判定为动态车辆。
进一步地，动态目标的位姿包括其位置以及方向角，其中，方向角为动态目标的x轴与自车的激光雷达坐标系x轴之间的夹角，动态目标的y轴正向定义为其不可见短边的法向量方向，步骤41具体包括：
步骤411，确定动态车辆的位置初值：针对每个点云聚类，计算点云聚类水平投影的最小外接矩形，该矩形的中心为动态车辆的位置初值(x0,y0)；
步骤412，确定动态车辆的方向角初值：首先，比较动态目标的可见正边与阈值长度，若小于阈值，则可见正边为可见短边，否则为可见长边；然后，求解可见短边相应对边，并得到不可见短边的法向量，该不可见短边的法向量为动态目标的y轴正方向；最后，将y轴正方向顺时针旋转90°，得到动态目标的x轴正方向，该动态目标的x轴正方向与自车的激光雷达坐标系x轴正方向夹角为动态目标的初始方向角θ0。
进一步地，步骤421中的查看是否有关联目标的方法具体包括：
沿着动态目标方向角初值θ0方向，在速度范围[-v,+v]内随机挑选一个速度，然后结合动态车辆位置初值(x0,y0)，计算待关联时刻在这个速度下的动态目标的中心位置，在待关联时刻目标的中心位置(xp,yp)附近半个车身范围内搜索有无点云聚类，如果有，则该点云聚类作为关联动态目标。
进一步地，步骤421中的设置关联动态目标和非关联动态目标的粒子群初始化参数的方法具体包括：
如果未包含关联动态目标，则设置第一粒子群初始化参数采样范围
Figure BDA0002671659570000041
以动态车辆位姿初值S0＝(x0,y0,θ0)为中心，在
Figure BDA0002671659570000042
采样范围内随机挑选n个粒子，作为初始化粒子群，并设置各参量的最大更新速度
Figure BDA0002671659570000043
如果包含关联动态目标，则对两个点云聚类进行点云配准，得到点云间的旋转矩阵和平移向量，从旋转矩阵可获取动态目标运行中的旋转角度，若大于某阈值，则判断关联错误，按照未包含关联目标的情况初始化粒子群；平移向量用作近似的动态目标的y轴方向，用以修正动态目标所处的动态目标方向角初值θ0，随后设置具有较小方向角搜索区间的第二粒子群初始化参数采样范围
Figure BDA0002671659570000044
以动态车辆位姿初值S0＝(x0,y0,θ0)为中心，从
Figure BDA0002671659570000045
采样范围内随机挑选n个粒子，作为初始化粒子群，并设置各参量的最大更新速度
Figure BDA0002671659570000046
进一步地，步骤1具体包括：步骤11，地面点云分割；和步骤12，地上点云聚类。
本发明提供的动态检测方法能够提升检测性能，提高车辆位姿估计的准确性，提高正确检测车辆数量，并能有效降低检测虚警数量。
附图说明
图1为本发明实施例提供的动态车辆检测的流程图。
图2为点云网格映射示意图。
图3和图4为动态车辆的方向角示意图。
图5为本发明实施例中的动态车辆的方向角初值求解的流程图。
图6为本发明实施例中的估计动态车辆位姿的流程图。
图7为本发明提供的车辆似然场测量模型示意图。
图8为本发明实施例提供的模型匹配过程示意图。
具体实施方式
下面结合附图和实施例对本发明进行详细的描述。
本发明实施例提供的动态车辆检测方法包括：
步骤1，预处理点云，得到点云聚类。
激光雷达通过圆周扫描的方式获取点云，获取每一帧点云的扫描区域在某一个水平面内呈现的形状是：以激光雷达为圆心，向外辐射的圆形面。该圆形面的径向既是激光的发射方向，也是激光信号接收方向，进而获得扫描区域内不同物体的距离信息。
步骤1中的点云预处理包括地面点云分割和地上点云聚类。
例如，可使用EKF或者GPR等方法进行地面点云分割。通过利用滤波、地面建模等手段，分割地面点云与地上点云，降低后续检测过程中的计算消耗。
在分割地面点云与地上点云后，结合点云特征对地上点云进行聚类，为后续动态目标提取奠定基础。可使用RBNN算法等进行地上点云聚类。比如：某棵树返回来的点云标记为同一类，某辆车返回来的点云标记为另一类。
步骤2，从点云聚类中，检测动态目标。
步骤2具体包括：
步骤21，虚拟扫描映射：将点云聚类投影到水平面极坐标网格图中，如图2所示，图2呈现的是扇形，为网格图的一部分，网格图中心是激光雷达，网格图径向为激光雷达的激光发射和激光信号接收方向，网格图的径向线是按照等间隔的角度选取得到，其与沿径向的间隔隔开的同心圆圈相交，形成一个一个的网格。
本实施例中，在点云投影所覆盖的各径向扇形区域中，将距离网格图中心最近的网格的状态(下文简称为“网格状态”)设置为“占用”状态，“占用”状态的网格所在圆圈与距离网格图中心径向最远的有点云投影的网格所在圆圈之间的网格状态设置为“遮挡”状态，没有点云投影的网格状态设置为“自由”状态。
步骤22，相邻映射差分：考虑自车运动，图3中的Oe表示自车，即激光雷达所在位置，Ot表示场景中待检测的车辆。统一相邻两帧点云的坐标系，将该相邻两帧点云的网格图中相应的网格状态进行差分运算，计算每个点云聚类在相邻两帧点云中状态改变的网格数量，若该网格数量大于网格数量阈值，则该点云聚类判定为动态目标。也就是说，经过前面网格变化数的筛选后，每个点云聚类对应一个动态目标，作为候选动态车辆。
其中，可利用车辆惯性导航系统信息统一相邻两帧点云的坐标系，也可以通过点云配准的方式统一两帧点云坐标系。上述的预设网格数量阈值受到网格尺寸的影响，当网图构建面积较大时，每个目标运动造成的状态变化相应较少，反之，网格较小时，每个目标运动造成的状态变化相应较大。也就是说，可以设置根据网格尺寸和目标距离激光雷达的距离自适应变化的网格阈值，也可以设置固定阈值，比如“4”。当然，也可以设置其它具体数值。
步骤3，从动态目标中识别出动态车辆。
在一个实施例中，步骤3，对于估计位姿的点云聚类，若满足运动一致性原则，即连续三帧点云中该聚类的速度和方向角变化均在预设阈值内，则将该点云聚类对应的动态目标判定为动态车辆，否则将其判断为环境干扰，进而完成场景中全部动态车辆提取。当然，也可以采用其他现有的方法实现步骤3。
步骤4，动态车辆检测，其具体包括：
步骤41，计算动态车辆位姿初值：位姿包括位置以及方向角。
结合图3和图4所示，“位置”是动态车辆的中心Ot在自车的激光雷达坐标系下的坐标值，激光雷达坐标系Oe-xeye以自身中心为原点(图3中的Oe)，以自车的纵向方向为y轴正向(图3中的ye)。横向向右方向为x轴正向(图3中的xe)，y轴和x轴在水平面内相互垂直。
动态车辆也具有其自身的坐标系，动态车辆坐标系Ot-xtyt包括动态车辆中心为原点Ot，yt轴正向为动态车辆的纵向方向，xt轴正向为横向向右方向，yt轴和xt轴在水平面内相互垂直。“方向角”则是动态车辆的xt轴相对于自车的xe轴之间的夹角θ。
通常在激光雷达视角下，对于每个点云聚类所对应的动态目标，计算点云聚类水平投影的最小外接矩形。根据矩形各边的法向量与各边的中点与激光雷达坐标系Oe-xeye的原点Oe的连线的夹角，该夹角小于90°的边为激光雷达的可见边，反之大于90°的边为不可见边。
也就是说，在自车Oe的激光雷达视角下，动态车辆Ot通常有两条可见边，即AB和BC，根据二者长度的不同，下文将AB称为平行于车辆纵向的可见长边，将BC称为平行于车辆横向的可见短边。同理，在激光雷达视角下，动态车辆有两条不可见边，即AD和CD，根据二者长度的不同，下文将CD称为平行于车辆横向的不可见长边，将AD称为平行于车辆纵向的不可见短边。那么，不可见短边的法向量方向为纵向方向，不可见长边的法向量方向为横向向右方向。其中的法向量都是指向矩形外。
并且，根据矩形各边的法向量与各边的中点与激光雷达坐标系Oe-xeye的原点Oe的连线的夹角的大小，将夹角较小的边定义为可见正边，夹角较大的可见边定义为可见副边，那么，图4中的AB对应夹角更小，为可见正边，BC为可见副边。
步骤411中的计算动态车辆位置初值(x0,y0)的一种实现方式具体包括：
对于每个点云聚类所对应的动态车辆sv，计算点云聚类水平投影的最小外接矩形，利用矩形中心确定为动态车辆位置初值(x0,y0)。
步骤411中的计算动态车辆位置初值(x0,y0)的另一种实现方式具体包括：
针对每个点云聚类，计算点云聚类水平投影的最小外接矩形，找出激光雷达视角下最小外接矩形中的可见边。当有两条可见边时，计算两边交点
Figure BDA0002671659570000071
当仅有一条可见边时，取该边任意端点
Figure BDA0002671659570000072
作为两边角点。设定矩形模型的长、宽，依据矩形角点与矩形中心的位置关系得到矩形中心坐标(x0,y0)，近似为动态车辆位置初值。
步骤412，计算动态车辆方向角初值θ0：如图5所示，len(·)表示长度计算，sv为可见正边的长度。首先，将动态目标的可见正边的长度sv与阈值tshort进行比较，若小于阈值，则可见正边为可见短边，否则为可见长边。然后，求解与可见短边相对的不可见短边，并得到不可见短边的法向量，即动态车辆的y轴方向(yt轴)；最后，将y轴顺时针旋转90°，可得x轴方向(xt轴)，其与激光雷达坐标系的xe轴方向夹角即为动态车辆初始方向角θ0。
步骤412中的计算动态车辆方向角初值θ0的另一种实现方式具体包括：
分别计算两条可见边与方向向量夹角，将最小夹角对应边所在方向定为动态车辆y轴方向，顺时针旋转90°得到动态车辆x轴方向，再计算动态车辆的x轴正方向与自车的激光雷达坐标系x轴正方向夹角，即为动态车辆的初始方向角。
步骤42，利用粒子群优化算法(Particle Swarm Optimization,PSO)，估计动态车辆位姿，其具体包括：
步骤421，根据动态车辆的位姿初值S0＝(x0,y0,θ0)，在其位置附近一定范围内进行点云聚类搜索，查看是否有关联动态目标，并设置有关联动态目标和无关联动态目标时的粒子群初始化参数。
如图6所示，作为步骤421中的查看是否有关联目标的方法的一种优选方式，其包括：
沿着动态车辆方向角初值θ0在速度范围[-v,+v]内随机挑选一个速度，然后结合动态车辆位置初值(x0,y0)，计算待关联时刻在这个速度下的动态车辆的中心位置，在待关联时刻目标的中心位置(xp,yp)附近半个车身范围内搜索有无点云聚类，如果有，则该点云聚类作为关联动态目标。其中，速度范围中的v例如可以选取35m/s，但不限于此。
作为步骤421中的查看是否有关联目标的方法的另一种优选方式，其包括：
使用动态车辆位置初值(x0,y0)作为目标位置，各点云聚类几何中心作为聚类的位置，计算该动态车辆与待关联时刻所有动态目标点云聚类的距离，选择距离最小的点云聚类并计算动态目标运动到该位置的速度，若速度符合一定阈值范围，则将该点云聚类作为关联动态目标，否则，该动态目标无关联动态目标。
步骤421中的设置有关联动态目标和无关联动态目标时的粒子群初始化参数的方法之一具体包括：
如果未包含关联动态目标，则设置第一粒子群初始化参数采样范围
Figure BDA0002671659570000081
以动态车辆位姿初值S0＝(x0,y0,θ0)为中心，在
Figure BDA0002671659570000082
采样范围内随机挑选n个粒子，作为初始化粒子群，并设置各参量的最大更新速度
Figure BDA0002671659570000083
需要说明的是，最大更新速度
Figure BDA0002671659570000084
要求不超过采样范围，取值并不唯一。本实施例中，
Figure BDA0002671659570000085
分别设定为与
Figure BDA0002671659570000086
相对应的数值，每次通过式(3)计算到更新的速度vk后需要与
Figure BDA0002671659570000087
进行比较，取其中的较小值作为当次迭代的速度。v0从
Figure BDA0002671659570000088
中随机采样得到的。
Figure BDA0002671659570000089
取π/2，即可在[-π/2，π/2]间随机取值，再朝粒子最优位置优化。这样可以在较大范围内进行最佳粒子搜索。粒子群有多个粒子组成。其中，
Figure BDA00026716595700000810
的值取决于车辆尺寸，
Figure BDA00026716595700000811
均设定为车辆模型长边L的一半。可以理解为以位姿参量中的位置为中心，半径L/2范围内随机挑选n个位置作为粒子初始化的位置。
如果包含关联动态目标，则对两个点云聚类进行点云配准，得到点云间的旋转矩阵和平移向量。从旋转矩阵可获取动态目标运行中的旋转角度，若大于设定阈值，则判断关联错误，按照未包含关联目标的情况初始化粒子群。其中，设定阈值为经验值，比如：激光雷达的扫描间隔是0.1s，一般的车辆经过0.1s即使在转弯过程方向角的改变也有限，本实施例中的角度阈值设定为20°
根据车辆的实际驾驶工况以及点云的采集周期，即：当车辆直线行驶时，车辆纵向y轴方向与车辆运动方向完全一致；当车辆转弯时，车辆纵向y轴方向为车辆轨迹切线，考虑车辆转弯速度有限，而且点云周期为0.1s，一帧时间内车辆转弯过程中，可以将车辆运动方向与车辆y轴近似表示。鉴于此，平移向量可以用来近似动态目标的y轴方向，用以修正动态目标所处的方向角初值θ0；随后设置第二粒子群初始化参数采样范围
Figure BDA0002671659570000091
以动态车辆位姿初值S0＝(x0,y0,θ0)为中心，从
Figure BDA0002671659570000092
采样范围内随机挑选n个粒子，作为初始化粒子群。其中，
Figure BDA0002671659570000093
同
Figure BDA0002671659570000094
取决于车辆尺寸。由于获得了动态目标方向信息，将
Figure BDA0002671659570000095
设置为较小区间，例如：没有关联目标时，
Figure BDA0002671659570000096
取π/2；若出现关联目标后，可将
Figure BDA0002671659570000097
取为π/36，本实施例中的取值是结合车辆最小转弯半径参数结合扫描间隔求出的一个适宜的值或者其它与该值相近的值均可。
步骤421中的设置有关联动态目标和无关联动态目标时的粒子群初始化参数的方法之二具体包括：
如果未包含关联动态目标，则设置第一粒子群初始化参数
Figure BDA0002671659570000098
以动态车辆位姿初值S0＝(x0,y0,θ0)为中心，在
Figure BDA0002671659570000099
采样范围内随机挑选n个粒子，作为初始化粒子群，并设置各参量的最大更新速度
Figure BDA00026716595700000910
如果包含关联动态目标，在初始化粒子群时首先通过位置差计算目标的平移向量，并利用平移向量修正动态目标所处的方向角初值θ0。以动态车辆位姿初值S0＝(x0,y0,θ0)中各参量作为高斯分布的均值，标准差分别为
Figure BDA00026716595700000911
在采样范围
Figure BDA00026716595700000912
中随机挑选n个粒子，作为初始化粒子群，设各粒子位置高斯函数值为Gn，将每个粒子最大更新速度设为
Figure BDA00026716595700000913
步骤422，将3D点云投影到水平x-y平面上，利用车辆似然场测量模型提供的式(1)和式(2)计算各粒子的适应度函数值，如图7所示为车辆似然场测量模型，R0为可见长边，R1为可见短边，R2为车辆内部，R3为车辆外部安全区域。
Figure BDA00026716595700000914
Figure BDA0002671659570000101
其中：
fS为点云聚类在粒子位置S＝(xs,ys,θs)下的适应度函数，粒子位置用于表示目标/点云聚类的位姿；
(xi,yi)为点云聚类中任一点的位置；
λi为归一化常数，λt的作用是使得fS值在[0,1]区间内，其取值对应为最大的
Figure BDA0002671659570000102
值；
Figure BDA0002671659570000103
为积分归一化常数；
βk表示经验系数，k＝0,...,3表示各积分区域，对应于图7中R0-R3区域；βk的具体数值由经验确定，原则是模型与点云匹配得越好，适应度函数值越大。从图7中可以看出，两条可见边区域R0和R1对于确定动态目标位姿很重要，因此取值要大一些，为正值，区域R2为车辆内部，对适应度函数取值也有正向影响，因此也取正值，比R0和R1的系数小一些。R3为模型外部，点云落在这一区域的数量越多，说明匹配越不准，适应度函数值应越小，因此R3对应的系数取负数。
Figure BDA0002671659570000104
表示位置S＝(xs,ys,θs)对应的各积分区域，如图8所示，图8的(1)为动态目标的原始点云，假设图8的(2)为(x1,y1,θ1)位姿下的动态目标模型，图8的(3)为(x2,y2,θ2)位姿下的动态目标模型，图8的(4)为(x3,y3,θ3)位姿下的动态目标模型，是三个不同的粒子表示的模型位姿，模型匹配过程具体是计算此时图8的(1)中的点云与图8的(2)-(4)模型位置的适应度函数，可得到与该点云聚类最匹配的模型的位姿参数；
gi(x,y)为点云聚类(xi,yi)在水平空间(x,y)位置处的二维正态分布，正态分布的均值位置就在这个点(xi,yi)位置；
σ2为测量噪声方差，σ比如取0.1。
本实施例通过启发式位姿估计算法(Pose Estimation with HeuristicAlgorithm,PE-HA)对点云实施模型拟合，以车辆似然场测量模型为例阐述了启发式优化算法中适应度函数的计算过程，提高了动态目标位姿估计准确性。除似然场测量模型外，其它测量模型构建的适应度函数同样适用本实施例阐述的位姿估计方法。
步骤423，在利用粒子群优化算法PSO估计动态目标的位姿时，首先，根据步骤421得出的是否为关联目标的结果，设置粒子各参量采样范围及对应粒子速度范围，当有关联目标时方向角的“采样范围”及“最大粒子速度”取值较小，当无关联目标时，方向角的“采样范围”及“最大粒子速度”取值较大。在该设置好的粒子各状态参量采样范围及对应粒子速度范围中随机初始化n个粒子位置及搜索速度。
将各粒子位置及对应适应度函数值与上一次循环结果进行比较(适应度函数初始值可置为0)，将更大的适应度函数值及对应粒子位置赋值为该粒子个体的最大适应值及最佳位置，并选取所有粒子中最大的适应度函数值对应位置为全局最佳位置。本实施例以PSO算法为例阐述了启发式优化在点云车辆位姿估计中的作用，除了使用PSO算法外，还可以采用其它启发式优化算法对动态目标的位姿进行优化，如风驱动优化算法、遗传算法等。
步骤424，根据粒子个体和全局最佳位置，利用下式(3)和式(4)更新粒子的速度和位置：
vk＝wvk-1+c1r1(pbest-pk-1)+c2r2(gbest-pk-1) (3)
pk＝pk-1+vk (4)
式中：
pbest为个体最佳粒子位置；
gbest为全局最佳粒子位置；
vk-1、vk分别为上一时刻和当前时刻粒子的速度；
pk-1和pk分别为上一时刻和当前时刻粒子的位置；
w为惯性因子；
c1和c2为加速常数；
r1和r2为两随机数，随机取均匀分布的随机数。
步骤425，返回步骤422，随后开展下一轮循环，将更新后的粒子位置再次代入适应度函数，计算各粒子对应适应值，与上一轮循环所得值进行比较，保留较大值对应位置为个体最佳位置，同时更新全局最佳位置，随后对粒子速度和位置再次更新，继续循环，直到满足最大循环次数或者最佳位置变化小于预设阈值，输出全局最佳粒子即为动态目标的最佳位姿。其中，“最大循环次数”例如可以取1000次。“最佳位置变化小于预设阈值”可以理解为最佳位置基本不变，因此，预设阈值可以设置很小，例如1e-6。
本实施例提供的这种结合点云配准的PE-HA方法有效利用了启发式优化算法的全局优化优势，并引入了多帧点云积累思想，降低了稀疏点云情况下位姿估计陷入局部极值的可能性。经过KITTI数据集进行动态车辆检测仿真测试，仅对车辆前方可视区域激光雷达数据进行了处理，检测范围为80m，实验结果如表1所示。根据如下指标计算方式，比较本发明基于PE-HA的动态车辆检测与基于PE-MSS的动态车辆检测结果，如表1所示：
Figure BDA0002671659570000121
其中P精确率，R为召回率，F1为F1得分，TP表示正确检测到的目标数量，FP表示不是目标但当作目标检出的数量，FN表示是目标但未检测到的数量。
表1
Figure BDA0002671659570000122
由表1可以看出：结合点云配准和启发式优化算法进行位姿估计，并将其引入动态车辆检测过程的方法获得了很好的检测性能提升，提高了车辆位姿估计的准确性，提高了正确检测车辆的数量，并有效降低了检测虚警数量，综合正确检测和虚警指标，获得了更好的F1得分，验证了算法的优越性。
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种动态车辆检测方法，其特征在于，包括：
步骤1，预处理点云；
步骤2，检测动态目标；以及，
步骤3，从动态目标中识别出动态车辆；
步骤4，检测动态车辆，其具体包括：
步骤41，计算动态车辆的位姿初值；
步骤42，利用粒子群优化算法估计动态车辆位姿，其具体包括：
步骤421，根据动态车辆的位姿初值，在其位置附近一定范围内进行点云聚类搜索，查看是否有关联动态目标，并设置有关联动态目标和无关联动态目标时的粒子群初始化参数；
步骤422，将步骤3输出的动态车辆的点云聚类投影到水平x-y平面上，利用下式(1)和式(2)计算各粒子的适应度函数值；
Figure FDA0002671659560000011
Figure FDA0002671659560000012
其中，fS为点云聚类在位置S＝(xs,ys,θs)下的适应度函数，(xi,yi)为点云聚类中任一点，λi为归一化常数，
Figure FDA0002671659560000013
为积分归一化常数，βk表示经验系数，k＝0,...,3表示各积分区域，
Figure FDA0002671659560000014
表示位置S＝(xs,ys,θs)处车辆模型的各积分区域，gi(x,y)为点云聚类(xi,yi)在空间(x,y)位置处的二维正态分布，σ2为测量噪声方差；
步骤423，将各粒子位置对应适应度函数值与上一次循环结果进行比较，将更大的适应度函数值及对应粒子位置赋值为该粒子个体的最大适应值及最佳位置，并选取所有粒子中最大的适应度函数值对应位置为全局最佳位置；
步骤424，利用粒子个体和全局最佳位置，更新粒子的速度和位置；
步骤425，返回步骤422，直到满足最大循环次数或者最佳位置变化小于预设阈值，输出全局最佳粒子即为优化位姿结果。
2.如权利要求1所述的动态车辆检测方法，其特征在于，步骤424利用下式(3)和式(4)更新粒子的速度和位置：
vk＝wvk-1+c1r1(pbest-pk-1)+c2r2(gbest-pk-1) (3)
pk＝pk-1+vk (4)
式中，pbest为个体最佳粒子位置，gbest为全局最佳粒子位置，vk-1、vk分别为上一时刻和当前时刻粒子的速度，pk-1和pk分别为上一时刻和当前时刻粒子位置，w为惯性因子，c1和c2为加速常数，r1和r2为两随机数。
3.如权利要求1或2所述的动态车辆检测方法，其特征在于，步骤2具体包括：
步骤21，将点云聚类投影到水平面极坐标网格图中，在点云投影所覆盖的各径向扇形区域中，将距离网格图中心最近的网格状态设置为“占用”状态，“占用”状态的网格所在圆周与距离网格图中心径向最远的有点云投影的网格所在圆周之间的网格的状态设置为“遮挡”状态，没有点云投影的网格的状态设置为“自由”状态；
步骤22，统一相邻两帧点云的坐标系，并将该相邻两帧点云的网格图中的网格状态进行差分运算，计算每个点云聚类在相邻两帧点云中状态改变的网格数量，若状态改变的网格数量大于阈值，则该点云聚类判定为动态目标。
4.如权利要求3所述的动态车辆检测方法，其特征在于，步骤3包括：判断动态目标在连续三帧点云中的速度和方向角变化是否均在预设阈值内，如果是，则该动态目标判定为动态车辆。
5.如权利要求4所述的动态车辆检测方法，其特征在于，动态目标的位姿包括其位置以及方向角，其中，方向角为动态目标的x轴与自车的激光雷达坐标系x轴之间的夹角，动态目标的y轴正向定义为其不可见短边的法向量方向，步骤41具体包括：
步骤411，计算动态车辆的位置初值(x0,y0)：针对每个点云聚类，计算点云聚类水平投影的最小外接矩形，该矩形的中心为动态车辆的位置初值(x0,y0)；
步骤412，计算动态车辆的方向角初值θ0：首先，比较动态目标的可见正边与阈值，若长度小于阈值，则可见正边为可见短边，否则为可见长边；然后，求解可见短边相应对边，并得到不可见短边的法向量，该不可见短边的法向量定义为动态目标的y轴正方向；最后，将y轴正方向顺时针旋转90°，得到动态目标的x轴正方向，该动态目标的x轴正方向与自车的激光雷达坐标系x轴正方向夹角为动态目标的初始方向角θ0。
6.如权利要求5所述的动态车辆检测方法，其特征在于，步骤421中的查看是否有关联目标的方法具体包括：
沿着动态目标方向角初值θ0方向，在速度范围[-v,+v]内随机挑选一个速度，然后结合动态车辆位置初值(x0,y0)，计算待关联时刻在这个速度下的动态目标的中心位置，在待关联时刻目标的中心位置(xp,yp)附近半个车身范围内搜索有无点云聚类，如果有，则该点云聚类作为关联动态目标。
7.如权利要求6所述的动态车辆检测方法，其特征在于，步骤421中的设置关联动态目标和非关联动态目标的粒子群初始化参数的方法具体包括：
如果未包含关联动态目标，则设置第一粒子群初始化参数采样范围
Figure FDA0002671659560000031
以动态车辆位姿初值S0＝(x0,y0,θ0)为中心，在
Figure FDA0002671659560000032
采样范围内随机挑选n个粒子，作为初始化粒子群，并设置各参量的最大更新速度
Figure FDA0002671659560000033
如果包含关联动态目标，则对两个点云聚类进行点云配准，得到点云间的旋转矩阵和平移向量，从旋转矩阵可获取动态目标运行中的旋转角度，若大于某阈值，则判断关联错误，按照未包含关联目标的情况初始化粒子群；平移向量用作近似的动态目标的y轴方向，用以修正动态目标所处的动态目标方向角初值θ0，随后设置具有较小方向角搜索区间的第二粒子群初始化参数采样范围
Figure FDA0002671659560000034
以动态车辆位姿初值S0＝(x0,y0,θ0)为中心，从
Figure FDA0002671659560000035
采样范围内随机挑选n个粒子，作为初始化粒子群，并设置各参量的最大更新速度
Figure FDA0002671659560000036
8.如权利要求1所述的动态车辆检测方法，其特征在于，步骤1具体包括：
步骤11，地面点云分割；和
步骤12，地上点云聚类。
CN202010935148.XA 2020-09-08 2020-09-08 一种动态车辆检测方法 Expired - Fee Related CN112084937B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010935148.XA CN112084937B (zh) 2020-09-08 2020-09-08 一种动态车辆检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010935148.XA CN112084937B (zh) 2020-09-08 2020-09-08 一种动态车辆检测方法

Publications (2)

Publication Number Publication Date
CN112084937A true CN112084937A (zh) 2020-12-15
CN112084937B CN112084937B (zh) 2021-03-19

Family

ID=73732651

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010935148.XA Expired - Fee Related CN112084937B (zh) 2020-09-08 2020-09-08 一种动态车辆检测方法

Country Status (1)

Country Link
CN (1) CN112084937B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113192054A (zh) * 2021-05-20 2021-07-30 清华大学天津高端装备研究院 基于2-3d视觉融合的繁杂零件检测与定位的方法及系统
CN113315961A (zh) * 2021-05-26 2021-08-27 浙江合众新能源汽车有限公司 用于透明a柱图像块映射的方法及装置
CN113837521A (zh) * 2021-06-23 2021-12-24 腾讯科技（深圳）有限公司 数据处理方法、装置、计算机可读存储介质及计算机设备
CN114612665A (zh) * 2022-03-15 2022-06-10 北京航空航天大学 基于法向量直方图特征的位姿估计及动态车辆检测方法
CN114663689A (zh) * 2022-05-18 2022-06-24 沈阳和研科技有限公司 一种多步进测量方法
WO2022166042A1 (zh) * 2021-02-05 2022-08-11 泉州装备制造研究所 一种点云极坐标编码方法及装置
WO2024024663A1 (ja) * 2022-07-29 2024-02-01 ソニーセミコンダクタソリューションズ株式会社 認識システム、認識方法および認識装置

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7424535B1 (ja) 2023-05-31 2024-01-30 トヨタ自動車株式会社 遠隔制御装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180075611A1 (en) * 2015-08-12 2018-03-15 Nvidia Corporation Model-based three-dimensional head pose estimation
CN108053443A (zh) * 2017-11-20 2018-05-18 中国科学院空间应用工程与技术中心 一种基于粒子群优化的物体点云位姿估算方法和系统
CN108645413A (zh) * 2018-06-06 2018-10-12 江苏海事职业技术学院 一种移动机器人的同时定位与地图创建的动态纠正方法
US20190064322A1 (en) * 2017-08-25 2019-02-28 Institute For Information Industry Vehicle detecting method and vehicle detecting system
CN110832275A (zh) * 2018-06-14 2020-02-21 北京嘀嘀无限科技发展有限公司 基于双目图像更新高分辨率地图的系统和方法
CN111085982A (zh) * 2019-12-30 2020-05-01 中国农业大学 一种果园机器人主动视觉探测系统及探测方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180075611A1 (en) * 2015-08-12 2018-03-15 Nvidia Corporation Model-based three-dimensional head pose estimation
US20190064322A1 (en) * 2017-08-25 2019-02-28 Institute For Information Industry Vehicle detecting method and vehicle detecting system
CN108053443A (zh) * 2017-11-20 2018-05-18 中国科学院空间应用工程与技术中心 一种基于粒子群优化的物体点云位姿估算方法和系统
CN108645413A (zh) * 2018-06-06 2018-10-12 江苏海事职业技术学院 一种移动机器人的同时定位与地图创建的动态纠正方法
CN110832275A (zh) * 2018-06-14 2020-02-21 北京嘀嘀无限科技发展有限公司 基于双目图像更新高分辨率地图的系统和方法
CN111085982A (zh) * 2019-12-30 2020-05-01 中国农业大学 一种果园机器人主动视觉探测系统及探测方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
S. ZAHRAN等: "Enhancement of real-time scan matching for uav indoor navigation using vehicle model", 《ISPRS ANNALS OF PHOTOGRAMMETRY, REMOTE SENSING & SPATIAL INFORMATION SCIENCES》 *
欧阳波: "基于三维点云的动态目标位姿估计", 《中国优秀硕士学位论文全文数据库信息科技辑》 *
赵绍安: "基于三维激光点云的移动机器人同步定位与构图算法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 *
默_存: "无人驾驶传感器融合系列（二）——激光雷达点云的聚类原理及实现", 《HTTPS://BLOG.CSDN.NET/WEIXIN_40215443/ARTICLE/DETAILS/93482726》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022166042A1 (zh) * 2021-02-05 2022-08-11 泉州装备制造研究所 一种点云极坐标编码方法及装置
CN113192054A (zh) * 2021-05-20 2021-07-30 清华大学天津高端装备研究院 基于2-3d视觉融合的繁杂零件检测与定位的方法及系统
CN113192054B (zh) * 2021-05-20 2023-04-28 清华大学天津高端装备研究院 基于2-3d视觉融合的繁杂零件检测与定位的方法及系统
CN113315961A (zh) * 2021-05-26 2021-08-27 浙江合众新能源汽车有限公司 用于透明a柱图像块映射的方法及装置
CN113837521A (zh) * 2021-06-23 2021-12-24 腾讯科技（深圳）有限公司 数据处理方法、装置、计算机可读存储介质及计算机设备
CN114612665A (zh) * 2022-03-15 2022-06-10 北京航空航天大学 基于法向量直方图特征的位姿估计及动态车辆检测方法
CN114663689A (zh) * 2022-05-18 2022-06-24 沈阳和研科技有限公司 一种多步进测量方法
WO2024024663A1 (ja) * 2022-07-29 2024-02-01 ソニーセミコンダクタソリューションズ株式会社 認識システム、認識方法および認識装置

Also Published As

Publication number Publication date
CN112084937B (zh) 2021-03-19

Similar Documents

Publication Publication Date Title
CN112084937B (zh) 一种动态车辆检测方法
CN109509210B (zh) 障碍物跟踪方法和装置
CN113168717B (zh) 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达
CN111665842B (zh) 一种基于语义信息融合的室内slam建图方法及系统
Zhou et al. T-LOAM: Truncated least squares LiDAR-only odometry and mapping in real time
Huang et al. A fast point cloud ground segmentation approach based on coarse-to-fine Markov random field
WO2022188663A1 (zh) 一种目标检测方法及装置
CN113432600A (zh) 基于多信息源的机器人即时定位与地图构建方法及系统
CN113345008B (zh) 考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法
Liu et al. Dynamic vehicle detection with sparse point clouds based on PE-CPD
CN110908374B (zh) 一种基于ros平台的山地果园避障系统及方法
CN113031004A (zh) 基于三维激光雷达的无人船水面目标检测及路径规划方法
CN111739066B (zh) 一种基于高斯过程的视觉定位方法、系统及存储介质
Wang et al. Map-enhanced ego-lane detection in the missing feature scenarios
CN116109601A (zh) 一种基于三维激光雷达点云的实时目标检测方法
CN114612665A (zh) 基于法向量直方图特征的位姿估计及动态车辆检测方法
CN115861968A (zh) 一种基于实时点云数据的动态障碍物剔除方法
Zhu et al. Deepego: Deep instantaneous ego-motion estimation using automotive radar
CN111738214B (zh) 一种激光点云中的无人机目标检测方法
CN111765883B (zh) 机器人蒙特卡罗定位方法、设备及存储介质
CN113487631A (zh) 基于lego-loam的可调式大角度探测感知及控制方法
CN116385997A (zh) 一种车载障碍物精确感知方法、系统及存储介质
CN112731335B (zh) 一种基于全区域激光扫描的多无人机协作定位方法
Li et al. An SLAM algorithm based on laser radar and vision fusion with loop detection optimization
Wang et al. Review on Application of Binocular Vision Technology in Field Obstacle Detection

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
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20210319