本發(fā)明屬于四旋翼無(wú)人機(jī)編隊(duì)控制，特別是涉及一種具有抗干擾能力的四旋翼無(wú)人機(jī)容錯(cuò)編隊(duì)控制方法。

背景技術(shù)：

1、四旋翼無(wú)人機(jī)是一種能垂直起降，靈活性高，穩(wěn)定性強(qiáng)的無(wú)人機(jī)系統(tǒng)，鑒于它的這些優(yōu)良特性，已在許多領(lǐng)域得到了廣泛的應(yīng)用，包括智慧農(nóng)業(yè)，環(huán)境監(jiān)測(cè)，電力巡檢，救援搜救，軍事察打。相比于單體四旋翼系統(tǒng)，由多個(gè)四旋翼組成的無(wú)人集群系統(tǒng)不僅有單體四旋翼無(wú)人機(jī)的優(yōu)點(diǎn)還具備多無(wú)人機(jī)系統(tǒng)自主性強(qiáng)、魯棒性高、時(shí)效性好、性價(jià)比高的優(yōu)勢(shì)。因此，四旋翼集群系統(tǒng)可以執(zhí)行難度更高、范圍更廣的任務(wù)。如何使四旋翼集群系統(tǒng)高效安全的執(zhí)行任務(wù)已經(jīng)成為本領(lǐng)域技術(shù)人員研究的熱點(diǎn)。

2、編隊(duì)控制是無(wú)人集群系統(tǒng)形成、維持、重構(gòu)、行動(dòng)的關(guān)鍵技術(shù)之一。基于分布式控制結(jié)構(gòu)的編隊(duì)控制中，每個(gè)四旋翼只需要與鄰域四旋翼進(jìn)行信息交換就可以實(shí)現(xiàn)整體的編隊(duì)控制，具備靈活性、容錯(cuò)性、可擴(kuò)展性、通信效率高等優(yōu)勢(shì)，可以提高集群系統(tǒng)的性能和魯棒性，適應(yīng)于四旋翼無(wú)人集群協(xié)同工作的編隊(duì)控制任務(wù)。在控制策略中，虛擬領(lǐng)航者簡(jiǎn)單、高效并可根據(jù)實(shí)際需求靈活擴(kuò)展實(shí)現(xiàn)更加復(fù)雜的編隊(duì)任務(wù)。因此，分布式控制結(jié)構(gòu)結(jié)合虛擬領(lǐng)航者控制策略成為編隊(duì)控制中較好的一種策略選擇。

3、隨著四旋翼無(wú)人機(jī)編隊(duì)控制應(yīng)用領(lǐng)域的不斷擴(kuò)展，編隊(duì)控制執(zhí)行任務(wù)的環(huán)境越來(lái)越復(fù)雜，對(duì)編隊(duì)控制的安全性，魯棒性提出了更高的要求。由于四旋翼無(wú)人機(jī)體積小，質(zhì)量輕，在復(fù)雜環(huán)境下容易遭受時(shí)變風(fēng)擾，大氣紊流等未知干擾，給編隊(duì)控制的穩(wěn)定性，安全性帶來(lái)挑戰(zhàn)，在一些嚴(yán)重的干擾下，個(gè)體四旋翼的損毀將會(huì)導(dǎo)致整個(gè)編隊(duì)控制失效。四旋翼無(wú)人機(jī)在執(zhí)行任務(wù)過(guò)程中由于外部復(fù)雜環(huán)境的影響和內(nèi)部機(jī)械結(jié)構(gòu)的長(zhǎng)時(shí)間工作運(yùn)轉(zhuǎn)會(huì)大大增加無(wú)人機(jī)發(fā)生故障的概率，且發(fā)生的故障多為突發(fā)性故障，尤其是執(zhí)行器故障（電機(jī)動(dòng)力損失、槳葉破損）在執(zhí)行任務(wù)期間經(jīng)常發(fā)生，嚴(yán)重影響編隊(duì)控制的飛行安全和任務(wù)完成。基于以上問(wèn)題，亟需一種具有抗干擾能力的四旋翼無(wú)人機(jī)容錯(cuò)編隊(duì)控制方法。

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

1、針對(duì)以上技術(shù)問(wèn)題，本發(fā)明提供一種具有抗干擾能力的四旋翼無(wú)人機(jī)容錯(cuò)編隊(duì)控制方法。

2、本發(fā)明解決其技術(shù)問(wèn)題采用的技術(shù)方案是：

3、一種具有抗干擾能力的四旋翼無(wú)人機(jī)容錯(cuò)編隊(duì)控制方法，所述方法包括以下步驟：

4、s100：在三維空間中建立一個(gè)由多個(gè)四旋翼無(wú)人機(jī)組成的無(wú)人機(jī)集群系統(tǒng)，并為每個(gè)無(wú)人機(jī)建立平移動(dòng)力學(xué)、旋轉(zhuǎn)動(dòng)力學(xué)、電機(jī)動(dòng)力學(xué)和執(zhí)行器故障模型，進(jìn)而建立四旋翼集群完整的動(dòng)力學(xué)模型，完整的動(dòng)力學(xué)模型的控制目標(biāo)是設(shè)計(jì)推力和轉(zhuǎn)矩使每個(gè)四旋翼無(wú)人機(jī)滿足預(yù)設(shè)條件；

5、s200：設(shè)計(jì)標(biāo)稱編隊(duì)控制器，根據(jù)平移動(dòng)力學(xué)和執(zhí)行器故障模型將存在偏差故障的平移動(dòng)力學(xué)模型進(jìn)行改寫，并結(jié)合高階滑模觀測(cè)器估計(jì)和補(bǔ)償執(zhí)行器偏差故障和外界時(shí)變擾動(dòng)，設(shè)計(jì)位置環(huán)的魯棒控制器，進(jìn)而得到每個(gè)四旋翼無(wú)人機(jī)的靜推力，實(shí)現(xiàn)各四旋翼無(wú)人機(jī)對(duì)虛擬領(lǐng)航者的軌跡跟蹤；

6、s300：解耦位置環(huán)和姿態(tài)環(huán)得到各四旋翼無(wú)人機(jī)的期望姿態(tài)，為標(biāo)稱系統(tǒng)設(shè)計(jì)姿態(tài)控制器，根據(jù)旋轉(zhuǎn)動(dòng)力學(xué)和執(zhí)行器故障模型將存在偏差故障的旋轉(zhuǎn)動(dòng)力學(xué)模型進(jìn)行改寫，并結(jié)合高階滑模觀測(cè)器解決執(zhí)行器偏差故障和外部時(shí)變擾動(dòng)對(duì)姿態(tài)環(huán)的影響，設(shè)計(jì)姿態(tài)環(huán)的魯棒控制器，從而實(shí)現(xiàn)各四旋翼無(wú)人機(jī)的姿態(tài)穩(wěn)定；

7、s400：根據(jù)完整動(dòng)力學(xué)模型、位置環(huán)的魯棒控制器和姿態(tài)環(huán)的魯棒控制器，改寫完整動(dòng)力學(xué)模型，基于牛頓修正法計(jì)算執(zhí)行器故障后各旋翼的升力系數(shù)實(shí)現(xiàn)在線故障診斷，根據(jù)故障后的控制效能矩陣計(jì)算每個(gè)電機(jī)的轉(zhuǎn)速，實(shí)現(xiàn)執(zhí)行器故障后的四旋翼無(wú)人機(jī)安全可靠編隊(duì)控制。

8、優(yōu)選地，建立慣性坐標(biāo)系i和機(jī)體坐標(biāo)系b，s100包括：

9、s110：根據(jù)牛頓第二定律，各四旋翼子系統(tǒng)的平移動(dòng)力學(xué)方程為：

10、(1)

11、式中，表示無(wú)人集群系統(tǒng)中四旋翼無(wú)人機(jī)的編號(hào)，和分別表示慣性坐標(biāo)系i下每個(gè)四旋翼無(wú)人機(jī)的位置和線速度，其中，分別是各四旋翼無(wú)人機(jī)在x軸，y軸和z軸方向的位置，分別是各四旋翼無(wú)人機(jī)在x軸，y軸和z軸方向的速度，是由歐拉角定義的旋轉(zhuǎn)矩陣，表示由機(jī)體坐標(biāo)到世界坐標(biāo)的轉(zhuǎn)換，為恒定的重力加速度，是常數(shù)向量，表示每個(gè)四旋翼無(wú)人機(jī)的質(zhì)量，是機(jī)體坐標(biāo)系下的力，表示無(wú)人機(jī)受到的總推力，為每個(gè)四旋翼無(wú)人機(jī)平動(dòng)動(dòng)力學(xué)的外界擾動(dòng)，其中，分別是各四旋翼無(wú)人機(jī)在x軸，y軸和z軸方向上受到的擾動(dòng)，包括空氣阻力，時(shí)變風(fēng)擾和其他未建模的部分；

12、s120：根據(jù)歐拉方程，姿態(tài)子系統(tǒng)的旋轉(zhuǎn)動(dòng)力學(xué)方程寫成：

13、（2）

14、式中，是歐拉角，其中分別是各四旋翼無(wú)人機(jī)的俯仰角、翻滾角和偏航角，表示機(jī)體坐標(biāo)系下的角速度，其中，是各四旋翼無(wú)人機(jī)俯仰角速度、翻滾角速度和偏航角速度，是慣性矩陣，其中，分別是各四旋翼無(wú)人機(jī)繞機(jī)體坐標(biāo)系x軸、y軸和z軸的轉(zhuǎn)動(dòng)慣量，表示力矩，其中，分別是各四旋翼無(wú)人機(jī)在x軸，y軸和z軸方向上的力矩，表示姿態(tài)子系統(tǒng)遭受到的擾動(dòng)，包括未建模部分和外部風(fēng)擾，其中，分別是各四旋翼無(wú)人機(jī)在俯仰，翻滾和偏航時(shí)受到的擾動(dòng)；

15、s130：根據(jù)四旋翼無(wú)人機(jī)的四個(gè)旋翼產(chǎn)生的升力、滾轉(zhuǎn)力矩、俯仰力矩和偏航力矩與電機(jī)轉(zhuǎn)速之間的關(guān)系，每個(gè)四旋翼無(wú)人機(jī)的控制輸入建模也即電機(jī)動(dòng)力學(xué)方程為：

16、（3）

17、式中，是控制輸入向量，是每個(gè)電機(jī)的轉(zhuǎn)速，其中，是各四旋翼無(wú)人機(jī)1，2，3，4號(hào)電機(jī)的轉(zhuǎn)速，是控制效能矩陣，其具體形式為：

18、（4）

19、?式中，是各無(wú)人機(jī)每個(gè)電機(jī)的升力系數(shù)，是各無(wú)人機(jī)的反力矩系數(shù)，是升阻比系數(shù)；

20、s140:?建立執(zhí)行器故障模型為：

21、（5）

22、式中，，其中是每個(gè)電機(jī)的升力失效系數(shù)；表示理想情況下標(biāo)稱系統(tǒng)的控制輸入，是未知的執(zhí)行器偏差故障；

23、?結(jié)合式（3）和式（5），執(zhí)行器故障模型重寫成：

24、（6）

25、s150:?結(jié)合平移動(dòng)力學(xué)（1），旋轉(zhuǎn)動(dòng)力學(xué)（2），電機(jī)動(dòng)力學(xué)（3）和執(zhí)行器故障模型（6），四旋翼集群完整的動(dòng)力學(xué)模型表示成：

26、（7）

27、式中，是無(wú)人集群中各四旋翼無(wú)人機(jī)的狀態(tài)，，其中，其中，是正弦算子，是余弦算子，。

28、優(yōu)選地，公式（5）中，根據(jù)和取不同的值，執(zhí)行器故障可以分為以下幾種類型：

29、（1）、當(dāng)且時(shí)，是執(zhí)行器部分失效故障，也叫做乘性故障；

30、（2）、當(dāng)且時(shí)，是執(zhí)行器偏差故障，也叫做加性故障；

31、（3）、當(dāng)且時(shí)，執(zhí)行器既存在部分失效故障也存在偏差故障，稱為混合故障；

32、（4）、當(dāng)且時(shí)，執(zhí)行器不存在故障。

33、優(yōu)選地，s100中預(yù)設(shè)條件具體為：

34、（8）

35、式中，是四旋翼無(wú)人機(jī)的編號(hào)，與無(wú)人機(jī)相鄰，是第個(gè)無(wú)人機(jī)的位置，表示無(wú)人機(jī)和之間的相對(duì)位置，決定所需的編隊(duì)模式，是虛擬領(lǐng)航者的時(shí)變軌跡，是的一階微分，表示虛擬領(lǐng)航者的時(shí)變速度也即編隊(duì)隊(duì)形的參考線速度。

36、優(yōu)選地，s200包括：

37、s210：根據(jù)二階系統(tǒng)的一致性控制協(xié)議設(shè)計(jì)標(biāo)稱編隊(duì)控制器實(shí)現(xiàn)理想狀態(tài)下的平移運(yùn)動(dòng)，控制器形式為：

38、（9）

39、式中，和是正常數(shù)控制參數(shù)矩陣，其中，分別是x軸、y軸和z軸方向上的位置控制參數(shù)，分別是x軸、y軸和z軸方向上的速度控制參數(shù)，和分別是和的一階微分，是的二階微分，是能與第個(gè)四旋翼無(wú)人機(jī)發(fā)生相互作用的無(wú)人機(jī)動(dòng)態(tài)鄰域集，是第個(gè)四旋翼無(wú)人機(jī)與虛擬領(lǐng)航者之間的通信拓?fù)渲担绻男頍o(wú)人機(jī)與領(lǐng)航者能通信則的值為1，否則為0，是鄰接矩陣中的元素，的值由下式給出：

40、（10）

41、s220：結(jié)合平移動(dòng)力學(xué)（1）和執(zhí)行器故障模型中第三種情況，將存在偏差故障的平移動(dòng)力學(xué)模型改寫為：

42、（11）

43、?式中，是集總擾動(dòng)，包括執(zhí)行器偏差故障的影響和外部時(shí)變擾動(dòng)；

44、s230：設(shè)計(jì)高階滑模觀測(cè)器估計(jì)和補(bǔ)償集總擾動(dòng)，其具體形式為：

45、(12)

46、式中，是各四旋翼的位置估計(jì)誤差，和分別是各四旋翼速度和集總擾動(dòng)的估計(jì)值，是輔助參數(shù)保證二階可導(dǎo)，是正常數(shù)；

47、s240：設(shè)計(jì)位置環(huán)的魯棒控制器為：

48、（13）

49、式中，是低通濾波器，其中是濾波器的截止頻率；

50、s250：位置環(huán)的完整控制器為：

51、（14）

52、s260：每個(gè)四旋翼無(wú)人機(jī)的靜推力輸入如下：

53、(15)

54、式中是向量的第三維數(shù)據(jù)。

55、優(yōu)選地，s300包括：

56、s310：解耦位置子系統(tǒng)和姿態(tài)子系統(tǒng)，選擇任意的偏航角，期望的翻滾角和俯仰角可以由下式獲得：

57、(16)

58、式中，和分別是控制器的第一維和第二維數(shù)據(jù)；

59、s320：定義每個(gè)四旋翼無(wú)人機(jī)的參考姿態(tài)為：

60、（17）

61、姿態(tài)誤差設(shè)計(jì)為：

62、（18）

63、式中，分別是俯仰角、翻滾角和偏航角誤差；

64、s330：為姿態(tài)子系統(tǒng)設(shè)計(jì)理想情況下的標(biāo)稱姿態(tài)控制器實(shí)現(xiàn)各四旋翼的姿態(tài)穩(wěn)定，姿態(tài)控制器具體形式為：

65、（19）

66、式中，和分別是正常數(shù)控制增益矩陣，其中，分別是俯仰角、翻滾角和偏航角的姿態(tài)控制參數(shù)，分別是俯仰角速度、翻滾角速度和偏航角速度控制參數(shù)；

67、s340：結(jié)合旋轉(zhuǎn)動(dòng)力學(xué)和執(zhí)行器故障模型第三種情況，存在執(zhí)行器偏差故障的旋轉(zhuǎn)動(dòng)力學(xué)模型重寫成：

68、（20）

69、式中，是一種擾動(dòng)，其中是執(zhí)行器偏差的第二維至第四維數(shù)據(jù)，是外部時(shí)變干擾；

70、s350：設(shè)計(jì)高階滑模觀測(cè)器估計(jì)和補(bǔ)償集總擾動(dòng)，其形式為：

71、(21)

72、式中，是姿態(tài)的估計(jì)誤差，和分別是角速度和集總擾動(dòng)的估計(jì)值，是輔助參數(shù)，保證能二階可微，均為正常數(shù)；

73、s360：姿態(tài)環(huán)的魯棒控制器設(shè)計(jì)為：

74、（22）

75、?式中，是低通濾波器，其中是濾波器截止頻率。

76、優(yōu)選地，s400包括：

77、s410：根據(jù)完整動(dòng)力學(xué)模型（7），位置子系統(tǒng)的魯棒控制器（13）和姿態(tài)子系統(tǒng)的魯棒控制（22），將完整動(dòng)力學(xué)模型改寫成：

78、（23）

79、式中，是集總擾動(dòng)項(xiàng)；

80、s420：通過(guò)實(shí)時(shí)信息在線估計(jì)各無(wú)人機(jī)的升力系數(shù)，對(duì)執(zhí)行器故障進(jìn)行診斷，動(dòng)力學(xué)（23）重寫成：

81、（24）

82、式中，的具體形式如下：

83、（25）

84、s430：定義目標(biāo)函數(shù)如下：

85、（26）

86、關(guān)于的一階和二階導(dǎo)分別為：

87、（27）

88、式中，是迭代次數(shù)；

89、s440：判斷是否成立，其中是允許誤差，如果成立，則停止計(jì)算，如果不成立，則令梯度方向?yàn)椋?/p>

90、（28）

91、式中，，其中是適當(dāng)?shù)恼?shù)，其作用是為了保證是正定的；因此，梯度方向的具體形式為：

92、（29）

93、s450：從出發(fā)，沿方向作一維搜索找到相應(yīng)的學(xué)習(xí)步長(zhǎng)使下降最快，一維搜素的方式為：

94、（30）

95、s460：令，并迭代公式（27）-（30）直到，得到相應(yīng)的值，從而可以得到每個(gè)轉(zhuǎn)子執(zhí)行器部分失效故障時(shí)的升力系數(shù)，由此可以得到執(zhí)行器部分失效后的控制效能分配矩陣為矩陣，最終，每個(gè)四旋翼飛行器的每個(gè)電機(jī)的期望轉(zhuǎn)速為：

96、（31）。

97、上述一種具有抗干擾能力的四旋翼無(wú)人機(jī)編隊(duì)容錯(cuò)控制方法，在位置環(huán)和姿態(tài)環(huán)引入高階滑模觀測(cè)器估計(jì)未知時(shí)變擾動(dòng)和執(zhí)行器偏差故障，提高了無(wú)人集群系統(tǒng)的魯棒性和安全性；針對(duì)無(wú)人集群系統(tǒng)中各四旋翼可能遭遇的執(zhí)行器失效故障，提出了基于牛頓修正法的容錯(cuò)控制策略實(shí)時(shí)在線診斷執(zhí)行器失效故障，使系統(tǒng)具備良好的容錯(cuò)能力，保障了整個(gè)系統(tǒng)的安全穩(wěn)定運(yùn)行。所提出的編隊(duì)控制器簡(jiǎn)單，魯棒性強(qiáng)，穩(wěn)定性高，穩(wěn)態(tài)和動(dòng)態(tài)性能好。