掘进机自主定位定向系统及方法与流程

本发明属于定位系统技术领域，涉及一种掘进机自主定位定向系统及方法，具体为一种基于惯性罗经和视觉测量的掘进机自主定位定向系统及定位定向方法。

背景技术：

掘进机的定位定向系统在地下巷道掘进工作中发挥着至关重要的作用，其直接影响着地下巷道掘进工作的施工效率和施工质量。传统的地下巷道掘进主要依靠激光标靶导向和人工目测实现，由掘进机驾驶员控制掘进机的掘进方向，而巷道掘进工作环境大多粉尘大、照明差、能见度低、噪音差，严重影响到工作人员的身体健康，同时加大了工作人员的工作量，降低了施工效率，因此，实现掘进机掘进过程的自动化尤为重要，而要实现掘进机的自动掘进就必须解决掘进机的自主定位定向的关键技术。

近年来，国内的科研机构和高等院校都致力于掘进机的自主定位定向系统的研究，现阶段相关技术主要有基于惯性测量技术位姿测量方法、基于超宽带测量技术的位姿测量方法、基于igps测量技术的位姿测量方法、基于空间交汇测量技术的位姿测量方法、基于机器视觉的位姿测量方法和基于惯性导航系统(惯导)和视觉测量的位姿(位置和姿态)测量方法。

(1)基于惯性测量技术的位姿测量技术利用惯性导航系统来实时确定运载体的位姿信息，但惯性导航系统存在累积误差，在无标定信号条件下持续测量精度低，误差随时间会越来越大，误差振荡快。可以参见文献：[1]呼守信.基于惯性测量的悬臂式掘进机位姿自动定位[j].机电产品开发与创新,2017,30(03):50-51；[2]陶云飞,杨健健,李嘉赓,张敏骏,符世琛,贾文浩,吴淼.基于惯性导航技术的掘进机位姿测量系统研究[j].煤炭技术,2017,36(01):235-237。

(2)基于超宽带测量技术的位姿测量方法是利用超宽带信号进行距离测量的无线电测距技术，该方法的测量精度易受周围环境的影响，可以参见文献：[3]符世琛.基于uwb测距的悬臂式掘进机位姿检测方法研究[d].中国矿业大学(北京),2018.[4]崔柳,徐会军,刘治翔,谢苗.基于超宽带定位的掘进机姿态监测仿真与实验研究[j].煤炭学报,2017,42(s1):267-274。

(3)基于igps测量技术的位姿测量方法利用三角测量原理来确定接收器位置坐标的坐标测量技术，复杂性高，其通常应用于对测量精度和测量实时性要求较高的制造领域，可以参见文献：[5]原世进.基于igps的悬臂式掘进机位姿测定系统研究[j].煤矿现代化,2018(02):94-95+98。

(4)基于空间交汇测量技术的位姿测量方法是一种针对未知空间点坐标测量方法，但方法的系统可靠性较差，技术风险大，可以参见文献：[6]吴淼,贾文浩,华伟,符世琛,陶云飞,宗凯,张敏骏.基于空间交汇测量技术的悬臂式掘进机位姿自主测量方法[j].煤炭学报,2015,40(11):2596-2602。

(5)基于机器视觉的位姿测量方法是根据摄像机获得的图像视觉信息对目标的位置和姿态进行测量的方法，但图像的获取在多粉尘，照明度低的情况下存在难度，从而影响到定位系统的精度，可以参见文献：[7]田原.基于机器视觉的掘进机空间位姿检测技术研究[j].矿山机械,2013,41(02):27-30；[8]杜雨馨,刘停,童敏明,董海波,周玲玲.基于机器视觉的悬臂式掘进机机身位姿检测系统[j].煤炭学报,2016,41(11):2897-2906。

技术实现要素：

本发明的目的是提供一种基于惯性罗经和视觉测量的掘进机自主定位定向方法，利用惯性罗经提供掘进机的第一姿态信息，再将视觉相机提供的第二姿态信息与第一姿态信息进行数据融合，得到更加准确的定位信息。

本发明的技术方案为：

本发明提供一种基于惯性罗经和视觉测量的掘进机自主定位定向方法，包括如下步骤：

(1)系统启动：自主定位定向系统启动，导航计算机开启，惯性罗经以及视觉相机启动；

(2)初始对准：惯性罗经进行初始对准，以获得精确的初始姿态角；

(3)数据采集：掘进机开始工作后，随着掘进机的运动，惯性罗经中的陀螺仪与加速度计会产生输出，采集数据信息，视觉相机会采集图像信息；

(4)数据同步：将步骤(2)中惯性罗经采集的数据信息与视觉相机采集的图像信息同时输入到导航计算机；

(5)视觉速度解算：将视觉相机得到的图像信息通过光流算法，解算出掘进机的速度信息；

(6)惯性解算：将步骤(5)中解算出的速度信息作为速度阻尼输入到惯性罗经中进行惯性解算，得到第一姿态信息以及位置信息；

(7)视觉姿态测量：根据视觉相机得到的图像信息，通过p3p算法，解算得到第二姿态信息；

(8)数据融合：将第一姿态信息与第二姿态信息通过卡尔曼滤波进行数据融合，解算出最终姿态信息。

优选的，所述步骤(2)为：将惯性罗经根据重力加速度进行粗对准，然后进行精对准。

优选的，所述步骤(6)为：

将步骤(5)中光流算法解算出的速度信息作为速度阻尼输入到惯性罗经中，惯性罗经通过视觉相机输入的速度阻尼，抑制姿态误差振荡，根据惯性罗经中的陀螺仪以及加速度计采样值，进行惯性解算，得到第一姿态信息以及位置信息。

优选的，所述步骤(5)中光流算法为：

假设原图是i(x，y，z)，移动后的图像是i(x+σx,y+σy,z+σz),其满足其中图像移动可以为i(x+σx,y+σy,z+σz,t+σt)＝i(x,y,z,t)，也就是说由上可得其中vx，vy为光流的值(二维图像没有z)，是图像在(x,y,z,t)这一点的梯度(即两帧图像块之间差值)，假设流(vx,vy,vz)在一个大小为m*m*m(m>1)的小窗中是一个常数，那么从像素1...n,n＝m*m*m中可以得到下列一组方程：

三个未知数但是有多于三个的方程，这个方程组自然是个超定方程，也就是说方程组内有冗余，方程组可以表示为：也就是采用最小二乘法为由以上过程即可求出速度信息。

优选的，所述步骤(5)中p3p算法为：

假设o为摄像机光心，控制点a，b，c与光心o之间的三条连线的长度分别记为x，y，z，已知它们之间的夹角分别为α，β，γ及|ab|＝c，|ac|＝b，|bc|＝a，求x，y，z，这就是p3p问题，其方程描述如下：

假设α，β，γ为锐角，假设x，y，z均为大于0的实数，将y和z表示成x的函数

p3p问题的解为以下四种模式：

模式1：

模式2：

模式3：

模式4：

在假设知道α，β，γ，a,b,c的条件下，仅选取了三个特征点，故只有三个方程；实际姿态测量中，选取50-150个特征点进行姿态解算，故可以解算出上述α，β，γ，a,b,c，x,y，z九个参数。

本发明还提供一种基于惯性罗经和视觉测量的掘进机自主定位定向系统，所述定位定向系统采用上述所述的定位定向方法，所述定位定向系统包括掘进机，所述掘进机搭载有惯性罗经、导航计算机以及视觉相机，所述视觉相机通过接口与所述导航计算机相连，所述惯性罗经通过接口与所述导航计算机相连；所述惯性罗经配备有陀螺仪和加速度计；

所述惯性罗经用以获得精确的初始姿态角及位置，并通过视觉相机输入的速度阻尼，抑制姿态误差振荡，根据惯性罗经中的陀螺仪以及加速度计采样值，进行惯性解算，得到掘进机第一姿态信息以及位置信息；

所述视觉相机用以获得图像信息，并通过光流算法实时计算出掘进机的速度信息。

本发明的有益效果为：

(1)本发明提供的基于惯性罗经和视觉测量的掘进机自主定位定向系统，利用惯性罗经提供掘进机的第一姿态信息，再将视觉相机提供的第二姿态信息与第一姿态信息通过卡尔曼滤波进行数据融合，得到更加准确的定位信息。

(2)本发明根据惯性罗经以及视觉相机组合导航方式，无需外界人为输入信息或进行基准标定，可以使掘进机全自动工作，并且省去了繁琐的劳动力。

(3)本发明采用全自主导航，不会产生人为标定基准所带来的误差，使得导航精度下降，故本发明保证了导航系统的精确性。

(4)本发明所采用的惯性罗经，通过视觉相机提供的图像信息解算出来的速度信息作为速度阻尼输入到惯性罗经中，也就是说由外界输入速度阻尼，大大改善了惯性导航系统误差随时间发散较快的问题，可以使得航向角误差减小，保证了姿态角的精确性。

附图说明

图1是本发明实施例一提供的一种基于惯性罗经和视觉测量的掘进机自主定位定向方法流程示意图；

图2是本发明实施例一提供的一种基于惯性罗经和视觉测量的掘进机自主定位定向方法中p3p算法原理示意图；

图3是本发明实施例二提供的一种基于惯性罗经和视觉测量的掘进机自主定位定向系统结构示意图。

具体实施方式

下面将根据附图以及具体实施方式对发明进行详细的说明。以下为本发明的优选实施例，本发明的实施例不限制本发明的保护范围，本发明的保护范围以其权利要求书为准。

实施例一

本发明的实施例一提供一种基于惯性罗经和视觉测量的掘进机自主定位定向方法，包括如下步骤：

(1)系统启动：自主定位定向系统启动，导航计算机开启，惯性罗经以及视觉相机启动；

(2)初始对准：将惯性罗经根据重力加速度进行粗对准，然后进行精对准，以减小初始时刻的角度误差，以获得精确的初始姿态角及位置；

(3)数据采集：掘进机开始工作后，随着掘进机的运动，惯性罗经中的陀螺仪与加速度计会产生输出，采集数据信息，视觉相机会采集图像信息；

(4)数据同步：将步骤(2)中惯性罗经采集的数据信息与视觉相机采集的图像信息同时输入到导航计算机；

(5)视觉速度解算：将视觉相机得到的图像信息通过光流算法，解算出掘进机的速度信息；

光流算法如下：假设原图是i(x,y,z)，移动后的图像是i(x+σx,y+σy,z+σz),其满足其中图像移动可以认为i(x+σx,y+σy,z+σz,t+σt)＝i(x,y,z,t)，也就是说(h.o.t.指更高阶，在移动足够小的情况下可以忽略)，由上可得其中vx，vy为光流的值(二维图像没有z)，是图像在(x,y,z,t)这一点的梯度(即两帧图像块之间差值)，假设流(vx,vy,vz)在一个大小为m*m*m(m>1)的小窗中是一个常数，那么从像素1...n,n＝m*m*m中可以得到下列一组方程：

三个未知数但是有多于三个的方程，这个方程组自然是个超定方程，也就是说方程组内有冗余，方程组可以表示为：也就是采用最小二乘法为由以上过程即可求出速度信息。

(6)惯性解算：将步骤(5)中解算出的速度信息作为速度阻尼输入到惯性罗经中进行惯性解算，得到第一姿态信息以及位置信息；

如果由惯性罗经自身提供速度阻尼，航向角误差振荡快，精度不够，本发明实施例通过由外界输入速度阻尼，也就是由视觉相机的图像信息解算出来的速度信息作为速度阻尼输入到惯性罗经中，可以抑制航向角误差振荡太快，防止误差发散，保证姿态角的精确性。

(7)视觉姿态测量：根据视觉相机得到的图像信息，通过p3p算法，解算得到第二姿态信息；

p3p算法计算姿态信息如下：

如图2所示，o为摄像机光心，控制点a，b，c与光心o之间的三条连线的长度分别记为x，y，z，已知它们之间的夹角分别为α，β，γ及|ab|＝c，|ac|＝b，|bc|＝a，求x，y，z，这就是p3p问题，其方程描述如下：

考虑到实际应用中的相机的视场角，以下讨论中均假设α，β，γ为锐角，另从实际物理意义出发,，假设x，y，z均为大于0的实数，通过配方将y和z表示成x的函数

考虑根号前的符号选取与不同组合，p3p问题的解必为以下4种模式。

模式1：

模式2：

模式3：

模式4：

上述只是验证本方法可以通过视觉测量的手段进行姿态解算，在假设知道α，β，γ，a,b,c的条件下，仅选取了三个特征点，故只有三个方程。实际姿态测量中，一般选取50-150个特征点进行姿态解算，故可以解算出上述α，β，γ，a,b,c，x,y，z九个参数。

(8)数据融合：将第一姿态信息与第二姿态信息通过卡尔曼滤波进行数据融合，解算出最终姿态信息。

本发明实施例将第一姿态信息与第二姿态信息通过卡尔曼滤波进行数据融合，可以减小航向角、横滚角、俯仰角的误差，进一步提高定位的精确性。

本实施例利用标准的卡尔曼滤波进行估计：选取地理坐标中姿态角、速度、位置、陀螺常值漂移和加速度计常值漂移为状态量取系统惯导解算出的姿态与视觉相机解算出的姿态信息差值为观测量，建立15维kalman滤波器：

式中：f是根据误差模型和状态矢量构成的15*15维系统矩阵；w为15维系统随机过程噪声序列；v为三维系统随机观测噪声序列；h＝[03×3i3×303×303×303×3]是3×15维观测矩阵。

实施例二

本发明的实施例二提供一种基于惯性罗经和视觉测量的掘进机自主定位定向系统，所述系统采用上述所述的任意一种定位定向方法，系统结构框图如图3所示，所述定位定向系统包括掘进机，所述掘进机搭载有惯性罗经、导航计算机以及视觉相机，所述视觉相机通过接口与所述导航计算机相连，所述惯性罗经通过接口与所述导航计算机相连，本实施例中接口可以采用串口；所述惯性罗经配备有陀螺仪和加速度计，用以采集数据信息；

所述惯性罗经用以获得精确的初始姿态角及位置，并通过视觉相机输入的速度阻尼，抑制姿态误差振荡，根据惯性罗经中的陀螺仪以及加速度计采样值，进行惯性解算，得到掘进机第一姿态信息以及位置信息；

所述视觉相机用以获得图像信息，并通过光流算法实时计算出掘进机的速度信息。

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。