[发明专利]一种基于机动约束和增量滤波的SINS自辅助导航方法

权利要求书

1.一种基于机动约束和增量滤波的SINS自辅助导航方法，其特征在于，具体方法如下：

(1)驱动AUV做水平方向上的周期性稳态运动，在向指定方向机动的同时，为导航坐标系下的速度添加高频分量；

(2)在完成步骤(1)的机动的基础上，获取安装在水下AUV上的惯性传感器实时数据，具体包括惯性测量元件所输出的陀螺仪和加速度计的输出数据，并利用传感器数据执行姿态解算算法，获得导航坐标系下的速度；

(3)设定数字高通滤波器的参数(f^p，f^s，A^p，A^s)；然后将步骤(2)中获得的导航坐标系下的水平速度输入至高通滤波器中，获得一组剔除了低频误差分量的导航坐标系下的水平速度量；

(4)建立卡尔曼滤波器，将步骤(3)中获得的速度分量的增量作为卡尔曼滤波器的量测量输入至卡尔曼滤波器中，将获得的速度误差量进行补偿，获得自辅助补偿后的高精度定位值。

2.根据权利要求1所述的一种基于机动约束和增量滤波的SINS自辅助导航方法，其特征在于：步骤(1)中导航系高频信息引入的方法如下：

对于AUV，首先保持恒定的前向速度v向前方作匀速直线运动，随后，在保持前向速度为v的条件下，利用舵鳍桨向AUV施加一个恒定的驱动力，使其在前进的同时，以ω的角速度转动t秒，随后，再利用舵鳍桨，向AUV施加相反的驱动力使其以的-ω的角速度向相反方向转动t秒，以此，理想的导航系速度为一个周期为的三角波高频信息。

3.根据权利要求1所述的一种基于机动约束和增量滤波的SINS自辅助导航方法，其特征在于：步骤(3)高通滤波器的选取方法如下：

S3.1：选取有限冲击滤波器IIR滤波器作为数字滤波器，其中需要设置如下几个参数：ω^p代表通带截止圆频率，根据步骤(2)中周期性机动约束的频率来设定，低于周期性机动的频率；ω^s代表阻带截止圆频率，设置为0.0015～0.002；A^p代表通带最大衰减，取0.1～1；A^s代表阻带最小衰减，取40～100；

S3.2：利用高频机动的频率计算取得数字滤波器的时延位数，然后将获得的数据减去时延位数，取得时延补偿后的速度值，其中时延位数N的计算方法如下：

其中，T_sp为采样周期，为滤波器的超前时间，由以下式子计算而得：

其中，为步骤(1)中引入的周期运动的角频率，H(z)为步骤(3)中IIR滤波器的传递函数。

4.根据权利要求1所述的一种基于机动约束和增量滤波的SINS自辅助导航方法，其特征在于：步骤(4)卡尔曼滤波器的量测变量和量测矩阵选取方法如下：

量测变量的选取：

其中，和分别表示经过步骤(3)的数字滤波器之后的获得的东向和北向速度；和分别表示步骤(2)中由惯性原件以及惯性解算算法获得的东向、北向速度；h_SINS表示由惯性解算算法获得的高度信息；h_pressure表示由高度计传感器获得的高度信息。

量测矩阵的选取：

其中F₂₁(k)(1：2，1：2)表示由卡尔曼滤波器k时刻传递矩阵中子矩阵的1～2行以及1～2列组成的新矩阵，Δt为两次卡尔曼滤波的时间间隔。