CN109186589A - 一种基于阵列式惯性单元的采煤机定位方法 - Google Patents

Abstract

一种基于阵列式惯性单元的采煤机定位方法，包括步骤：在采煤机的箱体上方将四个惯性单元布置成阵列式结构；将惯性传感器采集的角速度、加速度信息传送到控制器中进行去噪、融合解算、误差修正，将修正后的数据发送到PC上位机进行定位显示。本发明的一种基于阵列式惯性单元的采煤机定位方法，利用四个惯性单元构成阵列式结构，消除了环境因素对惯性单元产生的时变漂移；通过对传感器数据进行基于果蝇算法的小波去噪，降低了累积误差，提高了定位精度；通过无迹卡尔曼滤波(UKF)算法进行误差修正，进一步提高了定位精确度。

Description

一种基于阵列式惯性单元的采煤机定位方法

技术领域

本发明涉及阵列式惯性导航技术，尤其涉及一种基于阵列式惯性单元的采煤机定位方法。

背景技术

果蝇优化算法(Fruit Fly Optimization Algorithm,FOA)在2011年由Wen-TsaoPan提出的有一种新型的群体智能优化算法，是一种基于果蝇觅食行为推演出寻求全局优化的新方法。近年来，小波理论得到了非常迅速的发展，而且由于其具备良好的时频特性，因而实际应用也非常广泛。在去噪领域中，小波理论也同样受到了许多学者的重视，应用小波进行去噪并获得了非常好的效果。

采煤机智能控制是实现综采智能化的关键，为了实现采煤机智能控制，首先需要解决采煤机的定位问题。现有的利用红外、超声波及齿轮技术等采煤机定位方法不能测采煤机航向。利用惯性导航技术能够实现采煤机的轨迹监测。但是现有的惯导系统误差分析不够全面，随着时间的增长，累积误差增大，难以满足采煤机定位要求，现有的误差消除方法不够直观有效，不能满足采煤机定位准确性要求。

发明内容

本发明的目的在于提供了一种基于阵列式惯性单元的采煤机定位方法，以提高采煤机定位的精确性。

为实现上述目的，本发明的技术方案如下：

一种基于阵列式惯性单元的采煤机定位方法，包括步骤：

S1：在采煤机的箱体上方将惯性单元布置成阵列式结构组成惯性传感器；

S2：将惯性传感器采集的角速度、加速度信息传送到控制器中进行去噪、融合解算、误差修正；

S3：将步骤S2获得的数据发送到PC上位机进行定位显示。

进一步的，所述惯性传感器由四个惯性单元组成，四个惯性单元等间距分布在同一个圆周上。

进一步的，步骤S2中的去噪是采用基于果蝇算法的小波去噪。

进一步的，所述基于果蝇算法的小波去噪包括：用果蝇算法优化小波去躁的阈值，每个果蝇种群对应小波每一层的阈值，味道浓度值以去噪之后的信号和原始信号的最小均方误差为目标，其步骤具体如下：

S41：初始化基于果蝇算法的小波去噪算法参数，主要包括：小波基函数ψ(·)、小波分解层数J、改进的果蝇算法最大迭代次数N、每个果蝇种群中个体数量P、种群位置范围LR、随机飞行距离FR；初始化各个种群位置，以及每个种群中的个体位置，个体的随机飞行距离服从正态分布；每层的阈值通过一个果蝇种群进行优化；

S42：对含噪的四路采煤机传感器信号进行J层小波分解，获取各层小波系数；计算各种群中所有果蝇的距离与味道浓度判断值，每个味道浓度判断值对应一个阈值的可行解，对每种可行解组合进行去噪，之后进行小波重构，计算每种可行解下的适应度值，选出其中最大的记为smellbest，保存位置Si(i)；

S43：更新种群位置至对应的果蝇位置Si(new)，计算当代果蝇的位置、味道浓度判断值以及适应度，选择其中适应度最大的记为bestsmell，并记录对应果蝇的位置；比较全局最大值smellbest与当前最大值bestsmell，若当前最大值大于全局最大值，用bestsmell替代smellbest，同时用对应的果蝇位置替代果蝇种群的位置；

S44：判断当前迭代次数n是否达到最大迭代次数N，若达到，停止迭代并输出最优去噪效果对应的重构信号以及各阶去噪阈值；若否则重复步骤S43。

进一步的，所述步骤S2中的融合解算还包括：以东北天方向为正，构建角速度输出矩阵：

其中，ω_xt、ω_yt、ω_zt分别代表三个方向的真实角速度，ω_xr1、ω_xr2、ω_xr3、ω_xr4分别表示第一至第四个惯性单元x方向的随机漂移，ω_yr1、ω_yr2、ω_yr3、ω_yr4分别表示第一至第四个惯性单元y方向的随机漂移，ω_zr1、ω_zr2、ω_zr3、ω_zr4分别表示第一至第四个惯性单元z方向的随机漂移，t表示真实角速度，r表示随机漂移；

和加速度融合方程：

其中，a_xt、a_yt、a_zt分别代表三个方向的真实加速度，g_x、g_y、g_z分别代表三个方向的重力加速度分量，a_xr1、a_xr2、a_xr3、a_xr4分别表示第一至第四个惯性单元x方向的随机漂移，a_yr1、a_yr2、a_yr3、a_yr4分别表示第一至第四个惯性单元y方向的随机漂移，a_zr1、a_zr2、a_zr3、a_zr4分别表示第一至第四个惯性单元z方向的随机漂移，t表示真实角速度，r表示随机漂移。

进一步的，所述步骤S2中的误差修正采用无迹卡尔曼滤波算法进行滤波获得最优估计；所述步骤S3具体包括：将步骤S2中获得的最优估计通过USB转串口发送到PC上位机进行定位显示。

与现有技术相比，本发明的有益效果：

本发明的一种基于阵列式惯性单元的采煤机定位方法，利用四个惯性单元构成阵列式结构，消除了环境因素对惯性单元产生的时变漂移；通过对传感器数据进行基于果蝇算法的小波去噪，降低了累积误差，提高了定位精度；通过无迹卡尔曼滤波算法进行误差修正，进一步提高了定位精确度。

附图说明

图1是本发明实施例中基于阵列式惯性单元的采煤机定位方法的流程图；

图2是本发明实施例中四路惯性单元阵列分布图；

图3是本发明实施例中惯性传感器布置图；

图4是本发明实施例中惯性传感器和STM32控制板接线图；

图5是本发明实施例中四路MPU6050惯性单元的地址分配图；

图6是本发明实施例中STM32和PC主机连接图；

图7是本发明实施例中基于果蝇算法小波去噪流程图；

其中，1-左滚筒；2-左摇臂；3-左行走部；4-MPU6050惯性单元；5-固定圆盘；6-右行走部；7-右摇臂；8-右滚筒。

具体实施方式：

下面结合附图和具体较佳实施方式对本发明作进一步详细的说明。本发明的实施例只是一部分，并不是全部实施例，本领域的其他人员在没有作出创作性劳动前提下所获得其他所有实施例，都属于本发明的保护范围。

实施例

图1所示为本发明的一种基于阵列式惯性单元的采煤机定位方法：首先将四路惯性单元布置成阵列式结构构成惯性传感器，将其通过圆盘固连在采煤机的箱体上方，然后将惯性传感器和STM32控制器连接通讯，之后将STM32控制器和PC上位机连接。该平台通过安装在采煤机机身上的惯性传感器采集角速度、加速度信息，并传送到STM32控制器中对四路分别进行去噪，然后进行融合解算、误差修正，将修正后的数据发送到PC上位机进行定位显示。其中惯性传感器单元采用四路惯性单元，每路传惯性单元采用InvernSense公司生产的MPU6050惯性单元，STM32控制板采用ST公司开发的STM32F407。MPU6050与外设寄存器之间的通信采用400kHz的I2C接口或1MHz的SPI接口，支持的电源范围为2.5V-5V。四路惯性单元和STM32控制器通过2个I2C接口同时连接4个MPU6050，STM32控制板通过4通道数据采集实现惯性传感数据采集。STM32控制板和PC上位机通过R232串口转到USB接口传输数据。

去噪算法采用基于果蝇算法的小波去噪，其原理为用果蝇算法算法优化小波去躁的阈值，每个果蝇种群对应小波每一层的阈值，而味道浓度值以去噪之后的信号和原始信号的均方误差最小为目标。将此算法预先下载在STM32控制板中，当四路MPU6050的数据传过来时首先对其去噪，分别去噪角速度信号和加速度信号，为后续积分解算减少运算量，同时也减少了累积误差的原始项。

融合算法采用自定义的融合公式进行运算。

修正算法采用无迹卡尔曼滤波(UKF)算法进行进一步误差修正，UKF算法可以运用在采煤机惯性导航中，因为采煤机的行走是根据力学原理实现的，系统前后时刻的移动数据和惯性单元的输出数据有关，因此采用双边采样的UKF算法。该算法用于精对准过程，其原理为用UKF算法估计平台的初始误差，然后根据估计误差，采用陀螺施矩的方法将误差角消除，并对惯性器件的误差进行补偿。将此算法预先下载在STM32控制板中，利用该算法滤去解算后的数据实现进一步误差修正，得到一个最优的定位数据。

图2所示为四路惯性单元阵列式分布图，图中每个圆圈代表一个MPU6050惯性单元，运用右手螺旋定则，①号圈中×表示从右手四指从x轴转向y轴时，大拇指方向指向纸的里侧，即z轴由外指向里，同理③号z轴方向指向里侧，同理②和④号中的圆点用右手螺旋法则可知z轴由里指向外侧。由图可知，每个MPU惯性单元的x和y轴两两直接成180度，这在极大程度上消除了环境引起的时变漂移，而随机漂移也随着求平均的过程比单个时的误差要小。

根据四路惯性单元采用对称安装布局组成阵列式惯性传感器，可以得到惯性传感融合方程：

以东北天方向为正，其中1个轴发生转动时，惯性单元的输出值包括三部份：

ω_i＝ω_t+ω_b+ω_r(1)

其中，ω_i表示角速度传感器输出值，ω_t表示真实角速度，ω_b表示漂移，ω_r表示随机漂移。

4路MPU6050数据融合方程为：

采用4个相同的MPU6050惯性单元，假定同一轴有相等的ω_b，基于公式1和2构建角速度输出矩阵为：

温漂等环境造成的时变误差可以通过阵列方式消除，数据精度将取决于角速度数据随机漂移的阵列结果。理论上，即便阵列使随机漂移信号加倍，经过平均后也低于最大的漂移量。

对于4路3轴加速度融合采用同样的方法，需要解耦重力加速度分量。以东北天方向为正，当阵列式惯性传感器随采煤机平动时，加速度输出包括如下分量：

a_i＝a_t+g_i+a_b+a_r (4)

其中，a_i表示加速度传感器输出值，a_t表示真实加速度，g_i表示重力加速度分量，a_b表示漂移，a_r表示随机漂移。

4路加速度融合方程为：

基于公式4和5获得加速度输出矩阵为：

建立了以上公式后，就可以得到阵列式惯性单元所获得的融合数据。

图3所示为惯性传感器在采煤机上的位置示意图。1和8分别为采煤机的左右滚筒，2和7分别为采煤机的左右摇臂，3和6分别为采煤机的左右行走部，4为MPU6050型号的惯性单元，5为固连采煤机和四路惯性单元的圆盘。

图4所示为四路MPU6050惯性单元和STM32控制板的通讯连线图，①和③号MPU6050是正面放置，②和④号MPU6050是背面放置，四个MPU6050惯性单元都通过VCC和STM32控制板的VCC连接提供电源，GND也是四个MPU6050惯性单元通过一条线接入。MPU6050通过SCL、SDA引脚以PC接口的方式分别与STM32控制器SCL、SDA引脚通讯，在这种通讯方式中，MPU6050是作为从机而控制器是作为主机进行工作的。STM32控制板片选信号和MPU6050的AD0连接。MPU6050使用PC接口与控制器的通讯速率可达400kHz。MPU6050的电源可支持VDD范围：2.5V±5％,3.0V±5％，或3.3V±5％。MPU6050上的WHO-AM-I8位寄存器地址为0x75，MPU6050上电复位后该寄存器的默认值是0x68。其中Bit6：Bit1位的数据是110100，Bit0位的高低由引脚AD0决定，Bit7保留。对于MPU6050模块，当AD0接地或悬空时，WHO-AM-I寄存器的值为0x68，当AD0接高电平时WHO-AM-I寄存器的值为0x69。AD0引脚为用户提供了多传感器读取的可能，由于通讯选用I2C通讯，而MPU6050本身由于上述IO地址分配限制，每个I2C总线上只能连接2个MPU6050，故通过2个I2C接口连接4个MPU6050，MPU6050模块默认地址为0x68，而它只有一个地址位0xAD0，高电平为1，地址为0x69，悬空或低电平为0，采用默认地址。I2C要求两条总线：串行数据线SDA和串行时钟线SCL。每个连接到总线的外部设备设定唯一的地址。如果两个或更多主机同时初始化数据传输，通过冲突检测和仲裁防止数据被破坏。I2C双向数据传输位速率在标准模式下可达100kbit/s，快速模式下可达400kbit/s，高速模式下可达3.4Mbit/s。片上的滤波器可以滤去总线数据线上的毛刺波，保证数据完整，连接到相同总线的外部设备数量受到总线的最大电容400pF限制。采煤机定位采用4路MPU6050构成的阵列式惯性传感器，嵌入式主板采为STM32，选择I2C作为采煤机定位的IO接口。

图5所示为四个MPU6050惯性单元的地址分配图，①号地址为0#0x68，②号地址为1#0x68，③号地址为0#0x69，④号地址为1#0x69，①和③号用I2C0来通讯，②和④号采用I2C1来通讯，这样每个MPU6050都有自己的地址得到分配不会造成冲突，可以同时采得四路MPU6050惯性单元的数据。

图6所示是STM32控制板和PC主机之间的传输数据方式，通过R232串口传输，用USB转串口后接入PC主机，实现定位的显示。

图7所示为基于果蝇算法的小波去噪流程图，流程如下：

Step 1：初始化WT-IFOA去噪算法参数。主要包括：小波基函数ψ(·)、小波分解层数J、改进的果蝇算法最大迭代次数N、每个果蝇种群中个体数量P、种群位置范围LR、随机飞行距离FR。初始化各个种群位置，以及每个种群中的个体位置，个体的随机飞行距离服从正态分布。由于采煤机截割声音信号的去噪属于多参数联合优化问题，每一层小波系数都需要选择阈值进行线性收缩，因此每层的阈值都通过一个果蝇种群进行优化。

Step 2：对含噪的四路采煤机传感器信号进行J层小波分解，获取各层小波系数。计算各种群中所有果蝇的距离与味道浓度判断值，每个味道浓度判断值。对应一个阈值的可行解，对每种可行解组合进行去噪，随后进行小波重构，计算含噪信号与重构之后的信号之差作为噪声信号；计算每种可行解下的适应度值，选出其中最大的记为smellbest，保存位置Si(i)。

Step 3：更新种群位置至对应的果蝇位置Si(new)，计算当代果蝇的位置、味道浓度判断值以及适应度，选择其中适应度最大的记为bestsmell，并记录对应果蝇的位置。比较全局最大值smellbest与当前最大值bestsmell，若当前最大值大于全局最大值，用bestsmell替代smellbest，同时用对应的果蝇位置替代果蝇种群的位置。

Step 4：判断当前迭代次数n是否达到最大迭代次数N，若达到，停止迭代并输出最优去噪效果对应的重构信号以及各阶去噪阈值；否则重复第3步。

以上详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换，这些等同变换均属于本发明的保护范围。

Claims (6)

1.一种基于阵列式惯性单元的采煤机定位方法，其特征在于，包括步骤：

S1：在采煤机的箱体上方将惯性单元布置成阵列式结构组成惯性传感器；

S2：将惯性传感器采集的角速度、加速度信息传送到控制器中进行去噪、融合解算、误差修正；

S3：将步骤S2获得的数据发送到PC上位机进行定位显示。

2.根据权利要求1所述的一种基于阵列式惯性单元的采煤机定位方法，其特征在于，所述惯性传感器由四个惯性单元组成，四个惯性单元等间距分布在同一个圆周上。

3.根据权利要求2所述的一种基于阵列式惯性单元的采煤机定位方法，其特征在于，步骤S2中的去噪是采用基于果蝇算法的小波去噪。

4.根据权利要求3所述的一种基于阵列式惯性单元的采煤机定位方法，其特征在于，所述基于果蝇算法的小波去噪包括：用果蝇算法优化小波去躁的阈值，每个果蝇种群对应小波每一层的阈值，味道浓度值以去噪之后的信号和原始信号的最小均方误差为目标，其步骤具体如下：

S41：初始化基于果蝇算法的小波去噪算法参数，主要包括：小波基函数ψ(·)、小波分解层数J、改进的果蝇算法最大迭代次数N、每个果蝇种群中个体数量P、种群位置范围LR、随机飞行距离FR；初始化各个种群位置，以及每个种群中的个体位置，个体的随机飞行距离服从正态分布；每层的阈值通过一个果蝇种群进行优化；

S42：对含噪的四路采煤机传感器信号进行J层小波分解，获取各层小波系数；计算各种群中所有果蝇的距离与味道浓度判断值，每个味道浓度判断值对应一个阈值的可行解，对每种可行解组合进行去噪，之后进行小波重构，计算每种可行解下的适应度值，选出其中最大的记为smellbest，保存位置Si(i)；

S43：更新种群位置至对应的果蝇位置Si(new)，计算当代果蝇的位置、味道浓度判断值以及适应度，选择其中适应度最大的记为bestsmell，并记录对应果蝇的位置；比较全局最大值smellbest与当前最大值bestsmell，若当前最大值大于全局最大值，用bestsmell替代smellbest，同时用对应的果蝇位置替代果蝇种群的位置；

S44：判断当前迭代次数n是否达到最大迭代次数N；若达到，停止迭代并输出最优去噪效果对应的重构信号以及各阶去噪阈值；若否则重复步骤S43。

5.根据权利要求2所述的一种基于阵列式惯性单元的采煤机定位方法，其特征在于，所述步骤S2中的融合解算还包括：以东北天方向为正，构建角速度输出矩阵：

其中，ω_xt、ω_yt、ω_zt分别代表三个方向的真实角速度，ω_xr1、ω_xr2、ω_xr3、ω_xr4分别表示第一至第四个惯性单元x方向的随机漂移，ω_yr1、ω_yr2、ω_yr3、ω_yr4分别表示第一至第四个惯性单元y方向的随机漂移，ω_zr1、ω_zr2、ω_zr3、ω_zr4分别表示第一至第四个惯性单元z方向的随机漂移，t表示真实角速度，r表示随机漂移；

和加速度融合方程：

其中，a_xt、a_yt、a_zt分别代表三个方向的真实加速度，g_x、g_y、g_z分别代表三个方向的重力加速度分量，a_xr1、a_xr2、a_xr3、a_xr4分别表示第一至第四个惯性单元x方向的随机漂移，a_yr1、a_yr2、a_yr3、a_yr4分别表示第一至第四个惯性单元y方向的随机漂移，a_zr1、a_zr2、a_zr3、a_zr4分别表示第一至第四个惯性单元z方向的随机漂移，t表示真实角速度，r表示随机漂移。

6.根据权利要求1所述的一种基于阵列式惯性单元的采煤机定位方法，其特征在于，所述步骤S2中的误差修正采用无迹卡尔曼滤波算法进行滤波获得最优估计；所述步骤S3具体包括：将步骤S2中获得的最优估计通过USB转串口发送到PC上位机进行定位显示。