本發(fā)明應(yīng)用于三自由度直升機(jī)自動(dòng)控制領(lǐng)域，為三自由度直升機(jī)提供一種快速求解并適用于三自由度直升機(jī)的姿態(tài)調(diào)節(jié)及跟蹤控制方法。
背景技術(shù)：
：及意義如今飛行器快速的發(fā)展，各種飛行器層出不窮，良好性能與安全性能就尤為關(guān)鍵了。在各類(lèi)飛行器中，直升機(jī)較為特殊，它能夠快速起飛、降落，能夠適應(yīng)較困難的環(huán)境。直升機(jī)在現(xiàn)代化的各領(lǐng)域中憑借著它突出的優(yōu)點(diǎn)，都起到了不小的作用。本發(fā)明使用加拿大quanser公司開(kāi)發(fā)的三自由度直升機(jī)系統(tǒng)作為研究對(duì)象，其形態(tài)類(lèi)似于魚(yú)鷹戰(zhàn)機(jī)，擁有2個(gè)旋翼，具有高度軸，俯仰軸和旋轉(zhuǎn)軸3個(gè)自由度。它是典型的多輸入多輸出(mimo)、強(qiáng)耦合的非線性系統(tǒng)，它是模擬飛行器實(shí)地飛行的絕佳的參考，是模擬不同算法理想的科研模型。但是傳統(tǒng)的控制器設(shè)計(jì)會(huì)使飛行器全局性能不平衡，三自由度直升機(jī)飛行中系統(tǒng)參數(shù)會(huì)不斷變化且本身模型具有不確定性影響系統(tǒng)的精確。此外，三自由度直升機(jī)系統(tǒng)復(fù)雜程度高，求解時(shí)間隨著問(wèn)題規(guī)模的增大而增加。技術(shù)實(shí)現(xiàn)要素：本發(fā)明針對(duì)以上問(wèn)題提出了一種三自由度直升機(jī)快速模型預(yù)測(cè)控制方法，根據(jù)系統(tǒng)約束和性能指標(biāo)函數(shù)對(duì)一段時(shí)域內(nèi)的行為快速做出相應(yīng)預(yù)測(cè)，克服了控制問(wèn)題中帶約束控制求解時(shí)間太長(zhǎng)和僅限于處理小規(guī)模問(wèn)題的局限，由此提出了三自由度直升機(jī)快速模型預(yù)測(cè)控制方法。發(fā)明中利用凸優(yōu)化工具cvxgen描繪了三自由度直升機(jī)狀態(tài)模型并生成求解器，用該工具結(jié)合算法生成一種解決此類(lèi)問(wèn)題的求解工具，發(fā)明了提供一種性能高、求解速度快的三自由度直升機(jī)快速模型預(yù)測(cè)控制方法，同時(shí)也繼承了傳統(tǒng)模型預(yù)測(cè)控制問(wèn)題的系統(tǒng)處于最優(yōu)狀態(tài)這一特點(diǎn)，最后通過(guò)仿真驗(yàn)證了方法的可行性與優(yōu)化的效果。本發(fā)明提供了一種快速模型預(yù)測(cè)控制方法，分析了算法可處理的問(wèn)題規(guī)模范圍及控制性能，權(quán)衡了求解器的復(fù)雜度、求解速度以及實(shí)時(shí)性三個(gè)方面的性能。相較于傳統(tǒng)模型控制方法與顯示模型控制方法，本發(fā)明主要解決了隨著控制問(wèn)題規(guī)模增大需要更大存儲(chǔ)空間和求解時(shí)間的問(wèn)題。為使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清晰，下面就對(duì)本發(fā)明的技術(shù)方案作進(jìn)一步描述。三自由度直升機(jī)快速模型預(yù)測(cè)控制方法，包括如下步驟：步驟1)三自由度直升機(jī)建模；選取狀態(tài)輸入u＝[vfvb]t，輸出y＝[εpr]t；將高度軸微分運(yùn)動(dòng)方程中的-glacosε作為擾動(dòng)項(xiàng)，整理上述運(yùn)動(dòng)方程后可得系統(tǒng)的運(yùn)動(dòng)方程為：代入具體的相應(yīng)參數(shù)后得到最終狀態(tài)方程系數(shù)為：其中ε、p、λ分別為三自由度直升機(jī)狀態(tài)變量的高度角、俯仰角、旋轉(zhuǎn)角，以及它們的微分狀態(tài)變量，vf、vb是輸入狀態(tài)電壓，擾動(dòng)項(xiàng)la為直升機(jī)本體到高度軸之間的距離，g為重力加速度，f、h、c為最終狀態(tài)方程系數(shù)；步驟2)轉(zhuǎn)化qp問(wèn)題，內(nèi)點(diǎn)法求解；f(x)是該問(wèn)題中的性能指標(biāo)函數(shù)，如何優(yōu)化該函數(shù)稱(chēng)為二次規(guī)劃問(wèn)題，對(duì)于二次規(guī)劃問(wèn)題(quadraticprogramming,qp)，其標(biāo)準(zhǔn)形式可表示為：其中q∈rn，a∈rm×n，b∈rm，g∈rp×n，h∈rp；對(duì)于該問(wèn)題，最優(yōu)解的求解即是在多面體上極小化一個(gè)凸二次函數(shù)，它可利用其幾何解釋來(lái)說(shuō)明該問(wèn)題的多面體可行域、性能指標(biāo)函數(shù)的等位線和最優(yōu)點(diǎn)；將式(2)進(jìn)行轉(zhuǎn)化，求導(dǎo)后得到最優(yōu)化條件可表示為：其中s∈rp表示松弛變量，雙變量y∈rm，z∈rp，zisi＝0表示互補(bǔ)松弛條件。利用內(nèi)點(diǎn)法求解該問(wèn)題，首先進(jìn)行初始化；式(4)由(3)等式變換得到，求解式(4)確定4個(gè)原始對(duì)偶變量分別為x(0)＝x，y(0)＝y(tǒng)，其中αp＝inf{α|-z+α1≥0}，αd＝inf{α|z+α1≥0}；其次進(jìn)行迭代求解過(guò)程；迭代算法對(duì)初始點(diǎn)進(jìn)行優(yōu)化，進(jìn)過(guò)迭代后得到最優(yōu)解，它分為以下幾個(gè)步驟：s1：當(dāng)殘余變量和對(duì)偶間隙足夠小時(shí)則停止迭代，否則到s2；s2：通過(guò)求解(5)可得仿射變換方向，其中s＝diag(s)，z＝diag(z)；s3：通過(guò)求解(6)計(jì)算中心點(diǎn)校正方向；其中μ＝stz/p，α＝sup{α∈[0,1]|s+αδsaff≥0,z+αδzaff≥0}；s4：根據(jù)下式(7)更新原始對(duì)偶變量的搜索方向；s5：根據(jù)α＝min{1,0.99sup{α≥0|s+αδs≥0,z+αδz≥0}}確定近似步長(zhǎng)，再更新原始對(duì)偶變量；s6：返回到s1，直到求解出符合約束條件的全局最優(yōu)解；上述步驟中x、s、y、z分別為4個(gè)進(jìn)過(guò)優(yōu)化后的對(duì)偶變量，s＝diag(s)、z＝diag(z)，而對(duì)應(yīng)于預(yù)測(cè)控制問(wèn)題中a∈rn×n表示狀態(tài)矩陣，q∈rn×n代表狀態(tài)權(quán)重矩陣；步驟3)求解器的生成；利用凸優(yōu)化工具cvxgen生成高速求解器，首先在cvxxgen中編寫(xiě)代碼，代碼描述了該系統(tǒng)的問(wèn)題和系統(tǒng)的基本參數(shù)，其中編寫(xiě)部分包括維數(shù)(dimensions)部分、參數(shù)(parameters)部分、變量(variables)、目標(biāo)函數(shù)(minimize)和約束(subjectto)，用指定的語(yǔ)言描述出上述幾個(gè)部分并經(jīng)過(guò)編譯即得到求解器。該求解器根據(jù)代碼描述的基本參數(shù)和系統(tǒng)問(wèn)題計(jì)算出最優(yōu)解，生成輸入量u*應(yīng)用到系統(tǒng)之中，同時(shí)系統(tǒng)產(chǎn)生輸出量y與反饋量x。該求解器可在matlab或linux環(huán)境下使用；編譯完成后生成了若干文件，文件名為makefile、csolve.c、csolve.m、cvxsolve.m、description.cvxgen、ldl.c、make_csolve.m、solver.c、testsolver.c、util.c、solver.h，其中makefile是linux環(huán)境下需調(diào)用的基本makefile文件；csolve.c是運(yùn)行cvxgen求解器的可編譯文件，用于生成mex文件；csolve.m是csolve.c文件在m語(yǔ)言下的轉(zhuǎn)換；cvxsolve.m是cvx方法的求解文件，可直接調(diào)用；description.cvxgen是在cvxgen代碼編寫(xiě)頁(yè)面所描述問(wèn)題的源代碼；ldl.c是solver.c文件的基本測(cè)試代碼；make_csolve.m是通過(guò)調(diào)用mex命令可以生成csolve的mex文件；solver.c是求解qp問(wèn)題的核心文件；util.c是cvxgen算法中所有有效代碼。本發(fā)明具有以下優(yōu)點(diǎn)：1.本發(fā)明保留了三自由度直升機(jī)非線性模型，保障系統(tǒng)了穩(wěn)定性能。2.本發(fā)明采用內(nèi)點(diǎn)法迭代步數(shù)精確求解全局最優(yōu)解，優(yōu)化了全局并提高了系統(tǒng)性能。3.本發(fā)明利用了凸優(yōu)化理論，并應(yīng)用于三自由度直升機(jī)模型之中，求解所需時(shí)間提到了大幅的改善。4.本發(fā)明步驟明確結(jié)構(gòu)簡(jiǎn)單，擁有完善的理論支持。附圖說(shuō)明圖1是三自由度直升機(jī)實(shí)物圖圖2是三自由度直升機(jī)模型圖圖3是模型預(yù)測(cè)控制的基本框圖圖4是本發(fā)明的快速模型預(yù)測(cè)控制原始對(duì)偶內(nèi)點(diǎn)法求解qp問(wèn)題的流程圖圖5是本發(fā)明的基于凸優(yōu)化工具使用框圖圖6是本發(fā)明的基于凸優(yōu)化工具的代碼編寫(xiě)界面圖7是本發(fā)明的基于凸優(yōu)化工具的代碼編譯界面圖8是三自由度直升機(jī)高度角仿真曲線對(duì)比圖圖9是三自由度直升機(jī)俯仰角仿真曲線對(duì)比圖圖10是三自由度直升機(jī)旋轉(zhuǎn)角仿真曲線對(duì)比圖具體實(shí)施方式以下結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步說(shuō)明：本發(fā)明的三自由度直升機(jī)快速模型預(yù)測(cè)控制方法，包括以下步驟：步驟1)三自由度直升機(jī)建模；選取狀態(tài)輸入u＝[vfvb]t，輸出y＝[εpr]t；將高度軸微分運(yùn)動(dòng)方程中的-glacosε作為擾動(dòng)項(xiàng)，整理上述運(yùn)動(dòng)方程后可得系統(tǒng)的運(yùn)動(dòng)方程為：代入具體的相應(yīng)參數(shù)后得到最終狀態(tài)方程系數(shù)為：其中ε、p、λ分別為三自由度直升機(jī)狀態(tài)變量的高度角、俯仰角、旋轉(zhuǎn)角，以及它們的微分狀態(tài)變量，vf、vb是輸入狀態(tài)電壓，擾動(dòng)項(xiàng)la為直升機(jī)本體到高度軸之間的距離，g為重力加速度，f、h、c為最終狀態(tài)方程系數(shù)；圖2為三自由度直升機(jī)模型圖；步驟2)轉(zhuǎn)化qp問(wèn)題，內(nèi)點(diǎn)法求解；f(x)是該問(wèn)題中的性能指標(biāo)函數(shù)，如何優(yōu)化該函數(shù)稱(chēng)為二次規(guī)劃問(wèn)題，對(duì)于二次規(guī)劃問(wèn)題(quadraticprogramming,qp)，其標(biāo)準(zhǔn)形式可表示為：其中q∈rn，a∈rm×n，b∈rm，g∈rp×n，h∈rp；對(duì)于該問(wèn)題，最優(yōu)解的求解即是在多面體上極小化一個(gè)凸二次函數(shù)，它可利用其幾何解釋來(lái)說(shuō)明該問(wèn)題的多面體可行域、性能指標(biāo)函數(shù)的等位線和最優(yōu)點(diǎn)；將式(2)進(jìn)行轉(zhuǎn)化，求導(dǎo)后得到最優(yōu)化條件可表示為：其中s∈rp表示松弛變量，雙變量y∈rm，z∈rp，zisi＝0表示互補(bǔ)松弛條件。圖4是原始對(duì)偶內(nèi)點(diǎn)法求解qp問(wèn)題原理圖，它的詳細(xì)過(guò)程如下所示：利用內(nèi)點(diǎn)法求解該問(wèn)題，首先進(jìn)行初始化；式(4)由(3)等式變換得到，求解式(4)確定4個(gè)原始對(duì)偶變量分別為x(0)＝x，y(0)＝y(tǒng)，其中αp＝inf{α|-z+α1≥0}，αd＝inf{α|z+α1≥0}；(22)進(jìn)行迭代求解過(guò)程；迭代算法對(duì)初始點(diǎn)進(jìn)行優(yōu)化，進(jìn)過(guò)迭代后得到最優(yōu)解，它分為以下幾個(gè)步驟：s1：當(dāng)殘余變量和對(duì)偶間隙足夠小時(shí)則停止迭代，否則到s2；s2：通過(guò)求解(5)可得仿射變換方向，其中s＝diag(s)，z＝diag(z)；s3：通過(guò)求解(6)計(jì)算中心點(diǎn)校正方向；其中μ＝stz/p，α＝sup{α∈[0,1]|s+αδsaff≥0,z+αδzaff≥0}；s4：根據(jù)下式(7)更新原始對(duì)偶變量的搜索方向；s5：根據(jù)α＝min{1,0.99sup{α≥0|s+αδs≥0,z+αδz≥0}}確定近似步長(zhǎng)，再更新原始對(duì)偶變量；s6：返回到s1，直到求解出符合約束條件的全局最優(yōu)解；上述步驟中x、s、y、z分別為4個(gè)進(jìn)過(guò)優(yōu)化后的對(duì)偶變量，s＝diag(s)、z＝diag(z)對(duì)應(yīng)于預(yù)測(cè)控制問(wèn)題中a∈rn×n表示狀態(tài)矩陣，q∈rn×n代表狀態(tài)權(quán)重矩陣；步驟3)求解器的生成；圖5展示了凸優(yōu)化工具快速預(yù)測(cè)控制使用框圖；利用凸優(yōu)化工具cvxgen生成高速求解器，首先在cvxxgen中編寫(xiě)代碼，代碼描述了該系統(tǒng)的問(wèn)題和系統(tǒng)的基本參數(shù)，其中編寫(xiě)部分包括維數(shù)(dimensions)部分、參數(shù)(parameters)部分、變量(variables)、目標(biāo)函數(shù)(minimize)和約束(subjectto)，用指定的語(yǔ)言描述出上述幾個(gè)部分并經(jīng)過(guò)編譯即得到求解器。該求解器根據(jù)代碼描述的基本參數(shù)和系統(tǒng)問(wèn)題計(jì)算出最優(yōu)解，生成輸入量u*應(yīng)用到系統(tǒng)之中，同時(shí)系統(tǒng)產(chǎn)生輸出量y與反饋量x。該求解器可在matlab或linux環(huán)境下使用；圖6為代碼編寫(xiě)界面、圖7為代碼編譯界面；編譯完成后生成了若干文件，文件名為makefile、csolve.c、csolve.m、cvxsolve.m、description.cvxgen、ldl.c、make_csolve.m、solver.c、testsolver.c、util.c、solver.h，其中makefile是linux環(huán)境下需調(diào)用的基本makefile文件；csolve.c是運(yùn)行cvxgen求解器的可編譯文件，用于生成mex文件；csolve.m是csolve.c文件在m語(yǔ)言下的轉(zhuǎn)換；cvxsolve.m是cvx方法的求解文件，可直接調(diào)用；description.cvxgen是在cvxgen代碼編寫(xiě)頁(yè)面所描述問(wèn)題的源代碼；ldl.c是solver.c文件的基本測(cè)試代碼；make_csolve.m是通過(guò)調(diào)用mex命令可以生成csolve的mex文件；solver.c是求解qp問(wèn)題的核心文件；util.c是cvxgen算法中所有有效代碼。案例分析本發(fā)明通過(guò)以具體的三自由度直升機(jī)為對(duì)象，展現(xiàn)了快速模型預(yù)測(cè)控制在三自由度直升機(jī)建模、轉(zhuǎn)化為qp問(wèn)題與內(nèi)點(diǎn)法迭代求解、cvxgen求解器生成三個(gè)方面中的表現(xiàn)，同時(shí)通過(guò)與經(jīng)典的方法對(duì)比展現(xiàn)了本發(fā)明的優(yōu)越性能。本發(fā)明在通過(guò)半實(shí)物仿真平臺(tái)進(jìn)行實(shí)驗(yàn)，圖8給出的是三自由度直升機(jī)高度角仿真曲線，圖9給出的是三自由度直升機(jī)俯仰角仿真曲線，圖10給出的三自由度直升機(jī)旋轉(zhuǎn)仿真曲線，橫坐標(biāo)為時(shí)間，縱坐標(biāo)為角度。實(shí)驗(yàn)對(duì)象為基于凸優(yōu)化的快速模型預(yù)測(cè)控制(cvxgen)，比較對(duì)象分別為快速在線模型預(yù)測(cè)控制(fastmpc)、顯式模型預(yù)測(cè)控制(empc)和傳統(tǒng)的pid控制方法對(duì)，圖8中在同一初始狀態(tài)下，cvxgen調(diào)節(jié)至穩(wěn)定狀態(tài)的時(shí)間為1.8s左右，超調(diào)量為4.27％，穩(wěn)態(tài)誤差值達(dá)到-1.796e-008；圖9中同一初始狀態(tài)下，cvxgen至穩(wěn)定狀態(tài)的時(shí)間為2.4s左右，超調(diào)量為49.05％，穩(wěn)態(tài)誤差值達(dá)到-9.264e-007，；圖10中同一初始狀態(tài)下，cvxgen至穩(wěn)定狀態(tài)的時(shí)間為2s左右，超調(diào)量為4.18％，穩(wěn)態(tài)誤差值達(dá)到-2.539e-006；總結(jié)三張圖可得cvxgen控制效果優(yōu)于傳統(tǒng)pid、empc算法，調(diào)節(jié)時(shí)間小于其它算法，超調(diào)量也小于其它算法。表1中可看出，在高度角、俯仰角、旋轉(zhuǎn)角三個(gè)狀態(tài)參數(shù)中cvxgen算法的穩(wěn)態(tài)誤值小于empc和pid算法，表中可以看出cvxgen算法最優(yōu)即穩(wěn)態(tài)誤差最小，但與empc算法較為接近，但相比與pid，穩(wěn)態(tài)誤差遠(yuǎn)遠(yuǎn)小于pid算法。表2中用不同預(yù)測(cè)步長(zhǎng)來(lái)測(cè)試個(gè)算法的在線計(jì)算時(shí)間，可從表中看到，cvxgen算法隨著步長(zhǎng)t的增大，計(jì)算時(shí)間仍然保持很快，而傳統(tǒng)mpc算法與empc隨著步長(zhǎng)t的增大，在線計(jì)算時(shí)間顯著增大。從表1與表2中不難發(fā)現(xiàn)cvxgen在保持穩(wěn)態(tài)誤差即保持系統(tǒng)穩(wěn)定的情況下，大大提高了算法計(jì)算時(shí)間，提高了效率。表1不同算法三個(gè)角度的穩(wěn)態(tài)誤差值算法高度角俯仰角旋轉(zhuǎn)角cvxgen-1.796e-008-9.264e-007-2.539e-006empc-1.869e-0073.541e-007-4.597e-008pid-0.019170.02165-0.06275表2不同預(yù)測(cè)步長(zhǎng)下各算法在線計(jì)算時(shí)間比較(單位：秒)算法預(yù)測(cè)步長(zhǎng)t＝2t＝3t＝5t＝10t＝15cvxgen0.02890.03130.04950.0942/mpc0.26830.27780.30110.33970.3875empc19.181685.2733274.1230//當(dāng)前第1頁(yè)12