[發明專利]三維姿態測算方法及裝置

有效

• 申請號: 201410074723.6
• 申請日: 2014-03-04
• 公開（公告）號: CN103837115B
• 公開（公告）日: 2016-08-31
• 發明（設計）人: 左帥;任良才;溫讀夫
• 申請（專利權）人: 徐工集團工程機械股份有限公司
• 主分類號: G01B21/02
• 分類號: G01B21/02;G01B21/22;E02F5/02
• 代理公司: 中國國際貿易促進委員會專利商標事務所 11038
• 代理人: 顏鏑

技術領域

本發明涉及工程機械領域，尤其涉及一種基樁工程機械的地下工作裝置的三維姿態測算方法及裝置。

背景技術

雙輪銑槽機是一種通過鋼絲繩懸吊銑刀架進行大深度挖槽工作的工程機械裝備，主要應用于水利工程建設等施工中。施工時，銑刀架整體沒于深水泥漿中，其三維姿態與施工質量密切相關，并可作為銑刀架糾偏控制和監控顯示的重要依據，因此準確獲知銑刀架的三維姿態是提高雙輪銑槽機工作性能的關鍵因素。

如圖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，計算公式為：

θZ=∫0tωZdt.]]>

進一步的，所述根據所述鋼絲繩的下放長度值以及所述地下工作裝置分別繞X軸和Y軸的偏轉角度值計算出所述地下工作裝置分別沿X軸、Y軸和Z軸方向的偏離位移的操作具體包括：