自由漂浮空間機器人點到點避奇異運動控制方法

羊帆，張國良，張合新，宋海濤

1. 火箭軍工程大學 導彈工程學院，西安 710025 2. 成都信息工程大學 控制工程學院，成都 610225

隨著人類空間活動的日益增多，以及空間在軌服務任務的需求，研究利用智能機器人執行復雜、危險的空間任務成為了當前空間技術研究的重要方面，因具有較好的長期在軌服務能力，自由漂浮空間機器人(Free Floating Space Robot, FFSR)日益成為空間機器人研究的重要對象[1-2]。

FFSR是典型的非完整多體動力學系統，其控制與規劃問題一直是研究者關心的主要內容。其中，Umetani和Yoshida[3]對FFSR的運動學進行了相關研究，提出了廣義雅克比矩陣(Generalized Jacobian Matrix, GJM)概念，實現了FFSR的分解速度、加速度控制。Dubowsky和Papadopoulos[4]開展了FFSR的動力學建模問題研究，基于PD控制提出了FFSR的計算力矩控制方法。Gu和Xu[5]在FFSR標準動力學模型基礎上，將基座運動引入動力學方程，形成了FFSR的擴展動力學方程，并在此基礎上提出了規范形式擴展自適應控制方法。但是，該方法需要系統的加速度信息。為此， Parlaktuna和Ozkan[6]、Wang[7]采用低通濾波器對系統進行積分降階實現FFSR末端運動的自適應控制。Wang等[8]基于NMPC理論研究FFSR的避障跟蹤控制方法。此外，如神經網絡[9]、確定學習[10]、模糊控制[11]等智能控制方法亦被研究者用以實現FFSR的軌跡跟蹤控制。至此，通過學者的大量工作，無論是針對關節空間還是任務空間的FFSR跟蹤控制問題得到了基本解決。但是，也存在一些問題。具體來講，由于非完整約束存在，類似于地面機械臂直接控制關節軌跡從而實現末端位姿的期望運動存在一定的困難即關節空間軌跡不能直接向任務空間映射。若在任務空間下直接跟蹤末端期望軌跡，不可避免地存在奇異問題。徐文福[12]在其博士論文中分析指出當FFSR奇異發生時，將喪失末端位姿在某個或某幾個方向上的控制自由度(無論如何施加控制力矩均無法完成某個方向的運動)。因此，上述任務空間中FFSR末端軌跡控制方法多假設控制過程中不存在奇異問題，以保證控制力矩不發生突變。

為解決FFSR的奇異問題，一些學者從規劃角度出發開展了FFSR的避奇異規劃研究。夏進軍等[13]基于奇異轉化思想，將FFSR的避動力學奇異問題轉化為避虛擬機械臂的運動學奇異方法，實現了FFSR避奇異規劃。Nanos和Papadopoulos[14-15]通過調整機械臂初始構型方法，首先實現了平面2自由度FFSR動力學奇異回避，其后將此方法擴展到多自由度FFSR動力學奇異回避中。張福海等[16]將可操作度變化作為依據，預測系統運動過程中的奇異位形并提出一種能構造無奇異運動軌跡的任務重構法。徐文福等[17]針對FFSR動力學與運動學耦合問題進行了混合建模與分析，提出姿態自由耦合空間、自由耦合空間以及耦合度等概念，并結合建模分析過程研究了FFSR非完整路徑規劃問題與動力學奇異回避問題。進一步，通過對奇異條件的分離，采用阻尼倒數代替導致矩陣奇異的普通倒數，從而消除奇異對關節角速度產生的影響[18]。上述方法，通過采用避奇異規劃方法解決了FFSR在控制過程中的奇異問題。但是上述方法多考慮非冗余FFSR系統的奇異回避規劃問題，利用廣義逆(奇異倒數)代替GJM逆進行關節角規劃時犧牲了FFSR在末端的規劃精度，而采用調整初始位姿方法亦存在任務執行效率較低問題。為此，Wang等[19]基于貝塞爾曲線利用差分進化算法實現了7自由度(Degree of Freedom, DoF)的FFSR關節空間奇異回避規劃。此外，Xu等[20]通過任務分解方法，采用增加位置平衡臂以及姿態平衡臂等方法利用冗余關節運動實現FFSR的奇異回避與基座位姿穩定規劃與控制，取得較好的FFSR運動控制效果。雖然，當前關于FFSR奇異回避規劃與控制問題相對成熟，但是也存在一些問題。采用阻尼倒數或廣義逆實現奇異GJM代替法本質上犧牲了目標控制精度以實現奇異回避，采用初始位姿調整亦存在規劃軌跡不連續等問題。文獻[19]和文獻[20]為冗余FFSR的奇異回避規劃與控制提供了很好的思路但也存在計算分析過程復雜或者FFSR物理結構組成復雜等問題。

為了有效解決冗余FFSR系統點到點的避奇異規劃與跟蹤控制問題。本文提出一種冗余FFSR末端點到點避奇異運動控制方法。該方法首先基于離散狀態依賴李卡提方程(Discrete State Dependence Riccati Equation, DSDRE)控制器設計方法，利用FFSR的動力學和運動學方程實現了FFSR系統方程的偽線性重構。然后，在重構系統的基礎上利用DSDRE狀態調節器設計方法實現了FFSR的關節角速度和末端位姿的同時跟蹤控制。其次，根據控制器設計需求提出FFSR的避奇異約束函數，進而結合關節角約束實現FFSR在線避奇異規劃器設計，最后將控制方法與規劃方法相結合實現了冗余FFSR的點到點的避奇異運動控制。其整體結構框圖如圖1所示。圖中：yf為期望末端目標位姿；τm為控制力矩；xdes為期望狀態變量；ye為末端位姿；qb為基座的位姿向量；qm為關節角向量；sk為GJM的最小奇異值。

1 模型與假設

1.1 空間機器人模型

典型的FFSR擴展動力學方程可表述為[4]

(1)

由于基座處于漂浮狀態，且忽略了重力影響，FFSR系統滿足動量守恒，故FFSR系統還滿足

(2)

式中：M0為系統的初始動量。

進一步，將式(2)代入式(1)可得FFSR的標準動力學方程為

(3)

對式(1)求導并結合角動量及線動量守恒方程，可得FFSR的運動學方程為[4]

(4)

式中：J*(qb,qm)為FFSR的GJM，考慮本文以冗余FFSR系統為研究對象，則有J*(qb,qm)∈Rm×n,m

1.2 問題描述及假設

為了便于問題研究且不失一般性，作如下假設：

1) 一般而言，FFSR的基座質量遠大于桿件質量，且為保證安全FFSR運動一般較為緩慢，故假設在兩次數據采樣間基座位姿變化很小，故以當前基座位姿采樣值作為下一時刻基座運動的估計值。

3) 系統均為剛體，忽略微重力影響，系統初始動量M0=0。

4) FFSR初始關節配置，不為奇異位型，即FFSR初始狀態不存在奇異。

2 冗余FFSR的跟蹤控制器設計

為了實現FFSR的末端運動控制，本節中將基于DSDRE的調節器設計方法，設計冗余FFSR的末端軌跡跟蹤控制器。

2.1 FFSR系統方程的偽線性重構

由式(2)～式(4)可得：

(5)

(6)

式中：A(x)∈R(n+m)×(n+m)、B(x)∈R(n+m)×n為系統的狀態依賴系數(State Dependence Coefficient, SDC)矩陣[22]，表達式分別為

(7)

(8)

進一步，對偽線性狀態方程式(6)進行離散化可得

xk+1=Ad(xk)xk+Bd(xk)τmk

(9)

式中：xk為tk時刻的狀態變量；Ad(xk)、Bd(xk)為離散系統的狀態矩陣和輸入矩陣，分別滿足

Ad(xk)=TA(xk)+I=

(10)

(11)

式中：T為采樣周期；I為適當維數的單位矩陣；k={0,1,2,…,∞}表示采樣序列。

則由式(10)和式(11)可得：

(12)

式(9)為由FFSR系統方程重構的離線系統偽線性方程。

2.2 FFSR跟蹤控制器設計

由FFSR的偽線性重構狀態方程式(9)及式(12)可知，若FFSR滿足GJM行滿秩，則其SDC矩陣[AdBd]滿足逐點可控，則可根據DSDRE相關理論設計FFSR系統的優化跟蹤控制器。

ek=xk-xdesk

(13)

進一步，定義性能指標函數：

(14)

式中：Q、R為適當維數的正定對稱矩陣。

則可得反饋控制律為

(15)

式中：反饋增益為

(16)

Pk是李卡提方程的解：

(17)

定理1：若FFSR的GJM滿足行滿秩，即有rank(J*(qb,qm))=m，且滿足假設2)及假設3)，存在優化跟蹤控制律式(15)使得FFSR末端位姿運動穩定跟蹤期望位姿軌跡。

證明：因rank(J*(qb,qm))=m，則由式(12)可得SDC矩陣{Ad,Bd}滿足逐點可控。進一步，由優化理論可知對無限時間離散李卡提方程式(17)有唯一正定解。

由偽線性系統式(9)及控制律式(15)可得系統閉環方程

(18)

則閉環系統式(18)包含反饋項與前饋項，由于前饋項不影響系統的穩定性。故考慮閉環系統反饋環節為

(Adk-Fk)xk

(19)

則可構造Lyapunov函數：

滿足V(xk)≥0,?xk≠0，僅當xk=0,V(xk)=0。 則由式(19)可得：

(20)

3 冗余FFSR系統的避奇異運動規劃

由定理1可知，FFSR的GJM行滿秩式是保證跟蹤控制方法的有效的前提，但是，動基座使得GJM的奇異性與FFSR末端路徑運動密切相關，使得采用離線路徑規劃方法回避GJM奇異具有一定的難度。此外，實際應用中由于關節角及關節角速度約束存在也使FFSR離線避奇異規劃顯得異常復雜。為此，本節將利用冗余FFSR具有多逆運動學解這一特點研究冗余FFSR的在線軌跡規劃器。

3.1 FFSR的奇異性判定與避奇異約束

1) GJM的奇異性判定

考慮本文研究對象為冗余FFSR系統，其GJM為非方矩陣，為保證[AdBd]逐點可控，需要FFSR的GJM始終行滿秩。為使這一條件滿足，考慮以式(21)計算GJM奇異值，從而實現GJM奇異性判斷。

(21)

式中：函數fsvd(·)表示計算矩陣的最小奇異值。

根據式(21)計算結果，可給出如下FFSR奇異性定義。

定義1FFSR的奇異區域閾值Suf>0。若i時刻式(21)計算結果si≤Suf，則認為此時FFSR處于奇異位型，相應的控制律式(15)失去對系統的控制運動的控制能力。

定義2進入奇異區域閾值Sif>Suf。若i時刻式(21)計算結果si≤Sif，則認為FFSR運動路徑將發生奇異，需要進行適當的避奇異處理，以保證控制律式(15)的有效。

2) GJM奇異性估計與避奇異約束

(22)

為避免系統進入奇異區域以及進入奇異后迅速進行奇異回避運動，根據式(22)給出的期望奇異值，設計奇異回避約束函數為

(23)

3.2 在線軌跡規劃器設計

為實現FFSR點到點運動控制，結合定義的避奇異約束函數，定義規劃目標函數為

(24)

式中：α為規劃參數；γ、β為權系數，為保證FFSR進入奇異后迅速離開奇異區域保證控制器有效性，通常權系數選取時應滿足β?γ。

式(24)的規劃目標函數分為兩項，其分別保證FFSR末端趨向目標點運動和FFSR的GJM行滿秩使得跟蹤控制方法始終有效。

由于式(24)中并不顯含規劃參數，現就規劃參數與目標函數中各項關系以及其他規劃約束條件，進行分析。

(25)

由式(4)可計算期望末端位姿速度為

(26)

進一步，可得期望末端位姿及期望關節角為

(27)

由式(25)可以得到目標函數中的優化參數本質上為期望關節運動的角加速度。實際中由于自身結構限制，各關節關節角總是限制在某個區域內，為此，必須對目標函數式(24)中的搜索空間進行約束以保證關節角及角加速度合理性。

定義3目標函數參數搜索空間

(28)

則有

(29)

由式(22)、式(23)以及式(24)～式(29)可將FFSR的點到點運動規劃問題總結為非線性優化問題：

(30)

采用經典的內點法求解優化問題式(30)，即可獲得控制器期望狀態變量xdesk。

4 FFSR末端點到點避奇異運動控制

為實現FFSR的點到點運動控制本節中將對上述跟蹤控制方法與在線規劃方法進行綜合以實現冗余FFSR點到點避奇異運動控制。

定義4FFSR終點精度控制函數

(31)

式中：etrack為目標位姿控制精度。

則由式(31)可得當滿足etrack≤ε時認為達到設定目標位姿控制精度ε， FFSR運動停止。

整個點到點運動控制流程如圖2所示，為進一步說明本文方法，同時給出計算步驟：

步驟2tk時，獲得機器人關節角，基座位姿，末端位姿等初始值。

步驟3 計算式(21)判斷GJM是否奇異，若sk≤Suf說明FFSR處于奇異位型控制器失效，則停止；反之轉步驟4。

步驟5 利用規劃所得期望狀態量xdesk代入控制律式(15)，產生關節控制力矩τmk。

步驟6 根據式(31)計算停止精度條件，若滿足etrack≤ε表明達到目標位姿停止，反之轉步驟7。

步驟7 判斷FFSR運動時間是否超過設定時間，若超過則停止。反之轉步驟8。

步驟8k+1→k。轉步驟2。

定理2 若非線性優化問題(30)始終有解，且滿足假設1)～4)，則存在優化控制律(15)使得FFSR末端能夠到達目標位姿。

證明：

由假設4)可知初始時GJM非奇異，則控制律式(15)在初始有效，又非線性優化問題式(30)始終有解，表明在FFSR運動過程GJM始終行滿秩，由定理1可知FFSR末端能夠穩定跟蹤期望運動軌跡則由limk→∞x=xdes進一步可得k→∞,ye→yer。

5 數值仿真

5.1 方法有效性驗證

為驗證所提點到點控制方法有效性，同時簡化仿真計算復雜度。采用如圖3所示的平面4連桿冗余FFSR模型進行仿真驗證。

注 1：采用平面4連桿模型進行仿真主要為降低仿真計算復雜度，由于本文采用FFSR的一般模型進行研究故所提方法并不限于平面4連桿模型的點到點控制。其中，ai為連桿質心到下一關節的幾何長度；bi為前一關節到下一連桿質心幾何長度；Ii為連桿i的轉動慣量；mi為連桿i的質量；xE、yE為末端位置；qi為關節i的關節角；θ0為基座姿態角。系統的模型參數如表1所示。

桿件號ai/mbi/mmi/kgIi/(kg·m2)00.5255.66710.50.550.33320.50.540.33330.50.530.2540.130.15

給定系統初始與終止末端位姿分別為

給定系統初始末端位姿速度為0，基座初始位姿為0,關節角初始值為

qm0=[0.774 10.420 3 -1.024 8 1.247 6]Trad

控制器選取權值矩陣為

仿真采用MATLAB Spacedyn工具箱[22]，仿真時間為30 s，結果如圖4～圖9所示。其中圖4為控制力矩，圖5表示規劃以及控制運動的末端位姿軌跡曲線。圖6為末端軌跡跟蹤的跟蹤誤差，圖7為式(21)的最小奇異值變化情況，圖8為基座的位姿運動情況，圖9為終點精度控制函數曲線。圖10為優化參數狀態變化曲線。

仿真結果表明本文所提FFSR點到點避奇異運動控制方法，能夠實現FFSR末端由初始點到目標點位姿運動控制。

圖7反映了跟蹤過程中式(22)計算的GJM最小奇異值的變化情況，表明跟蹤過程在5 s前后兩次進入了預設的奇異處理區域。由于規劃目標函數式(30)中避奇異約束函數作用使得系統在很短的時間內離開了奇異區域從而保證了系統控制方法的持續有效性。圖8中FFSR的基座運動展現了FFSR基座因關節運動反作用力矩而發生位姿改變的特點。此外，圖9中終點精度曲線在5 s左右出現的跳躍變化從側面表明了當系統進行奇異回避規劃時為了保證最大的避奇異能力，規劃器適當降低了向目標運動的趨勢。圖10反映了規劃目標函數式(24)中規劃參數α的轉態變化情況，從圖中可以看到由于在規劃過程施加了參數搜索空間的約束條件，使得α的狀態曲線呈現上下界限制特點。

5.2 與廣義逆(Moore-Penrose Pseudo Invers)規劃方法的對比

由于FFSR系統具有冗余機械臂，其GJM行長大于列長，故無法對GJM直接求逆從而獲得關節角速度進行關節空間運動規劃，通常解決此問題的最簡單也是最常用方法是利用其廣義逆(Moore-Penrose pseudo invers)求解FFSR的逆運動學從而實現關節軌跡的規劃。進一步，利用適當的控制方法跟蹤規劃所得關節軌跡實現FFSR的運動控制。現就本文方法與廣義逆規劃方法在避奇異及點到點運動控制的能力進行比較。

為比較本文方法與利用廣義逆規劃方法(利用廣義逆求解逆運動學解)在點到點避奇異運動控制方法的性能。在利用廣義逆規劃方法規劃關節角軌跡時，采用與本文方法相同的期望末端軌跡即圖5中規劃末端位姿軌跡。此外，利用廣義逆規劃方法得到避奇異規劃軌跡后，需要選取合適關節軌跡跟蹤控制方法實現關節軌跡的跟蹤控制。為避免跟蹤誤差的影響，假設選取控制方法的關節軌跡跟蹤誤差為0，即利用規劃關節軌跡直接計算末端運動軌跡。

給定系統初始末端位姿速度為0，基座初始位姿為0,關節角初始值為

qm0=[0.774 10.420 3-1.024 81.247 6]Trad

仿真結果如圖11和圖12所示，其中圖11為末端位姿軌跡對比。圖12為基座變化對比。

由圖11的末端位姿軌跡結果看本文方法明顯優于直接使用廣義規劃方法，其中廣義逆規劃方法遠遠偏離了期望的末端目標位姿，這主要是由于采用廣義逆求解逆運動學時本質上是利用最小二乘法求解如下優化問題，從而獲得關節運動軌跡。

(32)

由式(32)可以看出，在利用廣義逆矩陣求解關節角軌跡時不可避免的存在規劃誤差，且當GJM存在奇異時，規劃誤差存在異常增大現象。故廣義逆規劃方法實際上是以犧牲運動控制精度為代價，實現關節期望軌跡的求解。因此，圖11中利用直接求逆法控制末端運動在終點控制精度上達不到預期效果。

此外，圖11中亦可以看到廣義逆方法在FFSR末端運動的初期與本文方法，但隨著仿真時間增長廣義逆規劃方法規劃誤差的不斷累計使得兩者逐漸產生不同最終遠離了期望目標位姿，特別的當5 s左右GJM產生奇異，由于本文方法進行了避奇異處理，而廣義逆方法因未考慮GJM奇異帶來的影響，從而產生了較大的規劃誤差。

6 結 論

1) 該方法實現了冗余FFSR系統對期望關節角速度與末端位姿的同時跟蹤。

2) 基于控制方法有效性前提條件定義了本文方法的GJM奇異性條件并提出了相應的避奇異約束函數。

3) 將跟蹤控制方法與避奇異在線規劃方法相結合實現冗余FFSR末端點到點避奇異控制且具有一定的控制精度，克服了廣義逆(奇異倒數)代替以及初始位姿調整等已有避奇異方法存在的末端控制精度損失、運動軌跡不連續和任務執行效率較低等問題。