姚緒梁，王曉偉，2，*

（1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，哈爾濱150001； 2.九江職業(yè)技術(shù)學(xué)院 機(jī)械工程學(xué)院，九江332007）

自主水下航行器（AUV）在海洋科學(xué)、水下勘探、搜索和救援、海底管道跟蹤、海底電纜維護(hù)和國防等領(lǐng)域得到廣泛的應(yīng)用。路徑跟蹤是AUV完成各種作業(yè)任務(wù)的關(guān)鍵技術(shù)。為了簡(jiǎn)化路徑規(guī)劃和降低計(jì)算量，AUV的路徑經(jīng)常基于路徑點(diǎn)進(jìn)行規(guī)劃，路徑點(diǎn)之間用直線進(jìn)行連接。所以，對(duì)直線路徑進(jìn)行跟蹤是AUV一項(xiàng)非常基本的控制任務(wù)。但是，AUV動(dòng)力學(xué)模型存在非線性、耦合性和不確定性。另外，為了節(jié)能、降低成本和提高可靠性，大部分AUV都被設(shè)計(jì)為欠驅(qū)動(dòng)的形式，在某些自由度（例如：橫向和垂向）缺少驅(qū)動(dòng)能力，以上原因?qū)е翧UV的路徑跟蹤控制變得非常棘手。

針對(duì)路徑跟蹤的導(dǎo)引律問題，國內(nèi)外進(jìn)行了大量的研究。文獻(xiàn)［1］應(yīng)用視線法（LOS）導(dǎo)引律來鎮(zhèn)定橫向跟蹤誤差，然后應(yīng)用反饋線性化方法設(shè)計(jì)了動(dòng)力學(xué)控制器，實(shí)現(xiàn)了欠驅(qū)動(dòng)水面船舶直線路徑跟蹤控制，但沒有考慮模型的不確定性。為了改善路徑跟蹤的效果，文獻(xiàn)［2］設(shè)計(jì)了一種前視距離時(shí)變的LOS導(dǎo)引律。文獻(xiàn)［3］針對(duì)無人水面艇的路徑跟蹤控制，應(yīng)用積分視線法（ILOS）導(dǎo)引律，并采用反饋線性化方法設(shè)計(jì)了航速和航向的動(dòng)力學(xué)控制器。另外，文獻(xiàn)［4］針對(duì)欠驅(qū)動(dòng)水面船舶的路徑跟蹤控制設(shè)計(jì)了一種向量場(chǎng)（VF）導(dǎo)引律。針對(duì)AUV動(dòng)力學(xué)模型的非線性、耦合性和不確定性，動(dòng)力學(xué)控制的設(shè)計(jì)也得到了廣泛的研究。目前，常用的方法有PID控制［5］、反饋線性化控制［6］、自適應(yīng)控制［7］、魯棒控制［8］、滑?？刂疲⊿MC）［9］、模糊控制［10］和神經(jīng)網(wǎng)絡(luò)控制［11］等。但是以上文獻(xiàn)都沒有考慮實(shí)際控制輸入的約束問題。文獻(xiàn)［12］考慮了輸入的約束，應(yīng)用模型預(yù)測(cè)控制（MPC）對(duì)LOS導(dǎo)引律進(jìn)行優(yōu)化，改善了船舶對(duì)直線路徑的跟蹤質(zhì)量。文獻(xiàn)［13］考慮了舵角的幅值和速度約束，應(yīng)用MPC設(shè)計(jì)控制器實(shí)現(xiàn)了AUV的縱傾和深度控制。但是文獻(xiàn)［12-13］都是基于名義模型，沒有考慮模型的不確定性?；诼窂近c(diǎn)的直線跟蹤控制雖然路徑規(guī)劃簡(jiǎn)單便于應(yīng)用，但是路徑在路徑點(diǎn)處是不光滑的。在AUV對(duì)路徑進(jìn)行跟蹤過程中，當(dāng)路徑在路徑點(diǎn)附近進(jìn)行切換時(shí)，跟蹤誤差會(huì)存在突變，從而使舵角很容易產(chǎn)生飽和。舵角頻繁出現(xiàn)飽和不僅會(huì)降低系統(tǒng)的穩(wěn)定性，而且還會(huì)增加AUV的阻力和能耗。文獻(xiàn)［14］針對(duì)船舶的路徑跟蹤控制，考慮了舵角的飽和并應(yīng)用反步法設(shè)計(jì)了控制器，但并沒有從根本上改善舵角的飽和現(xiàn)象。另外，在路徑跟蹤過程中，經(jīng)常需要避障，設(shè)計(jì)避障導(dǎo)引律［15］和對(duì)路徑進(jìn)行重新規(guī)劃［16］是目前常用的避障方法，但是這2種方法都會(huì)增加控制器的復(fù)雜度。

本文針對(duì)欠驅(qū)動(dòng)AUV三維直線路徑跟蹤和避障控制，基于級(jí)聯(lián)控制理論設(shè)計(jì)了運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)控制器。在運(yùn)動(dòng)學(xué)控制器中考慮了AUV角速度存在的約束，應(yīng)用MPC設(shè)計(jì)了導(dǎo)引律，負(fù)責(zé)產(chǎn)生期望的角速度信號(hào)。為了實(shí)現(xiàn)對(duì)期望速度信號(hào)的跟蹤，在動(dòng)力學(xué)控制中考慮了推進(jìn)器轉(zhuǎn)速和舵角的飽和，應(yīng)用SMC設(shè)計(jì)了動(dòng)力學(xué)控制器，可以有效地克服模型的不確定性，保證系統(tǒng)的魯棒性。

1 欠驅(qū)動(dòng)AUV運(yùn)動(dòng)模型

本文研究的欠驅(qū)動(dòng)AUV在尾部配置一個(gè)推進(jìn)器用來實(shí)現(xiàn)對(duì)縱向速度的控制、水平舵和方向舵分別實(shí)現(xiàn)對(duì)縱傾和艏搖的控制，在橫向和垂向沒有驅(qū)動(dòng)力。圖1為欠驅(qū)動(dòng)AUV三維直線路徑跟蹤示意圖，｛B｝：O-xyz和｛I｝：E-ξηζ分別為隨體坐標(biāo)系和固定坐標(biāo)系；φ和p分別為橫搖角和橫搖角速度；Rk為接受圓半徑。因?yàn)榇嬖诨謴?fù)力，AUV的橫搖很小，另外路徑跟蹤不需要對(duì)橫搖進(jìn)行控制，忽略橫搖后，AUV的運(yùn)動(dòng)學(xué)模型可以簡(jiǎn)化為如下五自由度的形式：

圖1 欠驅(qū)動(dòng)AUV三維直線路徑跟蹤示意圖Fig.1 Schematic diagram of underactuated AUV 3D straight path following

式中：η＝［ξ η ζ θ ψ］T表示AUV在固定坐標(biāo)系｛I｝中的位置和姿態(tài)，（ξ，η，ζ）為AUV在｛I｝中的坐標(biāo)，θ和ψ分別為縱傾角和艏搖角；v＝［u v w q r］T表示AUV在隨體坐標(biāo)系｛B｝中的速度分量，u、v、w、q、r分別為縱向、橫向、垂向速度和縱傾、艏搖角速度。J（η）為｛B｝到｛I｝的旋轉(zhuǎn)變換矩陣，表達(dá)式為

2 三維直線路徑跟蹤誤差模型

為了便于建立路徑跟蹤誤差模型，以當(dāng)前直線路徑的起點(diǎn)pk為原點(diǎn)定義坐標(biāo)系｛F｝：PxFyFzF。｛F｝坐標(biāo)系的xF軸與直線路徑重合并指向下一個(gè)路徑點(diǎn)pk+1，yF軸指向右側(cè)，zF軸指向下方。定義｛F｝坐標(biāo)系相對(duì)于｛I｝坐標(biāo)系的姿態(tài)為AF＝［θFψF］T，其計(jì)算公式為

式中：Δξ＝ξk+1-ξk，Δη＝ηk+1-ηk，Δζ＝ζk+1-ζk；（ξkηkζk），（ξk+1ηk+1ζk+1）分別為當(dāng)前直線路徑起點(diǎn)pk和終點(diǎn)pk+1在｛I｝坐標(biāo)系中的坐標(biāo)。定義AUV相對(duì)于｛I｝坐標(biāo)系的姿態(tài)為AB＝［θ ψ］T，則AUV相對(duì)于｛F｝坐標(biāo)系的姿態(tài)誤差計(jì)算如下：

式中：下標(biāo)e表示誤差。

對(duì)式（4）求導(dǎo)可得

定義AUV在｛I｝坐標(biāo)系中的坐標(biāo)PO＝［ξo，ηo，ζo］T，P在｛I｝坐標(biāo)系中的坐標(biāo)PP＝［ξk，ηk，ζk］T，則AUV相對(duì)于｛F｝坐標(biāo)系的位置誤差為

對(duì)式（6）求導(dǎo)可得

3 三維直線路徑跟蹤控制器的設(shè)計(jì)

控制器分為運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)控制器兩部分，首先在運(yùn)動(dòng)學(xué)控制器中應(yīng)用MPC設(shè)計(jì)了導(dǎo)引律，負(fù)責(zé)產(chǎn)生期望的縱傾角速度qd和艏搖角速度rd。然后，應(yīng)用SMC設(shè)計(jì)了動(dòng)力學(xué)控制器，負(fù)責(zé)產(chǎn)生實(shí)際的控制信號(hào)，即推進(jìn)器的轉(zhuǎn)速np、水平舵角δs和方向舵角δr。

3.1 運(yùn)動(dòng)學(xué)控制器

因?yàn)閯?dòng)力學(xué)控制器對(duì)期望速度信號(hào)的響應(yīng)有一定的延時(shí)，為了減小延時(shí)對(duì)控制效果的影響，添加如下低通濾波器：

式中：T1＞0、T2＞0為可調(diào)時(shí)間常數(shù)。

因?yàn)楸疚难芯康那夫?qū)動(dòng)AUV在橫向和垂向沒有驅(qū)動(dòng)力，所以v和w是非常小的，為了簡(jiǎn)化控制器設(shè)計(jì)，可對(duì)其進(jìn)行忽略。另外，縱向誤差xe不需要進(jìn)行控制。根據(jù)式（5）、式（7）和式（8），三維直線跟蹤可簡(jiǎn)化為以下系統(tǒng)的鎮(zhèn)定控制問題：

其中：T為采樣周期。

根據(jù)預(yù)測(cè)模型（11），未來的狀態(tài)變量可以通過式（12）進(jìn)行預(yù)測(cè)：

輸出量可以重新整理為如下矩陣形式：

式中：

考慮控制量的幅值存在如下約束：

通過用Uk，k參數(shù)化，可以將約束（15）整理為如下線性不等式的形式：

取上一采樣時(shí)刻最優(yōu)解Uk-1，k-1的后Nc-1個(gè)控制增量和當(dāng)前時(shí)刻最優(yōu)解Uk，k的最后一個(gè)控制增量組成一個(gè)當(dāng)前時(shí)刻的可行解，該可行解及在其控制作用下的預(yù)測(cè)輸出分別為

定義ΔJk，k＝Jk，k-Jk-1，k-1，則

所以只要滿足約束條件（19）則可以保證ΔJk，k≤0，從而保證MPC的穩(wěn)定：

因?yàn)槭剑?0）等號(hào)右邊第1項(xiàng)為常量，所以MPC每一步的優(yōu)化問題等價(jià)為求解如下約束問題：

通過對(duì)式（21）求解可以得到預(yù)測(cè)時(shí)域內(nèi)的控制量Uk，k，將Uk，k的第1個(gè)值uk，k代入式（22），即可得到當(dāng)前采樣時(shí)刻縱傾和艏搖角速度的最優(yōu)期望值為

3.2 動(dòng)力學(xué)控制器

因?yàn)楸疚难芯康那夫?qū)動(dòng)AUV在橫向和垂向沒有驅(qū)動(dòng)力，所以其橫向速度v和垂向速度w是非常小的，為了簡(jiǎn)化動(dòng)力學(xué)控制器的設(shè)計(jì)，可以將動(dòng)力學(xué)方程進(jìn)一步簡(jiǎn)化為

式中：

因?yàn)橥七M(jìn)器的推力和舵力是具有飽和限制的，定義輸入飽和量為

為了對(duì)輸入飽和進(jìn)行補(bǔ)償，設(shè)計(jì)如下輔助系統(tǒng)：

式中：

定義誤差變量：

式中：ud＞0為期望的航速。

定義滑模函數(shù)：

因?yàn)锳UV的期望航速ud被設(shè)置為常值，所以系統(tǒng)可以近似為自治的，可以認(rèn)為du、dq、dr有界且慢時(shí)變，其導(dǎo)數(shù)可以忽略。定義誤差變量：

定義Lyapunov函數(shù)：

對(duì)V求導(dǎo)可得

設(shè)計(jì)控制律和自適應(yīng)律為

式中：ρ、KT、DP、kL分別為流體密度、螺旋槳推力系數(shù)、螺旋槳直徑、舵的升力系數(shù)。

4 仿真結(jié)果與分析

本文采用REMUS-100作為仿真模型，模型參數(shù)可以參考文獻(xiàn)［17-18］。仿真采用了2種控制方法進(jìn)行了對(duì)比。

1）方法1

采用本文所設(shè)計(jì)的控制器，控制器的主要設(shè)計(jì)參數(shù)為：T＝0.1，T1＝0.5，T2＝0.5，Q11＝2，Q22＝2，Q33＝2，Q44＝2，Q55＝1，Q66＝1，R11＝2，R22＝2，Np＝10，Nc＝3，k1＝0.1，k2＝0.1，k3＝0.1，k4＝0.1，k5＝0.1，k6＝0.1，k7＝0.2，k8＝30，c11＝1，c12＝1，c13＝1，c21＝1，c22＝1，c23＝1，c31＝1，c32＝1，c33＝1，umax＝［0.15 0.2］T，umin＝［-0.15 -0.2］T。期望直線路徑基于表1所列路徑點(diǎn)生成。AUV的初始位姿和速度均為零。期望航速ud＝1m/s，路徑切換的接受圓半徑Rk＝10m。在仿真中設(shè)置了一個(gè)障礙物，其半徑為5m，在固定坐標(biāo)系中的坐標(biāo)為：PA＝［ξA，ηA，ζA］T＝［85，0，21］Tm。仿真中選擇從右側(cè)避障，懲罰項(xiàng)按式（32）進(jìn)行設(shè)置：

表1 路徑點(diǎn)Table 1 Way points m

2）方法2

運(yùn)動(dòng)學(xué)控制器采用了LOS導(dǎo)引律進(jìn)行路徑跟蹤和避障［15］，動(dòng)力學(xué)控制器采用PID控制器。為了驗(yàn)證控制器的魯棒性，仿真時(shí)段0～100 s水動(dòng)力參數(shù)增加20%，100～250 s采用名義水動(dòng)力參數(shù)，250～400 s水動(dòng)力參數(shù)減小20%。

圖2 路徑跟蹤和避障仿真結(jié)果Fig.2 Simulation results of path following and obstacle avoidance

圖3 路徑跟蹤誤差曲線Fig.3 Path following error curves

圖2為三維直線路徑跟蹤和避障示意圖及其在水平面、垂直面的投影圖?？梢钥吹?，2種控制方法都能實(shí)現(xiàn)對(duì)直線路徑的跟蹤和避障。但是通過對(duì)比也可以看到，在每次進(jìn)行路徑切換時(shí)，方法1的跟蹤效果更好一些，能夠更快地收斂到期望路徑上，另外方法1能夠更快更準(zhǔn)確地收斂到期望的橫向誤差從而更好地完成避障。圖3為路徑跟蹤誤差曲線?？梢钥吹?，每一段直線路徑上的位置誤差和姿態(tài)誤差都能收斂到零，同時(shí)可以看到方法1大約在250 s時(shí)將yed設(shè)置為了7 m，從而啟動(dòng)了避障，當(dāng)避障完成后yed又被設(shè)置為零，最終使AUV重新收斂到期望路徑上。圖4為實(shí)際的控制輸入曲線?？梢钥吹?，控制輸入存在有規(guī)律的抖振，其主要原因是直線路徑在路徑點(diǎn)處不光滑，路徑切換時(shí)跟蹤誤差存在跳變。仿真中根據(jù)實(shí)際情況將舵角的最大絕對(duì)值設(shè)置為25°，可以看到方法2中的舵角在路徑切換時(shí)出現(xiàn)了飽和現(xiàn)象，但方法1沒有出現(xiàn)飽和現(xiàn)象，整體上方法1的舵角更加平穩(wěn)，這樣更利于系統(tǒng)的穩(wěn)定性和節(jié)能。圖5為AUV的速度曲線?？梢钥吹剑?種控制方法都能使AUV的縱向速度收斂到期望值，但方法1的控制效果更穩(wěn)定一些，同時(shí)可以看到橫向和垂向速度很小而且是收斂的。方法1的縱傾角速度和艏搖角速度都在約束范圍之內(nèi)，從而有效地改善了舵角飽和現(xiàn)象。圖6為方法1的魯棒項(xiàng)系數(shù)和滑模函數(shù)曲線?？梢钥吹?，所有的滑模函數(shù)在路徑切換時(shí)都出現(xiàn)了一定的抖動(dòng)，但均是收斂的且趨于零，魯棒項(xiàng)系數(shù)能夠自動(dòng)進(jìn)行調(diào)整從而有效地減小魯棒項(xiàng)引起的抖振。

圖4 控制輸入曲線Fig.4 Control input curves

圖5 速度曲線Fig.5 Speed curves

圖6 魯棒項(xiàng)系數(shù)和滑模函數(shù)曲線Fig.6 Robust term coefficients and sliding mode function curves

5 結(jié) 論

1）本文基于級(jí)聯(lián)控制理論分別應(yīng)用MPC和SMC設(shè)計(jì)了運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)控制器，從而實(shí)現(xiàn)了欠驅(qū)動(dòng)AUV三維直線路徑跟蹤和避障控制。

2）在運(yùn)動(dòng)學(xué)控制器中設(shè)計(jì)了線性時(shí)變MPC導(dǎo)引律，并設(shè)計(jì)了MPC的穩(wěn)定約束條件。因?yàn)檫\(yùn)動(dòng)學(xué)方程不受動(dòng)力學(xué)模型不確定性的影響，所以運(yùn)動(dòng)學(xué)控制器采用MPC不僅能保證預(yù)測(cè)的效果，而且可以簡(jiǎn)化控制器的設(shè)計(jì)。

3）運(yùn)動(dòng)學(xué)控制器考慮了期望角速度的約束，可以有效地改善舵角的飽和現(xiàn)象，從而更利于系統(tǒng)的穩(wěn)定性和節(jié)能。

4）動(dòng)力學(xué)控制器采用SMC可以克服動(dòng)力學(xué)模型的不確定性，保證系統(tǒng)的魯棒性。

接下來將針對(duì)存在海浪、海流等環(huán)境干擾下的欠驅(qū)動(dòng)AUV三維路徑跟蹤進(jìn)行進(jìn)一步的研究。