本發(fā)明涉及三旋翼無人機容錯控制技術(shù)，具體講,涉及針對三旋翼無人機舵機堵塞故障的容錯控制方法。

背景技術(shù)：

近年來，多旋翼無人機在高空攝影、災(zāi)后救援、環(huán)境監(jiān)測等軍事和民用領(lǐng)域得到越來越廣泛的應(yīng)用。與傳統(tǒng)四旋翼無人機、六旋翼無人機不同，三旋翼無人機通常由三個電機和一個舵機組成，結(jié)構(gòu)更簡單、成本更低、能耗更小、機動性更強。三旋翼無人機依靠三個電機的轉(zhuǎn)動及舵機的偏轉(zhuǎn)實現(xiàn)俯仰、滾轉(zhuǎn)、偏航等動作，受無人機飛行穩(wěn)定性及自身工藝等影響，舵機極易發(fā)生堵塞故障，對無人機的飛行性能產(chǎn)生嚴重影響。

目前國內(nèi)外很多研究機構(gòu)已經(jīng)開始致力于三旋翼無人機的位姿控制研究，但是針對舵機發(fā)生堵塞故障時的位姿控制尚沒有相關(guān)研究文獻。三旋翼無人機作為一個四輸入六輸出的欠驅(qū)動系統(tǒng)，當舵機發(fā)生堵塞故障時，輸入量減少一個，這與四旋翼無人機執(zhí)行器發(fā)生故障的情況類似。針對四旋翼無人機執(zhí)行器發(fā)生故障時的姿態(tài)控制問題，目前采用較為廣泛的容錯控制策略大致有兩種：被動容錯和主動容錯。被動容錯利用控制器的魯棒性使得控制器對故障信息不敏感，從而達到容錯控制的目的；而主動容錯則通過故障診斷與故障隔離能夠在線檢測并分離出所發(fā)生故障，再根據(jù)故障模式進行故障重構(gòu)，以此達到容錯控制目的。

對于上述兩種容錯控制策略，國內(nèi)外很多研究單位，如麻省理工學(xué)院、瑞士聯(lián)邦理工大學(xué)、康考迪亞大學(xué)、南京航空航天大學(xué)、北京航空航天大學(xué)等，基于多種線性或非線性控制方法開展了相關(guān)研究，如變增益PID、反步法、滑?？刂啤⒛Ｐ蛥⒖甲赃m應(yīng)、模型預(yù)測控制等方法，并且對這些方法的控制效果具有數(shù)值仿真或?qū)嶋H飛行實驗的驗證(書籍：Automatic Flight Control Systems-Latest Development；著者：Youmin Zhang，Annas Chamseddine；出版年月：2012年；文章題目：Fault Tolerant and Flight Control Techniques with application to a Quadrotor UAV Testbed；頁碼：119–150)。

但是，當前各種容錯控制方法均有其各自的局限性。比如：在對執(zhí)行器故障進行動力學(xué)建模時，將其視為外部擾動力矩，進行了較大程度的近似，難以反映執(zhí)行器故障對無人機的真實影響(期刊：控制理論與應(yīng)用；著者：楊薈憭，姜斌，張柯；出版年月：2014年；文章題目：四旋翼直升機姿態(tài)系統(tǒng)的直接自我修復(fù)控制；頁碼：1053-1060)；部分容錯控制方法在平衡點處對四旋翼無人機的動力學(xué)模型進行了線性化處理，理論證明只能得到平衡點附近的穩(wěn)定結(jié)論，當執(zhí)行器發(fā)生故障時，飛行器姿態(tài)會發(fā)生突變，且多數(shù)情況下飛行器姿態(tài)會偏離平衡點較大位置，控制器應(yīng)用范圍難以保證(期刊：IEEE Transactions on Control Systems Technology；著者：Z.T.Dydek，A.M.Annaswamy，E.Lavretsky；出版年月：2013年7月；文章題目：Adaptive Control of Quadrotor UAVs：a Design Trade Study with Flight Evaluations；頁碼：1400–1406)；被動容錯方法應(yīng)用范圍有限，難以做到對外界擾動和執(zhí)行器故障魯棒性的兼容性，控制效果較差，而主動容錯控制方法則需要進行故障診斷和故障隔離，并在此基礎(chǔ)上進行故障重構(gòu)，算法復(fù)雜，難以實現(xiàn)工程應(yīng)用(期刊：Proceedings of the Institution of Mechanical Engineers,Part I,Journal of Systems and Control Engineering；著者：T.Li,Y.M.Zhang,B.W.Gordon；出版年月：2012年1月；文章題目：Passive and Active Nonlinear Fault-Tolerant Control of a Quadrotor UAV Based on Sliding Mode Control Technique；頁碼：12-23)。

技術(shù)實現(xiàn)要素：

填補現(xiàn)有研究對象的空白，針對三旋翼無人機發(fā)生舵機堵塞故障時的姿態(tài)控制問題開展研究。本發(fā)明采用的技術(shù)方案是，針對三旋翼無人機舵機堵塞故障的容錯控制方法，步驟如下：首先定義慣性坐標系{I}、機體坐標系{B}和目標坐標系{B_d}，通過分析舵機對三旋翼無人機的作用原理，并考慮外部擾動對其動力學(xué)特性的影響，得到三旋翼無人機執(zhí)行器發(fā)生故障時的非線性動力學(xué)模型：

式(1)中各變量定義如下：ω＝[ω₁ ω₂ ω₃]^T∈R^3×1表示機體坐標系{B}相對于慣性坐標系{I}的姿態(tài)角速度，ω₁,ω₂,ω₃分別表示滾轉(zhuǎn)角速度、俯仰角速度和偏航角速度，[·]^T表示矩陣的轉(zhuǎn)置，∈表示集合間的“屬于”關(guān)系，R^3×1表示3行1列的實數(shù)向量，表示求取ω的一階時間導(dǎo)數(shù)；J＝diag{[J₁ J₂ J₃]^T}∈R^3×3為轉(zhuǎn)動慣量矩陣，diag{[J₁ J₂ J₃]^T}表示向量[J₁ J₂ J₃]張成的對角矩陣，J₁,J₂,J₃分別表示繞各個坐標軸的轉(zhuǎn)動慣量；S(ω)表示求取ω對應(yīng)的反對稱矩陣；為一系數(shù)矩陣，其中l(wèi)表示前面某一電機中心到無人機軸心的距離，l₃表示舵機中心到無人機軸心的距離，α表示前面兩個電機連線與某一電機和無人機軸心連線之間的夾角，k為電機的升力系數(shù)，δ_f為舵機發(fā)生堵塞時的偏轉(zhuǎn)角度，l,l₃,α,k均為已知常數(shù)，δ_f為未知常數(shù)，sin(·)和cos(·)分別表示正弦和余弦函數(shù)；f_δ＝[f_δ1 f_δ2 f_δ3]^T∈R^3×1表示故障發(fā)生后的升力向量，f_δ1,f_δ2,f_δ3分別表示故障發(fā)生后三個電機產(chǎn)生的升力，D＝diag{[d₁ d₂ d₃]^T}∈R^3×3為外部擾動矩陣，diag{[d₁ d₂ d₃]^T}表示向量[d₁ d₂ d₃]張成的對角矩陣，d₁,d₂,d₃分別表示作用于各個通道的外部擾動；

定義變量λ₁＝-l₃cosδ_f，λ₂＝kcosδ_f+l₃sinδ_f，則λ₁,λ₂為未知常數(shù)，A(δ_f)可寫為式(1)可表示為

為避免姿態(tài)表示奇異性問題，采用基于單位四元數(shù)的姿態(tài)表示方法，機體坐標系{B}在慣性坐標系{I}下的表達用“等效軸角坐標系”方法，將{B}和{I}重合，將{B}繞矢量k₀∈R^3×1按右手定則旋轉(zhuǎn)角，得到當前姿態(tài)單位四元數(shù)其中且滿足k₀∈R^3×1為定義在坐標系{I}中的任意單位矢量，為坐標系{B}繞矢量k旋轉(zhuǎn)的任意角度；由機體坐標系{B}到慣性坐標系{I}的坐標變換矩陣用四元數(shù)表示為I₃為3×3的單位矩陣，S(q_v)表示求取q_v對應(yīng)的反對稱矩陣，同理，目標坐標系{B_d}在慣性坐標系{I}下的表達也用“等效軸角坐標系”方法，將{B_d}和{I}重合，將{B_d}繞矢量k_d∈R^3×1按右手定則旋轉(zhuǎn)角，得到目標姿態(tài)單位四元數(shù)其中且滿足k_d∈R^3×1同樣為定義在坐標系{I}中的任意單位矢量，為坐標系{B_d}繞矢量k_d旋轉(zhuǎn)的任意角度；由目標坐標系{B_d}到慣性坐標系{I}的坐標變換矩陣用四元數(shù)表示為S(q_vd)表示求取q_vd對應(yīng)的反對稱矩陣，為了描述三旋翼無人機當前姿態(tài)與目標姿態(tài)之間的差異，定義姿態(tài)誤差四元數(shù)

其中e₀和e_v同樣滿足由目標坐標系{B_d}到機體坐標系{B}的坐標變換矩陣示為S(e_v)表示求取e_v對應(yīng)的反對稱矩陣，定義角速度誤差其中ω_d∈R^3×1表示目標坐標系{B_d}相對于慣性坐標系{I}的姿態(tài)角速度；

為了對三旋翼無人機舵機堵塞故障進行更有針對性的容錯控制，采用基于自適應(yīng)滑模方法的觀測器技術(shù)對故障進行觀測，設(shè)計觀測器為：

其中表示對ω的估計值，表示求取的一階時間導(dǎo)數(shù)，v＝[v₁ v₂ v₃]^T∈R^3×1，表示求取v的一階時間導(dǎo)數(shù)，定義ω的估計誤差為分別表示對λ₁,λ₂的估計值，SIG2＝[k₂₁sign(e_ω1) k₂₂sign(e_ω2) k₂₃sign(e_ω3)]^T，SIG1＝[k₁₁|e_ω1|^1/2sign(e_ω1) k₁₂|e_ω2|^1/2sign(e_ω2) k₁₃|e_ω3|^1/2sign(e_ω3)]^T，其中k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均為正常數(shù)，e_ω1,e_ω2,e_ω3為e_ω的三個元素，sign(·)表示符號函數(shù)，|·|^1/2表示求取絕對值的次方；

定義變量z_1i＝|e_ωi|^1/2sign(e_ωi),z_2i＝v_i,i＝1,2,3，設(shè)計控制器為：

其中表示求取矩陣的逆矩陣，表示求取ω_d的一階時間導(dǎo)數(shù)，G＝diag{[g₁ g₂ g₃]^T}∈R^3×3為正常數(shù)增益矩陣，diag{[g₁ g₂ g₃]^T}表示向量[g₁ g₂ g₃]張成的對角矩陣，g₁,g₂,g₃均為正常數(shù)。為外部擾動估計矩陣，表示向量張成的對角矩陣，是對d₁,d₂,d₃的估計值；

若設(shè)計和的自適應(yīng)律滿足：

其中ε₁,ε₂,m,n,γ₁,γ₂,γ₃均為正常數(shù)，分別表示滾轉(zhuǎn)角速度誤差、俯仰角速度誤差和偏航角速度誤差，則姿態(tài)誤差四元數(shù)和角速度誤差漸近穩(wěn)定。

驗證步驟具體是，采用基于Lyapunov的分析方法可以證明當時間趨于無窮時，和e_v分別漸近收斂到[0 0 0]^T。

本發(fā)明的特點及有益效果是：

本發(fā)明較早地針對三旋翼無人機發(fā)生舵機堵塞故障時的姿態(tài)控制問題采用基于觀測器技術(shù)的方法進行研究。該方法既能對故障信息進行有效估計，對其進行很好地抑制，又不需要主動容錯控制所需要的故障隔離，大大地減少了計算量，提高了控制效率。實驗表明，該方法對三旋翼無人機舵機堵塞故障具有較好的魯棒性，當三旋翼無人機舵機發(fā)生堵塞故障時，無人機能夠較快地克服故障影響，保持姿態(tài)穩(wěn)定。

附圖說明：

圖1是本發(fā)明所用實驗平臺。

圖2是容錯控制實驗效果圖，圖中：

a是姿態(tài)誤差四元數(shù)變化曲線；

b是角速度誤差變化曲線；

c是控制輸入變化曲線；

d是電機轉(zhuǎn)速變化曲線；

e是角速度估計誤差變化曲線；

f是舵機堵塞故障估計值變化曲線；

g是外部擾動估計值變化曲線。

具體實施方式

本發(fā)明涉及一種三旋翼無人機容錯控制問題。針對三旋翼無人機舵機發(fā)生堵塞故障時的姿態(tài)控制問題，提出一種基于自適應(yīng)滑模觀測器技術(shù)的非線性容錯控制方法。

為填補現(xiàn)有研究對象的空白，針對三旋翼無人機發(fā)生舵機堵塞故障時的姿態(tài)控制問題開展研究。本發(fā)明采用的技術(shù)方案是基于自適應(yīng)滑模觀測器的容錯控制方法，利用觀測器對故障信息進行觀測，控制器的設(shè)計采用觀測器的觀測信息，對執(zhí)行器故障進行補償。

具體步驟如下：首先定義慣性坐標系{I}、機體坐標系{B}和目標坐標系{B_d}，通過分析舵機對三旋翼無人機的作用原理，并考慮外部擾動對其動力學(xué)特性的影響，得到三旋翼無人機執(zhí)行器發(fā)生故障時的非線性動力學(xué)模型：

式(1)中各變量定義如下：ω＝[ω₁ ω₂ ω₃]^T∈R^3×1表示機體坐標系{B}相對于慣性坐標系{I}的姿態(tài)角速度，ω₁,ω₂,ω₃分別表示滾轉(zhuǎn)角速度、俯仰角速度和偏航角速度，[·]^T表示矩陣的轉(zhuǎn)置，∈表示集合間的“屬于”關(guān)系，R^3×1表示3行1列的實數(shù)向量，表示求取ω的一階時間導(dǎo)數(shù)，下同；J＝diag{[J₁ J₂ J₃]^T}∈R^3×3為轉(zhuǎn)動慣量矩陣，diag{[J₁ J₂ J₃]^T}表示向量[J₁ J₂ J₃]張成的對角矩陣，J₁,J₂,J₃分別表示繞各個坐標軸的轉(zhuǎn)動慣量；S(ω)表示求取ω對應(yīng)的反對稱矩陣；為一系數(shù)矩陣，其中l(wèi)表示前面某一電機中心到無人機軸心的距離，l₃表示舵機中心到無人機軸心的距離，α表示前面兩個電機連線與某一電機和無人機軸心連線之間的夾角，k為電機的升力系數(shù)，δ_f為舵機發(fā)生堵塞時的偏轉(zhuǎn)角度，l,l₃,α,k均為已知常數(shù)，δ_f為未知常數(shù)，sin(·)和cos(·)分別表示正弦和余弦函數(shù)；f_δ＝[f_δ1 f_δ2 f_δ3]^T∈R^3×1表示故障發(fā)生后的升力向量，f_δ1,f_δ2,f_δ3分別表示故障發(fā)生后三個電機產(chǎn)生的升力，D＝diag{[d₁ d₂ d₃]^T}∈R^3×3為外部擾動矩陣，diag{[d₁ d₂ d₃]^T}表示向量[d₁ d₂ d₃]張成的對角矩陣，d₁,d₂,d₃分別表示作用于各個通道的外部擾動。

為方便分析，定義變量λ₁＝-l₃cosδ_f，λ₂＝kcosδ_f+l₃sinδ_f，則λ₁,λ₂為未知常數(shù)，A(δ_f)可寫為式(1)可表示為

為避免姿態(tài)表示奇異性問題，采用基于單位四元數(shù)的姿態(tài)表示方法，機體坐標系{B}在慣性坐標系{I}下的表達用“等效軸角坐標系”方法，將{B}和{I}重合，將{B}繞矢量k₀∈R^3×1按右手定則旋轉(zhuǎn)角，得到當前姿態(tài)單位四元數(shù)其中且滿足k₀∈R^3×1為定義在坐標系{I}中的任意單位矢量，為坐標系{B}繞矢量k旋轉(zhuǎn)的任意角度；由機體坐標系{B}到慣性坐標系{I}的坐標變換矩陣用四元數(shù)表示為I₃為3×3的單位矩陣，下同，S(q_v)表示求取q_v對應(yīng)的反對稱矩陣，同理，目標坐標系{B_d}在慣性坐標系{I}下的表達也可以用“等效軸角坐標系”方法，將{B_d}和{I}重合，將{B_d}繞矢量k_d∈R^3×1按右手定則旋轉(zhuǎn)角，得到目標姿態(tài)單位四元數(shù)其中且滿足k_d∈R^3×1同樣為定義在坐標系{I}中的任意單位矢量，為坐標系{B_d}繞矢量k_d旋轉(zhuǎn)的任意角度；由目標坐標系{B_d}到慣性坐標系{I}的坐標變換矩陣用四元數(shù)表示為S(q_vd)表示求取q_vd對應(yīng)的反對稱矩陣。為了描述三旋翼無人機當前姿態(tài)與目標姿態(tài)之間的差異，定義姿態(tài)誤差四元數(shù)

其中e₀和e_v同樣滿足由目標坐標系{B_d}到機體坐標系{B}的坐標變換矩陣示為S(e_v)表示求取e_v對應(yīng)的反對稱矩陣。定義角速度誤差其中ω_d∈R^3×1表示目標坐標系{B_d}相對于慣性坐標系{I}的姿態(tài)角速度。

為了對三旋翼無人機舵機堵塞故障進行更有針對性的容錯控制，采用基于自適應(yīng)滑模方法的觀測器技術(shù)對故障進行觀測，設(shè)計觀測器為：

其中表示對ω的估計值，表示求取的一階時間導(dǎo)數(shù)，v＝[v₁ v₂ v₃]^T∈R^3×1，表示求取v的一階時間導(dǎo)數(shù)，定義ω的估計誤差為分別表示對λ₁,λ₂的估計值，SIG2＝[k₂₁sign(e_ω1) k₂₂sign(e_ω2) k₂₃sign(e_ω3)]^T，SIG1＝[k₁₁|e_ω1|^1/2sign(e_ω1) k₁₂|e_ω2|^1/2sign(e_ω2) k₁₃|e_ω3|^1/2sign(e_ω3)]^T，其中k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均為正常數(shù)，e_ω1,e_ω2,e_ω3為e_ω的三個元素，sign(·)表示符號函數(shù)，|·|^1/2表示求取絕對值的次方。

定義變量z_1i＝|e_ωi|^1/2sign(e_ωi),z_2i＝v_i,i＝1,2,3，設(shè)計控制器為：

其中表示求取矩陣的逆矩陣，表示求取ω_d的一階時間導(dǎo)數(shù)，G＝diag{[g₁ g₂ g₃]^T}∈R^3×3為正常數(shù)增益矩陣，diag{[g₁ g₂ g₃]^T}表示向量[g₁ g₂ g₃]張成的對角矩陣，g₁,g₂,g₃均為正常數(shù)。為外部擾動估計矩陣，表示向量張成的對角矩陣，是對d₁,d₂,d₃的估計值。

若設(shè)計和的自適應(yīng)律滿足：

其中ε₁,ε₂,m,n,γ₁,γ₂,γ₃均為正常數(shù)，分別表示滾轉(zhuǎn)角速度誤差、俯仰角速度誤差和偏航角速度誤差，則姿態(tài)誤差四元數(shù)和角速度誤差漸近穩(wěn)定。采用基于Lyapunov的分析方法可以證明當時間趨于無窮時，和e_v分別漸近收斂到[0 0 0]^T。

可實現(xiàn)三旋翼無人機執(zhí)行器發(fā)生故障時的姿態(tài)控制，包括下列步驟：

首先定義慣性坐標系{I}、機體坐標系{B}和目標坐標系{B_d}，通過分析舵機對三旋翼無人機的作用原理，并考慮外部擾動對其動力學(xué)特性的影響，得到三旋翼無人機執(zhí)行器發(fā)生故障時的非線性動力學(xué)模型：

式(1)中各變量定義如下：ω＝[ω₁ ω₂ ω₃]^T∈R^3×1表示機體坐標系{B}相對于慣性坐標系{I}的姿態(tài)角速度，ω₁,ω₂,ω₃分別表示滾轉(zhuǎn)角速度、俯仰角速度和偏航角速度，[·]^T表示矩陣的轉(zhuǎn)置，∈表示集合間的“屬于”關(guān)系，R^3×1表示3行1列的實數(shù)向量，表示求取ω的一階時間導(dǎo)數(shù)，下同；J＝diag{[J₁ J₂ J₃]^T}∈R^3×3為轉(zhuǎn)動慣量矩陣，diag{[J₁ J₂ J₃]^T}表示向量[J₁ J₂ J₃]張成的對角矩陣，J₁,J₂,J₃分別表示繞各個坐標軸的轉(zhuǎn)動慣量；S(ω)表示求取ω對應(yīng)的反對稱矩陣；為一系數(shù)矩陣，其中l(wèi)表示前面某一電機中心到無人機軸心的距離，l₃表示舵機中心到無人機軸心的距離，α表示前面兩個電機連線與某一電機和無人機軸心連線之間的夾角，k為電機的升力系數(shù)，δ_f為舵機發(fā)生堵塞時的偏轉(zhuǎn)角度，l,l₃,α,k均為已知常數(shù)，δ_f為未知常數(shù)，sin(·)和cos(·)分別表示正弦和余弦函數(shù)；f_δ＝[f_δ1 f_δ2 f_δ3]^T∈R^3×1表示故障發(fā)生后的升力向量，f_δ1,f_δ2,f_δ3分別表示故障發(fā)生后三個電機產(chǎn)生的升力，D＝diag{[d₁ d₂ d₃]^T}∈R^3×3為外部擾動矩陣，diag{[d₁ d₂ d₃]^T}表示向量[d₁ d₂ d₃]張成的對角矩陣，d₁,d₂,d₃分別表示作用于各個通道的外部擾動。

為方便分析，定義變量λ₁＝-l₃cosδ_f，λ₂＝kcosδ_f+l₃sinδ_f，則λ₁,λ₂為未知常數(shù)，A(δ_f)可寫為式(1)可表示為

為避免姿態(tài)表示奇異性問題，采用基于單位四元數(shù)的姿態(tài)表示方法，機體坐標系{B}在慣性坐標系{I}下的表達用“等效軸角坐標系”方法，將{B}和{I}重合，將{B}繞矢量k₀∈R^3×1按右手定則旋轉(zhuǎn)角，得到當前姿態(tài)單位四元數(shù)其中且滿足k₀∈R^3×1為定義在坐標系{I}中的任意單位矢量，為坐標系{B}繞矢量k旋轉(zhuǎn)的任意角度；由機體坐標系{B}到慣性坐標系{I}的坐標變換矩陣用四元數(shù)表示為I₃為3×3的單位矩陣，下同，S(q_v)表示求取q_v對應(yīng)的反對稱矩陣，同理，目標坐標系{B_d}在慣性坐標系{I}下的表達也可以用“等效軸角坐標系”方法，將{B_d}和{I}重合，將{B_d}繞矢量k_d∈R^3×1按右手定則旋轉(zhuǎn)角，得到目標姿態(tài)單位四元數(shù)其中且滿足k_d∈R^3×1同樣為定義在坐標系{I}中的任意單位矢量，為坐標系{B_d}繞矢量k_d旋轉(zhuǎn)的任意角度；由目標坐標系{B_d}到慣性坐標系{I}的坐標變換矩陣用四元數(shù)表示為S(q_vd)表示求取q_vd對應(yīng)的反對稱矩陣。為了描述三旋翼無人機當前姿態(tài)與目標姿態(tài)之間的差異，定義姿態(tài)誤差四元數(shù)

其中e₀和e_v同樣滿足由目標坐標系{B_d}到機體坐標系{B}的坐標變換矩陣示為S(e_v)表示求取e_v對應(yīng)的反對稱矩陣。定義角速度誤差其中ω_d∈R^3×1表示目標坐標系{B_d}相對于慣性坐標系{I}的姿態(tài)角速度。

為了對三旋翼無人機舵機堵塞故障進行更有針對性的容錯控制，采用基于自適應(yīng)滑模方法的觀測器技術(shù)對故障進行觀測，設(shè)計觀測器為：

其中表示對ω的估計值，表示求取的一階時間導(dǎo)數(shù)，v＝[v₁ v₂ v₃]^T∈R^3×1，表示求取v的一階時間導(dǎo)數(shù)，定義ω的估計誤差為分別表示對λ₁,λ₂的估計值，SIG2＝[k₂₁sign(e_ω1) k₂₂sign(e_ω2) k₂₃sign(e_ω3)]^T，SIG1＝[k₁₁|e_ω1|^1/2sign(e_ω1) k₁₂|e_ω2|^1/2sign(e_ω2) k₁₃|e_ω3|^1/2sign(e_ω3)]^T，其中k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均為正常數(shù)，e_ω1,e_ω2,e_ω3為e_ω的三個元素，sign(·)表示符號函數(shù)，|·|^1/2表示求取絕對值的次方。

定義變量z_1i＝|e_ωi|^1/2sign(e_ωi),z_2i＝v_i,i＝1,2,3，設(shè)計控制器為：

其中表示求取矩陣的逆矩陣，表示求取ω_d的一階時間導(dǎo)數(shù)，G＝diag{[g₁ g₂ g₃]^T}∈R^3×3為正常數(shù)增益矩陣，diag{[g₁ g₂ g₃]^T}表示向量[g₁ g₂ g₃]張成的對角矩陣，g₁,g₂,g₃均為正常數(shù)。為外部擾動估計矩陣，表示向量張成的對角矩陣，是對d₁,d₂,d₃的估計值。

若設(shè)計和的自適應(yīng)律滿足：

其中ε₁,ε₂,m,n,γ₁,γ₂,γ₃均為正常數(shù)，分別表示滾轉(zhuǎn)角速度誤差、俯仰角速度誤差和偏航角速度誤差，則姿態(tài)誤差四元數(shù)和角速度誤差漸近穩(wěn)定。采用基于Lyapunov的分析方法可以證明當時間趨于無窮時，和e_v分別漸近收斂到[0 0 0]^T。

一、實驗平臺簡介

實驗平臺如圖1所示。該實驗平臺采用工控機作為仿真控制器，基于Matlab RTW工具箱的xPC目標作為實時仿真環(huán)境，采用自主設(shè)計的慣性測量單元作為姿態(tài)傳感器，俯仰角、滾轉(zhuǎn)角測量精度為±0.2°，偏航角測量精度為±0.5°，整個系統(tǒng)控制頻率為500Hz。

二、容錯控制實驗

本發(fā)明所采用方法中涉及的各參數(shù)取值如下：J＝diag{[1 1 2]^T}kg·m²，l＝0.16m，l₃＝0.25m，α＝26°，k＝0.05，D＝diag{[0.1 0.1 0.1]^T},ε₁＝ε₂＝10,m＝2，n＝1，k₁₁＝k₁₂＝5，k₁₃＝4,k₂₁＝k₂₂＝8,k₂₃＝3.5,γ₁＝γ₂＝0.1,γ₃＝0.2。初始姿態(tài)四元數(shù)和角速度分別為q_d＝[1 0 0 0]^T，ω_d＝[0 0 0]^Trad/s,三旋翼無人機的舵機在第30s發(fā)生堵塞故障，舵機堵塞角約為2.5°。實驗結(jié)果分別如圖2(a)、圖2(b)、圖2(c)、圖2(d)、圖2(e)、圖2(f)、圖2(g)所示。

圖2(a)表示三旋翼無人機姿態(tài)四元數(shù)變化曲線，在前30s，三旋翼無人機保持穩(wěn)定飛行，姿態(tài)四元數(shù)誤差小于0.05，在第30s，舵機發(fā)生堵塞故障，無人機姿態(tài)發(fā)生變化，并在5s內(nèi)迅速收斂到0。圖2(b)為角速度誤差的變化曲線，故障發(fā)生后，其在2s之內(nèi)迅速收斂到0。由此可見，控制目標得到很好的實現(xiàn)。圖2(c)和圖2(d)分別表示控制輸入曲線變化和電機轉(zhuǎn)速變化曲線，均在合理變化范圍內(nèi)。圖2(e)表示角速度估計誤差曲線，故障發(fā)生后迅速收斂到0。圖2(f)和圖2(g)分別表示對故障的估計值和對外部擾動的估計值，均為穩(wěn)定狀態(tài)，與理論計算結(jié)果相符。

經(jīng)過上述分析，證明了本發(fā)明所提算法的有效性。