CN112710304A - 一种基于自适应滤波的水下自主航行器导航方法 - Google Patents

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

Info

Publication number
CN112710304A
CN112710304A CN202011493095.7A CN202011493095A CN112710304A CN 112710304 A CN112710304 A CN 112710304A CN 202011493095 A CN202011493095 A CN 202011493095A CN 112710304 A CN112710304 A CN 112710304A
Authority
CN
China
Prior art keywords
auv
matrix
navigation
filtering
adaptive
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011493095.7A
Other languages
English (en)
Other versions
CN112710304B (zh
Inventor
彭星光
赵艺
宋保维
潘光
张福斌
高剑
李乐
张立川
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202011493095.7A priority Critical patent/CN112710304B/zh
Publication of CN112710304A publication Critical patent/CN112710304A/zh
Application granted granted Critical
Publication of CN112710304B publication Critical patent/CN112710304B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/50—Systems of measurement, based on relative movement of the target
    • G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Acoustics & Sound (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明公开了一种基于自适应滤波的水下自主航行器导航方法，组合导航***由惯性测量单元(IMU)和多普勒测速仪(DVL)构成，首先DVL获取AUV速度信息，IMU获取AUV的角速率和加速度信息，通过积分计算得到体速度和位置信息，将数据做融合处理，利用自适应增益卡尔曼滤波(AEKF)对位置、速度导航参数进行滤波处理，通过输出的参数误差估计值直接校正***输出的导航参数，达到提高导航精度的目的。本发明方法通过调整约束条件，可应用于不同的传感器噪声模型，对水下导航控制具有较好的自适应能力和较强的鲁棒性。

Description

一种基于自适应滤波的水下自主航行器导航方法
技术领域
本发明属于自主航行器技术领域，具体涉及一种自主航行器导航方法。
背景技术
认识、开发和利用海洋是建设海洋强国的基础，在各种海洋技术中，水下自主航行器能在一般潜航器不能达到的水域自主进行工作，被广泛应用在科研考察、水下作业和军事活动中。目前，水声导航***、多普勒速度计程仪(DVL)导航***和惯性导航***(INS)已成为水下自主航行器(AUV)组合导航***的重要组成部分。声学导航***包括长基线***(LBL)、短基线***(SBL)和超短基线***(USBL)。由于水下环境的复杂性，对依靠水下测量方位提出了很大的挑战。与LBL，SBL导航***相比，USBL通过测量AUV在船上信标之间的倾斜距离和方位角来实时定位AUV，被广泛应用于自主式水下机器人的定位和跟踪。
机载传感器输出的数据信号存在误差，为了AUV的精确定位和导航，需要对信号滤波。而用于消除异常误差的绝大多数滤波器，包括中值滤波、均值滤波、卡尔曼滤波(KF)、扩展卡尔曼滤波(EKF)都有自身的局限性。
发明内容
为了克服现有技术的不足，本发明提供了一种基于自适应滤波的水下自主航行器导航方法，组合导航***由惯性测量单元(IMU)和多普勒测速仪(DVL)构成，首先DVL获取AUV速度信息，IMU获取AUV的角速率和加速度信息，通过积分计算得到体速度和位置信息，将数据做融合处理，利用自适应增益卡尔曼滤波(AEKF)对位置、速度导航参数进行滤波处理，通过输出的参数误差估计值直接校正***输出的导航参数，达到提高导航精度的目的。本发明方法通过调整约束条件，可应用于不同的传感器噪声模型，对水下导航控制具有较好的自适应能力和较强的鲁棒性。
本发明解决其技术问题所采用的技术方案包括以下步骤：
步骤1：获取AUV的位置、速度和加速度信息并进行融合，融合结果表示为s1：
Figure BDA0002841305140000011
其中，v1为AUV的速度，由DVL测量得到；a1为AUV的加速度，由IMU利用加速度计测量得到；ρ1为AUV的位置，由v1积分计算得到；
步骤2：利用自适应增益卡尔曼滤波对位置、速度导航参数进行滤波处理；令k＝2；
步骤2-1：计算雅可比矩阵F：
Figure BDA0002841305140000021
Figure BDA0002841305140000022
Figure BDA0002841305140000023
其中，As为状态转移矩阵，
Figure BDA0002841305140000024
为状态转移矩阵As中的状态转移参数矩阵，O3×3为3×3阶的零矩阵，ρ1为AUV的位置，ρ2为AUV经过滤波后的位置；
步骤2-2：状态预测；
Sk＝FSk-1 (5)其中，Sk为在k时刻描述AUV运动量的状态向量，Sk-1为在k-1时刻描述AUV运动量的状态向量；
步骤2-3：协方差矩阵更新；
Pk＝FPk-1FT+Q (6)
其中，Pk为协方差矩阵，Q为过程误差矩阵；
步骤2-4：卡尔曼增益更新；
Figure BDA0002841305140000031
其中，Kgk为卡尔曼增益，R为观测噪声矩阵，Hk为***观测矩阵；
步骤2-5：置信度因子更新；
Figure BDA0002841305140000032
其中，βk表示在k时刻使局部轨迹平滑的置信度因子，βk-1表示在k-1时刻使局部轨迹平滑的置信度因子，Zk为使用来自多个传感器的测量数据构成的观测向量，ws为权重驱动矩阵，η为遗忘因子；
步骤2-6：自适应增益更新；
Figure BDA0002841305140000033
其中，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＝B
Sk-1＝C
步骤2-10：将Sk-1输出到导航***，实现水下自主航行器导航；
步骤2-11：重复执行步骤2-1到步骤2-10，直到达到设定迭代次数。
优选地，所述描述AUV运动量状态向量为15维。
优选地，所述来自多个传感器的测量数据构成观测向量为8维。
本发明的有益效果如下：
1.本发明采用的水下自主航行器导航算法可在有海流的情况下实现水下自主航行器的精确导航控制。
2.本发明方法采用的基于自适应增益卡尔曼滤波的水下自主航行器导航算法通过调整相应的约束条件，可应用于不同的传感器噪声模型，对水下导航控制具有较好的自适应能力和较强的鲁棒性。
3.本发明方法集成了多传感器信息，并建立了用于AUV的组合导航***，该***在定位精度、可靠性和能耗方面表现良好。
附图说明
图1为本发明的水下航行器导航***框图。
图2为本发明的自适应增益卡尔曼滤波算法流程图。
具体实施方式
下面结合附图和实施例对本发明进一步说明。
本发明的AUV组合导航***由惯性测量单元(IMU)和多普勒测速仪(DVL)构成。
如图1所示，本发明提供了一种基于自适应滤波的水下自主航行器导航方法，包括以下步骤：
步骤1：获取AUV的位置、速度和加速度信息并进行融合，融合结果表示为s1：
Figure BDA0002841305140000041
其中，v1为AUV的速度，由DVL测量得到；a1为AUV的加速度，由IMU利用加速度计测量得到；ρ1为AUV的位置，由v1积分计算得到；
步骤2：如图2所示，利用自适应增益卡尔曼滤波对位置、速度导航参数进行滤波处理；令k＝2；
步骤2-1：计算雅可比矩阵F：
Figure BDA0002841305140000042
Figure BDA0002841305140000051
Figure BDA0002841305140000052
其中，As为状态转移矩阵，
Figure BDA0002841305140000053
为状态转移矩阵As中的状态转移参数矩阵，O3×3为3×3阶的零矩阵，ρ1为AUV的位置，ρ2为AUV经过滤波后的位置；
步骤2-2：状态预测；
Sk＝FSk-1 (5)其中，Sk为在k时刻描述AUV运动量的状态向量，Sk-1为在k-1时刻描述AUV运动量的状态向量；
步骤2-3：协方差矩阵更新；
Pk＝FPk-1FT+Q (6)
其中，Pk为协方差矩阵，Q为过程误差矩阵；
步骤2-4：卡尔曼增益更新；
Figure BDA0002841305140000054
其中，Kgk为卡尔曼增益，R为观测噪声矩阵，Hk为***观测矩阵；
步骤2-5：置信度因子更新；
Figure BDA0002841305140000061
其中，βk表示在k时刻使局部轨迹平滑的置信度因子，βk-1表示在k-1时刻使局部轨迹平滑的置信度因子，Zk为使用来自多个传感器的测量数据构成的观测向量，ws为权重驱动矩阵；η为遗忘因子，可以调整历史经验和当前估计的比例；
步骤2-6：自适应增益更新；
Figure BDA0002841305140000062
其中，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＝B
Sk-1＝C
步骤2-10：将Sk-1输出到导航***，实现水下自主航行器导航；
步骤2-11：重复执行步骤2-1到步骤2-10，直到达到设定迭代次数。
优选地，所述描述AUV运动量状态向量为15维。
优选地，所述来自多个传感器的测量数据构成观测向量为8维。

Claims (3)

1.一种基于自适应滤波的水下自主航行器导航方法，其特征在于，包括以下步骤：
步骤1：获取AUV的位置、速度和加速度信息并进行融合，融合结果表示为s1：
Figure FDA0002841305130000011
其中，v1为AUV的速度，由DVL测量得到；a1为AUV的加速度，由IMU利用加速度计测量得到；ρ1为AUV的位置，由v1积分计算得到；
步骤2：利用自适应增益卡尔曼滤波对位置、速度导航参数进行滤波处理；令k＝2；
步骤2-1：计算雅可比矩阵F：
Figure FDA0002841305130000012
Figure FDA0002841305130000013
Figure FDA0002841305130000014
其中，As为状态转移矩阵，
Figure FDA0002841305130000015
为状态转移矩阵As中的状态转移参数矩阵，O3×3为3×3阶的零矩阵，ρ1为AUV的位置，ρ2为AUV经过滤波后的位置；
步骤2-2：状态预测；
Sk＝FSk-1 (5)
其中，Sk为在k时刻描述AUV运动量的状态向量，Sk-1为在k-1时刻描述AUV运动量的状态向量；
步骤2-3：协方差矩阵更新；
Pk＝FPk-1FT+Q (6)
其中，Pk为协方差矩阵，Q为过程误差矩阵；
步骤2-4：卡尔曼增益更新；
Figure FDA0002841305130000021
其中，Kgk为卡尔曼增益，R为观测噪声矩阵，Hk为***观测矩阵；
步骤2-5：置信度因子更新；
Figure FDA0002841305130000022
其中，βk表示在k时刻使局部轨迹平滑的置信度因子，βk-1表示在k-1时刻使局部轨迹平滑的置信度因子，Zk为使用来自多个传感器的测量数据构成的观测向量，ws为权重驱动矩阵，η为遗忘因子；
步骤2-6：自适应增益更新；
Figure FDA0002841305130000023
其中，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＝B
Sk-1＝C
步骤2-10：将Sk-1输出到导航***，实现水下自主航行器导航；
步骤2-11：重复执行步骤2-1到步骤2-10，直到达到设定迭代次数。
2.根据权利要求1所述的一种基于自适应滤波的水下自主航行器导航方法，其特征在于，所述描述AUV运动量状态向量为15维。
3.根据权利要求1所述的一种基于自适应滤波的水下自主航行器导航方法，其特征在于，所述来自多个传感器的测量数据构成观测向量为8维。
CN202011493095.7A 2020-12-17 2020-12-17 一种基于自适应滤波的水下自主航行器导航方法 Active CN112710304B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011493095.7A CN112710304B (zh) 2020-12-17 2020-12-17 一种基于自适应滤波的水下自主航行器导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011493095.7A CN112710304B (zh) 2020-12-17 2020-12-17 一种基于自适应滤波的水下自主航行器导航方法

Publications (2)

Publication Number Publication Date
CN112710304A true CN112710304A (zh) 2021-04-27
CN112710304B CN112710304B (zh) 2022-12-13

Family

ID=75543858

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011493095.7A Active CN112710304B (zh) 2020-12-17 2020-12-17 一种基于自适应滤波的水下自主航行器导航方法

Country Status (1)

Country Link
CN (1) CN112710304B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115657667A (zh) * 2022-10-12 2023-01-31 珠海云洲智能科技股份有限公司 航行器控制方法、装置、终端设备及计算机可读存储介质
CN116592896A (zh) * 2023-07-17 2023-08-15 山东水发黄水东调工程有限公司 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法
CN117367410A (zh) * 2023-12-04 2024-01-09 中南大学 状态估计方法、无人水下航行器及计算机可读存储介质

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6246960B1 (en) * 1998-11-06 2001-06-12 Ching-Fang Lin Enhanced integrated positioning method and system thereof for vehicle
US20050234644A1 (en) * 2004-04-17 2005-10-20 Ching-Fang Lin Positioning and navigation method and system thereof
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
US20120086606A1 (en) * 2010-10-08 2012-04-12 Mathews Michael B Doppler aided inertial navigation
CN103292812A (zh) * 2013-05-10 2013-09-11 哈尔滨工程大学 一种微惯性sins/gps组合导航***的自适应滤波方法
CN103389095A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/多普勒组合导航***的自适应滤波方法
CN110146075A (zh) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） 一种增益补偿自适应滤波的sins/dvl组合定位方法
CN110146076A (zh) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） 一种无逆矩阵自适应滤波的sins/dvl组合定位方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111504324A (zh) * 2020-04-27 2020-08-07 西北工业大学 一种噪声自适应滤波的水下组合导航方法
CN111678520A (zh) * 2020-06-09 2020-09-18 上海理工大学 一种水面无人艇导航***和导航方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6246960B1 (en) * 1998-11-06 2001-06-12 Ching-Fang Lin Enhanced integrated positioning method and system thereof for vehicle
US20050234644A1 (en) * 2004-04-17 2005-10-20 Ching-Fang Lin Positioning and navigation method and system thereof
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
US20120086606A1 (en) * 2010-10-08 2012-04-12 Mathews Michael B Doppler aided inertial navigation
CN103292812A (zh) * 2013-05-10 2013-09-11 哈尔滨工程大学 一种微惯性sins/gps组合导航***的自适应滤波方法
CN103389095A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/多普勒组合导航***的自适应滤波方法
CN110146075A (zh) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） 一种增益补偿自适应滤波的sins/dvl组合定位方法
CN110146076A (zh) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） 一种无逆矩阵自适应滤波的sins/dvl组合定位方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111504324A (zh) * 2020-04-27 2020-08-07 西北工业大学 一种噪声自适应滤波的水下组合导航方法
CN111678520A (zh) * 2020-06-09 2020-09-18 上海理工大学 一种水面无人艇导航***和导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
NAK YONG KO; DABIN JEONG; GYEONGSUB SONG: "Navigation of a Remotely Operated Underwater Vehicle Using IMU and DVL", 《19TH INTERNATIONAL CONFERENCE ON CONTROL, AUTOMATION AND SYSTEMS》 *
唐康华，江明明，翁炬: "基于自适应滤波的水下SINS/相控阵DVL组合导航算法设计", 《中国惯性技术学报》 *
段世梅等: "AUV组合导航***中H_∞滤波技术", 《鱼雷技术》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115657667A (zh) * 2022-10-12 2023-01-31 珠海云洲智能科技股份有限公司 航行器控制方法、装置、终端设备及计算机可读存储介质
CN115657667B (zh) * 2022-10-12 2023-08-29 珠海云洲智能科技股份有限公司 航行器控制方法、装置、终端设备及计算机可读存储介质
CN116592896A (zh) * 2023-07-17 2023-08-15 山东水发黄水东调工程有限公司 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法
CN116592896B (zh) * 2023-07-17 2023-09-29 山东水发黄水东调工程有限公司 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法
CN117367410A (zh) * 2023-12-04 2024-01-09 中南大学 状态估计方法、无人水下航行器及计算机可读存储介质
CN117367410B (zh) * 2023-12-04 2024-03-19 中南大学 状态估计方法、无人水下航行器及计算机可读存储介质

Also Published As

Publication number Publication date
CN112710304B (zh) 2022-12-13

Similar Documents

Publication Publication Date Title
CN112710304B (zh) 一种基于自适应滤波的水下自主航行器导航方法
CN108444478B (zh) 一种用于水下航行器的移动目标视觉位姿估计方法
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111596333B (zh) 一种水下定位导航方法及***
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN109579850B (zh) 基于对水速度辅助惯导的深水智能导航方法
CN110954132A (zh) Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法
CN102323586A (zh) 一种基于海流剖面的uuv辅助导航方法
Bo et al. Cooperative localisation of AUVs based on Huber-based robust algorithm and adaptive noise estimation
CN113252041B (zh) 一种适用于小型水下机器人的组合导航方法
CN108871379B (zh) 一种dvl测速误差在线标定方法
Kinsey et al. Towards in-situ calibration of gyro and doppler navigation sensors for precision underwater vehicle navigation
CN110703205B (zh) 基于自适应无迹卡尔曼滤波的超短基线定位方法
CN114967714A (zh) 一种自主式水下机器人抗扰运动控制方法及***
CN111708008B (zh) 一种基于imu和tof的水下机器人单信标导航方法
CN115014321B (zh) 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法
Ko et al. Synchronous and asynchronous application of a filtering method for underwater robot localization
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法
CN114415655B (zh) 一种基于改进slam的巡检机器人导航控制方法
Quraishi et al. Online kinematic and dynamic parameter estimation for autonomous surface and underwater vehicles
CN113984045A (zh) 水下机器人移动式对接目标运动状态估计方法及***
CN103940416B (zh) 一种电磁计程仪辅助的auv多程序并行解算导航方法
Fu et al. Adaptive Unscented Particle Filter Algorithm for Acoustic Positioning of AUV Docking in Shallow Water
CN117367410B (zh) 状态估计方法、无人水下航行器及计算机可读存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant