[发明专利]一种基于多状态约束的UWB-IMU组合室内定位方法

权利要求书

1.一种基于多状态约束的UWB-IMU组合室内定位方法，其特征在于，具体包含如下步骤：

S1.系统状态初始化；

S2.进行IMU的状态和协方差传播更新；

S3.判断是否检测到UWB数据，如果为是，则执行步骤S4，否则执行S7；

S4.进行UWB关键帧检测，如果是关键帧则执行S5，否则执行S6；

S5.进行UWB状态扩增；

所述步骤S5中，进行UWB状态扩增的步骤如下：

当接收到一个UWB数据帧之后会将当前时刻与上一时刻之间的所有IMU数据整体进行状态预测，在完成所有IMU数据的状态预测后，紧接着就是状态扩增，对于标称状态，直接将当前时刻IMU状态的标称状态拿来作为当前时刻IMU的标称状态；

对于误差状态，均值不用扩增，因为在观测发生前，所有误差状态的均值都为零，最重要的步骤是误差状态的协方差矩阵扩增，状态扩增分为两个步骤：克隆与修正两步；

S6.进行UWB多状态约束更新IMU状态；

S7.若UWB状态数量达到窗口大小，进行UWB状态边缘化，继续执行S2，否则直接执行S2。

2.根据权利要求1所述的基于多状态约束的UWB-IMU组合室内定位方法，其特征在于，所述步骤S5中状态扩增两个步骤具体如下：

第一步克隆如下：

第二步修正如下：

上式中，X_i为IMU的状态变量，为传统的15维状态变量,即IMU的位置误差、速度误差、姿态误差、加速度计零偏误差以及陀螺仪零偏误差，X_j为扩增的UWB状态变量，即UWB的三维位置误差状态，P_ii为IMU状态变量之间的协方差，P_jj为UWB状态变量之间的协方差，P_ij为IMU和UWB状态变量之间的协方差，J_ji为新增状态变量X_j相对X_i的雅克比矩阵，在多状态约束卡尔曼滤波中，新增误差UWB状态对当前误差状态的雅克比为：

其中X_U为UWB状态，X_I为IMU状态，上标T表示矩阵转置，第二项是当前UWB误差状态变量相对其他误差UWB状态变量的雅克比，为零矩阵，第一项为新增UWB误差状态变量相对IMU误差状态变量的雅克比，结果如下：

式中为UWB安装杆臂值，根据以上推导的内容即可进行系统状态的扩增。

3.根据权利要求1所述的基于多状态约束的UWB-IMU组合室内定位方法，其特征在于，所述步骤S6中，进行UWB多状态约束更新IMU状态的步骤如下：

状态扩增后开始构建观测更新对误差状态进行更新，也就是执行卡尔曼滤波中的后三步，首先是观测方程的构建，观测方程如下：

z_i＝H_iX+n_i (5)

上式表示第i个UWB状态对应的观测方程，z_i是对应的观测值，为第i个UWB状态对应的UWB测距值与理论推导的UWB标签与固定基站之间的距离之差。H_i和n_i分别为对应的观测方程和误差向量。将m个观测方程合并得到：

z＝HX+n (6)

以下为卡尔曼滤波的通用公式：

完成状态更新后即可等待下一次滤波状态扩增与更新，当UWB状态数量达到窗口最大值时，需要将对应的最老的UWB状态从窗口中删去，删去操作只需要将协方差矩阵中该UWB状态对应的行与列删去即可完成状态删除。