基于互补问题描述单边接触的空间机器人动力学建模与数值仿真 |
您所在的位置：网站首页 › 动力学建模与仿真 › 基于互补问题描述单边接触的空间机器人动力学建模与数值仿真 |
|
空间机器人一般由飞行器平台和空间机械臂共同组成。控制空间机械臂对目标飞行器进行操作, 是完成在轨维护任务的重要方式。目前, 有关空间机械臂末端自由运动[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中相关符号含义如下。 图 1. 空间机器人接触作业的基本构型 Figure 1. Configurations of space robot executing contact missions 图选项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。有如下关系成立: $ {\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)其中, 设B0的角速度为ω0, 第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)设Bi的质量为mi, 惯量阵为Ji∈R3×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=[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为机械臂的关节转角。空间机器人的动力学方程具有如下形式: $ \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∈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, $\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)由约束方程给出, FB和τ按一定控制律给出, 接触力λ未知, 广义加速度$\mathit{\boldsymbol{\ddot q}}$待求。 3 基于互补问题描述单边接触 3.1 互补问题的基本形式机械臂末端执行器与目标表面间的单边接触, 可能发生“接触‒分离”、“滑移‒黏滞”两类状态转移, 如图 2所示。 图 2. 单边接触的状态转移 Figure 2. Transitions of unilateral contact interaction 图选项设在Oc0xc0yc0zc0中, 接触点处法向距离为gN, 切向速度为${\dot g_T}$, gN和${\dot g_T}$的正方向分别与Oc0zc0轴和Oc0xc0轴的正方向一致。切向力采用库仑干摩擦模型[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所示。 图 3. 空间机器人算例构型 Figure 3. Configurations of a robotic demo 图选项 表 1. 符号含义及取值 Table 1. Nomenclatures and values 符号及取值 物理含义 单位 m0=1000, m1=30, m2=20, m3=10 刚体质量 kg l0=1, l1=1, l2=0.7, l3=0.5 刚体尺寸 m J0=601.667 基座惯量 kg · m2 L=2.5 初始距离 m μ=0.1 摩擦系数 − 表选项设广义坐标q=[x0, y0, θ0, θ1, θ2, θ3]T, 基座具有2个平动自由度和1个转动自由度, 基座控制力和力矩为FBx, FBy和FBθ, 机械臂关节力矩τ1~τ3需预先给定。机械臂由3根匀质细长杆组成, 杆间以转动副相连, 第3杆末端点P与无限长平面目标发生单边接触。 由式(6)和式(20)建立空间机器人的动力学模型, 仿真计算采用定步长Δt=0.01s, 初始广义坐标和广义速度分别设定为q0=[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}}$。设矢量qk的下角标表示第k步计算量, 具体的算法流程如下。 步骤1(初始化):令k=0, 设定广义坐标初值q0和广义速度初值${\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及预先给定的FBk和τk代入状态方程 $ \mathit{\boldsymbol{\dot X}} = \mathit{\boldsymbol{CX}} + \mathit{\boldsymbol{d}} $ (23)进行数值积分, 得到广义坐标qk和广义速度${\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], 事先确定基座控制力FBx, FBy, 力矩FBθ以及关节控制力矩τ, 如图 4和5所示。以此为控制输入, 按照上述算法求解算例机器人动力学, 结果如图 6~9所示。 图 4. 基座控制力和控制力矩 Figure 4. Control forces and torque of the satellite base 图选项 图 5. 关节控制力矩 Figure 5. Joint torques 图选项 图 6. 机械臂关节角 Figure 6. Joint angles 图选项图 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两个时间段内, 切向接触力数值为负, 空间机械臂末端与目标平面处于接触状态, 且相对目标平面向OIxI的正向运动; 2.81~5.00s时间段内, 切向接触力数值为正, 机械臂末端与目标平面处于接触状态, 且相对目标平面向OIxI的负向运动。图 9显示空间机器人的基座质心运动轨迹、基座转动、机械臂构型以及机械臂末端运动轨迹, 在输入控制力、力矩作用下, 空间机器人完成了一个机械臂末端运动幅度为0.2786m的局部接触作业任务, 其中短斜线是飞行器平台质心运动轨迹。 图 7. 末端法向接触力 Figure 7. Normal contact force 图选项 图 8. 末端切向接触力 Figure 8. Tangential contact force 图选项 图 9. 接触操作过程中空间机器人构型 Figure 9. Configurations of space robot in numerical simulations 图选项 5 结论本文以空间机器人连续接触作业为背景, 基于互补问题, 描述空间机械臂末端与目标的单边接触, 推导了具有单边接触的空间机器人动力学模型, 并对模型的有效性进行仿真验证。本文的研究表明, 基于互补问题的空间机器人动力学模型以力学约束为基础, 避免了采用弹簧‒阻尼并联模型调节刚度、阻尼系数的时间、资源开销, 为描述和计算单边接触过程中的分离、接触和滑移现象提供了一类紧凑的数学表达, 能够对未来空间机器人接触作业的工程应用提供一定的理论参考。 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |