一种基于自适应滤波的水下自主航行器导航方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN112710304A(43)申请公布日2021.04.27(21)申请号202011493095.7(22)申请日2020.12.17(71)申请人西北工业大学地址710072陕西省西安市友谊西路127号(72)发明人彭星光赵艺宋保维潘光张福斌高剑李乐张立川(74)专利代理机构西北工业大学专利中心61204代理人金凤(51)Int.Cl.G01C21/16(2006.01)G01S15/58(2006.01)权利要求书2页说明书5页附图1页(54)发明名称一种基于自适应滤波的水下自主航行器导航方法(57)摘要本发明公开了一种基于自适应滤波的水下自主航行器导航方法，组合导航系统由惯性测量单元(IMU)和多普勒测速仪(DVL)构成，首先DVL获取AUV速度信息，IMU获取AUV的角速率和加速度信息，通过积分计算得到体速度和位置信息，将数据做融合处理，利用自适应增益卡尔曼滤波(AEKF)对位置、速度导航参数进行滤波处理，通过输出的参数误差估计值直接校正系统输出的导航参数，达到提高导航精度的目的。本发明方法通过调整约束条件，可应用于不同的传感器噪声模型，对水下导航控制具有较好的自适应能力和较强的鲁棒性。CN112710304ACN112710304A权利要求书1/2页1.一种基于自适应滤波的水下自主航行器导航方法，其特征在于，包括以下步骤：步骤1：获取AUV的位置、速度和加速度信息并进行融合，融合结果表示为s1：其中，v1为AUV的速度，由DVL测量得到；a1为AUV的加速度，由IMU利用加速度计测量得到；ρ1为AUV的位置，由v1积分计算得到；步骤2：利用自适应增益卡尔曼滤波对位置、速度导航参数进行滤波处理；令k＝2；步骤2‑1：计算雅可比矩阵F：ss其中，A为状态转移矩阵，为状态转移矩阵A中的状态转移参数矩阵，O3×3为3×3阶的零矩阵，ρ1为AUV的位置，ρ2为AUV经过滤波后的位置；步骤2‑2：状态预测；Sk＝FSk‑1(5)其中，Sk为在k时刻描述AUV运动量的状态向量，Sk‑1为在k‑1时刻描述AUV运动量的状态向量；步骤2‑3：协方差矩阵更新；TPk＝FPk‑1F+Q(6)其中，Pk为协方差矩阵，Q为过程误差矩阵；步骤2‑4：卡尔曼增益更新；2CN112710304A权利要求书2/2页其中，Kgk为卡尔曼增益，R为观测噪声矩阵，Hk为系统观测矩阵；步骤2‑5：置信度因子更新；其中，βk表示在k时刻使局部轨迹平滑的置信度因子，βk‑1表示在k‑1时刻使局部轨迹平s滑的置信度因子，Zk为使用来自多个传感器的测量数据构成的观测向量，w为权重驱动矩阵，η为遗忘因子；步骤2‑6：自适应增益更新；其中，Kk为自适应增益，τ和c为滤波器系数，α为可调整的初始误差；步骤2‑7：协方差矩阵迭代；B＝(I15‑KgkHk)Pk(11)其中B为过渡变量；步骤2‑8：状态估计；C＝Sk+KkKgk[Zk‑HkSk](10)其中C为过渡变量；步骤2‑9：令k加1；Pk‑1＝BSk‑1＝C步骤2‑10：将Sk‑1输出到导航系统，实现水下自主航行器导航；步骤2‑11：重复执行步骤2‑1到步骤2‑10，直到达到设定迭代次数。2.根据权利要求1所述的一种基于自适应滤波的水下自主航行器导航方法，其特征在于，所述描述AUV运动量状态向量为15维。3.根据权利要求1所述的一种基于自适应滤波的水下自主航行器导航方法，其特征在于，所述来自多个传感器的测量数据构成观测向量为8维。3CN112710304A说明书1/5页一种基于自适应滤波的水下自主航行器导航方法技术领域[0001]本发明属于自主航行器技术领域，具体涉及一种自主航行器导航方法。背景技术[0002]认识、开发和利用海洋是建设海洋强国的基础，在各种海洋技术中，水下自主航行器能在一般潜航器不能达到的水域自主进行工作，被广泛应用在科研考察、水下作业和军事活动中。目前，水声导航系统、多普勒速度计程仪(DVL)导航系统和惯性导航系统(INS)已成为水下自主航行器(AUV)组合导航系统的重要组成部分。声学导航系统包括长基线系统(LBL)、短基线系统(SBL)和超短基线系统(USBL)。由于水下环境的复杂性，对依靠水下测量方位提出了很大的挑战。与LBL，SBL导航系统相比，USBL通过测量AUV在船上信标之间的倾斜距离和方位角来实时定位AUV，被广泛应用于自主式水下机器人的定位和跟踪。[0003]机载传感器输出的数据信号存在误差，为了AUV的精确定位和导航，需要对信号滤波。而用于消除异常误差的绝大多数滤波器，包括中值滤波、均值滤波、卡尔曼滤波(KF)、扩展卡尔曼滤波(EKF)都有自身的局限性。发明内容[0004]为了克服现有技术的不足，本发明