基于互补问题描述单边接触的空间机器人动力学建模与数值仿真

胡天健　王天舒　李俊峰　钱卫平

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