【技術實現步驟摘要】
本專利技術涉及一種面向多無人機協同定位的uwb與視覺融合定位方法及系統，屬于導航定位。
技術介紹
1、無人系統近年來獲得不斷進步，其具有在無人操控的場景下完成自身的移動并完成相應任務的能力，如環境勘探、災后救援、自動駕駛等。無人機作為無人系統的重要組成部分，包含眾多種類，按照飛行方式可分為固定翼無人機、旋翼無人機等，其中四旋翼無人機由于其具有成本低、小型化、動力學建模簡單、高機動能力等特性，使其成為在室內或室外環境中廣泛應用的理想的機器人平臺，一直是學者研究的重點。無人機飛行通常通過全球導航衛星系統(global?navigation?satellite?system，gnss)進行定位，使無人機的室外大量應用成為可能。然而真實環境通常復雜且未知，并經常出現gnss信號不佳甚至消失的情況，如室內、走廊、叢林等環境，在上述情況下，gnss無法提供周圍的環境信息，使其無法應用于混亂環境或室內密閉環境中的保障飛行。因此四旋翼無人機需要通過其它傳感器在無外界定位信號的情況下實現實時自主定位與建圖(simultaneous?localization?andmapping，slam)技術。
2、為了使slam技術應用在不同的場景下，產生了很多在無gnss的情況下使用不同傳感器的自主定位方案，如相機、多線激光雷達、紅外傳感器、毫米波雷達、慣性測量單元(inertial?measurement?unit，imu)等。相機、激光雷達等機載傳感器通常用于獲取空間中的結構化信息，然后通過獲取的信息求解傳感器自身相對于空間信息的位姿變化。由
3、近年來，由于多無人機系統在戰術圍捕、搜救等方面的巨大前景，導致多無人機協作成為當下熱門的研究主題。在執行協同任務時，為了達到編隊飛行、避障等目標，無人機不僅需要明確自身狀態，還需要對系統內其他個體的位置進行估計。
4、通常部署uwb錨點構成全局參考系g，以多傳感器融合的方法估計無人機位姿，這種方法延伸至多無人機系統依然適用，無人機只需共享信息即可獲取同伴的位置，但是這種依賴錨點的方法限制了多無人機的覆蓋范圍，不能即刻應用于未知場景。
技術實現思路
1、本專利技術的目的在于克服現有技術中的不足，提供一種解決在gnss拒止環境下面向多無人機協同定位的uwb與視覺融合定位方法及系統，
2、為達到上述目的，本專利技術是采用下述技術方案實現的：
3、第一方面，本專利技術提供了一種面向多無人機協同定位的uwb與視覺融合定位方法，包括以下步驟：
4、步驟1：獲取uwb、imu、雙目相機和定高雷達數據；
5、步驟2：通過無跡卡爾曼濾波器融合uwb、imu和定高雷達數據，估計出無人機在全局坐標系下的位姿；
6、步驟3：融合imu和雙目相機做數據構成視覺慣性里程計,估計無人機慣性系下的位姿，實現視覺定位，輸出得到無人機位姿和環境路標點，同時創建和廣播關鍵幀消息。
7、步驟4：構建位姿優化圖，采用全局一元位姿邊表示uwb定位測量結果，采用二元增量邊的形式表示視覺慣性里程計測量的位姿結果，計算代價函數的最小值，求出位姿狀態矢量的最優估計。
8、進一步的，步驟3：系統中的每個無人機創建和廣播關鍵幀消息以支持后續的協同定位工作，包括：
9、為開辟特征提取線程，根據平均視差和特征跟蹤的質量提取相機拍攝圖像幀的關鍵幀，當捕獲最新的關鍵幀時，執行以下步驟：
10、①創建新的關鍵幀消息
11、②提取1000個特征并計算對應的brief描述子，將它們寫入關鍵幀消息
12、③將里程計恢復出3d路標點并將其對應的描述子寫入關鍵幀消息
13、④將位姿估計矩陣寫入關鍵幀消息
14、⑤向網絡中廣播關鍵幀消息
15、關鍵幀消息包含豐富的視覺信息和運動信息，視覺信息用于查找無人機之間的回環，從而形成回環觀測，運動信息用于計算位姿增量，從而形成里程計增量觀測。
16、進一步的，步驟4：構建位姿優化圖，采用全局一元位姿邊表示uwb定位測量結果，采用二元增量邊的形式表示視覺慣性里程計測量的位姿結果，計算代價函數的最小值，求出位姿狀態矢量的最優估計，包括：
17、當無人機采集到最新的關鍵幀，它的里程計特征提取線程會向系統內廣播關鍵幀信息，詢問其他個體是否經歷過類似場景，其他個體的回環檢測程序會進行場景識別，從圖像數據庫中篩選出回環候選幀，向詢問方答復。
18、一旦詢問方收到答復將計算兩無人機相對位姿，這個相對位姿將作為協同定位算法的回環觀測送入優化器構建位姿圖。采用詞袋模型技術快速比較兩幀圖像的特征向量，然后使用結合ransac方法的pnp計算相對位姿；
19、將所述兩無人機相對位姿送入優化器構建位姿圖，以構建和求解位姿圖的方式實時優化和對齊系統內所有個體的位姿。
20、進一步的，所述回環檢測程序包括：
21、首先利用詞袋模型技術判斷是否兩幀圖像包含類似的場景，從而形成互回環；
22、采用kmeans++算法構造字典，對于每一張字典訓練圖像，圖像特征點使用brief描述子表征，kmeans++算法對描述子進行k中值聚類，把描述子空間離散化為k個二進制聚類，這些聚類形成字典樹中的第一層節點，然后對第一層中每個節點相關聯的描述子重復聚類操作再聚成k類得到第二層節點，這個步驟的遞歸深度為d。最后會得到一個擁有n個葉子節點的樹，每一個葉子節點即為視覺詞匯。
23、使用頻率—逆文檔頻率分配視覺詞匯權重。tf認為視覺詞匯在一幅圖像中出現的頻率越高，這個詞匯的區分度就高，應該加大權重，因此tf可表達為：
24、
25、其中mi表示該詞匯ωi在一幅圖像中出現的次數，m為圖像中的詞匯總數。
26、idf認本文檔來自技高網...
【技術保護點】
1.一種面向多無人機協同定位的UWB與視覺融合定位方法，其特征在于，包括以下步驟：
2.根據權利要求1所述的面向多無人機協同定位的UWB與視覺融合定位方法，其特征在于，系統中的每個無人機創建和廣播關鍵幀消息以支持后續的協同定位工作，包括：
3.根據權利要求1所述的面向多無人機協同定位的UWB與視覺融合定位方法，其特征在于，構建位姿優化圖，采用全局一元位姿邊表示UWB定位測量結果，采用二元增量邊的形式表示視覺慣性里程計測量的位姿結果，計算代價函數的最小值，求出位姿狀態矢量的最優估計，包括：
4.根據權利要求3所述的面向多無人機協同定位的UWB與視覺融合定位方法，其特征在于，所述回環檢測程序包括：
5.根據權利要求3所述的面向多無人機協同定位的UWB與視覺融合定位方法，其特征在于，無人機在交換信息時的通訊策略包括：
6.根據權利要求5所述的面向多無人機協同定位的UWB與視覺融合定位方法，其特征在于，執行帶RANSAC算法的PnP求解出進而推導出二者的相對位姿αTβ，包括：
7.根據權利要求3所述的面向多無人機協同
8.根據權利要求1所述的面向多無人機協同定位的UWB與視覺融合定位方法，其特征在于，通過無跡卡爾曼濾波器融合UWB、IMU和定高雷達數據，估計出無人機在全局坐標系下的位姿，包括：
9.根據權利要求1所述的面向多無人機協同定位的UWB與視覺融合定位方法，其特征在于，系統中的每個無人機采用基于滑動窗口的緊耦合視覺慣性里程計完成自主定位，包括：
10.一種面向多無人機協同定位的UWB與視覺融合定位系統，包括：
...【技術特征摘要】
1.一種面向多無人機協同定位的uwb與視覺融合定位方法，其特征在于，包括以下步驟：
2.根據權利要求1所述的面向多無人機協同定位的uwb與視覺融合定位方法，其特征在于，系統中的每個無人機創建和廣播關鍵幀消息以支持后續的協同定位工作，包括：
3.根據權利要求1所述的面向多無人機協同定位的uwb與視覺融合定位方法，其特征在于，構建位姿優化圖，采用全局一元位姿邊表示uwb定位測量結果，采用二元增量邊的形式表示視覺慣性里程計測量的位姿結果，計算代價函數的最小值，求出位姿狀態矢量的最優估計，包括：
4.根據權利要求3所述的面向多無人機協同定位的uwb與視覺融合定位方法，其特征在于，所述回環檢測程序包括：
5.根據權利要求3所述的面向多無人機協同定位的uwb與視覺融合定位方法，其特征在于，無人機在交換信息時的通訊策略包括：
【專利技術屬性】
技術研發人員：王聰，何雷，馬文峰，田輝，
申請(專利權)人：中國人民解放軍陸軍工程大學，
類型：發明
國別省市：
還沒有人留言評論。發表了對其他瀏覽者有用的留言會獲得科技券。