CN112254646A - 一种顶管机姿态识别系统及方法、存储介质 - Google Patents

Abstract

本发明提供了一种顶管机姿态识别系统及方法、存储介质，其方法包括：控制激光发射器发射激光束以分别依次在前屏和后屏上形成光斑；控制第一成像机构和第二成像机构分别拍摄获取前屏光斑图像、后屏光斑图像；控制设于箱体内的倾角机构测量获取箱体的角度参数；根据前、后屏光斑图像和角度参数计算得到顶管机的姿态信息。本发明实现顶管机姿态信息的实时监控，进而提高对顶管机操作的精准性和安全性，为快速、高效的施工提供有力保障。

Description

一种顶管机姿态识别系统及方法、存储介质

技术领域

本发明涉及顶管机导向技术领域，尤指一种顶管机姿态识别系统及方法、存储介质。

背景技术

顶管隧道掘进技术是地下暗挖的一种工程建设技术，是应用于现代地下工程、交通运输、市政建设工程中的管路铺设新技术，运用顶管技术能在管线穿越道路、河流时不用明挖，在施工过程中对道路交通、河流泄流不造成任何影响，具有自动化程度高、施工速度快等优势，并且在开挖时可以控制地面沉降，减少对地面建筑物的影响，另外在水下开挖时不会影响水面交通。

随着顶管施工技术的日益成熟，自动导向系统成为了顶管机施工的一个重要组成部分，而激光靶是顶管机自动导向系统中不可或缺的关键仪器，在掘进过程中，司机需要不断地通过激光靶获取顶管机与隧道设计轴线的相对位置关系，包括滚动角、俯仰角、方位角和偏差信息，以保证隧道沿正确的方向实现贯通。

目前本领域技术人员常使用全站仪结合单屏激光靶的导向系统，使用较多的激光靶导向系统有ZED自动导向系统、VMT导行系统和TACS导向系统，其原理都是依靠全站仪发射光束指向激光靶并在屏幕上成像形成光斑，工业相机对准屏幕采集光斑图像，具有使用方便、稳定性强的优点，其缺点是方位角测量范围小，且需要频繁更换全站仪的站点，并不能充分满足精密工程现场对高精度和便捷性的要求，并且全站仪价格较高，一定程度上增加了工程施工的成本。

发明内容

本发明的目的是提供一种顶管机姿态识别系统及方法、存储介质，实现顶管机姿态信息的实时监控，进而提高对顶管机操作的精准性和安全性，为快速、高效的施工提供有力保障。

本发明提供的技术方案如下：

本发明提供一种顶管机姿态识别系统，包括：处理器、激光发射器和设于顶管机内部的双屏激光靶；所述双屏激光靶包括：箱体、第一成像机构、第二成像机构以及置于箱体内的倾角机构；

所述箱体包括相对设置的前屏和后屏；

所述激光发射器用于发射激光束以分别依次在所述前屏和后屏上形成光斑；

所述第一成像机构设于所述箱体上，且第一成像机构的视野正对所述前屏以拍摄所述前屏上的光斑得到前屏光斑图像；

所述第二成像机构设于所述箱体上，且第二成像机构的视野正对所述后屏以拍摄所述后屏上的光斑得到后屏光斑图像；

所述倾角机构用于测量所述箱体的角度参数；

所述处理器，分别与所述倾角机构、第一成像机构和第二成像机构连接；

所述处理器包括：

图像识别模块，用于对所述前、后屏光斑图像进行图像识别查找到前、后屏光斑质心的映射点；

转换计算模块，用于根据所述映射点的像素坐标计算得到所述光斑质心的世界坐标；

姿态计算模块，根据所述世界坐标进行解算得到所述顶管机在掘进状态下的姿态信息；

所述图像识别模块包括：

搜索子模块，用于对整个前、后屏光斑图像分别进行搜索确定主光斑位置；

确定子模块，用于在主光斑位置的预设范围区域进行边界提取，将包围主光斑的环形区域确定为计算范围；

识别子模块，用于根据所述计算范围和加权质心法，识别查找到前、后屏光斑质心的映射点；

所述姿态计算模块包括：

查询子模块，用于根据前、后屏光斑质心分别在前屏和后屏的图像坐标查询标定数据表，得到光斑质心在前屏和后屏在激光靶坐标系下的空间坐标；

计算子模块，用于根据光斑质心的空间坐标，计算得到入射激光分别在激光靶坐标系下的第一直线方向矢量，以及工程坐标系下的第二直线方向矢量；

姿态获取子模块，用于根据入射激光的第一、第二直线方向矢量，以及所述倾角机构测量得到角度参数计算得到双屏激光靶的姿态角度，根据双屏激光靶的姿态角度，以及测量得到的盾首、盾尾特征点在工程坐标系下Y轴的空间矢量，计算得到所述顶管机在掘进状态下的姿态信息。

本发明还提供一种顶管机姿态识别方法，应用于所述的顶管机姿态识别系统，包括步骤：

控制激光发射器发射激光束以分别依次在前屏和后屏上形成光斑；

控制第一成像机构和第二成像机构分别拍摄获取前屏光斑图像、后屏光斑图像；

控制设于箱体内的倾角机构测量获取箱体的角度参数；

对所述前、后屏光斑图像进行图像识别查找到前、后屏光斑质心的映射点；

根据所述映射点的像素坐标计算得到所述光斑质心的世界坐标；

根据所述世界坐标进行解算得到所述顶管机在掘进状态下的姿态信息；

所述对所述前、后屏光斑图像进行图像识别查找到前、后屏光斑质心的映射点包括步骤：

对整个前、后屏光斑图像分别进行搜索确定主光斑位置；

在主光斑位置的预设范围区域进行边界提取，将包围主光斑的环形区域确定为计算范围；

根据所述计算范围和加权质心法，识别查找到前、后屏光斑质心的映射点；

所述根据所述世界坐标进行解算得到所述顶管机在掘进状态下的姿态信息包括步骤：

根据前、后屏光斑质心分别在前屏和后屏的图像坐标查询标定数据表，得到光斑质心在前屏和后屏在激光靶坐标系下的空间坐标；所述标定数据表预先根据光斑质心在图像坐标系和激光靶坐标系下的映射关系建立生成；

根据光斑质心的空间坐标，计算得到入射激光分别在激光靶坐标系下的第一直线方向矢量，以及工程坐标系下的第二直线方向矢量；

根据入射激光的第一、第二直线方向矢量，以及所述倾角机构测量得到角度参数计算得到双屏激光靶的姿态角度，根据双屏激光靶的姿态角度，以及测量得到的盾首、盾尾特征点在工程坐标系下Y轴的空间矢量，计算得到所述顶管机在掘进状态下的姿态信息。

本发明还提供一种存储介质，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现如所述的顶管机姿态识别方法所执行的操作。

通过本发明提供的一种顶管机姿态识别系统及方法、存储介质，能够实现顶管机姿态信息的实时监控，进而提高对顶管机操作的精准性和安全性，为快速、高效的施工提供有力保障。

附图说明

下面将以明确易懂的方式，结合附图说明优选实施方式，对一种顶管机姿态识别系统及方法、存储介质的上述特性、技术特征、优点及其实现方式予以进一步说明。

图1本发明一种双屏激光靶的结构示意图；

图2本发明一种双屏激光靶的成像内部结构示意图；

图3本发明一种顶管机姿态识别系统的结构示意图；

图4本发明一种双屏激光靶的相机坐标系OCXCYCZC定义示意图；

图5本发明一种双屏激光靶的工程坐标系OtXtYtZt和激光靶坐标系OSXSYSZS定义及相互关系示意；

图6是本发明一种顶管机姿态识别方法的一个实施例的流程图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其他实施例中也可以实现本申请。在其他情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所述描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其他特征、整体、步骤、操作、元素、组件和/或集合的存在或添加。

为使图面简洁，各图中只示意性地表示出了与本发明相关的部分，它们并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部件，仅示意性地绘示了其中的一个，或仅标出了其中的一个。在本文中，“一个”不仅表示“仅此一个”，也可以表示“多于一个”的情形。

还应当进一步理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

另外，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。

如图1所示，本发明提供一种双屏激光靶20，包括：

箱体3、激光发射器、第一成像机构2、第二成像机构24、处理器以及置于箱体3内的倾角机构7；

所述箱体3包括相对设置的前屏1和后屏4；

所述激光发射器用于发射激光束以分别依次在所述前屏1和后屏4上形成光斑；

所述第一成像机构2设于所述箱体3上，且所述第一成像机构2视野正对所述前屏1以拍摄所述前屏1上的光斑得到前屏光斑图像；

所述第二成像机构24设于所述箱体3上，且第二成像机构24的视野正对所述后屏4以拍摄所述后屏4上的光斑得到后屏光斑图像；

所述倾角机构7用于测量所述箱体3的角度参数；

所述处理器，分别与所述倾角机构7、第一成像机构2和第二成像机构24连接，用于根据前、后屏光斑图像和角度参数计算得到顶管机26的姿态信息。

具体的，顶管机26导向用的双屏激光靶20，设置于顶管机26内部，与顶管机26形成稳定的刚体连接。双屏激光靶20包括箱体3、第一成像机构2、第二成像机构24、处理器以及置于箱体3内的倾角机构7。

所述箱体3包括框架5、底板8、侧板6、前屏11和后屏4，所述侧板6、前屏11和后屏4安装在所述框架5上形成闭环，所述底板8设置于所述闭环的下端口。

所述倾角机构7置于箱体3内的倾角机构7，可设置在箱体3的框架内侧，也可设置在侧板内侧。当然，基于前述实施例，所述箱体3包括底板，所述倾角机构7包括防护支架和倾角测量传感器，所述倾角测量传感器通过防护支架安装于所述底板上。倾角机构7优选安装于所述底板上且位于所述双屏激光靶20的机械中心处，倾角机构7实现测量所述双屏激光靶20的角度参数，角度参数包括俯仰角和滚转角。

所述第一成像机构2和第二成像机构24的内部结构相同，可以安装在所述框架5上方，且采用对设结构，分别照射前后屏4幕，即第一成像机构2和第二成像机构24的安装点连接线与前、后屏4中心点连接线平行。

如图2所示，所述第一成像机构2和第二成像机构24包括防水头9、相机防护外壳10、防护端盖15、工业相机11、滤光片12、防护玻璃13和安装支架14，所述滤光片12能够有效去除杂光，增强成像效果。所述防护玻璃13采用方形高透玻璃片，对所述工业相机进行防水和防尘保护。所述第一成像机构2通过安装支架安装在所述后屏4上方，所述第二成像机构24通过安装支架安装在所述前屏1上方。示例性的，延续上述实施例，第一成像机构2通过安装支架14固定在双屏激光靶20的后屏4顶部的框架5上方，第二成像机构24固定在双屏激光靶20的前屏1顶部的框架5上方。

基于前述实施例，所述前屏1采用透明基板，所述后屏4采用非透明基板。示例性的，所述前屏11材料采用高透的亚克力、PS有机板、PET板等透明基板，使得激光能够穿透前屏1并留下清晰的光斑。所述后屏4材料采用白色磨砂亚克力、PS有机板、PET板等非透明基板，将激光的入射光斑成像。

如图4所示，本发明提供一种顶管机26姿态识别系统，包括：处理器、激光发射器和上述实施例中的设于顶管机26内部的双屏激光靶20；所述双屏激光靶20包括：箱体3、第一成像机构2、第二成像机构24以及置于箱体3内的倾角机构7；

所述激光发射器用于发射激光束以分别依次在所述前屏1和后屏4上形成光斑；

所述箱体3包括相对设置的前屏1和后屏4；

所述第一成像机构2设于所述箱体3上，且所述第一成像机构2视野正对所述前屏1以拍摄所述前屏1上的光斑得到前屏光斑图像；

所述第二成像机构24设于所述箱体3上，且第二成像机构24的视野正对所述后屏4以拍摄所述后屏4上的光斑得到后屏光斑图像；

所述倾角机构7用于测量所述箱体3的角度参数；

所述处理器，分别与所述倾角机构7、第一成像机构2和第二成像机构24连接，用于根据前、后屏光斑图像和角度参数计算得到顶管机26的姿态信息。

具体的，本实施例中双屏激光靶20的结构和效果参见上述实施例，在此不再一一赘述。本发明中的外界光源采用激光发射器发射一束准直激光19，所述处理器包括图像识别模块、转换计算模块和姿态计算模块。供电机构与双屏激光靶20连接，供电机构在处理器的控制下为所述双屏激光的第一成像机构2、第二成像机构24和倾角机构7供电，处理器分别与所述倾角机构7、第一成像机构2和第二成像机构24连接，以便倾角机构7、第一成像机构2和第二成像机构24将数据传输到处理器。处理器对所述相机11拍摄的原始光斑图像进行处理，以获得光斑质心的的像面坐标进而计算得到顶管机26的姿态信息。

通过采用上述技术方案，如图4所示，激光发射器25将光源发射至激光靶20处，要确保激光传输路径无遮挡。激光在前屏11处形成第一个光斑，部分余光射到后屏4上，形成第二个光斑，第一成像机构2和第二成像机构24分别采集各自光斑信号并传输给处理器，所述图像识别模块对接收的光斑信号进行姿态解算并生成双屏激光靶20的姿态信息，姿态信息包括方位角、滚转角和俯仰角信息，处理器根据双屏激光靶20的姿态角度进行解算，得到顶管机26的姿态信息，从而顶管机26的空间位置予以确定。

基于前述实施例，所述处理器包括：

图像识别模块，用于对所述前、后屏光斑图像进行图像识别查找到前、后屏4光斑质心的映射点；

转换计算模块，用于根据所述映射点的像素坐标计算得到所述光斑质心的世界坐标；

具体的，坐标系定义的具体步骤如下：

如图4所示，图像坐标系OTXTOYT映射的是激光束即平行光，其光学中心为入射激光束的交汇中心。我们定义一个相机坐标系OCXCYCZC，其原点O_C定义在相机的光学中心点处，O_CX_C方向和O_CY_C方向由相机像素变化方向决定，O_CZ_C方向由右手坐标系法则定义。

如图5所示，工程坐标系为施工地的大地坐标系O_tX_tY_tZ_t，Z_t轴是垂直向上的，X_t轴和Y_t轴的方向参照当地的的城市坐标系。激光靶坐标系OSXSYSZS的原点O_S定义在双屏激光靶20的机械中心，而X_S轴和Y_S轴与倾角机构7的两轴方向相同。顶管机坐标系ODXDYDZD的O_DZ_D轴是垂直向上的，O_CY_C方向是顶管机26的前进方向。

进一步，双屏激光靶20的屏幕标定具体步骤如下：

在施工地环境中选择一个参考坐标系作为工程坐标系OtXtYtZt来描述相机和双屏激光靶20的位置，设该工程坐标系OtXtYtZt下存在一个目标点的坐标为(X，Y，Z)。

双屏激光靶20屏幕标定的目的是找到图像坐标系OTXTOYT下，根据前屏1光斑质心（u1，v1），后屏4光斑质心（u2，v2）与激光靶坐标系OSXSYSZS的映射关系建立标定数据表。

由图5所示，以相机光点为坐标系原点O_C，O_CX_C和O_CY_C轴平行于图像的两条边，以光轴为Z轴所建立的坐标系。用点P_c(x_c，y_c，z_c)表示目标点Q在相机坐标系OCXCYCZC下的位置，用点P´(x，y)表示目标点Q在图像坐标系OTXTYT下的位置，用点P(u，v)表示目标点Q在像素坐标系OuOv下的位置。由于每一个像素在像素坐标系OuOv和图像坐标系OTXTOYT的坐标存在下列公式1所示的关系：

（1）

其中，(u0，v0)表示成像平面上的光学中心，（x，y）表示目标点Q在图像坐标系OTXTOYT的坐标，（u，v）表示目标点Q在像素坐标系OuOv的坐标。

由于目标点在工程坐标系OtXtYtZt与相机坐标系OCXCYCZC存在下列公式（2）所示的关系：

（2）

其中，（X_t，Y_t，Z_t，1）表示目标点在工程坐标系OtXtYtZt中的点P_t(x_t，y_t，z_t)的齐次坐标，（X_c，Y_c，Z_c，1）表示目标点在相机坐标系OCXCYCZC中的点P_c(x_c，y_c，z_c)的齐次坐标。

根据理想针孔成像模型，将它们之间关系写成齐次矩阵，具体计算如公式（3）所示：

(3)

其中，Z表示缩放因子，f表示相机光学中心到图像平面的距离，（x，y，1）表示目标点在成像机构的图像坐标系OTXTOYT中点P´(x，y)的齐次坐标，（X_c，Y_c，Z_c，1）表示目标点在相机坐标系OCXCYCZC中的点P_c(x_c，y_c，z_c)的齐次坐标。

图像处理通常在像素坐标系OuOv下进行，（u，v)表示该光束在像平面上的成像位置，(u0，v0)表示成像平面上的光学中心，即相机坐标系OCXCYCZC的O_CZ_C轴与像平面的交点，图像坐标系OTXTOYT的x轴方向是列数标号增加的方向，而y轴方向是行数标号增加的方向，将它们之间关系写成齐次矩阵，具体计算如公式（4）所示：

(4)

其中，（u，v，1）表示目标点在像素坐标系OuOv中的点P(u，v)的齐次坐标，（u0，v0）代表光学中心在像素坐标系OuOv下的坐标，dx和dy分别表示像素点P(u，v)在图像坐标系OTXTOYT下横轴x和纵轴y的物理尺寸。

因此，根据公式1-公式4的像素坐标系OuOv和激光靶坐标系OSXSYSZS之间点的转换关系，将其写成齐次矩阵，具体计算如公式（5）所示：

(5)

其中，A为3×4的矩阵，其称为投影矩阵。A1为相机内部参数矩阵，A2为相机外部参数矩阵，（X_t，Y_t，Z_t，1）表示目标点Q在工程坐标系OtXtYtZt下点P（x_t，y_t，z_t）的齐次坐标，R表示工程坐标系OtXtYtZt到相机坐标系OCXCYCZC的姿态矩阵，T表示工程坐标系OtXtYtZt到相机坐标系OCXCYCZC的平移向量。

对于每一个标记点，具体计算如公式（6）所示：

(6)

其中，（X_ti，Y_ti，Z_ti，1）表示在工程坐标系OtXtYtZt下空间第i个点的齐次坐标的坐标，（u_i，v_i，1）表示在像素坐标系OuOv下空间第i个点的齐次坐标的坐标，aij表示投影矩阵A的第i行j列元素。

由此，成像模型建立起了从相机坐标系OCXCYCZC下空间坐标点到像平面上光斑位置的函数关系，具体如公式（7）所示：

(7)

其中，X_ti，Y_ti，Z_ti分别表示在工程坐标系OtXtYtZt下空间第i个点在x轴、y轴和z轴的齐次坐标的坐标，u_i，v_i分别表示在像素坐标系OuOv下空间第i个点在u轴和v轴的齐次坐标的坐标，aij表示投影矩阵A的第i行j列元素。

反过来，已知了像平面下的光斑位置，也可以通过以上公式反求出前、后屏4光斑质心（u1，v1）、（u2，v2）在工程坐标系OtXtYtZt下的空间坐标点Ptf，Ptd。

基于前述实施例，所述图像识别模块包括：

搜索子模块，用于对整个前、后屏光斑图像分别进行搜索确定主光斑位置；

确定子模块，用于在主光斑位置的预设范围区域进行边界提取，将包围主光斑的环形区域确定为计算范围；

识别子模块，用于根据所述计算范围和加权质心法，识别查找到前、后屏4光斑质心的映射点。

具体的，光斑质心具体计算步骤如下：

首先搜索整个像面，对整个图像进行连通域标记，确定主光斑位置，然后通过在连通域确定的主光斑范围边界附近使用Sobel算子进行边界提取，得到包围光斑的环形区域作为质心法的计算范围，最后在确定的计算区域内使用加权质心法，利用图像灰度值f（x，y）的平方作为权值，设图像有效区域M×N个像素，将图像有效区域的质心作为目标点的像面坐标，在光斑中心提取应用中，理想光斑的质心即为光斑的中心(x0，y0)。具体的公式（8）描述如下：

（8）

其中，f（x，y）为图像坐标系OTXTOYT下的点P´(x，y)的图像灰度值，(x0，y0)为在图像坐标系OTXTOYT下光斑中心的坐标值。

姿态计算模块，根据所述世界坐标进行解算得到所述顶管机在掘进状态下的姿态信息。

基于前述实施例，所述姿态计算模块包括：

查询子模块，用于根据前、后屏4光斑质心分别在前屏1和后屏4的图像坐标查询标定数据表，得到光斑质心在前屏1和后屏4在激光靶坐标系OSXSYSZS下的空间坐标；所述标定数据表预先根据光斑质心在图像坐标系OTXTOYT和激光靶坐标系OSXSYSZS下的映射关系建立生成；

计算子模块，用于根据光斑质心在前屏1和后屏4的空间坐标，计算得到工程坐标系OtXtYtZt及激光靶坐标系OSXSYSZS下入射激光的直线方向矢量；

姿态获取子模块，用于根据入射激光的直线方向矢量，计算顶管机26从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt的滚动角和俯仰角为所述顶管机26的姿态信息。

具体的，双屏激光靶20方位角解算具体步骤如下：

（1）首先进行双屏激光靶20的屏幕标定，建立保存图像坐标系OTXTOYT下前、后屏4光斑质心（u1，v1）、（u2，v2）与激光靶坐标系OSXSYSZS下映射关系的标定数据表。通过查询标定数据表，得到光斑质心在激光靶坐标系OSXSYSZS下前后屏4空间坐标点Psf和Psd的坐标。根据Psf和Psd两个空间坐标点，计算得到激光靶坐标系OSXSYSZS下入射激光的第一直线方向矢量ls。

（2）根据前、后屏4光斑质心（u1，v1）、（u2，v2）在双屏的激光靶坐标系OSXSYSZS下的空间坐标点Psf，Psd，依据上述计算公式（6）可计算前、后屏4光斑质心（u1，v1）、（u2，v2）在工程坐标系OtXtYtZt下的空间坐标点Ptf，Ptd，进而根据空间坐标点Ptf，Ptd可计算得到工程坐标系OtXtYtZt下入射激光的第二直线方向矢量lt，根据第一直线方向矢量ls和第二直线方向矢量ls计算从激光靶坐标系OSXSYSZS到工程坐标系OtXtYtZt的第一姿态矩阵Rst（θ），计算公式如下列公式（9）所示：

（9）

采用欧拉角与XYZ轴对应姿态矩阵的关系来表示第一姿态矩阵Rst，计算公式如下列公式（10）所示：

（10）

其中，Rst为从激光靶坐标系OSXSYSZS到工程坐标系OtXtYtZt的第一姿态矩阵，Rx（Ψ）从激光靶坐标系OSXSYSZS到工程坐标系OtXtYtZt绕x轴旋转的姿态矩阵，Ry（θ）从激光靶坐标系OSXSYSZS到工程坐标系OtXtYtZt绕y轴旋转的姿态矩阵，Rz（γ）为从激光靶坐标系OSXSYSZS到工程坐标系OtXtYtZt绕z轴旋转的姿态矩阵。

其中，双屏激光靶20的方位角是通过上述公式（9）和公式（10）计算得到的角度（θ）。双屏激光靶20的滚动角（γ）和俯仰角（Ψ）则是根据设于双屏激光靶20箱体底部中的倾角机构7的测量得出，这些数据按每秒两次的频率传输到处理器，即绕x轴的姿态矩阵Rx（Ψ）、绕y轴的姿态矩阵Ry（θ）、绕z轴的姿态矩阵Rz（γ）已知，则根据上述公式可完成双屏激光靶20的方位角解算，进而获取到双屏激光靶的姿态角度（θ、Ψ和γ）。

进一步，所述顶管机26的姿态角度（即姿态信息）解算具体步骤如下：

（1）通过测量得到顶管机26在工程坐标系OtXtYtZt的盾首特征点坐标pds_t和盾尾特征点坐标pdw_t，以根据盾首特征点坐标pds_t和盾尾特征点坐标pdw_t计算得到盾首、盾尾特征点在工程坐标系OtXtYtZt下Y轴的空间矢量yd_t（X0，Y0，Z0）。

（2）已知顶管机26初始状态下的滚动角roll_d_t由倾角机构测量并外部输入，根据空间矢量yd_t（X0，Y0，Z0）代入下列公式（11）进行坐标反算，分别计算得到顶管机26在初始状态下的方位角yaw_d_t和俯仰角pitch_d_t；其中，顶管机26初始状态下的滚动角roll_d_t等于双屏激光靶20初始状态下的滚动角。

已知空间矢量yd_t（X0，Y0，Z0），当X0>0，Y0>0时，顶管机的方位角yaw_d_t和顶管机的俯仰角pitch_d_t计算公式如下列公式所示：

（11）

（3）其中顶管机26在初始状态下的俯仰角pitch_d_t、滚动角roll_d_t和方位角yaw_d_t已知，根据欧拉转换公式可得到工程坐标系OtXtYtZt到顶管机坐标系ODXDYDZD的第二转换矩阵Rtd，计算公式如下列公式（12）所示：

（12）

其中，Rtd为从工程坐标系OtXtYtZt到顶管机坐标系ODXDYDZD的第二转换矩阵，Rx（p）从工程坐标系OtXtYtZt到顶管机坐标系ODXDYDZD绕x轴旋转的姿态矩阵，Ry（h）从工程坐标系OtXtYtZt到顶管机坐标系ODXDYDZD绕y轴旋转的姿态矩阵，Rz（r）为从工程坐标系OtXtYtZt到顶管机坐标系ODXDYDZD绕z轴旋转的姿态矩阵，p为顶管机初始状态下的俯仰角pitch_d_t的简写，h为顶管机初始状态下的方位角yaw_d_t的简写，r为顶管机初始状态下的滚动角roll_d_t的简写。

（4）通过上述计算获取可获取激光靶坐标系OSXSYSZS到工程坐标系OtXtYtZt的第一姿态矩阵Rst，根据第一姿态矩阵Rst和第二转换矩阵Rtd，可以直接相乘计算得到顶管机坐标系ODXDYDZD到激光靶坐标系OSXSYSZS在实时状态下的第三姿态矩阵Rds，计算公式如下列公式（13）所示：

（13）

其中，inv（）为矩阵求逆符号。

（5）根据计算获取的第三姿态矩阵Rds，结合双屏激光靶20实时测量得到的第一姿态矩阵Rst，通过计算可得到顶管机26当前状态的第四转换矩阵Rdt，计算公式如下列公式（14）所示：

（14）

（6）根据上述计算可得到实时测量状态的顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt的第四转换矩阵Rdt，结合欧拉转换公式进行顶管机26姿态角度解算，即可得到顶管机26在掘进状态下实时变化的俯仰角、滚转角和方位角。

采用欧拉角与XYZ轴对应姿态矩阵的关系来表示第四转换矩阵Rdt，计算公式如下列公式（15）所示：

（15）

其中，Rdt为从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt的第四转换矩阵，Rx（α）从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕x轴旋转的姿态矩阵，Ry（β）从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕y轴旋转的姿态矩阵，Rz（ε）为从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕z轴旋转的姿态矩阵，其中，α、β和ε分别是顶管机26在掘进状态下实时变化的俯仰角、方位角和滚转角。

其中，顶管机26的姿态角度是通过上述公式（15）计算得到的角度（α、β和ε），即绕x轴的姿态矩阵Rx（α）、绕y轴的姿态矩阵Ry（β）、绕z轴的姿态矩阵Rz（ε）已知，则根据上述公式可反结算得到顶管机26在掘进状态下实时变化的俯仰角（α）、方位角（β）和滚转角（ε），进而获取到顶管机26的姿态角度。

导向的最终目的是通过坐标正反算获取盾首、盾尾等特征点与设计轴线的水平距离、垂直距离，即为水平偏差、垂直偏差，便于引导顶管机26按照设计线路进行隧道掘进，以盾首的水平偏差和垂直偏差解计算为例，具体计算步骤如下：

（1）通过上述公式（9）和公式（10）计算得到激光靶坐标系到工程坐标系的第一姿态矩阵Rst后，对第一姿态矩阵Rst进行求逆运算得到工程坐标系OtXtYtZt到激光靶坐标系OSXSYSZS的第五姿态矩阵Rts，即

。

（2）根据测量得到顶管机26在零位状态时工程坐标系OtXtYtZt下的盾首特征点坐标pds_t，通过欧拉转换以及上述第五姿态矩阵Rts求解盾首在激光靶坐标系OSXSYSZS下的坐标pds_s。

（3）通过双屏激光靶20测量获取零位状态时激光靶的姿态矩阵R0和盾首的姿态矩阵Rd0，根据欧拉转换公式计算在激光靶坐标系OSXSYSZS下盾首与激光靶机械中心的刚体关系R，计算公式如下列公式（16）所示：

（16）

（4）通过双屏激光靶20测量得到实时状态下的激光靶的姿态矩阵R´，由于盾首与激光靶机械中心的刚体关系R已知，根据欧拉转换公式可得到当前状态下的盾首姿态矩阵Rd1。

（5）根据盾首姿态矩阵Rd1进行坐标反算，得到工程坐标系OtXtYtZt下的盾首特征点的实时坐标(x，y，z)。

（6）将计划线和盾首特征点分别投影到XTOYT面和ZTOYT面，计算特征点到计划线盾首的水平距离和垂直距离，即盾首的水平偏差和垂直偏差，同理可求出盾尾的水平偏差和垂直偏差，驾驶员可根据各偏差数值实时进行顶管机26姿态的调整。

通过上述实施例获取到顶管机26的姿态信息后，将顶管机26的姿态信息发送至监控室上位机进行存储显示，以便操作人员根据需要进行姿态调整，增强了顶管施工的正确性。

本发明中的外界光源采用一束准直激光19，所述处理器包括图像识别模块、转换计算模块和姿态计算模块。供电机构与双屏激光靶20连接，供电机构在处理器的控制下为所述双屏激光的第一成像机构2、第二成像机构24和倾角机构7供电，处理器分别与所述倾角机构7、第一成像机构2和第二成像机构24连接，以便倾角机构7、第一成像机构2和第二成像机构24将数据传输到处理器。图像识别模块是对所述相机11拍摄的原始光斑图像进行处理，以获得光斑质心的的像面坐标，然后将方位角、滚转角、俯仰角和偏差数据信号输出至上位机进行显示。

综上所述，本发明包括以下至少一中有益技术效果：

1、本发明第一成像机构2和第二成像机构24采用对设结构安装在所述框架上方，分别照射前后屏4幕，在一定程度上增加了有效靶面面积，扩大了方位角测量范围。

2、本发明通过圆形滤光片有效去除杂光，使光斑更加清晰，增强了相机成像效果。

3、本发明通过采用加权质心法获取光斑质心，其中心提取的稳定性和准确性更高，增强了光斑识别的正确性，为获得精准地顶管机26姿态数据奠定基础。

4、本发明通过查询标定数据表获得前后屏4空间坐标点，获取速度快，正确率高，较大程度的提高了工程施工进度，增强了顶管施工的正确性。

如图6所示，本实施例提供一种顶管机26位置识别方法，包括步骤：

S100控制激光发射器发射激光束以分别依次在前屏1和后屏4上形成光斑；

S200控制第一成像机构2和第二成像机构24分别拍摄获取前屏光斑图像、后屏光斑图像；

S300控制设于箱体3内的倾角机构7测量获取箱体3的角度参数；

S410对所述前、后屏光斑图像进行图像识别查找到前、后屏4光斑质心的映射点；

S410对所述前、后屏光斑图像进行图像识别查找到前、后屏4光斑质心的映射点具体包括步骤：

S411对整个前、后屏光斑图像分别进行搜索确定主光斑位置；

S412在主光斑位置的预设范围区域进行边界提取，将包围主光斑的环形区域确定为计算范围；

S413根据所述计算范围和加权质心法，识别查找到前、后屏4光斑质心的映射点；

S420根据所述映射点的像素坐标计算得到所述光斑质心的世界坐标；

S430根据所述世界坐标进行解算得到所述顶管机26在掘进状态下的姿态信息；

S430根据所述世界坐标进行解算得到所述顶管机26在掘进状态下的姿态信息具体包括步骤：

S431根据前、后屏4光斑质心分别在前屏1和后屏4的图像坐标查询标定数据表，得到光斑质心在前屏1和后屏4在激光靶坐标系OSXSYSZS下的空间坐标；所述标定数据表预先根据光斑质心在图像坐标系OTXTOYT和激光靶坐标系OSXSYSZS下的映射关系建立生成；

S432根据光斑质心的空间坐标，计算得到入射激光分别在激光靶坐标系OSXSYSZS下的第一直线方向矢量，以及工程坐标系下OtXtYtZt的第二直线方向矢量；

S433根据入射激光的第一、第二直线方向矢量，以及所述倾角机构测量得到角度参数计算得到双屏激光靶20的姿态角度，根据双屏激光靶20的姿态角度，以及测量得到的盾首、盾尾特征点在工程坐标系OtXtYtZt下Y轴的空间矢量，计算得到所述顶管机26在掘进状态下的姿态信息。

基于前述实施例，还包括步骤：

预先根据光斑质心在图像坐标系和激光靶坐标系下的映射关系建立生成所述标定数据表。

基于前述实施例，所述根据入射激光的第一、第二直线方向矢量，以及所述倾角机构测量得到角度参数计算得到双屏激光靶的姿态角度包括步骤：

S43311将所述第一直线方向矢量和第二直线方向矢量代入下列公式计算得到激光靶坐标系到工程坐标系的第一姿态矩阵；

S43312根据第一姿态矩阵代入下列欧拉转换公式计算得到双屏激光靶的方位角；

S433313根据所述第一姿态矩阵和欧拉转换公式计算获取所述双屏激光靶的方位角；

S43314从所述倾角机构处获取所述双屏激光靶的滚动角和俯仰角以得到所述双屏激光靶的姿态角度；

其中，ls为第一直线方向矢量，lt为第二直线方向矢量；Rst为从激光靶坐标系到工程坐标系的第一姿态矩阵，Rx（Ψ）从激光靶坐标系到工程坐标系绕x轴旋转的姿态矩阵，Ry（θ）从激光靶坐标系到工程坐标系绕y轴旋转的姿态矩阵，Rz（γ）为从激光靶坐标系到工程坐标系OtXtYtZt绕z轴旋转的姿态矩阵；θ是双屏激光靶的方位角，γ是双屏激光靶的滚动角，Ψ是双屏激光靶的俯仰角；双屏激光靶的姿态角度包括双屏激光靶的方位角、滚动角和俯仰角。

基于前述实施例，所述根据双屏激光靶的姿态角度、盾首、盾尾特征点在工程坐标系下Y轴的空间矢量计算得到所述顶管机在掘进状态下的姿态信息包括步骤：

S43320从所述倾角机构处获取顶管机的滚动角，并获取盾首、盾尾特征点在工程坐标系下Y轴的空间矢量；

S43330根据所述空间矢量、顶管机的滚动角，计算得到所述顶管机在掘进状态下实时变化的姿态信息；所述姿态信息包括所述顶管机的俯仰角、滚转角和方位角。

基于前述实施例，所述S43330根据所述空间矢量、顶管机的滚动角，计算得到所述顶管机在掘进状态下实时变化的姿态信息包括步骤：

S43331根据所述空间矢量、顶管机的滚动角，代入下列公式进行坐标反算得到顶管机在初始状态下的方位角和俯仰角；

其中，yaw_d_t为顶管机在初始状态下的方位角，pitch_d_t为顶管机在初始状态下的俯仰角，X0，Y0，Z0分别是空间矢量在XYZ坐标轴下的坐标；

S43332根据所述顶管机在初始状态下的俯仰角、滚动角和方位角，代入欧拉转换公式计算得到工程坐标系到顶管机坐标系的第二转换矩阵；

其中，Rtd为从工程坐标系到顶管机坐标系的第二转换矩阵，Rx（p）从工程坐标系到顶管机坐标系绕x轴旋转的姿态矩阵，Ry（h）从工程坐标系到顶管机坐标系绕y轴旋转的姿态矩阵，Rz（r）为从工程坐标系到顶管机坐标系绕z轴旋转的姿态矩阵；p为顶管机初始状态下的俯仰角pitch_d_t的简写，h为顶管机初始状态下的方位角yaw_d_t的简写，r为顶管机初始状态下的滚动角roll_d_t的简写；inv（）为矩阵求逆符号；

S43333根据第一姿态矩阵和第二转换矩阵，相乘计算得到顶管机坐标系到激光靶坐标系在实时状态下的第三姿态矩阵；

S43334根据计算获取的第三姿态矩阵，结合双屏激光靶实时测量得到的第一姿态矩阵，代入下列公式计算得到顶管机当前状态的第四转换矩阵；

S43335根据第四转换矩阵结合欧拉转换公式进行顶管机姿态信息解算，得到顶管机在掘进状态下实时变化的俯仰角、滚转角和方位角；

其中，Rdt为从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt的第四转换矩阵，Rx（α）从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕x轴旋转的姿态矩阵，Ry（β）从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕y轴旋转的姿态矩阵，Rz（ε）为从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕z轴旋转的姿态矩阵，α、β和ε分别是顶管机在掘进状态下实时变化的俯仰角、方位角和滚转角。

基于前述实施例，所述根据前、后屏光斑图像和角度参数计算得到顶管机的姿态信息之后包括步骤：

将顶管机的姿态信息发送至监控室上位机进行存储显示。

具体的，本实施例是上述实施例对应的方法实施例，具体效果参见上述系统实施例，在此不再一一赘述。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各程序模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的程序模块完成，即将所述装置的内部结构划分成不同的程序单元或模块，以完成以上描述的全部或者部分功能。实施例中的各程序模块可以集成在一个图像识别模块中，也可是各个单元单独物理存在，也可以两个或两个以上单元集成在一个图像识别模块中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件程序单元的形式实现。另外，各程序模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。

本发明的一个实施例，一种存储介质，存储介质中存储有至少一条指令，指令由处理器加载并执行以实现上述双屏激光靶20对应实施例所执行的操作。例如，存储介质可以是只读内存(ROM)、随机存取存储器(RAM)、只读光盘(CD-ROM)、磁带、软盘和光数据存储设备等。

它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详细描述或记载的部分，可以参见其他实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

在本申请所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其他的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性、机械或其他的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可能集成在一个图像识别模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (14)

1.一种顶管机姿态识别系统，其特征在于，包括：处理器、激光发射器和设于顶管机内部的双屏激光靶；所述双屏激光靶包括：箱体、第一成像机构、第二成像机构以及置于箱体内的倾角机构；

所述箱体包括相对设置的前屏和后屏；

所述激光发射器用于发射激光束以分别依次在所述前屏和后屏上形成光斑；

所述第一成像机构设于所述箱体上，且第一成像机构的视野正对所述前屏以拍摄所述前屏上的光斑得到前屏光斑图像；

所述第二成像机构设于所述箱体上，且第二成像机构的视野正对所述后屏以拍摄所述后屏上的光斑得到后屏光斑图像；

所述倾角机构用于测量所述箱体的角度参数；

所述处理器，分别与所述倾角机构、第一成像机构和第二成像机构连接；

所述处理器包括：

图像识别模块，用于对所述前、后屏光斑图像进行图像识别查找到前、后屏光斑质心的映射点；

转换计算模块，用于根据所述映射点的像素坐标计算得到所述光斑质心的世界坐标；

姿态计算模块，根据所述世界坐标进行解算得到所述顶管机在掘进状态下的姿态信息；

所述图像识别模块包括：

搜索子模块，用于对整个前、后屏光斑图像分别进行搜索确定主光斑位置；

确定子模块，用于在主光斑位置的预设范围区域进行边界提取，将包围主光斑的环形区域确定为计算范围；

识别子模块，用于根据所述计算范围和加权质心法，识别查找到前、后屏光斑质心的映射点；

所述姿态计算模块包括：

查询子模块，用于根据前、后屏光斑质心分别在前屏和后屏的图像坐标查询标定数据表，得到光斑质心在前屏和后屏在激光靶坐标系下的空间坐标；

计算子模块，用于根据光斑质心的空间坐标，计算得到入射激光分别在激光靶坐标系下的第一直线方向矢量，以及工程坐标系下的第二直线方向矢量；

姿态获取子模块，用于根据入射激光的第一、第二直线方向矢量，以及所述倾角机构测量得到角度参数计算得到双屏激光靶的姿态角度，根据双屏激光靶的姿态角度，以及测量得到的盾首、盾尾特征点在工程坐标系下Y轴的空间矢量，计算得到所述顶管机在掘进状态下的姿态信息。

2.根据权利要求1所述的顶管机姿态识别系统，其特征在于，所述前屏采用透明基板，所述后屏采用非透明基板。

3.根据权利要求1所述的顶管机姿态识别系统，其特征在于，第一、第二成像机构均包括安装支架，所述第一成像机构通过安装支架安装在所述后屏上方，所述第二成像机构通过安装支架安装在所述前屏上方。

4.根据权利要求1所述的顶管机姿态识别系统，其特征在于，所述处理器还包括：

标定模块，用于预先根据光斑质心在图像坐标系和激光靶坐标系下的映射关系建立生成所述标定数据表。

5.根据权利要求1所述的顶管机姿态识别系统，其特征在于，所述姿态获取子模块包括：

激光靶姿态获取单元，用于根据第一直线方向矢量和第二直线方向矢量计算从激光靶坐标系到工程坐标系的第一姿态矩阵，根据所述第一姿态矩阵和欧拉转换公式计算获取所述双屏激光靶的方位角，从所述倾角机构处获取所述双屏激光靶的滚动角和俯仰角，进而得到所述双屏激光靶的姿态角度；姿态角度包括所述双屏激光靶的方位角、滚动角和俯仰角；

顶管机姿态获取单元，用于从所述倾角机构处获取顶管机的滚动角，并获取盾首、盾尾特征点在工程坐标系下Y轴的空间矢量，根据所述空间矢量、顶管机的滚动角，计算得到所述顶管机在掘进状态下实时变化的姿态信息；所述姿态信息包括所述顶管机的俯仰角、滚转角和方位角。

6.根据权利要求5所述的顶管机姿态识别系统，其特征在于，所述激光靶姿态获取单元包括：

第一矩阵计算子单元，用于将所述第一直线方向矢量和第二直线方向矢量代入下列公式计算得到激光靶坐标系到工程坐标系的第一姿态矩阵；

第一方位角计算子单元，用于根据第一姿态矩阵代入下列欧拉转换公式计算得到双屏激光靶的方位角；

其中，ls为第一直线方向矢量，lt为第二直线方向矢量；

为从激光靶坐标系到工程坐标系的第一姿态矩阵，

从激光靶坐标系到工程坐标系绕x轴旋转的姿态矩阵，

从激光靶坐标系到工程坐标系绕y轴旋转的姿态矩阵，

为从激光靶坐标系到工程坐标系

绕z轴旋转的姿态矩阵；

是双屏激光靶的方位角，

是双屏激光靶的滚动角，

是双屏激光靶的俯仰角；

激光靶姿态获取子单元，用于从所述倾角机构处获取所述双屏激光靶的滚动角和俯仰角以得到所述双屏激光靶的姿态角度。

7.根据权利要求5所述的顶管机姿态识别系统，其特征在于，所述顶管机姿态获取单元包括：

数据获取子单元，用于从所述倾角机构处获取顶管机的滚动角，并获取盾首、盾尾特征点在工程坐标系下Y轴的空间矢量；

初始角度计算子单元，用于根据所述空间矢量、顶管机的滚动角，代入下列公式进行坐标反算得到顶管机在初始状态下的方位角和俯仰角；

其中，

为顶管机在初始状态下的方位角，

为顶管机在初始状态下的俯仰角，X0，Y0，Z0分别是空间矢量在XYZ坐标轴下的坐标；

第二矩阵计算子单元，用于根据所述顶管机在初始状态下的俯仰角、滚动角和方位角，代入欧拉转换公式计算得到工程坐标系到顶管机坐标系的第二转换矩阵；

其中，Rtd为从工程坐标系到顶管机坐标系的第二转换矩阵，Rx（p）从工程坐标系到顶管机坐标系绕x轴旋转的姿态矩阵，Ry（h）从工程坐标系到顶管机坐标系绕y轴旋转的姿态矩阵，Rz（r）为从工程坐标系到顶管机坐标系绕z轴旋转的姿态矩阵；p为顶管机初始状态下的俯仰角pitch_d_t的简写，h为顶管机初始状态下的方位角yaw_d_t的简写，r为顶管机初始状态下的滚动角roll_d_t的简写；inv（）为矩阵求逆符号；

第三矩阵计算子单元，用于根据第一姿态矩阵和第二转换矩阵，相乘计算得到顶管机坐标系到激光靶坐标系在实时状态下的第三姿态矩阵；

第四矩阵计算子单元，用于根据计算获取的第三姿态矩阵，结合双屏激光靶实时测量得到的第一姿态矩阵，代入下列公式计算得到顶管机当前状态的第四转换矩阵；

顶管机姿态获取子单元，用于根据第四转换矩阵结合欧拉转换公式进行顶管机姿态信息解算，得到顶管机在掘进状态下实时变化的俯仰角、滚转角和方位角；

其中，Rdt为从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt的第四转换矩阵，Rx（α）从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕x轴旋转的姿态矩阵，Ry（β）从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕y轴旋转的姿态矩阵，Rz（ε）为从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕z轴旋转的姿态矩阵，α、β和ε分别是顶管机在掘进状态下实时变化的俯仰角、方位角和滚转角。

8.一种顶管机姿态识别方法，其特征在于，应用于权利要求1-7任一项所述的顶管机姿态识别系统，包括步骤：

控制激光发射器发射激光束以分别依次在前屏和后屏上形成光斑；

控制第一成像机构和第二成像机构分别拍摄获取前屏光斑图像、后屏光斑图像；

控制设于箱体内的倾角机构测量获取箱体的角度参数；

对所述前、后屏光斑图像进行图像识别查找到前、后屏光斑质心的映射点；

根据所述映射点的像素坐标计算得到所述光斑质心的世界坐标；

根据所述世界坐标进行解算得到所述顶管机在掘进状态下的姿态信息；

所述对所述前、后屏光斑图像进行图像识别查找到前、后屏光斑质心的映射点包括步骤：

对整个前、后屏光斑图像分别进行搜索确定主光斑位置；

在主光斑位置的预设范围区域进行边界提取，将包围主光斑的环形区域确定为计算范围；

根据所述计算范围和加权质心法，识别查找到前、后屏光斑质心的映射点；

所述根据所述世界坐标进行解算得到所述顶管机在掘进状态下的姿态信息包括步骤：

根据前、后屏光斑质心分别在前屏和后屏的图像坐标查询标定数据表，得到光斑质心在前屏和后屏在激光靶坐标系下的空间坐标；所述标定数据表预先根据光斑质心在图像坐标系和激光靶坐标系下的映射关系建立生成；

根据光斑质心的空间坐标，计算得到入射激光分别在激光靶坐标系下的第一直线方向矢量，以及工程坐标系下的第二直线方向矢量；

根据入射激光的第一、第二直线方向矢量，以及所述倾角机构测量得到角度参数计算得到双屏激光靶的姿态角度，根据双屏激光靶的姿态角度，以及测量得到的盾首、盾尾特征点在工程坐标系下Y轴的空间矢量，计算得到所述顶管机在掘进状态下的姿态信息。

9.根据权利要求8所述的顶管机姿态识别方法，其特征在于，还包括步骤：

预先根据光斑质心在图像坐标系和激光靶坐标系下的映射关系建立生成所述标定数据表。

10.根据权利要求8所述的顶管机姿态识别方法，其特征在于，所述根据入射激光的第一、第二直线方向矢量，以及所述倾角机构测量得到角度参数计算得到双屏激光靶的姿态角度包括步骤：

将所述第一直线方向矢量和第二直线方向矢量代入下列公式计算得到激光靶坐标系到工程坐标系的第一姿态矩阵；

根据第一姿态矩阵代入下列欧拉转换公式计算得到双屏激光靶的方位角；

根据所述第一姿态矩阵和欧拉转换公式计算获取所述双屏激光靶的方位角；

从所述倾角机构处获取所述双屏激光靶的滚动角和俯仰角以得到所述双屏激光靶的姿态角度；

其中，ls为第一直线方向矢量，lt为第二直线方向矢量；Rst为从激光靶坐标系到工程坐标系的第一姿态矩阵，Rx（Ψ）从激光靶坐标系到工程坐标系绕x轴旋转的姿态矩阵，Ry（θ）从激光靶坐标系到工程坐标系绕y轴旋转的姿态矩阵，Rz（γ）为从激光靶坐标系到工程坐标系OtXtYtZt绕z轴旋转的姿态矩阵；θ是双屏激光靶的方位角，γ是双屏激光靶的滚动角，Ψ是双屏激光靶的俯仰角；双屏激光靶的姿态角度包括双屏激光靶的方位角、滚动角和俯仰角。

11.根据权利要求10所述的顶管机姿态识别方法，其特征在于，所述根据双屏激光靶的姿态角度、盾首、盾尾特征点在工程坐标系下Y轴的空间矢量计算得到所述顶管机在掘进状态下的姿态信息包括步骤：

从所述倾角机构处获取顶管机的滚动角，并获取盾首、盾尾特征点在工程坐标系下Y轴的空间矢量；

根据所述空间矢量、顶管机的滚动角，计算得到所述顶管机在掘进状态下实时变化的姿态信息；所述姿态信息包括所述顶管机的俯仰角、滚转角和方位角。

12.根据权利要求11所述的顶管机姿态识别方法，其特征在于，所述根据所述空间矢量、顶管机的滚动角，计算得到所述顶管机在掘进状态下实时变化的姿态信息包括步骤：

根据所述空间矢量、顶管机的滚动角，代入下列公式进行坐标反算得到顶管机在初始状态下的方位角和俯仰角；

其中，yaw_d_t为顶管机在初始状态下的方位角，pitch_d_t为顶管机在初始状态下的俯仰角，X0，Y0，Z0分别是空间矢量在XYZ坐标轴下的坐标；

根据所述顶管机在初始状态下的俯仰角、滚动角和方位角，代入欧拉转换公式计算得到工程坐标系到顶管机坐标系的第二转换矩阵；

其中，Rtd为从工程坐标系到顶管机坐标系的第二转换矩阵，Rx（p）从工程坐标系到顶管机坐标系绕x轴旋转的姿态矩阵，Ry（h）从工程坐标系到顶管机坐标系绕y轴旋转的姿态矩阵，Rz（r）为从工程坐标系到顶管机坐标系绕z轴旋转的姿态矩阵；p为顶管机初始状态下的俯仰角pitch_d_t的简写，h为顶管机初始状态下的方位角yaw_d_t的简写，r为顶管机初始状态下的滚动角roll_d_t的简写；inv（）为矩阵求逆符号；

根据第一姿态矩阵和第二转换矩阵，相乘计算得到顶管机坐标系到激光靶坐标系在实时状态下的第三姿态矩阵；

根据计算获取的第三姿态矩阵，结合双屏激光靶实时测量得到的第一姿态矩阵，代入下列公式计算得到顶管机当前状态的第四转换矩阵；

根据第四转换矩阵结合欧拉转换公式进行顶管机姿态信息解算，得到顶管机在掘进状态下实时变化的俯仰角、滚转角和方位角；

其中，Rdt为从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt的第四转换矩阵，Rx（α）从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕x轴旋转的姿态矩阵，Ry（β）从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕y轴旋转的姿态矩阵，Rz（ε）为从顶管机坐标系ODXDYDZD到工程坐标系OtXtYtZt绕z轴旋转的姿态矩阵，α、β和ε分别是顶管机在掘进状态下实时变化的俯仰角、方位角和滚转角。

13.根据权利要求8-12任一项所述的顶管机姿态识别方法，其特征在于，所述根据前、后屏光斑图像和角度参数计算得到顶管机的姿态信息之后包括步骤：

将顶管机的姿态信息发送至监控室上位机进行存储显示。

14.一种存储介质，其特征在于，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现如权利要求8至权利要求13任一项所述的顶管机姿态识别方法所执行的操作。

Images