CN111207743A - 基于编码器与惯性设备紧耦合实现厘米级精确定位的方法 - Google Patents

Abstract

本发明涉及一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，属于煤矿智能开采数字控制技术领域，其特征在于，所述基于编码器与惯性设备紧耦合实现厘米级精确定位的方法包括：S1、选择13位的编码器安装到煤机行走齿轮上，感知煤机运行距离；S2、选用光纤惯导实现设备地理系下的姿态测量；S3、采用紧耦合的航位推算方案解算出设备的地理坐标；S4、采用初始对准偏差补偿算法，减小设备初始对准误差和安装偏差。通过采用上述技术方案，本发明利用编码器与惯性设备紧耦合的组合导航方案，同时通过初始偏差补偿算法，实现了初始对准误差和安装误差的有效补偿，大大提高了定位精度。

Description

基于编码器与惯性设备紧耦合实现厘米级精确定位的方法

技术领域

本发明属于煤矿智能开采数字控制技术领域，具体涉及一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法。

背景技术

近年来煤矿安全事故频发，较高的死亡率给人民生命财产安全带来了极大的危害。煤矿开采要想走出困境，必须大力发展智慧矿山、实现无人操作，而采煤机的精确导航定位技术则是实现无人值守的关键。

目前应用于地下的采煤机定位技术主要由下述几种：

a)井下射频定位技术；

b)采煤机支架传感定位技术；

c)轨道里程计定位技术；

d)自主定位系统。

其中a)依靠射频读写系统辨识提前布置好的电子标签，定位精度最高为10m，主要用于井下人员定位；b)类似于a)，依赖采煤机上的信号发射装置，与提前布置好的支架上的信号接收装置，缺点是实际生产过程中工作面支架移架频繁，无法实时定位；c)利用采煤机行走齿轮传感器检测采煤机在刮板上的行走距离，该行走距离较为精确，但由于行走轨道并非一条直线，随轨道刮板的推移，轨道方向和形态随机变化，无法满足定位的精度和实时性要求。d)是利用惯导系统实现自主导航定位，但传统的定位算法是构建地理坐标系，通过加速度计的二次积分实现导航定位，但基于加速度计元件精度的限制，无法实现井下厘米级的定位需求。

根据采煤机的实际应用需求，设备的实际作业面为50m—300m，定位精度达到20cm，方能满足采煤机智能开采需求。目前以上定位技术均不能满足定位精度需求，因此，设计开发一种满足上述需求的基于编码器与惯性设备紧耦合实现厘米级精确定位的方法显得是尤为重要。

发明内容

本发明为解决公知技术中存在的技术问题，提供一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，利用编码器与惯性设备紧耦合的组合导航方案，同时通过初始偏差补偿算法，实现了初始对准误差和安装误差的有效补偿，大大提高了定位精度。

本发明的目的是提供一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，至少包括：

S1、选择13位的编码器安装到煤机行走齿轮上，感知煤机运行距离；

S2、选用光纤惯导实现设备地理系下的姿态测量；

S3、采用紧耦合的航位推算方案解算出设备的地理坐标；

S4、采用初始对准偏差补偿算法，减小设备初始对准误差和安装偏差。

进一步，上述S1包括：

选用电子旋转多圈式绝对型编码器，通过联动装置安装到行进齿轮上；所述编码器的性能如下：

a)供电：10V-30V；

b)分辨率：13位8192；

c)精度：0.5°；

d)波特率：10-1000kbit/s；

e)接口类型：CANopen高速接口；

f)温度范围：-40℃—80℃；

g)防护等级：IP65.

所述编码器测得里程的分辨率为

里程精度为

其中M为编码器分辨率，M＝8192；

ρ为编码器精度，ρ＝0.5°；

R为采煤机轮子半径，R＝15cm。

更进一步，上述S2包括：

在采煤机航位推算算法中，里程计第i采样间隔所测量的里程增量ΔS_i在采煤机坐标系下的矢量表示式：

S_i——第i个采样间隔的里程增量；

S_i ^B——第i采样间隔在采煤机坐标系下的矢量；

设备位置推算如下式所示：

其中：

为当地地理坐标系到采煤机坐标系的姿态转换矩阵；

θ,γ分别为采煤机在第i个采样点的航向角、俯仰角和横滚角；

忽略二次项误差，设备的位置测量误差如下式所示：

其中：

为姿态转换误差矩阵；

△θ,△γ分别为采煤机在第i个采样点的航向角误差、俯仰角误差和横滚角误差；S_i为总行程，△S_i为行程误差；

选择陀螺漂移不大于0.006°/h的陀螺；

同时考虑同步带来的测量误差，设备采样频率为200Hz，设备最大运行速度为5m/s，则同步带来的最大误差为V_max×△t＝2.5cm；

设备总的位置测量误差不大于20cm。

更进一步，上述S4包括：

采用的初始误差标校方法是两位置法，具体为：

首先在起点将大地标准坐标点坐标(x₀,y₀,z₀)进行装订，然后进行初始对准，对准完毕转组合导航，使编码器和惯性设备组合装置运行200米后，将组合装置停下，测量设备终点大地标准点坐标(x₁,y₁,z₁)，同时记录设备位置推算坐标(x₁′,y₁′,z₁′)；

假设ONE表示东北天坐标系，ON′E′表示导航解算坐标系；β表示两坐标系的北向夹角，即惯性设备初始对准偏差；矢量OY为采煤机行走的真实轨迹；矢量OY′为航位推算轨迹，Y′点在N′系的坐标为(E_I′_MU,N_I′_MU)；矢量OY与OY′之间的夹角

为惯性元件的安装误差；

里程当量的标定如下式：

其中：

k——里程当量；

M——编码器脉冲数；

S——总行程；

x₀,y₀,z₀——起点坐标；

x₁,y₁,z₁——终点坐标；

向量OY与真北方向的夹角为：

α＝arctan(E_b/N_b)………………………………(7)

其中：

E_b＝x₁-x₀,N_b＝y₁-y₀…………………………(8)

矢量OY′与导航坐标之间的夹角为：

其中：

E_IMU′＝x₁′-x₀,N_IMU′＝y₁′-y₀………………………(10)

由图可以看出：

则

由式(7)到式(12)可知：

对于方位误差角的补偿，将

作为整体进行补偿，

为惯性导航元件测得的航向角，则采煤机的实际航向角为

对于俯仰角的补偿，按下式进行计算：

其中：

△h——高度误差；

L——总行驶里程；

θ为惯性导航元件测得的航向角，则采煤机的实际航向角为θ_re＝θ-△θ+η。

更进一步，上述S3包括：

紧耦合的卡尔曼滤波算法状态量的选取如下：

选择的状态变量包括导航解算的姿态角误差

东向、北向速度误差δv_x、δv_y，航位推算位置误差δx、δy、δz，陀螺常值漂移ε_x、ε_y、ε_z，加速度计测量常值偏差

和里程当量k；

系统观测方程的通式为：

其中，H为系统观测矩阵，

为系统的观测噪声向量，H和

的取值与所选择的匹配方式有关；

以位置信息为外观测信息，观测量的量测方程见式

式中：

H_v——观测方程转移矩阵；

V——观测量噪声矢量；

式中，下标s表示惯导系统解算输出，下标r表示航位推算输出；H_v＝[0_3×3 I_3×30_3×9]，V为3维速度观测白噪声；

根据卡尔曼滤波算法估算出陀螺漂移、加速度计零偏及里程当量误差ε_x、ε_y、ε_z、

k，不断修正系统输出。

本发明具有的优点和积极效果是：

通过采用上述技术方案，本发明通过选择高精度的编码器安装到煤机行走齿轮上，准确感知煤机运行里程；通过选用光纤惯导实现设备地理系下下姿态测量；通过采用初始对准偏差补偿算法，消除设备初始对准误差和安装偏差；通过采用紧耦合的航位推算方案解算出设备准确定地理坐标。

该方法可以满足采煤机的精度和实时性要求，对井下智能定位和无人操作系统提供了有效支撑。目前该方案已完成原理试验验证工作，可广泛推广应用到车载定位定向系统、煤矿、矿山、地铁等多个领域。

附图说明

图1为本发明优选实施例中编码器与惯性设备紧耦合的组合导航框图；

图2为本发明优选实施例中设备信息流向图；

图3为本发明优选实施例中里程当量及初始误差角标定原理图；

图4为本发明优选实施例中紧耦合导航算法框图；

图5为本发明优选实施例实验过程的设备输出小车行驶曲线图；

图6为本发明优选实施例实验过程的终点轨迹放大图。

具体实施方式

为能进一步了解本发明的发明内容、特点及功效，兹例举以下实施例，并配合附图详细说明如下：

如图1至图6所示，本发明的技术方案为：

一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，包括：

a)选择高精度的编码器安装到煤机行走齿轮上，准确感知煤机运行距离；

b)选用光纤惯导实现设备地理系下的姿态测量；

c)采用紧耦合的航位推算方案解算出设备准确的地理坐标；

d)采用初始对准偏差补偿算法，消除设备初始对准误差和安装偏差。

该方法通过航位推算算法，根据采煤机姿态和速度进行位置解算，而确定采煤机的三维位置与姿态，避免了加速度的两次积分，并降低了位置解算的频率，能有效抑制惯性导航的误差累积，大大提高了采煤机的定位精度。

该方法可以满足采煤机的精度和实时性要求，对井下智能定位和无人操作系统提供了有效支撑。目前该方法已完成原理验证和试验验证工作。

上述优选实施例具体步骤如下：

1、编码器元件误差分析及设备选型

目前陆用车载定位定向系统通用的里程计为脉冲型，分辨率为每转120个脉冲。基本的汽车轮径为1m左右，里程的分辨率为2.6cm；实际煤机的轮径为30cm，里程的分辨率为0.8cm，测量精度仅能达到5cm，而要到达实际需求的测量精度，行程的测量精度至少为0.5cm。通用的里程计远达不到系统测量精度需求。

为此，本系统选用电子旋转多圈式绝对型编码器，通过联动装置安装到行进齿轮上。

编码器的性能如下：

a)供电：10V-30V；

b)分辨率：8192(13位)；

c)精度：0.5°；

d)波特率：10-1000kbit/s；

e)接口类型：CANopen高速接口；

f)温度范围：-40℃—80℃；

g)防护等级：IP65。

根据以上性能分析，编码器测得里程的分辨率为

里程精度为

其中M为编码器分辨率，M＝8192；

ρ为编码器精度，ρ＝0.5°；

R为采煤机轮子半径，R＝15cm。

可以算出编码器测得的行程分辨率为0.01cm，行程测量精度为0.13cm。

该行程测量精度非常好的满足了设备里程测量的需求。

2、惯导系统误差分析及设备选型

由前文分析可知，井下环境没有天文导航、卫星GPS/GLONASS/北斗定位、天文导航等任何辅助手段，而完全依赖惯性设备的自主导航的定位方案远不能满足井下高精度的定位需求，因此设计编码器与惯性设备组合的航位推算方案。

在采煤机航位推算算法中，里程计第i采样间隔所测量的里程增量ΔS_i在采煤机坐标系(B系)下的矢量表示式：

S_i——第i个采样间隔的里程增量；

S_i ^B——第i采样间隔在采煤机坐标系下的矢量。

设备位置推算如下式所示：

其中

为当地地理坐标系(N系)到采煤机坐标系(B系)的姿态转换矩阵。

θ,γ分别为采煤机在第i个采样点的航向角、俯仰角和横滚角。

忽略二次项误差，设备的位置测量误差如下式所示：

其中：

为姿态转换误差矩阵；

△θ,△γ分别为采煤机在第i个采样点的航向角误差、俯仰角误差和横滚角误差。S_i为总行程，△S_i为行程误差。

由式(1)到式(3)可知，位置精度与横滚角γ无关。由上节分析可知，行程测量误差最大为0.13cm，

最大为0.13cm。

主要取决于总行程和航向误差，以最大行程为300m，则由航向误差引起的定位误差应不大于15cm，根据航位推算误差公式，设备航向误差不大于1.8′，则应选择陀螺漂移不大于0.006°/h的陀螺。

同时考虑同步带来的测量误差，设备采样频率为200Hz，设备最大运行速度为5m/s。则同步带来的最大误差为V_max×△t＝2.5cm。

所以理论上分析设备总的位置测量误差不大于20cm。

因此本系统选用光纤惯性导航设备，该设备成本低、可靠性高。惯性设备尺寸为245×165×220(mm³)，重量小于9kg。惯性设备的惯性元件选用光纤陀螺仪和石英挠性加速度计，主要性能指标分别如表1和表2所示。

表1光纤陀螺主要性能指标

表2石英挠性加速度计主要性能指标

3、系统信息流程及接口设计

设备的信号流程为：信号处理板中的FPGA每2.5ms采集陀螺和加速度计数据，最后通过双口RAM将陀螺和加速度计数据传递给数字处理器DSP；信号处理板通过CAN接口电路采集编码器脉冲信息，送给DSP进行航位推算及导航解算；信号处理板上的DSP通过串口进行发送接收外部信息。设备和编码器的供电由外部供电系统统一提供。

设备内外部信号流程如图2所示。

4、里程当量及初始误差角的标定方法

进行航位推算需要对量程当量进行标定，标定行程与编码器脉冲的对应关系。航位推算轨迹与实际运行轨迹之间存在一个偏差角，这个偏差角是由初始偏差角(初始对准误差角和安装误差角组成)和陀螺漂移引起的姿态误差积累引起，而初始误差角是引起航位推算轨迹漂移的主要原因，因此需要对初始偏差角进行补偿。由前文分析可知，横滚角不影响航位推算结果，因此只对航向角误差和俯仰角误差进行标定。

本系统采用的初始误差标校方法是两位置法。首先在起点将大地标准坐标点坐标(x₀,y₀,z₀)进行装订，然后进行初始对准，对准完毕转组合导航，使编码器和惯性设备组合装置运行200米后，将组合装置停下，测量设备终点大地标准点坐标(x₁,y₁,z₁)，同时记录设备位置推算坐标(x₁′,y₁′,z₁′)。

里程当量及初始误差角标定原理如图3所示。图中ONE表示东北天坐标系，ON′E′表示导航解算坐标系；β表示两坐标系的北向夹角，即惯性设备初始对准偏差；矢量OY为采煤机行走的真实轨迹。矢量OY′为航位推算轨迹，Y′点在N′系的坐标为(E_I′_MU,N_I′_MU)；矢量OY与OY′之间的夹角

为惯性元件的安装误差。

里程当量的标定如下式：

其中：

k——里程当量；

M——编码器脉冲数；

S——总行程；

x₀,y₀,z₀——起点坐标；

x₁,y₁,z₁——终点坐标。

由图4可知向量OY与真北方向的夹角为

α＝arctan(E_b/N_b)………………………………(7)

其中

E_b＝x₁-x₀,N_b＝y₁-y₀…………………………(8)

矢量OY′与导航坐标之间的夹角为

其中

E_IMU′＝x₁′-x₀,N_IMU′＝y₁′-y₀………………………(10)

由图可以看出

则

由式(7)到式(12)可知

对于方位误差角的补偿，将

作为整体进行补偿即可。

为惯性导航元件测得的航向角，则采煤机的实际航向角为

对于俯仰角的补偿，按下式进行计算：

其中：

△h——高度误差；

L——总行驶里程。

θ为惯性导航元件测得的航向角，则采煤机的实际航向角为θ_re＝θ-△θ+η。

5、编码器与惯性装置紧耦合导航算法

编码器与惯性设备进行紧耦合组合导航的算法流程如图4所示。

航位推算的算法在前文已讲，具体见式(1)到式(3)，在此不再细述。

本设备的导航解算通路有两个，一个通路的导航解算为航位推算，另一个通路的导航解算为传统的加速度计信息在地理坐标系下进行分解后，两次积分获得速度和位置信息。

紧耦合的卡尔曼滤波算法状态量的选取如下：

选择的状态变量包括导航解算的姿态角误差

东向、北向速度误差δv_x、δv_y，航位推算位置误差δx、δy、δz，陀螺常值漂移ε_x、ε_y、ε_z，加速度计测量常值偏差

和里程当量k。

系统观测方程的通式为

其中，H为系统观测矩阵，

为系统的观测噪声向量，它们的取值与所选择的匹配方式有关。

本系统以位置信息为外观测信息，观测量的量测方程见式

式中：

H_v——观测方程转移矩阵；

V——观测量噪声矢量。

式中，下标s表示惯导系统解算输出，下标r表示航位推算输出。H_v＝[0_3×3 I_3×30_3×9]，V为3维速度观测白噪声。

根据卡尔曼滤波算法估算出陀螺漂移、加速度计零偏及里程当量误差ε_x、ε_y、ε_z、

k，不断修正系统输出。

6、试验验证

试验工具：惯导IMU、编码器、试验小推车、电脑、UPS电源；

试验准备：将IMU固定在小车上，IMU指北方位与小车前进方向一致，编码器安装在小车后轮，UPS放到小车上，作为IMU的电源。

试验过程以下：

1)标定

将小车停在C点标准点，将C点标准点的位置装订到IMU,保持小车静态，向IMU发送对准指令，对准5min后，推着小车从C点行驶到B点，记录此时系统输出的位置信息，根据已知的C点和B点准确的位置信息和系统航位推算输出的B点的位置信息标定出系统与里程计之间的标度因数误差、航向安装角误差和俯仰安装角误差。

2)定位试验

标定完毕后，系统在C点装订准确的位置信息并且重新对准后，从C点出发，推小车行驶到B点，对比B点系统航位推算位置坐标和B点实际的位置坐标即可以得出系统航位推算的精度。记录完毕后拉着小车再从B点行驶到C点重新装订C点坐标，回程不记录。为了方便对比，将所有位置信息减去C点标准点位置，得到所有点相对C标准点是位置。

行驶轨迹如图5所示，终点轨迹放大图如图6所示，系统定位精度统计如表3所示。

表3系统定位精度统计表

从表3看出，行驶总行程为300.9334m时，位置误差最大值为11.7cm，满足20cm的定位精度要求。

本发明通过选择高精度的编码器安装到煤机行走齿轮上，准确感知煤机运行里程；通过选用光纤惯导实现设备地理系下下姿态测量；通过采用初始对准偏差补偿算法，消除设备初始对准误差和安装偏差；通过采用紧耦合的航位推算方案解算出设备准确定地理坐标。

该方法可以满足采煤机的精度和实时性要求，对井下智能定位和无人操作系统提供了有效支撑。目前该方案已完成原理试验验证工作，可广泛推广应用到车载定位定向系统、煤矿、矿山、地铁等多个领域。

以上所述仅是对本发明的较佳实施例而已，并非对本发明作任何形式上的限制，凡是依据本发明的技术实质对以上实施例所做的任何简单修改，等同变化与修饰，均属于本发明技术方案的范围内。

Claims (5)

1.一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，其特征在于，至少包括：

S1、选择13位的编码器安装到煤机行走齿轮上，感知煤机运行距离；

S2、选用光纤惯导实现设备地理系下的姿态测量；

S3、采用紧耦合的航位推算方案解算出设备的地理坐标；

S4、采用初始对准偏差补偿算法，减小设备初始对准误差和安装偏差。

2.根据权利要求1所述的基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，其特征在于，上述S1包括：

选用电子旋转多圈式绝对型编码器，通过联动装置安装到行进齿轮上；所述编码器的性能如下：

a)供电：10V-30V；

b)分辨率：13位8192；

c)精度：0.5°；

d)波特率：10-1000kbit/s；

e)接口类型：CANopen高速接口；

f)温度范围：-40℃—80℃；

g)防护等级：IP65。

所述编码器测得里程的分辨率为

里程精度为

其中M为编码器分辨率，M＝8192；

ρ为编码器精度，ρ＝0.5°；

R为采煤机轮子半径，R＝15cm。

3.根据权利要求2所述的基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，其特征在于，上述S2包括：

在采煤机航位推算算法中，里程计第i采样间隔所测量的里程增量ΔS_i在采煤机坐标系下的矢量表示式：

S_i——第i个采样间隔的里程增量；

S_i ^B——第i采样间隔在采煤机坐标系下的矢量；

设备位置推算如下式所示：

其中：

为当地地理坐标系到采煤机坐标系的姿态转换矩阵；

θ,γ分别为采煤机在第i个采样点的航向角、俯仰角和横滚角；

忽略二次项误差，设备的位置测量误差如下式所示：

其中：

为姿态转换误差矩阵；

△θ,△γ分别为采煤机在第i个采样点的航向角误差、俯仰角误差和横滚角误差；S_i为总行程，△S_i为行程误差；

选择陀螺漂移不大于0.006°/h的陀螺；

同时考虑同步带来的测量误差，设备采样频率为200Hz，设备最大运行速度为5m/s，则同步带来的最大误差为V_max×△t＝2.5cm；

设备总的位置测量误差不大于20cm。

4.根据权利要求3所述的基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，其特征在于，上述S4包括：

采用的初始误差标校方法是两位置法，具体为：

首先在起点将大地标准坐标点坐标(x₀,y₀,z₀)进行装订，然后进行初始对准，对准完毕转组合导航，使编码器和惯性设备组合装置运行200米后，将组合装置停下，测量设备终点大地标准点坐标(x₁,y₁,z₁)，同时记录设备位置推算坐标(x₁′,y₁′,z₁′)；

假设ONE表示东北天坐标系，ON′E′表示导航解算坐标系；β表示两坐标系的北向夹角，即惯性设备初始对准偏差；矢量OY为采煤机行走的真实轨迹；矢量OY′为航位推算轨迹，Y′点在N′系的坐标为(E′_IMU,N′_IMU)；矢量OY与OY′之间的夹角

为惯性元件的安装误差；

里程当量的标定如下式：

其中：

k——里程当量；

M——编码器脉冲数；

S——总行程；

x₀,y₀,z₀——起点坐标；

x₁,y₁,z₁——终点坐标；

向量OY与真北方向的夹角为：

α＝arctan(E_b/N_b)………………………………(7)

其中：

E_b＝x₁-x₀,N_b＝y₁-y₀…………………………(8)

矢量OY′与导航坐标之间的夹角为：

其中：

E_IMU′＝x₁′-x₀,N_IMU′＝y₁′-y₀………………………(10)

由图可以看出：

则

由式(7)到式(12)可知：

对于方位误差角的补偿，将

作为整体进行补偿，

为惯性导航元件测得的航向角，则采煤机的实际航向角为

对于俯仰角的补偿，按下式进行计算：

其中：

△h——高度误差；

L——总行驶里程；

θ为惯性导航元件测得的航向角，则采煤机的实际航向角为θ_re＝θ-△θ+η。

5.根据权利要求4所述的基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，其特征在于，上述S3包括：

紧耦合的卡尔曼滤波算法状态量的选取如下：

选择的状态变量包括导航解算的姿态角误差

东向、北向速度误差δv_x、δv_y，航位推算位置误差δx、δy、δz，陀螺常值漂移ε_x、ε_y、ε_z，加速度计测量常值偏差

和里程当量k；

系统观测方程的通式为：

其中，H为系统观测矩阵，

为系统的观测噪声向量，H和

的取值与所选择的匹配方式有关；

以位置信息为外观测信息，观测量的量测方程见式

式中：

H_v——观测方程转移矩阵；

V——观测量噪声矢量；

式中，下标s表示惯导系统解算输出，下标r表示航位推算输出；H_v＝[0_3×3 I_3×3 0_3×9]，V为3维速度观测白噪声；

根据卡尔曼滤波算法估算出陀螺漂移、加速度计零偏及里程当量误差ε_x、ε_y、ε_z、

k，不断修正系统输出。

Images