基于視覺的多四旋翼無人機(jī)協(xié)同slam的方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于無人機(jī)技術(shù)領(lǐng)域，尤其涉及一種基于視覺的多四旋翼無人機(jī)協(xié)同SLAM 方法。
【背景技術(shù)】
[0002] 無人機(jī)現(xiàn)已廣泛應(yīng)用于軍事及民用等眾多領(lǐng)域。在軍事上，無人機(jī)可完成空中預(yù) 警、偵察監(jiān)視、通訊中繼及小范圍內(nèi)攻擊等任務(wù)。在民用方面，無人機(jī)可以完成環(huán)境監(jiān)測(cè)、遙 感探測(cè)、災(zāi)區(qū)搜救、農(nóng)藥噴灑等任務(wù)。無人機(jī)位姿的準(zhǔn)確估計(jì)是無人機(jī)自主安全飛行的基 礎(chǔ)。目前普遍使用的定位方式是GPS定位，但在某些特殊環(huán)境下，GPS信號(hào)會(huì)缺失或受到干 擾，需要不依賴GPS的定位方法?；谝曈X的SLAM方法使用機(jī)載攝像頭采集圖像信息，慣 性導(dǎo)航單元獲取無人機(jī)的姿態(tài)角和加速度信息，是無人機(jī)自主飛行定位的一個(gè)重要研究方 向。
[0003] 四旋翼無人機(jī)是一種有四個(gè)螺旋槳的無人飛行器，依靠改變螺旋槳的速度來實(shí)現(xiàn) 飛行的各種姿態(tài)，具有結(jié)構(gòu)簡單，體型小巧，控制靈活等優(yōu)勢(shì)，適合用于室內(nèi)及復(fù)雜環(huán)境。在 復(fù)雜環(huán)境下，單架無人機(jī)在準(zhǔn)確率和效率上很難滿足要求，可采用多架無人機(jī)相互輔助，共 同完成任務(wù)，即協(xié)同。多無人機(jī)協(xié)同SLAM利用無人機(jī)之間的相對(duì)觀測(cè)獲得的距離和相對(duì)方 位信息，進(jìn)行資源共享和定位修正，從而能夠獲得比單架無人機(jī)定位更精確的結(jié)果。
[0004] 目前SLAM技術(shù)已廣泛應(yīng)用于地面移動(dòng)機(jī)器人領(lǐng)域，水下機(jī)器人與無人機(jī)領(lǐng)域也 有了一定的研究成果?；诰植孔拥貓D等思想，在地面移動(dòng)機(jī)器人的二維空間已經(jīng)進(jìn)行了 多機(jī)器人協(xié)同SLAM的研究，而在無人機(jī)活動(dòng)的三維空間內(nèi)，協(xié)同SLAM技術(shù)的研究工作還有 待進(jìn)一步展開，將是未來國內(nèi)外一個(gè)重要研究方向。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明所要解決的技術(shù)問題是針對(duì)【背景技術(shù)】中所涉及到的缺陷，提供一種基于視 覺的多四旋翼無人機(jī)協(xié)同SLAM方法。
[0006] 本發(fā)明為解決上述技術(shù)問題采用以下技術(shù)方案：
[0007] 基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法，包含以下步驟：
[0008] 步驟1)，在四旋翼無人機(jī)集群中選取一架四旋翼無人機(jī)作為主無人機(jī)，其余作為 輔助無人機(jī)；
[0009] 步驟2)，利用慣性導(dǎo)航單元獲取主無人機(jī)的姿態(tài)角和加速度信息，計(jì)算主無人機(jī) 在地面坐標(biāo)系下的位置；
[0010] 步驟3)，輔助無人機(jī)觀察主無人機(jī)的飛行變化，根據(jù)輔助無人機(jī)在地面坐標(biāo)系下 的位置及其與主無人機(jī)的相對(duì)位置計(jì)算得到主無人機(jī)在地面坐標(biāo)系下的位置變化量；
[0011] 步驟4)，根據(jù)步驟3)中得到的主無人機(jī)在地面坐標(biāo)系下的位置變化量對(duì)主無人 機(jī)和路標(biāo)位置進(jìn)行校正；
[0012] 步驟5)，當(dāng)主無人機(jī)將與輔助無人機(jī)之間的距離超過預(yù)設(shè)的第一距離值時(shí)，輔助 無人機(jī)調(diào)整位置接近主無人機(jī)，使得其與主無人機(jī)之間的距離為預(yù)設(shè)的第二距離值；
[0013]步驟6)，當(dāng)主無人機(jī)產(chǎn)生故障時(shí)，選取距離主無人機(jī)最近的輔助無人機(jī)作為新的 主無人機(jī)，繼續(xù)完成SLAM任務(wù)。
[0014] 作為本發(fā)明基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法進(jìn)一步的優(yōu)化方案，所述 步驟3)根據(jù)以下公式計(jì)算主無人機(jī)在地面坐標(biāo)系下的位置變化量：
[0015]
[0016] 式中，％= (P, , ，）'、弋=(/Vt爐/.)'分別為輔助無人機(jī)i在t_l時(shí)刻和t 時(shí)刻觀測(cè)與主無人機(jī)之間的相對(duì)位置，i=1，2,. . .，N-1 ;
[0017] N-1為輔助無人機(jī)的數(shù)量；
[0018] At為t-1時(shí)刻和t時(shí)刻之間的間隔時(shí)間；
[0019] AXi為輔助無人機(jī)i觀測(cè)到的主無人機(jī)在地面坐標(biāo)系下的位置變化量。
[0020] 作為本發(fā)明基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法進(jìn)一步的優(yōu)化方案，所述 步驟4)根據(jù)以下公式對(duì)主無人機(jī)位置進(jìn)行校正：
[0021]
[0022] 式中，足、為q分別為主無人機(jī)在t時(shí)刻和t_l時(shí)刻的校正位置，xt為主無人機(jī)t 時(shí)刻根據(jù)慣性導(dǎo)航單元得到的自身位置。
[0023] 作為本發(fā)明基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法進(jìn)一步的優(yōu)化方案，所述 步驟5)的詳細(xì)步驟如下：
[0024] 若在t時(shí)刻，輔助無人機(jī)i與主無人機(jī)之間的相對(duì)距離超過攝像頭最大觀測(cè)距離 的4/5,則輔助無人機(jī)i首先向主無人機(jī)發(fā)送暫停信號(hào)，然后根據(jù)該時(shí)刻得到的相對(duì)位置信 息x, = (A舊/，沿最短距離迅速飛行至距主無人機(jī)最大觀測(cè)距離1/5處懸停，向主無 人機(jī)發(fā)送繼續(xù)執(zhí)行信號(hào)。
[0025] 作為本發(fā)明基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法進(jìn)一步的優(yōu)化方案，步驟 6)的詳細(xì)步驟如下：
[0026] 主無人機(jī)產(chǎn)生故障時(shí)，比較該t時(shí)刻各輔助無人機(jī)對(duì)于主無人機(jī)的相對(duì)距離巧， 選取輔助無人機(jī)n作為新的主無人機(jī)，A, =minp,…= 1，2，一，#-1，迅速移至原主無人機(jī) 位置，繼續(xù)完成SLAM任務(wù)，其余輔助無人機(jī)任務(wù)保持不變。
[0027] 本發(fā)明采用以上技術(shù)方案與現(xiàn)有技術(shù)相比，具有以下技術(shù)效果：
[0028]本發(fā)明將N架四旋翼無人機(jī)中的一架作為主無人機(jī)，其余N-1架作為輔助無人機(jī)， 輔助無人機(jī)保持懸停狀態(tài)，與路標(biāo)一起估計(jì)和更新，通過觀察與主無人機(jī)的相對(duì)位置計(jì)算 主無人機(jī)在地面坐標(biāo)系下的絕對(duì)位置，有助于主無人機(jī)實(shí)時(shí)調(diào)整位姿估計(jì)，提高無人機(jī)位 姿估計(jì)及路標(biāo)位置估計(jì)的準(zhǔn)確性；另外，在主無人機(jī)產(chǎn)生意外無法完成SLAM的情況下，輔 助無人機(jī)能迅速替補(bǔ)，繼續(xù)執(zhí)行任務(wù)，具有良好的機(jī)動(dòng)性。
【附圖說明】
[0029] 圖1是本發(fā)明基于視覺的多四旋翼無人機(jī)SLAM方法流程圖；
[0030] 圖2是本發(fā)明中輔助無人機(jī)i觀測(cè)主無人機(jī)相對(duì)位置示意圖；
[0031] 圖3是主無人機(jī)與輔助無人機(jī)間通信過程流程圖。
【具體實(shí)施方式】
[0032] 下面結(jié)合附圖對(duì)本發(fā)明的技術(shù)方案做進(jìn)一步的詳細(xì)說明：
[0033] 如圖1所示，本發(fā)明基于視覺的多四旋翼協(xié)同SLAM方法如下：
[0034] 選取一架四旋翼無人機(jī)UAV1作為主無人機(jī)，其余兩架四旋翼無人機(jī)UAV2和UAV3 作為輔助無人機(jī)，并確保UAV2和UAV3在UAV1的觀測(cè)范圍以內(nèi)。三架四旋翼無人機(jī)均攜帶 有攝像頭和慣性導(dǎo)航單元。
[0035] UAV1進(jìn)行SLAM任務(wù)，路標(biāo)的總個(gè)數(shù)為M，將UAV2和UAV3也作為路標(biāo)進(jìn)行估計(jì)和 更新，利用機(jī)載攝像機(jī)采集圖像信息，利用機(jī)載慣性導(dǎo)航單元獲取無人機(jī)的姿態(tài)角和加速 度信息。公式（1)為主無人機(jī)SLAM的聯(lián)合概率分布
[0036]
(