一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方法
【專利摘要】一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方法，步驟如下：給定期望跟蹤值；制導(dǎo)誤差計(jì)算：計(jì)算期望位置與實(shí)際位置之間的距離誤差，角度誤差；動(dòng)力學(xué)方程縱橫向分解，控制器設(shè)計(jì)只取其橫向狀態(tài)量；求解離散化系統(tǒng)方程：對(duì)由以上步驟得到的平流層飛艇橫側(cè)向連續(xù)系統(tǒng)進(jìn)行線性化處理，并且也將誤差導(dǎo)數(shù)和進(jìn)行線性化處理。然后將飛艇橫向狀態(tài)量和誤差當(dāng)成擴(kuò)展?fàn)顟B(tài)量，并且對(duì)擴(kuò)展連續(xù)狀態(tài)空間方程離進(jìn)行離散化處理；預(yù)測(cè)系統(tǒng)未來(lái)動(dòng)態(tài)：根據(jù)由組合慣導(dǎo)等傳感器測(cè)量得到的當(dāng)前狀態(tài)量預(yù)測(cè)未來(lái)某一段時(shí)間的狀態(tài)量或輸出量；構(gòu)造模型預(yù)測(cè)控制目標(biāo)函數(shù)：由預(yù)測(cè)狀態(tài)量構(gòu)造目標(biāo)函數(shù)，并用標(biāo)準(zhǔn)QP算法進(jìn)行求解得到系統(tǒng)輸入量。
【專利說(shuō)明】一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方 法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明提供一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方法，它為欠 驅(qū)動(dòng)平流層飛艇提供一種考慮執(zhí)行機(jī)構(gòu)飽和問(wèn)題的跟蹤平面路徑的新控制方法，屬于自動(dòng) 控制【技術(shù)領(lǐng)域】。

【背景技術(shù)】
[0002] 平流層飛艇是依靠空氣浮力駐空，在遠(yuǎn)離地表的平流層全天候全天時(shí)連續(xù)工作的 浮空器，其具有飛行高度適中，執(zhí)行任務(wù)時(shí)間長(zhǎng)，生存能力強(qiáng)，搭載有效載荷大等優(yōu)點(diǎn)，且在 通信，監(jiān)控，交通管理等領(lǐng)域具有廣闊的軍事和民用前景。平流層飛艇是一種非常復(fù)雜的非 線性系統(tǒng)，往往對(duì)其進(jìn)行建模時(shí)都會(huì)出現(xiàn)一定的不確定性。而且，飛艇在平流層飛行時(shí)總會(huì) 有一定的外部干擾。這些問(wèn)題都會(huì)導(dǎo)致系統(tǒng)不穩(wěn)定。而且飛艇是一種運(yùn)動(dòng)較為緩慢的系統(tǒng)， 在實(shí)際運(yùn)動(dòng)過(guò)程中執(zhí)行機(jī)構(gòu)存在最大限位，滾轉(zhuǎn)角速度，偏航角速度等不能過(guò)大。這些約束 限制了飛艇路徑跟蹤過(guò)程中的輸入量的大小。
[0003] 為解決這些問(wèn)題，本發(fā)明"一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控 制方法"，提出了基于動(dòng)力學(xué)線性模型的平面路徑跟蹤控制方法。該方法綜合了基于視線制 導(dǎo)的路徑跟蹤算法和模型預(yù)測(cè)控制理論。根據(jù)本發(fā)明所提出的方法和理論設(shè)計(jì)的控制器， 可以很好的解決外界的干擾和建模不確定度對(duì)平流層飛艇系統(tǒng)穩(wěn)定性的影響，為平流層飛 艇的路徑跟蹤控制的工程實(shí)現(xiàn)提供了有效的設(shè)計(jì)手段。


【發(fā)明內(nèi)容】

[0004] (1)目的：本發(fā)明的目的在于提供一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑 跟蹤控制方法，控制工程師可以在實(shí)際設(shè)計(jì)中按照該方法理論的步驟并結(jié)合實(shí)際系統(tǒng)參數(shù) 實(shí)現(xiàn)平流層飛艇的考慮執(zhí)行機(jī)構(gòu)飽和和外界擾動(dòng)問(wèn)題的路徑跟蹤控制。
[0005] (2)技術(shù)方案：本發(fā)明"一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制 方法"，其主要內(nèi)容及程序是：先由給定期望跟蹤路徑進(jìn)行制導(dǎo)導(dǎo)航計(jì)算，生成跟蹤距離誤 差和偏航角度誤差；將平流層飛艇動(dòng)力學(xué)方程按照縱橫向分解，并最終得到橫向動(dòng)力學(xué)方 程進(jìn)行控制器設(shè)計(jì)計(jì)算，得到控制量。然后將平流層飛艇和誤差項(xiàng)作為狀態(tài)量，并對(duì)其方程 在參考點(diǎn)位置進(jìn)行線性化處理；將連續(xù)系統(tǒng)模型進(jìn)行離散化處理；由當(dāng)前狀態(tài)量和輸出量 預(yù)測(cè)未來(lái)狀態(tài)量和輸出量；構(gòu)造模型預(yù)測(cè)控制目標(biāo)函數(shù)，并計(jì)算目標(biāo)函數(shù)中具體參數(shù)；利 用標(biāo)準(zhǔn)QP算法對(duì)目標(biāo)函數(shù)進(jìn)行求解，得到當(dāng)前系統(tǒng)的輸入量。實(shí)際應(yīng)用中，飛艇的位置、姿 態(tài)、速度等狀態(tài)量由組合慣導(dǎo)等傳感器測(cè)量得到，將由該方法計(jì)算得到的控制量傳輸至舵 機(jī)和推進(jìn)螺旋槳等執(zhí)行裝置即可實(shí)現(xiàn)平流層飛艇平面路徑跟蹤功能。
[0006] 本發(fā)明"一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方法"，其具體步 驟如下：
[0007] 步驟一給定期望跟蹤值：給定期望平面路徑；給定期望俯仰角Θ。、期望滾轉(zhuǎn)角 φ。；給定期望速度Uc。
[0008] 步驟二制導(dǎo)誤差計(jì)算：計(jì)算期望位置與實(shí)際位置之間的距離誤差e，角度誤差 ψ#
[0009] 步驟三動(dòng)力學(xué)方程縱橫向分解：將動(dòng)力學(xué)方程按照縱橫向進(jìn)行分解，控制器設(shè)計(jì) 只取其橫向狀態(tài)量。
[0010] 步驟四求解離散化系統(tǒng)方程：對(duì)由以上步驟得到的平流層飛艇橫側(cè)向連續(xù)系統(tǒng) 進(jìn)行線性化處理，并且也將誤差導(dǎo)數(shù)?和，進(jìn)行線性化處理。然后將飛艇橫向狀態(tài)量和誤差 當(dāng)成擴(kuò)展?fàn)顟B(tài)量，并且對(duì)擴(kuò)展連續(xù)狀態(tài)空間方程離進(jìn)行離散化處理。
[0011] 步驟五預(yù)測(cè)系統(tǒng)未來(lái)動(dòng)態(tài)：根據(jù)由組合慣導(dǎo)等傳感器測(cè)量得到的當(dāng)前狀態(tài)量預(yù) 測(cè)未來(lái)某一段時(shí)間的狀態(tài)量或輸出量。
[0012] 步驟六構(gòu)造模型預(yù)測(cè)控制目標(biāo)函數(shù)：由預(yù)測(cè)狀態(tài)量構(gòu)造目標(biāo)函數(shù)，并用標(biāo)準(zhǔn)QP 算法進(jìn)行求解得到系統(tǒng)輸入量uk。
[0013] 其中，在步驟一中所述的給定期望平面路徑為一條直線yp =axp+b，Vyp為飛艇 期望平面位置；所述的給定期望俯仰角Θ。、期望滾轉(zhuǎn)角φ。均為零；所述的給定期望速度為 U。= [U。，V。，wJT = [V，0, 0]τ，V> 0為常數(shù)，u。，V。，W。為期望速度沿艇體坐標(biāo)系的分解量。
[0014] 其中，在步驟二中所述的計(jì)算期望位置與實(shí)際位置之間的距離誤差e，角度誤差 ，，其計(jì)算方法如下：
[0015] 1)計(jì)算期望直線路徑的方向角Ψρ =arctan(a)，方向角誤差r= ,其中Ψ 為飛艇偏航角，見(jiàn)圖1所示。 ：QX-V h
[0016] 2)計(jì)算飛艇當(dāng)前位置與期望路徑垂直距離6 = p= [X，y]T為平流層飛 艇的當(dāng)前位置。
[0017] 3)計(jì)算方向角誤差導(dǎo)數(shù)夕=r?r為飛艇偏航角速度；期望偏航角為 V7mv- 食？Λ > 0為控制參數(shù)。
[0018] 4)計(jì)算距離誤差導(dǎo)數(shù) ￡>asin(i^) +vοο%(ψ) ^
[0019] 其中，在步驟三中所述的將動(dòng)力學(xué)方程和期望速度值按照縱橫向進(jìn)行分解，其分 解方法如下：
[0020] 記平流層飛艇動(dòng)力學(xué)模型方程為：
[0021] MX =F(X)+A(X)+G(X)+Βμ(!)
[0022]其中M=[mik]eR6X6(i, k= 1，2,…，6)為質(zhì)量矩陣；X= [υτ,ωτ]τ，其中u= [u，v，w]T為平流層飛艇速度沿艇體坐標(biāo)系的分解量；F(X) = [f\，f2，…，f6]T為科里奧利力 和慣性力項(xiàng)；A(X) = [a!，a2,…，a6]T為氣動(dòng)力項(xiàng)；G(X) = [gl,g2,…，g6]T為重力和浮力項(xiàng)； μ=[μ μ2，…，μ 6]τ為控制量。動(dòng)力學(xué)模型方程⑴中各項(xiàng)的具體值隨不同飛艇結(jié)構(gòu) 和參數(shù)而不同，在實(shí)際應(yīng)用中根據(jù)實(shí)際情況確定。
[0023] 將方程（1)按照縱橫向運(yùn)動(dòng)分解為縱向動(dòng)力學(xué)方程： Γηη94?M X=F+』+G+S μ Λ Λ雄雙" mng mng --W amg ssagr^zong ?
[0025] 和橫向動(dòng)力學(xué)方程：
[0026] 麗又式蛛氣·^真(3)
[0027] 其中縱向狀態(tài)量父_8=[11，'\￥，9]1'，橫向狀態(tài)量父。 (；=[￥，口，1']1'，下標(biāo)為2〇1^和〇6 的各項(xiàng)分別表示縱向運(yùn)動(dòng)項(xiàng)和橫向運(yùn)動(dòng)項(xiàng)。在控制器設(shè)計(jì)中，取橫向狀態(tài)量為設(shè)計(jì)變量。 如果不考慮滾轉(zhuǎn)，則可以設(shè)P為零，從而不將P設(shè)為狀態(tài)量。設(shè)計(jì)控制器時(shí)只考慮橫向狀態(tài) 量，從而設(shè)前向速度U為常數(shù)。
[0028] 其中，在步驟四中所述的求解離散化系統(tǒng)方程，其計(jì)算方法如下：
[0029] 1)線性化處理
[0030] 定義擴(kuò)展?fàn)顟B(tài)變量?對(duì)擴(kuò)展后系統(tǒng)方程在參考點(diǎn)κ〇#ωνA〇f和 = 0進(jìn)行線性化。線性化后得：

【權(quán)利要求】
1. 一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方法，其特征在于：其具體 步驟如下： 步驟一給定期望跟蹤值：給定期望平面路徑；給定期望俯仰角Θ。、期望滾轉(zhuǎn)角Φ。；給 定期望速度U。。 步驟二制導(dǎo)誤差計(jì)算：計(jì)算期望位置與實(shí)際位置之間的距離誤差e，角度誤差 步驟三動(dòng)力學(xué)方程縱橫向分解：將動(dòng)力學(xué)方程按照縱橫向進(jìn)行分解，控制器設(shè)計(jì)只取 其橫向狀態(tài)量。 步驟四求解離散化系統(tǒng)方程：對(duì)由以上步驟得到的平流層飛艇橫側(cè)向連續(xù)系統(tǒng)進(jìn)行 線性化處理，并且也將誤差導(dǎo)數(shù)和#進(jìn)行線性化處理。然后將飛艇橫向狀態(tài)量和誤差當(dāng) 成擴(kuò)展?fàn)顟B(tài)量，并且對(duì)擴(kuò)展連續(xù)狀態(tài)空間方程離進(jìn)行離散化處理。 步驟五預(yù)測(cè)系統(tǒng)未來(lái)動(dòng)態(tài)：根據(jù)由組合慣導(dǎo)等傳感器測(cè)量得到的當(dāng)前狀態(tài)量預(yù)測(cè)未 來(lái)某一段時(shí)間的狀態(tài)量或輸出量。 步驟六構(gòu)造模型預(yù)測(cè)控制目標(biāo)函數(shù)：由預(yù)測(cè)狀態(tài)量構(gòu)造目標(biāo)函數(shù)，并用標(biāo)準(zhǔn)QP算法 進(jìn)行求解得到系統(tǒng)輸入量uk。
2. 根據(jù)權(quán)利要求1所述的一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方 法，其特征在于：在步驟一中所述的給定期望平面路徑為一條直線yp =axp+b，Vyp為飛艇 期望平面位置；所述的給定期望俯仰角Θ。、期望滾轉(zhuǎn)角φ。均為零； 所述的給定期望速度為U。= [U。，V。，wJT = [V，0, 0]τ,V>O為常數(shù)，u。，V。，w。為期望 速度沿艇體坐標(biāo)系的分解量。
3. 根據(jù)權(quán)利要求1所述的一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方 法，其特征在于：在步驟二中所述的制導(dǎo)誤差計(jì)算，其計(jì)算方法如下： 1) 計(jì)算期望路徑的方向角vp =arctan(a)，方向角誤差，其中Ψ為飛艇偏 航角，見(jiàn)圖1所示。 2) 計(jì)算飛艇當(dāng)前位置與期望路徑垂直距離《 = p= [x，y]TS平流層飛艇的 當(dāng)前位置。 3) 計(jì)算期望偏航角為Λ>0為控制參數(shù)。 4) 計(jì)算方向角誤差導(dǎo)數(shù)# =hr為飛艇偏航角速度。 5) 計(jì)算距離誤差導(dǎo)數(shù)S=HSinPHvcos(^)0
4. 根據(jù)權(quán)利要求1所述的一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方 法，其特征在于：在步驟三中所述的動(dòng)力學(xué)縱橫向分解，其計(jì)算方法如下： 記自治飛艇動(dòng)力學(xué)模型方程為： MX ^ F(Ar)^ A(X)^-G(X)+ B μ (1) 其中M= [mik]eR6x6 (i,k= 1，2,…，6)為質(zhì)量矩陣；X= [υτ,ωτ]τ，其中u= [u，v，w]T為平流層飛艇速度沿艇體坐標(biāo)系的分解量；F(X) = [f\，f2，…，f6]T為科里奧利力 和慣性力項(xiàng)汸⑴二匕^^…七^(guò)為氣動(dòng)力項(xiàng)而⑴二匕^^…七^(guò)為重力和浮力項(xiàng)；μ= [μi，μ2，…，μ6]τ為控制量。動(dòng)力學(xué)模型方程（1)中各項(xiàng)的具體值隨不同飛艇結(jié)構(gòu) 和參數(shù)而不同，在實(shí)際應(yīng)用中根據(jù)實(shí)際情況確定。 將方程（1)按照縱橫向運(yùn)動(dòng)分解為縱向動(dòng)力學(xué)方程： =K,,+ 4?, + + (2) 和橫向動(dòng)力學(xué)方程： 域,=，《 + 4*++4a (3) 其中縱向狀態(tài)Xzmg= [u，w，q]T，橫向狀態(tài)Xm= [v，p，r]T，下標(biāo)為zong和ce的各項(xiàng)分 別表示縱向運(yùn)動(dòng)項(xiàng)和橫向運(yùn)動(dòng)項(xiàng)。在控制器設(shè)計(jì)中，取橫向狀態(tài)量為設(shè)計(jì)變量。如果不考 慮滾轉(zhuǎn)，則可以設(shè)P為零，從而不將P設(shè)為狀態(tài)量。設(shè)計(jì)控制器時(shí)只考慮橫向狀態(tài)量，從而 設(shè)前向速度u為常數(shù)。
5. 根據(jù)權(quán)利要求1所述的一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方 法，其特征在于：在步驟四中所求解離散化系統(tǒng)方程，其計(jì)算方法如下： 1) 線性化處理 定義擴(kuò)展?fàn)顟B(tài)變量I= ·對(duì)擴(kuò)展后系統(tǒng)方程在參考點(diǎn)尤.和 =0進(jìn)行線性化。線性化后得： ^=J：,J+J：t,ru (4) 其中f=X-(和!? =M-為相對(duì)于參考點(diǎn)的誤差。fP和L為連續(xù)系統(tǒng)方程分別對(duì)X和U的偏導(dǎo)數(shù)。 其中誤差方程線性化后為 ?=UCOS(Wim)W+VCOS(Km) (5) ψ ------ r 2) 計(jì)算離散系統(tǒng)方程 由于采樣時(shí)間一般都有比較短，我們可以采用前向差分的方法對(duì)連續(xù)系統(tǒng)進(jìn)行離散 化，設(shè)采樣周期為T(mén)，線性化后的擴(kuò)展?fàn)顟B(tài)空間方程為： k = AX + Bu (6) 則離散化之后的離散系統(tǒng)狀態(tài)空間方程為： Im=AkI k+BKuK (7) 其中Ak =A*T+IBk =B*T。
6. 根據(jù)權(quán)利要求1所述的一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方 法，其特征在于：在步驟五中所述的預(yù)測(cè)系統(tǒng)未來(lái)動(dòng)態(tài)，其計(jì)算方法如下： 由組合慣導(dǎo)等傳感器測(cè)量得到當(dāng)前時(shí)刻的狀態(tài)量Xk，通過(guò)此時(shí)的測(cè)量值預(yù)測(cè)未來(lái)某個(gè) 預(yù)測(cè)時(shí)域內(nèi)的狀態(tài)量的值。設(shè)預(yù)測(cè)時(shí)域長(zhǎng)度為N。則未來(lái)某一時(shí)刻狀態(tài)量的預(yù)測(cè)值為：
其中尤..ρ-?λ+.ρ...，尤、、為在已知當(dāng)前時(shí)刻狀態(tài)變量Ijf的基礎(chǔ)上根據(jù)離散系統(tǒng)方程計(jì) 算出來(lái)的；4Aiil，…，ΙλI1為未知變量，即為目標(biāo)函數(shù)的優(yōu)化變量。
7.根據(jù)權(quán)利要求1所述的一種基于模型預(yù)測(cè)控制的平流層飛艇平面路徑跟蹤控制方 法，其特征在于：在步驟五中所述的構(gòu)造模型預(yù)測(cè)控制目標(biāo)函數(shù)，其計(jì)算方法如下： 在某一時(shí)刻Κ，定義目標(biāo)函數(shù)為JK，其表達(dá)式如下所示：
定義狀態(tài)量約束和執(zhí)行機(jī)構(gòu)約束如下： -β<U <β 為、瑪什―丨 ^min -^K--J - -^πα\ 其中Q為狀態(tài)量加權(quán)矩陣，R為輸入量加權(quán)矩陣。 根據(jù)方程（15)，可以將方程（16)繼續(xù)推導(dǎo)成標(biāo)準(zhǔn)二次型形式，如下所示：
其中：
在用標(biāo)準(zhǔn)QP解法求解該二次型時(shí)，dK只是與當(dāng)前狀態(tài)量有關(guān)的常值，與輸入量'沒(méi) 有關(guān)系。所以在求解時(shí)可以刪去dK。對(duì)上述標(biāo)準(zhǔn)二次型進(jìn)行求解可以得到優(yōu)化解為： & ^^^義?:^所以仄時(shí)刻的系統(tǒng)輸入量為^的第一項(xiàng)肩巧為當(dāng)前時(shí)刻的輸入 量。在下一個(gè)采樣點(diǎn)重復(fù)執(zhí)行步驟五和步驟六，即可求出下一時(shí)刻的輸入量€,?·這樣進(jìn) 行反復(fù)在線滾動(dòng)優(yōu)化，即可解決考慮外界擾動(dòng)和執(zhí)行機(jī)構(gòu)限制情況下的路徑跟蹤控制問(wèn) 題。
【文檔編號(hào)】G05D1/10GK104317300SQ201410486563
【公開(kāi)日】2015年1月28日 申請(qǐng)日期:2014年9月22日 優(yōu)先權(quán)日:2014年9月22日
【發(fā)明者】祝明, 余帥先, 劉麗莎, 閆柯瑜, 陳天 申請(qǐng)人:北京航空航天大學(xué)