CN117029872A - 一种基于ins/dvl/lbl紧组合的auv导航方法及导航系统 - Google Patents
一种基于ins/dvl/lbl紧组合的auv导航方法及导航系统 Download PDFInfo
- Publication number
- CN117029872A CN117029872A CN202310419119.1A CN202310419119A CN117029872A CN 117029872 A CN117029872 A CN 117029872A CN 202310419119 A CN202310419119 A CN 202310419119A CN 117029872 A CN117029872 A CN 117029872A
- Authority
- CN
- China
- Prior art keywords
- ins
- dvl
- lbl
- auv
- speed
- 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 32
- 238000012937 correction Methods 0.000 claims abstract description 31
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 18
- 238000001914 filtration Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000005259 measurement Methods 0.000 claims description 23
- 230000003044 adaptive effect Effects 0.000 claims description 15
- 230000005540 biological transmission Effects 0.000 claims description 13
- 238000006243 chemical reaction Methods 0.000 claims description 13
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000004044 response Effects 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 239000013535 sea water Substances 0.000 claims description 3
- 230000008054 signal transmission Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 230000008520 organization Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000010521 absorption reaction Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000005236 sound signal Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- 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/66—Sonar tracking systems
-
- 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/87—Combinations of sonar systems
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明提供一种基于INS/DVL/LBL紧组合的AUV导航方法及导航系统。步骤一、进行INS机械编排，其中INS机械编排为指北方位力学的机械编排；步骤二、采用卡尔曼滤波算法融合INS/DVL/LBL/IPS的导航参数，实时校正INS的误差；步骤三、基于步骤二的实时校正INS的误差，对INS的姿态、速度和位置误差进行反馈校正；步骤四、基于姿态、速度和位置误差的反馈校正实现AUV导航。针对当AUV的作业范围超过应答器的有效工作范围时，AUV无法接收到四个应答器信号，则无法通过LBL定位算法获得唯一位置解，此时无法进行INS/LBL松组合，有效应答器信号被丢弃未得到充分利用的问题。
Description
技术领域
本发明属于AUV导航领域，具体涉及一种基于INS/DVL/LBL紧组合的AUV导航方法及导航系统。
背景技术
AUV(Autonomous Underwater Vehicles，自主式水下航行器)组合导航是指将多个导航传感器的信息进行最优融合估计，精确解算其位置、速度及姿态的过程。为保证AUV的导航精度，现代先进的AUV一般都装备有惯性导航系统(Inertial Navigation System，INS)、全球导航卫星系统(Global Navigation Satellite System，GNSS)、长基线声学定位系统(Long Baseline Acoustic Positioning System，LBL)、多普勒计程仪(DopplerVelocity Log，DVL)、深度计(Intelligent Pressure Sensor，IPS)等导航传感器。
AUV在水下航行时，GNSS信号会迅速衰减，无法定位。由于INS可以输出姿态、速度和位置，一般作为AUV的核心传感器，但INS的误差通过导航方程被不断积分，导致INS解算的姿态、速度和位置随时间发散。DVL速度误差和LBL位置误差不随时间积累，INS与DVL/LBL之间具有良好的互补性，通过INS/DVL/LBL组合导航可以充分发挥各个子系统的优点。
DVL属于主动声呐，需要发射和接收声波，其接收到的声信号与周围海洋环境有很大关系。DVL工作在底跟踪模式时，会出现如下典型工况导致DVL底跟踪失效，如图1所示。
(1)AUV下方有海洋生物阻挡时，DVL发射的声波不能到达海底；
(2)当海底存在强吸声地质(如淤泥)时，DVL发射的声波无法反射回来；
(3)当AUV作业区域地形起伏变化较大时，如AUV下方存在海沟，超过了DVL底跟踪的有效作用距离；
(4)当AUV进行大角度机动时，DVL无法收到部分反射声波。
在四波束Janus配置的DVL中，若DVL有效波束数量小于三个，DVL底跟踪无效，则INS/DVL无法进行松组合。此时，部分有效波束信息被丢弃，未得到充分利用。因此，研究基于波束速度的INS/DVL紧组合具有重要意义，在DVL底跟踪无效时，紧组合仍可利用部分有效波束速度校正INS误差。
LBL通过测量应答器到AUV的传播时延，进而计算出距离，再利用几何学方法解算出AUV位置，该过程如图2所示。但是，目前的LBL定位算法一般基于以下假设或要求：
(1)基于AUV静止假设，忽略AUV运动导致的误差；
(2)要求AUV和应答器时钟严格同步；
(3)忽略LBL各应答器信号在不同时刻到达AUV；
(4)AUV搭载的LBL主控处理单元深度未知时，必须接收到四个应答器信号，才可获得AUV位置的唯一解。
但是，对海洋测绘、水下搜救等精细化作业场景，AUV运动导致的误差在数米级，误差过大。AUV长时间在水下航行，必然会存在时钟漂移，若应答器布放在海底也会存在时钟漂移。AUV在水下作业时，与每个应答器的距离并不相等，各应答器信号不会再同一时刻到达AUV，而现有的LBL定位算法是基于各应答器信号在同一时刻到达AUV这一假设。此外，当AUV的作业范围超过应答器的有效工作范围时，AUV无法接收到四个应答器信号，则无法通过LBL定位算法获得唯一位置解，此时无法进行INS/LBL松组合，有效应答器信号被丢弃未得到充分利用。
发明内容
本发明提供一种基于INS/DVL/LBL紧组合的AUV导航方法及导航系统，针对当AUV的作业范围超过应答器的有效工作范围时，AUV无法接收到四个应答器信号，则无法通过LBL定位算法获得唯一位置解，此时无法进行INS/LBL松组合，有效应答器信号被丢弃未得到充分利用的问题。
本发明通过以下技术方案实现：
1.一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，所述AUV导航方法包括以下步骤，
步骤一、采用经过零偏补偿的陀螺仪和加速度计数据，进行机械编排；
步骤二、采用尔曼滤波算法融合INS/DVL/LBL/PS等传感器数据，实时估计INS姿态、速度和位置的误差，并实时估计陀螺仪和加速度计的剩余零偏；
步骤三、基于步骤二的实时估计INS的误差，对INS的姿态、速度、位置误差进行自适应反馈校正，并对陀螺仪和加速度计零偏进行累加；
步骤四、经过步骤三反馈校正的INS姿态、速度和位置，作为AUV的导航信息。
进一步的：所述步骤一具体为，所述INS机械编排划分为姿态、速度和位置更新；
所述INS机械编排中待求解的微分方程组为公式(1)
其中，为方向余弦矩阵，/>为b系相对于n系的角速度，υn为AUV在北东地方向的速度，/>为比力，/>为地球自转角速度，gn为重力加速度，p为AUV的经纬度和深度，Mpv为速度与位置的转换矩阵；
公式(1)的离散化求解采用双子样算法实现；方向余弦矩阵微分方程的求解公式如下：
其中，T为陀螺仪和加速度计的采样间隔，Δθk-1和Δθk分别为k-1时刻和k时刻的角度增量，其余变量为中间变量；
速度微分方程的求解公式如下：
其中，Δvk-1和Δvk分别为k-1时刻和k时刻的速度增量，其余变量为中间变量；
位置微分方程的求解公式如下：
其中，RN和RE分别为子午圈和卯酉圈主曲率半径，L为纬度，λ为经度，d为深度，Re为地球长半轴，e为地球椭圆偏心率。
进一步的，所述步骤2具体为，状态空间模型可表示为公式(2)，
其中，Xk为状态向量，Zk为观测向量，Wk-1和Vk分别为系统噪声和量测噪声。状态转移矩阵Fk、系统噪声驱动矩阵Gk-1分别为：
进一步的，所述步骤二具体为，
卡尔曼滤波的状态向量为INS导航参数误差，以及陀螺仪和加速度计的剩余零偏，
其中，φ为失准角，δv为速度误差，δL为纬度误差，δλ为经度误差，δd为深度误差，ε为陀螺零偏，为加速度计零偏；
所述INS/PS分别采用松组合方式校正深度、紧组合方式校正速度及采用基于声双程传播模型的INS/LBL紧组合方式校正经纬度。
进一步的，当INS/PS采用松组合方式校正深度时；考虑到PS和INS之间存在较长的杆臂，两个位置的深度转换关系为：
其中，dPS为PS处的深度，dINS为INS处的深度，为速度与位置间的转换矩阵，/>为PS和INS之间的杆臂距离。下面推导INS/PS子系统的量测方程
其中，为PS输出的深度，/>为INS输出的深度，/>为INS输出的深度转换到PS位置处，ZPS为INS/PS子系统的量测向量，HPS为对应的量测矩阵。
进一步的，当INS/DVL采用紧组合方式校正速度时，定义DVL设备坐标系为d系，定义DVL的四个波束坐在坐标系为B系，两坐标系之间的转换矩阵为：
其中，当DVL采用“+”型安装时，α为0°；当DVL采用“X”型安装时，α为45°；β为DVL波束倾角，可通过查阅DVL手册获得。下面给出INS/DVL紧组合的公式。
其中，ZDVL为INS/DVL子系统的量测向量，为INS输出的速度，/>为DVL速度误差，/>为DVL和INS之间的杆臂，HDVL为INS/DVL子系统的量测矩阵，其余符号中间变量。
进一步的，当采用基于声双程传播模型的INS/LBL紧组合方式校正经纬度时，
其中，为单个LBL信标的声双程传播距离，/>为INS反演的声双程传播距离，ρT为查询信号的传输距离，ρR为应答信号的传输距离，τa为查询信号的传输时间，τb为信标中的信号处理时延，τc为应答信号的传输时间，δτLBL为LBL信号传输时间的误差，c为平均海水声速；对式(25)进行一阶泰勒展开，
其中，(xi，yi，zi)为LBL信标的ECEF坐标，为AUV端LBL换能器发射查询信号时的ECEF坐标，(xR，yR，zR)为AUV端LBL换能器接收应答信号时的ECEF坐标；ECEF坐标和大经纬度、深度的转换关系为：
其中，Ll、λl和dl分别为AUV端LBL换能器接收应答信号时的纬度、经度和深度，δLl、δλl和δdl分别为其对应的误差；
进一步的，INS/LBL子系统量测向量的构造方法具体为，
其中，δxl、δyl和δzl分别为AUV端LBL换能器接收应答信号时的ECEF坐标误差；
进一步的，所述步骤三具体为，
其中，κa为失准角的自适应反馈系数，κv为速度误差的自适应反馈系数，κp为位置误差的自适应反馈系数，κε为陀螺零偏的自适应反馈系数，为加速度计零偏的自适应反馈系数，/>和/>分别为反馈校正前、后的陀螺零偏，/>和/>分别为反馈校正前、后的加速度计零偏，/>和/>分别为反馈校正前、后的tk时刻陀螺角度增量，/>和/>分别为反馈校正前、后的tk时刻加速度计速度增量。自适应反馈系数的计算公式如下：
其中，σj为状态向量中第j维分量的可观测度，P0(jj)为初始均方误差阵的第j个对角线元素，Pk(jj)为k时刻均方误差阵的第j个对角线元素。
一种基于INS/DVL/LBL紧组合的AUV导航系统，使用如上述的AUV导航方法，
所述AUV导航系统包括INS模块和实时校正模块；
所述INS模块，使用机械编排算法解算姿态、速度和位置；
所述实时校正模块，采用卡尔曼滤波算法融合INS/DVL/LBL/PS的导航参数，实时校正INS的误差；对INS的姿态、速度和位置误差进行反馈校正；
所述AUV导航系统配合导航计算机使用，基于姿态、速度和位置误差的反馈校正实现AUV导航；
所述AUV导航系统对姿态角、速度、位置、角速度和加速度进行误差修正。本发明的有益效果是：
本发明在DVL部分波束无效时可对INS进行速度校正。
本发明在有效LBL应答器信号小于四个时，可对INS进行位置校正。
本发明不要求AUV和LBL应答器时钟同步。
本发明在DVL部分波束无效时可对INS进行速度约束。
本发明对AUV运动和静止均适用。
附图说明
图1是导致DVL底跟踪无效的典型工况。
图2是LBL定位原理示意图。
图3是本发明的INS/DVL/LBL紧组合方法示意图。
图4是本发明的INS/DVL/LBL/IPS数据融合示意图。
图5是本发明的AUV某次航行轨迹。
图6是本发明的组合导航时的位置误差，包括北向误差、动向误差、水平误差和深度误差。
图7是本发明的方法流程图。
具体实施方式
下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明通过对AUV航行数据进行半物理仿真，本方案可绝对定位误差优于0.5mRMS，可满足国际海道测量组织(International Hydrographic Organization)的最高导航精度要求，即Exclusive Order标准。
一种基于INS/DVL/LBL紧组合的AUV导航方法，所述AUV导航方法包括以下步骤，
步骤一、进行INS机械编排，其中INS机械编排为指北方位力学的机械编排；
步骤二、采用卡尔曼滤波算法融合INS/DVL/LBL/IPS的导航参数，实时校正INS的误差；
步骤三、基于步骤二的实时校正INS的误差，对INS的姿态、速度和位置误差进行反馈校正；
步骤四、基于姿态、速度和位置误差的反馈校正实现AUV导航。
进一步的，所述步骤1具体为，所述INS机械编排划分为姿态、速度和位置更新；
所述北方位力学的机械编排中带求解的微分方程组为公式(46)
/>
其中，为方向余弦矩阵，/>为b系相对于n系的角速度，vn为AUV在北东地方向的速度，/>为比力，/>为地球自转角速度，gn为重力加速度，p为AUV的经纬度和深度，Mpv为速度与位置的转换矩阵。
进一步的，所述步骤2具体为，状态空间模型可表示为公式(2)。
公式(1)的离散化求解采用双子样算法实现。方向余弦矩阵微分方程的求解公式如下：
其中，T为陀螺仪和加速度计的采样间隔，Δθk-1和Δθk分别为k-1时刻和k时刻的角度增量，其余变量为中间变量。
速度微分方程的求解公式如下：
其中，Δvk-1和Δvk分别为k-1时刻和k时刻的速度增量，其余变量为中间变量。
位置微分方程的求解公式如下：
其中，RN和RE分别为子午圈和卯酉圈主曲率半径，L为纬度，λ为经度，d为深度，Re为地球长半轴，e为地球椭圆偏心率。
3.根据权利要求1所述一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，所述步骤2具体为，状态空间模型可表示为公式(51)。
其中，Xk为状态向量，Zk为观测向量，Wk-1和Vk分别为系统噪声和量测噪声。状态转移矩阵Fk、系统噪声驱动矩阵Gk-1分别为：
进一步的，所述步骤2具体为，
卡尔曼滤波的状态向量为INS导航参数误差，以及陀螺仪和加速度计的剩余零偏，
其中，φ为失准角，δv为速度误差，δL为纬度误差，δλ为经度误差，δd为深度误差，ε为陀螺零偏，为加速度计零偏。
在本方案中，INS/PS采用松组合方式校正深度。考虑到PS和INS之间存在较长的杆臂，两个位置的深度转换关系为：
其中，dPS为PS处的深度，dINS为INS处的深度，为速度与位置间的转换矩阵，/>为PS和INS之间的杆臂距离。下面推导INS/PS子系统的量测方程
/>
其中，为PS输出的深度，/>为INS输出的深度，/>为INS输出的深度转换到PS位置处，ZOS为INS/PS子系统的量测向量，HPS为对应的量测矩阵。
在本方案中，INS/DVL采用紧组合方式校正速度。定义DVL设备坐标系为d系，定义DVL的四个波束坐在坐标系为B系，两坐标系之间的转换矩阵为：
其中，当DVL采用“+”型安装时，α为0°；当DVL采用“X”型安装时，α为45°；β为DVL波束倾角，可通过查阅DVL手册获得。下面给出INS/DVL紧组合的公式。
其中，ZDVL为INS/DVL子系统的量测向量，为INS输出的速度，/>为DVL速度误差，/>为DVL和INS之间的杆臂，HDVL为INS/DVL子系统的量测矩阵，其余符号中间变量。
本方案采用基于声双程传播模型的INS/LBL紧组合方式校正经纬度。
其中，为单个LBL信标的声双程传播距离，/>为INS反演的声双程传播距离，ρT为查询信号的传输距离，ρR为应答信号的传输距离，τa为查询信号的传输时间，τb为信标中的信号处理时延，τc为应答信号的传输时间，δτLBL为LBL信号传输时间的误差，c为平均海水声速。对式(25)进行一阶泰勒展开，
其中，(xi，yi，zi)为LBL信标的ECEF坐标，为AUV端LBL换能器发射查询信号时的ECEF坐标，(xR，yR，zR)为AUV端LBL换能器接收应答信号时的ECEF坐标。ECEF坐标和大经纬度、深度的转换关系为：/>
其中，Ll、λl和dl分别为AUV端LBL换能器接收应答信号时的纬度、经度和深度，δLl、δλl和δdl分别为其对应的误差。下面给出INS/LBL子系统量测向量的构造方法：
其中，δxl、δyl和δzl分别为AUV端LBL换能器接收应答信号时的ECEF坐标误差。
/>
/>
进一步的，所述步骤3具体为，
其中，κa为失准角的自适应反馈系数，κv为速度误差的自适应反馈系数，κp为位置误差的自适应反馈系数，κε为陀螺零偏的自适应反馈系数，为加速度计零偏的自适应反馈系数，/>和/>分别为反馈校正前、后的陀螺零偏，/>和/>分别为反馈校正前、后的加速度计零偏，/>和/>分别为反馈校正前、后的tk时刻陀螺角度增量，/>和/>分别为反馈校正前、后的tk时刻加速度计速度增量。自适应反馈系数的计算公式如下：
其中，σj为状态向量中第j维分量的可观测度，P0(jj)为初始均方误差阵的第j个对角线元素，Pk(jj)为k时刻均方误差阵的第j个对角线元素。
一种基于INS/DVL/LBL紧组合的AUV导航系统，使用如所述的AUV导航方法，
所述AUV导航系统包括INS模块和实时校正模块；
所述INS模块，用以指北方位力学的机械编排；
所述实时校正模块，采用卡尔曼滤波算法融合INS/DVL/LBL/IPS的导航参数，实时校正INS的误差；对INS的姿态、速度和位置误差进行反馈校正；
所述AUV导航系统配合导航计算机使用，基于姿态、速度和位置误差的反馈校正实现AUV导航。
一种基于INS/DVL/LBL紧组合的AUV导航系统，所述AUV导航系统对导航角、姿态角、速度、位置、角速度和加速度进行误差修正。
本发明使用LBL应答器的声双程传播时间作为观测量，无需AUV和应答器时间同步。
本发明基于可观测度分析理论，提出自适应INS误差反馈方法。对于可观测性弱的状态，降低其反馈系数；而对于可观测性强的状态，提高其反馈系数。
本发明通过使用紧组合算法，可以实现：当DVL有效波束数量小于三个时，仍可对INS速度误差进行校正；当LBL有效应答信号数量小于四个时，仍可对INS位置误差进行校正。
如图6所述本发明的AUV水平航行距离为1700.971m，绝对定位误差为0.315m RMS，可满足国际海道测量组织(International Hydrographic Organization)的最高导航精度要求，即Exclusive Order标准。
Claims (10)
1.一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，所述AUV导航方法包括以下步骤，
步骤一、采用经过零偏补偿的陀螺仪和加速度计数据，进行机械编排；
步骤二、采用尔曼滤波算法融合INS/DVL/LBL/PS等传感器数据，实时估计INS姿态、速度和位置的误差，并实时估计陀螺仪和加速度计的剩余零偏；
步骤三、基于步骤二的实时估计INS的误差，对INS的姿态、速度、位置误差进行自适应反馈校正，并对陀螺仪和加速度计零偏进行累加；
步骤四、经过步骤三反馈校正的INS姿态、速度和位置，作为AUV的导航信息。
2.根据权利要求1所述一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于：所述步骤一具体为，所述INS机械编排划分为姿态、速度和位置更新；
所述INS机械编排中待求解的微分方程组为公式(1)
其中，为方向余弦矩阵，/>为b系相对于n系的角速度，vn为AUV在北东地方向的速度，/>为比力，/>为地球自转角速度，gn为重力加速度，p为AUV的经纬度和深度，Mpv为速度与位置的转换矩阵；
公式(1)的离散化求解采用双子样算法实现；方向余弦矩阵微分方程的求解公式如下：
其中，T为陀螺仪和加速度计的采样间隔，Δθk-1和Δθk分别为k-1时刻和k时刻的角度增量，其余变量为中间变量；
速度微分方程的求解公式如下：
其中，Δvk-1和Δvk分别为k-1时刻和k时刻的速度增量，其余变量为中间变量；
位置微分方程的求解公式如下：
其中，RN和RE分别为子午圈和卯酉圈主曲率半径，L为纬度，λ为经度，d为深度，Re为地球长半轴，e为地球椭圆偏心率。
3.根据权利要求1所述一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，所述步骤2具体为，状态空间模型可表示为公式(2)，
其中，Xk为状态向量，Zk为观测向量，Wk-1和Vk分别为系统噪声和量测噪声；状态转移矩阵Fk、系统噪声驱动矩阵Gk-1分别为：
4.根据权利要求3所述一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，所述步骤二具体为，
卡尔曼滤波的状态向量为INS导航参数误差，以及陀螺仪和加速度计的剩余零偏，
其中，φ为失准角，δv为速度误差，δL为纬度误差，δλ为经度误差，δd为深度误差，ε为陀螺零偏，为加速度计零偏；
所述INS/PS分别采用松组合方式校正深度、紧组合方式校正速度及采用基于声双程传播模型的INS/LBL紧组合方式校正经纬度。
5.根据权利要求4所述一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，当INS/PS采用松组合方式校正深度时；考虑到PS和INS之间存在较长的杆臂，两个位置的深度转换关系为：
其中，dPS为PS处的深度，dINS为INS处的深度，为速度与位置间的转换矩阵，/>为PS和INS之间的杆臂距离；下面推导INS/PS子系统的量测方程
其中，为PS输出的深度，/>为INS输出的深度，/>为INS输出的深度转换到PS位置处，ZPS为INS/PS子系统的量测向量，HPS为对应的量测矩阵。
6.根据权利要求4所述一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，当INS/DVL采用紧组合方式校正速度时，定义DVL设备坐标系为d系，定义DVL的四个波束坐在坐标系为B系，两坐标系之间的转换矩阵为：
其中，当DVL采用“+”型安装时，α为0°；当DVL采用“X”型安装时，α为45°；β为DVL波束倾角，可通过查阅DVL手册获得；下面给出INS/DVL紧组合的公式；
其中，ZDVL为INS/DVL子系统的量测向量，为INS输出的速度，/>为DVL速度误差，为DVL和INS之间的杆臂，HDVL为INS/DVL子系统的量测矩阵，其余符号中间变量。
7.根据权利要求4所述一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，当采用基于声双程传播模型的INS/LBL紧组合方式校正经纬度时，
其中，为单个LBL信标的声双程传播距离，/>为INS反演的声双程传播距离，ρT为查询信号的传输距离，ρR为应答信号的传输距离，τa为查询信号的传输时间，τb为信标中的信号处理时延，τc为应答信号的传输时间，δτLBL为LBL信号传输时间的误差，c为平均海水声速；对式(25)进行一阶泰勒展开，
其中，(xi，yi，zi)为LBL信标的ECEF坐标，为AUV端LBL换能器发射查询信号时的ECEF坐标，(xR，yR，zR)为AUV端LBL换能器接收应答信号时的ECEF坐标；ECEF坐标和大经纬度、深度的转换关系为：
其中，Ll、λl和dl分别为AUV端LBL换能器接收应答信号时的纬度、经度和深度，δLl、δλl和δdl分别为其对应的误差。
8.根据权利要求7所述一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，INS/LBL子系统量测向量的构造方法具体为，
其中，δxl、δyl和δzl分别为AUV端LBL换能器接收应答信号时的ECEF坐标误差；
9.根据权利要求2所述一种基于INS/DVL/LBL紧组合的AUV导航方法，其特征在于，所述步骤三具体为，
其中，κa为失准角的自适应反馈系数，κυ为速度误差的自适应反馈系数，κp为位置误差的自适应反馈系数，κε为陀螺零偏的自适应反馈系数，为加速度计零偏的自适应反馈系数，/>和/>分别为反馈校正前、后的陀螺零偏，/>和/>分别为反馈校正前、后的加速度计零偏，/>和/>分别为反馈校正前、后的tk时刻陀螺角度增量，/>和/>分别为反馈校正前、后的tk时刻加速度计速度增量；自适应反馈系数的计算公式如下：
其中，σj为状态向量中第j维分量的可观测度，P0(jj)为初始均方误差阵的第j个对角线元素，Pk(jj)为k时刻均方误差阵的第j个对角线元素。
10.一种基于INS/DVL/LBL紧组合的AUV导航系统，其特征在于，使用如权利要求1-9的AUV导航方法，
所述AUV导航系统包括INS模块和实时校正模块；
所述INS模块，使用机械编排算法解算姿态、速度和位置；
所述实时校正模块，采用卡尔曼滤波算法融合INS/DVL/LBL/PS的导航参数，实时校正INS的误差；对INS的姿态、速度和位置误差进行反馈校正；
所述AUV导航系统配合导航计算机使用，基于姿态、速度和位置误差的反馈校正实现AUV导航；
所述AUV导航系统对姿态角、速度、位置、角速度和加速度进行误差修正。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310419119.1A CN117029872A (zh) | 2023-04-19 | 2023-04-19 | 一种基于ins/dvl/lbl紧组合的auv导航方法及导航系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310419119.1A CN117029872A (zh) | 2023-04-19 | 2023-04-19 | 一种基于ins/dvl/lbl紧组合的auv导航方法及导航系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117029872A true CN117029872A (zh) | 2023-11-10 |
Family
ID=88637823
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310419119.1A Pending CN117029872A (zh) | 2023-04-19 | 2023-04-19 | 一种基于ins/dvl/lbl紧组合的auv导航方法及导航系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117029872A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117346795A (zh) * | 2023-12-05 | 2024-01-05 | 青岛智能导航与控制研究所 | 融合惯导、多普勒计程仪和长基线的潜航器组合导航方法 |
-
2023
- 2023-04-19 CN CN202310419119.1A patent/CN117029872A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117346795A (zh) * | 2023-12-05 | 2024-01-05 | 青岛智能导航与控制研究所 | 融合惯导、多普勒计程仪和长基线的潜航器组合导航方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109324330B (zh) | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 | |
| CN109737956B (zh) | 一种基于双应答器的sins/usbl相位差紧组合导航定位方法 | |
| US20160124081A1 (en) | Metrology method and device for calibrating the geometry of a network of underwater acoustic beacons | |
| CN101900558B (zh) | 集成声纳微导航的自主式水下机器人组合导航方法 | |
| CN108594272B (zh) | 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法 | |
| CN103697910B (zh) | 自主水下航行器多普勒计程仪安装误差的校正方法 | |
| CN104316045A (zh) | 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法 | |
| US20090287414A1 (en) | System and process for the precise positioning of subsea units | |
| KR100906362B1 (ko) | 2개의 기준점에 대한 거리정보와 저정밀도 관성센서를 이용한 무인잠수정 선단의 의사 lbl 수중항법시스템 | |
| CN102829777A (zh) | 自主式水下机器人组合导航系统及方法 | |
| Xu et al. | A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles | |
| CN111323012B (zh) | 载体高动态环境下的ins辅助dvl测速误差补偿方法 | |
| CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及系统 | |
| CN112684207B (zh) | 一种深潜载人潜水器adcp速度估计与修正算法 | |
| CN110389318B (zh) | 一种基于立体六元阵的水下移动平台定位系统及方法 | |
| JP2018084445A (ja) | 水中音響測位システム | |
| CN117029872A (zh) | 一种基于ins/dvl/lbl紧组合的auv导航方法及导航系统 | |
| CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
| US20200109949A1 (en) | Method and apparatus for self-contained positioning of a mobile robot inside a tank | |
| CN109269496A (zh) | 双多普勒自适应切换测速的uuv深海组合导航装置及方法 | |
| AU2005268886A1 (en) | Method for an antenna angular calibration by relative distance measuring | |
| Chen | In-situ alignment calibration of attitude and ultra short baseline sensors for precision underwater positioning | |
| CN112083425B (zh) | 一种引入径向速度的sins/lbl紧组合导航方法 | |
| CN116222578B (zh) | 基于自适应滤波和最优平滑的水下组合导航方法及系统 | |
| Allotta et al. | Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements |
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 |