[发明专利]一种高冗余的室内煤场导航方法及系统

权利要求书

1.一种高冗余的室内煤场导航方法，其特征在于，包括步骤：

S1.分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据；

S2.判断获取的SLAM子系统的导航数据是否合格，若是，则选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；

S3.根据所述待融合数据建立卡尔曼滤波模型，根据所述建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据。

2.根据权利要求1所述的一种高冗余的室内煤场导航方法，其特征在于，所述步骤S1中获取的UWB子系统的导航数据为无人机当前时刻的位置x_U；获取的INS子系统的导航数据为无人机当前的加速度a_I；获取的SLAM子系统的导航数据为无人机当前时刻的位置加速度a_S、速度v_S、位置x_S。

3.根据权利要求2所述的一种高冗余的室内煤场导航方法，其特征在于，所述步骤S2具体为：

S21.分别计算SLAM子系统的位置x_S与UWB子系统的位置x_U之间的差值Δx以及协方差E；

S22.判断计算得到差值Δx是否超过第一预设阈值和/或判断计算得到的协方差E是否超过第二预设阈值，若是，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据。

4.根据权利要求3所述的一种高冗余的室内煤场导航方法，其特征在于，所述步骤S2中选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据具体为：

选取SLAM子系统中的加速度误差、速度误差、位置误差，并将选取的加速度误差、速度误差、位置误差作为组合导航系统的状态变量，表示为：

X(t)＝[a_x，a_y，a_z，δV_x，δV_y，δV_z，δL_x，δL_y，δL_z]^T

其中，a_x，a_y，a_z分别表示x、y、z轴的加速度误差；δV_x，δV_y，δV_z分别表示x、y、z轴的速度误差；δL_x，δL_y，δL_z分别表示x、y、z轴的位置误差；

根据运动学公式，建立状态变量更新的状态方程：

其中，F_k表示状态转移矩阵；G_k表示噪声驱动矩阵；W_k表示均值为0，方差为Q的噪声。表示k-1时刻状态量的后验，表示k时刻状态量的先验。

W_k＝[ω_x(k)，ω_y(k)，ω_z(k)]^T

选取SLAM子系统与UWB子系统中的位置差值Δx，并将选取的位置差值Δx作为组合导航系统的观测量，表示为：

Z(t)＝x_S-x_U＝Δx＝[δL_x+N_x，δL_y+N_y，δL_z+N_z]^T

其中，N_x，N_y，N_z表示白噪声。