SIX中文网
六自由度装配机器人的动态柔顺性控制 |
|
发布日期：2025-01-04 15:49 点击次数：140 |
工业装配机器人通常用于在具有环境约束的工作空间内或生产线上完成零部件的组装作业.因常运行在受限的工作环境中且常与被操作的零部件相接触，该类机器人装配作业任务的完成在很大程度上依赖于末端机械手的自适应或顺应性控制.
为了实现装配机器人对目标路径跟踪的有效控制，吕立新等[1]设计常规的比例微分控制器，以较准确地校正机器人的运行轨迹偏差，准确定位、抓取和装配目标工件.虽然比例微分控制器结构简单、易于实现，但控制系数为常数，不能动态地自适应机器人的复杂运行状态和参数时变状况.高亚军[2]研究六自由度工业装配机器人的运动学模型和轨迹规划方法.基于提出的轨迹规划方法，设计了相应的比例积分控制器.虽然规划的轨迹较平滑，易于实现，但设计的控制器主要用于空间轨迹跟踪控制，且主要是用于跟踪直线、圆弧和点位等相当规则的轨迹.王学林等[3]提出机器人末端执行器的抓持力跟踪阻抗控制算法，将接触环境等效为阻抗-导纳模型，使得末端执行器的力/位控制等效为期望的惯量-阻尼-刚度模型，以便动态地调节执行器输出力与环境接触变形的关系.虽然利用该控制算法有利于避免末端机械手对接触环境的变形和损伤，有一定的鲁棒性，但不能较好地实现空间目标位置的轨迹跟踪控制.陈兆芃等[4]研究多指灵巧手空间协调的阻抗控制，建立任意手指数目的物体空间坐标系，基于六维空间虚拟弹簧阻抗和关节力矩反馈，以控制被抓取物体姿态和位置.该控制方案涉及多个空间坐标系，算法复杂，且难于有效实施.樊绍巍等[5]研究机器人末端灵巧手的非线性补偿和关节力矩反馈的笛卡尔空间和关节空间的阻抗控制，采用扩展卡尔曼滤波器实现自适应的摩擦力观测和补偿.
上述常用的机器人阻抗控制的精度在很大程度上取决于操作者对环境的了解.实际上，因对环境了解不足，且无法有效地预测机器人动态特性，致使阻抗控制的精度较差，且难于保证机器人运行过程中的全局稳定状态.由于存在空间运动误差、动力学误差和末端接触刚度误差，单纯的末端执行器的空间轨迹跟踪控制或阻抗控制易引发机器人全局运动失稳，导致过大的末端接触力，并可能产生安全隐患，损坏机器人本体结构.工业装配机器人的高效稳定运行不仅依赖于末端机械手的空间目标轨迹跟踪控制，而且有赖于其与周围环境之间的接触控制.
本文针对工业六自由度装配机器人，提出动态柔顺性控制策略.不同于上述单纯的机器人空间轨迹跟踪控制或者工件装配时的接触力阻抗控制，该控制策略不仅能够实现高精度的轨迹跟踪，而且能够自适应动态地切换到工件装配时的接触力控制，并能够保持良好的柔顺性接触力.采用滑模控制算法设计相应的参考轨迹跟踪控制律，采用阻抗滤波器获得对应于期望接触力的目标装配轨迹.采用外部动力学参数动态辨识的方法，在线、实时地获得上述控制律中所需要的各常数矩阵和常数值.基于六自由度装配实验台上的现场装配和动态轨迹跟踪的对比性仿真实验，验证上述控制策略的可行性和有效性.
1 系统描述
1.1 动态特性
工业六自由度装配机器人常采用多个连杆串联铰接的结构形式，并在关节处安装有伺服电动机.关节空间的标准动力学模型可以表示为
$
\mathit{\boldsymbol{M}}\left( \mathit{\boldsymbol{\theta }} \right)\mathit{\boldsymbol{\ddot \theta }} + \mathit{\boldsymbol{C}}\left( {\mathit{\boldsymbol{\theta }},\mathit{\boldsymbol{\dot \theta }}} \right)\mathit{\boldsymbol{\dot \theta }} + \mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{\theta }} \right) = \tau .
$
(1)
式中：θ为机器人的6×1机器人关节角位移矢量，M(θ)为机器人的6×6惯性矩阵，C(θ, ${\mathit{\boldsymbol{\dot \theta }}}$)为离心力和哥氏力矩阵，G(θ)为重力项，τ为输入的6×1各关节驱动力矩矢量.
利用机器人雅可比矩阵J(θ)[6]，可以获得关节空间与末端执行器操作空间之间的变换关系：
$
\left. \begin{array}{l}
\mathit{\boldsymbol{\dot x}} = \mathit{\boldsymbol{J}}\left( \mathit{\boldsymbol{\theta }} \right)\mathit{\boldsymbol{\dot \theta }},\\
\mathit{\boldsymbol{\dot \theta }} = {\mathit{\boldsymbol{J}}^{ - 1}}\left( \mathit{\boldsymbol{\theta }} \right)\mathit{\boldsymbol{\dot x}}.
\end{array} \right\}
$
(2a)
$
\left. \begin{array}{l}
\mathit{\boldsymbol{\ddot x = J}}\left( \mathit{\boldsymbol{\theta }} \right)\mathit{\boldsymbol{\ddot \theta }} + \mathit{\boldsymbol{\dot J}}\left( \mathit{\boldsymbol{\theta }} \right)\mathit{\boldsymbol{\dot \theta }},\\
\mathit{\boldsymbol{\ddot \theta }} = {\mathit{\boldsymbol{J}}^{ - 1}}\left( \mathit{\boldsymbol{\theta }} \right)\mathit{\boldsymbol{\ddot x}} + \frac{{\rm{d}}}{{{\rm{d}}t}}\left[ {{\mathit{\boldsymbol{J}}^{ - 1}}\left( \mathit{\boldsymbol{\theta }} \right)} \right]\mathit{\boldsymbol{\dot x}}.
\end{array} \right\}
$
(2b)
$
\left. \begin{array}{l}
\mathit{\boldsymbol{\tau }} = {\mathit{\boldsymbol{J}}^{\rm{T}}}\left( \mathit{\boldsymbol{\theta }} \right)\mathit{\boldsymbol{F}},\\
\mathit{\boldsymbol{F = }}{\mathit{\boldsymbol{J}}^{ - {\rm{T}}}}\left( \mathit{\boldsymbol{\theta }} \right)\mathit{\boldsymbol{\tau }}.
\end{array} \right\}
$
(2c)
将式(2a)~(2c)代入式(1)，整理可得操作空间内的机器人动力学方程[6]：
$
{\mathit{\boldsymbol{M}}_{\rm{e}}}\left( \mathit{\boldsymbol{x}} \right)\mathit{\boldsymbol{\ddot x}} + {\mathit{\boldsymbol{C}}_{\rm{e}}}\left( {\mathit{\boldsymbol{x}},\mathit{\boldsymbol{\dot x}}} \right)\mathit{\boldsymbol{\dot x}} + {\mathit{\boldsymbol{G}}_{\rm{e}}}\left( \mathit{\boldsymbol{x}} \right) = \mathit{\boldsymbol{F}}.
$
(3)
式中：x为末端执行器的位置和姿态矢量；Me(x)为工作空间内等价的机器人惯性矩阵，Me(x)=J-T(θ)M(θ)J-1(θ); Ce为等价的离心力和哥氏力矩阵，Ce(x, ${\mathit{\boldsymbol{\dot x}}}$)=J-T(θ){C(θ, ${\mathit{\boldsymbol{\dot \theta }}}$)J-1(θ)+M(θ)d[J-1(θ)]/dt}；Ge为等价的重力项，Ge(x)=J-T(θ)G(θ)；F为机器人末端执行器的输出力矢量，F=J-T(θ)τ.
动力学方程(3)的特性[7]主要如下.
1) Me(x)为正定对称矩阵，即有Me(x)=MeT(x).
2) ${{\mathit{\boldsymbol{\dot M}}}_{\rm{e}}}$(x)-2Ce(x, ${\mathit{\boldsymbol{\dot x}}}$)为反对称矩阵，即有xT${{\mathit{\boldsymbol{\dot M}}}_{\rm{e}}}$(x)-2Ce(x, ${\mathit{\boldsymbol{\dot x}}}$)x=0.
1.2 控制策略
装配机器人在不同的工作状态时，末端执行器的控制需求应相应地调整.例如，当机器人在自由空间运动时，应着重调整末端执行器的位置和姿态，使之沿着规划的参考轨迹运动，以便快速、准确地到达目标装配位置；当机器人末端执行器到达指定的装配位置时，应当考虑到工件装配时的柔性，着重控制末端执行器与工件间的接触力，使之满足期望的接触力，以便使末端执行器与工件之间保持一定的柔顺性，达到既不损伤工件，又能够较快速、灵敏地实现装配的目的.
为此，设计装配机器人的动态柔顺性控制策略.如图 1所示，该控制策略主要由外环的参考轨迹给定模块、内环的轨迹跟踪控制器和动力学变换环节以及动力学参数辨识模块等构成.当机器人在关节空间自由运动时，将参考轨迹xr作为需要跟踪的目标轨迹，采用滑模控制算法设计相应的控制律；当机器人接触工件并执行装配作业时，目标轨迹经由阻抗滤波器获得，并以期望接触力作为该滤波器的输入，以较好地适应该期望接触力的要求.内环的轨迹跟踪控制器基于式(3)的动力学模型和滑模算法设计，以最大程度地减小输入的参考轨迹与实际轨迹之间的误差.动力学变换模块采用式(1)~(3)设计，用于机器人工作空间与关节空间内的动态特性相互变换.
图 1
六自由度机器人动态柔顺性控制原理
Fig. 1
Dynamic compliant control principle of 6 DOF assembly industrialrobot
上述控制律在实际应用时，须已知机器人末端运行的实际轨迹x(t)和参考轨迹值及一阶和二阶导数，并需要已知机器人惯性矩阵Me(x)、离心力和哥氏力矩阵Ce(x, ${\mathit{\boldsymbol{\dot x}}}$)和重力项Ge(x)等矩阵的值.机器人在运行过程中，这些矩阵的实际值常常随着各个关节角度的变化而连续变化，而且装配机器人由于高度集成化的结构设计，使得各个关节之间的连接和耦合较紧密；因此，很难按照各个关节的实际参数，采用解析方法实时计算控制律中的各矩阵值.
采用外部动力学参数动态辨识的方法，在线实时地获得上述控制律中所需要的各常数矩阵和常数值.通过该动力学参数辨识模块，可以实时地获得机器人运行过程中的各种动态参数，如惯性矩阵、离心力和哥氏力矩阵等.
上述控制策略不仅能够满足机器人移动时跟踪参考位姿的要求，而且考虑到装配时工件的柔性，能够保持一定的接触力，达到既保持装配快速性、准确性，又能保证装配质量的目标.整个控制算法设计中所需要的动态参数均可以由参数辨识模块得到，避免了离线参数计算所带来的误差和计算量.
2 动力学参数辨识
在设计上述动态柔顺性控制策略之前，应首先得知相关的动力学模型参数.采用最小二乘算法[8]，设计动力学参数辨识模块如下.
式(1)可以重写为如下形式：
$
{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}^{\rm{T}}}\left( t \right)\mathit{\boldsymbol{\gamma }} = \tau \left( t \right).
$
(4)
式中：ΦT(t)和γ均为动态模型参数的向量，
$
{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}^{\rm{T}}}\left( t \right) = \left[ {\mathit{\boldsymbol{\ddot \theta }},\mathit{\boldsymbol{\dot \theta }},1} \right],
$
(5)
$
\mathit{\boldsymbol{\gamma }} = \left[ \begin{array}{l}
\mathit{\boldsymbol{M}}\left( \mathit{\boldsymbol{\theta }} \right)\\
\mathit{\boldsymbol{C}}\left( \mathit{\boldsymbol{\theta }} \right)\\
\mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{\theta }} \right)
\end{array} \right].
$
(6)
定义函数最小平方辨识误差指标函数Ψ(γ)、P(t)和ε(t)分别为
$
\left. \begin{array}{l}
\mathit{\Psi }\left( \mathit{\boldsymbol{\gamma }} \right) = \int\limits_0^t {{\varepsilon ^{\lambda \left( {t - \upsilon } \right)}}{{\left[ {\tau \left( \upsilon \right) - {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}^{\rm{T}}}\left( \upsilon \right)\mathit{\boldsymbol{\gamma }}} \right]}^2}{\rm{d}}\upsilon } ;\\
0 < \lambda < 1.
\end{array} \right\}
$
(7)
$
P\left( t \right) = {\left[ {\int\limits_0^t {{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}^{\rm{T}}}\left( \upsilon \right)\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}\left( \upsilon \right){\rm{d}}\upsilon } } \right]^{ - 1}}.
$
(8)
式中：λ为给定的正常数.
按照最小二乘算法，可以获得能够最小化式(7)的最小平方误差指标的动力学模型参数的自适应递推律：
$
\mathit{\boldsymbol{\dot {\hat \gamma} }} = \mathit{\boldsymbol{P}}\left( t \right)\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}\left( t \right)\varepsilon \left( t \right).
$
(9)
式(9)的实时积分可以作为动力学模型参数γ的实时观测值.函数ε(t)可以设计为
$
\varepsilon \left( t \right) = \tau \left( t \right) - {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}^{\rm{T}}}\left( t \right)\mathit{\boldsymbol{\hat \gamma }}.
$
(10)
函数P(t)的自适应更新律为
$
\dot P\left( t \right) = \lambda P\left( t \right) - P\left( t \right)\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}\left( t \right){\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}^{\rm{T}}}\left( t \right)P\left( t \right).
$
(11)
通过式(7)~(11)可以自适应、实时地获得动力学模型中各参数的观测值，能够使得参数观测值与真实值之间的均方误差最小化，且满足一定的辨识精度指标.该方法简单易行，实时性和精度均较高.基于该方法，获得动力学模型参数观测值之后，可以基于上述模型参数值，设计相应的末端轨迹跟踪控制器.
3 控制器设计
针对操作空间动力学方程(3)，设计滑模轨迹跟踪控制器，以高效地跟踪给定的参考轨迹.设计阻抗滤波器，以生成装配作业时末端执行器的期望运动轨迹.
3.1 轨迹跟踪控制
设计动态滑模面为轨迹跟踪误差及导数的组合：
$
\mathit{\boldsymbol{s}} = \mathit{\boldsymbol{\dot e}} + \mathit{\boldsymbol{ \boldsymbol{\varLambda} e}}.
$
(12)
式中：Λ为正定对角增益常数矩阵，
$
\mathit{\boldsymbol{e}} = {\mathit{\boldsymbol{x}}_{\rm{r}}} - \mathit{\boldsymbol{x}}.
$
(13)
为了使得末端机械手轨迹跟踪的误差最小化，可以设计末端输入力的控制律为
$
\mathit{\boldsymbol{F}} = {\mathit{\boldsymbol{U}}_{\rm{a}}} + {\mathit{\boldsymbol{U}}_{{\rm{s1}}}} + {\mathit{\boldsymbol{U}}_{{\rm{s2}}}}.
$
(14)
式中：Ua为自适应控制律，Us1和Us2均为鲁棒控制律.
设计自适应控制律Ua为
$
{\mathit{\boldsymbol{U}}_{\rm{a}}} = {{\mathit{\boldsymbol{\hat M}}}_{\rm{e}}}{{\mathit{\boldsymbol{\ddot x}}}_{\rm{r}}} + {{\mathit{\boldsymbol{\hat G}}}_{\rm{e}}} + {{\mathit{\boldsymbol{\hat M}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} \dot e}} + {{\mathit{\boldsymbol{\hat C}}}_{\rm{e}}}{{\mathit{\boldsymbol{\dot x}}}_{\rm{r}}} + {{\mathit{\boldsymbol{\hat C}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} e}}.
$
(15)
为了证明上述控制律作用于装配机器人系统时的稳定性，定义Lyapunov函数为
$
V\left( {\mathit{\boldsymbol{s}}\left( t \right)} \right) = \frac{1}{2}{\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{M}}_{\rm{e}}}\mathit{\boldsymbol{s}}.
$
(16)
求取式(16)对时间的导数，考虑式(3)~(5)和应用性质1)和2)，可得
$
\begin{array}{l}
\dot V\left( {\mathit{\boldsymbol{s}}\left( t \right)} \right) = {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{M}}_{\rm{e}}}\left( {\mathit{\boldsymbol{\ddot e}} + \mathit{\boldsymbol{ \boldsymbol{\varLambda} \dot e}}} \right) + \frac{1}{2}{\mathit{\boldsymbol{s}}^{\rm{T}}}{{\mathit{\boldsymbol{\dot M}}}_{\rm{e}}}\mathit{\boldsymbol{s = }}\\
{\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{M}}_{\rm{e}}}\left( {\mathit{\boldsymbol{\ddot e}} + \mathit{\boldsymbol{ \boldsymbol{\varLambda} \dot e}}} \right) + {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{C}}_{\rm{e}}}\mathit{\boldsymbol{s = }}\\
{\mathit{\boldsymbol{s}}^{\rm{T}}}\left( {{\mathit{\boldsymbol{M}}_{\rm{e}}}{{\mathit{\boldsymbol{\ddot x}}}_{\rm{r}}} - \mathit{\boldsymbol{F}} + {\mathit{\boldsymbol{C}}_{\rm{e}}}\mathit{\boldsymbol{\dot x}} + {\mathit{\boldsymbol{G}}_{\rm{e}}} + {\mathit{\boldsymbol{M}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} \dot e}} + {\mathit{\boldsymbol{C}}_{\rm{e}}}\mathit{\boldsymbol{s}}} \right) = \\
{\mathit{\boldsymbol{s}}^{\rm{T}}}\left( {{\mathit{\boldsymbol{M}}_{\rm{e}}}{{\mathit{\boldsymbol{\ddot x}}}_{\rm{r}}} - \mathit{\boldsymbol{F}} + {\mathit{\boldsymbol{G}}_{\rm{e}}} + {\mathit{\boldsymbol{M}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} \dot e}} + {\mathit{\boldsymbol{C}}_{\rm{e}}}{{\mathit{\boldsymbol{\dot x}}}_{\rm{r}}} + {\mathit{\boldsymbol{C}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} e}}} \right).
\end{array}
$
(17)
将末端执行器的输入力控制律(14)、(15)代入式(17)，可得
$
\begin{array}{l}
\dot V\left( {\mathit{\boldsymbol{s}}\left( t \right)} \right) = {\mathit{\boldsymbol{s}}^{\rm{T}}}\left( {{{\mathit{\boldsymbol{\tilde M}}}_{\rm{e}}}{{\mathit{\boldsymbol{\ddot x}}}_{\rm{r}}} + {{\mathit{\boldsymbol{\tilde G}}}_{\rm{e}}} + {{\mathit{\boldsymbol{\tilde M}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} \dot e}} + {{\mathit{\boldsymbol{\tilde C}}}_{\rm{e}}}{{\mathit{\boldsymbol{\dot x}}}_{\rm{r}}} + } \right.\\
\;\;\;\;\;\;\;\;\;\;\;\;\left. {{{\mathit{\boldsymbol{\tilde C}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} e}} - {\mathit{\boldsymbol{U}}_{{\rm{s1}}}} - {\mathit{\boldsymbol{U}}_{{\rm{s2}}}}} \right) = - {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{U}}_{{\rm{s1}}}}\mathit{\boldsymbol{s}} + {\mathit{\boldsymbol{s}}^{\rm{T}}} \times \\
\;\;\;\;\;\;\;\;\;\;\;\;\left( {{{\mathit{\boldsymbol{\tilde M}}}_{\rm{e}}}{{\mathit{\boldsymbol{\ddot x}}}_{\rm{r}}} + {{\mathit{\boldsymbol{\tilde G}}}_{\rm{e}}} + {{\mathit{\boldsymbol{\tilde M}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} \dot e}} + {{\mathit{\boldsymbol{\tilde C}}}_{\rm{e}}}{{\mathit{\boldsymbol{\dot x}}}_{\rm{r}}} + {{\mathit{\boldsymbol{\tilde C}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} e}} - {\mathit{\boldsymbol{U}}_{{\rm{s2}}}}} \right).
\end{array}
$
(18)
定义由参数辨识所引起的辨识误差为
$
\mathit{\boldsymbol{ \boldsymbol{\varDelta} }} = {{\mathit{\boldsymbol{\tilde M}}}_{\rm{e}}}{{\mathit{\boldsymbol{\ddot x}}}_{\rm{r}}} + {{\mathit{\boldsymbol{\tilde G}}}_{\rm{e}}} + {{\mathit{\boldsymbol{\tilde M}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} \dot e}} + {{\mathit{\boldsymbol{\tilde C}}}_{\rm{e}}}{{\mathit{\boldsymbol{\dot x}}}_{\rm{r}}} + {{\mathit{\boldsymbol{\tilde C}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} e}}.
$
(19)
设该参数辨识误差具有一定的上、下限，界限值设定为δ，有
$
\left| \mathit{\boldsymbol{ \boldsymbol{\varDelta} }} \right| \le \delta .
$
(20)
为了使得装配机器人系统在控制律(14)、(15)的作用下，能够最终大范围渐进稳定，设计相应的鲁棒控制律：
$
\left. \begin{array}{l}
{\mathit{\boldsymbol{U}}_{{\rm{s1}}}} = {\mathit{\boldsymbol{k}}_1}\mathit{\boldsymbol{s}},\\
{\mathit{\boldsymbol{U}}_{{\rm{s2}}}} = {\mathit{\boldsymbol{k}}_2}{\mathop{\rm sgn}} \left( \mathit{\boldsymbol{s}} \right).
\end{array} \right\}
$
(21)
k1和k2分别为6×1正常数控制增益矩阵，k2可以定义为
$
{\mathit{\boldsymbol{k}}_2} = \mathit{\boldsymbol{\alpha }}\left( {\mathit{\boldsymbol{\delta }} + {\mathit{\boldsymbol{\eta }}_{\rm{s}}}} \right).
$
(22)
其中，α和ηs均为设定的常数矩阵，并且满足
$
\mathit{\boldsymbol{\alpha }} \ge 1,{\mathit{\boldsymbol{\eta }}_{\rm{s}}} \ge 0.
$
(23)
将鲁棒控制律式(21)、(22)代入式(18)，考虑式(19)，可得
$
\begin{array}{l}
\dot V\left( {\mathit{\boldsymbol{s}}\left( t \right)} \right) = {\mathit{\boldsymbol{s}}^{\rm{T}}}\left( {{{\mathit{\boldsymbol{\tilde M}}}_{\rm{e}}}{{\mathit{\boldsymbol{\ddot x}}}_{\rm{r}}} + {{\mathit{\boldsymbol{\tilde G}}}_{\rm{e}}} + {{\mathit{\boldsymbol{\tilde M}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} \dot e}} + {{\mathit{\boldsymbol{\tilde C}}}_{\rm{e}}}{{\mathit{\boldsymbol{\dot x}}}_{\rm{r}}} + } \right.\\
\;\;\;\;\;\;\;\;\;\;\;\left. {{{\mathit{\boldsymbol{\tilde C}}}_{\rm{e}}}\mathit{\boldsymbol{ \boldsymbol{\varLambda} e}} - {\mathit{\boldsymbol{k}}_1}\mathit{\boldsymbol{s}} - {\mathit{\boldsymbol{k}}_2}{\mathop{\rm sgn}} \left( \mathit{\boldsymbol{s}} \right)} \right) = - {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{k}}_1}\mathit{\boldsymbol{s}} + \\
\;\;\;\;\;\;\;\;\;\;\;{\mathit{\boldsymbol{s}}^{\rm{T}}}\left( {\mathit{\boldsymbol{ \boldsymbol{\varDelta} }} - {\mathit{\boldsymbol{k}}_2}{\mathop{\rm sgn}} \left( \mathit{\boldsymbol{s}} \right)} \right) = - {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{k}}_1}\mathit{\boldsymbol{s}} - {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathop{\rm sgn}} \left( \mathit{\boldsymbol{s}} \right) \times \\
\;\;\;\;\;\;\;\;\;\;\;\left[ {\mathit{\boldsymbol{\alpha }}\left( {\mathit{\boldsymbol{\delta }} + {\mathit{\boldsymbol{\eta }}_{\rm{s}}}} \right) - {\mathop{\rm sgn}} \left( \mathit{\boldsymbol{s}} \right)\mathit{\boldsymbol{ \boldsymbol{\varDelta} }}} \right].
\end{array}
$
(24)
考虑式(20)、(23)，可得
$
\mathit{\boldsymbol{\alpha \delta }} - {\mathop{\rm sgn}} \left( \mathit{\boldsymbol{s}} \right)\mathit{\boldsymbol{ \boldsymbol{\varDelta} }} \ge 0.
$
(25)
将式(25)代入式(24)，可得
$
\begin{array}{*{20}{c}}
{\dot V\left( {\mathit{\boldsymbol{s}}\left( t \right)} \right) \le - {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{k}}_1}\mathit{\boldsymbol{s}} - {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{\eta }}_{\rm{s}}}{\mathop{\rm sgn}} \left( \mathit{\boldsymbol{s}} \right) \le }\\
{ - {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{k}}_1}\mathit{\boldsymbol{s}} - {\mathit{\boldsymbol{\eta }}_{\rm{s}}}\left| \mathit{\boldsymbol{s}} \right|.}
\end{array}
$
(26)
根据式(26)可知，当采用包含自适应律和鲁棒律的控制律式(14)时，机器人控制系统的Lyapunov函数为常负定.按照Lyapunov的稳定性判据[9]可知，装配机器人的整体闭环控制系统大范围渐近稳定，系统的轨迹跟踪误差将在有限的时间内趋近于零值；各关节力矩和角位移均可以控制在界限内，最终收敛于给定的目标值；机器人末端执行器的轨迹最终收敛于给定的目标值.
为了证明上述结论，特别定义函数W(s(t))为
$
W\left( {\mathit{\boldsymbol{s}}\left( t \right)} \right) = - {\mathit{\boldsymbol{s}}^{\rm{T}}}{\mathit{\boldsymbol{k}}_1}\mathit{\boldsymbol{s}} - {\mathit{\boldsymbol{\eta }}_{\rm{s}}}\left| \mathit{\boldsymbol{s}} \right| \le - \dot V\left( {\mathit{\boldsymbol{s}}\left( t \right)} \right).
$
(27)
W(s(t))对时间积分，可以表达为
$
\int\limits_0^t {W\left( {\mathit{\boldsymbol{s}}\left( \tau \right)} \right){\rm{d}}\tau } = V\left( {\mathit{\boldsymbol{s}}\left( 0 \right)} \right) - V\left( {\mathit{\boldsymbol{s}}\left( t \right)} \right).
$
(28)
鉴于函数V(s(0))有界，V(s(t))非递增且有界，可得
$
\mathop {\lim }\limits_{t \to \infty } \int\limits_0^t {W\left( {\mathit{\boldsymbol{s}}\left( \tau \right)} \right){\rm{d}}\tau } < \infty .
$
(29)
因为函数${\dot W}$(s(τ))也有界，则W(s(τ))为一致连续且收敛.
根据Barblat定理[10]可得如下结论：
$
\mathop {\lim }\limits_{t \to \infty } W\left( {\mathit{\boldsymbol{s}}\left( t \right)} \right) = 0.
$
(30)
根据式(12)、(27)可知，当t→∞时，s(t)和跟踪误差e将收敛至零，x→xr.
上述控制策略内虽然存在辨识误差，但包含参数辨识误差的整体闭环控制系统将最终渐进稳定至滑模面s(t)=0内，末端执行器的实际运行轨迹将收敛至给定的参考轨迹上.整个闭环控制系统对于辨识误差和参数干扰具有相当的鲁棒性.
在实际应用时，需要将上述控制律(14)、(15)和(21)经由式(2c)变换为相应的各个关节力矩，并作为各关节执行器的输入控制量，以控制各关节跟踪相应的参考运动位移轨迹，因此实现末端执行器对于参考轨迹的准确跟踪.
为了消除在控制律执行过程中由符号函数sgn(s)所引起的内部颤振并提高控制律执行的效率，特别采用饱和函数sat(s)替代符号函数，因此有，
$
{\rm{sat}}\left( \mathit{\boldsymbol{s}} \right) = \left\{ \begin{array}{l}
1,s > \mu ;\\
\frac{s}{\mu }, - \mu < s \le \mu ;\\
- 1,s < \mu .
\end{array} \right.
$
(31)
式中：μ为滑模面s(t)内的边界层的厚度[11].
3.2 阻抗滤波器设计
当机器人接触并抓取到目标工件，并执行装配作业时，控制模式应转变成相应的阻抗控制方式，以保持末端执行器与工件的柔性接触力为相应的控制目标.此时，应根据阻抗滤波器和末端执行器的期望接触力，计算出新的末端参考运动轨迹xd.当末端执行器的位姿和期望轨迹存在误差时，将会在末端机械手上产生相应的阻抗力，对末端执行器的运动起到反馈和校正作用，以此引导机器人末端执行器跟踪期望的接触力，以便满足柔顺性装配的需要.
如图 1所示，设计该阻抗滤波器为
$
{\mathit{\boldsymbol{F}}_{\rm{d}}} = {\mathit{\boldsymbol{M}}_{\rm{d}}}\left( {{{\mathit{\boldsymbol{\ddot x}}}_{\rm{d}}} - \mathit{\boldsymbol{\ddot x}}} \right) + {\mathit{\boldsymbol{B}}_{\rm{d}}}\left( {{{\mathit{\boldsymbol{\dot x}}}_{\rm{d}}} - \mathit{\boldsymbol{\dot x}}} \right) + {\mathit{\boldsymbol{K}}_{\rm{d}}}\left( {{\mathit{\boldsymbol{x}}_{\rm{d}}} - \mathit{\boldsymbol{x}}} \right).
$
(32)
式中：Fd和F为末端执行器与目标工件之间的期望接触力和实际接触力；Md、Bd和Kd分别为目标实数非奇异系数矩阵，分别相当于阻抗惯量项、阻尼项和刚度项，并可以分别用于调整响应过程的平滑特性、能量的消耗特性及接触刚度[12].
上述阻抗滤波器通过建立目标阻抗和期望接触力之间的期望动态关系，通过调整式(10)的各系数矩阵的值，从而将末端接触力控制和末端执行器位姿的控制纳入到统一的柔顺性控制框架之内，不仅能够使得机器人有效地跟踪目标轨迹，而且能够较好地满足柔性接触的要求.
阻抗参数Md、Bd和Kd的确定和调整要根据末端执行器的操作对象和机器人运行条件来确定.当式(32)表达的阻抗滤波器系统处于稳态时，末端执行器的实际速度和加速度趋近于0，此时的末端执行器动力学特性类似于以xd为平衡位置的可编程弹簧.
当机器人控制系统满足一定的条件时，可以切换到上述接触力阻抗控制，此时的切换条件可以以实际检测到的接触力为判别依据.当机器人末端执行器在自由空间运动时，末端的实际接触力为零值；当末端执行器接触到工件并进行装配作业时，末端接触力应为非零值.采用双极性连续可微的sigmoid函数，设计相应的判别条件：
$
{\rm{sigm}}\left( {{F_{\rm{e}}}} \right) = \left\{ {\begin{array}{*{20}{l}}
{1,{F_{\rm{e}}} > \bar \omega ;}\\
{0,{F_{\rm{e}}} \le \bar \omega .}
\end{array}} \right.
$
(33)
式中：Fe为末端执行器与工件直接的接触力，该接触力可以由安装于末端执行器上面的力传感器检测获得；ω为较小的接触力阈值，该值可以根据实际情况设定.
根据式(11)可知，当式(33)的判别结果为0时，机器人控制系统可以执行自由空间的运动轨迹跟踪控制；当式(33)判别的结果为1时，可以根据式(10)执行末端接触力的阻抗滤波控制.此时，根据末端接触力偏差偏离期望接触力的程度，由阻抗滤波器(32)自动生成期望的末端执行器位姿，调整末端执行器与工件之间的作用力，实现末端执行器操作的动态柔顺性.当t→∞时，末端执行器的实际接触力将渐进趋近于期望接触力，执行器末端的实际位姿轨迹将最终收敛于期望的位姿轨迹，实现机器人末端执行器的动态柔顺性控制的要求.
4 仿真实验验证与分析
为了验证上述控制策略的可行性和高效性，基于六自由度装配实验台上进行现场装配和动态轨迹跟踪的仿真实验研究.如图 2所示，该装配实验平台安装有两台三菱电机的RV-4F型垂直六关节装配机器人[13]，主要用于完成典型低压电器，如空气开关的装配.每台机器人的本体质量为39 kg，最大可搬运工件质量为4 kg，最大合成速度为9 m/s，最大动作幅度为515 mm，位置重复精度为0.02 mm，允许惯性为0.2 kg·m2.机器人各关节均由带有全轴制动的AC伺服电机驱动，电机容量为400 W，各电机均自带绝对编码器，以便实时检测角位移，并反馈至上述控制器中.控制器采用Matlab编程，基于Robotics工具箱实现仿真控制.
图 2
六自由度装配机器人实验平台
Fig. 2
Experimental platform ofsix DOF assembly robot
控制过程中的主要参数设计如下：
$
\mathit{\boldsymbol{ \boldsymbol{\varLambda} }} = {\rm{diag}}\left[ {6.1,6,6.2,6.8,7.1,8} \right],
$
$
{\mathit{\boldsymbol{k}}_1} = {\rm{diag}}\left[ {12.8,12.2,10,16,15,18} \right],
$
$
{\mathit{\boldsymbol{k}}_2} = {\rm{diag}}\left[ {2.8,2.2,1.6,12,1.5,1.8} \right],\mu = 0.5,
$
机器人运动轨迹的初始值均标定为零值.
当机器人末端机械手与工件接触时，采用柔顺性的接触力控制的方式，此时的期望柔性接触力的初始值设定为12 N.柔顺力控制采样周期为10 ms，时间间隔为100 s，抓取并安装低压电器的的起始速度均为18 mm/s.在柔顺力控制过程中，最大力超调量设定为10%，抓取和安装时的稳态误差最大为±1.2 N.系统切换至柔顺性接触力控制时的较小接触力阈值ω设定为0.04.阻抗惯量项的初始值Md=diag[100, 102, 103, 101, 105, 98]，目标阻尼项的初始值Bd=diag[281, 269, 285, 278, 276, 286]，目标刚度项的初始值Kd=diag[1 861, 1 852, 1 895, 1 796, 1 827, 1 918].
为了验证提出的动态柔顺性控制的高效性，特别与典型的比例微分(proportional-derivative, PD)控制器[14]进行控制效果的对比研究.该典型比例微分控制器主要用于末端机械手的轨迹跟踪，不具备工件装配时的接触力柔顺性控制的功能.在仿真实验过程中，多次重复进行末端机械手轨迹跟踪与工件组装操作，以获取丰富的实际数据，选取合理的数据以求取轨迹跟踪和接触力控制的平均相对误差.
各关节轴角度跟踪的平均相对误差定义为
$
{\rm{AREJ}} = \frac{1}{N}\sum\limits_{j = 1}^N {\frac{{{\theta _j} - {\theta _{j{\rm{d}}}}}}{{{\theta _{j{\rm{d}}}}}} \times 100\% } .
$
(34)
末端机械手位置跟踪的平均相对误差定义为
$
{\rm{AREE}} = \frac{1}{{6N}}\sum\limits_{i = 1}^6 {\sum\limits_{j = 1}^N {\frac{{{x_{ij}} - {x_{{\rm{r}}ij}}}}{{{x_{{\rm{r}}ij}}}} \times 100\% } } .
$
(35)
末端接触力的平均相对误差定义为
$
{\rm{AREF}} = \frac{1}{{6N}}\sum\limits_{i = 1}^6 {\sum\limits_{j = 1}^N {\frac{{{F_{ij}} - {F_{{\rm{d}}ij}}}}{{{F_{{\rm{d}}ij}}}} \times 100\% } } .
$
(36)
式中：N为数据采样点的个数，θj和θjd分别为实际和参考关节角度的第j个采样值，xij和xrij分别为末端执行器位姿和参考位姿的第i个分量的第j个采样值，Fij和Fdij分别为实际和期望末端接触力的第i个力/力矩分量的第j个采样值.
4.1 空间轨迹跟踪
如图 3所示，在PD控制作用下，六自由度机器人各个关节角位移的稳态跟踪的相对误差为-8%~+10%，呈现明显的发散状特征；当采用提出的动态柔顺性控制时，机器人各个关节角位移跟踪的稳态相对误差基本可以较好地控制为-4%~+3%.与比例微分控制相比，动态柔顺性控制能够在较宽广的范围内更好地实现机器人各关节角位移的动态跟踪控制，跟踪精度能够维持地相对较高.
图 3
工业装配机器人的各个关节角位移轨迹跟踪的平均相对误差
Fig. 3
Relative tracking errors for joint angle in industrial robot
如图 4所示，在比例微分控制作用下，六自由度机器人末端执行器的位置轨迹的相对误差为-10%~+10%，没有明显的稳定性的迹象；当采用提出的动态柔顺性控制时，末端执行器对于位置轨迹的相对跟踪误差可以有效地降低为-3%~+3%，具有明显的收敛和稳定的趋势.该结果说明，相对于典型的PD控制，动态柔顺性控制具有良好的末端位置轨迹的大范围跟踪与控制的能力，动态自适应性能较佳.
图 4
工业装配机器人末端机械手位置轨迹跟踪的平均相对误差
Fig. 4
Average relative tracking error of end effector of industrial assembling robot
4.2 接触力柔顺性控制
如图 5所示，当进行工件装配操作时，典型的比例微分控制由于不具备接触力柔顺性控制的功能，致使执行器实际装配的位置轨迹与期望轨迹之间的相对误差较大，最大可以达到10%，且波动较剧烈；当采用动态柔顺性控制时，由于当末端执行器接触工件时，机器人控制系统可以自动切换成为带有阻抗滤波器的柔顺性控制，使得末端执行器不仅能够较好地实现期望装配位置的跟踪，而且能够与工件之间保持良好的柔顺性接触力，此时的装配位置轨迹的相对误差可以较好地控制并维持在-3%~+4%.
图 5
工业装配机器人的末端执行器位置轨迹跟踪的平均相对误差
Fig. 5
Average relative tracking error of end effector of industrial assembling robot
如图 6所示，当采用典型比例微分控制时，末端执行器与工件之间的接触力呈现明显的发散状态，误差为-10%~+8%，呈剧烈波动状况；当采用动态柔顺性控制时，由于具备接触力阻抗滤波和跟踪控制功能，此时的实际接触力与期望接触力之间的相对误差基本可以较好地维持在-4%~+4%，呈现明显的收敛迹象.相比于典型的比例微分控制，采用提出的动态柔顺性控制策略不仅能够实现空间位置轨迹的较精确的跟踪，而且能够实现柔顺性的接触力控制，使得工件装配时的实际接触力与期望接触力之间的误差尽量收敛至零值.
图 6
工业装配机器人的末端执行器的接触力平均相对误差
Fig. 6
Average relative tracking error of contact force of end effector in industrial assembling robot
5 结语
本文针对六自由度工业装配机器人，设计动态柔顺性控制策略，使之不仅能够保持快速高精度的参考轨迹跟踪，而且能够动态自适应地实现工件装配时的接触力柔顺性控制.构建机器人关节空间和末端执行器操作空间的标准动力学模型.给出该控制策略，相应地设计各个模块.采用滑模算法设计轨迹跟踪控制器；采用阻抗滤波器，生成装配作业时末端执行器的期望运动轨迹；采用sigmoid函数，设计轨迹跟踪与接触力控制的判别模块；采用最小二乘算法，设计动力学参数辨识模块.采用Lyapunov函数证明了该控制策略的大范围渐进稳定性和收敛性.基于六自由度工业装配机器人实验平台上，通过多次的动态轨迹跟踪的对比性仿真实验，验证了上述控制策略的可行性和有效性.
|
|
|
|
