一种基于分数阶容积卡尔曼滤波的AUV鲁棒导航定位方法及系统

本发明涉及水下航行器组合导航，尤其是一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法及系统。

背景技术：

1、捷联惯性导航系统（strapdown inertial navigation system, sins）具有自主性高、短时精度好、输出信息连续等优点，是auv实现自主导航的核心传感器；但其导航误差随时间累积发散，必须借助多普勒测速仪（doppler velocity log, dvl）等外部测速手段进行组合校正。sins/dvl组合导航通过卡尔曼滤波等状态估计算法对惯性导航误差与传感器误差进行最优融合，是当前auv水下长航时导航的主流技术途径。

2、在sins/dvl组合导航滤波中，惯性测量单元（imu）的零偏（bias）误差建模是影响导航精度的关键环节。针对imu零偏建模问题，传统组合导航滤波方法多采用常值偏置（constant bias）模型描述惯性器件的零偏误差。该类模型结构简洁、易于实现，但在复杂动态水下环境中，imu零偏往往呈现缓慢时变（slow-varying）的漂移特性，常值模型难以刻画零偏误差的这一物理本质，导致长航时导航过程中零偏补偿能力不足、导航误差持续累积。

3、为提高惯性器件误差建模精度，高斯-马尔可夫（gauss-markov）模型、高阶自回归（auto-regressive, ar）模型等时变误差模型被相继引入。这类模型虽能在一定程度上描述零偏误差的随机性与时间相关性，但通常需引入额外的状态变量、相关时间尺度参数或离线辨识参数，由此增加了滤波器的状态维数、计算负担与参数调节难度，限制了其在auv实际任务中的工程适用性。

4、与此同时，水下复杂环境还导致dvl量测噪声的统计特性往往未知或缓慢变化。针对量测噪声协方差在线估计问题，sage-husa自适应滤波、多模型自适应滤波（interactingmultiple model, imm）以及变分贝叶斯（variational bayesian, vb）等方法被相继提出并用于量测噪声协方差的自适应估计。然而，现有vb类方法的协方差更新通常依赖新息（innovation）序列的外积，当dvl量测受到突发野值（outlier）或强非高斯干扰影响时，异常新息将显著放大量测残差，并进一步污染噪声协方差的估计结果，致使滤波精度下降甚至出现发散。

5、为抑制异常量测对滤波更新的不利影响，最大相关熵准则（maximum correntropycriterion, mcc）被引入鲁棒滤波过程。但mcc的鲁棒性能对核宽（kernel width）参数较为敏感，核宽选取不当时难以兼顾正常量测的有效利用与异常量测的抑制，参数整定较为困难。

6、综上所述，在复杂水下环境下，sins/dvl组合导航同时面临imu零偏慢变漂移、dvl量测噪声时变以及非高斯野值干扰等多重挑战。现有方法多针对上述某一类问题独立进行改进，缺乏在统一滤波框架内协同处理多源误差与异常量测的能力。

技术实现思路

1、为了解决现有组合导航滤波方法在惯导零偏慢变漂移、量测噪声统计特性时变以及非高斯野值干扰抑制方面难以协同处理的问题，本发明提出一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法及系统。

2、本发明解决其技术问题所采用的技术方案是：一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法，包括如下步骤：

3、步骤1，获取捷联惯性导航系统输出的惯性测量单元数据和多普勒计程仪输出的速度量测数据；

4、步骤2，将组合导航误差状态划分为导航误差子状态和零偏子状态，所述导航误差子状态采用整数阶惯导误差动力学方程进行传播，所述零偏子状态基于历史时刻储存的零偏后验估计值通过分数阶递推方程进行传播，所述分数阶递推方程将零偏预测划分为当前时刻的不确定性传播项和由历史后验估计值构成的确定性修正项，且不扩展状态维数；

5、步骤3，通过零偏与导航误差之间的耦合矩阵将所述零偏子状态与所述导航误差子状态关联，构建统一的状态转移模型；基于所述状态转移模型生成容积点，经时间传播得到状态预测值、预测协方差和预测量测值；

6、步骤4，根据所述速度量测数据与所述预测量测值构造量测新息；将量测噪声协方差视为随机变量，以逆wishart分布作为其共轭先验，根据所述量测新息的统计特性通过变分贝叶斯方法递推估计所述量测噪声协方差；

7、步骤5，基于归一化后的量测新息构造各量测通道的自适应因子，根据所述自适应因子对预设的基准核宽进行逐通道动态调节，得到各量测通道的自适应核宽；基于所述自适应核宽计算各量测的相关熵权重，构造量测权重矩阵；

8、步骤6，利用所述量测权重矩阵对量测噪声协方差的更新过程进行鲁棒修正，构造等效量测噪声协方差；基于所述等效量测噪声协方差计算滤波增益，完成状态估计与协方差更新。

9、上述的一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法，所述导航误差子状态包括姿态误差、速度误差和位置误差共九维，所述零偏子状态包括陀螺仪零偏和加速度计零偏共六维；

10、所述九维导航误差子状态采用整数阶惯导误差动力学方程传播，所述六维零偏子状态采用分数阶递推方程传播。

11、上述的一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法，所述分数阶递推方程根据预设的分数阶阶数，对历史时间窗口内储存的零偏后验估计值进行加权求和，得到确定性修正项；所述当前时刻的不确定性传播项由零偏驱动白噪声的不确定性传播确定。

12、上述的一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法，所述方法还包括：

13、利用滤波得到的零偏估计值对所述惯性测量单元输出的角速度和比力进行补偿，将补偿后的角速度和比力用于所述整数阶惯导误差动力学方程的传播。

14、上述的一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法，所述步骤3中零偏与导航误差之间的耦合矩阵为：

15、

16、在一阶离散近似下：

17、其中，为载体系到导航系的方向余弦矩阵，其中，为陀螺零偏对导航误差的耦合矩阵，为加速度计零偏对导航误差的耦合矩阵，为采样周期；

18、陀螺仪零偏通过姿态误差通道影响导航误差传播，加速度计零偏通过速度误差通道影响导航误差传播，位置误差通过速度误差积分间接受到零偏影响。

19、上述的一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法，所述步骤3中容积点具体为：

20、

21、其中，为后验状态估计，为协方差，为的平方根矩阵，是的第列向量，其中是维数为的单位矩阵。

22、上述的一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法，所述步骤5中自适应核宽为：

23、

24、

25、其中，为第j维量测通道的自适应因子，为核宽最小收缩比例，为基准核宽；

26、相关熵权重为：

27、。

28、上述的一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法，所述步骤6等效量测噪声协方差为：

29、

30、

31、

32、其中，为量测噪声协方差的自由度参数，为量测噪声协方差的尺度矩阵，为量测噪声协方差矩阵，为量测权重矩阵，为k时刻各量测分量的鲁棒权重，m为量测向量的维数。

33、一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位系统，用于实施如上述的一种基于分数阶容积卡尔曼滤波的auv鲁棒导航定位方法，包括数据获取模块，其用于获取捷联惯性导航系统输出的惯性测量单元数据和多普勒计程仪输出的速度量测数据；

34、状态分块建模模块，其用于将组合导航误差状态划分为导航误差子状态和零偏子状态，所述导航误差子状态采用整数阶惯导误差动力学方程进行传播，所述零偏子状态基于历史时刻储存的零偏后验估计值通过分数阶递推方程进行传播，且不扩展状态维数；

35、状态转移构建模块，其用于通过零偏与导航误差之间的耦合矩阵将所述零偏子状态与所述导航误差子状态关联，构建统一的状态转移模型；

36、时间更新模块，其用于基于所述状态转移模型生成容积点，经时间传播得到状态预测值、预测协方差和预测量测值；

37、新息计算模块，其用于根据所述速度量测数据与所述预测量测值构造量测新息；

38、噪声协方差估计模块，其用于将量测噪声协方差视为随机变量，以逆wishart分布作为其共轭先验，根据所述量测新息的统计特性通过变分贝叶斯方法递推估计所述量测噪声协方差；

39、自适应核宽计算模块，其用于基于归一化后的量测新息构造各量测通道的自适应因子，根据所述自适应因子对预设的基准核宽进行逐通道动态调节，得到各量测通道的自适应核宽，并基于所述自适应核宽计算各量测的相关熵权重，构造量测权重矩阵；

40、鲁棒量测更新模块，其用于利用所述量测权重矩阵对量测噪声协方差的更新过程进行鲁棒修正，构造等效量测噪声协方差，并基于所述等效量测噪声协方差计算滤波增益，完成状态估计与协方差更新。

41、本发明的有益效果是，（1）导航精度提升：通过引入自适应最大相关熵变分贝叶斯分数阶容积卡尔曼滤波框架，所提算法在整个测试过程中保持了最小的位置误差和最稳定的误差变化趋势。零偏估计精度达91.47%–94.03%，相比整数阶模型，分数阶模型能够更准确地提取零偏中的低频变化趋势，减缓由imu误差建模不准确引起的导航误差累积。

42、（2）异常量测抑制能力增强：所提方法未出现传统算法中的误差发散现象，能够有效降低量测突发野值和非高斯噪声对状态更新的影响，提高复杂水下环境下滤波过程的鲁棒性。

43、（3）噪声自适应估计能力提高：通过变分贝叶斯方法估计量测噪声，并结合最大相关熵根据新息自适应调节量测权重，本发明能够在量测噪声统计特性未知或缓慢变化时实现更稳定的噪声协方差估计，避免异常新息对量测噪声估计结果的影响。

44、（4）稳定性更优：所提方法能够有效补偿惯导慢变零偏引起的导航误差累积，并在弱观测、噪声时变和异常量测并存条件下保持较低误差水平，为auv自主导航任务提供了精度更高、稳定性更强、环境适应能力更好的技术方案。