CN103837115A - 三维姿态测算方法及装置 - Google Patents

Abstract

本发明涉及一种三维姿态测算方法及装置，方法包括：通过设置在地下工作装置上的双轴倾角传感器读取地下工作装置绕X轴和Y轴的偏转角度值；通过设置在牵引地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值；根据钢丝绳的下放长度值以及地下工作装置分别绕X轴和Y轴的偏转角度值计算出地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。本发明消除了因地下工作装置偏离施工中心线时所带来的测算误差，并进一步的计算出地下工作装置沿X轴和Y轴的偏离位移，准确的获得基桩工程机械的地下工作装置在工作过程中的三维姿态，以作为地下工作装置纠偏控制和监控系统显示的重要依据。

Description

三维姿态测算方法及装置

技术领域

本发明涉及工程机械领域，尤其涉及一种基桩工程机械的地下工作装置的三维姿态测算方法及装置。

背景技术

双轮铣槽机是一种通过钢丝绳悬吊铣刀架进行大深度挖槽工作的工程机械装备，主要应用于水利工程建设等施工中。施工时，铣刀架整体没于深水泥浆中，其三维姿态与施工质量密切相关，并可作为铣刀架纠偏控制和监控显示的重要依据，因此准确获知铣刀架的三维姿态是提高双轮铣槽机工作性能的关键因素。

如图1所示，为现有的双轮铣槽机的外形示意图。其中钢丝绳A1连接铣刀架A2的上端，在铣刀架A2的下端设有多个铣轮A3。其中，用于描述铣刀架的三维姿态的参数包括沿X、Y、Z轴方向的位移，以及绕X、Y、Z轴方向的偏转角度。其中铣刀架的绕X、Y轴偏转角度是相对于垂直中心线的绝对角度，也是相对于施工中心线的角度，绕Z轴偏转角度是相对于铣刀架自身的角度；铣刀架在X轴和Y轴的偏离位移是相对于施工中心线的偏离位移。

对于现有技术，仅能测量沿Z轴方向的位移，以及绕X、Y轴方向的偏转角度，具体测量方法如下：

（1）沿Z轴方向的位移，在提升卷扬上安装编码器，以测量卷扬滚筒圈数和角度，转换成钢丝绳的下放长度，将其作为沿Z轴方向的位移，即铣刀架的下放深度；

（2）绕X、Y轴方向的偏转角度，在铣刀架上安装双轴倾角传感器，其输出值即为相应偏转角度值。

如图2所示，现有技术存在以下缺陷，使得其无法测得铣刀架工作的三维姿态：

（1）无法测得铣刀架沿X、Y轴方向的位移，即偏离施工中心线的距离；

（2）无法测得绕Z轴方向的偏转角度，因为铣刀架与双轮铣上车采用钢丝绳连接，这种柔性连接方式，增大了其测量难度，由于测得沿Z轴方向的位移不准确，直接将钢丝绳的下放长度作为铣刀架沿Z轴方向的位移，但是铣刀架工作中会出现偏离施工中心线的情况，这时钢丝绳会与施工中心线产生一定偏角，而不是重合，所以造成数据不准确。

发明内容

本发明的目的是提出一种三维姿态测算方法及装置，能够准确获得基桩工程机械的地下工作装置在工作过程中的三维姿态，以作为地下工作装置纠偏控制和监控系统显示的重要依据。

为实现上述目的，本发明提供了一种三维姿态测算方法，包括：

通过设置在地下工作装置上的双轴倾角传感器读取所述地下工作装置绕X轴和Y轴的偏转角度值；

通过设置在牵引所述地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值；

根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。

进一步的，所述编码器为多圈绝对式编码器，所述通过设置在牵引所述地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值的操作具体为：通过所述多圈绝对式编码器读取所述提升卷扬的卷扬滚筒的累计角度值，对所述累计角度值转换得到钢丝绳的下放长度值L_C。

进一步的，还包括所述地下工作装置绕Z轴的偏转角度值的测算步骤，具体为：通过设置在所述地下工作装置上的陀螺仪获取所述地下工作装置绕Z轴的偏转角速度值ω_Z，并根据所述偏转角速度值ω_Z计算出所述地下工作装置绕Z轴的偏转角度值θ_Z。

进一步的，所述根据所述偏转角速度值ω_Z计算出所述地下工作装置绕Z轴的偏转角度值θ_Z的操作具体为：通过对所述偏转角速度值ω_Z进行时间t上的积分，计算出所述地下工作装置绕Z轴的偏转角度值θ_Z，计算公式为：

${\mathrm{\θ}}_Z={\∫}_0^t{\mathrm{\ω}}_Z\mathrm{dt}.$

进一步的，所述根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移的操作具体包括：

根据所述地下工作装置的实际工作情况分别计算不同工作情况下所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移；

对不同工作情况下的所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移进行累加，得到所述地下工作装置分别沿X轴、Y轴和Z轴方向的总偏离位移。

进一步的，如果所述双轴倾角传感器测出的所述地下工作装置绕X轴和Y轴的偏转角度值均为0，判断所述地下工作装置处于垂直工作轨迹的工作状况，则所述地下工作装置沿Z轴方向的偏离位移L_Z=钢丝绳的下放长度值L_C，沿X轴和Y轴方向的偏离位移L_X和L_Y均为0；

如果所述双轴倾角传感器测出的所述地下工作装置绕X轴的偏转角度值θ_X和绕Y轴的偏转角度值θ_Y至少有一个偏转角度值大于0，且处于未启动纠偏装置的工作状态，则

L_Z=L_C·cosθ_X或L_Z=L_C·cosθ_Y；

L_X=L_C·sinθ_X或L_Y=L_C·sinθ_Y；

如果所述双轴倾角传感器测出的所述地下工作装置绕X轴和Y轴的偏转角度值至少有一个偏转角度值大于0，且处于已启动纠偏装置的工作状态，则

$L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt};$

$L_X={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Y={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt};$

其中，θ_Xt、θ_Yt和ΔL_Ct分别为纠偏轨迹中某一微元对应的X轴和Y轴的偏转角度值以及钢丝绳的下放长度值；

如果所述双轴倾角传感器测出的所述地下工作装置绕X轴和Y轴的偏转角度值至少有一个偏转角度值小于等于0，且处于已启动纠偏装置的工作状态，则

$L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt};$

$L_X=-{\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Y=-{\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt}.$

为实现上述目的，本发明提供了一种三维姿态测算装置，包括：

双轴倾角传感器，设置在地下工作装置上，用于读取所述地下工作装置绕X轴和Y轴的偏转角度值；

编码器，设置在牵引所述地下工作装置的钢丝绳的提升卷扬上，用于对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值；

控制器，分别与所述双轴倾角传感器和编码器连接，根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。

进一步的，所述编码器为多圈绝对式编码器，用于读取所述提升卷扬的卷扬滚筒的累计角度值，并对所述累计角度值转换得到钢丝绳的下放长度值L_C。

进一步的，还包括：

陀螺仪，设置在所述地下工作装置上，并连接所述控制器，用于获取所述地下工作装置绕Z轴的偏转角速度值ω_Z，以便所述控制器根据所述偏转角速度值ω_Z计算出所述地下工作装置绕Z轴的偏转角度值θ_Z。

进一步的，所述控制器包括：

偏离位移分算单元，用于根据所述地下工作装置的实际工作情况分别计算不同工作情况下所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移；

偏离位移总算单元，用于对不同工作情况下的所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移进行累加，得到所述地下工作装置分别沿X轴、Y轴和Z轴方向的总偏离位移。

基于上述技术方案，本发明根据钢丝绳的下放长度以及测得的X轴和Y轴的偏转角度值来计算地下工作装置沿Z轴方向的位移，而非现有技术中直接以钢丝绳的下放长度作为Z轴方向的位移，因此消除了因地下工作装置偏离施工中心线时所带来的测算误差，利用钢丝绳的下放长度和地下工作装置绕X轴和Y轴的偏转角度值还可以进一步的计算出地下工作装置沿X轴和Y轴的偏离位移，进而准确的获得基桩工程机械的地下工作装置在工作过程中的三维姿态，以作为地下工作装置纠偏控制和监控系统显示的重要依据。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为现有的双轮铣槽机的外形示意图。

图2为图1的双轮铣槽机在工作中偏离施工中心线的示意图。

图3为本发明三维姿态测算方法的一实施例的流程示意图。

图4为本发明三维姿态测算方法实施例中地下工作装置的工作轨迹划分示意图。

图5为本发明三维姿态测算方法实施例中地下工作装置的纠偏一区的微元分析图。

图6为本发明三维姿态测算装置的一实施例的结构示意图。

图7为本发明三维姿态测算装置的另一实施例的结构示意图。

具体实施方式

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

如图3所示，为本发明三维姿态测算方法的一实施例的流程示意图。在本实施例中，基桩工程机械地下工作装置的三维姿态测算流程包括以下步骤：

步骤101、通过设置在地下工作装置上的双轴倾角传感器读取所述地下工作装置绕X轴和Y轴的偏转角度值；

步骤102、通过设置在牵引所述地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值；

步骤103、根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。

在现有的双轮铣槽机的三维姿态测算中，只能够对铣刀架的沿Z轴位移和绕X轴和Y轴方向的偏转角度进行测算，但仅这三方面数据难以比较准确的体现出双轮铣槽机的整体的三维姿态，进而对纠偏、监视等后续流程造成了限制。而上述步骤103在已得到钢丝绳的下放长度值后，可以根据X轴和Y轴的偏转角度值来计算实际的地下工作装置沿Z轴的位移，而不是直接以钢丝绳的下放长度值来作为Z轴位移，因此获得的数值更为精确，而利用钢丝绳的下放长度值以及绕X轴和Y轴的偏转角度值则可以进一步计算出地下工作装置分别沿X轴、Y轴方向的偏离位移，因此本实施例能够实现更全面的三维姿态参数的测算，能够更为准确地体现出地下工作装置的整体三维姿态，能够作为地下工作装置纠偏控制和监控系统显示的重要依据。

在步骤102中的编码器可采用多圈绝对式编码器，通过该多圈绝对式编码器读取提升卷扬的卷扬滚筒的累计角度值θ_L，对累计角度值θ_L转换就可以得到钢丝绳的下放长度值L_C。在另一个实施例中，编码器也可以采用多圈增量式编码器，根据卷扬滚筒的圈数和角度来转换成钢丝绳的下放长度。

在另一个方法实施例中，还包括了地下工作装置绕Z轴的偏转角度值的测算步骤，具体为：通过设置在所述地下工作装置上的陀螺仪获取所述地下工作装置绕Z轴的偏转角速度值ω_Z，并根据所述偏转角速度值ω_Z计算出所述地下工作装置绕Z轴的偏转角度值θ_Z。在获得偏转角速度值ω_Z后，可以对偏转角速度值进行ω_Z进行时间t上的积分，来获得地下工作装置绕Z轴的偏转角度值θ_Z，计算公式为：

${\mathrm{\θ}}_Z={\∫}_0^t{\mathrm{\ω}}_Z\mathrm{dt}.$

通过陀螺仪的测量和积分计算获得了地下工作装置绕Z轴方向的偏转角度，克服了因钢丝绳柔性连接而带来的测量难度，进一步的实现地下工作装置的所有三维姿态的测算，为地下工作装置在X轴、Y轴和Z轴均提供了纠偏的依据，也能够使操作人员能够实时的对地下工作装置的三维姿态进行全面的监视。

在地下工作装置进行工作时，工作轨迹、工作情况有可能均在发生变化，而在不同的工作情况下的偏移位移的测算方式有所不同，此时若只采用单一的计算方式则可能不足够准确，因此步骤103可以采取区分不同工作情况下的偏移位移的计算，即根据地下工作装置的实际工作情况分别计算不同工作情况下地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移；对不同工作情况下的地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移进行累加，得到地下工作装置分别沿X轴、Y轴和Z轴方向的总偏离位移。

如图4所示，为本发明三维姿态测算方法实施例中地下工作装置的工作轨迹划分示意图。从图中可以看到，按照地下工作装置的工作轨迹（例如铣刀架的铣槽轨迹）可以至少划分成四个区域（或称工作状况），如下：

1、未出现偏角区：如果双轴倾角传感器测出的地下工作装置绕X轴和Y轴的偏转角度值均为0，可判断出地下工作装置处于垂直工作轨迹的工作状况，与施工中心线符合，则地下工作装置沿Z轴方向的偏离位移L_Z=钢丝绳的下放长度值L_C，沿X轴和Y轴方向的偏离位移L_X和L_Y均为0。

2、出现偏角，未纠偏区：如果双轴倾角传感器测出的所述地下工作装置绕X轴的偏转角度值θ_X和绕Y轴的偏转角度值θ_Y至少有一个偏转角度值大于0，且处于未启动地下工作装置的纠偏装置的工作状态，则

L_Z=L_C·cosθ_X或L_Z=L_C·cosθ_Y；

L_X=L_C·sinθ_X或L_Y=L_C·sinθ_Y。

3、出现偏角，纠偏一区：如果所述双轴倾角传感器测出的所述地下工作装置绕X轴和Y轴的偏转角度值至少有一个偏转角度值大于0，且处于已启动纠偏装置的工作状态，通过纠偏使地下工作装置轨迹偏转归正，则对纠偏轨迹进行微分，取该区域某一微元，如图5所示，此时图示弧线可近似为直角三角形的斜边，AC≈ΔL_C，所以由L_C和θ_X（或θ_Y）可以计算得出：沿Z轴方向的位移变化量ΔL_Z，以及沿X（或Y）轴方向的位移变化量ΔL_X（或ΔL_Y），分别对沿Z轴和X（或Y）轴位移变化量进行时间t的积分可得其对应的位移L_Z、L_X和L_Y，计算公式为：

$L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt};$

$L_X={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Y={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt};$

其中，θ_Xt、θ_Yt和ΔL_Ct分别为纠偏轨迹中某一微元对应的X轴和Y轴的偏转角度值以及钢丝绳的下放长度值。

4、出现偏角，纠偏二区：如果双轴倾角传感器测出的地下工作装置绕X轴和Y轴的偏转角度值至少有一个偏转角度值小于等于0，且处于已启动纠偏装置的工作状态，此时地下工作装置经过纠偏一区的动作后，偏转角度值变为0，但由于此时已相对于施工中心线产生了位移，因此需要向反方向纠偏，这样就使得偏转角度值变为负值，因此计算出的L_X和L_Y均为负值，计算公式如下：

$L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt};$

$L_X=-{\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Y=-{\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt}.$

总偏移位移的计算可以在上述任一个阶段进行或者实时进行，只需将本计算时段的偏移位移与之前的各工作情况下计算出的偏移位移进行累加即可得到本计算时段所对应的总偏移位移，进而体现出当前地下工作装置的偏移情况，为纠偏提供精确的指导，也能据此绘制出地下工作装置的工作过程，以供操作人员对工作情况进行监视。

在上述各三维姿态测算方法的实施例中，地下工作装置可对应为各类基桩工程机械的地下工作装置，例如双轮铣槽机等，这里就不再举例说明了。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。

如图6所示，为本发明三维姿态测算装置的一实施例的结构示意图。在本实施例中，三维姿态测算装置包括：双轴倾角传感器1、编码器2和控制器3。其中，双轴倾角传感器1设置在地下工作装置上，用于读取地下工作装置绕X轴和Y轴的偏转角度值。编码器2设置在牵引所述地下工作装置的钢丝绳的提升卷扬上，用于对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值。控制器3分别与双轴倾角传感器1和编码器2连接，根据钢丝绳的下放长度值以及地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。

在编码器的选择上，可采用多圈绝对式编码器，读取提升卷扬的卷扬滚筒的累计角度值，并对所述累计角度值转换得到钢丝绳的下放长度值L_C。在另一个实施例中，也可以采用多圈增量式编码器，根据卷扬滚筒的圈数和角度来转换成钢丝绳的下放长度。

如图7所示，为本发明三维姿态测算装置的另一实施例的结构示意图。与上一实施例相比，本实施例还包括：陀螺仪4，设置在地下工作装置上，并连接控制器3，用于获取地下工作装置绕Z轴的偏转角速度值ω_Z，以便控制器3根据偏转角速度值ω_Z计算出地下工作装置绕Z轴的偏转角度值θ_Z。

在上述实施例中，控制器3可以具体包括：偏离位移分算单元和偏离位移总算单元，其中偏离位移分算单元用于根据所述地下工作装置的实际工作情况分别计算不同工作情况下所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移，而偏离位移总算单元用于对不同工作情况下的所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移进行累加，得到所述地下工作装置分别沿X轴、Y轴和Z轴方向的总偏离位移。

本说明书中各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似的部分相互参见即可。对于装置实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制；尽管参照较佳实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者对部分技术特征进行等同替换；而不脱离本发明技术方案的精神，其均应涵盖在本发明请求保护的技术方案范围当中。

Claims (10)

1.一种三维姿态测算方法，包括：

通过设置在地下工作装置上的双轴倾角传感器读取所述地下工作装置绕X轴和Y轴的偏转角度值；

通过设置在牵引所述地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值；

根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。

2.根据权利要求1所述的三维姿态测算方法，其中所述编码器为多圈绝对式编码器，所述通过设置在牵引所述地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值的操作具体为：通过所述多圈绝对式编码器读取所述提升卷扬的卷扬滚筒的累计角度值，对所述累计角度值转换得到钢丝绳的下放长度值L_C。

3.根据权利要求1或2所述的三维姿态测算方法，其中还包括所述地下工作装置绕Z轴的偏转角度值的测算步骤，具体为：通过设置在所述地下工作装置上的陀螺仪获取所述地下工作装置绕Z轴的偏转角速度值ω_Z，并根据所述偏转角速度值ω_Z计算出所述地下工作装置绕Z轴的偏转角度值θ_Z。

4.根据权利要求3所述的三维姿态测算方法，其中所述根据所述偏转角速度值ω_Z计算出所述地下工作装置绕Z轴的偏转角度值θ_Z的操作具体为：通过对所述偏转角速度值ω_Z进行时间t上的积分，计算出所述地下工作装置绕Z轴的偏转角度值θ_Z，计算公式为：

${\mathrm{\θ}}_Z={\∫}_0^t{\mathrm{\ω}}_Z\mathrm{dt}.$

5.根据权利要求1或2所述的三维姿态测算方法，其中所述根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移的操作具体包括：

根据所述地下工作装置的实际工作情况分别计算不同工作情况下所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移；

对不同工作情况下的所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移进行累加，得到所述地下工作装置分别沿X轴、Y轴和Z轴方向的总偏离位移。

6.根据权利要求5所述的三维姿态测算方法，其中如果所述双轴倾角传感器测出的所述地下工作装置绕X轴和Y轴的偏转角度值均为0，判断所述地下工作装置处于垂直工作轨迹的工作状况，则所述地下工作装置沿Z轴方向的偏离位移L_Z=钢丝绳的下放长度值L_C，沿X轴和Y轴方向的偏离位移L_X和L_Y均为0；

如果所述双轴倾角传感器测出的所述地下工作装置绕X轴的偏转角度值θ_X和绕Y轴的偏转角度值θ_Y至少有一个偏转角度值大于0，且处于未启动纠偏装置的工作状态，则

L_Z=L_C·cosθ_X或L_Z=L_C·cosθ_Y；

L_X=L_C·sinθ_X或L_Y=L_C·sinθ_Y；

如果所述双轴倾角传感器测出的所述地下工作装置绕X轴和Y轴的偏转角度值至少有一个偏转角度值大于0，且处于已启动纠偏装置的工作状态，则

$L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt};$

$L_X={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Y={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt};$

其中，θ_Xt、θ_Yt和ΔL_Ct分别为纠偏轨迹中某一微元对应的X轴和Y轴的偏转角度值以及钢丝绳的下放长度值；

如果所述双轴倾角传感器测出的所述地下工作装置绕X轴和Y轴的偏转角度值至少有一个偏转角度值小于等于0，且处于已启动纠偏装置的工作状态，则

$L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Z={\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\cos {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt};$

$L_X=-{\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Xt}}\mathrm{dt}$ 或 $L_Y=-{\∫}_{t1}^{t2}{\mathrm{\Δt}}_{\mathrm{Ct}}\·\sin {\mathrm{\θ}}_{\mathrm{Yt}}\mathrm{dt}.$

7.一种三维姿态测算装置，包括：

双轴倾角传感器，设置在地下工作装置上，用于读取所述地下工作装置绕X轴和Y轴的偏转角度值；

编码器，设置在牵引所述地下工作装置的钢丝绳的提升卷扬上，用于对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值；

控制器，分别与所述双轴倾角传感器和编码器连接，根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。

8.根据权利要求7所述的三维姿态测算装置，其中所述编码器为多圈绝对式编码器，用于读取所述提升卷扬的卷扬滚筒的累计角度值，并对所述累计角度值转换得到钢丝绳的下放长度值L_C。

9.根据权利要求7或8所述的三维姿态测算装置，其中还包括：

陀螺仪，设置在所述地下工作装置上，并连接所述控制器，用于获取所述地下工作装置绕Z轴的偏转角速度值ω_Z，以便所述控制器根据所述偏转角速度值ω_Z计算出所述地下工作装置绕Z轴的偏转角度值θ_Z。

10.根据权利要求7或8所述的三维姿态测算装置，其中所述控制器包括：

偏离位移分算单元，用于根据所述地下工作装置的实际工作情况分别计算不同工作情况下所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移；

偏离位移总算单元，用于对不同工作情况下的所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移进行累加，得到所述地下工作装置分别沿X轴、Y轴和Z轴方向的总偏离位移。

Images