[发明专利]一种前后台并行解算容错组合导航方法

权利要求书

1.一种前后台并行解算容错组合导航方法，其特征在于实现步骤如下：

步骤一、进行前台导航；

在获得测距和测速信号后，直接与惯性外推得到的对应时刻对应波束方向的计算结果进行比对，残差满足检测阈值时，则认为波束测量结果有效；根据有效的残差进行闭环滤波修正惯性外推的位置和速度数据，得到前台导航值；

步骤二、进行后台导航，得到导航结果，即位置和速度估计值；

步骤三、将后台导航给出的导航结果与前台导航给出的导航结果相比对，如果两者之差超过预设阈值连续M个周期，则用后台导航得到的导航结果重置前台的导航结果，并将重置后的前台导航结果输出，M为预设周期数；否则，直接输出前台导航结果；

所述进行后台导航，具体为以下步骤：

(1)获取当前IMU数据，进行惯性导航，其中，速度用表示，位置用表示；

(2)将测距、测速的测量数据与步骤(1)得到的惯性导航结果进行比对，得到残差并累积一个时间窗后，对残差数据进行数据筛选和滤波；

(3)根据步骤(2)得到的滤波结果对惯性导航结果进行开环修正，得到后台组合导航值。

2.根据权利要求1所述的一种前后台并行解算容错组合导航方法，其特征在于：所述步骤(2)将测距、测速的测量数据与惯性导航结果进行比对，得到残差并累积一个时间窗后，对残差数据进行数据筛选和滤波，具体为：

(2.1)将当前时刻获得的数据存入第一数据缓存区buffer1内，并分别对测距数据和测速数据进行第一层筛选；单元数量为n_{filter_max}；所述存入buffer1的数据包括测距敏感器测量值与惯导的残差、对应测量时刻以及数据的有效标志，还包括测速敏感器三个波束的测量残差、对应测量时刻以及有效标志；测距敏感器包含一个波束，测速敏感器包含三个波束；

(2.2)利用上一个窗口缓存到第二数据缓存区buffer2内新息的统计结果对buffer2进行第二层筛选，buffer2单元数量为n_{filter_max}，buffer2中的数据为buffer1中存满后整体转移过来的数据，即当前buffer1中存储的为第i个窗口内的数据，buffer2内存储的是第i-1个窗口内的数据；根据对之前处理的第i-2个窗口内数据的统计结果，确定一个测量残差的方差检测阈值，如果buffer2内某一个单元的残差超过了该阈值，则将该单元对应的有效标志位置为无效false；

(2.3)对buffer2内通过第二层筛选后的数据进行打分；

(2.4)在当前时刻t_k，k＝(i-1)·n_{filter_max}+j，满足j＝n_filter-2时，根据分值，对buffer2内数据进行第三层筛选；n_filter是窗口内滤波处理开始的时间片编号；

遍历m＝1,…,n_{filter_max}，若到目前为止编号为m的数据单元仍判定有效，但该单元分值小于0，则将该单元的有效性改为无效；即

对测距，若buffer2[m].Flag_h＝true且S_h[m]0，则buffer2[m].Flag_h＝false；

对测速，若buffer2[m].Flag_v＝true且S_v[m]0，则buffer2[m].Flag_v＝false；

(2.5)在当前时刻t_k，k＝(i-1)·n_{filter_max}+j，满足j＝n_filter-1时，求出buffer2内的剩余有效数据均值，做第四层筛选，并对滤波器进行初始化；

(2.6)当前时刻为t_k，有k＝(i-1)·n_{filter_max}+j，当n_filter≤j≤n_{filter_max}-1时，对buffer2内的有效数据进行滤波；

(2.7)当前时刻为t_k，有k＝(i-1)·n_{filter_max}+j，当j＝n_{filter_max}时，即在窗口的最后一个周期，进行后台滤波的收敛判断，并进行相关参数和数据由buffer1向buffer2的转移。