[發明專利]一種多傳感器融合的巷道掘進裝備實時定位系統和方法

有效

• 申請號: 202010796825.4
• 申請日: 2020-08-10
• 公開（公告）號: CN111879314B
• 公開（公告）日: 2022-08-02
• 發明（設計）人: 秦念穩;肖正航;李建華;陳曉偉;曾苗筠;李兆陽;張宏
• 申請（專利權）人: 中國鐵建重工集團股份有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/20;G01C21/16;G01S17/08
• 代理公司: 長沙七源專利代理事務所(普通合伙) 43214
• 代理人: 張勇;周曉艷

1.一種多傳感器融合的巷道掘進設備實時定位方法，其特征在于，巷道掘進設備實時定位方法包括以下步驟：

步驟一：建立坐標系，坐標系包括大地坐標系O₀X₀Y₀Z₀、機身坐標系O₁X₁Y₁Z₁、前攝像頭坐標系O_C1X_C1Y_C1Z_C1和后攝像頭坐標系O_C2X_C2Y_C2Z_C2；

步驟二：通過指向激光獲取大地坐標系下激光光斑的坐標；

獲取大地坐標系下激光光斑的坐標的方法具體是：已知巷道指向激光的方向，通過激光光源到光斑的距離可得到光斑相對光源的坐標信息；結合激光發射器在巷道頂板上的安裝位置，根據公式(1)計算出大地坐標系下激光光斑的坐標；

其中，[X_D Y_D Z_D]^T是激光光斑在大地坐標系下的坐標；

[cosx_{S_D} cosy_{S_D} cosz_{S_D}]^T是指向激光的方向矢量；

L_{S_D}是激光光源與光斑之間的距離；

[X_S Y_S Z_S]^T是激光光源在大地坐標系下的坐標；

步驟三：獲取機身坐標系下激光光斑和光源的坐標；

獲取機身坐標系下激光光斑和光源的坐標的方法具體是：通過前攝像頭獲取激光光斑方向，并基于雙目視覺獲取前攝像頭至激光光斑的距離，得到光斑相對前攝像頭的坐標信息；結合前攝像頭在掘進裝備機身上的安裝位置，根據公式(2)計算出機身坐標系下激光光斑的坐標；

其中，[x_C1 y_C1 z_C1]^T是激光光斑在機身坐標系下的坐標；

[cosx_C1 cosy_C1 cosz_C1]^T是激光光斑相對前攝像頭的方向矢量；

L_{C1_D}是前攝像頭與激光光斑之間的距離；

[Δx_C1 Δy_C1 Δz_C1]^T是前攝像頭坐標系和機身坐標系原點間的矢量；

通過后攝像頭獲取激光光源方向，得到光源相對后攝像頭的坐標信息；結合后攝像頭在掘進裝備機身上的安裝位置，根據公式(3)計算出機身坐標系下激光光源的坐標；

其中，[x_C2 y_C2 z_C2]^T是激光光源在機身坐標系下的坐標；

[cosx_C2 cosy_C2 cosz_C2]^T是激光光源相對后攝像頭的方向矢量；

L_{C2_S}是后攝像頭與激光光源之間的距離；

[Δx_C2 Δy_C2 Δz_C2]^T是后攝像頭坐標系和機身坐標系原點間的矢量；

步驟四：通過機身和大地間的坐標轉換關系獲取大地坐標系下激光光斑和光源的坐標；

根據機身坐標系和大地坐標系間的相互轉換關系，結合已獲取的激光光斑在機身坐標系下的坐標，根據公式(4)計算出激光光斑在大地坐標系下的坐標；

其中，[ΔX ΔY ΔZ]^T表示機身坐標系和大地坐標系原點間的矢量，即掘進裝備機身在大地坐標系下的坐標；

[x_C1 y_C1 z_C1]^T是激光光斑在機身坐標系下的坐標；

R_X0、R_Y0、R_Z0是機身相對大地坐標系的分別沿X₀、Y₀、Z₀軸的旋轉矩陣；

α、β、γ分別表示機身相對大地坐標系的滾動角、俯仰角、偏航角；

結合已獲取的激光光源在機身坐標系下的坐標，根據公式(6)計算出激光光源在大地坐標系下的坐標；

其中，[x_C2 y_C2 z_C2]^T是激光光源在機身坐標系下的坐標；

步驟五：獲取測距雷達間的距離；

雷達之間的距離通過雷達發射機和雷達接收機的坐標信息由公式(7)計算得出；

其中，L_{LD1_LD2}是測距雷達間的距離；

[x_LD1 y_LD1 z_LD1]^T是雷達發射機在大地坐標系下的坐標；

[x_LD2 y_LD2 z_LD2]^T是雷達接收機在大地坐標系下的坐標，通過將雷達接收機從機身坐標系轉換到大地坐標系得到；

其中，[x_C y_C z_C]^T是雷達接收機在機身坐標系下的安裝位置坐標；

步驟六：進行掘進裝備機身在大地坐標系下的位姿解算；

具體是：機身位姿包括橫向偏移量、縱向偏移量、高程偏移量、滾動角、俯仰角、偏航角共六個參數，分別用ΔX、ΔY、ΔZ、α、β、γ表示，其中α和β通過安裝在機身上的慣性單元獲取，ΔX、ΔY、ΔZ、γ為待求解的未知量；在位姿解算過程中還包括后攝像頭至激光光源的距離L_{C2_S}、巷道指向激光光源到激光光斑的距離L_{S_D}、激光光源相對后攝像頭的俯仰角θ，共七個待求解的未知量，聯立公式(4)、(6)、(7)并相應代入其他公式，對未知量進行求解：

方程組(9)實際包含七個非線性方程，解算出使方程組兩端誤差最小時的七個未知量，進而得到掘進裝備在大地坐標系下六個位姿參數，獲取機身完整的位姿信息。