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

丁寧寧，唐元貴，姜志斌

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

0 引 言

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

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

為了穩(wěn)定機械臂擾動下的載體姿態(tài)，本文首次將隱式離散化后的STA[19]應(yīng)用到全海深A(yù)RV在懸浮作業(yè)時的姿態(tài)穩(wěn)定控制上。

1 系統(tǒng)建模

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

圖1 ARV坐標(biāo)系Fig.1 The coordinate frames of ARV

1）橫傾力矩方程

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

2）縱傾力矩方程

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

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

其中：

聯(lián)立式(5)和式(2), 可以得到慣性坐標(biāo)系下橫傾自由度的動力學(xué)方程：

其中：

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

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

2 控制器設(shè)計

2.1 連續(xù)域下的控制器設(shè)計

為了得到STA的離散化形式，需要回顧STA在連續(xù)域下的設(shè)計。為了在水下機器人這個2階系統(tǒng)上應(yīng)用STA，首先要進行滑模面設(shè)計，使得控制輸入出現(xiàn)在滑模變量的1階導(dǎo)上。為了簡便，這里選擇如下的線性滑模面：

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

滑模趨近率采用STA：

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

對滑模變量微分可得:

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

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

2.2 控制算法的隱式離散化

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 仿真實驗

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

3.1 任務(wù)描述

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

表1 ARV部分參數(shù)值Tab.1 Partial parameter values of the ARV

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

圖3 規(guī)劃的關(guān)節(jié)軌跡Fig.3 Planned joint trajectories

3.2 控制參數(shù)優(yōu)化

為了排除參數(shù)選擇對控制性能的影響，確保對比仿真實驗的公平性，采用多目標(biāo)優(yōu)化算法進行控制參數(shù)調(diào)優(yōu)。

為了評價控制參數(shù)的優(yōu)劣，引入如下2個目標(biāo)函數(shù)：

第1個目標(biāo)函數(shù)計算誤差的均方根，用來評價姿態(tài)的穩(wěn)定控制性能；第2個目標(biāo)函數(shù)計算了控制輸入的平均變化值，可用來衡量控制輸入的抖振大小。這2個目標(biāo)函數(shù)值都是越小越好。

待優(yōu)化的參數(shù)是 λ，k1和k2這3個參數(shù)，即它們將作為多目標(biāo)優(yōu)化的設(shè)計變量。為了提升效率，先手動調(diào)參確定大致的范圍?？刂茀?shù)整定范圍為：

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

多目標(biāo)優(yōu)化采用Matlab中Global Optimization Toolbox中的gamultiobj函數(shù)實現(xiàn)。

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

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

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

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

圖5 設(shè)計變量分布圖Fig.5 The scattergram of design variables

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

表2 選取的最優(yōu)設(shè)計點目標(biāo)函數(shù)值Tab.2 Objective function values for selected optimum design points

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

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

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

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

4 結(jié) 語

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