CN117451048A - 排水管道检测仪姿态解算方法及装置 - Google Patents
排水管道检测仪姿态解算方法及装置 Download PDFInfo
- Publication number
- CN117451048A CN117451048A CN202311315713.2A CN202311315713A CN117451048A CN 117451048 A CN117451048 A CN 117451048A CN 202311315713 A CN202311315713 A CN 202311315713A CN 117451048 A CN117451048 A CN 117451048A
- Authority
- CN
- China
- Prior art keywords
- data
- triaxial
- quaternion
- angular velocity
- magnetic induction
- 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 54
- 230000001133 acceleration Effects 0.000 claims abstract description 73
- 230000006698 induction Effects 0.000 claims abstract description 53
- 238000001914 filtration Methods 0.000 claims abstract description 34
- 238000004364 calculation method Methods 0.000 claims abstract description 16
- 230000008569 process Effects 0.000 claims abstract description 13
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 13
- 238000012937 correction Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 31
- 238000012545 processing Methods 0.000 claims description 14
- 230000002159 abnormal effect Effects 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 3
- 230000005358 geomagnetic field Effects 0.000 claims description 2
- 238000004590 computer program Methods 0.000 description 11
- 238000004891 communication Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 8
- 238000001514 detection method Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 238000013500 data storage Methods 0.000 description 4
- 238000003745 diagnosis Methods 0.000 description 4
- 230000004927 fusion Effects 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000003993 interaction Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 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/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
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
本发明的实施例提供了一种排水管道检测仪姿态解算方法及装置。该方法包括：获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据；利用三轴角速度数据对四元数进行先验估计；以先验估计后的四元数和三轴角速度常值偏差为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算排水管道检测仪的姿态角数据。以此方式，可以基于扩展卡尔曼滤波器将三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据进行融合，进而实现排水管道检测仪姿态解算，提高姿态角数据精度。
Description
技术领域
本发明涉及排水管网检测技术领域，尤其涉及一种排水管道检测仪姿态解算方法及装置。
背景技术
随着微机电传感器技术的发展，基于多传感器的姿态解算技术的应用也逐渐成熟，具备在排水管网领域应用的条件。排水管网检测领域检测成本高、检测步骤繁琐、检测周期长等痛点对相关从业人员提出了研发新技术的需求。应用单一陀螺仪的排水管网检测仪存在姿态解算有偏差，误差逐渐积累等问题；应用IMU组合即陀螺仪与加速度计的排水管网检测仪可以校正翻滚角和俯仰角上的误差，但由于加速度计的特点，无法校正陀螺仪的航偏角误差；而应用IMU和磁力计组合的排水管网检测仪在三个姿态角方向上都有比较好的精度，但需要注意运动加速度和随机磁场存在时对加速度计和磁力计的干扰。有鉴于此，如何提高姿态解算效果就成为了目前亟待解决的技术问题。
发明内容
本发明的实施例提供了一种排水管道检测仪姿态解算方法及装置。
第一方面，本发明的实施例提供了一种排水管道检测仪姿态解算方法，该方法包括：
获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据；
利用三轴角速度数据对四元数进行先验估计；
以先验估计后的四元数和三轴角速度常值偏差为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算排水管道检测仪的姿态角数据。
第二方面，本发明的实施例提供了一种排水管道检测仪姿态解算装置，该装置包括：
获取模块，用于获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据；
估计模块，用于利用三轴角速度数据对四元数进行先验估计；
计算模块，用于以先验估计后的四元数和三轴角速度常值偏差为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算排水管道检测仪的姿态角数据。
第三方面，本发明的实施例提供了一种电子设备，该电子设备包括：至少一个处理器；以及与至少一个处理器通信连接的存储器；存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行如上所述的方法。
第四方面，本发明的实施例提供了一种存储有计算机指令的非瞬时计算机可读存储介质，计算机指令用于使计算机执行如上所述的方法。
在本发明的实施例中，可以基于扩展卡尔曼滤波器将排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据进行融合，进而实现排水管道检测仪姿态解算，提高了排水管道检测仪的姿态角数据精度，对于重现检测仪在管道中的姿态，掌握排水管道运行状态具有重要意义。
应当理解，发明内容部分中所描述的内容并非旨在限定本发明的实施例的关键或重要特征，亦非用于限制本发明的范围。本发明的其它特征将通过以下的描述变得容易理解。
附图说明
结合附图并参考以下详细说明，本发明各实施例的上述和其他特征、优点及方面将变得更加明显。附图用于更好地理解本方案，不构成对本发明的限定在附图中，相同或相似的附图标记表示相同或相似的元素，其中：
图1示出了一种能够在其中实现本发明的实施例的示例性运行环境的示意图；
图2示出了本发明的实施例提供的一种排水管道检测仪姿态解算方法的流程图；
图3示出了本发明的实施例提供的一种扩展卡尔曼滤波器的实现示意图；
图4示出了本发明的实施例提供的一种排水管道检测仪姿态解算装置的结构图；
图5示出了一种能够实施本发明的实施例的示例性电子设备的结构图。
具体实施方式
为使本发明的实施例的目的、技术方案和优点更加清楚，下面将结合本发明的实施例中的附图，对本发明的实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的全部其他实施例，都属于本发明保护的范围。
另外，本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。
针对背景技术中出现的问题，本发明的实施例提供了一种排水管道检测仪姿态解算方法及装置。具体地，获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据；利用三轴角速度数据对四元数进行先验估计；以先验估计后的四元数和三轴角速度常值偏差为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算排水管道检测仪的姿态角数据。
如此一来，可以基于扩展卡尔曼滤波器将排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据进行融合，进而实现排水管道检测仪姿态解算，提高了排水管道检测仪的姿态角数据精度，对于重现检测仪在管道中的姿态，掌握排水管道运行状态具有重要意义。
下面结合附图，通过具体的实施例对本发明的实施例提供的排水管道检测仪姿态解算方法及装置进行详细地说明。
图1示出了一种能够在其中实现本发明的实施例的示例性运行环境的示意图，如图1所示，运行环境100中可以包括排水管道检测仪、终端设备。
其中，排水管道检测仪包括遥控开关、电源、微机电九轴惯性传感器、数据存储模块。终端设备可以是智能手机、平板电脑、笔记本电脑、掌上电脑或者超级移动个人计算机(Ultra-Mobile Personal Computer，UMPC)个人计算机(Personal Computer，PC)等，在此不做限制。
作为一个示例，用户通过检查井口将排水管道检测仪放入排水管道，在其接触水面时，通过遥控开关控制电源为微机电九轴惯性传感器和数据存储模块供电。
微机电九轴惯性传感器在启动之后用于采集排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据，并将其存储至数据存储模块。
示例性地，如图1所示，微机电九轴惯性传感器可以包括：陀螺仪、加速度计以及磁力计。
陀螺仪用于采集三轴角速度数据；
加速度计用于采集三轴加速度数据；
磁力计用于采集三轴磁感应强度数据。
在从排水管道中取出排水管道检测仪之后，用户将数据存储模块中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据导入终端设备中，由终端设备根据三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据进行排水管道检测仪姿态解算。
在一些实施例中，终端设备根据三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据进行排水管道检测仪姿态解算，可以包括以下方式：
以三轴角速度数据为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量，通过扩展卡尔曼滤波器进行数据融合，得到最优滤波值，此计算排水管道检测仪的姿态角数据，实现姿态解算，完成排水管道运行状态诊断。
或者，当三轴加速度数据受到运动加速度干扰时，采用三轴角速度数据和三轴磁感应强度数据通过扩展卡尔曼滤波器进行数据融合，得到最优滤波值，以此计算排水管道检测仪的姿态角数据，实现姿态解算，完成排水管道运行状态诊断。
或者，当三轴磁感应强度数据受到随机磁场干扰且无运动加速度干扰时，采用三轴角速度数据和三轴加速度计数据通过扩展卡尔曼滤波器进行数据融合，得到最优滤波值，以此计算排水管道检测仪的姿态角数据，实现姿态解算，完成排水管道运行状态诊断。
或者，当三轴磁感应强度数据受到随机磁场干扰且三轴加速度数据受到运动加速度干扰时，采用三轴角速度数据直接解算，计算排水管道检测仪的姿态角数据，实现姿态解算，完成排水管道运行状态诊断。
如此一来，终端设备可以根据三轴角速度数据、三轴加速度数据和三轴磁感应强度数据进行排水管道检测仪姿态解算，实现多传感器数据融合，计算得出相对准确的姿态角数据，实现对检测仪的姿态解算，便于后续根据检测仪的沿程姿态变化判断该排水管道的运行状况，完成对排水管道的缺陷检测。
图2示出了本发明的实施例提供的一种排水管道检测仪姿态解算方法的流程图，如图2所示，排水管道检测仪姿态解算方法200可以包括以下步骤：
S210，获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据。
其中，三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据均是时序数据，由排水管道检测仪中的微机电九轴惯性传感器采集。
具体地，三轴角速度数据由微机电九轴惯性传感器中的陀螺仪采集，三轴加速度数据由微机电九轴惯性传感器中的加速度计采集，三轴磁感应强度数据由微机电九轴惯性传感器中的磁力计采集。
S220，利用三轴角速度数据对四元数进行先验估计。
S230，以先验估计后的四元数和三轴角速度常值偏差为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算排水管道检测仪的姿态角数据。
在一些实施例中，可以以先验估计后的四元数参数和角速度零偏值为状态量，建立基于四元数更新方程的状态方程。
以三轴加速度数据和三轴磁感应强度数据为观测量，结合四元数表示的姿态矩阵，建立观测方程。
对三轴加速度数据和三轴磁感应强度数据进行异常值处理。
通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值。
利用四元数的最优滤波值，结合四元数与欧拉角转换关系，计算排水管道检测仪的姿态角数据。
作为一个示例，如图3所示，以先验估计后的四元数参数和陀螺仪角速度零偏值为状态量建立状态方程；以加速度计和磁力计测量值也即三轴加速度数据和三轴磁感应强度数据为观测量，结合四元数表示的姿态矩阵，建立观测方程；对加速度计和磁力计异常值进行处理，也即对三轴加速度数据和三轴磁感应强度数据进行异常值处理；通过泰勒展开的方式将状态方程、观测方程线性化，计算雅克比矩阵来代替原矩阵；在得出四元数的最优滤波值后，利用四元数的最优滤波值，结合四元数与欧拉角转换关系，计算排水管道检测仪的姿态角数据。
示例性地，状态方程为：
其中，q0,k,q1,k,q2,k,q3,k分别为k时刻四元数的四个参数值q0,q1,q2,q3；ωx,k,ωy,k,ωz,k分别为k时刻通过陀螺仪获取的三轴角速度数值；bωx,k，bωy,k，bωz,k分别为k时刻陀螺仪的角速度零偏值；bωx,k-1，bωy,k-1，bωz,k-1分别为k-1时刻陀螺仪的角速度零偏值；T为采样周期，这里陀螺仪、加速度计和磁力计的采样周期一致；q0,k-1,q1,k-1,q2,k-1,q3,k-1分别为k-1时刻四元数的四个参数；wk-1为系统噪声，其协方差阵为Q。
观测方程为：
其中，表示k时刻三轴加速度的估计值；/>表示k时刻三轴磁感应强度的估计值；(0,0,g)T表示重力加速度在基准坐标系下的三轴分量；(Hx0,Hy0,Hz0)T表示地磁场在基准坐标系下的三轴分量；/>表示由四元数表示的旋转矩阵，使用右手坐标系的Hamilton表达形式；vk为传感器噪声，其协方差阵为R。
用于线性化状态方程的雅克比矩阵为：
线性化后的状态方程表示为：
xk|k-1＝f(xk-1|k-1)+F(xk-xk-1|k-1)+uk
其中，xk表示k时刻四元数的四个参数q0,q1,q2,q3以及陀螺仪的角速度零偏bωx,k，bωy,k，bωz,k的估计值；xk|k-1表示k-1时刻的状态对k时刻四元数的预测值；xk-1|k-1表示k-1时刻的状态的最优估计值；uk为新的噪声矩阵，其协方差阵为Q2。
用于线性化观测方程的雅克比矩阵为：
线性化后的观测方程可以表示为：
Zk＝h(xk|k-1)+H(xk-xk|k-1)+sk
其中，Zk表示k时刻三轴加速度和三轴磁感应强度的估计值；xk表示k时刻四元数的四个参数q0,q1,q2,q3以及陀螺仪的角速度零偏bωx,k，bωy,k，bωz,k的估计值；xk|k-1表示k-1时刻的状态对k时刻四元数的预测值；sk为新的噪声矩阵，其协方差阵为R2。
值得注意的是，对三轴加速度数据和三轴磁感应强度数据进行异常值处理，也即处理加速度计和陀螺仪受运动加速度和磁场干扰如下所示：
加速度计与磁力计的异常值计算式为：
其中，ak-1和mk-1表示k-1时刻加速度计和磁力计的测量值。
扩展卡尔曼滤波的迭代方程为：
(1)初始状态
初始状态量X(0)、观测量Z(0)、协方差矩阵P(0)
(2)扩展卡尔曼滤波器时间更新过程
一步状态预测：
Xk|k-1＝f(Xk-1|k-1)
一步预测误差协方差：
Pk|k-1＝FkPk-1|k-1Fk T+Q2,k-1
(3)扩展卡尔曼滤波器观测更新过程
增益矩阵计算：
状态估计：
Xk|k＝Xk|k-1+Kk[Zk-h(XK)|K-1)]
估计误差协方差矩阵：
Pk|k＝(I-KkHk)Pk|k-1
当ba≤Δa0时，表示加速度计不受运动加速度干扰，无需对三轴加速度数据做处理，Δa0为加速度计异常值判断阈值；
当ba>Δa0时，表示加速度计受到运动加速度干扰，加速度计的可靠性小于陀螺仪，将三轴加速度数据用观测方程中陀螺仪估计的重力加速度分量代替；
当bm≤Δm0时，表示磁力计不受随机磁场干扰，无需对三轴磁感强度数据做处理，Δm0为磁力计异常值判断阈值；
当bm>Δm0时，表示磁力计受外界磁场干扰，进而当ba≤Δa0时，k时刻磁力计测量值由加速度计补偿陀螺仪后的四元数矩阵代替，当ba>Δa0，采用三轴角速度数据直接解算四元数姿态矩阵。
需要注意的是，扩展卡尔曼滤波器的滤波频率为微机电九轴惯性传感器的采样频率。
在本发明的实施例中，可以基于扩展卡尔曼滤波器将排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据进行融合，进而实现排水管道检测仪姿态解算，提高了排水管道检测仪的姿态角数据精度，对于重现检测仪在管道中的姿态，掌握排水管道运行状态具有重要意义。
需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于可选实施例，所涉及的动作和模块并不一定是本发明所必须的。
以上是关于方法实施例的介绍，以下通过装置实施例，对本发明所述方案进行进一步说明。
图4示出了本发明的实施例提供的一种排水管道检测仪姿态解算装置的结构图，如图4所示，排水管道检测仪姿态解算装置400可以包括：
获取模块410，用于获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据。
估计模块420，用于利用三轴角速度数据对四元数进行先验估计。
计算模块430，用于以先验估计后的四元数和三轴角速度常值偏差为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算排水管道检测仪的姿态角数据。
在一些实施例中，计算模块430具体用于：
以先验估计后的四元数参数和角速度零偏值为状态量，建立基于四元数更新方程的状态方程；
以三轴加速度数据和三轴磁感应强度数据为观测量，结合四元数表示的姿态矩阵，建立观测方程；
对三轴加速度数据和三轴磁感应强度数据进行异常值处理；
通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值；
利用四元数的最优滤波值，结合四元数与欧拉角转换关系，计算排水管道检测仪的姿态角数据。
可以理解的是，图4所示的排水管道检测仪姿态解算装置400中的各个模块/单元具有实现图2所示的排水管道检测仪姿态解算方法200中的各个步骤的功能，并能达到其相应的技术效果，为了简洁，在此不再赘述。
图5示出了一种能够实施本发明的实施例的示例性电子设备的结构图。电子设备500旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备500还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本发明的实现。
如图5所示，电子设备500可以包括计算单元501，其可以根据存储在只读存储器(ROM)502中的计算机程序或者从存储单元508加载到随机访问存储器(RAM)503中的计算机程序，来执行各种适当的动作和处理。在RAM503中，还可存储电子设备500操作所需的各种程序和数据。计算单元501、ROM502以及RAM503通过总线504彼此相连。输入/输出(I/O)接口505也连接至总线504。
电子设备500中的多个部件连接至I/O接口505，包括：输入单元506，例如键盘、鼠标等；输出单元507，例如各种类型的显示器、扬声器等；存储单元508，例如磁盘、光盘等；以及通信单元509，例如网卡、调制解调器、无线通信收发机等。通信单元509允许电子设备500通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
计算单元501可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元501的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。计算单元501执行上文所描述的各个方法和处理，例如方法200。例如，在一些实施例中，方法200可被实现为计算机程序产品，包括计算机程序，其被有形地包含于计算机可读介质，例如存储单元508。在一些实施例中，计算机程序的部分或者全部可以经由ROM502和/或通信单元509而被载入和/或安装到电子设备500上。当计算机程序加载到RAM503并由计算单元501执行时，可以执行上文描述的方法200的一个或多个步骤。备选地，在其他实施例中，计算单元501可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行方法200。
本文中以上描述的各种实施方式可以在数字电子电路系统、集成电路系统、现场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、片上系统(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本发明的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本发明的上下文中，计算机可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。计算机可读介质可以是计算机可读信号介质或计算机可读储存介质。计算机可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。计算机可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
需要注意的是，本发明还提供了一种存储有计算机指令的非瞬时计算机可读存储介质，其中，计算机指令用于使计算机执行方法200，并达到本发明的实施例执行其方法达到的相应技术效果，为简洁描述，在此不再赘述。
另外，本发明还提供了一种计算机程序产品，该计算机程序产品包括计算机程序，计算机程序在被处理器执行时实现方法200。
为了提供与用户的交互，可以在计算机上实施以上描述的实施例，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将以上描述的实施例实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)和互联网。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，也可以为分布式系统的服务器，或者是结合了区块链的服务器。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本发明中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本发明公开的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。
Claims (10)
1.一种排水管道检测仪姿态解算方法，其特征在于，所述方法包括：
获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据；
利用所述三轴角速度数据对四元数进行先验估计；
以先验估计后的四元数和三轴角速度常值偏差为状态量，以所述三轴加速度数据和所述三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算所述排水管道检测仪的姿态角数据。
2.根据权利要求1所述的方法，其特征在于，所述三轴角速度数据、所述三轴加速度数据以及所述三轴磁感应强度数据由所述排水管道检测仪中的微机电九轴惯性传感器采集。
3.根据权利要求2所述的方法，其特征在于，所述微机电九轴惯性传感器包括：陀螺仪、加速度计以及磁力计；
所述陀螺仪用于采集三轴角速度数据；
所述加速度计用于采集三轴加速度数据；
所述磁力计用于采集三轴磁感应强度数据。
4.根据权利要求3所述的方法，其特征在于，所述以先验估计后的四元数和所述三轴角速度常值偏差为状态量，以所述三轴加速度数据和所述三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算所述排水管道检测仪的姿态角数据，包括：
以先验估计后的四元数参数和角速度零偏值为状态量，建立基于四元数更新方程的状态方程；
以三轴加速度数据和三轴磁感应强度数据为观测量，结合四元数表示的姿态矩阵，建立观测方程；
对三轴加速度数据和三轴磁感应强度数据进行异常值处理；
通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值；
利用四元数的最优滤波值，结合四元数与欧拉角转换关系，计算所述排水管道检测仪的姿态角数据。
5.根据权利要求4所述的方法，其特征在于，所述状态方程为：
其中，q0,k,q1,k,q2,k,q3,k分别为k时刻四元数的四个参数值q0,q1,q2,q3；ωx,k,ωy,k,ωz,k分别为k时刻通过陀螺仪获取的三轴角速度数值；bωx,k，bωy,k，bωz,k分别为k时刻陀螺仪的角速度零偏值；bωx,k-1，bωy,k-1，bωz,k-1分别为k-1时刻陀螺仪的角速度零偏值；T为采样周期；q0,k-1,q1,k-1,q2,k-1,q3,k-1分别为k-1时刻四元数的四个参数；wk-1为系统噪声，其协方差阵为Q。
6.根据权利要求4所述的方法，其特征在于，所述观测方程为：
其中，表示k时刻三轴加速度的估计值；/>表示k时刻三轴磁感应强度的估计值；(0,0,g)T表示重力加速度在基准坐标系下的三轴分量；(Hx0,hy0,Hz0)T表示地磁场在基准坐标系下的三轴分量；/>表示由四元数表示的旋转矩阵，使用右手坐标系的Hamilton表达形式；vk为传感器噪声，其协方差阵为R。
7.根据权利要求5所述的方法，其特征在于，在通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正期间所用到的用于线性化状态方程的雅克比矩阵为：
线性化后的状态方程表示为：
xk|k-1＝f(xk-1|k-1)+F(xk-xk-1|k-1)+uk
其中，xk表示k时刻四元数的四个参数q0,q1,q2,q3以及陀螺仪的角速度零偏bωx,k，bωy,k，bωz,k的估计值；xk|k-1表示k-1时刻的状态对k时刻四元数的预测值；xk-1|k-1表示k-1时刻的状态的最优估计值；uk为新的噪声矩阵，其协方差阵为Q2。
8.根据权利要求6所述的方法，其特征在于，在通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正期间所用到的用于线性化观测方程的雅克比矩阵为：
线性化后的观测方程表示为：
Zk＝h(xk|k-1)+H(xk-xk|k-1)+sk
其中，Zk表示k时刻三轴加速度和三轴磁感应强度的估计值；xk表示k时刻四元数的四个参数q0,q1,q2,q3以及陀螺仪的角速度零偏bωx,k，bωy,k，bωz,k的估计值；xk|k-1表示k-1时刻的状态对k时刻四元数的预测值；sk为新的噪声矩阵，其协方差阵为R2。
9.根据权利要求8所述的方法，其特征在于，所述对三轴加速度数据和三轴磁感应强度数据进行异常值处理，包括：
加速度计与磁力计的异常值计算式为：
其中，ak-1和mk-1表示k-1时刻加速度计和磁力计的测量值；
当ba≤Δa0时，表示加速度计不受运动加速度干扰，无需对三轴加速度数据做处理，Δa0为加速度计异常值判断阈值；
当ba>Δa0时，表示加速度计受到运动加速度干扰，加速度计的可靠性小于陀螺仪，将三轴加速度数据用观测方程中陀螺仪估计的重力加速度分量代替；
当bm≤Δm0时，表示磁力计不受随机磁场干扰，无需对三轴磁感应强度数据做处理，Δm0为磁力计异常值判断阈值；
当bm>Δm0时，表示磁力计受外界磁场干扰，进而当ba≤Δa0时，k时刻磁力计测量值由加速度计补偿陀螺仪后的四元数矩阵代替，当ba>Δa0，采用三轴角速度数据直接解算四元数姿态矩阵。
10.一种排水管道检测仪姿态解算装置，其特征在于，所述装置包括：
获取模块，用于获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据；
估计模块，用于利用所述三轴角速度数据对四元数进行先验估计；
计算模块，用于以先验估计后的四元数和所述三轴角速度数据为状态量，以所述三轴加速度数据和所述三轴磁感应强度数据为观测量，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算所述排水管道检测仪的姿态角数据。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311315713.2A CN117451048A (zh) | 2023-10-11 | 2023-10-11 | 排水管道检测仪姿态解算方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311315713.2A CN117451048A (zh) | 2023-10-11 | 2023-10-11 | 排水管道检测仪姿态解算方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117451048A true CN117451048A (zh) | 2024-01-26 |
Family
ID=89582563
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311315713.2A Pending CN117451048A (zh) | 2023-10-11 | 2023-10-11 | 排水管道检测仪姿态解算方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117451048A (zh) |
-
2023
- 2023-10-11 CN CN202311315713.2A patent/CN117451048A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107560613B (zh) | 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 | |
| JP2021177168A (ja) | 車両のデッドレコニング方法、装置、デバイス、記憶媒体、及びプログラム | |
| CN111551174A (zh) | 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统 | |
| CN109724602A (zh) | 一种基于硬件fpu的姿态解算系统及其解算方法 | |
| EP3227634B1 (en) | Method and system for estimating relative angle between headings | |
| CN114692425A (zh) | 数字孪生技术的焊接机器人仿真方法、系统、设备及介质 | |
| CN109866217B (zh) | 机器人里程定位方法、装置、终端设备及计算机存储介质 | |
| CN115727871A (zh) | 一种轨迹质量检测方法、装置、电子设备和存储介质 | |
| CN106931965B (zh) | 一种确定终端姿态的方法及装置 | |
| CN117451048A (zh) | 排水管道检测仪姿态解算方法及装置 | |
| CN109674480B (zh) | 一种基于改进互补滤波的人体运动姿态解算方法 | |
| CN110864684A (zh) | 用户姿态测算方法 | |
| CN113936044B (zh) | 激光设备运动状态的检测方法、装置、计算机设备及介质 | |
| CN114120252B (zh) | 自动驾驶车辆状态的识别方法、装置、电子设备及车辆 | |
| Haotian et al. | Accurate attitude estimation of HB2 standard model based on QNCF in hypersonic wind tunnel test | |
| CN114622617A (zh) | 作业机械回转参数判定方法、装置及作业机械 | |
| CN114636418A (zh) | 一种基于mems测量姿态相对变化的方法、装置及电子设备 | |
| CN112880664A (zh) | 一种行驶设备的定位方法、装置、设备及存储介质 | |
| CN117806469A (zh) | 一种自动定位方法、装置、设备及介质 | |
| CN117889851A (zh) | 姿态确定方法、装置、设备及存储介质 | |
| CN117171581A (zh) | 排水管道缺陷状态诊断方法、装置、设备以及存储介质 | |
| CN115805593B (zh) | 力传感器安装信息确定方法、装置、设备和介质 | |
| CN116380056B (zh) | 惯性定位方法、装置、电子设备及存储介质 | |
| CN115096304B (zh) | 延时误差修正方法、装置、电子设备和存储介质 | |
| CN116448105B (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 |