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

在審

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

本發明屬于掘進機導航定位技術領域，具體是一種基于毫米波雷達和慣性導航的掘進機位姿檢測方法。包括以下步驟。S100～掘進機機身后端側面安裝的毫米波雷達，實時測量掘進機后端側面與煤壁側幫雷達探測點之間的距離，S200～掘進機機身前端側面安裝的毫米波雷達，實時測量掘進機前端側面與煤壁側幫雷達探測點之間的距離，S300～慣性導航固定在掘進機機身頂部，實時測量掘進機在航向、翻滾和俯仰三個方向的偏轉角度，S400～當掘進機沒有姿態變化時，毫米波雷達測量距離為機身側面與煤壁側幫之間的垂直距離；當機身發生姿態變化時，毫米波雷達測量的距離為機身側面與煤壁側幫的斜距，利用數據融合方法處理得到掘進機側身與煤壁側幫之間的垂直距離。

技術領域

本發明屬于掘進機導航定位技術領域，具體是一種基于毫米波雷達和慣性導航的掘進機位姿檢測方法。

背景技術

我國礦井下掘進工作面由于巷道斷面有限，空間狹小，有大量的粉塵、有害氣體，且溫度、濕度較高，環境較為惡劣，粉塵會危險職工的身體健康。為了減少工作人員，推進井下自動化的發展，掘進機的位姿檢測變得至關重要，大量的科研人員致力于掘進機導航定位的研究。

公號為CN104295297A的發明專利“一種掘進機截割頭定位系統及方法和掘進機”，該專利主要利用超聲波傳感器、慣性測量單元和全站儀進行掘進機機身位置信息的測量，再根據掘進機機身相對于巷道的位置、截割臂相對于機身、截割頭相對于截割臂之間的位置關系，計算出截割頭相對于巷道的位置信息。該方法提供了一種掘進機截割頭的位姿參數檢測方法，技術可行，但該方法結構復雜，且全站儀配套棱鏡易受到粉塵污染。

公號為CN102419433A的發明專利“一種掘進機姿態定位系統”，該專利應用GPS定位原理，采用無線信號進行掘進機位置檢測。該方法對環境的適應性較強，為掘進機位置檢測提供了方向，但煤礦井下掘進工作面環境復雜，會影響無線信號對掘進機位置的計算精度。

發明內容

本發明為了解決上述問題，提供一種基于毫米波雷達和慣性導航的掘進機位姿檢測方法。

本發明采取以下技術方案：一種基于毫米波雷達和慣性導航的掘進機位姿檢測方法，包括以下步驟。

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

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

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

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

步驟S300采取以下步驟：

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

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