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

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

?

基于互補(bǔ)問(wèn)題描述單邊接觸的空間機(jī)器人動(dòng)力學(xué)建模與數(shù)值仿真

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

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

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

動(dòng)力學(xué)； 空間機(jī)器人； 單邊接觸； 互補(bǔ)問(wèn)題

北京大學(xué)學(xué)報(bào)（自然科學(xué)版）第52卷第4期2016年7月

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

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

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

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

1　空間機(jī)器人構(gòu)型與坐標(biāo)系

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

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

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

Jj（j =1， 2， …， n）: 機(jī)械臂第j個(gè)關(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坐標(biāo)系定義

空間機(jī)器人通過(guò)機(jī)械臂第 n 個(gè)剛體 Bn的末端與目標(biāo)發(fā)生單點(diǎn)單邊接觸， 假設(shè)接觸點(diǎn)在 Bn上始終為同一個(gè)點(diǎn)， 而在目標(biāo)Bn+1表面移動(dòng)。建立本文所需坐標(biāo)系， 如圖1所示， 包括以下部分。

慣性系 OIxIyIzI: 以慣性空間中某點(diǎn) OI為原點(diǎn)建立的右手直角坐標(biāo)系。

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

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

主接觸坐標(biāo)系 Oc0xc0yc0zc0: Bn末端接觸點(diǎn) Oc0為原點(diǎn)， 接觸面法向指向 Bn方向?yàn)?zc0軸， 接觸點(diǎn)切向速度方向?yàn)閤c0軸， yc0軸與xc0軸、zc0軸成右手直角坐標(biāo)系。

次接觸坐標(biāo)系Oc1xc1yc1zc1: Bn+1上接觸點(diǎn)Oc1為原點(diǎn)， 坐標(biāo)軸與主接觸坐標(biāo)系各軸方向相反。

2　空間機(jī)器人的動(dòng)力學(xué)模型

2.1運(yùn)動(dòng)學(xué)分析

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

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

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

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

2.2動(dòng)力學(xué)方程推導(dǎo)

設(shè)空間機(jī)器人的廣義坐標(biāo)為 q = ［qBT， qJT］T∈R（n+6）×1， 其中， qB=［x0， y0， z0， α0， β0， γ0］T∈R6×1為基座的3個(gè)平動(dòng)和3個(gè)轉(zhuǎn)動(dòng)坐標(biāo)， qJ=［θ1， θ2，…， θn］T∈R6×1為機(jī)械臂的關(guān)節(jié)轉(zhuǎn)角?？臻g機(jī)器人的動(dòng)力學(xué)方程具有如下形式:其中， 矩陣 M（q）∈R（n+6）×（n+6）為機(jī)器人的質(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為機(jī)械臂關(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˙）可由動(dòng)能求得:

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

3　基于互補(bǔ)問(wèn)題描述單邊接觸

3.1互補(bǔ)問(wèn)題的基本形式

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

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

進(jìn)一步地， 定義正、負(fù)滑動(dòng)摩擦余量分別為

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

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

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

式（12）即為描述一般單點(diǎn)單邊接觸互補(bǔ)問(wèn)題的基本形式。

3.2平面單邊接觸的線性互補(bǔ)問(wèn)題

對(duì)于平面單邊接觸， 式（12）所示的互補(bǔ)問(wèn)題可以結(jié)合式（6）所示的空間機(jī)器人動(dòng)力學(xué)方程， 化歸為線性互補(bǔ)問(wèn)題。此時(shí)， 有關(guān)系式

那么， 接觸點(diǎn)位移的加速度為

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

并由式（6）得到

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

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

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

4　算例與仿真分析

4.1算例與算法

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

表1　符號(hào)含義及取值Table 1　Nomenclatures and values

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

由式（6）和式（20）建立空間機(jī)器人的動(dòng)力學(xué)模型， 仿真計(jì)算采用定步長(zhǎng) Δt=0.01 s， 初始廣義坐標(biāo)和廣義速度分別設(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問(wèn)題， 得到法向接觸力 λN和切向接觸力λT， 代入由式（6）建立的狀態(tài)方程進(jìn)行數(shù)值積分， 即能得到更新的廣義坐標(biāo) q 與廣義速度˙q。設(shè)矢量qk的下角標(biāo)表示第k步計(jì)算量， 具體的算法流程如下。

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

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

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

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

4.2仿真結(jié)果分析

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

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

5　結(jié)論

本文以空間機(jī)器人連續(xù)接觸作業(yè)為背景， 基于互補(bǔ)問(wèn)題， 描述空間機(jī)械臂末端與目標(biāo)的單邊接觸，推導(dǎo)了具有單邊接觸的空間機(jī)器人動(dòng)力學(xué)模型， 并對(duì)模型的有效性進(jìn)行仿真驗(yàn)證。本文的研究表明，基于互補(bǔ)問(wèn)題的空間機(jī)器人動(dòng)力學(xué)模型以力學(xué)約束為基礎(chǔ)， 避免了采用彈簧-阻尼并聯(lián)模型調(diào)節(jié)剛度、阻尼系數(shù)的時(shí)間、資源開(kāi)銷， 為描述和計(jì)算單邊接觸過(guò)程中的分離、接觸和滑移現(xiàn)象提供了一類緊湊的數(shù)學(xué)表達(dá)， 能夠?qū)ξ磥?lái)空間機(jī)器人接觸作業(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］ 徐文福， 劉宇， 強(qiáng)文義， 等. 自由漂浮空間機(jī)器人的笛卡爾連續(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］ 陳羅婧， 郝金華， 袁春柱， 等. “鳳凰”計(jì)劃關(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］ 徐文福， 周瑞興， 孟得山. 空間機(jī)器人在軌更換ORU的力/位混合控制方法. 宇航學(xué)報(bào)， 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

國(guó)家自然科學(xué)基金（11402004）資助

2015-12-30；

2016-02-02； 網(wǎng)絡(luò)出版日期: 2016-07-14