[发明专利]一种基于容错决策树的UWB辅助惯性定位方法

权利要求书

1.一种基于容错决策树的UWB辅助惯性定位方法，其特征在于，包括以下步骤：

步骤一，使用加速度和磁力计解算得到初始横滚角、俯仰角和航向角；

步骤二，采用陀螺仪分段拟合误差补偿模型区分补偿高低动态速率段，更进一步消除角速率输出误差，并根据步骤一中的初始横滚角、俯仰角和航向角，采用四元数算法获得姿态角信息数据；

步骤三，根据加速度特征数据，发现行人走路的波峰、零点和波谷数据有似正弦波的周期性规律，获得有效步数，并利用加速度与人员行走时刻步长的波峰波谷差值之间的关联性，进行自适应步长计算，得到惯性定位数据信息；

步骤四，采用UWB双向飞行时间测量UWB模块间距离，随后根据三边测距定位方法计算行人当前时刻的位置；

步骤五，将步骤四获得的UWB行人当前时刻的位置作为容错决策树判定模型的输入量，步骤三获得的惯性定位数据作为容错决策树判定模型的参考量，进行容错判定，得到当前时刻UWB单点定位数据；所述容错判定包括，第一层容错判定，获知UWB标签是否接收到定位基站的脉冲信号，判定是否存在UWB单点定位数据；第二层容错判定，根据行人运动规律特性，利用当前k时刻惯导解算对应的航向角ψ_k和初始航向角ψ_zero进行判断行人是否发生转弯模式变化；第三层容错判定，采用同一采样点的姿态角信息与UWB定位信息之差Δp，并结合行人运动步数与距离之间的关系，进行测量数据的活动阈值判定；

所述第三层容错判定的Δp规定如下：

式中，P_ins,k代表惯性导航在k时刻的位置数据，P_uwb,k代表UWB在k时刻单点定位距离数据，σ代表惯性导航与UWB位置数据之差的活动阈值，活动阈值σ能同时满足下列行走步数范围的关系式：

式中，step表示加速度解算的有效步数，σ₁、σ₂、σ₃表示分段步数对应的阈值；

步骤六，将步骤三获得的惯性定位数据和步骤五获得的UWB单点定位数据进行扩展卡尔曼滤波定位信息融合。

2.根据权利要求1所述一种基于容错决策树的UWB辅助惯性定位方法，其特征在于：步骤二所述陀螺仪分段拟合误差补偿模型，分析传感器自身的安装标定误差和非线性误差，利用公式(1)角速率输出误差模型，从而得到陀螺仪的补偿矩阵：

ω_x、ω_y、ω_z分别代表x、y、z轴的陀螺仪真实角速率，S_x、S_z、S_y代表x、y、z轴的陀螺仪标定因数，K_yx、K_zx、K_xy、K_zy、K_xz、K_yz代表x、y、z轴的陀螺仪安装误差系数，B_x、B_y、B_z分别代表x、y、z轴的陀螺仪零偏，w_x、w_y、w_z分别代表x、y、z轴的陀螺仪实际角速率测量值。

3.根据权利要求2所述一种基于容错决策树的UWB辅助惯性定位方法，其特征在于：所述分段拟合误差补偿中，选取三轴转台动态速率为-100°/s～100°/s，以10°/s作为间隔，共20个角速率，每组采集保留500kb数据，按照速率数据正负对称和动态速率高低，将20个角速率分为8组，每段包含的角速率密度为m，在每个分段组中对其数据进行二次拟合，得到该分段组更为接近真实角速率的标定因数。