基于子图匹配的复杂管道环境多源异构信息融合定位方法技术

技术编号：41314234 阅读：5 留言：0更新日期：2024-05-13 14:56

本发明专利技术公开了一种基于子图匹配的复杂管道环境多源异构信息融合定位方法。针对相似管道环境下的点云退化问题，使用扩展卡尔曼滤波方法利用预处理后的异构传感器信息构建机器人运动系统方程、测量方程，迭代更新卡尔曼增益并输出准确的局部先验位姿估计。针对传统定位算法累计误差随时间线性增长的问题，利用惯性测量单元数据对前置机械式激光雷达点云进行平面投影并构建子图，使用子图匹配思想紧耦合先验位姿与子图的局部约束因素从而估计机器人在滑动窗口内相邻数据帧间的运动增量，最终最小化所有因素残差以获得管道巡检机器人状态的最大后验估计。通过相应实验，验证了所提出定位方法的准确性与有效性，该方法能够有效减少管道巡检机器人在复杂狭窄管道环境下的定位误差，达到了厘米级的定位精度。

全部详细技术资料下载

【技术实现步骤摘要】

本专利技术属于管道巡检机器人定位，特别是一种基于子图匹配的复杂管道环境多源异构信息融合定位方法。

技术介绍

1、随着城市人口的持续增加，中央空调管道系统逐渐成为了高层建筑通风、调湿的关键设备。随着使用时间的增长，管道内部积累的真菌、细菌等污染物严重威胁着楼宇中人们的身体健康。由于管道结构的复杂性与多样性，无法通过人工对整栋建筑的管道网络进行检查，因此管道巡检机器人应运而生。为获取准确可靠的定位信息，现有的管道巡检机器人在运行过程中通常需要搭载轮式编码器、惯性测量单元(imu)和激光雷达等异构传感器设备，但轮式编码器存在因打滑、管道不平整而产生的测量误差，存在因机器人运动学建模不准确而产生的系统误差；imu存在因零偏、角度随机游走等而导致的累计误差；激光雷达在金属管道内部的强反射情况会导致点云畸变，且在长直管道中易发生点云退化。并且由于成本的限制与载荷的约束，小型的管道巡检机器人无法配备过于昂贵的高精度定位设备。

2、针对上述问题，急需一种适用于复杂金属管道环境下的，不依赖高精度传感器的多源异构信息融合定位方法实时定位方法。

技术实现思路

1、本专利技术的目的在于提供一种基于编码器、imu和激光雷达的复杂管道环境下的多源异构信息融合定位方法，解决金属管道环境下编码器、imu误差随运行时间累计，激光雷达点云畸变的问题，综合运用扩展卡尔曼滤波(ekf)、子图匹配(subgraph-matching)思想对多传感器数据进行融合，实现复杂管道环境下管道巡检机器人的实时准确定位。</p>

2、实现本专利技术目的的技术解决方案为：一方面，提供了一种基于子图匹配的复杂管道环境多源异构信息融合定位方法，所述方法包括以下步骤：

3、步骤1，建立管道巡检机器人系统动力学模型；

4、步骤2，将管道巡检机器人放置于复杂管道环境中采集异构传感器数据，所述传感器包括编码器、惯性测量单元即imu、侧面固态激光雷达和前置机械式激光雷达；

5、步骤3，对采集到的异构传感器数据进行预处理，获取管道巡检机器人及其周边环境的测量信息；

6、步骤4，根据编码器数据构建管道巡检机器人运动系统方程，初始化管道巡检机器人位姿状态与协方差；

7、步骤5，根据imu和侧面固态激光雷达数据构建并更新测量方程，基于扩展卡尔曼滤波即ekf方法计算卡尔曼增益并迭代更新，获得管道巡检机器人的先验位姿估计；

8、步骤6，利用imu数据对前置机械式激光雷达点云进行平面投影并构建子图；

9、步骤7，使用子图匹配思想紧耦合先验位姿与子图的局部约束来估计管道巡检机器人在滑动窗口内相邻数据帧间的运动增量，并最小化所有因素残差以求解管道巡检机器人状态的最大后验估计。

10、进一步地，步骤1所述建立管道巡检机器人的系统动力学模型，具体为：

11、

12、式中，vx，vy为管道巡检机器人机体坐标系下分别沿x，y方向运动的线速度，ω为管道巡检机器人机体坐标系相对全局坐标系旋转的角速度，l为机器人轮距，δψ为相邻时刻间管道巡检机器人偏航角的变化量，ψ为管道巡检机器人偏航角，vl，vr为分别为管道巡检机器人左、右轮运行的线速度。

13、进一步地，步骤2中采集异构传感器数据，包括：采集t时刻左、右轮编码器数据imu的三轴加速度计数据ax，ay，az，三轴陀螺仪数据gx，gy，gz，三轴磁力计数据mx，my，mz；侧面固态激光雷达的点云数据前置机械式激光雷达数据

14、进一步地，步骤3所述对采集到的异构传感器数据进行预处理，获取管道巡检机器人位姿信息，具体包括：

15、步骤3-1，通过编码器数据分别计算管道巡检机器人左、右轮线速度vl，vr，具体为：

16、

17、

18、式中，m为管道巡检机器人车轮转动完整一圈时编码器计数，分别为t时刻左、右轮编码器数据，分别为t+1时刻左、右轮编码器数据，d为管道巡检机器人轮子的直径，δt为两次采样的时间差，基于左右轮速度可以计算出vx，vy和ω；

19、之后表示由编码器数据计算出的管道巡检机器人在δt内的位姿变化量δxodom，δyodom和δψodom，具体为：

20、

21、再对δxodom，δyodom和δψodom进行累加即可得到机器人每一时刻的编码里程计位姿；

22、步骤3-2，对imu原始数据进行滤波处理：通过六轴imu互补滤波方法，融合陀螺仪、加速度计和磁力计的数据得到对应的四元数数据，具体为：

23、

24、其中，代表imu坐标系相对于世界坐标系的变化，q0，q1，q2，q3代表三维空间中用于表示机器人旋转角度的四元数；

25、由imu数据计算的四元数同样得到对应的三轴欧拉角：横滚角θimu，俯仰角φimu和偏航角ψimu；

26、步骤3-3，对侧面固态激光雷达-20°:+20°之间的点云数据进行最小二乘法拟合，获得t时刻侧面激光雷达相对管道壁的距离

27、获得相邻时刻间机器人y轴方向的坐标偏移量δylidar，具体为：

28、

29、其中，ibool为符号常数，对于右侧激光雷达ibool为+1，对于左侧激光雷达ibool为-1

30、进一步地，步骤4所述根据编码器数据构建管道巡检机器人运动系统方程，初始化管道巡检机器人位姿状态与协方差，具体包括：

31、定义状态量x＝[x y ψ]t，控制量u＝[v ω]t，以编码器的测量值构建系统方程f对机器人位姿进行先验估计，具体为：

32、

33、其中，为k时刻的先验估计，为k-1时刻的最佳后验估计，[vk ωk]t为k时刻的控制量，vk，ωk分别为k时刻管道机器人运动的线速度与角速度，wk为k时刻系统模型的过程噪声，满足高斯分布p(w):n(0,q)，其中q为高斯噪声的协方差矩阵。

34、进一步地，步骤5所述根据imu和侧面固态激光雷达数据构建并更新测量方程，基于扩展卡尔曼滤波即ekf方法计算卡尔曼增益并迭代更新，获得管道巡检机器人的先验位姿，具体包括：

35、步骤5-1，分别计算k时刻imu和侧面固态激光雷达先验误差的协方差矩阵和具体为：

36、

37、

38、其中，和分别表示k-1时刻imu和固态激光雷达的误差协方差矩阵，ak表示系统方程f相对于x的偏导数的雅可比矩阵，wk表示系统方程f相对于w的偏导数的雅可比矩阵，具体为：

39、

40、

41、步骤5-2，分别计算k时刻imu和侧面固态激光雷达对应的卡尔曼增益和具体为：

42、

43、

44、其中，h表示传感器观测函数h相对于x的偏导数的雅可比矩阵，v表示传感器观测函数h相对于v的偏导数的雅可比矩阵，h′和v′分别为h和v的转置，σ代表测量噪声，本文档来自技高网...

【技术保护点】

1.一种基于子图匹配的复杂管道环境多源异构信息融合定位方法，其特征在于，所述方法包括以下步骤：

2.根据权利要求1所述的基于子图匹配的复杂管道环境多源异构信息融合定位方法，其特征在于，步骤1所述建立管道巡检机器人的系统动力学模型，具体为：

3.根据权利要求1或2所述的基于子图匹配的复杂管道环境多源异构信息融合定位方法，其特征在于，步骤2中采集异构传感器数据，包括：采集t时刻左、右轮编码器数据encodertl，encodertr；IMU的三轴加速度计数据ax，ay，az，三轴陀螺仪数据gx，gy，gz，三轴磁力计数据mx，my，mz；侧面固态激光雷达的点云数据前置机械式激光雷达数据

4.根据权利要求3所述的基于EKF和子图匹配的管道环境下多传感器融合定位方法，其特征在于，步骤3所述对采集到的异构传感器数据进行预处理，获取管道巡检机器人及其周边环境的测量信息，具体包括：

5.根据权利要求4所述的基于子图匹配的复杂管道环境多源异构信息融合定位方法，其特征在于，步骤4所述根据编码器数据构建管道巡检机器人运动系统方程，初始化管道巡检机器人位姿状态与协方差，具体包括：

6.根据权利要求5所述的基于子图匹配的复杂管道环境多源异构信息融合定位方法，其特征在于，步骤5所述根据IMU和侧面固态激光雷达数据构建并更新测量方程，基于扩展卡尔曼滤波即EKF方法计算卡尔曼增益并迭代更新，获得管道巡检机器人的先验位姿估计，具体包括：

7.根据权利要求6所述的基于子图匹配的复杂管道环境多源异构信息融合定位方法，其特征在于，步骤6所述利用IMU数据对前置机械式激光雷达点云进行平面投影并构建子图，具体为：

8.根据权利要求7所述的基于子图匹配的复杂管道环境多源异构信息融合定位方法，其特征在于，步骤7所述使用子图匹配思想紧耦合先验位姿与子图的局部约束来估计管道巡检机器人在滑动窗口内相邻数据帧间的运动增量，并最小化所有因素残差以求解管道巡检机器人状态的最大后验估计，具体为：

9.基于权利要求1至8任意一项所述方法的基于子图匹配的复杂管道环境多源异构信息融合定位系统，其特征在于，所述系统包括：

10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至8中任一项所述方法。

...

【技术特征摘要】

1.一种基于子图匹配的复杂管道环境多源异构信息融合定位方法，其特征在于，所述方法包括以下步骤：

3.根据权利要求1或2所述的基于子图匹配的复杂管道环境多源异构信息融合定位方法，其特征在于，步骤2中采集异构传感器数据，包括：采集t时刻左、右轮编码器数据encodertl，encodertr；imu的三轴加速度计数据ax，ay，az，三轴陀螺仪数据gx，gy，gz，三轴磁力计数据mx，my，mz；侧面固态激光雷达的点云数据前置机械式激光雷达数据

4.根据权利要求3所述的基于ekf和子图匹配的管道环境下多传感器融合定位方法，其特征在于，步骤3所述对采集到的异构传感器数据进行预处理，获取管道巡检机器人及其周边环境的测量信息，具体包括：

<...

【专利技术属性】
技术研发人员：吴益飞，马寅锐，陈庆伟，郭健，李胜，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：

全部详细技术资料下载我是这个专利的主人