三维姿态测算方法及装置制造方法
【专利摘要】本发明涉及一种三维姿态测算方法及装置，方法包括：通过设置在地下工作装置上的双轴倾角传感器读取地下工作装置绕X轴和Y轴的偏转角度值；通过设置在牵引地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值；根据钢丝绳的下放长度值以及地下工作装置分别绕X轴和Y轴的偏转角度值计算出地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。本发明消除了因地下工作装置偏离施工中心线时所带来的测算误差，并进一步的计算出地下工作装置沿X轴和Y轴的偏离位移，准确的获得基桩工程机械的地下工作装置在工作过程中的三维姿态，以作为地下工作装置纠偏控制和监控系统显示的重要依据。
【专利说明】三维姿态测算方法及装置
【技术领域】
[0001]本发明涉及工程机械领域，尤其涉及一种基桩工程机械的地下工作装置的三维姿态测算方法及装置。
【背景技术】
[0002]双轮铣槽机是一种通过钢丝绳悬吊铣刀架进行大深度挖槽工作的工程机械装备，主要应用于水利工程建设等施工中。施工时，铣刀架整体没于深水泥浆中，其三维姿态与施工质量密切相关，并可作为铣刀架纠偏控制和监控显示的重要依据，因此准确获知铣刀架的三维姿态是提高双轮铣槽机工作性能的关键因素。
[0003]如图1所示，为现有的双轮铣槽机的外形示意图。其中钢丝绳Al连接铣刀架A2的上端，在铣刀架A2的下端设有多个铣轮A3。其中，用于描述铣刀架的三维姿态的参数包括沿X、Y、Z轴方向的位移，以及绕X、Y、Z轴方向的偏转角度。其中铣刀架的绕X、Y轴偏转角度是相对于垂直中心线的绝对角度，也是相对于施工中心线的角度，绕Z轴偏转角度是相对于铣刀架自身的角度；铣刀架在X轴和Y轴的偏离位移是相对于施工中心线的偏离位移。
[0004]对于现有技术，仅能测量沿Z轴方向的位移，以及绕X、Y轴方向的偏转角度，具体测量方法如下:
[0005](I)沿Z轴方向的位移，在提升卷扬上安装编码器，以测量卷扬滚筒圈数和角度，转换成钢丝绳的下放长度，将其作为沿Z轴方向的位移，即铣刀架的下放深度；
[0006](2)绕X、Y轴方向的偏转角度，在铣刀架上安装双轴倾角传感器，其输出值即为相应偏转角度值。
[0007]如图2所示，现有技术存在以下缺陷，使得其无法测得铣刀架工作的三维姿态:
[0008](I)无法测得铣刀架沿X、Y轴方向的位移，即偏离施工中心线的距离；
[0009](2)无法测得绕Z轴方向的偏转角度，因为铣刀架与双轮铣上车采用钢丝绳连接，这种柔性连接方式，增大了其测量难度，由于测得沿Z轴方向的位移不准确，直接将钢丝绳的下放长度作为铣刀架沿Z轴方向的位移，但是铣刀架工作中会出现偏离施工中心线的情况，这时钢丝绳会与施工中心线产生一定偏角，而不是重合，所以造成数据不准确。
【发明内容】
[0010]本发明的目的是提出一种三维姿态测算方法及装置，能够准确获得基桩工程机械的地下工作装置在工作过程中的三维姿态，以作为地下工作装置纠偏控制和监控系统显示的重要依据。
[0011]为实现上述目的，本发明提供了一种三维姿态测算方法，包括:
[0012]通过设置在地下工作装置上的双轴倾角传感器读取所述地下工作装置绕X轴和Y轴的偏转角度值；
[0013]通过设置在牵引所述地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值；
[0014]根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。
[0015]进一步的，所述编码器为多圈绝对式编码器，所述通过设置在牵引所述地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值的操作具体为:通过所述多圈绝对式编码器读取所述提升卷扬的卷扬滚筒的累计角度值，对所述累计角度值转换得到钢丝绳的下放长度值L。。
[0016]进一步的，还包括所述地下工作装置绕Z轴的偏转角度值的测算步骤，具体为:通过设置在所述地下工作装置上的陀螺仪获取所述地下工作装置绕Z轴的偏转角速度值ωζ，并根据所述偏转角速度值ωζ计算出所述地下工作装置绕Z轴的偏转角度值θζ。
[0017]进一步的，所述根据所述偏转角速度值ωζ计算出所述地下工作装置绕Z轴的偏转角度值θ ζ的操作具体为:通过对所述偏转角速度值ωζ进行时间t上的积分，计算出所述地下工作装置绕Z轴的偏转角度值θζ，计算公式为:
【权利要求】
1.一种三维姿态测算方法，包括: 通过设置在地下工作装置上的双轴倾角传感器读取所述地下工作装置绕X轴和Y轴的偏转角度值； 通过设置在牵引所述地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值； 根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。
2.根据权利要求1所述的三维姿态测算方法，其中所述编码器为多圈绝对式编码器，所述通过设置在牵引所述地下工作装置的钢丝绳的提升卷扬上的编码器对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值的操作具体为:通过所述多圈绝对式编码器读取所述提升卷扬的卷扬滚筒的累计角度值，对所述累计角度值转换得到钢丝绳的下放长度值Lc。
3.根据权利要求1或2所述的三维姿态测算方法，其中还包括所述地下工作装置绕Z轴的偏转角度值的测算步骤，具体为:通过设置在所述地下工作装置上的陀螺仪获取所述地下工作装置绕Z轴的偏转角速度值ωζ，并根据所述偏转角速度值ωζ计算出所述地下工作装置绕Z轴的偏转角度值θζ。
4.根据权利要求3所述的三维姿态测算方法，其中所述根据所述偏转角速度值0^计算出所述地下工作装置绕Z轴的偏转角度值θζ的操作具体为:通过对所述偏转角速度值?2进行时间t上的积分，计算出所述地下工作装置绕Z轴的偏转角度值θζ，计算公式为:
5.根据权利要求1或2所述的三维姿态测算方法，其中所述根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移的操作具体包括: 根据所述地下工作装置的实际工作情况分别计算不同工作情况下所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移； 对不同工作情况下的所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移进行累加，得到所述地下工作装置分别沿X轴、Y轴和Z轴方向的总偏离位移。
6.根据权利要求5所述的三维姿态测算方法，其中如果所述双轴倾角传感器测出的所述地下工作装置绕X轴和Y轴的偏转角度值均为O，判断所述地下工作装置处于垂直工作轨迹的工作状况，则所述地下工作装置沿Z轴方向的偏离位移Lz=钢丝绳的下放长度值Lc，沿X轴和Y轴方向的偏离位移Lx和Ly均为O ; 如果所述双轴倾角传感器测出的所述地下工作装置绕X轴的偏转角度值Θ x和绕Y轴的偏转角度值Θ Y至少有一个偏转角度值大于O，且处于未启动纠偏装置的工作状态，则
Lz=Lc.cos θ x 或 Lz=Lc.cos θ ；
Lx=Lc.sin θ x 或 Ly=Lc.sin θ ； 如果所述双轴倾角传感器测出的所述地下工作装置绕X轴和Y轴的偏转角度值至少有一个偏转角度值大于0，且处于已启动纠偏装置的工作状态，则
7.—种三维姿态测算装置，包括: 双轴倾角传感器，设置在地下工作装置上，用于读取所述地下工作装置绕X轴和Y轴的偏转角度值； 编码器，设置在牵引所述地下工作装置的钢丝绳的提升卷扬上，用于对卷扬滚筒的转动进行计数，并转换得到钢丝绳的下放长度值； 控制器，分别与所述双轴倾角传感器和编码器连接，根据所述钢丝绳的下放长度值以及所述地下工作装置分别绕X轴和Y轴的偏转角度值计算出所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移。
8.根据权利要求7所述的三维姿态测算装置，其中所述编码器为多圈绝对式编码器，用于读取所述提升卷扬的卷扬滚筒的累计角度值，并对所述累计角度值转换得到钢丝绳的下放长度值L。。
9.根据权利要求7或8所述的三维姿态测算装置，其中还包括: 陀螺仪，设置在所述地下工作装置上，并连接所述控制器，用于获取所述地下工作装置绕Z轴的偏转角速度值ωζ，以便所述控制器根据所述偏转角速度值ωζ计算出所述地下工作装置绕Z轴的偏转角度值θζ。
10.根据权利要求7或8所述的三维姿态测算装置，其中所述控制器包括: 偏离位移分算单元，用于根据所述地下工作装置的实际工作情况分别计算不同工作情况下所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移； 偏离位移总算单元，用于对不同工作情况下的所述地下工作装置分别沿X轴、Y轴和Z轴方向的偏离位移进行累加，得到所述地下工作装置分别沿X轴、Y轴和Z轴方向的总偏离位移。
【文档编号】G01B21/02GK103837115SQ201410074723
【公开日】2014年6月4日 申请日期:2014年3月4日 优先权日:2014年3月4日
【发明者】左帅, 任良才, 温读夫 申请人:徐工集团工程机械股份有限公司