本發(fā)明屬于無人機(jī)容錯，尤其涉及一種面向動力失效故障的無人機(jī)運(yùn)動規(guī)劃與容錯控制方法。

背景技術(shù)：

1、近年來，傳統(tǒng)無人機(jī)容錯理論方法遇到了新的挑戰(zhàn)，主要表現(xiàn)在缺乏應(yīng)對拒止環(huán)境的自主導(dǎo)航能力以及動力失效故障的快速調(diào)節(jié)能力。當(dāng)前的技術(shù)瓶頸包括：由于機(jī)動性能下降、飛行環(huán)境復(fù)雜、視覺感知距離受限、機(jī)載計(jì)算能力有限，無人機(jī)發(fā)生動力失效故障后最優(yōu)運(yùn)動規(guī)劃與容錯控制的挑戰(zhàn)性進(jìn)一步上升。為了確保動力失效無人機(jī)飛行過程的自主性、安全性、可靠性，其在拒止環(huán)境下的最優(yōu)運(yùn)動規(guī)劃、快速故障診斷、與容錯控制，是亟需研究的核心課題。

2、考慮到大多數(shù)現(xiàn)有運(yùn)動規(guī)劃方法適用于全姿態(tài)可控的無人機(jī)，如何針對動力失效后無人機(jī)機(jī)動性能降級、障礙規(guī)避、定位建圖性能下降等約束條件，設(shè)計(jì)最優(yōu)運(yùn)動規(guī)劃方法是影響其安全性能提升的一個瓶頸問題。盡管當(dāng)前研究工作為提升受損無人機(jī)飛行安全提供了一定的理論基礎(chǔ)，拒止環(huán)境下動力失效當(dāng)前無人機(jī)故障診斷方法適用于執(zhí)行器工作效率部分損失故障的精細(xì)估計(jì)，但是同時滿足實(shí)際應(yīng)用中多種動力失效模態(tài)下故障診斷快速性與精確性雙重要求的工作尚不多見。動力失效故障發(fā)生后無人機(jī)控制穩(wěn)定性嚴(yán)重下降，如何將無人機(jī)故障診斷結(jié)果和剩余動力約束納入控制器設(shè)計(jì)，提升動力失效無人機(jī)大姿態(tài)運(yùn)動下故障調(diào)節(jié)能力和容錯控制成功率仍是一個亟需解決的問題。

3、基于以上存在的問題，發(fā)明了一套強(qiáng)自主、快響應(yīng)、高安全的"運(yùn)動規(guī)劃"+"故障診斷+容錯控制”全回路容錯系統(tǒng)，確保動力失效故障發(fā)生后無人機(jī)具備在復(fù)雜未知環(huán)境下的自主導(dǎo)航與安全飛行能力。系統(tǒng)擁有低故障診斷時間，高診斷正確率；基于時空聯(lián)合優(yōu)化的運(yùn)動規(guī)劃方法與高可靠的容錯控制方法。能夠?qū)崿F(xiàn)對于無人機(jī)動力失效的快速響應(yīng)與智能控制。

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

1、本發(fā)明是針對現(xiàn)有技術(shù)的不足，提供了一種面向動力失效故障的無人機(jī)運(yùn)動規(guī)劃與容錯控制方法，該方法解決了故障無人機(jī)高速自轉(zhuǎn)引起的定位建圖、碰撞規(guī)避難題，以及突發(fā)故障對故障快速診斷、恢復(fù)穩(wěn)定性能的需求，提升了無人機(jī)實(shí)際飛行的自主安全性。

2、本發(fā)明是通過以下技術(shù)方案來實(shí)現(xiàn)的：

3、根據(jù)本發(fā)明的第二方面，提供一種面向動力失效故障的無人機(jī)運(yùn)動規(guī)劃與容錯控制方法，包括以下步驟：

4、s1：對動力失效無人機(jī)進(jìn)行動力學(xué)建模，建立運(yùn)動學(xué)和動力學(xué)模型；

5、s2：基于s1的建模結(jié)果，設(shè)計(jì)基于加速度自適應(yīng)優(yōu)化的無人機(jī)運(yùn)動規(guī)劃方法，從而

6、實(shí)現(xiàn)失效無人機(jī)自主導(dǎo)航和碰撞規(guī)避；

7、s3：通過設(shè)計(jì)觀測器估計(jì)無人機(jī)所受的合推力、扭矩是否與控制器輸出的合推力、扭矩匹配，診斷無人機(jī)是否受到失效故障；

8、s4：基于s3的故障診斷結(jié)果，設(shè)計(jì)基于非線性模型預(yù)測控制的容錯控制器，當(dāng)發(fā)生

9、故障時通過放棄偏航通道控制，確保對俯仰、滾轉(zhuǎn)以及高度三個通道的控制；

10、s5：通過設(shè)計(jì)氣動結(jié)構(gòu)偏轉(zhuǎn)氣流，降低無人機(jī)動力失效時的扭矩變化，從而降低無人機(jī)的自旋速度，確保無人機(jī)在失效故障情況下的準(zhǔn)確定位和穩(wěn)定控制。

11、進(jìn)一步地，所述步驟s1中建立無人機(jī)運(yùn)動學(xué)、動力學(xué)模型，其表達(dá)式如下：

12、

13、ivαb＝-wb×ivwb+τ-a(w)+τa；

14、其中，在運(yùn)動學(xué)方程中，m和ξ表示無人機(jī)的質(zhì)量和重心位置向量，t是合推力在機(jī)體坐標(biāo)系下的三維向量,ta是由于模型不匹配導(dǎo)致的干擾力，g表示重力加速度向量，zb表示旋轉(zhuǎn)矩陣的最后一列；在旋轉(zhuǎn)動力學(xué)中，表示四元數(shù)乘法，×表示叉乘運(yùn)算，q表示四元數(shù)，wb＝[p,q,r]t表示角速度，αb表示角加速度，iv表示慣性矩陣，τ表示由推力產(chǎn)生的力矩，τα表示由于模型不匹配產(chǎn)生的干擾力矩，a(w)＝[0,0,-γw,]t表示空氣阻力產(chǎn)生的氣動力矩；

15、綜合推力和旋翼引起的力矩取決于四個旋翼的推力，其表達(dá)式如下：

16、

17、其中，‖t‖表示合推力向量的模，t＝[t0,t1,t2,t3]t代表四個電機(jī)分別產(chǎn)生的推力，g表示混控矩陣

18、

19、其中rx,i和ry,i分別是半軸機(jī)臂ri在機(jī)體坐標(biāo)系中的x和y分量，kt表示力矩系數(shù)；

20、由于無人機(jī)在實(shí)際飛行時存在電機(jī)轉(zhuǎn)速響應(yīng)延遲，因此需要考慮電機(jī)動力學(xué)模型，采用一階模型表示電機(jī)響應(yīng)動力學(xué)，表達(dá)式如下：

21、

22、其中σ為由實(shí)驗(yàn)數(shù)據(jù)計(jì)算得到的時間常數(shù)，ui是第i個電機(jī)的輸入指令。

23、進(jìn)一步地，所述步驟s2具體為：采用了minco來生成最小控制代價的時空軌跡，實(shí)現(xiàn)平面輸出軌跡的線性復(fù)雜度時空變形；然后從平面輸出中恢復(fù)完整狀態(tài)；對于一個具有n＝2s-1的n維m段多項(xiàng)式軌跡，其中s是最優(yōu)化求解狀態(tài)的階數(shù)，構(gòu)建了一個線性復(fù)雜度的映射，表達(dá)式如下：

24、c＝m(q,t)；

25、其中是多項(xiàng)式系數(shù)矩陣，m(q,t)是從路徑點(diǎn)和時間分配到c的平滑映射；然后優(yōu)化變量(q,t)轉(zhuǎn)換為{q,t}；第i段軌跡片段pi(t)表示為在軌跡優(yōu)化中，在每段軌跡代價函數(shù)包含飛行時間、狀態(tài)約束、避障三個約束條件：

26、

27、其中λ是權(quán)重向量，用于在各個代價函數(shù)之間進(jìn)行權(quán)衡。

28、進(jìn)一步地，所述飛行時間、狀態(tài)約束、避障三個約束條件，具體為：

29、(4.1)飛行時間代價jt:為了加速導(dǎo)航進(jìn)程，首先通過最小化總時間來實(shí)現(xiàn)；表達(dá)式如下：

30、

31、(4.2)狀態(tài)約束代價js:限制線速度和加速度的大小以確保可行性；代價函數(shù)的公式如下：

32、js＝j(luò)s,v+js,a；

33、

34、其中vmax和amax是線速度和加速度的閾值；js,v是速度項(xiàng)的約束代價，js,a是加速度項(xiàng)的約束代價；

35、進(jìn)一步地，利用加速度可達(dá)性分析對剩余機(jī)動性和軌跡優(yōu)化參數(shù)之間進(jìn)行權(quán)衡，為保持在zi軸方向上的平衡，忽略水平方向的空氣阻力，無人機(jī)在z軸加速度為0，則得到：

36、

37、其中ades是無人機(jī)水平方向加速度，ts表示四個電機(jī)作用在無人機(jī)的合力；每個電機(jī)的輸出推力滿足0≤t≤tmax，tmax表示單個電機(jī)輸出的最大推力；且0≤ts≤4tmax；若發(fā)生動力失效故障后，第1號電機(jī)失效t1＝0，則為了保證無人機(jī)平衡此時需要其對側(cè)電機(jī)即第0號電機(jī)減小輸出推力，假設(shè)此時t0≈0，則得到此時無人機(jī)滿足0≤ts≤2tmax，即無人機(jī)發(fā)生動力失效后推導(dǎo)得到如下表達(dá)式:

38、

39、進(jìn)一步再得到：

40、

41、(1.3)避障代價jc：為了確保生成無碰撞軌跡，使用歐幾里德符號距離場esdf地圖獲取相鄰障礙物的距離和相應(yīng)的梯度信息；其表達(dá)式如下：

42、

43、其中e(·)是與從esdf獲取的最近障礙物之間的距離，ds是安全距離閾值；最終軌跡生成后，沿軌跡采樣點(diǎn)及其高階導(dǎo)數(shù)，并微分平坦性恢復(fù)期望的完整狀態(tài)和輸入，作為軌跡跟蹤控制的參考輸入。

44、進(jìn)一步地，所述步驟s4中設(shè)計(jì)基于非線性模型預(yù)測控制的容錯控制器，具體為：

45、利用非線性模型預(yù)測控制通過在滾動時間域內(nèi)求解有限時間最優(yōu)控制問題來生成控制律；即通過表達(dá)的狀態(tài)和控制輸入u＝[u1,u2,u3,u4]t，在離散時間形式下建模為xk+1＝f(xk,uk)；對于參考軌跡，代價函數(shù)是在有限時間域內(nèi)參考狀態(tài)和實(shí)際狀態(tài)之間的誤差；采用n步等間隔tn∈[t,t+h]，步長為dt＝h/n，其中h表示時間段長度；因此，得到如下約束優(yōu)化表達(dá)式：

46、

47、s.t.xi+1＝f(xi,ui),i＝k,k+1,…,k+n-1；

48、

49、其中k,n,xk:k+n分別表示當(dāng)前時間步、采樣時間步和預(yù)測的狀態(tài)軌跡；q,qn,r是權(quán)重矩陣；u,是控制律的上下限，w,是角速度的上下限；在正常飛行條件下，控制輸入的限制設(shè)定為如果第i個動力失效并被故障檢測模塊識別，將u的第i個元素調(diào)整為

50、所述代價函數(shù)包含兩個部分：第一部分是與u和x相關(guān)的每個時間步累積的運(yùn)行代價，第二部分是依賴于終端狀態(tài)的終端代價；利用偏航力矩進(jìn)行偏航控制，偏航控制的有效性相比俯仰和橫滾約低一個數(shù)量級，使得偏航控制易受電機(jī)飽和影響；

51、首先確定姿態(tài)誤差：

52、

53、其中qr是參考姿態(tài)，q是當(dāng)前姿態(tài)；

54、將姿態(tài)誤差分解為z和xy分量：

55、

56、構(gòu)建運(yùn)行代價的向量yi和對角權(quán)重矩陣q：

57、

58、q＝diag(qp,qxy,qz,qv,qw,qt,qu)；

59、qn＝diag(qp,qxy,qz,qv,qw,qt)；

60、代價函數(shù)中的參考值是預(yù)測時間域內(nèi)所有時間點(diǎn)的懸停參考值或軌跡中的采樣點(diǎn)；在動力失效診斷確認(rèn)發(fā)生故障后，和偏航運(yùn)動相關(guān)的權(quán)重系數(shù)設(shè)為零，其余權(quán)重系數(shù)不變；強(qiáng)制將其推力方向與參考對齊，同時放棄偏航運(yùn)動的控制。

61、進(jìn)一步地，當(dāng)無人機(jī)動力失效時，需要快速診斷故障信息，基于電機(jī)轉(zhuǎn)速反饋信息，采用如下方式進(jìn)行診斷；首先根據(jù)無人機(jī)動力學(xué)方程可知：

62、

63、其中tf是估計(jì)的合推力，每個推力由反饋的電機(jī)轉(zhuǎn)速結(jié)合推力系數(shù)求解得到，表示通過濾波器估計(jì)得到的加速度，表示估計(jì)的外力；表示估計(jì)的外力矩，αb,f、wb,f、τf分別表示通過濾波器得到的角加速度、角速度、以及力矩；進(jìn)而求解外力的和，表示為：

64、

65、根據(jù)混控矩陣得到：

66、

67、其中表示四個電機(jī)中推力的異常數(shù)值，結(jié)合控制器輸出的控制律t＝[t0,t1,t2,t3]t，則按照以下規(guī)則實(shí)現(xiàn)診斷：

68、

69、因此，當(dāng)某個電機(jī)對應(yīng)估計(jì)推力與控制器輸出量的比值超過臨界值0.8時，則認(rèn)為該電機(jī)發(fā)生動力失效故障，無人機(jī)控制器、規(guī)劃器將切換到容錯控制模式。

70、進(jìn)一步地，所述步驟s5具體為：由于無人機(jī)動力失效時會偏航力矩不平衡而進(jìn)入自旋；為降低每一個槳葉旋轉(zhuǎn)產(chǎn)生的扭矩即降低槳葉失效時整個系統(tǒng)受到的扭矩，從而降低系統(tǒng)的自旋速度；進(jìn)而設(shè)計(jì)了一種氣動結(jié)構(gòu)，其氣動特征與槳葉旋轉(zhuǎn)方向相關(guān)，在無人機(jī)上與螺旋槳一樣呈對稱安裝，能夠偏轉(zhuǎn)螺旋槳的下洗氣流，產(chǎn)生和螺旋槳相反方向的扭矩，用于抵消一部分螺旋槳旋轉(zhuǎn)產(chǎn)生的偏航扭矩，最終實(shí)現(xiàn)對動力失效情況下螺旋槳旋轉(zhuǎn)產(chǎn)生扭矩對于無人機(jī)系統(tǒng)的控制。

71、根據(jù)本發(fā)明的第二方面，提供一種電子設(shè)備，其特征在于，包括：

72、一個或多個處理器；

73、存儲器，用于存儲一個或多個程序；

74、當(dāng)所述一個或多個程序被所述一個或多個處理器執(zhí)行，使得所述一個或多個處理器實(shí)現(xiàn)所述面向動力失效故障的無人機(jī)運(yùn)動規(guī)劃與容錯控制的方法。

75、根據(jù)本發(fā)明的第三方面，提供一種計(jì)算機(jī)可讀存儲介質(zhì)，其上存儲有計(jì)算機(jī)指令，該指令被處理器執(zhí)行時實(shí)現(xiàn)所述面向動力失效故障的無人機(jī)運(yùn)動規(guī)劃與容錯控制方法的步驟。

76、本技術(shù)的實(shí)施例提供的技術(shù)方案可以包括以下有益效果：

77、1、本發(fā)明設(shè)計(jì)了一種能夠應(yīng)用于失效無人機(jī)的位置姿態(tài)控制算法，能夠通過給定預(yù)設(shè)目標(biāo)位置求解無人機(jī)的期望姿態(tài)，推力和軌跡。這種方法能夠提高無人機(jī)在失效狀態(tài)下的控制精度與穩(wěn)定性，并為無人機(jī)在失效狀態(tài)下的位置控制與導(dǎo)航提供技術(shù)基礎(chǔ)。通過卸載一個槳葉實(shí)現(xiàn)動力失效故障。

78、2、本發(fā)明為提高無人機(jī)在失效狀態(tài)下的可控性，設(shè)計(jì)了一種拒止環(huán)境下的無人機(jī)導(dǎo)航算法。無需依賴外部定位和運(yùn)算設(shè)備，僅通過機(jī)載傳感器即可實(shí)現(xiàn)無人機(jī)在失效狀態(tài)下的自主導(dǎo)航。并且使用的雷達(dá)傳感器使得無人機(jī)可以在強(qiáng)光，暗光，與室外環(huán)境下進(jìn)行導(dǎo)航。

79、3、本發(fā)明設(shè)計(jì)了一種診斷算法，能夠在無人機(jī)槳葉受到破壞失去動力后，快速診斷出失效的槳葉位置與程度，并使容錯控制算法介入無人機(jī)控制，保障無人機(jī)槳葉受到破壞后由正常狀態(tài)安全、快速轉(zhuǎn)入容錯控制狀態(tài)。實(shí)驗(yàn)中整個系統(tǒng)的診斷時間為125ms，并在失效診斷后快速恢復(fù)并按預(yù)定軌跡繼續(xù)飛行。