基于MPC導引律的AUV路徑跟蹤和避障控制

姚緒梁，王曉偉，2，*

（1.哈爾濱工程大學 自動化學院，哈爾濱150001； 2.九江職業技術學院 機械工程學院，九江332007）

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

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

本文針對欠驅動AUV三維直線路徑跟蹤和避障控制，基于級聯控制理論設計了運動學和動力學控制器。在運動學控制器中考慮了AUV角速度存在的約束，應用MPC設計了導引律，負責產生期望的角速度信號。為了實現對期望速度信號的跟蹤，在動力學控制中考慮了推進器轉速和舵角的飽和，應用SMC設計了動力學控制器，可以有效地克服模型的不確定性，保證系統的魯棒性。

1 欠驅動AUV運動模型

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

圖1 欠驅動AUV三維直線路徑跟蹤示意圖Fig.1 Schematic diagram of underactuated AUV 3D straight path following

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

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

為了便于建立路徑跟蹤誤差模型，以當前直線路徑的起點pk為原點定義坐標系｛F｝：PxFyFzF。｛F｝坐標系的xF軸與直線路徑重合并指向下一個路徑點pk+1，yF軸指向右側，zF軸指向下方。定義｛F｝坐標系相對于｛I｝坐標系的姿態為AF＝［θFψF］T，其計算公式為

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

式中：下標e表示誤差。

對式（4）求導可得

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

對式（6）求導可得

3 三維直線路徑跟蹤控制器的設計

控制器分為運動學和動力學控制器兩部分，首先在運動學控制器中應用MPC設計了導引律，負責產生期望的縱傾角速度qd和艏搖角速度rd。然后，應用SMC設計了動力學控制器，負責產生實際的控制信號，即推進器的轉速np、水平舵角δs和方向舵角δr。

3.1 運動學控制器

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

式中：T1＞0、T2＞0為可調時間常數。

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

其中：T為采樣周期。

根據預測模型（11），未來的狀態變量可以通過式（12）進行預測：

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

式中：

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

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

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

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

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

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

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

3.2 動力學控制器

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

式中：

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

為了對輸入飽和進行補償，設計如下輔助系統：

式中：

定義誤差變量：

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

定義滑模函數：

因為AUV的期望航速ud被設置為常值，所以系統可以近似為自治的，可以認為du、dq、dr有界且慢時變，其導數可以忽略。定義誤差變量：

定義Lyapunov函數：

對V求導可得

設計控制律和自適應律為

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

4 仿真結果與分析

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

1）方法1

采用本文所設計的控制器，控制器的主要設計參數為：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。在仿真中設置了一個障礙物，其半徑為5m，在固定坐標系中的坐標為：PA＝［ξA，ηA，ζA］T＝［85，0，21］Tm。仿真中選擇從右側避障，懲罰項按式（32）進行設置：

表1 路徑點Table 1 Way points m

2）方法2

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

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

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

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

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

圖5 速度曲線Fig.5 Speed curves

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

5 結 論

1）本文基于級聯控制理論分別應用MPC和SMC設計了運動學和動力學控制器，從而實現了欠驅動AUV三維直線路徑跟蹤和避障控制。

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

3）運動學控制器考慮了期望角速度的約束，可以有效地改善舵角的飽和現象，從而更利于系統的穩定性和節能。

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

接下來將針對存在海浪、海流等環境干擾下的欠驅動AUV三維路徑跟蹤進行進一步的研究。