雷超凡 ，初秀民 ，柳晨光 ，吳文祥 ，李松龍

(1. 武漢理工大學(xué) 國家水運(yùn)安全工程技術(shù)研究中心，湖北 武漢 430063；2. 武漢理工大學(xué) 智能交通系統(tǒng)研究中心，湖北 武漢 430063；3. 武漢理工大學(xué) 交通與物流工程學(xué)院，湖北 武漢 430063)

0 引 言

隨著現(xiàn)代智能控制理論被廣泛應(yīng)用于各行各業(yè)，船舶控制研究也進(jìn)入了智能控制時(shí)代。船舶運(yùn)動控制是其中的核心問題，軌跡跟蹤問題是船舶運(yùn)動控制的基礎(chǔ)，其含義是在所有控制時(shí)刻內(nèi)使船舶達(dá)到對應(yīng)時(shí)刻的參考位置。欠驅(qū)動水面船舶是遠(yuǎn)洋運(yùn)輸?shù)闹匾邦愋?，其特點(diǎn)是船舶缺少側(cè)推裝置，僅依靠船尾推進(jìn)裝置提供的縱向推力和舵提供的旋轉(zhuǎn)扭矩控制船舶運(yùn)動[1]。

在研究確定期望軌跡的軌跡跟蹤問題時(shí)，Serret-Frenet 坐標(biāo)系（以下簡稱SF 坐標(biāo)系）是常用的一種坐標(biāo)構(gòu)建方法。其原理是在運(yùn)動的每一時(shí)刻都在期望軌跡上建立一個(gè)隨時(shí)間變化的坐標(biāo)構(gòu)架，構(gòu)架的坐標(biāo)原點(diǎn)及坐標(biāo)軸方向均隨曲線上期望點(diǎn)的移動而變化。齊雪等[2]針對水下機(jī)器人的運(yùn)動問題引入了SF 坐標(biāo)系，采用反步法，通過靈活設(shè)置虛擬路徑上的參考點(diǎn)放寬了控制器的初始條件，提高了系統(tǒng)的魯棒性。王宏健等[3]結(jié)合SF 坐標(biāo)系對船舶軌跡跟蹤進(jìn)行研究，運(yùn)用反步法設(shè)計(jì)了動力學(xué)控制器，使得航行器能夠精準(zhǔn)地跟蹤曲線路徑。王威等[4]在SF 坐標(biāo)系下對智能車輛的路徑跟蹤問題進(jìn)行了研究，運(yùn)用模型預(yù)測控制，將速度設(shè)為定值，解決了控制延時(shí)的問題。綜合上述文獻(xiàn)可以看出，由于SF 坐標(biāo)系的建立具有時(shí)變特性，所以現(xiàn)有研究多結(jié)合反步法，而在模型預(yù)測控制當(dāng)中難以解決預(yù)測問題，現(xiàn)有研究則選擇了設(shè)置固定速度來簡化速度控制精度的問題。

對USV 的軌跡跟蹤控制，經(jīng)典控制方法有滑模控制、反步法（Backstepping）、神經(jīng)網(wǎng)絡(luò)法和模型預(yù)測控制法（model predictive control，MPC）等此外眾多學(xué)者也在不斷提出改進(jìn)算法。張成舉等[5]基于反步法和自適應(yīng)技術(shù)，提出一種非線性魯棒軌跡跟蹤控制策略，有效減小了反步法中虛擬變量直接求導(dǎo)的復(fù)雜性問題。葛科奇[6]提出了一種動態(tài)神經(jīng)模糊網(wǎng)絡(luò)的水面欠驅(qū)動船舶運(yùn)動控制算法，算法描述了船舶動力系統(tǒng)的受力特性，以黑箱形式模擬了船舶非線性模型，并對算法進(jìn)行了仿真驗(yàn)證。相較而言，模型預(yù)測控制方法采用滾動優(yōu)化機(jī)制，且能夠顯式處理約束，所以解決船舶運(yùn)動非線性系統(tǒng)及多約束問題具有獨(dú)特優(yōu)勢[1]。軌跡跟蹤控制算法在不斷進(jìn)步，但現(xiàn)有研究方向都聚焦于船舶位置控制的精度，而缺少對其航速控制效果的關(guān)注。本文提出一種Serret-Frenet 坐標(biāo)系下USV的時(shí)變參考狀態(tài)軌跡跟蹤模型預(yù)測控制方法。

1 坐標(biāo)系與模型

USV 水面運(yùn)動模型的建立需要依托相應(yīng)的坐標(biāo)系，通常運(yùn)動學(xué)特征依托于慣性坐標(biāo)系，記作{n}=動力學(xué)特征依托于附體坐標(biāo)系，記作描述參考狀態(tài)依托于SF坐標(biāo)系，記作慣性坐標(biāo)系定義為：原點(diǎn)On位 于期望軌跡起點(diǎn)，Onxn指向正北，Onyn指 向正東；附體坐標(biāo)系定義為：原點(diǎn)Ob位于船舶重心，Obxb沿 船舶對稱剖面指向船首，Obyb指向右舷；SF 坐標(biāo)系定義為：原點(diǎn)Of位于期望路徑上當(dāng)前時(shí)刻的目標(biāo)參考點(diǎn)，Of xf指向期望軌跡的切線方向，Of yf指向垂線方向。水面坐標(biāo)系關(guān)系如圖1 所示。

圖1 水面坐標(biāo)關(guān)系Fig. 1 Water surface coordinate relationship

基于Fossen 提出的船舶水動力建模方法構(gòu)建欠驅(qū)動船舶的動力學(xué)模型，其狀態(tài)空間方程表示為：

式中：x(t)=[x,y,φ,u,v,r]T為系統(tǒng)狀態(tài)變量；u(t)=[τu,0,τr]T為模型輸入控制量；e(t)=[eu,ev,er]T設(shè)定為干擾；x,y,φ 為 船舶位置及首向； τu,τr分別為船尾推力及轉(zhuǎn)向力矩；eu,ev,er分別為3 個(gè)自由度上的干擾，f表示映射關(guān)系，表示為：

式中：M表示慣性矩陣；C表示科式向心矩陣；D表示阻尼矩陣，上述矩陣均由水動力導(dǎo)數(shù)構(gòu)成，水動力導(dǎo)數(shù)可由經(jīng)驗(yàn)公式法求得；T為坐標(biāo)變換矩陣，用于附體坐標(biāo)系下變量向慣性坐標(biāo)系轉(zhuǎn)化：

現(xiàn)已知船舶期望軌跡表示為函數(shù)yr=g(xr)，將船舶實(shí)際位置用其重心位置點(diǎn)Q表示，則Q點(diǎn)在慣性坐標(biāo)系下坐標(biāo)為（x,y）,同時(shí)定義其在SF坐標(biāo)系下坐標(biāo)為（a,d）；引入合速度其與Onyn軸的夾角θ表示為那么基于坐標(biāo)可將軌跡跟蹤問題的控制目標(biāo)描述為以下兩點(diǎn)：

1）控制船舶重心靠近期望點(diǎn)，即

式中：i表示當(dāng)前仿真時(shí)刻；Np表示控制器預(yù)測步長。

2）控制船舶合速度方向趨近于期望軌跡在期望點(diǎn)的切線方向，切線方向通過對期望軌跡求一階導(dǎo)數(shù)得到，即

為了對曲線有更好的描述，建立從期望軌跡起點(diǎn)到當(dāng)前期望點(diǎn)的弧長s對坐標(biāo)x的變上限積分對應(yīng)關(guān)系:

利用式(5)可將式(3)和式(4)中的自變量x轉(zhuǎn) 換為s。

2 控制器設(shè)計(jì)

2.1 預(yù)測模型

為了減輕控制器計(jì)算負(fù)擔(dān)，保證控制實(shí)時(shí)性，采用線性模型預(yù)測控制器。運(yùn)用船舶運(yùn)動模型作為控制器預(yù)測迭代模型，將模型代入控制器還需要對其進(jìn)行線性化和離散化處理。

首先利用近似線性化方法對模型進(jìn)行線性化處理，將任意時(shí)刻的船舶運(yùn)動狀態(tài)空間方程以上一控制時(shí)刻平衡點(diǎn) (xr,ur)作為參考點(diǎn)進(jìn)行Taylor 展開，并與平衡點(diǎn)做差，得到線性差值表達(dá)式如下：

對其進(jìn)行近似離散化，引入步長Ts，則A，B變化可分步表示為：

經(jīng)過離散化后得到的預(yù)測模型為：

式中：k=1,2,···,Np為預(yù)測步數(shù)。

2.2 參考量設(shè)計(jì)

設(shè)計(jì)控制系統(tǒng)參考量。首先需要在期望軌跡上找到每一控制時(shí)刻對應(yīng)的期望點(diǎn) (xe,g(xe))，選取船舶當(dāng)前位置在期望軌跡上的投影點(diǎn)作為期望點(diǎn)，則其可通過求解方程得到：

引入對船舶的速度控制，將其設(shè)定為關(guān)于s的函數(shù)：

則根據(jù)期望速度，期望點(diǎn)在期望軌跡上的弧長變化量可表示為下式：

在預(yù)測步長Np內(nèi) 得到期望點(diǎn)慣性坐標(biāo)序列為下式：

對應(yīng)期望點(diǎn)坐標(biāo)序列的合速度參考量序列由期望軌跡的一階導(dǎo)數(shù)得到，表示為下式：

2.3 目標(biāo)函數(shù)及約束設(shè)計(jì)

為完成可變航速軌跡跟蹤控制的目的，基于以下4 個(gè)方面設(shè)置目標(biāo)函數(shù)：

1）使預(yù)測到船舶位置點(diǎn)的投影點(diǎn)靠近期望點(diǎn)序列位置；

2）使船舶當(dāng)前位置靠近期望點(diǎn)位置；

3）使船舶合速度方向靠近期望點(diǎn)的切線方向；

4）使控制系統(tǒng)輸出控制量盡可能小。

由此設(shè)計(jì)的目標(biāo)函數(shù)表示為：

式中：Qn(n=1,2,...,5) 和R為 權(quán)值和權(quán)重矩陣；Nc為控制步長；u為系統(tǒng)控制量。

通過目標(biāo)函數(shù)的設(shè)置，可變航速的軌跡跟蹤控制的優(yōu)化問題轉(zhuǎn)化為求解所設(shè)約束下目標(biāo)函數(shù)最小值對應(yīng)的控制量問題。依據(jù)禁止船舶倒航、動力機(jī)構(gòu)性能設(shè)置約束條件為：

3 仿真實(shí)驗(yàn)

3.1 仿真模型

選擇Cybership Ⅱ模型船作為研究對象[7]，該船是一艘以1∶70 的比例建造的補(bǔ)給船模型，其長度為LC=1.255 m ，寬度為BC=0.29 m，最大推進(jìn)力為2 N，最大推力下的平衡船速為0.58 m/s，最大轉(zhuǎn)向力矩為1.5 N·m 。其對應(yīng)矩陣M和D大小分別為：

3.2 控制器參數(shù)設(shè)置

期望軌跡函數(shù)及期望速度設(shè)置為：

USV 初始狀態(tài)向量設(shè)置為：

權(quán)重及權(quán)重矩陣設(shè)置為：

根據(jù)所選船型的動力機(jī)構(gòu)性能，將約束設(shè)置為：

預(yù)測步長和控制步長設(shè)置為：

基于風(fēng)、浪、流等設(shè)置隨機(jī)干擾：

式中， randi(·)表示均勻分布的隨機(jī)整數(shù)。

3.3 結(jié)果分析

對USV 運(yùn)動進(jìn)行210 步仿真，以文獻(xiàn)[1]所用軌跡跟蹤方法作為實(shí)驗(yàn)對比組，將每一仿真時(shí)刻的期望點(diǎn)慣性坐標(biāo)序列P1(i) 以 及期望速度序列P2(i)的第一項(xiàng)組合成矩陣P210×2，并將P作為文獻(xiàn)[1]所用方法的參考量矩陣，其余參數(shù)均與本文方法設(shè)置相同。得到的運(yùn)行對比結(jié)果如圖2～圖5 所示。

圖2 跟蹤結(jié)果對比Fig. 2 Tracking results comparison

由圖2 和圖3 可知，本文設(shè)計(jì)的控制方法良好地完成了軌跡跟蹤任務(wù)，初始位置與期望軌跡的偏差使得USV 在航行初與投影點(diǎn)存在較大距離，20 s 后能夠與期望軌跡貼合，此后仿真過程最大距離為0.023 m，與對比組進(jìn)行對比可以看出，本文方法在調(diào)整初始位置誤差時(shí)，超出量更少且收斂速度更快，但總體而言，2 種方法位置點(diǎn)與目標(biāo)點(diǎn)的距離保持能力相差不大。由圖4 可以看出，USV 航速從0 m/s 開始，經(jīng)過15 s 后能夠跟隨期望航速曲線，此后在干擾的作用下有小幅的波動，對比組表現(xiàn)出的航速波動大，本文方法具有更好的航速控制效果。由圖5 可以看出，軌跡跟蹤初期，控制器輸出較大推力以追逐期望曲線，在完成追逐后迅速保持較小的控制輸出，輸出推力和轉(zhuǎn)矩均由隨干擾的小幅度震蕩，相較而言，干擾對扭矩輸出的影響更為明顯，對比而言，本文提出的控制方法控制量的波動較小，控制更為穩(wěn)定。

圖3 位置點(diǎn)與目標(biāo)點(diǎn)的距離Fig. 3 Distance between position point and projection point

圖4 航速變換Fig. 4 Speed

圖5 控制器輸出Fig. 5 Controller output

4 結(jié) 語

本文以Serret-Frenet 坐標(biāo)系為基礎(chǔ)，建立USV 三自由度運(yùn)動模型，針對軌跡跟蹤航速時(shí)變控制問題，提出了一種基于模型預(yù)測控制算法的USV 軌跡跟蹤控制方法，通過仿真得到的結(jié)論如下：

1）在Serret-Frenet 坐標(biāo)系下利用投影法尋找當(dāng)前船舶位置在期望軌跡上對應(yīng)的期望點(diǎn)；

2）引入隨航行路程而變的期望速度，用對應(yīng)控制時(shí)刻的期望速度構(gòu)建參考位置序列作為系統(tǒng)參考量；

3）控制方法對USV 軌跡跟蹤和速度控制均有良好的效果。本文設(shè)計(jì)的控制方法可為USV 航行行為控制提供參考。