排水管道检测仪姿态解算方法及装置

本发明涉及排水管网检测，尤其涉及一种排水管道检测仪姿态解算方法及装置。

背景技术：

1、随着微机电传感器技术的发展，基于多传感器的姿态解算技术的应用也逐渐成熟，具备在排水管网领域应用的条件。排水管网检测领域检测成本高、检测步骤繁琐、检测周期长等痛点对相关从业人员提出了研发新技术的需求。应用单一陀螺仪的排水管网检测仪存在姿态解算有偏差，误差逐渐积累等问题；应用imu组合即陀螺仪与加速度计的排水管网检测仪可以校正翻滚角和俯仰角上的误差，但由于加速度计的特点，无法校正陀螺仪的航偏角误差；而应用imu和磁力计组合的排水管网检测仪在三个姿态角方向上都有比较好的精度，但需要注意运动加速度和随机磁场存在时对加速度计和磁力计的干扰。有鉴于此，如何提高姿态解算效果就成为了目前亟待解决的技术问题。

技术实现思路

1、本发明的实施例提供了一种排水管道检测仪姿态解算方法及装置。

2、第一方面，本发明的实施例提供了一种排水管道检测仪姿态解算方法，该方法包括：

3、获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据；

4、利用三轴角速度数据对四元数进行先验估计；

5、以先验估计后的四元数和三轴角速度常值偏差为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算排水管道检测仪的姿态角数据。

6、第二方面，本发明的实施例提供了一种排水管道检测仪姿态解算装置，该装置包括：

7、获取模块，用于获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据；

8、估计模块，用于利用三轴角速度数据对四元数进行先验估计；

9、计算模块，用于以先验估计后的四元数和三轴角速度常值偏差为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算排水管道检测仪的姿态角数据。

10、第三方面，本发明的实施例提供了一种电子设备，该电子设备包括：至少一个处理器；以及与至少一个处理器通信连接的存储器；存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行如上所述的方法。

11、第四方面，本发明的实施例提供了一种存储有计算机指令的非瞬时计算机可读存储介质，计算机指令用于使计算机执行如上所述的方法。

12、在本发明的实施例中，可以基于扩展卡尔曼滤波器将排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据进行融合，进而实现排水管道检测仪姿态解算，提高了排水管道检测仪的姿态角数据精度，对于重现检测仪在管道中的姿态，掌握排水管道运行状态具有重要意义。

13、应当理解，
技术实现要素：
部分中所描述的内容并非旨在限定本发明的实施例的关键或重要特征，亦非用于限制本发明的范围。本发明的其它特征将通过以下的描述变得容易理解。

技术特征：

1.一种排水管道检测仪姿态解算方法，其特征在于，所述方法包括：

2.根据权利要求1所述的方法，其特征在于，所述三轴角速度数据、所述三轴加速度数据以及所述三轴磁感应强度数据由所述排水管道检测仪中的微机电九轴惯性传感器采集。

3.根据权利要求2所述的方法，其特征在于，所述微机电九轴惯性传感器包括：陀螺仪、加速度计以及磁力计；

4.根据权利要求3所述的方法，其特征在于，所述以先验估计后的四元数和所述三轴角速度常值偏差为状态量，以所述三轴加速度数据和所述三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算所述排水管道检测仪的姿态角数据，包括：

5.根据权利要求4所述的方法，其特征在于，所述状态方程为：

6.根据权利要求4所述的方法，其特征在于，所述观测方程为：

7.根据权利要求5所述的方法，其特征在于，在通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正期间所用到的用于线性化状态方程的雅克比矩阵为：

8.根据权利要求6所述的方法，其特征在于，在通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正期间所用到的用于线性化观测方程的雅克比矩阵为：

9.根据权利要求8所述的方法，其特征在于，所述对三轴加速度数据和三轴磁感应强度数据进行异常值处理，包括：

10.一种排水管道检测仪姿态解算装置，其特征在于，所述装置包括：

技术总结
本发明的实施例提供了一种排水管道检测仪姿态解算方法及装置。该方法包括：获取排水管道检测仪随水漂流过程中的三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据；利用三轴角速度数据对四元数进行先验估计；以先验估计后的四元数和三轴角速度常值偏差为状态量，以三轴加速度数据和三轴磁感应强度数据为观测量并考虑运动加速度和随机磁场的干扰，通过扩展卡尔曼滤波器对先验估计后的四元数进行滤波校正，得到四元数的最优滤波值，以此计算排水管道检测仪的姿态角数据。以此方式，可以基于扩展卡尔曼滤波器将三轴角速度数据、三轴加速度数据以及三轴磁感应强度数据进行融合，进而实现排水管道检测仪姿态解算，提高姿态角数据精度。

技术研发人员：陶涛,孟金恒,肖云龙,马晓宇,信昆仑,李树平,颜合想,王嘉莹
受保护的技术使用者：同济大学
技术研发日：
技术公布日：2024/2/1