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

本发明专利技术公开了一种基于改进平方根无迹卡尔曼滤波的室内定位方法。该算法为：首先建立所述融合室内定位方法中UWB的定位模型和行人航位推算PDR模型，然后根据残差鉴别法判据来判断UWB定位模型定位结果中是否存在NLOS判据，若存在NLOS，则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合，对UWB解算坐标数据进行修正；若不存在NLOS，则采用UWB解析出当前坐标，完成实时定位。本发明专利技术利用行人航位推算PDR来补偿UWB信号丢失或非视距的情况，提高了系统的定位精度，具有精度高和鲁棒性强的优点。

【技术实现步骤摘要】
一种基于改进平方根无迹卡尔曼滤波的室内定位方法
本专利技术涉及室内定位
，特别是一种基于改进平方根无迹卡尔曼滤波的室内定位方法。
技术介绍
无线室内定位技术的应用范围十分广泛，该技术的出现解决了全球定位系统(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＝xi2+yi2+zi2；设定第一个定位基站为主传感器，则定位目标到从传感器和到主传感器之间的距离差为Ri,1，信号传播时间差，即TDOA测量值为ti,1，则：Ri,1＝cti,1＝Ri-R1(2)其中c为传输信号的速率即光速；根据式(1)和式(2)得：在式(1)中，令i＝1，则得：R12＝K1-2x1x-2y1y-2z1z+x2+y2+z2(4)用式(3)减去式(4)得：Ri,12+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-bk)，b为遗忘因子，取值范围为(0,1)；误差协方差矩阵表示k时刻的误差协方差平方根预测值；上标为T代表式该矩阵的转置；Hk为是k时刻近似的观测转移矩阵，Hk的表达式如式(11)所示：式(11)中，表示第i个采样点误差协方差权重；表示第i个采样点的观测预测值。当在滤波过程中趋近收敛时，误差协方差矩阵会逐渐减小，并且Hk是k时刻近似的观测转移矩阵，是一个有限常数，因此的值会逐渐减小，此时对观测噪声协方差的影响较小。因此，可通过简化观测噪声的协方差来提高计算效率，将式(10)简化如下：Sage-Husa算法通过自适应因子dk来调整观测噪声，dk越大，表明对传感器的观测噪声的统计特性的置信度越高，反之，则相反。在基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法中，对自适应因子dk进行改进，从而解决收敛速速与收敛值的不匹配的问题。通过自适应调节本文档来自技高网...

【技术保护点】
1.一种基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，包括以下步骤：步骤1：建立UWB定位模型；步骤2：建立行人航位推算PDR模型；步骤3：建立UWB/PDR组合定位模型；步骤4：建立改进平方根无迹卡尔曼滤波算法；步骤5：使用UWB进行实时定位，同时惯导依据初始位置进行行人航位推算，接收到TDOA数据后，用残差鉴别法鉴别是否存在NLOS情况：若存在NLOS，则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合，对UWB解算坐标数据进行修正，重复步骤5；若不存在NLOS，则采用UWB解析出当前坐标，完成实时定位。

【技术特征摘要】
1.一种基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，包括以下步骤：步骤1：建立UWB定位模型；步骤2：建立行人航位推算PDR模型；步骤3：建立UWB/PDR组合定位模型；步骤4：建立改进平方根无迹卡尔曼滤波算法；步骤5：使用UWB进行实时定位，同时惯导依据初始位置进行行人航位推算，接收到TDOA数据后，用残差鉴别法鉴别是否存在NLOS情况：若存在NLOS，则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合，对UWB解算坐标数据进行修正，重复步骤5；若不存在NLOS，则采用UWB解析出当前坐标，完成实时定位。2.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，步骤1所述的建立UWB定位模型，具体如下：在三维空间中，利用4个传感器所得到的TDOA测量值来估算待定位节点的位置，设定基站传感器的坐标为(xi，yi，zi)，其中i＝1，2，3，4，待定位点A的坐标为(x，y，z)，则第i个定位基站到A点的距离Ri为：其中Ki＝xi2+yi2+zi2；设定第一个定位基站为主传感器，则定位目标到从传感器和到主传感器之间的距离差为Ri，1，信号传播时间差，即TDOA测量值为ti，1，则：Ri，1＝cti，1＝Ri-R1(2)其中c为传输信号的速率即光速；根据式(1)和式(2)得：在式(1)中，令i＝1，则得：R12＝K1-2x1x-2y1y-2z1z+x2+y2+z2(4)用式(3)减去式(4)得：Ri，12+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值，得到待定位目标的位置。3.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，步骤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包括初始位姿、步频检测、步长估计和航向计算：所述步频检测，通过峰值检测法，基于行人的行进过程中产生的行进方向和竖直方向的正负加速度，来确定行人的步频；所述步长估计，根据行人的生理特征和行走习惯建立模型，得到步长估计；所述航向计算，通过扩展卡尔曼滤波融合陀螺仪、加速度计和磁力计的数据，得到航向角估计值。4.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，步骤4所述的建立改进平方根无迹卡尔曼滤波算法，具体如下：结合基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法和基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法，建立改进平方根无迹卡尔曼滤波算法，根据发散判据判断滤波器是否滤波发散，当未发散时，采用基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法，反之，则采用基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法；当在滤波过程中趋近收敛时，通过简化观测噪声的协方差来提高计算效率，得k时刻的观测噪声协方差Rk如下：式(10)中，Rk-1表示k-1时刻的观测噪声协方差，zk表示时k刻的观测量，为加权后k时刻的观测预测值，自适应因子dk＝(1-b)/(1-bk)，b为遗忘因子，取值范围为(0，1)；误差协方差矩阵表示k时刻的误差协方差平方根预测值；上标为T代表式该矩阵的转置；Hk为是k时刻近似的观测转移矩阵，Hk的表达式如式(11)所示：式(11)中，表示第i个采样点误差协方差权重；表示第i个采样点的观测预测值；当在滤波过程中趋近收敛时，误差协方差矩阵逐渐减小，并且Hk是k时刻近似的观测转移矩阵，为有限常数，因此的值逐渐减小，通过简化观测噪声的协方差来提高计算效率，将式(10)简化如下：通过自适应调节参数Wk来调节自适应因子dk，从而自适应调节不同传感器噪声特性对于系统的影响，将式(12...

【专利技术属性】
技术研发人员：刘源，杨行，包建东，胡明月，孙强，邢宗义，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：江苏,32