李岳明,王小平,張軍軍,曹 建,張英浩
(1. 哈爾濱工程大學(xué) 船舶工程學(xué)院,哈爾濱 150001; 2. 中國艦船研究設(shè)計(jì)中心,武漢 430064)
X型尾舵(下文簡稱X舵)是分離式舵的一種形式,最先應(yīng)用于潛艇.1961年美國海軍利用“大青花魚”號潛艇對X舵的可操縱性進(jìn)行實(shí)驗(yàn),獲得的結(jié)論為X舵優(yōu)于十字型尾舵(下文簡稱十字舵)[1].目前,各國都十分重視開發(fā)海洋資源及發(fā)展軍用水下無人裝備,智能水下機(jī)器人(AUV)成為了研究熱點(diǎn)之一,而X舵AUV比十字舵AUV具有更好的操縱性[2],因此X舵AUV獲得了廣泛關(guān)注.X舵AUV的產(chǎn)品有法國的A-18D、冰島的GAVIA、日本的Yumeiruka、沈陽自動化研究所的便攜式X舵AUV[3].X舵AUV擁有諸多優(yōu)點(diǎn)[4]:① 在近水面處舵效更好;② 卡舵時(shí)擁有更高的容錯(cuò)性;③ 可控制橫滾;④ 在舵面積相同的情況下,X舵比十字舵的轉(zhuǎn)彎半徑小.國外研究雖有涉及X舵AUV或潛艇等方面,但對其中的控制分配技術(shù)少有提及,因此控制分配技術(shù)的研究具有重要的意義.
控制分配是過驅(qū)動航行器與冗余控制系統(tǒng)中的一項(xiàng)關(guān)鍵技術(shù),控制分配技術(shù)的研究始于20世紀(jì)90年代.文獻(xiàn)[5]基于轉(zhuǎn)矩可達(dá)集提出直接分配法,利用幾何優(yōu)化法直接尋找可行解,但存在計(jì)算量較大的問題.文獻(xiàn)[6]針對水下機(jī)器人提出一種加權(quán)偽逆分配法,其計(jì)算過程雖然簡單有效,但未考慮執(zhí)行器約束.文獻(xiàn)[7]將序列二次規(guī)劃算法(SQP)用于7km載人潛水器的控制分配,考慮執(zhí)行器約束,分配精度較高,但同樣存在計(jì)算復(fù)雜的問題.文獻(xiàn)[8]針對X舵AUV提出一種基于 L’evy flight理論的控制分配方法,但由于其迭代是隨機(jī)的,所以計(jì)算量同樣較大.
AUV的工作環(huán)境十分復(fù)雜,在其自主作業(yè)過程中,對自身狀態(tài)控制的準(zhǔn)確性與穩(wěn)定性要求較高.復(fù)雜的控制分配算法會降低其控制的實(shí)時(shí)性,從而增加AUV的反應(yīng)時(shí)間.本文基于Lagrange乘子法和序列二次規(guī)劃算法,以考慮執(zhí)行器約束、控制分配精度、減少計(jì)算量為目標(biāo),提出一種改進(jìn)的適用于X舵AUV的控制分配方法,最后通過仿真試驗(yàn)驗(yàn)證該方法的有效性.
X舵AUV的運(yùn)動控制方法與十字舵AUV的控制方法基本一致,因此適用于十字舵AUV的運(yùn)動控制,如比例微分積分(PID)控制、模糊控制、S面控制等方法.X舵AUV在運(yùn)動控制體系結(jié)構(gòu)中與十字舵AUV的不同之處在于增加了控制分配環(huán)節(jié).X舵AUV的運(yùn)動控制體系如圖1所示.在X舵AUV的實(shí)際控制分配問題中,虛擬控制指令τ為3×1向量,包含橫傾力矩K、縱傾力矩M、偏航力矩N;u為4×1向量,包含各執(zhí)行機(jī)構(gòu)的輸入,分別對應(yīng)X舵的4個(gè)尾舵;r為期望控制目標(biāo);y為AUV的運(yùn)動狀態(tài)信息.X舵AUV執(zhí)行機(jī)構(gòu)的個(gè)數(shù)多于實(shí)際要求的自由度個(gè)數(shù),因此是一類具有冗余控制特性的航行器.
圖1 X舵AUV運(yùn)動控制體系Fig.1 X-rudder AUV motion control architecture
本文的研究重點(diǎn)是驗(yàn)證控制分配算法的可行性與正確性,因此可選用傳統(tǒng)的PID控制.傳統(tǒng)的PID控制以其結(jié)構(gòu)簡單、穩(wěn)定性好、工作可靠、調(diào)整方便等優(yōu)點(diǎn)獲得了廣泛應(yīng)用.由于研究對象并沒有裝備垂推與側(cè)推,只有一個(gè)主推,AUV通過尾舵產(chǎn)生的偏航力矩與縱傾力矩實(shí)現(xiàn)轉(zhuǎn)向與升沉運(yùn)動,所以對剩余的4個(gè)自由度分別設(shè)計(jì)一個(gè)獨(dú)立的控制器.
(1)
式中:kP,kI,kD分別為比例項(xiàng)、積分項(xiàng)、微分項(xiàng)的調(diào)節(jié)系數(shù),不同控制器的參數(shù)不同;O為控制輸出,在X舵AUV的運(yùn)動控制過程中,可取K,M,N中的任意項(xiàng).
對于深度控制,由于需通過改變AUV的縱傾角度來間接實(shí)現(xiàn)深度調(diào)節(jié),所以不能直接使用深度偏差作為控制輸入.使用虛擬變量期望縱傾角θd與實(shí)際縱傾角θ的偏差來代替深度偏差作為控制輸入,θd可通過下式獲得:
(2)
式中:zd為AUV的期望目標(biāo)深度;sat1(·)為飽和函數(shù);z為AUV的實(shí)際深度;C為常數(shù),其值可由不同AUV模型及實(shí)際工作任務(wù)而定.獲得期望縱傾角后,進(jìn)而可以獲得角度偏差eθ=θd-θ,將eθ帶入式(1)可獲得控制輸出.進(jìn)一步通過控制AUV的縱傾角度,就可實(shí)現(xiàn)對于AUV的深度控制.
線性控制分配問題一般可表述為如下形式:
(3)
s.t.ul≤u≤ut
式中:B為控制分配效率矩陣;ul與ut為所有執(zhí)行機(jī)構(gòu)的上下限約束向量.實(shí)際輸入與期望控制力矩的映射可以用函數(shù)f表示,非線性映射關(guān)系可以表示為
X舵有4個(gè)尾舵,每一個(gè)尾舵均可獨(dú)立偏轉(zhuǎn),因此其操舵組合靈活.X舵相對于十字舵也有諸多優(yōu)點(diǎn)[4].X舵AUV的三維模型如圖2所示,其X尾舵的配置形式如圖3所示.由圖3可知,從尾部往艏部看,序號1、2、3、4分別代表AUV尾部的4個(gè)舵葉,每一個(gè)舵葉都有一個(gè)單獨(dú)的舵機(jī)與之相連,舵與舵之間的夾角為90°,每個(gè)舵與水平軸及其垂直軸的夾角大小均為45°.根據(jù)文獻(xiàn)[2]的研究結(jié)果,45° X舵潛艇的水動力性能較為優(yōu)良,而X舵AUV可看成是縮小版的X舵潛艇.
圖2 X舵AUV的三維模型Fig.2 Three dimensional model of X-rudder AUV
圖3 X尾舵的配置形式Fig.3 Configuration form of X tail rudder
X舵AUV的工作環(huán)境及其分配環(huán)境比較復(fù)雜,為了減小復(fù)雜度,在保證模型合理的前提下,做出如下假設(shè):
(1) 從虛擬指令到執(zhí)行機(jī)構(gòu)的輸出映射均為線性;
(2) 每個(gè)舵的材料、表面平滑度、尺寸完全一樣;
(3) 不考慮舵與機(jī)器人本身以及舵與舵之間的耦合.
將問題線性化后,可獲得B.對于具體的某型X舵AUV,B與AUV的速度、尾舵等參數(shù)相關(guān),可表示為
(4)
式中:v為AUV的速度;ki(i=1,2,3,4)為X舵AUV舵葉的橫傾力矩系數(shù);mi為舵葉的縱傾力矩系數(shù);ni為舵葉的偏航力矩系數(shù).這些系數(shù)可以通過專業(yè)的流體計(jì)算軟件計(jì)算并加以處理獲得.
AUV的總力矩τ=[KMN]T,u=[u1u2u3u4]T,ui(i=1,2,3,4)分別對應(yīng)每一個(gè)舵葉的指令輸入,即需要偏轉(zhuǎn)的舵角大小.則式(3)可表示為
(5)
X舵AUV的控制分配問題,簡單而言就是通過已知的τ,求解未知的u.
偽逆(Pi)控制分配是常用的一種控制分配方法,許多的控制分配方法都以偽逆控制分配為基礎(chǔ)進(jìn)行開發(fā).偽逆法具有計(jì)算量小和易于實(shí)現(xiàn)等優(yōu)點(diǎn),但未考慮執(zhí)行機(jī)構(gòu)的飽和 (位置和偏轉(zhuǎn)速率) 約束限制,也不能實(shí)現(xiàn)可達(dá)集的全覆蓋.Bordignon等[9]通過F-18 High Alpha Research Vehicle (HARV)實(shí)驗(yàn)得出偽逆法只可實(shí)現(xiàn)可達(dá)集中的部分虛擬指令,最好的情況也只能達(dá)到其中的42.7%.
本文所提的偽逆是Moore-Penrose逆矩陣,可以通過證明[10]驗(yàn)證矩陣的偽逆是唯一的,欠定方程組的偽逆解即是最小范數(shù)解,是方程組的一個(gè)可行解,可表示為
u=BT(BBT)-1τ=B+τ
(6)
式中:B+為B的偽逆矩陣.通過偽逆法求出的解有時(shí)會超出執(zhí)行機(jī)構(gòu)的約束限制,所獲得的結(jié)果一般也不是最優(yōu)解.對于多目標(biāo)優(yōu)化求解問題,常采用非線性規(guī)劃方法進(jìn)行求解,例如序列二次規(guī)劃算法.
SQP是求解約束問題最有效的方法之一[11],其基本思想為每一步迭代均通過求解一個(gè)二次規(guī)劃子問題來確立一個(gè)下降方向,以減少價(jià)值函數(shù)獲取步長,重復(fù)這些步驟直到求得原問題的解.引用一種基于Lagrange函數(shù)Hesse矩陣的SQP,在原方法的基礎(chǔ)上改進(jìn)X舵AUV的控制分配,用以求解控制分配約束的優(yōu)化問題.
二次規(guī)劃思想中需要求解的問題可以用如下的通用方程組表示:
(7)
式中:f(u)為目標(biāo)優(yōu)化函數(shù);h(u)為等式約束函數(shù);g(u)為不等式約束函數(shù).針對以上問題求解,首先構(gòu)建Lagrange函數(shù)
L(x,μ,λ)=f(x)-μTh(x)-λTg(x)
(8)
式中:μ,λ分別為等式約束與不等式約束對應(yīng)的 Lagrange 乘子.再構(gòu)造二次規(guī)劃子問題
(9)
(1) 求解?,方法如下:
?k+1=?k-σkh(xk)
(10)
式中:σ為系數(shù)乘子.若‖h(uk+1)‖≥‖h(uk)‖,則修改σk+1=ησk,其中η∈(0,1)為系數(shù)乘子,σk=2,h(uk)為等式約束函數(shù)對應(yīng)的uk值,這里應(yīng)用了類似梯度下降法求解?.
(2) 求解d,將式(9)轉(zhuǎn)化為
(11)
(12)
式中:方程左邊的系數(shù)矩陣為Lagrange矩陣;a為等式約束h(uk)中的系數(shù)矩陣.
定理1設(shè)Bk∈Rn×n對稱正定,a行滿秩.若在式(9)的解d*滿足二階充分條件,即
(13)
對于式(9)中的不等式約束限制,設(shè)立一個(gè)飽和函數(shù)sat2()來考慮,表達(dá)式為
(14)
改進(jìn)二次規(guī)劃算法(IQP)計(jì)算步驟如下:
步驟1指定u初始迭代點(diǎn)u0=[0 0 0 0]T.
步驟3用改進(jìn)的算法求解子問題,獲得dk,?.
步驟4判斷,若‖dk‖1≤β1且 ‖hk‖+‖(gk)-‖1≤β2,停止計(jì)算,輸出解uk;否則,進(jìn)入步驟5.
步驟5選擇某種價(jià)值函數(shù)fv,設(shè)jk為滿足下式的最小正整數(shù)j,則有
令αk=ρjk,uk+1=uk+αkdk.
步驟7更新Ek,k=k+1,轉(zhuǎn)步驟3.
為了驗(yàn)證IQP的快速性,不考慮AUV的運(yùn)動控制模型,只考慮一個(gè)優(yōu)化求解問題,目標(biāo)函數(shù)、約束函數(shù)如式(13)所示.將改進(jìn)二次規(guī)劃算法的計(jì)算時(shí)間與序列二次規(guī)劃算法的計(jì)算時(shí)間進(jìn)行對比,參考文獻(xiàn)[12]中的舵升力解析模型建立方法.當(dāng)AUV的運(yùn)動速度為1 m/s時(shí),計(jì)算所得的控制分配效率矩陣參數(shù)如表1所示;SQP與IQP的計(jì)算結(jié)果對比如表2所示,其中:eK為橫傾力矩誤差;eM為縱傾力矩誤差;eN為偏航力矩誤差;t為計(jì)算時(shí)間.
表1 控制分配效率矩陣參數(shù)Tab.1 Parameters of control allocation efficiency matrix
表2 SQP與IQP的計(jì)算結(jié)果對比Tab.2 Comparison of SQP and IQP calculations
由表2可知,IQP減少了40%的計(jì)算時(shí)間,而分配結(jié)果誤差與SQP相當(dāng).
表3 運(yùn)動模型參數(shù)Tab.3 Parameters of motion model
minf=
(1-?)uTw1u+?(τ-Bu)Tw2(τ-Bu)
(13)
等式約束:τ=Bu
式中:w1,w2為單位對角權(quán)值矩陣,通過調(diào)節(jié)其大小可改變對應(yīng)舵的使用率及優(yōu)先使用權(quán);?=0.5,為能源消耗問題與分配精確問題的權(quán)重;ul=[-30°,-30°,-30°,-30°]T和ut=[30°,30°,30°,30°]T.
將Pi與IQP的控制分配結(jié)果進(jìn)行對比, AUV的縱向速度、深度、艏向角以及橫傾角的仿真控制曲線如圖4~7所示.其中,ψ為艏向角;φ為橫傾角.由于Pi分配結(jié)果在t=3 s時(shí)超出舵位置偏轉(zhuǎn)約束,所以采用Pi控制分配的結(jié)果比IQP更晚達(dá)到穩(wěn)定狀態(tài),且采用IQP的AUV姿態(tài)控制的效果更好.
X舵AUV采用兩種控制分配算法獲得的各個(gè)尾舵執(zhí)行的偏轉(zhuǎn)角度曲線如圖8~11所示.
X舵AUV采用兩種控制分配算法獲得的實(shí)際橫傾力矩、縱傾力矩、轉(zhuǎn)艏力矩與對應(yīng)3個(gè)自由度的期望力矩的分配誤差曲線如圖12~14所示.
從圖4~7可以看出,X舵AUV的艏向、軸向速度、深度在經(jīng)過一定時(shí)間的控制調(diào)整后都穩(wěn)定在了設(shè)定值.對于設(shè)定的X舵舵角的位置偏轉(zhuǎn)約束[-30°,30°],當(dāng)t=3 s時(shí),顯示Pi控制分配在舵1的分配上超出了約束限制(見圖8),此時(shí)Pi控制分配的結(jié)果與AUV實(shí)際的執(zhí)行器輸出會產(chǎn)生較大的誤差(見圖12~14).相對的,IQP分配角度曲線沒有超出約束,對于Pi控制分配超出約束部分,IQP將其分配到了其余的3個(gè)舵上,且K與N的分配誤差較小,可以忽略不計(jì),而M的分配誤差相對較大,但仍小于分配舵角值的1%,故是優(yōu)化算法計(jì)算時(shí)做出的取舍結(jié)果.從計(jì)算時(shí)間、分配誤差以及分配結(jié)果來看,仿真結(jié)果都證明了IQP的有效性與可行性.
圖4 AUV的縱向速度曲線Fig.4 Longitudinal speed curves of AUV
圖5 AUV的深度曲線Fig.5 Depth curves of AUV
圖6 AUV的艏向角曲線Fig.6 Yaw curves of AUV
圖7 AUV的橫傾角曲線Fig.7 Rolling angle curves of AUV
圖8 舵1偏轉(zhuǎn)角度曲線Fig.8 Deflecting angle curves of Rudder 1
圖9 舵2偏轉(zhuǎn)角度曲線Fig.9 Deflecting angle curves of Rudder 2
圖10 舵3偏轉(zhuǎn)角度曲線Fig.10 Deflecting angle curves of Rudder 4
圖11 舵4偏轉(zhuǎn)角度曲線Fig.11 Deflecting angle curves of Rudder 4
圖12 橫傾力矩分配誤差對比Fig.12 Comparison of rolling torque allocation errors
圖13 縱傾力矩分配誤差對比Fig.13 Comparison of pitch torque allocation errors
圖14 偏航力矩分配誤差對比Fig.14 Comparison of yaw torque allocation errors
針對X舵AUV的姿態(tài)控制分配問題,采用序列二次規(guī)劃方法,實(shí)現(xiàn)了執(zhí)行器輸出范圍約束條件下的準(zhǔn)確分配.為了改善序列二次規(guī)劃求解的計(jì)算效率,采用Lagrange乘子法替代光滑牛頓法.仿真試驗(yàn)結(jié)果表明,改進(jìn)的序列二次規(guī)劃方法有效地提高了控制分配的計(jì)算效率,保證了控制分配的精度,AUV姿態(tài)控制效果滿足作業(yè)任務(wù)需求.另外,所提方法基于約束函數(shù)的梯度計(jì)算可行下降方向,因此該算法同樣適用于非線性約束的控制分配模型.