空间机器人一般由飞行器平台和空间机械臂共同组成。控制空间机械臂对目标飞行器进行操作, 是完成在轨维护任务的重要方式。目前, 有关空间机械臂末端自由运动^[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中相关符号含义如下。

B_i (i=0, 1, …, n+1):系统第i个刚体, 其中B₀为基座, B_n+1为目标, B₁~B_n为机械臂的第1~n个刚体; 刚体质心记为O_i。

J_j (j=1, 2, …, n):机械臂第j个关节, 其关节转轴矢量为k_j∈R^3×1, 关节转角为θ_j。

b_s (s=0, 2, …, n-1)∈R^3×1:由O_s指向J_s+1的矢量, b_n为由O_n指向B_n末端的矢量。

a_t (t=1, 2, …, n)∈R^3×1:由J_t指向O_t的矢量, a_n+1为由B_n末端指向O_n+1的矢量。

1.2 坐标系定义

空间机器人通过机械臂第n个刚体B_n的末端与目标发生单点单边接触, 假设接触点在B_n上始终为同一个点, 而在目标B_n+1表面移动。建立本文所需坐标系, 如图 1所示, 包括以下部分。

惯性系O_Ix_Iy_Iz_I:以惯性空间中某点O_I为原点建立的右手直角坐标系。

基座本体坐标系O₀x₀y₀z₀:以基座质心O₀为原点建立的右手直角坐标系, 坐标轴与基座固连并沿惯量主轴方向。

目标本体坐标系O_n+1x_n+1y_n+1z_n+1:以目标质心O_n+1为原点建立的右手直角坐标系, 坐标轴与目标固连并沿惯量主轴方向。

主接触坐标系O_c0x_c0y_c0z_c0: B_n末端接触点O_c0为原点, 接触面法向指向B_n方向为z_c0轴, 接触点切向速度方向为x_c0轴, y_c0轴与x_c0轴、z_c0轴成右手直角坐标系。

次接触坐标系O_c1x_c1y_c1z_c1: B_n+1上接触点O_c1为原点, 坐标轴与主接触坐标系各轴方向相反。

2 空间机器人的动力学模型 2.1 运动学分析

设空间机器人各刚体质心O_i (i=0, 1, …, n)在O_Ix_Iy_Iz_I中的位置矢量为r_i, B_n末端(即接触点)在O_Ix_Iy_Iz_I中的位置矢量为p_e。有如下关系成立:

$ {\mathit{\boldsymbol{r}}_i} = {\mathit{\boldsymbol{r}}_0} + \sum\limits_{q = 0}^{i - 1} {{\mathit{\boldsymbol{b}}_q}} + \sum\limits_{q = 1}^i {{\mathit{\boldsymbol{a}}_q}}, $

(1)

$ {\mathit{\boldsymbol{p}}_e} = {\mathit{\boldsymbol{r}}_0} + \sum\limits_{q = 0}^n {{\mathit{\boldsymbol{b}}_q}} + \sum\limits_{q = 1}^n {{\mathit{\boldsymbol{a}}_q}} , $

(2)

则机器人各刚体质心的速度为

$ {\mathit{\boldsymbol{\dot r}}_i} = {\mathit{\boldsymbol{\dot r}}_0} + \sum\limits_{q = 0}^{i - 1} {{\mathit{\boldsymbol{\omega }}_q} \times {\mathit{\boldsymbol{b}}_q}} + \sum\limits_{q = 1}^i {{\mathit{\boldsymbol{\omega }}_q} \times {\mathit{\boldsymbol{a}}_q}} , $

(3)

其中, 设B₀的角速度为ω₀, 第i (=1, …, n)个刚体角速度ω_i的表达式为

$ {\mathit{\boldsymbol{\omega }}_i} = {\mathit{\boldsymbol{\omega }}_0} + \sum\limits_{q = 1}^i {{{\mathit{\boldsymbol{\dot \theta }}}_q}{\mathit{\boldsymbol{k}}_q}} ; $

(4)

设B_i的质量为m_i, 惯量阵为J_i∈R^3×3, 空间机器人的动能T为

$ T = \frac{1}{2}\sum\limits_{i = 0}^n {({m_i}\mathit{\boldsymbol{\dot r}}_i^{\rm{T}}{{\mathit{\boldsymbol{\dot r}}}_i} + \mathit{\boldsymbol{\omega }}_i^{\rm{T}}{\mathit{\boldsymbol{J}}_i}{\mathit{\boldsymbol{\omega }}_i})} 。 $

(5)

2.2 动力学方程推导

设空间机器人的广义坐标为q=[q_B^T, q_J^T]^T∈R^(n+6)×1, 其中, q_B=[x₀, y₀, z₀, α₀, β₀, γ₀]^T∈R^6×1为基座的3个平动和3个转动坐标, q_J=[θ₁, θ₂, …, θ_n]^T∈R^6×1为机械臂的关节转角。空间机器人的动力学方程具有如下形式:

$ \mathit{\boldsymbol{M}}(\mathit{\boldsymbol{q}})\mathit{\boldsymbol{\ddot q}} + \mathit{\boldsymbol{h}}(\mathit{\boldsymbol{q}},\;\mathit{\boldsymbol{\dot q}}) + \mathit{\boldsymbol{W}}(\mathit{\boldsymbol{q}})\mathit{\boldsymbol{\lambda }} = \mathit{\boldsymbol{S}}_B^{\rm{T}}{\mathit{\boldsymbol{F}}_B} + \mathit{\boldsymbol{S}}_J^{\rm{T}}\mathit{\boldsymbol{\tau }}, $

(6)

其中, 矩阵M(q)∈R^(n+6)×(n+6)为机器人的质量阵, h(q, $\mathit{\boldsymbol{\dot q}}$)∈R^(n+6)×1为离心力、科氏力矢量, W(q)∈R^(n+6)×2为接触约束矩阵, λ=[λ_N, λ_T]^T∈R^2×1为接触约束力, λ_N为法向接触力, λ_T为切向接触力, F_B=[F_Bx, F_By, F_Bz, F_Bα, F_Bβ, F_Bγ]^T∈R^6×1为基座控制力、力矩, τ∈R^n×1为机械臂关节控制力矩, S_B^T=[I_6×6; 0_n×6]∈R^(n+6)×6为基座控制力、力矩的选择矩阵, S_J^T=[0_6×n; I_n×n]∈R^(n+6)×n为关节控制力矩的选择矩阵。

根据第一类Lagrange方程, 质量阵M(q)和离心力、科氏力矢量h(q, $\mathit{\boldsymbol{\dot q}}$)可由动能求得:

$ \mathit{\boldsymbol{M}}(\mathit{\boldsymbol{q}}) = {\left[ {{M_{ij}}} \right]_{\left( {n + 6} \right) \times \left( {n + 6} \right)}} = {\left[ {\frac{{{\partial ^2}T}}{{\partial {{\dot q}_i}\partial {{\dot q}_j}}}} \right]_{(n + 6) \times (n + 6)}}, $

(7)

$ \mathit{\boldsymbol{h}}(\mathit{\boldsymbol{q}},\;\mathit{\boldsymbol{\dot q}}) = {\left[ {{h_i}} \right]_{(n + 6) \times 1}} = {\left[ {\frac{{\partial T}}{{\partial {q_i}}} - \sum\limits_{j = 1}^6 {\frac{{{\partial ^2}T}}{{\partial {{\dot q}_i}\partial {q_j}}}{{\dot q}_j}} } \right]_{(n + 6) \times 1}}。 $

(8)

W(q)由约束方程给出, F_B和τ按一定控制律给出, 接触力λ未知, 广义加速度$\mathit{\boldsymbol{\ddot q}}$待求。

3 基于互补问题描述单边接触 3.1 互补问题的基本形式

机械臂末端执行器与目标表面间的单边接触, 可能发生“接触‒分离”、“滑移‒黏滞”两类状态转移, 如图 2所示。

设在O_c0x_c0y_c0z_c0中, 接触点处法向距离为g_N, 切向速度为${\dot g_T}$, g_N和${\dot g_T}$的正方向分别与O_c0z_c0轴和O_c0x_c0轴的正方向一致。切向力采用库仑干摩擦模型^[17], 摩擦系数设为μ, 并假设静摩擦系数与滑动摩擦系数相等, 则单边接触状态的判定条件为

$ \left\{ \begin{array}{l} 分离:\;\;{g_N} > 0,\;\;{\lambda _N} = 0;\\ 接触:\;\;{g_N} = 0,\;\;{\lambda _N} > 0;\\ 滑移:\;\;{g_N} = 0,\;\;{\lambda _N} > 0,\;\;\left| {{{\dot g}_T}} \right| > 0,\;\;\left| {{\lambda _T}} \right| = \mu {\lambda _N};\\ 黏滞:\;\;{g_N} = 0,\;\;{\lambda _N} > 0,\;\;\left| {{{\dot g}_T}} \right| = 0,\;\;\left| {{\lambda _T}} \right| < \mu {\lambda _N}。 \end{array} \right. $

(9)

进一步地, 定义正、负滑动摩擦余量分别为

$\lambda _{T0}^ + = (\mu {\lambda _N} + {\lambda _T})/2,{\rm{ }}\lambda _{T0}^ - = (\mu {\lambda _N} - {\lambda _T})/2,$

(10)

并将切向加速度${\ddot g_T}$分解为正、负切向加速度:

$ \ddot g_T^ + = (|{\ddot g_T}| + {\ddot g_T})/2,{\rm{ }}\ddot g_T^ - = (|{\ddot g_T}| - {\ddot g_T})/2。 $

(11)

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

$ \lambda _{T0}^ + = \mu {\lambda _N} - \lambda _{T0}^ -, $

(12)

$ \ddot g_T^ - = \ddot g_T^ + - {\ddot g_T}, $

(13)

那么, 式(9)表示的空间机器人末端接触状态判定条件等价于如下加速度形式的互补问题:

$ \begin{array}{*{20}{c}} {{{\mathit{\boldsymbol{\ddot g}}}^*} \ge 0,}&{{\mathit{\boldsymbol{\lambda }}^*} \ge 0,}&{{{({{\mathit{\boldsymbol{\ddot g}}}^*})}^{\rm{T}}}{\mathit{\boldsymbol{\lambda }}^*} = 0} \end{array}, $

(14)

其中, ${\mathit{\boldsymbol{\ddot g}}^\mathit{\boldsymbol{*}}}$和λ^*的定义分别为

$ \begin{array}{*{20}{c}} {{{\mathit{\boldsymbol{\ddot g}}}^*} = {{[\begin{array}{*{20}{c}} {{{\ddot g}_N}}&{\ddot g_T^ + }&{\ddot g_T^ - } \end{array}]}^{\rm{T}}},}&{{\mathit{\boldsymbol{\lambda }}^*} = {{[\begin{array}{*{20}{c}} {{\lambda _N}}&{\lambda _{T0}^ + }&{\lambda _{T0}^ - } \end{array}]}^{\rm{T}}}} \end{array}。 $

(15)

式(12)即为描述一般单点单边接触互补问题的基本形式。

3.2 平面单边接触的线性互补问题

对于平面单边接触, 式(12)所示的互补问题可以结合式(6)所示的空间机器人动力学方程, 化归为线性互补问题。此时, 有关系式

$ \begin{array}{*{20}{c}} {{{\dot g}_N} = - \mathit{\boldsymbol{W}}_N^{\rm{T}}\mathit{\boldsymbol{\dot q}} - {{\tilde w}_N},}&{{{\dot g}_T} = \mathit{\boldsymbol{W}}_T^{\rm{T}}\mathit{\boldsymbol{\dot q}}} \end{array} + {\tilde w_T}, $

(16)

那么, 接触点位移的加速度为

$ \begin{array}{*{20}{c}} {{{\ddot g}_N} = - \mathit{\boldsymbol{W}}_N^{\rm{T}}\mathit{\boldsymbol{\ddot q}} - {w_N},}&{{{\ddot g}_T} = \mathit{\boldsymbol{W}}_T^{\rm{T}}\mathit{\boldsymbol{\ddot q}} + {w_T}} \end{array}\;。 $

(17)

设矢量ξ和η分别为

$ \begin{array}{*{20}{c}} {\mathit{\boldsymbol{\xi }} = {{\left[ {\begin{array}{*{20}{c}} {{{\ddot g}_N}}&{\ddot g_T^ - }&{\lambda _{T0}^ + } \end{array}} \right]}^{\rm{T}}},}&{\mathit{\boldsymbol{\eta }} = {{\left[ {\begin{array}{*{20}{c}} {{\lambda _N}}&{\lambda _{T0}^ - }&{\ddot g_T^ + } \end{array}} \right]}^{\rm{T}}}} \end{array}, $

(18)

并由式(6)得到

$ \mathit{\boldsymbol{\ddot q}} = {\mathit{\boldsymbol{M}}^{ - 1}}(\mathit{\boldsymbol{S}}_B^{\rm{T}}{\mathit{\boldsymbol{F}}_B} + \mathit{\boldsymbol{S}}_J^{\rm{T}}\mathit{\boldsymbol{\tau }} - \mathit{\boldsymbol{h}} - {\mathit{\boldsymbol{W}}_N}{\lambda _N} - {\mathit{\boldsymbol{W}}_T}{\lambda _T}), $

(19)

将其代入式(17), 结合式(12)和(13), 整理得到描述平面单点单边接触的线性互补问题(linearcomple-mentary problem, LCP), 形式为

$ \begin{array}{*{20}{c}} {\mathit{\boldsymbol{\xi }} = \mathit{\boldsymbol{A\eta }} + \mathit{\boldsymbol{b}},}&{\mathit{\boldsymbol{\eta }} \ge 0,}&{\mathit{\boldsymbol{\xi }} \ge 0,}&{{\mathit{\boldsymbol{\xi }}^{\rm{T}}}\mathit{\boldsymbol{\eta }} = 0} \end{array}, $

(20)

其中, 矩阵A和矢量b分别为

$ \mathit{\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{W}}_N^{\rm{T}}{\mathit{\boldsymbol{M}}^{ - 1}}({\mathit{\boldsymbol{W}}_N} + \mu {\mathit{\boldsymbol{W}}_T})}&{ - 2\mathit{\boldsymbol{W}}_N^{\rm{T}}{\mathit{\boldsymbol{M}}^{ - 1}}{\mathit{\boldsymbol{W}}_T}}&0\\ {\mathit{\boldsymbol{W}}_T^{\rm{T}}{\mathit{\boldsymbol{M}}^{ - 1}}({\mathit{\boldsymbol{W}}_N} + \mu {\mathit{\boldsymbol{W}}_T})}&{ - 2\mathit{\boldsymbol{W}}_T^{\rm{T}}{\mathit{\boldsymbol{M}}^{ - 1}}{\mathit{\boldsymbol{W}}_T}}&1\\ \mu &{ - 1}&0 \end{array}} \right], $

(21)

$ \left\{ \begin{array}{l} \mathit{\boldsymbol{b}} = {[\begin{array}{*{20}{c}} {{b_1}}&{{b_2}}&0 \end{array}]^{\rm{T}}},\\ {b_1} = - \mathit{\boldsymbol{W}}_N^{\rm{T}}{\mathit{\boldsymbol{M}}^{ - 1}}(\mathit{\boldsymbol{S}}_B^{\rm{T}}{\mathit{\boldsymbol{F}}_B} + \mathit{\boldsymbol{S}}_J^{\rm{T}}\mathit{\boldsymbol{\tau }} - \mathit{\boldsymbol{h}}) - {w_N},\\ {b_2} = - \mathit{\boldsymbol{W}}_T^{\rm{T}}{\mathit{\boldsymbol{M}}^{ - 1}}(\mathit{\boldsymbol{S}}_B^{\rm{T}}{\mathit{\boldsymbol{F}}_B} + \mathit{\boldsymbol{S}}_J^{\rm{T}}\mathit{\boldsymbol{\tau }} - \mathit{\boldsymbol{h}}) - {w_T}。 \end{array} \right. $

(22)

因此, 具有单边接触的空间机器人动力学模型可由式(6)和(20)组成的微分‒代数系统描述。

4 算例与仿真分析 4.1 算例与算法

以一个平面受控漂浮基座上的三连杆机械臂为例, 对建立的动力学模型进行仿真验证。算例的基本构型如图 3所示, 符号含义及取值如表 1所示。

设广义坐标q=[x₀, y₀, θ₀, θ₁, θ₂, θ₃]^T, 基座具有2个平动自由度和1个转动自由度, 基座控制力和力矩为F_Bx, F_By和F_Bθ, 机械臂关节力矩τ₁~τ₃需预先给定。机械臂由3根匀质细长杆组成, 杆间以转动副相连, 第3杆末端点P与无限长平面目标发生单边接触。

由式(6)和式(20)建立空间机器人的动力学模型, 仿真计算采用定步长Δt=0.01s, 初始广义坐标和广义速度分别设定为q₀=[0, L, 0, π/3, −π/3, arccos (3/5)]^T和${\mathit{\boldsymbol{\dot q}}_0}$=[0, 0, 0, 0, 0, 0]^T。采用经典的Lemke算法^[19]求解LCP问题, 得到法向接触力λ_N和切向接触力λ_T, 代入由式(6)建立的状态方程进行数值积分, 即能得到更新的广义坐标q与广义速度$\mathit{\boldsymbol{\dot q}}$。设矢量q_k的下角标表示第k步计算量, 具体的算法流程如下。

步骤1(初始化):令k=0, 设定广义坐标初值q₀和广义速度初值${\mathit{\boldsymbol{\dot q}}_0}$。

步骤2(LCP求解):基于Lemke算法求解式(20)所示的LCP, 得到ξ_k和η_k, 并进一步由式(18)和(10)求解法向接触力λ_Nk和切向接触力λ_Tk。有关Lemke算法解LCP的细节可参考文献[19]。

步骤3(数值积分):设状态变量X=[q; $\mathit{\boldsymbol{\dot q}}$]^T, 将λ_Nk, λ_Tk及预先给定的F_Bk和τ_k代入状态方程

$ \mathit{\boldsymbol{\dot X}} = \mathit{\boldsymbol{CX}} + \mathit{\boldsymbol{d}} $

(23)

进行数值积分, 得到广义坐标q_k和广义速度${\mathit{\boldsymbol{\dot q}}_k}$。

式(23)中矩阵C和矢量d分别为

$ \mathit{\boldsymbol{C}} = \left[ {\begin{array}{*{20}{c}} {{{\bf{0}}_{6 \times 6}}}&{{\mathit{\boldsymbol{I}}_{6 \times 6}}}\\ {{{\bf{0}}_{6 \times 6}}}&{{{\bf{0}}_{6 \times 6}}} \end{array}} \right], $

(24)

$ \mathit{\boldsymbol{d}} = \left[ {\begin{array}{*{20}{c}} {{{\bf{0}}_{6 \times 1}}}\\ {\mathit{\boldsymbol{M}}_k^{ - 1}\left( {\mathit{\boldsymbol{S}}_B^{\rm{T}}{\mathit{\boldsymbol{F}}_{Bk}} + \mathit{\boldsymbol{S}}_J^{\rm{T}}{\mathit{\boldsymbol{\tau }}_k} - {\mathit{\boldsymbol{h}}_k} - {\mathit{\boldsymbol{W}}_{Nk}}{\lambda _{Nk}} - {\mathit{\boldsymbol{W}}_{Tk}}{\lambda _{Tk}}} \right)} \end{array}} \right]。 $

(25)

步骤4 (结束判断):设tolK∈N^*, k=k+1;若k≤tolK, 则返回步骤2;若k > tolK, 则结束数值计算。

4.2 仿真结果分析

基于受限二次规划方法^[20-21], 事先确定基座控制力F_Bx, F_By, 力矩F_Bθ以及关节控制力矩τ, 如图 4和5所示。以此为控制输入, 按照上述算法求解算例机器人动力学, 结果如图 6~9所示。

图 6表明, 空间机械臂3个关节转角均随时间连续变化, 机械臂运动状态平稳。图 7表明, 在0.08~0.66 s和1.89~2.43 s两个时间段内, 法向接触力为零, 此时空间机械臂末端与目标平面处于分离状态; 其余时间段法向接触力非零且小于8N, 空间机械臂末端与目标平面处于接触状态。图 8表明, 在0.66~1.89 s和2.43~2.80 s两个时间段内, 切向接触力数值为负, 空间机械臂末端与目标平面处于接触状态, 且相对目标平面向O_Ix_I的正向运动; 2.81~5.00s时间段内, 切向接触力数值为正, 机械臂末端与目标平面处于接触状态, 且相对目标平面向O_Ix_I的负向运动。图 9显示空间机器人的基座质心运动轨迹、基座转动、机械臂构型以及机械臂末端运动轨迹, 在输入控制力、力矩作用下, 空间机器人完成了一个机械臂末端运动幅度为0.2786m的局部接触作业任务, 其中短斜线是飞行器平台质心运动轨迹。

5 结论

本文以空间机器人连续接触作业为背景, 基于互补问题, 描述空间机械臂末端与目标的单边接触, 推导了具有单边接触的空间机器人动力学模型, 并对模型的有效性进行仿真验证。本文的研究表明, 基于互补问题的空间机器人动力学模型以力学约束为基础, 避免了采用弹簧‒阻尼并联模型调节刚度、阻尼系数的时间、资源开销, 为描述和计算单边接触过程中的分离、接触和滑移现象提供了一类紧凑的数学表达, 能够对未来空间机器人接触作业的工程应用提供一定的理论参考。