基于移動機器人的運動學半物理系統的規劃方法

劉 琛,劉 宇,宋立濱,張繼文

(1.清華大學機械工程系,北京 100084；2.北京跟蹤與通信技術研究所,北京 100094)

0 引 言

目前,空間技術已經取得了較大發展,但在軌服務、空間對接等多飛行器之間的復雜任務仍然是一個非常大的挑戰。由于空間環境的特殊性以及空間飛行器高昂的成本,在進行相關技術的研究和驗證時,往往需要先在地面進行充分的實驗[1]。利用半物理(Hardware-in-the-loop,HIL)系統進行空間技術的驗證是一種較為常見的方式[2-3],如文獻[4]中對航天器逼近階段的導航研究和半物理仿真驗證。半物理仿真系統可以結合實際硬件測試和計算機仿真,在地面搭建代仿真空間對象的等效硬件系統,并且實現完整的控制閉環,模擬真實情況下的地面控制和空間操作過程,對研究空間對接、在軌服務等衛星間的相關技術有非常重要的作用[5]。

目前國內外在半物理系統的研究和搭建方面均有一些成果。德國宇航中心開發了一套EPOS(European proximit operations simulator)系統[6-7],該系統基于兩臺機械臂和一個直線導軌搭建而成,用于研究在軌條件下的衛星之間的交會對接任務。加拿大航天局為了研究交會對接中的接觸動力學過程,搭建了主要由兩臺固定基座的機械臂組成的STVF(SPDM task verification facility)系統[8-9],此外,還對利用空間操作臂抓捕自由翻滾的衛星的全過程進行了仿真[10]。文獻[11]中同樣通過基于兩臺機械臂的半物理系統對抓捕受損衛星的過程進行研究。在國內方面,哈工大通過力補償方式構建了用于模擬捕獲目標衛星的微重力地面驗證系統[12],并利用該系統對空間操作臂抓捕自由漂浮衛星進行研究。此外,空間技術研究院[13]、中國科學院[14]等單位也對半物理系統進行了相關的研究,但從結構上來看,均是以兩臺固定基座的機械臂為基礎進行搭建。

為了滿足對空間飛行器之間的大范圍相對運動進行運動學仿真的需求,在保留現有半物理系統操作能力的同時擴大半物理系統的工作空間十分必要。目前這種被廣泛采用的基于兩臺固定基座的機械臂的結構,保證了飛行器之間的接觸動力學仿真的可靠性,但同時嚴重限制了半物理系統工作空間的大小,在仿真較大范圍的相對運動時存在一定不足。部分半物理系統通過將機械臂安裝在直線導軌上的方式擴展工作空間,但由于直線導軌自身特點,在繼續擴展其他方向平移自由度時面臨結構復雜、成本過高等一系列問題。

文獻[15]對半物理仿真系統的基本問題和解決方案進行了分析和總結,對半物理系統的設計和調試過程提供了指導意義。在此基礎上,結合移動機器人在平面運動的靈活性,如圖1所示的基于移動機器人的半物理系統的工作空間可以被極大擴展,且平面內的運動具有較高靈活性,避免了直線導軌的復雜結構設計,同時,移動機器人的引入增加了半物理系統的平移關節,增加了系統的冗余性,可以滿足復雜的大范圍的相對運動軌跡的需求,在仿真較大范圍內的空間飛行器之間的相對運動時具有較大優勢。

圖1 基于移動機器人的半物理系統整體框架Fig.1 The framework of the mobile-robots-based hardware-in-the-loop simulation system

但是,基于移動機器人的半物理系統也存在明顯的缺點：移動機器人的運動精度明顯低于機械臂等執行機構的精度,與直線導軌相比也存在一定差距,將移動機器人引入半物理系統后將面臨整體系統精度下降的問題。

針對上述問題,本文采取通過外部測量設備實時測量和機械臂運動補償的方式,在離線規劃過程中,借鑒了超冗余機械臂的規劃方法,提前為機械臂留出實時運動補償的余量,并且保證其末端的靈活運動能力,從而完成整個半物理系統的實時運動補償控制。

1 問題描述

基于移動機器人的半物理系統可以將移動機器人與機械臂等執行機構相結合,通過兩個機械臂末端的相對運動模擬空間中飛行器之間的相對運動,并且通過動力學等效的方式模擬復雜任務中的飛行器之間的接觸動力學過程。這些任務對飛行器各自的運動精度提出了較高的要求。但是移動機器人的運動存在較多不確定性,精度較差,需要通過外部測量設備實時測量和機械臂等精度較高的執行機構實時運動補償的方式提高其精度,因此在規劃過程中需要保證機械臂基本實時補償的運動能力。

基于移動機器人的半物理系統具有較多冗余自由度,對于任意相對目標位姿,均存在對應的構型子空間,因此可以額外設計性能指標,在每一個構型子空間中根據性能指標計算最符合要求的構型,從而保證整個仿真過程的順利進行。

2 算法框架

為了實現在規劃過程中優化基于移動機器人的半物理系統的操作能力、避關節極限性能,為后續的運動補償環節創造條件,本文采取了一種基于超冗余機械臂的規劃方法,通過一套完整的方法簡化當前問題,提出的規劃方法框架如圖2所示。

圖2 算法框架Fig.2 The framework of the proposed algorithm

具體步驟如下：

1)通過運動學等效的方式,將基于移動機器人的半物理系統等效為一個串聯超冗余機械臂。

2)根據該超冗余機械臂的物理參數建立其運動學模型。

3)建立性能指標,并將該性能指標引入運動學模型中。

4)針對稀疏位姿點,利用五次多項式插值及球面線性插值等方式進行插值計算每個時刻的操作空間位姿及速度。

5)根據帶性能指標的運動學公式計算該時刻的關節空間構型及速度。

6)循環計算整條軌跡。

7)輸出規劃結果。

3 運動學模型的等效與建立

本文中的規劃問題均基于整體系統等效而成的超冗余機械臂,該超冗余機械臂的構建方法如下：以飛行器1為超冗余機械臂的基座,飛行器2為超冗余機械臂的末端,將移動機器人之間的相對運動等效為兩個平移關節和一個轉動關節的運動。此時,半物理系統被抽象為一個具有多自由度的超冗余機械臂,具有多個平移和轉動關節,給定的空間飛行器之間的大范圍相對運動軌跡即為超冗余機械臂的末端相對于基座的運動軌跡。等效過程示意圖如圖3所示。

圖3 超冗余機械臂等效圖(僅用于表示等效過程,不代表真實關節類型及數量)Fig.3 The equivalent process of the super redundant manipulator (only used to represent the equivalent process,not the real type and number of joints)

由于該機械臂高度冗余,對于任意給定的操作空間下的末端位姿,均有對應的無窮組關節構型。這些關節構型構成了該超冗余機械臂構型空間的一個子空間,在該子空間內可以根據特定目的設計性能指標以尋找對應的最優構型[16]。

此時,根據Denavit-Hartenberg方法建立超冗余機械臂各關節連桿的坐標系,根據該超冗余機械臂關節尺寸的參數,可以確定其D-H參數表,進而根據D-H參數表可以建立超冗余機械臂的運動學模型：

(1)

(2)

其中:J+(θ)為當前關節構型的雅可比矩陣的偽逆;(I-J+(θ)J(θ))為當前關節構型的雅可比矩陣的零空間;φ為任意優化向量;(I-J+(θ)J(θ))φ被稱為當前構型的超冗余機械臂的自運動。

在對超冗余機械臂求解過程中,考慮到機械臂需要對移動平臺的運動誤差進行補償,在對超冗余機械臂進行規劃時需要在保證末端位姿準確的情況下,使超冗余機械臂的操作靈活性盡可能高、各關節角度盡可能遠離關節極限,因此這里選定可操作度ω[17]與避關節極限系數h為性能指標,其定義分別為：

(3)

(4)

可操作度是機械臂的末端可操作度橢球的體積,當可操作度越接近零表示當前構型的末端可操作度橢球的某一個軸長度接近于零,該機械臂即將喪失沿該軸方向的運動能力,即奇異位形。計算可操作度的矢量梯度[18]可以得到：

(5)

(6)

其中:A=J(θ)JT(θ)=(aij)n×n,θi為超冗余機械臂的第i個關節角度。計算避關節極限系數的矢量梯度可以得到：

(7)

(8)

(9)

其中:a,b為調節系數,J和J+分別為當前關節構型對應的雅可比矩陣以及雅可比矩陣的偽逆。對公式進行離散化可以得到：

(10)

進一步整理可以得到:

θt+1-θt=J+(Pt+1-Pt)+α(I-J+J)·

(11)

其中:α為時間調節因子。此時即可根據上述公式并且結合當前關節構型計算下一時刻的構型,并且可以一定程度上保證超冗余機械臂的遠離關節極限性能和可操作度性能。

4 軌跡規劃方法

為了保證超冗余機械臂的末端運動按照給定的相對運動軌跡運動,這里采取在超冗余機械臂的操作空間內直接進行規劃的方式。通常情況下,給定的相對運動軌跡為一系列較為稀疏的路徑點,每個路徑點均包含時間、位姿、速度及加速度。為了實現操作空間內的較為平滑的運動,需要對路徑點之間進行插值以計算密集的插值點信息。路徑點之間的位置信息采用對每個方向的坐標分別進行五次多項式插值,姿態信息采用姿態四元數的球面線性插值(Spherical linear interpolation,SLERP)[19]與直線插值相結合的方式,插值方式為等時間間隔插值。該規劃過程具體如下：

對于任意相鄰路徑點,將相鄰路徑點之間的各方向位置坐標分別進行五次多項式插值[20]。五次多項式插值的表示形式為：

S(t)=At5+Bt4+Ct3+Dt2+Et+F

(12)

其中:S(t)為插值得到的關于時間的五次多項式,A,B,C,D,E,F分別為五次多項式的各階系數。根據插值結果可以得到相鄰路徑點之間的任意插值點的位置向量[px,py,pz]T和速度向量[vx,vy,vz]T。

相鄰路徑點之間任意一個插值點的姿態四元數可以根據如下公式計算：

qk=

(13)

其中:q表示姿態四元數,Ω=arccos(qt·qt+1)為相鄰路徑點姿態四元數的夾角,·表示四元數乘法。θthreshold為直線插值與球面圓弧插值的臨界角度。根據四元數運算規律,對于任意四元數q,有:

(14)

此時,經過整理可以得到當前插值點的轉動速度：

(15)

其中:q*為當前姿態四元數的共軛,[ωx,ωy,ωz]T為機械臂末端在笛卡爾坐標系下的沿各個方向的轉動角速度。

圖4 在各構型子空間中規劃的示意圖Fig.4 The schematic diagram of the planning in each configuration subspace

5 實驗系統搭建及結果分析

為了對上述規劃方法進行驗證和分析,本文設計并搭建了一套基于移動機器人的半物理系統并基于該系統對本文提出的方法展開詳細分析與驗證。該基于移動機器人的半物理系統整體結構示意圖如圖5所示。

圖5 基于移動機器人的半物理系統設計圖Fig.5 The overall design of the mobile robots based hardware-in-the-loop simulation system

該半物理系統主要分為三個主要組成部分：動作捕捉子系統、飛行器1子系統和飛行器2子系統。動作捕捉子系統可以提供實時精確測量結果,其原理為通過一定數量固定的高速相機捕捉粘貼于實驗環境中物體上的標志點位置,并且根據標志點相對位置關系解算出試驗場地內目標物體的實時精確位置姿態[21],同時測量頻率可以達到120 Hz,基本滿足實時性要求。該系統主要用來實時監測半物理系統中的移動機器人的運動,提供實時精確外部測量數據,結合控制及運動補償部分改善移動機器人的運動精度。飛行器1子系統主要包括移動機器人、并聯機構及飛行器1模型,飛行器2子系統主要包括移動機器人、機械臂及飛行器2模型。移動機器人基于麥克納姆輪設計,具有完整非約束的三個自由度,可以在平面內保證靈活性的自由運動。并聯機構為Stewart構型,用來小范圍調整飛行器1的位姿,機械臂為七軸冗余機械臂,可以在一定范圍內對飛行器2的位姿進行調整。

在上述整體系統框架下,基于ROS搭建了該基于移動機械臂的半物理系統,如圖6、圖7所示。圖8所示為動作捕捉系統軟件中的高速相機布置以及測量得到的移動機器人剛體位姿。各個子系統以及集控臺之間通過ROS的多機通信框架實現信息交互。子系統上均搭載微型計算機完成低層次的規劃和控制,保證控制頻率,微型計算機和集控臺之間通信,通過集控臺協調實現整體系統的控制。整體系統的軌跡規劃過程在集控臺上離線完成。該半物理系統的工作空間不會受到系統本身的限制,但由于實驗場地的限制,實際半物理系統的仿真空間為所用實驗場地的可用空間。

圖6 飛行器1子系統實物圖Fig.6 The subsystem for the aircraft 1

圖7 飛行器2子系統實物圖Fig.7 The subsystem for the aircraft 2

圖8 動作捕捉系統及測量到的移動機器人位姿Fig.8 The motion capture system and the measured pose of the mobile robots

針對上述基于移動機器人的半物理系統,根據第3節中的運動學等效方式將其等效為超冗余機械臂,移動機器人之間的平面內的運動等效為兩個平移和一個旋轉共三個自由度,由于并聯機構的六自由度運動的耦合性及一般情況下僅小范圍調整飛行器1的俯仰角度的功能,這里僅將并聯機構等效為超冗余機械臂中的一個轉動關節。此時,根據上述等效過程得到的超冗余機械臂的物理參數,建立該超冗余機械臂各連桿坐標系,如圖9所示,以及D-H參數表,見表1。

表1 超冗余機械臂D-H參數表Table 1 D-H parameters table for super redundant manipulator

圖9 超冗余機械臂的連桿坐標系Fig.9 Joint coordinate system of the super redundant manipulator

為了直觀地對上述規劃算法進行性能分析,這里給定一條典型路徑用來通過實例驗證算法性能。該路徑分為兩個階段,第一階段為飛行器2相對于飛行器1做直線運動,第二階段飛行器2以飛行器1周圍一點為圓心進行繞飛,第一階段持續時間為10 s,第二階段持續時間為10 s。其定義分別如下：

第一階段的平移部分軌跡為:

姿態四元數為[0,0,0.707,0.707]T并且保持不變。第二階段的平移部分軌跡為:

初始姿態四元數為[0,0,0.707,0.707]T,最終姿態四元數為[0,0,0,1]T,第二階段全過程中機械臂末端勻速轉動。

對上述軌跡進行等時間間隔均勻采點并根據軌跡公式進一步計算生成目標軌跡點,軌跡點之間的時間間隔為1 s。對生成的路徑點使用上述規劃及優化逆解方法進行求解和規劃,可以得到如圖10所示的各關節規劃結果。圖中優化前表示的數據為式(1)中的優化向量φ為零向量時獲得的規劃結果,優化后所表示的數據為根據式(10)中的方法計算得到的數據。由于關節2、3、4的運動由移動平臺完成,并且未列入優化向量中,規劃結果幾乎完全相同,因此并未直接畫出關節2、3、4三個關節的角度。從關節1及關節6的數據可以看出,相比較于未經過優化的方法,經過優化的規劃方法可以有效避免關節角度超限,同時對于其他原本遠離關節極限的關節,在原有位置的基礎上基本保持不變。因此,規劃結果驗證了該方法為后續控制過程中為移動機器人的運動誤差提供運動補償預留了足夠余量的結論。

圖10 各關節的規劃結果Fig.10 The planning results of every joint

在實際搭建出來的半物理系統上執行上述規劃結果,在實驗過程中,移動機器人、機械臂及并聯機構均完全根據規劃結果通過自身控制其進行運動,并未通過機械臂和并聯機構對移動機器人的運動進行誤差補償。移動機器人通過實時獲取動作捕捉系統的測量數據并進行閉環控制,其運動曲線如圖11所示,可以看出,移動機器人的各個方向的運動誤差范圍約為±10 mm。實驗過程中的半物理系統如圖12所示。

圖11 飛行器1子系統中的移動機器人運動曲線(上)和飛行器2子系統中的移動機器人運動曲線(下)Fig.11 The motion curve of the mobile robot in aircraft 1 subsystem (upper)and the motion curve of the mobile robot in aircraft 2 subsystem (lower)

圖12 運行過程中的半物理仿真系統Fig.12 The hardware-in-the-loop simulation system during the experiment

利用動作捕捉系統對實驗過程中兩個子系統的移動平臺位姿進行測量,并且結合機械臂及并聯機構高精度編碼器的測量數據,經過計算后可以得到兩個飛行器模型之間的相對位姿數據,將測量數據與規劃結果、期望位姿點進行比對,結果如圖13所示。期望路徑點表示對本文實驗所用的典型路徑直接采樣生成的目標軌跡點,規劃結果表示本文算法規劃得到的結果對應的超冗余機械臂的末端相對于基座的相對位姿數據,實際運動表示使用本系統進行仿真的過程中飛行器2模型相對于飛行器1模型的相對位姿數據。可以看出,在本文提出的規劃算法的基礎上,該半物理系統可以連續、平穩地再現飛行器模型之間的相對運動,兩個飛行器模型之間的相對運動與期望的運動存在一定誤差,各方向的位置誤差范圍約為±15 mm,姿態角偏差范圍約為±0.5°。對整體系統的運動誤差來源進行分析可以發現,移動機器人的誤差為當前系統運動誤差的主要來源,另外,動作捕捉系統的測量坐標系的X-Y平面與實際實驗場地的地面并未完全重合,導致移動機器人以及飛行器模型的運動出現了豎直方向上的運動偏差。因此,在當前情況下使用該半物理系統進行半物理仿真時,運動誤差較大,不能滿足精度要求,需要在后續過程中通過機械臂和并聯機構對移動機器人的誤差進行實時補償,以提高運動精度,這也驗證了本文提出的算法的重要性。

圖13 規劃和實驗結果對比Fig.13 Comparison between the planning and experiment results

6 結 論

本文針對能在極大范圍內擴展工作空間的基于移動機器人的半物理系統進行研究,面向引入移動機器人后可能引起的精度下降的問題,在規劃層面提前預留空間,為后續通過高精度外部測量設備反饋和各子系統之間地實時運動補償創造了條件。主要工作可以分為以下三點：

1)設計了一套基于超冗余機械臂的整體半物理系統的規劃方法,將半物理系統中的移動機器人和機械臂等效為一個串聯超冗余機械臂,利用超冗余機械臂的操作空間規劃方法對系統進行規劃。

2)設計了用于改進特定性能的超冗余機械臂逆解求解方法,求解時最大化超冗余機械臂的可操作度并且避免關節極限,為后續控制過程中的機械臂主動補償移動平臺誤差創造了充足的條件。

3)在自行搭建的基于移動機器人的半物理系統上進行了實驗驗證,實驗結果驗證了所提出的規劃方法的必要性和有效性。