一种面向异步多源测量融合定位的弹性滑动窗口因子图优化方法

本发明公开一种面向异步多源测量融合定位的弹性滑动窗口因子图优化方法，属于多源测量融合定位与因子图优化。

背景技术：

1、在实际多源异步测量融合定位中，不同测量源的频率通常相差较大。例如，视觉惯性里程计可按较高频率连续输出相对位姿测量，而gnss、磁力计、气压计等全局测量的输出频率较低，且可能在遮挡、干扰或设备限制条件下间歇缺失。若仍采用传统固定滑动窗口因子图优化方法，则每当一个新状态加入滑动窗口时，系统通常都会触发一次全窗口非线性优化，并在优化后进行边缘化以维持固定窗口大小。

2、该传统处理方式在多源异步测量条件下会造成明显的计算资源浪费。当状态按照高频测量源实例化，而低频测量尚未到达时，最新加入的状态可能仅受到一个测量因子约束。针对该单约束情形，本发明通过优化特性分析可知，窗口内较旧状态的最优估计保持为上一轮的最优估计不变，最新状态可由对应的单测量约束方程单独求解。因此，此时继续对整个滑动窗口执行全窗口非线性优化并非必要，其中涉及的全窗口线性化、迭代求解、边缘化和先验因子重构等操作均可避免，从而减少多源异步测量融合定位中的计算负担。

3、进一步地，在连续多个最新状态均仅受到单一测量因子约束的情况下，根据本发明对单约束状态优化特性的分析结论：最新状态可在已有窗口状态保持为上一轮最优估计的条件下由对应测量约束单独求解，因此无需触发全窗口优化。为减少非必要的窗口重构，并保证再次触发全窗口优化时滑动窗口内仍保留多类型测量约束，本发明在单约束状态更新期间不执行边缘化，并保持滑动窗口左边界不变，使新加入的单约束状态从窗口右侧依次填充；当最新状态重新受到两个或两个以上测量因子约束时，再对当前弹性窗口执行全窗口优化和批量边缘化。

技术实现思路

1、本发明的目的在于提供一种面向异步多源测量融合定位的弹性滑动窗口因子图优化方法，以解决现有技术中，固定滑动窗口因子图优化方法在多源异步测量融合定位中存在全窗口优化冗余、计算资源浪费的问题。

2、一种面向异步多源测量融合定位的弹性滑动窗口因子图优化方法，包括：

3、s1、获取不同测量源的异步观测信息，用预设的高频局部测量源的输出频率生成状态节点，将不同测量源的异步观测信息关联到对应状态节点，构建弹性滑动窗口内的状态集合；将与状态节点相关联的异步观测信息构建为因子图中的测量因子，

4、s2、统计最新状态直接关联的有效测量因子数量，依据最新状态有效测量因子数量划分单约束状态和多约束状态，若为单约束状态执行s3，若为多约束状态执行s4；

5、s3、对单约束状态的最新状态，根据有效测量因子构建最新状态优化目标函数并求解，得到最新状态的最优估计，在单约束状态求解过程中，保持弹性滑动窗口的其余状态不变，将最新状态保留在弹性滑动窗口右端，弹性滑动窗口右边界随最新状态加入而扩展；

6、s4、对多约束状态的最新状态，根据当前弹性滑动窗口内全部有效测量因子的集合构建全窗口优化目标函数并求解，得到当前窗口所有状态的最优估计；

7、s5、设置基础滑动窗口，若当前弹性滑动窗口大于基础滑动窗口，对超出的最旧状态进行批量边缘化；

8、s6、当步骤s3完成后，输出最新状态的最优估计并保持滑窗内其余状态的估计值不变；

9、当步骤s4和步骤s5完成后，输出最新状态的最优估计，并同步更新当前弹性滑动窗口内其余状态的最优估计；

10、得到最优估计后，返回步骤s1继续执行。

11、s1包括，s1.1、高频局部测量源为初始化时指定的输出频率最高的测量源；

12、设最新状态节点的状态为，当前弹性滑动窗口内的状态集合为：

13、；

14、式中，为当前弹性滑动窗口的左边界状态的时间步索引，为右边界状态的时间步索引；

15、每个状态至少包括移动平台的位置和姿态：

16、；

17、式中，为移动平台在世界坐标系下的位置，为移动平台从机体系到世界坐标系的姿态。

18、s1包括，s1.2、每个测量因子对应一个残差项：

19、；

20、式中，为与第个时间步关联的第个测量信息，为该测量信息对应的残差函数。

21、s2包括，统计直接关联的有效测量因子数量，若多个测量因子来源于同一测量源且具有相同测量类型的观测信息，合并为一类有效测量因子；若测量因子的时间戳超出与状态节点的最大允许时间差，则判定为观测质量异常，不计入有效测量因子数量；

22、当时，判定为单约束状态；当时，判定为多约束状态。

23、s3包括，设与直接关联的唯一有效测量因子为，对应测量为，构建最新状态优化目标函数：

24、；

25、式中，为最新状态的最优估计，为当前窗口中除最新状态外的已有状态估计。

26、s4包括，全窗口优化目标函数为：

27、；

28、式中，为全窗口优化目标，为由历史边缘化信息形成的先验残差，为当前弹性滑动窗口内全部有效测量因子的集合，为对应测量因子的信息矩阵，为马氏距离；

29、采用非线性最小二乘法对全窗口优化目标求解，得到最优状态估计。

30、s5包括，s5.1、设基础滑动窗口大小为，当前弹性滑动窗口中的状态数量为，；

31、当时，采用舒尔补或等效先验构建方式对最旧的个状态进行批量边缘化；

32、当时，不执行边缘化，保持当前窗口不变。

33、s5包括，s5.2、在因子图优化中，将被边缘化状态对应的变量块消去，约束信息通过舒尔补传递到保留状态的先验因子中；

34、完成批量边缘化后，更新弹性滑动窗口的左边界，保留最近的个状态及对应的测量因子，将新的先验因子作为下一轮弹性滑动窗口优化的历史约束，窗口大小恢复至基础窗口大小。

35、相对比现有技术，本发明具有以下有益效果：本发明根据最新状态受到的有效测量约束数量判断是否需要全窗口优化。当最新状态仅受到单一有效测量因子约束时，仅求解最新状态而不重复优化整个窗口，从而避免传统固定滑动窗口方法在高频单约束更新下产生大量冗余计算；在单约束条件下仍可快速计算并输出最新状态，不降低定位结果输出频率；同时避免全窗口非线性优化、矩阵求解和频繁边缘化操作，显著降低单次状态更新的时间消耗和处理器占用率；在连续单约束更新期间暂缓边缘化，并在多源约束重新形成后执行批量边缘化，相较于每次状态加入后均执行边缘化的传统方法，可减少先验因子重构次数，降低窗口维护开销，保持滑窗内测量的多样性；不要求各测量源同步输出，也不要求每个状态均具有多个测量因子约束，而是根据实际因子连接关系动态调整优化策略，适用于测量频率不同、低频测量间歇缺失、局部测量高频连续输出等实际融合定位场景；将大量单约束时刻的全窗口非线性优化替换为最新状态单独求解，显著降低计算峰值和平均计算负担，因此更适合部署于移动机器人、智能车辆、无人平台、地下空间巡检设备和其他嵌入式定位终端；核心判定依据为最新状态的有效测量约束数量，不依赖某一种特定测量源或特定残差形式。因此，gnss位置因子、视觉惯性相对位姿因子、磁力计方向因子、气压计高度因子、轮速因子、地图匹配因子以及其他可因子化的测量均可接入本发明方法。