CN114390431B - 一种基于超宽带的两基站二维相对定位方法及装置 - Google Patents

一种基于超宽带的两基站二维相对定位方法及装置 Download PDF

Info

Publication number
CN114390431B
CN114390431B CN202210026858.XA CN202210026858A CN114390431B CN 114390431 B CN114390431 B CN 114390431B CN 202210026858 A CN202210026858 A CN 202210026858A CN 114390431 B CN114390431 B CN 114390431B
Authority
CN
China
Prior art keywords
ranging value
reference point
corrected
circle
ranging
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
CN202210026858.XA
Other languages
English (en)
Other versions
CN114390431A (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.)
Shanghai Zexin Semiconductor Technology Co ltd
Original Assignee
Shanghai Zexin Semiconductor 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 Shanghai Zexin Semiconductor Technology Co ltd filed Critical Shanghai Zexin Semiconductor Technology Co ltd
Priority to CN202210026858.XA priority Critical patent/CN114390431B/zh
Publication of CN114390431A publication Critical patent/CN114390431A/zh
Application granted granted Critical
Publication of CN114390431B publication Critical patent/CN114390431B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/025—Services making use of location information using location based information parameters
    • H04W4/027—Services making use of location information using location based information parameters using movement velocity, acceleration information
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/029—Location-based management or tracking services
    • 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
    • H04W64/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明提供一种基于超宽带的两基站二维相对定位方法及装置，方法包括获取第一基站和第二基站之间的第一测距值d；确定UWB标签分别和第一基站、第二基站之间的第二测距值、第三测距值；对第二测距值、第三测距值做卡尔曼滤波消除高频误差后得到修正后的第二测距值RA和修正后的第三测距值RB；当|RA‑RB|≥d且max{RA,RB}＞＞min{RA,RB}或d≥RA+RB，判断所述修正后的第二测距值RA和/或所述修正后的第三测距值RB是否失真，如果失真则对RA和/或RB作无效处理，如果未失真则对RA和RB做缩放修正后进行定位。本发明提供的基于超宽带的两基站二维相对定位方法及装置，定位及跟踪性能稳定，且明显降低了硬件开销，提升了超宽带应用于二维定位场景的整体性能。

Description

一种基于超宽带的两基站二维相对定位方法及装置
技术领域
本发明实施例涉及超宽带领域，特别是涉及一种基于超宽带的两基站二维相对定位方法。
背景技术
超宽带(Ultra Wideband，简称UWB)技术由于其功耗低、安全性高、系统复杂度低和定位精度高等优点，成为了当前室内定位领域的热门研究方向。UWB定位原理与雷达定位相似，由待定位标签发射出连续脉冲，各监测基站接收到标签UWB脉冲信号分别与其完成DS_TWR(Double-Sided_Two-Way-Ranging，双边双向测距)测距流程后汇总上报，上层基于各基站接收信号到达角度、时间或时间差等信息，基于系统模型确定观测方程，从而解出信号源的估计位置。
一般来说，若结合信号到达角度进行定位，则基站需要支持多天线设计；若仅利用信号到达时间或时间差进行定位，则系统需要超过定位维度数量的基站数才能有效解算出目标的绝对位置，以相对基础的TOA(Time Of Arrival，到达时间)定位方案为例，往往需要至少四个基站才能定位三维目标，至少三个基站才能定位二维目标。定位精度会随基站数目减少而下降，若基站数目不足最少要求则会出现定位镜像或者模糊问题。
而在移动目标跟踪过程中，现有技术或者采用机器学习类方法，或者用传统卡尔曼滤波，或者依赖其他传感器计算速度、加速度、转向角等信息；机器学习计算复杂度较高，实现难度大，传统卡尔曼滤波难以消除定位模型导致的非线性误差，依赖传感器则需要额外的硬件开销。
因此需要提供一种定位方法及装置，能够解决上述问题。
发明内容
本发明提供一种基于超宽带的两基站二维相对定位方法及装置，定位及跟踪性能稳定，且明显降低了硬件开销，提升了超宽带应用于二维定位场景的整体性能。
本发明实施例提供一种基于超宽带的两基站二维相对定位方法，包括以下步骤：
获取第一基站和第二基站之间的第一测距值d；
确定UWB标签分别和所述第一基站、所述第二基站之间的第二测距值、第三测距值；
对所述第二测距值、所述第三测距值做卡尔曼滤波消除高频误差后得到修正后的第二测距值RA和修正后的第三测距值RB；
当RA+RB>d>|RA-RB|时，判断所述第二测距值和所述第三测距值有效；
当|RA-RB|≥d且max{RA,RB}＞＞min{RA,RB}或d≥RA+RB，判断所述修正后的第二测距值RA和/或所述修正后的第三测距值RB是否失真，取预设半径差作为模糊阈值，如果满足d≤|RA-RB|-模糊阈值，则对所述修正后的第二测距值RA和/或所述修正后的第三测距值RB作无效处理，如果满足|RA-RB|-模糊阈值<d≤|RA-RB|，则对所述修正后的第二测距值RA和所述修正后的第三测距值RB做缩放修正后进行定位。
优选地，所述对所述第二测距值、所述第三测距值做卡尔曼滤波消除高频误差后得到修正后的第二测距值RA和修正后的第三测距值RB包括使用以下非线性离散时间系统模型对所述第二测距值和所述第三测距值做卡尔曼滤波，以消除高频误差：
Xk＝f(Xk-1)+wk-1
Zk＝h(Xk)+vk
其中，状态向量Xk∈Rn，观测向量Zk∈Rm；wk-1与vk分别为相互独立的零均值系统高斯白噪声和观测白噪声，方差分别为Qk-1和Rk，f(·)与h(·)分别为系统的状态转移方程与观测方程。
优选地，所述卡尔曼滤波满足以下方程：
时间更新过程：
观测更新过程：
其中，Fk-1为系统状态转移矩阵，Pk,k-1为预测状态协方差矩阵，Pk和Pk-1分别为系统在k-1与k时刻的状态估计量，Kk为卡尔曼滤波增益矩阵，Hk为状态向量与观测向量之间的传递矩阵，I为单位矩阵。
优选地，当第一圆圈和第二圆圈内切或内含时，
当RA>RB+d时，将所述修正后的第二测距值RA和所述修正后的第三测距值RB分别加、减冗余长度相应比例，并引入增量，
参数k1、k2用于保证所述第一圆圈和所述第二圆圈相交，同时为保证定位精度，伪相交区域面积小于预设阈值，所述第一圆圈为以所述第一基站为圆心、所述修正后的第二测距值RA为半径形成的圆圈，所述第二圆圈为以所述第二基站为圆心、所述修正后的第三测距值RB为半径形成的圆圈，所述伪相交区域为第一参考点、第二参考点、第三参考点和第四参考围成的区域，所述第一参考点和所述第二参考点为所述第一圆圈和所述第二圆圈相交的两个点，所述第三参考点和所述第四参考点分别为所述第一圆圈和X轴相交的两个点。
优选地，当所述第一圆圈和所述第二圆圈相离或外切时，d≥RA+RB时，取冗余长度按比例分给所述修正后的第二测距值RA和所述修正后的第三测距值RB，并引入增量，
参数k1、k2用于保证所述第一圆圈和所述第二圆圈相交，伪相交区域面积小于预设阈值。
优选地，当所述第一圆圈和所述第二圆圈相交时，所述第一参考点和所述第二参考点的坐标通过以下公式计算：
当max(RA,RB,d)≠RB时，所述第一参考点和所述第二参考点位于第一、第四象限，当max(RA,RB,d)＝RB时，所述第一参考点和所述第二参考点位于第二、第三象限。
优选地，所述第一参考点和所述第二参考点的坐标通过以下公式计算：
进而可以得到所述伪相交区域的五个参考点坐标分别为：
若上一时刻有一般解且在当前时刻进入内含或内切情况，则计算所述伪相交区域所述第一参考点和、所述第二参考点、所述第三参考点和所述第四参考点与上一时刻估计位置的距离，选取相对距离最小的点作为当前时刻UWB标签位置的估计结果；若上一时刻已处于所述伪相交区域，则需联合速度与位置变化量进行判决。
优选地，所述第一参考点和所述第二参考点的坐标通过以下公式计算：
进而可以得到所述伪相交区域的五个参考点坐标分别为：
若上一时刻有一般解且在当前时刻进入内含或内切情况，则计算所述伪相交区域所述第一参考点和、所述第二参考点、所述第三参考点和所述第四参考点与上一时刻估计位置的距离，选取相对距离最小的点作为当前时刻UWB标签位置的估计结果；若上一时刻已处于所述伪相交区域，则需联合速度与位置变化量进行判决。
优选地，使用以下非线性离散时间系统模型对所述第二测距值和所述第三测距值做平方根容积卡尔曼滤波，以消除高频误差、提升跟踪精度：
Xk＝fk-1(Xk-1)+Γkwk-1
Zk＝hk(Xk)+Vk
其中，状态向量Xk∈Rn，Zk∈Rn为k时刻待定位目标观测向量；fk(·)为k-1时刻到k时刻的系统状态转移函数，hk(·)为观测函数，Γk为系统噪声转移矩阵，wk-1与Vk分别为n维度系统噪声与N维度观测噪声序列，将wk-1与Vk假定为互不相关的高斯噪声序列，具有如下统计特性：
其中，s和r分别为系统噪声和观测噪声的均值，Qk和Rk分别为系统噪声序列方差阵和观测噪声序列协方差矩阵，δkj为Kronecker-δ函数。
本发明实施例还提供一种基于超宽带的两基站二维相对定位装置，包含上述任一项所述的方法。
与现有技术相比，本发明实施例的技术方案具有以下有益效果：
本发明实施例的基于超宽带的两基站二维相对定位方法及装置，将定位基站数量从至少三个缩减到两个，提高了定位效率，减小了硬件开销，其次，基于几何关系提出一种对失真测距值的修正方式，一定程度上规避了减少基站数目带来的定位模糊与镜像问题，保证了相对较高的定位精度；
进一步地，定位系统属于非线性系统，传统卡尔曼滤波将产生一定线性化误差，而平方根容积卡尔曼滤波可以在容积卡尔曼滤波有效消除非线性误差的基础上进一步提升系统滤波稳定性，且相比于现有的机器学习类方法明显减小了运算复杂度，从而降低了定位系统对上层及基站的设备需求；
进一步地，定位及跟踪性能稳定，且明显降低了硬件开销，提升了超宽带应用于二维定位场景的整体性能。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，而不是全部实施例。对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明的一个实施例提供的基于超宽带的两基站二维相对定位方法的流程图；
图2A为本发明的一个实施例提供的基于超宽带的两基站二维相对定位方法中的各边长关系下两圆相交解算交点坐标示意图一；
图2B为本发明的一个实施例提供的基于超宽带的两基站二维相对定位方法中的各边长关系下两圆相交解算交点坐标示意图二；
图2C为本发明的一个实施例提供的基于超宽带的两基站二维相对定位方法中的各边长关系下两圆相交解算交点坐标示意图三；
图3A为本发明的一个实施例提供的基于超宽带的两基站二维相对定位方法中的各边长关系下两圆内含或者内切时解算交点坐标示意图一；
图3B为本发明的一个实施例提供的基于超宽带的两基站二维相对定位方法中的各边长关系下两圆内含或者内切时解算交点坐标示意图二；
图4为本发明的一个实施例提供的基于超宽带的两基站二维相对定位方法中的各边长关系下两圆相离或者外切时解算交点坐标示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面以具体的实施例对本发明的技术方案进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。
基于现有技术存在的问题，本发明实施例提供一种基于超宽带的两基站二维相对定位方法及装置，定位及跟踪性能稳定，且明显降低了硬件开销，提升了超宽带应用于二维定位场景的整体性能。
本发明实施例提供一种基于超宽带的两基站二维相对定位方法及装置，适用于某些基站难以布置或者无需过多布置的场景。基于UWB技术利用信号到达时间实现基站间、基站与UWB标签的测距过程；固定目标定位时，根据两基站所提供测距值进行相对定位，对会失真测距值做一定修正处理规避定位模糊以逼近真实位置；移动目标跟踪时，结合平方根容积卡尔曼滤波算法对标签实时速度与位置进行联合估计，可以很好地提升定位精度并降低模糊区域影响。
图1为本发明的一个实施例提供的基于超宽带的两基站二维相对定位方法的流程图。现在参看图1，本发明实施例提供一种基于超宽带的两基站二维相对定位方法，包括以下步骤：
步骤S101：获取第一基站和第二基站之间的第一测距值d；
步骤S102：确定UWB标签分别和所述第一基站、所述第二基站之间的第二测距值、第三测距值；
步骤S103：对所述第二测距值、所述第三测距值做卡尔曼滤波消除高频误差后得到修正后的第二测距值RA和修正后的第三测距值RB；
步骤S104：当|RA-RB|≥d且max{RA,RB}＞＞min{RA,RB}或d≥RA+RB，判断所述修正后的第二测距值RA和/或所述修正后的第三测距值RB是否失真，取预设半径差作为模糊阈值；
步骤105：如果满足d≤|RA-RB|-模糊阈值，则对所述修正后的第二测距值RA和/或所述修正后的第三测距值RB作无效处理
步骤106：如果满足|RA-RB|-模糊阈值<d≤|RA-RB|，则对所述修正后的第二测距值RA和所述修正后的第三测距值RB做缩放修正后进行定位。
在具体实施中，首先进行环境搭建及第一基站A和第二基站B初始化，根据定位场景需求放置第一基站A和第二基站B并启动完毕。
第一基站A和第二基站B上电后开始进行通讯，成功后进行一段固定时长的DS_TWR测距，完成将所有测距结果发送给对方，而后取本地测距与对方测距有效结果的均值d作为第一基站A和第二基站B之间的参考距离，此步骤通过多次测量减小不同基站天线延迟产生的测量误差。
以第一基站A和第二基站B所在直线作为x轴，第一基站A或第二基站B所在位置作为坐标原点O，并使另一基站处于x轴正半轴上，则另一基站在x轴上坐标即为第一基站A和第二基站B之间的参考距离，过原点O做直线AB的垂线记为y轴，从而确定了二维定位坐标系。
为便于下文说明，以第一基站A的真实位置为原点，则第一基站A和第二基站B为A(0,0),B(d,0)。
考虑到第一基站A和第二基站B定位系统提供的测量信息不足，求解TOA观测方程时会导致出现镜像解，即使在移动目标跟踪时两移动轨迹同样关于x轴对称，所以对基站来说标签在x轴两侧相对位置始终一致，因此在相对定位过程中无需将两解均反馈至用户，仅需呈现一侧的定位结果或跟踪轨迹至用户侧即可。
以下为UWB标签测距及修正的具体实现方式。
在具体实施中，UWB标签上电后，开始与第一基站A和第二基站B分别建立通讯。成功后通过DS_TWR方式确定与第一基站A和第二基站B的测距值分别为第二测距值和第三测距值。
对所述第二测距值、所述第三测距值做卡尔曼滤波消除高频误差后得到修正后的第二测距值RA和修正后的第三测距值RB包括使用以下非线性离散时间系统模型对所述第二测距值和所述第三测距值做卡尔曼滤波，以消除高频误差：
Xk＝f(Xk-1)+wk-1
Zk＝h(Xk)+vk
其中，状态向量Xk∈Rn，观测向量Zk∈Rm；wk-1与vk分别为相互独立的零均值系统高斯白噪声和观测白噪声，方差分别为Qk-1和Rk，f(·)与h(·)分别为系统的状态转移方程与观测方程。
在具体实施中，所述卡尔曼滤波满足以下方程：
时间更新过程：
观测更新过程：
其中，Fk-1为系统状态转移矩阵，Pk,k-1为预测状态协方差矩阵，Pk和Pk-1分别为系统在k-1与k时刻的状态估计量，Kk为卡尔曼滤波增益矩阵，Hk为状态向量与观测向量之间的传递矩阵，I为单位矩阵。
理想条件下，第一基站A和第二基站B与UWB标签应可构成三角形或处于同一直线上，UWB标签的真实位置应处于以第一基站A和第二基站B为圆心，以第二测距值、第三测距值为半径所做两圆的交点上。实际操作中，第一测距值d经多次测量平均后可认为较接近真实值，第二测距值和第三测距值受系统噪声与测量噪声影响往往与真实值有偏离，这将导致以第二测距值、第三测距值为半径所做圆并未交于一点而构成一个相交区域。在基站数量较多时，可在此区域内由最小二乘估计逼近标签真实位置。然而在只有两个基站的特定应用场景下，可将第二测距值、第三测距值与第一基站A、第二基站B的距离的大小关系作为其是否有效的判据。
在具体实施中，当RA+RB>d>|RA-RB|时，即以第二测距值和第三测距值所构成两圆满足相交位置关系时，此时定位方程组有两解，认为第二测距值和第三测距值有效且UWB标签应处于相交区域内。
若当|RA-RB|≥d且max{RA,RB}＞＞min{RA,RB}，则两圆内含或内切，方程组无解或有唯一解，此时相交区域已退化至一点或不存在，而由于噪声的存在，此交点亦不能直接作为UWB标签位置，第二测距值和/或第三测距值失真。
若d≥RA+RB，则两圆相离或外切，同理可认为第二测距值和/或第三测距值失真。
两圆内含或内切时，即使无法判定第二测距值和第三测距值各自的偏差程度，但若适当地放大过小测量值、缩小过大测量值后，能使得两圆相交，则可认为UWB标签在此伪相交区域内并能够按照一般方式求出交点坐标；若第二测距值和第三测距值相差过大，则说明至少一个基站的测距滤波未成功，可视为当前时刻测距值无效，均舍去不用或以前一时刻测量值进行定位。
结合目前市场UWB设备的测距精度，在本方法中暂时将5cm作为半径差的模糊阈值，即如果满足d≤|RA-RB|-5d≤|RA-RB|-5，将第二测距值和/或第三测距值作无效处理；若满足|RA-RB|-5<d≤|RA-RB|，则对第二测距值和第三测距值值分别做适当缩放修正后进行定位。
在具体实施中，当第一圆圈和第二圆圈内切或内含时，
当RA>RB+d时，将所述修正后的第二测距值RA和所述修正后的第三测距值RB分别加、减冗余长度相应比例，并引入增量，
参数k1、k2用于保证所述第一圆圈和所述第二圆圈相交，同时为保证定位精度，伪相交区域面积小于预设阈值，所述第一圆圈为以所述第一基站为圆心、所述修正后的第二测距值RA为半径形成的圆圈，所述第二圆圈为以所述第二基站为圆心、所述修正后的第三测距值RB为半径形成的圆圈，所述伪相交区域为第一参考点、第二参考点、第三参考点和第四参考围成的区域，所述第一参考点和所述第二参考点为所述第一圆圈和所述第二圆圈相交的两个点，所述第三参考点和所述第四参考点分别为所述第一圆圈和X轴相交的两个点。
在具体实施中，当所述第一圆圈和所述第二圆圈相离或外切时，d≥RA+RB时，取冗余长度按比例分给所述修正后的第二测距值RA和所述修正后的第三测距值RB，并引入增量，
参数k1、k2用于保证所述第一圆圈和所述第二圆圈相交，伪相交区域面积小于预设阈值。
参看图2A、图2B和图2C可知，在真实平面直角坐标系中，两交点横坐标相同、纵坐标互为相反数，即交点坐标：{P1(x,y),P2(x,-y)}。在相对定位坐标系中，二者关于基站互为镜像，仅保留一解即可，此解可通过求解至前一时刻估计位置的距离确认，初始时刻可默认取正纵坐标解。
参看图3A和图3B可知，以修正后的第二测距值和修正后的第三测距值为半径所做两圆相交于一月牙状伪相交区域。当将此伪相交区域控制在较小面积下时，可选取圆上第一参考点P1、第二参考点P2与x轴上两端点第三参考点P3、第四参考点P4共计四个参考点作为伪相交区域端点进行判定。
在具体实施中，当所述第一圆圈和所述第二圆圈相交时，所述第一参考点和所述第二参考点的坐标通过以下公式计算：
当max(RA,RB,d)≠RB时，所述第一参考点和所述第二参考点位于第一、第四象限，当max(RA,RB,d)＝RB时，所述第一参考点和所述第二参考点位于第二、第三象限。
在具体实施中，所述第一参考点和所述第二参考点的坐标通过以下公式计算：
进而可以得到所述伪相交区域的五个参考点坐标分别为：
若上一时刻有一般解且在当前时刻进入内含或内切情况，则计算所述伪相交区域所述第一参考点和、所述第二参考点、所述第三参考点和所述第四参考点与上一时刻估计位置的距离，选取相对距离最小的点作为当前时刻UWB标签位置的估计结果；若上一时刻已处于所述伪相交区域，则需联合速度与位置变化量进行判决。
观察图4可知，以修正后的第二测距值和修正后的第三测距值所做两圆相交于一纺锤状区域，d>RA,RB，交点在第一基站A、第二基站B之间。
当将此伪相交区域控制在较小面积下时，可选取圆上两交点第一参考点P1、第二参考点P2与x轴上两端点第三参考点P3、第四参考点P4以及此区域中心点P0共计五个参考点作为UWB标签可能位置进行判定。
所述第一参考点和所述第二参考点的坐标通过以下公式计算：
进而可以得到所述伪相交区域的五个参考点坐标分别为：
若上一时刻有一般解且在当前时刻进入内含或内切情况，则计算所述伪相交区域所述第一参考点和、所述第二参考点、所述第三参考点和所述第四参考点与上一时刻估计位置的距离，选取相对距离最小的点作为当前时刻UWB标签位置的估计结果；若上一时刻已处于所述伪相交区域，则需联合速度与位置变化量进行判决。
在具体实施中，使用以下非线性离散时间系统模型对所述第二测距值和所述第三测距值做协方差矩阵平方根卡尔曼滤波，以消除高频误差：
Xk＝fk-1(Xk-1)+Γkwk-1
Zk＝hk(Xk)+Vk
其中，状态向量Xk∈Rn，Zk∈Rn为k时刻待定位目标观测向量；fk(·)为k-1时刻到k时刻的系统状态转移函数，hk(·)为观测函数，Γk为系统噪声转移矩阵，wk-1与Vk分别为n维度系统噪声与N维度观测噪声序列，将wk-1与Vk假定为互不相关的高斯噪声序列，具有如下统计特性：
其中，s和r分别为系统噪声和观测噪声的均值，Qk和Rk分别为系统噪声序列方差阵和观测噪声序列协方差矩阵，δkj为Kronecker-δ函数。
在具体实施中，平方根容积卡尔曼滤波(Square Root Cubature Kalman Filter，SRCKF)算法实施过程具体如下：
SRCKF容积点与对应权值分别为：
时间更新过程：
初始化:
设置状态初值X0|0，误差协方差矩阵初值P0|0，协方差矩阵平方根因子初值S0|0，其中过程噪声Q和测量噪声R。
计算当前状态下2n个容积点(i＝1,2,...,2n):
其中，Sk-1|k-1＝chol{Pk-1|k-1},chol{·}表示对矩阵做Cholesky分解。
计算容积点通过非线性状态转移方程传播的容积点预测值(i＝1,2,...,2n)：
计算状态量预测值：
估计预测误差协方差矩阵的平方根因子Ck|k-1：
令误差向量χk|k-1为：
则：
其中，Tria(·)为对矩阵做QR分解，并返回下三角矩阵。
观测更新过程：
计算更新状态容积点(i＝1,2,...,2n)：
通过观测方程传播容积点(i＝1,2,...,2n)：
计算观测量预测值：
计算新息协方差矩阵的平方根因子：
其中，
计算新息互相关协方差平方根因子：
计算k时刻SRCKF滤波增益：
计算状态量更新值：
计算状态估计误差协方差矩阵的平方根因子：
本发明实施例还提供一种基于超宽带的两基站二维相对定位装置，包含上述任一项所述的方法。
综上所述，本发明实施例的基于超宽带的两基站二维相对定位方法及装置，将定位基站数量从至少三个缩减到两个，提高了定位效率，减小了硬件开销，其次，基于几何关系提出一种对失真测距值的修正方式，一定程度上规避了减少基站数目带来的定位模糊与镜像问题，保证了相对较高的定位精度；
进一步地，定位系统属于非线性系统，传统卡尔曼滤波将产生一定线性化误差，而平方根容积卡尔曼滤波可以在容积卡尔曼滤波有效消除非线性误差的基础上进一步提升系统滤波稳定性，且相比于现有的机器学习类方法明显减小了运算复杂度，从而降低了定位系统对上层及基站的设备需求；
进一步地，定位及跟踪性能稳定，且明显降低了硬件开销，提升了超宽带应用于二维定位场景的整体性能。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (9)

1.一种基于超宽带的两基站二维相对定位方法，其特征在于，包括以下步骤：
获取第一基站和第二基站之间的第一测距值d；
确定UWB标签分别和所述第一基站、所述第二基站之间的第二测距值、第三测距值；
对所述第二测距值、所述第三测距值做卡尔曼滤波消除高频误差后得到修正后的第二测距值RA和修正后的第三测距值RB；
当RA+RB＞d＞|RA-RB|时，判断所述第二测距值和所述第三测距值有效；
当|RA-RB|≥d且max{RA，RB}＞＞min{RA，RB}时或d≥RA+RB时，判断所述修正后的第二测距值RA和/或所述修正后的第三测距值RB是否失真，取预设半径差|RA-RB|作为模糊阈值，如果满足d≤|RA-RB|-模糊阈值，则对所述修正后的第二测距值RA和/或所述修正后的第三测距值RB作无效处理，如果满足|RA-RB|-模糊阈值＜d≤|RA-RB|，则对所述修正后的第二测距值RA和所述修正后的第三测距值RB做缩放修正后进行定位；
所述卡尔曼滤波满足以下方程：
时间更新过程：
观测更新过程：
其中，Fk-1为系统状态转移矩阵，Pk，k-1为预测状态协方差矩阵，Pk和Pk-1分别为系统在k-1与k时刻的状态估计量，Kk为卡尔曼滤波增益矩阵，Hk为状态向量与观测向量之间的传递矩阵，I为单位矩阵。
2.根据权利要求1所述的基于超宽带的两基站二维相对定位方法，其特征在于，
所述对所述第二测距值、所述第三测距值做卡尔曼滤波消除高频误差后得到修正后的第二测距值RA和修正后的第三测距值RB包括使用以下非线性离散时间系统模型对所述第二测距值和所述第三测距值做卡尔曼滤波，以消除高频误差：
Xk＝f(Xk-1)+wk-1
Zk＝h(Xk)+vk
其中，状态向量Xk∈Rn，观测向量Zk∈Rm；wk-1与vk分别为相互独立的零均值系统高斯白噪声和观测白噪声，方差分别为Qk-1和Rk，f(·)与h(·)分别为系统的状态转移方程与观测方程。
3.根据权利要求1所述的基于超宽带的两基站二维相对定位方法，其特征在于，
当第一圆圈和第二圆圈内切或内含时，
当RA＞RB+d时，将所述修正后的第二测距值RA和所述修正后的第三测距值RB分别加、减冗余长度相应比例，并引入增量，
参数k1、k2用于保证所述第一圆圈和所述第二圆圈相交，同时为保证定位精度，伪相交区域面积小于预设阈值，所述第一圆圈为以所述第一基站为圆心、所述修正后的第二测距值RA为半径形成的圆圈，所述第二圆圈为以所述第二基站为圆心、所述修正后的第三测距值RB为半径形成的圆圈，所述伪相交区域为第一参考点、第二参考点、第三参考点和第四参考围成的区域，所述第一参考点和所述第二参考点为所述第一圆圈和所述第二圆圈相交的两个点，所述第三参考点和所述第四参考点分别为所述第一圆圈和X轴相交的两个点。
4.根据权利要求3所述的基于超宽带的两基站二维相对定位方法，其特征在于，
当所述第一圆圈和所述第二圆圈相离或外切时，d≥RA+RB时，取冗余长度按比例分给所述修正后的第二测距值RA和所述修正后的第三测距值RB，并引入增量，
参数k1、k2用于保证所述第一圆圈和所述第二圆圈相交，伪相交区域面积小于预设阈值。
5.根据权利要求3所述的基于超宽带的两基站二维相对定位方法，其特征在于，当所述第一圆圈和所述第二圆圈相交时，所述第一参考点和所述第二参考点的坐标通过以下公式计算：
当max(RA，RB，d)≠RB时，所述第一参考点和所述第二参考点位于第一、第四象限，当max(RA，RB，d)＝RB时，所述第一参考点和所述第二参考点位于第二、第三象限。
6.根据权利要求3所述的基于超宽带的两基站二维相对定位方法，其特征在于，所述第一参考点和所述第二参考点的坐标通过以下公式计算：
进而可以得到所述伪相交区域的五个参考点坐标分别为：
若上一时刻有一般解且在当前时刻进入内含或内切情况，则计算所述伪相交区域所述第一参考点、所述第二参考点、所述第三参考点和所述第四参考点与上一时刻估计位置的距离，选取相对距离最小的点作为当前时刻UWB标签位置的估计结果；若上一时刻已处于所述伪相交区域，则需联合速度与位置变化量进行判决。
7.根据权利要求4所述的基于超宽带的两基站二维相对定位方法，其特征在于，所述第一参考点和所述第二参考点的坐标通过以下公式计算：
进而可以得到所述伪相交区域的五个参考点坐标分别为：
若上一时刻有一般解且在当前时刻进入内含或内切情况，则计算所述伪相交区域所述第一参考点、所述第二参考点、所述第三参考点和所述第四参考点与上一时刻估计位置的距离，选取相对距离最小的点作为当前时刻UWB标签位置的估计结果；若上一时刻已处于所述伪相交区域，则需联合速度与位置变化量进行判决。
8.根据权利要求1所述的基于超宽带的两基站二维相对定位方法，其特征在于，使用以下非线性离散时间系统模型对所述第二测距值和所述第三测距值做平方根容积卡尔曼滤波，以消除高频误差、提升跟踪精度：
Xk＝fk-1(Xk-1)+Γkwk-1
Zk＝hk(Xk)+Vk
其中，状态向量Xk∈Rn，Zk∈Rn为k时刻待定位目标观测向量；fk(·)为k-1时刻到k时刻的系统状态转移函数，hk(·)为观测函数，Γk为系统噪声转移矩阵，wk-1与Vk分别为n维度系统噪声与N维度观测噪声序列，将wk-1与Vk假定为互不相关的高斯噪声序列，具有如下统计特性：
其中，s和r分别为系统噪声和观测噪声的均值，Qk和Rk分别为系统噪声序列方差阵和观测噪声序列协方差矩阵，δkj为Kronecker-δ函数。
9.一种基于超宽带的两基站二维相对定位装置，其特征在于，包含如权利要求1至8任一项所述的方法。
CN202210026858.XA 2022-01-11 2022-01-11 一种基于超宽带的两基站二维相对定位方法及装置 Active CN114390431B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210026858.XA CN114390431B (zh) 2022-01-11 2022-01-11 一种基于超宽带的两基站二维相对定位方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210026858.XA CN114390431B (zh) 2022-01-11 2022-01-11 一种基于超宽带的两基站二维相对定位方法及装置

Publications (2)

Publication Number Publication Date
CN114390431A CN114390431A (zh) 2022-04-22
CN114390431B true CN114390431B (zh) 2024-04-26

Family

ID=81201684

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210026858.XA Active CN114390431B (zh) 2022-01-11 2022-01-11 一种基于超宽带的两基站二维相对定位方法及装置

Country Status (1)

Country Link
CN (1) CN114390431B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114928811B (zh) * 2022-05-05 2024-05-07 南京理工大学 一种基于神经网络的超宽带矿井下人员精确定位方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018014602A1 (zh) * 2016-07-19 2018-01-25 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
CN108038868A (zh) * 2017-10-17 2018-05-15 国网河南省电力公司郑州供电公司 基于三维数字模型的变电站复杂环境跨视场目标跟踪方法
CN111212476A (zh) * 2020-04-21 2020-05-29 杭州优智联科技有限公司 基于调频连续波的多基站超宽带定位方法
CN111948602A (zh) * 2020-08-17 2020-11-17 南京工程学院 基于改进Taylor级数的二维UWB室内定位方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018014602A1 (zh) * 2016-07-19 2018-01-25 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
CN108038868A (zh) * 2017-10-17 2018-05-15 国网河南省电力公司郑州供电公司 基于三维数字模型的变电站复杂环境跨视场目标跟踪方法
CN111212476A (zh) * 2020-04-21 2020-05-29 杭州优智联科技有限公司 基于调频连续波的多基站超宽带定位方法
CN111948602A (zh) * 2020-08-17 2020-11-17 南京工程学院 基于改进Taylor级数的二维UWB室内定位方法

Also Published As

Publication number Publication date
CN114390431A (zh) 2022-04-22

Similar Documents

Publication Publication Date Title
CN110849369B (zh) 机器人跟踪方法、装置、设备及计算机可读存储介质
Yan et al. An improved NLOS identification and mitigation approach for target tracking in wireless sensor networks
CN107315171B (zh) 一种雷达组网目标状态与系统误差联合估计算法
CN107462249B (zh) 一种基于rfid的室内定位方法、装置及系统
CN114390431B (zh) 一种基于超宽带的两基站二维相对定位方法及装置
CN112256023A (zh) 基于贝塞尔曲线的机场巡界机器人局部路径规划方法及系统
CN114998276B (zh) 一种基于三维点云的机器人动态障碍物实时检测方法
CN111964667B (zh) 一种基于粒子滤波算法的地磁_ins组合导航方法
CN109307857B (zh) 一种基于多传感器的自主跟随定位装置及方法
Mills et al. Tracking in a Hough Space with the Extended Kalman Filter.
Faion et al. Recursive bayesian calibration of depth sensors with non-overlapping views
CN115421153B (zh) 一种基于扩展卡尔曼滤波的激光雷达和uwb组合定位方法及系统
CN108334893B (zh) 一种多亮点聚类分析的水下细长体特征识别方法
CN110007298B (zh) 一种目标超前预测跟踪方法
CN112800889B (zh) 一种基于分布式矩阵加权融合高斯滤波的目标跟踪方法
Li et al. A single-shot pose estimation approach for a 2D laser rangefinder
CN114004900A (zh) 一种基于点线面特征的室内双目视觉里程计方法
Ma et al. A TDOA localization method for complex environment localization
Ito et al. A modified particle filter algorithm for wireless capsule endoscope location tracking
CN113702941A (zh) 一种基于改进icp的点云测速方法
CN116419147A (zh) 一种基于距离差分的非视距环境下室内uwb定位解算方法
CN114245291A (zh) 参考节点虚拟化为未知节点的测距定位方法
Zhai et al. Target tracking based on millimeter wave radar in complex scenes
Xin et al. Vehicle ego-localization based on the fusion of optical flow and feature points matching
Matsuyama et al. Estimation of pedestrian pose and velocity considering arm swing using point-cloud data

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