AUV 的精確航跡跟蹤系統(tǒng)的魯棒控制

李 娟，邊信黔，熊華勝，王宏健

(1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，150001 哈爾濱;2.清華大學(xué) 核能與新能源技術(shù)研究院，100084 北京)

AUV 的精確航跡跟蹤能力對于AUV 來說是一項(xiàng)很關(guān)鍵的行為能力，它是完成AUV 作業(yè)任務(wù)(如海底地形調(diào)查，遠(yuǎn)程航行等)的一種重要技術(shù)手段.受海流、海浪等不確定性環(huán)境干擾因素的影響，其水下運(yùn)動(dòng)是一個(gè)復(fù)雜的空間六自由度運(yùn)動(dòng).此外，AUV 的精確模型難以得到，即使能夠獲得較為精確的AUV 模型，該模型也會(huì)過于復(fù)雜，在進(jìn)行控制系統(tǒng)設(shè)計(jì)時(shí)必須進(jìn)行必要的簡化，導(dǎo)致模型出現(xiàn)誤差.對于AUV 這種狀態(tài)交叉耦合、非線性、時(shí)變性都非常嚴(yán)重的復(fù)雜運(yùn)動(dòng)，精確航跡跟蹤問題就更為復(fù)雜［1］.

對于航跡跟蹤控制問題，國內(nèi)外大量學(xué)者已經(jīng)做了深入的研究，并在無人機(jī)、無人艇、水面船等領(lǐng)域提出了一些成熟的控制技術(shù).文獻(xiàn)［2］設(shè)計(jì)了無人機(jī)目標(biāo)跟蹤系統(tǒng)的魯棒控制器，較好的實(shí)現(xiàn)了高低環(huán)與方位環(huán)的解耦，較準(zhǔn)確的實(shí)現(xiàn)了目標(biāo)跟蹤;文獻(xiàn)［3］基于滑模變結(jié)構(gòu)控制設(shè)計(jì)了飛艇的三維航跡控制率;在船舶方面，航跡跟蹤控制主要集中在水平面控制，文獻(xiàn)［4］假定動(dòng)力學(xué)模型的慣性矩陣為對角陣，基于視線方法和極坐標(biāo)變換，提出了具有全局魯棒性的狀態(tài)反饋控制器，克服了舷搖角速度不能為零的局限，得到了能夠跟蹤直線參考路徑的全局漸近穩(wěn)定控制器.文獻(xiàn)［5］則在MOOS－IvP 構(gòu)架下，設(shè)計(jì)命令濾波反步法，實(shí)現(xiàn)了表面船對復(fù)雜路徑的精確跟蹤.對于水下AUV，在文獻(xiàn)［6－11］都是利用Lypaunov 理論和Backstepping 方法，設(shè)計(jì)了欠驅(qū)動(dòng)AUV 的路徑跟蹤控制，并取得了比較好的跟蹤效果，但是仍然是集中在水平面的研究，僅有文獻(xiàn)［9］考慮了定常海流的影響，保證了路徑跟蹤誤差在常值海流影響下的全局漸近穩(wěn)定性.

本文基于魯棒H∞控制理論，將三維航跡跟蹤控制分解為水平面和垂直面跟蹤控制問題，考慮了線性近似產(chǎn)生的誤差、不確定性和外界干擾的影響，建立了AUV 水平面和垂直面的動(dòng)力學(xué)模型，設(shè)計(jì)了航向控制器和縱傾控制器，有效地實(shí)現(xiàn)了AUV 三維航跡跟蹤控制，具有很好的穩(wěn)態(tài)控制精度.

1 AUV 航跡跟蹤控制系統(tǒng)設(shè)計(jì)

AUV 的精確航跡跟蹤是指AUV 在航跡跟蹤制導(dǎo)器和控制器的共同作用下，能夠沿著指定的航線航行，并且AUV 實(shí)際航線與指令航線的偏差應(yīng)該在一定的范圍，即精確航跡跟蹤控制器應(yīng)當(dāng)滿足相應(yīng)的控制指標(biāo).三維航跡跟蹤可以認(rèn)為是水平面內(nèi)的航跡跟蹤與垂直面內(nèi)的航跡跟蹤的綜合.

對于AUV 的水平面精確航跡跟蹤問題，可以分解為兩個(gè)相對簡單的子問題:航向制導(dǎo)器和航向控制器.航向制導(dǎo)器是根據(jù)AUV 當(dāng)前的位置與設(shè)定的航跡之間水平面內(nèi)的偏差來調(diào)整AUV 的航向，以期縮小并消除這種水平面內(nèi)的航跡偏差;航向控制器是控制AUV 的航向，以期讓AUV 在給定的航向指令下航行.

與AUV 的水平面精確航跡跟蹤問題類同，AUV 的垂直面精確航跡跟蹤問題也可以分解為兩個(gè)相對簡單的子問題:縱傾制導(dǎo)器和縱傾控制器.縱傾制導(dǎo)器是根據(jù)AUV 當(dāng)前的位置與設(shè)定的航跡之間垂直面內(nèi)的偏差來調(diào)整AUV 的縱傾，以期縮小并消除這種垂直面內(nèi)的航跡偏差;縱傾控制器是控制AUV 的縱傾，以期讓AUV 在給定的縱傾指令下航行.

為了簡化設(shè)計(jì)，可以把三維航線跟蹤分別投影到固定坐標(biāo)系(地球坐標(biāo)系)的水平面(Eξη)和垂直面(Eξζ)中.然后，在兩個(gè)平面內(nèi)分別設(shè)計(jì)制導(dǎo)器，他們與航向控制器、縱傾控制器以及航速控制器組成一個(gè)聯(lián)合的精確航跡跟蹤控制系統(tǒng).其原理見圖1.

圖1 AUV 精確航跡跟蹤原理圖

2 AUV 水平面內(nèi)的精確航跡跟蹤控制

假設(shè)AUV 的三維空間指令航線在水平面Eξη 的投影為AB，即AB 為AUV 水平面內(nèi)所需要跟蹤的航跡(如圖2 所示)，令航跡對應(yīng)的方向?yàn)棣譨b.AUV 在水平面內(nèi)的實(shí)際位置為(ξauv，ηauv)，該位置時(shí)相對設(shè)定航跡線的水平面內(nèi)的航跡偏差為l(規(guī)定AUV 在設(shè)定航跡線的左邊時(shí)l 為正值，在右邊為負(fù)值).則在航跡跟蹤制導(dǎo)器的制導(dǎo)算法下得到的AUV 航向指令ψcom為

ψ' 是為了補(bǔ)償航跡偏差而得到的航向修正值.它的具體表達(dá)式為

式中:ψmax，lmax2，lmax1，Kp1，Kp，Kd，Ki均為常數(shù)，它是一組可調(diào)參數(shù).ψ' 的修整考慮到了AUV 航跡跟蹤的快速性和航跡跟蹤的性能問題.

圖2 水平面航向制導(dǎo)

2.1 AUV 航向控制器設(shè)計(jì)模型

在得出AUV 方向舵模式下的航向控制器設(shè)計(jì)模型時(shí)，假設(shè)AUV 的航行速度穩(wěn)定于u0，重心在運(yùn)動(dòng)坐標(biāo)系的原點(diǎn)上，并忽略垂直面運(yùn)動(dòng)和橫搖運(yùn)動(dòng)參數(shù)的影響(即認(rèn)為Z 方向的速度和X、Y向的角速度w，p，q 為小量)，并聯(lián)立AUV 的姿態(tài)方程［9，12］，可以得到

式中:m 和L 為AUV 的長度和質(zhì)量，dψ，dv，dr包括了設(shè)計(jì)模型中線性近似產(chǎn)生的誤差、不確定性和外界干擾，對于系統(tǒng)的有界輸入，它們是有界的.

令A(yù)UV 的艏向指令為常數(shù)ψr，滿足˙ψr=0，引入變量ψe=ψr-ψ 可得

因此可以得到AUV 方向舵模式下的航向控制器設(shè)計(jì)模型

考慮到AUV 運(yùn)動(dòng)過程中可能受到的外界定常干擾(如海流)會(huì)造成AUV 的艏向控制有靜態(tài)誤差，可以引入一個(gè)積分單元來消除這種定常干擾，即

2.2 AUV 航向控制器設(shè)計(jì)

航跡跟蹤時(shí)，航向控制器為魯棒H∞控制器.根據(jù)魯棒H∞控制理論［14］，對AUV 航向動(dòng)態(tài)系統(tǒng)(3)，可以得到幾個(gè)AUV 運(yùn)行工況的魯棒H∞狀態(tài)反饋控制器.

AUV 魯棒H∞狀態(tài)反饋控制器形式為

式中:K 為狀態(tài)反饋控制器的增益矩陣，x=［ψIψev r］T.

AUV 航速為2 kn 時(shí):

AUV 航速為6kn 時(shí):

AUV 航速為10 kn 時(shí):

基于Riccati 方程解插值的魯棒H∞控制原理，通過對設(shè)計(jì)好的第i 和第i+1 工況點(diǎn)的H∞控制器Ki和Ki+1進(jìn)行插值，可計(jì)算出任意第j 個(gè)工況點(diǎn)的控制器Kj［13］.當(dāng)AUV 航速為4 kn 時(shí)，插值結(jié)果:

對應(yīng)狀態(tài)反饋控制器作用下的閉環(huán)系統(tǒng)特征根為

3 AUV 垂直面內(nèi)的精確航跡跟蹤控制

假設(shè)AUV 的三維空間指令航線在垂直面Eξζ 的投影為CD，即CD 為AUV 垂直面內(nèi)所需要跟蹤的航跡(如圖3 所示)，令航跡CD 與水平面夾角對應(yīng)的傾角為θcd(規(guī)定斜向上為正，向下為負(fù)).AUV 在垂直面內(nèi)的實(shí)際位置為(ξauv，ζauv)，該位置時(shí)相對設(shè)定航跡線的垂直面內(nèi)航跡偏差為h(規(guī)定AUV 在設(shè)定航跡線的上面時(shí)h 為正值，在下面為負(fù)值).則在航跡跟蹤制導(dǎo)器的制導(dǎo)算法下得到的AUV 縱傾指令θcom為

θ' 是為了補(bǔ)償航跡偏差而得到的縱傾修正值.它的具體表達(dá)式為

式中，θmax，hmax2，hmax1，Kp1，Kp，Kd，Ki均 為常數(shù)，它是一組可調(diào)參數(shù)，θ' 的修正既考慮到了AUV 航跡跟蹤的快速性又考慮了航跡跟蹤的性能(如消除定常干擾)問題.

圖3 垂直面縱傾制導(dǎo)

3.1 AUV 縱傾控制器設(shè)計(jì)模型

假設(shè)AUV 的標(biāo)稱航行速度為u0，重心在運(yùn)動(dòng)坐標(biāo)系的原點(diǎn)上，并忽略水平面運(yùn)動(dòng)和橫搖運(yùn)動(dòng)參數(shù)的影響(即認(rèn)為v，p，r 為小量)，忽略運(yùn)動(dòng)參數(shù)的高階項(xiàng)，聯(lián)立AUV 的姿態(tài)方程［13，15］，可以得到

其中，dθ，dq，dw包括了設(shè)計(jì)模型中線性近似產(chǎn)生的誤差、不確定性因素和外界干擾，對于系統(tǒng)的有界輸入，它們是有界的.

對上述方程進(jìn)行分析可以知道，AUV 縱傾控制時(shí)，水平舵的控制作用可以認(rèn)為是由兩部分組成:一部分是用于克服AUV 縱傾穩(wěn)態(tài)下扶正力矩影響的δs0，一部分是用于調(diào)節(jié)AUV 縱傾動(dòng)態(tài)過程的控制作用δs1.即

由分析可知

令A(yù)UV 的縱傾角指令為常數(shù)θr，滿足˙θr=0，θ 為AUV 實(shí)際縱傾角，引入變量θe=θr-θ 可得

同樣，可以得到引入積分單元之后的縱傾動(dòng)態(tài)調(diào)節(jié)子系統(tǒng):

3.2 AUV 魯棒H∞縱傾控制器設(shè)計(jì)

航跡跟蹤時(shí)，縱傾控制器為魯棒H∞控制器.經(jīng)設(shè)計(jì)得到AUV 縱傾的魯棒H∞狀態(tài)反饋控制器形式為:

式中:K 為狀態(tài)反饋控制器的增益矩陣，

4 AUV 精確航跡跟蹤控制仿真

AUV 的精確航跡跟蹤試驗(yàn)主要是檢驗(yàn)航跡跟蹤算法、AUV 制導(dǎo)器、AUV 控制器綜合效果以及它的性能.由于AUV 航跡跟蹤算法并不是與AUV 航速有直接的關(guān)系，航速對它的影響是由中間控制器(如航向控制器)體現(xiàn).因此，這里僅選擇AUV 在4kn 航速下進(jìn)行一系列航跡跟蹤試驗(yàn)，并以此來驗(yàn)證AUV 精確航跡跟蹤控制的有效性，海流干擾模型可以參見文獻(xiàn)［15］.

4.1 同一平面的口字形航跡線跟蹤仿真試驗(yàn)

在水平面內(nèi)完成了不同海流下口字形航跡跟蹤仿真試驗(yàn)，跟蹤曲線如圖4 所示，誤差曲線如圖5 所示，從仿真結(jié)果可以看出，在所設(shè)計(jì)的航向控制器的作用下，AUV 能夠克服海流的影響快速跟蹤到設(shè)定航跡.

圖4 AUV 精確航跡跟蹤軌跡圖

4.2 三維航跡跟蹤仿真試驗(yàn)

從三維航跡跟蹤仿真試驗(yàn)可以看出，在所設(shè)計(jì)的航向控制器和縱傾控制器的共同作用下，AUV 也能夠克服外界干擾海流的影響運(yùn)動(dòng)到設(shè)定航跡上，跟蹤誤差收斂于0，且在海流的影響下控制效果良好，充分說明該控制器能夠有效克服外界干擾的影響.

圖5 AUV 航跡跟蹤誤差曲線

圖6 AUV 精確航跡跟蹤軌跡圖

圖7 AUV 航跡跟蹤誤差曲線

5 結(jié)論

本文主要研究了欠驅(qū)動(dòng)AUV 動(dòng)力學(xué)運(yùn)動(dòng)模型存在不確定性以及外界干擾下的AUV 精確航跡跟蹤問題，將三維跟蹤問題分為水平面控制和垂直面控制兩部分，分別完成了水平面內(nèi)的精確航跡跟蹤控制和垂直面的精確航跡跟蹤控制.仿真試驗(yàn)表明，本文提出的AUV 的精確航跡跟蹤控制器具有很好的控制性能，能夠較快的精確跟蹤設(shè)定航跡，能夠抑制海流對AUV 航跡跟蹤的定常干擾，有很好的穩(wěn)態(tài)控制精度.

［1］EVEN B，PAVLOV A，PETTERSEN K Y.Integral LOS control for path following of underactuated marine surface vessels in the presence of constant ocean currents［C］//Proceeding of the 47th IEEE Conference on Decision and Control.United States Mexico:Institute of Electrical and Electronics Engineers Inc，2008:49840－4991.

［2］陳瀾，吳梅，安靜文.無人偵察機(jī)目標(biāo)跟蹤系統(tǒng)魯棒控制［J］.測控技術(shù)，2005，24(5):58－61.

［3］王曉亮，劉丹，單雪雄.飛艇三維軌跡控制研究［J］.上海交通大學(xué)學(xué)報(bào):自然科學(xué)版，2006，40(12):2164－2168.

［4］DO K D，JIANG Z P，PAN J.Robust and adaptive path following for underactuated ship［J］.Automatic，2004，40:929－944.

［5］VLADIMIR D，DULA N.Command filtered backstepping design in MOOS-IvP helm［C］//Proceedings of the 2010 American Control Conference，ACC2010.Piscataway United states:IEEE Computer Society.2010:5997－6003.

［6］LAPIERRE L，SOETANTO D.Nonlinear path－following control for an AUV［J］.Ocean Engineering，2007，34:1734－1744.

［7］LIONEL L，BRUNO J.Robust nonlinear path-following control of an AUV［J］.Ocean Engineering，2008，33:1756－1784.

［8］SILVESTRE C，PASCOAL A.Control of the INFANTE AUV using gain scheduled static output feedback［J］.Control Engineering Practice，2004，12:1501－1509.

［9］施淑偉，嚴(yán)衛(wèi)生，高劍等.常值海流作用下的AUV 水平面路徑跟蹤控制［J］.兵工學(xué)報(bào)，2010，31(3):375－380.

［10］PEDRO AGUIAR A，HESPANHA J P.Trajectory-following and path-following of underactuated autonomous vehicles with parametric modeling uncertainty ［J］.IEEE Transactions on Automatic Control，2007，52(8):1435－1441.

［11］BIDYADHAR S.DEBABRATA A.Design of a path following controller for an underactuated AUV［J］.Archives of Control Sciences，2009，19(3):245－259.

［12］馬嶺，崔維成.NTSM 控制的AUV 路徑跟蹤控制研究［J］.中國造船，2006，47(4):76－83.

［13］熊華勝.AUV 魯棒控制技術(shù)研究［D］.哈爾濱:哈爾濱工程大學(xué)，2006:30－44.

［14］吳敏，何勇，佘錦華.魯棒控制理論［M］.高等教育出版社，2010:89－123.

［15］李殿璞.船舶運(yùn)動(dòng)與建模［M］.哈爾濱:哈爾濱工程大學(xué)出版社，1999:123－146.