1.一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法,SINS/DVL导航的已知量:多普勒计程仪,缩写为DVL,输出的载体系下的三维速度信息 ,其中, 表示载体坐标系下x方向的速度信息, 表示载体坐标系下y方向的速度信息,表示载体坐标系下z方向的速度信息;
捷联惯导系统,缩写为SINS,输出的导航坐标系下的三维速度信息: ,其中, 表示东向速度, 表示北向速度, 表示天向速度;
其特征在于,该方法包括如下步骤:
步骤1.基于状态变换原理构建的组合导航模型构建;
步骤2. 在步骤1的基础上进行滤波状态更新及系统噪声方差阵Qk和量测噪声方差阵Sk的构造;
步骤3. 引入自适应粒子滤波算法,对系统噪声方差阵Qk和量测噪声方差阵Sk进行修正,此过程分为两步,粒子滤波适应度函数构造和自适应粒子群优化算法;
步骤4.进行滤波量测更新。
2.根据权利要求1所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法,其特征在于,步骤1具体包括如下步骤:首先,基于状态变换原理建立SINS/DVL组合导航状态方程:其中, 表示组合导航状态向量, 表示组合导航状态转移矩阵; 表示组合导航系统噪声; 表示15维的组合导航状态向量,可表示如下:其中, 分别表示东向速度误差、北向速度误差和天向速度误差;
分别表示x方向姿态误差、y方向姿态误差和z方向姿态误差; 分别表示经度误差、纬度误差和高度误差; 分别表示x方向加速度计零偏误差、y方向加速度计零偏误差和z方向加速度计零偏误差; 分别表示x方向陀螺仪零偏误差、y方向陀螺仪零偏误差和z方向陀螺仪零偏误差;
组合导航状态转移矩阵 由四部分组成,具体如下:其中, 表示速度误差对应的状态转移子矩阵; 表示姿态误差对应的状态转移子矩阵; 表示位置误差对应的状态转移子矩阵; 表示一个6行15列的零矩阵,其中, 表述如下:其中, 表示地球系相对于惯性系的转动角速度; 表示导航系相对于地球系的旋转角速度; 表示导航坐标系下地球重力信息; 表示载体坐标系到导航坐标系的姿态转移矩阵;符号 表示求矩阵的反对称矩阵;
表述如下:
其中, 只是为了表述方便而定义的中间变量,不代表任何意义,且,的表达式分别为:
表示地球旋转角速率;L和h分别表示载体所在位置的地理纬度和高度;RM和RN分别表示载体所在位置的子午圈半径和卯酉圈半径;
表述如下:
其中, 只是为了表述方便而定义的中间变量,不代表任何意义,且的表达式分别为:
接着,给出SINS/DVL组合导航系统的量测方程:其中, 表示量测转移矩阵; 表示量测噪声矩阵; 表示量测矩阵,可表示如下:其中, 表示导航系到载体系的姿态转移矩阵; 表示DVL输出的载体系下的三维速度信息,量测转移矩阵 可表示如下:。
3.根据权利要求2所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法,其特征在于,步骤2具体包括如下步骤:进行滤波状态更新如下:
其中, 表示k时刻的组合导航状态转移矩阵; 表示k‑1时刻的组合导航状态向量估计值, 表示一步预测状态向量;
状态向量均方误差阵更新如下:
其中, 表示状态向量的均方误差阵; 表示k时刻的系统噪声方差阵, 表示一步预测协方差矩阵,上标T表示矩阵的转置;
基于Huber M估计原理,构造量测噪声方差阵 :其中, 表示量测噪声方差阵。
4.根据权利要求3所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法,其特征在于,步骤3具体包括如下步骤:首先适应度函数构造
将系统噪声方差阵Qk和量测噪声方差阵Sk作为粒子,而SINS/DVL组合导航系统性能主要取决于位置精度,所以将滤波器输出的位置误差的均方差作为适应度函数:其中,f(Qk, Sk)是Qk, Sk为参数时SINS/DVL组合导航系统位置误差的均方差;
为估计的位置值;M 为估计长度; 为参考的实际位置值;
然后进行自适应粒子群优化,具体包括如下过程:初始化粒子群参数,在参数区间内初始化粒子群的位置和速度,随机产生若干粒子并初始化最优粒子;
根据上述适应度函数,对每一个粒子的适应度进行评价,比较其与最优粒子的适应度值,若适应度更好则取代最优粒子;
粒子t=(Qk,Sk)迭代更新自己的速度和位置,每个粒子根据自身和群体经验调整轨迹向最优点靠拢,按以下方程式进行更新:式中,G为当前迭代次数; 为粒子t在第G代的位置, 为粒子t在第G+1代的位置, 为粒子t在第G+1代的速度; 为粒子t第G代为止搜索到的最好位置; 为整个粒子群第G代为止搜索到的最好位置, 为惯性权重; 为学习因子; 为0
1之间的随机数;
~
对惯性权重的改进如下:
其中,ustart和uend分别为惯性权重的初值和末值,其取值范围是:0.4≤uend≤ustart≤
0.9; 为迭代次数的最大值;
重复 和 ,更新最优粒子,直至满足迭代次数或精度要求,产生最优粒子。
5.根据权利要求4所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法,其特征在于,步骤4具体包括如下步骤:进行滤波量测更新:
其中, 表示k时刻的状态向量, 表示k时刻的量测向量, 表示k‑1时刻的状态估计权重; 只是为了表述方便而定义的中间变量,不代表任何意义,具体表述如下:权值矩阵中各元素的取值为:
其中, 表示符号函数; 表示因子;
均方误差阵更新为:
其中, 表示15行15列的单位矩阵; 表示k时刻的滤波增益, 表示k时刻的协方差矩阵, 表示k时刻的量测转移矩阵, 表示k时刻的量测向量矩阵中的第i个元素,表示k时刻矩阵A中的第i个元素, 表示k时刻的量测转移矩阵。