B样条曲面三维扩展目标跟踪算法

陈 振,李翠芸,李 想

(西安电子科技大学 电子工程学院,陕西 西安 710071)

1 引 言

目标跟踪是根据传感器获得的数据,对一定观测区域内运动目标的状态进行估计的过程。泊松多伯努利混合(Poisson Multi-Bernoulli Mixture,PMBM)[1]滤波器,在目标存在漏检、新生或者消亡的情况下,可以准确地估计多目标的状态,受到大量学者的青睐。随着传感器精度的不断提高,针对扩展目标估计的研究逐渐兴起,即除了估计目标的位置和数量外,还要拟合目标的具体形状。其中,KOCH[2]于2008年利用椭圆的长短轴和旋转角等参数可以用随机矩阵(Random Matrices,RM)表示的特性,用随机矩阵作为椭圆形扩展目标的形状参数。但随机矩阵可以利用椭圆形状建模出目标的轮廓和朝向,却无法建模出复杂形状的信息。2009年,BAUM等[3]通过伪量测进行形状的修正,提出随机超曲面模型,并于2011年提出了星凸随机超曲面模型[4]。该模型利用径向函数可以有效地估计形状不规则的目标,但是需要考虑一个约束条件[5]。文献[6]用乘性噪声模型对目标进行建模,能够在交叉、近邻场景中实现对目标的有效跟踪。为了细致地刻画目标形状,LAN等[7]提出了多椭圆来表示目标形状,通过传递多个随机矩阵以实现对目标形状的刻画。但这种方法需要先验设置椭圆数量,并且形状估计的效果受椭圆之间位置的影响。除此之外,高斯过程回归也常应用于扩展目标估计。文献[8]中将高斯过程用于扩展目标估计并采用了轴对称的协方差函数,在估计非轴对称目标时效果较差。随后,陈辉等[9]将随机超曲面与高斯过程回归模型结合,对星凸型扩展目标进行了研究,但存在径向函数计算开销大的问题。文献[10]中利用B样条曲线实现了对二维扩展目标的估计,即实现了对任意形状目标的估计,但B样条曲线受控制点选取的影响较大。文献[11]中通过标签来实现非椭球目标和子对象之间的关联,完成对子对象数量不等的非椭球目标的估计,能够自适应地调整子对象的数量,并且在扩展状态发生变化时具有更好的性能。文献[12]中提出了一种基于控制点的扩展目标估计方法来简化扩展形状描述问题,之后又针对量测不均匀分布提出了条件高斯混合模型[13]。YANG等[14]把B样条与PHD滤波器相结合,对星凸型目标进行估计,实现了较好的估计效果。

以上的研究是在二维平面对目标形状进行估计,形状估计和跟踪技术都比较多。尽管能够生成三维量测数据的激光雷达等传感器的应用越来越多,但在三维空间中解决扩展目标跟踪的问题的研究却比较少。这些研究中的大多数都是对基本几何模型的直接概括,并通过例如椭圆体或边界框来表示范围。文献[15]中通过径向函数描述投影的形状,并结合高斯过程来估计三维空间下的目标形状,但此方法需要大量的点云数据,使计算负载较大。文献[16]从稀疏点云测量中同时估计扩展三维对象表面和运动学状态,再结合高斯过程实现对三维汽车模型的扩展目标形状和运动估计,但此方法耗费计算资源较大,且最后实现的估计精度也较低。

针对上述问题,笔者提出了一种低量测率下基于B样条曲面的扩展目标(B-Spline-PMBM)滤波器。该算法通过小波聚类得到同一时刻的各个目标的量测集合,将量测集输入PMBM滤波器,获得目标质心运动状态并更新控制矩阵。随后,通过控制矩阵生成当前时刻的B样条节点,利用B样条曲面获得目标三维形状,结合目标质心状态与形状估计完成对三维扩展目标的跟踪。通过对比实验,验证了该算法在三维目标形状估计方面的有效性。

2 三维量测数据融合

三维扩展目标的估计不同于二维扩展目标,所需的量测数量多的同时要求量测的完整性高。一般凭借单个传感器,很难在单位时间内得到足够的量测对三维目标形状进行估计。因此,在多传感器探测下可以利用数据融合解决三维目标形状估计中的量测数量多和完整性要求高的问题,保证目标形状估计的准确度。笔者融合的数据来自3个同质的传感器,设置在3个不同的角度,分别能获得三维目标的不同角度的量测值。由于不同角度的传感器获得的量测的坐标系一般是相对于本传感器,融合即是指将3个传感器的量测首先经过坐标变换到同一坐标系下,然后利用小波变换将属于同一个目标的量测划分出来,经过上述步骤最终得到属于同一个目标的量测值。

(1)

小波聚类的主要步骤如下。

输入:同一时刻的多传感器量测数据。

输出:聚类量测簇。

(1)坐标系转换;

(2)根据预先设定的特征空间大小,划分特征空间并标记量测;

(3)对每一个特征空间使用小波聚类;

(4)寻找连通域,形成量测簇;

(5)再次标记量测簇;

(6)根据标记簇中各个特征空间的量测标记,将量测与量测所在簇进行标签,形成查找表;

(7)根据查找表,形成由一个个量测点组成的量测簇。

3 三维扩展目标建模

在对三维扩展目标进行跟踪时,需要对各个目标产生的量测数目、目标的运动状态以及目标的扩展形状三部分信息进行估计。其中三维目标扩展形状的建模是难点,传统算法通常是将扩展形状建模为椭球,但有些模型形状复杂且难以表征,例如立方体、十字架等。因此,笔者提出了用B样条曲面对三维扩展目标形状进行建模。

3.1 扩展目标状态

状态空间模型表示如下:

x=(xr,xc,xε)∈R+×Rn×Cd,

(2)

其中,xr∈R+,表示目标产生量测时的泊松率;R+表示正实数空间。xc∈Rn,表示目标的运动状态;Rn表示n维实数空间。xε∈Cd,表示目标扩展形状;Cd表示描述目标扩展形状的d维矩阵空间。

k时刻扩展目标的状态空间分布是上面提到的三部分之积,表示如下:

(3)

(4)

(5)

3.2 B样条曲面拟合

图1 B样条曲面示意图

如图1所示,给定参数轴u和v的节点矢量U=[u0,u1,…,um+p]和V=[v1,v2,…,vn+q],p×q阶B样条曲面可定义如下:

(6)

节点矢量t将区间[t1,tm]分成多个间隔,在每个间隔的基础上定义B样条曲线的基函数,即deBoor-Cox递归公式,表示为

(7)

其中,ti表示节点矢量t中的第i个元素,p表示基函数的阶数。

3.3 控制矩阵的获取

控制矩阵中的每一个元素都是三维空间中的一组三维坐标,称为控制点。控制矩阵C是通过对三维空间数据融合后的所有量测进行划分得到的。首先沿z轴划分m个区间,对每个区间沿顺时针方向划分为n个子空间,每个子空间包含一个控制点。子空间中的控制点由划分到该空间内的量测进行更新。

由上可知,m和n越大,表示划分子区间越多,B样条曲面的建模粒度越细,同时所需的有效量测数量越多,计算量越大。所以在用B样条曲面对扩展目标进行建模时,大小合适的控制矩阵是能否快速、准确地拟合目标扩展形状的关键。控制矩阵维数m和n的选取与采用传感器的精度有关。若m和n选取得过大,由于受到传感器精度的限制,则目标的形状估计并不能随着划分区间的增加而表现更多的细节信息。假设Δr为传感器的距离分辨力,理论上在进行控制矩阵的维数选取时,每一个划分区间应不小于Δr,可理解为

(8)

其中,r为初始化的目标半径。

4 B样条曲面的三维PMBM滤波算法

算法的流程如下:

步骤1 获得多目标量测。

首先对k时刻的量测集合进行多传感器数据融合,得到各个目标的量测簇集合。

步骤2 预测。

扩展目标的预测密度pk|k-1(x)表示如下:

(9)

其中,f(xk|xk-1)表示从当前时刻到下一时刻的转移密度。对上式第2行进行近似处理,量测率的预测结果如下:

(10)

其中,η为指数遗忘因子,p(xr,k-1|Z1:k-1)表示泊松率密度函数,fr(xr,k|k-1|xr,k-1)为泊松率转移概率密度。

式(9)的第3行为运动状态部分,在线性高斯运动模型下可得到闭合形式的解为

(11)

其中,p(xc,k-1|xε,k-1,Z1:k-1)表示运动状态概率密度,fc(xc,k|k-1|xc,k-1,xε,k|k-1)为对应的转移概率密度,F为状态转移矩阵,Q为过程噪声协方差矩阵。

式(9)的第4行为目标的扩展形状部分,该部分使用B样条曲面模型进行建模,其分布可以近似为

(12)

其中,p(xε,k-1|Z1:k-1)为扩展状态概率密度,fε(xε,k|k-1|xε,k-1)是扩展状态转移概率密度。

步骤3 更新。

输入多传感器融合后各目标的量测集。每一个扩展目标会根据该时刻划分给该目标的量测集W对目标的运动状态、目标量测率和目标扩展形状进行更新。若目标匹配到对应量测,则目标扩展状态的更新密度为

(13)

对目标运动状态参数的更新:

(14)

Pk|k=Pk|k-1-KkHPk|k-1,

(15)

(16)

(17)

(18)

(19)

5 仿真实验与分析

为了验证上面所提的B-Spline-PMBM算法跟踪三维扩展目标时对质心运动状态和形状演化的有效性,设置了3个实验。实验1对比在单目标场景下,估计椭球形三维扩展目标的形状时B-Spline-PMBM和GGIW-PMBM[17]的有效性;实验2对比多目标场景下,估计多种扩展目标形状时两种算法的有效性。在每个仿真实验中,蒙特卡罗次数均为100。实验3使用真实点云数据集验证所提出的算法对复杂形状的跟踪和估计效果。实验中使用的评价指标有两种:一种是对于目标质心运动性能的评价,采用的方法是最优子模式分配距离(Optimal Sub-Pattern Assignment,OSPA)[17];另一种是对于目标形状的估计性能的评价,采用的方法是交并比(Intersection-over-Union,IoU)[18]。

5.1 实验1

图2 跟踪椭球目标时传感器接收到的量测

(20)

(21)

图3 B-spline-PMBM跟踪椭球目标放大图

图4 GGIW-PMBM跟踪椭球目标放大图

图5为目标形状估计的IoU,图6为目标的质心位置平均OSPA距离。可以看出,两种算法对于椭球目标的形状均可实现良好的估计效果,并且估计的形状贴合真实目标。

两种算法的IoU最后都收敛到0.9左右,精度高。区别在于,GGIW-PMBM估计扩展形状时,使用的是长短轴,利用了椭球的方程;而B-Spline-PMBM使用的是控制矩阵。从图5中可以看出,前6 s内B-Spline-PMBM的收敛速度略慢,但收敛后B-Spline-PMBM的IoU稳定性明显优于GGIW-PMBM算法的。

图5 跟踪椭球目标时两种算法的IoU

从图6可以看出,B-Spline-PMBM算法的OSPA值小于GGIW-PMBM算法的且波动小。两种算法对目标质心的估计都是通过PMBM算法获得的,但B-Spline-PMBM算法在PMBM估计质心的基础上通过控制矩阵的形状信息对质心进行修正,所以质心估计得更准确。综上,两种算法对椭球目标的质心和扩展形状都能进行准确估计。表1为达到相同IoU时,单传感器与多传感器B-Spline-PMBM算法的运行时间对比。

图6 跟踪椭球目标时两种算法的OSPA

表1 B-Spline-PMBM单-多传感器算法的运行时间对比 s

从表1可以看出,由于多传感器B-Spline-PMBM算法获得的量测更多,经过三维量测融合后,三维形状的估计精度能在一定时间内达到较高的水平。单传感器B-Spline-PMBM算法只能获得目标某一个面的量测,数量有限且不够完整,所以估计精度不能随着时间的推移而升高。从表1中还可以看出,单传感器B-Spline-PMBM算法的IoU会在0.4左右停滞,难以完成对三维形状的准确估计。

5.2 实验2

为了研究B-Spline-PMBM和GGIW-PMBM算法同时估计多个三维目标形状的能力,设计了如下的跟踪场景:跟踪区域为[-200 m,200 m]×[-200 m,200 m]×[-200 m,200 m]。3个传感器的位置分别为[-200,200,200]、[-200,0,-200]和[0,0,-200]。图7是多目标量测,图中3个目标的运动轨迹并没有在运动过程中相交,其他参数例如目标存活概率、目标运动模型、目标量测率、杂波泊松率、目标检测概率、参数m、n的选取等,均与实验1相同。0为目标初始化参数。

图7 多目标量测

表2中,目标1是轴长分别为2 m、2 m和1 m的椭球;目标2是边长为2 m的立方体;目标3为十字架,长为10 m,宽为8 m,高为2 m。目标的体积差异不影响本章节算法的形状辨识性能,所有目标均初始化为一个球径为3 m的球体。图8与图10为运动轨迹图,图9与图11为目标放大图。

表2 目标的初始化参数

图8 B-Spline-PMBM跟踪立方体目标运动轨迹

图9 B-Spline-PMBM跟踪立方体目标放大图

图10 B-Spline-PMBM跟踪十字架目标运动轨迹

图11 B-Spline-PMBM跟踪十字架目标放大图

从图8至图11可以看出,即使十字架形目标形状复杂,B-Spline-PMBM依然可以相对准确地估计出跟踪目标的三维扩展形状。图12和图13分别为GGIW-PMBM跟踪立方体目标和十字架目标的局部放大图。

从图12和图13看出,GGIW-PMBM算法估计方形和十字架形的目标时,最终会收敛为一个球体,不能实现对形状的准确估计。因为GGIW-PMBM算法是根据长短轴和椭球方程来估计扩展目标状态的。结合两种算法的估计结果,可以看出在扩展目标形状估计方面,无论是从精度还是泛用性上来说,B-Spline-PMBM算法的估计能力明显优于GGIW-PMBM算法的。图14为B-Spline-PMBM算法与真实目标数目的对比。图15为两种算法的OSPA比较。

图12 GGIW-PMBM跟踪立方体目标放大图

图13 GGIW-PMBM跟踪十字架目标放大图

图14 目标数估计

图15 OSPA距离

不难看出,所提算法在多目标场景下不管是对目标质心位置的估计,还是对目标数目的估计都有良好的效果。但是,由于PMBM滤波器本身的特性,即引入了假设的思想,当目标数目发生变化时,会生成一个新的全局假设并根据权值判断多目标的状态。所以当目标2在31 s时刻死亡时,会存在目标2还未死亡的全局假设,从而导致了估计的目标数在31 s时刻依然为3;之后在32 s时刻,该全局假设的权值低于阈值,目标数目的估计收敛为真实值,因此出现了图14中估计结果相对真实结果的延迟。

5.3 实验3

为了验证B-Spline-PMBM算法在实际场景中的有效性,使用公开ShapeNetCore点云数据集中的汽车和飞机点云数据来验证本算法的三维形状估计效果。跟踪区域的大小设置为[-100 m,100 m]×[-100 m,100 m]×[-100 m,100 m],跟踪时长为30 s。使用随机下采样对原始点云进行处理,模拟不同时刻的目标点云数据。图16和图17是两种视角下的汽车原始点云图。

图16 汽车原始点云图(视角1)

图17 汽车原始点云图(视角2)

从图16和图17看,汽车点云数据形状比较规则,类似于立方体。

图18和图19是使用文中算法获取的汽车形状估计的结果。从图18和19可以看出,B-Spline-PMBM对于汽车这类轮廓形状比较规则的目标可以较为准确地估计出跟踪目标的三维扩展形状。对于汽车中尺寸较小的部件(如车轮),图18和图19中的结果无法明显地显示其形状。若需要更精细的形状估计结果,需要增加控制点的个数和量测的数量,计算复杂度会增大。

图18 汽车形状估计结果(视角1)

图19 汽车形状估计结果(视角2)

图20和图21是两种视角下的飞机原始点云图。图22和图23是使用文中算法获取的飞机形状估计的结果。

图20 飞机原始点云图(视角1)

图21 飞机原始点云图(视角2)

图22 飞机形状估计结果(视角1)

图23 飞机形状估计结果(视角2)

图20和图21中的飞机形状相比十字架目标更为复杂。从图22和23可以看出,B-Spline-PMBM能够拟合出飞机的机身和机翼形状,对于飞机的尾部和发动机能够粗略地拟合。由于图21中飞机尾部的点云高度大于飞机其他部分点云的,导致控制点拟合成曲面时,尾部区域所对应的控制点会与其相邻控制点进行连接,因此只能实现尾翼的大致拟合。

由仿真和真实点云数据验证获得的结果可以看出,对于简单形状目标和复杂形状目标,文中所述的算法都能较好地估计出三维扩展目标的形状。

6 总 结

针对低量测率下三维空间中的多扩展目标形状估计问题,笔者提出了在多传感器的情况下,基于B样条曲面的多目标PMBM滤波器。首先将得到的量测簇输入至PMBM滤波器,获得目标的质心并更新对应的控制矩阵;其次,通过控制矩阵进行B样条曲面估计;最后,结合质心状态,获得三维目标的扩展状态。仿真实验结果和真实点云数据集的验证结果表明,针对复杂形状的三维扩展目标,现有GGIW-PMBM滤波器不能准确地估计扩展形状,相比之下,B-Spline-PMBM滤波器的估计精度高且泛用性广,能够适用于产生点云数据的传感器如激光雷达的应用之中。

下一步将针对B样条曲面的基函数选取问题展开研究。