一种基于改进平方根无迹卡尔曼滤波的室内定位方法与流程

本发明涉及室内定位技术领域，特别是一种基于改进平方根无迹卡尔曼滤波的室内定位方法。

背景技术：

无线室内定位技术的应用范围十分广泛，该技术的出现解决了全球定位系统(gps)由于信号受障碍物遮蔽、室内环境复杂、反射严重等原因无法在室内提供精确定位服务的问题。

常见的室内定位技术有：wi-fi技术、射频识别技术、红外技术、zigbee技术、超声波技术、uwb技术等。这些技术有各自的优缺点，应用领域也各不相同。其中，超宽带技术(uwb)利用纳秒级的窄脉冲来传输数据，不需要载波，具有ghz量级的带宽。另外，超宽带技术具有较高的时间分辨率、较强的穿透力和抗多径能力，然而由于室内环境复杂，会发生uwb信号折射或反射的情况，或者由于障碍物阻挡导致非视距(nlos)误差。

技术实现要素：

本发明的目的在于提供一种精度高、稳定性好的基于改进平方根无迹卡尔曼滤波的室内定位方法。

实现本发明目的的技术解决方案为：一种基于改进平方根无迹卡尔曼滤波的室内定位方法，包括以下步骤：

步骤1：建立uwb定位模型；

步骤2：建立行人航位推算pdr模型；

步骤3：建立uwb/pdr组合定位模型；

步骤4：建立改进平方根无迹卡尔曼滤波算法；

步骤5：使用uwb进行实时定位，同时惯导依据初始位置进行行人航位推算，接收到tdoa数据后，用残差鉴别法鉴别是否存在nlos情况；

若存在nlos，则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与uwb的定位坐标进行融合，对uwb解算坐标数据进行修正，返回步骤5；

若不存在nlos，则采用uwb解析出当前坐标，完成实时定位。

本发明与现有技术相比，其显著优点为：(1)uwb定位模型进行tdoa解算时，统一基站高度为h，简化了计算过程，提高了计算效率；(2)基于改进sage-husa自适应平方根无迹卡尔曼滤波算法和基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法的改进平方根无迹卡尔曼滤波算法，利用二者优势，构成既可以抑制滤波发散又能保证滤波精度和效率的滤波算法；(3)在基于改进sage-husa自适应平方根无迹卡尔曼滤波算法中，对自适应因子dk进行改进，解决了收敛速读与收敛值不匹配的问题；(4)在状态变量误差协方差预测值中引入多重渐消因子，对不同的状态估计进行调整，使系统具有较好的跟踪能力；(5)利用行人航位推算pdr来补偿uwb信号丢失或非视距的情况，提高了系统的定位精度，保证了系统的稳定性和连续性。

附图说明

图1是本发明基于改进平方根无迹卡尔曼滤波的室内定位方法的行人航位推算模型示意图。

图2是本发明中的uwb/pdr组合定位原理图。

图3是本发明中的残差鉴别法流程图。

图4是本发明实施例中的uwb、pdr、sr-ukf、isr-ukf定位轨迹对比图。

图5是本发明实施例中的定位位置误差对比图。

具体实施方式

结合图1，本发明基于改进平方根无迹卡尔曼滤波的室内定位方法，包括以下步骤：

步骤1：建立uwb定位模型，具体如下：

在三维空间中，利用4个传感器所得到的tdoa测量值来估算待定位节点的位置，设定基站传感器的坐标为(xi,yi,zi)，其中i＝1,2,3,4，待定位点a的坐标为(x,y,z)，则第i个定位基站到a点的距离ri为：

其中ki＝xi²+yi²+zi²；

设定第一个定位基站为主传感器，则定位目标到从传感器和到主传感器之间的距离差为ri,1，信号传播时间差，即tdoa测量值为ti,1，则：

ri,1＝cti,1＝ri-r1(2)

其中c为传输信号的速率即光速；

根据式(1)和式(2)得：

在式(1)中，令i＝1，则得：

r1²＝k1-2x1x-2y1y-2z1z+x²+y²+z²(4)

用式(3)减去式(4)得：

ri,1²+2ri,1r1＝ki-k1-2(xi-x1)x-2(yi-y1)y-2(zi-z1)z(5)

设定传感器铺设在室内天花板上，高度为h，则zi＝h；设定主传感器的坐标为(0，0，h)，第二、三、四个传感器的坐标分别为：(x2，0，h)、(0，y3，h)、(x2，y3，h)，由式(5)得：

解式(6)得：

式(7)中：

将x，y值代入式(6)求得r1值，将x，y，r1值代入式(4)求得z值，得到待定位目标的位置。

步骤2：建立行人航位推算pdr模型，具体如下：

设定行人在室内二维平面中运动，对行人的运动进行超短时间采样，可将行人在此阶段的运动近似为直线运动。假设行人的初始位置坐标为m0(e0,n0),初始航向角为行人t1时间内的移动距离为l0，得到行人的当前位置坐标m1(e1,n1)：

由上，同理可以推出ti时刻的位置坐标mi(ei,ni)为：

上式(9)中，(ei-1,ni-1)表示ti-1时刻的位置坐标，为ti-1时刻的航向角，li-1表示采样时间内的位移。

由式(9)知，行人航位推算pdr包括初始位姿、步频检测、步长估计和航向计算：

所述步频检测，通过峰值检测法，基于行人的行进过程中产生的行进方向和竖直方向的正负加速度，来确定行人的步频；

所述步长估计，根据行人的生理特征和行走习惯建立模型，得到步长估计；

所述航向计算，通过扩展卡尔曼滤波融合陀螺仪、加速度计和磁力计的数据，得到航向角估计值。

步骤3：建立uwb/pdr组合定位定位模型，如图2所示。

步骤4：建立改进平方根无迹卡尔曼滤波算法，具体如下：

结合基于改进sage-husa自适应平方根无迹卡尔曼滤波算法和基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法，建立改进平方根无迹卡尔曼滤波算法，利用二者优势，构成既可以抑制滤波发散又能保证滤波精度和效率的滤波算法；根据发散判据判断滤波器是否滤波发散，当未发散时，采用基于改进sage-husa自适应平方根无迹卡尔曼滤波算法，反之，则采用基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法；

在sage-husa自适应滤波算法中，设定系统噪声和观测噪声的均值为0，由于系统噪声的协方差和观测噪声的协方差不能同时被实时动态估计，因此只能根据其中一个的协方差估计另一个的协方差；在室内定位系统中，系统噪声相对稳定，而观测噪声由传感器特性和外界环境因素引起，具有较大的不确定性，因而设定系统噪声已知，可得k时刻的观测噪声协方差rk如下：

式(10)中，rk-1表示k-1时刻的观测噪声协方差，zk表示时k刻的观测量，为加权后k时刻的观测预测值，自适应因子dk＝(1-b)/(1-b^k)，b为遗忘因子，取值范围为(0,1)；误差协方差矩阵表示k时刻的误差协方差平方根预测值；上标为t代表式该矩阵的转置；hk为是k时刻近似的观测转移矩阵，hk的表达式如式(11)所示：

式(11)中，表示第i个采样点误差协方差权重；表示第i个采样点的观测预测值。

当在滤波过程中趋近收敛时，误差协方差矩阵会逐渐减小，并且hk是k时刻近似的观测转移矩阵，是一个有限常数，因此的值会逐渐减小，此时对观测噪声协方差的影响较小。因此，可通过简化观测噪声的协方差来提高计算效率，将式(10)简化如下：

sage-husa算法通过自适应因子dk来调整观测噪声，dk越大，表明对传感器的观测噪声的统计特性的置信度越高，反之，则相反。

在基于改进sage-husa自适应平方根无迹卡尔曼滤波算法中，对自适应因子dk进行改进，从而解决收敛速速与收敛值的不匹配的问题。通过自适应调节参数wk来调节自适应因子dk，从而自适应调节不同传感器噪声特性对于系统的影响，将式(12)转换为：

式(13)中，ik为n×n的单位矩阵，wk为自适应调节参数，wk＝diag(m1,...,mn)，其中0＜mi＜1,i＝1,…,n，diag()为由向量构成的对角矩阵；

自适应因子dk如式(14)所示：

dk＝(dupper-dlower)·b^k+dlower(14)

式中，dupper为自适应因子取值的上限，dlower为自适应因子取值的下限，dupper和dlower的取值范围均为(0,1]，且dupper>dlower；b为遗忘因子，取值范围为(0.9,1)；

当b越接近于1时，dk收敛速度越慢，然而此时的dk的取值范围由dupper和dlower决定，并由dupper向dlower收敛，其收敛下限值为dlower，通过调整dlower，可确保新数据对观测噪声协方差的更新作用。

改进后的方法，不仅对于新数据的权重值可以设置其收敛于特定值dlower，而不依赖于遗忘因子b的取值，而且自适应因子dk收敛速度更慢，导致dk的数值在滤波起始的时候相对更大，1-dk的数值则相对更小。随着k值的不断增大，距离当前时刻较远的历史数据对于观测噪声协方差的更新作用较小，此时观测噪声协方差的更新依赖于距离当前时刻较近的历史数据。

在本发明中，dupper取值为0.95，dlower取值为0.4，b取值为0.99。

通过调整dlower，进行对观测噪声协方差的更新；

估计观测噪声统计特性的方法为：

上式(15)(16)中，是k-1时刻的观测噪声协方差的cholesky因子更新值。

步骤5：使用uwb进行实时定位，同时惯导依据初始位置进行行人航位推算，接收到tdoa数据后，用残差鉴别法鉴别是否存在nlos情况，结合图3，具体如下：

残差鉴别法的具体步骤为：设定有n个uwb基站传感器，首先设定残差d：

其中c为光速，ti,1为信号到达传感器i和主传感器的传播时间差；

如果待定位目标和各传感器节点是los传播，则能够求得待定位节点的坐标；相反，待定位节点和传感器之间的非视距信道会使得tdoa的三个双曲线面相交于一个区域，此时目标位置不确定，设定残差阈值进行两种情况的判断。

第一种情况：若存在nlos，则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与uwb的定位坐标进行融合，对uwb解算坐标数据进行修正，重复步骤5，具体如下：

在强跟踪卡尔曼滤波算法中，为保证滤波器的可靠收敛，可以牺牲一定的精度换取滤波稳定性，将状态变量误差协方差或者观测变量误差协方差乘以加权系数，来包含更多未建模的误差，使算法具有很强的突变状态跟踪能力，同时降低对初值和噪声统计特性的敏感性。在室内定位系统中，单一渐消因子不能满足要求，为了使滤波器具有应对系统模型不确定的鲁棒性，本发明在状态变量误差协方差预测值中引入多重渐消因子，对不同的状态估计进行调整，来保证系统具有较好的跟踪能力。

基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法的具体步骤如下：

步骤6.1、初始条件：

式(18)中，表示状态变量估计的初值，x0表示状态变量的初值，px,0表示状态变量估计的误差协方差初值，sx,0表示的cholesky因子；且k时刻引入多重渐消因子后的状态变量误差协方差平方根预测初值同sx,0。

步骤6.2、状态变量预测：

根据状态变量和误差协方差平方根sx,k-1利用无迹变换得到2n+1个sigma点，如下：

式(19)中，表示k-1时刻的第i个采样点，括号后的i或者i-n表示状态变量误差协方差平方根矩阵的第i或者i-n列；λ＝a²×(n+k)-n，其中a表示散布程度因子，取值范围为[10^-6,1]，n表示状态变量的维数，k表示辅助尺度因子，满足k+n≠0。

然后，根据式(19)对状态变量进行时间更新，见式(20)和(21)。

式中，表示第i个采样点的状态预测值，表示第i个采样点的状态值，表示k时刻的状态变量预测值，表示第i个采样点期望的权重，具体见式(22)。

其中误差协方差平方根sx,k-1使用^*sx,k-1替换。

步骤6.3、引入多重渐消因子的状态变量误差协方差平方根预测：

注：前加*代表引入多重渐消因子后的各变量值。

k时刻未引入多重渐消因子的误差协方差平方根预测值如式(24)所示，pxz,k是状态量和观测量的误差协方差矩阵，如式(25)所示：

式中，表示k时刻的误差协方差平方根预测值，表示第i个采样点误差协方差权重，qk是k时刻的系统噪声的方差，具体见式(26)，sign()表示取符号位。

式中，b表示验前分布因子，一般取2。

可得到由和构造的sigma采样点且如下：

表示引入强跟踪后的第i个采样点的观测预测值。

从而获得k时刻引入多重渐消因子后的误差协方差平方根预测值如式(30)所示：

式(29)中ζk表示多重渐消因子。

步骤6.4、观测变量更新：

式中，表示引入强跟踪后的第i个采样点的观测预测值；为由和构造的sigma采样点；表示引入多重渐消因子后的加权后k时刻的观测预测值。

步骤6.5、观测变量误差协方差平方根更新：

其中，^*sz,k表示引入多重渐消因子后的观测变量误差协方差平方根更新值，^*pxz,k是状态量和观测量的误差协方差矩阵。

步骤6.6、卡尔曼增益更新^*kk为：

步骤6.7、状态变量和状态变量误差协方差平方根^*sx,k更新：

重复步骤6.2至步骤6.7，得到在线最优状态估计。

当滤波器未出现发散现象时，序列是均值为0，方差阵为的高斯噪声序列，因此得到服从自由度为n的χ²分布的检验统计量，如式(39)：

式中，zk表示k时刻的观测量，观测变量误差协方差平方根sz,k的表达式为：

构造如下发散判据来判断滤波算法是否出现了发散现象：

h0:滤波器未出现发现现象；

h1:滤波器出现了发现现象；

根据式(39)，如果其中1-α是χ²分布的置信度，取值为95％，此时则接受h0，此时滤波器处于未发散状态；否则接受h1，此时滤波器处于发散状态。

第二种情况：若不存在nlos，则采用uwb解析出当前坐标，完成实时定位。

下面结合附图和实施例对本发明作进一步描述。

实施例1

图4的仿真结果表明，可以看出由于行人航迹推算中累积误差问题的存在，随着时间的推移，导致轨迹出现较大的波动；在前30s范围内，uwb的定位轨迹和规划路线基本保持一致，由于模拟了uwb信号被遮蔽的情况所导致的非视距误差问题的存在，导致在30s至42.5s之间，uwb信号漂移，从而导致定位轨迹出现较大的波动；sr-ukf和改进sr-ukf都能够融合行人航迹推算和uwb定位的位置信息，并有效提高定位精度。

结合图5可以看出，改进sr-ukf算法在定位精度和轨迹平滑性上都优于sr-ukf。使用sr-ukf与单纯的uwb定位相比，位置误差减少了约68.8％，使用改进sr-ukf与单纯的uwb定位相比，位置误差减少了约75.4％，因此，改进sr-ukf比sr-ukf具有更高的定位精度，可以有效提高定位的精度。相比于sr-ukf，改进sr-ukf算法将位置误差减少了21.3％，且位置误差的方差更小，即误差数据分布更集中，波动更小。