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

权利要求书

1.一种基于单目视觉的钻车姿态测量方法，其特征在于该测量系统包括三轴加速度计（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₂的转置矩阵。