張智豪 于瀟雁

(福州大學機械工程及自動化學院,福州 350116)

(福州大學流體動力與電液智能控制福建省高校重點實驗室,福州 350116)

引言

隨著空間技術的發(fā)展,空間機械臂的應用越來越廣泛[1-6].由于在太空微重力環(huán)境中載體自由浮動,空間機械臂系統(tǒng)中載體與臂桿之間存在強動力學耦合,這會使得載體的姿態(tài)在工作過程中難以保持穩(wěn)定,影響臂桿的正常工作[7-11].載體的姿態(tài)控制一般可以通過主動姿態(tài)控制系統(tǒng)ACS 來實現(xiàn)[12-15],但為了節(jié)省燃料,ACS 系統(tǒng)常常處于關閉狀態(tài),如何通過臂桿的運動規(guī)劃來減小載體的姿態(tài)變化就有了現(xiàn)實意義.

Dubowskys 等[16-17]提出基于增強擾動圖EDM 的關節(jié)運動規(guī)劃來減小機械臂運動對載體姿態(tài)的擾動.Nenchev 等[18]采用基于定姿態(tài)雅克比矩陣FARJM 的方法進行空間機械臂的零擾動規(guī)劃.Huang 等[19]利用雙臂空間機器人中一個機械臂補償來另一個機械臂運動造成的載體擾動.戈新生等[20-22]研究了通過機械臂運動對載體進行姿態(tài)調(diào)整的問題.張福海等[23-24]提出了一種笛卡爾空間的載體姿態(tài)無擾的空間機器人非完整路徑規(guī)劃方法.Nguyen-Huynh和Sharf[25]研究了系統(tǒng)捕獲非合作目標后的運動規(guī)劃問題,提出了自適應姿態(tài)無擾路徑規(guī)劃方案.Yoshida[26]通過指定自由向量來規(guī)劃機械臂的運動,實現(xiàn)手臂對基座的零反作用力.Zhang等[27]研究了基于隨機快速搜索樹的一種運動規(guī)劃算法,能夠在不需要求解逆運動學的情況下完成規(guī)劃任務并保留姿態(tài)約束.

上述方法[16-27]均是針對機械臂一般操作過程中運動學的非完整特性進行運動規(guī)劃,沒有考慮機械臂與待抓取目標之間的關系與關節(jié)的實際特性,而實際的空間機械臂關節(jié)通常都會存在“死區(qū)”特性.“死區(qū)”是指關節(jié)的輸入力矩大小達到某個特定值之前,其輸出力矩為零的輸入-輸出關系.當輸入信號經(jīng)由死區(qū)作用范圍時,其會有一定的損失,從而造成系統(tǒng)控制精度的偏差.若不能消除其影響,除造成輸出精度降低外,還會對系統(tǒng)造成極限環(huán)振蕩,甚至導致控制失效,從而無法完成空間作業(yè)[28-30].

本文研究了存在關節(jié)死區(qū)的漂浮基平面三連桿空間機械臂攔截目標前最后階段的無擾動空間規(guī)劃與控制問題.首先針對載體位姿均不受控條件下,建立存在關節(jié)死區(qū)的三連桿空間機械臂系統(tǒng)的拉格朗日動力學模型,推導出三連桿空間機械臂反作用零空間的數(shù)學模型,并對反作用零空間進行向量范數(shù)約束算法研究.進而提出了一種具有抗干擾性與高收斂性的終端滑?？刂扑惴▽崿F(xiàn)系統(tǒng)的姿態(tài)無擾控制,該方法采用變系數(shù)雙冪次趨近率與非奇異快速終端滑模面相結合的方式,提高系統(tǒng)狀態(tài)收斂速度與抗干擾性,同時考慮機械臂關節(jié)存在的死區(qū)特性,設計了自適應補償器,通過自適應控制來逼近死區(qū)特性的上界,以確保跟蹤控制的有效執(zhí)行.最后基于Lyapunov 函數(shù)法證明了系統(tǒng)的穩(wěn)定性,并通過系統(tǒng)數(shù)值仿真驗證了所提方法的有效性.

1 系統(tǒng)動力學方程

如圖1 所示為一平面三連桿空間機械臂系統(tǒng)與一待抓取旋轉(zhuǎn)的目標衛(wèi)星.該空間機械臂系統(tǒng)由自由漂浮的剛性載體B0,系統(tǒng)的剛性臂B1,B2,B3組成,其中P是待抓取的目標.

圖1 漂浮基三桿空間機器人與待抓取目標Fig.1 Floating based three-bar space robot and target to be grasped

假設該系統(tǒng)在xOy平面內(nèi)作平面運動.空間機械臂系統(tǒng)沿著軌道以速度v運動,由于軌道曲率半徑很大,在短時間的運動內(nèi),可以把軌道近似看成一條直線.被抓取目標為以角速度繞質(zhì)心旋轉(zhuǎn)的圓柱形物體,抓手手柄距離質(zhì)心rh.機器人系統(tǒng)軌道相對于x軸位置角為 ψ,它與目標質(zhì)心的距離為d.在抓取點,機械臂末端抓手與目標手柄之間的速度差為Δv.

建立各分體Bi(i=0,1,2,3) 的連體平動坐標系Oixiyi,其中O0與B0的質(zhì)心Oc0重合,Oc1,Oc2和Oc3分別為機械臂B1,B2和B3的質(zhì)心.各分體Bi之間以剛性旋轉(zhuǎn)副進行鉸接,其中,Oi(i=1,2,3) 為旋轉(zhuǎn)中心,xi(i=0,1,2,3)分別為機械臂Bi的對稱軸.

其他參數(shù)符號如下:l0——O1在x0軸上與O0的距離;

li——Bi沿xi軸的長度 (i=1,2,3) ;

bi——Oi到質(zhì)心Oci之間的距離 (i=1,2,3) ;

mi——各分體的質(zhì)量 (i=0,1,2,3) ;

Ji——各分體的中心轉(zhuǎn)動慣量 (i=0,1,2,3) ;

Oc——系統(tǒng)總質(zhì)心;

θ0——B0上x0軸相對x軸的載體姿態(tài)角;

θi——機械臂的關節(jié)角 (i=1,2,3) .

忽略微重力的影響,由拉格朗日第二類方程,可得到載體位姿均不受控的漂浮基空間機器人系統(tǒng)的動力學方程

其中,q=[x0y0θ0θ1θ2θ3]T為系統(tǒng)廣義坐標,D(q)∈R6×6為系統(tǒng)的對稱慣性矩陣;R6×1為包含科里奧力、離心力的列向量;τ=[τ1τ2τ3]T∈R3×1,τi(i=1,2,3) 為通過死區(qū)后關節(jié)i的輸出力矩.

由于機械設計與制造方面的原因,系統(tǒng)關節(jié)通常都會存在“死區(qū)”特性,影響系統(tǒng)的輸出精度.關節(jié)死區(qū)特性對控制系統(tǒng)造成的影響可以用圖2 來表示.其中u代表控制輸入,τ代表關節(jié)輸出,一般情況下兩者并不相同.

圖2 關節(jié)死區(qū)特性圖Fig.2 Characteristic diagram of joint dead-zone

一般情況下,系統(tǒng)的關節(jié)死區(qū)特性可用控制輸入u與系統(tǒng)輸出τ之間的關系來描述

其中d i+＞ 0,d i-＜ 0,kri=kli=ki＞ 0 為死區(qū)的左右斜率且為常數(shù),則可將系統(tǒng)的輸出分為如下兩部分

其中 δ(u)代表系統(tǒng)的關節(jié)死區(qū)特性,ku代表執(zhí)行機構的對控制輸入的響應,k=diag(ki),則有

其中 δi(u) 為 δ(u)中的元素,i=1,2,3 .

存在死區(qū)的空間機器人動力學方程可以寫為

一般漂浮基空間機器人系統(tǒng)(5),在全局范圍內(nèi)死區(qū)特性均有界,即

其中 δimax(i=1,2,3) 為未知的正常數(shù),是向量δmax∈R3×1中的元素,δmax代表死區(qū)的實際上界.

2 反作用零空間規(guī)劃

2.1 反作用零空間數(shù)學模型

由式(5)可將空間機器人系統(tǒng)動力學方程寫為

其中Dbb∈R3×3，Dbm∈R3×3，Dmm∈R3×3;[HbTHmT]T且Hb∈R3×1，Hm∈R3×1;Xb表示載體的基座坐標與姿態(tài)角,且存在關系Xb=[x0y0θ0]T;qb表示機器人連桿的關節(jié)角,且存在關系qb=[ θ1θ2θ3]T.

系統(tǒng)耦合矩陣Dbm為

本文基座姿態(tài)無擾動的同時允許基座的平移運動,故將其中反映機械臂關節(jié)角與基座姿態(tài)關系的第三行提取出來,記為修正耦合慣性矩陣,同時也降低了計算復雜度,即

忽略太空中的微重力,對式(1)第一行積分可得系統(tǒng)的線動量P與角動量L[18,33],此時空間機器人系統(tǒng)的廣義動量為

其中Dbb表示空間機器人系統(tǒng)中載體的動量.定義 Γm=Dbm為機械臂運動所產(chǎn)生的耦合動量.對其關于時間t求導,可得臂桿作用在載體上的反作用力

若機械臂運動所產(chǎn)生的耦合動量 Γm為常數(shù),則此時作用在基座上的反作用力與力矩Fm為0.當機械臂的自由度大于機器人系統(tǒng)載體的自由度時,若要滿足耦合動量為常數(shù),則機械臂關節(jié)角速度解為

其中,E為單位矩陣,為Dbm偽逆矩陣,ξ=[ ξ1ξ2ξ3]T為一3 階任意列向[34],ξ 能夠?qū)﹃P節(jié)角速度進行一定的調(diào)整以符合相應的約束條件.若耦合動量 Γm=0,且只考慮載體姿態(tài)時,可引入修正耦合慣性矩陣,則有

當控制系統(tǒng)按式(14)運動,此時機械臂的運動不會擾動載體姿態(tài)的運動,從而達到了無擾動狀態(tài).

2.2 向量范數(shù)約束法

根據(jù)反作用零空間的定義,ξ 是一個任意向量,適當?shù)?ξ可以確保無反應空間角速度、角加速度滿足某些約束條件.為了計算出實際的反作用零空間軌跡,需要計算出具體的 ξ 數(shù)值.考慮實際的空間機器人運行環(huán)境,設定兩個約束條件:

(1)機械臂運動對載體的耦合力矩約等于0;

(2)整個運動過程,各個關節(jié)的角速度、角加速度在一個合理的范圍內(nèi).

在滿足這兩個條件下,求解滿足條件的ξ,則有

將其推廣至機械臂運動的全過程,則每個時刻應滿足

其中 ()RNS為機械臂反作用零空間關節(jié)角速度向量,()RNS為機械臂反作用零空間關節(jié)角加速度向量.

根據(jù)式(12)反作用零空間運動的反作用力與力矩為

式中‖·‖2為歐幾里德范數(shù),則有

該算法通過求解式(19) 對 ξ 進行迭代,再由ξ求得無擾動期望軌跡.

3 非奇異快速終端滑?？刂?/h2>

在由 ξ 求得無擾動規(guī)劃軌跡后,則可以按照求得的軌跡進行控制,定義系統(tǒng)的增廣輸出誤差向量e為

其中ec=qb-(qb)d,

選取滑模面為

式中 α=diag(αi),β=diag(βi),其中(i=1,2,···,6),αi和βi均為正的控制器參數(shù);m，n，p，q均為正奇數(shù),并且滿足 2＞m/n＞1,p/q＞m/n.

對式(23)求導可得

由式(5)可得

進而式(24)可演化為

定義

令ki=1,δd=[0Tδ(u)T]T則有

其中Nδd代表了非線性死區(qū)特性對控制系統(tǒng)造成的影響.

采用帶有變系數(shù)的雙冪次趨近率

其中I為單位矩陣;μ1=dig[μ11,μ12,···,μ16],μ2=dig[μ21,μ22,···,μ26],μ1i和μ2i均為正常數(shù);‖e‖=digr1=p1/q1,r2=m1/n1,p1，q1，m1，n1均為正奇數(shù)且滿足r1＞1,0＜r2＜1;在趨近運動階段,當系統(tǒng)狀態(tài)遠離滑模面(|si|≥1) 時,)(2I+2‖e‖)sgn(s) 能夠以較大速度趨近滑模面,當系統(tǒng)接近滑模面(|si|＜1) 時,)(2I+2‖e‖)sgn(s)依然能夠保證足夠的趨近速度,兩部分可以保證趨近運動全程保持足夠的收斂速度.

為了彌補死區(qū)特性對控制系統(tǒng)造成的影響,本文在非奇異快速終端滑?？刂频幕A上,設計了自適應死區(qū)補償器(adaptive dead-zone compensator,ADC),其結構圖見圖3.

圖3 帶有自適應死區(qū)補償器的控制系統(tǒng)Fig.3 Control system with ADC

系統(tǒng)的控制力矩為

其中u1與u2分別為

其中,σ∈R3×1保證載體姿態(tài) θ0以及位置x0和y0所對應的控制力矩為0,為估測的死區(qū)最大值,且.u1為終端滑模控制器的輸出控制指令,實現(xiàn)跟蹤誤差在有限時間內(nèi)到達滑動模態(tài)面以及鎮(zhèn)定作用.u2為控制自適應死區(qū)補償器的輸出,用來消除死區(qū)對系統(tǒng)跟蹤精度的影響.其參數(shù)的更新律設計如下

該自適應死區(qū)補償器能夠從估計的死區(qū)最大值逼近實際的死區(qū)值,有效的消除死區(qū)所帶來的影響.

4 穩(wěn)定性證明

引入如下的Lyapunov 函數(shù)

式(34)對時間t求導可得

利用式(29)、式(31)和式(35)可變?yōu)?/p>

由控制器(32)可得

利用自適應更新率(33),則得

因V有下界且為0,則有系統(tǒng)是閉環(huán)穩(wěn)定,即實現(xiàn)機械臂各關節(jié)的跟蹤與關節(jié)死區(qū)特性參數(shù)的估計.

5 模擬仿真算例

為了驗證無擾動空間規(guī)劃與該控制方法的有效性,對圖1 所示的做平面運動的漂浮基剛性三桿空間機器人系統(tǒng)接近抓取目標過程進行無擾動空間規(guī)劃與控制.關節(jié)死區(qū)的實際上限為 δmax1=35,δmax2=45,δmax3=15 ;其他系統(tǒng)參數(shù)如表1 所示.

表1 空間機器人系統(tǒng)參數(shù)Table 1 space robot system parameters

控制器參數(shù)分別為p/q=5/3,m/n=11/9,α=diag(2,2,2,2,2,2),β=diag(4,5,6,5,4.5,5) .

采樣間隔為L=0.01 s .機械臂系統(tǒng)沿軌道運行速度為v=0.1 m/s,載體姿態(tài)角 θ0=-π/6,機器人系統(tǒng)軌道相對于x軸位置角為 ψ=π/3 .目標物體P與軌道之間的距離d為2.3 m,其自轉(zhuǎn)速度ωp=0.13 rad/s,設定末端執(zhí)行器與目標物體P之間速度差滿足 Δv≤0.02 m/s,對關節(jié)角加速度進行范數(shù)約束,初始參數(shù)設定為 ξ(0)=[0.03 0.02 0.02]T,從系統(tǒng)運行開始到抓取到目標共用時66 s.

設機械臂系統(tǒng)初始狀態(tài)為 θ1=0.8 rad,θ2=1.7 rad,θ3=-2.65 rad,系統(tǒng)仿真結果如圖4～圖10所示.其中,圖4～圖6 為關閉自適應死區(qū)補償器時關節(jié)角變化曲線,系統(tǒng)關節(jié)角無法跟蹤上無反應期望軌跡.為了展示自適應補償器的性能,關節(jié)死區(qū)的估測上限為.圖7～圖10 為開啟自適應死區(qū)補償器時的反作用零空間軌跡追蹤曲線.

當末端抓手接觸到目標手柄時,Δv=8.635 5 mm/s.圖10 表明空間機械臂接近目標過程中,自適應死區(qū)補償器能夠補償死區(qū)造成的影響,姿態(tài)角在軌跡追蹤開始時有較小變化,4 s 之后一直保持在 -π/6,實現(xiàn)了載體姿態(tài)無擾動;同時圖7～圖9中表明各關節(jié)角漸進追蹤上無擾動空間期望軌跡,5 s 之后一直保持在姿態(tài)無擾軌跡運行.與圖4～圖6的對比可以表明自適應死區(qū)補償器能夠有效補償死區(qū)造成的影響,使姿態(tài)角的軌跡穩(wěn)定,關節(jié)角能夠有效的追蹤.

圖4 關節(jié)角1 軌跡曲線(關閉死區(qū)補償器)Fig.4 Trajectory curve of joint angle 1 (closing ADC)

圖5 關節(jié)角2 軌跡曲線(關閉死區(qū)補償器)Fig.5 Trajectory curve of joint angle 2 (closing ADC)

圖6 關節(jié)角3 軌跡曲線(關閉死區(qū)補償器)Fig.6 Trajectory curve of joint angle 3 (closing ADC)

圖7 關節(jié)角1 軌跡曲線(開啟死區(qū)補償器)Fig.7 Trajectory curve of joint angle 1 (opening ADC)

圖8 關節(jié)角2 軌跡曲線(開啟死區(qū)補償器)Fig.8 Trajectory curve of joint angle 2 (opening ADC)

圖9 關節(jié)角3 軌跡曲線(開啟死區(qū)補償器)Fig.9 Trajectory curve of joint angle 3 (opening ADC)

圖10 姿態(tài)角軌跡曲線(開啟死區(qū)補償器)Fig.10 Attitude angle trajectory curve (opening ADC)

6 結論

(1) 針對載體位姿均不受控情況下,存在關節(jié)死區(qū)的剛性三桿空間機械臂在抓取旋轉(zhuǎn)目標過程中,引入反作用零空間概念,建立反作用零空間機器人系統(tǒng)動力學模型,提出零空間機械臂系統(tǒng)的向量范數(shù)約束法,實現(xiàn)了載體姿態(tài)無擾動空間規(guī)劃.

(2) 在反作用零空間規(guī)劃的基礎上考慮關節(jié)死區(qū)特性,設計自適應死區(qū)補償器,對死區(qū)特性進行補償.

(3) 通過理論分析與仿真結果證明了該控制方法在關節(jié)死區(qū)情況下,機械臂的各關節(jié)角仍能跟蹤上無反應空間下的期望軌跡,同時載體的姿態(tài)處于穩(wěn)定狀態(tài).