胡天健　王天舒　李俊峰　錢衛(wèi)平

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

?

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

胡天健1，2，?王天舒1李俊峰1錢衛(wèi)平2

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

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

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

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

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

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

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

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

1　空間機器人構(gòu)型與坐標系

1.1空間機器人構(gòu)型

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

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

Jj（j =1， 2， …， n）: 機械臂第j個關(guān)節(jié)， 其關(guān)節(jié)轉(zhuǎn)軸矢量為kj∈R3×1， 關(guān)節(jié)轉(zhuǎn)角為θ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的末端與目標發(fā)生單點單邊接觸， 假設(shè)接觸點在 Bn上始終為同一個點， 而在目標Bn+1表面移動。建立本文所需坐標系， 如圖1所示， 包括以下部分。

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

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

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

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

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

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

2.1運動學分析

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

則機器人各剛體質(zhì)心的速度為

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

設(shè)Bi的質(zhì)量為mi， 慣量陣為Ji∈R3×3， 空間機器人的動能T為

2.2動力學方程推導

設(shè)空間機器人的廣義坐標為 q = ［qBT， qJT］T∈R（n+6）×1， 其中， qB=［x0， y0， z0， α0， β0， γ0］T∈R6×1為基座的3個平動和3個轉(zhuǎn)動坐標， qJ=［θ1， θ2，…， θn］T∈R6×1為機械臂的關(guān)節(jié)轉(zhuǎn)角?？臻g機器人的動力學方程具有如下形式:其中， 矩陣 M（q）∈R（n+6）×（n+6）為機器人的質(zhì)量陣，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為機械臂關(guān)節(jié)控制力矩， SBT= ［I6×6； 0n×6］∈R（n+6）×6為基座控制力、力矩的選擇矩陣，SJT= ［06×n； In×n］∈R（n+6）×n為關(guān)節(jié)控制力矩的選擇矩陣。

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

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

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

3.1互補問題的基本形式

機械臂末端執(zhí)行器與目標表面間的單邊接觸，可能發(fā)生“接觸-分離”、“滑移-黏滯”兩類狀態(tài)轉(zhuǎn)移， 如圖2所示。

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

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

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

摩擦余量和分解的切向加速度滿足關(guān)系式

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

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

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

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

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

設(shè)矢量ξ和η分別為

并由式（6）得到

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

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

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

4　算例與仿真分析

4.1算例與算法

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

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

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

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

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

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

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

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

4.2仿真結(jié)果分析

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

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

5　結(jié)論

本文以空間機器人連續(xù)接觸作業(yè)為背景， 基于互補問題， 描述空間機械臂末端與目標的單邊接觸，推導了具有單邊接觸的空間機器人動力學模型， 并對模型的有效性進行仿真驗證。本文的研究表明，基于互補問題的空間機器人動力學模型以力學約束為基礎(chǔ)， 避免了采用彈簧-阻尼并聯(lián)模型調(diào)節(jié)剛度、阻尼系數(shù)的時間、資源開銷， 為描述和計算單邊接觸過程中的分離、接觸和滑移現(xiàn)象提供了一類緊湊的數(shù)學表達， 能夠?qū)ξ磥砜臻g機器人接觸作業(yè)的工程應(yīng)用提供一定的理論參考。

［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］ 徐文福， 劉宇， 強文義， 等. 自由漂浮空間機器人的笛卡爾連續(xù)路徑規(guī)劃. 控制與決策， 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］ 陳羅婧， 郝金華， 袁春柱， 等. “鳳凰”計劃關(guān)鍵技術(shù)及其啟示. 航天器工程， 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； 網(wǎng)絡(luò)出版日期: 2016-07-14