[發明專利]基于毫米波雷達和慣性導航的掘進機位姿檢測方法

在審

• 申請號: 202110084801.0
• 申請日: 2021-01-21
• 公開（公告）號: CN112902952A
• 公開（公告）日: 2021-06-04
• 發明（設計）人: 賈曲;田原;陳寧;索艷春;董孟陽;張婷;李濤;郭皇煌
• 申請（專利權）人: 中國煤炭科工集團太原研究院有限公司;山西天地煤機裝備有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/18;G01S13/42
• 代理公司: 太原晉科知識產權代理事務所(特殊普通合伙) 14110
• 代理人: 任林芳

1.一種基于毫米波雷達和慣性導航的掘進機位姿檢測方法，其特征在于：包括以下步驟，

S100～掘進機機身后端側面安裝的毫米波雷達，實時測量掘進機后端側面與煤壁側幫雷達探測點之間的距離，并將測量結果傳輸到數據處理模塊；

S200～掘進機機身前端側面安裝的毫米波雷達，實時測量掘進機前端側面與煤壁側幫雷達探測點之間的距離，并將測量結果傳輸到數據處理模塊；

S300～慣性導航固定在掘進機機身頂部，實時測量掘進機在航向、翻滾和俯仰三個方向的偏轉角度，并將慣性導航采集的航向、翻滾方向和俯仰方向的角度轉換為掘進機的姿態角；

S400～當掘進機沒有姿態變化時，毫米波雷達測量距離為機身側面與煤壁側幫之間的垂直距離；當機身發生姿態變化時，毫米波雷達測量的距離為機身側面與煤壁側幫的斜距，利用數據融合方法處理得到掘進機側身與煤壁側幫之間的垂直距離。

2.根據權利要求1所述的基于毫米波雷達和慣性導航的掘進機位姿檢測方法，其特征在于：所述的步驟S300采取以下步驟，

S301～建立載體坐標系，載體坐標系的原點為掘進機的質心O_b，則X_b軸方向為掘進機橫軸正右方，Y_b軸方向為掘進機縱軸正前方，Z_b軸方向正交X_b軸、Y_b軸并豎直向上；

S302～將地理坐標系下的姿態角轉換為掘進機的姿態角，其中地理坐標系用n表示，地理坐標系的原點為載體的質心O_n，取“東北天”三個方向作為坐標軸方向，則X_n軸方向為水平向東，Y_n軸方向為北極方向，Z_n軸方向為垂直于水平面向上；

而姿態角的定義：

俯仰角：載體坐標系繞地理坐標系旋轉，Y_b軸與O_nX_nY_n水平面的夾角，用θ表示；當Y_b軸的正半軸位于過坐標原點的水平面之上時，俯仰角為正，反之為負，旋轉范圍是-90°～90°；

翻滾角：載體坐標系繞地理坐標系旋轉，Z_b軸與O_nY_nZ_n平面的夾角，用γ表示，以載體向右傾斜為正，反之為負，旋轉范圍是-180°～180°；

偏航角：載體坐標系繞地理坐標系旋轉，Y_b軸在O_nX_nY_n水平面上的投影與Y_n之間的夾角，用表示。以載體右偏為正，反之為負，旋轉范圍是0°～360°；

S303～將三軸加速度作為一個向量，它的三個方向余弦分別是該向量與三個坐標軸之間的轉換矩陣，分別用xi、yj、zk、表示載體坐標系中的三軸加速度，x’i’、y’j’、z’k’表示地理坐標系中的三軸加速度，因加速度向量A是唯一的，所以滿足式(1)：

A＝xi+yj+zk＝x'i'+y'j'+z'k' (1)

用矩陣的形式表示式(1)：

變換矩陣得到式(2)：

兩單位向量的乘機等于他們的夾角的余弦值，即：

式中，α_m,m＝1,2,3地理坐標系x軸與掘進機坐標系各軸之間的夾角；β_n,n＝1,2,3表示地理坐標系y軸與掘進機坐標系各軸之間的夾角；φ_p,p＝1,2,3地理坐標系z軸與掘進機坐標系各軸之間的夾角。

3.根據權利要求2所述的基于毫米波雷達和慣性導航的掘進機位姿檢測方法，其特征在于：所述的步驟S400以下步驟，

設定s為掘進機沒有姿態變化時側身與煤壁側幫之間的距離；l為掘進機機身發生角度的偏航時側身與煤壁側幫之間的測量距離，可以計算此時掘進機側身與煤壁側幫之間的垂直距離為：

式中，d為掘進機側身與煤壁側幫之間的垂直距離；

當掘進機發生γ角度的翻滾時，可以計算此時掘進機側身與煤壁側幫之間的垂直距離為：

d＝l′cosγ (3-2)

式中，l′為掘進機機身發生γ角度的翻滾時側身與煤壁側幫之間的測量距離；

當掘進機機身發生角度的偏航同時發生了γ角度的翻滾時，掘進機側身與煤壁側幫之間的測量距離為L，可以計算此時掘進機側身與煤壁側幫之間的垂直距離為：

式中，D掘進機側身與煤壁側幫之間的垂直距離。