买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

摘要：本发明提供了一种运动约束辅助的基于改进Sage‑Husa自适应滤波的水下组合导航方法，包括：建立多普勒计程仪误差模型；构建基于卡尔曼滤波的水下组合导航系统的状态方程；根据水下潜航器向心加速度和前向速度间的关系建立约束条件，构建完整运动约束模型；建立两个量测方程；建立滤波方程，在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算，当量测噪声发生变化时，采用改进的Sage‑Husa自适应滤波算法进行时间更新、量测更新和滤波更新。本发明能够提高水下组合导航系统的滤波精度，较好地抑制量测干扰时的滤波发散，具有较好的鲁棒性和可靠性。

主权项：1.一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法，其特征在于，包括如下步骤：步骤1，根据多普勒计程仪的工作原理，建立多普勒计程仪误差模型；步骤2，在惯性导航误差方程的基础上，引入多普勒误差模型中速度误差、偏流角误差以及比例系数误差作为水下组合导航的状态量构建基于卡尔曼滤波的水下组合导航系统的状态方程；步骤3，建立载体坐标系，将水下潜航器的运动分解到垂直于载体坐标系Z轴平面和垂直于载体坐标系X轴平面，并根据水下潜航器向心加速度和前向速度间的关系建立约束条件，构建完整运动约束模型；具体包括如下过程：假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0，可得约束条件： 潜航器的任意运动都分解到垂直于z轴平面与垂直于x轴平面，根据运动学公式，可得： 根据惯性器件可得： 由此可得完整的运动约束模型为： 从而得到完整运动约束的误差模型为： 其中，为潜航器在载体坐标系下的速度；为从导航坐标系到载体坐标系的转移矩阵，表示导航坐标系下的姿态角，vn为导航坐标系下潜航器的速度，gn为导航坐标系下的重力加速度，为载体坐标系下陀螺仪x轴和z轴的零偏，arx、arz为潜航器载体坐标系中x和z方向的向心加速度值；为惯性器件x轴和z轴敏感角速度；fxb、fzb为加计x、z轴比力值；分别为地球自转角速度和由载体运动引起的角速度；步骤4，根据惯性器件以及多普勒计程仪的导航信息建立第一量测方程，并根据步骤2中完整运动约束模型建立第二量测方程；步骤5，对状态方程以及量测方程进行离散化处理，结合步骤2和步骤4建立滤波方程，在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算，当量测噪声发生变化时，采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新。

全文数据：

权利要求：

百度查询： 东南大学 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法