一种基于因子图优化的SINS/USBL/DVL组合导航方法

本发明属于水下航行器sins/usbl/dvl组合导航技术，具体为一种基于因子图优化的sins/usbl/dvl组合导航方法。

背景技术：

1、传统的组合导航算法一直以kalman算法为基础，图优化算法由于在视觉定位中的优异表现而备受认可，正在延伸到传统组合导航中。传统基于kalman滤波的组合导航采用马尔科夫性假设，认为当前时刻的状态只与上一时刻的状态有关，并要求系统噪声和观测噪声为符合正态分布的白噪声。由于水下环境的复杂性，传统的滤波算法往往存在一定的缺陷。图优化算法通过牺牲一定的计算效率进行反复的线性化处理，实现了最优的状态估计。随着视觉slam的发展，图优化算法被广泛采用和快速演进，获得了越来越广泛的认可，这为组合导航算法研究开辟了一条新思路。

技术实现思路

1、为了解决以上问题，本发明提出了一种基于因子图优化的sins/usbl/dvl组合导航方法。首先，确定滑动窗长度，构建滑动窗内的sins因子，基于四波束速度的dvl因子和基于斜距、方位角的usbl因子，建立因子图模型；其次，利用卡方故障检测算法对除当前时刻外的滑动窗内的所有历史因子进行数据故障判别，对滑窗内的usbl和dvl的粗差进行准确甄别和剔除；最后，利用滑窗内良好的usbl和dvl历史观测数据和当前时刻的量测信息，建立全局损失函数，利用最小二乘对滑动窗内的所有状态信息进行估计。

2、为实现上述目的，本发明采取的技术方案是：

3、一种基于因子图优化的sins/usbl/dvl组合导航方法，其特征在于，具体步骤如下：

4、步骤1：接收sins加速度计和陀螺仪数据、usbl斜距和方位角数据、dvl四波束速度数据：sins采集的x、y、z轴角速度信息和加速度信息usbl的斜距和方位角dvl四波束速度信息

5、步骤2：根据步骤1收到的数据建立sins因子、usbl斜距方位角紧组合因子和dvl四波束紧组合因子；

6、步骤3：根据滑动窗内量测类型，结合卡方故障诊断算法，重构状态矩阵和量测信息矩阵。

7、作为本发明进一步改进，步骤2具体如下：

8、2.1k时刻，组合导航系统方程如下：

9、

10、其中，

11、φn＝[φx φyφz]t表示载体俯仰角、横滚角、航向角误差；δv＝[δve δvn δvn]t表示载体东向、北向、天向速度误差；δp＝[δl δλ δh]t表示载体地理经度、纬度、高度误差；表示sins的加速度零偏；[εx εy εz]t表示sins的陀螺零偏，fk-1表示状态转移矩阵，wk表示系统噪声，zk表示系统量测值，hk表示量测方程转移矩阵，vk表示量测噪声；

12、根据系统方程，构造sins因子：

13、

14、其中，表示状态的一步预测；

15、相应地，其一步预测均方误差为：

16、

17、其中，pk-1表示前一时刻的均方误差，qk-1表示系统噪声分布；

18、2.2当接收到usbl斜距和方位角信息，根据sins解算位置、已标定的应答器位置，构建usbl紧组合因子：

19、usbl接收到应答器a的量测信息为：

20、

21、利用sins位置计算得到应答器在a系下的相对位置矢量为则应答器a的相对位置矢量和方位角、斜距之间的关系表达式为：

22、

23、则：

24、

25、其中，矢量为表述方便，记则矩阵的表达式为：

26、

27、根据sins的位置计算的应答器的相对位置矢量为：

28、

29、其中，表示sins与usbl之间安装误差角对应的转移矩阵，表示sins姿态转移矩阵，由地球坐标系到导航系的坐标转换矩阵，为地球坐标系下的相对位置矢量，表示为：

30、

31、则利用sins位置及应答器a位置计算得到斜距方位角信息表述为：

32、

33、其中，[α β r]t为方位角及斜距的真值，矩阵分别为

34、其中，

35、

36、则系统量测为：

37、

38、其中，hu＝[0 0 -1]，δu＝[δα δβ δd]t；

39、则k时刻usbl紧组合因子为：

40、rusbl，k(zk，xk)＝zusbl，k-husbl，kxk；

41、2.3当接收到dvl四波束速度信息，根据sins解算速度，构建dvl紧组合因子：

42、dvl四波束模型为：

43、

44、其中，δkd表示波束通道测速刻度因子误差；bd表示波束测速零偏；wd表示波束测速噪声；

45、sins解算得到的速度在dvl系下的投影为：

46、

47、则系统量测为：

48、

49、其中，

50、则k时刻dvl紧组合因子为：

51、rdvl，k(zdvl，k，xk)＝zdvl，k-hdvk，kxk。

52、作为本发明进一步改进，步骤3具体如下：

53、3.1根据滑动窗内的sins数据，更新滑动窗内的第i个状态转移概率矩阵：

54、

55、3.2采用卡方检测，对滑动窗内的历史量测信息(z1～zk-1时刻)进行删选，计算滑动窗内第i个量测卡方检测值：

56、

57、其中，ri-1表示量测噪声矩阵。

58、若λi＜t，则认为量测正常，不包含野值。若λi≥t，则认为量测包含野值。

59、3.3根据步骤3.2的删选结果，结合当前k时刻的量测值，重构滑动窗内的状态矩阵和量测矩阵：

60、

61、

62、

63、

64、其中，表示滑动窗内除当前时刻外其余卡方检测无故障的状态预测信息，表示当前时刻的状态预测信息，p1：k-1表示滑动窗内除当前时刻外其余卡方检测无故障的状态预测概率矩阵，pk|k-1表示当前时刻的状态概率矩阵，z1：k-1表示滑动窗内除当前时刻外其余卡方检测无故障的量测信息，zk表示当前时刻的量测信息，h1：k-1表示滑动窗内除当前时刻外其余卡方检测无故障的量测转移矩阵，hk表示当前时刻的量测转移矩阵；

65、3.4根据步骤2的因子图模型，建立系统代价函数：

66、

67、其中，∑pi|i-1表示第i个状态量的一步预测均方误差，rusbl表示usbl量测噪声矩阵，rdvl表示dvl量测噪声矩阵；

68、3.5对步骤3.4代价函数进行最小二乘估计，更新滑动窗内的所有量测对应下的状态估计结果x:

69、(1)计算系统增益：

70、k＝pht(hph+r)-1

71、(2)对滑动窗内所有的状态进行估计：

72、

73、(3)更新状态转移概率矩阵：

74、pl＝p-khp

75、(4)根据(2)的状态估计结果对滑动窗内所有的导航信息进行反馈。

76、本发明提出了一种基于因子图优化的sins/usbl/dvl组合导航方法。首先，确定滑动窗长度，构建滑动窗内的sins因子，基于四波束速度的dvl因子和基于斜距、方位角的usbl因子，建立因子图模型；其次，利用卡方故障检测算法对除当前时刻外的滑动窗内的所有历史因子进行数据故障判别，对滑窗内的usbl和dvl的粗差进行准确甄别和剔除；最后，利用滑窗内良好的usbl和dvl历史观测数据和当前时刻的量测信息，建立全局损失函数，利用最小二乘对滑动窗内的所有状态信息进行估计。