一种轨道平整度测量方法与流程

本发明涉及一种轨道平整度测量方法。

背景技术：

轨道机器人日常巡检监控任务都是在轨道上进行的，因此机器人对于轨道的稳定性和平整度存在着较高要求。为了保障巡检监控任务地顺利运行，需要对前期轨道安装工艺进行严格把关，轨道安装过程中会存在三维空间的角度偏差，而常见的人工采取倾角仪测量的方法只能获取轨道在二维角度上的偏差，并且测量误差较大，过程繁琐，当人工爬高去进行测量时存在安全隐患，效率较低。

技术实现要素：

为解决现有技术存在的问题，本发明提供一种轨道平整度测量方法，智能判断轨道安装的平整度，实现轨道安装检测过程操作的简易性、安全性和效率。

本发明提供的轨道平整度测量方法，包括以下步骤：

(1)将具有加速度传感器、角速度传感器、磁传感器的行走机构放置在轨道上，并移动到轨道相应位置；

(2)获取加速度传感器、角速度传感器的原始数据，根据原始数据得出行走机构的俯仰角、偏航角和翻滚角；行走机构的俯仰角、偏航角和翻滚角与轨道的俯仰角、偏航角和翻滚角相同。

(3)判断步骤(2)中获得的俯仰角、偏航角、翻滚角是否位于标准阈值区间内，如果位于标准阈值区间内则说明轨道平整，否则，轨道不平整。

优选地，所述步骤(2)具体包括；

(2.1)获取加速度传感器、角速度传感器的原始数据作为四元素，利用该四元素作为初值初始化四元素q₀、q₁、q₂、q₃，将四元素q₀、q₁、q₂、q₃转化为欧拉角θ、ψ、该组欧拉角作为第一组姿态角Q₀，θ、ψ、分别表示俯仰角、偏航角、翻滚角；

(2.2)对加速度传感器的原始数据进行中值滤波，得到加速度数据A-data，A-data包含分别对应x轴，y轴，z轴的A_x,A_y,A_z三个分量，利用这三个分量进行姿态解算，得到俯仰角θ，翻滚角以及z轴与重力加速度的夹角γ，其中

(2.3)获取磁传感器的原始数据并进行中值滤波，得到磁数据M-data，

该数据包含三个分量M_x,M_y,M_z，然后进行倾斜补偿：

根据倾斜补偿结果得到偏航角其中θ、为步骤(2.2)中得到俯仰角、翻滚角。

(2.4)对角速度传感器的原始数据进行均值滤波得到角速度数据ω-data，对ω-data积分运算后得到对应的角度Q_k＝(ω_k-ω_b)dt+Q_k-1，其中，Q_k为当前时刻的角度；ω_k为角速度传感器当前时刻的角速度；ω_b为当前时刻角速度的偏移量；dt为采样周期；Q_k-1为前一时刻的角度值。

(2.5)将步骤(2.2)、(2.3)、(2.4)获得的俯仰角θ、翻滚角偏航角ψ、当前时刻的角度Q_k通过卡尔曼滤波算法进行融合得到第二组姿态角Q₁；

(2.6)将第一组姿态角Q₀和第二组姿态角Q₁通过一阶互补滤波进行融合，得到融合后数据Q_t＝αQ₀+βQ₁，根据融合后数据Q得到最终准确的俯仰角、翻滚角、偏航角，其中，α，β对应Q₀、Q₁置信权重，α+β＝1。α取值范围为0.3-0.5，β取值范围：0.7～0.5。

为实现对不平整轨道位置的准确定位，本发明所述行走机构上还安装有定位系统，该定位系统用于对行走机构进行定位，确定行走机构在轨道上的位置，当检测到轨道不平整时，可以利用行走机构的位置确定不平整轨道的位置。当俯仰角、偏航角、翻滚角不在标准阈值区间内时，行走机构停止走动并发出报警，直至该轨道被修复平整，将行走机构重新放回该处，测得相关数据符合要求后，行走机构继续检测下一段轨道。

为了实现前期轨道安装工艺地严格把控，本发明使用陀螺仪对轨道稳定性和平整性进行测量，使用ARM处理器对数据进行处理，使用手持数据分析液晶屏显示轨道平整度信息，减少人工测量的误差。为了实现对轨道三维角度偏差的测量精确反馈，使用AHRS扩展分时算法和卡尔曼滤波算法对测量数据进行处理，使用数据分析液晶屏的偏差量及偏差曲线作为反馈量，使用异常指示灯判断轨道平整度，智能精准定位出异常倾斜的轨道区域。本发明使用行走机构装载测量设备，将测量设备拓展成为一个移动平台，使操作简易安全，提高轨道安装效率。本发明实现智能判断轨道安装的平整度，自动进行具体区域地标定，反馈偏差值，进行精准修正，使得机器人导轨的稳定性和平整性得到保证。

附图说明

图1为本发明流程图；

图2为手持数据分析液晶屏的控制界面；

图3为手持数据分析液晶屏的三维姿态角界面；

图4为手持数据分析液晶屏的偏差角界面。

具体实施方式

本发明的行走机构可采用申请号为201610186221.1的发明专利提供的机器人行走机构，该行走机构采用一个高性能有刷电机为动力，在驱动轮的下面有定位系统，结合电机编码器位置环能精准采集行走的距离，该行走机构具有八个导向轮，在行走机构与轨道接触的两端，各有4个导向轮，相互配合，紧贴轨道，不会产生晃动影响陀螺仪数据；导向模块可在水平面上转动一定角度以适应转向调节，可实现无死角转向，在转弯处不会产生运动方向受力。行走机构上安装有定位系统，采取9个磁钢与霍尔传感器配合，可实现无接触性，轨道不需要安装供电的情况下，准确定位机构行走的距离。行走机构上安装有陀螺仪、ARM处理器、三轴MEMS模块测量模块，也可以直接使用MPU9250传感器，该传感器集成了陀螺仪、加速度传感器和磁传感器。

本发明整体方案描述如下：

1、将行走机构放在需要测试平整度的轨道上；

2、将行走机构的上的ARM处理器、三轴MEMS模块测量模块上电；

3、在手持数据分析液晶屏设置轨道平整度的标准阈值区间(-0.6°～+0.6°)，该标准阈值区间是根据机器在轨道上运行保证水平姿态而设定，通过无线模块发送给轨道上的行走机构；

4、输入需要测试的轨道的距离，点击运行，如图2所示，为手持数据分析液晶屏的控制界面；

5、轨道的行走机构会在轨道上移动，当遇到某一段平整度不在标准阈值区间内的区域时会自动停止行走并发出红色闪烁灯；该段轨道平整度数据传输给手持数据分析液晶屏；由于测得翻滚角、俯仰角、偏航角三个数据，只要任意一个数据不再标准阈值区间内，都会停止行走。

6、手持数据分析液晶屏显示轨道平整度异常信息，在手持数据分析液晶屏观察偏差量及偏差曲线，如图3所示为手持数据分析液晶屏的三维姿态角界面，显示的是该段轨道的翻滚角、俯仰角、偏航角数据，图4所示为手持数据分析液晶屏的偏差角界面，显示偏差量及偏差曲线；

7、工人在异常轨道区域进行人工修正，直至异常闪烁红色变为正常(表示数据正常)，该段轨道平整度达到要求。

如图1所示，本发明获得准确的俯仰角、翻滚角、偏航角的具体步骤如下：

①第一组姿态角Q₀

使用InvenSense公司生产的MPU9250，MPU9250自带DMP(数字运动处理器)和MPL(嵌入式运动处理库)对MPU9250的加速度传感器和角速度传感器的原始数据进行处理，得到四元数并输出，用这个四元数作为初值初始化四元数q₀、q₁、q₂、q₃，由于欧拉角比较直观，x_b、y_b、z_b为该模块载体三轴的坐标，x_n、y_n、z_n:为参考坐标系下的坐标，需要将四元数转换成欧拉角：

首先，从四元素q₀、q₁、q₂、q₃转换成方向余弦矩阵：

然后再通过方向余弦矩阵转换成欧拉角：

式中T_ij(i,j＝1,2,3)表示方向余弦矩阵的第i行、第j列元素。这样就可以得到第一组姿态角Q₀(包括俯仰角、翻滚角、偏航角)。

②第二组姿态角Q₁

1)加速度数据处理

得到加速度计原始数据后，进行一次中值滤波，得到处理后的加速度数据A-data，A-data包含A_x,A_y,A_z(对应x轴，y轴，z轴)三个分量，然后利用这三个分量进行姿态解算：

式中，θ,分别对应俯仰角和翻滚角，γ为z轴与重力加速度的夹角，这样通过加速度计可以得到俯仰角和翻滚角，而偏航角没有经过校准，会出现明显漂移，需要使用磁传感器进行校正。

2)磁传感器数据处理

在得到磁传感器数据后，先进行中值滤波，得到处理后的磁数据M-data，该数据包含三个分量M_x,M_y,M_z，然后进行倾斜补偿：

其中θ、为步骤1)中得到俯仰角、翻滚角，偏航角可以通过如下三角函数关系计算：

3)角速度数据处理

得到角速度ω原始数据后，需要先进行一次均值滤波，去除噪声干扰，得到处理后数据ω-data，然后通过积分就可以得到对应的角度：

Q_k＝(ω_k-ω_b)dt+Q_k-1 (6)

其中，Q_k为当前时刻的角度；ω_k为陀螺仪当前时刻的角速度；ω_b为当前时刻角速度的偏移量；dt为采样周期；Q_k-1为前一时刻的角度值。

4)卡尔曼滤波

使用卡尔曼滤波算法对角度姿态数据进行融合，滤波器的输入量为系统观测量，输出量为系统状态量的预测，利用系统噪声和观测噪声的统计特性进行最优估计。假设系统的状态方程和测量方程描述如下：

其中，X_k，y_k分别对应状态向量和观测向量；A为第k-1时刻到第k时刻状态转移矩阵；B为输入控制向量的增益矩阵；U_k为输入控制向量；H为状态量到观测量的增益矩阵；W_k为系统输入噪声；v_k为观测噪声；假定系统输入噪声和观测噪声都服从正态分布，输入噪声的协方差为Q，观测噪声的协方差为R，则卡尔曼滤波可描述为以下5个公式：

状态量预测：

误差协方差预测：

P_k|k-1＝AP_k-1A^T+Q (9)

卡尔曼增益更新：

K_k＝P_k|k-1H^T[HP_k|k-1H^T+R]^-1 (10)

状态更新：

误差协方差更新：

P_k＝(1-K_kH)P_k|k-1 (12)

式中，为对当前状态真实值的预测；为基于k-1时刻对k时刻状态的预测；A为第k-1时刻到第k时刻状态转移矩阵；B为输入控制向量的增益矩阵；H为状态量到观测量的增益矩阵；y_k为第k时刻的观测量；K_k为卡尔曼增益；P_k为当前状态估计值的误差协方差矩阵；P_k|k-1为预测的误差协方差矩阵；I为单位矩阵。

陀螺仪角度状态用式(5)表示，加速度计和磁传感器对应角度作为观测反馈值。为了抑制陀螺仪漂移，需要对偏移量进行预测，所以系统状态方程为：

式中T表示采样周期；Q_k为当前时刻的角度；ω_b为当前时刻角速度的偏移量，ω_k-1、ω_k分别对应k时刻和k-1时刻的角速度。在优化过程中，各个初始值设置如下：

通过卡尔曼滤波算法将陀螺仪的角速度数据、加速度数据和磁传感器数据进行融合，可以得到第二组姿态角Q₁(包括俯仰角、翻滚角、偏航角)。

③一阶互补滤波

为了进一步提高获得姿态角的准确性，将第一组三个姿态角Q₀(俯仰角、翻滚角、偏航角)和第二组三个姿态角Q₁(俯仰角、翻滚角、偏航角)通过互补滤波进行融合得到融合数据Qt(俯仰角、翻滚角、偏航角)互补滤波公式如下：

Q_t＝αQ₀+βQ₁ (14)

α，β对应Q₀、Q₁置信权重，α+β＝1，一般设置α的范围值：0.3～0.5，设置β的范围值：0.7～0.5；Qt为融合数据，这样就能得到一个非常准确的姿态数据，通过显示屏进行显示。