基于隱式離散化超螺旋算法的水下機器人姿態控制

丁寧寧，唐元貴，姜志斌

(1.中國科學院中沈陽自動化研究所中機器人學國家重點實驗室，遼寧 沈陽 110016；2.中國科學院機器人與智能制造創新研究院，遼寧 沈陽 110169；3.遼寧省水下機器人重點實驗室，遼寧 沈陽 110169；4.中國科學院大學，北京 100049)

0 引 言

海洋中深度大于 6 500 m 的海域被稱為“深淵”，深淵區域大深度、超高壓的工作環境對于水下機器人是十分嚴峻的挑戰，因此人類對深淵的認知還在初級階段[1]。面向深淵探測，全海深自主遙控水下機器人(ARV) 應運而生。ARV 作為一種混合型水下機器人，其融合了自主水下機器人(AUV)和遙控水下機器人(ROV)的部分技術。相較于傳統 ROV，ARV 的作業范圍增大、對母船支持要求降低但作業能力也相應減弱。相較于AUV，其特點為巡航作業時可進行實時監控，一旦發現興趣點，即可切換為 ROV 模式利用機械手完成作業[2]。作業能力是 ARV 的一項重要能力，當ARV 進行懸浮作業時，由于機械臂對載體的耦合作用，會導致載體姿態發生較大變化，反過來又會影響機械臂作業。因此載體在機械臂擾動下的姿態穩定控制對于順利完成水下作業十分重要。

目前已經有很多控制算法被應用于水下機器人的控制中，如 PID 控制[3]、預測控制[4-5]、模糊邏輯控制[6-7]、基于神經網絡的控制[8-9]、滑模控制(SMC)[10-14]等。其中滑模控制對滿足匹配條件的干擾和攝動具有較強魯棒性，且算法簡單易于工程實現，在水下機器人上得到了廣泛應用。SMC的主要缺點在于存在抖振問題。對于水下機器人而言，抖振會增加推進器能耗甚至損壞推進器。在抖振抑制方面，高階滑模控制近年來受到了廣泛關注，其將不連續項隱藏到高階導數中，經過積分器的濾波作用，抖振得到了很大抑制。高階滑模中的超螺旋算法 (Super-Twisting Algorithm，STA) 是其中最流行的算法，因為其不需要知道滑模變量的導數信息。STA 在保持滑模控制魯棒性的同時具有良好的抖振抑制能力，然而它的離散實現多采用顯式歐拉法，其在無擾動時也會引發額外的抖振現象，稱為離散化抖振，而且離散化抖振會隨著采樣周期或控制增益值的增大而加劇[15]。為了抑制SMC采用顯式離散化引發的抖振，隱式(后向)歐拉離散法近年來得到了發展[16]，理論和實驗都驗證了其可有效抑制離散化抖振[17-18]。隱式法最近也被用到了STA上[19-20]，仿真與實驗結果顯示基于隱式離散化的STA有效抑制了離散化抖振且對采樣周期和過高的控制增益不敏感。

為了穩定機械臂擾動下的載體姿態，本文首次將隱式離散化后的STA[19]應用到全海深ARV在懸浮作業時的姿態穩定控制上。

1 系統建模

ARV的運動學和動力學模型涉及到2個坐標系，一個固定的大地坐標系{e}用于確定ARV在流場中的位置，一個體坐標系{b}固定于ARV上并隨其一起運動, 如圖1所示。

圖1 ARV坐標系Fig.1 The coordinate frames of ARV

1）橫傾力矩方程

ARV六自由度動力學模型在文獻[21]中進行了詳細的說明。由于機械臂運動主要影響的是橫傾和縱傾這2個自由度，因此僅對這2個自由度的控制進行研究。考慮機械臂擾動，未知環境擾動和未建模動力學后的縱傾和橫傾自由度的運動學和動力學模型如下：

2）縱傾力矩方程

其中：m為載體的質量,W為重力大小，B為浮力大小,[xGyGzG]T表示重心在體坐標系中的坐標，浮心坐標為[xByBzB]T, [IxIyIz]T和 [IxyIyzIzx]T分別表示慣性矩和慣性積，φ 表示橫傾角， θ表示縱傾角，[p q r]T表示姿態角速度， δs表示舵角， Δfφ和 Δfθ為橫傾和縱傾通道中模型不確定性項和未建模動力學項，uφ和uθ和為施加在xb軸和yb軸 上的廣義力矩，dm,φ和dm,θ為機械臂擾動項在xb軸和yb軸 上的分量，du,φ和du,θ為施加在xb軸和yb軸上的未知時變擾動。

由于橫傾自由度和縱傾自由度分析完全類似，因此這里僅取橫傾自由度進行分析。為了便于控制器設計，式(1)可簡化為：

其中：

聯立式(5)和式(2), 可以得到慣性坐標系下橫傾自由度的動力學方程：

其中：

為了便于控制器設計，橫傾和縱傾自由度的動力學方程不失一般性地可寫成如下通用形式：

其中x可表示橫傾角φ或縱傾角 θ變量。

2 控制器設計

2.1 連續域下的控制器設計

為了得到STA的離散化形式，需要回顧STA在連續域下的設計。為了在水下機器人這個2階系統上應用STA，首先要進行滑模面設計，使得控制輸入出現在滑模變量的1階導上。為了簡便，這里選擇如下的線性滑模面：

其中：e=x-xd，xd為狀態變量的期望值， λ為線性滑模面的增益，影響誤差的收斂速率。

滑模趨近率采用STA：

其中：k1，k2為STA控制增益，均為正常數。

對滑模變量微分可得:

比較式(11)和式(10), 在連續域下的控制器可設計為：

為了測試算法的魯棒性以及降低控制器對模型的依賴，控制器并沒有使用名義動力學模型信息。連續域下閉環系統的穩定性證明可參考文獻[22]。

2.2 控制算法的隱式離散化

為了抑制離散化引發的抖振，采用隱式歐拉法對式(12)進行離散化。

對于式(11)，其可改寫成如下緊湊的形式：

式(13)的離散化模型為:

其中：h為采樣周期；fk為f(tk)的簡化表示；和分別為和的近似。對于簡單的顯式歐拉法有

針對式(14), 采用隱式歐拉法的STA控制器可設計為[19]：

不同于顯式歐拉法使用的單值符號函數，式(15)的 sg n()為集值符號函數，即當x=0,s g n(x)∈[-1,1]而不是等于0。為了求解需要引入一些凸分析的概念。定義K?Rn為一個非空閉凸集，它在x?Rn上的正交錐定義為:

對于x?[-1,1]，有

根據凸分析，N[-1,1](x)為集值符號函數的逆函數，即x∈sgn(y)?y∈N[-1,1](x)。

將控制輸入式(15)代入式(18)中，可得到無擾動虛擬閉環系統：

式(19)可寫成如下的等價形式：

注意到 s gn(0)=[-1,1]，因此式(20)的確是集值的，且是一個含有未知變量的廣義方程。下面證明在t=tk時具有解析解，并且是唯一的。

廣義方程式(20)可寫成如下緊湊的形式：

最終可得到如下廣義方程：

定理1：廣義方程式(23)和式(21)僅有唯一解

證明：注意到函數g()和f()均是連續的.根據文獻[23]中推論2.2.5可得到式(23)至少有一個解, 而且解集是緊集。定義式(23)等價于

可以驗證對于所有x≠ck，有當x=ck，即f()是一個單調遞增函數，且x=ck時其函數值和導數值均為0，故可大致畫出其圖形，如圖2所示。根據文獻[23]中命題2.3.2和定理2.3.3,可得到廣義方程式(23)總有唯一解，這意味著集值映射和單值映射的曲線在圖2中有唯一的交點。

圖2 廣義等式(24)的圖解Fig.2 Graphical interpretation of the generalized equation (24)

由于式(21)中的2個映射為式(23)中相應副本的逆，因此它們的交點也是唯一的。這意味著在t=tk時刻,是唯一的，因此式(15)中定義的控制器也是唯一的。為了求解根據圖2可做出如下分類：

3 仿真實驗

為了驗證所提出控制算法的有效性和優越性，在Matlab/Simscape環境下進行仿真實驗。

3.1 任務描述

仿真中載體和機械臂的部分參數如表1所示。為了問題的簡化及排除其他因素的影響，仿真中忽略推進器動力學特性，傳感器獲取的信號假設是理想的。在仿真過程中，機械臂進行運動，規劃的關節軌跡如3所示。與此同時載體則依靠自身的控制維持姿態的穩定。控制器分別采用顯式歐拉法和隱式歐拉法離散的STA，且僅考慮橫傾和縱傾這2個自由度的控制。其中載體的初始位姿和速度均設置為0，縱傾角和橫傾角的目標值也設置為0。為了測試控制算法的魯棒性，模型參數假設存在30% ～ 40%的不確定性，施加的時變外部擾動如下：

表1 ARV部分參數值Tab.1 Partial parameter values of the ARV

為了研究采樣周期對2種離散化方法的影響，仿真中的采樣周期分別設置為0.001 s，0.01 s和0.1 s。

圖3 規劃的關節軌跡Fig.3 Planned joint trajectories

3.2 控制參數優化

為了排除參數選擇對控制性能的影響，確保對比仿真實驗的公平性，采用多目標優化算法進行控制參數調優。

為了評價控制參數的優劣，引入如下2個目標函數：

第1個目標函數計算誤差的均方根，用來評價姿態的穩定控制性能；第2個目標函數計算了控制輸入的平均變化值，可用來衡量控制輸入的抖振大小。這2個目標函數值都是越小越好。

待優化的參數是 λ，k1和k2這3個參數，即它們將作為多目標優化的設計變量。為了提升效率，先手動調參確定大致的范圍。控制參數整定范圍為：

為了避免推進器飽和，控制輸入也限定在一定范圍內：uφ∈[-250,50]，uθ∈[-70,230]。

多目標優化采用Matlab中Global Optimization Toolbox中的gamultiobj函數實現。

3.3 仿真結果與分析

圖4為不同采樣周期下經多目標優化得到的Pareto前沿, Pareto前沿上的點稱為Pareto最優解。可以發現隱式法對應的Pareto最優解CHAT值總體來看遠小于顯式法，且集中在一個較小值, 這表明隱式法有效地抑制了離散化抖振。顯式法的Pareto最優解CHAT值則分布在一個較大范圍內，表明顯式法容易引發較大的抖振。對比RMSE值可發現顯式法可達到更小的值，代價是產生了更大的抖振。對比不同采樣周期的Pareto前沿，發現隨著采樣周期的增加，顯式法和隱式法對應的Pareto前沿均向右移動，然而隱式法Pareto最優解的CHAT值增加較小，仍然集中在一個較小值。顯式法Pareto最優解對應的CHAT值分布范圍明顯擴大，這說明顯式法對采樣周期的增加較敏感，采樣周期的增加明顯增大了離散化抖振。

圖4 不同采樣周期對應的Pareto前沿Fig.4 Pareto fronts for different sampling time

為了分析2種離散化方法對增益的敏感性，圖5給出了采樣時間周期為0.1 s時得到的Pareto最優解對應的設計變量值分布圖，即控制器參數值分布圖。從圖5可以發現，顯式法的滑模面增益λ集中在較大處；對比隱式法對應的滑模面增益分布和Pareto前沿RMSE值的分布，可發現滑模面增益越大，RMSE值越小。對于STA趨近律相關增益k1和k2，顯式法只能取很小的值，否則便會引發劇烈抖振甚至使系統不穩定。這說明顯式法對較大增益很敏感，這會導致手動調參變得困難。隱式法則可在整個限定區間取值，可取較大增益值，說明隱式法對較大增益不敏感，控制參數的調優更加容易。

圖5 設計變量分布圖Fig.5 The scattergram of design variables

為了更加直觀地展示隱式法相對顯式法的區別和優勢, 在采樣周期0.1s對應的 Pareto 前沿上選擇最優設計點進行仿真實驗。為了確保公平的對比，在 RMSE值相似的情況下選取對應的最優點，選取的 Pareto 最優設計點對應的目標函數值如表2所示，相應的設計變量值如表3所示。

表2 選取的最優設計點目標函數值Tab.2 Objective function values for selected optimum design points

表3 選取的設計變量值Tab.3 The selected design variable values

圖6為橫傾角和縱傾角隨時間變化曲線圖。由于選擇了相似的RMSE值，可看到兩者的控制性能大致相當。顯式法對應的姿態角在初始階段振蕩較大，不過可實現較小的穩態誤差，隱式法對應的姿態角則比較平滑，但穩態誤差較大，不過根據RMSE的性能指標，兩者可認為大致相當。圖7為控制輸入隨時間變化曲線圖，從圖中的放大視圖可以發現顯式法對應的控制輸入存在明顯的抖振現象，隱式法則得到平滑的的控制輸入。

圖6 橫傾角和縱傾角隨時間變化曲線Fig.6 Time trajectories of the roll angle and pitch angle

圖7 控制輸入隨時間變化曲線Fig.7 Time trajectories of control inputs

4 結 語

本文針對具有作業能力的全海深ARV懸浮作業時載體的姿態控制問題提出了一種隱式離散化廣義超螺旋算法，并基于多目標優化算法完成了控制器參數優化。仿真結果表明，所提出的算法有效抑制了STA離散化帶來的抖振，且控制算法對采樣周期和過高控制增益不敏感。未來的工作包括將隱式離散化方法擴展到廣義超螺旋算法[24]以及進行水池實驗。