[發明專利]井下動態掘進機機身位姿參數測量方法及系統

有效

• 申請號: 202010858615.3
• 申請日: 2020-08-24
• 公開（公告）號: CN112066955B
• 公開（公告）日: 2022-04-29
• 發明（設計）人: 張旭輝;劉博興;張超;謝楠;張楷鑫;周創
• 申請（專利權）人: 西安科技大學
• 主分類號: G01C15/00
• 分類號: G01C15/00;G01C1/00
• 代理公司: 西安恒泰知識產權代理事務所 61216
• 代理人: 李婷;趙中霞

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為負則偏離中軸線向左。