D
[0037] 式中，xt={x"x2, ? ? ?，xj，zt={z"z2, ? ? ?，zj，ut={u"u2, ? ? ?，uj，nt= {npn2,. . .，nt}分別表示從初始時(shí)刻到t時(shí)刻主無人機(jī)的狀態(tài)量、觀測量、控制量和數(shù)據(jù)關(guān) 聯(lián)量的歷史信息集合；M= {mi，m2,. . .，mN}表示N個(gè)路標(biāo)構(gòu)成的環(huán)境地圖，各路標(biāo)之間相互 獨(dú)立;mN+1和mN+2分別表示作為路標(biāo)的輔助無人機(jī)UAV2和UAV3。
[0038] 間隔時(shí)間At前后，各輔助無人機(jī)分別觀測與主無人機(jī)之間的相對(duì)位置，如圖2所 示，得到式（2)~（5)，分別為UAV2在t-1時(shí)刻和t時(shí)刻觀測到的與主無人機(jī)UAV1的相對(duì) 位置氣4和氣以及UAV3在t-1時(shí)刻和t時(shí)刻觀測到的與主無人機(jī)UAV1的相對(duì)位置和 龍2;.
[0039]
[0043] 則UAV2和UAV3在兩次觀測后分別得到的UAV1在地面坐標(biāo)系下飛行的位置變化 A叉丄和Ax2
[0044]
[0046] UAV1根據(jù)自身慣導(dǎo)系統(tǒng)測量計(jì)算，得到t-1時(shí)刻和t時(shí)刻在地面坐標(biāo)系的位置xt : 和xt，結(jié)合UAV2和UAV3估計(jì)位置變化，修正UAV1位置，得UAVlt時(shí)刻的最終位置估計(jì)氧
[0047]
(8)
[0048] UAV1再對(duì)t-1時(shí)刻到t時(shí)刻觀察到的路標(biāo)進(jìn)行校正和更新。
[0049] 如圖3所示，若UAV2與UAV1之間的相對(duì)距離超過攝像頭最大觀測距離的4/5,則 UAV2向UAV1發(fā)送暫停信號(hào)，并迅速飛行至距UAV1最大觀測距離1/5處懸停，向UAV1發(fā)送 繼續(xù)執(zhí)行信號(hào)；若UAV3與UAV1之間的相對(duì)距離超過攝像頭最大觀測距離的4/5,同上述步 驟調(diào)整UAV3懸停位置。
[0050] 在UAV1完成SLAM任務(wù)前，重復(fù)上述估計(jì)、校正和調(diào)整步驟。若任務(wù)過程中UAV1產(chǎn) 生故障，則比較該t時(shí)刻UAV2和UAV3對(duì)于UAV1的相對(duì)距離A,和汽；若A. & 則將UAV2 作為替補(bǔ)主無人機(jī)，迅速沿咚鐵f方向飛行至原主無人機(jī)處，取代UAV1繼續(xù)完成SLAM 任務(wù)；反之，則將UAV3作為替補(bǔ)主無人機(jī)，迅速沿氣)"方向飛行至原主無人機(jī)處，取代UAV1繼續(xù)完成SLAM任務(wù)。
[0051] 本技術(shù)領(lǐng)域技術(shù)人員可以理解的是，除非另外定義，這里使用的所有術(shù)語（包括 技術(shù)術(shù)語和科學(xué)術(shù)語）具有與本發(fā)明所屬領(lǐng)域中的普通技術(shù)人員的一般理解相同的意義。 還應(yīng)該理解的是，諸如通用字典中定義的那些術(shù)語應(yīng)該被理解為具有與現(xiàn)有技術(shù)的上下文 中的意義一致的意義，并且除非像這里一樣定義，不會(huì)用理想化或過于正式的含義來解釋。
[0052] 以上所述的【具體實(shí)施方式】，對(duì)本發(fā)明的目的、技術(shù)方案和有益效果進(jìn)行了進(jìn)一步 詳細(xì)說明，所應(yīng)理解的是，以上所述僅為本發(fā)明的【具體實(shí)施方式】而已，并不用于限制本發(fā) 明，凡在本發(fā)明的精神和原則之內(nèi)，所做的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明 的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法，其特征在于，包含以下步驟： 步驟1)，在四旋翼無人機(jī)集群中選取一架四旋翼無人機(jī)作為主無人機(jī)，其余作為輔助 無人機(jī)； 步驟2)，利用慣性導(dǎo)航單元獲取主無人機(jī)的姿態(tài)角和加速度信息，計(jì)算主無人機(jī)在地 面坐標(biāo)系下的位置； 步驟3)，輔助無人機(jī)觀察主無人機(jī)的飛行變化，根據(jù)輔助無人機(jī)在地面坐標(biāo)系下的位 置及其與主無人機(jī)的相對(duì)位置計(jì)算得到主無人機(jī)在地面坐標(biāo)系下的位置變化量； 步驟4)，根據(jù)步驟3)中得到的主無人機(jī)在地面坐標(biāo)系下的位置變化量對(duì)主無人機(jī)和 路標(biāo)位置進(jìn)行校正； 步驟5)，當(dāng)主無人機(jī)將與輔助無人機(jī)之間的距離超過預(yù)設(shè)的第一距離值時(shí)，輔助無人 機(jī)調(diào)整位置接近主無人機(jī)，使得其與主無人機(jī)之間的距離為預(yù)設(shè)的第二距離值； 步驟6)，當(dāng)主無人機(jī)產(chǎn)生故障時(shí)，選取距離主無人機(jī)最近的輔助無人機(jī)作為新的主無 人機(jī)，繼續(xù)完成SLAM任務(wù)。2. 根據(jù)權(quán)利要求書1所述的基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法，其特征在于， 所述步驟3)根據(jù)以下公式計(jì)算主無人機(jī)在地面坐標(biāo)系下的位置變化量：式中，',=(A,,也/、a: = Y分別為輔助無人機(jī)i在t-1時(shí)刻和t時(shí)刻 觀測與主無人機(jī)之間的相對(duì)位置，i= 1，2,…，N-1 ; N-1為輔助無人機(jī)的數(shù)量； At為t-1時(shí)刻和t時(shí)刻之間的間隔時(shí)間； AXl為輔助無人機(jī)i觀測到的主無人機(jī)在地面坐標(biāo)系下的位置變化量。3. 根據(jù)權(quán)利要求書2所述的基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法，其特征在于， 所述步驟4)根據(jù)以下公式對(duì)主無人機(jī)位置進(jìn)行校正：式中，元、分別為主無人機(jī)在t時(shí)刻和t-1時(shí)刻的校正位置，xt為主無人機(jī)t時(shí)刻 根據(jù)慣性導(dǎo)航單元得到的自身位置。4. 根據(jù)權(quán)利要求書3所述的基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法，其特征在于， 所述步驟5)的詳細(xì)步驟如下： 若在t時(shí)刻，輔助無人機(jī)i與主無人機(jī)之間的相對(duì)距離超過攝像頭最大觀測距離的 4/5,則輔助無人機(jī)i首先向主無人機(jī)發(fā)送暫停信號(hào)，然后根據(jù)該時(shí)刻得到的相對(duì)位置信息 氣=(A,，&，n)'沿最短距離迅速飛行至距主無人機(jī)最大觀測距離1/5處懸停，向主無人 機(jī)發(fā)送繼續(xù)執(zhí)行信號(hào)。5. 根據(jù)權(quán)利要求書4所述的基于視覺的多四旋翼無人機(jī)協(xié)同SLAM方法，其特征在于， 步驟6)的詳細(xì)步驟如下： 主無人機(jī)產(chǎn)生故障時(shí)，比較該t時(shí)刻各輔助無人機(jī)對(duì)于主無人機(jī)的相對(duì)距離A，選取 輔助無人機(jī)n作為新的主無人機(jī)，A, =niinp,,，/=U，A'1-:1，迅速移至原主無人機(jī)位置， 繼續(xù)完成SLAM任務(wù)，其余輔助無人機(jī)任務(wù)保持不變。
【專利摘要】本發(fā)明公開了一種基于視覺的多四旋翼無人機(jī)協(xié)同SLAM的方法，該方法針對(duì)多架安裝有采集圖像信息的攝像頭和獲取無人機(jī)的姿態(tài)角和加速度信息的慣性導(dǎo)航單元且可相互通信的四旋翼無人機(jī)，其中一架為主無人機(jī)，其余為輔助無人機(jī)。主無人機(jī)負(fù)責(zé)完成SLAM任務(wù)，輔助無人機(jī)懸停于固定位置對(duì)主無人機(jī)進(jìn)行相對(duì)位置測量，幫助主無人機(jī)進(jìn)行實(shí)時(shí)定位修正。本發(fā)明主要應(yīng)用于多四旋翼無人機(jī)自主飛行定位，有助于主無人機(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)性。
【IPC分類】G05D1/08, G05D1/10
【公開號(hào)】CN105022401
【申請(qǐng)?zhí)枴緾N201510392379
【發(fā)明人】叢楚瀅, 王從慶, 丁臻極
【申請(qǐng)人】南京航空航天大學(xué)
【公開日】2015年11月4日
【申請(qǐng)日】2015年7月6日...