基于互補問題描述單邊接觸的空間機器人動力學建模與數值仿真

胡天健　王天舒　李俊峰　錢衛平

1. 清華大學航天航空學院， 北京 100084； 2. 北京跟蹤與通信技術研究所， 北京 100094； ? E-mail: lgi2004@163.com

?

基于互補問題描述單邊接觸的空間機器人動力學建模與數值仿真

胡天健1，2，?王天舒1李俊峰1錢衛平2

1. 清華大學航天航空學院， 北京 100084； 2. 北京跟蹤與通信技術研究所， 北京 100094； ? E-mail: lgi2004@163.com

針對彈簧-阻尼（spring-damp， SD）并聯模型描述機器人接觸作業時需要耗時調節剛度、阻尼系數， 并在接觸末端額外安裝力傳感器等缺陷， 基于互補問題， 描述空間機械臂末端與目標的單邊接觸， 推導具有緊湊數學形式的空間機器人接觸動力學模型。采用 Lemke 算法設計動力學模型的數值計算方法， 并通過數學仿真驗證動力學模型的有效性。

動力學； 空間機器人； 單邊接觸； 互補問題

北京大學學報（自然科學版）第52卷第4期2016年7月

Acta Scientiarum Naturalium Universitatis Pekinensis， Vol. 52， No. 4 （July 2016）

空間機器人一般由飛行器平臺和空間機械臂共同組成。控制空間機械臂對目標飛行器進行操作，是完成在軌維護任務的重要方式。目前， 有關空間機械臂末端自由運動［1-3］、碰撞與慣量突變［4-6］等的動力學建模已比較成熟， 并且進行了在軌試驗驗證［7-9］。隨著空間應用的不斷深入， 以目標表面焊接、涂覆、切割等為代表的連續接觸式操作任務將成為在軌服務技術發展的新方向［10-11］， 因此， 需要完善空間機器人連續接觸的動力學建模研究。

一些學者將空間機械臂末端與目標表面的接觸等效為一個彈簧-阻尼（spring-damp， SD）并聯模型［12-14］， 通過計算臂末端與目標表面在接觸點處的相對運動， 選擇合適的剛度、阻尼系數和相應的摩擦力模型， 確定接觸力的大小。然而， 選擇和調節SD模型的剛度、阻尼系數需要一定的時間開銷，還要在機械臂末端安裝力傳感器以實時反饋接觸力的大小［15］， 因此使用SD模型描述空間機器人的接觸動力學具有一定的局限性。

針對上述問題， 一些學者基于互補問題描述剛體與剛體間的不可穿透的單邊接觸［16-18］。受此啟發， 本文假設機械臂末端與目標發生剛性單邊接觸，研究基于互補問題描述單邊接觸的空間機器人動力學模型， 以及基于Lemke算法的空間機器人接觸動力學數值仿真方法， 以期采用力學約束擺脫接觸建模對 SD 模型的依賴， 為空間機器人接觸動力學提供一種更為緊湊的數學表達。

1　空間機器人構型與坐標系

1.1空間機器人構型

空間機器人接觸作業的基本構型如圖 1 所示，包括1個基座、1個n自由度空間機械臂和 1 個目標。圖1中相關符號含義如下。

Bi（i =0， 1， …， n+1）: 系統第i個剛體， 其中B0為基座， Bn+1為目標， B1～Bn為機械臂的第1～n個剛體； 剛體質心記為Oi。

Jj（j =1， 2， …， n）: 機械臂第j個關節， 其關節轉軸矢量為kj∈R3×1， 關節轉角為θj。

bs（s =0， 2， …， n-1）∈R3×1: 由Os指向Js+1的矢量， bn為由On指向Bn末端的矢量。

at（t =1， 2， …， n）∈R3×1: 由Jt指向Ot的矢量，an+1為由Bn末端指向On+1的矢量。

1.2坐標系定義

空間機器人通過機械臂第 n 個剛體 Bn的末端與目標發生單點單邊接觸， 假設接觸點在 Bn上始終為同一個點， 而在目標Bn+1表面移動。建立本文所需坐標系， 如圖1所示， 包括以下部分。

慣性系 OIxIyIzI: 以慣性空間中某點 OI為原點建立的右手直角坐標系。

基座本體坐標系 O0x0y0z0: 以基座質心 O0為原點建立的右手直角坐標系， 坐標軸與基座固連并沿慣量主軸方向。

目標本體坐標系 On+1xn+1yn+1zn+1: 以目標質心 On+1為原點建立的右手直角坐標系， 坐標軸與目標固連并沿慣量主軸方向。

主接觸坐標系 Oc0xc0yc0zc0: Bn末端接觸點 Oc0為原點， 接觸面法向指向 Bn方向為 zc0軸， 接觸點切向速度方向為xc0軸， yc0軸與xc0軸、zc0軸成右手直角坐標系。

次接觸坐標系Oc1xc1yc1zc1: Bn+1上接觸點Oc1為原點， 坐標軸與主接觸坐標系各軸方向相反。

2　空間機器人的動力學模型

2.1運動學分析

設空間機器人各剛體質心Oi（i =0， 1， …， n）在OIxIyIzI中的位置矢量為 ri， Bn末端（即接觸點）在OIxIyIzI中的位置矢量為pe。有如下關系成立:

則機器人各剛體質心的速度為

其中， 設B0的角速度為ω0， 第i （=1， …， n）個剛體角速度ωi的表達式為

設Bi的質量為mi， 慣量陣為Ji∈R3×3， 空間機器人的動能T為

2.2動力學方程推導

設空間機器人的廣義坐標為 q = ［qBT， qJT］T∈R（n+6）×1， 其中， qB=［x0， y0， z0， α0， β0， γ0］T∈R6×1為基座的3個平動和3個轉動坐標， qJ=［θ1， θ2，…， θn］T∈R6×1為機械臂的關節轉角。空間機器人的動力學方程具有如下形式:其中， 矩陣 M（q）∈R（n+6）×（n+6）為機器人的質量陣，h（q，q˙）∈R（n+6）×1為離心力、科氏力矢量， W（q）∈R（n+6）×2為接觸約束矩陣， λ =［λN， λT］T∈R2×1為接觸約束力， λN為法向接觸力， λT為切向接觸力， FB=［FBx， FBy， FBz， FBα， FBβ， FBγ］T∈R6×1為基座控制力、力矩，τ∈Rn×1為機械臂關節控制力矩， SBT= ［I6×6； 0n×6］∈R（n+6）×6為基座控制力、力矩的選擇矩陣，SJT= ［06×n； In×n］∈R（n+6）×n為關節控制力矩的選擇矩陣。

根據第一類 Lagrange 方程， 質量陣 M（q）和離心力、科氏力矢量h（q，q˙）可由動能求得:

W（q）由約束方程給出， FB和 τ 按一定控制律給出，接觸力λ未知， 廣義加速度q˙待求。

3　基于互補問題描述單邊接觸

3.1互補問題的基本形式

機械臂末端執行器與目標表面間的單邊接觸，可能發生“接觸-分離”、“滑移-黏滯”兩類狀態轉移， 如圖2所示。

設在Oc0xc0yc0zc0中， 接觸點處法向距離為 gN，切向速度為Tg˙， gN和Tg˙的正方向分別與 Oc0zc0軸和Oc0xc0軸的正方向一致。切向力采用庫侖干摩擦模型［17］， 摩擦系數設為 μ， 并假設靜摩擦系數與滑動摩擦系數相等， 則單邊接觸狀態的判定條件為

進一步地， 定義正、負滑動摩擦余量分別為

并將切向加速度Tg˙分解為正、負切向加速度:

摩擦余量和分解的切向加速度滿足關系式

那么， 式（9）表示的空間機器人末端接觸狀態判定條件等價于如下加速度形式的互補問題:

式（12）即為描述一般單點單邊接觸互補問題的基本形式。

3.2平面單邊接觸的線性互補問題

對于平面單邊接觸， 式（12）所示的互補問題可以結合式（6）所示的空間機器人動力學方程， 化歸為線性互補問題。此時， 有關系式

那么， 接觸點位移的加速度為

設矢量ξ和η分別為

并由式（6）得到

將其代入式（17）， 結合式（12）和（13）， 整理得到描述平面單點單邊接觸的線性互補問題（linear complementary problem， LCP）， 形式為

其中， 矩陣A和矢量b分別為

因此， 具有單邊接觸的空間機器人動力學模型可由式（6）和（20）組成的微分-代數系統描述。

4　算例與仿真分析

4.1算例與算法

以一個平面受控漂浮基座上的三連桿機械臂為例， 對建立的動力學模型進行仿真驗證。算例的基本構型如圖3所示， 符號含義及取值如表1所示。

表1　符號含義及取值Table 1　Nomenclatures and values

設廣義坐標q =［x0， y0， θ0， θ1， θ2， θ3］T， 基座具有2 個平動自由度和 1 個轉動自由度， 基座控制力和力矩為FBx， FBy和FBθ， 機械臂關節力矩 τ1～τ3需預先給定。機械臂由3根勻質細長桿組成， 桿間以轉動副相連， 第 3 桿末端點 P 與無限長平面目標發生單邊接觸。

由式（6）和式（20）建立空間機器人的動力學模型， 仿真計算采用定步長 Δt=0.01 s， 初始廣義坐標和廣義速度分別設定為 q0=［0， L， 0， π/3， -π/3，arccos（3/5）］T和0˙q=［0， 0， 0， 0， 0， 0］T。采用經典的Lemke 算法［19］求解LCP問題， 得到法向接觸力 λN和切向接觸力λT， 代入由式（6）建立的狀態方程進行數值積分， 即能得到更新的廣義坐標 q 與廣義速度˙q。設矢量qk的下角標表示第k步計算量， 具體的算法流程如下。

步驟 1 （初始化）: 令 k =0， 設定廣義坐標初值q0和廣義速度初值0˙q。

步驟 2 （LCP求解）: 基于 Lemke 算法求解式（20）所示的 LCP， 得到 ξk和 ηk， 并進一步由式（18）和（10）求解法向接觸力 λNk和切向接觸力 λTk。有關Lemke算法解LCP的細節可參考文獻［19］。

步驟 3 （數值積分）: 設狀態變量 X =［q； q˙］T，將λNk， λTk及預先給定的FBk和τk代入狀態方程進行數值積分， 得到廣義坐標 qk和廣義速度kq˙。式（23）中矩陣C和矢量d分別為

步驟4 （結束判斷）: 設tol K∈N*， k=k+1； 若k≤tol K， 則返回步驟 2； 若 k＞tol K， 則結束數值計算。

4.2仿真結果分析

基于受限二次規劃方法［20-21］， 事先確定基座控制力 FBx， FBy， 力矩 FBθ以及關節控制力矩 τ， 如圖4和5所示。以此為控制輸入， 按照上述算法求解算例機器人動力學， 結果如圖6～9所示。

圖6表明， 空間機械臂3個關節轉角均隨時間連續變化， 機械臂運動狀態平穩。圖 7 表明， 在0.08～0.66 s和1.89～2.43 s兩個時間段內， 法向接觸力為零， 此時空間機械臂末端與目標平面處于分離狀態； 其余時間段法向接觸力非零且小于 8 N， 空間機械臂末端與目標平面處于接觸狀態。圖 8 表明， 在0.66～1.89 s和2.43～2.80 s兩個時間段內， 切向接觸力數值為負， 空間機械臂末端與目標平面處于接觸狀態， 且相對目標平面向 OIxI的正向運動；2.81～5.00 s 時間段內， 切向接觸力數值為正， 機械臂末端與目標平面處于接觸狀態， 且相對目標平面向OIxI的負向運動。圖9顯示空間機器人的基座質心運動軌跡、基座轉動、機械臂構型以及機械臂末端運動軌跡， 在輸入控制力、力矩作用下， 空間機器人完成了一個機械臂末端運動幅度為 0.2786 m的局部接觸作業任務， 其中短斜線是飛行器平臺質心運動軌跡。

5　結論

本文以空間機器人連續接觸作業為背景， 基于互補問題， 描述空間機械臂末端與目標的單邊接觸，推導了具有單邊接觸的空間機器人動力學模型， 并對模型的有效性進行仿真驗證。本文的研究表明，基于互補問題的空間機器人動力學模型以力學約束為基礎， 避免了采用彈簧-阻尼并聯模型調節剛度、阻尼系數的時間、資源開銷， 為描述和計算單邊接觸過程中的分離、接觸和滑移現象提供了一類緊湊的數學表達， 能夠對未來空間機器人接觸作業的工程應用提供一定的理論參考。

［1］ Umetani Y， Yoshida K. Resolved motion rate control of space manipulators with generalized Jacobian matrix. IEEE Trans Rob Aut， 1989， 5（3）: 303-314

［2］ Murotsu Y， Tsujio S， Senda K， et al. Trajectory control of flexible manipulators on a free-flying space robot. IEEE Control Sys， 1992， 12（3）: 51-57

［3］ 徐文福， 劉宇， 強文義， 等. 自由漂浮空間機器人的笛卡爾連續路徑規劃. 控制與決策， 2008， 23（3）: 278-282

［4］ Flores-Abad A， Wei Z， Ma O， et al. Optimal control of space robots for capturing a tumbling object with uncertainties. J Guidance Control Dyn， 2014， 37（6）: 1-4

［5］ Nenchev G， Yoshida K. Impact analysis and postimpact motion control issues of a free-floating space robot subject to a force impulse. IEEE Trans Rob Aut，1999， 15（3）: 548-557

［6］ Yoshida K， Nakanishi H. Impedance matching in capturing a satellite by a space robot. IEEE/RSJ Intl Conf Intel Rob Sys， 2003， 3（4）: 3059-3064

［7］ Hirzinger G， Brunner B， Dietrich J， et al. ROTEX —the first remotely controlled robot in space. IEEE IntlConf Rob Aut， 1994， 3（3）: 2604-2611

［8］ Yoshida K， Unoversity T. Engineering test satelliteⅦ flight experiments for space robot dynamics and control: theories on laboratory test beds ten years ago，now in orbit. Intl J Rob Res， 2003， 22（5）: 321-335

［9］ Talebi H， Patel R， Asmer H. Neural network based dynamic modeling of flexible-link manipulators with application to the SSRMS. J Rob Sys， 2000， 17（7）: 385-401

［10］ 陳羅婧， 郝金華， 袁春柱， 等. “鳳凰”計劃關鍵技術及其啟示. 航天器工程， 2013， 22（5）: 119-128

［11］ Yoshimitsu T， Kubota T， Nakatani I， et al. Microhopping robot for asteroid exploration. Acta Astronautica， 2003， 52（2）: 441-446

［12］ Uyama N， Nakanishi H， Nagaoka K， et al. Impedancebased contact control of a free-flying space robot with a compliant wrist for non-cooperative satellite capture. IEEE/RSJ Intl Conf Intel Rob Sys， 2012， 57（1）: 4477-4482

［13］ Rastegari R， Moosavian S. Multiple impedance control of space free-flying robots via virtual linkages. Acta Astronautica， 2010， 66（5/6）: 748-759

［14］ 徐文福， 周瑞興， 孟得山. 空間機器人在軌更換ORU的力/位混合控制方法. 宇航學報， 2013， 34（10）: 1353-1361

［15］ Caccavale F， Chiacchio P， Chiaverini S. Task-space regulation of cooperative manipulators. Automatica. 2000， 36（6）: 879-887

［16］ Glocker C， Pfeiffer F. Complementarity problems in multibody systems with planar friction. Arch App Mech， 1993， 63（7）: 452-463

［17］ Pang J， Trinkle J. Complementarity formulations and existence of solutions of dynamics multi-rigid-body contact problems with Coulomb friction. Mathematical Programming， 1996， 73（2）:199-226

［18］ Anitescu M， Potra F. Formulating dynamic multirigid-body contact problems with friction as solvable linear complementarity problems. ASME Nonlinear Dynamics， 1997， 14（3）: 231-247

［19］ Lemke C. On complementary pivot theory. Mathematics of the Decision Sciences， 1968， 8（1）: 95-114

［20］ Escande A， Mansard N， Wieber P. Hierarchical quadratic programming: fast online humanoid-robot motion generation. Intl J Rob Res， 2014， 33（7）: 1006-1028

［21］ Zhang Y， Ge S， Lee T. A unified quadraticprogramming-based dynamical system approach to joint torque optimization of physically constrained redundant manipulators. IEEE Trans Sys Man Cyb，Part B: Cyb， 2004， 34（5）: 2126-2132

Modeling and Simulation of Space Robot with Unilateral Contact Based on Complementary Problem

HU Tianjian1，2，?， WANG Tianshu1， LI Junfeng1， QIAN Weiping2
1. School of Aerospace Engineering， Tsinghua University， Beijing 100084； 2. Beijing Institute of Tracking and Telecommunication Technology， Beijing 100094； ? E-mail: lgi2004@163.com

Traditionally， the contact between the end-effector and the target is modeled as a parallel spring-damp model， which requires a time-consumed tuning of values of stiffness and damping factor and an extra force sensor fixed on the end-effector. The above drawbacks inspire the application of complementary problem to uniformly describe the unilateral contact for space robot. A dynamical equation of the space robot with unilateral contact is derived， and a numerical method is developed utilizing the Lemke algorithm. By numerical calculation of a planar 3 degree-of-freedom （DOF） manipulator fastened on a 3 DOF floating base， the effectiveness of the dynamical model is verified.

dynamics； space robot； unilateral contact； complementary problem

O313

10.13209/j.0479-8023.2016.072

國家自然科學基金（11402004）資助

2015-12-30；

2016-02-02； 網絡出版日期: 2016-07-14