CN112066955B - 井下动态掘进机机身位姿参数测量方法及系统 - Google Patents

Abstract

本发明公开一种井下动态掘进机机身位姿参数测量方法及系统，建立巷道空间坐标系、全站仪测量坐标和掘进机刚体坐标系；使全站仪测量坐标系和巷道空间坐标系产生相互关系，全站仪测量掘进机机身的目标棱镜坐标则为巷道空间坐标系下的坐标值；目标棱镜在掘进机刚体坐标系下的坐标值是已知的，通过坐标转换，使巷道空间坐标系转换到掘进机机身刚体坐标系，求得掘进机的姿态角。本发明避免了依靠掘进机操作人员用眼去观察激光指向仪在岩石壁上的光斑的位置依靠经验判断掘进机的机身位姿对掘进机机身位姿进行调整，大大地提高了掘进机身的位姿测量精度，避免了超挖、欠挖等现象的发生，提高了工作效率及巷道的掘进质量。

Description

井下动态掘进机机身位姿参数测量方法及系统

技术领域

本发明属于矿用掘进机技术领域，具体涉及一种井下动态掘进机机身位姿参数测量方法及系统。

背景技术

掘进机广泛应用于隧道开挖、煤矿巷道掘进等施工中。在煤矿井下巷道综掘工作面，传统的定向方式为：在掘进机后方悬挂激光指向仪，激光指向仪通过照射将光斑投射到掘进机前方的岩石壁上，掘进机操作司机通过眼睛观察激光指向仪照射在岩石壁上的光斑所在的位置根据经验来判断掘进机掘进机的机身位姿状态并对掘进机机身位姿进行调整，此种方法跟操作工人的经验有很大的关系，因此效率低，精度差，容易导致超挖、欠挖的情况发生。

发明内容

针对现有技术中的缺陷和不足，本发明提供一种井下动态掘进机机身位姿参数测量方法及系统，解决现有的掘进机工作中受人为因素的影响难以保证掘进巷道精度的问题以及效率的问题。

为达到上述目的，本发明采取如下的技术方案：

一种井下动态掘进机机身位姿参数测量方法，动态掘进机机身位姿参数包括掘进机机身的姿态角以及掘进机机身中心偏移巷道中轴线的距离，其中掘进机机身的姿态角为偏航角、俯仰角以及横滚角；该方法包括以下步骤：

步骤1：确定巷道空间坐标系、掘进机机身刚体坐标系以及全站仪测量坐标系；并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，使全站仪测量坐标系下的坐标值为巷道空间坐标系下的坐标值；

步骤2：在掘进机机身中心位置安放目标棱镜作为全站仪测量目标棱镜；

步骤3：掘进机在运动的情况下，利用全站仪对目标棱镜进行连续测量，测量出目标棱镜和全站仪之间的斜距、顶天角和水平角，以及目标棱镜在全站仪测量坐标系下的坐标值，即目标棱镜在巷道空间坐标系下的坐标值；

步骤4：利用步骤3得到的动态掘进机在t、t+1和t+2时刻分别对应的目标棱镜在巷道空间坐标系下的坐标值以及已知的目标棱镜在掘进机机身刚体坐标系下的坐标值，进行掘进机机身位姿参数解算：

步骤4.1，设在t、t+1和t+2时刻测量的目标棱镜在巷道空间坐标系下的坐标值分别为(X₁，Y₁，Z₁)，(X₂，Y₂，Z ₂)，(X₃，Y₃，Z ₃)；目标棱镜在掘进机机身刚体坐标系下的坐标值为(X,Y,Z)；

步骤4.2，目标棱镜在巷道空间坐标系下的坐标值与目标棱镜在掘进机机身刚体坐标系下的坐标值的关系为：

且其中：

上式中，α₁,β₁,γ₁分别为t时刻掘进机机身的偏航角、俯仰角以及横滚角；α₂,β₂,γ₂分别为t+1时刻掘进机机身的偏航角、俯仰角以及横滚角；α₃,β₃,γ₃分别为t+2时刻掘进机机身的偏航角、俯仰角以及横滚角；利用以上关系式解算出三组掘进机机身的姿态角α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃；

步骤4.3，为使解算的掘进机机身姿态角具有唯一性，对解得的三组解利用欧拉约束法进行约束，使得解得的解具有唯一性，约束条件为：

其中：

将步骤4.2得到的α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃分别代入上式(2)-(5)中，具体的，上式(2)-(5)中的α取值为α₁、α₂或α₃中的一个，β取值为β₁、β₂或β₃中的一个，γ取值为γ₁、γ₂或γ₃中的一个，并且α、β和γ的取值需能同时满足式(1)，从而得到α、β和γ的最终唯一解，即掘进机机身的姿态角：偏航角、俯仰角以及横滚角；

步骤5：确定掘进机机身中心偏移巷道中轴线的距离，全站仪测量的目标棱镜的x值即为掘进机机身偏离巷道中轴线的距离，x为正值掘进机机身偏离巷道中轴线向右，x为负则偏离中轴线向左。

具体的，所述步骤1包括：

步骤1.1：建立巷道空间坐标系，巷道起始位置为原点，巷道中轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

建立掘进机机身刚体坐标系，原点为掘进机机身中心位置，机身纵轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

步骤1.2、确定全站仪位置，并建立全站仪测量坐标系：在巷道中轴线位置安放全站仪并进行调平，在巷道原点位置安放一个目标棱镜作为后视棱镜，利用后视棱镜和全站仪共同建立全站仪测量坐标系，并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，即使全站仪测量的坐标值为巷道空间坐标系下的坐标值。

一种井下动态掘进机机身位姿参数测量系统，包括：

坐标建立模块，用于建立巷道空间坐标系、掘进机机身刚体坐标系以及全站仪测量坐标系；并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，使全站仪测量坐标系下的坐标值为巷道空间坐标系下的坐标值；

测量目标模块，在掘进机机身中心位置安放目标棱镜作为全站仪测量目标棱镜；

全站仪测量模块，用于在掘进机在运动的情况下，利用全站仪对目标棱镜进行连续测量，测量出目标棱镜和全站仪之间的斜距、顶天角和水平角，以及目标棱镜在全站仪测量坐标系下的坐标值，即目标棱镜在巷道空间坐标系下的坐标值；并用于确定掘进机机身中心偏移巷道中轴线的距离，全站仪测量的目标棱镜的x值即为掘进机机身偏离巷道中轴线的距离，x为正值掘进机机身偏离巷道中轴线向右，x为负则偏离中轴线向左；

动态掘进机机身位姿解算模块，用于利用动态掘进机在t、t+1和t+2时刻分别对应的目标棱镜在巷道空间坐标系下的坐标值以及已知的目标棱镜在掘进机机身刚体坐标系下的坐标值，进行掘进机机身位姿参数解算。

具体的，所述坐标建立模块建立坐标系包括：

建立巷道空间坐标系，巷道起始位置为原点，巷道中轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

建立掘进机机身刚体坐标系，原点为掘进机机身中心位置，机身纵轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

确定全站仪位置，并建立全站仪测量坐标系：在巷道中轴线位置安放全站仪并进行调平，在巷道原点位置安放一个目标棱镜作为后视棱镜，利用后视棱镜和全站仪共同建立全站仪测量坐标系，并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，即使全站仪测量的坐标值为巷道空间坐标系下的坐标值。

具体的，所述动态掘进机机身位姿解算模块进行位姿解算包括：

设在t、t+1和t+2时刻测量的目标棱镜在巷道空间坐标系下的坐标值分别为(X₁，Y₁，Z₁)，(X₂，Y₂，Z ₂)，(X₃，Y₃，Z ₃)；目标棱镜在掘进机机身刚体坐标系下的坐标值为(X,Y,Z)；

目标棱镜在巷道空间坐标系下的坐标值与目标棱镜在掘进机机身刚体坐标系下的坐标值的关系为：

且其中：

上式中，α₁,β₁,γ₁分别为t时刻掘进机机身的偏航角、俯仰角以及横滚角；α₂,β₂,γ₂分别为t+1时刻掘进机机身的偏航角、俯仰角以及横滚角；α₃,β₃,γ₃分别为t+2时刻掘进机机身的偏航角、俯仰角以及横滚角；利用以上关系式解算出三组掘进机机身的姿态角α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃；

为使解算的掘进机机身姿态角具有唯一性，对解得的三组解利用欧拉约束法进行约束，使得解得的解具有唯一性，约束条件为：

其中：

将得到的α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃分别代入上式(2)-(5)中，具体的，上式(2)-(5)中的α取值为α₁、α₂或α₃中的一个，β取值为β₁、β₂或β₃中的一个，γ取值为γ₁、γ₂或γ₃中的一个，并且α、β和γ的取值需能同时满足式(1)，从而得到α、β和γ的最终唯一解，即掘进机机身的姿态角：偏航角、俯仰角以及横滚角。

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

本发明的通过全站测量的方法计算出掘进机机身的位姿参数，提高了掘进机机身位姿参数的测量精度，避免了依靠掘进机操作人员用眼去观察激光指向仪在岩石壁上的光斑的位置依靠经验判断掘进机的机身位姿对掘进机机身位姿进行调整，大大地提高了掘进机身的位姿测量精度，避免了超挖、欠挖等现象的发生，同事提高了工作效率及巷道的掘进质量。

附图说明

图1为全站仪测量位姿解算原理图。

以下结合说明书附图和具体实施方式对本发明做具体说明。

具体实施方式

遵从上述技术方案，以下给出本发明的具体实施例，需要说明的是本发明并不局限于以下具体实施例，凡在本申请技术方案基础上做的等同变换均落入本发明的保护范围。下面结合实施例对本发明做进一步详细说明。

实施例1：

本实施例提供一种井下动态掘进机机身位姿参数测量方法，动态掘进机机身位姿参数包括掘进机机身的姿态角以及掘进机机身中心偏移巷道中轴线的距离，其中掘进机机身的姿态角为偏航角、俯仰角以及横滚角；本发明的两个激光标靶一个放置在全站仪后方用于确定全站仪在巷道中的位置以及建立全站仪的测量坐标系，一个放置在掘进机机身的中心位置用于全站仪对其进行测量。

全站仪，为一种高精度的测量仪器，用来测量和目标棱镜之前的水平角，垂直角，斜跨以及水平距离。本发明所用全站仪的全称为：数字全站仪，型号为索佳Ix-1000。

该方法包括以下步骤：

步骤1：确定巷道空间坐标系、掘进机机身刚体坐标系以及全站仪测量坐标系；并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，使全站仪测量坐标系下的坐标值为巷道空间坐标系下的坐标值；

具体的，步骤1包括：

步骤1.1：建立巷道空间坐标系，巷道起始位置为原点，巷道中轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

建立掘进机机身刚体坐标系，原点为掘进机机身中心位置，机身纵轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

步骤1.2、确定全站仪位置，并建立全站仪测量坐标系：在巷道中轴线位置安放全站仪并进行调平，在巷道原点位置安放一个目标棱镜作为后视棱镜，利用后视棱镜和全站仪共同建立全站仪测量坐标系，并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，即使全站仪测量的坐标值为巷道空间坐标系下的坐标值。

步骤2：在掘进机机身中心位置安放目标棱镜作为全站仪测量目标棱镜；

步骤3：掘进机在运动的情况下，利用全站仪对目标棱镜进行连续测量，测量出目标棱镜和全站仪之间的斜距、顶天角和水平角，以及目标棱镜在全站仪测量坐标系下的坐标值，即目标棱镜在巷道空间坐标系下的坐标值；

步骤4：利用步骤3得到的动态掘进机在t、t+1和t+2时刻分别对应的目标棱镜在巷道空间坐标系下的坐标值(近似认为是在同一时刻三个特征点)以及已知的目标棱镜在掘进机机身刚体坐标系下的坐标值，进行掘进机机身位姿参数解算：

步骤4.1，设在t、t+1和t+2时刻测量的目标棱镜在巷道空间坐标系下的坐标值分别为(X₁，Y₁，Z₁)，(X₂，Y₂，Z ₂)，(X₃，Y₃，Z ₃)；目标棱镜在掘进机机身刚体坐标系下的坐标值为(X,Y,Z)；其中，t为运动过程中任意时间点；t+1，t+2为与t时刻相连续的两个时间点，即t，t+1，t+2是三个连续的时间点；

步骤4.2，目标棱镜在巷道空间坐标系下的坐标值与目标棱镜在掘进机机身刚体坐标系下的坐标值的关系为：

且其中：

上式中，α₁,β₁,γ₁分别为t时刻掘进机机身的偏航角、俯仰角以及横滚角；α₂,β₂,γ₂分别为t+1时刻掘进机机身的偏航角、俯仰角以及横滚角；α₃,β₃,γ₃分别为t+2时刻掘进机机身的偏航角、俯仰角以及横滚角；利用以上关系式解算出三组掘进机机身的姿态角α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃；

步骤4.3，为使解算的掘进机机身姿态角具有唯一性，对解得的三组解利用欧拉约束法进行约束，使得解得的解具有唯一性，约束条件为：

其中：

将步骤4.2得到的α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃分别代入上式(2)-(5)中，具体的，上式(2)-(5)中的α取值为α₁、α₂或α₃中的一个，β取值为β₁、β₂或β₃中的一个，γ取值为γ₁、γ₂或γ₃中的一个，并且α、β和γ的取值需能同时满足式(1)，从而得到α、β和γ的最终唯一解，即掘进机机身的姿态角：偏航角、俯仰角以及横滚角；

步骤5：确定掘进机机身中心偏移巷道中轴线的距离，全站仪测量的目标棱镜的x值即为掘进机机身偏离巷道中轴线的距离，x为正值掘进机机身偏离巷道中轴线向右，x为负则偏离中轴线向左。

实施例2：

本实施例提供一种井下动态掘进机机身位姿参数测量系统，包括：

坐标建立模块，用于建立巷道空间坐标系、掘进机机身刚体坐标系以及全站仪测量坐标系；并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，使全站仪测量坐标系下的坐标值为巷道空间坐标系下的坐标值；

具体的，坐标建立模块建立坐标系包括：

建立巷道空间坐标系，巷道起始位置为原点，巷道中轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

建立掘进机机身刚体坐标系，原点为掘进机机身中心位置，机身纵轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

确定全站仪位置，并建立全站仪测量坐标系：在巷道中轴线位置安放全站仪并进行调平，在巷道原点位置安放一个目标棱镜作为后视棱镜，利用后视棱镜和全站仪共同建立全站仪测量坐标系，并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，即使全站仪测量的坐标值为巷道空间坐标系下的坐标值。

测量目标模块，在掘进机机身中心位置安放目标棱镜作为全站仪测量目标棱镜；

全站仪测量模块，用于在掘进机在运动的情况下，利用全站仪对目标棱镜进行连续测量，测量出目标棱镜和全站仪之间的斜距、顶天角和水平角，以及目标棱镜在全站仪测量坐标系下的坐标值，即目标棱镜在巷道空间坐标系下的坐标值；并用于确定掘进机机身中心偏移巷道中轴线的距离，全站仪测量的目标棱镜的x值即为掘进机机身偏离巷道中轴线的距离，x为正值掘进机机身偏离巷道中轴线向右，x为负则偏离中轴线向左；

动态掘进机机身位姿解算模块，用于利用动态掘进机在t、t+1和t+2时刻分别对应的目标棱镜在巷道空间坐标系下的坐标值以及已知的目标棱镜在掘进机机身刚体坐标系下的坐标值，进行掘进机机身位姿参数解算。

具体的，动态掘进机机身位姿解算模块进行位姿解算包括：

设在t、t+1和t+2时刻测量的目标棱镜在巷道空间坐标系下的坐标值分别为(X₁，Y₁，Z₁)，(X₂，Y₂，Z₂)，(X₃，Y₃，Z₃)；目标棱镜在掘进机机身刚体坐标系下的坐标值为(X,Y,Z)；

目标棱镜在巷道空间坐标系下的坐标值与目标棱镜在掘进机机身刚体坐标系下的坐标值的关系为：

且其中：

上式中，α₁,β₁,γ₁分别为t时刻掘进机机身的偏航角、俯仰角以及横滚角；α₂,β₂,γ₂分别为t+1时刻掘进机机身的偏航角、俯仰角以及横滚角；α₃,β₃,γ₃分别为t+2时刻掘进机机身的偏航角、俯仰角以及横滚角；利用以上关系式解算出三组掘进机机身的姿态角α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃；

为使解算的掘进机机身姿态角具有唯一性，对解得的三组解利用欧拉约束法进行约束，使得解得的解具有唯一性，约束条件为：

其中：

将得到的α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃分别代入上式(2)-(5)中，具体的，上式(2)-(5)中的α取值为α₁、α₂或α₃中的一个，β取值为β₁、β₂或β₃中的一个，γ取值为γ₁、γ₂或γ₃中的一个，并且α、β和γ的取值需能同时满足式(1)，从而得到α、β和γ的最终唯一解，即掘进机机身的姿态角：偏航角、俯仰角以及横滚角。

仿真实验

本仿真实验中掘进机机身上放置有用于全站仪观测的目标棱镜，测量过程如下：

巷道设计，根据要求设计巷道的走向，宽度，巷道中轴线，以及航道的起始位置，设计巷道的三维空间坐标系；

根据设计好的巷道空间坐标系设计，利用全站仪后视棱镜建立全站仪的测量坐标系，并且使全站仪测量坐标系和巷道空间坐标系建立一定的关系，以掘进机机身中心为原点建立掘进机机身刚体坐标系；

利用全站仪对放置在掘进机机身的目标棱镜进行测量，得出其在巷道空间坐标系下的坐标值；

利用测量得到的掘进机在巷道空间坐标系下的坐标系对掘进机进行位姿解算。

Claims (4)

1.一种井下动态掘进机机身位姿参数测量方法，其特征在于，动态掘进机机身位姿参数包括掘进机机身的姿态角以及掘进机机身中心偏移巷道中轴线的距离，其中掘进机机身的姿态角为偏航角、俯仰角以及横滚角；该方法包括以下步骤：

步骤1：确定巷道空间坐标系、掘进机机身刚体坐标系以及全站仪测量坐标系；并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，使全站仪测量坐标系下的坐标值为巷道空间坐标系下的坐标值；

步骤2：在掘进机机身中心位置安放目标棱镜作为全站仪测量目标棱镜；

步骤3：掘进机在运动的情况下，利用全站仪对目标棱镜进行连续测量，测量出目标棱镜和全站仪之间的斜距、顶天角和水平角，以及目标棱镜在全站仪测量坐标系下的坐标值，即目标棱镜在巷道空间坐标系下的坐标值；

步骤4：利用步骤3得到的动态掘进机在t、t+1和t+2时刻分别对应的目标棱镜在巷道空间坐标系下的坐标值以及已知的目标棱镜在掘进机机身刚体坐标系下的坐标值，进行掘进机机身位姿参数解算：

步骤4.1，设在t、t+1和t+2时刻测量的目标棱镜在巷道空间坐标系下的坐标值分别为(X₁，Y₁，Z₁)，(X₂，Y₂，Z₂)，(X₃，Y₃，Z₃)；目标棱镜在掘进机机身刚体坐标系下的坐标值为(X,Y,Z)；

步骤4.2，目标棱镜在巷道空间坐标系下的坐标值与目标棱镜在掘进机机身刚体坐标系下的坐标值的关系为：

且其中：

上式中，α₁,β₁,γ₁分别为t时刻掘进机机身的偏航角、俯仰角以及横滚角；α₂,β₂,γ₂分别为t+1时刻掘进机机身的偏航角、俯仰角以及横滚角；α₃,β₃,γ₃分别为t+2时刻掘进机机身的偏航角、俯仰角以及横滚角；利用以上关系式解算出三组掘进机机身的姿态角α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃；

步骤4.3，为使解算的掘进机机身姿态角具有唯一性，对解得的三组解利用欧拉约束法进行约束，使得解得的解具有唯一性，约束条件为：

其中：

将步骤4.2得到的α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃分别代入上式(2)-(5)中，具体的，上式(2)-(5)中的α取值为α₁、α₂或α₃中的一个，β取值为β₁、β₂或β₃中的一个，γ取值为γ₁、γ₂或γ₃中的一个，并且α、β和γ的取值需能同时满足式(1)，从而得到α、β和γ的最终唯一解，即掘进机机身的姿态角：偏航角、俯仰角以及横滚角；

步骤5：确定掘进机机身中心偏移巷道中轴线的距离，全站仪测量的目标棱镜的x值即为掘进机机身偏离巷道中轴线的距离，x为正值掘进机机身偏离巷道中轴线向右，x为负则偏离中轴线向左。

2.如权利要求1所述的井下动态掘进机机身位姿参数测量方法，其特征在于，所述步骤1包括：

步骤1.1：建立巷道空间坐标系，巷道起始位置为原点，巷道中轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

建立掘进机机身刚体坐标系，原点为掘进机机身中心位置，机身纵轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

步骤1.2、确定全站仪位置，并建立全站仪测量坐标系：在巷道中轴线位置安放全站仪并进行调平，在巷道原点位置安放一个目标棱镜作为后视棱镜，利用后视棱镜和全站仪共同建立全站仪测量坐标系，并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，即使全站仪测量的坐标值为巷道空间坐标系下的坐标值。

3.一种井下动态掘进机机身位姿参数测量系统，其特征在于，包括：

坐标建立模块，用于建立巷道空间坐标系、掘进机机身刚体坐标系以及全站仪测量坐标系；并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，使全站仪测量坐标系下的坐标值为巷道空间坐标系下的坐标值；

测量目标模块，在掘进机机身中心位置安放目标棱镜作为全站仪测量目标棱镜；

全站仪测量模块，用于在掘进机在运动的情况下，利用全站仪对目标棱镜进行连续测量，测量出目标棱镜和全站仪之间的斜距、顶天角和水平角，以及目标棱镜在全站仪测量坐标系下的坐标值，即目标棱镜在巷道空间坐标系下的坐标值；并用于确定掘进机机身中心偏移巷道中轴线的距离，全站仪测量的目标棱镜的x值即为掘进机机身偏离巷道中轴线的距离，x为正值掘进机机身偏离巷道中轴线向右，x为负则偏离中轴线向左；

动态掘进机机身位姿解算模块，用于利用动态掘进机在t、t+1和t+2时刻分别对应的目标棱镜在巷道空间坐标系下的坐标值以及已知的目标棱镜在掘进机机身刚体坐标系下的坐标值，进行掘进机机身位姿参数解算；

所述动态掘进机机身位姿解算模块进行位姿解算包括：

设在t、t+1和t+2时刻测量的目标棱镜在巷道空间坐标系下的坐标值分别为(X₁，Y₁，Z₁)，(X₂，Y₂，Z₂)，(X₃，Y₃，Z₃)；目标棱镜在掘进机机身刚体坐标系下的坐标值为(X,Y,Z)；

目标棱镜在巷道空间坐标系下的坐标值与目标棱镜在掘进机机身刚体坐标系下的坐标值的关系为：

且其中：

上式中，α₁,β₁,γ₁分别为t时刻掘进机机身的偏航角、俯仰角以及横滚角；α₂,β₂,γ₂分别为t+1时刻掘进机机身的偏航角、俯仰角以及横滚角；α₃,β₃,γ₃分别为t+2时刻掘进机机身的偏航角、俯仰角以及横滚角；利用以上关系式解算出三组掘进机机身的姿态角α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃；

为使解算的掘进机机身姿态角具有唯一性，对解得的三组解利用欧拉约束法进行约束，使得解得的解具有唯一性，约束条件为：

其中：

将α₁,β₁,γ₁，α₂,β₂,γ₂，α₃,β₃,γ₃分别代入上式(2)-(5)中，具体的，上式(2)-(5)中的α取值为α₁、α₂或α₃中的一个，β取值为β₁、β₂或β₃中的一个，γ取值为γ₁、γ₂或γ₃中的一个，并且α、β和γ的取值需能同时满足式(1)，从而得到α、β和γ的最终唯一解，即掘进机机身的姿态角：偏航角、俯仰角以及横滚角。

4.如权利要求3所述的井下动态掘进机机身位姿参数测量系统，其特征在于，所述坐标建立模块建立坐标系包括：

建立巷道空间坐标系，巷道起始位置为原点，巷道中轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

建立掘进机机身刚体坐标系，原点为掘进机机身中心位置，机身纵轴线为y轴，垂直y轴向右为x轴，根据右手法则确定z轴，z轴方向竖直向上；

确定全站仪位置，并建立全站仪测量坐标系：在巷道中轴线位置安放全站仪并进行调平，在巷道原点位置安放一个目标棱镜作为后视棱镜，利用后视棱镜和全站仪共同建立全站仪测量坐标系，并确定全站仪在巷道空间坐标系中的位置，使全站仪测量坐标系和巷道空间坐标系建立相互关系，即使全站仪测量的坐标值为巷道空间坐标系下的坐标值。

Images