CN112710304B - 一种基于自适应滤波的水下自主航行器导航方法 - Google Patents
一种基于自适应滤波的水下自主航行器导航方法 Download PDFInfo
- Publication number
- CN112710304B CN112710304B CN202011493095.7A CN202011493095A CN112710304B CN 112710304 B CN112710304 B CN 112710304B CN 202011493095 A CN202011493095 A CN 202011493095A CN 112710304 B CN112710304 B CN 112710304B
- Authority
- CN
- China
- Prior art keywords
- auv
- matrix
- navigation
- adaptive
- filtering
- 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.)
- Active
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:
其中,v1为AUV的速度,由DVL测量得到;a1为AUV的加速度,由IMU利用加速度计测量得到;ρ1为AUV的位置,由v1积分计算得到;
步骤2:利用自适应增益卡尔曼滤波对位置、速度导航参数进行滤波处理;令k=2;
步骤2-1:计算雅可比矩阵F:
步骤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:卡尔曼增益更新;
其中,Kgk为卡尔曼增益,R为观测噪声矩阵,Hk为***观测矩阵;
步骤2-5:置信度因子更新;
其中,βk表示在k时刻使局部轨迹平滑的置信度因子,βk-1表示在k-1时刻使局部轨迹平滑的置信度因子,Zk为使用来自多个传感器的测量数据构成的观测向量,ws为权重驱动矩阵,η为遗忘因子;
步骤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=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:
其中,v1为AUV的速度,由DVL测量得到;a1为AUV的加速度,由IMU利用加速度计测量得到;ρ1为AUV的位置,由v1积分计算得到;
步骤2:如图2所示,利用自适应增益卡尔曼滤波对位置、速度导航参数进行滤波处理;令k=2;
步骤2-1:计算雅可比矩阵F:
步骤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:卡尔曼增益更新;
其中,Kgk为卡尔曼增益,R为观测噪声矩阵,Hk为***观测矩阵;
步骤2-5:置信度因子更新;
其中,βk表示在k时刻使局部轨迹平滑的置信度因子,βk-1表示在k-1时刻使局部轨迹平滑的置信度因子,Zk为使用来自多个传感器的测量数据构成的观测向量,ws为权重驱动矩阵;η为遗忘因子,可以调整历史经验和当前估计的比例;
步骤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=B
Sk-1=C
步骤2-10:将Sk-1输出到导航***,实现水下自主航行器导航;
步骤2-11:重复执行步骤2-1到步骤2-10,直到达到设定迭代次数。
优选地,所述描述AUV运动量的状态向量为15维。
优选地,所述来自多个传感器的测量数据构成的观测向量为8维。
Claims (3)
1.一种基于自适应滤波的水下自主航行器导航方法,其特征在于,包括以下步骤:
步骤1:获取AUV的位置、速度和加速度信息并进行融合,融合结果表示为s1:
其中,v1为AUV的速度,由DVL测量得到;a1为AUV的加速度,由IMU利用加速度计测量得到;ρ1为AUV的位置,由v1积分计算得到;
步骤2:利用自适应增益卡尔曼滤波对位置、速度导航参数进行滤波处理;令k=2;
步骤2-1:计算雅可比矩阵F:
步骤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:卡尔曼增益更新;
其中,Kgk为卡尔曼增益,R为观测噪声矩阵,Hk为***观测矩阵;
步骤2-5:置信度因子更新;
其中,βk表示在k时刻使局部轨迹平滑的置信度因子,βk-1表示在k-1时刻使局部轨迹平滑的置信度因子,Zk为使用来自多个传感器的测量数据构成的观测向量,ws为权重驱动矩阵,η为遗忘因子;
步骤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=B
Sk-1=C
步骤2-10:将Sk-1输出到导航***,实现水下自主航行器导航;
步骤2-11:重复执行步骤2-1到步骤2-10,直到达到设定迭代次数。
2.根据权利要求1所述的一种基于自适应滤波的水下自主航行器导航方法,其特征在于,所述描述AUV运动量的状态向量为15维。
3.根据权利要求1所述的一种基于自适应滤波的水下自主航行器导航方法,其特征在于,所述来自多个传感器的测量数据构成的观测向量为8维。
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 CN112710304A (zh) | 2021-04-27 |
CN112710304B true 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) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115657667B (zh) * | 2022-10-12 | 2023-08-29 | 珠海云洲智能科技股份有限公司 | 航行器控制方法、装置、终端设备及计算机可读存储介质 |
CN116592896B (zh) * | 2023-07-17 | 2023-09-29 | 山东水发黄水东调工程有限公司 | 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法 |
CN117367410B (zh) * | 2023-12-04 | 2024-03-19 | 中南大学 | 状态估计方法、无人水下航行器及计算机可读存储介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
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 | 西北工业大学 | 一种噪声自适应滤波的水下组合导航方法 |
Family Cites Families (5)
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 |
US7409290B2 (en) * | 2004-04-17 | 2008-08-05 | American Gnc Corporation | 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 |
CA2814009A1 (en) * | 2010-10-08 | 2012-04-12 | Loctronix Corporation | Doppler aided inertial navigation |
CN111678520A (zh) * | 2020-06-09 | 2020-09-18 | 上海理工大学 | 一种水面无人艇导航***和导航方法 |
-
2020
- 2020-12-17 CN CN202011493095.7A patent/CN112710304B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
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 | 西北工业大学 | 一种噪声自适应滤波的水下组合导航方法 |
Non-Patent Citations (2)
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》.2020,全文. * |
基于自适应滤波的水下SINS/相控阵DVL组合导航算法设计;唐康华,江明明,翁炬;《中国惯性技术学报》;20130228;第21卷(第1期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN112710304A (zh) | 2021-04-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112710304B (zh) | 一种基于自适应滤波的水下自主航行器导航方法 | |
CN108444478B (zh) | 一种用于水下航行器的移动目标视觉位姿估计方法 | |
CN109737956B (zh) | 一种基于双应答器的sins/usbl相位差紧组合导航定位方法 | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN111596333B (zh) | 一种水下定位导航方法及*** | |
CN106444806A (zh) | 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法 | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
CN109579850B (zh) | 基于对水速度辅助惯导的深水智能导航方法 | |
CN111026135B (zh) | 一种无人艇高性能航行前馈控制***及其控制方法 | |
CN110954132A (zh) | Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法 | |
CN102323586A (zh) | 一种基于海流剖面的uuv辅助导航方法 | |
CN109739088B (zh) | 一种无人船有限时间收敛状态观测器及其设计方法 | |
Bo et al. | Cooperative localisation of AUVs based on Huber-based robust algorithm and adaptive noise estimation | |
CN113252041B (zh) | 一种适用于小型水下机器人的组合导航方法 | |
CN104316025A (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的水下机器人单信标导航方法 | |
Wang et al. | Modelling of a surface vessel from free running test using low cost sensors | |
Ko et al. | Synchronous and asynchronous application of a filtering method for underwater robot localization | |
CN112947068B (zh) | 一种自主水下航行器一体化声学定位与跟踪控制方法 | |
CN113989327A (zh) | 一种基于卷积神经网络粒子滤波算法的单uuv目标状态估计方法 | |
CN103940416B (zh) | 一种电磁计程仪辅助的auv多程序并行解算导航方法 |
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 |