CN117848339A - 一种小型无人自主航行器的水面水下组合导航方法与装置 - Google Patents

一种小型无人自主航行器的水面水下组合导航方法与装置 Download PDF

Info

Publication number
CN117848339A
CN117848339A CN202311726803.0A CN202311726803A CN117848339A CN 117848339 A CN117848339 A CN 117848339A CN 202311726803 A CN202311726803 A CN 202311726803A CN 117848339 A CN117848339 A CN 117848339A
Authority
CN
China
Prior art keywords
ins
nss
navigation
underwater
error
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.)
Pending
Application number
CN202311726803.0A
Other languages
English (en)
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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202311726803.0A priority Critical patent/CN117848339A/zh
Publication of CN117848339A publication Critical patent/CN117848339A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Operations Research (AREA)
  • Navigation (AREA)

Abstract

本发明涉及水下机器人技术，旨在提供一种小型无人自主航行器的水面水下组合导航方法与装置。该方法是在S‑ASUV上同时搭载惯性导航系统INS和卫星导航系统NSS；当在水面工作时，采用INS/NSS松组合导航模式，INS和NSS各自独立工作；在NSS接收机正常工作状态下，利用NSS进行实时定位和导航；当无法接收到NSS信号时，采用INS/模型辅助导航模式，待恢复信号接收后再利用NSS信号进行位置校准和导航；在水下工作时，采用INS/模型辅助导航模式，即在不增加硬件的前提下，采用卡尔曼滤波器将INS和S‑ASUV的动力学模型融合以实现辅助导航。本发明步骤简便，成本较低；稳定可靠，结果准确；应用范围广，可以同时应用于各种形状的水面和水下航行器的导航控制。

Description

一种小型无人自主航行器的水面水下组合导航方法与装置
技术领域
本发明涉及水下机器人技术领域，尤其涉及一种小型的无人水面水下自主航行器(S-ASUV)的水面水下组合导航方法与装置。
背景技术的
近年来随着对江河湖海资源的开发和利用逐渐增加，对于能够在湖泊、河道、近海、引水涵洞等浅水区域作业的小型无人水面/水下自主航行器(S-ASUV)的需求也日益增多。目前现有的航行器体积较大，一般只能单独适用于水面或水下航行，且价格昂贵，不能满足小型河道、湖泊或涵洞的使用要求。S-ASUV是一种具有两栖能力的小型航行器，可在水面也可在水下自主航行。然而，受体积和造价限制，S-ASUV不适于搭载高精度、大体积导航定位传感器。此外，传统定位导航方法也无法同时适用于水面和水下的应用场景，如何实现导航成为S-ASUV研究要解决的关键问题之一。
惯性导航系统(Inertial Navigation System，简称INS)是目前应用最广泛的导航技术。但INS的导航误差会随时间累积，所以一般INS会配合其他导航传感器，例如全球卫星系统(Global Positioning System，简称GPS)、北斗卫星导航系统(BeiDou NavigationSatellite System，简称BDS)、多普勒计程仪(Doppler Velocity Log，简称DVL)、声学多普勒流速剖面仪(Acoustic Doppler Current Profilers，简称ADCP)等构成组合导航系统，综合多种导航信息，从而得到更准确的导航结果。而对于S-ASUV来说，由于价格和体型限制导致无法安装大型和高精度的INS、NSS、DVL和ADCP等，只能安装成本较低、体积较小、精度较低的导航定位传感器。因此，在导航过程中以何种方式去利用各类导航数据，成为实现可靠的S-ASUV导航方法的重要研究内容。
发明内容
本发明要解决的技术问题是，克服现有技术中的不足，提供小型无人自主航行器的水面水下组合导航方法与装置。
为解决技术问题，本发明的解决方案是：
提供一种小型无人自主航行器的水面水下组合导航方法，是在小型无人水面/水下自主航行器(S-ASUV)上同时搭载惯性导航系统INS和卫星导航系统NSS，所述卫星导航系统NSS是指全球定位系统GPS或北斗卫星导航系统BDS中的至少一种；根据S-ASUV工作场景的不同，将其导航模式分为两类：
(1)当S-ASUV在水面工作时，采用INS/NSS松组合导航模式，INS和NSS各自独立工作；在NSS接收机正常工作状态下，利用NSS进行实时定位和导航，把INS和NSS的位置、速度差作为量测值，通过卡尔曼滤波器给出最优估计结果，最后反馈给INS进行校正；当无法接收到NSS信号时，采用INS/模型辅助导航模式，待恢复信号接收后再利用NSS信号进行位置校准和导航；
(2)当S-ASUV在水下工作时，采用INS/模型辅助导航模式，即在不增加硬件的前提下，采用卡尔曼滤波器将INS和S-ASUV的动力学模型融合以实现辅助导航。
发明原理描述：
本发明中，当S-ASUV在水面工作时采用INS/NSS松组合导航模式，INS和NSS(GPS或北斗)各自独自工作。通过INS和GPS(或北斗)记录的S-ASUV航行状态信息计算并输出预测航行状态信息(包括S-ASUV的姿态、方位、速度、地理坐标)。在GPS(或北斗)接收机能够接收信号状态下，通过GPS(或北斗)进行定位，并将INS和NSS的位置、速度差作为量测值，通过卡尔曼滤波器给出最优估计结果，最后反馈给INS进行闭环校正；在GPS(或北斗)接收机无法接收信号状态下，利用INS/水面动力学模型进行短期导航，利用预先建立的S-ASUV的水面动力学模型辅助校准INS的导航误差；当GPS(或北斗)恢复信号接收后或利用水下声学定位系统再进行位置校准。当S-ASUV在水下工作时，采用INS/水下动力学模型辅助导航模式，利用预先建立的S-ASUV的水下动力学模型辅助校准INS的导航误差。其中，在水面和水下组合导航模式中，S-ASUV的预测位置、预测速度、预测姿态通过INS更新方程得到。
与现有技术相比，本发明的有益效果在于：
1、本发明步骤简便，成本较低。针对小型水面/水下自主航行器无法搭载高精度、高成本导航设备的问题，本发明基于水面航行模式给出了仅依靠低精度INS和NSS即可得到位置、速度、姿态的状态观测器，基于水下航行模式给出了仅依靠INS即可得到位置、速度、姿态的状态观测器，从而实现水面和水下不同环境下的导航。
2、本发明稳定可靠，结果准确。针对NSS可能由于地形的原因丢失信号或因为本身的定位精度在定位上有偏差而产生位置观测离群值的问题，本发明设计了增益自适应卡尔曼滤波器，同时给出了卡尔曼滤波结合INS和NSS的组合导航算法；针对INS测量误差随时间累计的问题，给出了卡尔曼滤波结合INS和动力学模型的组合导航算法。
3、本发明应用范围广，可以同时应用于各种形状的水面和水下航行器的导航控制。
附图说明
图1是本发明方法的流程示意图。
图2是水面工作时以INS/NSS组合导航的流程图。
图3是水下工作时通过建立动力学模型辅助校准INS导航的流程图。
图4为水面直线NSS导航航迹图。
图5为模拟NSS信号丢失和出现大范围离群值时的NSS导航航迹图。
图6为水面直线INS/NSS组合导航航迹图。
图7为S-ASUV结构示意图。
图8为水下INS/动力学模型组合导航航迹图。
具体实施方式
首先需要说明的是，本发明涉及水下机器人技术、水动力建模技术、控制技术、导航技术和定位技术。申请人认为，如在仔细阅读申请文件、准确理解本发明的实现原理和发明目的以后，在结合现有公知技术的情况下，本领域技术人员完全可以运用其掌握的软件编程技能和算法修改实现本发明。在本发明的实现过程中，会涉及到水面/水下机器人的应用；前述水面/水下机器人包括但不限于：水面机器人、水面航行器、水下机器人、水下航行器、潜水器、潜航器、深潜器、水下自走式航行器等；前述滤波方程包括但不限于：卡尔曼滤波、扩展卡尔曼滤波、粒子滤波、最小二乘滤波、无色卡尔曼滤波、无迹卡尔曼滤波、自适应卡尔曼滤波器、贝叶斯滤波等，凡本发明申请文件提及的均属此范畴，申请人不再一一列举。
下面结合附图进行进一步详细说明：
本发明小型无人水面/水下自主航行器(S-ASUV)，除了能够适应水面水下航行的常规艇身结构及部件，还包括装载于艇身上的下述设备或器件：数字信号处理器(DSP)、现场可编程门阵列(FPGA)、单片机、NSS接收机、惯性导航系统(INS)、测距器、水下摄像头和深度计；其中，数字信号处理器，用于处理水下摄像头采集的水下视频信息，并将处理后的环境特征地图传输给现场可编程门阵列进行运算；单片机用于采集测距器、深度计、INS、NSS的数据，并将处理后的数据传输给现场可编程门阵列进行运算；现场可编程门阵列用于加载并执行用于实现以软件程序的方式运行的水面水下组合导航方法，并输出S-ASUV的位置、速度和姿态信息。
所述NSS均指卫星导航系统，英文全称Navigation Satellite System，是指全球定位系统GPS或北斗卫星导航系统BDS中的至少一种。基于GPS(或北斗)的定位芯片属于成熟商品，可直接市购获得并按常规方式安装、调试，本发明无特别要求。
本发明中，当S-ASUV在水下工作时所述用于辅助INS导航的动力学模型，可以在不增加硬件的前提下事先构建。动力学模型的构建方式可由本领域技术人员根据现有技术的记载加以实现，本发明不做具体限制。例如，本发明是依据Fossen所著《Marine ControlSystem》中第一章相关推导内容，获得水下航行器矩阵形式下的动力学模型。
S-ASUV上的导航装置由卫星导航系统NSS、INS、电子罗盘(或磁罗盘)、摄像头、深度计、测距器、声信标、动力学模型和推进器模型组成。卫星导航系统NSS的天线用硬质杆支撑，硬质杆底座固定于S-ASUV背部；天线与S-ASUV通过硬质杆内的屏蔽信号线相连，所述硬质杆长度至少20厘米，以确保卫星导航信号的稳定性。
本发明所述的水面/水下组合导航方法，具体包括以下内容：
1、INS/NSS松组合导航模式
(1)当S-ASUV在水面工作时采用INS/NSS松组合导航模式，松组合是指INS和NSS各自独立工作。即：在NSS正常工作状态下，通过NSS接收机进行实时定位。
(2)当无法接收到NSS信号时，利用惯性导航系统(INS)姿态更新算法计算的预测姿态、预测速度、预测位置，利用预先建立的S-ASUV的水面动力学模型辅助校准INS的导航误差以用于短期导航；在恢复NSS信号接收后再进行位置校准，继续使用INS/NSS组合导航。
(2.1)当无法接收到NSS信号时，首先通过S-ASUV搭载的陀螺仪测得的角速度和上一时刻的姿态计算S-ASUV的预测姿态，姿态更新方程为：
式(1)中，
Δθk＝|Δθk| (3)
表示k时刻的姿态四元数，表示从k-1时刻到k时刻的姿态四元数变化，Δθk是陀螺仪在k-1时刻输出的角度增量，可以使用陀螺仪在k-1时刻的角速率输出乘以Δt得到角度增量。
(2.2)接着通过速度更新方程计算S-ASUV的预测速度：
式(4)中，
表示k时刻的惯导速度，为姿态四元数对应的姿态转换矩阵，Δvk是加速度计在单位时间内的比力增量，可采用k-1时刻的比力输出乘以采样周期计算得到。
(2.3)最后通过位置更新方程计算S-ASUV的预测位置：
式(6)中，
其中，表示k时刻的坐标，RM，RN，L，h分别表示地球子午圈主曲率半径、地球卯酉圈主曲率半径、地理纬度和高度。
(3)当S-ASUV在水面工作处于INS/NSS松组合导航模式下；在NSS正常工作时，通过INS和NSS记录的S-ASUV航行状态信息计算并输出预测的航行状态信息(包括S-ASUV的姿态、方位、速度、位置地理坐标)；然后把INS和NSS的位置和速度差值作为量测值，采用卡尔曼滤波器融合INS和NSS的方法给出最优估计结果；然后反馈给INS进行闭环校正，随时为进入INS/模型辅助导航模式做好准备。
所述卡尔曼滤波器为增益自适应卡尔曼滤波器，在观测状态下选择INS输出和辅助导航系统输出的差值，在水面运动时辅助导航系统为NSS导航系统，在水下运动时辅助导航系统为水下动力学模型导航系统。
(3.1)设计增益自适应卡尔曼滤波器
首先建立离散系统状态空间模型
Xk＝Φk，k-1Xk-1+Γk-1Wk-1 (8)
Zk＝HkXk+Vk (9)
其中，Xk为系统k时刻的状态向量，Zk为k时刻的观测向量，Φk，k-1为k-1到k时刻的系统状态转移矩阵，Wk-1为k-1时刻的系统噪声向量，Γk-1为k-1时刻的系统噪声转移矩阵，Hk为k时刻的观测矩阵，Vk为k时刻的观测噪声矩阵向量。
接着构造预测方程，在系统噪声未知的情况下，根据式(9)可以利用k-1时刻的状态估计量来预测k时刻的状态量，即
式(10)中，是k-1时刻的卡尔曼滤波估计值。
接着构造预测均方误差
式(11)中，Qk-1是k-1时刻系统噪声方差阵，Pk-1是的均方误差阵，即
接着设计卡尔曼滤波增益，为了改善NSS由于地形的原因丢失信号或因为本身的定位精度在定位上有偏差而产生位置观测离群值导致导航结果大范围偏移的情况，将普通卡尔曼滤波器改进为自适应卡尔曼滤波器，增加置信度函数ηk
式(13)中，Kk是k时刻的卡尔曼滤波增益，Rk是k时刻观测噪声方差阵，自适应置信度函数ηk计算方式如下
式(14)中，α和β分别是INS数据可信度和辅助导航方式数据可信度，且0<α，β<1
接着构造估计方程，在预测的基础上结合k时刻观测量Zk和滤波增益可计算出k时刻的卡尔曼滤波估计
接着计算估计均方误差，k时刻估计的均方误差为
Pk＝(I-KkHk)Pk|k-1 (16)
(3.2)INS融合NSS卡尔曼滤波器状态方程和量测方程为：
式(17)中，状态量X1选取INS的三维姿态误差(即式18的第1-3个分量)；三维速度误差(即式18的第4-6个分量)；三维位置误差δ(即式18的第7-9个分量)；三维陀螺仪的马尔可夫过程误差εrx、εry、εrz(即式18的第10-12个分量)和三维加速度计的马尔可夫过程误差(即式18的第13-15个分量)；
观测量Z1选取INS和电子罗盘的姿态误差、INS和NSS的速度、位置误差作为观测量：
式(19)中，ΘI，υI，pI分别表示INS更新得到的姿态、速度和位置向量；ΘMG，υG，pG分别表示电子罗盘姿态、NSS速度和位置。
式(17)中的状态转移矩阵F1表示为
预测噪声W1表示为
式(21)中，wg，wa分别表示陀螺仪角速率白噪声和加速度计比力白噪声，wrg，wra分别表示陀螺仪角速率和加速度计比力马尔可夫过程驱动白噪声。
式(17)中噪声转移矩阵G1表示为
观测矩阵H1表示为
H1＝[I9×9 09×6] (23)
观测噪声V1表示为
式(24)中，wMG，wgv，wgp分别表示电子罗盘姿态观测白噪声，NSS速度、位置观测白噪声。
(3.2)为了减小测量误差量、尽量增加导航精度，在设计组合导航算法前先建立INS误差方程：
式(25)中三个方程分别表示用于计算姿态、速度和位置的误差方程；
其中，姿态解算误差为速度误差δvn＝[δvN δvE δvD]T，位置误差δpn＝[δpN δpE δpD]T。
(3.3)在系统采样周期Δt小于预设阈值τ时，得到INS融合NSS卡尔曼滤波器状态方程和量测方程的离散化表达式为：
式(26)中，和分别表示k时刻INS/NSS卡尔曼滤波器的状态预测值和观测值。
2、INS/水下模型辅助导航模式
当S-ASUV在水下工作时，采用INS/模型辅助导航模式，利用预先建立的动力学模型校准INS导航误差，如图3所示。
(1)INS融合动力学模型卡尔曼滤波器状态方程和量测方程为
式(27)中，状态量X2选取INS的三维姿态误差(即式18-2的第1-3个分量)；三维速度误差(即式28第4-6个分量)；三维位置误差(即式28第7-9个分量)；三维陀螺仪的马尔可夫过程误差εrx、εry、εrz(即式28的第10-12个分量)；三维加速度计的马尔可夫过程误差 (即式28第13-15个分量)；
观测量Z2选取INS和电子罗盘的姿态误差、INS和动力学模型输出的速度误差、INS和动力学模型输出的北向和东向位置误差、INS和深度计的垂直误差：
式(29)中，vM表示动力学模型解算的导航坐标系速度，分别表示INS得到的NED坐标系位置，表示动力学模型解算的北向位置和东向位置，d是通过深度计得到的深度。
式(27)中，F2，G2，W2，H2中的各元素与F1，G1，W1，H1一致，观测噪声V2表示为
式(30)中，wMG，wmv分别表示电子罗盘姿态观测白噪声、动力学模型解算速度均方误差,wd分别表示动力学模型解算的北向和东向位置均方误差、深度计观测白噪声。
(2)在系统采样周期Δt小于预设阈值τ时，得到INS融合动力学模型卡尔曼滤波器状态方程和量测方程的离散化表达式为
式(31)中，和分别表示k时刻INS/动力学模型卡尔曼滤波器的状态预测值和观测值。
即使预先建立的动力学模型不够精确导致导航误差较大，本发明还可以在S-ASUV航行时利用水下摄像头采集水底环境特征，通过环境图像建立水下环境特征地图，并通过与INS信息融合获得S-ASUV自身位置以用于水下导航。具体包括：利用摄像机采集的水下图像信息，计算S-AUV的姿态和位置，并初始化AUV地图；利用ORB算法提取相邻图像特征点，并对其进行匹配，定义匹配完成的特征集；优化ORB特征匹配；利用本质矩阵、单应矩阵或PnP方法，求解S-AUV位姿。
此外，本发明还可选择使用测距器测量S-AUV到水下各个声信标的距离。利用三个及以上声信标的距离，通过三角关系解算出S-AUV的位置。解算出的位置在卡尔曼滤波器量测方程中进行融合，求得北向东向位置误差，以提高S-AUV的导航精度。
下面结合附图中的具体示例，对本发明做进一步详细说明：
需要指出的是，本发明所提出的方案适用于任何使用INS和NSS的水面/水下无人航行器的导航，此处仅为了叙述便利，选择较有代表性的对象以作说明。
本实例中S-ASUV水面INS/NSS组合导航方法，包括以下步骤：
步骤一，当S-ASUV在水面运动且NSS正常工作时，通过惯性导航系统(INS)和NSS记录的S-ASUV航行状态信息计算并输出预测的航行状态信息(包括S-ASUV的姿态、方位、速度、位置地理坐标)。
步骤二，把INS和NSS的位置和速度差值作为量测值，采用卡尔曼滤波器融合INS和NSS的方法给出最优估计结果。
步骤三，设计增益自适应卡尔曼滤波器。
首先建立卡尔曼滤波器通用离散系统状态空间模型
Xk＝Φk，k-1Xk-1+Γk-1Wk-1
Zk＝HkXk+Vk
其中，Xk为系统k时刻的状态向量，Zk为k时刻的观测向量，Φk，k-1为k-1到k时刻的系统状态转移矩阵，Wk-1为k-1时刻的系统噪声向量，Γk-1为k-1时刻的系统噪声转移矩阵，Hk为k时刻的观测矩阵，Vk为k时刻的观测噪声矩阵向量。
接着构造预测方程，在系统噪声未知的情况下，利用k-1时刻的状态估计量来预测k时刻的状态量，即
其中，是k-1时刻的卡尔曼滤波估计值。
接着构造预测均方误差
其中，Qk-1是k-1时刻系统噪声方差阵，Pk-1是的均方误差阵，即
接着设计卡尔曼滤波增益，为了改善NSS由于地形的原因丢失信号或因为本身的定位精度在定位上有偏差而产生位置观测离群值导致导航结果大范围偏移的情况，将普通卡尔曼滤波器改进为自适应卡尔曼滤波器，增加置信度函数ηk
式(13)中，Kk是k时刻的卡尔曼滤波增益，Rk是k时刻观测噪声方差阵，自适应置信度函数ηk计算方式如下
其中，α和β分别是INS数据可信度和辅助导航方式数据可信度，且0<α，β<1。本实例中自适应卡尔曼滤波器的置信度函数选取参考值α＝0.95，β＝0.98。
接着构造估计方程，在预测的基础上结合k时刻观测量Zk和滤波增益可计算出k时刻的卡尔曼滤波估计
接着计算估计均方误差，k时刻估计的均方误差为
Pk＝(I-KkHk)Pk|k-1
步骤四，反馈给INS进行闭环校正。
步骤五，当NSS信号丢失时，利用INS姿态更新算法计算预测的姿态、速度、位置，进行短期导航。
步骤六，NSS恢复信号接收后再进行位置校准，继续使用INS/NSS组合导航方法。
图4为水面直线NSS导航航迹图，图5为模拟NSS信号丢失和出现大范围离群值时的NSS导航航迹图，图6为水面直线INS/NSS组合导航航迹图。对比发现，NSS短暂丢失信号期间或NSS定位精度下降时，INS/NSS组合导航模式加上增益自适应卡尔曼滤波器能够缓解异常定位导致的航迹大范围波动的问题，提高水面导航精度和导航鲁棒性。
INS/水面动力学模型组合导航和INS/水力动力学模型组合导航实施步骤相近，不同之处在于水面工作方式下采用水面动力学模型。以INS/水下动力学模型组合导航为例，本实例中S-ASUV的INS/水下动力学模型组合导航方法，包括以下步骤：
步骤一，当S-ASUV在水下运动时，通过INS数据和在NSS恢复接收时或水下声学定位系统定位时记录的S-ASUV航行状态信息计算并输出预测的航行状态信息(包括S-ASUV的姿态、方位、速度、位置地理坐标)。
步骤二，建立S-ASUV动力学模型，图7为S-ASUV结构图。通用动力学模型，通过公式
其中η为广义坐标向量，J(η)为地坐标系和机体坐标系转换矩阵，v为速度和角速度向量，M为总质量矩阵，C(v)为科里奥利系数矩阵，D(v)为阻力系数矩阵，g(η)为恢复力和力矩向量，τ为系统输入力和力矩，w为系统内部和外部总干扰。利用上述通用动力学模型对S-ASUV的预测位置、速度、姿态进行更新。
在本实例中，考虑计算简便的需要，给出一种建立动力学模型线性化更新方程的方法。在遵循S-ASUV运动规律的前提下，提出以下假设：
假设1：S-ASUV运动过程中俯仰角和横滚角很小，并且此时旋转速度也很小。
基于此假设，可以简化为
式中
假设2：重心，浮心和原点O都在Ozb轴上，浮心和O重合，整体表现出微正浮力。则有
g(η)＝g＝[0，0，B-W，0，0，0]T
假设3：S-ASUV外形近似球形，具有三个对称面，其质量分布较均匀，对称性较好。
基于此假设，惯性积Ixy＝Ixz＝Iyz＝0。三个轴的转动惯量
Ix＝Iy＝Iz＝2mr2/5
进一步地，质量矩阵可以简化为
假设4：S-ASUV未加外部载荷，且在水下以低速运动。
基于此假设，可忽略模型中的状态二阶项。则有
C(v)=0
D(v)＝D＝-diag{Xu，Yv，Zw，Kp，Mq，Nr}
S-ASUV最终动力学模型如下
式中
D(v)＝diag{0.7318,0.7318,0.3747,0.0014,0.0014,0.0067}
g＝[0，0，9.7311，0，0，0]T
其中，t＝[f(p1)，f(p2)，f(p3)，f(p4)，f(p5)，f(p6)]T，表示控制量和螺旋桨推力之间的关系，可以利用螺旋桨推力测量方法得到。
步骤三，建立动力学模型更新方程。设动力学模型状态量为x，令
动力学模型状态微分方程
式中，u是模型输入量，z是外部干扰，分别表示为
u＝t，z＝w-g
各系数矩阵
在Δt的更新频率下得到动力学模型的离散更新方程
xk+1＝(I+AΔt)xk+HΔtuk+EΔtzk
上述更新方程初始值是在NSS恢复接收时或水下声学定位系统定位时记录的位置、速度数据和电子罗盘记录的航向、姿态数据以及深度计记录的深度数据。通过不断迭代获得每一时刻的机体坐标系速度vb和导航坐标系位置pn等导航参数，通过姿态转换矩阵把vb转换为导航坐标系下速度vM，即
步骤四，把INS和通过动力学模型计算得到的位置和速度差值作为量测值，采用卡尔曼滤波器融合INS和NSS的方法给出最优估计结果。
步骤五，设计增益自适应卡尔曼滤波器。设计过程同水面INS/NSS组合导航，其中自适应置信度函数ηk
α和β分别是INS数据可信度和动力学模型导航方式数据可信度，且0<α，β<1。本实例中自适应卡尔曼滤波器的置信度函数选取参考值α＝0.95，β＝0.9。
图8为水下INS/动力学模型组合导航航迹图，相比于水下单纯依靠INS进行导航结果快速发散而无法可靠的导航定位，动力学模型辅助导航在短时间内能大致计算速度和位移，但由于没有更多的外部观测信息，定位误差会逐渐加大在只使用动力学模型辅助INS情况下，定位误差会缓慢累积，因此需要定期浮出水面校正。

Claims (10)

1.一种小型无人自主航行器的水面水下组合导航方法，其特征在于，是在小型无人水面/水下自主航行器(S-ASUV)上同时搭载惯性导航系统INS和卫星导航系统NSS，所述卫星导航系统NSS是指全球定位系统GPS或北斗卫星导航系统BDS中的至少一种；
根据S-ASUV工作场景的不同，将其导航模式分为两类：
(1)当S-ASUV在水面工作时，采用INS/NSS松组合导航模式，INS和NSS各自独立工作；在NSS接收机正常工作状态下，利用NSS进行实时定位和导航，把INS和NSS的位置、速度差作为量测值，通过卡尔曼滤波器给出最优估计结果，最后反馈给INS进行校正；当无法接收到NSS信号时，采用INS/模型辅助导航模式，待恢复信号接收后再利用NSS信号进行位置校准和导航；
(2)当S-ASUV在水下工作时，采用INS/模型辅助导航模式，即在不增加硬件的前提下，采用卡尔曼滤波器将INS和S-ASUV的动力学模型融合以实现辅助导航。
2.根据权利要求1所述的方法，其特征在于，在INS/NSS松组合导航模式下，当无法接收到NSS信号时：
(1)通过所搭载的陀螺仪测得的角速度和上一时刻的姿态数据计算得到S-ASUV的当前姿态：
式中，
Δθk＝|Δθk| (3)
表示k时刻的姿态四元数，表示从k-1时刻到k时刻的姿态四元数变化，Δθk是陀螺仪在k-1时刻输出的角度增量，使用陀螺仪在k-1时刻的角速率输出乘以Δt得到角度增量；
(2)通过速度更新方程计算得到S-ASUV的预测速度：
式(4)中，
表示k时刻的惯导速度，为姿态四元数对应的姿态转换矩阵，Δvk是加速度计在单位时间内的比力增量，采用k-1时刻的比力输出乘以采样周期计算得到；
(3)通过位置更新方程计算得到S-ASUV的预测位置：
式(6)中，
其中，表示k时刻的坐标，RM，RN，L，h分别表示地球子午圈主曲率半径、地球卯酉圈主曲率半径、地理纬度和高度。
3.根据权利要求1所述的方法，其特征在于，在INS/NSS松组合导航模式下，当NSS正常工作时，通过INS和NSS记录的S-ASUV航行状态信息计算并输出的预测航行状态信息；然后把INS和NSS的位置和速度差值作为量测值，采用卡尔曼滤波器融合INS和NSS的方法给出最优估计结果；然后反馈给INS进行闭环校正，随时为进入INS/模型辅助导航模式做好准备。
4.根据权利要求1所述的方法，其特征在于，所述卡尔曼滤波器为增益自适应卡尔曼滤波器，在观测状态下选择INS输出和辅助导航系统输出的差值，在水面运动时辅助导航系统为NSS导航系统，在水下运动时辅助导航系统为动力学模型导航系统；
通过以下方式构建所述增益自适应卡尔曼滤波器：
(1)建立离散系统状态空间模型
Xk＝Φk，k-1Xk-1+Γk-1Wk-1 (8)
Zk＝HkXk+Vk (9)
其中，Xk为系统k时刻的状态向量，Zk为k时刻的观测向量，Φk，k-1为k-1到k时刻的系统状态转移矩阵，Wk-1为k-1时刻的系统噪声向量，Γk-1为k-1时刻的系统噪声转移矩阵，Hk为k时刻的观测矩阵，Vk为k时刻的观测噪声矩阵向量；
(2)构造预测方程
在系统噪声未知的情况下，根据式(9)利用k-1时刻的状态估计量来预测k时刻的状态量，即
式(10)中，是k-1时刻的卡尔曼滤波估计值；
(3)构造预测均方误差
式(11)中，Qk-1是k-1时刻系统噪声方差阵，Pk-1是的均方误差阵，即
(4)设计卡尔曼滤波增益
为改善NSS由于地形的原因丢失信号或因为本身的定位精度在定位上有偏差，产生位置观测离群值导致导航结果大范围偏移的情况，将普通卡尔曼滤波器改进为自适应卡尔曼滤波器，增加置信度函数ηk：
式(13)中，Kk是k时刻的卡尔曼滤波增益，Rk是k时刻观测噪声方差阵，自适应置信度函数ηk计算方式如下：
式(14)中，α和β分别是INS数据可信度和辅助导航方式数据可信度，且0<α，β<1；
(5)构造估计方程
在预测的基础上，结合k时刻观测量Zk和滤波增益，计算出k时刻的卡尔曼滤波估计：
(6)计算估计均方误差
k时刻估计的均方误差为
Pk＝(I-KkHk)Pk|k-1 (16)。
5.根据权利要求4所述的方法，其特征在于，采用增益自适应卡尔曼滤波器融合INS和NSS时，包括针对卡尔曼滤波器状态方程和量测方程的计算：
式(17)中，状态量X1选取INS的三维姿态误差三维速度误差δvN、δvE、δvD；三维位置误差δpN、δpE、δpD；三维陀螺仪的马尔可夫过程误差εrx、εry、εrz和三维加速度计的马尔可夫过程误差▽rx、▽ry、▽rz；
观测量Z1选取INS和电子罗盘的姿态误差、INS和NSS的速度、位置误差作为观测量：
式(19)中，ΘI，vI，pI分别表示INS更新得到的姿态、速度和位置向量；ΘMG，vG，pG分别表示电子罗盘姿态、NSS速度和位置；
式(17)中的状态转移矩阵F1表示为
预测噪声W1表示为
式(21)中，Wg，Wa分别表示陀螺仪角速率白噪声和加速度计比力白噪声，wrg，wra分别表示陀螺仪角速率和加速度计比力马尔可夫过程驱动白噪声；
噪声转移矩阵G1表示为
观测矩阵H1表示为
H1＝[I9×9 09×6] (23)
观测噪声V1表示为
式(24)中，wMG，wgv，wgp分别表示电子罗盘姿态观测白噪声，NSS速度和位置的观测白噪声。
6.根据权利要求4所述的方法，其特征在于，为了提高INS/NSS组合导航模式下的导航精度，按下述方式使用增益自适应卡尔曼滤波计算INS的测量误差：
式(25)中三个方程分别表示用于计算姿态、速度和位置的误差方程；
其中，姿态解算误差为速度误差δvn＝[δvN δvE δvD]T，位置误差δpn＝[δpN δpE δpD]T。
7.根据权利要求4所述的方法，其特征在于，在NSS系统的采样周期Δt小于预设阈值τ时，得到INS融合NSS卡尔曼滤波器状态方程和量测方程的离散化表达式为：
式(26)中，和分别表示k时刻INS/NSS卡尔曼滤波器的状态预测值和观测值。
8.根据权利要求1所述的方法，其特征在于，在INS/模型辅助导航模式下，利用预先建立的动力学模型校准INS导航误差，INS融合动力学模型的卡尔曼滤波器状态方程和量测方程为：
式(27)中，状态量X2选取INS的三维姿态误差三维速度误差δvN、δvE、δvD；三维位置误差δpN、δpE、δpD；三维陀螺仪的马尔可夫过程误差εrx、εry、εrz；三维加速度计的马尔可夫过程误差▽rx、▽ry、▽rz；
观测量Z2选取INS和电子罗盘的姿态误差、INS和动力学模型输出的速度误差、INS和动力学模型输出的北向和东向位置误差、INS和深度计的垂直误差：
式(29)中，vM表示动力学模型解算的导航坐标系速度，分别表示INS得到的NED坐标系位置，表示动力学模型解算的北向位置和东向位置，d是通过深度计得到的深度；
式(27)中，F2，G2，W2，H2中的各元素与F1，G1，W1，H1一致，观测噪声V2表示为：
式(30)中，wMG，wmv分别表示电子罗盘姿态观测白噪声、动力学模型解算速度均方误差,wd分别表示动力学模型解算的北向和东向位置均方误差、深度计观测白噪声；
当系统采样周期Δt小于预设阈值τ时，将INS融合动力学模型卡尔曼滤波器状态方程和量测方程的离散化表达式为：
式(31)中，和分别表示k时刻INS/动力学模型卡尔曼滤波器的状态预测值和观测值。
9.根据权利要求1所述的方法，其特征在于，如预先建立的动力学模型不够精确导致导航误差较大时：
(1)在S-ASUV航行时利用水下摄像头采集水底环境特征，通过环境图像建立水下环境特征地图，并通过与INS信息融合获得S-ASUV自身位置以用于水下导航；具体包括：利用摄像机采集的水下图像信息，计算S-AUV的姿态和位置，并初始化AUV地图；利用ORB算法提取相邻图像特征点，并对其进行匹配，定义匹配完成的特征集；优化ORB特征匹配；利用本质矩阵、单应矩阵或PnP方法，求解S-AUV位姿；或者，
(2)选择使用测距器测量S-AUV到水下各个声信标的距离，利用三个及以上声信标的距离通过三角关系解算出S-AUV的位置；将解算出的位置在卡尔曼滤波器量测方程中进行融合，求得北向东向位置误差。
10.一种能够实现小型无人自主航行器水面水下组合导航的装置，其特征在于，包括能够在水面水下航行的小型无人自主航行器，其艇身上装载下述设备或器件：数字信号处理器、现场可编程门阵列、单片机、NSS接收机、惯性导航系统、测距器、水下摄像头和深度计；
所述数字信号处理器，用于处理水下摄像头采集的水下视频信息，并将处理后的环境特征地图传输给现场可编程门阵列进行运算；
所述单片机，用于采集测距器、深度计、INS、NSS的数据，并将处理后的数据传输给现场可编程门阵列进行运算；
所述现场可编程门阵列，用于加载并执行用于实现权利要求1所述水面水下组合导航方法的软件程序，输出S-ASUV的位置、速度和姿态信息。
CN202311726803.0A 2023-12-14 2023-12-14 一种小型无人自主航行器的水面水下组合导航方法与装置 Pending CN117848339A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311726803.0A CN117848339A (zh) 2023-12-14 2023-12-14 一种小型无人自主航行器的水面水下组合导航方法与装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311726803.0A CN117848339A (zh) 2023-12-14 2023-12-14 一种小型无人自主航行器的水面水下组合导航方法与装置

Publications (1)

Publication Number Publication Date
CN117848339A true CN117848339A (zh) 2024-04-09

Family

ID=90533641

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311726803.0A Pending CN117848339A (zh) 2023-12-14 2023-12-14 一种小型无人自主航行器的水面水下组合导航方法与装置

Country Status (1)

Country Link
CN (1) CN117848339A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118393971A (zh) * 2024-06-24 2024-07-26 浙江大学 基于无人船和rov协同的海底管道跟踪巡线控制系统及方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118393971A (zh) * 2024-06-24 2024-07-26 浙江大学 基于无人船和rov协同的海底管道跟踪巡线控制系统及方法

Similar Documents

Publication Publication Date Title
Kinsey et al. A survey of underwater vehicle navigation: Recent advances and new challenges
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航系统
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN104457754B (zh) 一种基于sins/lbl紧组合的auv水下导航定位方法
CN111829512B (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN107797125B (zh) 一种减小深海探测型auv导航定位误差的方法
CN102829777A (zh) 自主式水下机器人组合导航系统及方法
CN111596333B (zh) 一种水下定位导航方法及系统
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN210719199U (zh) 一种水下机器人多设备组合导航系统
CN110806209A (zh) 一种水下机器人多设备组合导航系统及方法
CN117848339A (zh) 一种小型无人自主航行器的水面水下组合导航方法与装置
CN112197765B (zh) 一种实现水下机器人精细导航的方法
CN110763872A (zh) 一种多普勒测速仪多参数在线标定方法
CN113252041B (zh) 一种适用于小型水下机器人的组合导航方法
CN115523920B (zh) 一种基于视觉惯性gnss紧耦合的无缝定位方法
CN104061930B (zh) 一种基于捷联惯性制导和多普勒计程仪的导航方法
CN112015086B (zh) 一种欠驱动水面船有限时间路径跟踪输出反馈控制方法
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN114459476B (zh) 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法
CN113503872A (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
Lee et al. An integrated navigation system for autonomous underwater vehicles with two range sonars, inertial sensors and Doppler velocity log
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN117870687A (zh) 一种auv水下组合导航定位系统和实现方法
CN117970248A (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