[发明专利]一种水下组合导航系统行进间对准与姿态估计方法

权利要求书

1.一种水下组合导航系统行进间对准与姿态估计方法，其特征在于，该方法分为以下步骤：

S1：将自主式水下航行器AUV的捷联惯性导航系统SINS与多普勒测速仪DVL接入导航计算机，将AUV投放至任务区域；当AUV潜入水下，无法接收到GNSS信号时，通过传统方法进行姿态粗对准；

S2：通过导航计算机接收到捷联惯性导航系统中陀螺与加速度计输出的角速度与加速度增量，利用导航计算机进行导航解算：

定义坐标系：选取“东—北—天”地理坐标系为导航坐标系，记为n系；选取“右—前—上”坐标系为载体坐标系，记为b系；地心惯性坐标系记为i系；地球坐标系记为e系；计算导航坐标系记为n′系；

S2.1执行导航计算机计时器，初始化时刻k＝0；

S2.2计时器时间更新k＝k+1；

S2.3执行惯性导航解算

通过陀螺输出的采样角增量和加速度计输出的采样比力速度增量解算得到k时刻的SINS姿态姿态矩阵速度和位置

S3：构建13维离散化的基于马氏距离的VBRAKF滤波模型与观测模型，具体步骤如下：

S3.1构建组合导航系统13维基于马氏距离的VBRAKF的滤波模型：

S3.1.1执行VBRAKF状态一步预测更新为：

式(1)中，表示k-1时刻输出的状态向量X的后验估计，表示状态向量X从k-1时刻至k时刻的一步预测；X表示包含SINS姿态误差速度误差δv，位置误差δP和陀螺漂移误差ε^b、加速度零偏误差的13维状态向量，X的表达式为：

的初始值设置为0_1×13表示1×13维的零矩阵，上标T表示矩阵转置；

式(2)中，位置误差为δP＝[δL,δλ]，δL和δλ分别为SINS的纬度误差与经度误差；速度误差δv＝[δv_E,δv_N]，δv_E和δv_N分别为东向速度误差和北向速度误差；姿态失准角分别表示x轴、y轴和z轴方向的姿态失准角；陀螺漂移误差为加速度计零偏误差为

式(1)中，Φ_k|k-1为从k-1时刻到k时刻的状态一步预测矩阵，计算公式为：

Φ_k|k-1≈I+F(k-1)T_s (3)

式(3)中，I为13维单位矩阵，T_s为离散化时间间隔，F(k-1)为k-1时刻的SINS系统模型矩阵；F(k-1)的表达式为：

其中0_i×j为i×j维的零矩阵，F_ij为子矩阵，各子矩阵的表达式为：

式(5)-式(14)中，R_e为地球半径，v_E(k-1)为k-1时刻SINS的东向速度，v_N(k-1)为k-1时刻SINS的北向速度，L(k-1)为k-1时刻SINS的纬度，ω_ie为地球自转角速度，f_U(k-1)表示k-1时刻加速度计输出的比力在天向的投影，f_N(k-1)表示k-1时刻加速度计输出的比力在北向的投影，f_E(k-1)表示k-1时刻加速度计输出的比力在东向的投影，表示k-1时刻导航计算机解算得到的姿态矩阵，表示姿态矩阵的前两行；

S3.1.2执行VBRAKF状态一步预测均方误差矩阵更新：

式(15)中，P_k-1|k-1称为k-1时刻的状态后验估计均方误差矩阵，P_k|k-1称为从k-1时刻到k时刻的状态一步预测均方误差矩阵，利用式(15)，状态后验估计均方误差矩阵P_k-1|k-1更新成为状态一步预测均方误差矩阵P_kk-1；

P_k-1|k-1初始值P_0|0为：

其中diag表示矩阵对角线元素，μg为加速度计零偏误差单位，1μg＝10^-6g≈9.8×10^-6m/s²，13×13矩阵其他元素为0；

Q(k-1)称为k-1时刻的系统噪声矩阵，是一个13×13维的对角矩阵，由SINS陀螺和加速度计的随机误差决定；由于SINS误差特性稳定，因此系统噪声矩阵是恒定的，即Q(k-1)＝Q(0)，初始值为：

S3.2将导航计算机SINS解算速度减去DVL输出的观测速度在计算导航系下的投影速度作为k时刻的观测量Z(k)，构建VBRAKF观测模型：

式(18)中，为k时刻DVL输出的载体坐标系下的速度；

式(18)中第二至第六行是根据观测量Z(k)构建观测模型，为k时刻SINS真实的姿态矩阵，为k时刻的理想导航坐标系至计算导航坐标系间的姿态矩阵，即k时刻的姿态误差矩阵的逆矩阵，表示k时刻的姿态误差，×为叉乘符号，表示的反对称矩阵，其表达式为：

式(18)中，vⁿ(k)为k时刻SINS真实的速度，δvⁿ(k)为k时刻SINS真实速度误差；

Z(k)＝H(k)X(k)+V(k)为组合导航系统的观测模型，H(k)为k时刻的观测矩阵，其表达式为：

式(20)中，I_2×2为2×2的单位矩阵，0_2×2为2×2的零矩阵；实际式(20)计算过程中，用代替

V(k)为k时刻的观测噪声矩阵，由k时刻的东向速度观测噪声V_E(k)和北向速度观测噪声V_N(k)组成，其表达式为：

设R_E(k),R_N(k)分别为k时刻的东向、北向速度观测噪声方差，满足如下条件：

E[·]表示求其期望值；观测噪声V(k)的协方差矩阵为：

观测噪声协方差矩阵R(0)的初始值为：

R(0)＝diag{(0.1m/s)²,(0.1m/s)²} (24)

S4：构建基于马氏距离的VBRAKF鲁棒自适应模块，进行VBRAKF状态估计更新：

S4.1将k时刻观测噪声协方差矩阵R(k)的先验分布选择为逆Wishart分布：

将观测噪声协方差矩阵R(k)的先验分布选择为逆Wishart分布，其表达式为：

p(R(k)|Z(1:(k-1)))＝IW(R(k)；u_k|k-1,U_k|k-1) (25)

式(25)中，p(R(k)|Z_1:k-1)为观测噪声协方差矩阵R(k)的先验分布，Z(1:(k-1))为[1,k-1]时间段内所有的观测量，p(R(k)|Z(1:(k-1)))具体表示在[1,k-1]内所有的观测量先验已知下，对k时刻观测噪声协方差矩阵R(k)的概率分布作估计；IW(R(k)；u_k|k-1,U_k|k-1)称为R(k)的概率分布为逆Wishart分布，u_k|k-1称为逆Wishart分布的自由度参数，U_k|k-1称为逆Wishart分布的逆尺度矩阵；

S4.2更新逆Wishart分布的自由度参数与逆尺度矩阵

通过k-1时刻输出的后验估计u_k-1|k-1和U_k-1|k-1更新u_k|k-1和U_k|k-1：

U_k|k-1＝ρU_k-1|k-1 (26)

u_k|k-1＝ρ(u_k-1|k-1-m-1)+m+1 (27)

式(26)、(27)中ρ表示变分衰减因子；

其中，逆Wishart分布U_k-1|k-1的初值U_0|0：

U_0|0＝(u_0|0-m-1)R(0) (28)

初值u_0|0＝10；

S4.3构建基于马氏距离的鲁棒化模块，计算k时刻的新息e(k)的马氏距离，并计算膨胀因子ω(k)；新息表示观测量Z(k)与状态一步预测量之间的差，膨胀因子ω(k)用来衡量观测量Z(k)被污染的程度：

S4.3.1计算新息e(k)马氏距离：

式(29)中M(k)为k时刻新息e(k)的马氏距离，马氏距离公式中间部分利用的是上一时刻估计得到的近似为k时刻的观测噪声矩阵R(k)；

S4.3.2通过S4.3.1得到的马氏距离判断观测量Z(k)是否为野值，计算膨胀因子ω(k)以衡量观测量Z(k)被污染的程度；

具体判断方法为：

选择统计门限函数对于观测量Z(k)，若S4.3.1计算的马氏距离满足M(k)²≤η²，则Z(k)被标记为正常观测量，此时膨胀因子为ω(k)＝1；

如果马氏距离满足M(k)²＞η²，则Z(k)被标记为野值；此时，膨胀因子ω(k)＞1，通过牛顿迭代法求解膨胀因子ω(k)，公式为：

式(30)中，上标(j)表示牛顿迭代次数，当前时刻观测噪声协方差阵R(k)利用上一时刻估计值近似，即膨胀因子迭代的初值ω(k)⁽⁰⁾＝1；每次迭代后的均方误差矩阵：

S4.4构建基于马氏距离的变分贝叶斯鲁棒自适应模块，通过变分贝叶斯方法进行固定点迭代，估计观测噪声协方差矩阵及状态向量：

利用式(1)、(15)、(26)、(27)得到的P_k|k-1，U_k|k-1，u_k|k-1初始化固定点迭代：

中上标(0)表示当前固定点迭代次数为0，第i次固定点迭代近似的参数表示形式为

变分贝叶斯固定点迭代估计的步骤为：

S4.4.1计算第i次迭代变分贝叶斯估计均方误差矩阵B(k)⁽ⁱ⁾，公式为：

S4.4.2计算第i+1次迭代逆尺度矩阵近似估计观测噪声协方差矩阵

S4.4.3计算第i+1次迭代的滤波增益K(k)⁽ⁱ⁺¹⁾

S4.4.4计算第i+1次迭代近似的状态后验估计以及状态后验估计均方误差阵

S4.4.5循环执行S4.4.1-S4.4.4，当i＝N-1时，结束迭代；其中，N代表迭代次数；输出k时刻迭代计算完成的状态向量状态后验估计均方误差矩阵观测噪声协方差矩阵逆尺度矩阵

输出的状态向量是通过VBRAKF得到的k时刻SINS解算导航信息包含的误差，包括k时刻AUV的姿态误差位置误差δP(k)，速度误差δv(k)，陀螺漂移误差ε^b(k)和加速度计零偏误差

S5：构建导航误差反馈补偿回路，得到k时刻AUV的姿态、位置、速度在任意时刻的修正值，并进入k+1时刻循环执行S2.2-S4.2，直到导航结束：

S5.1将S3.3.6得到的滤波估计误差值反馈补偿给S2得到导航解算结果：

S5.1.1将k时刻姿态误差转换为姿态误差矩阵

其中，简记三角函数由S4.4得到；

S5.1.2将S5.1.1得到的姿态误差矩阵S4.3得到的速度误差δv(k)与位置误差δP(k)反馈修正给S2.3得到的SINS解算姿态位置与速度具体方法如下：

式(39)中，表示的转置矩阵；通过式(39)-(41),即得到了k时刻导航修正值，包括高精度的姿态速度和位置

S5.2进入下一时刻，循环执行S2.2-S5.2，得到k+1时刻的高精度姿态速度和位置直到导航结束。