本發(fā)明涉及多旋翼無人機(jī)集群協(xié)同控制領(lǐng)域，特別是一種基于視覺的異構(gòu)多旋翼無人機(jī)集群協(xié)同導(dǎo)引控制方法。

背景技術(shù)：

1、在發(fā)明專利“基于預(yù)設(shè)性能四旋翼無人機(jī)視覺伺服目標(biāo)跟蹤控制方法”中，提出了一種預(yù)設(shè)性能四旋翼無人機(jī)視覺伺服目標(biāo)跟蹤控制方法，該方法是通過四旋翼無人機(jī)的機(jī)載相機(jī)獲取目標(biāo)特征點(diǎn)的空間圖像坐標(biāo)和目標(biāo)特征點(diǎn)的圖像平面坐標(biāo)，獲得四旋翼無人機(jī)機(jī)載相機(jī)焦距，計(jì)算目標(biāo)特征點(diǎn)的圖像特征和目標(biāo)特征點(diǎn)的雅可比矩陣，將多個(gè)目標(biāo)特征點(diǎn)的圖像特征構(gòu)成圖像特征向量，將多個(gè)目標(biāo)特征點(diǎn)的雅可比矩陣構(gòu)成疊加陣，獲取機(jī)載相機(jī)的速度，通過坐標(biāo)系轉(zhuǎn)換即可將相機(jī)速度轉(zhuǎn)換為無人機(jī)的飛行速度，從而實(shí)現(xiàn)對無人機(jī)的視覺伺服控制，同時(shí)，該方法提前做了性能預(yù)設(shè)，對控制系統(tǒng)做了一定的約束，避免控制量過大導(dǎo)致目標(biāo)離開相機(jī)的視野，導(dǎo)致控制方法失效。該方法目的是為了解決四旋翼無人機(jī)視覺伺服控制中，由于機(jī)載相機(jī)的視角限制和無人機(jī)運(yùn)動(dòng)與機(jī)載相機(jī)的運(yùn)動(dòng)耦合導(dǎo)致無人機(jī)所跟蹤的目標(biāo)極易丟失，從而導(dǎo)致任務(wù)失敗的問題。

2、以上發(fā)明有效的前提條件是無人機(jī)必須安裝相機(jī)，不適用于沒有安裝相機(jī)的無人機(jī)，但在無人機(jī)集群中，不會(huì)每架無人機(jī)都安裝相機(jī)，每架無人機(jī)都安裝相機(jī)必然帶來使用成本的增加，同時(shí)，相機(jī)的重量也會(huì)降低無人機(jī)的續(xù)航時(shí)間，因此，上述發(fā)明對無人機(jī)集群來說不是最優(yōu)的方法。

3、視覺傳感器為無人機(jī)提供了諸如顏色和形狀等豐富的環(huán)境信息，這使得無人機(jī)能夠?qū)崿F(xiàn)目標(biāo)識別和跟蹤等功能，因此，視覺傳感器對于增強(qiáng)無人機(jī)的能力至關(guān)重要。在搜救、偵察等任務(wù)場景下，相較于單架無人機(jī)，無人機(jī)集群能夠提高任務(wù)的執(zhí)行效率，然而，在無人機(jī)集群中，如果每架無人機(jī)都安裝視覺傳感器，必會(huì)導(dǎo)致資源的浪費(fèi)和成本的增加。鑒于此，一種常見的做法是僅讓部分無人機(jī)安裝視覺傳感器，并通過通信網(wǎng)絡(luò)將視覺數(shù)據(jù)分享給整個(gè)集群，但是這種方法存在局限性，即那些未直接安裝視覺傳感器的無人機(jī)可能難以采用現(xiàn)有的視覺伺服控制算法來完成特定任務(wù)，例如目標(biāo)跟蹤、孔徑穿越等。

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

1、針對無人機(jī)集群中部分未安裝視覺傳感器的無人機(jī)不能執(zhí)行基于視覺伺服控制任務(wù)的問題，提出了一種基于視覺的異構(gòu)多旋翼無人機(jī)集群協(xié)同導(dǎo)引控制方法。該方法旨在支持異構(gòu)多旋翼無人機(jī)集群(異構(gòu)是指集群中部分無人機(jī)安裝了視覺傳感器，其它沒有安裝)進(jìn)行協(xié)同導(dǎo)引控制，即讓集群中未安裝視覺傳感器的無人機(jī)也能執(zhí)行目標(biāo)跟蹤和孔徑穿越等基于視覺伺服控制的任務(wù)。

2、本發(fā)明的技術(shù)方案為：

3、一種基于視覺的異構(gòu)多旋翼無人機(jī)集群協(xié)同導(dǎo)引控制方法，共分為兩個(gè)階段，在第一階段，未安裝視覺傳感器的多旋翼無人機(jī)飛到安裝視覺傳感器的多旋翼無人機(jī)的視覺傳感器能夠觀察到的位置，在第二階段，安裝視覺傳感器的多旋翼無人機(jī)導(dǎo)引未安裝視覺傳感器的多旋翼無人機(jī)向感興趣的目標(biāo)飛去；包括以下步驟：

4、第一階段

5、步驟1，無人機(jī)集群上電啟動(dòng)，無人機(jī)集群中的每架無人機(jī)能夠?qū)崟r(shí)獲取在全局坐標(biāo)系下的位置和姿態(tài)，安裝視覺傳感器的多旋翼無人機(jī)開始工作，未安裝視覺傳感器的多旋翼無人機(jī)等待安裝視覺傳感器的多旋翼無人機(jī)發(fā)送過來的指令；

6、步驟2，安裝視覺傳感器的多旋翼無人機(jī)對感興趣的需要跟蹤、穿越的目標(biāo)進(jìn)行搜索，利用視覺傳感器獲取的實(shí)時(shí)圖像通過深度神經(jīng)網(wǎng)絡(luò)的目標(biāo)識別算法檢查是否存在感興趣的目標(biāo)，并計(jì)算目標(biāo)在圖像系下的中心位置；

7、步驟3，安裝視覺傳感器的多旋翼無人機(jī)發(fā)現(xiàn)感興趣的目標(biāo)后懸停，保持位置和姿態(tài)不變，基于當(dāng)前全局坐標(biāo)系下的位置和姿態(tài)計(jì)算出機(jī)體坐標(biāo)系下這個(gè)無人機(jī)前方的點(diǎn)在全局坐標(biāo)系下的位置，將該位置和當(dāng)前姿態(tài)角通過通信中間件發(fā)送給集群中一架未安裝視覺傳感器的多旋翼無人機(jī)，

8、步驟4，未安裝視覺傳感器的多旋翼無人機(jī)接收到該位置和安裝視覺傳感器的多旋翼無人機(jī)的姿態(tài)角后，飛到全局坐標(biāo)系下的該位置，并在飛行過程中調(diào)整姿態(tài)，和安裝視覺傳感器的多旋翼無人機(jī)保持一致；

9、第二階段

10、步驟5，安裝視覺傳感器的多旋翼無人機(jī)的視覺傳感器能夠同時(shí)獲取未安裝視覺傳感器的多旋翼無人機(jī)和感興趣的目標(biāo)，計(jì)算它們在圖像坐標(biāo)系下的圖像偏差，再將偏差轉(zhuǎn)換到機(jī)體坐標(biāo)系下，然后通過通信網(wǎng)絡(luò)發(fā)送給未安裝視覺傳感器的多旋翼無人機(jī)，

11、步驟6，未安裝視覺傳感器的多旋翼無人機(jī)基于接收到的圖像偏差和安裝視覺傳感器的多旋翼無人機(jī)姿態(tài)角度后，通過pid控制器分別計(jì)算出未安裝視覺傳感器的多旋翼無人機(jī)全局坐標(biāo)系下的線速度和機(jī)體坐標(biāo)系下的角速度，然后利用多旋翼無人機(jī)位置和姿態(tài)控制器計(jì)算出每個(gè)電機(jī)的轉(zhuǎn)速，使得未安裝視覺傳感器的多旋翼無人機(jī)的姿態(tài)角和安裝視覺傳感器的多旋翼無人機(jī)保持一致，并向感興趣的目標(biāo)中心移動(dòng)，從而實(shí)現(xiàn)異構(gòu)集群的協(xié)同導(dǎo)引控制；

12、步驟7，重復(fù)上述步驟3到步驟6，實(shí)現(xiàn)對集群中其它未安裝視覺傳感器的多旋翼無人機(jī)的協(xié)同導(dǎo)引控制。

13、進(jìn)一步的，具體包括以下步驟：

14、一、第一階段

15、步驟1，無人機(jī)集群上電啟動(dòng)，無人機(jī)集群中的每架無人機(jī)能夠?qū)崟r(shí)獲取在全局坐標(biāo)系下的位置和姿態(tài)，安裝視覺傳感器的多旋翼無人機(jī)開始工作，未安裝視覺傳感器的多旋翼無人機(jī)等待安裝視覺傳感器的多旋翼無人機(jī)發(fā)送過來的指令；

16、步驟2，安裝視覺傳感器的多旋翼無人機(jī)對感興趣的需要跟蹤、穿越的目標(biāo)進(jìn)行搜索，利用視覺傳感器獲取的實(shí)時(shí)圖像通過深度神經(jīng)網(wǎng)絡(luò)的目標(biāo)識別算法檢查是否存在感興趣的目標(biāo)，并計(jì)算目標(biāo)在圖像系下的中心位置；

17、步驟3，安裝視覺傳感器的多旋翼無人機(jī)發(fā)現(xiàn)感興趣的目標(biāo)后懸停，保持位置和姿態(tài)不變，基于當(dāng)前全局坐標(biāo)系下的位置和姿態(tài)計(jì)算出機(jī)體坐標(biāo)系下這個(gè)無人機(jī)前方的點(diǎn)在全局坐標(biāo)系下的位置將和當(dāng)前姿態(tài)角通過通信中間件發(fā)送給集群中一架未安裝視覺傳感器的多旋翼無人機(jī)，計(jì)算方式如下所示：

18、

19、式(1)中，為安裝視覺傳感器的多旋翼無人機(jī)在全局坐標(biāo)系下的橫滾角φt、俯仰角θt和偏航角ψt構(gòu)建的機(jī)體坐標(biāo)系到全局坐標(biāo)系的旋轉(zhuǎn)矩陣；為機(jī)體坐標(biāo)系到全局坐標(biāo)系的平移矩陣,xt、yt、zt分別是安裝視覺傳感器的多旋翼無人機(jī)在全局坐標(biāo)系下的x、y、z三個(gè)坐標(biāo)軸上的分量；是安裝視覺傳感器的多旋翼無人機(jī)前方的一個(gè)點(diǎn)，lx、ly、lz是該點(diǎn)在機(jī)體坐標(biāo)系下x、y、z三個(gè)坐標(biāo)軸上的分量；

20、步驟4，未安裝視覺傳感器的多旋翼無人機(jī)接收到和安裝視覺傳感器的多旋翼無人機(jī)的姿態(tài)角后，飛到全局坐標(biāo)系下的這個(gè)點(diǎn)，并在飛行過程中調(diào)整姿態(tài)，和安裝視覺傳感器的多旋翼無人機(jī)保持一致；

21、二、第二階段

22、步驟5，安裝視覺傳感器的多旋翼無人機(jī)的視覺傳感器能夠同時(shí)獲取未安裝視覺傳感器的多旋翼無人機(jī)和感興趣的目標(biāo)，計(jì)算它們在圖像坐標(biāo)系下的圖像偏差再將轉(zhuǎn)換到機(jī)體坐標(biāo)系下然后通過通信網(wǎng)絡(luò)發(fā)送給未安裝視覺傳感器的多旋翼無人機(jī)，和計(jì)算方式如下所示：

23、

24、式(2)中，(ut,vt)為感興趣的目標(biāo)在圖像坐標(biāo)系下的中心位置；(uq,vq)為未安裝視覺傳感器的多旋翼無人機(jī)在圖像坐標(biāo)系下的中心位置；式(3)中，f為視覺傳感器的焦距，是視覺傳感器的內(nèi)參，為已知常量；

25、步驟6，未安裝視覺傳感器的多旋翼無人機(jī)基于接收到的圖像偏差和安裝視覺傳感器的多旋翼無人機(jī)姿態(tài)角度后，通過pid控制器分別計(jì)算出未安裝視覺傳感器的多旋翼無人機(jī)全局坐標(biāo)系下的線速度和機(jī)體坐標(biāo)系下的角速度，然后利用多旋翼無人機(jī)位置和姿態(tài)控制器計(jì)算出每個(gè)電機(jī)的轉(zhuǎn)速，使得未安裝視覺傳感器的多旋翼無人機(jī)的姿態(tài)角和安裝視覺傳感器的多旋翼無人機(jī)保持一致，并向感興趣的目標(biāo)中心移動(dòng)，從而實(shí)現(xiàn)異構(gòu)集群的協(xié)同導(dǎo)引控制；

26、機(jī)體坐標(biāo)系下的線速度的pid控制器設(shè)計(jì)為：

27、

28、式(4)中，和為線速度的pid控制器的比例、積分和微分參數(shù)，需要進(jìn)行參數(shù)整定；

29、機(jī)體坐標(biāo)系下的角速度的pid控制器設(shè)計(jì)為：

30、

31、式(5)中，φt′、θt′、ψt′分別為未安裝視覺傳感器的橫滾角、俯仰角和航向角；和為角速度的pid控制器的比例、積分和微分參數(shù)，需要進(jìn)行參數(shù)整定；

32、將vb通過全局坐標(biāo)系下的φt′、θt′、ψt′構(gòu)建的旋轉(zhuǎn)矩陣變換到全局坐標(biāo)系下，全局坐標(biāo)系下的線速度計(jì)算公式為：

33、

34、式(6)中，vw為未安裝視覺傳感器的多旋翼無人機(jī)在全局坐標(biāo)系下的速度控制指令；

35、將rb、vw給多旋翼無人機(jī)位置和姿態(tài)控制器，由位置和姿態(tài)控制器來計(jì)算出每個(gè)電機(jī)的轉(zhuǎn)速，實(shí)現(xiàn)對無人機(jī)運(yùn)動(dòng)控制，使得未安裝視覺傳感器的多旋翼無人機(jī)的姿態(tài)角和安裝視覺傳感器的多旋翼無人機(jī)保持一致，并向感興趣的目標(biāo)中心移動(dòng)，從而實(shí)現(xiàn)異構(gòu)集群的協(xié)同導(dǎo)引控制；

36、步驟7，重復(fù)上述步驟3到步驟6，實(shí)現(xiàn)對集群中其它未安裝視覺傳感器的多旋翼無人機(jī)的協(xié)同導(dǎo)引控制。

37、進(jìn)一步的，集群中每架無人機(jī)可以通過動(dòng)作捕捉系統(tǒng)或多傳感器融合定位技術(shù)實(shí)時(shí)計(jì)算出自身在全局坐標(biāo)系下的位置和姿態(tài)，集群中每架無人機(jī)通過通信中間件發(fā)送自身位置和姿態(tài)給集群中其它無人機(jī)，并實(shí)時(shí)獲取其它無人機(jī)在全局坐標(biāo)系下的位置和姿態(tài)。

38、進(jìn)一步的，所需要的坐標(biāo)系有全局坐標(biāo)系、機(jī)體坐標(biāo)系和圖像坐標(biāo)系，其中全局坐標(biāo)系和機(jī)體坐標(biāo)系為右手系，圖像坐標(biāo)系表示三維空間物體在圖像平面上的投影，坐標(biāo)原點(diǎn)為圖像的左上角，u軸平行于圖像平面水平向右，v軸垂直于u軸向下，全局坐標(biāo)系和機(jī)體坐標(biāo)系的x軸方向與視覺傳感器光軸方向一致，全局坐標(biāo)系原點(diǎn)由集群定位方式確定，機(jī)體坐標(biāo)系原點(diǎn)為視覺傳感器的光心位置。

39、優(yōu)選的，無人機(jī)集群的全局坐標(biāo)系下的位置和姿態(tài)由動(dòng)作捕捉系統(tǒng)提供，利用yolov5基于深度神經(jīng)網(wǎng)絡(luò)的目標(biāo)識別算法計(jì)算氣球和未安裝視覺傳感器的多旋翼無人機(jī)在圖像坐標(biāo)系下的實(shí)時(shí)中心位置，集群無人機(jī)間通過無線路由進(jìn)行通信，多旋翼無人機(jī)位置和姿態(tài)控制器選擇px4。

40、本發(fā)明的有益效果在于：

41、本發(fā)明提出了一種基于視覺的異構(gòu)多旋翼無人機(jī)集群協(xié)同導(dǎo)引控制方法，該方法使得在異構(gòu)無人機(jī)集群中部分沒有安裝視覺傳感器的無人機(jī)也能完成依賴自身視覺傳感器的任務(wù)。通過減少安裝視覺傳感器的無人機(jī)數(shù)量，降低了無人機(jī)集群的使用成本，同時(shí)，減輕了無人機(jī)重量，提高了無人機(jī)的續(xù)航時(shí)間。