水下滑翔機輕量化建模及執行器約束下非線性MPC 控制器設計

王潔茹,李 崇,綦聲波,趙圓圓

(中國海洋大學 工程學院,山東 青島,266100)

0 引言

水下滑翔機作為一種新型智能海上移動觀測平臺,因其觀測范圍大、續航能力強及便于儀器搭載等優點,被廣泛應用于海洋勘測和通信等領域,成為認識、理解和開發海洋的重要載體[1]。隨著海洋科學的不斷發展以及海洋研究的不斷深入,水下觀測任務對水下滑翔機提出了低功耗、高穩定性及精準姿態控制的要求[2]。水下滑翔機的姿態控制精度直接決定了其所采集觀測數據的準確性。但水下滑翔機動力學模型復雜、建模維度過高等問題也給其運動分析及后續姿態控制帶來巨大挑戰。

水下滑翔機的動力學建模是分析水下滑翔機運動特性及控制器設計的重要基礎。美國普林斯頓大學Leonard 教授是水下滑翔機研究領域的先驅,其以通過內部質量塊的主動分配來進行姿態調節的浮力推進式固定翼滑翔機為研究對象,利用幾何關系和動力學原理對水下滑翔機進行分析,建立了相應6 自由度的動力學模型[3-4]。Graver 等[5]基于拉格朗日法,建立了11 自由度的動力學模型,并完成了仿真實驗及海試驗證。侯巍等[6]基于牛頓-歐拉法建立了水下滑翔機的6 自由度模型,對模型進行合理簡化后設計相應控制器。Song 等[7]基于拉格朗日法建立了六維度的考慮洋流和浮力損失的水下滑翔機動力學模型。范雙雙[8]采用參數化分析方法建立了洋流影響下的5 自由度水下滑翔機多體系統動力學模型。

總體而言,水下滑翔機的建模多基于牛頓-歐拉法、拉格朗日法等動力學原理,并考慮內部各機構的科氏力、向心力及其耦合作用以及各類水動力參數的干擾,模型較為詳盡地對滑翔機的線速度和各機構位置等狀態量進行了描述,但存在動力學模型維度過高、耦合程度大及欠缺對于執行器機構的實際建模等問題。

針對水下滑翔機精準姿態控制的需求,研究人員提出了一系列的控制方法。Lenaord 等[4]在簡化的水下滑翔機動力學模型上設計了線性二次型調節器(linear quadratic regulator,LQR)并進行了海試驗證。Mahmoudian 等[9]在所建立的動力學模型上設計了基于前饋/反饋環節的運動控制器并成功應用于水下滑翔機。Tchilian 等[10]針對水下滑翔機豎直平面上的動力學方程設計了LQR 控制器。Wang 等[11]提取水下滑翔機在豎直平面的動力學方程,設計了模糊自適應線性自抗擾控制器。孫秀軍[12]利用浮基多剛體理論推導了水下滑翔機的動力學方程,使用反向傳播(back propagation,BP)神經網絡設計了適用于水下滑翔機的軌跡跟蹤控制器,同時設計了具有高自適應和容錯能力的比例-積 分-微 分(proportional-integral-derivative,PID)神經網絡定深運動和姿態解耦控制器。嚴升等[13]對水下滑翔機的縱剖面運動方程進行精確建模,設計了PID 控制器,并完成了控制策略優化。李志超等[14]針對飛翼式滑翔機建立了縱垂面的動力學方程,設計了跟蹤微分模糊PID 控制器,并通過實驗驗證其有效性。陳弈煿等[15]建立了水下滑翔機水平面內運動模型,構建了參數自整定的PID控制方法。綜上所述,現有水下滑翔機的控制算法主要集中于PID、LQR 及其衍生算法,雖可實現良好的控制效果,但在控制算法的設計中,多針對于線性模型,尚未對滑翔機自身姿態極限、執行機構的物理約束及速率極限等實際約束問題進行研究。

文中基于拉格朗日法,針對水下滑翔機實際工作狀況及執行機構延遲影響,忽略次要影響因素建立輕量級滑翔機動力學模型以降低模型復雜度,便于對水下滑翔機作運動分析及相應控制器設計。并進一步提取其縱垂面的動力學方程,基于模型預測控制理論,引入實際物理約束,設計了有效的水下滑翔機俯仰角回路控制器。仿真驗證了文中模型和控制器的有效性。

1 水下滑翔機輕量化建模

1.1 動力學建模

為便于描述建模過程中各參量之間的轉換,文中對相應坐標系進行定義(如圖1 所示),慣性坐標系E0:(i,j,k)定義在大地坐標系下,以水下滑翔機入水點作為原點,i軸和j軸位于水平面且相互垂直,k軸則沿重力方向,以向下為正方向。機體坐標系e0:(x,y,z)以浮心作為原點,x軸沿水下滑翔機機體主軸指向其艏部,y軸垂直于x軸指向其右翼方向,z軸分別垂直于x軸和y軸,向下為正方向。速度坐標系 π0:(π1,π2,π3)的原點與機體坐標系的原點一致,將機體坐標系繞y軸旋轉角度 -α,此時機體坐標z軸則為速度坐標系的 π3軸,然后繞π3軸 旋轉角度 β,即可得到最終確定的速度坐標系。圖1 中,θp為俯仰角;?為橫滾角;ψ為航向角;Vt為系統在機體坐標系下的合速度。

圖1 坐標系定義Fig.1 Definition of coordinate system

水下滑翔機的建?；诶窭嗜談恿W原理,為了便于建模過程中受力分析方便,根據水下滑翔機結構組成特點及工作機理,將其看成由質量塊所組成的系統,包括殼體、控制系統等非運動部件的固定質量塊,用以調節水下滑翔機俯仰角和橫滾角的可移動質量塊以及表示凈浮力的浮力調節質量塊,并以rs,rr,rb分別表示它們在機體坐標系下的坐標。

根據各部件質量塊的受力情況,獲取其在機體坐標系下的系統總動能(包括平動動能和轉動動能)。浮力調節質量塊主要是靠水囊的抽放水來控制,其位置固定于浮心上,不會相對于機體運動,對系統總動能并無貢獻,因此水下滑翔機相對于機體坐標系下的總動能為

式中:T為總動能;Ts、Tr和Tf分別為固定質量塊、可移動質量塊以及水阻尼項的動能;v=[V,Ω]T為水下滑翔機機體坐標下的廣義速度(包含線速度及角速度);M為廣義慣性矩陣;MA為由科氏力及水動力引起的附加質量矩陣;Ct為總附加耦合項;CA為科氏力及水動力作用引起的附加耦合項;It為總慣性矩陣;Is為固定質量塊的慣性矩陣;Ir為可移動質量塊的慣性矩陣;IA為附加慣性矩陣;ms和mr分別為固定質量塊和可移動質量塊的質量。

隨后,通過總動能T對速度(包括線速度和角速度)求導,獲得水下滑翔機相對于機體坐標系下的總動量為

式中,定義 η=[P,Π]T為相對與機體坐標系下的動量(包括平動量和角動量分別為P和 Π)。

同時,分析水下滑翔機在慣性坐標系下的受力和力矩情況,得

式中:p和 π為水下滑翔機在慣性坐標系下的平動量和角動量;qsE,qrE,qbE為各質量塊相對于慣性坐標系原點的位置;fext,τext分別為慣性坐標系下的水動力和水動力矩;kw為慣性坐標系沿z軸方向的單位向量;mb為浮力調節質量塊的質量;g為重力加速度。

經過坐標轉換矩陣變換后,得到其相對于機體坐標系下的總動量,并與總動能進行關聯,解得水下滑翔機在機體坐標系下的合外力,從而建立滑翔機的動力學模型,即

式中:F=REB fext,T=REBτext分別為機體坐標系下的水動力和水動力力矩;REB為坐標轉換矩陣;g為重力加速度;V=[V1,V2,V3]T。

水下滑翔機上浮和下潛過程中姿態角的改變主要通過執行機構(如圖2 所示)中的電機動作,利用齒輪傳動完成可移動質量塊的平移和橫滾來實現。而電機轉速受限和齒輪機構傳動都導致可移動質量塊位置的改變需要一定時間。實際運作過程中,電機推動可移動質量塊不斷運動,需經過相應時間方可到達指定位置,輸入量與可移動質量塊移動距離之間呈現積分關系。因此結合上述內容,建立水下滑翔機最終動力學模型為

圖2 水下滑翔機及其執行機構Fig.2 Underwater glider and its actuator

式中: τ為時間常數;u為系統的輸入量。

1.2 模型輕量化

建模過程中,水動力項主要用于描述水下滑翔機運動過程中與周圍水體相互作用所導致的額外施加于水下滑翔機系統的作用力,其大小主要取決于其速度及其速度方向[5,16]。但在實際工作中,鑒于水下滑翔機是弱驅動器,且為了采集到盡可能多的精確數據,其實際轉向速度及穩定滑翔的速度較慢。因此水動力項在水下滑翔機穩定滑翔過程中所產生的影響微弱。則可依此將式(5)中的F和T忽略。

進一步地,式(5)中P×Ω、Π×Ω及P×V分別為由科氏力引起的線速度和角速度之間的耦合項。而上文分析已知,水下滑翔機穩定滑行過程中,其線速度和角速度較小,即可將上述物理量及式(5)里M所包含的相關科氏力項忽略。一般情況下,水下滑翔機是一個質量分布均勻的整體,則rs=0;而由于將浮力質量塊位置固定于浮心上,則rb=0[8],基于此對M再次簡化,得到最終的輕量化模型為

式中: 系統慣性矩陣Io=Is+Ir和θ=[?,θp,ψ]T分別為定義在機體坐標系下的線速度和角速度。

1.3 縱垂面輕量化建模

水下滑翔機在工作狀態下的主要運動形式是在縱垂面上進行“V”型鋸齒狀運動,通過所搭載傳感器完成對各類海洋數據的收集。因此,文中針對水下滑翔機的主要工作模式,提取滑翔機在縱垂面的輕量級動力學模型,并基于此進行后續運動分析及控制器設計。將水下滑翔機的運動約束在縱垂平面上(即機體坐標系下x-y平面及慣性坐標系下的i-k平面),有

式中: θp為水下滑翔機的俯仰角;Io2為其相對于y軸的慣性矩陣分量。

由文獻[16]得水下滑翔機機體長度Lglider=1.99 m,機身直徑Dglider=0.22 m,則rr33與rr11為十分微小的變化量,將其近似為一個常數,即mr(2rr33+2rr11)=D,則式(7)可化為

2 基于模型預測控制的水下滑翔機姿態控制算法設計

模型預測控制(model predictive control,MPC)是近年來被廣泛應用于工業生產的一種反饋控制策略,相較于其他控制算法,其能夠處理含有約束條件的多變量非線性控制問題,實現多目標優化[17]。針對水下滑翔機控制的相應算法應具備低計算量、低功耗的特性[18],文中已建立了輕量化水下滑翔機動力學模型,以減輕所設計控制器的計算負擔,下面將基于MPC 設計水下滑翔機的姿態控制器。

2.1 模型近似線性化和離散化

水下滑翔機進行俯仰角調節時,主要通過執行機構在x軸方向的平移運動實現。因此,基于上文推導的水下滑翔機動力學模型(式(8)),將rr3置0,即可得到水下滑翔機的俯仰角回路方程為

根據目標軌跡對系統模型進行線性化。假設系統的參考軌跡為

將偏差量xs-xr,uin-ur設置成新的變量和。f相對于參考狀態量的雅克比矩陣為A(t),相對于參考控制量的雅克比矩陣為B(t),則上式變為

利用歐拉法對式(13)進行離散化,可得離散化后的模型為

式中:Akt,t=A(t)·Δt+I,Δt為采樣間隔時間,I為單位矩陣;Bk,t=B(t)·Δt;Pout為輸出矩陣。

2.2 目標函數設計

為使水下滑翔機平穩、快速地跟蹤期望軌跡,以系統的控制量、控制增量以及狀態量偏差來構造目標函數,并加入終端不等式項確保系統的穩定性。

式中:Np為預測時域;Nc為控制時域;Q和R為權重系數矩陣;Pε為終端懲罰矩陣。

定義

則式(16)可以寫成緊湊的矩陣形式

根據模型預測控制原理,基于式(15)預測系統未來的動態,可得其預測狀態序列,寫成矩陣形式為

進一步,可得系統的預測輸出序列為

由式(19)和式(20)得

聯合式(18)和式(21)得

系統經過一段時間到達終端域后,在終端域內可以通過無約束線性反饋控制系統。結合式(15),必能找到一個線性反饋增益K,即存在一個線性狀態反饋u=Kε,保持kt,t+kt,tK漸進穩定,且總是存在一個鄰域 Ωε,使得系統滿足控制量約束和輸出約束。

通過求解Lyapunov 方程

即可得唯一確定的正定陣Pε(即終端懲罰矩陣)。

2.3 約束條件設置

考慮到水下滑翔機自身姿態極限,如俯仰角不能超過 ±90°、執行機構的運動范圍最大不超過機體長度以及執行電機的功率及轉速有限等因素,通過設置umin、umax、Δumin、Δumax、ymin和ymax,對其輸入量、輸入增量及系統輸出量進行約束。為保證閉環系統的穩定性,另加入終端約束ε(kt+Np|kt)∈Ωε。

最終設置約束條件如下

將約束條件(24)與目標函數(22)相結合,可將該控制問題轉化成以下優化求解問題

假設該優化問題有解,記為ΔU*(kt),根據滾動優化的原理,只取第1 個元素作為控制輸入增量,即

因此,定義每個時刻作用于系統的實際非線性控制量為

2.4 穩定性證明

假設在kt時刻優化問題有解,記為

則相對應得預測狀態序列和預測輸出序列為

假設系統無干擾及建模誤差,則

在kt+1時刻,預選一個預測控制序列為

ΔUkt+1的 前Nc-1個元素為kt時刻優化解的后Nc-1個元素,在此基礎上選取最后一個元素為Kε*(kt+Nc|kt)。

此時對應的預測狀態序列和預測輸出序列為

由于最優解必定優于可行解,有

由此得J*kt是單調遞減的。當且僅當yˉ=0,Δu=0,ε=0時其值為0,即閉環系統是漸進穩定的。

3 仿真實驗

3.1 運動仿真

針對上文所建水下滑翔機縱垂面動力學方程(8),并與輕量化建模前的模型進行對比,在Matlab平臺上進行仿真實驗。

仿真中所用的模型參數參考文獻[16],具體見表1。表中:KM、KM0和Kq分別為計算水動力時所用到的系數;IA2為附加慣性矩陣分量;Is2為固定質量塊的慣性矩陣分量;Ir2為可移動質量塊的慣性矩陣分量。

表1 實驗參數列表Table 1 List of experimental parameters

分析已知,水下滑翔機主要通過改變電池包在x軸方向上的位置改變俯仰角,從而實現在縱垂面的姿態調整,因此令rr3=0 m。選取初始狀態量為: 俯仰角角速度=0(°)/s,俯仰角 θ0=0,可移動質量塊在x軸上的移動距離rr1_0=0 m;初始控制輸入為: 系統控制輸入u=0 m/s2。

圖3為不同輸入階躍信號下俯仰角變化曲線。其中,以u作為輸入,分別在30 s 時加入0.2 m/s2和-0.2 m/s2的階躍信號,可以發現,在相同的輸入作用下,兩者的運動變化曲線基本重合,運動變化趨勢一致,驗證了文中所建立輕量化模型的可行性。

圖3 不同輸入階躍信號下俯仰角變化曲線Fig.3 Change curves of pitch angle under different input step signals

分析圖3 可知: 當u為零時,俯仰角 θp角度不變,角速度不變,可移動質量塊的位置rr1保持不變;當u為常值時,俯仰角以固定速率變化并最終穩定于 ±90°,角速度隨著俯仰角的變化而變化,可移動質量塊的位置則隨著輸入作用以一定速率不斷變化。顯然,輸入信號數值相同,符號相反,俯仰角及其角速度以及可移動質量塊位置的絕對值相同,變化趨勢相反,符合滑翔機的工作原理,驗證了所建立輕量化模型的有效性。

3.2 控制器仿真

針對水下滑翔機俯仰角回路的控制需求,利用所設計控制器對滑翔機俯仰角進行控制?；铏C搭載聲學多普勒海流記(acoustic doppler dual current profiler,AD2CP)進行海流觀測時,為了保證所采集數據的精確性,其最佳工作狀態為±17.4°[19];為了實現長航程觀測,其最佳工作狀態為± 22.5°[20]。因此分別針對水下滑翔機以上2 種工作狀態檢驗所設計控制器。

1) 工作狀態為±17.4°時

預測時域Np作為模型預測控制算法中的重要參數,代表算法預測未來動態的時長,表示控制器對系統未來發展趨勢的預測程度,對控制效果有直接影響。因此,首先設置不同的預測時域對比不同的輸出結果,選擇不同參數如下:Np=80,Nc=80;Np=100,Nc=80;Np=160,Nc=80。對應的結果輸出如圖4 所示。

圖4 不同預測時域對比結果Fig.4 Comparison of different prediction time domains

較小的預測時域無法預測足夠多的未來動向,可能導致跟蹤效果較差,過大的預測時域則會使算法考慮更多的未來軌跡趨勢變化,引入誤差導致控制效果變差。綜合考慮后,最終確定預測時域和控制時域分別為:Np=100,Nc=80。

為對比控制效果,另設計了PID 控制器進行對比仿真實驗,調整PID 的參數使其保持最佳的控制效果,通過試湊法確定最終的PID 控制參數為:kp=-1,kd=-5。時間t=40、80、120、160 s為滑翔機的狀態切換點,兩者的輸出比對結果如圖5所示。圖中,RL-MPC(real-time linearization MPC)為文中設計的實時線性化模型預測控制器。

圖5 ±17.4°時控制效果對比Fig.5 Comparison of control effect at ±17.4°

2) 工作狀態為±22.5°時。

工作狀態為±22.5°時,設置約束條件為: -1.5 ≤Δu≤1.5、-20≤u≤20、-b≤y≤b,b=[25 25 0.8]T。

參考1)中的調參步驟,選取相應的預測時域和控制時域為:Np=100,Nc=80。

同樣,為了對比控制效果,設計了相應的PID控制器,其控制參數為:kp=-1,kd=-5。兩者輸出對比結果如圖6 所示。

圖6 ±22.5°時控制效果對比Fig.6 Comparison of control effect at ±22.5°

從圖5 與圖6 的仿真結果可以看出,水下滑翔機通過輸入信號調節可移動質量塊的位置來改變自身狀態,能對控制指令實現及時響應。同時,對比二者的俯仰角調節結果可以發現,文中所設計控制器與PID 控制器均可做到無超調地實現對水下滑翔機俯仰角的穩定控制。兩控制器的性能指標如表2 所示。

表2 RL-MPC 與PID 控制器性能指標對比Table 2 Performance Index of RL-MPC and PID Controller

由上表可知,在0～40 s 時間段,水下滑翔機完成下潛運動,在工作狀態為±17.4°時,PID 所需上升時間和調節時間分別為9.27 s 和13.06 s;RL-MPC控制器的上升時間和調節時間為2.41 s 和3.60 s,分別較PID 縮短74.0%和72.4%。在工作狀態為±22.5°時,PID 所用上升時 間和調節時間 分別為9.40 s 和13.23 s;RL-MPC 控制器的上升時間和調節時間為2.60 s 和3.88 s,分別較PID 縮短72.3%和70.7%。

在40～80 s 時間段,水下滑翔機完成上浮運動,在工作狀態為±17.4°時,PID 所需上升時間和調節時間分別為9.47 s 和13.42 s;RL-MPC 控制器的上升時間和調節時間為2.50 s 和3.75 s,分別較PID縮短73.6%和72.1%;在工作狀態為±22.5°時,PID所用上升時間和調節時間分別為9.48 s 和13.51 s,RL-MPC 控制器的上升時間和調節時間為2.65 s和3.92 s,分別較PID 縮短72%和71%。顯然,RLMPC 控制器比PID 控制器的控制性能更好,體現在更快的響應速度、更短的調節時間和上升時間,說明采用該控制器的水下滑翔機能及時、快速地實現滑翔機姿態的改變,從而可更好地收集海洋數據,提升數據的準確性。

4 結束語

針對水下滑翔機控制算法未充分考慮實際物理約束限制的問題,以及動力學建模普遍維度過高而難以準確分析運動狀況并設計相應控制器的問題,依據水下滑翔機的工作原理,忽略建模過程中的次要影響因素,并進一步將傳動機構的延遲納入建模之中,建立了輕量化的水下滑翔機動力學模型,利用更簡單的模型實現對滑翔機運動狀態的準確描述。同時針對水下滑翔機縱垂平面的俯仰角回路控制問題,考慮狀態量和控制機構物理極限等約束情況,設計了非線性約束下的模型預測控制器,以實現對水下滑翔機俯仰角的精確控制。

對比實驗表明,文中所提出的輕量化模型符合水下滑翔機的工作原理,能實現對水下滑翔機運動情況的準確描述,仿真驗證了其有效性。在±17.4°和 ±22.5°這2 種水下滑翔機常見工況下,所設計的RL-MPC 控制算法能有效提高水下滑翔機的控制精度?，F階段的驗證工作均在仿真環境中進行,海試試驗有待下一步開展,同時控制器在實際試驗中的可靠性也有待進一步分析和研究。