[發明專利]掘進機的位姿標定方法及系統

在審

• 申請號: 202011531231.7
• 申請日: 2020-12-22
• 公開（公告）號: CN112665613A
• 公開（公告）日: 2021-04-16
• 發明（設計）人: 劉洋;崔玲玲;張東旭
• 申請（專利權）人: 三一重型裝備有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01C21/16;G01C11/34
• 代理公司: 北京中強智尚知識產權代理有限公司 11448
• 代理人: 黃耀威

1.一種掘進機的位姿標定方法，其特征在于，包括：

獲取第一圖像采集裝置相對激光靶面上光斑的第一位移矢量，以及第二圖像采集裝置相對所述光斑的第二位移矢量，其中，所述第一位移矢量和所述第二位移矢量空間內不平行，所述激光靶懸掛在激光指向儀前側，所述激光指向儀用于在所述激光靶面上形成所述光斑；

根據所述第一位移矢量、所述第二位移矢量以及圖像采集裝置坐標系相對捷聯慣導殼體坐標系的第一投影關系，確定所述圖像采集裝置坐標系相對捷聯慣導的安裝矩陣，其中，所述捷聯慣導設于所述掘進機上；

通過所述安裝矩陣確定所述圖像采集裝置坐標系相對所述捷聯慣導的安裝角，以標定所述捷聯慣導在巷道內的位姿信息。

2.根據權利要求1所述的方法，其特征在于，根據所述第一位移矢量、所述第二位移矢量以及圖像采集裝置坐標系相對捷聯慣導殼體坐標系的第一投影關系，確定所述圖像采集裝置坐標系相對捷聯慣導的安裝矩陣，包括：

基于雙目視覺算法確定所述第一位移矢量在所述圖像采集裝置坐標系內的第一投影矢量，以及所述第二位移矢量在所述圖像采集裝置坐標系內的第二投影矢量，其中，所述圖像采集裝置坐標系是基于所述第一圖像采集裝置或第二圖像采集裝置的中心位置為原點、光軸方向為z軸、焦平面且與z垂直方向為x軸構建的空間直角坐標系；

確定所述第一位移矢量在所述捷聯慣導殼體坐標系內的第三投影矢量，以及所述第二位移矢量在所述捷聯慣導殼體坐標系內的第四投影矢量，其中，所述捷聯慣導殼體坐標系是以所述捷聯慣導的殼體為原點、掘進機的前進方向為y軸、掘進機豎直面且與y軸豎直方向為z軸構建的空間直角坐標系；

根據所述第一投影矢量、所述第二投影矢量、所述第三投影矢量、所述第四投影矢量和所述第一投影關系，計算所述安裝矩陣。

3.根據權利要求2所述的方法，其特征在于，確定所述第一位移矢量在所述捷聯慣導殼體坐標系內的第三投影矢量，以及所述第二位移矢量在所述捷聯慣導殼體坐標系內的第四投影矢量，包括：

獲取所述第一圖像采集裝置相對激光測距裝置的第三位移矢量；以及，獲取所述第二圖像采集裝置相對所述激光測距裝置的第四位移矢量；以及，獲取所述激光測距裝置相對所述光斑的第五位移矢量，其中，所述激光測距裝置設于所述巷道內，位于所述第一圖像采集裝置、所述第二圖像采集裝置和所述激光靶之間；

分別測量所述第三位移矢量、所述第四位移矢量和所述第五位移矢量在巷道坐標系內對應的第五投影矢量、第六投影矢量和第七投影矢量；

根據所述第五投影矢量、所述第六投影矢量、所述第七投影矢量、地理坐標系相對所述巷道坐標系的第二投影關系、所述捷聯慣導殼體坐標系相對所述地理坐標系的第三投影關系，計算所述第三投影矢量和所述第四投影矢量，其中，所述巷道坐標系是以所述激光指向儀為原點、水平面內激光發射方向為y軸、豎直方向為z軸構建的空間直角坐標系。

4.根據權利要求3所述的方法，其特征在于，根據所述第五投影矢量、所述第六投影矢量、所述第七投影矢量、地理坐標系相對所述巷道坐標系的第二投影關系、所述捷聯慣導殼體坐標系相對地理坐標系的第三投影關系，計算所述第三投影矢量和所述第四投影矢量，包括：

基于矢量合成法則，根據所述第五投影矢量、所述第六投影矢量和所述第七投影矢量計算所述第一圖像采集裝置在巷道坐標系內的第八投影矢量，以及，所述第二圖像采集裝置在巷道坐標系內的第九投影矢量；

根據所述第二投影關系和所述第三投影關系將所述第八投影矢量和所述第九投影矢量轉換至捷聯慣導殼體坐標系，得到所述第三投影矢量和所述第四投影矢量，其中，所述第二投影關系和所述第三投影關系為已知量。

5.根據權利要求2所述的方法，其特征在于，在根據所述第一投影矢量、所述第二投影矢量、所述第三投影矢量、所述第四投影矢量和所述第一投影關系，計算所述安裝矩陣之后，所述方法還包括：

分別對所述第一投影矢量、所述第二投影矢量、所述第三投影矢量和所述第四投影矢量進行歸一化處理；

根據歸一化處理結果構造所述第一圖像采集裝置的第一歸一化矢量和所述第二圖像采集裝置的第二歸一化矢量，其中，所述歸一化處理結果均為單位正交矩陣；

根據所述第一歸一化矢量和所述第二歸一化矢量標定所述安裝矩陣。