[發(fā)明專(zhuān)利]一種基于機(jī)器視覺(jué)和慣性導(dǎo)航的掘進(jìn)機(jī)位姿檢測(cè)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110084687.1
• 申請(qǐng)日: 2021-01-21
• 公開(kāi)（公告）號(hào): CN112857367B
• 公開(kāi)（公告）日: 2023-10-13
• 發(fā)明（設(shè)計(jì)）人: 賈曲;田原;陳寧;索艷春;董孟陽(yáng);張婷;李濤;郭皇煌
• 申請(qǐng)（專(zhuān)利權(quán)）人: 中國(guó)煤炭科工集團(tuán)太原研究院有限公司;山西天地煤機(jī)裝備有限公司
• 主分類(lèi)號(hào): G01C21/16
• 分類(lèi)號(hào): G01C21/16
• 代理公司: 太原晉科知識(shí)產(chǎn)權(quán)代理事務(wù)所(特殊普通合伙) 14110
• 代理人: 任林芳

1.一種基于機(jī)器視覺(jué)和慣性系統(tǒng)的掘進(jìn)機(jī)位姿檢測(cè)方法，其特征在于：包括以下步驟，

S100～慣性系統(tǒng)和工業(yè)相機(jī)剛性連接，并且固定在掘進(jìn)機(jī)機(jī)身頂部；慣性系統(tǒng)用來(lái)檢測(cè)掘進(jìn)機(jī)的姿態(tài)，根據(jù)當(dāng)?shù)氐慕?jīng)緯高參數(shù)對(duì)慣性系統(tǒng)進(jìn)行初對(duì)準(zhǔn)，對(duì)準(zhǔn)結(jié)束后開(kāi)始采集掘進(jìn)機(jī)地理坐標(biāo)系下的X軸、Y軸和Z軸角度參數(shù)，根據(jù)掘進(jìn)機(jī)姿態(tài)角的定義，求出其姿態(tài)角，并傳輸至交換機(jī)；

S200～工業(yè)相機(jī)以視頻流方式實(shí)時(shí)顯示巷道頂板信息，控制主機(jī)以圖片格式保存視頻流中的幀序列，對(duì)圖像進(jìn)行處理，求出圖像中目標(biāo)的位置和旋轉(zhuǎn)、平移參數(shù)，其中，巷道頂板特征是巷道的頂板托盤(pán)，呈現(xiàn)正方形；

S300～交換機(jī)通過(guò)以太網(wǎng)與控制主機(jī)進(jìn)行數(shù)據(jù)傳輸，控制主機(jī)對(duì)圖像數(shù)據(jù)進(jìn)行處理、目標(biāo)檢測(cè)與識(shí)別、目標(biāo)跟蹤，對(duì)慣性系統(tǒng)采集的數(shù)據(jù)進(jìn)行數(shù)據(jù)轉(zhuǎn)換，求出掘進(jìn)機(jī)實(shí)時(shí)姿態(tài)參數(shù)；

S400～利用組合導(dǎo)航檢測(cè)掘進(jìn)機(jī)的位置參數(shù)增量與實(shí)時(shí)姿態(tài)參數(shù)。

2.根據(jù)權(quán)利要求1所述的機(jī)器視覺(jué)和慣性系統(tǒng)的掘進(jìn)機(jī)位姿檢測(cè)方法，其特征在于：步驟S100中，需要根據(jù)掘進(jìn)機(jī)地理坐標(biāo)系下的X軸、Y軸和Z軸角度求出掘進(jìn)機(jī)的姿態(tài)角；建立載體坐標(biāo)系，坐標(biāo)系原點(diǎn)為掘進(jìn)機(jī)的質(zhì)心O_b，X_b軸方向?yàn)榫蜻M(jìn)機(jī)橫軸正右方，Y_b軸方向?yàn)榫蜻M(jìn)機(jī)縱軸正前方，Z_b軸方向正交X_b軸、Y_b軸豎直向上。

姿態(tài)角的定義：

俯仰角：載體坐標(biāo)系繞地理坐標(biāo)系旋轉(zhuǎn)，Y_b軸與O_nX_nY_n水平面的夾角，用θ表示，當(dāng)Y_b軸的正半軸位于過(guò)坐標(biāo)原點(diǎn)的水平面之上時(shí)，俯仰角為正，反之為負(fù)，旋轉(zhuǎn)范圍是-90°～90°；

翻滾角：載體坐標(biāo)系繞地理坐標(biāo)系旋轉(zhuǎn)，Z_b軸與通過(guò)載體Y_b軸的鉛垂面間的夾角，用γ表示。以載體向右傾斜為正，反之為負(fù)，旋轉(zhuǎn)范圍是-180°～180°；

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

對(duì)應(yīng)的坐標(biāo)變換矩陣如下：

偏航角的旋轉(zhuǎn)矩陣為：

俯仰角θ的旋轉(zhuǎn)矩陣為：

翻滾角γ的旋轉(zhuǎn)矩陣為：

三個(gè)基本旋轉(zhuǎn)矩陣進(jìn)行合并，求出載體坐標(biāo)系與地理坐標(biāo)系的旋轉(zhuǎn)矩陣：

式中，為單位正交矩陣，有：

載體坐標(biāo)系與地理坐標(biāo)系之間的轉(zhuǎn)換可以借助姿態(tài)矩陣完成，該姿態(tài)矩陣可以簡(jiǎn)化表示為：

由于地理坐標(biāo)系到載體坐標(biāo)系的旋轉(zhuǎn)過(guò)程中坐標(biāo)系始終保持直角坐標(biāo)系，所以為正交矩陣，則有：

對(duì)比式(6)與式(7)中的姿態(tài)角和姿態(tài)矩陣，可在已知姿態(tài)矩陣的情況下反推求出姿態(tài)角，則有：

3.根據(jù)權(quán)利要求2所述的機(jī)器視覺(jué)和慣性系統(tǒng)的掘進(jìn)機(jī)位姿檢測(cè)方法，其特征在于：所述的步驟S200中，掘進(jìn)機(jī)行進(jìn)過(guò)程中，工業(yè)相機(jī)的視野范圍內(nèi)至少覆蓋兩個(gè)目標(biāo)。