[发明专利]一种基于机器视觉和惯性导航的掘进机位姿检测方法

权利要求书

1.一种基于机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：包括以下步骤，

S100～惯性系统和工业相机刚性连接，并且固定在掘进机机身顶部；惯性系统用来检测掘进机的姿态，根据当地的经纬高参数对惯性系统进行初对准，对准结束后开始采集掘进机地理坐标系下的X轴、Y轴和Z轴角度参数，根据掘进机姿态角的定义，求出其姿态角，并传输至交换机；

S200～工业相机以视频流方式实时显示巷道顶板信息，控制主机以图片格式保存视频流中的帧序列，对图像进行处理，求出图像中目标的位置和旋转、平移参数，其中，巷道顶板特征是巷道的顶板托盘，呈现正方形；

S300～交换机通过以太网与控制主机进行数据传输，控制主机对图像数据进行处理、目标检测与识别、目标跟踪，对惯性系统采集的数据进行数据转换，求出掘进机实时姿态参数；

S400～利用组合导航检测掘进机的位置参数增量与实时姿态参数。

2.根据权利要求1所述的机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：步骤S100中，需要根据掘进机地理坐标系下的X轴、Y轴和Z轴角度求出掘进机的姿态角；建立载体坐标系，坐标系原点为掘进机的质心O_b，X_b轴方向为掘进机横轴正右方，Y_b轴方向为掘进机纵轴正前方，Z_b轴方向正交X_b轴、Y_b轴竖直向上。

姿态角的定义：

俯仰角：载体坐标系绕地理坐标系旋转，Y_b轴与O_nX_nY_n水平面的夹角，用θ表示，当Y_b轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°；

翻滚角：载体坐标系绕地理坐标系旋转，Z_b轴与通过载体Y_b轴的铅垂面间的夹角，用γ表示。以载体向右倾斜为正，反之为负，旋转范围是-180°～180°；

偏航角：载体坐标系绕地理坐标系旋转，Y_b轴在O_nX_nY_n水平面上的投影与Y_n之间的夹角，用表示。以载体右偏为正，反之为负，旋转范围是0°～360°；

对应的坐标变换矩阵如下：

偏航角的旋转矩阵为：

俯仰角θ的旋转矩阵为：

翻滚角γ的旋转矩阵为：

三个基本旋转矩阵进行合并，求出载体坐标系与地理坐标系的旋转矩阵：

式中，为单位正交矩阵，有：

载体坐标系与地理坐标系之间的转换可以借助姿态矩阵完成，该姿态矩阵可以简化表示为：

由于地理坐标系到载体坐标系的旋转过程中坐标系始终保持直角坐标系，所以为正交矩阵，则有：

对比式(6)与式(7)中的姿态角和姿态矩阵，可在已知姿态矩阵的情况下反推求出姿态角，则有：

3.根据权利要求2所述的机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：所述的步骤S200中，掘进机行进过程中，工业相机的视野范围内至少覆盖两个目标。