基于MPC導(dǎo)引律的AUV路徑跟蹤和避障控制

姚緒梁，王曉偉，2，*

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

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

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

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

1 欠驅(qū)動AUV運動模型

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

圖1 欠驅(qū)動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)前直線路徑的起點pk為原點定義坐標(biāo)系｛F｝：PxFyFzF。｛F｝坐標(biāo)系的xF軸與直線路徑重合并指向下一個路徑點pk+1，yF軸指向右側(cè)，zF軸指向下方。定義｛F｝坐標(biāo)系相對于｛I｝坐標(biāo)系的姿態(tài)為AF＝［θFψF］T，其計算公式為

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

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

對式（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相對于｛F｝坐標(biāo)系的位置誤差為

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

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

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

3.1 運動學(xué)控制器

因為動力學(xué)控制器對期望速度信號的響應(yīng)有一定的延時，為了減小延時對控制效果的影響，添加如下低通濾波器：

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

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

其中：T為采樣周期。

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

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

式中：

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

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

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

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

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

因為式（20）等號右邊第1項為常量，所以MPC每一步的優(yōu)化問題等價為求解如下約束問題：

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

3.2 動力學(xué)控制器

因為本文研究的欠驅(qū)動AUV在橫向和垂向沒有驅(qū)動力，所以其橫向速度v和垂向速度w是非常小的，為了簡化動力學(xué)控制器的設(shè)計，可以將動力學(xué)方程進(jìn)一步簡化為

式中：

因為推進(jìn)器的推力和舵力是具有飽和限制的，定義輸入飽和量為

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

式中：

定義誤差變量：

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

定義滑模函數(shù)：

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

定義Lyapunov函數(shù)：

對V求導(dǎo)可得

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

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

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

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

1）方法1

采用本文所設(shè)計的控制器，控制器的主要設(shè)計參數(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所列路徑點生成。AUV的初始位姿和速度均為零。期望航速ud＝1m/s，路徑切換的接受圓半徑Rk＝10m。在仿真中設(shè)置了一個障礙物，其半徑為5m，在固定坐標(biāo)系中的坐標(biāo)為：PA＝［ξA，ηA，ζA］T＝［85，0，21］Tm。仿真中選擇從右側(cè)避障，懲罰項按式（32）進(jìn)行設(shè)置：

表1 路徑點Table 1 Way points m

2）方法2

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

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

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

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

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

圖5 速度曲線Fig.5 Speed curves

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

5 結(jié) 論

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

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

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

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

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