王 巖，朱齊丹，劉志林，楊 震

哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，哈爾濱 150001

欠驅(qū)動(dòng)船舶的運(yùn)動(dòng)規(guī)劃和全局指數(shù)跟蹤控制

王 巖，朱齊丹，劉志林，楊 震

哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，哈爾濱 150001

1 引言

近年來，欠驅(qū)動(dòng)系統(tǒng)的運(yùn)動(dòng)控制問題一直是倍受關(guān)注的研究熱點(diǎn)。欠驅(qū)動(dòng)水面船舶（Underactuated Surface Vessels，USV）由于橫向上未裝備驅(qū)動(dòng)裝置，缺少可用的控制輸入，因此針對(duì)一般運(yùn)動(dòng)系統(tǒng)的基于向量的控制器設(shè)計(jì)方法不能有效地用以解決USV的運(yùn)動(dòng)控制問題[1-2]。欠驅(qū)動(dòng)船舶執(zhí)行跟蹤敵對(duì)目標(biāo)、巡邏勘察、近遠(yuǎn)程避障等作戰(zhàn)任務(wù)時(shí)，均需要以一定的速度沿著指定的目標(biāo)航跡航行。因此航跡跟蹤控制系統(tǒng)是USV使命中重要的組成部分。在航跡跟蹤控制器設(shè)計(jì)過程中，期望連續(xù)可導(dǎo)的目標(biāo)信號(hào)，然而在實(shí)際中這種連續(xù)可導(dǎo)的目標(biāo)航跡是難以指定的，甚至是不可實(shí)現(xiàn)的，比如在避障控制中目標(biāo)航跡是復(fù)雜多變的，如此連續(xù)多變的目標(biāo)航跡幾乎是難以設(shè)計(jì)的。即使在執(zhí)行簡(jiǎn)單的航跡跟蹤任務(wù)中，連續(xù)的目標(biāo)航跡可以給出，此時(shí)設(shè)計(jì)出來的航跡跟蹤控制器也無法具有普遍意義，難以推廣到操縱性、實(shí)時(shí)性和路況復(fù)雜多變的環(huán)境中。因此設(shè)計(jì)離散目標(biāo)點(diǎn)指導(dǎo)下的航跡跟蹤控制方法就成為實(shí)現(xiàn)USV航跡跟蹤控制的迫切需要，并可廣泛應(yīng)用到多種操縱控制任務(wù)中。而運(yùn)動(dòng)規(guī)劃算法可以將離散的目標(biāo)位置點(diǎn)轉(zhuǎn)為期望的連續(xù)航跡信號(hào)和符合安全運(yùn)動(dòng)約束的期望姿態(tài)信息，可兼顧實(shí)用性和控制器設(shè)計(jì)方便的雙重需要。

目前對(duì)于USV的航跡跟蹤的研究，主要存在兩條不同的研究路線：一種是基于虛擬指導(dǎo)船舶的航跡跟蹤控制方法[3-8]，另外一種是基于運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)回路的跟蹤控制方法。所謂的虛擬船舶是指控制器設(shè)計(jì)中存在一個(gè)虛擬運(yùn)動(dòng)的船舶，該船舶的附加質(zhì)量和水動(dòng)力系數(shù)與實(shí)際受控船舶一致，在期望的縱向推力和轉(zhuǎn)向力矩驅(qū)動(dòng)下由虛擬指導(dǎo)船舶實(shí)時(shí)產(chǎn)生實(shí)際受控船舶的操縱運(yùn)動(dòng)可物理實(shí)現(xiàn)的期望航跡和期望運(yùn)動(dòng)姿態(tài)。虛擬船舶方法存在的主要問題是：由于難以預(yù)知復(fù)雜的期望航跡的參考控制輸入，導(dǎo)致所設(shè)計(jì)的航跡跟蹤控制器難以跟蹤任意指定航跡，即使是操縱運(yùn)動(dòng)可實(shí)現(xiàn)的。因此，文獻(xiàn)[3-8]在仿真或?qū)嶒?yàn)都是跟蹤一條指定的圓或圓弧航跡。但優(yōu)勢(shì)為不僅可以跟蹤指定的位置變量，還可以同時(shí)實(shí)現(xiàn)對(duì)艏搖角、艏搖角速度、縱向速度、側(cè)向速度的姿態(tài)跟蹤?；谶\(yùn)動(dòng)學(xué)和動(dòng)力學(xué)回路的跟蹤控制方法僅僅能夠?qū)崿F(xiàn)USV的期望位置跟蹤，不能實(shí)現(xiàn)其他姿態(tài)的跟蹤[9-10]。而且也未能解決關(guān)于在未知復(fù)雜的環(huán)境下如何得到符合安全運(yùn)動(dòng)約束和動(dòng)力學(xué)條件約束的連續(xù)期望航跡問題。

從工程角度出發(fā)，為實(shí)現(xiàn)欠驅(qū)動(dòng)船舶的航跡跟蹤控制器應(yīng)用于實(shí)際問題，本文主要由以下幾部分組成：首先，為解決欠驅(qū)動(dòng)船舶不能跟蹤任意指定航跡問題，運(yùn)用運(yùn)動(dòng)規(guī)劃方法，利用離散目標(biāo)點(diǎn)規(guī)劃出符合動(dòng)力學(xué)約束的連續(xù)期望軌跡；其次，引入微分同胚變換，建立兩個(gè)級(jí)聯(lián)子系統(tǒng)構(gòu)成的航跡跟蹤誤差動(dòng)態(tài)方程，并運(yùn)用反步法對(duì)變換后的誤差子系統(tǒng)設(shè)計(jì)了全局指數(shù)狀態(tài)反饋控制律。最后利用本文提出的基于規(guī)劃期望軌跡的全局指數(shù)航跡跟蹤控器進(jìn)行了仿真實(shí)驗(yàn)，結(jié)果表明所提出的方法能有效地實(shí)現(xiàn)USV航跡的精確跟蹤。

2 欠驅(qū)動(dòng)船舶的運(yùn)動(dòng)模型

考慮如下欠驅(qū)動(dòng)水面船舶模型[11]：

3 規(guī)劃期望航跡

運(yùn)動(dòng)規(guī)劃算法以離散的目標(biāo)位置點(diǎn)為基本信息，通過樣條函數(shù)對(duì)目標(biāo)位置點(diǎn)進(jìn)行分段擬合，不僅輸出分段連續(xù)的期望航跡，同時(shí)輸出具有安全運(yùn)動(dòng)約束的目標(biāo)姿態(tài)，形成航跡跟蹤控制器有效的期望輸入，從而實(shí)現(xiàn)姿態(tài)約束及安全運(yùn)動(dòng)約束條件下的USV航跡跟蹤控制，既能充分發(fā)揮USV優(yōu)良的操縱性能，又能實(shí)現(xiàn)良好的跟蹤控制性能。

基于運(yùn)動(dòng)規(guī)劃的USV航跡跟蹤控制分為4個(gè)步驟：

（2）根據(jù)連續(xù)化航跡信息，估計(jì)USV的姿態(tài)信息(-u,-v,-ψ,-r)。

（3）利用動(dòng)力學(xué)特性獲得期望的控制輸入(τ1d，τ3d)，由此確定被跟蹤的期望姿態(tài)(xd，yd，ψd，ud，vd，rd)。

（4）設(shè)計(jì)航跡跟蹤控制器，跟蹤期望姿態(tài)(xd，yd，ψd，ud，vd，rd)。

3.1 參考航跡的多項(xiàng)式擬合

假設(shè)離散目標(biāo)點(diǎn) (xk-1，yk-1)、(xk，yk)和 (xk，yk)、(xk+1，yk+1)分別存在曲線Sk-1(θ)和Sk(θ)，其中θ為參考期望軌跡參數(shù)。

定義向量θVEC為：

因此，根據(jù)式（1），大地坐標(biāo)系下的船速Ud() t為：

若船舶航速的動(dòng)態(tài)特性可以表征為：

式（5）和（6）聯(lián)立，可知θ(t)和Ud(t)的動(dòng)態(tài)變化過程為：

其中，Uref為參考速度，Ts為速度動(dòng)態(tài)時(shí)間常數(shù)。

下面需要根據(jù)連續(xù)性條件確定目標(biāo)航跡的軌跡參數(shù)[a3a2a1a0]T和[b3b2b1b0]T。

易知，若存在N+1個(gè)點(diǎn)，則存在N個(gè)曲線S，也即存在4N個(gè)參數(shù)對(duì)(a，b)。因此，需要建立4N個(gè)約束方程，以確定擬合參數(shù)。

這里，以某連續(xù)的三個(gè)離散點(diǎn)為例，存在連續(xù)曲線Sk-1(θ)和Sk(θ)，應(yīng)滿足連續(xù)性條件為：

若Sk-1(θ)和Sk(θ)對(duì)軌跡參數(shù)變量θ的一階和二階導(dǎo)數(shù)滿足連續(xù)條件：

因?yàn)檫B續(xù)性及可導(dǎo)性條件是對(duì)于單點(diǎn)闡述的，因此除首尾兩個(gè)點(diǎn)外，可以建立4(N-1)個(gè)方程。同時(shí)，首尾點(diǎn)位于首尾曲線S1(θ)和SN(θ)上并滿足可導(dǎo)性條件，這樣一共可建立4N個(gè)約束方程，由此可確定式（3）中的4N個(gè)軌跡參數(shù)。

3.2 參考姿態(tài)的估計(jì)

考慮USV已跟蹤上目標(biāo)軌跡，根據(jù)式（1）可知：

圖1 航跡向、真航向和漂角三者間關(guān)系

式（15）的微分結(jié)果為：

同時(shí)，根據(jù)式（15）可知：

根據(jù)式（14），結(jié)合式（10）、（13）和（15）可以計(jì)算得出當(dāng)軌跡不為直線時(shí)，角速度的計(jì)算公式為：

至此，可以總結(jié)船舶參考狀態(tài)的估算過程：首先，根據(jù)已知參考航跡和，通過式（19）和（20）能夠計(jì)算得出USV縱向速度，之后根據(jù)式（15）計(jì)算得到側(cè)移速度，最后根據(jù)式（14）和（18）計(jì)算得到航向角和航向角速度。

3.3 期望控制輸入的規(guī)劃

前面兩節(jié)已經(jīng)給出USV運(yùn)動(dòng)可行的連續(xù)參考軌跡、航向角和船體坐標(biāo)系下的所有速度和加速度，下面將利用USV的動(dòng)力學(xué)模型（1）直接計(jì)算得出相應(yīng)開環(huán)期望控制輸入：

將式（21）和（22）中的期望控制輸入(τud，τrd)代入整個(gè)運(yùn)動(dòng)方程（1），可求得USV的操作性可實(shí)現(xiàn)的期望航跡(xd，yd，ψd，ud，vd，rd)。

運(yùn)動(dòng)規(guī)劃算法和開環(huán)控制律（21）、（22）可以看作是設(shè)計(jì)閉環(huán)軌跡跟蹤控制律的一部分。當(dāng)USV在跟蹤軌跡上，且模型精確，同時(shí)不存在外界干擾時(shí)，開環(huán)控制律（21）、（22）能有效保證USV的精確跟蹤。但是，由于模型的不確定性等原因，需要設(shè)計(jì)帶有反饋的閉環(huán)控制率以保證USV的精確跟蹤。

4 指數(shù)跟蹤控制器設(shè)計(jì)

由上章可得根據(jù)期望點(diǎn)所設(shè)計(jì)的物理可實(shí)現(xiàn)期望軌跡(xd，yd，ψd，ud，vd，rd)和期望輸入(τ1d,τ3d)，滿足：

4.1 控制目標(biāo)的分解簡(jiǎn)化

為利于控制器設(shè)計(jì)，引入狀態(tài)變換和輸入變換[5]：

則整理后新的動(dòng)態(tài)方程為：

同理，經(jīng)坐標(biāo)變換：

則有：

定義跟蹤誤差：

則有：

引理1[6]如果存在一種控制律使得系統(tǒng)（31）全局指數(shù)穩(wěn)定，則該控制律同樣能使得系統(tǒng)（30）全局指數(shù)穩(wěn)定。

在接下來的控制器設(shè)計(jì)中，只需要考慮設(shè)計(jì)反饋控制律使得系統(tǒng)（31）全局指數(shù)穩(wěn)定即可。

將系統(tǒng)（31）表示成如下兩個(gè)子系統(tǒng)：

由此控制目標(biāo)可通過如下兩個(gè)步驟實(shí)現(xiàn)：

步驟1將z6e=σ看成虛擬控制輸入，設(shè)計(jì)部分狀態(tài)反饋控制律w2使得系統(tǒng)（32）全局指數(shù)穩(wěn)定。

步驟2設(shè)計(jì)部分狀態(tài)反饋控制律w1使得系統(tǒng)（33）全局指數(shù)穩(wěn)定。

4.2 步驟1的實(shí)現(xiàn)

定理1針對(duì)線性系統(tǒng)（32），易構(gòu)造如下控制律：使得其閉環(huán)系統(tǒng)全局指數(shù)穩(wěn)定（GES）。

4.3 步驟2的實(shí)現(xiàn)

定理2對(duì)于系統(tǒng)（33），若rd滿足標(biāo)準(zhǔn)激勵(lì)條件：

其中，δ＞0，則在控制律（36）作用下的閉環(huán)系統(tǒng)GES。

針對(duì)系統(tǒng)（33），定義變量σ=z4e+k3z6dz2e，z6d≠0，則有：

定義如下Lyapunov函數(shù)：

求導(dǎo)并代入控制律（36），則

由比較定理[12]：

由于z4d、z6d(rd)有界，且當(dāng)t→∞時(shí)，z6e→0，顯然W收斂到0，因此V收斂到0，進(jìn)而，當(dāng)t→∞時(shí)，z2e，σ→0，因此當(dāng)t→∞時(shí)，z4e→0。

4.4 控制目標(biāo)的實(shí)現(xiàn)

定理3在控制律：的作用下誤差系統(tǒng)（30）和（31）的閉環(huán)系統(tǒng)GES。

證明 由引理1、定理1、定理2可知。

由定理3，可知ze全局指數(shù)收斂于零，則有(x-xd，y-yd，ψ-ψd，u-ud，v-vd，r-rd)全局指數(shù)收斂于零，從而實(shí)現(xiàn)了欠驅(qū)動(dòng)船舶對(duì)任意可行航跡的全局指數(shù)跟蹤控制。

5 仿真結(jié)果

將通過仿真實(shí)驗(yàn)驗(yàn)證所提控制律的實(shí)際有效性，仿真環(huán)境基于MATLAB/SIMULINK工具搭建。期望狀態(tài)(xd，yd，ψd，ud，vd，rd)和船舶狀態(tài)(x，y，ψ，u，v，r)經(jīng)過微分同胚變換后構(gòu)成了整個(gè)控制器的輸入，經(jīng)控制律（42）解算后得到船舶的實(shí)際控制輸入，從而完成輸出反饋控制律的閉環(huán)仿真驗(yàn)證。仿真中所選船模的參數(shù)為[5]：m11=200kg，m22=250kg，m33=80kg，d11=70kg/s，d22=100kg/s，d33= 50kg/s。船舶的初始狀態(tài)設(shè)為(x(0)，y(0)，ψ(0)，u(0)，v(0)，r(0))= (0，-2，0，0，0，0)。控制律（42）中的控制參數(shù)設(shè)為k1=1，k2=1.2，k3=6，k3=1.6，滿足定理3中穩(wěn)定性分析中確定的參數(shù)限定條件。離散的期望點(diǎn)為：yk=[-0.5,0,2,2,3,4,5.5]，xk=[0，0.6，1.5，3，4，4，32]。仿真結(jié)果如圖2～6所示。

圖2 規(guī)劃期望軌跡

圖3 船舶的實(shí)際與期望位置和艏搖響應(yīng)

圖4 船舶的實(shí)際與期望的速度和角速度響應(yīng)

圖5 實(shí)際與期望的控制輸入

圖6 實(shí)際航跡與期望航跡

6 結(jié)束語

通過對(duì)USV航跡跟蹤的研究，利用多項(xiàng)式擬合獲得連續(xù)參考軌跡，進(jìn)而估計(jì)各個(gè)參考姿態(tài)；結(jié)合參考姿態(tài)和船舶動(dòng)力學(xué)模型，得到期望的控制輸入，進(jìn)而規(guī)劃出操作性可實(shí)現(xiàn)的全部期望姿態(tài)。同時(shí)，為實(shí)現(xiàn)欠驅(qū)動(dòng)船舶的航跡快速跟蹤控制，引入微分同胚變換，基于反步法設(shè)計(jì)原理，針對(duì)變換后的誤差動(dòng)態(tài)方程設(shè)計(jì)了全局指數(shù)狀態(tài)反饋航跡跟蹤控制律，實(shí)現(xiàn)了欠驅(qū)動(dòng)USV在任意可行航跡條件下的全局指數(shù)航跡跟蹤控制。

[1]高炳團(tuán)，陳宏鈞，張曉華.欠驅(qū)動(dòng)機(jī)械系統(tǒng)控制設(shè)計(jì)綜述[J].電機(jī)與控制學(xué)報(bào)，2006，10（5）：541-546.

[2]郭晨，汪洋，孫福春，等.欠驅(qū)動(dòng)水面船舶運(yùn)動(dòng)控制研究綜述[J].控制與決策，2009，24（3）：321-329.

[3]Lefeber E，Pettersen K Y，Nijmeijer H.Tracking control of an underactuated ship[J].IEEE Transactions on Control Systems Technology，2003，11（1）：52-61.

[4]Jiang Z P.Global tracking control of underactuated ships by Lyapunov's direct method[J].Automatica，2002，38（2）：301-309.

[5]Dong Wenjie，Guo Yi.Nonlinear tracking control of underactuated surface vessel[C]//American Control Conference.Portland，USA：[s.n.]，2005：4351-4356.

[6]Ghommam J，Mnif F，Derbel N.Global stabilization and tracking control of underactuated surface vessels[J].IET Control Theory and Applications，2010，4：71-88.

[7]孟威，郭晨，孫富春，等.欠驅(qū)動(dòng)水面船舶的非線性滑模軌跡跟蹤控制[J].哈爾濱工程大學(xué)學(xué)報(bào)，2012，33（5）：1-5.

[8]朱齊丹，于瑞亭，劉志林.欠驅(qū)動(dòng)船舶全局K指數(shù)航跡跟蹤的級(jí)聯(lián)反步法[J].船舶工程，2012，34（1）：47-51.

[9]Ashrafiuon H，Muske K，McNinch L，et al.Sliding mode tracking control of surface vessels[J].IEEE Transactions on Industrial Electronics，2008，55（11）：4004-4012.

[10]廖煜雷，莊佳園，李曄，等.欠驅(qū)動(dòng)無人艇軌跡跟蹤的滑?？刂品椒╗J].應(yīng)用科技學(xué)報(bào)，2011，29（4）：428-434.

[11]Fossen T I.Guidance and control of ocean vehicles[M]. New York：Wiley，1994.

[12]Khalil H K.Nonlinear systems[M].3rd ed.New Jersey，USA：Prentice-Hall，2002.

WANG Yan,ZHU Qidan,LIU Zhilin,YANG Zhen

College of Automation,Harbin Engineering University,Harbin 150001,China

In order to realize tracking arbitrary specified trajectory,a motion planning method is presented.All the desired attitudes are obtained by combining cubic spline interpolation,ship dynamic model,and discrete expected dots.Meanwhile,in order to realize fast tracking trajectory of underactuated surface vessels,global exponential tracking control law is proposed.Trajectory tracking error dynamic equation consists of two cascade subsystems based on diffeomorphism transformation.Then global exponential tracking control law is designed by using backstepping for error subsystems.The results of simulation experiments indicate that the proposed controller can effectively track arbitrary specified curve trajectory.

underactuated surface vessel;motion planning;tracking control;backstepping;exponential stabilization

針對(duì)目前欠驅(qū)動(dòng)船舶航跡跟蹤控制難以實(shí)現(xiàn)跟蹤任意可行航跡問題，提出一種運(yùn)動(dòng)規(guī)劃方法。利用多項(xiàng)式擬合，并結(jié)合船舶動(dòng)力學(xué)模型，通過離散期望點(diǎn)規(guī)劃出操作性可實(shí)現(xiàn)的全部期望姿態(tài)。同時(shí)，為實(shí)現(xiàn)欠驅(qū)動(dòng)船舶的航跡快速跟蹤控制，提出一種全局指數(shù)航跡跟蹤控制律。引入微分同胚變換，建立兩個(gè)級(jí)聯(lián)的子系統(tǒng)構(gòu)成的航跡跟蹤誤差動(dòng)態(tài)方程；基于反步法的設(shè)計(jì)原理，運(yùn)用Lyapunov直接方法對(duì)變換后的誤差系統(tǒng)設(shè)計(jì)了全局指數(shù)航跡跟蹤控制律。仿真結(jié)果驗(yàn)證了所提出的全局指數(shù)航跡跟蹤控制律能夠有效實(shí)現(xiàn)跟蹤任意可行航跡。

欠驅(qū)動(dòng)船舶；運(yùn)動(dòng)規(guī)劃；跟蹤控制；反步；指數(shù)穩(wěn)定

A

U664

10.3778/j.issn.1002-8331.1209-0043

WANG Yan,ZHU Qidan,LIU Zhilin,et al.Motion planning and global exponential trajectory tracing of underactuated surface vessels.Computer Engineering and Applications,2013,49（5）：18-22.

國(guó)家自然科學(xué)基金（No.50909026）；黑龍江省自然科學(xué)基金（No.F200916）；中央高?；緲I(yè)務(wù)專項(xiàng)資金（No.HEUCFR1116）。

王巖（1984—），男，博士研究生，主要研究方向?yàn)橹悄芸刂评碚撆c應(yīng)用、船舶控制；朱齊丹（1965—），男，工學(xué)博士，教授，博士生導(dǎo)師，主要研究方向?yàn)闄C(jī)器人控制、智能控制等；劉志林（1977—），男，工學(xué)博士，講師，主要研究方向?yàn)榛祀s系統(tǒng)、預(yù)測(cè)控制、船舶控制；楊震（1984—），女，博士研究生，主要研究方向?yàn)榇斑\(yùn)動(dòng)姿態(tài)預(yù)報(bào)、智能優(yōu)化理論與方法。E-mail：wangyan0406@hrbeu.edu.cn

2012-09-10

2012-10-26

1002-8331（2013）05-0018-05

CNKI出版日期：2012-11-06 http://www.cnki.net/kcms/detail/11.2127.TP.20121106.1628.005.html