基于UWB/SINS组合的行人导航研究

陈 昊，张海华，陈玮光，刘锡祥，汪宋兵

(1.国网江苏省电力有限公司检修分公司，南京 211102；2.东南大学仪器科学与工程学院，南京 210096)

0 引言

随着我国电力事业的发展，人们对供电可靠性的要求日益提高。变电站作为电网的重要组成部分，其日常的运维与检修工作对电网的安全稳定运行至关重要[1-3]。变电站，尤其是500kV及以上的大型变电站，常有规模较大的工程施工工作和检修工作，此时将有大量来自不同单位的工作人员进出变电站，长时间在一次带电设备区作业。一方面，需要在管理措施层面加强现场管控，确保作业人员不误入带电间隔，与各类带电体保持足够的安全距离；另一方面，需要在技术措施层面积极采用新技术，实时掌握作业人员的位置动态，提高现场工作的安全性。此外，变电站设备区障碍物多，负荷波动特征多变[4-5]、电磁环境复杂[6-7]，局部区域卫星信号衰减严重，全球定位系统(Global Positioning System，GPS)设备无法有效定位。与此同时，变电站内人员在这些区域又有着相同的定位精度需求，在此背景下，在全区域内无差别实现实时跟踪个人的位置信息并提供位置服务，为施工和检修人员提供更高级别的安全保障成为重要的研究课题。

目前，可用于行人导航定位的技术种类繁多，适用范围和精度各有不同，其中惯性设备个人导航系统具有高稳定性、连续性与自主性的优点[8-11]，成为研究的热点方向之一；然而低成本的惯性设备精度低，且其积分工作方式导致定位误差会随着时间累积，限制了其精确定位的时间长度[12]。利用无线技术实现行人定位的方案繁多[13-15]，其中超宽带(Ultra Wideband，UWB)拥有独特的定位优势，其具有时间分辨率高、穿透能力强、发射功率低的特点[16]；但UWB易受可视性和多径效应影响，使得定位出现不连续与不稳定的缺点[17]。本文结合惯性导航与UWB的优点，在传统的捷联惯导算法的基础上引入了零速检测，并通过阈值法剔除了UWB错误信息，使用联邦Kalman滤波将零速、UWB位置和航向信息进行融合，获得了高精度、连续且稳定的定位信息。通过现场实验证明了该方法能够大幅度提升系统定位精度，并进一步加强系统的稳定性与可靠性。

1 UWB/SINS算法定位模型

初始对准阶段，采用磁力计和加速度计输出数据对捷联惯性导航系统(Strapdown Inertial Navigation System，SINS)进行姿态角对准；行走过程中，利用陀螺仪输出积分获得姿态，加速度计积分获得速度，速度积分获得位置；在行走过程中，利用加速度计和陀螺仪输出数据进行零速检测，并利用UWB数据解算出位置与航向信息；采用联邦Kalman滤波融合零速度信息、位置信息以及航向信息，校正系统状态量，系统结构如图1所示。

图1 系统结构Fig.1 System structure

1.1 初始对准

(1)

(2)

根据式(2)可以获得初始横滚角与俯仰角R0和P0，P0∈(-π/2,π/2),R0∈(-π,π)。

(3)

(4)

表示载体坐标系b系到水平坐标系L系的姿态转换矩阵。不考虑偏向磁分量，当规定航向角北偏东为正，北偏西为负时，初始航向角表示为

H0=-tan-1(myLmxL),H0∈(-π,π)

(5)

1.2 零速检测算法

零速检测是零速修正的前提，本文采用广义似然比检测，并通过判断运动模式设置差异化的阈值来适应不同速度的步态。

1)判断运动模式

这里将惯性测量单元(Inertial Measurement Unit，IMU)的x轴看作行走方向的前向轴，可以很大程度反映行走速度，通过计算x轴基于时间窗口的平均加速度来判断运动快慢，设k时刻的x轴的平均加速度为

(6)

根据经验值设置2个阈值Thv1和Thv2，划分三种运动速度模式mod1、mod2和mod3，分别代表慢速模式、中速模式和快速模式，划分标准如下

(7)

式中，modi表示运动模式i。

2)广义似然比检验模型

定义k时刻IMU的输出为

(8)

(9)

(10)

Thi为运动模式i下的经验阈值，zero_flag表示零速标志，其值为1时表示零速，其值为0时表示非零速。根据不同的运动速度设置不同的阈值可以更加准确地判断零速。

1.3 UWB定位

UWB解算位置会受到可视性误差的影响出现跳变，而融合后的解算位置不出现跳变误差；且行人行走属于一个连续相对较慢的过程，本文利用式(11)进行UWB误差数据剔除

(11)

ThL为阈值，uwb_flag表示UWB数据是否可用，其值为1时表示可用，否则不可用。

剔除误差数据后，UWB提供的位置相对准确，其航向角相对磁力计将更加稳定，利用两点确定直线的方式确定航向角，计算方程如下

(12)

2 导航系统联邦Kalman滤波器

组合导航系统联邦Kalman滤波器由SINS、零速修正系统、UWB系统以及磁力计构成，以SINS作为公共的参考系统，联邦Kalman滤波结构如图2所示。

图2中，Xi和Pi分别表示各个子滤波器的局部状态估计值与协方差矩阵；Xm和Pm分别表示主滤波器的状态估计值与协方差矩阵；Xg和Pg分别表示公共系统的状态估计值与协方差矩阵；βi表示系数。

2.1 导航系统联邦Kalman滤波算法

(13)

对子滤波器分别进行时间更新

(14)

对子滤波器分别进行量测更新

Pi,k=(I-Ki,kHi,k)Pi,k|k-1(I-Ki,kHi,k)T+

(15)

由于主滤波器没有量测信息，所以只进行时间更新

(16)

将子滤波器与主滤波器的估计信息融合成为新的全局状态估计信息，如下

(17)

最后按式(18)重置子滤波器与主滤波器

(18)

2.2 子滤波器系统模型

当选取支撑相位内的速度输出作为伪观测值时，传感器误差的可观测性较差，利用Kalman滤波计算难以得到精确的传感器误差估计[18]，且磁力计与UWB无累计误差，因此在联邦Kalman滤波中各个子滤波器不考虑传感器误差，选择速度、位置与姿态作为状态量，即X[(δφ)T(δvn)T(δpn)T]T。

1)SINS/零速检测系统子滤波器

SINS/零速检测系统子滤波器模型如下

(19)

其中，Wk-1和V均视为彼此不相关的零均值白噪声序列；状态转移矩阵Fk|k-1与噪声输入矩阵Gk|k-1具体如下

(20)

对SINS/零速检测系统子滤波器进行速度融合。通过观测值对系统的速度进行修正，观测向量及观测矩阵分别为

(21)

其中，Z=03×1-vn中03×1表示零速检测获得的零速矢量，vn表示惯性导航解算获得的速度矢量。

2)SINS/UWB系统子滤波器

其状态方程与SINS/零速检测系统子滤波器相同，观测方程不同；对SINS/UWB子滤波器进行位置、航向融合。通过观测值对位置、航向进行修正，观测向量及观测矩阵分别为

(22)

式中，Hs是SINS解算出来的航向角，Hu是UWB解算出来的航向角，Ps是SINS解算出的位置，Pu是UWB解算得到的位置。

3)SINS/磁力计子滤波器

其状态方程与SINS/零速检测系统组合子滤波器相同，对SINS/磁力计子滤波器进行航向融合。通过观测值对航向进行修正，观测向量及观测矩阵分别为

(23)

式中，Hs是SINS解算出来的航向角，Hc是磁力计解算出来的航向角。

3 现场应用

捷联惯性导航实验设备采用法国SBG SYSTEMS公司的Ellipse2-A系列，产品内部包含3个正交的加速度计和3个正交的陀螺仪以及1个三轴磁力计。SBG安装于脚部(建议安装处为试验中采用的右脚脚踝)，对行人行走数据进行采集，实验中坐标系采用的是前右下坐标系，系统的采集频率为100Hz(见图3)。UWB采用的是研创物联的Mini3s系列，其最大测量距离为50m，定位误差为35cm，频率为3.5Hz，UWB数据包含可用基站数目，当可用基站数小于3时，弃用UWB数据信息(见图4)。

图3 SBGFig.3 SBG Ellipse 2-A AHRS used in the experiment

图4 超宽带Fig.4 UWB Mini 3s and in the experiment

现场试验区域为南京地区某500kV变电站设备区14m×16m的矩形区域，围绕该区域行走1圈，总路程60m。当可用基站数据少于3时，就不使用UWB数据。图5中，(0,0)是起点也是终点，围绕该矩形区域逆时针旋转；从(0,0)出发依次经过(-14,0)(-14,16)(0,16)回到(0,0)；点表示WUB解算位置；虚线表示SINS解算获得的轨迹，实线表示SINS-UWB组合导航轨迹，图中五角星的位置为4个基站的位置。

图5 定位轨迹Fig.5 Location trajectory

误差曲线如图6所示。图6中，虚线为SINS轨迹误差，点划线为UWB解算估计误差，实线为SINS-UWB联合解算误差。统计3条曲线定位误差，如表1所示。

图6 定位误差Fig.6 Location error

表1 定位误差Tab.1 Location error

从表1中可知，SINS与UWB定位误差均值与最大值均大于SINS-UWB组合导航，其中SINS是由于误差累计导致位置与航向发生漂移，而UWB主要由于可视性误差与多径效应导致定位误差；组合导航方法可以平滑轨迹曲线，其轨迹更加接近于真实轨迹，误差未明显受到变电站复杂电磁环境的干扰。

4 结论

采用UWB/SINS组合系统实现了变电站现场区域的定位功能，在传统的捷联解算基础上引入了零速检测，并通过阈值法剔除了UWB错误信息，最后利用联邦Kalman滤波融合了UWB、磁力计、零速等信息，获得了较高的系统定位精度。现场试验表明，组合系统的定位误差小于UWB系统与SINS，能够消除UWB系统中的可视性误差以及SINS的累计误差，稳定性与可靠性都有进一步提升，适用于高精度变电站现场区域无差别导航。