一种基于发射坐标系的水下航行体导航方法与流程

1.本发明涉惯性导航领域，具体是一种基于发射坐标系的水下航行体导航方法。


背景技术：

2.在某些特定的水下航行体运动轨迹测试应用中，水下航行体初始位置无法确定，需要测试出航行体相对于发射起始点的运动轨迹和相对位置信息。实际测试中，航行体在发射起始点处于系泊状态， x、y、z三个姿态轴(x、y、z三个姿态轴符合右手定则)定义如图 1所示。在初始对准过程中，航行体处于系泊状态，x轴会发生周期性的旋转。在发射后的航行过程中，x轴会由朝天方向逐渐变成朝水平方向(角度变化超过90
°
)，y轴或者z轴中的某一个轴向会逐渐变成朝天或者朝地的方向(角度变化超过90
°
)。上述情况会导致在初始对准时无法获取准确的x轴方向初始角度信息，同时水下航行体在发射后的运动轨迹会使得姿态角发生大范围变化，而在惯性导航领域，姿态角的定义一般遵循如下的定义域：航向角俯仰角θ＝[-90
°
,90
°
]，横滚角γ＝(-180
°
,180
°
]。综上所述，要想获得航行体相对于发射起始点的运动轨迹和相对位置信息，必须解决该情况下的初始对准问题才能得到3个姿态角初始值。


技术实现要素：

[0003]
本发明的目的是提供一种基于发射坐标系的水下航行体导航方法，包括以下步骤：
[0004]
1)对系泊状态下的水下航行体进行初始对准，得到航行体姿态角初始值。
[0005]
姿态角包括航向角、横滚角和俯仰角。
[0006]
在系泊状态下，航行体初始对准中的一个水平姿态角为航向角初始值，另一个水平姿态角作为俯仰角初始值，天向轴方向姿态角作为横滚角初始值。
[0007]
获取航行体姿态角初始值的方法包括捷联惯导自主对准方法。
[0008]
2)初始对准后，置横滚角初始值为0
°
。
[0009]
3)发射水下航行体，并解算水下航行体在航行过程中的姿态角。
[0010]
解算水下航行体在航行过程中的姿态角的步骤包括：
[0011]
3.1)建立坐标系，包括水下航行体本体坐标系oxbybzb和导航系 ox
nynzn
。
[0012]
水下航行体本体坐标系oxbybzb的xb轴与航行体纵轴重合，且xb轴正方向指向头部。
[0013]
水下航行体本体坐标系oxbybzb的yb轴在航行体纵向对称面内，与xb轴垂直，正方向指向上。
[0014]
水下航行体本体坐标系oxbybzb的zb轴与xb轴、yb轴成右手坐标系。
[0015]
导航系ox
nynzn
为发射坐标系，取水下航行体所在地域的水平坐标系，以发射点为原点。
[0016]
水下航行体在初始对准时，x轴朝天，导航系ox
nynzn
的yn轴、zn轴在当地水平面内。yn
轴正方向指向水下航行体本体坐标系yb轴在当地水平的投影，xn轴在当地铅垂线上，正方向指向上。xn轴和yn轴、zn轴成右手坐标系。
[0017]
水下航行体绕x轴转动为横滚，绕y轴转动为航向，绕z轴转动为俯仰。
[0018]
水下航行体本体坐标系oxbybzb的原点定义在航行体质心，且与发射系原点重合。
[0019]
3.2)基于水下航行体本体坐标系oxbybzb和导航系ox
nynzn
的转换关系，建立水下航行体在航行过程中的姿态矩阵即：
[0020][0021][0022]
式中，ψ为航向角，θ为俯仰角，γ为横滚角。与互为转置关系。
[0023]
水下航行体本体坐标系oxbybzb和导航系ox
nynzn
的转换关系为：导航系ox
nynzn
绕y轴转动角度ψ，变换为坐标系ox1y1z1。导航系 ox1y1z1绕z轴转动角度θ，变换为坐标系ox2y2z2。坐标系ox2y2z2绕x轴转动角度γ，变换为水下航行体本体坐标系oxbybzb。
[0024]
3.3)解算姿态矩阵得到水下航行体在航行过程中的姿态角。
[0025]
本发明的技术效果是毋庸置疑的，本发明对该特定条件进行导航算法的编排设计，可以满足航行体在发射坐标系下的全姿态导航解算。本发明可以在水下航行体系泊状态下朝天轴周期性旋转以及发射后3个姿态角角度变化都可能超过90
°
的情况下，解决航行体在发射坐标系下的导航解算问题。
附图说明
[0026]
图1为系泊状态，x、y、z三个姿态轴；
[0027]
图2为航行体；
[0028]
图3为导航系到本体系的转化关系。
具体实施方式
[0029]
下面结合实施例对本发明作进一步说明，但不应该理解为本发明上述主题范围仅限于下述实施例。在不脱离本发明上述技术思想的情况下，根据本领域普通技术知识和惯用手段，做出各种替换和变更，均应包括在本发明的保护范围内。
[0030]
实施例1：
[0031]
参见图1至图3，一种基于发射坐标系的水下航行体导航方法，包括以下步骤：
[0032]
1)对系泊状态下的水下航行体进行初始对准，得到航行体姿态角初始值。
[0033]
姿态角包括航向角、横滚角和俯仰角。
[0034]
在系泊状态下，航行体初始对准中的一个水平姿态角为航向角初始值，另一个水平姿态角作为俯仰角初始值，天向轴方向姿态角作为横滚角初始值。
[0035]
获取航行体姿态角初始值的方法包括捷联惯导自主对准方法。
[0036]
2)初始对准后，置横滚角初始值为0
°
。
[0037]
3)发射水下航行体，并解算水下航行体在航行过程中的姿态角。
[0038]
解算水下航行体在航行过程中的姿态角的步骤包括：
[0039]
3.1)建立坐标系，包括水下航行体本体坐标系oxbybzb和导航系 ox
nynzn
。
[0040]
水下航行体本体坐标系oxbybzb的xb轴与航行体纵轴重合，且xb轴正方向指向头部。
[0041]
水下航行体本体坐标系oxbybzb的yb轴在航行体纵向对称面内，与xb轴垂直，正方向指向上。
[0042]
水下航行体本体坐标系oxbybzb的zb轴与xb轴、yb轴成右手坐标系。
[0043]
导航系ox
nynzn
为发射坐标系，取水下航行体所在地域的水平坐标系，以发射点为原点。
[0044]
水下航行体在初始对准时，x轴朝天，导航系ox
nynzn
的yn轴、zn轴在当地水平面内。yn轴正方向指向水下航行体本体坐标系yb轴在当地水平的投影，xn轴在当地铅垂线上，正方向指向上。xn轴和yn轴、zn轴成右手坐标系。
[0045]
水下航行体绕x轴转动为横滚，绕y轴转动为航向，绕z轴转动为俯仰。
[0046]
水下航行体本体坐标系oxbybzb的原点定义在航行体质心，且与发射系原点重合。
[0047]
3.2)基于水下航行体本体坐标系oxbybzb和导航系ox
nynzn
的转换关系，建立水下航行体在航行过程中的姿态矩阵即：
[0048][0049][0050]
式中，ψ为航向角，θ为俯仰角，γ为横滚角。与互为转置关系。n系是导航系，b系是航行体本体坐标系。
[0051]
水下航行体本体坐标系oxbybzb和导航系ox
nynzn
的转换关系为：导航系ox
nynzn
绕y轴转动角度ψ，变换为坐标系ox1y1z1。导航系 ox1y1z1绕z轴转动角度θ，变换为坐标系ox2y2z2。坐标系ox2y2z2绕 x轴转动角度γ，变换为水下航行体本体坐标系oxbybzb。
[0052]
3.3)解算姿态矩阵得到水下航行体在航行过程中的姿态角。
[0053]
实施例2：
[0054]
一种基于发射坐标系的水下航行体导航方法，包括以下步骤：
[0055]
1)实际使用中，航向角选取为初始对准中的一个水平姿态角。
[0056]
换言之，初始航向角由水平姿态对准获得。利用惯性空间中地球重力加速度信息的捷联惯导自主对准方法得到两个水平角度初始值，其中一个作为航向角初始值，另一个初始角度值作为俯仰角初始值，天向轴方向取做横滚角，并在初始对准完第一时刻置0。一般情况下，航行体的航向角定义为绕天向轴旋转。但在水下系泊条件下，航行体会绕天向轴会来回旋转而无法进行航向角的初始对准，从而无法进行后续的导航解算。通过该设计，在相对坐标系下，巧妙的由初始水平对准得到航向角初始值，同时得到发射坐标系下横滚角和俯仰角的初始值，使得该算法具备进行捷联导航解算的条件，可以得到航行体在发射坐标系下相对于发射点的相对位置和姿态信息。
[0057]
2)在完成系泊状态下初始对准后，针对水下航行体航行过程中的运动轨迹会使得姿态角发生大范围变化的情况，发明一种捷联矩阵转换方法，能够设计出满足航行体轨迹测量的全姿态导航算法。
[0058]
实施例3：
[0059]
一种基于发射坐标系的水下航行体导航方法，包括以下步骤：
[0060]
1)坐标系定义：
[0061]
航行体本体坐标系oxbybzb原点定义在航行体质心，与发射系原点重合。xb轴与航行体纵轴重合，正方向指向头部；yb轴在航行体纵向对称面内，与xb轴垂直，正方向指向上；zb轴与xb轴、yb轴成右手坐标系；
[0062]
导航系ox
nynzn
为发射坐标系，取当地水平坐标系，以发射点为原点；
[0063]
2)初始对准阶段各姿态角求取过程：
[0064]
定义为绕x轴转动为横滚，绕y轴转动为航向，绕z轴转动为俯仰。
[0065]
2.1)利用惯性空间中地球重力加速度信息的捷联惯导自主对准方法得到航向角与俯仰角的初始对准结果作为导航解算开始时刻航向角与俯仰角的初始值。
[0066]
2.2)初始对准结束后进入导航解算过程时刻置横滚角初始值为 0
°
。
[0067]
3)导航算法姿态矩阵求取过程：
[0068]
导航系经过三次旋转到达本体坐标系，通过三次旋转可以唯一确定三个姿态角(ψ为航向角，θ为俯仰角，γ为横滚角)。导航系在初始对准结束后唯一确定，之后的姿态角都由该时刻的本体系与导航系之间的转化关系唯一确定。
[0069]
航行体在初始对准时，x轴朝天，导航系(ox
nynzn
)yn轴、zn轴在当地水平面内，yn轴正方向指向本体系yb轴在当地水平的投影，xn轴在当地铅垂线上，正方向指向上；xn轴轴和yn轴、zn轴成右手坐标系。定义为绕x轴转动为横滚，绕y轴转动为航向，绕z轴转动为俯仰，转动顺序为y、z、x；三次旋转过程如下式所示。
[0070]
具体的转动关系如下所示：
[0071][0072]
由此得到导航计算的姿态阵为：
[0073][0074][0075]
通过以上列出的姿态转换矩阵进行导航解算即可解算出水下航行体在初始对准完成后航行过程中的水平姿态角和航向角。
[0076]
实施例4：
[0077]
一种基于发射坐标系的水下航行体导航方法的仿真验证，包括以下步骤：
[0078]
利用实际水下测试数据进行算法仿真分析。航行体的测量惯导采用的惯性测量仪表是mems陀螺和石英加速度计。mems陀螺的零偏稳定性精度是20
°
/h(全温度域)，石英加速度计的零偏稳定性精度是0.3mg(全温度域)。水下航行体从发射开始到航行结束的测试时间是40s左右(包括发射前的20s初始对准数据)。表1、表2、表3分别给出了3次水下航行体发射后的20s内的导航姿态角误差和相对位置误差，表中的姿态角误差与位置误差通过对比同时搭载测量的高精度惯导获得，高精度惯导的导航解算以地理坐标系为导航坐标系。
[0079]
表1
[0080][0081]
由以上可得出发射坐标系下的全姿态导航解算结果与测试惯导仪表的精度相匹配，发射坐标系下的全姿态导航解算能够满足水下航行体的测试精度要求。