陳剛，施良才，張晶晶，施建偉

（1.浙江理工大學(xué)機(jī)械工程學(xué)院，杭州 310018；2.浙江凱富博科科技有限公司，金華 321035）

1 引 言

21世紀(jì)是海洋的世紀(jì)，海洋擁有豐富的資源，對人類社會發(fā)展起著至關(guān)重要的作用，國家也傾注了很多資源在海洋探索上。潛水器是海洋探索中非常重要的工具，使得海洋探索向更深更廣的領(lǐng)域進(jìn)展。常說的水下機(jī)器人主要分為ROV（Remotely Operated Vehicle）和AUV（Autonomous Underwater Vehicle）。除了這兩種，還有一種為水下步行機(jī)器人，能夠完成ROV和AUV不能完成的任務(wù)。足式具有越障能力強(qiáng)的優(yōu)勢，更加適合水底環(huán)境［1-2］。并且六足在行走中是采用三條腿站立，該運(yùn)動的模式具有良好的穩(wěn)定性［3-5］。對于海洋探索來說，海流會對機(jī)器人的穩(wěn)定性造成巨大的影響。水下六足機(jī)器人可以根據(jù)海流的變化調(diào)整自身姿態(tài)，以減少受到的影響，提高穩(wěn)定性。因此，首要解決的問題是如何保證水下機(jī)器人的姿態(tài)精確控制，以保證機(jī)器人的穩(wěn)定性?；？刂朴捎隰敯粜詮?qiáng)、對干擾不敏感、對于水下復(fù)雜環(huán)境的適應(yīng)性更好的特性而受到了廣泛的關(guān)注。

滑?？刂剖且环N非線性控制方法，其通過在滑模面不斷來回穿梭，使得系統(tǒng)誤差逐漸收斂到0，因此對外界干擾不敏感。滑?？刂剖窃谙到y(tǒng)漸進(jìn)穩(wěn)定的前提下設(shè)計出來，為了加快系統(tǒng)收斂速度，Zak等［6］提出了終端滑模，大大加快收斂速度，實現(xiàn)了有限時間收斂。但終端滑模存在奇異現(xiàn)象，F(xiàn)eng等［7-9］提出的非奇異終端滑模解決了奇異問題。高階滑?？刂仆ㄟ^設(shè)計連續(xù)的控制率，解決了傳統(tǒng)滑模因符號函數(shù)的高頻切換帶來的抖振［10-13］。在水下機(jī)器人領(lǐng)域，Mobayen等［14］提出了二階滑?？刂?，使得水下機(jī)器人的下潛深度得到了良好的控制。Zhou等［15］提出了基于Hurwitz的滑?？刂品椒?，通過Hurwitz矩陣確定最佳滑模面，大大削弱了抖振。Ding等［16］通過將模糊和滑?？刂葡嘟Y(jié)合的方式，使得水下機(jī)器人能夠在波浪的干擾下保持穩(wěn)定。Hong等［17］提出了一種新型滑模控制方法，使得浮力對水下機(jī)器人的下潛深度和俯仰角影響大大降低。

雖然滑?？刂品椒ㄒ延写罅垦芯?，但這些方法在水下足式機(jī)器人的應(yīng)用上能否得到良好的控制效果，尚無明確的研究。因此本文針對水下六足機(jī)器人提出了一種基于非奇異終端滑?？刂品椒?。

2 六足機(jī)器人運(yùn)動學(xué)模型分析

本文設(shè)計的水下機(jī)器人由機(jī)器人機(jī)體和六條腿組成，六條腿呈3-3制對稱布局，如圖1（a）所示，圖中數(shù)字為各條腿的編號。每條腿由根關(guān)節(jié)、髖關(guān)節(jié)、膝關(guān)節(jié)三個關(guān)節(jié)構(gòu)成，腿的末端采用球形結(jié)構(gòu)。機(jī)體由長方體框架和圓筒構(gòu)成，電氣元件置于密封的圓筒中，圓筒的引出線纜為上位機(jī)的控制線纜和電源線纜。機(jī)器人具有6個自由度，即3個平動、3個轉(zhuǎn)動，保證機(jī)器人可以任意控制機(jī)體姿態(tài)。機(jī)器人質(zhì)量主要集中于機(jī)體上，腿的質(zhì)量遠(yuǎn)小于機(jī)體質(zhì)量，并且機(jī)器人六條腿為均勻?qū)ΨQ分布，因此可以認(rèn)為機(jī)器人的重心和形心重合。

圖1 機(jī)器人模型圖Fig.1 Robot model diagram

2.1 水下六足機(jī)器人D-H建模

水下六足機(jī)器人的每條腿可以看作三自由度的三連桿機(jī)構(gòu)，機(jī)體可以看作長方體框架和圓筒的組合。如圖2所示，建立世界坐標(biāo)系{W}、機(jī)器人機(jī)體坐標(biāo)系{B}，其中世界坐標(biāo)系{W}與大地固定不動，機(jī)器人機(jī)體坐標(biāo)系{B}與機(jī)器人本體固連。機(jī)器人機(jī)體坐標(biāo)系{B}的坐標(biāo)原點(diǎn)位于機(jī)器人重心，x軸指向3號腿并與機(jī)體橫軸平行，z軸垂直向上于機(jī)體，y軸由右手定則確定。機(jī)器人腿部根關(guān)節(jié)坐標(biāo)系{Uj0}與機(jī)器人機(jī)體坐標(biāo)系{B}平行，位于每條腿的根關(guān)節(jié)處。并且按照D-H建立腿部坐標(biāo)系{Uj1}、{Uj2}、{Uj3}，j=1，2，3，4，5，6。由于腿1、2坐標(biāo)系建立方法相同，腿3～6坐標(biāo)系建立方法相同，所以圖2用腿2和腿5的坐標(biāo)系來代表其他坐標(biāo)系。水下六足機(jī)器人各腿的D-H參數(shù)如表1～2所示。

表1 腿1、2的D-H參數(shù)Table 1 D-H parameters of legs 1 and 2

表2 腿3、4、5、6的D-H參數(shù)Table 2 D-H parameters of legs 3,4,5 and 6

圖2 水下六足機(jī)器人D-H模型Fig.2 D-H model of underwater hexapod robot

2.2 水下六足機(jī)器人正運(yùn)動學(xué)

機(jī)器人正運(yùn)動學(xué)是通過機(jī)器人每條腿關(guān)節(jié)角去求解每條腿的末端位置。根據(jù)圖2的D-H模型和建立的坐標(biāo)系，假設(shè)已知每條腿的根關(guān)節(jié)原點(diǎn)坐標(biāo)大腿和小腿長度l2、l3，j腿的各個關(guān)節(jié)角度、、。那么可以得到坐標(biāo)系之間的齊次變換矩陣。

為了書寫方便，定義sθj，i=sinθj，i，cθj，i=cosθj，i，i=1，2，3，下同。坐標(biāo)系{U1j}相對于坐標(biāo)系{U0j}的齊次變換矩陣為

坐標(biāo)系{U2j}相對于坐標(biāo)系{U1j}的齊次變換矩陣為

坐標(biāo)系{U3j}相對于坐標(biāo)系{U2j}的齊次變換矩陣為

那么坐標(biāo)系{U3j}相對于坐標(biāo)系{U0j}的齊次變換矩陣便可由如下公式得到

式中，θj，2，3=(θj，2+θj，3)。

假設(shè)末端E點(diǎn)的位置為3PEj=[l30 0 1]T，根據(jù)坐標(biāo)變換理論，得到0PEj的位置為

假設(shè)末端Ej以機(jī)器人機(jī)體坐標(biāo)系{B}為參考系，那么其位置為

3 非奇異終端滑模控制器設(shè)計以及姿態(tài)閉環(huán)控制

3.1 水下六足機(jī)器人姿態(tài)控制模型

本文的水下六足機(jī)器人姿態(tài)控制模型建立在機(jī)器人的腿落足于海底陸地上，基于第2節(jié)得到的機(jī)器人D-H模型，可以得到機(jī)器人第j條腿的末端在世界坐標(biāo)系{W}的位置為

式中，WBR為機(jī)器人坐標(biāo)系{B}相對于世界坐標(biāo)系的三階旋轉(zhuǎn)矩陣，為坐標(biāo)系{Uj0}相對于機(jī)器人機(jī)體坐標(biāo)系{B}的旋轉(zhuǎn)矩陣，為第j腿的末端位置在坐標(biāo)系{U0j}中的表示。由于坐標(biāo)系{U0j}與機(jī)器人機(jī)體坐標(biāo)系{B}坐標(biāo)軸平行，式（7）可改寫為

對式（8）左右兩邊對時間求導(dǎo)可得

當(dāng)機(jī)器人的腿落足于海底陸地上時，即支撐狀態(tài)，其末端位置不變，即=0，并且坐標(biāo)系{Uj0}為固定坐標(biāo)系，則=0，那么式（9）可以改寫為

由式（7）和式（8）可以得到

將式（11）代入式（10）可以得到

可以由第j條腿關(guān)節(jié)角速度ωqj表示為

式中，Jj為j條腿的雅可比矩陣。

可以表示為機(jī)器人姿態(tài)角速度與旋轉(zhuǎn)矩陣WBR的叉乘

式中，H將世界坐標(biāo)系{W}上的姿態(tài)角速度轉(zhuǎn)換到機(jī)器人坐標(biāo)系{B}上

將式（13）和式（14）代入式（12）可以得到

式中，S()為反對稱矩陣。

由此得到第j腿的姿態(tài)控制模型，并推廣到機(jī)器人整體，得到水下六足機(jī)器人姿態(tài)控制模型為

式中

φ為落足支撐腿的數(shù)量。

3.2 基于非奇異終端滑模的水下六足機(jī)器人的姿態(tài)控制方法

首先定義水下六足機(jī)器人姿態(tài)誤差eζ為

式中，ζact=[γactβactαact]T為機(jī)器人的實際姿態(tài)，ζexp=[γexpβexpαexp]T為機(jī)器人的目標(biāo)姿態(tài)。

然后，建立非奇異終端滑模面為

式中，s=［s1，s2，s3］T，c=diag（c1，c2，c3），a=diag（a1，a2，a3），aj、cj均為正實數(shù)，pj、qj均為正奇數(shù)，且pj/qj>1，j=1，2，3，eζ=[eζ1，eζ2，eζ3]T。

采用雙冪次趨近律結(jié)合負(fù)指數(shù)項的吸引子組成新的趨近律為

式中，rj、gj為正奇數(shù)，且rj/gj>1，oj、uj為正奇數(shù)，且0

本文提出的趨近律可以分三個部分：k1|s|r/gsign(s)、k2s和k3|s|o/usign(s)，這三部分在趨近的不同階段趨近速率不同，如圖3所示。由圖可以知道當(dāng)s>1時，k1|s|r/gsign(s)趨近速度明顯比另外兩段快，該趨近律起主要作用，當(dāng)s<1時，k3|s|o/usign(s)趨近速度快于另外兩段，其起主要作用。而k2s這段不管在何種階段，趨近速度都介于兩者之間，起到加快和穩(wěn)定作用。并從圖3可知，三段結(jié)合的趨近律在任何階段的趨近速度均快于任意一種或者兩種的結(jié)合。

圖3 不同階段三段趨近律趨近速率對比Fig.3 Comparison of three stage approach rate

對式（20）兩邊求導(dǎo)可以得到

將式（22）和式（21）聯(lián)立可得

那么經(jīng)過非奇異終端滑模修正的機(jī)器人姿態(tài)角速度可以表示為

將式（23）代入式（24）可得

將式（17）改寫為

式（26）中的可由式（25）得到，因此水下六足機(jī)器人姿態(tài)控制模型可以表示為式（26）。

本文所提出基于非奇異終端滑模水下六足機(jī)器人控制系統(tǒng)，其姿態(tài)誤差能夠在有限時間內(nèi)快速趨近于0，證明如下：

采用Lyapunov函數(shù)

對式（27）兩邊求導(dǎo)可以得到

由于r、g、o、u為正奇數(shù)，則|s|r/gsign(s)sT=sr+g/g>0，|s|o/usign(s)sT=so+u/u>0，因此k1|s|r/gsign(s)sT+k2ssT+k3|s|o/usign(s)sT>0，得到<0。所以根據(jù)李雅普諾夫穩(wěn)定性判據(jù)可知本文所設(shè)計的雙冪次趨近律的非奇異終端滑?？稍谟邢迺r間內(nèi)收斂到0，則水下六足機(jī)器人的姿態(tài)控制誤差可在有限時間內(nèi)趨于0。因此，本文所設(shè)計的雙冪次趨近律的非奇異終端滑?？刂破鳚u進(jìn)穩(wěn)定。

4 仿 真

為驗證所設(shè)計的水下六足機(jī)器人姿態(tài)閉環(huán)控制算法的有效性和精確性，利用MATLAB/SIMULINK和ADAMS聯(lián)合仿真的方式對第3節(jié)提出的基于非奇異終端滑模姿態(tài)閉環(huán)算法進(jìn)行仿真驗證。本文所設(shè)計的水下機(jī)器人主要在海底活動，其質(zhì)量相對較大，使其能夠穩(wěn)定地站在海底上。本文主要研究的是水下六足機(jī)器人的姿態(tài)精確控制問題，在姿態(tài)控制過程中，機(jī)器人的每條腿的運(yùn)動比較緩慢。其運(yùn)動產(chǎn)生的水阻力比較小，相對于其質(zhì)量來說，產(chǎn)生的影響可以忽略不計。機(jī)器人的體積較大，浮力對機(jī)器人產(chǎn)生的影響較大，尤其在機(jī)器人調(diào)節(jié)姿態(tài)的過程中，浮力可能會使機(jī)器人側(cè)翻。因此在ADAMS機(jī)器人模型中加入浮力，去模擬水下環(huán)境。為了能夠更加真實模擬水下環(huán)境，在MATLAB/SIMULINK中的姿態(tài)閉環(huán)控制模型中加入方差為0，均值為5的高斯噪聲，以此模擬海流。

具體流程如下：先通過ADAMS建立起水下六足機(jī)器人模型，其次通過MATLAB/SIMULINK建立起機(jī)器人的姿態(tài)閉環(huán)模型。MATLAB/SIMULINK使用非奇異終端滑模（NTSMC）姿態(tài)閉環(huán)模型計算出機(jī)器人的各關(guān)節(jié)角度，并將其傳輸?shù)紸DAMS建立的機(jī)器人模型中，機(jī)器人根據(jù)關(guān)節(jié)角運(yùn)動，同時將機(jī)器人姿態(tài)和各個關(guān)節(jié)角、機(jī)器人實際姿態(tài)和機(jī)器人實際位置反饋給MATLAB/SIMULINK，MATLAB/SIMILINK根據(jù)此反饋并基于非奇異終端滑模姿態(tài)閉環(huán)控制模型計算機(jī)器人關(guān)節(jié)角，重復(fù)循環(huán)，從而實現(xiàn)MATLAB/SIMULINK和ADAMS的聯(lián)合仿真。MATLAB/SIMULINK與ADAMS聯(lián)合仿真框圖如圖4所示。本文將通過2種階躍跟蹤和3種正弦跟蹤仿真實驗，并與傳統(tǒng)滑模和PID進(jìn)行對比來驗證設(shè)計的姿態(tài)閉環(huán)控制算法的優(yōu)越性。

圖4 MATLAB/SIMULINK與ADAMS聯(lián)合仿真Fig.4 Joint simulation of MATLAB/SIMULINK and ADAMS

4.1 基于非奇異終端滑模的水下六足機(jī)器人的姿態(tài)階躍響應(yīng)仿真

為了充分驗證水下六足機(jī)器人姿態(tài)控制的快速性和精確性，采用2種機(jī)器人目標(biāo)姿態(tài)（EXP），其分別為[α，β，γ]=[5°，5°，5°]、[α，β，γ]=[3°，3°，3°]，機(jī)器人初始姿態(tài)為[α，β，γ]=[0°，0°，0°]。并且構(gòu)建水下六足機(jī)器人的滑?？刂破鳎⊿MC）和PID控制器進(jìn)行對比，其中滑?？刂破骰Ｃ鎠=creη，趨近律采用冪次趨近律=-k|s|bsign(s)，其中k>0，1

仿真中機(jī)器人運(yùn)動的過程如圖5所示。仿真結(jié)果如圖6和圖7所示。在階躍3°仿真實驗中，基于非奇異終端滑模姿態(tài)控制航向角和橫滾角響應(yīng)時間比采用冪次趨近律的航向角和橫滾角響應(yīng)時間減少0.1～0.2 s，比PID姿態(tài)閉環(huán)的航向角和橫滾角響應(yīng)時間減少1.5 s?；诜瞧娈惤K端滑模姿態(tài)控制的俯仰角比采用冪次趨近律的俯仰角響應(yīng)時間減少0.3～0.4 s，比PID姿態(tài)閉環(huán)俯仰角響應(yīng)時間減少1.5 s?；诜瞧娈惤K端滑模姿態(tài)控制航向角穩(wěn)態(tài)誤差與采用冪次趨近律的航向角穩(wěn)態(tài)誤差相近，遠(yuǎn)小于PID姿態(tài)閉環(huán)航向角穩(wěn)態(tài)誤差，基于非奇異終端滑模姿態(tài)控制的橫滾角和俯仰角穩(wěn)態(tài)誤差小于采用冪次趨近律和PID姿態(tài)閉環(huán)的橫滾角和俯仰角穩(wěn)態(tài)誤差。在階躍5°的仿真實驗中，采用冪次趨近律的階躍響應(yīng)速度有所加快，但仍明顯小于基于非奇異終端滑模姿態(tài)控制的響應(yīng)速度。基于非奇異終端滑模姿態(tài)控制和冪次趨近律的姿態(tài)控制航向角穩(wěn)態(tài)誤差相近，但基于非奇異終端滑模姿態(tài)控制的橫滾角和俯仰角穩(wěn)態(tài)誤差明顯優(yōu)于采用冪次趨近律姿態(tài)閉環(huán)控制和PID姿態(tài)閉環(huán)控制的橫滾角和俯仰角穩(wěn)態(tài)誤差。

圖5 機(jī)器人仿真運(yùn)動圖Fig.5 Robot simulation motion diagram

圖6 階躍3°姿態(tài)曲線Fig.6 Step 3 ° attitude curve

圖7 階躍5°姿態(tài)曲線Fig.7 Step 5 ° attitude curve

通過階躍響應(yīng)仿真結(jié)果可以發(fā)現(xiàn)，基于非奇異終端滑模姿態(tài)閉環(huán)控制器（NTSMC）的響應(yīng)時間明顯小于采用冪次趨近律滑模姿態(tài)閉環(huán)控制器（SMC）和PID姿態(tài)閉環(huán)控制器，并且穩(wěn)態(tài)誤差也得到一定的減小。

4.2 基于非奇異終端滑模的水下六足機(jī)器人的姿態(tài)正弦跟蹤仿真

采用3種正弦跟蹤仿真實驗，驗證水下六足機(jī)器人非奇異終端滑模姿態(tài)控制（NTSMC）的魯棒性。并將水下六足機(jī)器人的滑模控制器（SMC）和PID控制器進(jìn)行對比，其中這些控制器的參數(shù)與階躍仿真中完全一致，正弦跟蹤曲線為

式中，A為幅值數(shù)值為2，ω為角頻率數(shù)值為π/10、π/20以及π/30。

正弦跟蹤仿真結(jié)果如圖8～10所示。在相同幅值的條件下，正弦跟蹤誤差會隨著周期的增加而減小。從圖中可以得到，基于非奇異終端滑模姿態(tài)閉環(huán)控制的姿態(tài)正弦跟蹤誤差始終小于采用冪次趨近律的滑模姿態(tài)控制和PID姿態(tài)閉環(huán)控制的姿態(tài)正弦跟蹤誤差。冪次趨近律的姿態(tài)跟蹤誤差與基于非奇異終端滑模姿態(tài)閉環(huán)控制的姿態(tài)正弦跟蹤誤差的差值會隨著周期的增加而減小，但冪次趨近律的姿態(tài)跟蹤誤差始終大于基于非奇異終端滑模姿態(tài)閉環(huán)控制的姿態(tài)正弦跟蹤誤差?；赑ID的姿態(tài)閉環(huán)控制的正弦跟蹤存在明顯的時滯性?；诜瞧娈惤K端滑模姿態(tài)閉環(huán)控制具有良好的魯棒性和精確性，其正弦跟蹤誤差小于0.1°。

圖8 2sin(πt/10)姿態(tài)曲線Fig.8 Attitude curve of 2sin(πt/10)

圖9 2sin(πt/20)姿態(tài)曲線Fig.9 Attitude curve of 2sin(πt/20)

圖10 2sin(πt/30)姿態(tài)曲線Fig.10 Attitude curve of 2sin(πt/30)

5 結(jié) 論

本文基于提出的機(jī)器人運(yùn)動學(xué)模型，建立了基于落足點(diǎn)的姿態(tài)控制方法，并與非奇異終端滑模控制器結(jié)合起來進(jìn)行了穩(wěn)定性證明，提出了適用于水下六足機(jī)器人的非奇異終端滑模姿態(tài)閉環(huán)控制器。非奇異終端滑模姿態(tài)閉環(huán)控制器采用三段式趨近律使得收斂速度得到進(jìn)一步提升。最后經(jīng)過仿真驗證了非奇異終端滑模姿態(tài)閉環(huán)控制的姿態(tài)跟蹤性能，同時階躍跟蹤和正弦跟蹤仿真表明，其擁有比PID和傳統(tǒng)滑模更快的響應(yīng)速度、收斂速度和更好的跟蹤性能，能夠快速消除姿態(tài)誤差，姿態(tài)誤差始終小于0.1°，響應(yīng)速度提高0.1～0.2 s。仿真充分驗證了所提出的非奇異終端滑?？刂破鲗λ铝銠C(jī)器人姿態(tài)跟蹤的有效性。后續(xù)在仿真實驗基礎(chǔ)上，將對水下六足機(jī)器人開展實物實驗以驗證本文所提出的控制器性能。由于趨近律參數(shù)對整個系統(tǒng)的收斂速度有較大影響，后續(xù)將自適應(yīng)方法與非奇異終端滑模控制器結(jié)合，并根據(jù)系統(tǒng)誤差調(diào)整趨近律參數(shù)，使系統(tǒng)收斂速度得到進(jìn)一步提升。