CN112747747B

CN112747747B - 一种改进的uwb/imu融合室内行人定位方法

Info

Publication number: CN112747747B
Application number: CN202110077136.2A
Authority: CN
Inventors: 刘期烈; 万志鹏; 周文敏; 陈澄; 屈喜龙; 黄东; 刘竟成; 成奎; 胡壹
Original assignee: Chongqing University of Post and Telecommunications
Current assignee: Chongqing University of Post and Telecommunications
Priority date: 2021-01-20
Filing date: 2021-01-20
Publication date: 2022-10-11
Anticipated expiration: 2041-01-20
Also published as: CN112747747A

Abstract

本发明涉及室内定位技术领域，具体涉及一种改进的UWB/IMU融合室内行人定位方法，包括建立UWB测量模型，获取位置的估计值；建立IMU测量模型，利用加速度和角速度解算得到位置的估计值；计算根据UWB测量模型和IMU测量模型得到的位置的估计值的差值；若该差值小于设定的阈值则将UWB测量模型和IMU测量模型得到的位置的估计值进行融合；否则判断UWB测量模型的定位准确度，若UWB测量模型的定位准确度小于设置的阈值，则将UWB测量模型的得到的二维坐标加上差分气压计测量的高度作为最终的位置预测值；否则将通过测距解算出的高度确定每个测距值的权值，并通过加权最小二乘法得到二维定位坐标加上差分气压计测量的高度作为位置预测值；本发明克服了单一定位系统在复杂环境下的局限性。

Description

一种改进的UWB/IMU融合室内行人定位方法

技术领域

本发明涉及室内定位技术领域，具体涉及一种改进的UWB/IMU融合室内行人定位方法。

背景技术

随着近距离通信技术及无线定位技术的发展，人们期待移动设备可以感知位置从而提供导航定位服务。而室内定位技术弥补了GPS因信号受建筑物遮挡而无法用于室内环境定位的不足，将定位服务从城市室外空间扩展至建筑物内部空间。

目前出现了各式各样的室内定位技术，比如红外线定位、超声波定位、Wi-Fi定位、蓝牙定位、UWB定位、IMU定位等。这些定位技术各有优缺点，应对的场景也各不相同。其中，UWB定位技术凭借其定位精度高、传输率高、功耗低等特点，使得其十分适用于室内定位领域。但是在室内复杂环境下，UWB定位技术容易受到NLOS环境影响。而IMU集成了加速度计、陀螺仪等传感器，可以测量加速度、角速度等运动状态信息。但是IMU器件本身存在测量误差，且由于IMU定位使用的是相对位移，会造成误差的累积。因此，使用单一定位系统进行定位，仍然存在很大的局限性，无法实现复杂的室内环境下的高精度定位。

发明内容

为了能够在复杂室内环境进行高精度定位，本发明提出一种改进的UWB/IMU融合室内行人定位方法，具体包括以下步骤：

建立UWB测量模型，利用N个基站的位置信息以及标签坐标信息，获取位置的估计值；

建立IMU测量模型，利用加速度和角速度解算得到位置的估计值；

计算根据UWB测量模型和IMU测量模型得到的位置的估计值的差值；

若该差值不大于设定的阈值则将UWB测量模型和IMU测量模型，则直接将该差值作为观测量输入卡尔曼滤波模型获取最终定位；

若该差值大于设定的阈值且UWB测量模型定位准确，则在IMU测量模型之后的测量值中引入矫正因子δJ，矫正因子为k时刻IMU测量模型与UWB测量模型定位差值，在k+1时刻及之后的IMU测量值都要引入该矫正因子，表示为：

P(k+1)_IMU'＝P(k+1)_IMU-δJ；

其中，P(k+1)_IMU为k+1时刻IMU测量模型得出的位置的估计值，P(k+1)_IMU'为P(k+1)_IMU通过矫正因子矫正够的值；

判断UWB测量模型是否定位准确时，若UWB测量模型的定位准确度大于设置的阈值，则将UWB测量模型的得到的二维坐标加上差分气压计测量的高度作为UWB测量模型最终的位置预测值；否则通过测距解算出的高度确定每个测距值的权值，并通过加权最小二乘法得到二维定位坐标加上差分气压计测量的高度作为位置预测值。

进一步的，建立UWB测量模型获取位置的估计值包括：

若一共有N个基站，计算第i个基站的二维位置坐标与标签的二维坐标之间的距离；

以其他N-1个基站与标签之间的距离减去第i个基站与标签之间的距离构建方程式；

使用最小二乘法求解目标最优估计位置，将方程式转换为矩阵形式；

构造代价函数，利用最小二乘法求解代价函数，该代价函数最小值即为位置的估计值。

进一步的，根据代价函数获取位置的估计值表示为：

其中，

为根据UWB测量模型获取的位置的估计值矩阵；A为从线性化后的方程组中提取的系数矩阵，b从线性化后的方程组中提取的常数项矩阵。

进一步的，建立IMU测量模型获取位置的估计值包括：

惯性导航系统在载体运动时，通过陀螺仪输出不断改变的角速度信息，进而得到载体的运动姿态及航向角信息；

通过加速度计输出载体坐标系下的加速度信息，并通过一次积分得到载体的运动速度、二次积分得到载体的运动距离。

进一步的，IMU测量模型获取位置的估计值表示为：

其中，a为加速度计测得的加速度，ω为陀螺仪测得的角速度，t为运动时间；v₀、s₀和θ₀分别表示目标初始的速度、位移和运动方位角；v₁、s₁和θ₁分别表示目标当前的速度、位移和运动方位角。

进一步的，判断UWB测量模型的定位准确度的过程包括：

通过标签与任意三个基站的距离构造一个四面体，结算的得到当前四面体中标签的高度；

计算标签与所有基站组合构成四面体中标签的高度，并与差分气压计测得标签高度进行比较，得到每个组合的高度差值；

筛选出每个组合的高度差值中最小的一个值与设置的阈值进行比较，若小于阈值则UWB测量模型的定位准确，否则UWB测量模型的定位不准确。

进一步的，通过测距解算出的高度确定每个测距值的权值，并通过加权最小二乘法得到二维定位坐标加上差分气压计测量的高度作为位置预测值包括以下步骤：

计算每个组合的高度差值的平均值，将该平均值的倒数作为基站测距值的可信度；

根据勾股定理计算得到标签与每个基站的水平距离，以其他N-1个基站与标签之间的距离减去第i个基站与标签之间的距离、可信度作为权值构建方程式；

进一步的，根据代价函数计算得到的位置估计值表示为：

其中，

为根据UWB测量模型获取的位置的估计值矩阵；A为从方程式中提取出的系数矩阵，b为从方程式中提取出的常数项矩阵；β为基站测距值的可信度矩阵。

进一步的，将UWB测量模型和IMU测量模型得到的位置的估计值进行融合包括：

将UWB测量模型和IMU测量模型得到的位置的估计值的差值输入卡尔曼滤波模型进行滤波；

若UWB测量模型和IMU测量模型得到的位置的估计值的差值大于阈值且UWB测量模型的定位准确，则在IMU测量模型之后的测量值中引入矫正因子δJ，若矫正因子的为k时刻IMU与UWB定位差值，则在k+1时刻及之后的IMU测量值都要引入该矫正因子，表示为：

P(k+1)_IMU'＝P(k+1)_IMU-δJ；

其中，P(k+1)_IMU为k+1时刻IMU测量模型得出的测量值；P(k+1)_IMU'为P(k+1)_IMU通过矫正因子矫正够的值。

本发明与现有技术相比，本发明有以下有益效果：

(1)利用UWB与IMU定位系统的互补特性，克服了单一定位系统在复杂环境下的局限性；

(2)通过引入差分气压计，降低UWB定位受NLOS信号的影响，可以得到精度更高的单一定位结果；

(3)通过在整体融合模型中引入IMU累计误差判定门限，可对UWB、IMU当前定位精度进行判定，可以对IMU定位值进行实时矫正，进而获取更高的融合定位精度。

附图说明

图1为本发明一种改进的UWB/IMU融合室内行人定位方法框架示意图；

图2为本发明中UWB基于TOA测距进行定位示意图；

图3为本发明中UWB/IMU基础融合定位模型示意图；

图4是发明中UWB测量模型改进流程图；

图5是发明中IMU累计误差鉴别流程图；

图6是本发明在NLOS环境下进行UWB动态定位，三边定位算法与改进后的差分加权定位算法的定位轨迹对比图；

图7是本发明在NLOS环境下改进融合定位，基础融合定位与改进后的融合定位的定位轨迹对比图。。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供一种改进的UWB/IMU融合室内行人定位方法，如图1，包括以下步骤：

P(k+1)_IMU'＝P(k+1)_IMU-δJ；

如图2所示，由于室内环境比较复杂，受NLOS影响，TOA测距值比真实值大，使得由测距值得到的三个圆不能相交于一个点，而是相交于一个区域，在本实施例中设标签坐标为(x,y)，第i个基站的坐标为(x_i,y_i)，其中i＝1,2,...,N，以

表示第i个基站与标间两点间的测量距离，根据二维坐标系下的两点间距离公式：

将式(1)扩展到N个基站，并用第2个到第N个等式分别减去第一个等式，可以得到如下方程组：

使用最小二乘法求解目标最优估计位置，将式(2)转换成矩阵形式：

AX＝b

其中，从线性化后的方程组中提取的系数矩阵A、X和从线性化后的方程组中提取的常数项矩阵b分别由式(4)、(5)和(6)给出：

构造代价函数如式(7)所示：

利用最小二乘法求解式(7)，即求该代价函数最小值，可得到位置的估计值：

惯性导航系统在载体运动时，通过陀螺仪输出不断改变的角速度信息，进而得到载体的运动姿态及航向角等信息；通过加速度计输出载体坐标系下的加速度信息，然后一次积分得到载体的运动速度、二次积分得到载体的运动距离：

建立UWB/IMU基础融合定位模型，如图3所示。在融合模型中引入差分气压计，对UWB测量模型进行改进，结合图4，具体如下：

通过UWB系统的标签与其中任意三个基站的距离，可以构造出一个四面体，因此可以得到

个不同的四面体；

以标签与A₁、A₂、A₃基站组成的四面体为例，设标签的三维坐标为(x,y,z)，设A₁基站的三维坐标为(x₁,y₁,z₁)，有：

即可解算出当前四面体中，标签的高度z，作为在选用当前四面体时解算出的标签高度h_k，其中

将这些四面体解算出的标签高度h_k与差分气压计测得标签高度h_barometer进行比较，得到每个组合的高度差值δh_k，以此作为每个基站组合测距受NLOS误差影响情况；

找出UWB的

种定位选择中所得定位高度与差分气压计测得高度相差最小的一组，对其差值|δh(k)|进行如下判断：

|δh(k)|＝|h(k)_UWB-h(k)_barometer| (11)

其中，T_h表示对高度差值设定的阈值，此时可分为两种工作模式：若F₁(k)的值为“1”，判定UWB定位较准，此时直接选择此种定位结果所得到的二维坐标(x,y)，加上差分气压计所得高度h_barometer，得到三维定位结果(x,y,h_barometer)；否则，通过测距解算出的高度确定每个测距值的权值，并通过加权最小二乘法得到二维定位坐标(x,y)，加上差分气压计所得高度h_barometer，得到三维定位结果(x,y,h_baromete)。具体过程如下所示：

求每个基站参与的组合的高度差值之和，再除以个数，即可得到每个基站参与造成的平均高度差δh_i，然后根据该差值即可确定该基站测距值的可信度β_i：

β_i＝1/δh_i (13)

已知标签与每个基站三维测距值d_i3d，使用勾股定理即可得到标签与每个基站测得的水平距离d_i2d：

最后，类比式(7)和(8)，可以构造出加权最小二乘的代价函数和标签位置估计值：

其中：

在融合模型中引入IMU累计误差判定门限，提高融合定位精度，结合图5，具体如下：

UWB和IMU分别完成定位后，将该这两个定位结果相减，在将该差值δP(k)作为观测量输入卡尔曼滤波之前，可求其绝对值并对该值进行判断：

|δP(k)|＝|P(k)_UWB-P(k)_barometer| (18)

其中，T_P表示对定位差值设定的阈值，此时有两种工作模式：若F₂(k)的值为“1”,判定此时UWB与IMU定位结果相差较大，若同时有F₁(k)的值为“1”，则说明此时UWB定位结果比较准确，IMU定位结果累计误差较大。可在IMU之后的测量值中，引入矫正因子δJ，其大小为k时刻IMU与UWB定位差值：

δJ＝P(k)_IMU-P(k)_UWB (20)

因此，在k+1时刻及之后的IMU测量值都要引入该矫正因子：

P(k+1)_IMU'＝P(k+1)_IMU-δJ (21)

否则，直接将UWB与IMU定位结果进行融合，完成实时定位。

如图6所示为NLOS环境下UWB动态定位。由图可知，在NLOS环境下，使用三边测量定位方法得到的动态移动轨迹，平均误差为42cm。而使用差分加权定位算法，平均误差为26cm，相比于三边定位算法定位精度提高了38.09％，可以得到与真实路线更吻合的轨迹。在NLOS环境下，两种定位算法都存在跳点，导致轨迹不够平滑。

如图7所示为绕预定轨迹行走多圈以后，即长时间行走之后，两种融合定位方法轨迹对比。由图可知，在长时间行走后，UWB定位效果与之前差不多，而IMU由于存在误差累计，导致整个轨迹偏移，定位精度降低，从而导致普通融合定位方法定位效果变差，平均误差为30cm。另外，由于IMU起始点误差较大，导致融合轨迹存在跳点。所以可以在融合定位中引入矫正因子，就可以动态纠正IMU累计误差，可以获得更好的融合定位效果，平均误差为17cm。改进的融合模型相比于基础融合模型，定位精度提高了32.50％，且得到的改进融合轨迹较为平滑。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。