雙推進(jìn)無人艇軌跡跟蹤控制系統(tǒng)設(shè)計(jì)

cd

(武漢理工大學(xué) a.智能交通系統(tǒng)研究中心；b.航運(yùn)學(xué)院；c.國家水運(yùn)安全工程技術(shù)研究中心；d.內(nèi)河航運(yùn)技術(shù)湖北省重點(diǎn)實(shí)驗(yàn)室,武漢 430063)

由于無人艇所處環(huán)境的復(fù)雜性導(dǎo)致其極易受到外界環(huán)境的干擾，以模型為基礎(chǔ)的無人艇軌跡跟蹤存在著許多不確定因素[1]，因此，實(shí)現(xiàn)無人艇對軌跡的精確跟蹤需要控制器具有更好的魯棒性和快速收斂能力。已有的軌跡跟蹤控制方法[2-6]都是根據(jù)單漿單舵無人艇的橫向和縱向位置誤差直接計(jì)算出無人艇的縱向推力和轉(zhuǎn)船力矩，控制律的設(shè)計(jì)比較復(fù)雜，對無人艇數(shù)學(xué)模型的精度要求較高，在實(shí)用性上稍有不足。與傳統(tǒng)的單漿單舵無人艇相比，雙推進(jìn)無人艇可有效降低USV行駛速度對回轉(zhuǎn)控制的影響，減小回轉(zhuǎn)的半徑與用時(shí)，具有良好的機(jī)動(dòng)性與操縱性，適合在多種特殊的航行環(huán)境中使用。本文以“iNav-III”號無人艇為研究對象，嘗試采用雙閉環(huán)軌跡跟蹤控制方法實(shí)現(xiàn)間接軌跡跟蹤控制。

1 控制系統(tǒng)原理

系統(tǒng)原理見圖1。

圖1 軌跡跟蹤控制系統(tǒng)結(jié)構(gòu)

系統(tǒng)采用雙閉環(huán)控制結(jié)構(gòu)，制導(dǎo)環(huán)與控制環(huán)分離?；诨？刂评碚撛O(shè)計(jì)的雙推進(jìn)無人艇的軌跡跟蹤控器屬于制導(dǎo)環(huán)，基于PID神經(jīng)網(wǎng)絡(luò)設(shè)計(jì)的無人艇航速艏搖角速度協(xié)同控制器屬于控制環(huán)。軌跡跟蹤控制器根據(jù)參考軌跡的位置坐標(biāo)和無人艇的實(shí)際位置坐標(biāo)計(jì)算出航速艏搖角速度協(xié)同控制器所需的期望航速ucmd和期望艏搖角速度ωcmd。航速艏搖角速度控制器將期望航速和艏搖角速度與傳感器反饋的實(shí)際航速和艏搖角速度進(jìn)行比較，輸出兩個(gè)推進(jìn)器的控制信號控制無人艇，從而實(shí)現(xiàn)軌跡跟蹤。

2 軌跡跟蹤控制

2.1 航速艏搖角速度協(xié)同控制器

2.1.1 航速艏搖角速度協(xié)同控制原理

航速艏搖角速度協(xié)同控制器主要由兩個(gè)單變量PID神經(jīng)網(wǎng)絡(luò)(SPIDNN)構(gòu)成，見圖2。

圖2 航速艏搖角速度協(xié)同控制器

一個(gè)SPIDNN的輸入分別為期望和實(shí)際的縱向速度，其輸出代表著雙推進(jìn)無人艇達(dá)到該期望縱向速度所需的力的大小。另一個(gè)SPIDNN的輸入分別為期望的艏搖角速度和船實(shí)際的艏搖角速度，其輸出代表著雙推進(jìn)無人艇達(dá)到期望艏搖角速度所需的力矩大小。將兩個(gè)SPIDNN的輸出進(jìn)行相加和相減，并分別作為雙推進(jìn)無人艇兩個(gè)推進(jìn)器的控制信號控制兩個(gè)電機(jī)的轉(zhuǎn)速，就可以達(dá)到雙推進(jìn)無人艇航速艏搖角速度協(xié)同控制的效果，并且控制器中的權(quán)值可以根據(jù)無人艇航速誤差和艏搖角速度誤差進(jìn)行學(xué)習(xí)調(diào)整，實(shí)現(xiàn)自適應(yīng)的功能，具有良好的控制效果。

單變量PID神經(jīng)網(wǎng)絡(luò)是3層前向神經(jīng)網(wǎng)絡(luò)，具有動(dòng)態(tài)非線性的特點(diǎn)[8]，結(jié)構(gòu)見圖3。具有輸入層、隱含層、輸出層，各層神經(jīng)元的數(shù)目分別是2、3、1。輸入層兩個(gè)輸入信號分別是指令信號和被控對象的輸出信號，隱含層為比例、積分、微分神經(jīng)元，分別對應(yīng)于PID控制的3個(gè)環(huán)節(jié)，其傳遞函數(shù)分別為比例、積分、微分函數(shù)。輸入信息先通過隱含層的3個(gè)動(dòng)態(tài)神經(jīng)元處理，按照神經(jīng)元權(quán)值進(jìn)行加權(quán)組合得到結(jié)果，最后結(jié)果在輸出層輸出。SPIDNN先將輸入信息向前傳播得到輸出，然后將得到的誤差信息反向傳播，對網(wǎng)絡(luò)的權(quán)值進(jìn)行修正，達(dá)到自適應(yīng)調(diào)節(jié)控制器參數(shù)的效果。

圖3 PID神經(jīng)網(wǎng)絡(luò)控制結(jié)構(gòu)

2.1.2 SPIDNN前向算法

SPIDNN輸入層的輸入與輸出相等，隱層第i個(gè)神經(jīng)元的輸入為

(1)

式中：1wij為輸入層第j個(gè)節(jié)點(diǎn)至隱層第i個(gè)節(jié)點(diǎn)的權(quán)值。

隱含層比例、積分、微分神經(jīng)元的輸出qi(k),i=1、2、3的計(jì)算如下。

1)比例環(huán)節(jié)。

(2)

2)積分環(huán)節(jié)。

3)微分環(huán)節(jié)。

輸出層神經(jīng)元的輸入是隱層各節(jié)點(diǎn)輸出的加權(quán)和。

(5)

式中：2wI為隱含層節(jié)點(diǎn)i至輸出層節(jié)點(diǎn)的權(quán)值。

輸出層神經(jīng)元的輸出，即網(wǎng)絡(luò)的輸出為

(6)

2.1.3 SPIDNN反向?qū)W習(xí)算法

PID與BP神經(jīng)網(wǎng)絡(luò)的反向?qū)W習(xí)算法相同，都采用梯度下降法。反向?qū)W習(xí)的目的都是將誤差控制在較小的范圍內(nèi)，故將目標(biāo)函數(shù)定義為

(7)

經(jīng)k步訓(xùn)練，調(diào)整隱含層至輸出層權(quán)值算法。

(8)

式中：η2為2w的學(xué)習(xí)步長。

(9)

經(jīng)k步訓(xùn)練，調(diào)整輸入層至隱含層權(quán)值算法。

(10)

式中：η1為1w的學(xué)習(xí)步長。

(11)

2.2 基于全局穩(wěn)定的滑模軌跡跟蹤控制器

2.2.1 軌跡跟蹤控制原理

滑模軌跡跟蹤控制器是由內(nèi)外環(huán)構(gòu)成的控制系統(tǒng)，位置子系統(tǒng)為外環(huán)，姿態(tài)子系統(tǒng)為內(nèi)環(huán)。外環(huán)的位置控制系統(tǒng)根據(jù)位置誤差產(chǎn)生速度控制指令vr和期望航向角θd，期望航向角θd再被傳遞給內(nèi)環(huán)的姿態(tài)控制系統(tǒng)，姿態(tài)控制系統(tǒng)通過滑?？刂扑惴▽?shí)現(xiàn)對中間指令信號的跟蹤，并產(chǎn)生艏搖角速度控制指令ωr?；＼壽E控制器將產(chǎn)生的航速控制指令和艏搖角速度控制指令傳遞給航速艏搖角速度協(xié)同控制器，實(shí)現(xiàn)對參考軌跡的跟蹤。原理見圖4。

圖4 軌跡跟蹤控制器原理示意

2.2.2 位置控制律設(shè)計(jì)

通過設(shè)計(jì)位置控制律，實(shí)現(xiàn)x跟蹤xd，y跟蹤yd。取理想軌跡[xdyd]，則誤差跟蹤方程為

(12)

式中：xe=x-xd；ye=y-yd。

(13)

設(shè)計(jì)理想的位姿指令[xdyd]時(shí)，需要使θd的值域滿足(-π/2,π/2)。

設(shè)計(jì)控制律為

(14)

式中：a、p1>0;b、p2>0。

則模型變?yōu)?/p>

(15)

根據(jù)文獻(xiàn)[9]提出的動(dòng)態(tài)系統(tǒng)全局漸進(jìn)穩(wěn)定定理可實(shí)現(xiàn)xe→0，ye→0。

動(dòng)態(tài)系統(tǒng)全局漸進(jìn)穩(wěn)定定理可表述為

(16)

式中：α、k>0。則當(dāng)t→∞時(shí)，η→0。

至此，可得實(shí)際的位置控制律為

(17)

2.2.3 姿態(tài)控制律設(shè)計(jì)

通過設(shè)計(jì)姿態(tài)控制律ω，實(shí)現(xiàn)無人艇航向角θ跟蹤θd。取θe=θ-θd，取滑模函數(shù)為s=θe，則

設(shè)計(jì)姿態(tài)控制律為

(18)

式中：k>0；η>0。

(19)

式中：α為任意常數(shù)。

Vθ(t)≤e-2k(t-t0)Vθ(t0)

(20)

可見Vθ(t)為指數(shù)收斂，即s為指數(shù)收斂，從而θe指數(shù)收斂，艏向角θ指數(shù)收斂于θd，收斂速度取決于控制律中k的值。

3 實(shí)船實(shí)驗(yàn)

“iNav-III”號雙推進(jìn)無人艇是無舵雙槳的形式，該雙推進(jìn)無人艇長1.75 m、寬0.75 m、吃水0.12～0.13 m。艇尾裝有兩個(gè)推進(jìn)器，最大航速可達(dá)2 m/s，最大艏搖角速度可達(dá)40 (°)/s。艇內(nèi)裝有電子羅經(jīng)、慣性測量與導(dǎo)航系統(tǒng)、差分GPS以及數(shù)傳電臺。

實(shí)船實(shí)驗(yàn)在武漢理工大學(xué)露天操縱船池進(jìn)行，實(shí)驗(yàn)場景見圖5。實(shí)船實(shí)驗(yàn)中，將SPIDNN參考指令信號的輸入神經(jīng)元與隱含層間的初始權(quán)重置為1，將反饋信號與隱含層間的初始權(quán)重置為-1，將傳統(tǒng)PID控制器的經(jīng)驗(yàn)參數(shù)用于SPIDNN隱含層至輸出層的初始權(quán)重，兩個(gè)單變量PID神經(jīng)網(wǎng)絡(luò)隱含層至輸出層的初始權(quán)值2w1=0.7，2w2=0.2，2w3=0.1。學(xué)習(xí)速率η1和η2皆置為0.05。滑模軌跡跟蹤控制器中a=0.5，b=0.5，p1=0.4，p2=0.4，k=0.2，η=0.1。

圖5 雙推進(jìn)無人艇及其試驗(yàn)場景

為驗(yàn)證所設(shè)計(jì)的基于PID神經(jīng)網(wǎng)絡(luò)的航速艏搖角速度協(xié)同控制器的實(shí)際控制效果，進(jìn)行航速為1.0 m/s，艏搖角速度為0.2 rad/s的雙推進(jìn)無人艇航速艏搖角速度協(xié)同控制實(shí)驗(yàn)。將其與普通的PID控制器進(jìn)行對比，普通PID控制器的控制參數(shù)與PID神經(jīng)網(wǎng)絡(luò)隱含層至輸出層的初始權(quán)重相同，航速和艏搖角速度控制結(jié)果分別見圖6、7。

圖6 航速控制

圖7 艏搖角速度控制

實(shí)驗(yàn)結(jié)果表明，基于PID神經(jīng)網(wǎng)絡(luò)的航速艏搖角速度協(xié)同控制器能夠快速而有效地控制無人艇的艏向速度和艏搖角速度收斂到較小的誤差范圍內(nèi)，并且由于神經(jīng)網(wǎng)絡(luò)自學(xué)習(xí)的能力使其能夠得到比普通PID更好的控制效果。

進(jìn)行直線軌跡跟蹤實(shí)驗(yàn)，無人艇所跟蹤的直線軌跡用函數(shù)表達(dá)如下。

(22)

直線軌跡跟蹤試驗(yàn)效果見圖8，無人艇的初始速度為0 m/s，航向角為100°。軌跡跟蹤過程中的位置誤差見圖9，穩(wěn)定后最大位置誤差為1.232 m，實(shí)驗(yàn)結(jié)果表明，所設(shè)計(jì)的軌跡跟蹤控制器能夠較好地實(shí)現(xiàn)對直線軌跡的跟蹤。

圖8 直線軌跡跟蹤的路徑

圖9 直線軌跡跟蹤的位置誤差

無人艇在跟蹤過程中不可能一直走直線，當(dāng)遇到障礙物或有任務(wù)要求時(shí)需要具有對曲線軌跡進(jìn)行跟蹤的能力，因此，設(shè)計(jì)如式(23)所示的圓形跟蹤軌跡進(jìn)行軌跡跟蹤試驗(yàn)。

(23)

實(shí)驗(yàn)結(jié)果見圖10，無人艇的初始速度為0，初始航向角為94°，軌跡跟蹤位置誤差見圖11，最大位置誤差為1.205 m。

圖10 圓形軌跡跟蹤路徑

圖11 圓形軌跡跟蹤位置誤差

試驗(yàn)結(jié)果表明，所設(shè)計(jì)的軌跡跟蹤控制器能夠較好的實(shí)現(xiàn)對曲線軌跡的跟蹤。

為了驗(yàn)證軌跡跟蹤控制器對非圓曲線的跟蹤能力，設(shè)計(jì)一組參考軌跡為正弦曲線的軌跡跟蹤試驗(yàn)，參考軌跡的函數(shù)表達(dá)式如下。

(24)

雙推進(jìn)無人艇對正弦曲線進(jìn)行軌跡跟蹤的軌跡見圖12，無人艇的初始速度為0，初始航向角為24°，軌跡跟蹤位置誤差見圖13，跟蹤穩(wěn)定后的最大誤差為0.907 m。圓形曲線和正弦曲線軌跡跟蹤結(jié)果表明，軌跡跟蹤控制器能夠?qū)崿F(xiàn)對復(fù)雜曲線的軌跡跟蹤。

圖12 正弦曲線軌跡跟蹤的軌跡

圖13 正弦曲線軌跡跟蹤的位置誤差

對以上3組軌跡跟蹤實(shí)驗(yàn)穩(wěn)定后的軌跡跟蹤誤差數(shù)據(jù)進(jìn)行統(tǒng)計(jì)，所有軌跡的軌跡跟蹤平均誤差均小于0.661 m，證明所設(shè)計(jì)的軌跡跟蹤控制器具有良好的控制效果。進(jìn)一步對軌跡跟蹤位置誤差圖進(jìn)行觀察后可以發(fā)現(xiàn)，雖然設(shè)計(jì)的軌跡跟蹤控制系統(tǒng)能夠讓無人艇實(shí)現(xiàn)對曲線和直線軌跡較為精確地跟蹤，但是軌跡跟蹤的誤差在進(jìn)入穩(wěn)定狀態(tài)后，會(huì)在零到最大跟蹤誤差值之間進(jìn)行波動(dòng)。分析其原因，一方面是使用滑?？刂扑惴ㄔO(shè)計(jì)控制律時(shí)，由于開關(guān)函數(shù)的存在，會(huì)產(chǎn)生滑模控制結(jié)構(gòu)本質(zhì)上的抖振現(xiàn)象；另一方面是由于從控制信號發(fā)送到執(zhí)行機(jī)構(gòu)執(zhí)行需要一段響應(yīng)時(shí)間以及受到無人艇自身慣性的影響，無人艇不可能立即達(dá)到期望的運(yùn)動(dòng)狀態(tài)。

4 結(jié)論

1)以雙推進(jìn)無人艇為研究對象，采用PID神經(jīng)網(wǎng)絡(luò)和滑模變結(jié)構(gòu)控制算法對無人艇的軌跡跟蹤控制系統(tǒng)進(jìn)行研究，提出的雙閉環(huán)軌跡跟蹤控制方法運(yùn)用到實(shí)船實(shí)驗(yàn)中，實(shí)現(xiàn)了對各種直線和曲線軌跡的跟蹤控制。

2)在實(shí)船實(shí)驗(yàn)中，無人艇的軌跡跟蹤誤差會(huì)在一小段范圍內(nèi)抖動(dòng)。這是由于滑模變結(jié)構(gòu)控制在本質(zhì)上的不連續(xù)開關(guān)特性導(dǎo)致的抖振現(xiàn)象，抖振不可以完全消除，但可以通過調(diào)整滑模控制律的參數(shù)減少抖振的影響。

3)針對實(shí)船實(shí)驗(yàn)中軌跡跟蹤存在著的抖振問題，下一步的研究可以考慮根據(jù)位置誤差自適應(yīng)調(diào)整控制律的參數(shù)得到最優(yōu)的控制效果，實(shí)現(xiàn)更加精確的軌跡跟蹤。