[發明專利]一種四點式掘進機自動定位定向方法

有效

• 申請號: 201611235639.3
• 申請日: 2016-12-28
• 公開（公告）號: CN106840137B
• 公開（公告）日: 2022-12-27
• 發明（設計）人: 田原;張建廣;楊文杰;趙永紅;梁萬吉;王學成;白雪峰;閻志偉;王智輝;王瑤;陳露;田秋實
• 申請（專利權）人: 中國煤炭科工集團太原研究院有限公司;山西天地煤機裝備有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00
• 代理公司: 太原晉科知識產權代理事務所(特殊普通合伙) 14110
• 代理人: 任林芳;王芳

1.一種四點式掘進機自動定位定向方法，其特征在于：基于機器視覺技術的四點法獲得掘進機機身空間位置和姿態參數；具體方法為：在掘進機機身上表面選取四個特征點，將視覺定位定向儀安裝在掘進機后方巷道頂板上，其指向與巷道涉及方向一致，所述四個特征點取在掘進機機身的上表面，呈“口”字形分布，范圍盡可能大，且四點相對于掘進機機身縱向軸線對稱，A、B、C、D為選定的特征點，其中AB和CD均垂直于機身中軸線MN，以AB和CD及其中點連線MN組成的結構表示掘進機機身在水平面內的投影，成像平面L和焦點O構成攝像機成像幾何模型，X軸表示掘進機機身橫向偏移方向，Z軸表示掘進機機身前進方向，與X軸、Z軸正交的Y軸表示掘進機機身縱向偏移方向；

利用視覺定位定向儀中的工業智能攝像機對在掘進機上選定的四個特征點成像，分析特征點圖像與已知的四個特征點之間的相對位置關系，計算得到掘進機在工業智能攝像機坐標系中的空間位置及空間姿態的偏轉角、橫滾角、俯仰角、機身的橫向偏移量、機身的縱向偏移量和機身到工業智能攝像機的距離六個參數，通過標定工業智能攝像機與視覺定位定向儀上的激光器之間的相對位置關系，將掘進機在工業智能攝像機坐標系中的空間位置和姿態參數轉換到代表巷道設計基準下的掘進機空間位置和姿態參數。

2.根據權利要求1所述的一種四點式掘進機自動定位定向方法，其特征在于：所述掘進機在工業智能攝像機坐標系中的偏轉角、橫滾角、俯仰角、機身的橫向偏移量、機身的縱向偏移量和機身到工業智能攝像機的距離的計算方法為：

機身與工業智能攝像機之間距離根據機身與工業智能攝像機之間距離根據

計算出，各特征點A、B、C、D在工業智能攝像機坐標系中的橫向縱向二維空間坐標(x_A，y_A)、(x_B,y_B)、(x_C,y_C)、(x_D,y_D)根據

計算出，AB和CD的中點M、N的橫向縱向二維空間坐標(x_M,y_M)、(x_N,y_N)根據

計算出，將MN投影在水平面上，則MN，即掘進機機身中軸線在工業智能攝像機坐標系中的偏轉角α根據

計算出；將MN投影在鉛垂面上，則機身俯仰角β根據

計算出；A、B的像點在工業智能攝像機坐標系中與其橫軸的夾角即為掘進機機身的橫滾角γ；

以MN的中點為機身中心，則機身的縱向偏移、橫向偏移、與工業智能攝像機之間的距離即掘進機機身的空間位置坐標(x，y，z)根據

可計算出；

其中：A、B、C、D為選定的特征點，A’、B’、C’、D’分別為A、B、C、D的像，AB、CD為機身特征點間距，MN為AB、CD的中點連線，f為工業智能攝像機鏡頭焦距。