空間機械臂動力學特性的仿真分析研究

韓東宇，陳歡龍，余春華

(1. 南京理工大學 理學院，江蘇 南京 210094；2. 上海宇航系統工程研究所，上海 201109)

0 引言

航天飛行器的研發設計中，往往無法全面考慮外太空惡劣工況下所出現的突發狀況，在航天器在軌運行中，也容易發生表面結構脫離，燃料不足，模塊單元分離或預設性動作失諧等情況，這些通常需要空間機器人的協助操作。特別是遇到大型空間組裝及搬運任務時，這些在失重環境下的程序性動作顯得復雜而難以滿足功能性需求。因此空間機器人的研制對于在軌操作有重要的意義。

為實現航天器研發及運行過程中所需的維修、加注、回收等有效利用，同時為減少突發勞損等航天器非自然破壞而引發的高昂成本，以及減小未來宇航員可能發生的艙外作業的危險性，空間機械臂的研究逐漸進入各個國家航天領域的視野[1-2]。

圖2 平移關節鎖定時的機械臂D-H坐標系Fig.2 D-H coordinate system of manipulator with translational joint locking

空間多自由度機械臂的研究除了需要克服失重條件，另一個難點在于漂浮基座空間位置的不確定性，即基座的可移動性，使機械臂的運動多了移動導軌或者氣浮平臺這個冗余自由度，在完成規定動作的目標任務時，如何分配運動指令到各個關節和移動基座，成為核心的技術指標[3]。另外，衛星基座的運動還要考慮到多種非確定性擾動因子。這就使通常的數學解析或全物理實驗都受到限制[4]。數學解析或純數字仿真無法真實還原太空環境，尤其是瞬間碰撞的數據無法通過建模準確測出；而全物理實驗中的重力補償與完全失重區別較大不能準確模擬，所以國際上目前最新的方法多是采用數學仿真加實驗平臺的半物理方法[5]。

用數學仿真完成動力學過程，將運動指令直接賦給實驗平臺的電機驅動，將接觸碰撞的力學數據反饋給仿真控制平臺，進行運動補償，從而實現空間機械臂移動路徑的跟蹤規劃。而數學仿真的主要困難在于多剛體系統的動力學特性分析。目前機械臂的研究分析一般從結構學、運動學及動力學三方面展開[6]，運動學方程和動力學方程是機械臂運動規劃的基礎。

對多關節運動機構的研究，通常方法是分別在每個坐標系上建立空間坐標系，利用齊次坐標轉換的方法，求解出每個關節的位置和姿態，直到末端執行機構的位姿[7]，即Denavit-Hartenberg方法，但是建立坐標系方案的細節也有很多種[8-9]。

本實驗研究的對象為星體基座帶三臂的空間機械臂系統，即給定各個關節值求末端操作器的物理量。主要困難仍然是由于自由飄浮基座的位置在關節角變化過程中同時變化，不能簡單用地面固定基座的方法求解，需要綜合考慮基座位姿變化帶來的影響[10]。在研究空間或移動基座機械臂問題上，較為通行的方案是在固定基座機械臂的運動學方程基礎上，乘以一個基座隨移動導軌運動的參數矩陣，從而全面反映整體自由度坐標系的運動特性[11-12]。但此方法易出現冗余自由度，難以實現運動指令分配的自動化[13]。而這一問題的解決主要依靠閉環控制系統完成，通過機械臂的動力學仿真反饋運動數據，進行運動補償，以實現運動分配自動化。

本文將探討運用SimMechanics工具搭建動力學系統平臺，進而封裝成子模塊，嵌入到控制系統中調試，進行路徑規劃，并加載到仿真機中運行。并應用Matlab和ADAMS作聯合仿真[14]，進行動力學特性的對比分析。

1 機械臂移動路徑系統的原理模型

1.1 參數化多臂系統建模

機械臂抓捕系統動力學建模主要包括三個部分：多機械臂系統的建模、操控飛行器平臺的建模以及操作目標飛行器的建模。機械臂抓捕系統的動力學模型示意圖如圖1所示。

圖1 機械臂抓捕系統動力學模型示意圖Fig.1 Schematic diagram of the dynamic model of robotic arm arresting system

空間操控機械臂共有9個關節，采用3-1-1-1-3的偏置式布局方式，9個關節中包含7個轉動關節和2個移動關節，其中各轉動關節的關節變量分別為θ1，θ2，θ3，θ4，θ5，θ6，θ7。當機械臂執行抓捕任務時，需將兩個平移關節鎖定，此時坐標系定義如圖2所示。將機械臂的展開狀態定義為初始參考構型。

利用機構動力學仿真分析商業軟件ADAMS，建立全參數化的多機械臂系統動力學模型。利用Matlab/Simulink與ADAMS聯合仿真，進行運動學、動力學以及平臺和多臂系統的協調控制分析。其核心問題在于核驗仿真結果的正確性，所以要基于數學動力學方程的推算與對比。參數化機械臂動力學模型的展開和收攏效果示意如圖3所示。

圖3 機械展開和收攏效果示意圖Fig.3 Schematic diagram of mechanical unfolding and folding

1.2 機械臂抓捕動力學仿真分析

機械臂動力學仿真分析主要包括以下幾個部分：機械臂位姿反解、關節空間的路徑規劃、笛卡兒空間的路徑規劃、飛行器的平臺控制、機械臂與平臺的協同控制、末端工具位姿調整等。

機械臂路徑規劃、位姿反解程序、平臺姿態控制在Matlab/Simulink中編寫程序實現，機械臂動力學模型在ADAMS中實現，通過ADAMS/Control模塊與Simulink控制程序進行信息交互，Simulink向ADAMS輸入機械臂每個關節的運動指令和操控飛行器平臺的控制力矩，ADAMS向Simulink反饋狀態量，完成聯合仿真。

1.3 機械臂的路徑規劃與抓捕及其主要問題

對機械臂進行路徑規劃時，機械臂的基座坐標系與抓捕目標坐標系是相對固定的，因此需要將操控飛行器平臺與大地固定連接，而且需要鎖定機械臂的兩個平移關節，并鎖定第二個轉動關節。

考慮單臂的路徑規劃。機械臂處于初始構型時，7個轉動關節相對于D-H坐標系下的關節角為[180° 0° 70° 90° -70° 0° 45°]。

由此可實現以下全過程的仿真分析：機械臂末端工具由起始位置到目標特定點的各個關節的空間路徑規劃；機械臂末端到達預期位置和姿態時，末端執行機構接收指令開始工作，實施抓捕動作。

其中主要問題在于核驗仿真分析的正確性，首先對仿真所基于的數學原理進行分析，之后再運用兩套仿真系統的結果數據作對比。

2 機械臂系統動力學建模

2.1 算法原理模型

通過對比國內外常用機械臂建模方法的優劣，結合研究方向，采用基于Lagrange的建模方法。

基本假設：本實驗研究的機械臂系統為剛體系統，且各零件之間彼此剛性連接；機械臂與載體及每條機械臂的臂與臂之間只有一個轉動關節；忽略重力梯度影響。

2.2 運動學方程

采用基于D-H參數法的正運動學建模，正運動學方程可以描述相鄰連桿坐標系之間的相對空間關系，從而可以根據每個連桿之間的相互關系，由基坐標系遞推到末端坐標系的表達式，即可推導出正運動學方程。

考慮基座慣性姿態由俯仰角θb、滾動角φb和偏航角ψb給出，則從慣性坐標系到基座坐標系的坐標轉換矩陣為

(1)

三條剛性臂的D-H參數一致，其中O0-x0y0z0坐標原點與安裝坐標系原點轉換矩陣為固定值。

對于任意一個滿足前述假設的多自由度運動體系，定義各種運動學參數如下。

R安裝坐標系原點Oc關于大地坐標系O0x0y0z0的位置矢徑。

v0安裝坐標系原點Oc關于大地坐標系O0x0y0z0的瞬時速度。

基于上述定義，可以給出多自由度上任一關節點的角速度和角加速度，以及任一點的位置，進一步可以求出其速度和加速度。

第i組機械臂中第j節臂的質心位置為

(2)

第i組機械臂中第j節臂的質心線速度為

(3)

第i組機械臂中第j節機械臂的質心角速度為

(4)

2.3 動力學方程

三臂多自由度機械臂系統的拉格朗日公式為

(5)

系統的動能為

(6)

為計算方便將T寫成如下形式

T=T0+T1+T2

(8)

(9)

(10)

(11)

系統的廣義坐標選為

(12)

式中：θT為角位移矩陣。

根據需要改寫為

式中：δ0為本體的歐拉角，q(m)表示第m臂的關節角，系統動力學方程的形式為

(16)

式中：C為一個N×1維矢量，包含了微重力環境下所有的非線性速度項；Q為一個N×1維廣義力矢量。H,C和Q為線位移σ0和角位移θ以及其一次求導的方程式。如除本體外，施加在系統其他部分的外力為零，則Q可以表示為

(17)

式中：0fs和0ns為施加在飛行器本體上的靜力；JQ為一個N×N的雅克比矩陣；τK×1為施加在各個臂的每個關節的控制力矩。為了獲得多臂多自由度空間機械臂的顯式動力學表達形式，且減小計算的復雜度，在求解質量矩陣H、非線性速度項C和廣義力Q之前，需要做一些預計算處理。

質量矩陣H是加速度項的系數，經過計算得到的Hij為

(18)

此處左上標代表的是進行微分的坐標系，如果缺省表示慣性坐標系。

式中的非線性速度項為

(19)

式中：C1為廣義位移一次求導的系數矩陣；C2為廣義位移的常量矩陣；而

(20)

(21)

如果除了本體，施加在系統其他部分的外力為零，雅克比矩陣JQ由下式決定，即

(22)

式中：T0為本體坐標系到慣性坐標系的轉換矩陣，S0為歐拉角速率到角速度的轉換矩陣；Ik×k為k階單位陣。定義如下

(23)

0ω0為本體坐標系下的角速率矩陣。

至此，便完成了完整的動力學求解過程。

3 基于Matlab與SimMechanics模塊的動力學算法驗證

3.1 動力學模型的構建

這里利用Matlab中的Simulink和SimMechanics作機械系統的建模和仿真分析，并將其模型的實時處理結果加載到仿真機中做硬件在回路的仿真。

SimMechanics內嵌于Simscape中，并擴展了其三維機械系統建模的能力，并應用sensor和source模塊將其拓撲模型的物理信息反饋于控制系統，實現對閉環控制算法的開發。

由于每一個支鏈的結構都是相同的，并且支鏈內部的各個參數涉及轉動方向和移動方向的計算。坐標系以模型的初始狀態為基礎，根據D-H參數，為方便調試，每條支鏈在實體模塊中建立一個過渡坐標系CS3,并用變量名表示。

Position標簽下的CS1、CS2、CG、CS3根據初始狀態計算，CG為每個實體初始重心，CS1為該桿與固定平臺或上一實體連接的鉸點坐標，CS3為初始狀態下該桿的長度，CS2為理論上與移動副連接的點坐標，因此，把它和CS1的Show Port勾選。方括號中的實際坐標值根據參考的坐標系，結合D-H參數實際測量后填寫。變量采用結構體(Struct)形式。Orientation下的CG、CS1、CS2和CS3，主要是運動副坐標系的旋轉方式，具體參數值的設置主要根據D-H參數定義的旋轉變換矩陣。同時將每個臂節的質量慣量參數設計成變量輸入到對話窗的Mass和Inertia一欄中。

定義好一個臂節的參數后，就將其封裝成一個子模塊，嵌套進一個七自由度機械臂的模塊，最后一個臂節的實體中CS2的參數涉及到末端位置姿態數據輸出，要添加額外的關節傳感器接口。

圖4為單只機械臂一個臂節的內部模塊選用及構架圖，相應的英文名稱為具體選取的模塊類型，主要包括actuator(驅動模塊)，用于傳輸外部驅動數據如速度、力、力矩等，通過圖中的接口drivermo由外部控制系統輸入；sensor(實體傳感器模塊)測出revolute(關節模塊)的運動數據包括角度angle、角速度avelocity，并通過該名稱的接口反饋給外部控制系統，eye為用戶對相應實體的定義，因末端抓捕器含手眼視覺，所以定義為“eye_00”，實體內部參數的定義上面已經說明，當勾選“Show Port”時，將作為接口連接其他臂節，CS3未勾選，只作為過渡坐標系統，便于不同型號下D-H參數的輸入。

圖4 單只臂節的內部架構Fig.4 Internal structure of single arm

圖5 一條機械臂的內部架構Fig.5 Internal structure of a mechanical arm

圖6 整個平臺動力學的架構Fig.6 Architecture of the whole platform dynamics

圖5為七個臂節通過名為“conn”的接口首尾相接構成一個完整的機械臂arm，并和其余三個機械臂共同接于ground坐標系下的星體安裝接口。其中每一個模塊為一個臂節，由圖4的臂節詳圖縮合而成，其輸入端口需從控制系統取得每個臂節的驅動參數，由driver接口輸入，并通過鍵標指號“D_A～D_G”引導，其輸出端口由“avelocity”“angle”端口向控制系統反饋每一個關節的角速度和角度運動參量，通過“A1～G1”“A_2～G_2”引導。

最后將完全相同的三條七自由度機械臂根據接口位置定義連接到操控飛行器平臺上，如圖6所示。將機械環境變量中的重力輸入選項勾掉，設置固定基座的中心坐標默認為零，平臺與其連接的關節選用Six-Dof 六自由度連接，表示飛行器完全不受約束，其中“Env”代表機械環境變量，ground為慣性坐標系，XY_MoForce為控制系統傳入的飛行器平臺的驅動數據，CS1接口為要向控制系統輸出星體各項運動參量的接口，包括位置p，速度v，角速度ω，姿態矩陣R和加速度a；并可通過顯示器將其數據顯示為隨時間變化的運動曲線。對于其他各條機械臂，除了關注各個關節運動數據外，還特別關注末端的運動數據,包括位置、姿態、移動速度、角速度。

SimMechanics模塊最好由每個模塊本身功能及原始英文加編號定義，便于Simulink中的控制系統以及和其他軟件的聯合調用，切不可譯成中文，否則會出現調用失敗。

將整個平臺動力學SimMechanics模塊搭建好后，就可以設置輸入輸出接口，然后串行到控制系統中，接收外部輸入的驅動信號、輸出位置、速度、姿態等反饋信號，進行控制調試。

3.2 正逆運動學以及正動力學的驗算

將正逆運動學方程由3.2節推導的公式編入程序，寫成m文件，或寫入Embeded函數中，輸入不同的關節角指令，查驗每個關節的位置姿態矩陣，并將運行結果輸出到workspace，和SimMechanics每個實體的傳感器sensor檢測到的運動參數作比對，監視其輸出結果，核驗動力學模型的正確性。

為核驗動力學模型的正確性，將ADAMS和SimMechanics搭建的動力學模型的運算結果進行比對，在第一個關節角處輸入一個正弦力矩驅動，查驗各關節角度、角速度以及角加速度10 s內運行的變化曲線，仿真結果如圖7～9所示。

圖7 單只機械臂各個關節角10 s內的變化Fig.7 Variation of each joint angle of the single arm in 10 s

圖8 單只機械臂各個關節角角速度10 s內的變化Fig.8 Variation of angular velocity of each joint angle of the single arm in 10 s

圖9 單只機械臂各個關節角角加速度10 s內的變化Fig.9 Variation of acceleration of each joint angle of the single arm in 10 s

經過核驗，發現在不同關節輸入不同驅動下，其運算后的角度、角速度及角加速度曲線與ADAMS驗算的仿真曲線基本一致。圖10～11為在星體上的第一安裝坐標系原點上輸入一個正弦驅動后，其末端的工況曲線，包括機械臂末端相對于本體基座的姿態角(圖10)，相對于平臺的角速度(圖11)。從而驗證了兩種動力學模型構建的正確性。

圖10 機械臂末端相對于平臺的姿態角Fig.10 Attitude angle of the manipulator relative to the platform

圖11 機械臂對于平臺的相對角速度Fig.11 Relative angular velocity of the manipulator to the platform

4 結束語

本文首先應用拉格朗日公式推導了空間平臺帶3個機械臂的運動學方程和動力學方程，并利用SimMechanics工具箱以及ADAMS軟件分別實現了對空間操控飛行器帶三個七自由度機械臂的動力學模型的搭建，然后通過算法程序計算了末端執行器的靜位姿態，核驗了運動學理論方程的正確性，再通過在不同的關節處輸入不同的關節驅動，觀察各個自由度的工況曲線，核查動力學模型的正確性。

動力學模型的搭建，實現了對空間機械臂在軌操作和抓捕等的仿真和運動模擬，為研究空間機器人問題奠定基礎，并可嵌入到控制系統中，實現閉環聯合仿真測試，也可以為進一步半物理實驗做好基礎準備工作。

該多自由度機械臂動力學特性的理論分析和數學仿真是機器人學應用的核心問題。在多體動力學的諸多領域，該多自由度機械臂有廣泛的應用前景：航天領域如在軌操作、艙外維修等；醫學、建設、機械、生活領域，如醫用助力臂、車間沖壓機械手、自動焊接設備、吊車抓捕、家用洗碗機等，建立模型時只需在機械環境變量中設置重力慣性環境。

[1] 翟光, 仇越, 梁斌, 等. 在軌捕獲技術發展綜述[J]. 機器人, 2008, 30(5): 467-477.

[2] 黃渙. 空間機器人功能行為建模與飛行任務仿真[D]. 長沙: 國防科技大學, 2009.

[3] 趙陽, 張大偉, 田浩, 等. 飛行器在軌任務[J]. 上海航天, 2009, 26(3): 38-41.

[4] SHAO Z F, TANG X Q, CHEN X, et al. Inertia match of a 3-RRR reconfigurable planar parallel manipulator [J]. Chinese Journal of Mechanical Engineering, 2009, 22(6): 791-799.

[5] 胡俊峰. 柔性機器人機構動力學分析及振動控制研究[D]. 廣州: 華南理工大學, 2010.

[6] NIKU S B. Introduction to robotics analysis, control, applications[M]. 2th ed. Cincinnati: Prentice Hall, 2001.

[7] 黃獻龍, 梁斌, 陳建新, 等. EMR系統機器人運動學分析和求解[J]. 宇航學報, 2001, 22(2): 18-25.

[8] 梁斌, 劉良棟, 李庚田, 等. 空間機器人的動力學等價機械臂[J]. 自動化學報, 1998, 24(6): 761-767.

[9] 王鴻鵬, 洪炳熔. 自由飛行空間機器人通用運動學模型及其仿真[J]. 宇航學報, 2000, 21(3): 45-51.

[10] 胡松華, 薛力軍, 強文義, 等. 空間機器人系統動力學建模與控制仿真研究[C]∥中國控制與決策學術年會論文集.南京: [s.n.], 2005: 1978-1987.

[11] 于登云, 孫京, 馬興瑞. 空間機械臂技術與發展分析[J]. 航天器工程, 2007, 16(4): 1-8.

[12] 范成建, 熊光明, 周明飛. 虛擬樣機軟件 MSC. ADAMS應用與提高[M]. 北京: 機械工業出版社, 2006.

[13] 張國亮. 空間機器人遙操作中基于視覺的局部自主控制研究[D]. 哈爾濱: 哈爾濱工業大學, 2010.

[14] ROVNER R. SimMechanics 3 getting started guide[M]. Massachusetts: Intel Robot System, 2009: 1-199.