一种激光雷达跟踪方法、装置、电子设备和存储介质

1.本发明涉及激光雷达跟踪领域，尤其涉及一种激光雷达跟踪方法、装置、电子设备和计算机可读存储介质。


背景技术：

2.随着科技的发展，智能交通技术可以有效提高交管部门道路车流调度能力，提高交通运输效率，增强交通安全性进而减少由现有交通问题造成的能源消耗和环境污染。激光雷达具有厘米级空间分辨率，适于局部交通环境感知，目前被广泛应用于同步定位与建图(slam)、移动测图、目标识别与跟踪等方面。
3.传统的多目标跟踪(multi object tracking,mot)主要利用连续的传感器观测数据对目标建立对应关系,同时根据建立的匹配代价函数持续对多目标的运动状态进行估计与预测。然而，由于激光雷达不同帧扫描角度的变化，扫描到的运动目标点云形状通常也不断变化，现有目标跟踪方法在跟踪时忽略了目标点云质心位置变化，以相同的质心位置表征目标的位置，从而导致跟踪目标轨迹与位置变化的估计与实际情况存在较大误差，难以精确解算目标的真实运动轨迹，也无法精确估计目标航向变化。
4.因此，亟需解决现有技术中仅以目标的质心位置表征目标位置所导致的目标轨迹与位置变化的估计与实际情况误差较大，不能够对跟踪目标的轨迹变化作出准确估计的问题。


技术实现要素：

5.有鉴于此，有必要提供一种激光雷达跟踪方法、装置、电子设备和计算机可读存储介质，用以解决现有技术中跟踪目标的状态参数估计不准确以及误跟踪的问题。
6.为了解决上述问题，本发明提供一种激光雷达跟踪方法，包括：
7.获取多帧原始点云数据；
8.利用第一预设算法对相邻两帧的原始点云数据进行处理，确定跟踪目标；
9.利用第二预设算法确定所述跟踪目标在相邻两帧的位姿，根据所述跟踪目标在相邻两帧的位姿，得到所述跟踪目标的速度初值；
10.利用第三预设算法根据所述跟踪目标在相邻两帧的位姿、所述跟踪目标的速度初值和所述跟踪目标在相邻两帧的原始点云数据，得到所述跟踪目标的轨迹估计结果。
11.进一步地，利用第一预设算法对相邻帧的原始点云数据进行处理，得到跟踪目标，包括：
12.根据所述相邻帧的原始点云数据，提取待跟踪目标；
13.利用最近邻数据关联算法计算所述待跟踪目标在相邻两帧的相似度；
14.根据所述相邻两帧的相似度，确定跟踪目标。
15.进一步地，所述相邻两帧的相似度matsimi(i,j)包括整数部分possimi
i,j
和小数部分geosimi
i,j
，
16.matsimi(i,j)＝possimi
i,j
+geosimi
i,j
；其中：
[0017][0018][0019]
为相邻两帧中待跟踪目标的质心平面距离，d
thre
为预设的平面距离阈值，和分别为第k帧和第k+1帧中的待跟踪目标的边长，radius＝2(length+width)，round表示将数字四舍五入到个位数。
[0020]
进一步地，当相邻两帧中所述待跟踪目标的质心平面距离大于所述平面距离阈值时，确定所述待跟踪在相邻两帧中的相似度为0，且所述待跟踪目标不是跟踪目标。
[0021]
进一步地，对于第k帧原始点云数据中的待跟踪目标，在第k+1帧点云数据中选择相似度最大且大于0的待跟踪作为跟踪目标。
[0022]
进一步地，利用第二预设算法确定所述跟踪目标在相邻两帧的位姿，包括：利用卡尔曼滤波计算所述跟踪目标在相邻两帧的位姿。
[0023]
进一步地，利用第三预设算法根据所述跟踪目标在相邻两帧的位姿、所述跟踪目标的速度初值和所述跟踪目标在相邻两帧的原始点云数据，得到所述跟踪目标的轨迹估计结果，包括：
[0024]
根据所述跟踪目标在相邻两帧的位姿，得到初始变换参数；
[0025]
根据所述初始变换参数、所述跟踪目标在相邻两帧的原始点云数据，利用icp算法求解精确变换参数；
[0026]
根据所述精确变换参数和所述跟踪目标的速度初值，得到所述跟踪目标的轨迹估计结果。
[0027]
本发明还提供一种激光雷达跟踪装置，包括：
[0028]
获取模块，用于获取多帧原始点云数据；
[0029]
跟踪目标确定模块，用于利用第一预设算法对相邻两帧的原始点云数据进行处理，确定跟踪目标；
[0030]
计算模块，利用第二预设算法确定所述跟踪目标在相邻两帧的位姿，根据所述跟踪目标在相邻两帧的位姿，得到所述跟踪目标的速度初值；
[0031]
轨迹估计模块，利用第三预设算法根据所述跟踪目标在相邻两帧的位姿、所述跟踪目标的速度初值和所述跟踪目标在相邻两帧的原始点云数据，得到所述跟踪目标的轨迹估计结果。
[0032]
本发明还提供一种电子设备，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如上述任一技术方案所述的激光雷达跟踪方法。
[0033]
本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机该程序被处理器执行时，实现如上述任一技术方案所述的激光雷达跟踪方法。
[0034]
采用上述实施例的有益效果是：本发明提供的激光雷达跟踪方法，首先，利用第一预设算法对相邻两帧的原始点云数据进行处理，确定跟踪目标；其次，利用第二预设算法确定跟踪目标在相邻两帧的位姿并得到跟踪目标的速度初值；最后，利用第三预设算法根据
跟踪目标在相邻两帧的位姿、跟踪目标的速度初值和跟踪目标在相邻两帧的原始点云数据，得到跟踪目标的轨迹估计结果。本发明提供的方法利用帧间目标点云匹配结果对帧间位置变化量与航向角变化量进行解算，实现了对跟踪目标位姿变化和运动轨迹的精准估计。
附图说明
[0035]
图1为本发明提供的激光雷达跟踪方法的应用系统一实施例的场景示意图；
[0036]
图2为本发明提供的激光雷达跟踪方法一实施例的流程示意图
[0037]
图3为本发明提供的激光雷达跟踪装置一实施例的结构示意图；
[0038]
图4是本发明提供的电子设备一实施例的结构框图。
具体实施方式
[0039]
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本技术一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
[0040]
本发明提供了一种激光雷达跟踪方法、装置、电子设备和计算机可读存储介质，以下分别进行详细说明。
[0041]
图1为本发明提供的激光雷达跟踪装置的应用场景示意图，该系统可以包括服务器100，服务器100中集成激光雷达跟踪装置，如图1中的服务器。
[0042]
本发明实施例中服务器100主要用于：
[0043]
获取多帧原始点云数据；
[0044]
利用第一预设算法对相邻两帧的原始点云数据进行处理，确定跟踪目标；
[0045]
利用第二预设算法确定所述跟踪目标在相邻两帧的位姿，根据所述跟踪目标在相邻两帧的位姿，得到所述跟踪目标的速度初值；
[0046]
利用第三预设算法根据所述跟踪目标在相邻两帧的位姿、所述跟踪目标的速度初值和所述跟踪目标在相邻两帧的原始点云数据，得到所述跟踪目标的轨迹估计结果。
[0047]
本发明实施例中，该服务器100可以是独立的服务器，也可以是服务器组成的服务器网络或服务器集群，例如，本发明实施例中所描述的服务器100，其包括但不限于计算机、网络主机、单个网络服务器、多个网络服务器集或多个服务器构成的云服务器。其中，云服务器由基于云计算(cloud computing)的大量计算机或网络服务器构成。
[0048]
可以理解的是，本发明实施例中所使用的终端200可以是既包括接收和发射硬件的设备，即具有能够在双向通信链路上，执行双向通信的接收和发射硬件的设备。这种设备可以包括：蜂窝或其他通信设备，其具有单线路显示器或多线路显示器或没有多线路显示器的蜂窝或其他通信设备。具体的终端200可以是台式机、便携式电脑、网络服务器、掌上电脑(personal digital assistant，pda)、移动手机、平板电脑、无线终端设备、通信设备、嵌入式设备等，本实施例不限定终端200的类型。
[0049]
本领域技术人员可以理解，图1中示出的应用环境，仅仅是与本发明方案一种应用场景，并不构成对本发明方案应用场景的限定，其他的应用环境还可以包括比图1中所示更多或更少的终端，例如图1中仅示出2个终端，可以理解的，该激光雷达跟踪装置还可以包括一个或多个其他终端，具体此处不作限定。
[0050]
另外，参照图1所示，该激光雷达跟踪装置还可以包括存储器300，用于存储数据，如点云数据，跟踪目标速度初值等数据。
[0051]
需要说明的是，图1所示的激光雷达跟踪装置的场景示意图仅仅是一个示例，本发明实施例描述的激光雷达跟踪装置以及场景是为了更加清楚的说明本发明实施例的技术方案，并不构成对于本发明实施例提供的技术方案的限定，本领域普通技术人员可知，随着激光雷达跟踪装置的演变和新业务场景的出现，本发明实施例提供的技术方案对于类似的技术问题，同样适用。
[0052]
本发明实施例提供了一种激光雷达跟踪方法，其流程示意图，如图2所示，所述激光雷达跟踪方法包括：
[0053]
步骤s201、获取多帧原始点云数据；
[0054]
步骤s202、利用第一预设算法对相邻两帧的原始点云数据进行处理，确定跟踪目标；
[0055]
步骤s203、利用第二预设算法确定所述跟踪目标在相邻两帧的位姿，根据所述跟踪目标在相邻两帧的位姿，得到所述跟踪目标的速度初值；
[0056]
步骤s204、利用第三预设算法根据所述跟踪目标在相邻两帧的位姿、所述跟踪目标的速度初值和所述跟踪目标在相邻两帧的原始点云数据，得到所述跟踪目标的轨迹估计结果。
[0057]
在本发明实施例中，首先，利用第一预设算法对相邻两帧的原始点云数据进行处理，确定跟踪目标；其次，利用第二预设算法确定跟踪目标在相邻两帧的位姿并得到跟踪目标的速度初值；最后，利用第三预设算法根据跟踪目标在相邻两帧的位姿、跟踪目标的速度初值和跟踪目标在相邻两帧的原始点云数据，得到跟踪目标的轨迹估计结果。本发明提供的方法利用帧间目标点云匹配结果对帧间位置变化量与航向角变化量进行解算，实现了对跟踪目标位姿变化和运动轨迹的精准估计。
[0058]
优选的，在步骤s202中，利用第一预设算法对相邻帧的原始点云数据进行处理，得到跟踪目标，包括：
[0059]
根据所述相邻帧的原始点云数据，提取待跟踪目标；
[0060]
利用最近邻数据关联算法计算所述待跟踪目标在相邻两帧的相似度；
[0061]
根据所述相邻两帧的相似度，确定跟踪目标。
[0062]
优选的，所述相邻两帧的相似度matsimi(i,j)包括整数部分possimi
i,j
和小数部分geosimi
i,j
，
[0063]
matsimi(i,j)＝possimi
i,j
+geosimi
i,j
；其中：
[0064][0065][0066]
为通过同一激光雷达测定的相邻两帧中待跟踪目标的质心平面距离，d
thre
为预设的平面距离阈值，和分别为第k帧和第k+1帧中的待跟踪目标的边长，radius＝2(length+width)，round表示将数字四舍五入到个位数。
[0067]
作为一个具体的实施例，对于城市场景中通行的目标，目标的运动速度一般不超
过20m/s，激光雷达的采样频率为5hz，因此，相邻帧中同一跟踪目标的平面位置变化应小于(20m/s
×
0.2s)＝4m，可选择d
thre
＝8m。
[0068]
优选的，当相邻两帧中所述待跟踪目标的质心平面距离大于所述平面距离阈值时，确定所述待跟踪在相邻两帧中的相似度为0，且所述待跟踪目标不是跟踪目标。
[0069]
优选的，对于第k帧原始点云数据中的待跟踪目标，在第k+1帧点云数据中选择相似度最大且大于0的待跟踪作为跟踪目标。
[0070]
作为一个具体的实施例，识别第k帧中所有的待跟踪目标，汇总为集合识别第k+1帧中所有的待跟踪目标，汇总为集合确定跟踪目标即：在连续两帧中找出相同的待跟踪目标；所述两帧中相同的待跟踪目标即为跟踪目标。确定所述跟踪目标后，将gk和g
k+1
融合，并将其中相同的待跟踪目标合并，组成新的第k+1帧待跟踪目标集合，用于与第k+2帧的待跟踪目标集合进行计算。
[0071]
作为一个具体的实施例：对于相邻两帧中的待跟踪目标，计算其在第k帧和第k+1帧中的相似度，选择在相邻两帧中相似度最大且possimi大于0的待跟踪目标作为跟踪目标；当有新的目标出现在激光雷达的测距范围时，在待跟踪目标列表中加入所述新的待跟踪目标。
[0072]
作为一个具体的实施例，当连续帧中相同的待跟踪目标被遮挡或离开激光雷达的测距范围(待跟踪目标在连续5帧中均未跟踪到)，则判定对所述待跟踪目标失去跟踪，并在待跟踪目标列表中删除所述待跟踪目标，结束对所述待跟踪目标的跟踪。
[0073]
优选的，在步骤s203中，利用第二预设算法确定所述跟踪目标在相邻两帧的位姿，包括：利用卡尔曼滤波计算所述跟踪目标在相邻两帧的位姿。
[0074]
作为一个具体的实施例，在步骤s203中，由于城市道路环境地面起伏较小，所述跟踪目标在同一个固定激光雷达的扫描范围内运动时，可认为其在高程上变化很小，仅进行平面运动。利用卡尔曼滤波对两帧中的计算所述跟踪目标在第k帧和第k+1帧中的质心位置分别为和设定所述跟踪目标进行匀速直线运动，根据公式vk＝(x
k+1-xk)/t；vk＝(y
k+1-yk)/t；t为激光雷达采样周期，得到所述跟踪目标的速度初值。
[0075]
优选的，在步骤s204中，利用第三预设算法根据所述跟踪目标在相邻两帧的位姿、所述跟踪目标的速度初值和所述跟踪目标在相邻两帧的原始点云数据，得到所述跟踪目标的轨迹估计结果，包括：
[0076]
根据所述跟踪目标在相邻两帧的位姿，得到初始变换参数；
[0077]
根据所述初始变换参数、所述跟踪目标在相邻两帧的原始点云数据，利用icp算法求解精确变换参数；
[0078]
根据所述精确变换参数和所述跟踪目标的速度初值，得到所述跟踪目标的轨迹估计结果。
[0079]
作为一个具体的实施例，由于道路中动态目标在激光雷达相邻帧(采样间隔为0.05-0.2hz)中位姿变化通常不大，以利用卡尔曼滤波计算得到的相邻两帧中跟踪目标的质心距离变化差值作为初始变换参数中的位移初值；将icp函数参数中的距离阈值d
outliers
设为0.1m；使用pcl库提供的icp函数进行相邻帧跟踪目标的点云进行配准，得到精确变换
参数；再结合所述跟踪目标的速度初值，对所述跟踪目标的轨迹进行估计。
[0080]
作为一个具体的实施例，对于在连续多帧内成功跟踪到的跟踪目标，将所述跟踪目标在激光雷达多视角扫描得到的点云结果进行融合，恢复出所述跟踪目标的三维形状。
[0081]
本发明实施例提供了一种激光雷达跟踪装置，其结构框图，如图3所示，所述激光雷达跟踪装置3000包括：
[0082]
获取模块3001，用于获取多帧原始点云数据；
[0083]
跟踪目标确定模块3002，用于利用第一预设算法对相邻两帧的原始点云数据进行处理，确定跟踪目标；
[0084]
计算模块3003，利用第二预设算法确定所述跟踪目标在相邻两帧的位姿，根据所述跟踪目标在相邻两帧的位姿，得到所述跟踪目标的速度初值；
[0085]
轨迹估计模块3004，利用第三预设算法根据所述跟踪目标在相邻两帧的位姿、所述跟踪目标的速度初值和所述跟踪目标在相邻两帧的原始点云数据，得到所述跟踪目标的轨迹估计结果。
[0086]
如图4所示，上述激光雷达跟踪方法，本发明还相应提供了一种电子设备，该电子设备可以是移动终端、桌上型计算机、笔记本、掌上电脑及服务器等计算设备。该电子设备包括处理器10、存储器20及显示器30。
[0087]
存储器20在一些实施例中可以是计算机设备的内部存储单元，例如计算机设备的硬盘或内存。存储器20在另一些实施例中也可以是计算机设备的外部存储设备，例如计算机设备上配备的插接式硬盘，智能存储卡(smart media card,smc)，安全数字(secure digital,sd)卡，闪存卡(flash card)等。进一步地，存储器20还可以既包括计算机设备的内部存储单元也包括外部存储设备。存储器20用于存储安装于计算机设备的应用软件及各类数据，例如安装计算机设备的程序代码等。存储器20还可以用于暂时地存储已经输出或者将要输出的数据。在一实施例中，存储器20上存储有激光雷达跟踪程序40，该激光雷达跟踪程序40可被处理器10所执行，从而实现本发明各实施例的激光雷达跟踪方法。
[0088]
处理器10在一些实施例中可以是一中央处理器(central processing unit,cpu)，微处理器或其他数据处理芯片，用于运行存储器20中存储的程序代码或处理数据，例如执行激光雷达跟踪程序等。
[0089]
显示器30在一些实施例中可以是led显示器、液晶显示器、触控式液晶显示器以及oled(organic light-emitting diode，有机发光二极管)触摸器等。显示器30用于显示在计算机设备的信息以及用于显示可视化的用户界面。计算机设备的部件10-30通过系统总线相互通信。
[0090]
本实施例还提供了一种计算机可读存储介质，其上存储有激光雷达跟踪程序，所述计算机该程序被处理器执行时，实现如上述任一技术方案所述的激光雷达跟踪方法。
[0091]
本发明公开的一种激光雷达跟踪方法、装置、电子设备及计算机可读存储介质，首先，利用第一预设算法对相邻两帧的原始点云数据进行处理，确定跟踪目标；其次，利用第二预设算法确定跟踪目标在相邻两帧的位姿并得到跟踪目标的速度初值；最后，利用第三预设算法根据跟踪目标在相邻两帧的位姿、跟踪目标的速度初值和跟踪目标在相邻两帧的原始点云数据，得到跟踪目标的轨迹估计结果；利用帧间目标点云匹配结果对帧间位置变化量与航向角变化量进行解算，实现了对跟踪目标的位姿变化和运动轨迹的精准估计。
[0092]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。本技术所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(rom)、可编程rom(prom)、电可编程rom(eprom)、电可擦除可编程rom(eeprom)或闪存。易失性存储器可包括随机存取存储器(ram)或者外部高速缓冲存储器。作为说明而非局限，ram以多种形式可得，诸如静态ram(sram)、动态ram(dram)、同步dram(sdram)、双数据率sdram(ddrsdram)、增强型sdram(esdram)、同步链路(synchlink)dram(sldram)、存储器总线(rambus)直接ram(rdram)、直接存储器总线动态ram(drdram)、以及存储器总线动态ram(rdram)等。
[0093]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。