一种基于伪距残差加权的最小二乘定位算法

秦明峰，胡丽格

(1. 卫星导航系统与装备国家重点实验室，河北石家庄 050081；2. 中国电子科技集团公司第五十四研究所，河北石家庄 050081)

1 引言

导航与位置服务在物联网、智慧城市、公共安全和救灾减灾等领域发挥着基础性支撑作用，虽然全球卫星导航定位系统(GNSS)可以在室外提供较高的导航定位精度，但面对复杂的室内场景，GNSS信号由于遮挡和多径，其定位效果不如室外[1，2]。但是现代社会人们四分之三以上的时间都是室内活动，室内高精度、高稳定性和高连续性的导航与位置服务有着广阔的发展前景。为此，开展室内复杂环境下的定位导航服务技术研究有着重要的意义[3]。

室内定位需要考虑在室内的复杂环境下高精度、低成本和实现复杂度低等方面的要求，常见的室内定位技术有：红外线[4]、超声波[5]、蓝牙[6]、WIFE[7]、ZigBee[8]、伪卫星[9]、地磁[10]、惯导[11]、视觉[12]和UWB[13]等室内定位技术。UWB定位技术[14]根据测得定位标签相对于两个不同定位基站之间无线电信号传播的时间，从而得出定位标签相对于定位基站的距离，通过多个距离进行定位，由于UWB的信号体制能够有效抑制多径，因此可实现6-10cm的定位精度[15]，在室内定位领域有较好的应用前景。但是受室内建筑布局、结构材料、装修等影响因素，UWB基站收到的测距信号未必是直达信号，这样的信号参与定位必然会影响室内定位的定位精度。应用在UWB室内定位中的定位方法主要有最小二乘算法[16]、迭代最小二乘算法[17]和卡尔曼滤波算法[18]，最小二乘和迭代最小二乘算法不能消除非直达信号对定位精度的影响，卡尔曼滤波算法需要首先建立复杂环境模型，当环境发生变化时需要对模型进行修改，实现较为复杂。

本文从UWB定位的定位原理出发，结合室内导航定位的特点，提出一种基于伪距残差的最小二乘加权算法，该算法基于伪距残差通过自适应调整最小二乘加权的权值，来降低非直达信号对最小二乘定位的影响，实验结果表明：该算法能够有效提升室内复杂环境下UWB定位的定位精度和定位稳定性。

2 室内UWB导航定位模型

UWB测距技术可分为两类：单程测距和双程测距，单程测距应用于基站之间需要时钟同步，基站构建实现起来较为复杂，但是能够支持更多的标签卡同时定位，该方式误差更多来源于基站网络的时钟同步精度。双程测距不需要基站进行时钟同步，采用计算收发时间的方式测定基站与标签卡之间的距离，该方式实现较为简单，目前情况下实现的精度相比单程测距要高，但是能够同时支持的定位标签卡有数量限制。下面简要介绍一下UWB定位的定位原理。

通过测量从载体到三个或更多个UWB基站传播时间的方法，可以获得载体到基站的距离，分别以每个基站为中心、以到每个基站的距离为半径绘制三个或多个圆，多个圆的交点即为载体的位置。如图1所示。

图1 基于TOA的UWB定位原理

载体与基站的距离关系可表述为

(1)

其中，(x，y，z)为载体位置坐标，(xsi，ysi，zsi)为第i个基站的位置坐标，c为电磁波传播速度，ti为载体信号到达第i个基站的时间，ni为系统噪声，N为基站数量，ri为载体到第ri个基站的距离。

将式(1)转换成关于载体坐标的线性模型，对式(1)两边分别求平方，进行变换可以得到

(2)

令

(3)

T=(x2+y2+z2)

(4)

将式(3)和(4)代入到式(2)，进一步展开可得

(5)

将N个基站的位置距离关系写成矩阵可得

H=AX+e

(6)

其中，X=[x，y，z，T]T，e=[e1，e2，…，eN]。

(7)

(8)

3 最小二乘的定位算法

最小二乘算法[19，20]最早由最早是在1975年由高斯(Karl Gauss)在测量恒星轨道时提出，该算法通过最小化平方和求解最优解，并使得这些求得的数据与实际数据之间误差的平方和为最小，由于具有较好的普适性和稳定性，且实现较为简单，在信号处理中有着广泛的应用。

最小二乘算法是构造使误差平方和最小的函数，因此，构造代价函数为

J=E(eTe)=(AX-H)T(AX-H)

(9)

进一步展开可变为

J=XTATAX-2XTATH+HTH

(10)

使上述代价函数最小的解即为当前载体的位置坐标，即

(11)

对上式进行求导，对应极值即为载体位置坐标，即

(12)

最后可得到方程的最优解为

X=(ATA)-1AH

(13)

该解即为最小二乘算法得到的最优解。

4 基于伪距残差加权的最小二乘算法

当UWB信号在室内传输时，由于室内环境的复杂性，容易受到反射、衍射和多径等多种因素的影响，上节中介绍的最小二乘算法定位精度将严重降低，这是因为最小二乘算法针对的是服从正态分布观测样本，当观测值有悖于正态分布假设、样本遭受异常污染时，最小二乘估计不具有抗干扰性。为了改善最小二乘在费直射情况下的定位精度，需要对算法进行改进，提升算法在个别基站信号非视距条件下的算法的稳定性。

可通过对最小二乘算法的各个基站的数据进行加权降低非直达信号对定位结果的影响。即将式(13)改写为

X=(ATWA)-1AWTH

(14)

加权是为了降低非视距信号对最小二乘定位结果的影响，但是，环境是不断变化的，传统的固定加权或根据测距距离的加权无法消除非视距信号对定位的影响。当某个基站的伪距测量值不是直达信号时，测量伪距变化和估算伪距变化之间的差值变大，因此，可用伪距变换率的残差构造随残差变化的权值矢量，自适应降低非直达信号对最小二乘定位的影响。

伪距残差的构造方法可描述为：假设k时刻载体的位置坐标为(xk，yk，zk)，N个基站的测量伪距分别为r1，k，…ri，k，…，rN，k，k+1时刻载体的位置坐标为(xk+1，yk+1，zk+1)，N个基站的测量伪距分别为r1，k+1，…ri，k+1，…，rN，k+1。设

(15)

(16)

其中，Δxk=xk+1-xk，Δyk=yk+1-yk，Δzk=zk+1-zk。

(17)

其中，ρi，k=ri，k+1-ri，k为k到k+1时刻伪距的变化率。

建立伪距变化量的观测方程

Q=Fv+ε

(18)

其中，ε为伪距变化和位置估算不一致引起的误差。

定义估计的伪距残差为

ε=Q-Fv

(19)

当所有基站的伪距测量为直达径时，ε应为接近于0很小的值，当收到的某个基站的信号为非直达径时，ε对应该基站的伪距残差值变大，因此，可根据伪距残差的变化构造加权向量，约束最小二乘算法性能。

则伪距残差的方差为

R=E[εεT]=(Q-Fv)(Q-Fv)T

(20)

令

(21)

构造最小二乘加权的约束函数。

基于残差加权的最小二乘法算法流程如图2所示。

图2 基于残差的最小二乘算法流程

可描述为：

1) 获取能收到节点的测距结果；

2) 判断节点数量是否≥3，若≥3，进行定位，否则该次数据无法实现定位；

3) 构建观测量F和Q，并计算伪距残差协方差矩阵，并根据参加协方差矩阵构建加权矩阵W；

4) 构建观测矩阵A和H，根据式(14)计算定位结果，并输出定位结果。

5 室内定位性能试验

5.1 实验环境介绍

在有走廊、门厅和实验室的楼内布设UWB接收基站，具体位置如图4红色点位所示。由全站仪标定基站的精确位置，标定精度在毫米量级。区域内的每个位置点至少有三个以上的基站能够实现视距接收。行走的真实轨迹事先由全站仪进行标定，图中黑线基站无遮挡条件下定位精度的验证轨迹，红线为部分基站有遮挡条件下的验证轨迹。

图3 实验环境及设备安装位置示意图

图3给出实验环境及设备安装位置布局示意图，图中红色圆点表示安装的UWB基站。实验采用基于双向测距的UWB定位设备，该测距方式不需要考虑基站和标签之间的时间同步问题，实现较为简单，有助于降低硬件成本。为了消除系统误差，在用全站仪标定好基站位置后，再标定一些点位，消除基站测距误差的影响。每个点位采集100次测距结果，采用中值滤波计算距离观测值的绝对误差，通过不同点的最小二乘拟合得到每个基站的测距补偿值。

5.2 室内无遮挡简单环境的导航定位实验

首先在一个室内尺度较小的方形无遮挡区域架设4颗UWB接收基站，在定位中仅采用这四个基站的测距结果，在3m×4m的房间内沿图3中黑线运动，在无遮挡条件下UWB定位如图4所示。

图4 室内无遮挡方形区域定位图

从上图可以看出：在无遮挡所有基站都是收到直达信号的条件下，最小二乘(LS)，非线性最小二乘(NLS)[21]和基于残差加权最小二乘算法(RWLS)都具有较好的定位性能，定位性能并无明显差别，定位精度均优于10cm。

为了对比分析以上三种算法的定位性能，利用位置误差的绝对值，误差的均方根等进行分析。首先分析在参与定位所有基站无遮挡环境下导航定位精度，如表1所示。

表1 定位误差的实验结果

从上表中可以看出：在无遮挡条件下，三种算法定位精度相差不大，均优于10cm。

5.3 室内有遮挡复杂环境下的导航定位实验

实验2标签的运行轨迹如图3中红线所示，由于实际运行轨迹是由事先标校的离散定位点连线形成的线段，为了分析本文提出算法的定位精度，将线段分为几段直线并进行线性插值，并让总点数等于该运行轨迹内定位解算的点数，通过定位结果与最近的插值点比较确定算法的定位精度。如下图所示：

图5 室内收到多个非直达信号的定位图

从图5中可以看出：最小二乘、非线性最小二乘算法在存在多个非直达信号的条件下，定位结果存在一定的发散情况，定位精度变差而本文提出的基于伪距残差加权的最小二乘算法能够有效降低非直达信号对定位精度的影响，在室内复杂环境下具有更好的收敛性。

进一步分析在有部分基站收不到直达信号的条件下的定位精度，如表2所示。

表2 定位误差的实验结果

从上表中可以看出：在有遮挡条件下，最小二乘算法的均方误差在59cm左右，非线性最小二乘算法的定位精度略优于最小二乘，但是两者定位精度基本在一个量级，本文提出的基于伪距残差加权的最小二乘算法的定位精度为17.8cm，明显优于前两种算法的定位精度。

6 结束语

针对室内复杂环境下的导航定位，在分析最小乘算法的基础上，结合UWB定位的特点，提出一种基于伪距残差加权的最小二乘室内导航定位算法，在有足够参与定位测距值的条件下，该算法能够有效消除非直达信号对UWB定位精度的影响，提升了算法的鲁棒性。实验结果表明：该算法在室内复杂环境下具有较好的定位精度，不存在非直达信号的条件下定位精度优于10cm，存在多个非直达信号的情况下，定位精度优于20cm，能够满足室内高精度导航定位的需求。