申请/专利权人：中国人民解放军国防科技大学

申请日：2022-04-17

公开（公告）日：2024-04-05

公开（公告）号：CN114777812B

主分类号：G01C25/00

分类号：G01C25/00

优先权：

专利状态码：有效-授权

法律状态：2024.04.05#授权;2022.08.09#实质审查的生效;2022.07.22#公开

摘要：本发明涉及捷联惯性导航技术领域，具体涉及到一种水下组合导航系统行进间对准与姿态估计方法。在AUV水下组合导航系统进行行进间对准及姿态估计时，本发明先通过马氏距离方法判断观测信息是否被野值污染：如果判定DVL观测信息没有被污染，利用变分贝叶斯方法自适应观测噪声协方差矩阵；反之，如果DVL观测信息被污染，通过计算膨胀因子以衡量被污染程度，将膨胀因子引入变分贝叶斯方法自适应过程，自适应得到观测噪声协方差。最后通过VBRAKF状态估计得到高精度的水下运载体姿态信息以及导航信息。运用本发明方法进行组合导航，系统有着更高的导航精度，更强的可靠性，满足自主、精确水下长航时、长航程的导航定位能力。

主权项：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的表达式为： 的初始值设置为01×13表示1×13维的零矩阵，上标T表示矩阵转置；式2中，位置误差为δP＝[δL,δλ]，δL和δλ分别为SINS的纬度误差与经度误差；速度误差δv＝[δvE,δvN]，δvE和δvN分别为东向速度误差和北向速度误差；姿态失准角分别表示x轴、y轴和z轴方向的姿态失准角；陀螺漂移误差为加速度计零偏误差为式1中，Φk|k-1为从k-1时刻到k时刻的状态一步预测矩阵，计算公式为：Φk|k-1≈I+Fk-1Ts3式3中，I为13维单位矩阵，Ts为离散化时间间隔，Fk-1为k-1时刻的SINS系统模型矩阵；Fk-1的表达式为： 其中0i×j为i×j维的零矩阵，Fij为子矩阵，各子矩阵的表达式为： 式5-式14中，Re为地球半径，vEk-1为k-1时刻SINS的东向速度，vNk-1为k-1时刻SINS的北向速度，Lk-1为k-1时刻SINS的纬度，ωie为地球自转角速度，fUk-1表示k-1时刻加速度计输出的比力在天向的投影，fNk-1表示k-1时刻加速度计输出的比力在北向的投影，fEk-1表示k-1时刻加速度计输出的比力在东向的投影，表示k-1时刻导航计算机解算得到的姿态矩阵，表示姿态矩阵的前两行；S3.1.2执行VBRAKF状态一步预测均方误差矩阵更新： 式15中，Pk-1|k-1称为k-1时刻的状态后验估计均方误差矩阵，Pk|k-1称为从k-1时刻到k时刻的状态一步预测均方误差矩阵，利用式15，状态后验估计均方误差矩阵Pk-1|k-1更新成为状态一步预测均方误差矩阵Pkk-1；Pk-1|k-1初始值P0|0为： 其中diag表示矩阵对角线元素，μg为加速度计零偏误差单位，1μg＝10-6g≈9.8×10-6ms2，13×13矩阵其他元素为0；Qk-1称为k-1时刻的系统噪声矩阵，是一个13×13维的对角矩阵，由SINS陀螺和加速度计的随机误差决定；由于SINS误差特性稳定，因此系统噪声矩阵是恒定的，即Qk-1＝Q0，初始值为： S3.2将导航计算机SINS解算速度减去DVL输出的观测速度在计算导航系下的投影速度作为k时刻的观测量Zk，构建VBRAKF观测模型： 式18中，为k时刻DVL输出的载体坐标系下的速度；式18中第二至第六行是根据观测量Zk构建观测模型，为k时刻SINS真实的姿态矩阵，为k时刻的理想导航坐标系至计算导航坐标系间的姿态矩阵，即k时刻的姿态误差矩阵的逆矩阵，表示k时刻的姿态误差，×为叉乘符号，表示的反对称矩阵，其表达式为： 式18中，vnk为k时刻SINS真实的速度，δvnk为k时刻SINS真实速度误差；Zk＝HkXk+Vk为组合导航系统的观测模型，Hk为k时刻的观测矩阵，其表达式为： 式20中，I2×2为2×2的单位矩阵，02×2为2×2的零矩阵；实际式20计算过程中，用代替Vk为k时刻的观测噪声矩阵，由k时刻的东向速度观测噪声VEk和北向速度观测噪声VNk组成，其表达式为： 设REk,RNk分别为k时刻的东向、北向速度观测噪声方差，满足如下条件： E[·]表示求其期望值；观测噪声Vk的协方差矩阵为： 观测噪声协方差矩阵R0的初始值为：R0＝diag{0.1ms2,0.1ms2}24S4：构建基于马氏距离的VBRAKF鲁棒自适应模块，进行VBRAKF状态估计更新：S4.1将k时刻观测噪声协方差矩阵Rk的先验分布选择为逆Wishart分布：将观测噪声协方差矩阵Rk的先验分布选择为逆Wishart分布，其表达式为：pRk|Z1:k-1＝IWRk；uk|k-1,Uk|k-125式25中，pRk|Z1:k-1为观测噪声协方差矩阵Rk的先验分布，Z1:k-1为[1,k-1]时间段内所有的观测量，pRk|Z1:k-1具体表示在[1,k-1]内所有的观测量先验已知下，对k时刻观测噪声协方差矩阵Rk的概率分布作估计；IWRk；uk|k-1,Uk|k-1称为Rk的概率分布为逆Wishart分布，uk|k-1称为逆Wishart分布的自由度参数，Uk|k-1称为逆Wishart分布的逆尺度矩阵；S4.2更新逆Wishart分布的自由度参数与逆尺度矩阵通过k-1时刻输出的后验估计uk-1|k-1和Uk-1|k-1更新uk|k-1和Uk|k-1：Uk|k-1＝ρUk-1|k-126uk|k-1＝ρuk-1|k-1-m-1+m+127式26、27中ρ表示变分衰减因子；其中，逆Wishart分布Uk-1|k-1的初值U0|0：U0|0＝u0|0-m-1R028初值u0|0＝10；S4.3构建基于马氏距离的鲁棒化模块，计算k时刻的新息ek的马氏距离，并计算膨胀因子ωk；新息表示观测量Zk与状态一步预测量之间的差，膨胀因子ωk用来衡量观测量Zk被污染的程度：S4.3.1计算新息ek马氏距离： 式29中Mk为k时刻新息ek的马氏距离，马氏距离公式中间部分利用的是上一时刻估计得到的近似为k时刻的观测噪声矩阵Rk；S4.3.2通过S4.3.1得到的马氏距离判断观测量Zk是否为野值，计算膨胀因子ωk以衡量观测量Zk被污染的程度；具体判断方法为：选择统计门限函数对于观测量Zk，若S4.3.1计算的马氏距离满足Mk2≤η2，则Zk被标记为正常观测量，此时膨胀因子为ωk＝1；如果马氏距离满足Mk2＞η2，则Zk被标记为野值；此时，膨胀因子ωk＞1，通过牛顿迭代法求解膨胀因子ωk，公式为： 式30中，上标j表示牛顿迭代次数，当前时刻观测噪声协方差阵Rk利用上一时刻估计值近似，即膨胀因子迭代的初值ωk0＝1；每次迭代后的均方误差矩阵： S4.4构建基于马氏距离的变分贝叶斯鲁棒自适应模块，通过变分贝叶斯方法进行固定点迭代，估计观测噪声协方差矩阵及状态向量：利用式1、15、26、27得到的Pk|k-1，Uk|k-1，uk|k-1初始化固定点迭代： 中上标0表示当前固定点迭代次数为0，第i次固定点迭代近似的参数表示形式为变分贝叶斯固定点迭代估计的步骤为：S4.4.1计算第i次迭代变分贝叶斯估计均方误差矩阵Bki，公式为： S4.4.2计算第i+1次迭代逆尺度矩阵近似估计观测噪声协方差矩阵 S4.4.3计算第i+1次迭代的滤波增益Kki+1 S4.4.4计算第i+1次迭代近似的状态后验估计以及状态后验估计均方误差阵 S4.4.5循环执行S4.4.1-S4.4.4，当i＝N-1时，结束迭代；其中，N代表迭代次数；输出k时刻迭代计算完成的状态向量状态后验估计均方误差矩阵观测噪声协方差矩阵逆尺度矩阵输出的状态向量是通过VBRAKF得到的k时刻SINS解算导航信息包含的误差，包括k时刻AUV的姿态误差位置误差δPk，速度误差δvk，陀螺漂移误差εbk和加速度计零偏误差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得到的速度误差δvk与位置误差δPk反馈修正给S2.3得到的SINS解算姿态位置与速度具体方法如下： 式39中，表示的转置矩阵；通过式39-41,即得到了k时刻导航修正值，包括高精度的姿态速度和位置S5.2进入下一时刻，循环执行S2.2-S5.2，得到k+1时刻的高精度姿态速度和位置直到导航结束。

全文数据：

权利要求：

百度查询： 中国人民解放军国防科技大学 一种水下组合导航系统行进间对准与姿态估计方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD