一种水下航行器未知输入与位置估计方法及系统

本技术属于水下航行器协同定位，具体涉及一种水下航行器未知输入与位置估计方法及系统。

背景技术：

1、受全球定位系统信号在水下的快速衰减以及水下恶劣的声学信道条件的影响，自主水下航行器自身位置估计一直是一个艰巨的任务。与传统的惯性系统和声基线系统相比，协同定位系统具有精度高、航程远、成本低、鲁棒性好的优点，可以大大增强多水下航行器系统的协同作业能力。然而，信息处理与传输所消耗的时间导致协同定位所需观测信息存在时变时间延时，此外，协同定位系统传感器如罗经、多普勒计程仪等受自身结构、工作原理及水下环境因素如海洋生物障碍、海底强吸声地质、深海沟等影响可能出现故障或失效。以上恶劣条件的出现均可能导致定位精度降低甚至估计结果无效。

2、罗经和多普勒计程仪故障都会导致无法获得有效的控制输入，从而导致先验信息的不准确，这必然会影响定位精度。目前，现有算法可以在实时外部观测的辅助下完成对控制输入的估计，如经典的增广卡尔曼滤波器、经过状态解耦优化的滤波器等。然而，这些方法依赖于相对准确的观测信息，而水下通信延时的存在导致观测数据存在延时偏差，此偏差的存在使得非线性控制输入估计的准确性无法得到保证，进而对位置估计产生不利影响。

技术实现思路

1、本技术旨在解决现有技术的不足，提出一种水下航行器未知输入与位置估计方法及系统，能够有效缓解观测值中的延时偏差对未知输入估计及位置估计的影响，提高水下航行器在非理想作业环境中的定位精度。

2、为实现上述目的，本技术提供了如下方案：

3、一种水下航行器未知输入与位置估计方法，包括以下步骤：

4、获取水下航行器的航行数据，并基于所述航行数据构建系统状态空间模型；

5、对所述系统状态空间模型中的量测方程进行第一次优化，得到优化后量测方程，并基于所述优化后量测方程构建延时递推框架；

6、基于所述延时递推框架，搭建未知非线性控制输入和延时到达观测信息之间的递推关联；

7、基于所述递推关联得到未知非线性控制输入估计值；

8、基于所述未知非线性控制输入估计值，得到位置状态后验估计表达式和第一估计误差协方差表达式；

9、对所述后验估计表达式进行第二次优化，得到最终位置状态表达式和第二估计误差协方差表达式。

10、优选的，所述航行数据包括：位置数据、速度数据和航向数据。

11、优选的，所述系统状态空间模型的构建方法包括：

12、基于所述位置数据、所述速度数据和所述航向数据构建离散时间模型：

13、

14、

15、

16、其中，和分别为前向和右向速度，为航向，χm，[k]＝[am，[k]，bm，[k]]为领航者在k时刻的位置坐标，χ[k]＝[a[k]，b[k]]t为跟随者在k时刻的位置坐标，γ[k]为领航者与跟随者之间的相对距离，(·)t为转置操作，δt为采样周期，q[k-l]＝[qx，[k-l]，qy，[k-l]]t为过程噪声，r[k]为量测噪声；

17、基于所述离散时间模型构建所述系统状态空间模型：

18、

19、其中，状态转移矩阵φ[k-1]为二维单位矩阵，γ[k-1]＝diag(δt，δt)和u[k-1]＝[ux，uy]t分别为控制输入，且有和为量测方程。

20、优选的，所述优化后量测方程为：

21、

22、其中，τ为延时采样时刻，c[k-τ]＝(pxy，[k-τ，k-τ-1])t(pyy，[k-τ，k-τ-1])-1为观测矩阵，γ[k-τ]为k-τ时刻相对距离量测值，为k-τ时刻相对距离预测值，为k-τ时刻跟随者位置的一步预测估计值；

23、η[k-τ]＝r[k-τ]+l[k-τ]包含量测噪声与线性化误差两部分，l[k-τ]为k-τ时刻的统计线性化误差，线性化误差的协方差表示为：

24、θ[k-τ]＝e(l[k-τ]l[k-τ]t)

25、＝pyy，[k-τ，k-τ-1]-(pxy，[k-τ，k-τ-1])t(pxx，[k-τ，k-τ-1])-1pxy，[k-τ，k-τ-1]-r[k-τ]

26、其中，pyy，[k-τ，k-τ-1]和pxy，[k-τ，k-τ-1]分别为自协方差与互协方差矩阵，r[k-τ]＝e(r[k-τ]r[k-τ]t)为k-τ时刻量测噪声协方差矩阵。

27、优选的，所述延时递推框架为：

28、

29、其中，

30、为延时转化为的偏差：

31、

32、υ[k]中的延时带来的附加项

33、

34、优选的，所述递推关联包括：

35、

36、其中，

37、进一步得到：

38、

39、其中，pxx，[k，k-1]为状态的一步预测误差协方差，

40、κ[k]＝e(η[k-τ]η[k-τ]t)＝r[k-τ]+θ[k-τ]，

41、

42、

43、l[k]为t[k]经过cholesky分解后的下三角矩阵。

44、优选的，所述未知非线性控制输入估计值的计算方法包括：

45、基于所述递推关联构建递推关联的线性回归表达式：

46、

47、基于所述线性回归表达式计算所述未知非线性控制输入估计值：

48、

49、其中，

50、优选的，所述后验估计表达式为：

51、

52、其中，k[k]为增益矩阵；

53、所述第一估计误差协方差表达式为：

54、

55、优选的，所述第二次优化的方法包括：舍去所述后验估计表达式中与未知输入相关项，得到所述最终位置状态表达式和所述第二估计误差协方差表达式；

56、所述最终位置状态表达式为：

57、

58、所述第二估计误差协方差表达式为：

59、

60、其中，

61、本技术还提供了一种水下航行器未知输入与位置估计系统，包括：模型构建模块、框架构建模块、关联构建模块、估计模块、后验状态模块和位置估计模块；

62、所述模型构建模块用于获取水下航行器的航行数据，并基于所述航行数据构建系统状态空间模型；

63、所述框架构建模块用于对所述系统状态空间模型中的量测方程进行第一次优化，得到优化后量测方程，并基于所述优化后量测方程构建延时递推框架；

64、所述关联构建模块用于基于所述延时递推框架，搭建未知非线性控制输入和延时到达观测信息之间的递推关联；

65、所述估计模块用于基于所述递推关联得到未知非线性控制输入估计值；

66、所述后验状态模块用于基于所述未知非线性控制输入估计值，得到位置状态后验估计表达式和第一估计误差协方差表达式；

67、所述位置估计模块用于对所述后验估计表达式进行第二次优化，得到最终位置状态表达式和第二估计误差协方差表达式。

68、与现有技术相比，本技术的有益效果为：

69、(1)本技术将航向角、前向速度和右向速度建模为非线性单元，降低了估计模型的复杂性；

70、(2)本技术通过重构观测矩阵和测量噪声方差矩阵对测量方程进行修正，建立了基于线性回归的延迟框架，从模型角度削弱了非实时观测对未知输入估计的影响；

71、(3)本技术构建了延迟到达量测与非线性控制输入估计之间的递推关联，并进一步推导了不受非线性输入干扰的后验估计表达形式，有效削弱未知输入对位置估计的影响，提高了系统的鲁棒性。