趙 純，于存貴，何 慶，徐 強，姚建勇

(1.南京理工大學機械工程學院，南京 210094；2.上海宇航系統(tǒng)工程研究所，上海 201109)

0 引 言

運載火箭燃料加注的自動化是實現(xiàn)火箭安全發(fā)射的重要保障，高性能自動加注機器人技術(shù)在國內(nèi)外都得到了廣泛的關注。目前，比較有代表性的是俄羅斯的“架棲”技術(shù)和美國的“箭棲”技術(shù)[1]?！凹軛奔夹g(shù)能實現(xiàn)連接器的自動對接和脫落，但是其加注活門僅能安裝在箭體尾部，環(huán)境適應性較差?！凹龡奔夹g(shù)通過人工方式實現(xiàn)加注裝置的對接，并通過火箭的升力實現(xiàn)強行脫落。該種方式結(jié)構(gòu)簡單，但是其缺少主動對接功能。目前國內(nèi)也取得了一些進展，最新成果是由黃小妮等[2]提出的一種以電驅(qū)SCARA機械臂為依托的“箭架兩棲”加注技術(shù)，其能實現(xiàn)加注連接器的自動對接脫落。然而由于電驅(qū)機械臂的固有特性，其無論是承載力還是響應速度均未達到理想狀態(tài)?？紤]到電液伺服型并聯(lián)式加注機器人具有剛度大、精度高、自重載荷比小和響應速度快等優(yōu)點，其已成為新一代自動加注技術(shù)的理想選項[3]。

然而，由于并聯(lián)機器人本質(zhì)上是一個時變、多變量和強耦合的非線性系統(tǒng)，其高性能控制器設計仍是一個難點。目前關于多關節(jié)機器人的控制方法主要可分為兩類：集中控制策略和分散控制策略。集中控制策略是指一類基于系統(tǒng)動力學模型進行算法設計的控制方法。文獻[4]針對氣動并聯(lián)平臺提出了一種自適應魯棒控制器，其通過自適應方法實現(xiàn)了對動力學模型中緩變不確定的在線估計。文獻[5-7]針對空間機器人/機械臂，提出了一類神經(jīng)網(wǎng)絡控制方法。其通過神經(jīng)網(wǎng)絡在線估計和補償系統(tǒng)動力學模型中的不確定，并通過魯棒控制器抑制動力學擾動觀測誤差。然而，雖然集中控制策略在仿真或?qū)嶒炇噎h(huán)境下取得了良好的軌跡跟蹤性能，但是其對于系統(tǒng)精確模型的依賴性，以及復雜的模型解算，使得該類方法在實際場合中的應用開展并不明朗。

為了簡化控制器設計，分散控制策略將多關節(jié)機器人分解為一組受外部干擾的多軸系統(tǒng)，進而將控制器設計任務簡化為針對各關節(jié)設計高性能控制器。該方法的關鍵在于如何有效處理關節(jié)間的強耦合動力學干擾。文獻[8-9]分別使用神經(jīng)網(wǎng)絡和模糊邏輯控制器來逼近關節(jié)中的強外部擾動，但是其復雜的在線計算未從本質(zhì)上簡化控制器設計。為了高效地處理各關節(jié)所受耦合外部干擾，干擾觀測器(Disturbance observer,DOB)和擴張狀態(tài)觀測器(Extended state observer,ESO)在分散控制中得到了廣泛應用。文獻[10]提出了一種基于DOB的自適應魯棒分散控制策略，并將其應用到一種二自由度平面并聯(lián)機器人的研究中，但是其未考慮驅(qū)動器的動態(tài)特性。文獻[11]針對液壓驅(qū)動Stewart平臺提出了一種分散級聯(lián)控制方案，其通過設計DOB實現(xiàn)了對關節(jié)中外干擾的在線估計，但是其假定外干擾的變化率近似于零，這在實際應用場合中具有較大的局限性。文獻[12]針對一種Stewart平臺，提出了一種基于關節(jié)PD控制器和ESO干擾觀測器的并聯(lián)機器人分散控制策略，但是其并未對系統(tǒng)的穩(wěn)定性進行理論證明。杜艷麗等[13]將ESO與動態(tài)面反演技術(shù)結(jié)合，研究了一種可重構(gòu)模塊機器人的反演分散控制，其克服了反演控制中控制器微分膨脹的問題。其它類似的多關節(jié)機器人分散控制策略可參考文獻[14-15]。

在實際加注任務中，執(zhí)行機構(gòu)需同時承載柔順對接機構(gòu)、加注管路和液體燃料的質(zhì)量，故而難以獲得其精確的數(shù)學模型；另外，對接誤差也將使得箭體對加注機器人施加一個外部干擾力。因此，考慮到控制器的工程可實現(xiàn)性，無需系統(tǒng)完整動力學模型的執(zhí)行機構(gòu)分散控制策略顯然更適合于實際的加注任務。

但是，現(xiàn)有分散控制策略中，無論DOB型或ESO型觀測器都只能實現(xiàn)對關節(jié)擾動誤差的有界估計，同時控制器均只實現(xiàn)了對關節(jié)中匹配不確定的有效估計和補償，而對引入驅(qū)動器后產(chǎn)生的關節(jié)不匹配不確定性問題，現(xiàn)有成果均未加以考慮。為了解決擾動觀測器的快速收斂問題，文獻[16]提出了一種有限時間收斂干擾觀測器(FTDO)。目前FTDO技術(shù)已經(jīng)得到了較為廣泛的研究和應用。文獻[17]基于FTDO研究了一類二階系統(tǒng)的有限時間跟蹤控制問題。文獻[18]通過融合自適應魯棒控制和FTDO觀測器研究了單軸液壓驅(qū)動器的軌跡跟蹤控制問題，但是其對驅(qū)動器的匹配不確定只通過自適應的方法加以估計，該種方法對于匹配不確定中可能存在快速時變干擾并不適用。文獻[19]結(jié)合滑?？刂品椒ê虵TDO技術(shù)針對一類機器人的有限時間控制方法進行了研究，但是該控制器基于機器人動力學模型展開，且未考慮驅(qū)動器的動態(tài)特性。

基于以上分析，本文針對并聯(lián)式加注機器人的空間軌跡跟蹤控制問題，基于分散控制的思想，通過融合有限時間干擾觀測器(FTDO)和魯棒控制方法，提出了一種系統(tǒng)全局有限時間收斂的加注機器人動力學解耦反步魯棒控制策略。首先通過結(jié)合位置反解模型和軌跡生成技術(shù)，實現(xiàn)了系統(tǒng)基于工作空間和關節(jié)空間的期望軌跡映射；其次，將執(zhí)行機構(gòu)進行關節(jié)分散化處理，并通過在各關節(jié)中引入兩個FTDO，實現(xiàn)了對關節(jié)中匹配與不匹配不確定的有限時間估計和補償；同時，基于系統(tǒng)的動力學解耦模型，通過設計反步魯棒控制器從理論上保證了系統(tǒng)的穩(wěn)定性，并通過Lyapunov方法證明了系統(tǒng)跟蹤誤差的有限時間收斂。最后通過試驗驗證了本文控制器的有效性。

1 系統(tǒng)建模與問題描述

1.1 系統(tǒng)總體布局

對于受風載作用的火箭箭體，其運動主要體現(xiàn)在三個平動自由度，而在三個轉(zhuǎn)動自由度上的運動量較小(不大于±0.5°)[1]。針對此特點，自動加注機器人可采用三平動并聯(lián)執(zhí)行機構(gòu)串聯(lián)六自由度柔順對接機構(gòu)的混聯(lián)結(jié)構(gòu)方案，其總體結(jié)構(gòu)如圖1所示。兩者的具體功能可以劃分為：

圖1 運載火箭并聯(lián)式加注機器人整體示意圖Fig.1 Schematic diagram of hydraulic parallel filling robot for launch vehicle

1)執(zhí)行機構(gòu)采用電液伺服型3-PSS并聯(lián)機器人，其功能是實現(xiàn)柔順對接機構(gòu)與箭體在空間三個平動自由度上的對接、隨動和脫落。關于3-PSS機構(gòu)的具體結(jié)構(gòu)原理可參考文獻[20]。

2)柔順對接機構(gòu)采用彈簧支鏈4-CCC型的結(jié)構(gòu)方案，其主要功能是起到執(zhí)行機構(gòu)動平臺與地面連接器面板在空間六自由度上的柔順連接作用。關于柔順對接機構(gòu)的具體結(jié)構(gòu)原理可以參考文獻[1]。

1.2 3-PSS機構(gòu)位置反解建模

參考文獻[20]，可建立如圖2所示的執(zhí)行機構(gòu)簡化模型。定義r=[x,y,z]T為動平臺中心點O′在Oxyz中的位置矢量，則執(zhí)行機構(gòu)的位置反解模型為

圖2 3-PSS機構(gòu)簡化示意圖Fig.2 Schematic diagram of 3-PSS mechanism

(1)

式中：e3=[0,0,1]T，lAi和lBi分別是Ai和Bi在坐標系Oxyz和O′x′y′z′中的位置矢量，qi為液壓桿的伸縮量，L為連桿桿長。

1.3 并聯(lián)機構(gòu)動力學模型及其關節(jié)分散化

對于3-PSS并聯(lián)機構(gòu)，其基于關節(jié)空間的動力學方程可以描述為下述形式

(2)

式中：τ為液壓缸的驅(qū)動力向量；q為三液壓缸的位移矢量；M，C，G分別為慣性矩陣，離心和哥氏力矩陣和重力項；D=DS+DN為系統(tǒng)的建模誤差項(DS為外部干擾力，DN為執(zhí)行機構(gòu)其它建模誤差項)。

針對動力學模型(2)，其有如下性質(zhì)[10]：

性質(zhì)1.矩陣M為對稱正定矩陣，同時存在常數(shù)μmax>μmin>0，cH>0，cG>0，使得下面各式成立

(3)

式中：I為三階單位矩陣。

假設受風載作用的箭體在笛卡爾空間的運動為rd=[xd,yd,zd,ψxd,ψyd,ψzd]T?？紤]到箭體的擺動是有界且連續(xù)的，因此有如下性質(zhì)：

定義r=φ1(q)=[x,y,z]T和Δr=rd-r分別為執(zhí)行機構(gòu)在工作空間的軌跡輸出和軌跡跟蹤誤差。因此，執(zhí)行機構(gòu)動平臺所受外部干擾力可以定義為

F=K(rd,q)Δr

(4)

式中：K(rd,q)∈R3×6為柔順機構(gòu)的等效剛度系數(shù)。

由此可得關節(jié)空間的等效外部干擾力為[7]

DS(t)=(J(q))-1KΔr=φ2(K,rd,q)

(5)

式中：J(q)為并聯(lián)機器人的雅克比矩陣。

將式(2)分解為一組受外部擾動關節(jié)子系統(tǒng)的集合，可得各關節(jié)輸出力的動力學平衡方程為

(6)

1.4 液壓驅(qū)動器建模

關節(jié)液壓驅(qū)動器的結(jié)構(gòu)如圖3所示。

圖3 液壓驅(qū)動器示意圖Fig.3 Schematic diagram of hydraulic actuator

液壓驅(qū)動器的動態(tài)平衡方程可以描述為[21]

(7)

式中：Pi1和Pi2是液壓缸兩腔壓力,Ai1和Ai2為兩腔的有效作用面積,Bim為液壓缸的理想摩擦系數(shù),dim為驅(qū)動器中除了外干擾之外的其它未建模干擾。

忽略液壓缸的外泄漏，則液壓缸兩腔的壓力動態(tài)可以建模為[21]

(8)

式中：Vi1=Vi01+Ai1qi,Vi2=Vi02-Ai2qi分別表示液壓缸兩腔的有效容積，其中Vi01和Vi02分別為兩腔的初始容積；βe為油液彈性模量；Cit為內(nèi)泄漏系數(shù)；Qi1和Qi2分別為液壓缸的兩腔流量；PiL=Pi1-Pi2為液壓缸兩腔壓差；pid1和pid2分別為液壓缸兩腔的壓力動態(tài)建模誤差。其中Qi1和Qi2可分別定義為[21]

(9)

式中：ku為伺服閥總的流量增益,Ps為油源壓力,Pr為回油壓力,函數(shù)s(v)有如下定義

(10)

(11)

式中：

本文控制器的設計目標為：對于給定的工作空間期望軌跡rd，通過式(1)可得到系統(tǒng)關節(jié)空間的期望軌跡qd，進而設計有界的控制輸入u，使得關節(jié)軌跡輸出q盡可能地跟蹤qd。

為便于后續(xù)控制器設計，首先有如下假設：

假設1.液壓系統(tǒng)在一般工況下工作，即各液壓缸的兩腔油壓滿足0

假設2.關節(jié)中的不匹配不確定性DiN(·)和dim(·)有界，且其二階可導且有界。同時，關節(jié)的匹配不確定di3(t)有界，且其一階導數(shù)存在且有界。

結(jié)合式(5)、式(6)和假設2可知，存在有界常數(shù)L2ip和L3iq使得下式成立

(12)

式中：p∈N0:2，q∈N0:1。

2 控制器設計

2.1 關節(jié)期望軌跡生成

(13)

式中：κ1，κ2，κ3為濾波器設計常數(shù)。

2.2 關節(jié)空間有限時間動力學解耦

基于性質(zhì)1可知，存在非負常數(shù)β1,β2和β3，使得下述不等式成立

(14)

因此，基于式(5)、式(6)、式(14)和假設2，可知存在非負常數(shù)λi1,λi2和λi3，使得下式成立

(15)

參考文獻[16]，可針對關節(jié)不匹配不確定di2(t)設計如下形式的有限時間干擾觀測器

(16)

(17)

(18)

(19)

至此，基于式(16)，可得執(zhí)行機構(gòu)基于關節(jié)空間的動力學解耦模型為

(20)

2.3 關節(jié)空間反步魯棒控制器設計

步驟1：基于式(20)，定義如下的誤差變量

(21)

由式(20)和(21)可得

(22)

為x3設計一個虛擬控制函數(shù)α2，其形式如下

(23)

式中：α2a是系統(tǒng)的名義模型補償項，α2s為魯棒控制項，k2s1為正定對角矩陣。

定義z3=x3-α2，同時將式(23)代入式(22)，可得

(24)

設計α2s2滿足如下的鎮(zhèn)定條件：

(25)

式中：ε2>0為可任意小的設計參數(shù)。

(26)

步驟2：對z3兩端進行求導，可得

(27)

(28)

針對di3(t)可設計如下形式的擾動觀測器

(29)

(30)

(31)

(32)

結(jié)合式(27)和(29)，控制律u可以設計為：

(33)

(34)

設計us2滿足如下的鎮(zhèn)定條件

(35)

式中：ε3>0為可任意小的設計參數(shù)。

(36)

2.4 主要結(jié)果

定理1.選擇合適的控制反饋增益矩陣k1,k2s1和k3s1，使得如下矩陣Λ為正定陣

(37)

式中：Λ2=diag(-1/2,-1/2,-1/2)。同時，選用控制器(33)，則可以得到如下結(jié)論：

(1)閉環(huán)系統(tǒng)中所有信號均有界，且如下定義的Lyapunov函數(shù)

(38)

滿足下述不等式

V≤e-μtV(0)+(η/μ)(1-e-μt)

(39)

式中：η=ε2+ε3，μ=2λmin(Λ)min{1,1/λmax(M)}，λmin(·)和λmax(·)分別表示矩陣的最小和最大特征值。

證.首先證明結(jié)論(1)。對式(38)兩端進行微分，并考慮到式(21)、式(22)和式(27)，整理可得

(40)

注意到式(25)和式(35)，因此有

(41)

根據(jù)對比原理[22]可得

V≤e-μtV(0)+(η/μ)(1-e-μt)

(42)

(43)

3 試驗分析

3.1 試驗建立

圖4 并聯(lián)式加注機器人驗證平臺Fig.4 Experimental platform of parallel filling robot

另外，液壓驅(qū)動器采用行程為400 mm的單出桿液壓缸(活塞桿直徑28 mm，液壓缸內(nèi)徑40 mm)。同時，各驅(qū)動器包含一個型號為Atos DLHZO-TEB-SN-NP-040-L71的比例伺服閥，其額定流量為40 L/min；一個型號為MTS R系列高精度磁致伸縮位移傳感器，其精度為±25 μm；兩個型號為MEAS M5200-C00002-250 BG的壓力傳感器，其精度為0.1 MPa。

實時控制系統(tǒng)采用基于Matlab/xPC技術(shù)的方案，關于該技術(shù)的具體工作原理可參考文獻[11]。反饋信號的AD采集和控制信號的DA輸出均采用同一塊NI PCI-6229板卡?？刂葡到y(tǒng)的采樣時間為1 ms。

3.2 試驗對比

為了更好地驗證本文所設計控制器的有效性，選取以下3種控制策略進行試驗對比分析：

2)關節(jié)直接魯棒控制器(Direct robust controller,DRC)。DRC控制器中不包含關節(jié)擾動補償項，但其它控制器參數(shù)與DDRCFTDO相同，即k1=[850,850,850],k2=[530,530,530],k3=[320,320,320]。

3)速度前饋PID控制器(Velocity feed-forward PID,VFPID)?？刂破鲄?shù)設計為：kip=1400,kii=350,kid=0，速度前饋增益為kiv=12 Vs/m。

3.2.1工況一

首先校核執(zhí)行機構(gòu)對于箭體搖擺運動的隨動跟蹤性能?？紤]到在液體燃料加注過程中，箭體因燃料重力作用造成的豎直下沉運動極其緩慢(不大于15 mm/h),假設箭體受側(cè)風作用，其運動規(guī)律如下

(44)

不同控制器在工作空間和關節(jié)空間的軌跡跟蹤誤差情況分別如圖5～6所示。在最后兩個運動跟蹤周期中，不同控制器在各軸(方向)上的最大跟蹤誤差如表1所示。

表1 各軸(方向)最大跟蹤誤差Table 1 Maximum tracking error of each axis (direction)

由試驗結(jié)果可知，DDRCFTDO控制器無論在關節(jié)空間還是工作空間均取得了最優(yōu)的軌跡跟蹤控制精度。這是由于在DDRCFTDO控制器中，F(xiàn)TDO的引入使得控制器實現(xiàn)了對關節(jié)中匹配和不匹配不確定的精確估計和補償，進而有效地減小了關節(jié)擾動(特別是外部擾動)對關節(jié)控制性能的影響。另外，相對DDRCFTDO，DRC控制器在關節(jié)空間各軸上的跟蹤精度均較差。分析其原因，主要是因為在隨動過程中，DRC控制器缺少對關節(jié)強外部擾動的有效補償處理，同時控制器的反饋增益較小，故而影響到了各關節(jié)的隨動精度。另外，結(jié)合圖6和表1中的指標數(shù)據(jù)可知，在關節(jié)空間內(nèi)，VFPID控制器的性能指標均優(yōu)于DRC控制器，這是由于VFPID控制器通過速度前饋獲得了部分系統(tǒng)動力學補償，同時高增益反饋有效地抑制了關節(jié)跟蹤誤差。值得注意的是，在x軸向，DRC指標好于VFPID，這也充分說明了并聯(lián)機器人關節(jié)空間和工作空間軌跡映射的強非線性。

為了驗證FTDO的觀測精度，本文將FTDO的擾動觀測值與關節(jié)擾動名義值進行對比分析(名義值可通過ADAMS對給定運動仿真分析獲得)，對比結(jié)果如圖7所示。很明顯，F(xiàn)TDO擾動觀測結(jié)果與關節(jié)名義擾動走向大體一致，但是兩者之間也存在一定的偏差。這是由于關節(jié)不匹配不確定中不僅含有外部時變干擾，同時還存在一定的系統(tǒng)建模誤差(如摩擦建模誤差和其它未建模動態(tài)等)。因此，可以認為FTDO在實際工況中，對關節(jié)中的不匹配不確定具有良好的觀測性能。同時，圖8顯示了FTDO對于關節(jié)匹配不確定的觀測情況，可見其處于合理范圍之內(nèi)。

圖5 工作空間軌跡跟蹤誤差Fig.5 Trajectory tracking errors in work space

圖6 關節(jié)空間軌跡跟蹤誤差Fig.6 Trajectory tracking errors in joint space

圖10 關節(jié)空間軌跡跟蹤誤差Fig.10 Trajectory tracking errors in joint space

3.2.2工況二

為了更一般地驗證控制器的性能，給定如下空間三維期望軌跡進行控制器跟蹤性能校核

(45)

同時考慮到在實際任務中，液體燃料流量的變化將造成執(zhí)行機構(gòu)負載的改變，為了驗證系統(tǒng)在不同載荷下的性能，對系統(tǒng)進行負載配重20 kg。

不同控制器在工作空間和關節(jié)空間的軌跡跟蹤誤差情況分別如圖9～10所示。在最后兩個運動跟蹤周期中，不同控制器在各軸(方向)上的最大跟蹤誤差如表2所示。

由圖9～10和表2可知，DDRCFTDO控制器在關節(jié)空間和工作空間仍然取得最好的軌跡跟蹤性能。由于FTDO良好的觀測性能，期望軌跡和負載變化對關節(jié)的跟蹤控制性能影響并不明顯。而DRC和VFPID由于缺少有效的關節(jié)擾動補償，其關節(jié)跟蹤精度均有一定程度的惡化，這也進一步體現(xiàn)了本文所設計控制器的有效性。

表2 各軸(方向)最大跟蹤誤差Table 2 Maximum tracking error of each axis (direction)

4 結(jié) 論

本文針對某電液伺服型并聯(lián)式加注機器人的空間軌跡跟蹤控制問題，提出了一種基于關節(jié)有限時間擾動觀測器的執(zhí)行機構(gòu)動力學解耦反步魯棒控制策略。試驗研究表明，本文所設計控制器具有良好的軌跡跟蹤精度和較強的關節(jié)抗干擾能力?？刂破髟诓煌r下，均能夠保證執(zhí)行機構(gòu)空間軌跡跟蹤精度不大于1 mm，其遠小于柔順機構(gòu)的空間可補償范圍。因此，本文所設計控制器能夠滿足在實際加注任務中對加注機器人軌跡跟蹤性能的要求。

另外，該控制器無需系統(tǒng)完整的動力學模型信息，結(jié)構(gòu)簡單可靠，在其它機器人相關的控制研究中同樣具有良好的工程應用前景。