基于视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端技术方案

本发明专利技术提供了一种基于视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端，其不受巷道高度变化的影响，能够在掘进机工作的过程中，校正捷联惯导的误差，以给出准确的姿态和位置测量结果，确保挖掘精度，方法包括以下步骤：在巷道内设置激光靶和激光指向仪，激光指向仪在激光靶上形成光斑；获取激光靶到巷道起始位置的位移；在掘进机上设置摄像头，通过摄像头拍摄光斑，获取光斑相对摄像头的位移；根据激光靶到巷道起始位置的位移和光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移；掘进机的捷联惯导系统根据计算得到的掘进机相对巷道起始位置的位移进行误差校正。

The method, system and terminal of heading machine's attitude correction based on vision and strapdown inertial navigation

【技术实现步骤摘要】
基于视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端
本专利技术涉及掘进机位姿测量
，具体涉及一种基于视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端。
技术介绍
掘进机挖掘巷道时，要求挖掘出的巷道沿激光指向仪给出的方向，且上下左右边界相对激光指向仪光线的距离满足预定要求。现有掘进机一般采用人工操作，挖掘一段距离之后检查挖掘误差，然后进行人工修正的方法。此类方法存以下缺点：1、不能连续工作、效率低2、挖掘精度不高3、人员工作环境差和危险大公开号为CN103234513B的中国专利技术专利《基于摄像头和航姿仪的掘进自动导向系统及导向方法》提出了在车上安装一个摄像头和一个激光靶，通过摄像头检测光斑偏离激光靶中心的水平距离，然后根据该信息和掘进机航姿信息调整掘进机位置和方向，始终让掘进机保持在激光指向仪轴线上，以确保挖掘精度的方法。该方法要求激光指向仪始终能够照射到车上的激光靶中心附近，但巷道一般存在较大的高度变化，很难保证激光指向仪始终能够照射到车上的激光靶，所以该方法很难在工程中实际应用。本文档来自技高网...

【技术保护点】
1.基于视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于，包括以下步骤：/n步骤1：在巷道内设置激光靶和激光指向仪，激光指向仪在激光靶上形成光斑；/n步骤2：获取激光靶到巷道起始位置的位移；/n步骤3：在掘进机上设置摄像头，通过摄像头拍摄光斑，获取光斑相对摄像头的位移；/n步骤4：根据激光靶到巷道起始位置的位移和光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移；/n步骤5：掘进机的捷联惯导系统根据计算得到的掘进机相对巷道起始位置的位移进行误差校正。/n

【技术特征摘要】
1.基于视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于，包括以下步骤：
步骤1：在巷道内设置激光靶和激光指向仪，激光指向仪在激光靶上形成光斑；
步骤2：获取激光靶到巷道起始位置的位移；
步骤3：在掘进机上设置摄像头，通过摄像头拍摄光斑，获取光斑相对摄像头的位移；
步骤4：根据激光靶到巷道起始位置的位移和光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移；
步骤5：掘进机的捷联惯导系统根据计算得到的掘进机相对巷道起始位置的位移进行误差校正。


2.根据权利要求1所述的基于视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于：在步骤2中，激光靶到巷道起始位置的位移通过如下公式计算：



其中，L为激光靶到巷道起始位置的距离，φ为激光指向仪的方位信息。


3.根据权利要求2所述的基于视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于：在步骤3中，在掘进机上设置第一摄像头和第二摄像头，第一摄像头测量光斑在第一摄像头归一化成像平面上的像点坐标(u5,v5)，第二摄像头测量光斑在第二摄像头归一化成像平面上的像点坐标(u6,v6)，光斑在第一摄像头的坐标中的位置(X,Y,Z)与光斑在第一摄像头归一化成像平面上的像点坐标(u5,v5)以及光斑在第二摄像头归一化成像平面上的像点坐标(u6,v6)之间的关系表示为：



其中，fx、fy为摄像头参数；d为第一摄像头和第二摄像头之间的距离；
光斑相对第一摄像头的位移在掘进机的导航坐标系n中坐标通过如下公式表示：



其中，为第一摄像头5相对掘进机的安装矩阵，为捷联惯导姿态矩阵。


4.根据权利要求3所述的基于视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于：在步骤4中，掘进机相对巷道起始位置的位移通过如下公式表示：



其中，为掘进机相对巷道起始位置的位移。


5.基于视觉和捷联惯导的掘进机位姿纠偏系统，其特征在于，包...

【专利技术属性】
技术研发人员：赵长山，
申请(专利权)人：北京航天众信科技有限公司，
类型：发明
国别省市：北京;11