空間機械臂系統軌跡規劃仿真分析

王一全，王 敏，楊 聞，王興龍

（中國空間技術研究院 通信衛星事業部，北京 100094）

0 引言

隨著世界各國空間利用的廣度和深度逐漸拓展，對航天器的研制需求不斷提高。一方面，運載火箭整流罩包絡和發射重量的限制，很難滿足超大型衛星的研制需要；另一方面，傳統衛星從發射到壽命結束的一次性工作方式，已難適應對航天器的靈活性和發射效益要求。在這種情況下，空間在軌組裝衛星系統成為未來空間系統發展的一個重要方向。相對于傳統衛星平臺而言，在軌組裝衛星系統的主要特點是可重構，包括整星構型的重構、功能的重構以及業務重構等。空間機械臂是航天器實現在軌重組技術的關鍵執行部件，用于實現目標部件的捕捉、轉移和釋放等功能，同時也可用于空間碎片清理以及空間環境探測。但是在空間微重力環境下，空間機械臂在執行任務時會對星本體產生耦合擾動，且受到空間環境中不確定因素的干擾。國內外學者對空間機械臂模型等進行了大量研究[1-6]，但給出的具體動力學模型以及相關實驗結論較少。

本文以空間三自由度機械臂為研究對象，采用適用于空間微重力環境的拉格朗日方程，推導機械臂系統的動力學方程，仿真得到不同關節力矩下各關節轉角的動力學響應，分析并驗證動力學模型的準確性，旨在為空間在軌服務的機械臂系統設計提供參考。

1 空間機械臂動力學建模

1.1 動力學建模基本假設

建模過程中對于機械臂的處理過程需要用到以下基本假設：

1）空間機械臂的臂桿視為剛性桿件，不考慮臂桿的柔性變形和振動。

2）空間機械臂的關節視為鉸鏈連接，不考慮關節間隙損耗等影響，不研究關節內部結構，只計算關節的輸出轉角和驅動力矩；關節質量作為相應臂桿質量的一部分進行計算，臂桿之間的關節處可視為機械臂末端的一個質點。

3）整個機械臂系統（含基座）均視為處在空間微重力的環境中，故在動力學建模中不考慮重力梯度力矩和其他環境力矩的影響。

4）空間目標捕捉過程分為逼近階段、捕獲階段以及穩定調整階段。由于在捕獲階段，機械臂系統會與目標物接觸碰撞，碰撞產生的影響不僅與目標物的參數特性有關，也會對整個機械臂產生一個反向作用力，使所研究問題無法討論，且該碰撞過程不在軌跡規劃任務需求中，故所討論情況只研究機械臂從初始位置平穩到達目標位置的逼近階段，不考慮運動過程中的外部碰撞等干擾。

5）在捕捉過程中，假設機械臂系統初始的角度和角速度均為0，運動到目標點時刻的速度也為0，此假設可作為動力學模型的邊界條件。

1.2 研究構型描述

由于不同構型機械臂所建模型不同，本文采用類似人手的三自由度機械臂。如圖1所示，該機械臂為具有三自由度的鏈式結構，該三自由度機械臂構型3個關節分別用3個直流力矩電機驅動，電機軸與旋轉方向已在圖中標出。

圖1 三自由度機械臂構型示意Fig.1 Schematic diagram of the 3-DOF manipulator

臂桿1：繞著豎直方向旋轉，其轉角記為θ1，運動范圍為180°，桿長記為l1，質心位置記為l1c，質量記為m1，轉動慣量記為I1；

臂桿2：在紙面內旋轉，其轉角記為θ2，運動范圍為180°，桿長記為l2，質心位置記為l2c，質量記為m2，轉動慣量記為I2；

臂桿3：在紙面內旋轉，其轉角記為θ3，運動范圍為180°，桿長記為l3，質心位置記為l3c，質量記為m3，轉動慣量記為I3。

機械臂的主要技術參數和質量參數分別參見表1和表2。

表1 機械臂主要技術參數Table 1 Main technical parameters of the manipulator

表2 機械臂質量參數特性Table 2 Quality characteristics of the manipulator

1.3 拉格朗日方程推導

拉格朗日方程是基于能量平衡的方程，相對于牛頓-歐拉公式，適合于相互約束下的多連桿運動的分析，利用廣義坐標系以功和能來表達系統運動，不做功的力和內部約束力將會自動消除，可以直接導出動力學完整形式的方程式。通常把拉格朗日函數L定義為系統的動能T和位能P之差，即

T和P可以用任何方便的坐標系來表示。由于本文研究對象為空間剛性機械臂，處于空間微重力環境下，重力不對系統產生作用，故而可不考慮其系統的位能和勢能P，只考慮動能T，因此根據式(1)求拉格朗日函數

通過推導可得最后方程形式

其中Ti為第i桿的動能；q為廣義變量；I為轉動慣量。

1.4 機械臂的動力學建模

已知各個臂桿的質心速度和質心角速度，帶入到拉格朗日方程中，可得系統的動力學方程。計算機械臂系統動能為

整理后可得簡化結果為

臂桿1的關節驅動力矩為

臂桿2的關節驅動力矩為

臂桿3的關節驅動力矩為

將以上3個臂桿的驅動力矩方程整理在一起，即得到系統的動力學方程。該方程可以表達系統各個關節的驅動力矩與各個關節轉角的角度、角速度和角加速度間的顯式函數關系。

2 動力學模型仿真

基于1.4節求出的動力學方程，可將其在MatLab軟件中進行動力學仿真，步驟為：1）設置仿真參數；2）設置仿真時間為5 s；3）設置仿真步長為0.1 s；4）設置各個初始值為0。

2.1 已知驅動力矩，求解角度和角速度

2.1.1 只存在臂桿1的驅動力矩的仿真結果

給定臂桿1的關節驅動力矩，同時令臂桿2和臂桿3的關節驅動力矩為0，所得仿真結果如圖2所示。

圖2 只存在驅動力矩1條件下臂桿轉動角度和角速度隨時間的變化Fig.2 The changes of angle and angular velocity against the time in the mere presence of driving force 1

在該種情況下，只存在驅動力矩1，可以看出臂桿1的轉動角速度是均勻遞增的，此時臂桿2與臂桿3的轉動角度和角速度均不發生變化，因為臂桿1的轉角所在平面與臂桿2和臂桿3的轉角所在平面正交，所以轉角間不會發生動力學耦合，故而不會產生相互影響，與實際相符合。

2.1.2 只存在臂桿2的驅動力矩的仿真結果

給定臂桿2的關節驅動力矩，同時令臂桿1和臂桿3的關節驅動力矩保持0，所得仿真結果如圖3所示。

圖3 只存在驅動力矩2條件下臂桿轉動角度和角速度隨時間的變化Fig.3 The changes of angle and angular velocity against the time in the mere presence of driving force 2

在該種情況下，只存在驅動力矩2，可以看出臂桿1的轉動角速度不變，而臂桿2與臂桿3的轉動角度和角速度均發生變化，而關節2與關節3的角度和角加速度互相影響；并且從圖3可以看出驅動力矩2對臂桿3的影響較大，因此臂桿2的運動很大程度上直接影響臂桿3的運動。仿真結果分析與實際相符合，這也體現了動力學模型的準確性。

2.1.3 只存在臂桿3的驅動力矩的仿真結果

給定臂桿3的關節驅動力矩，同時令臂桿1和臂桿2的關節驅動力矩為0，仿真結果如圖4所示。

圖4 只存在驅動力矩3條件下臂桿轉動角度和角速度隨時間的變化Fig.4 The changes of angle and angular velocity against the time in the mere presence of driving force 3

在該種情況下，只存在驅動力矩3，可以看出臂桿1的轉動角度和角速度同樣是不變的，此時臂桿2與臂桿3的轉動角度和轉角速度均發生變化，且轉角間存在相互影響。從圖4可以看出，驅動力矩3對臂桿3的影響較大，對臂桿2的運動也產生一定程度的影響，這體現出臂桿2與臂桿3間動力學耦合運動的結果。

2.2 已知運動軌跡反解力矩的仿真結果

1）首先給出目標點位置坐標，進行運動學反解，求出對應于目標點處各個關節轉角的目標值。

2）由給定的初值條件和運動學反解計算得到的終止條件，求出各個關節的關節轉角插值函數。

3）將所得的關節轉角插值函數進行求導得到各個關節角速度的插值函數，再求導即可得到各個關節角加速度的插值函數。

4）將所得到的各個關節角度、角速度和角加速度代入動力學模型方程(6)～(8)中進行仿真計算。

5）給出仿真時間、步長及仿真環境變量，即可得到各個關節的力矩曲線變化圖，按照該曲線控制各個關節的力矩即可使機械臂按照所設計的路徑進行運動到達目標位置。

仿真結果如圖5所示，可以看出機械臂可按照預期約束條件平穩到達指定目標點。

圖5 根據已知運動軌跡求解出的各關節驅動力矩控制曲線Fig.5 The driving torque for each joint extrapolated from specific motion conditions

3 空間點到點路徑規劃

3.1 直線插補法

通常情況的笛卡兒點到點路徑的直線插補法原理是：在已知初始點和目標點的情況下，根據插補次數和取點個數進行等間距插補。如從起始點P0(x0,y0,z0)到終點Pf(xf,yf,zf)的直線路程中間進行n次插補，中間坐標變化量記為 (Δx,Δy,Δz)，則有：

當已知時間間隔和運行速度時，插補的次數為

式中：v為運行速度；Ts為時間間隔。因此直線上各個插補點的坐標可表示為

3.2 二分逐點逼近插補法

由于等間距插補所需的插補次數多，且終止位置附近的控制精度相對低。為了提高路徑運動效率，且使得末端在逼近位置附近能更加平穩準確地捕捉目標，可采取前半段路徑快速到位，后半段路徑盡可能平穩、小擾動的路徑規劃方法。故本文提出一種新的插補方法——二分逐點逼近法，用于空間環境下快速精確地實現任務的路徑軌跡策略，也可以更好地適應空間機械臂的捕捉要求。

已知起始點P0(x0,y0,z0)和終點Pf(xf,yf,zf)，中間路徑點記為Pi(xi,yi,zi)，則有二分逐點逼近插補法的遞推表達式

式中 ξ為捕捉精度。

該方法運動效率高，且省略路徑前半段中不必要的插補點，在靠近目標位置的關鍵路徑點上更加細化精確，不僅能提高機械臂任務執行的運動效率，且在末端捕捉目標時有更高的精度。該方法同樣適用于捕捉非靜態目標物，通過逐點二分法逐漸逼近目標點直至滿足捕捉精度要求，即可捕捉該目標物。

3.3 兩種方法的仿真對比

將上述2種方法在MatLab中進行仿真，設置初始點、終止點以及規定距離，給出捕捉精度并設置相同的仿真步長，可分別得到其路徑散點圖。從起始點(0, 0, 0)運動到目標點(10, 10, 10)，路徑插補點計數為100，仿真時間以到達精度或到達目標時刻為準，步長均為1 s，仿真結果如圖6、圖7所示。

圖6 等間距直線插補法的MatLab仿真Fig.6 MatLab simulation of equi-distance linear interpolation

圖7 二分逐點逼近直線插補法的MatLab仿真Fig.7 MatLab simulation of point by point approximation for linear interpolation

對比2種軌跡規劃方法的仿真圖，等間距直線插補法的插補精度為0.17，二分逐點逼近插補法的精度數量級遠遠小于等間距直線插補法。若以精度0.1為參考，二分逐點逼近法只需要8個插補計數點精度已可達到0.067，滿足距離目標點精度要求，相比于等間距直線插補法，在運動時間和插補個數上均有顯著優化和縮減。因此不難看出：二分逐點逼近插補法具有運動效率高（即不需要很多插補點即可快速接近目標點），且越接近目標位置控制精度越高等優點，適用于高精度捕捉路徑規劃設計。

4 結束語

本文給出了空間三自由度機械臂系統的拉格朗日動力學方程，利用MatLab軟件對其進行了仿真建模，并給出已知驅動力矩求各個關節運動曲線以及已知運動情況求解各個關節驅動力矩的仿真結果。此外，根據仿真模型研究了空間環境下在軌機械臂系統的軌跡規劃問題，分別介紹了等間距直線插補法和二分逐點逼近直線插補法2種逼近策略，并對這2種軌跡規劃方法進行了仿真分析對比，結果驗證了本文所提出的二分逐點逼近直線插補法性能更優。該方法是一種創新性的路徑插補方法，經實踐驗證可用作一些空間任務中路徑逼近的插補策略，具有工程意義和價值。

以上針對空間三自由度機械臂系統所開展的軌跡規劃仿真分析工作，為后續機械臂路徑規劃研究工作提供了理論和模型基礎。