CN111352066B - 基于粒子滤波的定位方法、装置、计算机设备和存储介质 - Google Patents

基于粒子滤波的定位方法、装置、计算机设备和存储介质 Download PDF

Info

Publication number
CN111352066B
CN111352066B CN202010230278.3A CN202010230278A CN111352066B CN 111352066 B CN111352066 B CN 111352066B CN 202010230278 A CN202010230278 A CN 202010230278A CN 111352066 B CN111352066 B CN 111352066B
Authority
CN
China
Prior art keywords
target object
particle filter
particle
positioning
weight
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.)
Active
Application number
CN202010230278.3A
Other languages
English (en)
Other versions
CN111352066A (zh
Inventor
沈博
薛胜利
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xi'an Zhenyou Communication Technology Co ltd
Original Assignee
Xi'an Zhenyou Communication Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Xi'an Zhenyou Communication Technology Co ltd filed Critical Xi'an Zhenyou Communication Technology Co ltd
Priority to CN202010230278.3A priority Critical patent/CN111352066B/zh
Publication of CN111352066A publication Critical patent/CN111352066A/zh
Application granted granted Critical
Publication of CN111352066B publication Critical patent/CN111352066B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0278—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves involving statistical or probabilistic considerations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/14—Determining absolute distances from a plurality of spaced points of known location

Abstract

本发明公开基于粒子滤波的定位方法、装置、计算机设备和存储介质，将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取目标对象的原始测量位置；创建粒子滤波器，对粒子数量及初始状态参数进行初始化；基于状态转移方程预测目标对象的下一时刻位置；粒子滤波器根据原始测量位置计算每个粒子对目标对象的位置影响权重，获取的权值进行归一化处理；经过反复迭代处理后，当粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录并输出。粒子滤波方法与目标测量算法相结合，提高预测目标对象位置的精度，使得预测位置更接近目标对象的位置，达到对巷道中监控目标的精确定位。

Description

基于粒子滤波的定位方法、装置、计算机设备和存储介质
技术领域
本发明涉及地理位置无线定位技术领域，尤其涉及一种基于粒子滤波的定位方法、装置、计算机设备和存储介质。
背景技术
矿井人员定位系统在矿井安全生产过程中有着举足轻重的地位，目前使用的矿井人员定位系统，主要分为区域人员定位和精确人员定位两种类型，精确人员定位技术由于能够提供更精确的人员定位的信息，所以矿井用户对这项技术的重视程度日益提高。
基于飞行时间测距法(Time of flight，简称TOF)的测距技术是目前主要的精确定位技术。该测距信息是假设在视距的情况下测得的距离，由于受到巷道中其他物体的遮挡、巷道的起伏和转弯的影响，从目标到定位分站的信号通过反射传播，将会导致测量的时间变长，从而测得的距离会变大，另外由于定位分站时钟的误差，例如时钟误差1ns就会有30厘米的误差，也会导致目标测量距离与实际距离有所偏差，所以需要通过算法优化，提高目标定位准确度。
因此，现有技术还有待于改进和发展。
发明内容
鉴于上述现有技术的不足之处，本发明为解决现有技术缺陷和不足，提出了一种基于粒子滤波的飞行时间测距法对矿井人员或车辆的运动进行精确定位的方法及装置。
本发明解决技术问题所采用的技术方案如下：
一种基于粒子滤波的定位方法，所述定位方法包括如下步骤：
将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置；
创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置；
基于状态转移方程预测所述目标对象的下一时刻位置；
所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理；
经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录并输出。
作为进一步的改进技术方案，所述将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置具体包括如下步骤：
在矿井巷道中各目标对象上设置定位标签；
各目标对象上的定位标签通过UWB协议与距离其最近的两个定位分站-分站1和分站2，分别进行通讯连接，获取所述目标对象到两个定位分站的第一距离r1和第二距离r2；
根据分站1(x1，y1)、分站2(x2，y2)的坐标、第一距离r1、第二距离r2计算出第一距离r1和第二距离r2与矿井巷道中心线的四个焦点a1(xa1，ya1)，a2(xa2,ya2),b1(xb1,yb2)，b2(xb2,yb2)；
计算所述四个焦点的两点间距离a1b1、a1b2、a2b1、a2b2，并取间距最小的一组两点间距离，计算其中点位置A(xa,ya)作为所述目标对象的原始测量位置。
作为进一步的改进技术方案，所述创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置包括如下具体步骤：
构建一个递归的贝叶斯状态估计器；
设置粒子数量≤20个，初始化每个粒子的权重相等，为1/N，其中N为粒子数量；
根据前一次的测量位置、矿井巷道走向和宽度信息将粒子的初始位置设置于所述矿井巷道的内部测量位置附近。
作为进一步的改进技术方案，所述基于状态转移方程预测所述目标对象的下一时刻位置中，所述状态转移是指所述目标对象运动状态随时间的更新，通过模型中的状态方程来描述所述目标对象运动到下一时刻的测量位置，其中模型为匀速运动模型和匀加速运动模型。
作为进一步的改进技术方案，所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理具体包括如下步骤：
所述粒子滤波器获取所述目标对象的原始测量位置A(xa,ya)；
根据每个粒子对所述目标对象的原始测量位置A(xa,ya)影响的权重，采用马尔可夫过程计算出每个粒子对所述原始测量位置A(xa,ya)影响的权值
Figure BDA0002429080040000031
对所述权值根据公式
Figure BDA0002429080040000032
进行权值归一化处理。
作为进一步的改进技术方案，所述经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录具体包括如下步骤：
对所述粒子进行权值归一化处理经过几次迭代后，粒子出现少数粒子权重大，其他粒子权重很小的粒子退化情况；
当有效粒子Nn下降到门限值NT时，对粒子进行再采样校正，将权重很小的粒子删除，有效粒子调整为
Figure BDA0002429080040000033
再对每个有效粒子进行初始化为权重相等，为1/N，并将有效粒子的位置重新设置于所述矿井巷道的内部测量位置附近；
所述粒子滤波器根据粒子当前分布及其权重触发重采样；
最后经过反复迭代计算出目标对象的估计位置，当所述目标对象的估计位置小于等于预定义的偏差阈值时，将最终的目标对象的估计位置作为所述目标对象的真实位置输出。
作为进一步的改进技术方案，所述预定义的偏差阈值≤30厘米。
本发明还提供一种基于粒子滤波的定位装置，所述装置包括：
原始测量位置测量单元，用于将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置；
粒子滤波器初始化单元，用于创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置；
状态转移预测单元，用于基于状态转移方程预测目标对象的下一时刻位置；
目标对象定位计算单元，用于将所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理；经过反复迭代后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录并输出。
本发明还提供一种计算机设备，包括存储器和处理器，所述存储器中存储有计算机可读指令，所述计算机可读指令被所述处理器执行时，使得所述处理器执行如上述基于粒子滤波的定位方法的步骤。
本发明还提供一种存储有计算机可读指令的可读存储介质，所述计算机可读指令被一个或多个处理器执行时，使得一个或多个处理器执行上述基于粒子滤波的定位方法的步骤。
与现有技术相比较，本发明提出了基于矿井巷道走向的双站中值定位算法，基于巷道和定位分站位置的先验知识，降低定位算法的复杂度，提高了系统测量的精度。引入适合多目标跟踪的粒子滤波算法，对巷道中目标对象的测量位置计算粒子的影响权重，并对权值进行归一化处理，经过几次反复迭代计算后，当目标对象的估计值满足预定义的阈值时，估计值作为滤波结果，输出作为目标对象的真实位置估计。粒子滤波方法于目标测量算法相结合，提高了预测目标对象位置的精度，使得预测位置更接近目标对象的真实位置，能够达到对矿井巷道中人员和车辆的精确定位。
附图说明
下面结合附图对本发明的具体实施方式作进一步的说明，其中：
图1是本发明中一种计算机设备的内部结构框图；
图2是本发明一种基于粒子滤波的定位方法优选实施例的流程图；
图3是本发明基于粒子滤波的定位方法中目标对象测量位置算法的流程图；
图4是本发明目标对象测量位置算法中双站定位目标测量位置情况1的结构示意图；
图5是本发明目标对象测量位置算法中双站定位目标测量位置情况2的结构示意图；
图6是本发明目标对象测量位置算法中双站定位目标测量位置情况3的结构示意图；
图7是本发明基于粒子滤波的定位方法的算法流程图；
图8是本发明基于粒子滤波的定位装置的结构框图。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
可以理解，本发明所使用的术语“第一”、“第二”等可在本文中用于描述各种参数，但这些参数不受这些术语限制。这些术语仅用于将第一参数与另一个参数区分。
图1为一个实施例中计算机设备的内部结构示意图。如图1所示，该计算机设备包括通过系统总线连接的处理器、非易失性存储介质、存储器和网络接口。其中，该计算机设备的非易失性存储介质存储有操作系统、数据库和计算机可读指令，数据库中可存储有控件信息序列，该计算机可读指令被处理器执行时，可使得处理器实现一种基于粒子滤波的定位方法。该计算机设备的处理器用于提供计算和控制能力，支撑整个计算机设备的运行。该计算机设备的存储器中可存储有计算机可读指令，该计算机可读指令被处理器执行时，可使得处理器执行一种基于粒子滤波的定位方法。该计算机设备的网络接口用于与终端连接通信。本领域技术人员可以理解，图2中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
本发明一种基于粒子滤波的定位方法如图2本发明基于粒子滤波的定位方法优选实施例的流程图，结合图7本发明基于粒子滤波的定位方法的算法流程图所示。该基于粒子滤波的定位方法包括如下步骤S100-S500。
步骤S100，将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置；
本发明实施例中，矿井中的各目标对象是指在矿井巷道中的人员和车辆，人员和车辆在巷道中的运动精确定位，以便于进行管理。获取所述目标对象的原始测量位置具体包括如下步骤：
在矿井巷道中各目标对象上设置定位标签；
各目标对象上的定位标签通过UWB协议与距离其最近的两个定位分站-分站1和分站2，分别进行通讯连接，获取所述目标对象到两个定位分站的第一距离r1和第二距离r2；其中，目标对象的测量位置算法参见图3本发明基于粒子滤波的定位方法中目标对象测量位置算法的流程图所示。TOF测距是通过设备间发送和接收带有时间戳的信号，计算整个过程花费的时间，根据电磁波以光速传播的原理，得到两个设备间的距离值。其中，设备间通信过程又分为单边双向测距和双边双向测距两种模式。单边双向测距模式中测距误差主要与设备接收并发送数据的延迟时间有关，延迟时间越大则测距误差越大；双边双向测距模式误差受到两个设备晶振频率误差、时间戳准确度和设备接收并发送数据的延迟时间有关。定位标签通过UWB协议与两个定位分站分别通讯，通过计算通讯时延得到定位标签距离两个定位分站的距离，第一距离r1和第二距离r2。
根据分站1(x1,y1)、分站2(x2,y2)的坐标、第一距离r1、第二距离r2计算出第一距离r1和第二距离r2与矿井巷道中心线的四个焦点a1(xa1,ya1)，a2(xa2,ya2)，b1(xb1，yb2)，b2(xb2，yb2)；目标对象的原始测量位置是双站独立测距得到的位置重点，两个相邻分站分别上报测得的目标距离，以定位分站为圆心，以测得距离为半径可以标示两个圆，每个圆与巷道中心线有两个交点，两个相邻分站可得到四个交点，两个分站1和分站2的定位点存在三种情形，相离或重合。具体两个相邻分站1和分站2与目标对象双站独立测距的定位点存在的三种情况参看图4、5和6双站定位目标测量位置的情形1、2和3。
计算所述四个焦点的两点间距离a1b1、a1b2、a2b1、a2b2，并取间距最小的一组两点间距离，计算其中点位置A(xa，ya)作为所述目标对象的原始测量位置。
步骤S200，创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置；
粒子滤波技术在非线性、非高斯系统表现出来的优越性，决定了它的应用范围非常广泛。另外，粒子滤波器的多模态处理能力也是其广泛应用的原因之一。目前，粒子滤波已经被广泛应用于各个领域，在跟踪定位领域中该算法在雷达跟踪以及视频跟踪中都得到了广泛应用。本发明实施例采用将目标对象测量算法和粒子滤波方法结合，使预测出来的目标对象的位置更接近目标对象的真实位置，提高了跟踪精度。
所述创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置包括如下具体步骤：
构建一个递归的贝叶斯状态估计器；递归的贝叶斯状态估计器使用离散粒子来近似估计状态的后验概率分布。
利用初始化方法设置粒子数量≤20个，初始化每个粒子的权重相等，为1/N，其中N为粒子数量，粒子数量为根据经验值确定的初始值，该经验值基于UWB技术的测距偏差，跟踪目标对象数量(一般≤1500个)，实际使用中对定位精度的要求，综合考虑算法运算量和速度因素来定义的。
为了提高算法的效率和准确度，需要根据前一次的测量位置、矿井巷道走向和宽度信息，将粒子的初始位置设置于所述矿井巷道的内部测量位置附近。这种初始化的策略有助于将粒子聚集到更接近估计值的位置，从跟踪开始的阶段就有利于将粒子聚集到更接近估计值的状态。
步骤S300，基于状态转移方程预测所述目标对象的下一时刻位置；
本发明实施例中，系统状态转移时指运动目标对象运动状态随时间的更新，通过模型中的状态方程来描述所述目标对象运动到下一时刻的测量位置。由于监控目标为矿井巷道中的工作的人员和车辆，目标对象只能在矿井巷道中运动，可以假设目标对象属于匀速运动模型和匀加速运动模型，所述系统状态转移方程采用高阶自回归模型，其方程为:Ck＝ACk-2+BCk-1+Cjk-1,其中，A、B、C均为常数；Ck-2和Ck-1为目标在前两次位置；Cjk-1为归一化噪声，是指运动目标状态随时间的更新需要通过系统模型中的状态方程来扫描，目标对象运动可以从二维平面运行简化为一维直线运动，目标为匀速或加速运动且速度较低(≤40千米/小时，即≤12米/秒)，根据前一测量位置的采样范围不超过±(12×定位间隔)米，例如定位间隔5秒时采样范围为±60米，定位间隔2秒时采样范围为±24米，定位间隔0.5秒时采样范围为±6米。
步骤S400，所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理；
本发明实施例中，所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理具体包括如下步骤：
所述粒子滤波器获取所述目标对象的原始测量位置A(xa,ya)；
根据每个粒子对所述目标对象的原始测量位置A(xa,ya)影响的权重，采用马尔可夫过程计算出每个粒子对所述原始测量位置A(xa,ya)影响的权值
Figure BDA0002429080040000091
对所述权值根据公式
Figure BDA0002429080040000092
进行权值归一化处理。
步骤S500，经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录并输出。
本发明实施例中，所述经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录具体包括如下步骤：
对所述粒子进行权值归一化处理经过几次迭代后，粒子出现少数粒子权重大，其他粒子权重很小的粒子退化情况；
当有效粒子Nn下降到门限值NT时，对粒子进行再采样校正，将权重很小的粒子删除，有效粒子调整为
Figure BDA0002429080040000093
再对每个有效粒子进行初始化为权重相等，为1/N，并将有效粒子的位置重新设置于所述矿井巷道的内部测量位置附近；
所述粒子滤波器根据粒子当前分布及其权重触发重采样；
最后经过反复迭代计算出目标对象的估计位置，当所述目标对象的估计位置小于等于预定义的偏差阈值时，将最终的目标对象的估计位置作为所述目标对象的真实位置输出。其中，预定义的偏差阈值≤30厘米。
所述目标对象的原始测量位置通过上述算法的优化，得到粒子滤波后的目标估计位置A’(xa’,ya’)，该位置信息可以作为目标对象显示数据在数据库中记录并通过可视化页面呈现，以便于进行可视化管理。
本发明还提供一种基于粒子滤波的定位装置，参见图8所示的本发明基于粒子滤波的定位装置的结构框图所示。
所述基于粒子滤波的定位装置10包括：
原始测量位置测量单元20，用于将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置；
粒子滤波器初始化单元30，用于创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置；
状态转移预测单元40，用于基于状态转移方程预测目标对象的下一时刻位置；
目标对象定位计算单元50，用于将所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理；经过反复迭代后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录并输出。
本发明装置的其他工作原理与上述基于粒子滤波的定位方法相同，此处不赘述。
本发明还提供一种计算机设备，包括存储器和处理器，所述存储器中存储有可在所述处理器上运行的计算机可读指令，所述计算机可读指令被所述处理器执行时实现以下步骤：
将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置；
创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置；
基于状态转移方程预测所述目标对象的下一时刻位置；
所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理；
经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录并输出。
在一个实施例中，将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置具体包括如下步骤：
在矿井巷道中各目标对象上设置定位标签；
各目标对象上的定位标签通过UWB协议与距离其最近的两个定位分站-分站1和分站2，分别进行通讯连接，获取所述目标对象到两个定位分站的第一距离r1和第二距离r2；其中，目标对象的测量位置算法参见图3本发明基于粒子滤波的定位方法中目标对象测量位置算法的流程图所示。TOF测距是通过设备间发送和接收带有时间戳的信号，计算整个过程花费的时间，根据电磁波以光速传播的原理，得到两个设备间的距离值。其中，设备间通信过程又分为单边双向测距和双边双向测距两种模式。单边双向测距模式中测距误差主要与设备接收并发送数据的延迟时间有关，延迟时间越大则测距误差越大；双边双向测距模式误差受到两个设备晶振频率误差、时间戳准确度和设备接收并发送数据的延迟时间有关。定位标签通过UWB协议与两个定位分站分别通讯，通过计算通讯时延得到定位标签距离两个定位分站的距离，第一距离r1和第二距离r2。
根据分站1(x1,y1)、分站2(x2,y2)的坐标、第一距离r1、第二距离r2计算出第一距离r1和第二距离r2与矿井巷道中心线的四个焦点a1(xa1,ya1)，a2(xa2,ya2),b1(xb1,yb2)，b2(xb2,yb2)；目标对象的原始测量位置是双站独立测距得到的位置重点，两个相邻分站分别上报测得的目标距离，以定位分站为圆心，以测得距离为半径可以标示两个圆，每个圆与巷道中心线有两个交点，两个相邻分站可得到四个交点，两个分站1和分站2的定位点存在三种情形，相离或重合。具体两个相邻分站1和分站2与目标对象双站独立测距的定位点存在的三种情况参看图4、5和6双站定位目标测量位置的情形1、2和3。
计算所述四个焦点的两点间距离a1b1、a1b2、a2b1、a2b2，并取间距最小的一组两点间距离，计算其中点位置A(xa,ya)作为所述目标对象的原始测量位置。
在一个实施例中，所述创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置包括如下具体步骤：
构建一个递归的贝叶斯状态估计器；递归的贝叶斯状态估计器使用离散粒子来近似估计状态的后验概率分布。
利用初始化方法设置粒子数量≤20个，初始化每个粒子的权重相等，为1/N，其中N为粒子数量，粒子数量为根据经验值确定的初始值，该经验值基于UWB技术的测距偏差，跟踪目标对象数量(一般≤1500个)，实际使用中对定位精度的要求，综合考虑算法运算量和速度因素来定义的。
为了提高算法的效率和准确度，需要根据前一次的测量位置、矿井巷道走向和宽度信息，将粒子的初始位置设置于所述矿井巷道的内部测量位置附近。这种初始化的策略有助于将粒子聚集到更接近估计值的位置，从跟踪开始的阶段就有利于将粒子聚集到更接近估计值的状态。
在一个实施例中，系统状态转移时指运动目标对象运动状态随时间的更新，通过模型中的状态方程来描述所述目标对象运动到下一时刻的测量位置。由于监控目标为矿井巷道中的工作的人员和车辆，目标对象只能在矿井巷道中运动，可以假设目标对象属于匀速运动模型和匀加速运动模型，所述系统状态转移方程采用高阶自回归模型，其方程为:Ck＝ACk-2+BCk-1+Cjk-1,其中，A、B、C均为常数；Ck-2和Ck-1为目标在前两次位置；Cjk-1为归一化噪声，是指运动目标状态随时间的更新需要通过系统模型中的状态方程来扫描，目标对象运动可以从二维平面运行简化为一维直线运动，目标为匀速或加速运动且速度较低(≤40千米/小时，即≤12米/秒)，根据前一测量位置的采样范围不超过±(12×定位间隔)米，例如定位间隔5秒时采样范围为±60米，定位间隔2秒时采样范围为±24米，定位间隔0.5秒时采样范围为±6米。
在一个实施例中，所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理具体包括如下步骤：
所述粒子滤波器获取所述目标对象的原始测量位置A(xa,ya)；
根据每个粒子对所述目标对象的原始测量位置A(xa,ya)影响的权重，采用马尔可夫过程计算出每个粒子对所述原始测量位置A(xa,ya)影响的权值
Figure BDA0002429080040000131
对所述权值根据公式
Figure BDA0002429080040000132
进行权值归一化处理。
在一个实施例中，所述经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录具体包括如下步骤：
对所述粒子进行权值归一化处理经过几次迭代后，粒子出现少数粒子权重大，其他粒子权重很小的粒子退化情况；
当有效粒子Nn下降到门限值NT时，对粒子进行再采样校正，将权重很小的粒子删除，有效粒子调整为
Figure BDA0002429080040000133
再对每个有效粒子进行初始化为权重相等，为1/N，并将有效粒子的位置重新设置于所述矿井巷道的内部测量位置附近；
所述粒子滤波器根据粒子当前分布及其权重触发重采样；
最后经过反复迭代计算出目标对象的估计位置，当所述目标对象的估计位置小于等于预定义的偏差阈值时，将最终的目标对象的估计位置作为所述目标对象的真实位置输出。其中，预定义的偏差阈值≤30厘米。
所述目标对象的原始测量位置通过上述算法的优化，得到粒子滤波后的目标估计位置A’(xa’,ya’)，该位置信息可以作为目标对象显示数据在数据库中记录并通过可视化页面呈现，以便于进行可视化管理。
本发明还提供一种存储有计算机可读指令的可读存储介质，可读存储介质为计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，该计算机程序被处理器执行时实现以下步骤：
将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置；
创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置；
基于状态转移方程预测所述目标对象的下一时刻位置；
所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理；
经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录并输出。
在一个实施例中，将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置具体包括如下步骤：
在矿井巷道中各目标对象上设置定位标签；
各目标对象上的定位标签通过UWB协议与距离其最近的两个定位分站-分站1和分站2，分别进行通讯连接，获取所述目标对象到两个定位分站的第一距离r1和第二距离r2；其中，目标对象的测量位置算法参见图3本发明基于粒子滤波的定位方法中目标对象测量位置算法的流程图所示。TOF测距是通过设备间发送和接收带有时间戳的信号，计算整个过程花费的时间，根据电磁波以光速传播的原理，得到两个设备间的距离值。其中，设备间通信过程又分为单边双向测距和双边双向测距两种模式。单边双向测距模式中测距误差主要与设备接收并发送数据的延迟时间有关，延迟时间越大则测距误差越大；双边双向测距模式误差受到两个设备晶振频率误差、时间戳准确度和设备接收并发送数据的延迟时间有关。定位标签通过UWB协议与两个定位分站分别通讯，通过计算通讯时延得到定位标签距离两个定位分站的距离，第一距离r1和第二距离r2。
根据分站1(x1,y1)、分站2(x2,y2)的坐标、第一距离r1、第二距离r2计算出第一距离r1和第二距离r2与矿井巷道中心线的四个焦点a1(xa1,ya1)，a2(xa2,ya2),b1(xb1,yb2)，b2(xb2,yb2)；目标对象的原始测量位置是双站独立测距得到的位置重点，两个相邻分站分别上报测得的目标距离，以定位分站为圆心，以测得距离为半径可以标示两个圆，每个圆与巷道中心线有两个交点，两个相邻分站可得到四个交点，两个分站1和分站2的定位点存在三种情形，相离或重合。具体两个相邻分站1和分站2与目标对象双站独立测距的定位点存在的三种情况参看图4、5和6双站定位目标测量位置的情形1、2和3。
计算所述四个焦点的两点间距离a1b1、a1b2、a2b1、a2b2，并取间距最小的一组两点间距离，计算其中点位置A(xa,ya)作为所述目标对象的原始测量位置。
在一个实施例中，所述创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置包括如下具体步骤：
构建一个递归的贝叶斯状态估计器；递归的贝叶斯状态估计器使用离散粒子来近似估计状态的后验概率分布。
利用初始化方法设置粒子数量≤20个，初始化每个粒子的权重相等，为1/N，其中N为粒子数量，粒子数量为根据经验值确定的初始值，该经验值基于UWB技术的测距偏差，跟踪目标对象数量(一般≤1500个)，实际使用中对定位精度的要求，综合考虑算法运算量和速度因素来定义的。
为了提高算法的效率和准确度，需要根据前一次的测量位置、矿井巷道走向和宽度信息，将粒子的初始位置设置于所述矿井巷道的内部测量位置附近。这种初始化的策略有助于将粒子聚集到更接近估计值的位置，从跟踪开始的阶段就有利于将粒子聚集到更接近估计值的状态。
在一个实施例中，系统状态转移时指运动目标对象运动状态随时间的更新，通过模型中的状态方程来描述所述目标对象运动到下一时刻的测量位置。由于监控目标为矿井巷道中的工作的人员和车辆，目标对象只能在矿井巷道中运动，可以假设目标对象属于匀速运动模型和匀加速运动模型，所述系统状态转移方程采用高阶自回归模型，其方程为:Ck＝ACk-2+BCk-1+Cjk-1,其中，A、B、C均为常数；Ck-2和Ck-1为目标在前两次位置；Cjk-1为归一化噪声，是指运动目标状态随时间的更新需要通过系统模型中的状态方程来扫描，目标对象运动可以从二维平面运行简化为一维直线运动，目标为匀速或加速运动且速度较低(≤40千米/小时，即≤12米/秒)，根据前一测量位置的采样范围不超过±(12×定位间隔)米，例如定位间隔5秒时采样范围为±60米，定位间隔2秒时采样范围为±24米，定位间隔0.5秒时采样范围为±6米。
在一个实施例中，所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理具体包括如下步骤：
所述粒子滤波器获取所述目标对象的原始测量位置A(xa,ya)；
根据每个粒子对所述目标对象的原始测量位置A(xa,ya)影响的权重，采用马尔可夫过程计算出每个粒子对所述原始测量位置A(xa,ya)影响的权值
Figure BDA0002429080040000161
对所述权值根据公式
Figure BDA0002429080040000162
进行权值归一化处理。
在一个实施例中，所述经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录具体包括如下步骤：
对所述粒子进行权值归一化处理经过几次迭代后，粒子出现少数粒子权重大，其他粒子权重很小的粒子退化情况；
当有效粒子Nn下降到门限值NT时，对粒子进行再采样校正，将权重很小的粒子删除，有效粒子调整为
Figure BDA0002429080040000171
再对每个有效粒子进行初始化为权重相等，为1/N，并将有效粒子的位置重新设置于所述矿井巷道的内部测量位置附近；
所述粒子滤波器根据粒子当前分布及其权重触发重采样；
最后经过反复迭代计算出目标对象的估计位置，当所述目标对象的估计位置小于等于预定义的偏差阈值时，将最终的目标对象的估计位置作为所述目标对象的真实位置输出。其中，预定义的偏差阈值≤30厘米。
所述目标对象的原始测量位置通过上述算法的优化，得到粒子滤波后的目标估计位置A’(xa’,ya’)，该位置信息可以作为目标对象显示数据在数据库中记录并通过可视化页面呈现，以便于进行可视化管理。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，该计算机程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，前述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)等非易失性存储介质，或随机存储记忆体(Random Access Memory，RAM)等。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
应当理解的是，以上所述仅为本发明的较佳实施例而已，并不足以限制本发明的技术方案，对本领域普通技术人员来说，在本发明的精神和原则之内，可以根据上述说明加以增减、替换、变换或改进，而所有这些增减、替换、变换或改进后的技术方案，都应属于本发明所附权利要求的保护范围。

Claims (9)

1.一种基于粒子滤波的定位方法，其特征在于，包括如下步骤：
将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置；
创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置；
基于状态转移方程预测所述目标对象的下一时刻位置；
所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理；
经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录并输出；
所述将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置具体包括如下步骤：
在矿井巷道中各目标对象上设置定位标签；
各目标对象上的定位标签通过UWB协议与距离其最近的两个定位分站-分站1和分站2，分别进行通讯连接，获取所述目标对象到两个定位分站的第一距离r1和第二距离r2；
根据分站1(x1,y1)、分站2(x2,y2)的坐标、第一距离r1、第二距离r2计算出以分站1为圆心第一距离r1为半径的圆和以分站2为圆心第二距离r2为半径的圆与矿井巷道中心线的四个交点a1(xa1,ya1)，a2(xa2,ya2),b1(xb1,yb1)，b2(xb2,yb2)；
计算所述四个交点的两点间距离a1b1、a1b2、a2b1、a2b2，并取间距最小的一组两点间距离，计算其中点位置A(xa,ya)作为所述目标对象的原始测量位置。
2.根据权利要求1所述的一种基于粒子滤波的定位方法，其特征在于，所述创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置包括如下具体步骤：
构建一个递归的贝叶斯状态估计器；
设置粒子数量≤20个，初始化每个粒子的权重相等，为1/N，其中N为粒子数量；
根据前一次的测量位置、矿井巷道走向和宽度信息将粒子的初始位置设置于所述矿井巷道的内部测量位置附近。
3.根据权利要求1所述的一种基于粒子滤波的定位方法，其特征在于，所述基于状态转移方程预测所述目标对象的下一时刻位置中，所述状态转移是指所述目标对象运动状态随时间的更新，通过模型中的状态方程来描述所述目标对象运动到下一时刻的测量位置，其中模型为匀速运动模型和匀加速运动模型。
4.根据权利要求2所述的一种基于粒子滤波的定位方法，其特征在于，所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理具体包括如下步骤：
所述粒子滤波器获取所述目标对象的原始测量位置A(xa,ya)；
根据每个粒子对所述目标对象的原始测量位置A(xa,ya)影响的权重，采用马尔可夫过程计算出每个粒子对所述原始测量位置A(xa,ya)影响的权值
Figure FDA0003319488540000021
对所述权值根据公式
Figure FDA0003319488540000022
进行权值归一化处理。
5.根据权利要求4所述的一种基于粒子滤波的定位方法，其特征在于，所述经过反复迭代处理后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录具体包括如下步骤：
对所述粒子进行权值归一化处理经过几次迭代后，粒子出现少数粒子权重大，其他粒子权重很小的粒子退化情况；
当有效粒子Nn下降到门限值NT时，对粒子进行再采样校正，将权重很小的粒子删除，有效粒子调整为
Figure FDA0003319488540000023
再对每个有效粒子进行初始化为权重相等，为1/N，并将有效粒子的位置重新设置于所述矿井巷道的内部测量位置附近；
所述粒子滤波器根据粒子当前分布及其权重触发重采样；
最后经过反复迭代计算出目标对象的估计位置，当所述目标对象的估计位置小于等于预定义的偏差阈值时，将最终的目标对象的估计位置作为所述目标对象的真实位置输出。
6.根据权利要求5所述的一种基于粒子滤波的定位方法，其特征在于，所述预定义的偏差阈值≤30厘米。
7.一种基于粒子滤波的定位装置，其特征在于，所述装置包括：
原始测量位置测量单元，用于将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置；
粒子滤波器初始化单元，用于创建粒子滤波器，对粒子数量及初始状态参数进行初始化设置；
状态转移预测单元，用于基于状态转移方程预测目标对象的下一时刻位置；
目标对象定位计算单元，用于将所述粒子滤波器根据所述原始测量位置计算每个粒子对所述目标对象的位置影响权重，获取的权值进行归一化处理；经过反复迭代后，当所述粒子滤波器滤波计算出目标对象的估计位置满足预定义的偏差阈值时，将该目标对象的估计位置作为目标对象的真实位置记录并输出；
所述将矿井中各目标对象通过UWB协议与最近的两个定位分站通讯，以获取所述目标对象的原始测量位置具体包括如下步骤：
在矿井巷道中各目标对象上设置定位标签；
各目标对象上的定位标签通过UWB协议与距离其最近的两个定位分站-分站1和分站2，分别进行通讯连接，获取所述目标对象到两个定位分站的第一距离r1和第二距离r2；
根据分站1(x1,y1)、分站2(x2,y2)的坐标、第一距离r1、第二距离r2计算出第一距离r1和第二距离r2与矿井巷道中心线的四个焦点a1(xa1,ya1)，a2(xa2,ya2),b1(xb1,yb1)，b2(xb2,yb2)；
计算所述四个焦点的两点间距离a1b1、a1b2、a2b1、a2b2，并取间距最小的一组两点间距离，计算其中点位置A(xa,ya)作为所述目标对象的原始测量位置。
8.一种计算机设备，包括存储器和处理器，其特征在于，所述存储器中存储有计算机可读指令，所述计算机可读指令被所述处理器执行时，使得所述处理器执行如权利要求1至6任一项权利要求所述基于粒子滤波的定位方法的步骤。
9.一种存储有计算机可读指令的可读存储介质，其特征在于，所述计算机可读指令被一个或多个处理器执行时，使得一个或多个处理器执行如权利要求1至6任一项权利要求所述基于粒子滤波的定位方法的步骤。
CN202010230278.3A 2020-03-27 2020-03-27 基于粒子滤波的定位方法、装置、计算机设备和存储介质 Active CN111352066B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010230278.3A CN111352066B (zh) 2020-03-27 2020-03-27 基于粒子滤波的定位方法、装置、计算机设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010230278.3A CN111352066B (zh) 2020-03-27 2020-03-27 基于粒子滤波的定位方法、装置、计算机设备和存储介质

Publications (2)

Publication Number Publication Date
CN111352066A CN111352066A (zh) 2020-06-30
CN111352066B true CN111352066B (zh) 2022-02-22

Family

ID=71193161

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010230278.3A Active CN111352066B (zh) 2020-03-27 2020-03-27 基于粒子滤波的定位方法、装置、计算机设备和存储介质

Country Status (1)

Country Link
CN (1) CN111352066B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112068174B (zh) * 2020-08-18 2021-11-23 三一专用汽车有限责任公司 定位方法、定位装置和计算机可读存储介质
CN113311384B (zh) * 2021-04-29 2023-11-10 中国人民解放军国防科技大学 一种单边双向测距方法及装置
CN115267667B (zh) * 2022-09-28 2023-04-07 长沙迪迈数码科技股份有限公司 井下高精度定位修正方法、装置、设备及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104796866A (zh) * 2015-05-06 2015-07-22 北京我联科技有限公司 室内定位方法和装置
CN104950288A (zh) * 2015-04-30 2015-09-30 成都恒高科技有限公司 巷道环境定位方法、装置、系统以及定位标签卡
CN105898865A (zh) * 2016-06-17 2016-08-24 杭州电子科技大学 非线性非高斯条件下基于ekf和pf的协同定位方法
CN107132504A (zh) * 2016-02-29 2017-09-05 富士通株式会社 基于粒子滤波的定位追踪装置、方法及电子设备
CN109870716A (zh) * 2017-12-01 2019-06-11 北京京东尚科信息技术有限公司 定位方法和定位装置及计算机可读存储介质
CN110488818A (zh) * 2019-08-08 2019-11-22 深圳市银星智能科技股份有限公司 一种基于激光雷达的机器人定位方法、装置和机器人
CN110708659A (zh) * 2019-10-12 2020-01-17 山东爱拓软件开发有限公司 一种适用于矿井路径的人员定位方法及系统
CN110702093A (zh) * 2019-09-27 2020-01-17 五邑大学 基于粒子滤波的定位方法、装置、存储介质及机器人

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104950288A (zh) * 2015-04-30 2015-09-30 成都恒高科技有限公司 巷道环境定位方法、装置、系统以及定位标签卡
CN104796866A (zh) * 2015-05-06 2015-07-22 北京我联科技有限公司 室内定位方法和装置
CN107132504A (zh) * 2016-02-29 2017-09-05 富士通株式会社 基于粒子滤波的定位追踪装置、方法及电子设备
CN105898865A (zh) * 2016-06-17 2016-08-24 杭州电子科技大学 非线性非高斯条件下基于ekf和pf的协同定位方法
CN109870716A (zh) * 2017-12-01 2019-06-11 北京京东尚科信息技术有限公司 定位方法和定位装置及计算机可读存储介质
CN110488818A (zh) * 2019-08-08 2019-11-22 深圳市银星智能科技股份有限公司 一种基于激光雷达的机器人定位方法、装置和机器人
CN110702093A (zh) * 2019-09-27 2020-01-17 五邑大学 基于粒子滤波的定位方法、装置、存储介质及机器人
CN110708659A (zh) * 2019-10-12 2020-01-17 山东爱拓软件开发有限公司 一种适用于矿井路径的人员定位方法及系统

Also Published As

Publication number Publication date
CN111352066A (zh) 2020-06-30

Similar Documents

Publication Publication Date Title
CN111352066B (zh) 基于粒子滤波的定位方法、装置、计算机设备和存储介质
Chai et al. Reference tag supported RFID tracking using robust support vector regression and Kalman filter
CN112946624B (zh) 一种基于航迹管理方法的多目标跟踪方法
US9354305B2 (en) Method for producing at least information for track fusion and association for radar target tracking, and storage medium thereof
CN111736145B (zh) 一种基于高斯混合概率假设密度滤波的多机动目标多普勒雷达跟踪方法
CN109061616B (zh) 一种移动目标定位方法
CN108717174B (zh) 基于信息论的预测快速协方差交互融合无源协同定位方法
KR102311227B1 (ko) 확장 칼만필터를 이용한 차량 위치 추적 방법 및 장치
Wu et al. UAV-based target tracking: Integrating sensing into communication signals
Guo et al. Automatic parking system based on improved neural network algorithm and intelligent image analysis
CN110213713A (zh) 一种基于时空域联合处理的集中式协作定位系统和方法
CN116630370A (zh) 多模型pbp-tpmb机动扩展目标跟踪方法
CN116500602A (zh) 一种基于无源分布式雷达系统的多目标跟踪航迹管理方法
CN115421153B (zh) 一种基于扩展卡尔曼滤波的激光雷达和uwb组合定位方法及系统
EP4160269A1 (en) Systems and methods for onboard analysis of sensor data for sensor fusion
CN113835061B (zh) 一种信号载波频率先验误差存在下单平台多普勒两阶段闭式定位方法
US20100289699A1 (en) Method and Apparatus for Determining a Position of a Mobile Station in a Wireless Communication System
Guan et al. Indoor localization with asymmetric grid-based filters in large areas utilizing smartphones
Piperigkos et al. 5G enabled cooperative localization of connected and semi-autonomous vehicles via sparse Laplacian processing
Zhu et al. Receiver selection for multi-target tracking in multi-static Doppler radar systems
Alagoz et al. A theoretical investigation on moving average filtering solution for fixed-path map matching of noisy position data
CN117169818B (zh) 一种面向海面机动目标跟踪的雷达波形设计方法
CN113343728B (zh) 一种基于非线性优化方法的rfid定位方法
Coraluppi et al. Hierarchical multi-hypothesis tracking with application to multi-scale sensor data
CN113359837B (zh) 一种使用单架无人机追踪移动源的方法

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