[發明專利]基于視覺和慣性導航系統的井下移動裝備定位方法及系統

在審

• 申請號: 202011498005.3
• 申請日: 2020-12-17
• 公開（公告）號: CN112697131A
• 公開（公告）日: 2021-04-23
• 發明（設計）人: 司壘;趙世豪;王忠賓;譚超;閆海峰;劉送永;江紅祥;許少毅
• 申請（專利權）人: 中國礦業大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16;G01C21/20
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 田凌濤

1.一種基于視覺和慣性導航系統的井下移動裝備定位方法，其特征在于，基于按照預設位置安裝在目標移動裝備上、且基線方向與目標移動裝備的運行方向相同的雙目相機，以及按照預設位置安裝在目標移動裝備上的慣性導航定位裝置，實現對目標移動裝備的定位，其中慣性導航定位裝置包括至少一個慣性單元，各慣性單元上分別均包括用于檢測慣性單元在三個預設方向上的三軸角速度的三軸陀螺儀，以及用于檢測慣性單元在三個預設方向上的三軸加速度的三軸加速度計；

定位方法包括如下步驟：

步驟1、針對目標移動設備：以其運動起始位置為原點，建立三維的導航坐標系；

針對慣性導航定位裝置：以目標移動設備的重心為原點，建立三維的載體坐標系；并根據三維的導航坐標系和三維的載體坐標系，獲取載體坐標系到導航坐標系的載體-導航姿態轉移矩陣；

根據慣性導航定位裝置中各慣性單元檢測的三軸角速度和三軸加速度，分別獲取慣性導航定位裝置在載體坐標系下的角速度ω和加速度f；結合載體-導航姿態轉移矩陣，對慣性導航定位裝置在載體坐標系下的角速度和加速度進行姿態矩陣解算，分別獲取慣性導航定位裝置在導航坐標系下的位置坐標和角度坐標；

步驟2：針對雙目相機：根據雙目相機所捕獲的圖像，使用ORB算法提取圖像中立體特征點的特征點信息，結合特征點匹配算法和立體視覺算法，獲取雙目相機的位置坐標和角度坐標；

步驟3、根據預設的擬合權重值，結合慣性導航定位裝置在導航坐標系下的角度坐標和雙目相機的角度坐標，獲取目標移動設備的角度坐標；根據預設的權重值，結合慣性導航定位裝置在導航坐標系下的位置坐標和雙目相機的位置坐標，獲取目標移動設備的位置坐標。

2.根據權利要求1所述的基于視覺和慣性導航系統的井下移動裝備定位方法，其特征在于，各慣性單元分別均包括：設置在載體坐標系的原點處的第一慣性單元5，以及分別設置在載體坐標系的三個坐標軸上且距離載體坐標系原點的距離彼此相等的第二慣性單元6、第三慣性單元7、第四慣性單元8。

3.根據權利要求2所述的基于視覺和慣性導航系統的井下移動裝備定位方法，其特征在于，在步驟1中，根據如下公式：

慣性導航定位裝置在載體坐標系下的角速度ω；

其中，ω_x、ω_y、ω_z分別為慣性導航定位裝置的角速度ω在載體坐標系的x軸、y軸、z軸方向的分量；ω_x1、ω_x2、ω_x3、ω_x4分別為第一慣性單元5、第二慣性單元6、第三慣性單元7、第四慣性單元8的角速度在載體坐標系的x軸方向的分量；ω_y1、ω_y2、ω_y3、ω_y4分別為第一慣性單元5、第二慣性單元6、第三慣性單元7、第四慣性單元8的角速度在載體坐標系的y軸方向的分量；ω_Z1、ω_Z2、ω_Z3、ω_Z4分別為第一慣性單元5、第二慣性單元6、第三慣性單元7、第四慣性單元8的角速度在載體坐標系的z軸方向的分量；a、b、c、d分別為第一慣性單元5、第二慣性單元6、第三慣性單元7、第四慣性單元8的預設角速度權重值。