[发明专利]基于单目视觉的钻车姿态测量方法

说明书

技术领域

本发明涉及一种钻车控制领域中的姿态测量方法，特别涉及一种基于单目视觉的钻车姿态测量方法。

背景技术

煤矿用液压掘进钻车工作时需要明确钻杆的姿态以便实现定向钻进。在实际操作过程中，通常使用价格昂贵的随钻测量系统测量钻杆姿态。在多数非精确钻进应用中，只需要已知钻车自身姿态，然后通过人工矫正的方式即可实现定向钻进。此方案与随钻测量系统相比，具有成本低、可靠性高等特点。目前尚未查阅到与此方案类似的专利和产品。

测量姿态通常使用带有三轴加速度计和三轴磁强计的姿态传感器。但由于这类传感器容易受到电磁干扰的影响，而钻车自身具有强电磁干扰，因此传统的姿态传感器不能直接安装在钻车上使用。必须使传感器与钻车相隔一定距离，以便降低钻车的电磁干扰信号对传感器的影响。

发明内容

本发明的目的在于克服上述不足，提供一种基于单目视觉的钻车姿态测量方法，避免传感器受到钻车电磁干扰，降低测量成本。

本发明的目的是这样实现的：一种基于单目视觉的钻车姿态测量方法，该测量系统包括三轴加速度计、三轴磁强计、摄像机、辅助平台、矿用工业计算机、钻车以及由光点I、光点II、光点III与光点IV组成的光点矩阵，所述测量方法包括以下步骤：

步骤一：建立地理坐标系，以辅助平台附近某点为坐标原点，分别使地理坐标系的X轴、Y轴和Z轴指向地理东方、地理北方和地心；

建立摄像机坐标系，以摄像机的光心作为坐标系原点，以摄像机光轴向外方向为Z轴正向，以成像平面水平向左方法为X轴正向，Y轴正方向通过右手法则确定；

建立辅助平台坐标系，以辅助平台的表面中心为原点，以水平向右为X轴正方向，以辅助平台截面向里为Y轴正方向，Z轴正方向通过右手法则确定；

建立光点矩形坐标系，以光点矩形的中心为原点，以光点矩形水平向右为X轴正方向，光点矩形水平向下为Y轴正方向，Z轴正方向通过右手法则确定；

建立钻车坐标系，以钻车的表面某一点为原点，以钻车水平向前为X轴正方向，水平向右为Y轴正方向，Z轴正方向通过右手法则确定；

步骤二：设置摄像机内参数，测量光点矩形的长度和宽度；

步骤三：利用三坐标仪标定摄像机坐标系与辅助平台坐标系的相对姿态R₁，光点矩形坐标系与钻车坐标系的相对姿态R₂；

步骤四：利用摄像机采集光点图像，以光点图像坐标作为特征点，利用正交迭代方法和步骤二中获得的摄像机内参数求解P4P问题，计算得到光点矩形坐标系相对于摄像机坐标系的姿态R₃；

步骤五：利用姿态传感器测量辅助平台坐标系相对于地理坐标系的姿态R₄，计算R₄包括以下步骤：

5.1、利用三轴加速度计测量辅助平台所处位置的重力加速度矢量[g_x, g_y, g_z]^T，其中g_x, g_y和g_z分别为三轴加速度计的输出值；

5.2、利用三轴磁强计测量辅助平台所处位置的磁场强度[m_x, m_y, m_z]，其中m_x, m_y和m_z分别为三轴磁强计的输出值；

5.3、利用下式计算R₄：

其中，

g为重力加速度值。

步骤六：通过坐标变换计算钻车坐标系相对于地理坐标系的姿态R：

R = R₂^TR₃R₁R₄

其中，R₂^T为姿态R₂的转置矩阵。

与现有技术相比，本发明的有益效果：

本发明一种基于单目视觉的钻车姿态测量方法，能够对钻车姿态进行非接触式测量，避免钻车电磁信号对姿态传感器的干扰，系统使用的摄像机、矿用工业计算机以及姿态传感器安装方便，维护方便，造价较低，所使用到的姿态测量方法算法成熟，可靠性高，测量精度高。

附图说明

图1为本发明的钻车姿态测量系统示意图。

其中：

地理坐标系 1