一种基于PHD滤波的群体机器人分布式多目标跟踪方法与流程

本发明属于机器人跟踪技术领域，涉及一种基于phd滤波的群体机器人分布式多目标跟踪方法。

背景技术：

近年来，随着工业化和信息化的迅速发展，机器人技术已经成为国内外众多学者与专家的研究热点。其中，群体智能算法方向的研究尤为深入，诸如滤波算法、遗传算法、蚁群算法、粒子群算法等等，一些群体智能算法已经运用在了实际的机器人系统中，并发挥了很大作用，例如：国外文献《studyofformationcontrolandobstacleavoidanceofswarmrobotsusingevolutionaryalgorithms》将细菌觅食(bfoa)和粒子群优化(pso)算法用于群体机器人的编队控制和避障上，国内文献《一种适用于多机器人搜索动态目标的改进粒子群算法》提出了一种改进的粒子群算法，可以更为有效地寻找并跟踪动态目标。

对目标进行搜索跟踪是机器人技术中的一项常规任务，在国防、军事、环境监控、智能交通、灾后救援等领域都有着极为广泛的应用价值。多目标跟踪是指利用机器人的传感器量测数据对其搜索范围内的多个目标的位置、速度等运动状态进行估计的方法，这个过程可以看成一个典型的滤波问题。然而，在很多实际的应用场景中，由于往往会存在目标个数未知、分布复杂，杂波密集、传感器观测误差等问题，干扰因素很多，如何有效准确地估计目标的数目和运动状态，提高对目标的识别和跟踪性能成为当前多目标跟踪问题的关键难点。

传统的多目标跟踪方法主要有联合概率数据互联(jpda)和多假设跟踪(mht)，前者建立数据关联的基础上，将多目标的跟踪问题转化为多个单目标状态的估计问题，在目标个数固定、杂波数量较少、观测信息误差小的简单环境中跟踪效果较好，受到了广泛研究，但是面对多目标、杂波密集和观测数据误差较大的环境，计算复杂，易产生组合爆炸，鲁棒性和跟踪精度也严重降低，这就大大限制了它的实际应用。基于随机有限集(rfs)的多目标跟踪概率假设密度(phd)滤波方法有效避免了数据关联问题，利用一阶统计矩近似多目标后验概率密度分布函数，即phd，phd峰值对应目标状态，求phd积分得到多目标强度，同时将多目标状态集的后验phd解耦，投影在多个单目标状态空间上，避免了多目标贝叶斯滤波器在rfs空间上进行递推，降低了计算难度，为多目标跟踪问题提供了一种新的解决方案。

在phd滤波方法中，通常是假设杂波在搜索区域呈均匀分布，且数量满足泊松分布，根据传感器得到的所有量测信息对当前目标的运动状态进行估计，获取最优估计值，而利用持续的后续量测信息对某一时刻状态估计进行平滑，从而得到更加精确的估计结果。运用phd滤波方法，虽然从一定程度上改善了复杂环境下多个目标的跟踪性能，但是没有充分利用目标的观测信息，造成有用量测数据的丢失，虽然能较为准确地对目标运动状态进行估计，但是并没有从机器人的运动上解决如何实现同步跟踪，此外对于多机器人的路径规划问题也还存在着很大的局限性。

目前关于机器人多目标跟踪的方案有很多，这些方案或是极大程度地提高了对微弱目标的识别性能，或是解决了低信噪比条件下目标的检测与跟踪，或是在基于phd滤波的多目标跟踪方法中结合了目标幅值信息，提高了量测信息的利用率，或是解决了新生目标强度未知时的多目标运动状态与目标数目估计问题。虽然这些技术解决了多目标跟踪的某个方面的不足，但是都没有考虑到如何实现群机器人之间的协调控制与复杂环境下的全局路径规划问题，也没有提出机器人多目标跟踪的动力学模型。然而，在实际的应用中，对多目标的搜索跟踪往往由多个机器人来共同完成，必须要互相通信，充分利用机器人的量测信息，避免碰撞，合理规划路径，减少搜索工作量，完成对目标状态的实时分析，才能提高多机器人在复杂环境下搜索与跟踪的效率。

技术实现要素：

有鉴于此，本发明的目的在于提供一种基于phd滤波的群体机器人分布式多目标跟踪方法，充分利用了传感器量测信息，通过phd滤波方法实现对多个目标运动状态的准确估计，融入power图搜索法，合理规划全局路径，同时使机器人路径远离障碍物，在搜索到目标以后采用一种基于当前测量模型的加速度方差自适应跟踪方法，计算出测量信息与加速度之间的函数关系，实时估算加速度扰动量并完成加速度方差自适应调整，这种方法可以有效反映目标的机动状态，提高跟踪性能。

为达到上述目的，本发明提供如下技术方案：

一种基于phd滤波的群体机器人分布式多目标跟踪方法，包括以下步骤：

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

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

式中，且

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

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

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

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

s5：重复s3和s4，直至机器人视野里出现目标；

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

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

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

进一步，步骤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的函数。

进一步，步骤s4中，根据移动机器人运动状态和得到的量测数据，新时刻的phd由下面的phd滤波方法计算获得：

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

进一步，步骤s6中，当机器人发现目标时刻时，在惯性坐标系xoy中，二维情况下，目标的运动模型为：

其中，

x(t)是目标的状态向量，x,分别是目标的位置，速度，加速度，fx,fy分别是机动频率在惯性坐标系ox、oy轴上的投影分量且fx＝fy＝f，为机动加速度均值分量，w是均值为零、方差为2fρ²的高斯白噪声，wx、wy为w在坐标轴的分量；

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

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

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

y(t)＝g(t)x(t)+j(t)

其中g(t)表示观测矩阵，j(t)表示观测噪声；

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

y(k)＝g[k]x(k)+λ(k)

式中，状态噪声λ(kt)是均值为零，方差为γ(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)是增益矩阵，q(k|k)为协方差矩阵，γ(k)为噪声协方差矩阵，c(k)是考虑多余回波影响的修正项；

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

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

本发明的有益效果在于：①基于phd滤波器的多目标跟踪方法避免了传统的量测—状态数据关联问题，通过对后验密度的一阶统计矩的预测与更新，最终获取目标状态及目标数估计，跟踪精度和稳定性较高；

②针对实际场景中机器人和目标移动的物理特性，融入基于质心power图的全局覆盖路径规划算法，使得本发明具有良好的避障特性，同时产生更佳的运动路径；

③提出了一种加速度方差自适应跟踪算法，通过计算当前测量信息与加速度之间的函数关系，实时估算出加速度扰动量，进行加速度方差自适应调整，可以较好地反映目标的机动情况，具有更高的跟踪性能。

附图说明

为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：

图1为本发明所述多目标搜索跟踪方法的流程示意图；

图2为本发明所述的voronoi图；

图3为本发明所述的power图；

图4为本发明实施例所述t1时刻x1机器人的通信情况示意图。

具体实施方式

下面将结合附图，对本发明的优选实施例进行详细的描述。

一种基于phd滤波的群机器人多目标搜索跟踪方法，包括以下步骤：

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

power图是一种加权的voronoi图，如附图2，voronoi图根据离散点的位置将空间进行划分，让每块voronoi区域内的点到对应离散点的距离最小，而power图给每个离散点赋予权重，重新定义voronoi图中的距离概念，优化了传统的voronoi图空间划分方案，如附图3所示。

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

式中，j≠i,j＝1,...,n,且

第二步，求解每个机器人对应的power单元的加权质心，记为满足：

式中ρ(x)为当前时刻的每个power图的重要性权重函数，与搜索覆盖的常数项和以目标位置为中心的径向基函数线性相关，在本方法中用phd来代替，即可以引导机器人向高目标密度的区域搜索。

初始时刻t＝0，求得初始质心为hi(0)。

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

假设每个机器人都配备了各种传感器，可以完成相关数据的采集和处理，机器人之间可以进行通信，实现信息的交互。

在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的函数。

若在该时刻，群机器人分布如附图4，所有机器人的量测范围为2，则对于机器人x1，与之进行数据交互的机器人有x2和x6，通过运算求得归一化常数λ(·)，否则λ(·)将只由机器人x1的状态信息和量测数据决定。

第四步，根据收集的量测数据，通过phd滤波算法，更新phd并将求得的新的phd作为权值，执行第二步，从而获得新的质心位置坐标h′。

根据移动机器人运动状态和得到的量测数据，新时刻的phd可以由下面的phd滤波方法计算获得：

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

第五步，重复第三步和第四步，直至机器人视野里出现目标。

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

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

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

假设当机器人发现目标时刻时，在惯性坐标系xoy中，二维情况下，目标的运动模型为：

其中，

x(t)是目标的状态向量，x,分别是目标的位置，速度，加速度，fx,fy分别是机动频率在惯性坐标系ox、oy轴上的投影分量且fx＝fy＝f，为机动加速度均值分量，w为均值为零，方差为2fρ²的高斯白噪声，wx、wy为w在坐标轴的分量。

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

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

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

y(t)＝g(t)x(t)+j(t)

其中g(t)表示观测矩阵，j(t)表示观测噪声。

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

y(k)＝g[k]x(k)+λ(k)

式中，状态噪声λ(kt)是均值为零，方差为γ(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)是增益矩阵，q(k|k)为协方差矩阵，γ(k)为噪声协方差矩阵，c(k)是考虑多余回波影响的修正项。

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

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

这种算法特点是基于当前的统计模型，目标的机动加速度概率密度采用修正的瑞利分布来描述，加速度均值等于当前时刻加速度的预测值，这样在估计目标状态的同时辨识出机动加速度均值，从而实时修正加速度的统计分布，并通过方差反馈到下一时刻的滤波增益上，进而实现了闭环自适应。

最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明权利要求书所限定的范围。