基于力矩前馈的机器人直线度算法

李彦华， 李会英， 刘仁芬

（1.河北轨道运输职业技术学院，石家庄050051；2.石家庄铁道大学四方学院，石家庄050043）

0 引 言

工业机器人通常都是多关节串联式机器人，结构悬臂较长，转动惯量较大，具有典型的残余振动特性。机器人关节或机械臂耦合会带来非线性力的变化，一个关节的振动还可以传递给另外的关节。对机器人的振动进行抑制控制，不仅可以提高工业机器人和机械系统精度和性能，还可以简化机械结构。采用力矩前馈抑制技术后的驱动器，定位时间大幅减少，同时路径跟踪变得更加精准和平稳。汪克峰等［1］提出了一种用于表面永磁同步电动机平滑控制的负载转矩前馈补偿控制策略，建立了卡尔曼滤波器，利用机械运动学方程实现了对转速和负载转矩的精确估计，将负载转矩扰动和电动机参数波动转化为负载转矩的变化。Bing等提出了一种永磁同步机转矩前馈控制技术，利用参数非线性，随直流和正交电流变化的特点，提出了一种新的最优工作面，该工作平面结合了每安培最大转矩（MTPA）曲线、电流极限圆和每伏特最大转矩（MTPV）曲线、电压限制和在非线性参数变化下的转矩计算。徐淑正等［3］针对电液负载系统压力前馈控制，提出了一种不同于传统补偿方法的舵压前馈补偿方法。姚诚伟等［4］利用加速度前馈对三轴惯性稳定平台进行不平衡转矩补偿，为了减小不平衡转矩的影响，提出了一种更好的实用解决方案，并提供给万向驱动，Matlab仿真结果表明，该方法提高了ISP的精度。

目前，对机器人的精度高、速度快、执行安全性有更高的要求，引入动力学的控制是一条有效的途径［5-6］。本文利用指数积（POE）法建立了各关节连杆的相对转换关系，开发了一套动力学前馈控制系统（电流力控制、动力学前馈）。最后提出了机器人直线度的概念，可以反映机器人运动的平稳性、轨迹一致性和轨迹跟踪性等参数的好坏。

1 机器人控制系统架构

1.1 POE 运动学

本文研究的传统机器人在运动过程中实现可编程，并且实现可变的预定步进姿势和力感测，如图1所示。

1.1.1 机器人正运动学

手臂运动学正解（串联开链机器人的正向运动学公式）：对于n个转动／移动关节的串联机器人，设base0为基坐标系，Tooln＋1为工具坐标系，则应用POE计算机器人的正向运动学模型仅需3步完成：

图1 柔性下肢康复机器人（mm）

（1）机器人末端初始位姿gst（0）

（2）关节对应的运动旋量坐标ξ＾i

（3）代入POE公式，得：

由于：

转换为T矩阵的表达形式为：

1.1.2 机器人反运动学

与正向运动学模型的输入、输出正好相反，逆向运动学由给定的机器人末端位姿解计算对应的关节运动量［7-9］。注意：正向运动学的解唯一，而逆向运动学可能有多解、唯一解或者无解等不同情况。

给定机器人运动学正解映射gst，一个期望位姿gd，逆运动学即是解算如下方程：

运动学逆解可以分为两种思路：

（1）解析解（位置级解法）。①几何法直接解算：对于简单的平面运动模型，一般直接根据几何关系求解。②P-K子问题：将运动学逆问题转化成三类P-K子问题，而后通过求解子问题得到逆解。

（2）数值解（速度级解法）。数值解即合理选择数值算法，对gst（θ）＝gd进行求解，例如牛顿迭代法。

P-K子问题法的基本技巧是：将POE运动学模型应用于某些特殊点，比如两个或多个轴的交点，可以消除这些轴关节的耦合，从而消掉其对应的变化矩阵eξ＾θ。

子问题1 绕1个轴的旋转，求θ

子问题2 绕2个有序轴的旋转，求θ1和θ2

子问题3 旋转至给定距离，求θ

1.2 Elmo驱动器控制结构

动力学控制有较多，如计算力矩法、Lagrange、牛顿欧拉法等。但各大机器人公司对完整的动力学控制方案都是保密的。图2是Elmo公司驱动器的控制结构，可以看到Elmo驱动器提供了力矩前馈的控制接口和控制结构，可以将计算的理论值下发至驱动器［14-15］。

图2 Elmo驱动器的控制结构

2 机器人动力学前馈

控制系统结构：综合运动学、动力学、伺服控制结构。动力学简化：简化机器人动力学计算。动力学参数标定：惯性参数、连杆参数等标定。

2.1 动力学前馈

针对机器人本体建立动力学模型时，采用牛顿-欧拉递推方程。受限于机器人控制系统的实时性要求，如果采用拉格朗日方程会增加运算量［10-13］。

牛顿-欧拉内推，从基座到末端计算速度和加速度：

牛顿-欧拉外推，从末端到基座计算惯性力和关节力矩：

其中各个关节实际的受力可以根据关节电流计算而来。通过重力、摩擦力可以分析得出各个关节受力情况，最终通过伺服力矩输出到机器人本体，以此实现对机器人的直接控制，如图3所示。还需要考虑手臂基座的倾斜，重力加速度在各轴的分量需要做相应的调整。

图3 零力控制原理框图

2.2 摩擦力采集

相较于采用力矩传感器方法，无传感器零力控制的方式省去了在每个关节增加传感器的成本，避免因为加装传感器而带来的柔性改变，基础动力学模型相对简化［16］。但无传感器模式电流信号噪声较大，电流信号响应频率高，容易混杂高频噪声，该问题需要谨慎考虑。

在理想的动力学模型中，摩擦力近似为零，在机器人运动过程中，可以实时计算得出当前驱动机器人运动所需要的力矩，同时通过伺服电流来获取机器人当前的实际输出力矩，而两者的差值即为当前的实际摩擦力。

式中：Tactual为实际采集的力矩；Ttheory为动力学模型所输出的当前理论力矩。

通过滤波后的驱动器信息数据可以直接获得效果较好的摩擦力数据，摩擦力模型可以通过大量数据的拟合来完成。

目前机器人关节摩擦力的基础模型主要有3个选择：①库伦模型＋黏性摩擦模型；②Stribeck模型；③n次多项式模型。采用模型①分析摩擦力时，因速度而来的变化不够敏感，导致最终推动关节时易产生过补偿，造成机器人不易停止甚至在牵引速度较快时发生飞车；模型②的特征是在高速段的拟合较为准确，在低速段却会有一定程度的失真；模型③的特征是在低速段的拟合较为准确，而高速段会远离实际数据。

以机器人的关节1为例，驱动机器人第1个轴在不同速度下做往复运动，采集到摩擦力的2004组数据，如图4所示。

图4 机器人关节摩擦力

从图4可见，所采集的摩擦力存在周期性的尖峰，通过分析其周期所对应的关节速度可知：①通过关节电流采集力矩信号伴随有高频噪声；②摩擦力与关节角速度相关。为得到有效的力矩信号，在控制系统中加入二阶巴特沃斯低通滤波器。

常规的低频电流数据可得到较为完整的保留，信号的大部分高频成分被滤波器滤除。

2.3 摩擦力矩拟合方法

在曲线拟合中普遍采用最小二乘法，需要提前确定离散点分布情况的阶次，即使是相同的离散点，所拟合的多项式阶次不同也会造成曲线有很大差异［9-10］。拟合函数超过3次时所拟合曲线的误差就会很大，并出现病态问题。所以最小二乘法应用范围还是很有限的，离散点规律过于复杂，最小二乘法拟合出来的曲线误差会非常大。虽然采用切比雪夫算法拟合曲线差值达不到要求，需重新计算，使得计算繁烦，但是借助于牛顿差值，使得拟合曲线各阶差值在所规定的范围内，这样的指标使得拟合曲线误差很小，所得拟合曲线准确。

根据对机器人的摩擦力矩Tf的测试，可以看出摩擦力矩的大小与机器人的重力矩、加速转矩和转动角速度的大小相关。为了简化线性化拟合，将摩擦力矩拟合公式确定如下：

式中：Tf为摩擦力矩；Tm为重力矩；φ为重心与旋转关节中心连线与水平线方向的夹角；为转动角速度；为加速转矩；a、b、c分别为重力矩、角速度、加速转矩的影响因数。

（1）重力矩影响因数a的确定。a的确定可以通过mglcos φ求解。在机器人执行程序前后这两个时间段中，没有受到角速度和角加速度的影响。因此，可以选择这两段时间对a进行拟合。mg为机器人绕关节旋转部分的总质量；l为机器人绕关节旋转部分的重心到旋转关节的距离；φ为重心与旋转关节中心连线与水平线方向的夹角。

（2）角速度影响因数b的确定。对测量软件得到的数据分析计算后得到Tf，将Tf中的重力矩影响去除，这时Tf只有角速度和加速转矩的影响。由于机器人在程序控制下运行，存在平稳运行时间段。选取角速度平稳运行时对b进行拟合。

（3）加速转矩影响因数c的确定。在重力矩和角速度影响因数确定之后，通过简单地运算使Tf中只剩下加速转矩的影响。机器人处在加速和减速阶段的c进行拟合。

2.4 前馈控制

图5为前馈控制模型。图中：WM（s）为前馈控制器；WO（s）为过程控制通道传递函数；Wf（s）为过程扰动通道传递函数。

图5 前馈控制模型

不变性原理决定的动态前馈控制器是由被过程扰动通道与控制通道特性之比决定的，即：

因实现了完全补偿，达到了被控量不受扰动影响的控制效果。前馈模型的结构有时比较复杂，甚至难以实现（见图6）。

图6 验证前馈接口

3 实验结果及分析

求出了机器人动力学前馈模型，对模型参数采用辨识方法实现了动力学参数获取，将力矩作为前馈值发送至驱动器。机器人轨迹精度和稳定性好坏依靠千分表进行测试。本文提出了机器人直线度的概念，即机器人走直线的精度。从实验可以看出，采用力矩前馈控制法可有效地改善机器人的性能。

3.1 采集位置曲线

利用激光跟踪仪对机器人在不同速度下进行直线行走过程中的轨迹进行测试，如图7所示。

图7 机器人在不同速度下直线行走过程中的轨迹

3.2 直线度分析

从图7采集到的带动力学前馈和不带动力学前馈的直线对比，如表1所示。

表1 直线度对比

机器人直线度是反映机器人震荡强弱的一个重要指标，依靠动力学前馈可以很好地提高机器人的直线度。

4 结 语

针对传统机器人的结构参数特点，运用指数积（POE）运动控制求解了运动学问题，然后构建了动力学方程，同时提出了一种摩擦力矩拟合的方法来构建控制算法模型。采用力矩前馈控制（动力学前馈补偿）法，有效地改善了机器人的性能。提出了机器人直线度的概念，可以反映机器人运动的平稳性、轨迹一致性和轨迹跟踪性等参数的好坏。将动力学应用到控制器中，机器人控制器在运动周期内，按照运动学位置、速度、加速度计算动力学结果，将其按前馈方式加到电流环的计算结果，输出给驱动器。但是由于控制器周期在4 ms，驱动器周期500 μm级别，所以算法上可以随着控制器周期的缩短，达到更好的效果。