欢迎来到知嘟嘟! 联系电话:13336804447 卖家免费入驻,海量在线求购! 卖家免费入驻,海量在线求购!
知嘟嘟
我要发布
联系电话:13336804447
知嘟嘟经纪人
收藏
专利号: 2023100696892
申请人: 河海大学
专利类型:发明专利
专利状态:已下证
专利领域: 测量;测试
更新日期:2026-11-16
缴费截止日期: 暂无
价格&联系人
年费信息
委托购买

摘要:

权利要求书:

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时刻的量测转移矩阵。