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

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

A method of centimeter level precise positioning based on close coupling between encoder and inertial equipment

【技术实现步骤摘要】
基于编码器与惯性设备紧耦合实现厘米级精确定位的方法
本专利技术属于煤矿智能开采数字控制
，具体涉及一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法。
技术介绍
近年来煤矿安全事故频发，较高的死亡率给人民生命财产安全带来了极大的危害。煤矿开采要想走出困境，必须大力发展智慧矿山、实现无人操作，而采煤机的精确导航定位技术则是实现无人值守的关键。目前应用于地下的采煤机定位技术主要由下述几种：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采样间隔所测量的里程增量ΔSi在采煤机坐标系下的矢量表示式：Si——第i个采样间隔的里程增量；SiB——第i采样间隔在采煤机坐标系下的矢量；设备位置推算如下式所示：其中：为当地地理坐标系到采煤机坐标系的姿态转换矩阵；θ,γ分别为采煤机在第i个采样点的航向角、俯仰角和横滚角；忽略二次项误差，设备的位置测量误差如下式所示：其中：为姿态转换误差矩阵；△θ,△γ分别为采煤机在第i个采样点的航向角误差、俯仰角误差和横滚角误差；Si为总行程，△Si为行程误差；选择陀螺漂移不大于0.006°/h的陀螺；同时考虑同步带来的测量误差，设备采样频率为200Hz，设备最大运行速度为5m/s，则同步带来的最大误差为Vmax×△t＝2.5cm；设备总的位置测量误差不大于20cm。更进一步，上述S4包括：采用的初始误差标校方法是两位置法，具体为：首先在起点将大地标准坐标点坐标(x0,y0,z0)进行装订，然后进行初始对准，对准完毕转组合导航，使编码器和惯性设备组合装置运行200米后，将组合装置停下，测量设备终点大地标准点坐标(x1,y1,z1)，同时记录设备位置推算坐标(x1′,y1′,z1′)；假设ONE表示东北天坐标系，ON′E′表示导航解算坐标系；β表示两坐标系的北向夹角，即惯性设备初始对准偏差；矢量OY为采煤机行走的真实轨迹；矢量OY′为航位推算轨迹，Y′点在N′系的坐标为(EI′MU,NI′MU)；矢量OY与OY′之间的夹角为惯性元件的安装误差；里程当量的标定如下式：其中：k——里程当量；M——编码器脉冲数；S——总行程；x0,y0,z0——起点坐标；x1,y1,z1——终点坐标；向量OY与真北方向的夹角为：α＝arctan(Eb/Nb)………………………………(7)其中：Eb＝x1-x0,Nb＝y1-y0…………………………(8)矢量OY′与导航坐标之间的夹角为：其中：EIMU′＝x1′-x0,NIMU′＝y1′-y0………………………(10)由图可以看出：则由式(7)到式(12)可知：对于方位误差角的补偿，将作为整体进行补偿，为惯性导航元件测得的航向角，则采煤机的实际航向角为对于俯仰角的补偿，按下式进行计算：其中：△h——高度误差；L——总行驶里程；θ为惯性导航元件测得的航向角，则采煤机的实际航向角为θre＝θ-△θ+η。更进一步，上述S3包括：紧耦合的卡尔曼滤波算法状态量的选取如下：选择的状态变量包括导航解算的姿态角误差东向、北向速度误差δvx、δvy，航位推算位置误差δx、δy、δz，陀螺常值漂移εx、εy、εz，加速度计测量常值偏差和里程当量k；系统观测方程的通式为：其中，H为系统观测矩阵，为系统的观测噪声向量，H和的取值与所选择的匹配方式有关；以位置信息为外观测信息，观测量的量测方程见式式中：Hv——观测方程转移矩阵；V——观测量噪声矢量；式中，下标s表示惯导系统解算输出，下标r表示航位推算输出；Hv＝[03×3I3×303×9]，V为3维速度观测白噪声；根据卡尔曼滤波算法估算出陀螺漂移、加速度计零偏及里程当量误差εx、εy、εz、k，不断修正系统输出。本专利技术具有的优点和积极效果是：通过采用上述技术方案，本专利技术通过选择高精度的编码器安装到煤机行走齿轮上，准确感知煤机运行里程；通过选用光纤惯导实现设备地理系下下姿态测量；通过采用初始对准偏差补偿算法，消除设备初始对准误差和安本文档来自技高网...

【技术保护点】
1.一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，其特征在于，至少包括：/nS1、选择13位的编码器安装到煤机行走齿轮上，感知煤机运行距离；/nS2、选用光纤惯导实现设备地理系下的姿态测量；/nS3、采用紧耦合的航位推算方案解算出设备的地理坐标；/nS4、采用初始对准偏差补偿算法，减小设备初始对准误差和安装偏差。/n

【技术特征摘要】
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采样间隔所测量的里程增量ΔSi在采煤机坐标系下的矢量表示式：



Si——第i个采样间隔的里程增量；
SiB——第i采样间隔在采煤机坐标系下的矢量；
设备位置推算如下式所示：



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




θ,γ分别为采煤机在第i个采样点的航向角、俯仰角和横滚角；
忽略二次项误差，设备的位置测量误差如下式所示：



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




△θ,△γ分别为采煤机在第i个采样点的航向角误差、俯仰角误差和横滚角误差；Si为总行程，△Si为行程误差；
选择陀螺漂移不大于0.006°/h的陀螺；
同时考虑同步带来的测量误差，设备采样频率为200Hz，设备最大运行速度为5m/s，则同步带来的最大误差为Vmax×△t＝2.5cm；
设备总的位置测量误差不大于20cm。


4.根据权利要求3所述的基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，其特征在于，上述S4包括：
采用的初始误差标校方法是两位置法，具体为：
首先在起点将大地标准坐标点坐标(x0,y0,z0)进行装订，然后进行初始对准，对准完毕转组合导航，使编码器和惯性...

【专利技术属性】
技术研发人员：邵珠瑞，侯斌，赵政，赵帅，李永波，
申请(专利权)人：中国船舶重工集团公司第七零七研究所，
类型：发明
国别省市：天津;12