CN116839574A - 一种基于mcckf的管道潜航机器人组合导航方法、装置、存储介质及设备 - Google Patents
一种基于mcckf的管道潜航机器人组合导航方法、装置、存储介质及设备 Download PDFInfo
- Publication number
- CN116839574A CN116839574A CN202310893246.5A CN202310893246A CN116839574A CN 116839574 A CN116839574 A CN 116839574A CN 202310893246 A CN202310893246 A CN 202310893246A CN 116839574 A CN116839574 A CN 116839574A
- Authority
- CN
- China
- Prior art keywords
- navigation
- error
- nonlinear
- equation
- data
- 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 33
- 238000003860 storage Methods 0.000 title claims abstract description 12
- 238000005259 measurement Methods 0.000 claims abstract description 72
- 238000001914 filtration Methods 0.000 claims abstract description 46
- 230000006870 function Effects 0.000 claims description 51
- 239000013598 vector Substances 0.000 claims description 43
- 238000004590 computer program Methods 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000009826 distribution Methods 0.000 claims description 5
- 230000014509 gene expression Effects 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 238000005457 optimization Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 7
- 238000012545 processing Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- VNWKTOKETHGBQD-UHFFFAOYSA-N methane Chemical compound C VNWKTOKETHGBQD-UHFFFAOYSA-N 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005315 distribution function Methods 0.000 description 1
- 239000010842 industrial wastewater Substances 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 239000003345 natural gas Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000001902 propagating effect Effects 0.000 description 1
- 239000010865 sewage Substances 0.000 description 1
- 239000000126 substance Substances 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
- 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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- 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/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于MCCKF的管道潜航机器人组合导航方法、装置、存储介质及设备，属于管道潜航机器人导航技术领域，方法包括：获取管道潜航机器人的导航数据，管道潜航机器人上搭载的组合导航系统包括捷联惯性系统和多普勒计程仪；根据所述导航数据构建捷联惯性系统的非线性误差模型；根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程；将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，利用所述滤波数据对所述导航数据进行校正得到优化导航数据；基于所述优化导航数据完成组合导航；本发明能够提高组合导航的准确性和稳定性。
Description
技术领域
本发明涉及一种基于MCCKF的管道潜航机器人组合导航方法、装置、存储介质及设备，属于管道潜航机器人导航技术领域。
背景技术
近年来，随着城市建设日渐完善，地下管网已然成为我们日常生活和工业等方面重要的组成部分，在城市供排水、石油化工、天然气等方面有着广泛应用，其保证了城市正常运转。城市排水管道作为城市排水系统的重要内容之一，不仅需要处理日常生活污水和雨水的排放，还承担收集和输送工业废水的功能。然而地下的复杂环境和管道内物质都可能会对管道造成一定破坏，如果管道发生故障，不仅造成巨大经济损失，还可能危害人身安全。同时新旧管网的叠加使得管网规模更加错综复杂，这对地下管道的检测和维护提出了更高的要求
为了尽可能避免管道故障引起巨大事故的发生，需要我们及时检测到管道的隐患处并进行精确定位，而管道潜航机器人(Remote Operated Vehicle，ROV)具有高精度导航系统且可以长时间在水下工作，同时还可以装备多种传感器和设备以达到检测或水下作业等功能，是此工作的最佳之选。但由于管道中水下环境的复杂性，实现高精度导航是ROV所面临的主要挑战之一，而且高精度导航也是管道潜航机器人高效完成工作的关键。
捷联惯性导航系统(SINS)以其良好的独立性、抗干扰性和主动性而被广泛应用于管道潜航机器人，但长时间工作系统误差会累计增加，使得导航精度降低。使用多个导航设备数据进行融合可以很好地修正惯导的累积误差。水下组合导航系统采用的导航系统有多普勒计程仪(DVL)、全球卫星定位系统(GNSS)、罗盘、磁力计、长基线(LBL)和短基线(SBL)定位系统等等。在水下工作时，不能接收到GNSS信号，通常使用SINS/DVL组合导航，该导航系统利用DVL提供的速度信息对SINS导航误差进行估计补偿，从而使ROV具有较高的导航精度。
在组合导航系统中，卡尔曼滤波可以把不同导航设备的数据融合实现最优估计。为了获得较好的滤波估计，系统噪声和量测噪声通常为统计特性已知且满足高斯分布，采用KF能够获得很好的滤波效果。但是管道环境复杂多变，ROV大幅度运动时传感器的误差漂移会导致导航系统测量误差增加，从而使得测量噪声统计特性改变呈现重尾非高斯特征，导致采用传统KF的导航系统精度降低，影响导航系统的稳定性。
综上所述，现有技术中存在的缺陷包括：精度和稳定性低。
发明内容
本发明的目的在于提供一种基于MCCKF的管道潜航机器人组合导航方法、装置、存储介质及设备，解决现有技术中存在的精度和稳定性低的问题。
为实现以上目的，本发明是采用下述技术方案实现的：
第一方面，本发明提供了一种基于MCCKF的管道潜航机器人组合导航方法，包括：
获取管道潜航机器人的导航数据，管道潜航机器人上搭载的组合导航系统包括捷联惯性系统和多普勒计程仪；
根据所述导航数据构建捷联惯性系统的非线性误差模型；
根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程；
将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，利用所述滤波数据对所述导航数据进行校正得到优化导航数据；
基于所述优化导航数据完成组合导航。
结合第一方面，进一步的，所述导航数据包括捷联惯性导航系统输出的速度、位置和姿态，以及多普勒计程仪输出的速度。
结合第一方面，进一步的，所述根据所述导航数据构建管道潜航机器人上捷联惯性系统的非线性误差模型，包括：
选取东北天坐标系作为导航坐标系，选取管道潜航机器人自身坐标系作为载体坐标系，在导航坐标系和载体坐标系下构建的非线性误差模型包括速度误差方程、姿态误差方程和位置误差方程；
所述速度误差方程为：
其中，为速度误差微分；/>为载体坐标系到导航坐标系的姿态转换阵；φ为失准角误差，φ＝[φE φN φU]，φE、φN和φU分别为俯仰角误差、横滚角误差和偏航角误差，为地球自转角速度，/>为导航坐标系相对地球坐标系的旋转角速度，/>和/>分别为/>和/>的误差；vn为载体的速度在导航坐标系下的投影，δvn为速度误差， 和/>分别为载体的东向速度误差、北向速度误差和天向速度误差；fb为加速度计输出的比力，δfb为加速度计输出的比力的测量误差， 为加速度零偏，ωg为加速度随机噪声；
所述姿态误差方程为：
其中，为失准角误差微分，/>为导航坐标系相对于惯性坐标系旋转角速度在导航坐标系下的投影，/> 为/>的误差，/>为陀螺漂移误差，εb为随机常值漂移，ωφ为陀螺仪随机噪声；
所述位置误差方程为：
其中，L、λ和h分别表示纬度、经度和高度，δL、δλ和δh分别表示纬度误差、经度误差和高度误差，和/>分别表示纬度误差微分、经度误差微分和高度误差微分，/>和分别表示载体的东向速度和北向速度，RN和RM分别表示卯酉圈曲率半径和子午圈曲率半径，tan为正弦函数，sec为正割函数。
结合第一方面，进一步的，所述根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程，包括：
选取非线性误差模型中产生的以下数据组成状态向量：捷联惯性导航系统的速度误差和位置误差的前两维、三维姿态角误差、三维加速度计零偏和三维陀螺仪零偏，根据状态向量建立如下非线性状态方程：
其中，是状态向量的微分，f(x，t)是t时刻的非线性状态连续函数，w(t)是t时刻的陀螺仪和加速度计随机噪声；
所述状态向量的表达式为：
其中，和/>分别表示x、y和z维的加速度零偏，εx、εy和εz分别表示x、y和z维的陀螺仪零偏，T是向量的转置符号；
根据非线性误差模型中产生的以及多普勒计程仪输出的以下数据构建量测方程：捷联惯性导航系统的东向速度和北向速度、多普勒计程仪输出的东向速度和北向速度，构建的量测方程如下：
其中，z为量测向量，h(x，t)为t时刻的非线性量测连续函数，v(t)为量测噪声，和/>分别表示多普勒计程仪输出的东向速度和北向速度。
结合第一方面，进一步的，在所述构建组合导航系统的非线性状态方程和量测方程后，对所述非线性状态方程和所述量测方程离散化，离散化的结果如下：
xk＝f(xk-1)+wk-1
zk＝h(xk)+vk
其中，xk是离散化的状态向量，f(xk-1)是离散化的非线性状态连续函数，wk-1是离散化的陀螺仪和加速度计随机噪声，zk是离散化的量测向量，h(xk)是离散化的非线性量测连续函数，vk是离散化的量测噪声；其中，wk-1满足均值为0、协方差为Qk的高斯分布，vk满足均值为0、协方差为Rk的重尾非高斯分布。
结合第一方面，进一步的，所述将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，包括：
在现有的容积卡尔曼滤波的基础上，做出如下改进：在构建代价函数时使用学生t核函数替换高斯核函数构建最大相关熵代价函数，在求解最优状态估计时采用定点迭代方法进行求解；
所述最大相关熵代价函数的表达式为：
其中，n为状态向量的维数，m为量测向量的维数，Sv，σ(*)表示学生t核函数，ex，i表示状态预测误差向量ex的第i个分量，ey，i表示残差向量ey的第i个分量，通过构造状态预测误差向量和残差向量的代价函数，将求解最优状态估计问题转化为求解arg maxJ，arg max表示寻找具有最大评分的参量。
结合第一方面，进一步的，所述利用所述滤波数据对所述导航数据进行校正得到优化导航数据，包括：利用所述滤波数据中包含的误差数据对所述导航数据进行校正，得到优化导航数据。
第二方面，本发明还提供了一种基于MCCKF的管道潜航机器人组合导航装置，包括：
数据获取模块，被配置为：获取管道潜航机器人的导航数据，管道潜航机器人上搭载的组合导航系统包括捷联惯性系统和多普勒计程仪；
非线性误差模型构建模块，被配置为：根据所述导航数据构建捷联惯性系统的非线性误差模型；
非线性状态方程和量测方程构建模块，被配置为：根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程；
数据优化模块，被配置为：将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，利用所述滤波数据对所述导航数据进行校正得到优化导航数据；
组合导航模块，被配置为：基于所述优化导航数据完成组合导航。
第三方面，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时，实现如第一方面中任一项所述的基于MCCKF的管道潜航机器人组合导航方法。
第四方面，本发明还提供了一种设备，包括：
存储器，用于存储指令；
处理器，用于执行所述指令，使得所述设备执行实现如第一方面中任一项所述的基于MCCKF的管道潜航机器人组合导航方法的操作。
与现有技术相比，本发明所达到的有益效果是：
本发明提供的一种基于MCCKF的管道潜航机器人组合导航方法、装置、存储介质及设备，构建了捷联惯性系统的非线性误差模型，并采用最大相关熵容积卡尔曼滤波对导航信息进行融合估计，提高导航的精确度和稳定性，从而确保管道潜航机器人顺利高效完成管道检测定位任务；
将最大相关熵相关原理引入容积卡尔曼滤波算法中，使用学生t核函数替换高斯核函数构建最大相关熵代价函数，使得该滤波算法对于系统测量噪声为非高斯分布有更好的估计效果，并利用定点迭代求解得到最优状态估计，提高了滤波算法的精度。
附图说明
图1是本发明实施例提供的一种基于MCCKF的管道潜航机器人组合导航方法的流程图；
图2是本发明实施例提供的组合导航系统的示意图；
图3是本发明实施例提供的最大相关熵容积卡尔曼滤波算法的流程图。
具体实施方式
下面结合附图对本发明作进一步描述，以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
实施例1
如图1所示，本发明实施例提供了一种基于MCCKF的管道潜航机器人组合导航方法，包括以下步骤：
S1、获取管道潜航机器人的导航数据，管道潜航机器人上搭载的组合导航系统(如图2所示)包括捷联惯性系统(SINS)和多普勒计程仪(DVL)。
管道潜航机器人的导航数据包括捷联惯性导航系统输出的速度、位置和姿态，以及多普勒计程仪输出的速度。
S2、根据所述导航数据构建捷联惯性系统的非线性误差模型。
非线性误差模型的构建过程如下：选取东北天坐标系作为导航坐标系(n系)，选取管道潜航机器人自身坐标系作为载体坐标系(b系)，在导航坐标系和载体坐标系下构建的非线性误差模型包括速度误差方程、姿态误差方程和位置误差方程；
速度误差方程为：
其中，为速度误差微分；/>为载体坐标系到导航坐标系的姿态转换阵；φ为失准角误差，φ＝[φE φN φU]，φE、φN和φU分别为俯仰角误差、横滚角误差和偏航角误差，为地球自转角速度，/>为导航坐标系相对地球坐标系的旋转角速度，/>和/>分别为/>和/>的误差；vn为载体的速度在导航坐标系下的投影，δvn为速度误差， 和/>分别为载体的东向速度误差、北向速度误差和天向速度误差；fb为加速度计输出的比力，δfb为加速度计输出的比力的测量误差， 为加速度零偏，ωg为加速度随机噪声；
姿态误差方程为：
其中，为失准角误差微分，/>为导航坐标系相对于惯性坐标系旋转角速度在导航坐标系下的投影，/> 为/>的误差，/>为陀螺漂移误差，εb为随机常值漂移，ωφ为陀螺仪随机噪声；
位置误差方程为：
其中，L、λ和h分别表示纬度、经度和高度，δL、δλ和δh分别表示纬度误差、经度误差和高度误差，和/>分别表示纬度误差微分、经度误差微分和高度误差微分，/>和分别表示载体的东向速度和北向速度，RN和RM分别表示卯酉圈曲率半径和子午圈曲率半径，tan为正弦函数，sec为正割函数。
S3、根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程。
构建以非线性误差模型为基础的非线性状态方程和量测方程。
选取非线性误差模型中产生的以下数据组成状态向量：捷联惯性导航系统的速度误差和位置误差的前两维、三维姿态角误差、三维加速度计零偏和三维陀螺仪零偏，状态向量的表达式为：
其中，和/>分别表示x、y和z维的加速度零偏，εx、εy和εz分别表示x、y和z维的陀螺仪零偏，T是向量的转置符号。
根据状态向量建立如下非线性状态方程：
其中，是状态向量的微分，f(x，t)是t时刻的非线性状态连续函数，w(t)是t时刻的陀螺仪和加速度计随机噪声。
根据非线性误差模型中产生的以及多普勒计程仪输出的以下数据构建量测方程：捷联惯性导航系统的东向速度和北向速度、多普勒计程仪输出的东向速度和北向速度，构建的量测方程如下：
其中，z为量测向量，h(x，t)为t时刻的非线性量测连续函数，v(t)为量测噪声，和/>分别表示多普勒计程仪输出的东向速度和北向速度。
S4、将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，利用所述滤波数据对所述导航数据进行校正得到优化导航数据。
为了滤波算法处理数据方便，非线性状态方程和量测方程得到后需要进行离散化，以采样周期Ts为滤波时间步长，将状态方程和量测方程离散化，得到离散化后的滤波模型：
xk＝f(xk-1)+wk-1
zk＝h(xk)+vk
其中，xk是离散化的状态向量，f(xk-1)是离散化的非线性状态连续函数，wk-1是离散化的陀螺仪和加速度计随机噪声，zk是离散化的量测向量，h(xk)是离散化的非线性量测连续函数，vk是离散化的量测噪声；其中，wk-1满足均值为0、协方差为Qk的高斯分布，vk满足均值为0、协方差为Rk的重尾非高斯分布。
随后使用最大相关熵容积卡尔曼滤波(MCCKF)对SINS和DVL输出信息进行融合估计，其流程如图3所示，具体步骤包括：
时间更新方程：估计方差Pk-1Cholesky分解得根据容积求积分规则，计算容积点/>其中，/>n为状态向量维数，{1}i表示n维单位球面与坐标轴的第i个交点坐标；通过非线性状态方程传播容积点，获得相应的状态采样点/>由此可得k时刻的一步状态预测/>和一步预测方差Pk|k-1：
量测更新方程：一步预测协方差矩阵Pk|k-1Cholesky分解得 量测噪声协方差Rk Cholesky分解得/>计算容积点根据量测方程传播容积点，获得量测采样点Zi，k|k-1＝h(Xi，k|k-1)。由此得出k时刻的一步量测预测/>状态量测交叉协方差Pxz，k|k-1和量测协方差为Pzz，k|k-1：
基于最大相关熵原理，通过建立代价函数并求解其最大值得到k时刻的第i次迭代状态
两个随机变量的相关熵表示如下：
V(X，Y)＝E(κ(X，Y))＝∫∫κ(x，y)pX，Y(x，y)dxdy
其中，相关熵V(X，Y)可以度量的两个变量广义相似度；κ(X，Y)为核函数，本发明采用学生t核函数Sv，σ(e)，e＝X-Y，v用来控制核函数的形状，σ为核带宽；pX，Y(x，y)为联合概率分布函数；E(·)表示数学期望；当采用学生t核函数时，建立基于最大熵准则的代价函数为：
其中，n为状态向量的维数，m为量测向量的维数，Sv，σ(*)表示学生t核函数，ex，i表示状态预测误差向量ex的第i个分量，ey，i表示残差向量ey的第i个分量，通过构造状态预测误差向量和残差向量的代价函数，将求解最优状态估计问题转化为求解arg maxJ，arg max表示寻找具有最大评分的参量。
滤波更新方程：由最大熵准则的代价函数对求偏导，经过定点迭代的/>更新得到最优状态估计/>滤波增益矩阵/>和估计误差协方差Pk：
式中：
定点迭代更新具体步骤如下：
(1)给定和收敛门限ε，/>表示k时刻的第i次迭代；
(2)将替换成/>计算ex，ey，Λx和Λy；
(3)进而计算 和/>得到/>
(4)如果满足则得到最优估计/>否则返回步骤(2)继续迭代更新。
如图2所示，利用滤波数据中包含的误差数据对导航数据进行校正，得到优化导航数据，通过组合导航系统输出。
S5、基于所述优化导航数据完成组合导航。
实施例2
本发明实施例还提供了一种基于MCCKF的管道潜航机器人组合导航装置，包括：
数据获取模块，被配置为：获取管道潜航机器人的导航数据，管道潜航机器人上搭载的组合导航系统包括捷联惯性系统和多普勒计程仪；
非线性误差模型构建模块，被配置为：根据所述导航数据构建捷联惯性系统的非线性误差模型；
非线性状态方程和量测方程构建模块，被配置为：根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程；
数据优化模块，被配置为：将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，利用所述滤波数据对所述导航数据进行校正得到优化导航数据；
组合导航模块，被配置为：基于所述优化导航数据完成组合导航。
实施例3
本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时，实现如实施例1提供的基于MCCKF的管道潜航机器人组合导航方法：
获取管道潜航机器人的导航数据，管道潜航机器人上搭载的组合导航系统包括捷联惯性系统和多普勒计程仪；
根据所述导航数据构建捷联惯性系统的非线性误差模型；
根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程；
将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，利用所述滤波数据对所述导航数据进行校正得到优化导航数据；
基于所述优化导航数据完成组合导航。
实施例4
本发明实施例还提供了一种设备，包括：
存储器，用于存储指令；
处理器，用于执行所述指令，使得所述设备执行实现如实施例1提供的基于MCCKF的管道潜航机器人组合导航方法的操作：
获取管道潜航机器人的导航数据，管道潜航机器人上搭载的组合导航系统包括捷联惯性系统和多普勒计程仪；
根据所述导航数据构建捷联惯性系统的非线性误差模型；
根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程；
将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，利用所述滤波数据对所述导航数据进行校正得到优化导航数据；
基于所述优化导航数据完成组合导航。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。
Claims (10)
1.一种基于MCCKF的管道潜航机器人组合导航方法，其特征在于，包括：
获取管道潜航机器人的导航数据，管道潜航机器人上搭载的组合导航系统包括捷联惯性系统和多普勒计程仪；
根据所述导航数据构建捷联惯性系统的非线性误差模型；
根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程；
将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，利用所述滤波数据对所述导航数据进行校正得到优化导航数据；
基于所述优化导航数据完成组合导航。
2.根据权利要求1所述的基于MCCKF的管道潜航机器人组合导航方法，其特征在于，所述导航数据包括捷联惯性导航系统输出的速度、位置和姿态，以及多普勒计程仪输出的速度。
3.根据权利要求1所述的基于MCCKF的管道潜航机器人组合导航方法，其特征在于，所述根据所述导航数据构建管道潜航机器人上捷联惯性系统的非线性误差模型，包括：
选取东北天坐标系作为导航坐标系，选取管道潜航机器人自身坐标系作为载体坐标系，在导航坐标系和载体坐标系下构建的非线性误差模型包括速度误差方程、姿态误差方程和位置误差方程；
所述速度误差方程为：
其中，为速度误差微分；/>为载体坐标系到导航坐标系的姿态转换阵；φ为失准角误差，φ＝[φE φN φU]，φE、φN和φU分别为俯仰角误差、横滚角误差和偏航角误差，/>为地球自转角速度，/>为导航坐标系相对地球坐标系的旋转角速度，/>和/>分别为/>和/>的误差；vn为载体的速度在导航坐标系下的投影，δvn为速度误差， 和/>分别为载体的东向速度误差、北向速度误差和天向速度误差；fb为加速度计输出的比力，δfb为加速度计输出的比力的测量误差， 为加速度零偏，ωg为加速度随机噪声；
所述姿态误差方程为：
其中，为失准角误差微分，/>为导航坐标系相对于惯性坐标系旋转角速度在导航坐标系下的投影，/> 为/>的误差，/>为陀螺漂移误差，εb为随机常值漂移，ωφ为陀螺仪随机噪声；
所述位置误差方程为：
其中，L、λ和h分别表示纬度、经度和高度，δL、δλ和δh分别表示纬度误差、经度误差和高度误差，和/>分别表示纬度误差微分、经度误差微分和高度误差微分，/>和/>分别表示载体的东向速度和北向速度，RN和RM分别表示卯酉圈曲率半径和子午圈曲率半径，tan为正弦函数，sec为正割函数。
4.根据权利要求3所述的基于MCCKF的管道潜航机器人组合导航方法，其特征在于，所述根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程，包括：
选取非线性误差模型中产生的以下数据组成状态向量：捷联惯性导航系统的速度误差和位置误差的前两维、三维姿态角误差、三维加速度计零偏和三维陀螺仪零偏，根据状态向量建立如下非线性状态方程：
其中，是状态向量的微分，f(x,t)是t时刻的非线性状态连续函数，w(t)是t时刻的陀螺仪和加速度计随机噪声；
所述状态向量的表达式为：
其中，和/>分别表示x、y和z维的加速度零偏，εx、εy和εz分别表示x、y和z维的陀螺仪零偏，T是向量的转置符号；
根据非线性误差模型中产生的以及多普勒计程仪输出的以下数据构建量测方程：捷联惯性导航系统的东向速度和北向速度、多普勒计程仪输出的东向速度和北向速度，构建的量测方程如下：
其中，z为量测向量，h(x,t)为t时刻的非线性量测连续函数，v(t)为量测噪声，和/>分别表示多普勒计程仪输出的东向速度和北向速度。
5.根据权利要求1所述的基于MCCKF的管道潜航机器人组合导航方法，其特征在于，在所述构建组合导航系统的非线性状态方程和量测方程后，对所述非线性状态方程和所述量测方程离散化，离散化的结果如下：
xk＝f(xk-1)+wk-1
zk＝h(xk)+vk
其中，xk是离散化的状态向量，f(xk-1)是离散化的非线性状态连续函数，wk-1是离散化的陀螺仪和加速度计随机噪声，zk是离散化的量测向量，h(xk)是离散化的非线性量测连续函数，vk是离散化的量测噪声；其中，wk-1满足均值为0、协方差为Qk的高斯分布，vk满足均值为0、协方差为Rk的重尾非高斯分布。
6.根据权利要求1所述的基于MCCKF的管道潜航机器人组合导航方法，其特征在于，所述将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，包括：
在现有的容积卡尔曼滤波的基础上，做出如下改进：在构建代价函数时使用学生t核函数替换高斯核函数构建最大相关熵代价函数，在求解最优状态估计时采用定点迭代方法进行求解；
所述最大相关熵代价函数的表达式为：
其中，n为状态向量的维数，m为量测向量的维数，Sv,σ(*)表示学生t核函数，ex,i表示状态预测误差向量ex的第i个分量，ey,i表示残差向量ey的第i个分量，通过构造状态预测误差向量和残差向量的代价函数，将求解最优状态估计问题转化为求解arg maxJ，arg max表示寻找具有最大评分的参量。
7.根据权利要求1所述的基于MCCKF的管道潜航机器人组合导航方法，其特征在于，所述利用所述滤波数据对所述导航数据进行校正得到优化导航数据，包括：利用所述滤波数据中包含的误差数据对所述导航数据进行校正，得到优化导航数据。
8.一种基于MCCKF的管道潜航机器人组合导航装置，其特征在于，包括：
数据获取模块，被配置为：获取管道潜航机器人的导航数据，管道潜航机器人上搭载的组合导航系统包括捷联惯性系统和多普勒计程仪；
非线性误差模型构建模块，被配置为：根据所述导航数据构建捷联惯性系统的非线性误差模型；
非线性状态方程和量测方程构建模块，被配置为：根据所述非线性误差模型和所述多普勒计程仪输出信息构建组合导航系统的非线性状态方程和量测方程；
数据优化模块，被配置为：将所述非线性状态方程和量测方程输入到最大相关熵容积卡尔曼滤波中，得到滤波数据，利用所述滤波数据对所述导航数据进行校正得到优化导航数据；
组合导航模块，被配置为：基于所述优化导航数据完成组合导航。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时，实现如权利要求1-7中任一项所述的基于MCCKF的管道潜航机器人组合导航方法。
10.一种设备，其特征在于，包括：
存储器，用于存储指令；
处理器，用于执行所述指令，使得所述设备执行实现如权利要求1-7中任一项所述的基于MCCKF的管道潜航机器人组合导航方法的操作。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310893246.5A CN116839574A (zh) | 2023-07-19 | 2023-07-19 | 一种基于mcckf的管道潜航机器人组合导航方法、装置、存储介质及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310893246.5A CN116839574A (zh) | 2023-07-19 | 2023-07-19 | 一种基于mcckf的管道潜航机器人组合导航方法、装置、存储介质及设备 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116839574A true CN116839574A (zh) | 2023-10-03 |
Family
ID=88163341
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310893246.5A Pending CN116839574A (zh) | 2023-07-19 | 2023-07-19 | 一种基于mcckf的管道潜航机器人组合导航方法、装置、存储介质及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116839574A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117346795A (zh) * | 2023-12-05 | 2024-01-05 | 青岛智能导航与控制研究所 | 融合惯导、多普勒计程仪和长基线的潜航器组合导航方法 |
-
2023
- 2023-07-19 CN CN202310893246.5A patent/CN116839574A/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 |
|---|---|---|
| CN109724599B (zh) | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 | |
| Nebot et al. | Initial calibration and alignment of low‐cost inertial navigation units for land vehicle applications | |
| Li et al. | A novel backtracking navigation scheme for autonomous underwater vehicles | |
| Wang et al. | Land vehicle dynamics-aided inertial navigation | |
| CN111596333B (zh) | 一种水下定位导航方法及系统 | |
| CN105806363B (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
| CN104655131A (zh) | 基于istssrckf的惯性导航初始对准方法 | |
| CN109945860A (zh) | 一种基于卫星紧组合的ins和dr惯性导航方法与系统 | |
| Xu et al. | A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles | |
| CN116839574A (zh) | 一种基于mcckf的管道潜航机器人组合导航方法、装置、存储介质及设备 | |
| CN110940340A (zh) | 基于小型uuv平台的多传感器信息融合方法 | |
| CN108344413A (zh) | 一种水下滑翔器导航系统及其低精度与高精度转换方法 | |
| Liu et al. | Modified sage-husa adaptive Kalman filter-based SINS/DVL integrated navigation system for AUV | |
| Wang et al. | A model-free calibration method of inertial navigation system and Doppler sensors | |
| Jameian et al. | A robust and fast self-alignment method for strapdown inertial navigation system in rough sea conditions | |
| Shin et al. | Navigation Kalman filter design for pipeline pigging | |
| Tang et al. | Invariant error-based integrated solution for SINS/DVL in earth frame: Extension and comparison | |
| Pei et al. | Initial self-alignment for marine rotary SINS using novel adaptive Kalman filter | |
| CN114777812A (zh) | 一种水下组合导航系统行进间对准与姿态估计方法 | |
| CN104101345A (zh) | 基于互补重构技术的多传感器姿态融合方法 | |
| CN103616026A (zh) | 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 | |
| Zhaopeng et al. | Online estimation of DVL misalignment angle in SINS/DVL integrated navigation system | |
| Ambrosovskaya et al. | Deep integration of INS and DP: From theory to experiments | |
| Ben et al. | System reset of strapdown INS for pipeline inspection gauge | |
| Golovan | INS/odometer integration: positional approach |
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 |