基于变增益自抗扰技术的机器人轨迹跟踪控制方法

张 磊，鲁 凯，高春侠，刘荣明，丁 浩，田 伟，洪 伟

（1.中国石油大学（华东）新能源学院，山东青岛 266580；2.中海石油（中国）有限公司天津分公司，天津 300459）

1 引言

焊接机器人因其有效工作空间有限，需要使用额外的辅助设备［1，2］.实现焊接任务有赖于轨迹规划及轨迹跟踪控制，因此，对二者的研究就显得很有必要.机器人是一种非线性、强耦合的系统，传统的线性控制方法很难实现精确控制［3］.

目前已经有许多轨迹规划方法.文献［4］提出一种改进快速扩展随机树的多机器人编队路径规划算法，用于解决多机器人在复杂环境下的编队路径规划问题.文献［5］采用B-spline 进行轨迹规划，考虑角度、速度、动态转矩约束，但是转矩约束可能会与其他约束冲突.文献［6］提出一种可以在动态环境下实时地为移动机器人规划出一条安全且最短的路径的方法.文献［7］提出一种高斯混合样条的算法，利用运动数据集生成轨迹.综上所述，目前已有的方法计算量都较大.因此，本文在现有的方法基础之上，提出一种改进的三次样条插值的轨迹规划方法.

为了实现精确轨迹跟踪，大量的控制方法被提出.文献［8］利用线性自抗扰技术实现高速四轮车的轨迹跟踪，但是未考虑峰值现象；文献［9］采用滑模控制方法设计轨迹跟踪控制规律；文献［10］提出一种考虑关节角加速度约束的仿人机器人偏摆力矩控制方法；文献［11］设计一种基于移动机器人运动模型的模糊开闭环PID-P型非线性离散迭代学习控制方法；文献［12］提出一种二维控制器，使用H∞框架解决规范之间权衡的约束控制设计问题.这些方法能够解决轨迹跟踪问题，但是只是考虑运动学约束，未考虑对实际研究价值的动力学约束.

综上所述，为了抑制峰值现象，提高跟踪精度，本文提出了一种变增益自抗扰技术的机器人轨迹跟踪控制方法.首先，采用跟踪微分器将逆运动学求解的关节变量处理为插值点区间；然后，根据目标函数选取插值点拟合得到的轨迹曲线；最后，在考虑机器人动力学下设计轨迹跟踪控制器.通过仿真和实验验证，本文提出的方法可以保证机器人轨迹跟踪的准确性，能抑制初始峰值，且具有较强的鲁棒性.

2 二自由度机器人数学模型

图1为二自由度机器人简化模型示意图，根据平焊工艺要求，设焊点法线向量为［x1，y1，z1］.θ1，θ2为机器人关节变量，建立机器人运动学方程［14］可以求出θ1，θ2的表达式为

图1 二自由度机器人简化模型

根据式（1）求出的关节角度进行轨迹规划，为了提高控制精度需要建立机器人的动力学方程［15］.根据拉格朗日力学建立的动力学方程如下为

其中，

各项的含义分别如下：x,分别表示为机器人的关节角、角速度与角加速度；M（x）表示机器人的转动惯量矩阵；C(x,)为向心科氏力矩；G（x）为重力、摩擦力和未知扰动的总和；u为关节控制力矩，即系统的控制输入；m1为关节1 与关节2 之间连杆的质量；m2为关节2圆盘的质量；d1为关节1 圆心距离地面的距离；d2为关节1 与关节2 圆心之间距离；τd1，τd1分别为关节的未知扰动.

3 轨迹跟踪自抗扰控制器设计

3.1 轨迹规划算法

轨迹规划是在逆运动学求解基础之上，且满足路径平滑前提下［2］进行的，本文提出的改进型三次样条插值拟合可以降低机器人的应力水平和减少能量消耗.求解最优轨迹规划的具体步骤如下：（1）将通过逆运动学求解的关节变量采用TD 进行处理为6个数据区间；（2）根据步长为0.05 的规则在每个区间选取一个点，采用改进型三次样条插值拟合路径曲线；（3）建立冲击力目标函数，将步骤（2）得到的曲线带入到目标函数，求取目标函数值；（4）对不同点拟合到的曲线进行目标函数值比较，选取最小值的一组解作为最优解.

3.2 跟踪微分器

为了解决插值关键点选取的问题，采取跟踪微分器的思想.将由逆运动学求解的关节变量处理为6 个区间的数据.跟踪微分器主要是用来根据指令安排期望的过渡过程及其导数，TD［17］的一般形式为

其中，v（t）为期望值；q1（k）的为期望速度安排的过渡过程；q2（k）为q1（k）的微分信号；h为步长；参数h0的作用是于消除超调现象；参数r的作用是决定跟踪期望信号的速度.

3.3 三次样条曲线插值拟合

机器人的轨迹由关键点组成，采用三次样条拟合关键点［13］.机器人轨迹规划一般采用样条曲线.三次样条曲线的二阶导数连续，三次样条曲线插值［18］是将整个区间等分，通过关键点进行拟合.需要满足的条件为：（1）拟合函数在已知点的函数值等于原函数值；（2）拟合分段函数是二阶连续；（3）端点的二阶导数；（4）关键插值点选取.主要步骤如下.

定义样条曲线的表达式为

为了保证插值的连续性和微分连续性，则

定义步长为hi=xi+1-xi，边界条件首尾的两个端点的微分值分别为A 和B，根据式（4）和式（5）建立如下矩阵方程：

其中

根据式（6）可以求出样条曲线的系数，然后根据插值点求出每个子区间（xi≤x≤xi+1）的方程为

3.4 冲击力目标函数

角加加速度为物体角加速度的变化率，角加加速度过大意味着物体所受力变化过快，会形成冲击力.冲击力不仅对关节的运行产生影响，而且影响机器人的寿命.为了提高跟踪精度，减少机器人的谐振频率和系统的机械磨损，规划出角加加速度值较小的轨迹显得十分重要［19］.以角加加速度的绝对值对时间的积分来表示关节冲击大小，即

3.5 路径跟踪自抗扰控制器

线性自抗扰控制器为了实现控制效果需要很大的观测器增益，较大增益可能会出现峰值现象，因此针对这些问题，设计了一种变增益自抗扰控制器.ADRC 是一种不依赖系统模型的新型控制技术，具有超调小、响应快、精度高的优点，图2 为3 阶ADRC 控制器结构图［17］.ADRC包括3部分：安排过渡过程（TD）、反馈控制规律（NF）和扩张状态观测器（ESO）.

图2 3阶ADRC控制器结构

图2 中，r0为控制目标；v1是r0的跟踪信号；v2是r0的跟踪信号的微分；z1是系统输出y的估计；z2是系统输出y的微分估计；z3是对象总扰动f（x，w（t），t）的估计；e1，e2是误差；u0是NF 的输出；b0是b（t）的估计；u是ADRC 的控制量.

将式（2）改写为下列状态空间形式：

其 中，Α(q)=M(x)-1(-C(x,)+G(x)),B(q)=M(x)-1；θ=［θ1 θ2］T；τ=［τ1，τ2］T.

动力学方程中系数矩阵包含耦合变量，将耦合变量视为内部扰动，动力学方程进一步化为

在式（10）的基础之上可以实现两个关节独立控制，在原系统其中一个关节控制系统添加一个新的状态变量f1，原系统新的状态方程如下：

建立扩张状态观测器（ESO）：

根据式（11）和式（12）得出系统的误差模型如下：

根据对偶性可以将式（13）转化为标准形式的可控系统，求取误差模型的解为

其中，系统eAt在初始时刻会因为高增益带宽值很大，因此采用常数高增益观测器会带来观测误差，即初始峰值现象.因此，本文设计了一种变增益自抗扰控制器.与式（13）对应的标准系统如下：

其中，ai（t）是时变参数.

定义式（13）的可控矩阵为M，式（15）的可控矩阵为Mc.矩阵M的定义如下：

根据式（16）可以求出：

从式（17）可以看出，设计合适的ai，式（13）和式（15）是可控的，定义z=T（t）e，其中变换矩阵T（t）为

假定ai光滑且有界，通过式（17）和式（18）很容易证出T（t）是一个李雅普诺夫变换矩阵.这意味着：（1）T（t）和T（t）的导数是连续的；（2）T（t）是非奇异的；3）T（t）和T（t）的导数有界.由李雅普诺夫变换可得

为了得到增益li（t），定义L（t）=［-l1（t），-l1（t），-l1（t）］T.将T（t）分解为n+1 列向量的形式，根据（19）可以得到

通过将状态误差矩阵（13）转化为标准型（15），得到了观测器增益li（t）表达式，即

结合文献［20］，a1，a2，a3选取为

文献［21］中的sigmoid函数，取

建立控制规律：

3.6 扩张状态观测器误差系统的稳定性分析

误差式（13）可转为

其中

当l1（t）＞0，l2（t）＞0，l3（t）＞0有界且l1（t）l2（t）-l3（t）＞0时，可以得到主对角元素均为正的矩阵D，即

使得DA（e（t））为正定对称矩阵，在式（26）中，γ，δ1，δ2均为无穷小正数，且

取误差式（13）的Lyapunov函数为

其中，t＞0，常数C为充分大的常数.

在式（13）的平衡点局部，V（t）中第一项的被积函数值为有界，因此，只要常数C取足够大的常数，就可以保证V（t）＞0.Lyapunov函数（28）的导数为

因此，系统（13）在其平衡点处渐进稳定.

4 轨迹规划优化及跟踪仿真分析

本文研究对象的物理约束如表1 所示.首先，验证TD 算法的可行性，将传统和改进轨迹规划方法进行对比分析，将变增益ADRC 和传统ADRC 控制效果进行对比；然后，使用ADAMS进行能量消耗分析.

表1 二自由度机器人约束条件

4.1 TD算法的可行性分析

为了保证机器人的平稳运行，采用TD 进行过渡安排，图3 中关节变量预期值分别为2°，4°，6°，8°.在图3中，不同给定值都在5s达到给定值.假设关节2的始末为15°和90°，关节1的始末为-5°和45°，经过TD 得到的数据如表2所示.

图3 关节变量跟踪值

表2 关节变量插值点数据

4.2 轨迹规划对比分析

以机器人的关节2为例，分别采用传统插值拟合和改进插值拟合方法进行仿真.假设始末角度分别为15°和90°，根据表2 选取中间点的数据为t=｛0，1，1.5，2，2.5，3，3.5，5｝，θ=｛15，20，30，45，60，72.8，81.3，90｝.为了避免速度突变，设边界条件为A=0，B=0，将数据经过拟合得到运动轨迹，根据表1 的数据进行选取插值点，选取的规则为在每个子区间以步长0.01范围为0.2选取插值点，选取三次样条曲线中三次项系数最小的作为插值点，最终选取中间点数据的时间序列为t=｛0，0.95，1.5，2.05，2.6，3.15，3.65，5｝，θ=｛15，20，30，45，60，72.8，81.3，90｝.为了避免始末位置速度突变，设定样条曲线的边界条件中A=0，B=0，将数据经过拟合得到运动轨迹.仿真结果如图4～图7 所示.

图4 关节2轨迹曲线

图5 关节2速度曲线

图6 关节2加速度曲线

图7 关节2加加速度曲线

图4～7 中，Trace1，Speed1，Acceleration1，Jerk1 为传统方法的轨迹、速度、加速度、加加速度；Trace2，Speed2，Acceleration2，Jerk2 为优化方法的轨迹、速度、加速度、加加速度.采用传统的三次样条插值拟合出的轨迹曲线是很平滑的，速度和加速度都是连续的且不是很大，但是关节的加加速度是很大的，求得一般三次样条插值拟合的冲击力目标函数值F=105.89.采用改进的三次样条插值拟合出的轨迹曲线冲击力目标函数值F=63.74，比传统的三次样条插值拟合降低了39.8%.改进的三次样条插值拟合算法明显降低了冲击力对机器人的影响.

4.3 变增益ADRC和传统ADRC控制效果对比

假定关节拟合的期望轨迹为0.1 sint，外部扰动为0.1 sintm.ESO 参数设计如图8 所示，变增益ADRC 和传统ADRC 的仿真结果如图9 所示，二者控制量的仿真结果如图10所示.

图8 ESO时变参数

图9 关节轨迹曲线

在图8 中，LESO 为线性自抗扰控制参数，AESO为变增益自抗扰控制参数，可以看出线性自抗扰的参数为常量，变增益自抗扰控制器的参数为从零开始渐变的时变参数；在图9 中，Ideal 为期望轨迹，Trace1 为传统ADRC 仿真结果，Trace1 为传统ADRC仿真结果，可以看出二者都可以在稳定时达到预期轨迹，但是，传统自抗扰控制在初始阶段出现峰值现象，而变增益自抗扰控制可以很好地跟踪轨迹；在图10 中，LESO 为传统ADRC 的控制量，AESO 为变增益ADRC 的控制量，从图10 可以看出前者的控制量在初始阶段很大.针对观测器的初始误差较大的问题，线性自抗扰控制方法的增益为较大的常数不能解决此问题，而变增益采用时变增益可以解决传统自抗扰方法带来的初始控制量较大的问题，从而解决峰值现象.

图10 关节控制变量

4.4 虚拟样机功率消耗仿真实验结果对比

使用ADAMS 对虚拟机械系统进行功率消耗分析.以机器人的关节1为例对能量消耗进行对比，仿真结果如图11 所示.Energy2 为传统方法对应能量曲线，Energy1为优化方法对应能量曲线.

图11 关节1电机能量消耗

根据图11可以看出，传统方法的关节1的电机功率消耗在1.5 s时达功率消耗峰值，峰值为3636 W；优化的方法的关节1的电机功率消耗在1.5 s时达功率消耗峰值，峰值为2830 W，比传统插值拟合降低了28.48%.优化的三次样条曲线插值拟合算法明显降低了电机消耗的能量.

5 实验验证

本文的实验平台由控制柜、六自由度机器人本体、二自由度机器人和焊机4 部分组成.其实验平添环境如图12 所示，其中二自由度机器人的参数为m1=40 kg，m2=20 kg，d1=0.3 m，d2=0.2 m，r=0.4 m，在二自由度机器人验证提出的改进轨迹规划及跟踪控制方法的有效性.

图12 实验环境

结合实验样机的实际参数，对样机进行实验.采用常规ADRC 控制的轨迹跟踪曲线如图13 所示；采用变增益ADRC 控制的轨迹跟踪曲线如图14 所示；采用常规ADRC 控制的轨迹动态响应曲线如图15 所示；采用变增益ADRC 控制的轨迹动态响应曲线如图16所示.

图13 常规控制轨迹跟踪曲线

图14 优化控制轨迹跟踪曲线

图15 关节1常规轨迹动态响应

图16 关节1优化轨迹动态响应

从图13 和图14 可以看出，采用常规的自抗扰技术会在初始阶段存在峰值现象，而采用变增益自抗扰技术可以解决初始阶段的峰值现象；从图15 和图16 可以看出，常规方法的动态响应时间为0.7 s，变增益方法的动态响应时间为0.2 s，明显快于常规方法.实验表明，本文提出的变增益自抗扰控制的轨迹跟踪效果及动态响应优于常规自抗扰方法.

6 结论

（1）根据焊接工艺要求建立机器人运动学方程，以及根据拉格朗日力学建立机器人动力学方程.利用跟踪微分器算法（TD）将逆运动学求解的关节变量处理为6 个子数据区间，采用TD 算法可以保证各个关节同时到达预期值.

（2）设定插值点选取规则，然后进行三次样条曲线插值拟合，以及建立以角加加速度的绝对值对时间积分的目标函数，规划出角加加速度较小的轨迹以提高跟踪精度、减少机器人的谐振频率、系统的机械磨损和电机能量消耗.

（3）采用变增益自抗扰控制器对轨迹进行跟踪控制，克服了传统自抗扰控制的初始阶段峰值现象，仿真和实验结果表明，该方法能保证机器人轨迹跟踪的准确性、稳定性和动态响应能力，能克服外界干扰的影响，具有较强的鲁棒性.

（4）目标函数的设计仅考虑了角加加速度因素，需要进一步考虑其他因素，进一步完善目标函数的设计.结合实验样机，进一步验证优化轨迹规划方法降低电机能量消耗的效果.