一种适用于室内遮挡环境下的UWB高精度定位方法

专利主权项内容

1.一种适用于室内遮挡环境下的UWB高精度定位方法，其特征在于，包括如下步骤：步骤1，在室内空间中预先部署多个UWB基站且获取该室内空间结构信息，以及将室内空间预定位置设置为坐标原点，绘制以该坐标原点为基准且反映该室内空间结构信息的室内空间二维平面图；步骤2，利用全站仪打点的测量方法获取室内空间中已部署的各UWB基站相对于坐标原点的水平距离，并在室内空间二维平面图中分别标注出各UWB基站相对于坐标原点的坐标位置；步骤3，建立能够区分已部署各UWB基站LOS区域和NLOS区域的UWB基站LOS/NLOS区域信息图谱；其中，UWB基站LOS/NLOS区域信息图谱的建立过程包括如下步骤a1～a5：步骤a1，将室内空间二维平面图划分成若干所需要边长的小方格；其中，该室内空间二维平面图由划分的该若干小方格所组成；步骤a2，将每一个小方格的四个顶点均作为采集点，且由所有的采集点形成对应该室内空间二维平面图的采集点集合；步骤a3，以部署在室内空间中的任一个UWB基站为源点，对该源点利用射线法遍历采集点集合内的所有采集点；步骤a4，由遍历的每一个采集点记录对应该UWB基站的UWB基站信息；其中，对应该UWB基站的UWB基站信息包括北向坐标、东向坐标和UWB基站的LOS/NLOS信息，UWB基站为LOS基站则标记其LOS信息为1，UWB基站为NLOS基站则标记其LOS信息为0；步骤a5，对部署在室内空间中的所有UWB基站均执行步骤a3和步骤a4，且以记录的所有UWB基站信息形成的图谱作为所述UWB基站LOS/NLOS区域信息图谱；步骤4，在定位过程中，根据针对定位终端在当前时刻的上一时刻的定位结果以及已建立的UWB基站LOS/NLOS区域信息图谱，在室内空间中已部署的所有UWB基站中挑选出所有LOS基站，分别获取该所有LOS基站中每一个LOS基站的测距值；其中，LOS基站的测距值为LOS基站到定位终端在当前时刻所处位置的直线距离；LOS基站的挑选过程包括如下步骤b1～b3：步骤b1，以当前时刻的上一时刻解算出来的定位终端位置点作为圆心，以设定距离值作为半径，构建统计用圆形区域；步骤b2，统计该统计用圆形区域内各UWB基站的LOS/NLOS信息；步骤b3，将统计用圆形区域内UWB基站的LOS信息为0的UWB基站排除，并将该统计用圆形区域内UWB基站的LOS信息为1的UWB基站均作为LOS基站；步骤5，构建针对抗差自适应扩展卡尔曼滤波的抗差自适应因子；步骤6，利用获取的所有LOS基站的测距值以及构建的抗差自适应因子，构建抗差自适应扩展卡尔曼滤波模型；其中，该抗差自适应扩展卡尔曼滤波模型的构建包括如下步骤c1～c11：步骤c1，设置针对扩展卡尔曼滤波的非线性状态估计方程和观测量的观测方程；其中；非线性状态估计方程为：X＝FX+Bu+w；kkk-1kkk其中，X为k时刻的状态向量且二维定位结果中的状态向量X为(x, y)，(x, y)表示k时刻解算出来的定位终端所处的定位位置，X为k-1时刻的状态向量，F为k时刻的状态转移矩阵，B为k时刻的控制矩阵，u为k时刻的控制向量，w为在k时刻符合零均值且协方差矩阵为Q的正态分布过程噪声；kkkkkkk-1kkkkk观测量的观测方程为：Z＝HX+v；kkkk其中，Z为k时刻的观测向量，即UWB基站的测距值，H为k时刻的观测矩阵，v为在k时刻符合零均值且协方差矩阵为R的正态分布观测噪声；kkkk步骤c2，计算扩展卡尔曼滤波中非线性状态估计方程的状态转移矩阵以及观测方程的观测矩阵；其中：F＝df/dx∣；kXk, k-1, ukH＝dh/dx∣；kXk, k-1其中，X表示k-1时刻对k时刻状态的最佳估计，即解算出的定位终端在k-1时刻的最佳估计位置；f(·)表示非线性函数，h(·)表示非线性函数；k, k-1步骤c3，设置状态预测方程及卡尔曼增益矩阵；其中：状态预测方程标记为X＝f(X, u)；X表示k-1时刻状态的最佳估计；k, k-1k-1, k-1kk-1, k-1卡尔曼增益矩阵标记为K，K＝PH(HPH+R)；kkk, k-1kkk, k-1kTk-1步骤c4，根据已设置的状态预测方程及卡尔曼增益矩阵，获取更新后状态估计方程和更新后验估计误差协方差矩阵；其中：更新后状态估计方程为：X＝X+K(Z-HX)；k, kk, k-1kkkk, k-1更新后验估计误差协方差矩阵为：P＝(I-KH)P；I为单位矩阵；k, kkkk, k-1步骤c5，计算得到抗差自适应扩展卡尔曼滤波信息以及该信息的新息协方差矩阵；其中，抗差自适应扩展卡尔曼滤波信息标记为V，V＝Z-h(x)；kkkk, k-1步骤c6，计算所得抗差自适应扩展卡尔曼滤波信息的新息协方差矩阵以及新息协方差；其中，新息协方差矩阵标记为P，P＝E[VV]；新息协方差标记为P，P＝HPH+R；kZkZkkZkckckk, k-1kTk步骤c7，根据所得新息协方差矩阵以及新息协方差，计算自适应因子；其中，该自适应因子标记为s，s＝diag(P)/diag(P)；kkkZkc步骤c8，利用所得自适应因子对系统的测量噪声进行调整，得到系统调整后的测量噪声；其中，系统调整后的测量噪声标记为R；k’当s≤k时，R＝1；k0k’当k<s<k时，R＝(s/k)[(k-k)/(k-s)]；0k1k’k0101k2当s≥k时，R＝+∞；k1k’其中，k和k均为稳健参数；01步骤c9，根据所得系统调整后的测量噪声和已设置的卡尔曼增益矩阵，得到抗差自适应扩展卡尔曼滤波的增益；其中，该抗差自适应扩展卡尔曼滤波的增益标记为K；K＝PH(HPH+R)；k’k’k, k-1kkk, k-1kTk’-1步骤c10，利用所得抗差自适应扩展卡尔曼滤波的增益更新状态估计和误差协方差矩阵，得到抗差自适应后的状态估计，以优化因随机行人遮挡而产生的NLOS误差；其中，抗差自适应后的状态估计标记为X＝X+K(Z-HX)；k, kk, k-1k’kkk, k-1步骤c11，根据当前时刻的上一时刻的状态估计值和当前时刻的观测向量，并结合该抗差自适应扩展卡尔曼滤波的增益，解算出当前时刻的最佳状态向量，且将该最佳状态向量作为当前时刻定位终端位置的最佳估计值；步骤7，根据挑选出的所有LOS基站数量做出判断处理：当所有LOS基站数量大于3时，利用构建的抗差自适应扩展卡尔曼滤波模型解算定位终端位置；否则，利用历史轨迹约束解算定位终端位置；其中，利用约束解算定位终端位置的过程包括如下步骤：步骤d1，以历史轨迹判断运动趋势，且以解算出来的定位终端在当前时刻的上一时刻所处位置点作为切点，沿定位终端在上一时刻的运动趋势方向做出历史轨迹在该切点处的切线，并利用几何方法计算出定位终端在当前时刻的位置坐标；其中，历史轨迹为曲线；步骤d2，根据挑选出所有LOS基站数量做出判断处理：当所有LOS基站数量为1时，以该UWB基站所处位置为圆心且以该UWB基站的测距值为半径画圆；获取该圆与已做出的切线之间的交点数量，且根据获取的交点数量做出判断处理：当交点数量为1时，将该交点的坐标作为定位终端在当前时刻的定位结果；当交点数量为2时，将位于当前2个交点中且与解算出来的定位终端在上一时刻位置点最近的交点作为定位终端在当前时刻的定位结果；当交点数量为0时，将位于该圆上且与已做出的切线距离最近的点作为定位终端在当前时刻的定位结果；当所有LOS基站数量为2时，分别以每个UWB基站为圆心且以每个UWB基站各自的测距值为半径画圆，得到两个圆；获取该两个圆分别与已做出的切线的交点；根据获取的交点数量做出判断处理：当获取的交点数量大于0时，计算获取的所有交点所形成图形的重心，并以该重心作为定位终端在当前时刻的定位结果；当获取的交点数量为0时，获取该两圆的两圆心连线与已做出的切线的交点，并以获取的该交点作为定位终端在当前时刻的定位结果。