[发明专利]一种基于PHD滤波的群体机器人分布式多目标跟踪方法

权利要求书

1.一种基于PHD滤波的群体机器人分布式多目标跟踪方法，其特征在于：包括以下步骤：

S1：假设在初始状态下，群机器人在给定的方形约束边界内随机分布，根据多机器人所在的位置坐标和权重，构造出Power图，划分搜索区域；

假设给定的搜索区域为E∈R²，获取机器人坐标点并完成编号预设初始PHD，即作为x的权值，则机器人坐标点集的一个Power图满足：

式中，且

S2：求解每个机器人对应的Power单元的加权质心，记为满足：

式中ρ(x)为当前时刻的每个Power图的重要性权重函数，与搜索覆盖的常数项和以目标位置为中心的径向基函数线性相关，用PHD来代替，即能够引导机器人向高目标密度的区域搜索，初始时刻t＝0，求得初始质心为h_i(0)；

S3：所有的机器人开始由当前位置X移动至对应的h_i(0)，收集相关量测信息，计算归一化常数λ(·)，并更新Power图；

S4：根据收集的量测数据，通过PHD滤波算法，更新PHD并将求得的新的PHD作为权值，执行步骤S2，从而获得新的质心位置坐标H′；

S5：重复S3和S4，直至机器人视野里出现目标；

随着机器人的移动，它们的传感器持续更新量测信息Q_k，收到新的量测数据后，机器人将计算产生新的从而又有新的质心坐标H″，机器人继续搜索工作；

当目标的位置未知且接近平衡时，机器人将尝试以初始速度覆盖搜索区域；当发现当前区域内没有目标时，减少，机器人将避开这些区域；当机器人检测到目标时，将增加，机器人将开始执行第六步去追踪目标；

S6：机器人发现目标后，观测目标的运动状态，采用加速度方差自适应跟踪算法，利用当前雷达测量信息与加速度之间的函数关系，实时估算出加速度扰动量，进行加速度方差自适应调整，保持对该目标的同步追踪。

2.根据权利要求1所述的基于PHD滤波的群体机器人分布式多目标跟踪方法，其特征在于：步骤S3中，每个机器人都配备了各种传感器，能完成相关数据的采集和处理，机器人之间能进行通信，实现信息的交互；

在t时刻，机器人k的自身状态为传感器测量的一组有d个测量值的数据为

搜索目标位置为

根据机器人收集的观测数据，通过计算得到归一化常数，计算方式如下：

Υ_q，θ(x)＝β(q|g(x|θ))η_w(x|θ)

式中，为归一化常数，γ_q，θ(x)为机器人在θ状态下机器人检测到目标位置x并获得数据q的概率，Z(q|θ)为描述杂波分布的函数，η_w(x|θ)为机器人测量数据没有误测或噪声干扰的概率，g(x|θ)为机器人在θ状态下检测到目标的位置x的概率，β(q|g(x|θ))为在g(x|θ)的情况下返回相关数据q的函数。

3.根据权利要求1所述的基于PHD滤波的群体机器人分布式多目标跟踪方法，其特征在于：步骤S4中，根据移动机器人运动状态和得到的量测数据，新时刻的PHD由下面的PHD滤波方法计算获得：

上式中，为t时刻目标的预测PHD，为t时刻目标的PHD，f(x|ω)描述的是目标在初始时刻的位置ω向目标位置x的运动概率，η_s(ω)为ω状态时目标不离开所处Power单元内的概率。

4.根据权利要求1所述的基于PHD滤波的群体机器人分布式多目标跟踪方法，其特征在于：步骤S6中，当机器人发现目标时刻时，在惯性坐标系XOY中，二维情况下，目标的运动模型为：

其中，

X(t)是目标的状态向量，x，分别是目标的位置，速度，加速度，f_x，f_y分别是机动频率在惯性坐标系OX、OY轴上的投影分量且f_x＝f_y＝f，为机动加速度均值分量，W是均值为零、方差为2fρ²的高斯白噪声，w_x、w_y为W在坐标轴的分量；

当机器人的采样周期为T时，对上式进行离散化处理，得到下面形式：

式中，状态噪声Ω(k-1)是均值为零的离散时间白噪声序列；

当机器人的雷达正处XOY惯性坐标系的原点时，二维情况下的目标的观测方程表示为：

Y(t)＝G(t)X(t)+J(t)

其中G(t)表示观测矩阵，J(t)表示观测噪声；

对上面的雷达观测方程进行离散化：

Y(k)＝G[k]X(k)+Λ(k)

式中，状态噪声Λ(k)是均值为零，方差为Γ(k)＝[Δ+τx(k)]²的离散时间白噪声序列，Δ为探测器量测偏差；τ为相对误差系数；

用N(k|k-1)、M(k)代替统计模型状态离散方程中的向量：

对当前模型的离散化方程应用卡尔曼滤波，得到的基于当前统计模型的机动目标跟踪方法，描述为：

A(k)＝Q(k|k-1)G^T(k)[G(k)Q(k|k-1)G^T(k)+Γ(k)]^-1

Q(k|k-1)＝N(k|k-1)Q(k-1|k-1)N^T(k|k-1)+C(k)

Q(k|k)＝Q(k|k-1)-A(k)G(k)Q(k|k-1)

式中，是的下一采样周期的预测值，A(k)是增益矩阵，Qk|k)为协方差矩阵，Γ(k)为噪声协方差矩阵，C(k)是考虑多余回波影响的修正项；

采用加速度方差自适应算法，故有

式中，表示当前目标的预测加速度均值，表示加速度的方差，a_max为最大机动加速度。