CN113189541B - 一种定位方法、装置及设备 - Google Patents
一种定位方法、装置及设备 Download PDFInfo
- Publication number
- CN113189541B CN113189541B CN202110441865.1A CN202110441865A CN113189541B CN 113189541 B CN113189541 B CN 113189541B CN 202110441865 A CN202110441865 A CN 202110441865A CN 113189541 B CN113189541 B CN 113189541B
- Authority
- CN
- China
- Prior art keywords
- position information
- mobile terminal
- base station
- representing
- time
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 82
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 30
- 238000005457 optimization Methods 0.000 claims abstract description 21
- 239000011159 matrix material Substances 0.000 claims description 91
- 230000008569 process Effects 0.000 claims description 24
- 238000004891 communication Methods 0.000 claims description 20
- 238000001914 filtration Methods 0.000 claims description 19
- 230000007704 transition Effects 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 11
- 238000005259 measurement Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 description 9
- 238000005516 engineering process Methods 0.000 description 6
- 230000003190 augmentative effect Effects 0.000 description 4
- 238000010276 construction Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000000802 evaporation-induced self-assembly Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
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/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
-
- 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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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/0009—Transmission of position information to remote stations
-
- 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/0284—Relative positioning
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Mobile Radio Communication Systems (AREA)
- Navigation (AREA)
Abstract
本发明实施例提供了一种定位方法、装置及设备,涉及信息技术领域,其中,定位方法可以包括:获取移动终端的初始位置信息,以及各个基站的位置信息;根据移动终端的初始位置信息和各个基站的位置信息,计算移动终端分别与各个基站的距离;基于各个基站的位置信息以及移动终端分别与各个基站的距离,计算移动终端的估算位置信息;通过迭代优化算法,对估算位置信息进行优化,得到优化后的位置信息。通过本发明实施例提供的定位方法、装置及设备,能够提高定位精度。
Description
技术领域
本发明涉及通信技术领域,特别是涉及一种定位方法、装置及设备。
背景技术
随着无线通信技术的日益发展,人们对智能化生活的需求也在不断提升,基于位置的服务(Location Based Services,LBS)也逐渐成为人们生活中重要的组成部分,例如目标定位、导航追踪、智慧城市、应急救援等领域。随着第五代通信网络(5G)这样一个万物互联时代的到来,高精度定位技术在车联网、智能家居、工业4.0、虚拟现实(VirtualReality,VR)等场景下的应用将更加广泛。
但室内环境相比较于室外环境更加复杂,信号传播存在多径、非视距(Non-LineOf Sight,NLOS)等多种干扰。其中,建筑物对信号的反射和折射引起的NLOS误差是最主要的误差来源,严重影响了定位的精度和可靠性。NLOS的基站相较于基站(Line Of Sight,LOS)基站将对到达时间差(Time Difference Of Arrival,TDOA)结果造成较大误差,因此会导致室内定位误差增大。因此,如何提高定位精度对于提高位置服务的可靠性以及基于位置服务的应用开发和普及具有十分重大的意义。
发明内容
本发明实施例的目的在于提供一种定位方法、装置及设备,以提高定位精度。具体技术方案如下:
第一方面,本发明实施例提供了一种定位方法,包括:
获取移动终端的初始位置信息,以及各个基站的位置信息;
根据所述移动终端的初始位置信息和各个基站的位置信息,计算所述移动终端分别与各个基站的距离;
基于各个基站的位置信息以及所述移动终端分别与各个基站的距离,计算所述移动终端的估算位置信息;
通过迭代优化算法,对所述估算位置信息进行优化,得到优化后的位置信息。
可选的,所述根据所述移动终端的初始位置信息和各个基站的位置信息,计算所述移动终端分别与各个基站的距离,包括:
针对各个序列组合,构建Cayley-Menger行列式,其中,序列组合是所述移动终端与所有n个基站中的n-1个基站排列组合得到的;
针对各个序列组合对应的Cayley-Menger行列式,计算所述移动终端与相关基站的距离,所述相关基站为所述序列组合所包括的基站。
可选的,所述基于各个基站的位置信息以及所述移动终端分别与各个基站的距离,计算所述移动终端的估算位置信息,包括:
针对每个序列组合,对所述序列组合得到的所述移动终端与相关基站的距离,通过公式:计算所述序列组合对应的残差平方和;并计算所述残差平方和的最小值,得到所述序列组合对应的估算值;
其中,Res(x,Si1)为序列组合Si1对应的残差平方和,i表示序列组合Sii对应的第i个相关基站,di表示移动终端x与第i个相关基站的距离,xi表示第i个相关基站的位置信息;
对每个序列组合得到所述估算值,通过公式进行加权,得到所述移动终端的估算位置信息;
其中,X为所述估算位置信息,为每个序列组合残差平方和最小时得到的位置估算值。
可选的,所述通过迭代优化算法,对所述估算位置信息进行优化,得到优化后的位置信息,包括:
通过公式xk1+1=xk1-(HTH)-1HTG,对所述估算位置信息进行优化,得到优化后的位置信息;
其中,xk1+1为第k1+1次迭代得到的位置信息,xk1表示第k1次迭代得到的位置信息, gk表示梯度向量,/>λ表示梯度下降学习率因子,I表示对角线值均为1其他位置为0的矩阵。
可选的,所述优化后的位置信息为所述移动终端在前一时刻的位置信息;
在所述通过迭代优化算法,对所述估算位置信息进行优化,得到优化后的位置信息之后,所述方法还包括:
基于所述优化后的位置信息,通过卡尔曼滤波算法,预测所述移动终端当前时刻的位置信息;
对预测的当前时刻的位置信息进行校正,得到校正后的位置信息。
可选的,所述基于所述优化后的位置信息,通过卡尔曼滤波算法,预测所述移动终端当前时刻的位置信息,包括:
通过公式:预测所述移动终端当前时刻的位置信息;
其中,表示k-1时刻位置信息,/>表示预测的当前时刻的位置信息,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>表示预测的第k时刻的后验估计误差的均方误差矩阵,Pk-1表示第k-1时刻的后验估计误差的均方误差矩阵,Q表示各个过程噪声向量的协方差矩阵;
所述对预测的当前时刻的位置信息进行校正,得到校正后的位置信息,包括:
通过公式: 对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,yk为观测值,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Q表示各个过程噪声向量的协方差矩阵;R表示各个测量噪声向量的协方差矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵;Pk表示第k时刻的后验估计误差的均方误差矩阵;/>表示第k时刻的先验估计误差的均方误差矩阵。
可选的,所述基于所述优化后的位置信息,通过卡尔曼滤波算法,预测所述移动终端当前时刻的位置信息,包括:
通过公式预测所述移动终端当前时刻的位置信息;
其中,表示预测的当前时刻的位置信息,/>表示k-1时刻校正后位置信息的横坐标,/>表示k-1时刻校正后位置信息的纵坐标,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>ΔXarc=Xk-Xk-1,ΔZarc=Zk-Zk-1,ΔXpdr=(stepk-stepk-1)*steplk*cosθ,ΔYpdr=(stepk-stepk-1)*steplk*sinθ,Xk、Zk表示k时刻ARCore输出的转换坐标值,stepk、steplk表示k时刻的步数与步长,stepk-1表示k-1时刻的步数,state表示ARCore状态,x、y表示到达时间差TDOA下的坐标值,θ表示每次移动方向相对于正北方向的夹角;
所述对预测的当前时刻的位置信息进行校正,得到校正后的位置信息,包括:
通过公式:对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵。
第二方面,本发明实施例提供了一种定位装置,包括:
获取模块,用于获取移动终端的初始位置信息,以及各个基站的位置信息;
计算模块,用于根据所述移动终端的初始位置信息和各个基站的位置信息,计算所述移动终端分别与各个基站的距离;基于各个基站的位置信息以及所述移动终端分别与各个基站的距离,计算所述移动终端的估算位置信息;
优化模块,用于通过迭代优化算法,对所述估算位置信息进行优化,得到优化后的位置信息。
可选的,所述计算模块,具体用于针对各个序列组合,构建Cayley-Menger行列式,其中,其中,序列组合是所述移动终端与所有n个基站中的n-1个基站排列组合得到的;
针对各个序列组合对应的Cayley-Menger行列式,计算所述移动终端与相关基站的距离,所述相关基站为所述序列组合所包括的基站。
可选的,计算模块,具体用于针对每个序列组合,对序列组合得到的移动终端与相关基站的距离,通过公式:计算序列组合对应的残差平方和;并计算残差平方和的最小值,得到序列组合对应的估算值;
其中,Res(x,Si1)为序列组合Si1对应的残差平方和,i表示序列组合Si1对应的第i个相关基站,di表示移动终端x与第i个相关基站的距离,xi表示第i个相关基站的位置信息;
对每个序列组合得到估算值,通过公式进行加权,得到移动终端的估算位置信息;
其中,X为估算位置信息,为每个序列组合残差平方和最小时得到的位置估算值。
可选的,优化模块,具体用于通过公式xk1+1=xk1-(HTH)-1HTG,对估算位置信息进行优化,得到优化后的位置信息;
其中,xk1+1为第k1+1次迭代得到的位置信息,xk1表示第k1次迭代得到的位置信息, gk表示梯度向量,/>λ表梯度下降学习率因子,I表示对角线值均为1其他位置为0的矩阵。
可选的,优化后的位置信息为移动终端在前一时刻的位置信息;
所述装置还包括:
预测模块,用于在通过迭代优化算法,对估算位置信息进行优化,得到优化后的位置信息之后,基于优化后的位置信息,通过卡尔曼滤波算法,预测移动终端当前时刻的位置信息;
校正模块,用于对预测的当前时刻的位置信息进行校正,得到校正后的位置信息。
可选的,预测模块,具体用于通过公式: 预测移动终端当前时刻的位置信息;
其中,表示k-1时刻位置信息,/>表示预测的当前时刻的位置信息,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>表示预测的第k时刻的后验估计误差的均方误差矩阵,Pk-1表示第k-1时刻的后验估计误差的均方误差矩阵,Q表示各个过程噪声向量的协方差矩阵;
校正模块,具体用于通过公式: 对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,yk为观测值,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Q表示各个过程噪声向量的协方差矩阵;R表示各个测量噪声向量的协方差矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵;Pk表示第k时刻的后验估计误差的均方误差矩阵;/>表示第k时刻的先验估计误差的均方误差矩阵。
可选的,预测模块,具体用于通过公式预测移动终端当前时刻的位置信息;
其中,表示预测的当前时刻的位置信息,/>表示k-1时刻校正后位置信息的横坐标,/>表示k-1时刻校正后位置信息的纵坐标,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>ΔXarc=Xk-Xk-1,ΔZarc=Zk-Zk-1,ΔXpdr=(stepk-stepk-1)*steplk*cosθ,ΔYpdr=(stepk-stepk-1)*steplk*sinθ,Xk、Zk表示k时刻ARCore输出的转换坐标值,stepk、steplk表示k时刻的步数与步长,stepk-1表示k-1时刻的步数,state表示ARCore状态,x、y表示到达时间差TDOA下的坐标值,θ表示每次移动方向相对于正北方向的夹角;
可选的,校正模块,具体用于通过公式:
对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵。
本发明实施例还提供一种定位设备,包括处理器、通信接口、存储器和通信总线,其中,处理器,通信接口,存储器通过通信总线完成相互间的通信;
存储器,用于存放计算机程序;
处理器,用于执行存储器上所存放的程序时,实现第一方面所述的方法步骤。
本发明实施例还提供了一种计算机可读存储介质,该计算机可读存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现上述第一方面所述的方法步骤。
本发明实施例还提供了一种包含指令的计算机程序产品,当其在计算机上运行时,使得计算机执行上述第一方面所述的方法步骤。
本发明实施例有益效果:
本发明实施例提供的定位方法、装置及设备,通过获取移动终端的初始位置信息,以及各个基站的位置信息;根据移动终端的初始位置信息和各个基站的位置信息,计算移动终端分别与各个基站的距离;基于各个基站的位置信息以及移动终端分别与各个基站的距离,计算移动终端的估算位置信息;通过迭代优化算法,对估算位置信息进行优化,得到优化后的位置信息。能够先对获取的初始位置信息,基于各个基站的位置信息以及移动终端分别与各个基站的距离,计算移动终端的估算位置信息,然后,对得到的估算位置信息进一步迭代优化,得到优化后的位置信息,如此,能够使得确定的移动终端的位置信息更加准确,提高对移动终端定位的精度。
当然,实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,还可以根据这些附图获得其他的实施例。
图1为本发明实施例提供的定位方法的一种流程图;
图2为本发明实施例提供的定位方法的另一种流程图;
图3为本发明实施例中增强现实构建平台ARCore坐标系示意图;
图4为本发明实施例提供的定位装置的一种结构示意图;
图5为本发明实施例提供的定位装置的另一种结构示意图;
图6为本发明实施例提供的定位设备的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员基于本申请所获得的所有其他实施例,都属于本发明保护的范围。
针对基站的定位大多是通过测量信号到达时间(Time Of Arrival,TOA),到达时间差(Time Difference Of Arrival,TDOA),接收信号强度(Received Signal StrengthIndicator,RSSI)、到达角(Angle Of Arrival,AOA)以及这几类参量的混合参数(如TOA/AOA,TDOA/AOA)等特征参数来估计目标位置。基于TOA的定位算法通过信号到达时间计算节点间距离,但是需要严格的时间同步,增加了硬件实现的成本和难度。为了降低对时间同步的要求,广泛使用TDOA计算距离信息。且当目标处于移动状态的时候,如何持续的对目标跟踪,将其过去的信息与现在的信息有效联系起来就需要新的算法来实现。
本发明实施例提供了一种定位方法,具体地可以是室内环境中的定位方法,以提高对定位精度,另外,还可以实现持续地目标跟踪,也可以理解为目标在动态过程中的定位。
下面对本发明实施例提供的定位方法进行详细说明。
本发明实施例提供的定位方法的执行主体可以电子设备,具体地电子设备可以包括服务器、终端,等等。
本发明实施例提供了一种定位方法,可以包括:
获取移动终端的初始位置信息,以及各个基站的位置信息;
根据移动终端的初始位置信息和各个基站的位置信息,计算移动终端分别与各个基站的距离;
基于各个基站的位置信息以及移动终端分别与各个基站的距离,计算移动终端的估算位置信息;
通过迭代优化算法,对估算位置信息进行优化,得到优化后的位置信息。
本发明实施例中,通过获取移动终端的初始位置信息,以及各个基站的位置信息;根据移动终端的初始位置信息和各个基站的位置信息,计算移动终端分别与各个基站的距离;基于各个基站的位置信息以及移动终端分别与各个基站的距离,计算移动终端的估算位置信息;通过迭代优化算法,对估算位置信息进行优化,得到优化后的位置信息。能够先对获取的初始位置信息,基于各个基站的位置信息以及移动终端分别与各个基站的距离,计算移动终端的估算位置信息,然后,对得到的估算位置信息进一步迭代优化,得到优化后的位置信息,如此,能够使得确定的移动终端的位置信息更加准确,提高对移动终端定位的精度。
图1为本发明实施例提供的定位方法的一种流程图,参见图1,本发明实施例提供的定位方法,可以包括:
S101,获取移动终端的初始位置信息,以及各个基站的位置信息。
位置信息可以通过坐标来表示。例如,移动终端的坐标为x(x,y),各个基站的坐标为xi(xi,yi),i=1,…,N,N表示基站的数量。
S102,根据移动终端的初始位置信息和各个基站的位置信息,计算移动终端分别与各个基站的距离。
一种可实现方式中,移动终端到各个基站的距离ri可以表示为
S103,基于各个基站的位置信息以及移动终端分别与各个基站的距离,计算移动终端的估算位置信息。
一种可实现方式中,可以针对各个序列组合,构建Cayley-Menger行列式,其中,序列组合是移动终端与所有n个基站中的n-1个基站排列组合得到的;针对各个序列组合对应的Cayley-Menger行列式,计算移动终端与相关基站的距离,相关基站为序列组合所包括的基站。
针对每个序列组合,对序列组合得到的移动终端与相关基站的距离,通过公式:计算序列组合对应的残差平方和;并计算残差平方和的最小值,得到序列组合对应的估算值;
其中,Res(x,Si1)为序列组合Si1对应的残差平方和,i表示序列组合Sii对应的第i个相关基站,di表示移动终端x与第i个相关基站的距离,xi表示第i个相关基站的位置信息。di针对序列组合,通过构建的Cayley-Menger行列式,所计算的移动终端与相关基站的距离。
对每个序列组合得到估算值,通过公式进行加权,得到移动终端的估算位置信息。
其中,X为估算位置信息,为每个序列组合残差平方和最小时得到的位置估算值。
具体地,假设参考基站为x1(x1,y1),则
di,1=di-d1=ri,1+nLosi,1+nNLosi,1=cti,1,i=2,…,N
其中,ri,1=ri-r1,nLosi,1为系统测量误差,服从均值为0,方差为σ2的高斯分布;nNLosi,1为由NLOS引起的附加时延误差,其均值为μNLosi,1,方差为ni,1与nNLosi,1之间是相互独立的。
Cayley-Menger行列式在距离几何中被广泛使用,首先需要建立Cayley-Menger矩阵,它由两个n点序列(p1,p2,…,pn)与(q1,q2,…,qn)∈Rm构成,如式:
d(p1,q1)表示p1与q1两点的欧氏距离,当上述两个序列相同时,Cayley-Menger行列式可以定义为D(p1,p2,…,pn)=|M({p1,p2,…,pn})|。在m维空间中由p1,p2,…,pn组成的Cayley-Menger矩阵的秩至多为m+1,由矩阵性质可以得出在二维空间中D(p1,p2,…,pn)=0。
例如,基站x1,x2,x3,x4与终端x,则可以有如下序列组合:S1(x,x1,x2,x3)、S2(x,x1,x2,x4)、S3(x,x1,x3,x4)、S4(x,x2,x3,x4)。
对于序列组合(x,x1,x2,x3),移动终端与该序列组合中各个基站(即基站x1,x2,x3,也可以理解为序列组合对应的相关基站)的距离可以分别表示为d1、d2=d1+c·t2,1、d3=d1+c·t3,1。根据式可得 (a、b、c为系数),可求得d1,进而求得d2与d3。
同理,可对序列组合(x,x1,x2,x4)、(x,x1,x3,x4)、(x,x2,x3,x4)进行计算。对于(x,x1,x2,x4)可以得到移动终端分别与基站x1,x2,x4的距离,对于(x,x1,x3,x4)可以得到移动终端分别与x1,x3,x4的距离,对于(x,x2,x3,x4)可以得到移动终端分别与x2,x3,x4的距离。
对于每一个序列组合的残差平方和Res(x,Si),则移动终端位置估计为:
通过最小二乘法可以求得最后对每一个组合求得的x的位置估计与残差平方和按照式进行加权,可以得到移动终端的位置坐标,也即上述所说的移动终端的估算位置信息。
S104,通过迭代优化算法,对估算位置信息进行优化,得到优化后的位置信息。
一种可实现方式中,可以通过公式xk1+1=xk1-(HTH)-1HTG,对估算位置信息进行优化,得到优化后的位置信息;
其中,xk1+1为第k1+1次迭代得到的位置信息,xk1表示第k1次迭代得到的位置信息, gk表示梯度向量,/>λ表示梯度下降学习率因子,I表示对角线值均为1其他位置为0的矩阵。
对一维多元函数f1(x)在xk1进行泰勒级数展开,忽略二次项以上的高阶项可以得到:
其中,梯度向量Hessian矩阵/>求f1(x)的极小值可以通过令其导数为0:
所以每次迭代过程中xk1+1=xk1-Hk -1gk,但是当Hessian构造矩阵非正定导致不能确定搜索方向,引起迭代的发散。本发明实施例中可以引入梯度思想,对于超定方程式可以通过最小二乘思想转化为式对Δx求导求解:
HΔx+G=0
‖HΔx+G‖2=(HΔx+G)T(HΔx+G)=0
其中,Δx=xk+1-xk,最后可求得迭代过程如式所示:
xk+1=xk-(HTH)-1HTG
本发明实施例中,能够先对获取的初始位置信息,基于各个基站的位置信息以及移动终端分别与各个基站的距离,计算移动终端的估算位置信息,然后,对得到的估算位置信息进一步迭代优化,得到优化后的位置信息,如此,能够使得确定的移动终端的位置信息更加准确,提高对移动终端定位的精度。本发明根据距离几何关系的性质,提出一种基于Cayley-Menger行列式的最小残差加权方法来抑制NLOS的影响,使用Cayley-Menger行列式得到距离估计,再进行残差加权来抑制NLOS误差影响得到初始位置,最后通过修正牛顿迭代以解决Hessian构造矩阵非正定和奇异导致算法失效的问题得到最终的位置估计值,能够在在未知复杂环境中,抑制NLOS影响带来的定位误差,提高定位精度。
同时,本发明实施例中采用Cayley-Menger行列式的方式求解,行列式的计算相比如一些凸优化求解以及神经网络算法的计算复杂度低,求解速度快,更能满足在移动过程中定位的时效性,同时对多个结果进行残差加权的方式来克服NLOS误差带来的影响,以提高定位精度,能够实现低复杂度且高效的室内定位。
一种可选的实施例中,优化后的位置信息为移动终端在前一时刻的位置信息;
在S104之后,如图2所示,还可以包括:
S105,基于优化后的位置信息,通过卡尔曼滤波算法,预测移动终端当前时刻的位置信息。
S106,对预测的当前时刻的位置信息进行校正,得到校正后的位置信息。
一种可实现方式中,通过公式:预测移动终端当前时刻的位置信息;
其中,表示k-1时刻位置信息,也可以理解为第k-1时刻卡尔曼滤波对系统状态xk-1的最优估计值,/>表示预测的当前时刻的位置信息,也可以理解为第k时刻卡尔曼滤波的先验估计值,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>表示预测的第k时刻的后验估计误差的均方误差矩阵,Pk-1表示第k-1时刻的后验估计误差的均方误差矩阵,Q表示各个过程噪声向量的协方差矩阵;
对预测的当前时刻的位置信息进行校正,得到校正后的位置信息,包括:
通过公式: 对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,yk为观测值,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Q表示各个过程噪声向量的协方差矩阵;R表示各个测量噪声向量的协方差矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵;Pk表示第k时刻的后验估计误差的均方误差矩阵;/>表示第k时刻的先验估计误差的均方误差矩阵。
通过上述实施例得到移动终端一时刻时的位置信息,为了实现在移动终端移动过程对移动终端进行跟踪,本发明实施例中,可以理解为上述实施例得到的是初始定位点,之后可以通过自适应的卡尔曼滤波方法对目标进行动态跟踪,即可以理解为在移动终端处于移动状态的时候,持续的对移动终端进行跟踪定位。
对目标进行动态定位和跟踪,需要根据运动状态建立状态转换方程,离散的运动状态转换方程如下所示:
xk=Axk-1+wk-1
其中,状态向量xk=[xkykΔxkΔyk],xk、yk分别表示k时刻在x轴和y轴方向上的位置坐标,Δxk、Δyk分别表示k时刻在x轴和y轴方向上的位置变化量。状态转移矩阵过程噪声向量wk为均值为0的正态高斯白噪声,其协方差矩阵为Q=cov(wk)=E(wkwk T)。
运动的观测方程可以表示为:
yk=Cxk+vk
测量关系矩阵测量噪声向量vk为均值为0的正态高斯白噪声,其协方差矩阵为R=cov(vk)=E(vkvk T)。
卡尔曼滤波的预测过程如下:
卡尔曼滤波的校正过程如下:
ARCore(增强现实构建平台)可以使用不同的应用程序接口让移动终端如手机感知其环境,可以跟踪其相对于现实世界环境中的位置。步行者航位推算技术(PedestrianDead Reckoning,PDR)是通过惯性测量单元(Inertial Measurement Unit,IMU)感知物体在移动过程中的加速度、角速度、磁力和压力等数据,并使用这些数据对物体移动过程中的步数、步长以及航向角进行测量与统计。
手机的方向并不总是指向正北方向,ARCore在启动时就将当前手机所指向的方向定义为x轴正方向,并以此建立右手坐标系,中指方向定义为z轴的正方向,y轴方向为食指所指方向,原点为手机位置。定义角度为初始方向与正北方向的夹角,此夹角是通过手机传感器获取的方向角。
ARCore坐标系确立之后需要将其映射到XOY(NOE,X(E),Y(N))坐标系中,xoz坐标系在XOY坐标系中如图3所示,映射关系可以使用坐标旋转公式确定,映射公式如式:
Xinit、Yinit表示初始位置坐标。
增强现实技术的ARCore并不能持续对当前环境进行特征点捕捉,当所处环境不足以支持特征点提取进行位置感知时ARCore将失效,ARCore状态有效性可以表示为公式: xar(k)、zar(k)表示k时刻ARCore的输出值。
本发明实施例中可以通过第k2-1次的卡尔曼滤波最优估计值,并结合第k2、k2-1次的ARCore或者PDR输出值对第k2次的卡尔曼滤波预测值矩阵和观测值矩阵进行重构。
可以通过公式预测移动终端当前时刻的位置信息;
其中,表示预测的当前时刻的位置信息,也可以理解为第k时刻卡尔曼滤波的先验估计值,/>表示k-1时刻校正后位置信息的横坐标,/>表示k-1时刻校正后位置信息的纵坐标,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>ΔX′ar、ΔZ′ar表示当前时刻与上一时刻的位置变化量,ΔXarc=Xk-Xk-1,ΔZarc=Zk-Zk-1,ΔXpdr=(stepk-stepk-1)*steplk*cos,ΔYpdr=(stepk-stepk-1)*steplk*sinθ,Xk、Zk表示k时刻ARCore输出的转换坐标值,stepk、steplk表示k时刻的步数与步长,stepk-1表示k-1时刻的步数,state表示ARCore状态,x、y表示到达时间差TDOA下的坐标值,θ表示每次移动方向相对于正北方向的夹角;
对预测的当前时刻的位置信息进行校正,得到校正后的位置信息,包括:
通过公式:对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵。
另外,可以根据每n次的测量输出值倾斜角的方差决定是否对ARCore的初始方向角进行纠正,避免其进行特征点分析偏差导致输出坐标值发生偏移。过程如下:
对跟踪过程中n次自适应卡尔曼滤波最优估计测量的n-1段求倾斜角的方差D(θ),根据方差大小及n次ARCore行进距离决定是否对初始方向角进行校正,/>的校正如式:/>
其中,距离/> 方差D和距离d需要根据实际测量进行确定,/>表示第n次卡尔曼滤波的输出纵坐标值,/>表示第1次卡尔曼滤波的输出纵坐标值,/>表示第n次卡尔曼滤波的输出横坐标值,/>表示第1次卡尔曼滤波的输出横坐标值,Zn表示第n次基于ARCore输出纵坐标值,Z1表示第1次基于ARCore输出纵坐标值,Xn表示第n次基于ARCore输出横坐标值,X1表示第1次基于ARCore输出横坐标值。
本发明实施例提供了基于视觉辅助的时差定位和目标跟踪的方式,可以在未知的复杂环境中,减小NLOS误差对定位精度的影响,提升了视距传输下的室内定位和目标跟踪精度。结合增强现实的视觉辅助和步行者航位推算技术,提出重构卡尔曼滤波的观测值和预测值的自适应算法对目标进行动态定位和跟踪。
对应于上述实施例提供的定位方法,本发明实施例还提供了一种定位装置,如图4所示,可以包括:
获取模块401,用于获取移动终端的初始位置信息,以及各个基站的位置信息;
计算模块402,用于根据移动终端的初始位置信息和各个基站的位置信息,计算移动终端分别与各个基站的距离;基于各个基站的位置信息以及移动终端分别与各个基站的距离,计算移动终端的估算位置信息;
优化模块403,用于通过迭代优化算法,对估算位置信息进行优化,得到优化后的位置信息。
可选的,计算模块402,具体用于针对各个序列组合,构建Cayley-Menger行列式,其中,其中,序列组合是移动终端与所有n个基站中的n-1个基站排列组合得到的;针对各个序列组合对应的Cayley-Menger行列式,计算移动终端与相关基站的距离,相关基站为序列组合所包括的基站。
可选的,计算模块402,具体用于针对每个序列组合,对序列组合得到的移动终端与相关基站的距离,通过公式:计算序列组合对应的残差平方和;并计算残差平方和的最小值,得到序列组合对应的估算值;
其中,Res(x,Si1)为序列组合Si1对应的残差平方和,i表示序列组合Si1对应的第i个相关基站,di表示移动终端x与第i个相关基站的距离,xi表示第i个相关基站的位置信息;
对每个序列组合得到估算值,通过公式进行加权,得到移动终端的估算位置信息;
其中,X为估算位置信息,为每个序列组合残差平方和最小时得到的位置估算值。
可选的,优化模块403,具体用于通过公式xk1+1=xk1-(HTH)-1HTG,对估算位置信息进行优化,得到优化后的位置信息;
其中,xk1+1为第k1+1次迭代得到的位置信息,xk1表示第k1次迭代得到的位置信息, gk表示梯度向量,/>λ表示梯度下降学习率因子,I表示表示对角线值均为1其他位置为0的矩阵。
可选的,优化后的位置信息为移动终端在前一时刻的位置信息;
如图5所示,装置还包括:
预测模块501,用于在通过迭代优化算法,对估算位置信息进行优化,得到优化后的位置信息之后,基于优化后的位置信息,通过卡尔曼滤波算法,预测移动终端当前时刻的位置信息;
校正模块502,用于对预测的当前时刻的位置信息进行校正,得到校正后的位置信息。
可选的,预测模块501,具体用于通过公式: 预测移动终端当前时刻的位置信息;
其中,表示k-1时刻位置信息,/>表示预测的当前时刻的位置信息,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>表示预测的第k时刻的后验估计误差的均方误差矩阵,Pk-1表示第k-1时刻的后验估计误差的均方误差矩阵,Q表示各个过程噪声向量的协方差矩阵;
校正模块502,具体用于通过公式: 对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,yk为观测值,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Q表示各个过程噪声向量的协方差矩阵;R表示各个测量噪声向量的协方差矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵;Pk表示第k时刻的后验估计误差的均方误差矩阵;/>表示第k时刻的先验估计误差的均方误差矩阵。
可选的,预测模块501,具体用于通过公式预测移动终端当前时刻的位置信息;
其中,表示预测的当前时刻的位置信息,/>表示表示k-1时刻校正后位置信息的横坐标,/>表示表示k-1时刻校正后位置信息的纵坐标,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>
ΔXarc=Xk-Xk-1,ΔZarc=Zk-Zk-1,ΔXpdr=(stepk-stepk-1)*steplk*cosθ,ΔYpdr=(stepk-stepk-1)*steplk*sinθ,Xk、Zk表示k时刻ARCore输出的转换坐标值,stepk、steplk表示k时刻的步数与步长,stepk-1表示k-1时刻的步数,state表示ARCore状态,x、y表示到达时间差TDOA下的坐标值,θ表示每次移动方向相对于正北方向的夹角;
可选的,校正模块502,具体用于通过公式:
对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵。
本发明实施例提供的定位装置是应用上述定位方法的装置,则上述定位方法的所有实施例均适用于该装置,且均能达到相同或相似的有益效果。
本发明实施例还提供了一种定位设备,如图6所示,包括处理器601、通信接口602、存储器603和通信总线604,其中,处理器601,通信接口602,存储器603通过通信总线604完成相互间的通信。
存储器603,用于存放计算机程序;
处理器601,用于执行存储器603上所存放的程序时,实现上述实施例中定位方法的方法步骤。
上述定位设备提到的通信总线可以是外设部件互连标准(Peripheral ComponentInterconnect,PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture,EISA)总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示,图中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
通信接口用于上述定位设备与其他设备之间的通信。
存储器可以包括随机存取存储器(Random Access Memory,RAM),也可以包括非易失性存储器(Non-Volatile Memory,NVM),例如至少一个磁盘存储器。可选的,存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(Network Processor,NP)等;还可以是数字信号处理器(Digital SignalProcessor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
在本发明提供的又一实施例中,还提供了一种计算机可读存储介质,该计算机可读存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现上述实施例中定位方法的方法步骤。
在本发明提供的又一实施例中,还提供了一种包含指令的计算机程序产品,当其在计算机上运行时,使得计算机执行上述实施例中定位方法的方法步骤。
在上述实施例中,可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时,可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时,全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中,或者从一个计算机可读存储介质向另一个计算机可读存储介质传输,例如,所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质,(例如,软盘、硬盘、磁带)、光介质(例如,DVD)、或者半导体介质(例如固态硬盘Solid State Disk(SSD))等。
需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述,各个实施例之间相同相似的部分互相参见即可,每个实施例重点说明的都是与其他实施例的不同之处。尤其,对于装置、设备、计算机可读存储介质以及计算机程序产品实施例而言,由于其基本相似于方法实施例,所以描述的比较简单,相关之处参见方法实施例的部分说明即可。
以上所述仅为本发明的较佳实施例,并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等,均包含在本发明的保护范围内。
Claims (7)
1.一种定位方法,其特征在于,包括:
获取移动终端的初始位置信息,以及各个基站的位置信息;
根据所述移动终端的初始位置信息和各个基站的位置信息,计算所述移动终端分别与各个基站的距离;
基于各个基站的位置信息以及所述移动终端分别与各个基站的距离,计算所述移动终端的估算位置信息;
通过迭代优化算法,对所述估算位置信息进行优化,得到优化后的位置信息;
根据所述移动终端的初始位置信息和各个基站的位置信息,计算所述移动终端分别与各个基站的距离,包括:
针对各个序列组合,构建Cayley-Menger行列式,其中,序列组合是所述移动终端与所有n个基站中的n-1个基站排列组合得到的;
针对各个序列组合对应的Cayley-Menger行列式,计算所述移动终端与相关基站的距离,所述相关基站为所述序列组合所包括的基站;
所述基于各个基站的位置信息以及所述移动终端分别与各个基站的距离,计算所述移动终端的估算位置信息,包括:
针对每个序列组合,对所述序列组合得到的所述移动终端与相关基站的距离,通过公式:计算所述序列组合对应的残差平方和;并计算所述残差平方和的最小值,得到所述序列组合对应的估算值;
其中,Res(x,Si1)为序列组合Si1对应的残差平方和,i表示序列组合Si1对应的第i个相关基站,di表示移动终端x与第i个相关基站的距离,xi表示第i个相关基站的位置信息;
对每个序列组合得到所述估算值,通过公式进行加权,得到所述移动终端的估算位置信息;
其中,X为所述估算位置信息,为每个序列组合残差平方和最小时得到的位置估算值。
2.根据权利要求1所述的方法,其特征在于,所述通过迭代优化算法,对所述估算位置信息进行优化,得到优化后的位置信息,包括:
通过公式xk1+1=xk1-(HTH)-1HTG,对所述估算位置信息进行优化,得到优化后的位置信息;
其中,xk1+1为第k1+1次迭代得到的位置信息,xk1表示第k1次迭代得到的位置信息, gk表示梯度向量,/>λ表示梯度下降学习率因子,I表示对角线值均为1其他位置为0的矩阵。
3.根据权利要求1所述的方法,其特征在于,所述优化后的位置信息为所述移动终端在前一时刻的位置信息;
在所述通过迭代优化算法,对所述估算位置信息进行优化,得到优化后的位置信息之后,所述方法还包括:
基于所述优化后的位置信息,通过卡尔曼滤波算法,预测所述移动终端当前时刻的位置信息;
对预测的当前时刻的位置信息进行校正,得到校正后的位置信息。
4.根据权利要求3所述的方法,其特征在于,所述基于所述优化后的位置信息,通过卡尔曼滤波算法,预测所述移动终端当前时刻的位置信息,包括:
通过公式:预测所述移动终端当前时刻的位置信息;
其中,表示k-1时刻位置信息,/>表示预测的当前时刻的位置信息,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>表示预测的第k时刻的后验估计误差的均方误差矩阵,Pk-1表示第k-1时刻的后验估计误差的均方误差矩阵,Q表示各个过程噪声向量的协方差矩阵;
所述对预测的当前时刻的位置信息进行校正,得到校正后的位置信息,包括:
通过公式: 对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,yk为观测值,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Q表示各个过程噪声向量的协方差矩阵;R表示各个测量噪声向量的协方差矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵;Pk表示第k时刻的后验估计误差的均方误差矩阵;/>表示第k时刻的先验估计误差的均方误差矩阵。
5.根据权利要求3所述的方法,其特征在于,所述基于所述优化后的位置信息,通过卡尔曼滤波算法,预测所述移动终端当前时刻的位置信息,包括:
通过公式预测所述移动终端当前时刻的位置信息;
其中,表示预测的当前时刻的位置信息,/>表示k-1时刻校正后位置信息的横坐标,/>表示k-1时刻校正后位置信息的纵坐标,A表示从第k-1时刻到第k时刻的状态转移矩阵;B表示在第k-1时刻系统输入量与系统状态之间的关系矩阵,uk-1表示系统的输入向量,/>ΔX′ar、ΔZ′ar表示当前时刻与上一时刻的位置变化量,ΔXarc=Xk-Xk-1,ΔZarc=Zk-Zk-1,ΔXpdr=(stepk-stepk-1)*steplk*cosθ,ΔYpdr=(stepk-stepk-′)*steplk*sinθ,Xk、Zk表示k时刻ARCore输出的转换坐标值,stepk、steplk表示k时刻的步数与步长,stepk-1表示k-1时刻的步数,state表示ARCore状态,x、y表示到达时间差TDOA下的坐标值,θ表示每次移动方向相对于正北方向的夹角;
所述对预测的当前时刻的位置信息进行校正,得到校正后的位置信息,包括:
通过公式:对预测的当前时刻的位置信息进行校正,得到校正后的位置信息;
其中,表示校正后的位置信息,C表示观测值与系统状态之间的关系矩阵;Kk表示第k时刻的卡尔曼滤波增益系数矩阵。
6.一种定位装置,其特征在于,包括:
获取模块,用于获取移动终端的初始位置信息,以及各个基站的位置信息;
计算模块,用于根据所述移动终端的初始位置信息和各个基站的位置信息,计算所述移动终端分别与各个基站的距离;基于各个基站的位置信息以及所述移动终端分别与各个基站的距离,计算所述移动终端的估算位置信息;
优化模块,用于通过迭代优化算法,对所述估算位置信息进行优化,得到优化后的位置信息;
所述计算模块,具体用于针对各个序列组合,构建Cayley-Menger行列式,其中,其中,序列组合是所述移动终端与所有n个基站中的n-1个基站排列组合得到的;针对各个序列组合对应的Cayley-Menger行列式,计算所述移动终端与相关基站的距离,所述相关基站为所述序列组合所包括的基站;
所述基于各个基站的位置信息以及所述移动终端分别与各个基站的距离,计算所述移动终端的估算位置信息,包括:
针对每个序列组合,对所述序列组合得到的所述移动终端与相关基站的距离,通过公式:计算所述序列组合对应的残差平方和;并计算所述残差平方和的最小值,得到所述序列组合对应的估算值;
其中,Res(x,Si1)为序列组合Si1对应的残差平方和,i表示序列组合Si1对应的第i个相关基站,di表示移动终端x与第i个相关基站的距离,xi表示第i个相关基站的位置信息;
对每个序列组合得到所述估算值,通过公式进行加权,得到所述移动终端的估算位置信息;
其中,X为所述估算位置信息,为每个序列组合残差平方和最小时得到的位置估算值。
7.一种定位设备,其特征在于,包括处理器、通信接口、存储器和通信总线,其中,处理器,通信接口,存储器通过通信总线完成相互间的通信;
存储器,用于存放计算机程序;
处理器,用于执行存储器上所存放的程序时,实现权利要求1-5任一所述的方法步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110441865.1A CN113189541B (zh) | 2021-04-23 | 2021-04-23 | 一种定位方法、装置及设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110441865.1A CN113189541B (zh) | 2021-04-23 | 2021-04-23 | 一种定位方法、装置及设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113189541A CN113189541A (zh) | 2021-07-30 |
CN113189541B true CN113189541B (zh) | 2023-12-12 |
Family
ID=76978191
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110441865.1A Active CN113189541B (zh) | 2021-04-23 | 2021-04-23 | 一种定位方法、装置及设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113189541B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114510679B (zh) * | 2021-12-15 | 2024-04-12 | 成都飞机工业(集团)有限责任公司 | 设备位置信息的获得方法、装置、终端设备以及存储介质 |
CN114526741B (zh) * | 2022-03-14 | 2024-05-24 | 桂林电子科技大学 | 对象定位方法、电子设备以及存储介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109548141A (zh) * | 2018-11-01 | 2019-03-29 | 上海理工大学 | 基于卡尔曼滤波算法的室内环境基站坐标位置标定方法 |
CN110174104A (zh) * | 2019-05-30 | 2019-08-27 | 北京邮电大学 | 一种组合导航方法、装置、电子设备及可读存储介质 |
WO2020103908A1 (zh) * | 2018-11-23 | 2020-05-28 | 中兴通讯股份有限公司 | 确定终端位置的方法、装置及存储介质 |
CN111385743A (zh) * | 2020-03-11 | 2020-07-07 | 北京邮电大学 | 一种位置估计方法、装置及电子设备 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9945953B2 (en) * | 2014-09-04 | 2018-04-17 | Khalifa University Of Science, Technology And Research | Methods and devices for bias estimation and correction |
CN109959381B (zh) * | 2017-12-22 | 2021-06-04 | 深圳市优必选科技有限公司 | 一种定位方法、装置、机器人及计算机可读存储介质 |
-
2021
- 2021-04-23 CN CN202110441865.1A patent/CN113189541B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109548141A (zh) * | 2018-11-01 | 2019-03-29 | 上海理工大学 | 基于卡尔曼滤波算法的室内环境基站坐标位置标定方法 |
WO2020103908A1 (zh) * | 2018-11-23 | 2020-05-28 | 中兴通讯股份有限公司 | 确定终端位置的方法、装置及存储介质 |
CN110174104A (zh) * | 2019-05-30 | 2019-08-27 | 北京邮电大学 | 一种组合导航方法、装置、电子设备及可读存储介质 |
CN111385743A (zh) * | 2020-03-11 | 2020-07-07 | 北京邮电大学 | 一种位置估计方法、装置及电子设备 |
Non-Patent Citations (2)
Title |
---|
CMD-Based NLOS Identification and Mitigation in Wireless Sensor Networks;Hongxue Diao 等;《2019 IEEE International Conference on Communications Workshops (ICC Workshops)》;第1-6页 * |
利用测距值优化的室内三维定位算法;何杰 等;《北京邮电大学学报》;第40卷(第3期);第38-42页 * |
Also Published As
Publication number | Publication date |
---|---|
CN113189541A (zh) | 2021-07-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2021203871A1 (zh) | 协同定位方法、装置、设备和存储介质 | |
CN107659893B (zh) | 一种误差补偿方法、装置、电子设备及可读存储介质 | |
CN106912105B (zh) | 基于pso_bp神经网络的三维定位方法 | |
CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质 | |
CN113189541B (zh) | 一种定位方法、装置及设备 | |
CN107484123B (zh) | 一种基于集成HWKNN的WiFi室内定位方法 | |
CN109379711B (zh) | 一种定位方法 | |
CN113091742B (zh) | 一种融合定位方法、装置、电子设备及存储介质 | |
CN114554397B (zh) | 蓝牙定位终端及系统 | |
CN112394383B (zh) | 一种卫星与5g基站组合定位方法及装置 | |
CN111008364A (zh) | 一种双观测者协同被动定位方法及系统 | |
CN110132281A (zh) | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 | |
CN118011315A (zh) | 一种uwb超宽带室内定位方法及装置 | |
CN111982121A (zh) | 一种混合视距和非视距环境下的高精度定位方法 | |
CN111678513A (zh) | 一种超宽带/惯导紧耦合的室内定位装置与系统 | |
CN112822633B (zh) | 一种基于误差补偿的定位方法,系统,终端设备及可读存储介质 | |
CN111444467B (zh) | 基于实时定位轨迹数据进行局部线性插值和预测的方法 | |
CN108834053B (zh) | 一种定位方法、装置及设备 | |
CN113347709B (zh) | 一种基于uwb的室内定位方法及系统 | |
CN115792800A (zh) | 一种基于网格搜索的双站三维交叉定位方法 | |
CN111123323B (zh) | 一种提高便携设备定位精度的方法 | |
CN112230194B (zh) | 一种基于平移阵列的解模糊方法、设备及存储介质 | |
CN114705223A (zh) | 多移动智能体在目标跟踪中的惯导误差补偿方法及系统 | |
CN114742141A (zh) | 基于icp点云的多源信息数据融合研判方法 | |
CN114219022A (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 |