本發(fā)明涉及機(jī)器人，特別是涉及一種利用地面激光雷達(dá)點(diǎn)云輔助的無人機(jī)集群分布式協(xié)同定位方法。

背景技術(shù)：

1、單目視覺慣性里程計(jì)(vio)技術(shù)是結(jié)合單目攝像機(jī)圖像與慣性測(cè)量單元(imu)數(shù)據(jù)實(shí)現(xiàn)實(shí)時(shí)定位和姿態(tài)估計(jì)的綜合技術(shù)。它可幫助無人機(jī)和機(jī)器人在缺乏gps信號(hào)的復(fù)雜環(huán)境中無需人工參與而自主移動(dòng)完成既定任務(wù)。單目vio能夠顯著提升移動(dòng)設(shè)備的自主化和智能化水平。單目vio系統(tǒng)的搭建依賴于高效的圖像處理算法和精確的慣性數(shù)據(jù)融合，需要完成環(huán)境感知、數(shù)據(jù)同步和軌跡估計(jì)等功能，并盡可能實(shí)現(xiàn)跨平臺(tái)的通用性和實(shí)時(shí)性。

2、同步定位與地圖構(gòu)建(slam)是一種技術(shù)，通過在未知環(huán)境中使用傳感器(如激光雷達(dá)、相機(jī)、imu等)實(shí)時(shí)估計(jì)機(jī)器人或設(shè)備的位置，同時(shí)構(gòu)建出該環(huán)境的地圖。slam技術(shù)能夠使機(jī)器人在不依賴預(yù)先構(gòu)建地圖的情況下自主導(dǎo)航，有效應(yīng)對(duì)動(dòng)態(tài)和復(fù)雜的環(huán)境變化，并廣泛應(yīng)用于無人駕駛、無人機(jī)、服務(wù)機(jī)器人等領(lǐng)域。

3、因子圖優(yōu)化是一種將多變量問題建模為圖形結(jié)構(gòu)并通過全局優(yōu)化求解的方法。它通過將位姿和觀測(cè)值表示為節(jié)點(diǎn)，將約束表示為邊，形成一個(gè)因子圖，從而有效地解決機(jī)器人定位與地圖構(gòu)建(slam)中的大規(guī)模優(yōu)化問題。因子圖優(yōu)化能夠處理復(fù)雜的多維度數(shù)據(jù)關(guān)系，提高估計(jì)的精度和魯棒性，并廣泛應(yīng)用于視覺slam、激光slam等領(lǐng)域。

4、分布式位姿估計(jì)是一種在機(jī)器人群體中常用的方法，旨在無需集中控制即可實(shí)現(xiàn)多個(gè)機(jī)器人之間的位姿推算和數(shù)據(jù)共享。在這種方法中，每個(gè)機(jī)器人獨(dú)立進(jìn)行本地傳感器數(shù)據(jù)的處理，并通過有限的通信與其他機(jī)器人交換信息，從而實(shí)現(xiàn)全局一致的位姿估計(jì)。這種分布式的架構(gòu)提高了系統(tǒng)的魯棒性和可擴(kuò)展性，即使部分機(jī)器人出現(xiàn)故障，整個(gè)系統(tǒng)仍能正常工作。同時(shí)，這種方法減少了對(duì)集中計(jì)算資源的依賴，使得系統(tǒng)在動(dòng)態(tài)和復(fù)雜的環(huán)境中表現(xiàn)出更高的靈活性和適應(yīng)性。

5、一種基于視覺的協(xié)同同步定位與地圖構(gòu)建(cslam)方法，通過多機(jī)器人共享視覺信息來實(shí)現(xiàn)環(huán)境感知和地圖構(gòu)建。該方法利用每個(gè)機(jī)器人搭載的攝像頭獲取環(huán)境圖像，通過視覺slam算法獨(dú)立提取圖像特征點(diǎn)并構(gòu)建局部地圖。然后，機(jī)器人之間通過無線通信網(wǎng)絡(luò)共享這些特征點(diǎn)和地圖數(shù)據(jù)，通過全局優(yōu)化算法進(jìn)行協(xié)同地圖構(gòu)建和位姿估計(jì)。首先，每個(gè)機(jī)器人利用視覺slam算法從圖像中提取特征點(diǎn)并生成局部地圖；接著，通過無線通信將這些數(shù)據(jù)共享給其他機(jī)器人，并使用全局優(yōu)化算法整合多個(gè)局部地圖，得到一致的全局地圖。該方法提高了多機(jī)器人系統(tǒng)的環(huán)境感知能力和地圖構(gòu)建精度，顯著提升了運(yùn)算效率和定位準(zhǔn)確性，在復(fù)雜和動(dòng)態(tài)環(huán)境中具有較好的適應(yīng)性和穩(wěn)定性。然而，由于各機(jī)器人獨(dú)立構(gòu)建地圖和提取特征點(diǎn)，初始誤差可能在全局優(yōu)化過程中被放大，需要精確的通信和同步機(jī)制來保證整體性能?；谝曈X的協(xié)同同步定位與地圖構(gòu)建(cslam)方法通常會(huì)在軌跡較長時(shí)面臨誤差積累和漂移的問題，同時(shí)難以獲取在環(huán)境中的絕對(duì)位姿。

6、一種基于gps或rtk的協(xié)同同步定位與地圖構(gòu)建(cslam)方法，通過多機(jī)器人共享高精度定位信息來實(shí)現(xiàn)環(huán)境感知和地圖構(gòu)建。每個(gè)機(jī)器人配備的gps或rtk模塊能夠獲取精確的位置信息，隨后這些定位數(shù)據(jù)和地圖信息通過無線通信網(wǎng)絡(luò)在機(jī)器人之間共享。協(xié)同地圖構(gòu)建和位姿估計(jì)則依賴于全局優(yōu)化算法。具體而言，首先每個(gè)機(jī)器人利用其gps或rtk模塊獲取高精度位置信息并生成局部地圖；然后，這些數(shù)據(jù)通過無線通信共享給其他機(jī)器人，并通過全局優(yōu)化算法整合多個(gè)局部地圖，從而構(gòu)建全局地圖?；趃ps或rtk的協(xié)同同步定位與地圖構(gòu)建(cslam)方法高度依賴gps或rtk信號(hào)，該方法無法在gps信號(hào)不好的野外復(fù)雜環(huán)境或室內(nèi)環(huán)境中正常工作。

7、需要說明的是，在上述背景技術(shù)部分公開的信息僅用于對(duì)本技術(shù)的背景的理解，因此可以包括不構(gòu)成對(duì)本領(lǐng)域普通技術(shù)人員已知的現(xiàn)有技術(shù)的信息。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明的主要目的在于解決上述背景技術(shù)中存在的問題，提供一種利用地面激光雷達(dá)點(diǎn)云輔助的無人機(jī)集群分布式協(xié)同定位方法。

2、為實(shí)現(xiàn)上述目的，本發(fā)明采用以下技術(shù)方案：

3、在本發(fā)明第一方面，一種利用地面激光雷達(dá)點(diǎn)云輔助的無人機(jī)集群分布式協(xié)同定位方法，包括以下步驟：

4、步驟一：使用無人機(jī)搭載的單目相機(jī)和imu，通過視覺特征點(diǎn)的提取和追蹤，結(jié)合imu的預(yù)積分?jǐn)?shù)據(jù)，實(shí)現(xiàn)無人機(jī)的初始位姿估計(jì)；

5、步驟二：地面無人車通過激光雷達(dá)實(shí)時(shí)獲取周圍環(huán)境的點(diǎn)云數(shù)據(jù)，并結(jié)合imu數(shù)據(jù)，采用fast-lio算法，通過計(jì)算點(diǎn)到面殘差作為系統(tǒng)觀測(cè)，并利用迭代誤差狀態(tài)卡爾曼濾波ieskf，實(shí)時(shí)構(gòu)建無人車的位姿估計(jì)和稠密三維點(diǎn)云地圖；

6、步驟三：結(jié)合無人機(jī)的稀疏特征點(diǎn)云和無人車的稠密點(diǎn)云數(shù)據(jù)，構(gòu)建因子圖優(yōu)化問題求解待優(yōu)化變量，包括相對(duì)位姿變量、相機(jī)相關(guān)變量和視覺特征變量，以實(shí)現(xiàn)無人機(jī)集群中各無人機(jī)的實(shí)時(shí)相對(duì)位姿估計(jì)；

7、步驟四：利用基于點(diǎn)面匹配的點(diǎn)云殘差因子、視覺特征點(diǎn)重投影殘差因子和imu預(yù)積分殘差因子，形成無人機(jī)集群系統(tǒng)的因子圖，每架無人機(jī)接收激光雷達(dá)構(gòu)建的環(huán)境信息，并發(fā)布位姿信息，實(shí)現(xiàn)基于共享激光雷達(dá)地圖的實(shí)時(shí)六自由度位姿估計(jì)；

8、步驟五：在無人機(jī)集群中，每臺(tái)無人機(jī)獨(dú)立運(yùn)行分布式位姿估計(jì)模塊，使用層級(jí)式ba對(duì)輸入的激光雷達(dá)稠密點(diǎn)云和相機(jī)稀疏特征點(diǎn)云進(jìn)行配準(zhǔn)并計(jì)算殘差，通過最小化因子圖中的殘差，實(shí)現(xiàn)無人機(jī)集群中各無人機(jī)之間的實(shí)時(shí)相對(duì)位姿的精確求解；

9、步驟六：進(jìn)行無人機(jī)集群與無人車之間的位姿信息和環(huán)境地圖數(shù)據(jù)的實(shí)時(shí)同步。

10、在本發(fā)明第二方面，一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，存儲(chǔ)有計(jì)算機(jī)程序，其特征在于，所述計(jì)算機(jī)程序由處理器執(zhí)行時(shí)實(shí)現(xiàn)所述的利用地面激光雷達(dá)點(diǎn)云輔助的無人機(jī)集群分布式協(xié)同定位方法。

11、在本發(fā)明第三方面，一種計(jì)算機(jī)程序產(chǎn)品，包括計(jì)算機(jī)程序，所述計(jì)算機(jī)程序由處理器執(zhí)行時(shí)實(shí)現(xiàn)所述的利用地面激光雷達(dá)點(diǎn)云輔助的無人機(jī)集群分布式協(xié)同定位方法。

12、本發(fā)明具有如下有益效果：

13、本發(fā)明提供了一種創(chuàng)新的無人機(jī)集群分布式協(xié)同定位方法，該方法有效結(jié)合了地面激光雷達(dá)點(diǎn)云與無人機(jī)搭載的單目相機(jī)和imu數(shù)據(jù)，實(shí)現(xiàn)了在非結(jié)構(gòu)化環(huán)境下的魯棒且實(shí)時(shí)的位姿估計(jì)。通過構(gòu)建基于點(diǎn)面匹配的點(diǎn)云殘差因子、視覺特征點(diǎn)重投影殘差因子和imu預(yù)積分殘差因子，本發(fā)明優(yōu)化了因子圖并提高了位姿估計(jì)的精度，減少了長軌跡中的漂移問題。此外，本發(fā)明的分布式架構(gòu)允許多架無人機(jī)共享同一激光雷達(dá)地圖并獨(dú)立求解實(shí)時(shí)位姿，不依賴先驗(yàn)的環(huán)境地圖信息，能夠快速且魯棒地估計(jì)初始位姿，并利用環(huán)境信息實(shí)時(shí)校正視覺-慣性里程計(jì)的位姿估計(jì)漂移誤差。這種方法特別適用于災(zāi)后搜救、大區(qū)域地圖覆蓋性探索和復(fù)雜地形協(xié)同導(dǎo)航等場(chǎng)景，能夠顯著提高執(zhí)行任務(wù)的效率和效果，實(shí)現(xiàn)對(duì)環(huán)境更加全面的覆蓋和更加高效的搜索。同時(shí)，本發(fā)明解決了傳統(tǒng)基于視覺的cslam方法在軌跡較長時(shí)面臨的誤差積累和漂移問題，以及基于gps或rtk的cslam方法在gps信號(hào)不佳環(huán)境中的局限性。

14、本發(fā)明實(shí)施例中的其他有益效果將在下文中進(jìn)一步述及。