[发明专利]一种基于UWB/IMU/气压计的室内行人3D定位方法

权利要求书

1.一种基于UWB/IMU/气压计的室内行人3D定位方法，其特征在于：具体包含如下步骤；

S1：读取气压计的读数，将数据变化值与设定的阈值进行比较：若数据变化值小于阈值，说明不涉及高程变化，待定位人员在某一楼层运动，则转入步骤S2；若数据变化值大于阈值，说明待定位人员涉及高程变化，在楼梯间上下楼梯，则转入步骤S5；

S2：根据气压计示数确定待定位人员所在的楼层；读取UWB信号，比较接收到的UWB信号的个数与满足定位要求的信号个数，若接收到的信号个数少于指定要求，即不满足UWB定位要求，则用IMU单独定位，转入步骤S3；若接收到的信号个数大于等于指定要求，则采用UWB/IMU紧组合的方法进行定位，转入步骤S4；

S3：当不满足UWB定位要求时，使用IMU单独定位；利用IMU输出的加速度、角速度信息，进行惯导解算，得到位置与速度信息，添加地图约束条件，对出现在不可到达区域的定位点进行修正，输出相对准确的位置信息，跳转至步骤S6；

S4：对UWB设备进行标定，获得测距模型；获取UWB标签与基站之间的距离信息；利用已经获得的测距模型对定位过程中获取的距离信息进行预处理，以获得更加准确的距离值，作为EKF的观测数据之一；

IMU输出的加速度与角速度信息采用惯导方法进行解算，获得导航信息，并将速度与位置作为EKF的观测数据之二；

利用EKF得到导航信息的误差估计并与惯导解算出的导航信息做差，得到UWB/IMU紧组合的定位结果；

与已知的该楼层的地图信息相比较，对定位结果加以地图约束，对出现在不可到达区域的定位结果点进行处理，得到最终的UWB/IMU/地图滤波组合的定位结果，即待定位人员所在的楼层、楼层内的平面定位已实现，跳转至步骤S6；

S5：当待定位人员在上下楼梯时，根据气压计示数的变化来判断上楼或下楼：若气压计的示数变小，则为上楼；若气压计示数变大，则为下楼；

楼梯间的定位需要提供信息包括三维坐标，IMU输出加速度、角速度，经过惯导方法解算得到载体的三维坐标，用气压计对高程信息进行修正，得到更为准确的楼梯间的三维坐标，跳转至步骤S6；

S6：待定位人员某一时刻的定位结果已知，跳转至S1进行下一时刻的定位。

2.根据权利要求1所述的一种基于UWB/IMU/气压计的室内行人3D定位方法，其特征在于：在步骤S2中，UWB/IMU紧组合的方法具体如下：

式中，为状态向量，为四个参考节点的位置坐标向量；为k时刻15维惯导解算误差向量，f_k为姿态误差向量，为速度误差向量，为位置误差向量，为加速度计误差向量，为陀螺仪误差向量，且这5个误差向量均含有3个元素；ω_k代表协方差矩阵为Q_k的均值为零的正态分布系统噪声；为惯导解算误差的状态转移矩阵；

观测方程为：

式中，为速度误差的观测向量；为UWB的观测向量，为标签与各基站的距离测量值；为系统的观测噪声矩阵，其协方差矩阵为R_k。