Gravity Compensation Algorithm of Humanoid Manipulator Trajectory Tracking Control Based on Trigonometric Function
-
摘要:
机器人手臂的运动控制主要通过轨迹跟踪控制来完成，重力作为空间机械臂中最大的干扰力，有必要通过补偿方法来提高控制精度和效率.常规的重力补偿算法需要精确的动力学参数和复杂的矩阵变换运算，很难运用到实时控制中.基于机械臂空间几何特性和扭矩平衡原理提出一种比较简便实用的重力补偿算法，该算法不需要复杂的空间坐标变换计算，不需要获得所有精确的动力学模型和参数，只需要对机械臂的最大重力矩进行估计和简单的三角函数计算，而且该算法可以推广到适用于空间N个关节的非平行轴机械臂.采用最容易分析的PD控制器加重力补偿控制两关节和三关节机械臂来进行仿真以验证重力补偿后的控制效果.通过对机械臂进行重力补偿和不补偿以及与其他补偿方法进行仿真对比，结果显示该重力补偿算法能够较好地实现轨迹跟踪控制，控制精度较高，能有效地减轻控制器的工作负担，提高系统的稳定性，补偿效果较好.
Abstract:Humanoid manipulator motion control is mainly carried out by trajectory tracking control. As the largest disturbance of a spatial manipulator, gravity force is necessarily compensated to improve the control precision and efficiency. The conventional gravity compensation algorithm requires accurate dynamic parameters and complex matrix transformation operation, which is difficult to be used in real time control. Based on the geometric features and torque balance principle of spatial manipulators, this paper proposed a simple and practical gravity compensation algorithm that does not need complex computation of coordinate transformation and the accurate dynamic models and parameters, only needs the largest gravity torque estimation of manipulator and the calculation of simple trigonometric function, while the algorithm is also suitable for non-parallel axis spatial manipulators with N joints. To verify the gravity compensation control effect, the PD controller with gravity compensation was used to controll the manipulators of two and three joints based on simulation. It was simulated by gravity compensation and without compensation as well as comparison with other compensation methods. Results show that the gravity compensation algorithm can improve the trajectory tracking control with high control precision. The compenstion method can effectively reduce the work burden of a controller and improve the stability of the system. The compensation effect is satisfactory.
-
服务型机器人有着与人相似的外表和结构，主要依靠机械手臂来完成日常操作任务，要完成这些动作及任务需要采用轨迹跟踪控制.由于机械臂在空间运动中的驱动力绝大部分都是为了克服自身的重力做功，为了减小重力干扰并提高控制精度，有必要在控制过程中对机械臂自身的重力进行补偿.
目前，关于重力补偿的研究取得了一定的进展，主要可分为机械结构法和控制分析法[1].机械结构法指的是在机械结构上增加一些补偿装置[2-5]，例如采用在每个关节增加弹簧的方法来补偿自身重力.控制分析法指的是在控制策略上增加前馈或者反馈环节来对自身重力进行补偿，本文研究的是控制分析法.文献[6-7]提出了具有固定重力补偿的机械臂固定点位置控制算法，这种算法比较简单，适用于操作空间中固定位置控制，但是用于轨迹跟踪控制误差会比较大.文献[8-9]采用最小二乘法和BP神经网络对机械臂连杆的质量和质心等动力学参数进行识别，估算出了重力项，但采用这种方法进行计算相当费时且复杂，不便于实时控制.文献[10-11]采用对重力矩实时准确估值的重力补偿算法实现机械臂轨迹跟踪控制，但是在实际运用中，由于各种动力学参数很难准确得到，因此很难保证重力补偿的准确性.文献[12-13]通过拉格朗日方程建立系统动力学模型，计算各部分重心在关节空间中所产生的转矩，并利用力矩电动机输出相应的转矩进行重力补偿，该方法的缺点是多自由度的重力矩阵运算量比较大，而且拉格朗日方程的准确参数也不容易获得.文献[14]在传统固定重力补偿的基础上推导出各关节的重力补偿函数，针对除冰机器人右臂提出了一种比较精确的在线重力补偿PD控制策略，但是这种补偿算法需要事先知道机械臂的质量、质心等参数，而且只适用于平行轴机械臂.
通过以上综述可看出，常规的重力补偿算法需要精确的动力学参数和复杂的矩阵变换运算，很难运用到实时控制中；一些改进的实时控制算法虽然计算简单，但是只适用于平行轴机械臂[15].针对以上研究的一些不足，本文研究一种比较简单的重力补偿方法.该算法不需要很复杂的计算，也不需要获得所有精确的动力学模型和参数就能取得比较好的控制效果，而且还适用于非平行轴机械臂.
1. 机械臂重力补偿算法
1.1 机械臂重力补偿问题分析
众所周知，根据拉格朗日建模定理, 具有n自由度的机械臂动力学方程可以表示为
$$ \mathit{\boldsymbol{M}}\left( \mathit{\boldsymbol{q}} \right)\mathit{\boldsymbol{\ddot q}} + \mathit{\boldsymbol{C}}\left( {\mathit{\boldsymbol{q}},\mathit{\boldsymbol{\dot q}}} \right)\mathit{\boldsymbol{\dot q}} + \mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{q}} \right) = \mathit{\boldsymbol{ \boldsymbol{\varGamma} }} $$ (1) 式中：q(t)表示关节的角位移向量，是有界可微向量；M(q)表示机械臂的惯性矩阵；$\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})$表示哥氏力和离心力矢量; G(q)表示重力向量；Γ表示输入力矩.
机械臂通常都工作在低速状态下，由式(1)可看出，相较于惯性力、哥氏力和离心力，机器人的动态性能受到重力的影响更大.如果把重力视为干扰，那么大部分输入的力矩所做功都用来克服重力矩的干扰了，如果在控制过程中能有效地对重力进行补偿，将会极大提高控制的效率.
1.2 机械臂重力矩的计算
根据机器人拉格朗日方程式，具有n个关节的机械臂第i个关节的重力项可表示为
$$ {g_i} = - \sum\limits_{k = i}^n {{m_k}} {\mathit{\boldsymbol{g}}^{\rm{T}}}\frac{{{\partial ^0}{\mathit{\boldsymbol{T}}_k}}}{{\partial {\mathit{\boldsymbol{q}}_i}}}{}^k{\mathit{\boldsymbol{p}}_k} $$ (2) 式中：mk为机器人臂各连杆质量；g为在基础坐标系中的重力加速度向量；0Tk为第k坐标系相对于基础坐标系的齐次变换矩阵; kpk表示从第k坐标系原点到连杆k质心的距离向量.那么，重力矩阵
$$ \mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{q}} \right) = {\left[ {{g_1},{g_2},{g_3}, \cdots ,{g_n}} \right]^{\rm{T}}} $$ (3) 如果能应用式(2)准确计算出每一时刻的重力项并进行补偿，也就是令重力补偿项
$$ {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_0} = \mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{q}} \right) $$ (4) 令输入力矩
$$ \mathit{\boldsymbol{ \boldsymbol{\varGamma} }} = {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_0} + \mathit{\boldsymbol{ \boldsymbol{\varGamma} '}} $$ (5) 将式(3)(4)带入式(1)，得
$$ \mathit{\boldsymbol{M}}\left( \mathit{\boldsymbol{q}} \right)\mathit{\boldsymbol{\ddot q}} + \mathit{\boldsymbol{C}}\left( {\mathit{\boldsymbol{q}},\mathit{\boldsymbol{\dot q}}} \right)\mathit{\boldsymbol{\dot q}} = \mathit{\boldsymbol{ \boldsymbol{\varGamma} '}} $$ (6) 式中Γ′为重力补偿后的输入力矩.由此可看出这样就能完全补偿重力项，也就是说能够完全消除重力矩的干扰.但是这种应用的前提条件是必须知道机器人臂各连杆诸如质量和质心位置等动力学参数.从式(2)可以看到，要计算机械臂各关节重力矩，必须事先知道质量mk和质心距离kpk两个参数值，而采用常规的计测方法很难获得它们较为准确的值，而且这种方法有大量的关于旋转变换矩阵的计算，当关节较多的时候计算量非常大，因此这种方法很难应用在实际控制中.
1.3 与重力垂直的平行轴机械臂重力补偿算法
n个关节都为旋转关节的机械臂坐标如图 1所示，x为连杆方向，z为旋转轴方向.在基础坐标系{0}中，重力方向为-y0方向，旋转轴方向z0垂直于平面x0y0向外，如不特别说明，接下来研究的基础坐标系都采用图 1所示的坐标系.关节角q1定义为基础坐标系中x0与连杆x1的夹角，qn定义为连杆xn-1与xn的夹角；根据关节角的定义与重力的性质可知，当此机械臂沿与重力垂直的方向完全展开时，即q1=q2=…=qn=0时受到的重力矩最大.
如果机械臂满足2个条件：1)所有的关节都为旋转关节；2)对于所有连杆，第i个连杆的质心位于第i个坐标系和第i+1个坐标系原点的连线上，则各关节受到的重力矩极大值可以通过
$$ \left[ {\begin{array}{*{20}{c}} {{{\bar g}_1}}\\ {{{\bar g}_2}}\\ {{{\bar g}_3}}\\ \vdots \\ {{{\bar g}_{n - 1}}}\\ {{{\bar g}_n}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{m_1}l_1^{\rm{c}} + {l_1}\sum\limits_{k = 2}^n {{m_k}} }\\ {{m_2}l_2^{\rm{c}} + {l_2}\sum\limits_{k = 3}^n {{m_k}} }\\ {{m_3}l_3^{\rm{c}} + {l_3}\sum\limits_{k = 4}^n {{m_k}} }\\ \vdots \\ {{m_{n - 1}}l_{n - 1}^{\rm{c}} + {l_{n - 1}}{m_n}}\\ {{m_n}l_n^{\rm{c}}} \end{array}} \right]g $$ (7) 推导出.式中：$\overline{g}_{n} $为第n个关节重力矩的极大值；mn为第n个连杆的重力；ln为第n个连杆的长度；lnc为第n个连杆的质心与第n个坐标系原点的距离；g为重力加速度.
与重力垂直的平行轴机械臂指的是所有关节轴角θn=kπ(k为整数)，也就是说所有机械臂关节轴都与重力方向垂直且相互平行.根据力矩的性质和机械臂的空间几何特性，以各关节的最大重力矩和各关节角为变量，设计在线重力补偿算法.
$$ \left[ {\begin{array}{*{20}{c}} {\bar G\left( {{q_1}} \right)}\\ {\bar G\left( {{q_2}} \right)}\\ {\bar G\left( {{q_3}} \right)}\\ \vdots \\ {\bar G\left( {{q_{n - 1}}} \right)}\\ {\bar G\left( {{q_n}} \right)} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\left( {{m_1}l_1^{\rm{c}} + {l_1}\sum\limits_{k = 2}^n {{m_k}} } \right)g\cos {q_1}}\\ {\left( {{m_2}l_2^{\rm{c}} + {l_2}\sum\limits_{k = 3}^n {{m_k}} } \right)g\cos \left( {{q_1} + {q_2}} \right)}\\ {\left( {{m_3}l_3^{\rm{c}} + {l_3}\sum\limits_{k = 4}^n {{m_k}} } \right)g\cos \left( {{q_1} + {q_2} + {q_3}} \right)}\\ \vdots \\ {\left( {{m_{n - 1}}l_{n - 1}^{\rm{c}} + {l_{n - 1}}{m_n}} \right)g\cos \left( {\sum\limits_{i = 1}^{n - 1} {{q_i}} } \right)}\\ {{m_n}l_n^{\rm{c}}g\cos \left( {\sum\limits_{i = 1}^n {{q_i}} } \right)} \end{array}} \right] $$ (8) 式中：$\overline{G}\left(q_{n}\right) $为第n个关节需要补偿的力矩；qi为第i个关节角.
1.4 一般的空间机械臂重力补偿算法
关节轴角θn定义为旋转关节轴zn与水平面z0x0的夹角，如图 2所示.
空间机械臂在控制过程中是否需要重力补偿以及需要补偿多少，与关节轴角密切相关，只要关节轴角$\theta_{n} \neq \frac{k \pi}{2} $ (k为非零整数)就会受到重力的干扰，都有必要对重力进行补偿.根据力矩的性质和机械臂的空间几何特性，以各关节的最大重力矩、关节轴角和关节角为变量，设计在线重力补偿算法：
$$ \left[ {\begin{array}{*{20}{c}} {\bar G\left( {{q_1}} \right)}\\ {\bar G\left( {{q_2}} \right)}\\ {\bar G\left( {{q_3}} \right)}\\ \vdots \\ {\bar G\left( {{q_{n - 1}}} \right)}\\ {\bar G\left( {{q_n}} \right)} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\left( {{m_1}l_1^{\rm{c}} + {l_1}\sum\limits_{k = 2}^n {{m_k}} } \right)g\cos \left( {{\delta _1}{q_1}} \right)}\\ {\left( {{m_2}l_2^{\rm{c}} + {l_2}\sum\limits_{k = 3}^n {{m_k}} } \right)g{\delta _2}\cos \left( {{\delta _1}{q_1} + {q_2}} \right)}\\ {\left( {{m_3}l_3^{\rm{c}} + {l_3}\sum\limits_{k = 4}^n {{m_k}} } \right)g{\delta _3}\cos \left( {{\delta _1}{q_1} + {\delta _2}{q_2} + {q_3}} \right)}\\ \vdots \\ {\left( {{m_{n - 1}}l_{n - 1}^{\rm{c}} + {l_{n - 1}}{m_n}} \right)g{\delta _{n - 1}}\cos \left( {\sum\limits_{i = 1}^{n - 2} {{\delta _i}{q_i}} + {q_{n - 1}}} \right)}\\ {{m_n}l_n^{\rm{c}}g{\delta _n}\cos \left( {\sum\limits_{i = 1}^{n - 1} {{\delta _i}{q_i}} + {q_n}} \right)} \end{array}} \right] $$ (9) 式中δn表示补偿系数.
一般的空间机械臂重力补偿算法式(9)与重力垂直的平行轴机械臂重力补偿算法式(8)相比较多了一个补偿系数δn，该系数跟关节轴角有关，定义为
$$ \left[ {\begin{array}{*{20}{c}} {{\delta _1}}\\ {{\delta _2}}\\ {{\delta _3}}\\ \vdots \\ {{\delta _{n - 1}}}\\ {{\delta _n}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\cos {\theta _1}}\\ {\cos {\theta _2}}\\ {\cos {\theta _3}}\\ \vdots \\ {\cos {\theta _{n - 1}}}\\ {\cos {\theta _n}} \end{array}} \right] $$ (10) 当第n个关节轴角$\theta_{n}=\frac{k \pi}{2} $ (k为非零整数)时，补偿系数为零，因为此时关节轴与重力方向平行，重力对关节的旋转运动几乎没有影响，所以不需要重力补偿；以前研究得比较多的是所有关节轴角都为θn=kπ(k为整数)的平行轴机械臂，这个时候所有关节轴与重力方向垂直，补偿系数为1，关节的旋转运动受到重力的影响最大.当关节轴的方向与重力的方向既不平行也不垂直的时候，每个关节的补偿系数δn用来表示重力对该关节旋转运动的影响程度，该值越大表示重力的干扰越大，并根据算法进行相应的重力补偿计算.因此，经过补偿系数修正的重力补偿算法适合于任意关节轴角的非平行轴机械臂，从而拓展了本文算法的应用范围.
2. 控制方案设计及稳定性分析
2.1 控制方案设计
为了验证机械臂轨迹跟踪控制系统补偿算法的可行性及有效性，并且分析方便，采用最简单的PD控制，双关节的机械臂系统控制流程如图 3所示.
控制系统的期望输入是qd，输出变量是实时关节角q，误差变量定义为
$$ \mathit{\boldsymbol{e}} = {\mathit{\boldsymbol{q}}_{\rm{d}}} - \mathit{\boldsymbol{q}},\mathit{\boldsymbol{\dot e}} = {\mathit{\boldsymbol{\dot q}}_{\rm{d}}} - \mathit{\boldsymbol{\dot q}} $$ (11) 其中PD控制器利用输入与输出的偏差来进行控制，其控制律为
$$ \mathit{\boldsymbol{ \boldsymbol{\varGamma} }} = {\mathit{\boldsymbol{K}}_{\rm{P}}}\mathit{\boldsymbol{e}} + {\mathit{\boldsymbol{K}}_{\rm{v}}}\mathit{\boldsymbol{\dot e}} + \mathit{\boldsymbol{\bar G}}\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}}} \right) $$ (12) 式中：KP、Kv为n维正定对角常数矩阵；G中各变量的定义参见式(9).
$$ \mathit{\boldsymbol{\bar G}}\left( \mathit{\boldsymbol{q}} \right) = {\left[ {G\left( {{q_1}} \right),G\left( {{q_2}} \right), \cdots ,G\left( {{q_n}} \right)} \right]^{\rm{T}}} $$ (13) 联立式(1)和式(12)，得
$$ \mathit{\boldsymbol{M}}\left( \mathit{\boldsymbol{q}} \right)\mathit{\boldsymbol{\ddot q}} + \mathit{\boldsymbol{C}}\left( {\mathit{\boldsymbol{q}},\mathit{\boldsymbol{\dot q}}} \right)\mathit{\boldsymbol{\dot q}} + \mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{q}} \right) = {\mathit{\boldsymbol{K}}_{\rm{P}}}\mathit{\boldsymbol{e}} + {\mathit{\boldsymbol{K}}_{\rm{v}}}\mathit{\boldsymbol{\dot e}} + \mathit{\boldsymbol{\bar G}}\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}}} \right) $$ (14) 2.2 系统稳定性分析
将闭环控制方程式(14)联立关系式(11)改写成状态空间形式
$$ \begin{array}{*{20}{c}} {\frac{{\rm{d}}}{{{\rm{d}}t}}\left[ {\begin{array}{*{20}{c}} \mathit{\boldsymbol{e}}\\ {\mathit{\boldsymbol{\dot q}}} \end{array}} \right] = }\\ {\left[ {\begin{array}{*{20}{c}} { - \mathit{\boldsymbol{\dot q}}}\\ {\mathit{\boldsymbol{M}}{{\left( \mathit{\boldsymbol{q}} \right)}^{ - 1}}\left[ {{\mathit{\boldsymbol{K}}_{\rm{P}}}\mathit{\boldsymbol{e}} + {\mathit{\boldsymbol{K}}_{\rm{v}}}\mathit{\boldsymbol{\dot q}} - \mathit{\boldsymbol{C}}\left( {\mathit{\boldsymbol{q}},\mathit{\boldsymbol{\dot q}}} \right)\mathit{\boldsymbol{\dot q}} + \mathit{\boldsymbol{\bar G}}\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}}} \right) - \mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{q}} \right)} \right]} \end{array}} \right]} \end{array} $$ (15) 构造李亚普洛夫函数
$$ V\left( {\mathit{\boldsymbol{e}},\mathit{\boldsymbol{\dot q}}} \right) = {\mathit{\boldsymbol{q}}^{\rm{T}}}\mathit{\boldsymbol{P}}\left( \mathit{\boldsymbol{e}} \right)\mathit{\boldsymbol{\dot q}} + h\left( \mathit{\boldsymbol{e}} \right) $$ (16) 式中
$$ \left\{ \begin{array}{l} \mathit{\boldsymbol{P}}\left( \mathit{\boldsymbol{e}} \right) = \frac{1}{2}M\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}} - \mathit{\boldsymbol{e}}} \right)\\ h\left( \mathit{\boldsymbol{e}} \right) = u\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}} - \mathit{\boldsymbol{e}}} \right) - {k_{\rm{u}}} + \frac{1}{2}{\mathit{\boldsymbol{e}}^{\rm{T}}}{\mathit{\boldsymbol{K}}_{\rm{P}}}\mathit{\boldsymbol{e}} + {\mathit{\boldsymbol{e}}^{\rm{T}}}\mathit{\boldsymbol{\bar G}}\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}}} \right) + \\ \;\;\;\;\;\;\frac{1}{2}\mathit{\boldsymbol{\bar G}}{\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}}} \right)^{\rm{T}}}\mathit{\boldsymbol{K}}_{\rm{P}}^{ - 1}\mathit{\boldsymbol{\bar G}}\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}}} \right) \end{array} \right. $$ (17) 式中u(q)表示机械臂系统的势能函数.
$$ {k_{\rm{u}}} = \mathop {\min }\limits_\mathit{\boldsymbol{q}} \left\{ {u\left( \mathit{\boldsymbol{q}} \right)} \right\} $$ (18) 假设机械臂都是旋转关节，那么
$$ u\left( \mathit{\boldsymbol{q}} \right) - {k_{\rm{u}}} \ge 0 $$ (19) 可以证明该李亚普洛夫函数是非负定的.
将此函数对时间进行求导
$$ \begin{array}{*{20}{c}} {\dot V\left( {\mathit{\boldsymbol{e}},\mathit{\boldsymbol{\dot q}}} \right) = {{\mathit{\boldsymbol{\dot q}}}^{\rm{T}}}\mathit{\boldsymbol{M}}\left( \mathit{\boldsymbol{q}} \right)\mathit{\boldsymbol{\ddot q}} + \frac{1}{2}{{\mathit{\boldsymbol{\dot q}}}^{\rm{T}}}\mathit{\boldsymbol{\dot M}}\left( \mathit{\boldsymbol{q}} \right)\mathit{\boldsymbol{\dot q}} + }\\ {{{\mathit{\boldsymbol{\dot q}}}^{\rm{T}}}\mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{q}} \right) + {\mathit{\boldsymbol{e}}^{\rm{T}}}{\mathit{\boldsymbol{K}}_{\rm{P}}}\mathit{\boldsymbol{\dot e}} + {{\mathit{\boldsymbol{\dot e}}}^{\rm{T}}}\mathit{\boldsymbol{\bar G}}\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}}} \right)} \end{array} $$ (20) 根据重力矩阵的定义
$$ \mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{q}} \right) = \frac{\partial }{{\partial \mathit{\boldsymbol{q}}}}u\left( \mathit{\boldsymbol{q}} \right) $$ (21) 并联立式(15)，得
$$ \begin{array}{*{20}{c}} {\dot V\left( {\mathit{\boldsymbol{e}},\mathit{\boldsymbol{\dot q}}} \right) = {\mathit{\boldsymbol{q}}^{\rm{T}}}{\mathit{\boldsymbol{K}}_{\rm{P}}}\mathit{\boldsymbol{e}} - {{\mathit{\boldsymbol{\dot q}}}^{\rm{T}}}{\mathit{\boldsymbol{K}}_{\rm{v}}}\mathit{\boldsymbol{\dot q}} + {{\mathit{\boldsymbol{\dot q}}}^{\rm{T}}}\mathit{\boldsymbol{\bar G}}\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}}} \right) + }\\ {{\mathit{\boldsymbol{e}}^{\rm{T}}}{\mathit{\boldsymbol{K}}_{\rm{P}}}\mathit{\boldsymbol{\dot e}} + {{\mathit{\boldsymbol{\dot e}}}^{\rm{T}}}\mathit{\boldsymbol{\bar G}}\left( {{\mathit{\boldsymbol{q}}_{\rm{d}}}} \right)} \end{array} $$ (22) 利用机械臂动力学方程的性质[16]
$$ {\mathit{\boldsymbol{\dot q}}^{\rm{T}}}\left[ {\frac{1}{2}\mathit{\boldsymbol{\dot M}}\left( \mathit{\boldsymbol{q}} \right) - \mathit{\boldsymbol{C}}\left( {\mathit{\boldsymbol{q}},\mathit{\boldsymbol{\dot q}}} \right)} \right]\mathit{\boldsymbol{\dot q}} = 0 $$ (23) 根据式(11)，得
$$ \mathit{\boldsymbol{\dot e}} = - \mathit{\boldsymbol{\dot q}} $$ (24) 联立式(22)~(24)，得
$$ \dot V\left( {\mathit{\boldsymbol{e}},\mathit{\boldsymbol{\dot q}}} \right) = - {\mathit{\boldsymbol{\dot q}}^{\rm{T}}}{\mathit{\boldsymbol{K}}_v}\mathit{\boldsymbol{\dot q}} \le 0 $$ (25) 由式(25)可知，当Kv正定时，$ \dot{V}(\boldsymbol{e}, \dot{\boldsymbol{q}})$半负定.根据李亚普洛夫稳定性判据，位置误差e和速度$\dot{\boldsymbol{q}}(t)$在任何初始条件下都是有界的.根据La Salle定理[15]可知，$ [\boldsymbol{e}, \dot{\boldsymbol{q}}]$ =[0, 0]是系统全局渐近稳定平衡点，从而证明了该系统的稳定性，满足控制系统的基本要求.
3. 机械臂重力补偿仿真及研究
3.1 两关节机械臂仿真
为了验证该重力补偿控制方法的有效性，对图 3所示的两关节平行轴机械臂进行实验仿真测试，要求机械臂的末端移动轨迹为一个圆，如图 4所示.
采用ADAMS软件进行两关节机械臂的建模，采用MATLAB SIMULINK进行PD重力补偿控制器的设计，仿真测试方案如下.
1) 建立如图 4所示的双关节平行轴机械臂模型，各参数如表 1所示.
表 1 两关节机械臂参数Table 1. Parameters of 2-joint manipulator参数 l1、l2/mm l1c、l1c/mm m1、m2/kg 数值 250 125 0.88 2) 要使机械臂的末端运动轨迹按指定的轨迹运动，这时需要通过轨迹方程计算出2个关节的关节变量，然后将这2个关节变量作为控制系统模型的关节输入.设q1、q2分别为关节1和关节2的角位移，l1、l2为2个关节的长度，非线性方程组为
$$ \left\{ \begin{array}{l} x = {l_1}\cos {q_1} + {l_2}\cos {q_2}\\ y = {l_1}\sin {q_1} + {l_2}\sin {q_2} \end{array} \right. $$ (26) 将时间区间t取为0到6.28s，中间取400个点，解出每一个时间点的关节角从而得出整个时间区间上的关节空间轨迹.
3) 利用式(9)所示的算法和图 2所示的控制流程，利用反馈回来的关节角进行重力补偿，得到双关节平行轴机械臂重力补偿算法为
$$ \left\{ \begin{array}{l} \bar G\left( {{q_1}} \right) = \left[ {\left( {{m_1}l_1^{\rm{c}} + {m_2}{l_1}} \right)g + {m_2}l_2^{\rm{c}}g} \right]\cos {q_1}\\ \bar G\left( {{q_2}} \right) = {m_2}l_2^{\rm{c}}g\cos \left( {{q_1} + {q_2}} \right) \end{array} \right. $$ (27) PD控制器的参数选为$ \boldsymbol{K}_{\mathrm{p}}=\boldsymbol{K}_{\mathrm{v}}=\left[\begin{array}{cc}{40} & {0} \\ {0} & {20}\end{array}\right]$，先看不进行重力补偿时的仿真结果：
从图 5可以看出，PD控制器只是利用偏差来进行控制的，如果不加重力补偿，无法消除重力的干扰，因此误差比较大，很难进行轨迹跟踪控制.为了验证本文重力补偿算法的性能，同时采用文献[6-7]中的固定重力补偿算法来进行仿真对比.
从图 6可以看出，采用重力补偿后能够较好地实现轨迹跟踪控制，由图 7可知，本文采用的重力补偿算法能使跟踪误差|e| < 1.5°，控制精度较高，而图 8中固定补偿算法的跟踪误差超过了3°，由此看出，本文的重力补偿算法在误差控制方面做得更好.
然后输入正弦信号检验该系统的动态跟踪性能，假设关节1和2的输入信号为
$$ \left\{ \begin{array}{l} {\varphi _1} = 70\sin t\\ {\varphi _2} = - 70\sin t \end{array} \right. $$ (28) 两关节机械臂在不同的正弦信号下的跟踪效果如图 9所示.从图 9看出，该重力补偿算法对于一般的正弦波也有比较好的跟踪性能，跟踪误差|e| < 2°，能较好地实现轨迹跟踪控制.
3.2 三关节机械臂仿真
为了进一步验证该重力补偿控制算法的有效性，对图 10所示的三关节机械臂进行重力补偿仿真研究，其重力参数如表 2所示.
表 2 三关节机械臂参数Table 2. Parameters of 3-joint manipulator参数 杆长/mm 质量/kg L1 L2 L3 M1 M1 M3 数值 100 125 125 1.00 0.88 0.88 由于不考虑末端姿态，因此只采用控制末端位置的前3个关节来进行轨迹跟踪控制实验，3个关节角的轨迹运动曲线为
$$ \left\{ \begin{array}{l} {\varphi _1} = {\rm{ \mathsf{ π} }}/3\sin \left( {0.5t + {\rm{ \mathsf{ π} }}} \right)\\ {\varphi _2} = {\rm{ \mathsf{ π} }}/3\sin \left( {0.5t} \right)\\ {\varphi _3} = {\rm{ \mathsf{ π} }}/3\sin \left( {0.5t} \right) - 0.355\;6{\rm{ \mathsf{ π} }} \end{array} \right. $$ (29) 同样利用式(12)所示的基于三角函数的重力补偿控制算法，其中参数设置为
$$ {\mathit{\boldsymbol{K}}_{\rm{p}}} = \left[ {\begin{array}{*{20}{c}} {1\;000}&{}&{}\\ {}&{500}&{}\\ {}&{}&{500} \end{array}} \right]\;\;\;\;\;{\mathit{\boldsymbol{K}}_{\rm{v}}} = \left[ {\begin{array}{*{20}{c}} {10}&{}&{}\\ {}&{100}&{}\\ {}&{}&{100} \end{array}} \right] $$ $$ \mathit{\boldsymbol{G}} = \left[ {\begin{array}{*{20}{c}} 0&{}&{}\\ {}&{94\cos {q_2}}&{}\\ {}&{}&{18\cos \left( {{q_2} + {q_3}} \right)} \end{array}} \right] $$ 由于第1个关节的旋转轴与重力方向平行，根据式(10)可知其重力补偿系数$ \delta_{1}=0 \Rightarrow G_{1}=0$，故不需要重力补偿，实验结果如图 11、12所示，分别表示各关节的角位移和跟踪误差曲线.
可以看出，本文重力补偿算法在本实验中的误差|e| < 0.05rad，而固定补偿算法的误差超过了0.06rad，因此本文的补偿算法补偿效果更好，能较好地实现轨迹跟踪控制.
4. 结论
1) 本文提出了一种算法简单且容易实施的重力补偿方法.本文基于机械臂空间几何特性和扭矩平衡原理并通过引入“补偿系数”的概念应用到重力补偿中.
2) 该算法不需要通过复杂的矩阵空间变换来计算重力补偿值，只需要知道质量和质心参数，并通过简单的三角函数变换就能够对重力进行在线补偿，能拓展应用到多关节非平行轴机械臂中.
3) 经过仿真及实验研究分析可知，该补偿方法算法简单，能较好地对重力进行补偿，从而能够较好地完成机械臂的轨迹跟踪任务.
-
表 1 两关节机械臂参数
Table 1 Parameters of 2-joint manipulator
参数 l1、l2/mm l1c、l1c/mm m1、m2/kg 数值 250 125 0.88 
下载: 导出CSV
表 2 三关节机械臂参数
Table 2 Parameters of 3-joint manipulator
参数 杆长/mm 质量/kg L1 L2 L3 M1 M1 M3 数值 100 125 125 1.00 0.88 0.88
下载: 导出CSV
-
[1] MACHORRO-FERNANDEZ F A, PARRA-VEGA V, OLGUIN-DIAZ E. Active mechanical compensation to obtain gravity-free robots:modeling, control, design and preliminary experimental results[J]. IEEE Int Conf Robot, 2009, 7(5):4346-4351.
[2] CHO C, KANG S. Design of a static balancing mechanism with unit gravity compensators[J]. IEEE Int C Int Robot, 2011, 1(9):1857-1862.
[3] MASHAYEKHI A, NAHVI A, YAZDANI M, et al. VirSense:a novel haptic device with fixed-base motors and a gravity compensation system[J]. Industrial Robot:an International Journal, 2014, 41(1):37-49. doi: 10.1108/IR-02-2013-328
[4] KIM S H, CHO C H. Incomplete gravity compensator for a 4-DOF manipulator[J]. Journal of Mechanical Science and Technology, 2015, 29(10):4417-4426. doi: 10.1007/s12206-015-0940-1
[5] CHO C, KANG S. Design of a static balancing mechanism for a serial manipulator with an unconstrained joint space using one-DOF gravity compensators[J]. IEEE T Robot, 2014, 30(2):421-431. doi: 10.1109/TRO.2013.2289017
[6] 杨玉贵, 彭光正, 王涛, 等.具有固定重力补偿的模糊免疫PD控制在气动人工肌肉驱动机械手位置控制中的应用[J].机器人, 2008(3):259-263. doi: 10.3321/j.issn:1002-0446.2008.03.012 YANG Y G, PENG G Z, WANG T, et al. Application of fuzzy immune PD controller with fixed gravity compensation to displacement control for manipulator actuated by pneumatic muscle actuator[J]. Robot, 2008(3):259-263. (in Chinese) doi: 10.3321/j.issn:1002-0446.2008.03.012
[7] 王战中, 王文龙, 靳超松, 等.具有固定重力补偿的机器人控制研究[J].机械设计与制造, 2015(3):166-169. doi: 10.3969/j.issn.1001-3997.2015.03.046 WANG Z Z, WANG W L, JIN C S, et al. Research of robot controlling with fixed gravity compensation[J]. Machinery Design and Manufacture, 2015(3):166-169. (in Chinese) doi: 10.3969/j.issn.1001-3997.2015.03.046
[8] 谢光辉, 张进春.一种仿人机器人臂的重力补偿研究[J].机械传动, 2010(7):26-28. doi: 10.3969/j.issn.1004-2539.2010.07.007 XIE G H, ZHANG J C. Experimental research on gravity compensation for humanoid robot arm[J]. Journal of Mechanical Transmission, 2010(7):26-28. (in Chinese) doi: 10.3969/j.issn.1004-2539.2010.07.007
[9] 谢光辉, 金敉娜, 张进春, 等.利用BP-NN算法的机器人臂重力补偿研究[J].自动化仪表, 2012(2):22-24. doi: 10.3969/j.issn.1000-0380.2012.02.007 XIE G H, JIN M N, ZHANG J C, et al. Research on gravity compensation using BP-NN algorithm for robot arm[J]. Process Automation Instrumentation, 2012(2):22-24. (in Chinese) doi: 10.3969/j.issn.1000-0380.2012.02.007
[10] 赵杰, 任思璟, 于宗艳.基于重力补偿的机械臂PD控制系统研究[J].现代科学仪器, 2012(5):58-59. http://d.old.wanfangdata.com.cn/Periodical/xdkxyq201205013 ZHAO J, REN S J, YU Z Y. Study of manipulator PD control system based on gravity compensation[J]. Modern Scientific Instruments, 2012(5):58-59. (in Chinese) http://d.old.wanfangdata.com.cn/Periodical/xdkxyq201205013
[11] LIU Y C, CHOPRA N. Gravity-compensation-driven position regulation for robotic systems under input/output delays[J]. IEEE Transactions on Control Systems Technology, 2014, 22(3):995-1005. doi: 10.1109/TCST.2013.2275746
[12] 王宏民, 杜志江, 闫志远, 等.混联式主操作手重力补偿算法[J].机器人, 2014(1):111-116, 128. http://d.old.wanfangdata.com.cn/Periodical/jqr201401015 WANG H M, DU Z J, YAN Z Y, et al. Gravity compensation algorithm for hybrid master manipulator[J]. Robot, 2014(1):111-116, 128. (in Chinese) http://d.old.wanfangdata.com.cn/Periodical/jqr201401015
[13] KIM E J, SEKI K, IWASAKI M. Motion control of industrial robots by considering serial two-link robot arm model with joint nonlinearities[J]. Journal of Mechanical Science and Technology, 2014, 28(4):1519-1527. doi: 10.1007/s12206-014-0139-x
[14] 陈彦杰, 王耀南.基于在线重力补偿的输电线除冰机器人轨迹跟踪控制[J].信息与控制, 2013(4):401-406. http://d.old.wanfangdata.com.cn/Periodical/xxykz201304001 CHEN Y J, WANG Y N. Tracking control of deicing robot on voltage transmission lines based on online gravity compensation[J]. Information and Control, 2013(4):401-406. (in Chinese) http://d.old.wanfangdata.com.cn/Periodical/xxykz201304001
[15] EMMERICH C, NORDMANN A, SWADZBA A, et al. Assisted gravity compensation to cope with the complexity of kinesthetic teaching on redundant robots[C]//IEEE International Conference on Robotics and Automation. Piscataway: IEEE, 2013: 4322-4328.
[16] KELLY R, SANTIBÁÑEZ V, LORÍA A. Control of robot manipulators in joint space[M]. Berlin: Springer, 2005.
计量
- 文章访问数: 300
- HTML全文浏览量: 10
- PDF下载量: 91
