[发明专利]一种小型水下机器人组合导航定位方法

说明书

技术领域

本发明涉及的是一种小型水下机器人自主导航与自主校正方法。

背景技术

小型水下机器人执行水下任务过程中，需为其控制系统和制导系统提供准确、平滑的位置、深度、姿态以及载体系下的速度和加速度信息，并能自动上浮至水面进行自主校正。小型水下机器人导航定位系统往往存在由海流或海浪干扰导致的系统模型失配问题，并且导航定位系统一般仅能提供未经滤波的加速度、深度信息和导航系下的速度信息，这些信息需经滤波和坐标变换到艇体系后方可被机器人控制系统使用。而取多路导航信号的导航方法通常是简单导航信息的备份，而没有进行有效的信息融合。本发明采用强跟踪无迹卡尔曼滤波算法有效地解决了小型水下机器人导航定位系统的模型失配问题，对多路导航设备提供的导航信息进行融合，提高了定位精度，平滑了航向、高度信息，并能直接为控制系统提供平滑的艇体系下的速度与加速度信息。《控制与决策》(1990，第5期)“非线性系统的带次优渐消因子的扩展卡尔曼滤波”提出了强跟踪滤波器有效解决了EKF由于模型失配导致的鲁棒性差、滤波发散等问题，但要求非线性系统状态函数和量测函数必须是连续可微的，而且强跟踪滤波器需要计算非线性函数的雅可比矩阵，不但过程繁琐易出错，而且对非线性函数的线性化近似精度通常偏低，当系统具有强非线性时精度严重下降，甚至发散。本发明采用的强跟踪无迹卡尔曼滤波算法通过采用UT变换克服了上述问题。《航空电子技术》(2008，第39卷第4期)“强跟踪UKF滤波在SINS/GPS组合导航中的应用研究”，针对大机动条件下飞行器SINS/GPS组合导航系统的滤波发散问题，设计了强跟踪无迹卡尔曼滤波算法，通过渐消因子在线调整卡尔曼滤波增益从而提高无迹卡尔曼滤波算法的跟踪性能。《仪器仪表学报》(2008，第29卷第8期)“强跟踪UKF方法及其在故障辨识中的应用”也基于强跟踪无迹卡尔曼滤波算法设计了GNSS/SINS组合导航系统。本发明采用的强跟踪无迹卡尔曼滤波算法同上述算法的渐消因子的计算方法不同，且更为合理。

发明内容

本发明的目的在于提供一种既能有效解决小型水下机器人因海流或海浪干扰而导致的导航定位系统模型失配问题，又能平滑小型水下机器人控制系统所需的航向、深度以及在艇体系下的速度与加速度信息的集自主水下导航定位与自主水面位置校正功能的小型水下机器人导航定位方法。

本发明的目的是这样实现的：

本发明的小型水下机器人组合导航定位方法主要包括如下步骤：1)采用强跟踪无迹卡尔曼滤波器进行组合导航系统的数据融合；2)取导航系下的位置矢量和航向角与载体系下速度矢量和加速度矢量作为滤波器状态矢量，滤波结果被机器人控制与制导系统直接使用；3)取深度计输出的深度信息同航姿参考系统输出的航向角、姿态角和载体系下的加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主导航与数据滤波；4)取GPS接收机输出的水平位置信息，航姿参考系统输出的航向、姿态及载体系下加速度信息与载体系下多普勒测速声纳的速度信息，通过强跟踪无迹卡尔曼滤波器实现水下机器人的自主校正与数据滤波；5)通过GPS接收机输出信号的有效位实现水上和水下两个不同量测方程的切换。

1、所述采用强跟踪无迹卡尔曼滤波器进行组合导航系统数据融合的步骤如下：

系统噪声方差阵为Q，观测噪声方差阵为R；

(1)选择对称点采样策略，根据P(k|k)计算k时刻Sigma点集{χ_i}，其中i＝1、...、L；

(2)计算经过非线性状态方程传播后的Sigma点χ_i(k+1|k)，由χ_i(k+1|k)计算得状态向量X(k)一步预测估计