[发明专利]一种用于解决合并量测问题的Gibbs采样方法

权利要求书

1.一种用于解决合并量测问题的Gibbs采样方法，其特征在于，包括以下步骤：

S1.获取预测LMB信息：

已知k-1时刻的LMB信息其中是k-1时刻轨迹i的标签，不同轨迹的标签互不相同；是k-1时刻轨迹i的存在概率，和分别是k-1时刻轨迹i的第c个高斯分量的权重、期望和协方差矩阵；是k-1时刻轨迹i的高斯分量数目，n_k-1是k-1时刻轨迹数目；具体方法如下：

S11.对于k-1时刻存在，k时刻依然存在的轨迹，称之为存活轨迹；对于存活的轨迹i＝1,…,n_k-1，需要得到的预测信息为：

其中下标k|k-1表示已知k-1时刻的信息的前提下，预测得到k时刻的信息；

使用Kalman运动模型进行预测，模型如下：

x_k|k-1＝F_k-1x_k-1+ε_k-1

其中x_k|k-1和x_k-1分别表示k时刻单个轨迹的预测运动状态和k-1时刻单个轨迹的运动状态；F_k-1是k-1时刻的运动状态转移矩阵，将初始运动状态映射为预测后的运动状态；ε_k-1是k-1时刻的过程噪声，服从期望为0，协方差矩阵为Q_k-1的高斯分布；

具体预测方程如下：

其中为k-1时刻轨迹i存活到k时刻的存活概率；为矩阵转置操作；

可知在存活轨迹的预测中，标签保持不变，轨迹数目和每一个轨迹下的高斯分量数目也都不变；

S12.对于k时刻进入观测范围的轨迹，称之为新生轨迹；对于新生的轨迹i_b＝1,…,b_k，已知以下先验信息为：

其中下标b代表新生轨迹，以区别于存活轨迹；为新生轨迹i_b的标签，它的选取原则是各个新生轨迹的标签必须互不相同，且与已经使用过的存活轨迹的标签不能相同；为新生轨迹i_b的存活概率；和分别是k时刻新生轨迹i_b的第c_b个高斯分量的权重、期望和协方差矩阵，有是k时刻新生轨迹i_b的高斯分量数目，b_k为k时刻新生轨迹的总数；

将存活轨迹和新生轨迹的预测信息合并，得到预测信息：

其中，n_k|k-1＝n_k-1+b_k为k时刻的预测轨迹数，也即存活轨迹数加上新生轨迹数；

S2.将LMB信息转为GLMB信息：

现在已知k时刻的预测LMB信息LMB_k|k-1，需要在n_k|k-1个预测轨迹中挑选出不同的轨迹的组合，作为不同的假设，构成预测GLMB信息；用假设标签集、假设权重和对应的高斯分量参数来表示预测GLMB信息为：

具体获取方式如下：

其中，I(h)是第h个假设的标签上标集合；是第h个假设中所有被选取的预测目标的标签的集合；是第h个假设的权重，等于所有被选出的轨迹的存在概率以及所有未被选中的轨迹的不存在概率之积；是第h个假设的相关高斯分量信息，由所有被选取的预测目标的高斯分量信息组成；

只保留超过权重阈值W_τ的权重，重新索引这些假设为：

其中H_k|k-1为超过权重阈值的预测假设个数，满足

预测假设的权重最后有一个归一化过程，从而得到完整的预测GLMB信息；

S3.获取更新GLMB信息：

已知k时刻的预测LMB信息LMB_k|k-1和预测GLMB信息GLMB_k|k-1；利用观测信息，将预测轨迹和观测信息关联起来，然后更新相应的预测信息；需要得到的更新GLMB信息为其中H_k为k时刻更新假设个数；关联通过Gibbs采样的方法实现，预测信息的更新通过Kalman观测模型实现；具体方法如下：

S31.使用Kalman观测模型进行更新，模型如下：

x_k＝ψ_k(x_k|k-1)+η_k

其中x_k表示k时刻单个轨迹的运动状态；ψ_k(·)是k时刻的观测函数，将预测运动状态映射为更新后的运动状态；η_k是k时刻的观测噪声，服从期望为0，协方差矩阵为R_k的高斯分布；

计算成本矩阵C，它的元素值反映了将某个预测轨迹和某个观测值相关联的相对可能性，它是一个n_k|k-1×(M_k+1)的矩阵，矩阵中位置(i,j)处的元素值：

其中，为k时刻预测轨迹i被传感器检测到的检测概率；det(·)为对矩阵求行列式操作；矩阵为h_k(·)的雅可比矩阵，为函数f(x)在x₀处的偏导数；是k时刻的观测集Z_k中第j个单观测，在合并量测情况下，可能由一个或者多个轨迹产生，也可能由杂波产生；M_k是k时刻观测的数目；k(·)为杂波在观测空间上的强度函数；

S32.对于每一个预测假设，分别要做次Gibbs采样，产生个更新假设；其中round(·)为四舍五入操作；每一个预测假设的Gibbs采样都是独立进行的，且流程一致，因此只给出一个预测假设的采样过程，其余预测假设的采样遍历此步骤即可，具体流程如下：

S321.根据Gibbs采样的思想，第一次无需采样，直接任意指定初始样本值即可；初始值为该预测假设中所有的预测轨迹均和j＝M_k+1关联，也即该预测假设中所有的预测轨迹漏检；该预测假设产生的第一个更新假设的标签集和相应的高斯分量信息均不变，即假设权重其中代表多目标指数；该更新权重的获取可以理解为，在原预测假设权重的基础上，乘以该预测假设中所有轨迹漏检的概率；由于所有预测轨迹都漏检，因此所有的观测都是由杂波产生，因此再乘以所有观测均为杂波的强度函数；这样，就得到了第一个更新假设

S322.从第二个到第更新假设开始，每一次Gibbs采样都按照成本矩阵C来进行，对第二个更新假设具体是：首先只保留C中对应于I(h)的行，每一行的采样代表该预测假设中一个预测目标和观测的关联过程；由于合并量测中，多个目标可能对应同一个观测，因此每一行的采样方法一样：对于C中任意一行，找出所有大于等于1的元素，列出它们的元素值，进行归一化处理，按照概率进行采样，采样的元素对应的列数即为轨迹i的关联值θ_i；如果所有元素均小于1，则选取最大的元素对应的列数作为θ_i；处理完该预测假设下所有预测轨迹对应的行之后，则完成了一次预测假设的更新；该预测假设产生的第二个更新假设的标签集不变，即假设权重该更新权重的获取方式是将S321步中的替换为高斯分量信息通过Kalman滤波器更新，具体如下，

其中为Kalman增益矩阵，E为单位矩阵；然后将预测轨迹i的所有高斯分量归一化，即可得到更新假设的高斯分量信息为：

其中对于该预测假设下所有的预测轨迹i，都有即高斯分量的数目全都保持不变；

其余假设遍历S322即可；

S323.完成个更新假设后，如果其中有重复的关联结果，则去掉重复，得到个关联结果不重复的更新假设；若没有重复的，则令重新索引为

S33.将所有的预测假设按照S32步处理，然后将所有的更新假设权重归一化，得到完整的更新GLMB信息

S4.将GLMB信息转回LMB信息：

得到更新后的LMB信息具体获取方式如下：

更新轨迹i的标签从所有的更新假设的标签集中选取；更新轨迹i的存在概率为所有包含该轨迹的更新假设的权重之和；高斯分量信息是更新GLMB信息中带有相同标签的高斯分量信息的集合；为更新轨迹i高斯分量数目，取决于该轨迹和多少个观测有关联；n_k＝n_k|k-1为更新轨迹的数目；

S5.轨迹修剪：

首先只保留大于等于存在概率门限r_th的更新轨迹；

然后对于余留下来的轨迹，需要继续清理各自的高斯分量，分为高斯剪枝、高斯合并和高斯限数三个步骤：剪枝和去除轨迹类似，只保留高于门限w_th的高斯分量；高斯合并是将在状态空间上接近的所有高斯分量合并为一个高斯分量；高斯限数是指，如果一个轨迹的高斯分量数目超过上限v_up，则保留v_up个权重较大的分量；最后，归一化这些高斯分量权重，得到修剪后的LMB信息为LMB'_k；

S6.状态估计：

首先估计k时刻目标数：其中|·|表示取集合元素数目操作；即存在概率超过门限r_ex的轨迹视为目标，该时刻目标数的估计等于所有超过门限的轨迹数之和；

再来估计每个目标的运动状态：其中即对于每一个被判定为目标的轨迹，采用期望最大期望估计法，用高斯分量的加权和作为目标状态的估计；

最后，得到k时刻的状态提取信息

完成第一个时刻的跟踪后，后续步骤不断迭代，得到完整的跟踪轨迹。