水下航行器的导航定位方法及系统与流程

本发明涉及导航定位，具体地，涉及水下航行器的导航定位方法及系统，更为具体地，涉及基于水面水下多传感器组合的水下航行器导航定位方法。

背景技术：

1、水下导航定位技术在海洋探测与水下军事应用中占有重要地位，直接反应了水下航行器执行任务的精准性与安全性。水下环境具有动态复杂、未知多变等特点。随着海洋开发的不断深入与海洋领域军事对抗技术的不断升级发展，水下导航定位技术也在不断发展进步，惯性导航、声学导航及其组合导航等技术得到不断发展与应用。

2、惯性导航系统sins可实现高速率的信息更新，导航信息时延低，系统稳定不易受干扰，但其误差随时间与距离的增长不断累加，仅适用于短时间的定位，用于长时间定位时往往需要其他传感器进行累计误差修正。多普勒计程仪dvl利用声学信息进行速度测量，无累计误差，信息更新频率较低，可用于惯性导航系统中位移的累计误差修正。超短基线定位系统usbl可较准确的测量距离信息，但其无法准确获得方位信息且需布置基站。gps全球卫星定位系统可全天候提供高精度的位置信息，但其无法在水下使用。水下航行器在远洋执行任务时往往由母船搭载后释放入水，母船可搭载usbl基站与gps，其与水下航行器协同工作，通过将4类传感器的信息有效组合起来，从而提高水下航行器的导航定位精度与性能。

技术实现思路

1、针对现有技术中的缺陷，本发明的目的是提供一种水下航行器的导航定位方法及系统。

2、根据本发明提供的一种水下航行器的导航定位方法，包括：

3、步骤s1：水下航行器放置于水中静置一段时间，取该段时间内sins输出的姿态角均值，并作为航行器的初始姿态角；

4、步骤s2：水下航行器开始航行，利用sins输出的姿态角和加速度信息进行卡尔曼滤波器的时间更新，获得水下航行器的位置与速度；

5、步骤s3：对dvl测得的速度信息进行刻度误差与杆臂误差消除处理，得到sins中心处的速度测量值；

6、步骤s4：将sins中心处的速度测量值作为卡尔曼滤波器的观测值，结合时间更新得到的状态量，进行卡尔曼滤波器的测量更新，得到水下航行器的位置与速度初始估计；

7、步骤s5：将母船gps获得的绝对位置信息消除杆臂误差，usbl测得的距离信息消除杆臂误差，结合其与水下航行器的位置与速度初始估计关系构建误差模型，对初始估计进行修正，从而获得水下航行器的最优位置估计；

8、步骤s6：利用获得的水下航行器的最优位置估计计算得到水下航行器的姿态、速度信息，最终输出水下航行器的位置、速度与姿态角信息。

9、优选地，所述步骤s1采用：所述sins配置于水下航行器的重心处，选取sins中心的初始位置为坐标原点，以东向为x轴，北向为y轴，天向为z轴建立导航坐标系；以载体轴向前向为x轴，载体左向为y轴，载体向上为z轴建立载体坐标系；记在静止条件下母船gps测得的位置均值作为初始位置，为sins测得的姿态角均值作为初始姿态角，为φ0(φ0,θ0,ψ0)，由姿态角φ(φ,θ,ψ)得到旋转矩阵：

10、

11、设母船gps中心与sins中心距离为lg，则gps位置均值消除杆臂误差得到sins中心的初始位置:

12、x0＝xg0-r(φ,θ,ψ)lg  ⑵

13、则水下航行器的初始位置坐标x0(x0,y0)，初始速度为v0(0,0,0)，初始姿态角为φ0(φ0,θ0,ψ0)。

14、优选地，所述步骤s2采用：

15、取卡尔曼滤波器的状态变量为：

16、x＝[x y vx vy vz]t

17、其中，x,y为水下航行器导航坐标系下x轴方向和y轴方向的位置坐标，vx,vy,vz为水下航行器载体坐标系下三轴方向的速度；

18、根据水下航行器的运动方程，得到卡尔曼滤波器的状态转移方程：

19、

20、其中，

21、

22、其中，w是过程噪声，服从正态分布w～(0,q)；θ表示偏航角，ψ表示俯仰角，φ表示滚动角，δt表示两次卡尔曼滤波间的时间间隔；

23、将卡尔曼滤波器状态方程离散化：

24、xk＝ωk-1xk-1+wk-1

25、其中，

26、

27、依据状态转移方程进行卡尔曼滤波时间更新，而后对误差协方差矩阵进行更新计算：

28、

29、其中，q表示过程噪声的方差，pk-1|k-1表示上一时刻卡尔曼滤波得到的状态变量的方差矩阵，pk|k-1表示预测更新得到的当前时刻的状态变量方差矩阵，上标t表示矩阵转置。

30、优选地，所述步骤s3采用：

31、dvl测得的速度信息为vd＝[vx,vy,vz]，在当前水文条件当前设备情况下dvl的刻度因子为k，则进行刻度因素误差消除后的速度值为kvd；设dvl测量坐标中心与sins坐标中心的距离为ld，sins输出的角速度为ωs，则杆臂误差为ωs×ld；则由dvl测得的速度信息kvd消除杆臂误差转换至sins中心的速度测量值为：

32、

33、优选地，所述步骤s4采用：

34、以速度为量测量得到卡尔曼滤波器的量测方程为：

35、

36、其中，z为量测量，h含义如下：

37、

38、其中，u是测量噪声，服从正态分布u～(0,r)；

39、将上述卡尔曼滤波器量测方程离散化得：

40、zk＝hk-1xk-1+uk-1

41、k时刻的量测量则测量更新依据如下步骤进行：

42、计算卡尔曼滤波增益：

43、kk＝pk|k-1ht(hpk|k-1ht+r))  (14)

44、其中，r表示测量噪声的方差矩阵；

45、更新卡尔曼滤波最优估计：

46、

47、其中，表示k时刻的速度量测量；

48、更新误差协方差矩阵：

49、pk|k＝(i-kkh)pk|k-1  (16)

50、其中，i表示单位对角矩阵。

51、优选地，所述步骤s5采用：母船与水下航行器通过脐带缆相连，进行实时通信；母船上配置有gps设备，设当前时刻gps获得的位置信息为xg，利用相邻时刻的gps信息得到母船相对初始航向的航向角变化为设gps中心与布置于母船上的usbl基站中心间的距离为lg，则usbl基站的绝对位置为：

52、xu＝xg-r(0,θg,0)lg

53、设usbl移动站中心与水下航行器sins中心间距离为lu，则移动站的初始估计位置为：

54、xu＝xk-r(φ,θ,ψ)lu

55、usbl基站可获得其相对于usbl移动站的距离lu与方位信息；利用已求得水下航行器位置、姿态角与速度的初始估计，usbl基站位置以及usbl测得的距离，构建如下速度误差模型：

56、以usbl基站中心为圆心，usbl测得的距离为半径，v1为usbl移动站中心指向基站中心的向量，长度为两者距离，v2为初始估计的水下航行器速度向量；估计得到的usbl基站与移动站距离与usbl测得的实际距离lu存在两种关系：(1)(2)已知量lu、v1与v2构成三角几何关系，将水下航行器初始估计位置、母船gps获得的绝对位置及usbl测得距离信息抽象为几何关系；其中，a表示以船gps获得的绝对位置为中心、usbl测得的距离为半径的圆上某一点与水下航行器初始估计位置的相对距离，所述圆上某一点为过水下航行器初始估计位置的以v2为方向向量的直线与圆上的交点，取距离水下航行器初始估计位置最近的点；b表示usbl测得的距离，即上述圆上某一点与船gps获得的绝对位置间的距离；c表示水下航行器初始估计位置与船gps获得的绝对位置间的距离；β＝acos[(v1·v2)/(|v1||v2|)]；

57、设速度误差为δv，所经历的时间为δt，则a＝δvδt，b、c及β已知，求得a的大小为：

58、

59、根据计算结果对初始估计的位置及速度进行补偿从而得到水下航行器的最优估计。

60、优选地，所述步骤s6采用：

61、直接将水下航行器在航行过程中输出的俯仰角ψ和滚动角φ作为真值；偏航角θ利用水下航行器前后两次的位置计算：

62、

63、根据本发明提供的一种水下航行器的导航定位系统，包括：

64、模块m1：水下航行器放置于水中静置一段时间，取该段时间内sins输出的姿态角均值，并作为航行器的初始姿态角；

65、模块m2：水下航行器开始航行，利用sins输出的姿态角和加速度信息进行卡尔曼滤波器的时间更新，获得水下航行器的位置与速度；

66、模块m3：对dvl测得的速度信息进行刻度误差与杆臂误差消除处理，得到sins中心处的速度测量值；

67、模块m4：将sins中心处的速度测量值作为卡尔曼滤波器的观测值，结合时间更新得到的状态量，进行卡尔曼滤波器的测量更新，得到水下航行器的位置与速度初始估计；

68、模块m5：将母船gps获得的绝对位置信息消除杆臂误差，usbl测得的距离信息消除杆臂误差，结合其与水下航行器的位置与速度初始估计关系构建误差模型，对初始估计进行修正，从而获得水下航行器的最优位置估计；

69、模块m6：利用获得的水下航行器的最优位置估计计算得到水下航行器的姿态、速度信息，最终输出水下航行器的位置、速度与姿态角信息。

70、优选地，所述模块m1采用：所述sins配置于水下航行器的重心处，选取sins中心的初始位置为坐标原点，以东向为x轴，北向为y轴，天向为z轴建立导航坐标系；以载体轴向前向为x轴，载体左向为y轴，载体向上为z轴建立载体坐标系；记在静止条件下母船gps测得的位置均值作为初始位置，为sins测得的姿态角均值作为初始姿态角，为φ0(φ0,θ0,ψ0)，由姿态角φ(φ,θ,ψ)得到旋转矩阵：

71、

72、设母船gps中心与sins中心距离为lg，则gps位置均值消除杆臂误差得到sins中心的初始位置:

73、x0＝xg0-r(φ,θ,ψ)lg  ⑵

74、则水下航行器的初始位置坐标x0(x0,y0)，初始速度为v0(0,0,0)，初始姿态角为φ0(φ0,θ0,ψ0)；

75、所述模块m2采用：

76、取卡尔曼滤波器的状态变量为：

77、x＝[x y vx vy vz]t

78、其中，x,y为水下航行器导航坐标系下x轴方向和y轴方向的位置坐标，vx,vy,vz为水下航行器载体坐标系下三轴方向的速度；

79、根据水下航行器的运动方程，得到卡尔曼滤波器的状态转移方程：

80、

81、其中，

82、

83、其中，w是过程噪声，服从正态分布w～(0,q)；θ表示偏航角，ψ表示俯仰角，φ表示滚动角，δt表示两次卡尔曼滤波间的时间间隔；

84、将卡尔曼滤波器状态方程离散化：

85、xk＝ωk-1xk-1+wk-1

86、其中，

87、

88、依据状态转移方程进行卡尔曼滤波时间更新，而后对误差协方差矩阵更新计算：

89、

90、其中，q表示过程噪声的方差，pk-1|k-1表示上一时刻卡尔曼滤波得到的状态变量的方差矩阵，pk|k-1表示预测更新得到的当前时刻的状态变量方差矩阵，上标t表示矩阵转置；

91、所述模块m3采用：

92、dvl测得的速度信息为vd＝[vx,vy,vz]，在当前水文条件当前设备情况下dvl的刻度因子为k，则进行刻度因素误差消除后的速度值为kvd；设dvl测量坐标中心与sins坐标中心的距离为ld，sins输出的角速度为ωs，则杆臂误差为ωs×ld；则由dvl测得的速度信息kvd消除杆臂误差转换至sins中心的速度测量值为：

93、

94、优选地，所述模块m4采用：

95、以速度为量测量得到卡尔曼滤波器的量测方程为：

96、

97、其中，z为量测量，h含义如下：

98、

99、其中，u是测量噪声，服从正态分布u～(0,r)；

100、将上述卡尔曼滤波器量测方程离散化得：

101、zk＝hk-1xk-1+uk-1

102、k时刻的量测量则测量更新依据如下步骤进行：

103、计算卡尔曼滤波增益：

104、kk＝pk|k-1ht(hpk|k-1ht+r))  (14)

105、其中，r表示测量噪声的方差矩阵；

106、更新卡尔曼滤波最优估计：

107、

108、其中，表示k时刻的速度量测量；

109、更新误差协方差矩阵：

110、pk|k＝(i-kkh)pk|k-1  (16)

111、其中，i表示单位对角矩阵；

112、所述模块m5采用：母船与水下航行器通过脐带缆相连，进行实时通信；母船上配置有gps设备，设当前时刻gps获得的位置信息为xg，利用相邻时刻的gps信息得到母船相对初始航向的航向角变化为设gps中心与布置于母船上的usbl基站中心间的距离为lg，则usbl基站的绝对位置为：

113、xu＝xg-r(0,θg,0)lg

114、设usbl移动站中心与水下航行器sins中心间距离为lu，则移动站的初始估计位置为：

115、xu＝xk-r(φ,θ,ψ)lu

116、usbl基站可获得其相对于usbl移动站的距离lu与方位信息；利用已求得水下航行器位置、姿态角与速度的初始估计，usbl基站位置以及usbl测得的距离，构建如下速度误差模型：

117、以usbl基站中心为圆心，usbl测得的距离为半径，v1为usbl移动站中心指向基站中心的向量，长度为两者距离，v2为初始估计的水下航行器速度向量；估计得到的usbl基站与移动站距离与usbl测得的实际距离lu存在两种关系：(1)(2)已知量lu、v1与v2构成三角几何关系，将水下航行器初始估计位置、母船gps获得的绝对位置及usbl测得距离信息抽象为几何关系；其中，a表示以船gps获得的绝对位置为中心、usbl测得的距离为半径的圆上某一点与水下航行器初始估计位置的相对距离，所述圆上某一点为过水下航行器初始估计位置的以v2为方向向量的直线与圆上的交点，取距离水下航行器初始估计位置最近的点；b表示usbl测得的距离，圆上某一点与船gps获得的绝对位置间的距离；c表示水下航行器初始估计位置与船gps获得的绝对位置间的距离；β＝acos[(v1·v2)/(|v1||v2)]；

118、设速度误差为δv，所经历的时间为δt，则a＝δvδt，b、c及β已知，求得a的大小为：

119、

120、计算结果对初始估计的位置及速度进行补偿从而得到水下航行器的最优估计；

121、所述模块m6采用：

122、直接将水下航行器在航行过程中输出的俯仰角ψ和滚动角φ作为真值；偏航角θ利用水下航行器前后两次的位置计算：

123、

124、与现有技术相比，本发明具有如下的有益效果：

125、1、本发明采用sins与dvl进行组合导航基础上，利用gps与usbl信息建立几何误差模型，利用误差模型修正导航结果，在提高导航精度的同时有效降低了导航算法的复杂程度。

126、2、新方法考虑了dvl的刻度误差与杆臂误差，对误差进行补偿，有效提高了水下航行器的导航精度。