[发明专利]一种无人潜航器惯性/多普勒组合导航方法

权利要求书

1.一种无人潜航器惯性/多普勒组合导航方法，其特征在于，包括以下步骤：

(1)确定无人潜航器在高纬度航行区域的导航坐标系及位置表示方式，包括如下步骤：

(1.1)确定无人潜航器高纬度地区航行时的导航坐标系，高纬度地区导航坐标系确定为横坐标系；首先将地球坐标系e绕其x_e轴旋转-90°，进而绕第一次旋转之后中间坐标系的z_e′轴旋转-90°得到横地球坐标系e′；即以90°E经线与赤道平面的交点为伪北极点，以90°W经线与赤道平面的交点为伪南极点，以0°/180°经线圈作为伪赤道，其中，横坐标系t的定义为：y_t轴指向伪北极点，z_t轴垂直于当地水平面，x_t轴与y_t轴、z_t轴构成右手正交坐标系；

将横地球坐标系e′与地球坐标系e之间的方向余弦矩阵表示为：

(1.2)将无人潜航器在横坐标系中位置表示为(L^t，λ^t，h)，其中，L^t表示横纬度，λ^t表示横经度，h表示无人潜航器距离水平面的高度；

(2)确定无人潜航器在横坐标系下的更新方程，包括姿态更新方程、速度更新方程、位置更新方程，具体实施如下：

(2.1)确定横坐标系下的姿态更新方程为：

其中，表示横坐标系相对于载体坐标系b的方向余弦矩阵，表示载体坐标系相对于惯性坐标系i的旋转角速度，表示横坐标系相对于惯性坐标系的旋转角速度；

(2.2)确定横坐标系下的速度v^t的更新方程为：

其中，

式中，f^b表示载体坐标系下表示的比力，g^t表示横坐标系下表示的重力矢量，表示横坐标系相对于惯性坐标系的旋转角速度在横坐标系下的投影，表示横地球坐标系相对于惯性坐标系的旋转角速度在横坐标系下的投影，表示横坐标系相对于横地球坐标系的旋转角速度在横坐标系下的投影，ω_ie表示地球旋转角速度，R_e为地球半径，表示横坐标系下的东向速度，表示横坐标系下的北向速度；

(2.3)确定横坐标系下的位置更新方程为：

式中，表示横坐标系下的垂向速度；

(3)确定无人潜航器在横坐标系下的姿态误差方程、速度误差方程、位置误差方程，具体实施如下：

确定姿态误差φ^t的方程如下：

其中，表示横坐标系相对于惯性坐标系的旋转角速度误差，示载体坐标系b相对于惯性坐标系的旋转角速度误差；

确定速度误差δv^t的方程如下：

其中，f^t表示横坐标系下表示的比力，，表示横地球坐标系相对于惯性坐标系的旋转角速度误差，表示横坐标系相对于横地球坐标系的旋转角速度误差，δf^b表示比力误差；

确定位置误差的方程如下：

式中，δL^t,δλ^t,δh分别表示横纬度误差、横经度误差、高度误差，分别表示横坐标系下表示的东向速度误差、北向速度误差、垂向速度误差；

(4)确定无人潜航器导航参数在地理坐标系与横坐标系之间的转换关系并进行转换，导航参数的转换包括姿态转换、速度转换、位置转换；

其中，无人潜航器姿态参数在地理坐标系、横坐标系之间的转换关系为：

式中，表示地理坐标系n与载体坐标系b之间的方向余弦矩阵，表示横坐标系与地理坐标系之间的方向余弦矩阵，表示地理坐标系与横坐标系之间的方向余弦矩阵，与互为转置矩阵，且有：

式中，表示地球坐标系与地理坐标系之间的方向余弦矩阵，表示横坐标系与横地球坐标系之间的方向余弦矩阵，L、λ分别表示地理坐标系下的纬度、经度；

无人潜航器速度参数在地理坐标系、横坐标系之间的转换关系为：

式中，vⁿ表示地理坐标系下表示的速度；

无人潜航器位置参数在横坐标系、地理坐标系之间的转换关系为：

L^t＝arcsin(cos L sinλ) L＝arcsin(cos L^tcosλ^t)

高度h在横坐标系、地理坐标系中保持不变；

(5)完成INS/DVL组合导航滤波器在地理坐标系与横坐标系之间的转换，其中INS/DVL组合导航滤波器采用闭环反馈校正方式，具体实施如下：

(5.1)分别确定地理坐标系与横坐标系下的系统误差状态为：

地理坐标系下的系统误差状态xⁿ(t)为

横坐标系下的系统误差状态x^t(t)为

其中，分别表示地理坐标系下表示的东向、北向、垂向姿态误差，分别表示横坐标系下表示的东向、北向、垂向姿态误差，别表示地理坐标系下表示的东向、北向、垂向速度误差，δL,δλ分别表示地理坐标系下的纬度、经度误差，分别表示x、y、z轴向陀螺常值零偏，分别表示x、y、z轴向加表常值零偏，k,δΨ,δθ分别表示DVL的标度因数误差、方位安装误差角、俯仰安装误差角；

(5.2)分别确定姿态误差、速度误差、位置误差在地理坐标系与横坐标系下间的转换关系为：

首先确定地理坐标系下姿态误差φⁿ与横坐标系下姿态误差φ^t之间的转换关系

式中，

其次确定地理坐标系下速度误差δvⁿ与横坐标系下速度误差δv^t之间的转换关系

进而确定横坐标系下表示的纬度误差δL^t、经度误差δλ^t、高度误差δh与地理坐标系下表示的纬度误差δL、经度误差δλ、高度误差δh的转换关系

陀螺常值零偏加表常值零偏DVL的标度因数误差、方位安装误差角、俯仰安装误差角k,δΨ,δθ、高度误差δh在地理坐标系与横坐标系下保持不变；

确定横坐标系下的系统误差状态x^t(t)与地理坐标系下的系统误差状态xⁿ(t)之间的转换关系如下：

x^t(t)＝Φxⁿ(t),xⁿ(t)＝Φ^-1x^t(t)

其中，Φ为转换系数矩阵，并且根据φ^t与φⁿ之间的转换关系，δv^t与δvⁿ之间的转换关系，δL^t,δλ^t与δL、δλ之间的转换关系，并考虑高度误差δh、陀螺常值零偏加表常值零偏DVL的标度因数误差、方位安装误差角、俯仰安装误差角k,δΨ,δθ在地理坐标系与横坐标系下的不变性进行确定；

(5.3)根据步骤(5.2)，确定地理坐标系下系统误差状态协方差矩阵Pⁿ(t)与横坐标系下系统误差状态协方差矩阵P^t(t)的转换关系：

Pⁿ(t)＝Φ^-1P^t(t)Φ^-T

式中，示横坐标系下表示的系统误差状态估计值，表示地理坐标系下表示的系统误差状态估计值；

(5.4)当无人潜航器在中纬度、高纬度地区跨纬度航行时，闭环反馈INS/DVL组合导航滤波器完成在地理坐标系与横坐标系之间的协方差矩阵转换，转换方式按照步骤(5.3)所述，鉴于系统误差状态在每次闭环反馈校正后为0，其不需要转换。