CN109298725B - 一种基于phd滤波的群体机器人分布式多目标跟踪方法 - Google Patents
一种基于phd滤波的群体机器人分布式多目标跟踪方法 Download PDFInfo
- Publication number
- CN109298725B CN109298725B CN201811446461.6A CN201811446461A CN109298725B CN 109298725 B CN109298725 B CN 109298725B CN 201811446461 A CN201811446461 A CN 201811446461A CN 109298725 B CN109298725 B CN 109298725B
- Authority
- CN
- China
- Prior art keywords
- target
- robot
- phd
- acceleration
- robots
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000001914 filtration Methods 0.000 title claims abstract description 27
- 230000001133 acceleration Effects 0.000 claims abstract description 46
- 238000005259 measurement Methods 0.000 claims abstract description 41
- 230000003044 adaptive effect Effects 0.000 claims abstract description 14
- 238000010606 normalization Methods 0.000 claims abstract description 8
- 230000001360 synchronised effect Effects 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 12
- 150000001875 compounds Chemical class 0.000 claims description 10
- 238000010586 diagram Methods 0.000 claims description 7
- 238000013179 statistical model Methods 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 239000013598 vector Substances 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims description 4
- 230000003993 interaction Effects 0.000 claims description 4
- 125000004432 carbon atom Chemical group C* 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 239000000126 substance Substances 0.000 claims description 3
- 239000002245 particle Substances 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000015572 biosynthetic process Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000001580 bacterial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005315 distribution function Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 230000002431 foraging effect Effects 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000010223 real-time analysis Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/12—Target-seeking control
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Feedback Control In General (AREA)
Abstract
本发明涉及一种基于PHD滤波的群体机器人分布式多目标跟踪方法，包括S1：在初始状态下，群机器人在给定的约束边界内随机分布，根据多机器人位置坐标和权重，构造Power图，划分搜索区域；S2：求解每个机器人对应的Power单元的加权质心；S3：所有的机器人开始由当前位置X移动至对应的hi(0)，收集相关量测信息，计算归一化常数，更新Power图；S4：根据量测数据，更新PHD并将新PHD作为权值，执行S2，获得新的质心位置坐标；S5：重复S3和S4，直至机器人视野里出现目标；S6：机器人发现目标后，观测目标的运动状态，利用当前雷达测量信息与加速度之间的函数关系，实时估算出加速度扰动量，进行加速度方差自适应调整，保持对该目标的同步追踪。
Description
技术领域
本发明属于机器人跟踪技术领域，涉及一种基于PHD滤波的群体机器人分布式多目标跟踪方法。
背景技术
近年来，随着工业化和信息化的迅速发展，机器人技术已经成为国内外众多学者与专家的研究热点。其中，群体智能算法方向的研究尤为深入，诸如滤波算法、遗传算法、蚁群算法、粒子群算法等等，一些群体智能算法已经运用在了实际的机器人系统中，并发挥了很大作用，例如：国外文献《Study of Formation Control and Obstacle Avoidance ofSwarm Robots using Evolutionary Algorithms》将细菌觅食(BFOA)和粒子群优化(PSO)算法用于群体机器人的编队控制和避障上，国内文献《一种适用于多机器人搜索动态目标的改进粒子群算法》提出了一种改进的粒子群算法，可以更为有效地寻找并跟踪动态目标。
对目标进行搜索跟踪是机器人技术中的一项常规任务，在国防、军事、环境监控、智能交通、灾后救援等领域都有着极为广泛的应用价值。多目标跟踪是指利用机器人的传感器量测数据对其搜索范围内的多个目标的位置、速度等运动状态进行估计的方法，这个过程可以看成一个典型的滤波问题。然而，在很多实际的应用场景中，由于往往会存在目标个数未知、分布复杂，杂波密集、传感器观测误差等问题，干扰因素很多，如何有效准确地估计目标的数目和运动状态，提高对目标的识别和跟踪性能成为当前多目标跟踪问题的关键难点。
传统的多目标跟踪方法主要有联合概率数据互联(JPDA)和多假设跟踪(MHT)，前者建立数据关联的基础上，将多目标的跟踪问题转化为多个单目标状态的估计问题，在目标个数固定、杂波数量较少、观测信息误差小的简单环境中跟踪效果较好，受到了广泛研究，但是面对多目标、杂波密集和观测数据误差较大的环境，计算复杂，易产生组合爆炸，鲁棒性和跟踪精度也严重降低，这就大大限制了它的实际应用。基于随机有限集(RFS)的多目标跟踪概率假设密度(PHD)滤波方法有效避免了数据关联问题，利用一阶统计矩近似多目标后验概率密度分布函数，即PHD，PHD峰值对应目标状态，求PHD积分得到多目标强度，同时将多目标状态集的后验PHD解耦，投影在多个单目标状态空间上，避免了多目标贝叶斯滤波器在RFS空间上进行递推，降低了计算难度，为多目标跟踪问题提供了一种新的解决方案。
在PHD滤波方法中，通常是假设杂波在搜索区域呈均匀分布，且数量满足泊松分布，根据传感器得到的所有量测信息对当前目标的运动状态进行估计，获取最优估计值，而利用持续的后续量测信息对某一时刻状态估计进行平滑，从而得到更加精确的估计结果。运用PHD滤波方法，虽然从一定程度上改善了复杂环境下多个目标的跟踪性能，但是没有充分利用目标的观测信息，造成有用量测数据的丢失，虽然能较为准确地对目标运动状态进行估计，但是并没有从机器人的运动上解决如何实现同步跟踪，此外对于多机器人的路径规划问题也还存在着很大的局限性。
目前关于机器人多目标跟踪的方案有很多，这些方案或是极大程度地提高了对微弱目标的识别性能，或是解决了低信噪比条件下目标的检测与跟踪，或是在基于PHD滤波的多目标跟踪方法中结合了目标幅值信息，提高了量测信息的利用率，或是解决了新生目标强度未知时的多目标运动状态与目标数目估计问题。虽然这些技术解决了多目标跟踪的某个方面的不足，但是都没有考虑到如何实现群机器人之间的协调控制与复杂环境下的全局路径规划问题，也没有提出机器人多目标跟踪的动力学模型。然而，在实际的应用中，对多目标的搜索跟踪往往由多个机器人来共同完成，必须要互相通信，充分利用机器人的量测信息，避免碰撞，合理规划路径，减少搜索工作量，完成对目标状态的实时分析，才能提高多机器人在复杂环境下搜索与跟踪的效率。
发明内容
有鉴于此，本发明的目的在于提供一种基于PHD滤波的群体机器人分布式多目标跟踪方法，充分利用了传感器量测信息，通过PHD滤波方法实现对多个目标运动状态的准确估计，融入Power图搜索法，合理规划全局路径，同时使机器人路径远离障碍物，在搜索到目标以后采用一种基于当前测量模型的加速度方差自适应跟踪方法，计算出测量信息与加速度之间的函数关系，实时估算加速度扰动量并完成加速度方差自适应调整，这种方法可以有效反映目标的机动状态，提高跟踪性能。
为达到上述目的，本发明提供如下技术方案：
一种基于PHD滤波的群体机器人分布式多目标跟踪方法，包括以下步骤：
S1：假设在初始状态下，群机器人在给定的方形约束边界内随机分布，根据多机器人所在的位置坐标和权重，构造出Power图，划分搜索区域；
假设给定的搜索区域为E∈R2，获取机器人坐标点并完成编号
预设初始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ρ2的高斯白噪声，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)]2的离散时间白噪声序列，Δ为探测器量测偏差；τ为相对误差系数；
用N(k|k-1)、M(k)代替统计模型状态离散方程中的向量：
对当前模型的离散化方程应用卡尔曼滤波，得到的基于当前统计模型的机动目标跟踪方法，描述为：
A(k)＝Q(k|k-1)GT(k)[G(k)Q(k|k-1)GT(k)+Γ(k)]-1
Q(k|k-1)＝N(k|k-1)Q(k-1|k-1)NT(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∈R2，获取机器人坐标点并完成编号
预设初始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ρ2的高斯白噪声，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)]2的离散时间白噪声序列，Δ为探测器量测偏差；τ为相对误差系数。
用N(k|k-1)、M(k)代替统计模型状态离散方程中的向量：
对当前模型的离散化方程应用卡尔曼滤波，得到的基于当前统计模型的机动目标跟踪方法，可以描述为：
A(k)＝Q(k|k-1)GT(k)[G(k)Q(k|k-1)GT(k)+Γ(k)]-1
Q(k|k-1)＝N(k|k-1)Q(k-1|k-1)NT(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为最大机动加速度。
这种算法特点是基于当前的统计模型，目标的机动加速度概率密度采用修正的瑞利分布来描述，加速度均值等于当前时刻加速度的预测值，这样在估计目标状态的同时辨识出机动加速度均值，从而实时修正加速度的统计分布，并通过方差反馈到下一时刻的滤波增益上，进而实现了闭环自适应。
最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明权利要求书所限定的范围。
Claims (4)
1.一种基于PHD滤波的群体机器人分布式多目标跟踪方法，其特征在于：包括以下步骤：
S1：假设在初始状态下，群机器人在给定的方形约束边界内随机分布，根据多机器人所在的位置坐标和权重，构造出Power图，划分搜索区域；
假设给定的搜索区域为E∈R2，获取机器人坐标点并完成编号
预设初始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：机器人发现目标后，观测目标的运动状态，采用加速度方差自适应跟踪算法，利用当前雷达测量信息与加速度之间的函数关系，实时估算出加速度扰动量，进行加速度方差自适应调整，保持对该目标的同步追踪。
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，
分别是目标的位置，速度，加速度，fx，fy分别是机动频率在惯性坐标系OX、OY轴上的投影分量且fx＝fy＝f，
为机动加速度均值分量，W是均值为零、方差为2fρ2的高斯白噪声，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)
式中，状态噪声Λ(k)是均值为零，方差为Γ(k)＝[Δ+τx(k)]2的离散时间白噪声序列，Δ为探测器量测偏差；τ为相对误差系数；
用N(k|k-1)、M(k)代替统计模型状态离散方程中的向量：
对当前模型的离散化方程应用卡尔曼滤波，得到的基于当前统计模型的机动目标跟踪方法，描述为：
A(k)＝Q(k|k-1)GT(k)[G(k)Q(k|k-1)GT(k)+Γ(k)]-1
Q(k|k-1)＝N(k|k-1)Q(k-1|k-1)NT(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)是考虑多余回波影响的修正项；
采用加速度方差自适应算法，故有
式中，
表示当前目标的预测加速度均值，
表示加速度的方差，amax为最大机动加速度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811446461.6A CN109298725B (zh) | 2018-11-29 | 2018-11-29 | 一种基于phd滤波的群体机器人分布式多目标跟踪方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811446461.6A CN109298725B (zh) | 2018-11-29 | 2018-11-29 | 一种基于phd滤波的群体机器人分布式多目标跟踪方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109298725A CN109298725A (zh) | 2019-02-01 |
| CN109298725B true CN109298725B (zh) | 2021-06-15 |
Family
ID=65141677
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811446461.6A Expired - Fee Related CN109298725B (zh) | 2018-11-29 | 2018-11-29 | 一种基于phd滤波的群体机器人分布式多目标跟踪方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109298725B (zh) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110018489B (zh) * | 2019-04-25 | 2022-11-08 | 上海蔚来汽车有限公司 | 基于激光雷达的目标追踪方法、装置及控制器和存储介质 |
| CN110244756B (zh) * | 2019-04-29 | 2021-09-24 | 福州大学 | 无人机快速追踪协同避障方法 |
| CN110471096A (zh) * | 2019-09-11 | 2019-11-19 | 哈尔滨工程大学 | 一种分布式海底飞行节点群体定位方法 |
| CN110673826B (zh) * | 2019-10-25 | 2023-02-28 | 山东大学 | 群机器人在区域搜索中的随机搜索方法及系统 |
| CN111650942B (zh) * | 2020-06-12 | 2021-09-28 | 湖南大学 | 一种基于扰动观测器的有限时间收敛车辆编队控制方法 |
| CN113433828B (zh) * | 2021-08-25 | 2022-01-18 | 南京航空航天大学 | 一种多机器人动态目标跟踪协同控制方法 |
| CN116382307B (zh) * | 2023-06-05 | 2023-08-01 | 南开大学 | 基于未知连通区域质心的多机器人自主探索方法及系统 |
| CN116591575B (zh) * | 2023-07-18 | 2023-09-19 | 山东锐泽自动化科技股份有限公司 | 基于机器视觉的旋转门安全控制方法及系统 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101894381A (zh) * | 2010-08-05 | 2010-11-24 | 上海交通大学 | 动态视频序列中多目标跟踪系统 |
| CN103345577A (zh) * | 2013-06-27 | 2013-10-09 | 江南大学 | 变分贝叶斯概率假设密度多目标跟踪方法 |
| CN104156984A (zh) * | 2014-05-30 | 2014-11-19 | 西北工业大学 | 一种不均匀杂波环境下多目标跟踪的概率假设密度方法 |
| CN104637070A (zh) * | 2014-12-15 | 2015-05-20 | 江南大学 | 基于概率假设密度的目标数变化的视频跟踪算法 |
| EP2980603A1 (en) * | 2014-07-31 | 2016-02-03 | Honeywell International Inc. | Two step pruning in a phd filter |
| CN105761276A (zh) * | 2015-12-15 | 2016-07-13 | 江南大学 | 基于迭代ransac自适应新生目标强度估计的gm-phd多目标跟踪算法 |
| CN106372646A (zh) * | 2016-08-30 | 2017-02-01 | 上海交通大学 | 基于srck‑gmcphd滤波的多目标跟踪方法 |
-
2018
- 2018-11-29 CN CN201811446461.6A patent/CN109298725B/zh not_active Expired - Fee Related
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101894381A (zh) * | 2010-08-05 | 2010-11-24 | 上海交通大学 | 动态视频序列中多目标跟踪系统 |
| CN103345577A (zh) * | 2013-06-27 | 2013-10-09 | 江南大学 | 变分贝叶斯概率假设密度多目标跟踪方法 |
| CN104156984A (zh) * | 2014-05-30 | 2014-11-19 | 西北工业大学 | 一种不均匀杂波环境下多目标跟踪的概率假设密度方法 |
| EP2980603A1 (en) * | 2014-07-31 | 2016-02-03 | Honeywell International Inc. | Two step pruning in a phd filter |
| CN104637070A (zh) * | 2014-12-15 | 2015-05-20 | 江南大学 | 基于概率假设密度的目标数变化的视频跟踪算法 |
| CN105761276A (zh) * | 2015-12-15 | 2016-07-13 | 江南大学 | 基于迭代ransac自适应新生目标强度估计的gm-phd多目标跟踪算法 |
| CN106372646A (zh) * | 2016-08-30 | 2017-02-01 | 上海交通大学 | 基于srck‑gmcphd滤波的多目标跟踪方法 |
Non-Patent Citations (3)
| Title |
|---|
| Novel Multiple-Model Probability Hypothesis Density Filter for Multiple Maneuvering Targets Tracking;Shaohua Hong，等;《IEEE》;20091231;第189-192页 * |
| 多传感器高斯混合PHD融合多目标跟踪方法;申屠晗，等;《自动化学报》;20170630;第43卷(第6期);第1028-1037页 * |
| 质心Power图下覆盖路径规划算法;郑利平，等;《系统仿真学报》;20170531;第29卷(第5期);第1120-1124页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109298725A (zh) | 2019-02-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109298725B (zh) | 一种基于phd滤波的群体机器人分布式多目标跟踪方法 | |
| CN111736145B (zh) | 一种基于高斯混合概率假设密度滤波的多机动目标多普勒雷达跟踪方法 | |
| CN108734725B (zh) | 基于高斯过程的概率数据关联滤波扩展目标跟踪方法 | |
| CN114019498A (zh) | 一种基于分数阶卡尔曼滤波的毫米波雷达单目标跟踪方法 | |
| CN111274529B (zh) | 一种鲁棒的高斯逆威沙特phd多扩展目标跟踪算法 | |
| CN110728026B (zh) | 一种基于角速度量测的末端弹道目标被动跟踪方法 | |
| CN112612034A (zh) | 基于激光帧和概率地图扫描的位姿匹配方法 | |
| CN116576868A (zh) | 一种多传感器融合精确定位及自主导航方法 | |
| Wang et al. | Research on multi-maneuvering target tracking JPDA algorithm | |
| Liu et al. | Asynchronous multi-sensor fusion multi-target tracking method | |
| Nussbaum et al. | EM-based Radar Signal Processing and Tracking | |
| CN112083410A (zh) | 机动目标跟踪方法 | |
| CN112285697A (zh) | 一种多传感器多目标空时偏差校准与融合方法 | |
| Wang et al. | An advanced algorithm for Fingerprint Localization based on Kalman Filter | |
| Westenkirchner et al. | Joint Tracking and Registration in Multi-Target Multi-Sensor Surveillance Using Factor Graphs | |
| Li et al. | Distributed Radar Interval Distance Estimation Based on Deep Neural Network | |
| Zhang et al. | Maneuvering target tracking algorithm based on multiple models in radar networking | |
| CN116047495B (zh) | 一种用于三坐标雷达的状态变换融合滤波跟踪方法 | |
| Fong | Multi-sensor data fusion via federated adaptive filter | |
| Coraluppi | Robustness in Multiple-Hypothesis Tracking | |
| CN114624688A (zh) | 一种基于多传感器联合的跟踪定位方法 | |
| Song et al. | Cooperative Positioning Algorithm Based on Manifold Gradient Filtering in UAV-WSN | |
| Zhang et al. | A SLAM Method Based on Cardinality Balanced Multi-Bernoulli Filter | |
| CN118011311A (zh) | 一种三维单站高动态目标跟踪方法 | |
| Liu et al. | Multi-hypothesis Data Association for Multiple Target Tracking |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20210615 Termination date: 20211129 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |