[发明专利]一种基于移动三维扫描技术的抖动检测分段滤波方法

说明书

本发明提供一种基于移动三维扫描技术的抖动检测分段滤波方法，包括：记录巡检机器人运动过程中的当前总位移数和抖动检测段数；当检测到最近两段抖动间隔距离为巡检机器人前后轮轴心距左右时，判定巡检机器人前后两轮均在钢管轨道上运行，采用预设的状态空间模型对巡检机器人中激光陀螺惯性测量单元的惯性导航和里程计数据进行滤波解算；巡检机器人在其它位置时，则采用普通组合导航的卡尔曼滤波算法解算巡检机器人的惯性导航和里程计数据。本发明解决了在惯性导航/里程计组合导航过程中，里程计可能会由于滑行、弹起悬空等故障造成组合导航误差增大的问题。

技术领域

本发明属于煤炭开采技术领域，尤其涉及一种基于移动三维扫描技术的抖动检测分段滤波方法。

背景技术

高精度脉冲式激光扫描仪通过采集空间位置的点云数据，经过数据采集、航位推算、点云融合算法、可视化软件等处理后可完成特定场景的空间定位及数字化建模。

激光扫描仪包含激光扫描单元和激光陀螺惯性测量单元。由于激光扫描仪安装于巡检机器人上，其在专门设计的柔性轨道上运行。如图1所示，柔性轨道系统由1.4m长的平直钢管和0.1m长的弹簧连接而成。扫描仪运动过程中，在平直钢管上可平稳运行，但经过弹簧时，整机会出现一定幅度的抖动。由于抖动产生时，弹起过程中会向上加速，下降过程中会出现失重现象，加速度计可敏感地检测出此时扫描仪的加速度变化，且此时陀螺仪测得的角度数据也会出现大幅波动。在搭建的模拟轨道上测试时，加速度计原始数据如图2所示。

从图2可以看出，从300s到350s间加速度数据出现小幅波动，这是由于静基座校准完成后，先开启三维激光扫描仪，内部电机开启后引起一定震动，350s之后开始执行巡检任务，巡检机器人开始运动。可以看出巡检机器人运动过程中，激光陀螺惯性测量单元加速度数据会周期性地出现一定幅度的高频波动，且是先出现一个小幅抖动，再出现大幅抖动。本次测试过程中，激光陀螺惯性测量单元安装在巡检机器人尾部，向前运动时，车头先经过弹簧，抖动对惯性导航系统影响较小，惯性导航系统所在的一端车轮之后再经过弹簧，产生的抖动对惯性导航系统影响较大，检测出来的加速度数据变化幅度前后两次分别为一小一大。由于巡检机器人运动速度以及连接弹簧的转弯幅度影响，每个周期抖动时加速度数据的变化幅度也会有一定变化。因此，在惯性导航/里程计组合导航过程中，里程计可能会由于滑行、弹起悬空等故障，使得里程计测得的里程数据小于实际运动里程，造成组合导航误差增大。

为了解决以上存在的问题，人们一直在寻求一种技术解决方案。

发明内容

为了解决上述问题，有必要提供一种基于移动三维扫描技术的抖动检测分段滤波方法。

本发明第一方面提出一种基于移动三维扫描技术的抖动检测分段滤波方法，包括：

记录巡检机器人运动过程中的当前总位移数和抖动检测段数；

当检测到最近两段抖动间隔距离为巡检机器人前后轮轴心距左右时，判定巡检机器人前后两轮均在钢管轨道上运行，采用预设的状态空间模型对巡检机器人中激光陀螺惯性测量单元的惯性导航和里程计数据进行滤波解算；

巡检机器人在其它位置时，则采用普通组合导航的卡尔曼滤波算法解算巡检机器人的惯性导航和里程计数据；

状态空间模型为：

记巡检机器人直线运动状态下的姿态误差方程为：

其中，

ω_iθ表示地球自转角速率，L表示机器人所处地理纬度，δ表示此项为后缀引起的误差项(如代表由计算地球自转角速度引起的误差)，表示激光陀螺惯性测量单元在x，y，z轴的标定刻度系数；

令