[發(fā)明專利]一種多無人機(jī)協(xié)作SLAM方法與系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 202010534426.0
• 申請(qǐng)日: 2020-06-12
• 公開（公告）號(hào): CN111812978B
• 公開（公告）日: 2023-01-24
• 發(fā)明（設(shè)計(jì)）人: 楊志華;李子恒;齊曉晗;趙喜雙
• 申請(qǐng)（專利權(quán)）人: 哈爾濱工業(yè)大學(xué)（深圳）（哈爾濱工業(yè)大學(xué)深圳科技創(chuàng)新研究院）
• 主分類號(hào): G05B13/04
• 分類號(hào): G05B13/04
• 代理公司: 深圳市添源創(chuàng)鑫知識(shí)產(chǎn)權(quán)代理有限公司 44855
• 代理人: 周椿

1.一種多無人機(jī)協(xié)作SLAM方法，其特征在于，包括以下步驟：

S1、檢測(cè)無人機(jī)與地面站之間的通信是否正常,如果正常，則傳遞無人機(jī)的關(guān)鍵幀以及對(duì)應(yīng)的無人機(jī)編號(hào)；

S2、相似場(chǎng)景判斷，如果對(duì)比當(dāng)前的某一圖片與另一無人機(jī)的某時(shí)刻圖片相似，則首先計(jì)算出兩個(gè)關(guān)鍵幀對(duì)應(yīng)的相對(duì)位姿,再結(jié)合單機(jī)SLAM結(jié)果計(jì)算出相對(duì)初始位姿；

S3、將所有相似場(chǎng)景的相對(duì)初始位姿,納入到一個(gè)集合中，再進(jìn)行一個(gè)自適應(yīng)數(shù)量的聚類，并從中選擇最優(yōu)的聚類，得到關(guān)聯(lián)數(shù)據(jù)；

S4、將關(guān)聯(lián)數(shù)據(jù)與單機(jī)SLAM結(jié)果結(jié)合到一起進(jìn)行優(yōu)化,最后返回優(yōu)化后的全局地圖信息與飛行軌跡；

在步驟S2中，相似場(chǎng)景判斷包括：在單機(jī)SLAM中，無人機(jī)將實(shí)時(shí)拍到的圖片進(jìn)行ORB特征提取和進(jìn)行特征點(diǎn)的描述，將所對(duì)應(yīng)的描述通過預(yù)先訓(xùn)練好的詞袋模型轉(zhuǎn)換成對(duì)應(yīng)的單詞并記錄下來；在后續(xù)的飛行過程中，任意一無人機(jī)將每一幀新拍攝的圖片所對(duì)應(yīng)的單詞與另一無人機(jī)已經(jīng)存儲(chǔ)的單詞進(jìn)行對(duì)比，并與所設(shè)定的閾值進(jìn)行對(duì)比，如果超過該閾值，則認(rèn)為其是相似場(chǎng)景；

在步驟S2中，通過ICP算法計(jì)算出關(guān)鍵幀對(duì)應(yīng)的相對(duì)位姿，再結(jié)合單機(jī)SLAM中存儲(chǔ)的無人機(jī)的飛行軌跡信息，求得該相似場(chǎng)景所對(duì)應(yīng)的相對(duì)初始位姿；

相似場(chǎng)景所對(duì)應(yīng)的相對(duì)初始位姿通過下式求得：

上式中，運(yùn)算符號(hào)代表的是4x4的位姿所進(jìn)行的運(yùn)算，X代表單機(jī)SLAM的結(jié)果，包括地圖信息以及飛行軌跡信息，U代表了識(shí)別到相似場(chǎng)景時(shí)兩架無人機(jī)間相對(duì)位姿的集合，T代表兩架無人機(jī)的相對(duì)初始位姿；

在步驟S3中，針對(duì)所有相似場(chǎng)景的相對(duì)初始位姿,首先進(jìn)行AP聚類,然后進(jìn)行聚類選擇，得到關(guān)聯(lián)數(shù)據(jù)；

在步驟S3中，首先將相對(duì)初始位姿的數(shù)據(jù)轉(zhuǎn)換成AP算法能夠利用的度量單位：

上式表示為兩個(gè)不同時(shí)刻的相對(duì)初始位姿的距離；其中T1和T2代表不同時(shí)刻的相對(duì)初始位姿，∑代表了4x4維變換矩陣的協(xié)方差矩陣，n相對(duì)應(yīng)的取值為4；

將每個(gè)聚類的中心視為每個(gè)聚類中最具有代表性的相對(duì)初始位姿，相對(duì)應(yīng)的就有對(duì)應(yīng)數(shù)量的相對(duì)初始位姿Ti，i＝1,2,3,...n，針對(duì)這生成的n個(gè)假設(shè)，接下來的目的就是從中選擇最優(yōu)的聚類：

上式表示在已知單機(jī)SLAM結(jié)果和所有相似場(chǎng)景對(duì)應(yīng)的相對(duì)位姿的情況下，從生成的多個(gè)聚類h中選擇最優(yōu)聚類的方法；其中h_i代表不同相似場(chǎng)景生成的不同聚類，代表單機(jī)SLAM的結(jié)果，U代表了識(shí)別到相似場(chǎng)景時(shí)兩架無人機(jī)間相對(duì)位姿的集合；

上式后面的式子表示成：

上式代表了應(yīng)用貝葉斯公式來對(duì)進(jìn)行轉(zhuǎn)換求解的過程；其中h代表不同相似場(chǎng)景生成的不同聚類，代表單機(jī)SLAM的結(jié)果，U代表了識(shí)別到相似場(chǎng)景時(shí)兩架無人機(jī)間相對(duì)位姿的集合，T代表相對(duì)初始位姿；

其中，與假設(shè)無關(guān)，故不需要計(jì)算；先驗(yàn)概率用CRP過程來模擬，單獨(dú)計(jì)算即可；剩余部分則如下：

該部分代表著在某個(gè)具體聚類h以及所有的單機(jī)SLAM結(jié)果的條件下，每個(gè)相對(duì)初始位姿的概率；其中h代表不同相似場(chǎng)景生成的不同聚類，代表單機(jī)SLAM的結(jié)果，T代表相對(duì)初始位姿，T₀代表該聚類中心所對(duì)應(yīng)的相對(duì)初始位姿，Σ₀代表該聚類中心所對(duì)應(yīng)的協(xié)方差矩陣；

上式代表了在某個(gè)具體聚類h中，在已知單機(jī)SLAM結(jié)果以及此時(shí)的相對(duì)初始位姿的條件下，求出來相似場(chǎng)景所對(duì)應(yīng)的相對(duì)位姿的概率；其中h代表不同相似場(chǎng)景生成的不同聚類，代表單機(jī)SLAM的結(jié)果，U代表了識(shí)別到相似場(chǎng)景時(shí)兩架無人機(jī)間相對(duì)位姿的集合，T代表相對(duì)初始位姿，Σ_in代表某個(gè)相似場(chǎng)景在該聚類下屬于內(nèi)點(diǎn)時(shí)所對(duì)應(yīng)的協(xié)方差矩陣，Σ_out代表某個(gè)相似場(chǎng)景在該聚類下屬于外點(diǎn)時(shí)所對(duì)應(yīng)的協(xié)方差矩陣；

最后設(shè)置一個(gè)閾值C，用來判斷最后取得的最優(yōu)聚類是否為數(shù)據(jù)關(guān)聯(lián)，如果認(rèn)定為數(shù)據(jù)關(guān)聯(lián)，則將兩架無人機(jī)的數(shù)據(jù)關(guān)聯(lián)起來；

在步驟S4中，將關(guān)聯(lián)數(shù)據(jù)與單機(jī)SLAM結(jié)果結(jié)合到一起代入到一個(gè)協(xié)作SLAM的優(yōu)化公式中：

上式代表了在已知單無人機(jī)的觀測(cè)信息以及多個(gè)無人機(jī)之間所有“相遇”所對(duì)應(yīng)的相對(duì)位姿以后，求得協(xié)作SLAM最后優(yōu)化結(jié)果的過程；其中X代表了各個(gè)無人機(jī)的位姿信息，Z代表了無人機(jī)在飛行過程中的觀測(cè)信息，U代表無人機(jī)之間經(jīng)過驗(yàn)證的“相遇”時(shí)刻的相對(duì)位姿，X^*代表經(jīng)過優(yōu)化過后的各個(gè)無人機(jī)的位姿信息，r代表不同無人機(jī)，k代表在某個(gè)時(shí)刻的觀測(cè)值的數(shù)量序號(hào)，m代表觀測(cè)值的數(shù)量，i代表在飛行過程中“相遇”的序號(hào)，相對(duì)應(yīng)的N代表“相遇”的次數(shù)；

最后應(yīng)用光束平差優(yōu)化算法來對(duì)上式進(jìn)行優(yōu)化求解，即得到優(yōu)化后的地圖和位姿信息。

2.一種多無人機(jī)協(xié)作SLAM系統(tǒng)，其特征在于：包括可讀存儲(chǔ)介質(zhì)，所述可讀存儲(chǔ)介質(zhì)中存儲(chǔ)有執(zhí)行指令，所述執(zhí)行指令被處理器執(zhí)行時(shí)用于實(shí)現(xiàn)如權(quán)利要求1所述的方法。