一种基于UWB、RFID、INS多源联合定位技术的定位系统及定位方法

摘要

本发明涉及一种基于UWB、RFID、INS多源联合定位技术的定位系统及定位方法，包括定位终端、UWB定位模块、RFID定位模块、INS定位模块；UWB定位模块向定位终端发送UWB信号，判断定位终端转发回来的UWB信号是否满足预设的定位要求，如果满足，则采用TDOA和AOA定位估计中的相关算法获取定位终端的位置；否则，则采用RFID定位技术和INS定位技术获取所述定位终端的位置。本发明为室内突发状况的救援活动提供了可靠地定位服务，节约了救援时间，提高了救援活动的成功率，在公共安全领域可以广泛应用。

说明书

技术领域

本发明涉及一种基于UWB、RFID、INS多源联合定位技术的定位系统及定位方法，属于公共安全以及空间定位技术领域。

背景技术

在日益现代化的社会中，人们生活的信息化及智能化程度日新月异。随着经济的发展，还出现了大量集餐饮、购物、娱乐等功能于一体的大型便民场所、娱乐场所、购物场所、工作场所等一系列人口密集区域。对于这类特定环境来说，人员密度较大，环境结构复杂，人们对该区域的认识有限，一旦发生险情，处于该环境内的人员容易惊惶失措、盲目逃生，不仅人身安全受到极大威胁而且还会造成极大的经济损失。同时，复杂的室内环境构造也给救援活动造成了负担。进行救援活动时，确保救援人员的安全也是一项十分重要的任务，因此对事故源或者救援人员的准确位置获取显得尤为重要。

目前，已有的室内外导航定位技术按照其定位机制的不同主要分为：基于全球定位系统的定位技术、基于ZigBee的定位技术、基于射频标签的定位技术、基于无线局域网络的定位技术、基于惯性导航系统的定位技术等。

一、基于全球定位系统的定位技术

在大尺度定位方面，全球导航卫星系统(Global Navigation Satellite System，GNSS)涵盖了这个领域包括全球的、区域的和增强在内的所有卫星导航系统，如美国的全球定位系统(GPS)、俄罗斯的格洛纳斯卫星系统(Glonass)、欧洲的伽利略(Galileo)、中国的北斗卫星导航系统(Compass)，以及相关的增强系统，如美国的广域增强系统(WAAS)、欧洲的静地导航重叠系统(EGNOS)和日本的多功能运输卫星增强系统(MSAS)等，还涵盖在建和以后要建设的其他卫星导航系统。其中，GPS系统尤其得到了广泛的应用，可以实现导航、定位、授时等功能。但由于建筑内部接收不到GPS系统信号，所以不能直接采用GPS系统解决建筑内人员定位问题。

二、基于ZigBee的定位技术

ZigBee是一种新兴的短距离、低成本的无线网络技术，其优点是降低了系统的复杂度、功耗和数据速率，主要用于短距离无线连接。应用到定位中，需要在定位环境中构建众多的zigbee节点，由定位目标携带移动节点，方能在各个环境中实现对目标的定位。这种方法的主要缺点是定位精度差、依赖于布设定位节点的数量和位置，但架设难点多，传输距离较近等问题，因此这种系统实际应用的较少。

三、基于WiFi的定位技术

WiFi定位的主要原理是，通过侦测附近周围所有AP的MAC地址，发送到相关位置服务器上，服务器通过MAC地址的坐标，计算出所在地。客户端只需要侦听周围有哪些AP，检测每个AP信号的强弱，发送给定位服务器，服务器根据这些信息，查询每个AP在数据库中记录的坐标，通过计算，就能得出客户端的具体位置。因此，客户端侦听到的AP信号越多，定位就会越精确。但大范围部署AP成本较高，且需要有源客户端设备，维护工作量大。定位精确度室内可达到10m，与我们理想情况略有偏差。

UWB无线电技术在短距离精确定位方面具有独特的优势，通常采用功率谱密度极低、脉冲宽度极窄的基带脉冲来携带信息，时间分辨率极高，具有穿透树木、墙壁等障碍物的能力和天生的抗多径能力。

射频识别(Radio Frequency Identification,RFID)技术，又称无线射频识别，可通过无线电讯号识别特定目标并读写相关数据，无需识别系统与特识别目标之间建立机械或光学接触，可以在室内达到1-3米的定位精确度。随着社会经济的发展，RFID技术被广泛应用于自动收费和生产制造、物流、门禁系统、图书管理、供应链、动物和车辆识别等领域。因此人口密集的各种大型场所一般都有预先铺设好的的RFID设备，可以直接应用，无需其他的额外支出。

惯性导航系统(Inertial Navigation System,INS)也称作惯性参考系统，是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯性导航的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。

发明内容

针对现有技术的不足，本发明提供了一种基于UWB、RFID、INS多源联合定位技术的定位系统；

本发明还提供了上述定位系统的定位方法；

本发明在室内采用UWB定位技术进行定位，根据检测到的UWB信号能否达到准确定位的要求，决定是否切换至RFID定位技术定位，并采用INS定位技术对RFID定位结果进行修正，实现室内无缝精确定位。

本发明的技术方案为：

一种基于UWB、RFID、INS多源联合定位技术的定位系统，包括定位终端、UWB定位模块、RFID定位模块、INS定位模块；

所述UWB定位模块向所述定位终端发送UWB信号，判断所述定位终端转发回来的UWB信号是否满足预设的定位要求，如果满足，则采用TDOA和AOA定位估计中的相关算法获取所述定位终端的位置；否则，则采用RFID定位技术和INS定位技术获取所述定位终端的位置。

根据本发明优选的，所述定位终端内嵌UWB信号变频转发模块、信息处理单元、可更换的RFID射频标签及INS惯性测量单元；所述UWB定位模块包括不少于2个移动参考基站，每个移动参考基站均架设一个UWB信号收发机；

每个移动参考基站发送的UWB信号频率相同，接收频段固定；移动参考基站发射不同频率的UWB信号，定位终端接收UWB信号，并将接收到的UWB信号发送至信息处理单元，与此同时，还通过所述UWB信号变频转发模块对UWB信号变频，变频后的UWB信号返回至对应频段的移动参考基站；

INS惯性测量单元测量定位终端的加速度、角度、经度、纬度和高度，将测量得到的物理信息发送至所述信息处理单元，所述信息处理单元将测量得到的物理信息编码到UWB信号中，并通过所述UWB信号变频转发模块将UWB信号转发到所述UWB信号收发机。

RFID射频标签是一个可更换模块，可以根据具体应用场所选择对应可识别的RFID标签。

根据本发明优选的，所述INS惯性测量单元包括三轴数字加速度计、三轴数字罗盘、三轴数字陀螺仪。

上述定位系统的定位方法，具体步骤包括：

(1)每个移动参考基站向所述定位终端发射不同频率的UWB信号，即以UWB基带脉冲为基础的载波调制信号；

(2)所述定位终端接收UWB信号，并将接收到的UWB信号发送至所述信息处理单元，与此同时，还通过所述UWB信号变频转发模块对UWB信号变频，变频后的UWB信号返回至对应频段的移动参考基站；

(3)判断所述定位终端返回的变频后的UWB信号是否满足预设的定位要求，如果满足，进入步骤(4)，否则，进入步骤(5)；

(4)所述移动参考基站从返回的步骤(3)所述的变频后的UWB信号中检测与发射的UWB信号的时间差，将检测到的时间差及所述的变频后的UWB信号发送到指挥控制中心，指挥控制中心采用TDOA和AOA定位估计中的相关算法来计算双程传播时延，对双程传播时延进行定标处理，得到所述定位终端的位置信息；

(5)采用RFID定位技术和INS定位技术获取所述定位终端的位置。

根据本发明优选的，所述步骤(5)中，定位场所设有若干个RFID阅读器及一台主机，主机与若干个RFID阅读器通信，具体步骤包括：

a、通过所述主机获取若干个RFID阅读器的位置信息，并通过所述RFID阅读器检测所述定位终端的RFID射频标签，获取到标签信息，将标签信息传送到主机，指挥控制中心调用主机中的标签信息，采用TDOA定位算法对标签信息进行处理，对处理后的数据进行定标，获取所述定位终端的位置信息；

b、所述INS惯性测量单元测量所述定位终端的加速度、角度、经度、纬度和高度，将测量得到的物理信息发送至所述信息处理单元，所述信息处理单元将测量得到的物理信息编码到UWB信号中，并通过所述UWB信号变频转发模块将UWB信号转发到所述UWB收发机，经过卡尔曼滤波器，将物理信息利用联邦卡尔曼滤波估计算法进行数据融合，计算得出准确的所述定位终端的位置信息。

根据本发明优选的，所述步骤(3)，具体步骤包括：

c、设定所述定位终端返回的变频后的UWB信号在t时刻的信号值为y(t)，计算以下参量：UWB信号能量E_y、UWB信号的最大幅值y_max、平均附加时延τ_MED、均方根时延τ_RMS、峭度k，计算公式分别如式(Ⅰ)、式(II)、式(III)、式(IV)、式(Ⅴ)所示：

$E_y=\underset{-\infty }{\overset{\infty }{\int }}{\left|y\left(t\right)\right|}^{2}dt---\left(I\right);$

$y_{max}=\underset{t}{max}\left|y\left(t\right)\right|---\left(II\right);$

${\tau }_{MED}=\underset{-\infty }{\overset{\infty }{\int }}t\Lambda \left(t\right)dt---\left(III\right);$

式(III)中，

${\tau }_{RMS}=\underset{-\infty }{\overset{\infty }{\int }}{(t-{\tau }_{MED})}^2\Lambda \left(t\right)dt---\left(IV\right);$

$k=\frac{1}{{\sigma }_{\left|y\right|}^{4}T}\underset{T}{\int }{\left[\right|y\left(t\right)|-{\mu }_{\left|y\right|}]}^{4}dt---\left(V\right);$

式(Ⅴ)中，T是指信号周期，

d、计算得出不同标准环境下达到视距情况的UWB信号能量、UWB信号的最大幅值、平均附加时延、均方根时延、峭度的最小值为阈值，即：E_y′、y_max′、τ_MED′、τ_RMS′、k′，所述视距情况是指没有遮挡物、直接观测到的情况；如果步骤c计算得到的UWB信号能量E_y、UWB信号的最大幅值y_max、平均附加时延τ_MED、均方根时延τ_RMS、峭度k均分别大于其阈值E_y′、y_max′、τ_MED′、τ_RMS′、k′，则判定满足预设的定位要求，选用UWB信号的参考基站差分定位方案进行实时定位，否则，选用RFID技术进行定位。

根据本发明优选的，TDOA和AOA定位估计中的相关算法是指TDOA估计和AOA估计中的LTS-ESPRIT算法。

根据本发明优选的，经过卡尔曼滤波器，将物理信息利用联邦卡尔曼滤波估计算法进行数据融合，计算得出准确的所述定位终端的位置信息，所述卡尔曼滤波器包括位置子滤波器、速度子滤波器、主滤波器，所述位置子滤波器、所述速度子滤波器均连接所述主滤波器，所述INS定位模块分别连接所述位置子滤波器、所述速度子滤波器，所述RFID定位模块分别连接所述位置子滤波器、所述速度子滤波器，具体步骤包括：

e、所述位置子滤波器取所述INS定位模块与所述RFID定位模块的位置量测值之差Z₁(t)作为观测值，量测方程为：

$Z_1\left(t\right)=\left(\begin{array}{c}{\lambda }_{ins}-{\lambda }_{RFID}\\ L_{ins}-L_{RFID}\\ h_{ins}-h_{RFID}\end{array}\right)=H_{1}X\left(t\right)+V_1\left(t\right)---\left(VI\right)$

式(VI)中，λ_ins表示INS定位模块测量得到的经度值，L_ins表示INS定位模块测量得到的纬度值，h_ins表示INS定位模块测量得到的高度值，λ_RFID表示RFID定位模块测量得到的经度值，L_RFID表示RFID定位模块测量得到的纬度值，h_RFID表示RFID定位模块测量得到的高度值，V₁(t)表示高斯白噪声；

$H_1=\left(\begin{array}{ccccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right);$

f、所述速度子滤波器取所述INS定位模块与所述RFID定位模块的速度量测值之差Z₂(t)作为观测值，量测方程为：

$Z_2\left(t\right)=\left(\begin{array}{c}{v}_{ie}-v_{\mathrm{Re}}\\ v_{in}-v_{Rn}\\ v_{iu}-v_{Ru}\end{array}\right)=H_{2}X\left(t\right)+V_2\left(t\right)---\left(VII\right)$

v_ie为INS定位模块计算到的东向速度，v_in为INS定位模块计算到的北向速度，为INS定位模块计算到的天向速度，v_Re为RFID定位模块计算到的东向速度、v_Rn为RFID定位模块计算到的北向速度、v_Ru为RFID定位模块计算到的天向速度；

$H_2=\left(\begin{array}{ccccccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right);$

g、采用联邦滤波的信息融合方法，主滤波器将所述位置子滤波器得到的观测值Z₁(t)及所述速度子滤波器得到的观测值Z₂(t)进行融合，得到误差状态的全局估计值：

$\left(\begin{array}{c}{P}_g={(P_1^{-1}+P_2^{-1})}^{-1}\\ {\hat{X}}_g=P_g(P_1^{-1}{\hat{X}}_1+P_2^{-1}{\hat{X}}_2)\end{array}\right)---\left(VIII\right)$

P_g为联邦滤波后估计的协方差矩阵、P₁为所述位置子滤波器估计的协方差矩阵、P₂为所述速度子滤波器估计的协方差矩阵、联邦滤波估计的误差状态的全局估计值，为所述位置子滤波器估计的值，即所述位置子滤波器估计的状态参量；为所述速度子滤波器估计的值，即为所述速度子滤波器估计的状态；

h、用步骤a得到的所述定位终端的位置信息减去误差状态的全局估计值，得到实际的各种移动量，计算得出准确的所述定位终端的位置信息。

根据本发明优选的，所述步骤(4)，设定共设有n个移动参考基站，k＝{1,2,3…i…n}，

①第k个移动参考基站MR_k向所述定位终端发送UWB信号并接收转发回的变频的UWB信号，对移动参考基站MR_k而言，一次来回程时间t_observa为：

t_observa＝t_trans+t_delay>

式(IX)中，t_delay表示移动参考基站MR_k和定位终端之间传输的双程时延；t_trans是指实际传播时间；

②利用TDOA和AOA联合估计算法，利用接收信号较强的两个移动参考基站的信息计算出对应路径传播的时延以及传播角度，利用速度和时延相乘得到具体的距离信息，之后对定位终端的位置进行地图上的定标；

③设定n个移动参考基站的空间位置坐标分别为(x₁,y₁,z₁)，(x₂,y₂,z₂)，…(x_i,y_i,z_i)…(x_n,y_n,z_n)；定位终端坐标(x,y,z)的求取公式为：

$c·t_{trans}=c·(t_{observa}-t_{delay})=2\sqrt{{(x_k-x)}^2+{(y_k-y)}^2+{(z_k-z)}^2}---\left(X\right)$

C指的是光速，即3×10⁸m/s。

本发明的有益效果为：

1、本发明采用UWB定位技术：采用移动参考基站可以节约成本，重复利用，随时需要，随时移动；采用UWB基带脉冲为基础的载波调制信号形式的信号利于芯片设计和射频天线单元实现；收发信号在不同频率范围进行传输，发送信号不会对接收信号造成干扰，提高了系统可靠性；信号的发射与接收都由移动参考基站来完成，发送和接收基带处理部分使用同一时钟源，提高系统时钟精度，实现对多个定位终端的定位追踪；定位终端只进行模拟转发，不涉及任何数字信号处理，可以极大的降低设备成本，利于在大范围救援行动中应用推广。

2、本发明采用RFID定位技术：RFID阅读器读取标签时无需接触，避免了磨损，增长了使用寿命；RFID射频标签体积小、容量大，便于更换，可重复使用；RFID识别无需可见光源、强抗污染能力强，具有强耐久性，穿透性较强，可以工作在恶劣环境下，读取距离可远可进，RFID定位支持移动目标识别、多个目标快速读写、非可视目标识别、实时目标跟踪定位等；

3、本发明采用RFID定位技术：采用捷联惯导，将惯性测量装置直接安装在载体上,用计算机的数字计算来代替平台的模拟跟踪功能；省去了惯性平台，其成本、体积和重量都大幅降低，并且惯性仪表便于安装和维护，提高了系统的性能和可靠性；数字计算平台采用我们新提出的算法，提高了卡尔曼滤波结果的准确性。

4、本发明为室内突发状况的救援活动提供了可靠地定位服务，节约了救援时间，提高了救援活动的成功率，在公共安全领域可以广泛应用。

附图说明

图1为本发明基于UWB、RFID、INS多源联合定位技术的定位系统的连接框图；

图2为本发明基于UWB、RFID、INS多源联合定位技术的定位系统的定位方法的流程图；

图3为UWB定位模块的系统构架图；

图4为RFID定位模块的系统构架图；

图5为定位终端的系统构架图；

图6为本发明基于UWB、RFID、INS多源联合定位技术的定位系统的实施方案示意图；

图7为卡尔曼滤波器的结构框图。

具体实施方式

下面结合说明书附图和实施例对本发明作进一步限定，但不限于此。

实施例1

一种基于UWB、RFID、INS多源联合定位技术的定位系统，包括定位终端、UWB定位模块、RFID定位模块、INS定位模块；总控制台、所述RFID定位模块、所述INS定位模块、临时指挥中心依次连接，总控制台、所述UWB定位模块、临时指挥中心依次连接。如图1所示。

所述UWB定位模块向所述定位终端发送UWB信号，判断所述定位终端转发回来的UWB信号是否满足预设的定位要求，如果满足，则采用TDOA和AOA定位估计中的相关算法获取所述定位终端的位置；否则，则采用RFID定位技术和INS定位技术获取所述定位终端的位置。

所述定位终端内嵌UWB信号变频转发模块、信息处理单元、可更换的RFID射频标签及INS惯性测量单元；如图5所示，所述UWB定位模块包括4个移动参考基站，每个移动参考基站均架设一个UWB信号收发机；如图3所示。

每个移动参考基站发送的UWB信号频率相同，接收频段固定；移动参考基站发射不同频率的UWB信号，定位终端接收UWB信号，并将接收到的UWB信号发送至信息处理单元，与此同时，还通过所述UWB信号变频转发模块对UWB信号变频，变频后的UWB信号返回至对应频段的移动参考基站；

INS惯性测量单元测量定位终端的加速度、角度、经度、纬度和高度，将测量得到的物理信息发送至所述信息处理单元，所述信息处理单元将测量得到的物理信息编码到UWB信号中，并通过所述UWB信号变频转发模块将UWB信号转发到所述UWB收发机。

RFID射频标签是一个可更换模块，可以根据具体应用场所选择对应可识别的RFID标签。

所述INS惯性测量单元包括三轴数字加速度计、三轴数字罗盘、三轴数字陀螺仪。

实施例2

实施例1所述定位系统的定位方法，如图2所示，具体步骤包括：

(1)每个移动参考基站向所述定位终端发射不同频率的UWB信号，即以UWB基带脉冲为基础的载波调制信号；

(2)所述定位终端接收UWB信号，并将接收到的UWB信号发送至所述信息处理单元，与此同时，还通过所述UWB信号变频转发模块对UWB信号变频，变频后的UWB信号返回至对应频段的移动参考基站；

(3)判断所述定位终端返回的变频后的UWB信号是否满足预设的定位要求，如果满足，进入步骤(4)，否则，进入步骤(5)；

(4)所述移动参考基站从返回的步骤(3)所述的变频后的UWB信号中检测与发射的UWB信号的时间差，将检测到的时间差及所述的变频后的UWB信号发送到指挥控制中心，指挥控制中心采用TDOA和AOA定位估计中的相关算法来计算双程传播时延，对双程传播时延进行定标处理，得到所述定位终端的位置信息；

(5)采用RFID定位技术和INS定位技术获取所述定位终端的位置。

所述步骤(5)中，定位场所设有若干个RFID阅读器及一台主机，如图4所示，主机与若干个RFID阅读器通信，具体步骤包括：

a、通过所述主机获取若干个RFID阅读器的位置信息，并通过所述RFID阅读器检测所述定位终端的RFID射频标签，获取到标签信息，将标签信息传送到主机，指挥控制中心调用主机中的标签信息，采用TDOA定位算法对标签信息进行处理，对处理后的数据进行定标，获取所述定位终端的位置信息；

b、所述INS惯性测量单元测量所述定位终端的加速度、角度、经度、纬度和高度，将测量得到的物理信息发送至所述信息处理单元，所述信息处理单元将测量得到的物理信息编码到UWB信号中，并通过所述UWB信号变频转发模块将UWB信号转发到所述UWB收发机，经过卡尔曼滤波器，将物理信息利用联邦卡尔曼滤波估计算法进行数据融合，计算得出准确的所述定位终端的位置信息。

所述步骤(3)，具体步骤包括：

c、设定所述定位终端返回的变频后的UWB信号在t时刻的信号值为y(t)，计算以下参量：UWB信号能量E_y、UWB信号的最大幅值y_max、平均附加时延τ_MED、均方根时延τ_RMS、峭度k，计算公式分别如式(Ⅰ)、式(II)、式(III)、式(IV)、式(Ⅴ)所示：

$E_y=\underset{-\infty }{\overset{\infty }{\int }}{\left|y\left(t\right)\right|}^{2}dt---\left(I\right);$

$y_{max}=\underset{t}{max}\left|y\left(t\right)\right|---\left(II\right);$

${\tau }_{MED}=\underset{-\infty }{\overset{\infty }{\int }}t\Lambda \left(t\right)dt---\left(III\right);$

式(III)中，

${\tau }_{RMS}=\underset{-\infty }{\overset{\infty }{\int }}{(t-{\tau }_{MED})}^2\Lambda \left(t\right)dt---\left(IV\right);$

$k=\frac{1}{{\sigma }_{\left|y\right|}^{4}T}\underset{T}{\int }{\left[\right|y\left(t\right)|-{\mu }_{\left|y\right|}]}^{4}dt---\left(V\right);$

式(Ⅴ)中，T是指信号周期，

d、计算得出不同标准环境下达到视距情况的UWB信号能量、UWB信号的最大幅值、平均附加时延、均方根时延、峭度的最小值为阈值，即：E_y′、y_max′、τ_MED′、τ_RMS′、k′，所述视距情况是指没有遮挡物、直接观测到的情况；如果步骤c计算得到的UWB信号能量E_y、UWB信号的最大幅值y_max、平均附加时延τ_MED、均方根时延τ_RMS、峭度k均分别大于其阈值E_y′、y_max′、τ_MED′、τ_RMS′、k′，则判定满足预设的定位要求，选用UWB信号的参考基站差分定位方案进行实时定位，否则，选用RFID技术进行定位。

TDOA和AOA定位估计中的相关算法是指TDOA估计和AOA估计中的LTS-ESPRIT算法。

经过卡尔曼滤波器，其结构框图如图7所示，将物理信息利用联邦卡尔曼滤波估计算法进行数据融合，计算得出准确的所述定位终端的位置信息，所述卡尔曼滤波器包括位置子滤波器、速度子滤波器、主滤波器，所述位置子滤波器、所述速度子滤波器均连接所述主滤波器，所述INS定位模块分别连接所述位置子滤波器、所述速度子滤波器，所述RFID定位模块分别连接所述位置子滤波器、所述速度子滤波器，具体步骤包括：

e、所述位置子滤波器取所述INS定位模块与所述RFID定位模块的位置量测值之差Z₁(t)作为观测值，量测方程为：

$Z_1\left(t\right)=\left(\begin{array}{c}{\lambda }_{ins}-{\lambda }_{RFID}\\ L_{ins}-L_{RFID}\\ h_{ins}-h_{RFID}\end{array}\right)=H_{1}X\left(t\right)+V_1\left(t\right)---\left(VI\right)$

式(VI)中，λ_ins表示INS定位模块测量得到的经度值，L_ins表示INS定位模块测量得到的纬度值，h_ins表示INS定位模块测量得到的高度值，λ_RFID表示RFID定位模块测量得到的经度值，L_RFID表示RFID定位模块测量得到的纬度值，h_RFID表示RFID定位模块测量得到的高度值，V₁(t)表示高斯白噪声；

$H_1=\left(\begin{array}{ccccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right);$

f、所述速度子滤波器取所述INS定位模块与所述RFID定位模块的速度量测值之差Z₂(t)作为观测值，量测方程为：

$Z_2\left(t\right)=\left(\begin{array}{c}{v}_{ie}-v_{\mathrm{Re}}\\ v_{in}-v_{Rn}\\ v_{iu}-v_{Ru}\end{array}\right)=H_{2}X\left(t\right)+V_2\left(t\right)---\left(VII\right)$

v_ie为INS定位模块计算到的东向速度，v_in为INS定位模块计算到的北向速度，为INS定位模块计算到的天向速度，v_Re为RFID定位模块计算到的东向速度、v_Rn为RFID定位模块计算到的北向速度、v_Ru为RFID定位模块计算到的天向速度；

$H_2=\left(\begin{array}{ccccccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right);$

g、采用联邦滤波的信息融合方法，主滤波器将所述位置子滤波器得到的观测值Z₁(t)及所述速度子滤波器得到的观测值Z₂(t)进行融合，得到误差状态的全局估计值：

$\left(\begin{array}{c}{P}_g={(P_1^{-1}+P_2^{-1})}^{-1}\\ {\hat{X}}_g=P_g(P_1^{-1}{\hat{X}}_1+P_2^{-1}{\hat{X}}_2)\end{array}\right)---\left(VIII\right)$

P_g为联邦滤波后估计的协方差矩阵、P₁为所述位置子滤波器估计的协方差矩阵、P₂为所述速度子滤波器估计的协方差矩阵、联邦滤波估计的误差状态的全局估计值，为所述位置子滤波器估计的值，即所述位置子滤波器估计的状态参量；为所述速度子滤波器估计的值，即为所述速度子滤波器估计的状态；

h、用步骤a得到的所述定位终端的位置信息减去误差状态的全局估计值，得到实际的各种移动量，计算得出准确的所述定位终端的位置信息。

所述步骤(4)，共设有4个移动参考基站，k＝{1,2,3,4}，

①第k个移动参考基站MR_k向所述定位终端发送UWB信号并接收转发回的变频的UWB信号，对于移动参考基站MR_k，一次来回程时间t_observa为：

t_observa＝t_trans+t_delay>

式(IX)中，t_delay表示移动参考基站MR_k和定位终端之间传输的双程时延；t_trans是指实际传播时间；

②利用TDOA和AOA联合估计算法，利用接收信号较强的两个移动参考基站的信息计算出对应路径传播的时延以及传播角度，利用速度和时延相乘得到具体的距离信息，之后对定位终端的位置进行地图上的定标；

③设定n个移动参考基站的空间位置坐标分别为(x₁,y₁,z₁)，(x₂,y₂,z₂)，…(x_i,y_i,z_i)…(x_n,y_n,z_n)；定位终端坐标(x,y,z)的求取公式为：

$c·t_{trans}=c·(t_{observa}-t_{delay})=2\sqrt{{(x_k-x)}^2+{(y_k-y)}^2+{(z_k-z)}^2}---\left(X\right)$

C指的是光速，即3×10⁸m/s。

本实施例的实施方案示意图如图6所示。