基于自适应窗口因子图的载体状态鲁棒估计方法和装置

本发明涉及导航定位，尤其涉及一种基于自适应窗口因子图的载体状态鲁棒估计方法和装置。

背景技术：

1、无人车辆因其低风险、高效率、灵活性及环境适应性，被广泛应用于科研勘探、军事侦察和灾害救援等领域。精确定位是无人车辆高效安全抵达目的地的前提条件。然而，无人车辆面临极其复杂的任务环境。具体而言，云雾和沙尘暴等通过破坏光照条件与降低能见度，从而削弱光学传感器的观测能力。同时，沙质或松软地表易致车轮滑移，进而降低里程计的观测精度。高密度城市区域存在信号衰减和信号多径效应，进而加剧卫星导航系统的定位误差。这些因素共同导致感知的不确定性。进一步，无人车辆的平台约束(如尺寸、重量、功耗)显著限制了其导航传感器的数量与选型。因此观测数据同时呈现稀疏性与高变异性。此外，无人车辆有限的计算资源难以满足复杂估计算法的实时性要求，其引发的计算延迟会危及车辆在不确定环境中的安全性。因此在资源受限条件下，轻量级鲁棒状态估计方法对保障车辆运行的安全性、准确性及实时性至关重要。

2、在实现本发明过程中，申请人发现现有技术中至少存在如下问题：

3、当前基于滑动窗口的因子图优化方法在面对异常观测时表现出鲁棒性不足的问题。

技术实现思路

1、本发明实施例提供一种基于自适应窗口因子图的载体状态鲁棒估计方法和装置，以至少解决当前基于滑动窗口的因子图优化方法在面对异常观测时鲁棒性不足的问题。

2、为达上述目的，一方面，本发明实施例提供一种基于自适应窗口因子图的载体状态鲁棒估计方法，包括：

3、获取最近历史时间范围内第一传感器采集的多种状态参数的观测数据和至少一种第二传感器各自采集的至少一种状态参数的观测数据；

4、将同一时间戳和同一种状态参数对应的所述第一传感器的观测数据和所有第二传感器的观测数据合并为所述时间戳下所述状态参数的观测数据集，得到最近历史时间范围内每个时间戳下每种状态参数的观测数据集；

5、对每个时间戳下每种状态参数的观测数据集进行有效性判断，将每个观测数据集标记为有效数据集或异常数据集；

6、根据最近历史时间范围内所有观测数据集的异常判定结果，建立考虑异常数据分布的窗口边界优化模型，以最小化总预测误差为目标，解算所述窗口边界优化模型，将所述最近历史时间范围划分为至少一个子滑动窗口；

7、针对每个子滑动窗口，采用基于高斯假设的传统因子图优化方法处理所述子滑动窗口内所有时间戳下每种状态参数的观测数据集，得到所述子滑动窗口内所有时间戳下每种状态参数的估计结果。

8、另一方面，本发明实施例提供一种基于自适应窗口因子图的载体状态鲁棒估计装置，包括：

9、观测数据采集单元，用于获取最近历史时间范围内第一传感器采集的多种状态参数的观测数据和至少一种第二传感器各自采集的至少一种状态参数的观测数据；

10、数据集归并单元，用于将同一时间戳和同一种状态参数对应的所述第一传感器的观测数据和所有第二传感器的观测数据合并为所述时间戳下所述状态参数的观测数据集，得到最近历史时间范围内每个时间戳下每种状态参数的观测数据集；

11、数据集异常性标记单元，用于对每个时间戳下每种状态参数的观测数据集进行有效性判断，将每个观测数据集标记为有效数据集或异常数据集；

12、窗口划分单元，用于根据最近历史时间范围内所有观测数据集的异常判定结果，建立考虑异常数据分布的窗口边界优化模型，以最小化总预测误差为目标，解算所述窗口边界优化模型，将所述最近历史时间范围划分为至少一个子滑动窗口；

13、状态估计单元，用于针对每个子滑动窗口，采用基于高斯假设的传统因子图优化方法处理所述子滑动窗口内所有时间戳下每种状态参数的观测数据集，得到所述子滑动窗口内所有时间戳下每种状态参数的估计结果。

14、上述技术方案具有如下有益效果：通过将多种传感器采集的观测数据按时间戳和状态参数进行整理，生成观测数据集；基于多通道异常检测方法将观测数据集标记为有效数据集或异常数据集，该方法将检测对象从离散观测转化为整体观测数据集，简化了异常识别流程；基于数据集异常检测结果，以最小化总预测误差为目标，以子滑动窗口长度为优化变量，建立了基于异常数据分布的窗口边界优化模型，该模型综合考虑了异常幅度、累积效应及多异常观测的时序分布等因素，相较于固定滑动窗口因子图优化方法(fswfgo，fixed sliding window factor graph optimization method)和基于方差的自适应滑动窗口因子图优化方法(vaswfgo，variance-based adaptive sliding window factorgraph optimization method)，本发明考虑异常数据分布的自适应滑动窗口因子图优化方法(adaswfgo，anomaly distribution-based adaptive sliding window factor graphoptimization method)可实现更高的估计精度。

1.一种基于自适应窗口因子图的载体状态鲁棒估计方法，其特征在于，包括：

2.如权利要求1所述的基于自适应窗口因子图的载体状态鲁棒估计方法，其特征在于，对每个时间戳下每种状态参数的观测数据集进行有效性判断，将每个观测数据集标记为有效数据集或异常数据集，包括：

3.如权利要求2所述的基于自适应窗口因子图的载体状态鲁棒估计方法，其特征在于，所述基于先验知识的阈值法，包括：

4.如权利要求2所述的基于自适应窗口因子图的载体状态鲁棒估计方法，其特征在于，所述基于多源数据的统计法，包括：

5.如权利要求2所述的基于自适应窗口因子图的载体状态鲁棒估计方法，其特征在于，所述状态参数包括位置参数和速度参数，对应的观测数据分别为位置观测和速度观测；所述基于时间序列的增量法，包括：

6.如权利要求1所述的基于自适应窗口因子图的载体状态鲁棒估计方法，其特征在于，根据最近历史时间范围内所有观测数据集的异常判定结果，建立考虑异常数据分布的窗口边界优化模型，解算所述窗口边界优化模型，将所述最近历史时间范围划分为至少一个子滑动窗口，包括：

7.一种基于自适应窗口因子图的载体状态鲁棒估计装置，其特征在于，包括：观测数据采集单元，用于获取最近历史时间范围内第一传感器采集的多种状态参数的观测数据和至少一种第二传感器各自采集的至少一种状态参数的观测数据；

8.如权利要求7所述的基于自适应窗口因子图的载体状态鲁棒估计装置，其特征在于，数据集异常性标记单元，用于针对每个时间戳下每种状态参数的观测数据集，当基于先验知识的阈值法、基于多源数据的统计法和基于时间序列的增量法三种方法，对所述观测数据集的有效性判定结果均为有效时，所述观测数据集被标记为有效数据集，否则所述观测数据集被标记为异常数据集。

9.如权利要求7所述的基于自适应窗口因子图的载体状态鲁棒估计装置，其特征在于，窗口划分单元，包括：