CN112533149A - 一种基于uwb移动节点的移动目标定位算法 - Google Patents

一种基于uwb移动节点的移动目标定位算法 Download PDF

Info

Publication number
CN112533149A
CN112533149A CN202011362064.8A CN202011362064A CN112533149A CN 112533149 A CN112533149 A CN 112533149A CN 202011362064 A CN202011362064 A CN 202011362064A CN 112533149 A CN112533149 A CN 112533149A
Authority
CN
China
Prior art keywords
node
equation
error
distance
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.)
Granted
Application number
CN202011362064.8A
Other languages
English (en)
Other versions
CN112533149B (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.)
Guilin University of Technology
Original Assignee
Guilin University of Technology
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 Guilin University of Technology filed Critical Guilin University of Technology
Priority to CN202011362064.8A priority Critical patent/CN112533149B/zh
Publication of CN112533149A publication Critical patent/CN112533149A/zh
Application granted granted Critical
Publication of CN112533149B publication Critical patent/CN112533149B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/029—Location-based management or tracking services
    • 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
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/02—Systems for determining distance or velocity not using reflection or reradiation using radio waves
    • 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
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明提供一种基于UWB移动节点的移动目标定位算法，属于精密测量技术领域。该算法首先建立UWB定位模型，对基站与节点间的测量距离进行测距和预处理，距离测量过程中利用CIR信号特征采用测距误差缓解模糊推理定位算法对NLOS误差进行一步缓解，然后利用KF算法中新息与新息方差之间的差变化来调节新息的值，对一步缓解后距离数据再次进行缓解，通过两步缓解来提高测距精度；最后通过LS定位算法完成定位。静态和动态定位实验结果表明，在NLOS情况下，本发明提出定位算法精度较高，有效改善了UWB移动节点定位精度低问题。

Description

一种基于UWB移动节点的移动目标定位算法
【技术领域】
本发明涉及精密测量技术领域，具体涉及一种基于UWB移动节点的移动目标定位算法。
【背景技术】
传统的GPS卫星室外定位技术受建筑物遮挡、环境复杂等因素的影响，定位误差比较大，已经逐渐不能满足室内外定位需求。为此，基于Wi-Fi的定位、射频识别定位、超声波定位、蓝牙定位及超宽带(Ultra Wideband,UWB)定位等定位方案相继孕育而出。相比于其他定位和跟踪技术，基于UWB的定位跟踪技术会有其独特优势：1)UWB定位技术有其他定位技术所无法达到的厘米级定位精度；2)UWB定位技术具有极高的时钟分辨率，使其具有良好的抗分径能力；3)由于UWB信号采用的是IEEE 802.15.4.a标准，使得UWB信号频谱范围与传统通信电子信号(例如：Wi-Fi信号、移动数字通信信号)频谱范围差异巨大，可以有效避免复杂环境中其他电子信号干扰；4)UWB信号在其频谱范围内包含低频部分，使得UWB信号具有强的穿透性。这些特点使其在复杂环境中具有高可靠性，更有利于复杂环境下动态数据的收集和对移动物体的实时定位和跟踪。因此，UWB定位和跟踪技术在定位和跟踪领域具有广阔的应用前景。
UWB定位技术相比较传统定位技术相比优势突出，但UWB定位系统在实际运行过程中容易产生了非视距(Non-Line of Sight,NLOS)误差、多径误差、时钟漂移误差以及天线延迟导致的误差，其中NLOS误差是最主要的误差，对测距、定位和跟踪的精度、实时性、鲁棒性都会产生较大影响。针对UWB定位技术存在的测距误差问题，已经有许多专家学者对静态场景下的UWB节点定位技术进行了研究，并对测距误差的抑制产生一定效果。但是对测距误差识别和缓解测距精度效果有限，并且在实际定位与跟踪应用中目标对象一般是移动的，相应的UWB节点位置也是动态变化的，节点每个时刻都可能离开原来的位置，每次确定当前时刻对象的位置时，该节点已经可能在下一个位置了。因此，节点的移动性导致传统静态定位方法存在以下的问题：1)移动节点需要频繁定位以适应目标对象的移动性，需要定位和跟踪算法具有很好的实时性；2)节点移动的同时会带来一些不利的变化，如网络拓扑变化、节点之间通信信道中NLOS影响会实时发生改变等，这些变化导致定位和跟踪困难或精度达不到要求。显然，UWB节点的移动性给传统UWB定位和跟踪方法带来了极大的挑战，研究高效、可靠、准确、快速的UWB移动节点定位和跟踪算法具有重要的学术价值和实际意义。
【发明内容】
本发明的发明目的在于：针对上述存在的问题，提供一种基于UWB移动节点的移动目标定位算法，本发明的方法是基于模糊推理与抗NLOS自适应卡尔曼滤波融合定位算法，拥有更高定位精度。
为了实现上述目的，本发明采用的技术方案如下：
一种基于UWB移动节点的移动目标定位算法，包括以下步骤：
(1)建立UWB定位模型，在三维空间中，布置至少3个基站节点，在待测移动目标上设置标签节点；
(2)利用UWB定位模型对基站节点和标签节点间的距离进行测距，并经过预处理消除时钟偏移和时钟漂移带来的误差，得到消除误差后的距离测试值，并得到移动目标的粗略位置坐标；
(3)将步骤(2)每次测距采集到CIR信号的接收信号强度指示RSSI、第一路径功率强度FPPL、上升时间RT特征采用测距误差缓解模糊推理定位算法得到NLOS测距误差估计；
(4)将步骤(2)消除误差后的距离测试值与步骤(3)模糊得到的NLOS测距误差估计相减，得到模糊后的距离数据；
(5)将模糊后距离数据作为观测值代入自适应抗NLOS卡尔曼算法，得到最终距离估计数据；
(6)将最终距离估计数据代入鲁棒性更强LS定位算法，完成标签节点的位置估计。
本发明中，进一步地，步骤(2)的具体步骤为：
设任一基站节点为节点A，待测移动目标为节点B，测试过程中，节点A向节点B发送测距信号，并记录下发送信号时间Ta1,节点B在接收到测距信号时记录下信号到达时间Tb1,并在间隔Treply1后向节点A发送确认UWB信号，在发送时记录下发送时刻Tb2,节点A收到节点B发送信号后，进行类似节点B操作，记录下接收时间Ta2,延迟Treply2的时间再次向节点B发送信号，并记录发送时间Ta3，节点B再次在接收到信号后,记录下信号到达时间Tb3；节点A与节点B经过三次通信后，得出节点A与节点B真实传输信号的时间Ttof的值：
Figure BDA0002804273640000031
其中，Tround1＝Ta2-Ta1，Tround2＝Tb3-Tb2，Treply1＝Tb2-Tb1，Treply2＝Ta3-Ta2；
根据每个基站节点与标签节点之间距离计算公式di＝cti，其中c为信号传输的速率即光速，消除误差后的距离测试值为：
di＝cTtof (2)
根据消除误差后的距离测试值，基于测距原理建立坐标方程，采用最小二乘法，计算UWB移动目标的坐标，获取移动目标的粗略位置坐标。
本发明中，进一步地，步骤(3)所述测距误差缓解模糊推理定位算法所用的模糊集和模糊规则的建立过程为：
3.1非视距实验场景下，将基站节点放在固定位置，标签节点位置从相距0.5m处开始，然后将障碍物相距基站10cm的位置开始，采集500次实验数据为一组，每测一组就将障碍物到基站距离增加20cm，一直测到障碍物无法在标签和基站之间为止，当所有障碍物测完后，增加标签与基站距离，重复之前操作，一直测到3米为止；试验过程中采集UWB的CIR信号特征RSSI、FPPL、RT以及计算得到NLOS测距误差；
3.2视距实验场景下，即无障碍物情况，将基站放在固定位置，标签位置从相距0.5m处开始，每间隔0.5m作为一个测试点，一直测到3米为止；试验过程中采集UWB的CIR信号特征RSSI、FPPL、RT以及计算得到NLOS测距误差；
3.3对步骤3.1和3.2采集到的实验数据进行分析，将输入信号特征RSSI分成5个模糊集合，用非常大、大、中、小和非常小来划分集合，将输入信号特征FPPL分为6个模糊集合，用非常大、大、中、小、非常小以及非常非常小来划分，对于输入信号特征RT是与NLOS测距误差影响成正相关的，分为非常非常小、非常小、小、中、大5个模糊集合，同时对于输出NLOS误差分为非常小、小、中、大、非常大5个模糊集合；结合实验数据得到模糊规则；
将每一次测距采集到的CIR信号特征RSSI、FPPL、RT与所述模糊规则进行对应，得到对应的NLOS测距误差的等级及NLOS测距误差估计。
本发明中，进一步地，步骤(5)所述的自适应抗NLOS卡尔曼算法为，采用卡尔曼滤波算法对模糊后的距离数据进行滤波，利用3倍新息的方差与新息比较来判断是否有NLOS存在，如果存在则通过对新息进行固定比例缩放来减小NLOS测距误差，具体步骤为：
假设在短的时间内，标签节点与基站节点之间距离是均匀变化的，标签节点运动状态为x(k)＝[d(k)v(k)]T，其中：d(k)表示k时刻标签节点到基站的距离，，为除去噪声后的估计值；v(k)表示k时刻标签节点的速度；w(k-1)＝[wd(k-1)wv(k-1)]T表示标签在运动过程中必然受到过程噪声干扰，其中：wd(k-1)表示k-1时刻距离过程噪声，wv(k-1)表k-1时刻示速度过程噪声，于是可以得到系统状态方程：
Figure BDA0002804273640000041
其中：T代表采样时间；将式(3.5)化为矩阵的形式，如公式(3.6)所示：
Figure BDA0002804273640000042
上式进一步简化可得矩阵形式状态方程模型，即式(3.7)，其中是Fk状态转移矩阵，wk-1是k-1时刻过程噪声，服从均值为0，方差矩阵为Qk-1的高斯分布；
xk＝Fkxk-1+wk-1 (5)
在基于滤波卡尔曼滤波系统中，设观测量为模糊后的测距距离值，z(k)表示第k时刻模糊后距离数据，r(k)表示测距误差，于是观测方程可以写成：
Figure BDA0002804273640000043
将(6)式写成矩阵形式为：
zk＝Hkxk+vk (7)
其中，Hk为观测矩阵，vk表示k时刻观测噪声，服从均值为0，方差矩阵为Rk的高斯分布；
式(5)和式(7)组成用于对测距数据滤波的卡尔曼滤波系统模型；假设k-1时刻卡尔曼滤波估计状态为
Figure BDA0002804273640000044
以及k时刻模糊后距离数据为zk，则卡尔曼滤波计算k时刻估计状态
Figure BDA0002804273640000045
的步骤为：
5.1一步状态预测
根据k-1时刻KF估计状态
Figure BDA0002804273640000051
预测k时刻状态，即根据k-1时刻观测值zk-1对真实状态xk线性最小方差估计，即：
Figure BDA0002804273640000052
其中，
Figure BDA0002804273640000053
表示k-1时刻最小均方估计；由于公式(5)可知，wk-1只对xk的值影响，因此E{wk-1/zk-1}的值为0于是可以得到一步预测状态公式(9)为：
Figure BDA0002804273640000054
5.2误差协方差矩阵预测
定义：
Figure BDA0002804273640000055
Figure BDA0002804273640000056
其中，
Figure BDA0002804273640000057
表示一步预测估计代替真实状态xk引起误差，
Figure BDA0002804273640000058
表示将一步预测估计值
Figure BDA0002804273640000059
代入观测方程得到预测观测值；
Figure BDA00028042736400000510
表示预测观测值与真实观测值之间的误差，也称为新息Δk；设第k时刻新息Δk和其方差矩阵Dk为：
Figure BDA00028042736400000511
Figure BDA00028042736400000512
根据3倍方差理论并结合式(12)和式(13)可以得到一个测量值是否受到NLOS的判断公式：
Figure BDA00028042736400000513
其中，
Figure BDA00028042736400000514
表示Dk矩阵对角线上第i个元素，
Figure BDA00028042736400000515
表达Δk的第i个元素，当式(3.27)不满足时候，对其新息修正，如公式(3.28)所示：
Figure BDA00028042736400000516
其中，a表示新息修正系数，a根据新息与新息方差之间差变化来自动调节其取值，自动调节α的方法如下：
Figure BDA00028042736400000517
其中，b为修正系数α的最大缩小比例，取值范围在0到1之间；
由公式(10)和(11)可知，新息中包含部分真实状态的信息，于是对
Figure BDA00028042736400000518
进行适当加权获得
Figure BDA00028042736400000519
信息，于是修正后状态估计为：
Figure BDA00028042736400000520
其中，Kk为卡尔曼增益矩阵；假设修正后估计
Figure BDA0002804273640000061
与真实状态xk之间估计误差为：
Figure BDA0002804273640000062
于是估计误差的协方差矩阵定义为：
Figure BDA0002804273640000063
一步预测误差的协方差矩阵定义为：
Figure BDA0002804273640000064
并且已知
Figure BDA0002804273640000065
其中Rk表示观测噪声方差，估计误差与观测噪声互不相关，即
Figure BDA0002804273640000066
代入式(3.17)可得：
Pk＝(I-KkHk)Pk|k-1(I-KkHk)T+KkRkKT (21)
要写出递推公式，还要分析从Pk-1到Pk|k-1的递推公式；将公式(3.7)和公式(3.11)代入一步预测状态估计误差公式可得：
Figure BDA0002804273640000067
由于k-1时刻估计误差与当前k时刻过程噪声之间也互不相关，即:
Figure BDA0002804273640000068
代入式(20)可得一步预测误差的协方差矩阵：
Pk|k-1＝FkPk-1Fk T+Qk-1 (23)
在得到一步预测误差协方差的递推公式后，需要进一步讨论增益矩阵Kk值的选择，以使得估计误差协方差Pk最小；
5.3增益矩阵Kk
为了使得估计误差协方差Pk最小，通常矩阵的迹最小，即：
Figure BDA0002804273640000069
将式(23)代入上式，根据矩阵迹的求导公式，同时考虑到Pk|k-1、Rk为对称矩阵，计算可得：
Figure BDA00028042736400000610
令公式(25)等于零，即可求得卡尔曼滤波增益矩阵为：
Figure BDA0002804273640000071
进一步将式(26)代入公式(23)，可将估计误差协方差矩阵简化为：
Pk＝(I-KkHk)Pk|k-1 (27)
5.4更新过程，根据式(14)更新状态估计
Figure BDA0002804273640000072
5.5更新过程，根据式(24)更新估计误差的协方差矩阵Pk；
5.6置k＝k+1，循环进行步骤5.1到步骤5.5，进行下一次滤波计算，得到最终状态估计
Figure BDA0002804273640000073
通过标签节点的运动状态方程x(k)＝[d(k)v(k)]T得到最终距离估计数据。
进一步地，步骤(6)所述的将最终估计距离数据代入鲁棒性更强LS定位算法，具体过程为：将得到三个基站距离数据d1、d2、d3代入到TOA定位方程中，即：
Figure BDA0002804273640000074
在公式(28)中，从第二个方程到最后一个方程依次减去第一方程，得：
Figure BDA0002804273640000075
为了方便求解，可将其转换成线性方程组矩阵形式为：
AX＝b (30)
其中，
Figure BDA0002804273640000076
对公式(30)用最小二乘原理可得到标签的位置估计：
X＝(ΑTA)-1ATb (31)
综上所述，由于采用了上述技术方案，本发明的有益效果是：
1、本发明对UWB的信道冲击响应CIR信号特征与NLOS误差变化规律进行分析，给出一种基于CIR信号特征模糊推理定位算法来缓解NLOS误差的方法，来提高定位精度。定位过程首先利用CIR信号特征对NLOS误差进行一步缓解，然后利用KF算法中新息与新息方差之间的差变化来调节新息的值，对一步缓解后距离数据再次进行缓解，通过两步缓解来提高测距精度；最后通过LS定位算法完成定位。在NLOS情况下，静态和动态定位实验结果表明，本发明提出定位算法精度较高，有效改善了UWB移动节点定位精度低问题。
2、本发明在测距过程中还对测距数据进行预处理，消除时钟偏移和时钟漂移带来的误差，进一步提高测试的精度。
附图说明
图1为本发明基于模糊推理与自适应抗NLOS卡尔曼滤波融合定位算法的整体流程图；
图2为本发明步骤(5)中自适应抗NLOS卡尔曼滤波算法流程图；
图3为静态定位实验得到的定位散点图；
图4为静态定位实验得到的CDF图；
图5为动态定位实验得到的定位轨迹图；
图6为动态定位实验得到的定位轨迹的RMSE曲线图。
具体实施方式
以下结合附图和具体实施方式对本发明作进一步详述。
一种基于UWB移动节点的移动目标定位算法，总体来说是一种基于模糊推理与自适应抗NLOS卡尔曼滤波融合定位算法，其流程图见图1，包括以下步骤：
(1)建立UWB定位模型，在三维空间中，布置至少3个基站节点，在待测移动目标上设置标签节点；
(2)利用UWB定位模型对基站节点和标签节点间的距离进行测距，并经过预处理消除时钟偏移和时钟漂移带来的误差，得到消除误差后的距离测试值，并得到移动目标的粗略位置坐标；
(3)将步骤(2)每次测距采集到CIR信号的接收信号强度指示RSSI、第一路径功率强度FPPL、上升时间RT特征采用测距误差缓解模糊推理定位算法得到NLOS测距误差估计；
(4)将步骤(2)消除误差后的距离测试值与步骤(3)模糊得到的NLOS测距误差估计相减，得到模糊后的距离数据；
(5)将模糊后距离数据作为观测值代入自适应抗NLOS卡尔曼算法，得到最终距离估计数据；
(6)将最终距离估计数据代入鲁棒性更强LS定位算法，完成标签节点的位置估计。
下面对每个步骤进行具体地阐述：
(1)建立UWB定位模型，在三维空间中，布置至少3个基站节点，在待测移动目标上设置标签节点，通过在节点上设置传感器来进行测距；
(2)利用UWB定位模型对基站节点和标签节点间的距离进行测距，并经过预处理消除时钟偏移和时钟漂移带来的误差，得到消除误差后的距离测试值，并得到移动目标的粗略位置坐标；具体步骤为：
设任一基站节点为节点A，待测移动目标为节点B，测试过程中，节点A向节点B发送测距信号，并记录下发送信号时间Ta1,节点B在接收到测距信号时记录下信号到达时间Tb1,并在间隔Treply1后向节点A发送确认UWB信号，在发送时记录下发送时刻Tb2,节点A收到节点B发送信号后，进行类似节点B操作，记录下接收时间Ta2,延迟Treply2的时间再次向节点B发送信号，并记录发送时间Ta3，节点B再次在接收到信号后,记录下信号到达时间Tb3；节点A与节点B经过三次通信后，得出节点A与节点B真实传输信号的时间Ttof的值：
Figure BDA0002804273640000091
其中，Tround1＝Ta2-Ta1，Tround2＝Tb3-Tb2，Treply1＝Tb2-Tb1，Treply2＝Ta3-Ta2；
根据每个基站节点与标签节点之间距离计算公式di＝cti，其中c为信号传输的速率即光速，消除误差后的距离测试值为：
di＝cTtof (2)
根据消除误差后的距离测试值，基于测距原理建立坐标方程，采用最小二乘法，计算UWB移动目标的坐标，获取移动目标的粗略位置坐标。
(3)将步骤(2)每次测距采集到CIR信号的接收信号强度指示RSSI、第一路径功率强度FPPL、上升时间RT特征采用测距误差缓解模糊推理定位算法得到NLOS测距误差估计；
首先讨论UWB的CIR信号特征选取，标准UWB的CIR信号常用的特征有：总能量，最大幅度，归一化最强路径，信噪比，上升时间，平均超量延迟，均方根延迟和峰度。基于CIR信号特征测距误差缓解模糊推理算法是基于DWM1000硬件给出三个CIR信号特征，它们分别是接收信号强度指示(RSSI)、第一路径功率强度(FPPL)、上升时间(RT)。
接收信号强度指示(RSSI)是在几乎所有无线电设备都具备信号特征，DWM1000也不例外，接收信号能量反映接收功率平均水平，这与标准CIR信号中总能量特征是非常相关的，因此通常用接收能量替代。
第一路径功率强度是由DWM1000官方给出的，用来统计最先到达接收器第一路径信号功率估计。
标准上升时间定义为：
RTstandard＝tstop-tstart (32)
其中，tstart＝min{ti:|r(ti)|≥0.1Fmax}，tstop＝min{ti:|r(ti)|≥0.9Fmax}，ti表示CIR第i次的采样时间，r(ti)表示ti时刻采样的幅值，Fmax表示CIR信号所有采样点中最大幅值。但最大幅值的采样时间
Figure BDA0002804273640000101
可以通过官方指定寄存器读取，而标准上升时间公式中定义中tstart与tstop需要对CIR信号所有采样的幅值进行比较才能获得，读取CIR采样点幅值需要大量时间，会大大延长测距时间，因此采用标准上升时间定义是不合适。于是，本发明将上升时间重新定义为第一路径第一采样点所在的时间与最大幅值的采样点所在时间之差，上升时间公式(33)所示。
Figure BDA0002804273640000102
其中，
Figure BDA0002804273640000103
表示第一路径第一采样幅值F1所在采样时间，这个值可以通过官方指定寄存器读取。
经过试验分析CIR的RSSI、FPPL和RT信号特征与测距误差的关系，结果证明CIR的RSSI、FPPL以及RT特征从不同程度上反映了NLOS情况严重程度。RSSI反映是NLOS大体上严重程度，可以有效进行对分类，而FPPL、RT对NLOS小变化进行反映，因此基于CIR信号特征测距误差缓解系统是可行的。
所述测距误差缓解模糊推理定位算法所用的模糊集和模糊规则的建立过程为：
3.1非视距实验场景下，将基站节点放在固定位置，标签节点位置从相距0.5m处开始，然后将障碍物相距基站10cm的位置开始，采集500次实验数据为一组，每测一组就将障碍物到基站距离增加20cm，一直测到障碍物无法在标签和基站之间为止，当所有障碍物测完后，增加标签与基站距离，重复之前操作，一直测到3米为止；试验过程中采集UWB的CIR信号特征RSSI、FPPL、RT以及计算得到NLOS测距误差；
3.2视距实验场景下，即无障碍物情况，将基站放在固定位置，标签位置从相距0.5m处开始，每间隔0.5m作为一个测试点，一直测到3米为止；试验过程中采集UWB的CIR信号特征RSSI、FPPL、RT以及计算得到NLOS测距误差；
3.3对步骤3.1和3.2采集到的实验数据进行分析，将输入信号特征RSSI分成5个模糊集合，用非常大、大、中、小和非常小来划分集合，将输入信号特征FPPL分为6个模糊集合，用非常大、大、中、小、非常小以及非常非常小来划分，对于输入信号特征RT是与NLOS测距误差影响成正相关的，分为非常非常小、非常小、小、中、大5个模糊集合，同时对于输出NLOS误差分为非常小、小、中、大、非常大5个模糊集合；结合实验数据得到模糊规则；本发明根据实际测试提供一种模糊规则如下：
1)If(RSSI is非常大)and(FPPL is非常大)and(RT is非常非常小)then(测距误差is非常小)；
2)If(RSSI is非常大)and(FPPL is非常大)and(RT is非常小)then(测距误差is非常小)；
3)If(RSSI is非常大)and(FPPL is大)and(RT is非常非常小)then(测距误差is非常小)；
4)If(RSSI is非常大)and(FPPL is大)and(RT is非常小)then(测距误差is非常小)；
5)If(RSSI is非常大)and(FPPL is中)and(RT is非常非常小)then(测距误差is小)；
6)If(RSSI is非常大)and(FPPL is小)and(RT is大)then(测距误差is小)；
7)If(RSSI is大)and(FPPL is非常大)and(RT is非常非常小)then(测距误差is小)；
8)If(RSSI is大)and(FPPL is大)and(RT is非常非常小)then(测距误差is小)；
9)If(RSSI is大)and(FPPL is大)and(RT is非常小)then(测距误差is小)；
10)If(RSSI is大)and(FPPL is中)and(RT is小)then(测距误差is小)；
11)If(RSSI is大)and(FPPL is非常非常小)and(RT is小)then(测距误差is小)；
12)If(RSSI is大)and(FPPL is非常小)and(RT is小)then(测距误差is小)；
13)If(RSSI is大)and(FPPL is非常小)and(RT is中)then(测距误差is中)；
14)If(RSSI is大)and(FPPL is中)and(RT is非常非常小)then(测距误差is中)；
15)If(RSSI is大)and(FPPL is中)and(RT is非常小)then(测距误差is中)；
16)If(RSSI is中)and(FPPL is小)and(RT is小)then(测距误差is中)；
17)If(RSSI is中)and(FPPL is小)and(RT is中)then(测距误差is中)；
18)If(RSSI is非常大)and(FPPL is中)and(RT is小)then(测距误差is大)；
19)If(RSSI is非常大)and(FPPL is小)and(RT is小)then(测距误差is大)；
20)If(RSSI is大)and(FPPL is小)and(RT is小)then(测距误差is大)；
21)If(RSSI is中)and(FPPL is非常小)and(RT is中)then(测距误差is大)；
22)If(RSSI is小)and(FPPL is小)and(RT is小)then(测距误差is大)；
23)If(RSSI is小)and(FPPL is小)and(RT is大)then(测距误差is大)；
24)If(RSSI is非常大)and(FPPL is非常小)and(RT is大)then(测距误差is非常大)；
25)If(RSSI is小)and(FPPL is非常小)and(RT is大)then(测距误差is非常大)
将每一次测距采集到的CIR信号特征RSSI、FPPL、RT与所述模糊规则进行对应，得到对应的NLOS测距误差的等级及NLOS测距误差估计。
(4)将步骤(2)消除误差后的距离测试值与步骤(3)模糊得到的NLOS测距误差估计相减，得到模糊后的距离数据；
(5)将模糊后距离数据作为观测值代入自适应抗NLOS卡尔曼算法，得到最终距离估计数据；所述的自适应抗NLOS卡尔曼算法为，采用卡尔曼滤波算法对模糊后的距离数据进行滤波，利用3倍新息的方差与新息比较来判断是否有NLOS存在，如果存在则通过对新息进行固定比例缩放来减小NLOS测距误差，流程图参见图2，具体步骤为：
假设在短的时间内，标签节点与基站节点之间距离是均匀变化的，标签节点运动状态为x(k)＝[d(k)v(k)]T，其中：d(k)表示k时刻标签节点到基站的距离，为除去噪声后的估计值；v(k)表示k时刻标签节点的速度；w(k-1)＝[wd(k-1)wv(k-1)]T表示标签在运动过程中必然受到过程噪声干扰，其中：wd(k-1)表示k-1时刻距离过程噪声，wv(k-1)表k-1时刻示速度过程噪声，于是可以得到系统状态方程：
Figure BDA0002804273640000121
其中：T代表采样时间；将式(3)化为矩阵的形式，如公式(4)所示：
Figure BDA0002804273640000122
上式进一步简化可得矩阵形式状态方程模型，即式(5)，其中是Fk状态转移矩阵，wk-1是k-1时刻过程噪声，服从均值为0，方差矩阵为Qk-1的高斯分布；
xk＝Fkxk-1+wk-1 (5)
在基于滤波卡尔曼滤波系统中，设观测量为模糊后的测距距离值，z(k)表示第k时刻模糊后距离数据，r(k)表示测距误差，于是观测方程可以写成：
Figure BDA0002804273640000123
将(6)式写成矩阵形式为：
zk＝Hkxk+vk (7)
其中，Hk为观测矩阵，vk表示k时刻观测噪声，服从均值为0，方差矩阵为Rk的高斯分布；
式(5)和式(7)组成用于对测距数据滤波的卡尔曼滤波系统模型；假设k-1时刻卡尔曼滤波估计状态为
Figure BDA0002804273640000131
以及k时刻模糊后距离数据为zk，则卡尔曼滤波计算k时刻估计状态
Figure BDA0002804273640000132
的步骤为：
5.1一步状态预测
根据k-1时刻KF估计状态
Figure BDA0002804273640000133
预测k时刻状态，即根据k-1时刻观测值zk-1对真实状态xk线性最小方差估计，即：
Figure BDA0002804273640000134
其中，
Figure BDA0002804273640000135
表示k-1时刻最小均方估计；由于公式(5)可知，wk-1只对xk的值影响，因此E{wk-1/zk-1}的值为0于是可以得到一步预测状态公式(9)为：
Figure BDA0002804273640000136
5.2误差协方差矩阵预测
定义：
Figure BDA0002804273640000137
Figure BDA0002804273640000138
其中，
Figure BDA0002804273640000139
表示一步预测估计代替真实状态xk引起误差，
Figure BDA00028042736400001310
表示将一步预测估计值
Figure BDA00028042736400001311
代入观测方程得到预测观测值；
Figure BDA00028042736400001312
表示预测观测值与真实观测值之间的误差，也称为新息Δk；设第k时刻新息Δk和其方差矩阵Dk为：
Figure BDA00028042736400001313
Figure BDA00028042736400001314
根据3倍方差理论并结合式(12)和式(13)可以得到一个测量值是否受到NLOS的判断公式：
Figure BDA00028042736400001315
其中，
Figure BDA00028042736400001316
表示Dk矩阵对角线上第i个元素，
Figure BDA00028042736400001317
表达Δk的第i个元素，当式(3.27)不满足时候，对其新息修正，如公式(15)所示：
Figure BDA00028042736400001318
其中，a表示新息修正系数，a根据新息与新息方差之间差变化来自动调节其取值，自动调节α的方法如下：
Figure BDA0002804273640000141
其中，b为修正系数α的最大缩小比例，取值范围在0到1之间；
由公式(10)和(11)可知，新息中包含部分真实状态的信息，于是对
Figure BDA0002804273640000142
进行适当加权获得
Figure BDA0002804273640000143
信息，于是修正后状态估计为：
Figure BDA0002804273640000144
其中，Kk为卡尔曼增益矩阵；假设修正后估计
Figure BDA0002804273640000145
与真实状态xk之间估计误差为：
Figure BDA0002804273640000146
于是估计误差的协方差矩阵定义为：
Figure BDA0002804273640000147
一步预测误差的协方差矩阵定义为：
Figure BDA0002804273640000148
并且已知
Figure BDA0002804273640000149
其中Rk表示观测噪声方差，估计误差与观测噪声互不相关，即
Figure BDA00028042736400001410
代入式(3.17)可得：
Pk＝(I-KkHk)Pk|k-1(I-KkHk)T+KkRkKT (21)
要写出递推公式，还要分析从Pk-1到Pk|k-1的递推公式；将公式(3.7)和公式(3.11)代入一步预测状态估计误差公式可得：
Figure BDA00028042736400001411
由于k-1时刻估计误差与当前k时刻过程噪声之间也互不相关，即:
Figure BDA00028042736400001412
代入式(3.17)可得一步预测误差的协方差矩阵：
Pk|k-1＝FkPk-1Fk T+Qk-1 (23)
在得到一步预测误差协方差的递推公式后，需要进一步讨论增益矩阵Kk值的选择，以使得估计误差协方差Pk最小；
5.3增益矩阵Kk
为了使得估计误差协方差Pk最小，通常矩阵的迹最小，即：
Figure BDA00028042736400001413
将式(23)代入上式，根据矩阵迹的求导公式，同时考虑到Pk|k-1、Rk为对称矩阵，计算可得：
Figure BDA0002804273640000151
令公式(25)等于零，即可求得卡尔曼滤波增益矩阵为：
Figure BDA0002804273640000152
进一步将式(26)代入公式(23)，可将估计误差协方差矩阵简化为：
Pk＝(I-KkHk)Pk|k-1 (27)
5.4更新过程，根据式(14)更新状态估计
Figure BDA0002804273640000153
5.5更新过程，根据式(24)更新估计误差的协方差矩阵Pk；
5.6置k＝k+1，循环进行步骤5.1到步骤5.5，进行下一次滤波计算，得到最终状态估计
Figure BDA0002804273640000154
通过标签节点的运动状态方程x(k)＝[d(k)v(k)]T得到最终距离估计数据。
(6)将最终距离估计数据代入鲁棒性更强LS定位算法，完成标签节点的位置估计，具体过程为：将得到三个基站距离数据d1、d2、d3代入到TOA定位方程中，即：
Figure BDA0002804273640000155
在公式(28)中，从第二个方程到最后一个方程依次减去第一方程，得：
Figure BDA0002804273640000156
为了方便求解，可将其转换成线性方程组矩阵形式为：
AX＝b (30)
其中，
Figure BDA0002804273640000157
对公式(30)用最小二乘原理可得到标签的位置估计：
X＝(ΑTA)-1ATb (31)。
定位算法实验
为了更好验证本发明基于模糊推理与自适应卡尔曼滤波融合定位算法的性能，对基于模糊推理定位算法(只采用本发明的步骤(1)-(4)得到距离数据，通过最小二乘法计算位置坐标)，自适应抗NLOS卡尔曼定位算法(即没有进行步骤(2)-(5)的处理，直接将初始采集到的距离值进行步骤(5)和(6)的处理)以及基于模糊推理与自适应抗NLOS卡尔曼滤波融合定位算法三种算法在真实NLOS环境下进行静态定位、动态定位实验对比。
(一)静态定位实验
本次实验场选择在实验室内长为200cm、宽为300cm矩形区域进行，先将三个基站节点依次进行编号A1、A2、A3，分别放置矩形区域三个顶角，基站A1为坐标原点，设为(0,0)，基站A2和基站A3分别为(200,0)、(0,300)，而后将标签节点依次放置于(50,150)、(50,175)、(50,200)、(75,200)、(100,200)处进行定位测试，在然后将总基站节点在定位区域的外围放好，并通过USB线与装有上位机计算机相连接，最后是铁皮障碍物放置在(0,68)处。
本次实验的实验步骤为：
(1)按照节点类别分别将编译好的程序下载到各个UWB节点中。
(2)设置实验环境，包括布置基站节点、标签节点、总基站、数据处理计算机以及障碍物；
(3)实验环境设置后，按顺序依次打开总基站节点、基站节点和标签节点，进行组网通信。
(4)打开定位系统软件，首先设置算法参数，基于模糊推理与自适应卡尔曼滤波融合定位算法和自适应抗NLOS卡尔曼定位算法参数设置如下：
Figure BDA0002804273640000161
然后将基站节点实际坐标在软件里进行设置，并选择定位模式，最后打开串口，定位系统会每间隔0.5秒进行一次定位，当采集到600多次定位数据就关闭串口，在调整标签节点位置，进行下一次定位实验，直到所有定位测试点测试完毕。
(5)最后采集到定位数据用Matlab进行数据分析。
实验结果分析：
本次实验在NLOS条件下对5个位置进行静态定位测试，经过Matlab对这5个测试点定位数据分析，可以得到对比三个算法在5个测试点均方根误差，如表1所示。
表1非视距情况下静态定位RMSE
Figure BDA0002804273640000171
从表1中数据可知，本发明基于模糊推理与自适应卡尔曼滤波融合定位算法在5次定位测试中的均方根误差最小，模糊推理算法次之，自适应抗NLOS卡尔曼算法的均方根误差最大。
为了更直观比较三种算法的性能，将对标签节点在(50,150)实验数据采用散点图和CDF图进行分析，算法定位散点图如图3所示，同时三种算法的CDF如图4所示。
从图3中可以明显看出基于模糊推理与自适应卡尔曼滤波融合定位算法的散点图中心和基于模糊推理定位算法散点图中心比较靠近目标坐标位置，其中基于模糊推理与自适应卡尔曼滤波融合定位算法相比较基于模糊推理定位算法散点图更为集中，由此可见基于模糊推理定位算法融合自适应抗NLOS卡尔曼算法后对于融合前存在一些模糊估计NLOS误差与实际NLOS误差偏差较大情况有了很好改善。从图4中，基于模糊推理与自适应抗NLOS卡尔曼融合算法的误差距离小于9.1cm的概率为0.97，而同等条件下，模糊推理算法概率为0.69，自适应抗NLOS卡尔曼算法概率仅为0，综上所述，对存在NLOS室内静态目标的定位来说，基于模糊推理与自适应卡尔曼滤波融合定位算法相比基于模糊推理算法、自适应抗NLOS卡尔曼算法，定位精度最佳。
(二)动态定位实验
本次实验场选择在实验室内长为200cm、宽为300cm矩形区域进行，先将三个基站节点依次进行编号A1、A2、A3，分别放置矩形区域三个顶角，基站A1为坐标原点，设为(0,0)，基站A2和基站A3分别为(200,0)、(0,300)，而后将标签节点放置到高为4cm小车，因此UWB标签与UWB实际测得距离应为：
Figure BDA0002804273640000172
dc表示测得距离，并将小车放置在(50,50)处，为了更好进行动态定位测试，本文采用小车循迹的方式，接下来按照预先设定轨迹铺设黑色线，黑色线起点和终点分别设置在(50,50)和(150,249.7)处。然后将总基站节点在定位区域的外围放好，并通过USB线与装有上位机计算机相连接，最后是两个铁皮障碍物放置，分别放置在(11,35)和(64,240)处。
本次实验的实验步骤为：
(1)按照节点类别分别将编译好的程序下载到各个UWB节点中。
(2)设置实验环境，包括布置基站节点、标签节点、总基站、数据处理计算机以及障碍物；
(3)实验环境设置后，按顺序依次打开总基站节点、基站节点和标签节点，进行组网通信。
(4)打开定位系统软件，首先设置算法参数，基于模糊推理与自适应卡尔曼滤波融合定位算法和自适应抗NLOS卡尔曼定位算法参数设置如下：
Figure BDA0002804273640000181
然后将基站节点实际坐标在软件里进行设置，并选择跟踪模式，最后打开串口开始接收数据，并打开小车电源，小车以19.77cm/s速度按照地面上黑线轨迹作匀速运动，在小车移动过程中，系统每间隔0.5秒进行一次定位，一直到小车到达最终位置，就停止数据采集。
(5)最后采集到定位数据用Matlab进行数据分析。
实验结果分析：
本次实验在NLOS条件下进行动态态定位测试，经过Matlab对定位数据分析，可以得到三种算法定位轨迹如图5所示，定位轨迹的RMSE曲线图如图6所示，以及算法的X方向、Y方向以及定位RMSE如表2所示。
表2非视距情况下移动定位RMSE
Figure BDA0002804273640000182
从图5和图6可知，模糊推理与自适应抗NLOS卡尔曼融合算法的定位轨迹在后半段比较贴近目标轨迹，并且定位RMSE大部分时刻都低于其他两种算法，这主要得益于历史信息与CIR信号特征信息结合能够更好抗NLOS误差。从表2数据可知，基于模糊推理与自适应抗NLOS卡尔曼融合算法同基于模糊推理算法和自适应抗NLOS卡尔曼算法相比定位精度均有不同程度地提高，同自适抗NLOS卡尔曼算法相比，在X方向RMSE降低了50.27％，在Y方向上RMSE降低了32.20％，定位RMSE降低了41.69％，而同基于模糊推理算法相比，在X方向的RMSE降低了22.49％，Y方向RMSE降低了15.25％，定位RMSE降低了18.46％。综上所述，对存在NLOS室内移动目标的定位来说，本发明基于模糊推理与自适应卡尔曼滤波融合定位算法与基于模糊推理的定位算法、自适应抗NLOS卡尔曼定位算法相比，定位精度最佳。
上述说明是针对本发明较佳可行实施例的详细说明，但实施例并非用以限定本发明的专利申请范围，凡本发明所提示的技术精神下所完成的同等变化或修饰变更，均应属于本发明所涵盖专利范围。

Claims (5)

1.一种基于UWB移动节点的移动目标定位算法，其特征在于，包括以下步骤：
(1)建立UWB定位模型，在三维空间中，布置至少3个基站节点，在待测移动目标上设置标签节点；
(2)利用UWB定位模型对基站节点和标签节点间的距离进行测距，并经过预处理消除时钟偏移和时钟漂移带来的误差，得到消除误差后的距离测试值，并得到移动目标的粗略位置坐标；
(3)将步骤(2)每次测距采集到CIR信号的接收信号强度指示RSSI、第一路径功率强度FPPL、上升时间RT特征采用测距误差缓解模糊推理定位算法得到NLOS测距误差估计；
(4)将步骤(2)消除误差后的距离测试值与步骤(3)模糊得到的NLOS测距误差估计相减，得到模糊后的距离数据；
(5)将模糊后距离数据作为观测值代入自适应抗NLOS卡尔曼算法，得到最终距离估计数据；
(6)将最终距离估计数据代入鲁棒性更强LS定位算法，完成标签节点的位置估计。
2.根据权利要求1所述的一种基于UWB移动节点的移动目标定位算法，其特征在于：步骤(2)的具体步骤为：
设任一基站节点为节点A，待测移动目标为节点B，测试过程中，节点A向节点B发送测距信号，并记录下发送信号时间Ta1,节点B在接收到测距信号时记录下信号到达时间Tb1,并在间隔Treply1后向节点A发送确认UWB信号，在发送时记录下发送时刻Tb2,节点A收到节点B发送信号后，进行类似节点B操作，记录下接收时间Ta2,延迟Treply2的时间再次向节点B发送信号，并记录发送时间Ta3，节点B再次在接收到信号后,记录下信号到达时间Tb3；节点A与节点B经过三次通信后，得出节点A与节点B真实传输信号的时间Ttof的值：
Figure FDA0002804273630000011
其中，Tround1＝Ta2-Ta1，Tround2＝Tb3-Tb2，Treply1＝Tb2-Tb1，Treply2＝Ta3-Ta2；
根据每个基站节点与标签节点之间距离计算公式di＝cti，其中c为信号传输的速率即光速，消除误差后的距离测试值为：
di＝cTtof (2)
根据消除误差后的距离测试值，基于测距原理建立坐标方程，采用最小二乘法，计算UWB移动目标的坐标，获取移动目标的粗略位置坐标。
3.根据权利要求1所述的一种基于UWB移动节点的移动目标定位算法，其特征在于：步骤(3)所述测距误差缓解模糊推理定位算法所用的模糊集和模糊规则的建立过程为：
3.1非视距实验场景下，将基站节点放在固定位置，标签节点位置从相距0.5m处开始，然后将障碍物相距基站10cm的位置开始，采集500次实验数据为一组，每测一组就将障碍物到基站距离增加20cm，一直测到障碍物无法在标签和基站之间为止，当所有障碍物测完后，增加标签与基站距离，重复之前操作，一直测到3米为止；试验过程中采集UWB的CIR信号特征RSSI、FPPL、RT以及计算NLOS测距误差；
3.2视距实验场景下，即无障碍物情况，将基站放在固定位置，标签位置从相距0.5m处开始，每间隔0.5m作为一个测试点，一直测到3米为止；试验过程中采集UWB的CIR信号特征RSSI、FPPL、RT以及计算NLOS测距误差；
3.3对步骤3.1和3.2采集到的实验数据进行分析，将输入信号特征RSSI分成5个模糊集合，用非常大、大、中、小和非常小来划分集合，将输入信号特征FPPL分为6个模糊集合，用非常大、大、中、小、非常小以及非常非常小来划分，对于输入信号特征RT是与NLOS测距误差影响成正相关的，分为非常非常小、非常小、小、中、大5个模糊集合，同时对于输出NLOS误差分为非常小、小、中、大、非常大5个模糊集合；结合实验数据得到模糊规则；
将每一次测距采集到的CIR信号特征RSSI、FPPL、RT与所述模糊规则进行对应，得到对应的NLOS测距误差的等级及NLOS测距误差估计。
4.根据权利要求1所述的一种基于UWB移动节点的移动目标定位算法，其特征在于：步骤(5)所述的自适应抗NLOS卡尔曼算法为，采用卡尔曼滤波算法对模糊后的距离数据进行滤波，利用3倍新息的方差与新息比较来判断是否有NLOS存在，如果存在则通过对新息进行固定比例缩放来减小NLOS测距误差，具体步骤为：
假设在短的时间内，标签节点与基站节点之间距离是均匀变化的，标签节点运动状态为x(k)＝[d(k) v(k)]T，其中：d(k)表示k时刻标签节点到基站的距离，为除去噪声后的估计值；v(k)表示k时刻标签节点的速度；w(k-1)＝[wd(k-1) wv(k-1)]T表示标签在运动过程中必然受到过程噪声干扰，其中：wd(k-1)表示k-1时刻距离过程噪声，wv(k-1)表k-1时刻示速度过程噪声，于是可以得到系统状态方程：
Figure FDA0002804273630000031
其中：T代表采样时间；将式(3)化为矩阵的形式，如公式(4)所示：
Figure FDA0002804273630000032
上式进一步简化可得矩阵形式状态方程模型，即式(5)，其中Fk是状态转移矩阵，wk-1是k-1时刻过程噪声，服从均值为0，方差矩阵为Qk-1的高斯分布；
xk＝Fkxk-1+wk-1 (5)
在基于滤波卡尔曼滤波系统中，设观测量为模糊后的测距距离值，z(k)表示第k时刻模糊后距离数据，r(k)表示测距误差，于是观测方程可以写成：
Figure FDA0002804273630000033
将(6)式写成矩阵形式为：
zk＝Hkxk+vk (7)
其中，Hk为观测矩阵，vk表示k时刻观测噪声，服从均值为0，方差矩阵为Rk的高斯分布；
式(5)和式(7)组成用于对测距数据滤波的卡尔曼滤波系统模型；假设k-1时刻卡尔曼滤波估计状态为
Figure FDA0002804273630000034
以及k时刻模糊后距离数据为zk，则卡尔曼滤波计算k时刻估计状态
Figure FDA0002804273630000035
的步骤为：
5.1一步状态预测
根据k-1时刻KF估计状态
Figure FDA0002804273630000036
预测k时刻状态，即根据k-1时刻观测值zk-1对真实状态xk线性最小方差估计，即：
Figure FDA0002804273630000037
其中，
Figure FDA0002804273630000038
表示k-1时刻最小均方估计；由于公式(5)可知，wk-1只对xk的值影响，因此E{wk-1/zk-1}的值为0于是可以得到一步预测状态公式(9)为：
Figure FDA0002804273630000039
5.2误差协方差矩阵预测
定义：
Figure FDA00028042736300000310
Figure FDA0002804273630000041
其中，
Figure FDA0002804273630000042
表示一步预测估计代替真实状态xk引起误差，
Figure FDA0002804273630000043
表示将一步预测估计值
Figure FDA0002804273630000044
代入观测方程得到预测观测值；
Figure FDA0002804273630000045
表示预测观测值与真实观测值之间的误差，也称为新息Δk；设第k时刻新息Δk和其方差矩阵Dk为：
Figure FDA0002804273630000046
Figure FDA0002804273630000047
根据3倍方差理论并结合式(12)和式(13)可以得到一个测量值是否受到NLOS的判断公式：
Figure FDA00028042736300000420
其中，
Figure FDA0002804273630000048
表示Dk矩阵对角线上第i个元素，
Figure FDA0002804273630000049
表达Δk的第i个元素，当式(14)不满足时候，对其新息修正，如公式(15)所示：
Figure FDA00028042736300000410
其中，a表示新息修正系数，a根据新息与新息方差之间差变化来自动调节其取值，自动调节α的方法如下：
Figure FDA00028042736300000411
其中，b为修正系数α的最大缩小比例，取值范围在0到1之间；
由公式(10)和(11)可知，新息中包含部分真实状态的信息，于是对
Figure FDA00028042736300000412
进行适当加权获得
Figure FDA00028042736300000413
信息，于是修正后状态估计为：
Figure FDA00028042736300000414
其中，Kk为卡尔曼增益矩阵；假设修正后估计
Figure FDA00028042736300000415
与真实状态xk之间估计误差为：
Figure FDA00028042736300000416
于是估计误差的协方差矩阵定义为：
Figure FDA00028042736300000417
一步预测误差的协方差矩阵定义为：
Figure FDA00028042736300000418
并且已知
Figure FDA00028042736300000419
其中Rk表示观测噪声方差，估计误差与观测噪声互不相关，即
Figure FDA0002804273630000051
代入式(20)可得：
Pk＝(I-KkHk)Pk|k-1(I-KkHk)T+KkRkKT (21)
要写出递推公式，还要分析从Pk-1到Pk|k-1的递推公式；将公式(5)和公式(9)代入一步预测状态估计误差公式可得：
Figure FDA0002804273630000052
由于k-1时刻估计误差与当前k时刻过程噪声之间也互不相关，即:
Figure FDA0002804273630000053
代入式(20)可得一步预测误差的协方差矩阵：
Figure FDA0002804273630000054
在得到一步预测误差协方差的递推公式后，需要进一步讨论增益矩阵Kk值的选择，以使得估计误差协方差Pk最小；
5.3增益矩阵Kk
为了使得估计误差协方差Pk最小，通常矩阵的迹最小，即：
Figure FDA0002804273630000055
将式(23)代入上式，根据矩阵迹的求导公式，同时考虑到Pk|k-1、Rk为对称矩阵，计算可得：
Figure FDA0002804273630000056
令公式(25)等于零，即可求得卡尔曼滤波增益矩阵为：
Figure FDA0002804273630000057
进一步将式(26)代入公式(23)，可将估计误差协方差矩阵简化为：
Pk＝(I-KkHk)Pk|k-1 (27)
5.4更新过程，根据式(14)更新状态估计
Figure FDA0002804273630000058
5.5更新过程，根据式(24)更新估计误差的协方差矩阵Pk；
5.6置k＝k+1，循环进行步骤5.1到步骤5.5，进行下一次滤波计算，得到最终状态估计
Figure FDA0002804273630000059
通过标签节点的运动状态方程x(k)＝[d(k)v(k)]T得到最终距离估计数据。
5.根据权利要求1所述的一种基于UWB移动节点的移动目标定位算法，其特征在于：步骤(6)所述的将最终估计距离数据代入鲁棒性更强LS定位算法，具体过程为：将得到三个基站距离数据d1、d2、d3代入到TOA定位方程中，即：
Figure FDA0002804273630000061
在公式(28)中，从第二个方程到最后一个方程依次减去第一方程，得：
Figure FDA0002804273630000062
为了方便求解，可将其转换成线性方程组矩阵形式为：
AX＝b (30)
其中，
Figure FDA0002804273630000063
对公式(30)用最小二乘原理可得到标签的位置估计：
X＝(ΑTA)-1ATb (31)。
CN202011362064.8A 2020-11-27 2020-11-27 一种基于uwb移动节点的移动目标定位算法 Active CN112533149B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011362064.8A CN112533149B (zh) 2020-11-27 2020-11-27 一种基于uwb移动节点的移动目标定位算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011362064.8A CN112533149B (zh) 2020-11-27 2020-11-27 一种基于uwb移动节点的移动目标定位算法

Publications (2)

Publication Number Publication Date
CN112533149A true CN112533149A (zh) 2021-03-19
CN112533149B CN112533149B (zh) 2022-06-07

Family

ID=74994404

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011362064.8A Active CN112533149B (zh) 2020-11-27 2020-11-27 一种基于uwb移动节点的移动目标定位算法

Country Status (1)

Country Link
CN (1) CN112533149B (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113376580A (zh) * 2021-06-30 2021-09-10 中国科学院深海科学与工程研究所 一种提高水声定位系统定位精度的位置解算方法
CN114205903A (zh) * 2021-12-07 2022-03-18 中国矿业大学 一种基于uwb的联合定位方法
CN114285500A (zh) * 2021-12-14 2022-04-05 电子科技大学 一种uwb室内定位信道质量评估方法
CN114501322A (zh) * 2022-01-20 2022-05-13 同济大学 一种差分UWB(ultra-wide band)定位方法
CN114608580A (zh) * 2022-03-10 2022-06-10 吉林大学 一种室内移动目标定位方法及系统
CN114938537A (zh) * 2022-06-24 2022-08-23 润芯微科技(江苏)有限公司 一种基于uwb实现的辅助定位方法
CN115348540A (zh) * 2022-08-16 2022-11-15 青岛柯锐思德电子科技有限公司 一种nlos环境下连续定位的跟踪方法
CN115639521A (zh) * 2022-10-24 2023-01-24 青岛柯锐思德电子科技有限公司 一种基于uwb的移动目标运动状态判断方法
WO2023151657A1 (zh) * 2022-02-10 2023-08-17 维沃移动通信有限公司 信息处理方法及通信设备
CN117518076A (zh) * 2023-11-07 2024-02-06 中南大学 一种管道内机器人定位的非视距误差识别和缓解方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104936148A (zh) * 2015-07-03 2015-09-23 中南大学 一种基于模糊knn的wifi室内定位方法
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
CN109946731A (zh) * 2019-03-06 2019-06-28 东南大学 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法
US20200116817A1 (en) * 2018-10-16 2020-04-16 The Regents Of The University Of Michigan Low-power, long-range rf localization system and method
CN111050282A (zh) * 2019-12-27 2020-04-21 杭州电子科技大学 一种多次模糊推理加权knn定位方法
CN111238480A (zh) * 2020-01-14 2020-06-05 西安电子科技大学 基于距离残差的nlos识别方法及在室内定位中的应用
CN111811503A (zh) * 2020-07-15 2020-10-23 桂林电子科技大学 基于超宽带和二维码的无迹卡尔曼滤波融合定位方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104936148A (zh) * 2015-07-03 2015-09-23 中南大学 一种基于模糊knn的wifi室内定位方法
US20200116817A1 (en) * 2018-10-16 2020-04-16 The Regents Of The University Of Michigan Low-power, long-range rf localization system and method
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
CN109946731A (zh) * 2019-03-06 2019-06-28 东南大学 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法
CN111050282A (zh) * 2019-12-27 2020-04-21 杭州电子科技大学 一种多次模糊推理加权knn定位方法
CN111238480A (zh) * 2020-01-14 2020-06-05 西安电子科技大学 基于距离残差的nlos识别方法及在室内定位中的应用
CN111811503A (zh) * 2020-07-15 2020-10-23 桂林电子科技大学 基于超宽带和二维码的无迹卡尔曼滤波融合定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
张宝军: "基于CNN 和在线学习的UWB 室内定位算法", 《传感技术学报》 *
许坤: "基于联邦滤波的室内移动机器人定位", 《电光与控制》 *

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113376580B (zh) * 2021-06-30 2022-12-02 中国科学院深海科学与工程研究所 一种提高水声定位系统定位精度的位置解算方法
CN113376580A (zh) * 2021-06-30 2021-09-10 中国科学院深海科学与工程研究所 一种提高水声定位系统定位精度的位置解算方法
CN114205903A (zh) * 2021-12-07 2022-03-18 中国矿业大学 一种基于uwb的联合定位方法
CN114205903B (zh) * 2021-12-07 2023-05-12 中国矿业大学 一种基于uwb的联合定位方法
CN114285500A (zh) * 2021-12-14 2022-04-05 电子科技大学 一种uwb室内定位信道质量评估方法
CN114501322A (zh) * 2022-01-20 2022-05-13 同济大学 一种差分UWB(ultra-wide band)定位方法
WO2023151657A1 (zh) * 2022-02-10 2023-08-17 维沃移动通信有限公司 信息处理方法及通信设备
CN114608580A (zh) * 2022-03-10 2022-06-10 吉林大学 一种室内移动目标定位方法及系统
CN114938537A (zh) * 2022-06-24 2022-08-23 润芯微科技(江苏)有限公司 一种基于uwb实现的辅助定位方法
CN115348540A (zh) * 2022-08-16 2022-11-15 青岛柯锐思德电子科技有限公司 一种nlos环境下连续定位的跟踪方法
CN115348540B (zh) * 2022-08-16 2023-05-16 青岛柯锐思德电子科技有限公司 一种nlos环境下连续定位的跟踪方法
CN115639521A (zh) * 2022-10-24 2023-01-24 青岛柯锐思德电子科技有限公司 一种基于uwb的移动目标运动状态判断方法
CN117518076A (zh) * 2023-11-07 2024-02-06 中南大学 一种管道内机器人定位的非视距误差识别和缓解方法
CN117518076B (zh) * 2023-11-07 2024-04-19 中南大学 一种管道内机器人定位的非视距误差识别和缓解方法

Also Published As

Publication number Publication date
CN112533149B (zh) 2022-06-07

Similar Documents

Publication Publication Date Title
CN112533149B (zh) 一种基于uwb移动节点的移动目标定位算法
CN109275095B (zh) 一种基于蓝牙的室内定位系统、定位设备和定位方法
CN109444813B (zh) 一种基于bp和dnn双神经网络的rfid室内定位方法
WO2019136918A1 (zh) 一种室内定位的方法、服务器及定位系统
CN109490826B (zh) 一种基于无线电波场强rssi的测距与位置定位方法
JP6251930B2 (ja) 位置推定システム
JP7239958B2 (ja) 動的環境に基づく自己適応室内融合位置決め方法
CN109816071A (zh) 一种基于rfid的室内目标追踪方法
CN110636436A (zh) 基于改进chan算法的三维uwb室内定位方法
Jadidi et al. Gaussian processes online observation classification for RSSI-based low-cost indoor positioning systems
CN112040405A (zh) 一种基于核极限学习机与粒子滤波的室内定位方法
Svertoka et al. Evaluation of real-life LoRaWAN localization: Accuracy dependencies analysis based on outdoor measurement datasets
CN113271542B (zh) 一种基于蓝牙及可见光的室内移动终端定位方法
CN108337639A (zh) 一种基于牛顿插值的蒙特卡罗移动节点定位算法
Alba et al. Uncertainty-boosted radiomap-based indoor positioning with RSSI fingerprints
Li et al. A dynamic adaptive indoor ranging model based on RSSI
Wang et al. WiFi positioning algorithm in tunnel based on Fuzzy C-means clustering and KNN algorithm
CN111487583A (zh) 一种数据融合下移动目标无线增强定位方法
CN110944293A (zh) 基于路径衰减和卡尔曼滤波融合的无线电搜索定位方法
CN114548159B (zh) 信号干扰下的超宽带精确定位方法
Wang et al. An advanced algorithm for Fingerprint Localization based on Kalman Filter
Chen et al. An Optimized Positioning Algorithm Based on Improved Gaussian Filtering
CN114051209B (zh) 一种基于智能反射面和场景几何模型的指纹定位方法
CN112566055B (zh) 一种基于射频指纹匹配的室内定位算法
Yang et al. An effective calibration method for wireless indoor positioning system with mixture Gaussian distribution model

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