本發(fā)明涉及自動(dòng)控制
技術(shù)領(lǐng)域：
，具體涉及一種基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制方法及系統(tǒng)。
背景技術(shù)：
：四旋翼無(wú)人飛行器在軍事、工業(yè)、民用領(lǐng)域有著廣泛的用途，在偵查監(jiān)視、電子干擾、武器攻擊、交通監(jiān)視、森林防火、地質(zhì)勘探、災(zāi)害搜救、航拍成圖等領(lǐng)域有著廣泛的應(yīng)用前景。上述應(yīng)用均需要高精度的自動(dòng)飛行控制，其中，姿態(tài)控制是四旋翼飛行器穩(wěn)定飛行最為基本的控制要求。四旋翼飛行器的姿態(tài)運(yùn)動(dòng)具有強(qiáng)耦合、多變量、非線性。不確定等特點(diǎn)，且易受外界干擾，因此，姿態(tài)控制是其飛行控制的關(guān)鍵技術(shù)和難點(diǎn)。目前，對(duì)于四旋翼的姿態(tài)控制，有如下幾種控制方法：1.基于學(xué)習(xí)的控制方法：基于模糊控制、神經(jīng)網(wǎng)絡(luò)自適應(yīng)算法、魯棒神經(jīng)網(wǎng)絡(luò)控制設(shè)計(jì)四旋翼飛行器的姿態(tài)控制器。這類(lèi)控制器雖然不需要四旋翼的運(yùn)動(dòng)模型，但是需要大量的實(shí)驗(yàn)和飛行數(shù)據(jù)來(lái)訓(xùn)練系統(tǒng)，算法復(fù)雜且對(duì)硬件要求較高，且大部分研究停留在仿真階段，沒(méi)有得到實(shí)際應(yīng)用。2.線性控制方法：基于PID、LQ控制方法設(shè)計(jì)四旋翼姿態(tài)控制器。該類(lèi)線性控制器實(shí)現(xiàn)較為簡(jiǎn)單，但是當(dāng)四旋翼飛行器脫離標(biāo)稱(chēng)條件或者大范圍機(jī)動(dòng)時(shí)，其控制器性能會(huì)明顯下降。目前也有基于自抗擾理論設(shè)計(jì)的姿態(tài)控制方法，但并未在硬件中得到實(shí)現(xiàn)。3.基于模型的非線性控制方法：基于反饋線性化、反步法和滑模控制設(shè)計(jì)四旋翼控制器，這類(lèi)控制方法需要依靠精確的模型，在能夠得到模型參數(shù)的前提下，這類(lèi)方法能使四旋翼控制性能和運(yùn)動(dòng)范圍得到增強(qiáng)。如滑模控制具有較好的魯棒性，但是由于這類(lèi)控制器并未對(duì)系統(tǒng)干擾進(jìn)行實(shí)時(shí)觀測(cè)，當(dāng)干擾較大時(shí)，其控制效果也并不理想。因此，有必要提供一種能夠?qū)崿F(xiàn)難度小、對(duì)硬件系統(tǒng)要求低，且能有效增強(qiáng)系統(tǒng)剛干擾能力，實(shí)現(xiàn)對(duì)四旋翼飛行器姿態(tài)高品質(zhì)控制的姿態(tài)控制方法。技術(shù)實(shí)現(xiàn)要素：針對(duì)現(xiàn)有技術(shù)中的上述不足之處，本發(fā)明提供了一種的基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制，包括：根據(jù)模型參數(shù)推導(dǎo)出四旋翼飛行器動(dòng)力學(xué)模型，根據(jù)所述四旋翼飛行器動(dòng)力學(xué)模型構(gòu)造滑模面，根據(jù)所述滑模面獲取滑模控制律，以實(shí)現(xiàn)對(duì)四旋翼飛行器三個(gè)姿態(tài)角回路的控制；根據(jù)四旋翼的滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的控制輸入輸出構(gòu)建ESO，利用所述ESO對(duì)系統(tǒng)總擾動(dòng)進(jìn)行實(shí)時(shí)估計(jì)；采用所述滑?？刂坡膳c所述ESO相結(jié)合，以實(shí)現(xiàn)對(duì)所述四旋翼飛行器姿態(tài)的控制。本發(fā)明提供的基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制方法利用ESO(EXtendedStateObserver，簡(jiǎn)稱(chēng)ESO，中文意思為：擴(kuò)張狀態(tài)觀測(cè)器)實(shí)現(xiàn)對(duì)系統(tǒng)總擾的實(shí)時(shí)估計(jì)，并將其與滑模控制結(jié)合，不僅能夠?qū)崿F(xiàn)四旋翼飛行器的姿態(tài)穩(wěn)定，同時(shí)對(duì)姿態(tài)角指令具有良好的跟蹤性能，且相對(duì)于普通的滑?？刂凭哂懈鼜?qiáng)的抗干擾能力。進(jìn)一步地，四旋翼飛行器動(dòng)力學(xué)模型參數(shù)包括轉(zhuǎn)動(dòng)慣量Jx、Jy、Jz；升力系數(shù)cT；扭矩系數(shù)cQ；電機(jī)的時(shí)間常數(shù)T。進(jìn)一步地，獲取所述滑模控制模型包括如下步驟：(1)將四旋翼視為剛體，則所述四旋翼飛行器姿態(tài)非線性動(dòng)態(tài)方程為：φ·=p+qsinφtanθ+rcosφtanθθ·=qcosφ-rsinφψ·=secθ(qsinφ+rcosφ)]]>p·=[qr(Jy-Jz)+τφ]·1/Jxq·=[pr(Jz-Jx)+τθ]·1/Jyr·=[pq(Jx-Jy)+τψ]·1/Jz]]>其中：φ,θ,ψ分別為滾轉(zhuǎn)角、俯仰角、偏航角；p,q,r分別為本體角速度ω在本體坐標(biāo)系x,y,z軸上的分量；τφ,τθ,τψ分別為三個(gè)本體軸方向的控制力矩；Jx,Jy,Jz分別為四旋翼沿著x,y,z軸方向的轉(zhuǎn)動(dòng)慣量；(2)將所述非線性動(dòng)態(tài)方程改寫(xiě)為狀態(tài)空間的形式：X·=f(X,U)]]>其中，U為輸入矢量，X為狀態(tài)矢量，具體表達(dá)式如下：狀態(tài)變量：x1=φx3=θx5=ψx2=x·1=φ·x4=x·3=θ·x6=x·5=ψ·]]>輸入矢量：U＝[U1U2U3]T＝[τφτθτψ]T(3)姿態(tài)角變化率與本體角速率之間的轉(zhuǎn)換矩陣，在懸?；蛘咝〗嵌蕊w行情況下，作為單位矩陣，獲得所述滑?？刂颇Ｐ停篺(X,U)=φ·(Jy-Jz)Jxθ·ψ·+1JxU1θ·(Jz-Jx)Jyφ·ψ·+1JyU2ψ·(Jx-Jy)Jzφ·θ·+1JzU3]]>其中，其中:φ,θ,ψ分別為滾轉(zhuǎn)角、俯仰角、偏航角；p,q,r分別為本體角速度ω在本體坐標(biāo)系x,y,z軸上的分量；τφ,τθ,τψ分別為三個(gè)本體軸方向的控制力矩；Jx,Jy,Jz分別為四旋翼沿著x,y,z軸方向的轉(zhuǎn)動(dòng)慣量。進(jìn)一步地，在根據(jù)模型參數(shù)推導(dǎo)出四旋翼飛行器動(dòng)力學(xué)模型，根據(jù)所述四旋翼飛行器動(dòng)力學(xué)模型構(gòu)造滑模面的過(guò)程中，構(gòu)造的滑模面為獲得的滑模控制律為：U1=(Jz-Jy)x4x6-Jx(α12z1+k1sign(s2)+k2s2-x··1d)]]>U2=(Jx-Jz)x2x6-Jy(α22z3+k3sign(s3)+k4s3-x··3d)]]>U3=(Jy-Jx)x2x4-Jz(α32z5+k5sign(s4)+k6s4-x··5d)]]>其中，z3=x3d-x3s3=x4-x·3d-α2z3z5=x5d-x5s4=x6-x·5d-α3z5]]>其中，sign代表符號(hào)函數(shù)，用近似的連續(xù)飽和函數(shù)sat(s)來(lái)替代所述符號(hào)函數(shù)；函數(shù)表達(dá)式為：sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；構(gòu)造二階濾波器：其中，為輸入值，Xc為輸出值。由此，得到在時(shí)域中的表達(dá)式：選取阻尼比ξ＝0.8，自然頻率ωn＝4.375。進(jìn)一步地，在根據(jù)四旋翼的滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的控制輸入輸出構(gòu)建ESO的過(guò)程中，e=z1-yz·1=z2-β01ez·2=z3-β02fal(e,a1,h)+b0U1z·3=-β03fal(e,a2,h)]]>其中，b0為控制量系數(shù)1/Jx，選擇預(yù)設(shè)的參數(shù)β01,β02,β03和ai(i＝1,2)，令a1＝0.5,a2＝0.25。進(jìn)一步地，在采用所述滑?？刂坡膳c所述ESO相結(jié)合的過(guò)程中，e=z1φ-x1z·1φ=z2φ-β01ez·2φ=z3φ-β02fal(e,0.5,h)+(1/Jx)U1z·3φ=-β03fal(e,0.25,h)U1=(Jz-Jy)x4x6-Jx(α12z1+k1sat(s2)+k2s2-x··1d)-Jxz3φ]]>同理，得到其他兩個(gè)姿態(tài)回路的控制輸出為：U2=(Jx-Jz)x2x6-Jy(α22z3+k3sat(s3)+k4s3-x··3d)-Jyz3θ]]>U3=(Jy-Jx)x2x4-Jz(α32z5+k5sat(s4)+k6s4-x··5d)-Jzz3ψ]]>其中，z3φ,z3θ,z3ψ分別為滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的ESO得到的被擴(kuò)張狀態(tài)量。本發(fā)明還提供了基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制系統(tǒng)，包括：姿態(tài)角回路控制單元，用于根據(jù)模型參數(shù)推導(dǎo)出四旋翼飛行器動(dòng)力學(xué)模型，根據(jù)所述四旋翼飛行器動(dòng)力學(xué)模型構(gòu)造滑模面，根據(jù)所述滑模面獲取滑?？刂坡?，以實(shí)現(xiàn)對(duì)四旋翼飛行器三個(gè)姿態(tài)角回路的控制；系統(tǒng)總擾實(shí)時(shí)估計(jì)單元，用于根據(jù)四旋翼的滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的控制輸入輸出構(gòu)建ESO，利用所述ESO對(duì)系統(tǒng)總擾動(dòng)進(jìn)行實(shí)時(shí)估計(jì)；四旋翼飛行器姿態(tài)控制單元，用于采用所述滑模控制律與所述ESO相結(jié)合，以實(shí)現(xiàn)對(duì)所述四旋翼飛行器姿態(tài)的控制。進(jìn)一步地，四旋翼飛行器動(dòng)力學(xué)模型參數(shù)包括轉(zhuǎn)動(dòng)慣量Jx、Jy、Jz；升力系數(shù)cT；扭矩系數(shù)cQ；電機(jī)的時(shí)間常數(shù)T。進(jìn)一步地，姿態(tài)角回路控制單元獲取滑?？刂颇Ｐ桶ㄈ缦虏襟E：(1)將四旋翼視為剛體，則四旋翼飛行器姿態(tài)非線性動(dòng)態(tài)方程為：φ·=p+qsinφtanθ+rcosφtanθθ·=qcosφ-rsinφψ·=secθ(qsinφ+rcosφ)]]>p·=[qr(Jy-Jz)+τφ]·1/Jxq·=[pr(Jz-Jx)+τθ]·1/Jyr·=[pq(Jx-Jy)+τψ]·1/Jz]]>其中:φ,θ,ψ分別為滾轉(zhuǎn)角、俯仰角、偏航角；p,q,r分別為本體角速度ω在本體坐標(biāo)系x,y,z軸上的分量；τφ,τθ,τψ分別為三個(gè)本體軸方向的控制力矩；Jx,Jy,Jz分別為四旋翼沿著x,y,z軸方向的轉(zhuǎn)動(dòng)慣量；(2)將所述非線性動(dòng)態(tài)方程改寫(xiě)為狀態(tài)空間的形式：X·=f(X,U)]]>其中，U為輸入矢量，X為狀態(tài)矢量，具體表達(dá)式如下：狀態(tài)變量：x1=φx3=θx5=ψx2=x·1=φ·x4=x·3=θ·x6=x·5=ψ·]]>輸入矢量：U＝[U1U2U3]T＝[τφτθτψ]T(3)姿態(tài)角變化率與本體角速率之間的轉(zhuǎn)換矩陣，在懸停或者小角度飛行情況下，作為單位矩陣，獲得所述滑?？刂颇Ｐ停篺(X,U)=φ·(Jy-Jz)Jxθ·ψ·+1JxU1θ·(Jz-Jx)Jyφ·ψ·+1JyU2ψ·(Jx-Jy)Jzφ·θ·+1JzU3]]>其中，φ,θ,ψ分別為滾轉(zhuǎn)角、俯仰角、偏航角；p,q,r分別為本體角速度ω在本體坐標(biāo)系x,y,z軸上的分量；τφ,τθ,τψ分別為三個(gè)本體軸方向的控制力矩；Jx,Jy,Jz分別為四旋翼沿著x,y,z軸方向的轉(zhuǎn)動(dòng)慣量。進(jìn)一步地，姿態(tài)角回路控制單元在根據(jù)模型參數(shù)推導(dǎo)出四旋翼飛行器動(dòng)力學(xué)模型，根據(jù)所述四旋翼飛行器動(dòng)力學(xué)模型構(gòu)造滑模面，根據(jù)所述滑模面獲取滑模控制律的過(guò)程中，構(gòu)造的滑模面為獲得的滑?？刂坡蔀椋篣1=(Jz-Jy)x4x6-Jx(α12z1+k1sign(s2)+k2s2-x··1d)]]>U2=(Jx-Jz)x2x6-Jy(α22z3+k3sign(s3)+k4s3-x··3d)]]>U3=(Jy-Jx)x2x4-Jz(α32z5+k5sign(s4)+k6s4-x··5d)]]>其中，z3=x3d-x3s3=x4-x·3d-α2z3z5=x5d-x5s4=x6-x·5d-α3z5]]>其中，sign代表符號(hào)函數(shù)，用近似的連續(xù)飽和函數(shù)sat(s)來(lái)替代所述符號(hào)函數(shù)；函數(shù)表達(dá)式為：sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；構(gòu)造二階濾波器：其中，為輸入值，Xc為輸出值。由此，得到在時(shí)域中的表達(dá)式：選取阻尼比ξ＝0.8，自然頻率ωn＝4.375。進(jìn)一步地，系統(tǒng)總擾實(shí)時(shí)估計(jì)單元在根據(jù)四旋翼的滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的控制輸入輸出構(gòu)建ESO的過(guò)程中，e=z1-yz·1=z2-β01ez·2=z3-β02fal(e,a1,h)+b0U1z·3=-β03fal(e,a2,h)]]>其中，b0為控制量系數(shù)1/Jx，選擇預(yù)設(shè)的參數(shù)β01,β02,β03和ai(i＝1,2)，令a1＝0.5,a2＝0.25。進(jìn)一步地，四旋翼飛行器姿態(tài)控制單元在采用所述滑模控制律與所述ESO相結(jié)合的過(guò)程中，e=z1φ-x1z·1φ=z2φ-β01ez·2φ=z3φ-β02fal(e,0.5,h)+(1/Jx)U1z·3φ=-β03fal(e,0.25,h)U1=(Jz-Jy)x4x6-Jx(α12z1+k1sat(s2)+k2s2-x··1d)-Jxz3φ]]>同理，得到其他兩個(gè)姿態(tài)回路的控制輸出為：U2=(Jx-Jz)x2x6-Jy(α22z3+k3sat(s3)+k4s3-x··3d)-Jyz3θ]]>U3=(Jy-Jx)x2x4-Jz(α32z5+k5sat(s4)+k6s4-x··5d)-Jzz3ψ]]>其中，z3φ,z3θ,z3ψ分別為滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的ESO得到的被擴(kuò)張狀態(tài)量。采用上述技術(shù)方案，包括以下有益技術(shù)效果：1)利用ESO實(shí)現(xiàn)對(duì)系統(tǒng)總擾的實(shí)時(shí)估計(jì)；2)并將ESO與滑?？刂坡山Y(jié)合，不僅能夠?qū)崿F(xiàn)四旋翼飛行器的姿態(tài)穩(wěn)定，同時(shí)對(duì)姿態(tài)角指令具有良好的跟蹤性能，且相對(duì)于普通的滑?？刂凭哂懈鼜?qiáng)的抗干擾能力。附圖說(shuō)明圖1示例性地示出了本發(fā)明基于滑模和ESO的四旋翼飛行器姿態(tài)控制流程示意圖；圖2示例性地示出了本發(fā)明滑?？刂颇Ｐ团cESO結(jié)合的結(jié)構(gòu)原理圖；圖3示例性地示出了本發(fā)明滑?？刂颇Ｐ椭卸A低通濾波器結(jié)構(gòu)圖；圖4示例性地示出了本發(fā)明四旋翼滾轉(zhuǎn)角響應(yīng)曲線對(duì)比圖；圖5示例性地示出了本發(fā)明四旋翼俯仰角響應(yīng)曲線對(duì)比圖；圖6示例性地示出了本發(fā)明四旋翼偏航角響應(yīng)曲線對(duì)比圖；圖7示例性地示出了本發(fā)明姿態(tài)各回路ESO干擾估計(jì)量；圖8-1和圖8-2示例性地示出了本發(fā)明兩種控制方法的滾轉(zhuǎn)角實(shí)際曲線對(duì)比圖；圖9-1和圖9-2示例性地示出了本發(fā)明兩種控制方法的俯仰角實(shí)際曲線對(duì)比圖；圖10-1和圖10-2示例性地示出了本發(fā)明外擾下的兩種控制方法姿態(tài)角實(shí)際曲線對(duì)比圖；圖11示例性地示出了本發(fā)明基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制系統(tǒng)邏輯結(jié)構(gòu)示意圖。具體實(shí)施方式下面通過(guò)具體的實(shí)施例并結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步的詳細(xì)描述。如圖1所示，本發(fā)明提供了一種的基于滑模控制律和ESO的四旋翼飛行器姿態(tài)控制，包括：S110：根據(jù)四旋翼飛行器動(dòng)力學(xué)模型參數(shù)獲取滑模控制模型，根據(jù)滑模控制模型構(gòu)造滑模面，根據(jù)滑模面獲取滑?？刂坡?，以實(shí)現(xiàn)對(duì)四旋翼飛行器三個(gè)姿態(tài)角回路的控制；S120：根據(jù)四旋翼的滾轉(zhuǎn)角、仰俯角或者偏航角建立ESO，利用ESO對(duì)系統(tǒng)總擾進(jìn)行實(shí)時(shí)估計(jì)；S130：采用滑模控制模型與所述ESO相結(jié)合，以實(shí)現(xiàn)對(duì)四旋翼飛行器姿態(tài)的控制。上述為基于滑模控制律和ESO的四旋翼飛行器姿態(tài)控制方法的具體流程，在獲得滑?？刂颇Ｐ椭?，通過(guò)實(shí)驗(yàn)測(cè)試的方法獲取模型參數(shù)；其中，實(shí)際測(cè)量的參數(shù)包括轉(zhuǎn)動(dòng)慣量Jx,Jy,Jz；升力系數(shù)cT；扭矩系數(shù)cQ；電機(jī)的時(shí)間常數(shù)T。在步驟S110中，獲得滑?？刂颇Ｐ偷牟襟E包括：(1)將四旋翼視為剛體，忽略旋轉(zhuǎn)運(yùn)動(dòng)產(chǎn)生的陀螺效應(yīng)，得到的四旋翼飛行器姿態(tài)非線性動(dòng)態(tài)方程為：φ·=p+qsinφtanθ+rcosφtanθθ·=qcosφ-rsinφψ·=secθ(qsinφ+rcosφ)]]>p·=[qr(Jy-Jz)+τφ]·1/Jxq·=[pr(Jz-Jx)+τθ]·1/Jyr·=[pq(Jx-Jy)+τψ]·1/Jz]]>其中，φ,θ,ψ分別為滾轉(zhuǎn)角、俯仰角、偏航角；p,q,r分別為本體角速度ω在本體坐標(biāo)系x,y,z軸上的分量；τφ,τθ,τψ分別為三個(gè)本體軸方向的控制力矩；Jx,Jy,Jz分別為四旋翼沿著x,y,z軸方向的轉(zhuǎn)動(dòng)慣量。(2)將上式改寫(xiě)為狀態(tài)空間的形式：其中U是輸入矢量，X是狀態(tài)矢量，其具體表達(dá)式如下：狀態(tài)變量：x1=φx3=θx5=ψx2=x·1=φ·x4=x·3=θ·x6=x·5=ψ·]]>輸入矢量：U＝[U1U2U3]T＝[τφτθτψ]T(3)姿態(tài)角變化率與本體角速率之間的轉(zhuǎn)換矩陣，在懸?；蛘咝〗嵌蕊w行情況下，可以看作為單位矩陣，即由此得到控制模型：f(X,U)=φ·(Jy-Jz)Jxθ·ψ·+1JxU1θ·(Jz-Jx)Jyφ·ψ·+1JyU2ψ·(Jx-Jy)Jzφ·θ·+1JzU3]]>其中，φ,θ,ψ分別為滾轉(zhuǎn)角、俯仰角、偏航角；p,q,r分別為本體角速度ω在本體坐標(biāo)系x,y,z軸上的分量；τφ,τθ,τψ分別為三個(gè)本體軸方向的控制力矩；Jx,Jy,Jz分別為四旋翼沿著x,y,z軸方向的轉(zhuǎn)動(dòng)慣量。根據(jù)滑?？刂颇Ｐ蜆?gòu)造滑模面：構(gòu)造擴(kuò)展李雅普諾夫函數(shù)：令s·2=-k1sign(s2)-k2s2=x·2-x··1d-α1z·1=Jy-JzJxx4x6+1JxU1-x··1d+α1(s2+α1z1)]]>其中，k1,k2>0，sign代表符號(hào)函數(shù)。由此推出滑?？刂坡蔀椋篣1=(Jz-Jy)x4x6-Jx(α12z1+k1sign(s2)+k2s2-x··1d)]]>U2=(Jx-Jz)x2x6-Jy(α22z3+k3sign(s3)+k4s3-x··3d)]]>U3=(Jy-Jx)x2x4-Jz(α32z5+k5sign(s4)+k6s4-x··5d)]]>其中：z3=x3d-x3s3=x4-x·3d-α2z3z5=x5d-x5s4=x6-x·5d-α3z5]]>用連續(xù)飽和函數(shù)sat(s)代替符號(hào)函數(shù)，sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5。構(gòu)造二階濾波器：為輸入值，Xc為輸出值。由此，得到該式在時(shí)域中的表達(dá)式：選取阻尼比ξ＝0.8，自然頻率ωn＝4.375。在本發(fā)明的實(shí)施例中，二階濾波器的具體結(jié)構(gòu)如圖3所示。在圖1所示的步驟S120中，ESO具體結(jié)構(gòu)實(shí)現(xiàn)如下：以四旋翼的滾轉(zhuǎn)角通道為例，說(shuō)明ESO的具體結(jié)構(gòu)實(shí)現(xiàn)。結(jié)合狀態(tài)空間方程，將四旋翼的滾轉(zhuǎn)角φ方程寫(xiě)成如下形式：x·1=x2x·2=f1(φ,φ·,θ,θ·,ψ,ψ·)+ω2(t)+b0U1y=x1x·1=x2x·2=x3+b0U1x·3=ω0y=x1]]>其中，b0為控制量系數(shù)1/Jx；ω2(t)為外界干擾量；為各種擾動(dòng)作用(包括建模、未建模動(dòng)態(tài)和外擾)的總和。令總擾動(dòng)量看成未知的被擴(kuò)張狀態(tài)變量，即x3＝a，則式子變成如下形式的線性系統(tǒng)：然后對(duì)上述系統(tǒng)建立非線性擴(kuò)張狀態(tài)觀測(cè)器:e=z1-yz·1=z2-β01ez·2=z3-β02fal(e,a1,h)+b0U1z·3=-β03fal(e,a2,h)]]>其中，fal(e,a,δ)=|e|asign(e)|e|>δe/δ-a|e|≤δ---(25)]]>選擇適當(dāng)?shù)膮?shù)β01,β02,β03和ai(i＝1,2)，通常情況下取a1＝0.5,a2＝0.25,則擴(kuò)張狀態(tài)觀測(cè)器輸出量zi能很好地估計(jì)原系統(tǒng)各狀態(tài)量xi，其中z3→x3＝f(·)+ω(t)，雖然f(·)和ω(t)的具體函數(shù)表達(dá)式未知，但被擴(kuò)張狀態(tài)z3仍可以很好地估計(jì)出系統(tǒng)總擾動(dòng)量a。同樣，其他兩個(gè)角回路，即俯仰角θ和偏航角ψ采用相同的算法處理，則可以實(shí)現(xiàn)各個(gè)通道的動(dòng)態(tài)補(bǔ)償線性化。在步驟S130中，以滾轉(zhuǎn)角φ回路為例，基于滑模控制律和ESO相結(jié)合，其公式如下：e=z1φ-x1z·1φ=z2φ-β01ez·2φ=z3φ-β02fal(e,0.5,h)+(1/Jx)U1z·3φ=-β03fal(e,0.25,h)U1=(Jz-Jy)x4x6-Jx(α12z1+k1sat(s2)+k2s2-x··1d)-Jxz3φ]]>同理，可以得到其他兩個(gè)姿態(tài)回路的控制輸出為：U2=(Jx-Jz)x2x6-Jy(α22z3+k3sat(s3)+k4s3-x··3d)-Jyz3θ---(27)]]>U3=(Jy-Jx)x2x4-Jz(α32z5+k5sat(s4)+k6s4-x··5d)-Jzz3ψ---(28)]]>其中，z3φ,z3θ,z3ψ分別為滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的ESO得到的被擴(kuò)張狀態(tài)量。在圖2所示的實(shí)施中，詳細(xì)地示出了本發(fā)明中ESO和滑?？刂颇Ｐ偷南嘟Y(jié)合的原理，其中，本發(fā)明中滑?？刂颇Ｐ鸵卜Q(chēng)為滑?？刂破?。在本發(fā)明中，在下面的實(shí)施例中進(jìn)行仿真驗(yàn)證和試飛驗(yàn)證。為了驗(yàn)證本文所設(shè)計(jì)的控制器及其抗干擾效果，設(shè)定四旋翼飛行器的姿態(tài)初始值為(φ,θ,ψ)＝(30°,-45°,-15°)。當(dāng)0s<t<6s時(shí)，φd＝θd＝ψd＝0°；當(dāng)6s<t<20s時(shí)，φd＝θd＝ψd＝10°；當(dāng)20s<t<30s時(shí)，φd＝θd＝ψd＝sin(t)(單位:度)；當(dāng)9.5s<t<10.5s時(shí)在三個(gè)姿態(tài)回路上同時(shí)施加0.008N·m的外部干擾，當(dāng)14.5s<t<15.5s時(shí)在三個(gè)姿態(tài)回路上同時(shí)施加-0.008N·m的外部干擾?；？刂颇Ｐ筒糠值膮?shù)通過(guò)調(diào)試取α1＝α2＝α3＝1,k1＝k3＝k5＝2.5,k2＝k4＝3,k6＝2.擴(kuò)張狀態(tài)觀測(cè)器部分的參數(shù)參考文獻(xiàn)[19]所說(shuō)，h為采樣步長(zhǎng)，這里為0.004，參數(shù)β01,β02,β03是由采樣步長(zhǎng)h來(lái)決定的(不管怎樣的被控對(duì)象，采用步長(zhǎng)一定，則可以使用相同的β01,β02,β03)，這里取β01＝β02＝β03＝80，三個(gè)姿態(tài)回路采用相同的ESO參數(shù)。為了更好地說(shuō)明本控制器的優(yōu)越性，在相同初始條件、跟蹤指令和外部干擾的情況下，將與單獨(dú)滑?？刂破鞯慕Y(jié)果進(jìn)行對(duì)比。仿真結(jié)果如圖4至圖6所示。圖4至圖6為四旋翼飛行器姿態(tài)控制各回路的仿真曲線對(duì)比圖。對(duì)比的是單獨(dú)滑?？刂坪蛶в蠩SO的滑模控制。從圖中可以看出，整個(gè)過(guò)程中，兩種控制方法都能使四旋翼姿態(tài)角快速響應(yīng)并跟蹤姿態(tài)指令；但是，在9.5s<t<10.5s和14.5s<t<15.5s時(shí)段，由于系統(tǒng)擾動(dòng)的作用，帶有ESO的滑?？刂葡鄬?duì)于單獨(dú)滑模控制，其姿態(tài)角變化量較小，可見(jiàn)其算法抗干擾性強(qiáng)；此外，圖4和圖6可以看出，帶有ESO的滑?？刂葡?，滾轉(zhuǎn)角和偏航角響應(yīng)時(shí)間更短。圖7示出了姿態(tài)各回路ESO干擾估計(jì)量。從圖7可以看出，當(dāng)快速響應(yīng)跟蹤指令和受到外部干擾時(shí)，ESO的值不為零，而其他時(shí)間段為零，表明其能夠?qū)崟r(shí)估計(jì)出擾動(dòng)總和。在本發(fā)明的實(shí)施例中，試飛時(shí)，整個(gè)四旋翼硬件平臺(tái)在室外進(jìn)行等高模式試飛，其能穩(wěn)定在一個(gè)指定高度，同時(shí)姿態(tài)控制使用了本文設(shè)計(jì)的控制器，由遙控器給出姿態(tài)指令。同樣為了更好說(shuō)明本控制器的優(yōu)越性，將在滾轉(zhuǎn)角回路和俯仰角回路與單獨(dú)滑模控制器的試飛結(jié)果進(jìn)行對(duì)比；此外，為了更好說(shuō)明本控制器的抗干擾性，試飛中給這兩個(gè)姿態(tài)角回路分別掛載108g的物體，作為施加的外部干擾力矩，并進(jìn)行結(jié)果對(duì)比。實(shí)際的結(jié)果圖如圖8至圖10所示。其中，需要說(shuō)明的是，圖8和圖9分別為兩種控制方法下的滾轉(zhuǎn)角和俯仰角實(shí)際曲線對(duì)比圖。從圖8-1和圖9-1中可以看出單獨(dú)滑?？刂茣r(shí)，滾轉(zhuǎn)角實(shí)際跟蹤誤差最大達(dá)到5.5度，零度附近波動(dòng)在±5.7°之間；俯仰角實(shí)際跟蹤誤差最大達(dá)到4.5度，零度附近波動(dòng)在±5.1°之間；基于滑模和ESO控制時(shí)，滾轉(zhuǎn)角實(shí)際跟蹤誤差最大達(dá)到4.6度，零度附近波動(dòng)在±5.7°之間；俯仰角實(shí)際跟蹤誤差最大達(dá)到2度，零度附近波動(dòng)在±3°之間；表明基于滑模和ESO的控制方法的跟蹤性能更準(zhǔn)確，實(shí)際波動(dòng)范圍更小。在本發(fā)明的實(shí)施例中，圖10-1和圖10-2為外擾力矩下兩種控制方法的姿態(tài)角實(shí)際曲線對(duì)比圖。從圖10-1中可以看出在常值滾轉(zhuǎn)力矩干擾下，基于滑模和ESO控制的滾轉(zhuǎn)角曲線震動(dòng)明顯較小，且震動(dòng)范圍能維持在1.2°以下；在常值俯仰力矩干擾下，基于滑模和ESO控制的俯仰角曲線偏離零度線范圍較小，且其俯仰角有大于零的情況，表明該控制器有補(bǔ)償外部干擾的能力，有較強(qiáng)的抗干擾能力。上述為本發(fā)明提供的基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制方法，與基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制方法相對(duì)應(yīng)，本發(fā)明還提供一種基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制系統(tǒng)。圖11示出了根據(jù)本發(fā)明實(shí)施例的基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制系統(tǒng)邏輯結(jié)構(gòu)，如圖11所示，本發(fā)明提供的基于滑?？刂坡珊虴SO的四旋翼飛行器姿態(tài)控制系統(tǒng)邏輯結(jié)構(gòu)1100包括：姿態(tài)角回路控制單元1110、系統(tǒng)總擾實(shí)時(shí)估計(jì)單元1120和四旋翼飛行器姿態(tài)控制單元1130。其中，姿態(tài)角回路控制單元1110，用于根據(jù)模型參數(shù)推導(dǎo)出四旋翼飛行器動(dòng)力學(xué)模型，根據(jù)所述四旋翼飛行器動(dòng)力學(xué)模型構(gòu)造滑模面，根據(jù)所述滑模面獲取滑?？刂坡?，以實(shí)現(xiàn)對(duì)四旋翼飛行器三個(gè)姿態(tài)角回路的控制；系統(tǒng)總擾實(shí)時(shí)估計(jì)單元1120，用于根據(jù)四旋翼的滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的控制輸入輸出構(gòu)建ESO，利用所述ESO對(duì)系統(tǒng)總擾動(dòng)進(jìn)行實(shí)時(shí)估計(jì)；四旋翼飛行器姿態(tài)控制單元1130，用于采用所述滑?？刂坡膳c所述ESO相結(jié)合，以實(shí)現(xiàn)對(duì)所述四旋翼飛行器姿態(tài)的控制。進(jìn)一步地，四旋翼飛行器動(dòng)力學(xué)模型參數(shù)包括轉(zhuǎn)動(dòng)慣量Jx、Jy、Jz；升力系數(shù)cT；扭矩系數(shù)cQ；電機(jī)的時(shí)間常數(shù)T。進(jìn)一步地，姿態(tài)角回路控制單元1110獲取滑模控制模型包括如下步驟：(1)將四旋翼視為剛體，則四旋翼飛行器姿態(tài)非線性動(dòng)態(tài)方程為：φ·=p+qsinφtanθ+rcosφtanθθ·=qcosφ-rsinφψ·=secθ(qsinφ+rcosφ)]]>p·=[qr(Jy-Jz)+τφ]·1/Jxq·=[pr(Jz-Jx)+τθ]·1/Jyr·=[pq(Jx-Jy)+τψ]·1/Jz]]>其中:φ,θ,ψ分別為滾轉(zhuǎn)角、俯仰角、偏航角；p,q,r分別為本體角速度ω在本體坐標(biāo)系x,y,z軸上的分量；τφ,τθ,τψ分別為三個(gè)本體軸方向的控制力矩；Jx,Jy,Jz分別為四旋翼沿著x,y,z軸方向的轉(zhuǎn)動(dòng)慣量；(2)將所述非線性動(dòng)態(tài)方程改寫(xiě)為狀態(tài)空間的形式：X·=f(X,U)]]>其中，U為輸入矢量，X為狀態(tài)矢量，具體表達(dá)式如下：狀態(tài)變量：x1=φx3=θx5=ψx2=x·1=φ·x4=x·3=θ·x6=x·5=ψ·]]>輸入矢量：U＝[U1U2U3]T＝[τφτθτψ]T(3)姿態(tài)角變化率與本體角速率之間的轉(zhuǎn)換矩陣，在懸?；蛘咝〗嵌蕊w行情況下，作為單位矩陣，獲得所述滑模控制模型：f(X,U)=φ·(Jy-Jz)Jxθ·ψ·+1JxU1θ·(Jz-Jx)Jyφ·ψ·+1JyU2ψ·(Jx-Jy)Jzφ·θ·+1JzU3]]>其中，φ,θ,ψ分別為滾轉(zhuǎn)角、俯仰角、偏航角；p,q,r分別為本體角速度ω在本體坐標(biāo)系x,y,z軸上的分量；τφ,τθ,τψ分別為三個(gè)本體軸方向的控制力矩；Jx,Jy,Jz分別為四旋翼沿著x,y,z軸方向的轉(zhuǎn)動(dòng)慣量。進(jìn)一步地，姿態(tài)角回路控制單元1110在根據(jù)模型參數(shù)推導(dǎo)出四旋翼飛行器動(dòng)力學(xué)模型，根據(jù)所述四旋翼飛行器動(dòng)力學(xué)模型構(gòu)造滑模面，根據(jù)所述滑模面獲取滑模控制律的過(guò)程中，構(gòu)造的滑模面為獲得的滑?？刂坡蔀椋篣1=(Jz-Jy)x4x6-Jx(α12z1+k1sign(s2)+k2s2-x··1d)]]>U2=(Jx-Jz)x2x6-Jy(α22z3+k3sign(s3)+k4s3-x··3d)]]>U3=(Jy-Jx)x2x4-Jz(α32z5+k5sign(s4)+k6s4-x··5d)]]>其中，z3=x3d-x3s3=x4-x·3d-α2z3z5=x5d-x5s4=x6-x·5d-α3z5]]>其中，sign代表符號(hào)函數(shù)，用近似的連續(xù)飽和函數(shù)sat(s)來(lái)替代所述符號(hào)函數(shù)；函數(shù)表達(dá)式為：sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；構(gòu)造二階濾波器：其中，為輸入值，Xc為輸出值。由此，得到在時(shí)域中的表達(dá)式：選取阻尼比ξ＝0.8，自然頻率ωn＝4.375。進(jìn)一步地，系統(tǒng)總擾實(shí)時(shí)估計(jì)單元1120在四旋翼的滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的控制輸入輸出構(gòu)建ESO的過(guò)程中，e=z1-yz·1=z2-β01ez·2=z3-β02fal(e,a1,h)+b0U1z·3=-β03fal(e,a2,h)]]>其中，，b0為控制量系數(shù)1/Jx，選擇預(yù)設(shè)的參數(shù)β01,β02,β03和ai(i＝1,2)，令a1＝0.5,a2＝0.25。進(jìn)一步地，四旋翼飛行器姿態(tài)控制單元1130在采用所述滑?？刂坡膳c所述ESO相結(jié)合的過(guò)程中，e=z1φ-x1z·1φ=z2φ-β01ez·2φ=z3φ-β02fal(e,0.5,h)+(1/Jx)U1z·3φ=-β03fal(e,0.25,h)U1=(Jz-Jy)x4x6-Jx(α12z1+k1sat(s2)+k2s2-x··1d)-Jxz3φ]]>同理，得到其他兩個(gè)姿態(tài)回路的控制輸出為：U2=(Jx-Jz)x2x6-Jy(α22z3+k3sat(s3)+k4s3-x··3d)-Jyz3θ]]>U3=(Jy-Jx)x2x4-Jz(α32z5+k5sat(s4)+k6s4-x··5d)-Jzz3ψ]]>其中，z3φ,z3θ,z3ψ分別為滾轉(zhuǎn)角回路、俯仰角回路和偏航角回路的ESO得到的被擴(kuò)張狀態(tài)量。本發(fā)明利用ESO實(shí)現(xiàn)對(duì)系統(tǒng)總擾的實(shí)時(shí)估計(jì)；并將ESO與滑?？刂坡山Y(jié)合，實(shí)現(xiàn)四旋翼飛行器的姿態(tài)穩(wěn)定，同時(shí)對(duì)姿態(tài)角指令具有良好的跟蹤性能，相對(duì)于普通的滑模控制具有更強(qiáng)的抗干擾能力。以上所述僅為本發(fā)明的優(yōu)選實(shí)施例而已，并不用于限制本發(fā)明，對(duì)于本領(lǐng)域的技術(shù)人員來(lái)說(shuō)，本發(fā)明可以有各種更改和變化。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。當(dāng)前第1頁(yè)1 2 3