[發明專利]一種基于機械臂視覺感知的掘進機位姿檢測系統及方法

有效

• 申請號: 202011081832.2
• 申請日: 2020-10-12
• 公開（公告）號: CN112197756B
• 公開（公告）日: 2023-10-17
• 發明（設計）人: 楊健健;張強;王超;常博深;王曉林
• 申請（專利權）人: 中國礦業大學（北京）
• 主分類號: G01C15/00
• 分類號: G01C15/00;G01B11/02;G01B11/26
• 代理公司: 北京友誼嘉知識產權代理事務所(普通合伙) 16075
• 代理人: 張會會

1.一種基于機械臂視覺感知的掘進機位姿檢測系統及方法，包括有掘進機及煤巷內已裝有的激光指向儀，其特征在于：還包括：機械臂、可編程計算機控制器、CCD圖像傳感器、機載傾角傳感器組成，其中，

激光指向儀，是由煤礦地測部位安裝在煤巷頂部，按照巷道設計要求給出掘進方向指向，是巷道掘進唯一的方向基準；

機械臂，安裝在掘進機上，由可編程計算機控制器控制機械臂末端桿件與激光指向儀發射的激光光路重合，通過反饋各個關節姿態信息，從而解算掘進機的偏向角和偏向位移；

可編程計算機控制器，包括工控機、多軸控制器、電機驅動器、直流伺服電機，工控機通過TCP協議與多軸控制器通信，電機驅動器控制直流伺服電機，從而控制機械臂運動；

CCD圖像傳感器，安裝在機械臂末端桿件上，接收來自激光指向儀發射的激光信號，反饋機械臂末端桿件與激光指向儀發射的激光光路重合信號到可編程計算機控制器。

機載傾角傳感器包括橫向傾角傳感器，軸向傾角傳感器和A/D轉換模塊，安裝在掘進機后機箱內，分別測量掘進機的滾動角和俯仰角，其中，橫向傾角傳感器的角度測量方向設定為與機身中線垂直方向，該傾角傳感器的輸出信號反映機身平面與水平面在機身橫向上絕對夾角，軸向傾角傳感器的角度測量方向應設定為與機身中線的平行方向，該傾角傳感器的輸出信號反映機身平面與水平面在機身縱方向上絕對夾角，完成掘進機的機身位姿滾動角和俯仰角的參數測量。

2.根據權利要求1所述的系統，其特征在于其中所述的激光指向儀發出的激光線路，其安裝位置由地測部門標定，指示巷道設計方向。

3.根據權利要求1所述的系統，其特征在于所述的掘進機偏向角分辨誤差小于0.1度，偏向位移的分辨誤差小于5厘米。

4.根據權利要求1所述的系統，其特征在于所述的橫向傾角傳感器和軸向傾角傳感器分別測得的數據經A/D轉換模塊傳入工控機，分別得到掘進機的滾動角和俯仰角，測量誤差小于0.1度。

5.根據權利要求1所述的系統，其特征在于所述的掘進機的偏向角和偏向位移，可由機械臂上CCD圖像傳感器感知激光指向儀發出的激光光斑位置確定，當掘進機發生偏向時，則可通過機械臂反饋的關節角度信息傳入工控機，將偏向角和偏向位移位姿參數解算出來。

6.根據權利要求1-5中任意一項所述的系統，其特征在于所述的位姿檢測算法符合以下公式：

其中，w為激光指向儀發射激光在CCD圖像傳感器上光斑位置矩陣，R為激光光斑半徑，ρ為極坐標半徑，θ為極坐標角度；s為機械臂坐標矩陣，L為機械臂長度，α為機械臂關節轉角，d為機械臂連桿偏移長度,δ為機械臂關節扭轉角，n為機械臂桿件數量。

7.一種用權利要求1所述的基于機械臂視覺感知的掘進機位姿檢測系統及方法，其特征在于：包括:

固定安裝好激光指向儀激光束指向巷道設計方向；

啟動機械臂，可編程計算機控制器控制安裝在掘進機上的機械臂；

機械臂末端桿件與激光指向儀激光光路重合，CCD圖像傳感器接收到激光光斑位置信息并傳輸到工控機；

通過機械臂反饋各個關節姿態信息，多軸控制器將數據傳送給工控機，工控機解算掘進機的偏向角和偏向位移；

掘進機后安裝的機載傾角傳感器包括橫向傾角傳感器和軸向傾角傳感器，分別測量掘進機機身滾動角和俯仰角；

機載傾角傳感器中測量的數據信息經過A/D轉換模塊傳入工控機，分別得到掘進機的滾動角和俯仰角。