基于改進(jìn)二次規(guī)劃算法的X舵智能水下機(jī)器人控制分配

李岳明，王小平，張軍軍，曹 建，張英浩

(1. 哈爾濱工程大學(xué) 船舶工程學(xué)院，哈爾濱 150001； 2. 中國艦船研究設(shè)計中心，武漢 430064)

X型尾舵(下文簡稱X舵)是分離式舵的一種形式，最先應(yīng)用于潛艇.1961年美國海軍利用“大青花魚”號潛艇對X舵的可操縱性進(jìn)行實驗，獲得的結(jié)論為X舵優(yōu)于十字型尾舵(下文簡稱十字舵)[1].目前，各國都十分重視開發(fā)海洋資源及發(fā)展軍用水下無人裝備，智能水下機(jī)器人(AUV)成為了研究熱點之一，而X舵AUV比十字舵AUV具有更好的操縱性[2]，因此X舵AUV獲得了廣泛關(guān)注.X舵AUV的產(chǎn)品有法國的A-18D、冰島的GAVIA、日本的Yumeiruka、沈陽自動化研究所的便攜式X舵AUV[3].X舵AUV擁有諸多優(yōu)點[4]：① 在近水面處舵效更好；② 卡舵時擁有更高的容錯性；③ 可控制橫滾；④ 在舵面積相同的情況下，X舵比十字舵的轉(zhuǎn)彎半徑小.國外研究雖有涉及X舵AUV或潛艇等方面，但對其中的控制分配技術(shù)少有提及，因此控制分配技術(shù)的研究具有重要的意義.

控制分配是過驅(qū)動航行器與冗余控制系統(tǒng)中的一項關(guān)鍵技術(shù)，控制分配技術(shù)的研究始于20世紀(jì)90年代.文獻(xiàn)[5]基于轉(zhuǎn)矩可達(dá)集提出直接分配法，利用幾何優(yōu)化法直接尋找可行解，但存在計算量較大的問題.文獻(xiàn)[6]針對水下機(jī)器人提出一種加權(quán)偽逆分配法，其計算過程雖然簡單有效，但未考慮執(zhí)行器約束.文獻(xiàn)[7]將序列二次規(guī)劃算法(SQP)用于7km載人潛水器的控制分配，考慮執(zhí)行器約束，分配精度較高，但同樣存在計算復(fù)雜的問題.文獻(xiàn)[8]針對X舵AUV提出一種基于 L’evy flight理論的控制分配方法，但由于其迭代是隨機(jī)的，所以計算量同樣較大.

AUV的工作環(huán)境十分復(fù)雜，在其自主作業(yè)過程中，對自身狀態(tài)控制的準(zhǔn)確性與穩(wěn)定性要求較高.復(fù)雜的控制分配算法會降低其控制的實時性，從而增加AUV的反應(yīng)時間.本文基于Lagrange乘子法和序列二次規(guī)劃算法，以考慮執(zhí)行器約束、控制分配精度、減少計算量為目標(biāo)，提出一種改進(jìn)的適用于X舵AUV的控制分配方法，最后通過仿真試驗驗證該方法的有效性.

1 X舵AUV的運動控制

1.1 運動控制體系結(jié)構(gòu)

X舵AUV的運動控制方法與十字舵AUV的控制方法基本一致，因此適用于十字舵AUV的運動控制，如比例微分積分(PID)控制、模糊控制、S面控制等方法.X舵AUV在運動控制體系結(jié)構(gòu)中與十字舵AUV的不同之處在于增加了控制分配環(huán)節(jié).X舵AUV的運動控制體系如圖1所示.在X舵AUV的實際控制分配問題中，虛擬控制指令τ為3×1向量，包含橫傾力矩K、縱傾力矩M、偏航力矩N；u為4×1向量，包含各執(zhí)行機(jī)構(gòu)的輸入，分別對應(yīng)X舵的4個尾舵；r為期望控制目標(biāo)；y為AUV的運動狀態(tài)信息.X舵AUV執(zhí)行機(jī)構(gòu)的個數(shù)多于實際要求的自由度個數(shù)，因此是一類具有冗余控制特性的航行器.

圖1 X舵AUV運動控制體系Fig.1 X-rudder AUV motion control architecture

1.2 控制器設(shè)計

本文的研究重點是驗證控制分配算法的可行性與正確性，因此可選用傳統(tǒng)的PID控制.傳統(tǒng)的PID控制以其結(jié)構(gòu)簡單、穩(wěn)定性好、工作可靠、調(diào)整方便等優(yōu)點獲得了廣泛應(yīng)用.由于研究對象并沒有裝備垂推與側(cè)推，只有一個主推，AUV通過尾舵產(chǎn)生的偏航力矩與縱傾力矩實現(xiàn)轉(zhuǎn)向與升沉運動，所以對剩余的4個自由度分別設(shè)計一個獨立的控制器.

(1)

式中：kP，kI，kD分別為比例項、積分項、微分項的調(diào)節(jié)系數(shù)，不同控制器的參數(shù)不同；O為控制輸出，在X舵AUV的運動控制過程中，可取K,M,N中的任意項.

對于深度控制，由于需通過改變AUV的縱傾角度來間接實現(xiàn)深度調(diào)節(jié)，所以不能直接使用深度偏差作為控制輸入.使用虛擬變量期望縱傾角θd與實際縱傾角θ的偏差來代替深度偏差作為控制輸入，θd可通過下式獲得：

(2)

式中：zd為AUV的期望目標(biāo)深度；sat1(·)為飽和函數(shù)；z為AUV的實際深度；C為常數(shù)，其值可由不同AUV模型及實際工作任務(wù)而定.獲得期望縱傾角后，進(jìn)而可以獲得角度偏差eθ=θd-θ，將eθ帶入式(1)可獲得控制輸出.進(jìn)一步通過控制AUV的縱傾角度，就可實現(xiàn)對于AUV的深度控制.

2 X舵AUV控制分配的數(shù)學(xué)模型

2.1 控制分配問題描述

線性控制分配問題一般可表述為如下形式：

(3)

s.t.ul≤u≤ut

式中：B為控制分配效率矩陣；ul與ut為所有執(zhí)行機(jī)構(gòu)的上下限約束向量.實際輸入與期望控制力矩的映射可以用函數(shù)f表示，非線性映射關(guān)系可以表示為

2.2 X舵AUV的控制分配模型

X舵有4個尾舵，每一個尾舵均可獨立偏轉(zhuǎn)，因此其操舵組合靈活.X舵相對于十字舵也有諸多優(yōu)點[4].X舵AUV的三維模型如圖2所示，其X尾舵的配置形式如圖3所示.由圖3可知，從尾部往艏部看，序號1、2、3、4分別代表AUV尾部的4個舵葉，每一個舵葉都有一個單獨的舵機(jī)與之相連，舵與舵之間的夾角為90°，每個舵與水平軸及其垂直軸的夾角大小均為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) 每個舵的材料、表面平滑度、尺寸完全一樣；

(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è)的流體計算軟件計算并加以處理獲得.

AUV的總力矩τ=[KMN]T，u=[u1u2u3u4]T，ui(i=1,2,3,4)分別對應(yīng)每一個舵葉的指令輸入，即需要偏轉(zhuǎn)的舵角大小.則式(3)可表示為

(5)

X舵AUV的控制分配問題，簡單而言就是通過已知的τ，求解未知的u.

3 X舵AUV的控制分配

3.1 偽逆法

偽逆(Pi)控制分配是常用的一種控制分配方法，許多的控制分配方法都以偽逆控制分配為基礎(chǔ)進(jìn)行開發(fā).偽逆法具有計算量小和易于實現(xiàn)等優(yōu)點,但未考慮執(zhí)行機(jī)構(gòu)的飽和 (位置和偏轉(zhuǎn)速率) 約束限制，也不能實現(xiàn)可達(dá)集的全覆蓋.Bordignon等[9]通過F-18 High Alpha Research Vehicle (HARV)實驗得出偽逆法只可實現(xiàn)可達(dá)集中的部分虛擬指令，最好的情況也只能達(dá)到其中的42.7%.

本文所提的偽逆是Moore-Penrose逆矩陣，可以通過證明[10]驗證矩陣的偽逆是唯一的，欠定方程組的偽逆解即是最小范數(shù)解，是方程組的一個可行解，可表示為

u=BT(BBT)-1τ=B+τ

(6)

式中：B+為B的偽逆矩陣.通過偽逆法求出的解有時會超出執(zhí)行機(jī)構(gòu)的約束限制，所獲得的結(jié)果一般也不是最優(yōu)解.對于多目標(biāo)優(yōu)化求解問題，常采用非線性規(guī)劃方法進(jìn)行求解，例如序列二次規(guī)劃算法.

3.2 改進(jìn)二次規(guī)劃控制分配算法

SQP是求解約束問題最有效的方法之一[11]，其基本思想為每一步迭代均通過求解一個二次規(guī)劃子問題來確立一個下降方向，以減少價值函數(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è)立一個飽和函數(shù)sat2()來考慮，表達(dá)式為

(14)

改進(jìn)二次規(guī)劃算法(IQP)計算步驟如下：

步驟1指定u初始迭代點u0=[0 0 0 0]T.

步驟3用改進(jìn)的算法求解子問題，獲得dk,?.

步驟4判斷，若‖dk‖1≤β1且 ‖hk‖+‖(gk)-‖1≤β2，停止計算，輸出解uk；否則，進(jìn)入步驟5.

步驟5選擇某種價值函數(shù)fv，設(shè)jk為滿足下式的最小正整數(shù)j，則有

令αk=ρjk，uk+1=uk+αkdk.

步驟7更新Ek，k=k+1，轉(zhuǎn)步驟3.

4 控制分配算法驗證及其仿真分析

4.1 控制分配方法靜態(tài)對比實驗

為了驗證IQP的快速性，不考慮AUV的運動控制模型，只考慮一個優(yōu)化求解問題，目標(biāo)函數(shù)、約束函數(shù)如式(13)所示.將改進(jìn)二次規(guī)劃算法的計算時間與序列二次規(guī)劃算法的計算時間進(jìn)行對比，參考文獻(xiàn)[12]中的舵升力解析模型建立方法.當(dāng)AUV的運動速度為1 m/s時，計算所得的控制分配效率矩陣參數(shù)如表1所示；SQP與IQP的計算結(jié)果對比如表2所示，其中：eK為橫傾力矩誤差；eM為縱傾力矩誤差；eN為偏航力矩誤差；t為計算時間.

表1 控制分配效率矩陣參數(shù)Tab.1 Parameters of control allocation efficiency matrix

表2 SQP與IQP的計算結(jié)果對比Tab.2 Comparison of SQP and IQP calculations

由表2可知，IQP減少了40%的計算時間，而分配結(jié)果誤差與SQP相當(dāng).

4.2 X舵AUV控制分配方法的動態(tài)對比實驗

表3 運動模型參數(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時超出舵位置偏轉(zhuǎn)約束，所以采用Pi控制分配的結(jié)果比IQP更晚達(dá)到穩(wěn)定狀態(tài)，且采用IQP的AUV姿態(tài)控制的效果更好.

X舵AUV采用兩種控制分配算法獲得的各個尾舵執(zhí)行的偏轉(zhuǎn)角度曲線如圖8～11所示.

X舵AUV采用兩種控制分配算法獲得的實際橫傾力矩、縱傾力矩、轉(zhuǎn)艏力矩與對應(yīng)3個自由度的期望力矩的分配誤差曲線如圖12～14所示.

從圖4～7可以看出，X舵AUV的艏向、軸向速度、深度在經(jīng)過一定時間的控制調(diào)整后都穩(wěn)定在了設(shè)定值.對于設(shè)定的X舵舵角的位置偏轉(zhuǎn)約束[-30°,30°],當(dāng)t=3 s時，顯示Pi控制分配在舵1的分配上超出了約束限制(見圖8)，此時Pi控制分配的結(jié)果與AUV實際的執(zhí)行器輸出會產(chǎn)生較大的誤差(見圖12～14).相對的，IQP分配角度曲線沒有超出約束，對于Pi控制分配超出約束部分，IQP將其分配到了其余的3個舵上，且K與N的分配誤差較小，可以忽略不計，而M的分配誤差相對較大，但仍小于分配舵角值的1%，故是優(yōu)化算法計算時做出的取舍結(jié)果.從計算時間、分配誤差以及分配結(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

5 結(jié)語

針對X舵AUV的姿態(tài)控制分配問題，采用序列二次規(guī)劃方法，實現(xiàn)了執(zhí)行器輸出范圍約束條件下的準(zhǔn)確分配.為了改善序列二次規(guī)劃求解的計算效率，采用Lagrange乘子法替代光滑牛頓法.仿真試驗結(jié)果表明，改進(jìn)的序列二次規(guī)劃方法有效地提高了控制分配的計算效率，保證了控制分配的精度，AUV姿態(tài)控制效果滿足作業(yè)任務(wù)需求.另外，所提方法基于約束函數(shù)的梯度計算可行下降方向，因此該算法同樣適用于非線性約束的控制分配模型.