[发明专利]基于UWB双标签的航向计算及其性能分析方法

权利要求书

1.一种基于UWB双标签的航向计算及其性能分析方法，其特征在于，包括：

步骤1，对双标签到至少三个基站之间的距离分别进行测算；

步骤2，基于UKF算法建立UWB系统的定位解算模型；

步骤3，依据步骤1所得的各个距离，结合步骤2所得的定位结算模型获取所述双标签道位置；

步骤4，建立基于UWB双标签道航向解算模型，利用双标签的位置信息进行运动载体的航向解算。

2.根据权利要求1所述的一种基于UWB双标签的航向计算及其性能分析方法，其特征在于，所述步骤1对双标签到至少三个基站之间的距离分别进行测算的具体方法包括：

步骤11，标签向基站发送一个带有标识和时间戳的数据包，基站接收数据包并记录当前时刻；

步骤12，基站向标签发送一个带有标识和时间戳的数据包，标签接收数据包并记录当前时刻；

步骤13，标签再向基站发送一个带有标识和时间戳的数据包，基站接收数据包之后进行信号飞行时间T_prop计算：

其中T_round1为标签信号往返时间，T_round2为基站信号往返时间，T_reply1为基站信号回复时间，T_reply2为标签信号回复时间，

步骤14，标签到基站之间的距离r为：

r＝c×T_prop

其中，c为光速。

3.根据权利要求1所述的一种基于UWB双标签的航向计算及其性能分析方法，其特征在于，所述步骤2基于UKF算法建立UWB系统的定位解算模型的具体方法包括：

步骤21，建立系统状态量方程

状态量为X＝[x，v_x，y，v_y]，其中(x，y)表示标签的位置坐标，(v_x，v_y)表示标签的运动速度，T为采样时间间隔；

步骤22，建立系统观测方程如下：

观测量Z＝[r₁，r₂，r₃，r₄]表示标签至各个基站的距离，UKF传播流程首先求解误差协方差矩阵的均方根采用Cholesky分解求解：

计算sigma点：

式中下标：，i表示矩阵第i列；

每个sigma点可通过系统模型进行传播：

传播过后的状态预测及其误差协方差为：

UKF的观测更新流程由下式产生新的sigma点：

sigma点和平均观测新息可由下式解算：

则观测新息的协方差为：

步骤23，更新UKF卡尔曼增益、状态向量更新和误差协方差得到UWB系统的定位解算模型：

式中：为k时刻状态估计值，为k时刻状态预测值，K_k为k时刻卡尔曼增益值，为k时刻平均观测新息。

4.根据权利要求3所述的一种基于UWB双标签的航向计算及其性能分析方法，其特征在于，所述步骤3依据步骤1所得的各个距离，结合步骤2所得的定位结算模型获取所述双标签道位置的具体方法包括：

在UWB定位区域内，2个标签实时测量至少到3个基站之间的距离，利用该距离信息，按照步骤2进行状态估计，分别得到各个时刻2个标签的状态估计值，根据步骤2中状态量的选取，提取2个标签状态量中的位置信息，得到2个标签的坐标Tag1(x_Tag1，y_Tag1)和Tag2(x_Tag2，y_Tag2)。

5.根据权利要求4所述的一种基于UWB双标签的航向计算及其性能分析方法，其特征在于，所述步骤4建立基于UWB双标签道航向解算模型，利用双标签的位置信息进行运动载体的航向解算的具体方法包括：

根据步骤3得到2个标签的位置信息分别为Tag1(x_Tag1，y_Tag1)和Tag2(x_Tag2，y_Tag2)，利用双标签的位置信息进行运动载体的航向解算，航向