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

权利要求书

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

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

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

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、选用光纤惯导实现设备地理系下的姿态测量；

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

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

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

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

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

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

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

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

其中：为姿态转换误差矩阵；

分别为煤机在第i个采样点的航向角误差、俯仰角误差和横滚角误差；S_i为第i采样间隔所测量的里程增量，ΔS_i为第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＝θ-Δθ+η。

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

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

选择的状态变量包括导航解算的姿态角误差东向、北向速度误差δv_x、δv_y，航位推算位置误差δx、δy、δz，陀螺常值漂移ε_x、ε_y、ε_z，加速度计测量常值偏差和里程当量k；

系统观测方程的通式为：

其中，H为系统观测矩阵，为系统的观测噪声向量，H和的取值与所选择的匹配方式有关；

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

式中：

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

V——观测量噪声矢量；

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

根据卡尔曼滤波算法估算出陀螺漂移、加速度计零偏及里程当量误差ε_x、ε_y、ε_z、k，不断修正系统输出。