CN117970248A - 基于伴随式声信标辅助的水下航行器协同导航定位方法 - Google Patents
基于伴随式声信标辅助的水下航行器协同导航定位方法 Download PDFInfo
- Publication number
- CN117970248A CN117970248A CN202410131703.1A CN202410131703A CN117970248A CN 117970248 A CN117970248 A CN 117970248A CN 202410131703 A CN202410131703 A CN 202410131703A CN 117970248 A CN117970248 A CN 117970248A
- Authority
- CN
- China
- Prior art keywords
- uuv
- usbl
- matrix
- measurement
- usv
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 74
- 238000005259 measurement Methods 0.000 claims abstract description 64
- 238000001914 filtration Methods 0.000 claims abstract description 33
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 19
- 238000002474 experimental method Methods 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 70
- 230000008569 process Effects 0.000 claims description 14
- 230000003044 adaptive effect Effects 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 9
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 7
- 238000009434 installation Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 238000012883 sequential measurement Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000004590 computer program Methods 0.000 claims 1
- 230000004927 fusion Effects 0.000 abstract 1
- 230000001360 synchronised effect Effects 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 5
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005562 fading Methods 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Classifications
-
- 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
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
-
- 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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明涉及基于伴随式声信标辅助的水下航行器协同导航定位方法。本方法包括：USV搭载RTK GPS与USBL定位系统，针对UUV进行同步跟踪伴随；建立捷联惯性导航系统误差方程；UUV获取多普勒速度信息；UUV获取USV搭载的USBL定位信息，建立组合导航系统量测方程；采用自适应方差受限序贯卡尔曼滤波算法进行数据融合；搭建协同定位实验平台，在湖态实验中进行验证。本方法将USBL定位设备固定在USV上进行伴随式辅助定位，UUV搭载SINS/DVL/USBL，进行组合导航，显著提升了UUV导航系统的作业精度。
Description
技术领域
本发明属于水下航行器导航定位方法技术领域，尤其涉及一种基于伴随式声信标辅助的水下航行器协同导航定位方法。
背景技术
无人水下航行器(Unmanned Underwater Vehicle，UUV)指没有人驾驶、靠遥控或自主控制的在水下航行的设备，它可代替潜水员完成探测、救援、排除水雷、铺设线缆等危险任务，具备很大的优势，在未来的海洋探索方面将发挥重要的作用，因此，对于无人水下航行器的相关技术研究具有很大的应用前景和战略价值。
UUV水下导航中，声学导航与惯性导航的组合是较为有效的方式，声学导航有助于消除惯导的累计误差，实现水下高精度的导航定位。
多普勒测速仪(Doppler Velocity Log，DVL)是现阶段海上航行载体测速的重要导航手段之一，基于多普勒效应它能够长航时地提供载体精确的速度测量信息，与捷联惯导系统具有良好的互补性，从而被广泛应用在各个领域。在这种装置中，通过发射波(通常是微波或激光)并接收反射回来的波，可以测量频率的变化并计算物体的运动速度。
超短基线定位系统(Ultra Short Base Line，USBL)是一种水下声学定位技术，被普遍应用于海洋石油勘探开发、海洋打捞等方面，主要用于确定ROV、AUV、潜水员、水下其他载体的精确位置。超短基线定位系统由发射换能器、应答器、接收基阵组成。发射换能器和接收基阵安装在船上，应答器固定在水下载体UUV上。发射换能器发出一个声脉冲，应答器收到后，回发声脉冲，接收基阵收到后，测量出基阵方向间的相位差，并根据声波的到达时间计算出水下装置到基阵的距离R，从而计算得到UUV在基阵坐标系下的位置。
目前，可以用作UUV水下高精度导航定位的主要是声学方法，包括：
(1)基于惯导与多普勒的声学定位方法。惯性导航系统通过测量加速度和角速度等信息，推算水下目标的运动状态，包括位置、速度和姿态。多普勒传感器发射声波信号，声波在水下传播，当声波与水下目标相互作用并返回传感器时，通过测量多普勒频移，获取水下目标的相对速度信息。利用惯性导航系统提供的速度信息和多普勒频移测量的速度信息进行融合，以获得更准确的水下目标速度。将惯性导航系统提供的位置信息与融合后的速度信息结合，计算水下目标的位置。将融合后的速度和位置信息结合时间，推导出水下目标的位移，实现目标的定位。然而，从组合导航原理出发，本方法无法消除初始位置误差，且随着时间累积，位置误差会存在不确定性的位置发散。
(2)基于多个固定信标的长基线声学定位方法。此方法要求在海底事先安置3～4个信标，并测定它们的准确位置。在使用过程中，修订后的信标位置被存储在UUV中。通过实时测量UUV与各信标之间的距离，系统计算UUV相对于每个信标的位置。然而，该方法的局限在于UUV只能在信标覆盖的有限区域内进行定位，此外，信标的校准和布放过程相对繁琐。
发明内容
为了克服现有方法存在的上述缺陷，本发明提出了一种新的基于伴随式声信标辅助的水下航行器协同导航定位方法，本方法将USBL定位设备固定在USV(Unmanned SurfaceVehicle)上进行伴随式辅助定位，UUV搭载SINS/DVL/USBL OEM，进行组合导航，显著提升了UUV导航系统的精度。
具体地，本发明提供了一种基于伴随式声信标辅助的水下航行器协同导航定位方法，本方法包括：
S1.确认USBL与USBL OEM的位置，使其满足USBL的发射器方向性和覆盖范围；
S2.将UUV置于USV的下方，USBL与USBL OEM两设备间的距离为D，两设备间的连线与USBL安装垂向形成夹角为θ，且满足以下声学约束条件：
S3.在此约束下，USV根据UUV位置进行实时跟踪伴随，采用基于视线法的位置跟踪PID算法；
S4.将USBL搭载在USV中心，USBL返回的UUV相对位置矢量转换为当地导航坐标系下为(xP yP zP)，此时：
S5.当USV在水面跟踪UUV时，将跟踪点设为UUV在水面的投影点，使二者相对距离保持在定位约束范围内；
S6.设为UUV所在USV中心航向，则有：
S7.计算二者间航向角误差距离跟踪误差Δp(t)：
式中，USV自身航向角为pxymin为水平距离安全距离最小值；
S8.通过调整USV舵角控制输出α(t)和航速输出β(t)使其保持相对位置，控制指令分别为：
式中，比例系数为kp，积分系数为ki＝kpT/Ti，微分系数为kd＝kpT/Td，其中，积分周期和微分周期分别为Ti,Td，T为系统采样周期。
进一步地，本发明基于伴随式声信标辅助的水下航行器协同导航定位方法步骤S1中包括：
S11.捷联惯性导航系统的完整误差状态矢量表述为：
式中，为姿态误差矢量，δv＝[δvE δvN δvU]为速度误差矢量，δp＝[δL δλ δh]为位置误差矢量，ε＝[εx εy εz]为陀螺仪漂移误差矢量，为加速度计漂移误差矢量；
S12.基于SINS/DVL/USBL的组合导航系统状态方程由惯性导航系统的误差状态组成，惯性导航系统的误差状态方程表示为：
式中，F(t)为状态转移矩阵，其中Fa∈R9×9表示捷联惯导系统误差参数的系统矩阵，其为9阶的方阵；G(t)为系统噪声矩阵，此处取为单位阵；W(t)＝[01×9 ωgx ωgy ωgz ωax ωax ωax]T代表所建模的惯性系统噪声，其中[ωgx ωgy ωgz]T代表陀螺随机白噪声，[ωax ωay ωaz]T代表三轴加速度计随机白噪声。
进一步地，本发明基于伴随式声信标辅助的水下航行器协同导航定位方法步骤S2中包括：
S21.利用DVL得到的载体实时高精度速度信息修正惯导误差；
S22.DVL测得载体相对载体坐标系下的三轴速度分量，将DVL输出的速度矢量变换到地理坐标系，然后SINS输出相应的速度信息与其作差得到量测值：
式中，vEi、vNi、vUi分别为惯导地理坐标系下东向、北向、天向速度；vEd、vNd、vUd分别为多普勒测速仪输出变换后地理坐标系下东向、北向、天向速度；
S23.进一步可表示为：
式中，δvEi、δvNi、δvUi分别为惯导地理坐标系下东向、北向、天向测速误差；δvEd、δvNd、δvUd分别为多普勒测速仪输出变换后的地理坐标系下东向、北向、天向测速误差，设其为零均值的高斯白噪声；
S24.基于SINS/DVL的组合导航系统量测方程进一步可表示为：
Zd＝HdX+Vd；
式中，Hd为量测矩阵，Hd＝[03×3 I3×3 03×9]，Vd＝[δvED δvND δvUD]为系统的量测噪声，为零均值的高斯白噪声。
进一步地，本发明基于伴随式声信标辅助的水下航行器协同导航定位方法步骤S3中包括：
S31.λusv、Lusv、husv分别代表USV经度、纬度、高度，因此地球直角坐标系下UUV位置为：
式中，[xuuv yuuv zuuv]为地球直角坐标系下位置，RN为地球半径，e代表偏心率，为相应坐标系转换矩阵；
S32.将UUV位置坐标转换回地理坐标系下：
S33.当USBL连接水面船外置GNSS后，USBL端解算出UUV的位置信息，包括经度、纬度、高度；与UUV自身安装的SINS设备作差，作为新的量测信息加入到上述S22中的SINS/DVL组合导航系统；量测值Zdu如下式：
S34.基于SINS/DVL/USBL组合的USV/UUV协同导航系统量测方程为：
Zdu＝HduX+Vdu；
式中，Hdu＝[06×3 I6×6 06×6]，Vdu＝[δvED δvND δvUD δLu δλu δhu]为系统的量测噪声，设Vdu为零均值的高斯白噪声；
S35.得到SINS/DVL/USBL协同导航系统的状态空间表达式：
式中，X(t)为导航系统的状态方程，F(t)为状态转移矩阵，G(t)为系统噪声驱动矩阵，Z(t)为导航系统的量测量的差值，为SINS输出与USBL、DVL的差值；W(t)和V(t)分别为系统噪声和量测噪声。
进一步地，本发明基于伴随式声信标辅助的水下航行器协同导航定位方法步骤S4中包括：
S41.采用方差受限的序贯Sage-Husa自适应滤波算法对SINS/DVL/USBL协同导航系统进行滤波处理，导航系统状态空间方程为：
S42.上式中，Xk为k时刻的系统状态，Φk|k-1为状态转移矩阵；Γk|k-1为系统噪声输入矩阵；Zk为k时刻的量测值，其为状态向量Xk的函数；Hk为量测矩阵；Wk和Vk分别为系统噪声和量测噪声；同时Wk和Vk满足下式：
式中，Qk为系统的噪声协方差矩阵，为非负定矩阵；Rk为量测噪声协方差矩阵，为正定矩阵；
S43.卡尔曼滤波的步骤及各公式为：
(1)状态的一步预测：
(2)均方误差的一步预测：
(3)卡尔曼增益矩阵计算：
(4)状态更新：
(5)状态协方差矩阵更新：
Pk＝(I-KkHk)Pk|k-1；
S44.假设量测噪声方差阵Rk未知，下面给出Rk的自适应估计方法：
(1)在卡尔曼滤波过程中，量测预测误差(新息)公式为：
(2)针对上式两边同时求方差，可得：
(3)移项之后，量测噪声方差表达式为：
式中，为理论上随机序列集总平均；
S45.在自适应滤波算法应用过程中以时间平均代替，为了保证算法保持适当大小的自适应能力，Rk的等加权递推估计方法可构造如下：
式中，β0＝1，0＜b＜1为渐消因子；
S46.采用序贯滤波的方法对的对角线元素大小进行限制，采用序贯滤波方法在进行第i个标量序贯量测更新时，标量量测方程为：
再利用下限条件以保证为正，同时利用上限条件快速降低量测的可信度：
进一步地，本发明基于伴随式声信标辅助的水下航行器协同导航定位方法步骤S5中包括：
S51.USV搭载S2C R 18/34H USBL定位系统、RTK GPS端；UUV搭载惯导、DVL、S2C R18/34H OEM应答器；
S52.将基于SINS/DVL/USBL OEM的组合导航系统移植到UUV工控机系统上；
S53.执行导航任务，在实验中验证所设计的USV/UUV协同导航系统和滤波算法。
综上，由于USBL安装在移动且具备高精度的RTK GPS的USV上，随UUV伴随航行，从而实现了较大范围内的UUV水下导航和定位。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面对本发明实施例叙述中需要使用的附图作简要介绍，显而易见地，下述附图仅是本发明中记载的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明基于伴随式声信标辅助的水下航行器协同导航定位方法的整体架构示意图。
图2为USBL发射器方向性图。
图3为USV与UUV安装布放位置示意图。
图4为USV、UUV水平面投影图。
图5为卡尔曼滤波流程图。
图6为USV平台搭载图。
图7为UUV平台搭载图。
图8为UUV运动轨迹图。
图9为UUV位置误差图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合具体实施例及相应的附图对本发明的技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例，本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。
同时，应理解，本发明的保护范围并不局限于下述特定的具体实施方案；还应当理解，本发明实施例中使用的术语是为了描述特定的具体实施方案，而不是为了限制本发明的保护范围。
实施例：基于伴随式声信标辅助的水下航行器协同导航定位方法
如图1所示，本方法包括以下步骤：
S1.EvoLogics S2C R 18/34H USBL与S2C R 18/34H OEM之间的定位需要满足USBL的发射器方向性如图2所示，二者之间的安装与布放位置如图3所示；
S2.此时UUV应处于USV的下方，USBL与OEM两设备间的距离为D，两设备间的连线与USBL安装垂向形成夹角为θ，为获得良好的定位精度与通讯效果，应满足声学约束条件如下式：
S3.在此约束下，UUV在执行任务时，为保障UUV持续获得较高精度的定位效果和通讯效果，USV根据UUV位置进行实时跟随；为实现USV搭载USBL移动协同定位，提出基于视线法的位置跟踪PID算法；
S4.USBL搭载在USV中心，USBL坐标系与USV载体坐标系不考虑安装误差，近似重合，此时USBL返回的UUV相对位置矢量通过坐标系转换的方法，转换为当地导航坐标系下为(xP yP zP)；
S5.设定UUV任务执行阶段通常水下深度不变，主要进行平面运动；USV在水面跟踪UUV时，将跟踪点设为UUV在水面的投影点，可使二者相对距离保持在定位约束范围内；此时二者在水平面内的位置如图4所示；
S6.为UUV所在USV中心航向：
S7.计算二者间航向角误差距离跟踪误差Δp(t)：
式中，USV自身航向角为pxymin为水平距离安全距离最小值；
S8.通过调整USV舵角控制输出α(t)和航速输出β(t)使其保持相对位置，控制指令分别为：
式中，比例系数为kp，积分系数为ki＝kpT/Ti，微分系数为kd＝kpT/Td，其中，积分周期和微分周期分别为Ti,Td，T为系统采样周期。
进一步地，本方法步骤S1中包括：
S11.捷联惯性导航系统误差方程的建立，状态误差矢量方程中，一般包含位置误差，即经度误差、纬度误差和高度方向误差；速度误差，包括东向速度误差、北向速度误差、天向速度误差；以及三个姿态角的误差，包括俯仰角pitch、横滚角roll、航向角yaw；加速度计的漂移误差和陀螺仪的漂移误差。因此，惯性导航系统的完整误差状态矢量表述为：
式中，为姿态误差矢量，δv＝[δvE δvN δvU]为速度误差矢量，δp＝[δL δλ δh]为位置误差矢量，ε＝[εx εy εz]为陀螺仪漂移误差矢量，为加速度计漂移误差矢量；
S12.基于SINS/DVL的UUV自主导航系统状态方程由惯性导航系统的误差状态组成，将平台误差角方程、位置误差方程和速度误差方程以及惯性仪器的误差方程综合到一起，因此惯性导航系统的误差状态方程可以表示为：
式中，F(t)代表状态转移矩阵，其中Fa∈R9×9表示捷联惯导系统误差参数的系统矩阵，其为9阶的方阵；根据捷联惯性导航系统的误差分析推得结论，可以得到其速度、姿态和位置误差方程：
(1)速度误差方程：
(2)姿态误差方程：
(3)位置误差方程：
S13.为了方便表示Fa，将Fa划分为3×3的块矩阵，令则其中的每个块矩阵表示如下：
式中，F31取零矩阵；同理，Fb可以写为下述矩阵形式，即Fc∈R6×6为陀螺仪和加速度计的误差状态所对应的状态矩阵，此时将两者的误差考虑为白噪声，这里Fc取零矩阵，0∈R9×6；G(t)为系统噪声矩阵，这里取为单位阵；W(t)＝[01×9 ωgxωgy ωgz ωax ωax ωax]T表示所建模的惯性系统噪声，其中[ωgx ωgy ωgz]T代表陀螺随机白噪声，[ωax ωay ωaz]T代表三轴加速度计随机白噪声。
进一步地，本方法步骤S2中包括：
S21.利用DVL得到的载体实时高精度速度信息建立量测方程，并修正惯导误差；
S22.DVL测得载体相对载体坐标系下的三轴速度分量，将DVL输出的速度矢量变换到地理坐标系，然后SINS输出相应速度信息与其作差得到量测值：
式中，vEi、vNi、vUi分别为惯导地理坐标系下东向、北向、天向速度；vEd、vNd、vUd分别为多普勒测速仪输出变换后地理坐标系下东向、北向、天向速度；
S23.进一步可表示为：
式中，δvEi、δvNi、δvUi分别为惯导地理坐标系下东向、北向、天向测速误差，δvEd、δvNd、δvUd分别为多普勒测速仪输出变换后的地理坐标系下东向、北向、天向测速误差，设其为零均值的高斯白噪声；
S24.SINS/DVL组合导航系统量测方程进一步可表示为：
Zd＝HdX+Vd；
式中，Hd为量测矩阵，Hd＝[03×3 I3×3 03×9]，Vd＝[δvED δvND δvUD]为系统的量测噪声，为零均值的高斯白噪声。
进一步地，本方法步骤S3中包括：
S31.针对超短基线定位坐标系(u系)，获取的应答器的位置矢量为pxyz，USBL载体坐标系(busv系)与u系视为重合，将超短基线定位系统下应答器位置矢量利用坐标转换矩阵转换到USV载体坐标系下，最终由地理坐标系(n系)转换到地球物理坐标系(e系)得到的转换矩阵为：
S32.λusv、Lusv、husv分别代表USV经度、纬度、高度，因此地球直角坐标系下UUV位置为：
式中，[xuuv yuuv zuuv]为地球直角坐标系下位置，RN为地球半径，e代表偏心率，为相应坐标系转换矩阵；
S33.将UUV位置坐标转换回地理坐标系下：
S34.当USBL连接水面船外置GNSS后，USBL端解算出UUV的位置信息，包括经度、纬度、高度；与UUV自身安装的SINS设备作差，作为新的量测信息加入到上述S22中的SINS/DVL组合导航系统中；量测值Zdu可以选为下式：
S34.基于SINS/DVL/USBL组合的USV/UUV协同导航系统量测方程为：
Zdu＝HduX+Vdu；
式中，Hdu＝[06×3 I6×6 06×6]，Vdu＝[δvED δvND δvUD δLu δλu δhu]为系统的量测噪声，设Vdu为零均值的高斯白噪声；
S35.得到SINS/DVL/USBL协同导航系统的状态空间表达式：
式中，X(t)为导航系统的状态方程，F(t)为状态转移矩阵，G(t)为系统噪声驱动矩阵，Z(t)为导航系统的量测量的差值，为SINS输出与USBL、DVL的差值；W(t)和V(t)分别为系统噪声和量测噪声。
进一步地，本方法步骤S4中包括：
S41.采用方差受限的序贯Sage-Husa自适应滤波算法对SINS/DVL/USBL协同导航系统进行滤波处理；导航系统状态空间方程为：
S42.上式中，Xk为k时刻的系统状态，Φk|k-1为状态转移矩阵；Γk|k-1为系统噪声输入矩阵；Zk为k时刻的量测值，其为状态向量Xk的函数；Hk为量测矩阵；Wk和Vk分别为系统噪声和量测噪声；同时Wk和Vk满足下式：
式中，Qk为系统的噪声协方差矩阵，为非负定矩阵；Rk为量测噪声协方差矩阵，为正定矩阵；
S43.卡尔曼滤波的步骤及各公式为：
(1)状态的一步预测：
(2)均方误差的一步预测：
(3)卡尔曼增益矩阵计算：
(4)状态更新：
(5)状态协方差矩阵更新：
Pk＝(I-KkHk)Pk|k-1；
S44.当给定初始状态X(0)，初始协方差P0，初始时刻的量测，则可以根据S43中的步骤(1)和(2)更新系统的状态和状态误差协方差，然后利用步骤(3)计算系统的滤波增益；最后利用步骤(4)和步骤(5)进行状态的进一步估计和协方差的预测。在实际应用过程中，当出现量测Zk无法观测的情况时，此时的卡尔曼滤波器可以直接根据步骤(1)和步骤(2)进行时间更新，这样可以确保系统短时间内的精度，算法流程如图5所示；
S45.假设量测噪声方差阵Rk未知，下面给出Rk的自适应估计方法：
(1)在卡尔曼滤波过程中，量测预测误差(新息)公式为：
(2)针对上式两边同时求方差，可得：
(3)移项之后，量测噪声方差表达式为：
式中，为理论上随机序列集总平均；
S46.在自适应滤波算法应用过程中可以时间平均代替，Rk的等加权递推估计方法可构造如下：
其中，初值可根据实际情况取为较大的方差阵；
S47.依据上式，当k→∞时，1/k→0，表示长时间滤波后，自适应能力将逐渐削弱，直至失去自适应调整效果；
S48.为了保证算法保持适当大小的自适应能力，可将等加权式改为指数渐消记忆加权平均：
式中，β0＝1，0＜b＜1称为渐消因子；
S49.采用序贯滤波的方法对的对角线元素大小进行限制可有效避免此问题；假设为对角线矩阵，采用序贯滤波方法在进行第i个标量序贯量测更新时，标量量测方程为：
再利用下限条件以保证为正，同时利用上限条件快速降低量测的可信度(也可用来判断量测是否异常，若视为异常则放弃本次量测更新，这实际上包含了故障检测和隔离的思想)；
通过上述处理，可将量测噪声始终限制在之内，从而获得较好的自适应能力和滤波可靠性。
进一步地，本发明方法步骤S5中包括：
S51.USV搭载S2C R 18/34H USBL定位系统、RTK GPS端，UUV搭载惯导、DVL、S2C R18/34H OEM应答器，如图6、图7所示；
S52.将基于SINS/DVL/USBL OEM的组合导航系统移植到UUV控制系统上；
S53.执行导航任务，在实验中验证所设计的USV/UUV协同导航系统和滤波算法，最终UUV的运动轨迹以及导航误差如图8、图9所示。
以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容作出些许更动或修饰获得等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均应包含在本发明的权利要求保护范围之内。
Claims (7)
1.基于伴随式声信标辅助的水下航行器协同导航定位方法，其特征在于，所述方法包括：
S1.确认USBL与USBL OEM的位置，使其满足USBL的发射器方向性和覆盖范围；
S2.将UUV置于USV的下方，USBL与USBL OEM两设备间的距离为D，两设备间的连线与USBL安装垂向形成夹角为θ，且满足以下声学约束条件：
S3.在此约束下，USV根据UUV位置进行实时跟踪伴随，采用基于视线法的位置跟踪PID算法；
S4.将USBL搭载在USV中心，USBL返回的UUV相对位置矢量转换为当地导航坐标系下为(xP yP zP)，此时：
S5.当USV在水面跟踪UUV时，将跟踪点设为UUV在水面的投影点，使二者相对距离保持在定位约束范围内；
S6.设为UUV所在USV中心航向，则有：
S7.计算二者间航向角误差距离跟踪误差Δp(t)：
式中，USV自身航向角为pxymin为水平距离安全距离最小值；
S8.通过调整USV舵角控制输出α(t)和航速输出β(t)使其保持相对位置，控制指令分别为：
式中，比例系数为kp，积分系数为ki＝kpT/Ti，微分系数为kd＝kpT/Td，其中，积分周期和微分周期分别为Ti,Td，T为系统采样周期。
2.根据权利要求1所述的基于伴随式声信标辅助的水下航行器协同导航定位方法，其特征在于，步骤S1中包括：
S11.捷联惯性导航系统的完整误差状态矢量表述为：
式中，为姿态误差矢量，δv＝[δvE δvN δvU]为速度误差矢量，δp＝[δLδλ δh]为位置误差矢量，ε＝[εx εy εz]为陀螺仪漂移误差矢量，为加速度计漂移误差矢量；
S12.基于SINS/DVL/USBL的组合导航系统状态方程由惯性导航系统的误差状态组成，惯性导航系统的误差状态方程表示为：
式中，F(t)为状态转移矩阵，其中Fa∈R9×9表示捷联惯导系统误差参数的系统矩阵，其为9阶的方阵；G(t)为系统噪声矩阵，此处取为单位阵；W(t)＝[01×9ωgx ωgy ωgz ωax ωax ωax]T代表所建模的惯性系统噪声，其中[ωgx ωgy ωgz]T代表陀螺随机白噪声，[ωax ωay ωaz]T代表三轴加速度计随机白噪声。
3.根据权利要求2所述的基于伴随式声信标辅助的水下航行器协同导航定位方法，其特征在于，步骤S2中包括：
S21.利用DVL得到的载体实时高精度速度信息修正惯导误差；
S22.DVL测得载体相对载体坐标系下的三轴速度分量，将DVL输出的速度矢量变换到地理坐标系，然后SINS输出相应的速度信息与其作差得到量测值：
式中，vEi、vNi、vUi分别为惯导地理坐标系下东向、北向、天向速度；vEd、vNd、vUd分别为多普勒测速仪输出变换后地理坐标系下东向、北向、天向速度；
S23.进一步可表示为：
式中，δvEi、δvNi、δvUi分别为惯导地理坐标系下东向、北向、天向测速误差；δvEd、δvNd、δvUd分别为多普勒测速仪输出变换后的地理坐标系下东向、北向、天向测速误差，设其为零均值的高斯白噪声；
S24.基于SINS/DVL的组合导航系统量测方程进一步可表示为：
Zd＝HdX+Vd；
式中，Hd为量测矩阵，Hd＝[03×3 I3×3 03×9]，Vd＝[δvED δvND δvUD]为系统的量测噪声，为零均值的高斯白噪声。
4.根据权利要求3所述的基于伴随式声信标辅助的水下航行器协同导航定位方法，其特征在于，步骤S3中包括：
S31.λusv、Lusv、husv分别代表USV经度、纬度、高度，因此地球直角坐标系下UUV位置为：
式中，[xuuv yuuv zuuv]为地球直角坐标系下位置，RN为地球半径，e代表偏心率，为相应坐标系转换矩阵；
S32.将UUV位置坐标转换回地理坐标系下：
S33.当USBL连接水面船外置GNSS后，USBL端解算出UUV的位置信息，包括经度、纬度、高度；与UUV自身安装的SINS设备作差，作为新的量测信息加入到S22中的SINS/DVL组合导航系统；量测值Zdu如下式：
S34.基于SINS/DVL/USBL组合的USV/UUV协同导航系统量测方程为：
Zdu＝HduX+Vdu；
式中，Hdu＝[06×3 I6×6 06×6]，Vdu＝[δvED δvND δvUD δLu δλu δhu]为系统的量测噪声，设Vdu为零均值的高斯白噪声；
S35.得到SINS/DVL/USBL协同导航系统的状态空间表达式：
式中，X(t)为导航系统的状态方程，F(t)为状态转移矩阵，G(t)为系统噪声驱动矩阵，Z(t)为导航系统的量测量的差值，为SINS输出与USBL、DVL的差值；W(t)和V(t)分别为系统噪声和量测噪声。
5.根据权利要求4所述的基于伴随式声信标辅助的水下航行器协同导航定位方法，其特征在于，步骤S4中包括：
S41.采用方差受限的序贯Sage-Husa自适应滤波算法对SINS/DVL/USBL协同导航系统进行滤波处理，导航系统状态空间方程为：
S42.上式中，Xk为k时刻的系统状态，Φk|k-1为状态转移矩阵；Γk|k-1为系统噪声输入矩阵；Zk为k时刻的量测值，其为状态向量Xk的函数；Hk为量测矩阵；Wk和Vk分别为系统噪声和量测噪声；同时Wk和Vk满足下式：
式中，Qk为系统的噪声协方差矩阵，为非负定矩阵；Rk为量测噪声协方差矩阵，为正定矩阵；
S43.卡尔曼滤波的步骤及各公式为：
(1)状态的一步预测：
(2)均方误差的一步预测：
(3)卡尔曼增益矩阵计算：
(4)状态更新：
(5)状态协方差矩阵更新：
Pk＝(I-KkHk)Pk|k-1；
S44.假设量测噪声方差阵Rk未知，下面给出Rk的自适应估计方法：
(1)在卡尔曼滤波过程中，量测预测误差公式为：
(2)针对上式两边同时求方差，可得：
(3)移项之后，量测噪声方差表达式为：
式中，为理论上随机序列集总平均；
S45.在自适应滤波算法应用过程中以时间平均代替，为了保证算法保持适当大小的自适应能力，Rk的等加权递推估计方法构造如下：
式中，β0＝1，0＜b＜1为渐消因子；
S46.采用序贯滤波的方法对的对角线元素大小进行限制，采用序贯滤波方法在进行第i个标量序贯量测更新时，标量量测方程为：
再利用下限条件以保证为正，同时利用上限条件快速降低量测的可信度：
6.根据权利要求5所述的基于伴随式声信标辅助的水下航行器协同导航定位方法，其特征在于，步骤S5中包括：
S51.USV搭载S2C R 18/34H USBL定位系统、RTK GPS端；UUV搭载惯导、DVL、S2C R 18/34H OEM应答器；
S52.将基于SINS/DVL/USBL OEM的组合导航系统移植到UUV工控机系统上；
S53.执行导航任务，在实验中验证所设计的USV/UUV协同导航系统和滤波算法。
7.一种计算机可读存储介质，所述存储介质上存储有计算机程序，所述程序被处理器执行时实现权利要求1-6任一项所述的基于伴随式声信标辅助的水下航行器协同导航定位方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410131703.1A CN117970248A (zh) | 2024-01-31 | 2024-01-31 | 基于伴随式声信标辅助的水下航行器协同导航定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410131703.1A CN117970248A (zh) | 2024-01-31 | 2024-01-31 | 基于伴随式声信标辅助的水下航行器协同导航定位方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117970248A true CN117970248A (zh) | 2024-05-03 |
Family
ID=90855013
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410131703.1A Pending CN117970248A (zh) | 2024-01-31 | 2024-01-31 | 基于伴随式声信标辅助的水下航行器协同导航定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117970248A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118393971A (zh) * | 2024-06-24 | 2024-07-26 | 浙江大学 | 基于无人船和rov协同的海底管道跟踪巡线控制系统及方法 |
-
2024
- 2024-01-31 CN CN202410131703.1A patent/CN117970248A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118393971A (zh) * | 2024-06-24 | 2024-07-26 | 浙江大学 | 基于无人船和rov协同的海底管道跟踪巡线控制系统及方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Kepper et al. | A navigation solution using a MEMS IMU, model-based dead-reckoning, and one-way-travel-time acoustic range measurements for autonomous underwater vehicles | |
| CN102829777B (zh) | 自主式水下机器人组合导航系统及方法 | |
| Salavasidis et al. | Terrain‐aided navigation for long‐endurance and deep‐rated autonomous underwater vehicles | |
| CN103744098B (zh) | 基于sins/dvl/gps的auv组合导航系统 | |
| Bao et al. | Integrated navigation for autonomous underwater vehicles in aquaculture: A review | |
| Kinsey et al. | A survey of underwater vehicle navigation: Recent advances and new challenges | |
| Lee et al. | Simulation of an inertial acoustic navigation system with range aiding for an autonomous underwater vehicle | |
| Eustice et al. | Experimental results in synchronous-clock one-way-travel-time acoustic navigation for autonomous underwater vehicles | |
| CN112097763B (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
| Whitcomb et al. | Advances in Doppler-based navigation of underwater robotic vehicles | |
| Grenon et al. | Enhancement of the inertial navigation system for the Morpheus autonomous underwater vehicles | |
| CN1325932C (zh) | 载人潜水器的组合导航定位方法 | |
| CN106679662B (zh) | 一种基于tma技术的水下机器人单信标组合导航方法 | |
| CN102252677A (zh) | 一种基于时间序列分析的变比例自适应联邦滤波方法 | |
| Armstrong et al. | AUV navigation in the presence of a magnetic disturbance with an extended Kalman filter | |
| CN103697910A (zh) | 自主水下航行器多普勒计程仪安装误差的校正方法 | |
| CN107966145B (zh) | 一种基于稀疏长基线紧组合的auv水下导航方法 | |
| CN111595348A (zh) | 一种自主水下航行器组合导航系统的主从式协同定位方法 | |
| CN117970248A (zh) | 基于伴随式声信标辅助的水下航行器协同导航定位方法 | |
| Troni et al. | Preliminary experimental evaluation of a Doppler-aided attitude estimator for improved Doppler navigation of underwater vehicles | |
| CN104061930B (zh) | 一种基于捷联惯性制导和多普勒计程仪的导航方法 | |
| CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
| Liu et al. | Improved path following for autonomous marine vehicles with low-cost heading/course sensors: Comparative experiments | |
| Allotta et al. | Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements | |
| CN116026324B (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 |