一種多無(wú)人機(jī)分布式隊(duì)形控制的方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及無(wú)人機(jī)編隊(duì)控制領(lǐng)域，具體涉及一種多無(wú)人機(jī)分布式隊(duì)形控制的方 法。
【背景技術(shù)】
[0002] 無(wú)人機(jī)是一種擁有動(dòng)力裝置的無(wú)人駕駛飛行系統(tǒng)。它具有廣泛的應(yīng)用，既可以用 于航拍，交通巡邏等民用領(lǐng)域，也可以用于偵查、監(jiān)控等軍事領(lǐng)域。在一些危險(xiǎn)復(fù)雜或者人 難以進(jìn)入的環(huán)境中，需要進(jìn)行搜索或者搜救活動(dòng)，比如災(zāi)后緊急救援，雪地搜救現(xiàn)場(chǎng)等，一 般只能通過(guò)直升機(jī)來(lái)進(jìn)行勘察任務(wù)，成本巨大且非常危險(xiǎn)，無(wú)人機(jī)的作用就顯得尤為突出。
[0003] 目前在災(zāi)后搜救、安防掃描等眾多應(yīng)用中，大都采用單個(gè)無(wú)人機(jī)進(jìn)行地毯式搜索 的方法。然而，受限于單個(gè)無(wú)人機(jī)續(xù)航能力的不足，如果要進(jìn)行大面積地毯式搜索，不僅容 易漏掉目標(biāo)，而且效率還十分低下。不僅如此，如果在大面積環(huán)境中還有一些障礙物的話， 更是難以處理。
[0004] 因此，單個(gè)無(wú)人機(jī)很難滿足大面積搜索或者搜救活動(dòng)的需求，如果任務(wù)緊急，那情 況將更加嚴(yán)重。通過(guò)多個(gè)無(wú)人機(jī)編隊(duì)，協(xié)作搜索來(lái)避免大面積應(yīng)用場(chǎng)景中單個(gè)無(wú)人機(jī)不高 效的問(wèn)題，可以使得整個(gè)搜索過(guò)程高效準(zhǔn)確。
[0005] 目前無(wú)人機(jī)編隊(duì)控制方法大都以集中式方法進(jìn)行編隊(duì)控制，集中式的控制方法往 往需要建立覆蓋所有節(jié)點(diǎn)的星形通信網(wǎng)絡(luò)。中國(guó)發(fā)明專利(CN 102591358 A)公開(kāi)一種多無(wú) 人機(jī)的動(dòng)態(tài)編隊(duì)控制方法，屬于飛行控制技術(shù)領(lǐng)域，包括步驟一:隊(duì)形保持方法;步驟二:避 障方法;步驟三:基于行為的編隊(duì)過(guò)程，其中基于行為的編隊(duì)過(guò)程分別為行為分解與控制實(shí) 現(xiàn)。該專利引入了基于行為的編隊(duì)控制方法，降低了對(duì)編隊(duì)無(wú)線數(shù)據(jù)鏈更新率的要求，增強(qiáng) 了無(wú)人機(jī)群編隊(duì)的避障能力；同時(shí)引入了虛擬結(jié)構(gòu)作為參考。但是，現(xiàn)有技術(shù)中沒(méi)有存在通 過(guò)及時(shí)調(diào)整隊(duì)形的尺度，并且采用分布式控制進(jìn)行編對(duì)的方法。

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

[0006] 本發(fā)明的目的在于針對(duì)現(xiàn)有技術(shù)的不足，提供一種多無(wú)人機(jī)分布式隊(duì)形控制的方 法，通過(guò)分布式控制無(wú)人機(jī)進(jìn)行編隊(duì)，同時(shí)可以調(diào)整隊(duì)形的尺度避開(kāi)障礙物。
[0007] 本發(fā)明為解決技術(shù)問(wèn)題采取如下技術(shù)方案：
[0008] -種多無(wú)人機(jī)分布式隊(duì)形控制的方法，包括如下步驟：
[0009] 1)在預(yù)設(shè)區(qū)域內(nèi)部署η架無(wú)人機(jī)，η為η 2 3的整數(shù);所述的無(wú)人機(jī)能夠主動(dòng)測(cè)量鄰 居無(wú)人機(jī)的相對(duì)位置信息；
[0010] 2)對(duì)所述的η架無(wú)人機(jī)設(shè)定期望隊(duì)形，所述的期望隊(duì)形表示為：
[0011] ξ = [ξι,ξ2,···,ξη]τ,
[0012] 其中，ξ為期望隊(duì)形的矩陣表示，ξ:為第1架無(wú)人機(jī)的位置，ξ2為第2架無(wú)人機(jī)的位 置，ξη為第η架無(wú)人機(jī)的位置，Τ為矩陣的轉(zhuǎn)置；
[0013] 3)根據(jù)步驟2)中設(shè)定的期望隊(duì)形，確定每架無(wú)人機(jī)在期望隊(duì)形中與之測(cè)量和通信 的鄰居無(wú)人機(jī)，得到期望隊(duì)形中無(wú)人機(jī)的測(cè)量關(guān)系和通信關(guān)系；
[0014] 4)對(duì)第1架無(wú)人機(jī)和第2架無(wú)人機(jī)進(jìn)行隊(duì)形控制，所述的第1架無(wú)人機(jī)和第2架無(wú)人 機(jī)設(shè)定第一類速度控制器，通過(guò)主動(dòng)測(cè)量對(duì)方的相對(duì)位置信息，進(jìn)行速度控制使得彼此的 運(yùn)動(dòng)距離與期望隊(duì)形設(shè)定的距離一致；
[0015] 5)對(duì)第3~η架無(wú)人機(jī)同時(shí)進(jìn)行隊(duì)形控制，所述的第3~η架無(wú)人機(jī)設(shè)定第二類速度 控制器，通過(guò)主動(dòng)測(cè)量步驟3)中確定的期望隊(duì)形中鄰居無(wú)人機(jī)的相對(duì)位置信息，進(jìn)行速度 控制使得彼此的隊(duì)形動(dòng)態(tài)的達(dá)到期望隊(duì)形。
[0016] 上述技術(shù)方案不依賴于全局參考坐標(biāo)系，利用無(wú)人機(jī)主動(dòng)測(cè)量鄰居無(wú)人機(jī)的相對(duì) 位置，從而實(shí)現(xiàn)無(wú)人機(jī)編隊(duì)控制的方法。對(duì)第1架和第2架無(wú)人機(jī)設(shè)定第一類速度控制器，使 得兩者保持設(shè)定的距離，其次，對(duì)第3~η架無(wú)人機(jī)設(shè)定第二類速度控制器，進(jìn)行速度的控制 得到期望隊(duì)形。同時(shí)，可以通過(guò)對(duì)第一類速度控制器設(shè)定的距離進(jìn)行調(diào)整，達(dá)到對(duì)期望隊(duì)形 的尺度的調(diào)整，避開(kāi)障礙物。
[0017] 所述的預(yù)設(shè)區(qū)域?yàn)槠矫?，所述的無(wú)人機(jī)在平面內(nèi)運(yùn)動(dòng)的動(dòng)態(tài)模型如下：
[0018] z7 i = vi, i = 1,2, ··· ,η；
[0019] 其中Vl為第i架無(wú)人機(jī)的速度向量，ζ、為第i架無(wú)人機(jī)的位置向量求導(dǎo)。
[0020] 作為優(yōu)選，所述的步驟4)中第一類速度控制器為：
[0021]
[0022] 其中，Z1為第1架無(wú)人機(jī)在其慣性平面坐標(biāo)系下的位置向量，以復(fù)數(shù)表示;Z2為第2 架無(wú)人機(jī)在其慣性平面坐標(biāo)系下的位置向量，以復(fù)數(shù)表示；I Izrzsl |和| |Z2-Z1| |為第1架 無(wú)人機(jī)與第2架無(wú)人機(jī)之間的歐幾里得距離;ri2為第1架無(wú)人機(jī)和第2架無(wú)人機(jī)之間的期望 距離;k為調(diào)節(jié)速度的比例系數(shù);vo為隊(duì)形穩(wěn)定時(shí)，無(wú)人機(jī)的期望運(yùn)動(dòng)速度向量;為第1架 無(wú)人機(jī)相對(duì)于慣性坐標(biāo)系的速度控制向量，zS為第2架無(wú)人機(jī)相對(duì)于慣性坐標(biāo)系的速度向 量控制量。
[0023] 作為優(yōu)選，所述的步驟5)中第二類速度控制器為：
[0024]
[0025]其中，Ni(g)和Ni(H)分別為苐i架尤人機(jī)在期望隊(duì)形中具有測(cè)量關(guān)系的鄰居無(wú)人機(jī) 集合和具有通訊關(guān)系的鄰居無(wú)人機(jī)集合;Zl為第i架無(wú)人機(jī)在其慣性平面坐標(biāo)系下的位置 向量，W為第j架無(wú)人機(jī)在其慣性平面坐標(biāo)系下的位置向量;m為第i架無(wú)人機(jī)對(duì)第1架或者 第2架無(wú)人機(jī)的速度估計(jì),?為第j架無(wú)人機(jī)對(duì)第1架或者第2架無(wú)人機(jī)的速度估計(jì);ζ'i為第i 架無(wú)人機(jī)相對(duì)于慣性坐標(biāo)系的速度向量控制量，的導(dǎo)數(shù)，cU為需要被設(shè)計(jì)的控制參 數(shù)，Wl溈合適的權(quán)重指數(shù)， &1溈任意正實(shí)數(shù)。
[0026]作為優(yōu)選，所述的第一類速度控制器和第二類速度控制器分別簡(jiǎn)化如下：
[0027]
[0029] 式中，1表示第1架無(wú)人機(jī)或第2架無(wú)人機(jī)，f表示第3~η架無(wú)人機(jī)，12為所有元素為 1的列向量；[0030] 其中，Z1= [zl，Z2]T，Zf= [Ζ3,…，Ζη]τ;[0031 ] ni= [m，n2]T，nf= [η3,…，ηη]τ;
[0028]
[0032]
[0033] 式中，Ιη-2為η-2維的單位矩陣；
[0034] 其中L用復(fù)拉普拉斯表示，表示為：
[0035]
[0036] 式中，〇2X2為2X2的0矩陣，02χ(η-2)為2X (η-2)的0矩陣；
[0037] 其中D表示為：
[0038] D = diag(0,0，d3，···，dn)，Df = diag(d3，···，dn);其中dn為對(duì)角矩陣Df的最后一個(gè)元 素；
[0039]其中Η用拉普拉斯表示，表示為：
[0040]
[0041] 作為進(jìn)一步優(yōu)選，所述的第二類速度控制器需要滿足以下條件：
[0042] l)L| = 0;2)rank(L) =n_2;3)det(Lff)關(guān)0;4)det(Hff)關(guān)0;5)DfLff的特征值在復(fù) 數(shù)右半平面;其中L為期望隊(duì)形的復(fù)拉普拉斯表示，ξ為期望隊(duì)形，rank(L)為矩陣L的秩，det (Lff)為矩陣Lff的行列式，det(Hff)為Hff的行列式，D fLff為兩個(gè)矩陣相乘。
[0043]作為優(yōu)選，所述的第一類速度控制器和第二類速度控制器控制的控制周期為0.05 ~0.15s。該控制周期范圍，可以使得無(wú)人機(jī)及時(shí)調(diào)整期望速度，更快的達(dá)到期望隊(duì)形。
[0044] 作為優(yōu)選，所述的期望隊(duì)形的尺度通過(guò)改變第一類速度控制器設(shè)定的距離進(jìn)行調(diào) 整。通過(guò)調(diào)整期望隊(duì)形的尺度，可以靈活的避開(kāi)障礙物。
[0045] 同現(xiàn)有技術(shù)相比，本發(fā)明的有益效果體現(xiàn)在：
[0046] (1)本發(fā)明中的速度控制器具有很好的收斂性，保證系統(tǒng)是穩(wěn)定的。
[0047] (2)對(duì)于每個(gè)無(wú)人機(jī)而言，僅僅只需要用到鄰居無(wú)人機(jī)的相對(duì)位置信息就能實(shí)現(xiàn) 編隊(duì)控制，從實(shí)現(xiàn)的角度考慮，相比于現(xiàn)有的編隊(duì)控制算法，本發(fā)明需要更少的計(jì)算資源并 且所需控制量更容易獲取。
[0048] (3)隊(duì)形的尺度可以根據(jù)不同的環(huán)境進(jìn)行變換，使得在進(jìn)行目標(biāo)搜索任務(wù)中，能更 加適應(yīng)環(huán)境的變化。
【附圖說(shuō)明】
[0049] 圖1本發(fā)明實(shí)施例中的期望隊(duì)形圖；
[0050] 圖2本發(fā)明實(shí)施例中的通信圖；
[00511圖3本發(fā)明實(shí)施例中隊(duì)形控制圖；
[0052]圖4是本發(fā)明實(shí)施例中期望隊(duì)形的尺度縮小0.5倍的效果圖。
【具體實(shí)施方式】
[0053]在水平高度為5米的水平面內(nèi)，部署5架四旋翼無(wú)人機(jī)(基于開(kāi)源飛控pixhawk的自 制四旋翼，型號(hào)：軸距550mm)。無(wú)人機(jī)的硬件系統(tǒng)主要分為底層飛控部分和上層上位機(jī)部 分。飛控部分控制無(wú)人機(jī)的飛行穩(wěn)定，上層上位機(jī)負(fù)責(zé)具體的算法控制，并且將計(jì)算得到的 控制速度發(fā)送給底層飛控，從而實(shí)現(xiàn)無(wú)人機(jī)的自主飛行。
[0054]所述的無(wú)人機(jī)能夠主動(dòng)測(cè)量鄰居無(wú)人機(jī)的相對(duì)位置信息，主要通過(guò)內(nèi)置激光掃描 儀，能夠主動(dòng)測(cè)量鄰居無(wú)人機(jī)和自身的相對(duì)位置信息，通過(guò)極坐標(biāo)轉(zhuǎn)換成算法所需的向量。
[0055] 所述的無(wú)人機(jī)在水平面內(nèi)運(yùn)動(dòng)的動(dòng)態(tài)模型如下：
[0056] z7 i = vi, i = 1,2, ··· ,η；
[0057] 其中Vl為第i架無(wú)人機(jī)的速度向量，ζ、為第i架無(wú)人機(jī)的位置向量求導(dǎo)。
[0058]對(duì)5架無(wú)人機(jī)設(shè)定期望隊(duì)形，期望隊(duì)形表示為：| = [-1，1，0，-11，-21]7，期望隊(duì)形 圖如圖1所示，其中箭頭表示為5架無(wú)人機(jī)之間的測(cè)量關(guān)系;通信圖如圖2,其中箭頭表示為5 架無(wú)人機(jī)之間的通信關(guān)系。
[0059] 然后對(duì)第1架無(wú)人機(jī)和第2架無(wú)人機(jī)進(jìn)行隊(duì)形控制，所述的第1架