基于MEMS/UWB组合的室内定位方法

郁嘉宇，赵 忠，李 凡，黄思文

(西北工业大学自动化学院，西安 710072)

0 引言

随着人们生活水平的不断提高，各种通信和定位技术也在不断发展和完善，复杂的道路网络和空间结构带给人们生活和工作的压力，使得人们对定位的需求愈加强烈[1]。根据范围和场景，定位可分为室内定位和室外定位。室外定位技术主要借助于全球导航卫星系统(Global Navigation Satellite System, GNSS)，利用卫星和移动对象之间的直接通信，可完成精确定位[2]。而对于室内场景，GNSS信号易受遮挡，无法实现定位。

现已出现多种室内定位解决方案，包括红外线、射频识别、视觉、WiFi、蓝牙信标(如iBeacon)、超声波、LED信标、地磁场、超宽带(Ultra-Wide Bandwidth, UWB)及微惯性器件等定位技术[3-4]。它们都有各自的优势和不足。

UWB定位系统具有功耗低、抗多路径效应效果好、安全性高、信号穿透力好等优点；但其不提供姿态信息，动态范围和鲁棒性较差，对外界硬件设施依赖程度较高，存在信号遮挡(铁材料和结构复杂的混凝土墙壁等)、信号作用距离有限、信号易被吸收和成本高等问题。微机电系统(Micro-Electro-Mechanical System，MEMS)定位精度与惯性器件本身精度密切相关，不易受环境干扰、独立工作能力强、短期精度高，可以准确测得定位目标的运动参数，但其定位误差会随时间增长[5-6]。当下MEMS惯性传感器朝着低成本、低功耗、高精度的方向发展，在室内定位领域将会有更大的应用空间。

UWB与MEMS定位在全球范围内受到了广泛关注与深入研究。刘见辉等提出了惯性导航系统(Inertial Navigation System，INS)和扫描仪相结合的测量系统，同时在该量测系统中又加入了UWB以解决INS误差积累的问题[7]。邬春明等结合差分全球定位系统(Differential Global Positioning System，DGPS)和UWB，利用Kalman滤波器消除UWB非视距误差，采用粒子滤波器进行数据融合，可将整体定位精度提高19%[8]。王芳等采用UWB与里程计信息融合的定位技术，提高了里程计航位推算稳定性与精度[9]。Qigao Fan等通过建立INS/UWB融合滤波的误差模型，提出了模糊适应卡尔曼滤波(Fuzzy Adaptive Kalman Filter，FAKF)，根据模糊控制理论对量测噪声进行调整[10]。Ascher C等将UWB和INS进行紧组合，利用INS得到用户的轨迹和航向信息，结合UWB精确的定位数据，可完成室内环境下的实时定位[11]。

本文旨在将UWB和MEMS两种技术相结合，采用合适的算法对两系统进行信息与数据融合，充分发挥两者在室内定位中的优势，解决UWB系统由于外在因素无法完成有效定位、MEMS长期定位精度差等问题。本文所构建的混合定位系统，在复杂室内环境下定位的连续性、稳定性和可靠性得到大大增强[12]。此外，成本较高的UWB定位设备也会因低成本MEMS的加入使得成本有所下降，在保证定位精度的前提下，UWB定位基站的布设成本也会有所降低。

1 基础定位系统

1.1 UWB定位系统

UWB定位技术源于超宽带无载波通信技术，其利用纳秒至微秒级的非正弦波窄脉冲进行通信，工作频段为3.1～10.6GHz，可直接脉冲调制，不受载波干扰[13]。UWB定位在本质上与GPS卫星定位原理一致，即自己搭建基站，利用发射的UWB脉冲信号进行测距或者定向，然后根据和GPS定位类似的算法，计算出每个基站到移动站(定位标签)的距离，从而解算出移动站的空间坐标。UWB定位简易示意图如图1所示。

系统主要由定位基站(也称接收机)、定位标签和中心处理器组成[13]。

按照测量参数不同，将UWB定位方法分为基于接收信号强度法(Received Signal Strength Indication, RSSI)和基于到达角度法(Angle of Arrival, AOA)、基于到达时间法(Time of Arrival, TOA)和基于到达时间差法(Time Difference of Arrival, TDOA)等[14-16]。本文采用AOA/TDOA算法，充分利用距离或角度信息，其原理如图2所示。

传感器能得到定位目标的到达时间差Δt和到达角度α1和α2，设移动站MS(x0,y0)、基准站BS1(x1,y1)和BS2(x2,y2)，则

(1)

其中，c代表光速。联立式(1)两式即可求得MS的二维坐标。对于三维情况，TDOA公式与二维类似，如式(2)所示。

(2)

AOA包括方向角λ和仰角φ，如式(3)所示。

(3)

本文所使用的AOA/TDOA算法，避免了TOA方法严格时钟同步问题，采用了天线阵列，虽然设备相对比较复杂，但是精度得到了保证。每个传感器探测一个标签的二维到达角度(AOA，方向角和仰角)，同时记录下感应的TDOA，从传感器得来的数据可通过相应的算法综合起来使用，从而得到定位结果。

1.2 MEMS惯性定位系统

首先介绍本文坐标系的定义及坐标系间的转换关系。载体坐标系(b系)是指在定位过程中载体位置处的坐标系，本文所指b系原点在载(车)体重心，x轴沿载(车)体横轴向前，y轴沿载(车)体向左，z轴沿载(车)体竖直向上，如图3所示。

(4)

MEMS惯性定位算法由初始对准和定位解算两部分组成，前者为后者提供初始姿态信息[17]。对惯性系统而言，其MEMS陀螺精度太低以至无法感测地球自转角速度(约15(°)/h)，因而低精度MEMS捷联惯导系统初始自对准无法完成方位对准，只能根据加速度计输出解算出载体俯仰角和横滚角[17]，即水平对准。方位对准则需要利用其他外部信息给出。

(1)水平初始对准

静基座条件下，重力加速度只在地垂线方向上有输出(输出为g)，有：

(5)

(6)

(2)方位初始对准

对于初始航向角，若以地理坐标系为导航坐标系，则初始航向角是指与真北的夹角，此角度需要磁力计或磁罗盘等提供；若以地面正交坐标系为导航坐标系，则直接装订初始方位角为0°，最终解算出来的坐标即为该坐标系下的结果。对于初始航向问题，本文采用直接装订的方式，将初始时刻计算得到的磁航向作为系统初始航向装订。

(3)定位解算和反馈校正

图4所示为惯导解算过程的详细框图，主要显示了根据陀螺仪和加速度计输出计算车辆最终姿态、速度和位置的过程。

相关算法和公式如下所述[19]：

设计15维Kalman滤波器，其状态向量对应为：

(7)

KF估计出来的这些误差量将被反馈到三轴陀螺仪、三轴加速度计、常规惯导解算的姿态、速度和位置，通过修正得到更为精确的定位结果。

(8)

根据补偿后的陀螺输出对姿态矩阵进行更新：

(9)

(10)

由于MEMS器件采样频率较高，故可认为在极短的时间内受到一个恒力，载体做匀变速直线运动。用梯形算法计算速度和位置，公式如下：

(11)

前2个部分得到的姿态、速度、位置信息是惯导解算直接得到的信息，误差会随时间的增加而累积，故需利用KF估计量对其进行进一步修正：

(12)

通过KF得到的3个的姿态角误差(小角度)对姿态矩阵进行修正：

(13)

(14)

(15)

(16)

其中，arctan2(a，b)为反正切函数，值域为(-π，π)，与横滚角、航向角定义范围一致。

2 Kalman滤波器设计

2.1 动态零速修正

假设小车在匀速、刹车、加速、转弯等不同运动状态过程中，不发生侧滑与跳跃，则可以认为车辆的天向与侧向速度在车体运动中始终保持为零，即存在动态零速：

(17)

可以利用车辆的天向速度与侧向速度为零的速度信息，辅以里程计输出的轴向速度信息与惯导解算得到的速度信息作差，估算捷联解算的误差信息，进行反馈修正，从而提高定位精度。

以15维状态向量为例，状态量取惯导系统的3个姿态误差角、3个速度误差、3个位置误差，三轴陀螺常值漂移、三轴加计常值偏置，如下所示：

(18)

(19)

定位算法简化后的误差模型：

(20)

根据定位算法的误差模型和Kalman状态量的选择，可以得到状态转移矩阵为：

(21)

(22)

根据量测向量的选取，可得到对应的量测矩阵：

(23)

通过状态量的估计，完成Kalman滤波的估计与补偿，可以得到最终的定位结果。

2.2 MEMS/UWB融合滤波器设计

根据实际UWB系统的输出信息以及所采用的MEMS定位算法，本文最终采用KF松组合方式实现两系统的融合定位[7，20-22]。单独的车载MEMS定位系统，是采用带动态零速修正的惯导解算算法，利用实际解算速度和里程计速度动态零速构造量测信息对惯导误差进行修正，此过程通过KF实现。

对于捷联惯导而言，以速度误差为观测量进行状态估计与反馈，进而抑制惯导误差积累，此过程不依赖于外界其他硬件设施。对于MEMS/UWB融合而言，在MEMS基础上加入了UWB位置信息，需要构造位置误差观测量再进行KF。考虑到2个系统的独立性，组合结构图如图5所示。

在上述2个KF当中，考虑到其状态量相同，只是量测信息有所不同，因此时间更新相似，可根据条件判断选择对应的量测更新。因此，2个KF估计器是可以合并的。对图5进行合并，可得到MEMS和UWB的松组合方式的结构框架如图6所示。

由UWB提供位置信息P以及里程计信息与动态零速信息，形成六维量测向量：

(24)

其中：Δpk=pINS-pUWB,Δvk由式(19)可得，则其对应的量测矩阵为：

(25)

3 试验与分析

本文中在7.5m×7.5m×3m的实验室里布设4个UWB定位传感器，大致布局如图7所示。

UWB标签固定在小车顶部，MEMS惯性传感器固定在小车中心。UWB标签示意如图8所示。 MEMS器件选用ADI公司的ADIS6448十轴惯性器件，其外形如图9所示，参数如表1所示。

表1 ADIS16448主要参数(A+G)

本文试验的路线设计为U型运动路线，便于分区间进行误差计算和定位精度分析。往返一次试验路径为A-B-C-D-C-B-A，轨迹示意如图10所示。

重复10次U型往返路径作为融合试验路径。整个试验的运动时间为348.2860s，运动距离为253.4369m。

融合过程中，由于MEMS解算周期与UWB位置输出周期不一致，并且两者周期存在非整数倍关系，所以需要考虑长时间解算的时钟同步性问题。MEMS输出频率为102.4Hz，UWB输出频率为9.25Hz，根据两者的时间关系，每20s进行一次数据调整。一次调整时间偏差从0.1270s减小到0.0195s。时钟同步性调整如图11所示。

MEMS、UWB、MEMS/UWB三者定位效果如图12～图14所示。

由图12可以看出，MEMS定位结果平滑，但随着时间的积累，由于航向误差的存在与不断积累，解算路径慢慢偏离实际运动路径，误差逐渐增加。

由图13可以看出，UWB属于绝对定位，其误差在一个合理的范围内，整体运动轨迹在实际运动轨迹的两侧跳动分布。由图14可以看出，MEMS/UWB融合定位因为有了UWB位置信息的加入，对MEMS的积累误差有着明显的抑制作用，可以得到近似于UWB定位的效果。对于UWB而言，该融合算法在UWB定位系统出现定位失效的情况下，仍能保持比较好的定位效果。表2所示为三种定位方式的终点坐标以及误差情况。

表2 融合定位试验的终点坐标及误差对比结果

由表2可以看出，融合定位对MEMS定位的积累误差有着显著的修正和抑制作用，将相对误差从0.2401%降到0.0265%。而UWB自身定位精度较高，融合对UWB定位精度无显著提升作用，但对UWB定位易受遮挡的缺陷有着很好的弥补作用。整体而言，MEMS/UWB融合定位可适当弥补两者的缺陷，拥有比单独MEMS子系统更好的定位效果。

4 结论

本文以室内定位为背景，针对UWB定位存在易受环境影响与遮挡的问题和MEMS定位中误差随时间积累的问题，文中完成了以下工作：

1)设计了以里程计速度信息和动态零速作为观测量的MEMS惯导解算算法，通过卡尔曼滤波器抑制惯导的误差积累，实现了短期误差在1%以内的定位效果。

2)通过分析UWB和MEMS这2个定位子系统，在MEMS定位算法的基础上，完善量测方程，建立速度、位置六维量测方程，完成MEMS/UWB融合算法的设计，实现两者的融合定位。

3)针对融合过程中MEMS与UWB时钟不同步的问题，提出了定时调整方法，减小时钟不同步带来的误差。

本文提出的MEMS/UWB组合的室内定位方法实现了两者的优势互补，解决了MEMS定位长期发散问题与UWB定位短期失效问题。MEMS/UWB融合定位拥有比单独子系统定位更好的定位精度与鲁棒性。