[发明专利]基于UWB无线定位与IMU融合的导航定位系统及方法

权利要求书

1.一种基于UWB无线定位与IMU融合的导航定位系统,其特征在于，包括：

安装于指定工作区域内的至少四个UWB基站；

由两个UWB定位标签和两个IMU组成的标签装置；其中一个所述UWB定位标签与一个IMU以刚体连接方式安装在一起组成第一复合标签，另外一个所述UWB定位标签和IMU以刚体连接安装在一起组成第二复合标签，所述第一复合标签与第二复合标签以设定距离间隔安装在固定组件上组成所述标签装置；

主控装置，用于接收所述标签装置数据，采用卡尔曼滤波器紧耦合的方式对UWB标签数据和IMU数据进行实时融合，得到当前位置数据和航向角数据；

由所述标签装置及主控装置组成的定位装置。

2.根据权利要求1所述的基于UWB无线定位与IMU融合的导航定位系统,其特征在于，所述第一复合标签与第二复合标签之间的距离为0.5m~1.0m。

3.一种应用权利要求1或2所述导航定位系统进行导航定位的方法，其特征在于，包括如下步骤：

步骤1，在多个UWB基站中选取其中一个作为参考坐标原点，精确测量其他基站的相对位置坐标，从而建立参考坐标系；

步骤2，获取UWB定位标签到各基站的UWB测距信息；获取IMU的加速度计和陀螺仪信息进行姿态解算，得到标签装置的IMU线性加速度信息；

步骤3，采用紧耦合的方式构建卡尔曼滤波器模型，对UWB测距信息和IMU线性加速度信息进行融合；所述卡尔曼滤波器模型包括位置卡尔曼滤波器模型及角度卡尔曼滤波器模型；

步骤4，启动布置于工作区域内的定位装置，进行位置卡尔曼滤波器模型初始化及角度卡尔曼滤波器模型初始化，得到初始位置坐标和航向角，并进入位置跟踪状态及角度跟踪状态；

步骤5，初始化完成后，实时获取UWB定位标签和IMU的测量数据，并对位置卡尔曼滤波器模型和角度卡尔曼滤波器模型进行实时更新，得到当前位置数据和航向角数据。

4.根据权利要求3所述的方法，其特征在于，所述卡尔曼滤波器模型用于：

系统状态预测，计算公式为：

U(k)

上式中，A为系统的状态转移矩阵，表示本次系统的最优估计值,在本系统构建的公式中指由位置、速度、加速度计误差一同构成的矩阵，表示系统的控制矩阵，表示系统的控制量，本系统中指加速度矩阵；

系统协方差预预测，计算公式为:

上式中，A为系统的状态转移矩阵，为本次系统先验协方差最优化估计值，为上一次系统的先验协方差最优化估计值，Ｑ为系统的过程噪声协方差矩阵；

系统的卡尔曼增益计算，计算公式为:

上式中，表示k时刻系统的卡尔曼增益，H为系统的测量矩阵，R为测量噪声协方差矩阵，；

滤波器更新，最优状态估计公式为:

上式中，表示k时刻的系统最优状态估计值，表示k时刻的系统测量值，为k-1时刻的系统最优状态估计值，为k-2时刻系统的最优状态估计值，为k时刻系统的卡尔曼增益，H为系统的测量矩阵;

滤波器更新，最优状态协方差公式：

上式中，为k时刻系统后验估计协方差，为k时刻系统的卡尔曼增益，H为系统的测量矩阵，为k-1时刻系统的后验协方差最优化估计结果。