基于单目视觉的钻车姿态测量方法

摘要

本发明涉及一种基于单目视觉的钻车姿态测量方法，利用摄像机采集光点图像，利用矿用工业计算机处理该光点图像，以光点的图像坐标作为特征点，利用摄像机内参数和公知的正交迭代方法求解P4P问题，计算出光点矩形相对于摄像机的姿态，并结合光点矩形相对于钻车的姿态、摄像机相对于辅助平台的姿态、辅助平台相对于地理坐标系的姿态，通过坐标变换计算出钻车相对于地理坐标系的姿态。本发明能够对钻车姿态进行非接触式测量，避免钻车电磁信号对姿态传感器的干扰，所使用的测量方法算法成熟，可靠性高，测量精度高。

主权项

一种基于单目视觉的钻车姿态测量方法，其特征在于该测量系统包括三轴加速度计（2）、三轴磁强计（3）、摄像机（5）、辅助平台（7）、矿用工业计算机（8）、钻车（16）以及由光点I（11）、光点II（12）、光点III（13）与光点IV（14）组成的光点矩形（10），所述测量方法包括以下步骤：步骤一：建立地理坐标系（1），以辅助平台（7）附近某点为坐标原点，分别使地理坐标系（1）的X轴、Y轴和Z轴指向地理东方、地理北方和地心；建立摄像机坐标系（4），以摄像机（5）的光心作为坐标系原点，以摄像机（5）光轴向外方向为Z轴正向，以成像平面水平向左方向为X轴正向，Y轴正方向通过右手法则确定；建立辅助平台坐标系（6），以辅助平台（7）的表面中心为原点，以水平向右为X轴正方向，以辅助平台7截面向里为Y轴正方向，Z轴正方向通过右手法则确定；建立光点矩形坐标系（9），以光点矩形（10）的中心为原点，以光点矩形（10）水平向右为X轴正方向，光点矩形（10）水平向下为Y轴正方向，Z轴正方向通过右手法则确定；建立钻车坐标系（15），以钻车（16）的表面某一点为原点，以钻车（16）水平向前为X轴正方向，水平向右为Y轴正方向，Z轴正方向通过右手法则确定；步骤二：设置摄像机（5）内参数，测量光点矩形（10）的长度和宽度；步骤三：利用三坐标仪标定摄像机坐标系（4）与辅助平台坐标系（6）的相对姿态R₁，光点矩形坐标系（9）与钻车坐标系（15）的相对姿态R₂；步骤四：利用摄像机（5）采集光点图像，以光点图像坐标作为特征点，利用正交迭代方法和步骤二中获得的摄像机内参数求解P4P问题，计算得到光点矩形坐标系（9）相对于摄像机坐标系（4）的姿态R₃；步骤五：利用姿态传感器测量辅助平台坐标系（6）相对于地理坐标系（1）的姿态R₄，计算R₄包括以下步骤：5.1、利用三轴加速度计（2）测量辅助平台（7）所处位置的重力加速度矢量[g_x, g_y, g_z]^T，其中g_x, g_y和g_z分别为三轴加速度计（2）的输出值；5.2、利用三轴磁强计（3）测量辅助平台（7）所处位置的磁场强度[m_x, m_y, m_z]，其中m_x, m_y和m_z分别为三轴磁强计（3）的输出值；5.3、利用下式计算R₄：其中，g为重力加速度值；步骤六：通过坐标变换计算钻车坐标系（15）相对于地理坐标系（1）的姿态R：R = R₂^TR₃R₁R₄其中，R₂^T为姿态R₂的转置矩阵。