[发明专利]采用伪距和位置信息的室内行人UWB/INS紧组合导航系统及方法

摘要

本发明公开了一种采用伪距和位置信息的室内行人UWB/INS紧组合导航系统及方法，包括：惯性导航器件INS、UWB定位标签、UWB参考节点、数据处理系统；所述惯性导航器件INS和UWB定位标签分别设置在行人的鞋子和身体上，所述UWB参考节点设置在任意位置，所述惯性导航器件INS和UWB定位标签分别与数据处理系统连接。本发明有益效果：通过将参考节点的位置信息引入状态变量中，并通过滤波器完成对参考节点位置信息的预估，减少了组合导航系统需要预先获取参考节点位置信息以及组合导航系统的定位精度依赖于预先获取到的参考节点位置信息精度的缺点。

主权项

1.一种采用伪距和位置信息的室内行人UWB/INS紧组合导航方法，其特征是，包括：(1)以惯性导航器件INS的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件INS测量得到的目标行人的东向和北向位置信息的平方和与UWB定位标签测量得到的伪距的平方之差作为系统观测量，构建UWB/INS组合导航模型；(2)利用所述UWB/INS组合导航模型，通过迭代扩展卡尔曼滤波器将惯性导航器件INS在本地相对坐标系中获取的目标行人的导航信息以及UWB定位标签在本地相对坐标系中获取的伪距信息进行数据融合；最终得到当前时刻目标行人最优的导航信息以及参考节点位置的最优估计；所述迭代扩展卡尔曼滤波器的状态方程为：其中，为15维INS误差向量；为2M维参考节点坐标向量；M为参考节点数目；为惯性导航器件INS测量得到的导航坐标系下目标行人的东向、北向和天向三个方向的加速度误差；φ_k、φ_k+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下目标行人的横摇、纵摇和航向角误差；分别为k时刻和k+1时刻INS测量得到的导航坐标系下目标行人的东向、北向和天向三个方向的速度误差；分别为k时刻和k+1时刻导航坐标系下目标行人的东向、北向和天向三个方向的加速度误差；分别为k时刻和k+1时刻导航坐标系下目标行人的东向、北向和天向三个方向的角速度误差；分别为k时刻和k+1时刻导航坐标系下目标行人的东向、北向和天向三个方向的位置误差；ω_k为状态噪声；T为采样周期；I为单位矩阵；为从载体系到导航系的状态转移矩阵。