CN115267667A - 井下高精度定位修正方法、装置、设备及存储介质 - Google Patents

井下高精度定位修正方法、装置、设备及存储介质 Download PDF

Info

Publication number
CN115267667A
CN115267667A CN202211186364.4A CN202211186364A CN115267667A CN 115267667 A CN115267667 A CN 115267667A CN 202211186364 A CN202211186364 A CN 202211186364A CN 115267667 A CN115267667 A CN 115267667A
Authority
CN
China
Prior art keywords
positioning
dimensional
value
observation value
dimensional observation
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.)
Granted
Application number
CN202211186364.4A
Other languages
English (en)
Other versions
CN115267667B (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.)
Changsha Dimai Technology Co.,Ltd.
Original Assignee
Changsha Digital Mine 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 Changsha Digital Mine Co ltd filed Critical Changsha Digital Mine Co ltd
Priority to CN202211186364.4A priority Critical patent/CN115267667B/zh
Publication of CN115267667A publication Critical patent/CN115267667A/zh
Application granted granted Critical
Publication of CN115267667B publication Critical patent/CN115267667B/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/0205—Details
    • G01S5/021—Calibration, monitoring or correction
    • 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/06—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
    • 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/10—Position of receiver fixed by co-ordinating a plurality of position lines defined by path-difference measurements, e.g. omega or decca systems
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management

Abstract

本申请公开了一种井下高精度定位修正方法、装置、设备及存储介质。该方法包括：获取定位对象在井下定位场景中测量的三维定位坐标；基于起始时刻的三维定位坐标和起始时刻之后的采样时刻的三维定位坐标生成定位对象的一维观测值；基于一维观测值构建定位对象的运动状态方程；基于卡尔曼滤波和运动状态方程对定位对象的一维观测值进行修正，得到修正后的一维观测值；基于修正后的一维观测值还原得到定位对象的修正后的三维定位坐标。可以将三维空间的定位坐标降维为受限空间内的一维观测值，可以准确地构建出定位对象的运动状态方程，且降维后可进一步提高滤波算法的效率，进而能够有效地实现井下定位的误差修正，提高定位精度和定位修正效率。

Description

井下高精度定位修正方法、装置、设备及存储介质
技术领域
本申请涉及井下定位领域，尤其涉及一种井下高精度定位修正方法、装置、设备及存储介质。
背景技术
矿山井下开采有一定的危险性，开采需要在井下布置大量的设备、车辆、人员，各种自然或人为因素造成的灾害、事故对井下工作人员的人身安全造成了极大的威胁。对井下人员、车辆有效的组织管理，以及事故、灾害的救援都需要掌握井下人员、车辆、设备的精确位置数据，井下设备的远程控制、智能控制更离不开井下设备的实时精确位置数据。
精确定位技术已成为矿井安全生产的重要技术支撑，相关技术中，往往基于UWB（Ultra Wide Band，超带宽）定位技术实现井下精确定位，在井下每隔一定距离布置UWB定位基站，人员、车辆或设备携带UWB定位标签，UWB定位基站与UWB定位标签之间通过测量脉冲信号的飞行时间（Time of flight，TOF），精确计算UWB定位标签到UWB定位基站之间的距离。由于UWB定位基站的坐标是已知的，根据UWB定位标签与多个UWB定位基站之间的精确距离，可精确计算出UWB定位标签的坐标。
UWB测距的精确性是相对于Wi-Fi、蓝牙或ZigBee而言的，UWB静态测距精度仍有30cm左右的误差，动态测距误差会更大。在UWB测距结果的基础上，经过基于测距的空间定位算法计算的定位对象空间坐标也相应的具有一定的误差。对UWB定位误差的修正通常采用卡尔曼滤波、扩展卡尔曼滤波、容积卡尔曼滤波、无迹卡尔曼滤波、粒子滤波、正则粒子滤波或高斯粒子滤波等方法，上述方法均依赖于定位对象空间运动状态方程的构建，由于井下定位对象三维空间运动状态的未知性，难以构建较准确的运动状态方程，故修正效果不理想。
发明内容
有鉴于此，本申请实施例提供了一种井下高精度定位修正方法、装置、设备及存储介质，旨在有效修正井下定位误差，提高井下定位精度。
本申请实施例的技术方案是这样实现的：
第一方面，本申请实施例提供了一种井下高精度定位修正方法，包括：
获取定位对象在井下定位场景中测量的三维定位坐标；
基于起始时刻的三维定位坐标和所述起始时刻之后的采样时刻的三维定位坐标生成所述定位对象的一维观测值，所述一维观测值表征所述定位对象相应于所述起始时刻至所述采样时刻沿井下巷道的无向图的最短距离；
基于所述一维观测值构建所述定位对象的运动状态方程；
基于卡尔曼滤波和所述运动状态方程对所述定位对象的所述一维观测值进行修正，得到修正后的一维观测值；
基于所述修正后的一维观测值还原得到所述定位对象的修正后的三维定位坐标。
在一些实施例中，所述运动状态方程表示如下：
Figure 645170DEST_PATH_IMAGE001
其中，
Figure 509220DEST_PATH_IMAGE002
为定位对象i在采样时刻t的状态值，
Figure 323593DEST_PATH_IMAGE003
为定位对象i在采样时刻t-1的状态值，
Figure 490263DEST_PATH_IMAGE004
为定位对象i在采样时刻t的滑动时间窗口平均速度，
Figure 22875DEST_PATH_IMAGE005
，
Figure 159459DEST_PATH_IMAGE006
为定位对象i在采样时刻t的一维观测值，N为滑动时间窗口的大小，f为采样频率。
在一些实施例中，所述基于卡尔曼滤波和所述运动状态方程对所述定位对象的所述一维观测值进行修正，得到修正后的一维观测值，包括：
计算定位对象i在采样时刻t的先验估计值
Figure 93917DEST_PATH_IMAGE007
；其中，
Figure 680756DEST_PATH_IMAGE008
，
Figure 169506DEST_PATH_IMAGE009
为定位对象i在采样时刻t的运动状态突变值，
Figure 437676DEST_PATH_IMAGE010
表示出现运行状态突变，
Figure 164324DEST_PATH_IMAGE011
表示未出现运动状态突变；
计算定位对象i在采样时刻t的卡尔曼增益
Figure 594168DEST_PATH_IMAGE012
；其中，
Figure 412957DEST_PATH_IMAGE013
，
Figure 219239DEST_PATH_IMAGE014
为定位对象i在采样时刻t-1的协方差，Q为所述运动状态方程的误差标准差，R为所述一维观测值的误差标准差；
计算定位对象i在采样时刻t的修正后的一维观测值
Figure 800393DEST_PATH_IMAGE015
；其中，
Figure 666718DEST_PATH_IMAGE016
，
Figure 457957DEST_PATH_IMAGE017
为定位对象i在采样时刻t的突变影响值，
Figure 67930DEST_PATH_IMAGE018
表示受运动状态突变影响，
Figure 769169DEST_PATH_IMAGE020
表示不受运动状态突变影响。
在一些实施例中，定位对象i在采样时刻t的运动状态突变值
Figure 540816DEST_PATH_IMAGE021
计算如下：
Figure 835662DEST_PATH_IMAGE022
其中，
Figure 983747DEST_PATH_IMAGE023
为瞬时加速度阈值，
Figure 539493DEST_PATH_IMAGE024
为滑动时间窗口的平均加速度阈值。
在一些实施例中，定位对象i在采样时刻t的突变影响值
Figure 482041DEST_PATH_IMAGE025
计算如下：
Figure 247872DEST_PATH_IMAGE026
其中，
Figure 199648DEST_PATH_IMAGE027
为定位对象i的运动状态突变的持续影响时长。
在一些实施例中，所述方法还包括：
生成定位对象i在采样时刻t的协方差
Figure 609900DEST_PATH_IMAGE028
，具体如下：
Figure 988929DEST_PATH_IMAGE029
其中，
Figure 491324DEST_PATH_IMAGE030
为定位对象i初始的协方差。
在一些实施例中，所述基于所述修正后的一维观测值还原得到所述定位对象的修正后的三维定位坐标，包括：
Figure 981211DEST_PATH_IMAGE031
其中，
Figure 511549DEST_PATH_IMAGE032
为定位对象i在采样时刻t测量的三维定位坐标，
Figure 61479DEST_PATH_IMAGE033
为定位对象i在采样时刻t的一维观测值，
Figure 536323DEST_PATH_IMAGE034
为定位对象i在采样时刻t的修正后的一维观测值，
Figure 829901DEST_PATH_IMAGE035
为定位对象i在采样时刻t的修正后的三维定位坐标。
第二方面，本申请实施例提供了一种井下高精度定位修正装置，包括：
获取模块，用于获取定位对象在井下定位场景中测量的三维定位坐标；
一维观测值生成模块，用于基于起始时刻的三维定位坐标和所述起始时刻之后的采样时刻的三维定位坐标生成所述定位对象的一维观测值，所述一维观测值表征所述定位对象相应于所述起始时刻至所述采样时刻沿井下巷道的无向图的最短距离；
运动状态方程模块，用于基于所述一维观测值构建所述定位对象的运动状态方程；
一维修正模块，用于基于卡尔曼滤波和所述运动状态方程对所述定位对象的所述一维观测值进行修正，得到修正后的一维观测值；
三维修正模块，用于基于所述修正后的一维观测值还原得到所述定位对象的修正后的三维定位坐标。
第三方面，本申请实施例提供了一种井下高精度定位修正设备，包括：处理器和用于存储能够在处理器上运行的计算机程序的存储器，其中，
所述处理器，用于运行计算机程序时，执行本申请实施例第一方面所述方法的步骤。
第四方面，本申请实施例提供了一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时，实现本申请实施例第一方面所述方法的步骤。
本申请实施例提供的技术方案，将井下定位场景中测量的定位对象的三维定位坐标转换为一维观测值，并基于该一维观测值构建定位对象的运动状态方程；基于卡尔曼滤波和运动状态方程对定位对象的一维观测值进行修正，得到修正后的一维观测值；再基于修正后的一维观测值还原得到定位对象的修正后的三维定位坐标。如此，可以将三维空间的定位坐标降维为受限空间内的一维观测值，可以准确地构建出定位对象的运动状态方程，且降维后可进一步提高滤波算法的效率，进而能够有效地实现井下定位的误差修正，提高定位精度和定位修正效率。
附图说明
图1为本申请实施例井下高精度定位修正方法的流程示意图；
图2为本申请一应用示例中井下矿山巷道的无向图示意图；
图3为本申请一应用示例中三维定位坐标降维为一维观测值的示意图；
图4为本申请一应用示例中运动状态未出现突变时的误差比对示意图；
图5为本申请一应用示例中运动状态出现突变时的误差比对示意图；
图6为本申请实施例井下高精度定位修正装置的结构示意图；
图7为本申请实施例井下高精度定位修正设备的结构示意图。
具体实施方式
下面结合附图及实施例对本申请再作进一步详细的描述。
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请。
本申请实施例提供了一种井下高精度定位修正方法，可以应用于井下定位的数据处理设备中，如图1所示，该方法包括：
步骤101，获取定位对象在井下定位场景中测量的三维定位坐标。
示例性地，数据处理设备获取的定位对象的三维定位坐标可以为基于UWB测距技术获取的，UWB测距技术可以参照相关技术，在此不再赘述。
步骤102，基于起始时刻的三维定位坐标和所述起始时刻之后的采样时刻的三维定位坐标生成所述定位对象的一维观测值，所述一维观测值表征所述定位对象相应于所述起始时刻至所述采样时刻沿井下巷道的无向图的最短距离。
这里，井下巷道的无向图可以为基于井下巷道的中心线构建的，其中，无向图的边为巷道，中间节点为巷道交叉口，端节点为巷道入口或巷道终点。例如，无向图可以表示为G=<V，E>，其中，V是非空集合，称为顶点集；E是V中元素构成的无序二元组的集合，称为边集。
示例性地，假设定位对象i在第一次进入定位场景后观测得到的三维空间定位坐标为
Figure 214746DEST_PATH_IMAGE036
；在采样时刻t定位对象i测得到的三维定位坐标为
Figure 935577DEST_PATH_IMAGE037
，计算无向图G中
Figure 914029DEST_PATH_IMAGE038
与
Figure 745718DEST_PATH_IMAGE039
的最短距离为
Figure 985070DEST_PATH_IMAGE040
，从而可以将三维定位坐标
Figure 876803DEST_PATH_IMAGE041
降维为一维观测值
Figure 326238DEST_PATH_IMAGE042
。
步骤103，基于所述一维观测值构建所述定位对象的运动状态方程。
示例性地，所述运动状态方程表示如下：
Figure 961619DEST_PATH_IMAGE043
其中，
Figure 321056DEST_PATH_IMAGE044
为定位对象i在采样时刻t的状态值，
Figure 383690DEST_PATH_IMAGE045
为定位对象i在采样时刻t-1的状态值，
Figure 569690DEST_PATH_IMAGE046
为定位对象i在采样时刻t的滑动时间窗口平均速度，
Figure 743182DEST_PATH_IMAGE047
，
Figure 957126DEST_PATH_IMAGE048
为定位对象i在采样时刻t的一维观测值，N为滑动时间窗口的大小，f为采样频率。
需要说明的是，本申请实施例将位于三维空间的定位坐标
Figure 190661DEST_PATH_IMAGE049
降维为受限空间内的一维观测值
Figure 614689DEST_PATH_IMAGE050
，可以准确地构建出定位对象i的运动状态方程，并可以提高滤波算法的效率。
步骤104，基于卡尔曼滤波和所述运动状态方程对所述定位对象的所述一维观测值进行修正，得到修正后的一维观测值。
示例性地，所述基于卡尔曼滤波和所述运动状态方程对所述定位对象的所述一维观测值进行修正，得到修正后的一维观测值，包括：
计算定位对象i在采样时刻t的先验估计值
Figure 591872DEST_PATH_IMAGE034
；其中，
Figure 925902DEST_PATH_IMAGE051
，
Figure 408967DEST_PATH_IMAGE052
为定位对象i在采样时刻t的运动状态突变值，
Figure 992395DEST_PATH_IMAGE053
表示出现运行状态突变，
Figure 710952DEST_PATH_IMAGE054
表示未出现运动状态突变；
计算定位对象i在采样时刻t的卡尔曼增益
Figure 961805DEST_PATH_IMAGE055
；其中，
Figure 599460DEST_PATH_IMAGE056
，
Figure 670184DEST_PATH_IMAGE057
为定位对象i在采样时刻t-1的协方差，Q为所述运动状态方程的误差标准差，R为所述一维观测值的误差标准差；
计算定位对象i在采样时刻t的修正后的一维观测值
Figure 926853DEST_PATH_IMAGE034
；其中，
Figure 32212DEST_PATH_IMAGE058
，
Figure 90036DEST_PATH_IMAGE059
为定位对象i在采样时刻t的突变影响值，
Figure 913635DEST_PATH_IMAGE060
表示受运动状态突变影响，
Figure 708416DEST_PATH_IMAGE061
表示不受运动状态突变影响。
需要说明的是，运动状态方程的误差标准差Q及一维观测值的误差标准差R可以预先设定，定位对象的协方差可以理解为先验估计值与一维观测值的协方差，协方差的大小，可以理解为通过先验估计值和一维观测值计算最优估计值时两者的权重。初始的协方差一般设的比较大（一般>1），代表一维观测值的权重更大，后面会逐步迭代更新至合适的协方差值。
示例性地，定位对象i在采样时刻t的运动状态突变值
Figure 933861DEST_PATH_IMAGE062
计算如下：
Figure 913318DEST_PATH_IMAGE063
其中，
Figure 161897DEST_PATH_IMAGE064
为瞬时加速度阈值，
Figure 822685DEST_PATH_IMAGE065
为滑动时间窗口的平均加速度阈值。
示例性地，定位对象i在采样时刻t的突变影响值
Figure 715686DEST_PATH_IMAGE066
计算如下：
Figure 803728DEST_PATH_IMAGE067
其中，
Figure 539603DEST_PATH_IMAGE068
为定位对象i的运动状态突变的持续影响时长。
在一些实施例中，该方法还包括：
生成定位对象i在采样时刻t的协方差
Figure 66399DEST_PATH_IMAGE069
，具体如下：
Figure 857DEST_PATH_IMAGE070
其中，
Figure 197483DEST_PATH_IMAGE071
为定位对象i初始的协方差。
步骤105，基于所述修正后的一维观测值还原得到所述定位对象的修正后的三维定位坐标。
示例性地，所述基于所述修正后的一维观测值还原得到所述定位对象的修正后的三维定位坐标，包括：
Figure 217392DEST_PATH_IMAGE072
其中，
Figure 62726DEST_PATH_IMAGE073
为定位对象i在采样时刻t测量的三维定位坐标，
Figure 789373DEST_PATH_IMAGE074
为定位对象i在采样时刻t的一维观测值，
Figure 219218DEST_PATH_IMAGE075
为定位对象i在采样时刻t的修正后的一维观测值，
Figure 788739DEST_PATH_IMAGE076
为定位对象i在采样时刻t的修正后的三维定位坐标，即基于修正后的一维观测值
Figure 595021DEST_PATH_IMAGE075
还原得到的三维定位坐标。
可以理解的是，本申请实施例的方法，将井下定位场景中测量的定位对象的三维定位坐标转换为一维观测值，并基于该一维观测值构建定位对象的运动状态方程；基于卡尔曼滤波和运动状态方程对定位对象的一维观测值进行修正，得到修正后的一维观测值；再基于修正后的一维观测值还原得到定位对象的修正后的三维定位坐标。如此，可以将三维空间的定位坐标降维为受限空间内的一维观测值，可以准确地构建出定位对象的运动状态方程，且降维后可进一步提高滤波算法的效率，进而能够有效地实现井下定位的误差修正，提高定位精度和定位修正效率；此外，本申请实施例方法能够自动识别运动状态突变的情况，自适应地进行卡尔曼滤波降噪，有效地实现井下UWB定位的误差修正，从而可以为井下人员、车辆、设备提供实时、稳定、精确的位置数据。
在一应用示例中，基于卡尔曼滤波和运动状态方程对定位对象的一维观测值进行修正，得到修正后的一维观测值的过程具体包括以下步骤：
步骤1）、设置初始的协方差
Figure 176175DEST_PATH_IMAGE077
；
步骤2）、t=0；
步骤3）、t=t+1；
步骤4）、计算t时刻定位对象i的滑动时间窗口平均速度
Figure 42500DEST_PATH_IMAGE078
，
Figure 584471DEST_PATH_IMAGE079
；
步骤5）、建立一维观测值的运动状态方程：
Figure 194444DEST_PATH_IMAGE080
；
其中，F表示一个抽象的函数，函数有两个参数i和t，F(i,t)表示定位对象i在采样时刻t的状态值。
步骤6）、设t-1时刻定位对象i经自适应卡尔曼滤波修正后的一维观测值为
Figure 630104DEST_PATH_IMAGE081
，根据运动状态方程和突变状态，计算t时刻定位对象i的先验估计值
Figure 667331DEST_PATH_IMAGE082
，
Figure 945865DEST_PATH_IMAGE083
步骤7）、计算时刻定位对象的卡尔曼增益
Figure 93950DEST_PATH_IMAGE084
；
Figure 649696DEST_PATH_IMAGE085
步骤8）、计算时刻定位对象进行自适应卡尔曼滤波修正后的值
Figure 857823DEST_PATH_IMAGE086
，
Figure 872922DEST_PATH_IMAGE087
步骤9）、更新协方差
Figure 824697DEST_PATH_IMAGE088
；
Figure 234950DEST_PATH_IMAGE089
且更新后跳转至步骤3），以继续对一维观测值进行修正。
下面结合一应用示例对本申请实施例的井下高精度定位修正方法进行示例性说明。
图2示出了本应用示例中井下矿山巷道的无向图示意图，其中，该无向图基于巷道中心线1构建而成。
定位对象i在第一次进入定位场景后观测得到的三维空间定位坐标为
Figure 613979DEST_PATH_IMAGE090
；t时刻，定位对象i测得到的三维空间定位坐标为
Figure 867106DEST_PATH_IMAGE091
，计算无向图中
Figure 356993DEST_PATH_IMAGE092
与
Figure 887331DEST_PATH_IMAGE090
的最短距离为
Figure 171682DEST_PATH_IMAGE093
，如图3中的粗实线部分所示。
设定滑动时间窗口的大小N为5，瞬时加速度阈值
Figure 662837DEST_PATH_IMAGE094
为0.6m/s2，滑动时间窗口平均加速度阈值
Figure 956415DEST_PATH_IMAGE095
为0.2m/s2，定位的采样频率f为1，即每秒采集一次，一维观测值的误差标准差R为0.3m，运动状态方程的误差标准差Q为4.5m，定位对象i突变状态的持续影响时长∆T为10s，初始的协方差
Figure 341260DEST_PATH_IMAGE096
为5。
t1至t300时刻，定位对象运动状态未出现突变，经自适应卡尔曼滤波修正后，修正之后的定位坐标误差与测量结果的定位坐标误差对比如图4所示。
T301至t600时刻，定位对象运动状态出现6次突变，经自适应卡尔曼滤波修正后，修正之后的定位坐标误差与测量结果的定位坐标误差对比如图5所示。
由此可见，本申请实施例的方法，不论定位对象是否出现运动状态突变，均能有效减少定位坐标误差，提高定位精度。
为了实现本申请实施例的方法，本申请实施例还提供一种井下高精度定位修正装置，设置在井下高精度定位修正设备，如图6所示，该井下高精度定位修正装置包括：获取模块601、一维观测值生成模块602、运动状态方程模块603、一维修正模块604和三维修正模块605。
获取模块601用于获取定位对象在井下定位场景中测量的三维定位坐标；一维观测值生成模块602用于基于起始时刻的三维定位坐标和所述起始时刻之后的采样时刻的三维定位坐标生成所述定位对象的一维观测值，所述一维观测值表征所述定位对象相应于所述起始时刻至所述采样时刻沿井下巷道的无向图的最短距离；运动状态方程模块603用于基于所述一维观测值构建所述定位对象的运动状态方程；一维修正模块604用于基于卡尔曼滤波和所述运动状态方程对所述定位对象的所述一维观测值进行修正，得到修正后的一维观测值；三维修正模块605用于基于所述修正后的一维观测值还原得到所述定位对象的修正后的三维定位坐标。
在一些实施例中，所述运动状态方程表示如下：
Figure 62092DEST_PATH_IMAGE097
其中，
Figure 24231DEST_PATH_IMAGE098
为定位对象i在采样时刻t的状态值，
Figure 855921DEST_PATH_IMAGE099
为定位对象i在采样时刻t-1的状态值，
Figure 360852DEST_PATH_IMAGE100
为定位对象i在采样时刻t的滑动时间窗口平均速度，
Figure 252585DEST_PATH_IMAGE101
，
Figure 240305DEST_PATH_IMAGE102
为定位对象i在采样时刻t的一维观测值，N为滑动时间窗口的大小，f为采样频率。
在一些实施例中，一维修正模块604具体用于：
计算定位对象i在采样时刻t的先验估计值
Figure 813369DEST_PATH_IMAGE103
；其中，
Figure 235123DEST_PATH_IMAGE104
，
Figure 360073DEST_PATH_IMAGE105
为定位对象i在采样时刻t的运动状态突变值，
Figure 234489DEST_PATH_IMAGE106
表示出现运行状态突变，
Figure 345664DEST_PATH_IMAGE107
表示未出现运动状态突变；
计算定位对象i在采样时刻t的卡尔曼增益
Figure 356345DEST_PATH_IMAGE108
；其中，
Figure 589881DEST_PATH_IMAGE109
，
Figure 764641DEST_PATH_IMAGE110
为定位对象i在采样时刻t-1的协方差，Q为所述运动状态方程的误差标准差，R为所述一维观测值的误差标准差；
计算定位对象i在采样时刻t的修正后的一维观测值
Figure 741824DEST_PATH_IMAGE111
；其中，
Figure 810275DEST_PATH_IMAGE112
，
Figure 542607DEST_PATH_IMAGE113
为定位对象i在采样时刻t的突变影响值，
Figure 391614DEST_PATH_IMAGE114
表示受运动状态突变影响，
Figure 110172DEST_PATH_IMAGE115
表示不受运动状态突变影响。
在一些实施例中，定位对象i在采样时刻t的运动状态突变值
Figure 95445DEST_PATH_IMAGE116
计算如下：
Figure 982368DEST_PATH_IMAGE117
其中，
Figure 318671DEST_PATH_IMAGE118
为瞬时加速度阈值，
Figure 575340DEST_PATH_IMAGE119
为滑动时间窗口的平均加速度阈值。
在一些实施例中，定位对象i在采样时刻t的突变影响值
Figure 680699DEST_PATH_IMAGE120
计算如下：
Figure 489255DEST_PATH_IMAGE121
其中，
Figure 312855DEST_PATH_IMAGE122
为定位对象i的运动状态突变的持续影响时长。
在一些实施例中，一维修正模块604还用于：
生成定位对象i在采样时刻t的协方差
Figure 107635DEST_PATH_IMAGE123
，具体如下：
Figure 67501DEST_PATH_IMAGE124
其中，
Figure 63270DEST_PATH_IMAGE125
为定位对象i初始的协方差。
在一些实施例中，三维修正模块605具体用于：
Figure 108587DEST_PATH_IMAGE126
其中，
Figure 707058DEST_PATH_IMAGE127
为定位对象i在采样时刻t测量的三维定位坐标，
Figure 787010DEST_PATH_IMAGE128
为定位对象i在采样时刻t的一维观测值，
Figure 609472DEST_PATH_IMAGE129
为定位对象i在采样时刻t的修正后的一维观测值，
Figure 204402DEST_PATH_IMAGE130
为定位对象i在采样时刻t的修正后的三维定位坐标。
实际应用时，获取模块601、一维观测值生成模块602、运动状态方程模块603、一维修正模块604和三维修正模块605，可以由井下高精度定位修正设备中的处理器来实现。当然，处理器需要运行存储器中的计算机程序来实现它的功能。
需要说明的是：上述实施例提供的井下高精度定位修正装置在进行井下高精度定位修正时，仅以上述各程序模块的划分进行举例说明，实际应用中，可以根据需要而将上述处理分配由不同的程序模块完成，即将装置的内部结构划分成不同的程序模块，以完成以上描述的全部或者部分处理。另外，上述实施例提供的井下高精度定位修正装置与井下高精度定位修正方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。
基于上述程序模块的硬件实现，且为了实现本申请实施例的方法，本申请实施例还提供一种井下高精度定位修正设备。图7仅仅示出了该设备的示例性结构而非全部结构，根据需要可以实施图7示出的部分结构或全部结构。
如图7所示，本申请实施例提供的井下高精度定位修正设备700包括：至少一个处理器701、存储器702、用户接口703和至少一个网络接口704。井下高精度定位修正设备700中的各个组件通过总线系统705耦合在一起。可以理解，总线系统705用于实现这些组件之间的连接通信。总线系统705除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图7中将各种总线都标为总线系统705。
其中，用户接口703可以包括显示器、键盘、鼠标、轨迹球、点击轮、按键、按钮、触感板或者触摸屏等。
本申请实施例中的存储器702用于存储各种类型的数据以支持井下高精度定位修正设备的操作。这些数据的示例包括：用于在井下高精度定位修正设备上操作的任何计算机程序。
本申请实施例揭示的井下高精度定位修正方法可以应用于处理器701中，或者由处理器701实现。处理器701可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，井下高精度定位修正方法的各步骤可以通过处理器701中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器701可以是通用处理器、数字信号处理器（DSP，DigitalSignal Processor），或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。处理器701可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者任何常规的处理器等。结合本申请实施例所公开的方法的步骤，可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于存储介质中，该存储介质位于存储器702，处理器701读取存储器702中的信息，结合其硬件完成本申请实施例提供的井下高精度定位修正方法的步骤。
在示例性实施例中，井下高精度定位修正设备可以被一个或多个应用专用集成电路（ASIC，Application Specific Integrated Circuit）、DSP、可编程逻辑器件（PLD，Programmable Logic Device）、复杂可编程逻辑器件（CPLD，Complex Programmable LogicDevice）、FPGA、通用处理器、控制器、微控制器（MCU，Micro Controller Unit）、微处理器（Microprocessor）、或者其他电子元件实现，用于执行前述方法。
可以理解，存储器702可以是易失性存储器或非易失性存储器，也可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器（ROM，Read Only Memory）、可编程只读存储器（PROM，Programmable Read-Only Memory）、可擦除可编程只读存储器（EPROM，Erasable Programmable Read-Only Memory）、电可擦除可编程只读存储器（EEPROM，Electrically Erasable Programmable Read-Only Memory）、磁性随机存取存储器（FRAM，ferromagnetic random access memory）、快闪存储器（Flash Memory）、磁表面存储器、光盘、或只读光盘（CD-ROM，Compact Disc Read-Only Memory）；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器（RAM，Random AccessMemory），其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器（SRAM，Static Random Access Memory）、同步静态随机存取存储器（SSRAM，Synchronous Static Random Access Memory）、动态随机存取存储器（DRAM，Dynamic Random Access Memory）、同步动态随机存取存储器（SDRAM，SynchronousDynamic Random Access Memory）、双倍数据速率同步动态随机存取存储器（DDRSDRAM，Double Data Rate Synchronous Dynamic Random Access Memory）、增强型同步动态随机存取存储器（ESDRAM，Enhanced Synchronous Dynamic Random Access Memory）、同步连接动态随机存取存储器（SLDRAM，SyncLink Dynamic Random Access Memory）、直接内存总线随机存取存储器（DRRAM，Direct Rambus Random Access Memory）。本申请实施例描述的存储器旨在包括但不限于这些和任意其它适合类型的存储器。
在示例性实施例中，本申请实施例还提供了一种存储介质，即计算机存储介质，具体可以是计算机可读存储介质，例如包括存储计算机程序的存储器702，上述计算机程序可由井下高精度定位修正设备的处理器701执行，以完成本申请实施例方法所述的步骤。计算机可读存储介质可以是ROM、PROM、EPROM、EEPROM、Flash Memory、磁表面存储器、光盘、或CD-ROM等存储器。
需要说明的是：“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
另外，本申请实施例所记载的技术方案之间，在不冲突的情况下，可以任意组合。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请披露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种井下高精度定位修正方法，其特征在于，包括：
获取定位对象在井下定位场景中测量的三维定位坐标；
基于起始时刻的三维定位坐标和所述起始时刻之后的采样时刻的三维定位坐标生成所述定位对象的一维观测值，所述一维观测值表征所述定位对象相应于所述起始时刻至所述采样时刻沿井下巷道的无向图的最短距离；
基于所述一维观测值构建所述定位对象的运动状态方程；
基于卡尔曼滤波和所述运动状态方程对所述定位对象的所述一维观测值进行修正，得到修正后的一维观测值；
基于所述修正后的一维观测值还原得到所述定位对象的修正后的三维定位坐标。
2.根据权利要求1所述的方法，其特征在于，所述运动状态方程表示如下：
Figure 370114DEST_PATH_IMAGE001
其中，
Figure 21675DEST_PATH_IMAGE002
为定位对象i在采样时刻t的状态值，
Figure 535833DEST_PATH_IMAGE003
为定位对象i在采样时刻t-1的状态值，
Figure 665463DEST_PATH_IMAGE004
为定位对象i在采样时刻t的滑动时间窗口平均速度，
Figure 214256DEST_PATH_IMAGE005
，
Figure 771139DEST_PATH_IMAGE006
为定位对象i在采样时刻t的一维观测值，N为滑动时间窗口的大小，f为采样频率。
3.根据权利要求2所述的方法，其特征在于，所述基于卡尔曼滤波和所述运动状态方程对所述定位对象的所述一维观测值进行修正，得到修正后的一维观测值，包括：
计算定位对象i在采样时刻t的先验估计值
Figure 21861DEST_PATH_IMAGE007
；其中，
Figure 955182DEST_PATH_IMAGE008
，
Figure 358481DEST_PATH_IMAGE009
为定位对象i在采样时刻t的运动状态突变值，
Figure 86266DEST_PATH_IMAGE010
表示出现运行状态突变，
Figure 575016DEST_PATH_IMAGE011
表示未出现运动状态突变；
计算定位对象i在采样时刻t的卡尔曼增益
Figure 46448DEST_PATH_IMAGE012
；其中，
Figure 304254DEST_PATH_IMAGE013
，
Figure 468520DEST_PATH_IMAGE014
为定位对象i在采样时刻t-1的协方差，Q为所述运动状态方程的误差标准差，R为所述一维观测值的误差标准差；
计算定位对象i在采样时刻t的修正后的一维观测值
Figure 717271DEST_PATH_IMAGE015
；其中，
Figure 726815DEST_PATH_IMAGE016
，
Figure 104707DEST_PATH_IMAGE017
为定位对象i在采样时刻t的突变影响值，
Figure 174294DEST_PATH_IMAGE018
表示受运动状态突变影响，
Figure 372057DEST_PATH_IMAGE019
表示不受运动状态突变影响。
4.根据权利要求3所述的方法，其特征在于，定位对象i在采样时刻t的运动状态突变值
Figure 185292DEST_PATH_IMAGE020
计算如下：
Figure 683270DEST_PATH_IMAGE022
其中，
Figure 189338DEST_PATH_IMAGE023
为瞬时加速度阈值，
Figure 123664DEST_PATH_IMAGE024
为滑动时间窗口的平均加速度阈值。
5.根据权利要求3所述的方法，其特征在于，定位对象i在采样时刻t的突变影响值
Figure 475011DEST_PATH_IMAGE025
计算如下：
Figure 827495DEST_PATH_IMAGE026
其中，
Figure 238885DEST_PATH_IMAGE027
为定位对象i的运动状态突变的持续影响时长。
6.根据权利要求3所述的方法，其特征在于，所述方法还包括：
生成定位对象i在采样时刻t的协方差
Figure 411240DEST_PATH_IMAGE028
，具体如下：
Figure 566278DEST_PATH_IMAGE029
其中，
Figure 773269DEST_PATH_IMAGE030
为定位对象i初始的协方差。
7.根据权利要求1所述的方法，其特征在于，所述基于所述修正后的一维观测值还原得到所述定位对象的修正后的三维定位坐标，包括：
Figure 870407DEST_PATH_IMAGE031
其中，
Figure 530058DEST_PATH_IMAGE032
为定位对象i在采样时刻t测量的三维定位坐标，
Figure 754366DEST_PATH_IMAGE033
为定位对象i在采样时刻t的一维观测值，
Figure 550284DEST_PATH_IMAGE034
为定位对象i在采样时刻t的修正后的一维观测值，
Figure DEST_PATH_IMAGE035
为定位对象i在采样时刻t的修正后的三维定位坐标。
8.一种井下高精度定位修正装置，其特征在于，包括：
获取模块，用于获取定位对象在井下定位场景中测量的三维定位坐标；
一维观测值生成模块，用于基于起始时刻的三维定位坐标和所述起始时刻之后的采样时刻的三维定位坐标生成所述定位对象的一维观测值，所述一维观测值表征所述定位对象相应于所述起始时刻至所述采样时刻沿井下巷道的无向图的最短距离；
运动状态方程模块，用于基于所述一维观测值构建所述定位对象的运动状态方程；
一维修正模块，用于基于卡尔曼滤波和所述运动状态方程对所述定位对象的所述一维观测值进行修正，得到修正后的一维观测值；
三维修正模块，用于基于所述修正后的一维观测值还原得到所述定位对象的修正后的三维定位坐标。
9.一种井下高精度定位修正设备，其特征在于，包括：处理器和用于存储能够在处理器上运行的计算机程序的存储器，其中，
所述处理器，用于运行计算机程序时，执行权利要求1至7任一项所述方法的步骤。
10.一种存储介质，所述存储介质上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现权利要求1至7任一项所述方法的步骤。
CN202211186364.4A 2022-09-28 2022-09-28 井下高精度定位修正方法、装置、设备及存储介质 Active CN115267667B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211186364.4A CN115267667B (zh) 2022-09-28 2022-09-28 井下高精度定位修正方法、装置、设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211186364.4A CN115267667B (zh) 2022-09-28 2022-09-28 井下高精度定位修正方法、装置、设备及存储介质

Publications (2)

Publication Number Publication Date
CN115267667A true CN115267667A (zh) 2022-11-01
CN115267667B CN115267667B (zh) 2023-04-07

Family

ID=83756930

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211186364.4A Active CN115267667B (zh) 2022-09-28 2022-09-28 井下高精度定位修正方法、装置、设备及存储介质

Country Status (1)

Country Link
CN (1) CN115267667B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115984027A (zh) * 2023-03-20 2023-04-18 长沙迪迈数码科技股份有限公司 基于uwb的井下人员聚集预警方法、装置及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101187702A (zh) * 2007-12-06 2008-05-28 上海大学 井下煤矿人员实时无线定位方法
US20090227266A1 (en) * 2008-03-04 2009-09-10 Samsung Electronics Co., Ltd. Location measurement method based on predictive filter
CN106997039A (zh) * 2016-01-22 2017-08-01 中国矿业大学(北京) 虚拟平面约束下重构一维空间的煤矿井下toa定位方法
CN107071896A (zh) * 2017-03-31 2017-08-18 中国矿业大学(北京) 一种利用非视距信号实现煤矿弯曲巷道目标定位方法
CN108761389A (zh) * 2018-05-24 2018-11-06 西安科技大学 一种抑制nlos时延误差的矿井toa目标定位方法
CN110109055A (zh) * 2019-05-23 2019-08-09 南通云之建智能科技有限公司 一种基于rssi测距的室内定位方法
CN111352066A (zh) * 2020-03-27 2020-06-30 西安震有信通科技有限公司 基于粒子滤波的定位方法、装置、计算机设备和存储介质
CN113038377A (zh) * 2021-02-25 2021-06-25 大连易飞扬科技有限公司 一种基于uwb技术c-twr的井下一维精确定位方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101187702A (zh) * 2007-12-06 2008-05-28 上海大学 井下煤矿人员实时无线定位方法
US20090227266A1 (en) * 2008-03-04 2009-09-10 Samsung Electronics Co., Ltd. Location measurement method based on predictive filter
CN106997039A (zh) * 2016-01-22 2017-08-01 中国矿业大学(北京) 虚拟平面约束下重构一维空间的煤矿井下toa定位方法
CN107071896A (zh) * 2017-03-31 2017-08-18 中国矿业大学(北京) 一种利用非视距信号实现煤矿弯曲巷道目标定位方法
CN108761389A (zh) * 2018-05-24 2018-11-06 西安科技大学 一种抑制nlos时延误差的矿井toa目标定位方法
CN110109055A (zh) * 2019-05-23 2019-08-09 南通云之建智能科技有限公司 一种基于rssi测距的室内定位方法
CN111352066A (zh) * 2020-03-27 2020-06-30 西安震有信通科技有限公司 基于粒子滤波的定位方法、装置、计算机设备和存储介质
CN113038377A (zh) * 2021-02-25 2021-06-25 大连易飞扬科技有限公司 一种基于uwb技术c-twr的井下一维精确定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
JIAN YIN LU 等: "Robot indoor location modeling and simulation based on Kalman filtering", 《EURASIP JOURNAL ON WIRELESS COMMUNICATIONS AND NETWORKING》 *
张卓然 等: "强跟踪修正SRCKF算法在单站无源跟踪中的应用", 《计算机工程》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115984027A (zh) * 2023-03-20 2023-04-18 长沙迪迈数码科技股份有限公司 基于uwb的井下人员聚集预警方法、装置及存储介质
CN115984027B (zh) * 2023-03-20 2023-06-06 长沙迪迈数码科技股份有限公司 基于uwb的井下人员聚集预警方法、装置及存储介质

Also Published As

Publication number Publication date
CN115267667B (zh) 2023-04-07

Similar Documents

Publication Publication Date Title
US10627241B2 (en) Map-centric map matching method and apparatus
US11300964B2 (en) Method and system for updating occupancy map for a robotic system
US10614324B2 (en) Method and apparatus for identifying static obstacle
US20200159637A1 (en) Index anomaly detection method and apparatus, and electronic device
CN111402339A (zh) 一种实时定位方法、装置、系统及存储介质
US20210254995A1 (en) Methods, apparatuses, systems, and storage media for storing and loading visual localization maps
CN115267667B (zh) 井下高精度定位修正方法、装置、设备及存储介质
US9910878B2 (en) Methods for processing within-distance queries
CN111678513A (zh) 一种超宽带/惯导紧耦合的室内定位装置与系统
JP2022177202A (ja) レーザーレーダーと測位装置の校正方法、機器及び自律運転車両
US9460243B2 (en) Selective importance sampling
CN115291166B (zh) 井下高精度定位方法、装置、设备及存储介质
CN112461258A (zh) 一种参数修正的方法及装置
US20220314980A1 (en) Obstacle tracking method, storage medium and unmanned driving device
CN112036030B (zh) 矿体组合约束建模方法、装置、设备及存储介质
CN114565670A (zh) 一种位姿优化方法及装置
CN112990256A (zh) 多传感器噪声计算方法及信息融合方法、存储介质及设备
US11037066B2 (en) Estimation of abnormal sensors
CN114383621B (zh) 基于网格地图的轨迹纠偏方法、电子设备和存储介质
CN113593025B (zh) 地质体模型更新方法、装置、设备及存储介质
CN116817928B (zh) 基于因子图优化的卫导/惯导列车多源融合定位的方法
CN114926549B (zh) 三维点云处理方法、装置、设备以及存储介质
CN112927350B (zh) 多域地质体模型构建方法、装置、设备及存储介质
CN115209428B (zh) 井下uwb定位基站布置方法、装置、设备及存储介质
CN113221202B (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
CP03 Change of name, title or address

Address after: 410205 10th floor, C1 building, Lugu enterprise Plaza, 27 Wenxuan Road, Yuelu District, Changsha City, Hunan Province

Patentee after: Changsha Dimai Technology Co.,Ltd.

Address before: 410205 10th floor, C1 building, Lugu enterprise Plaza, 27 Wenxuan Road, Yuelu District, Changsha City, Hunan Province

Patentee before: CHANGSHA DIGITAL MINE CO.,LTD.

CP03 Change of name, title or address