基于Udwadia-Kalaba理论的协作机器人轨迹跟踪控制*

韩 江，王凡志，董方方，夏 链

(合肥工业大学机械工程学院CIMS研究所，合肥 230009)

0 引言

协作机器人作为一种复杂的动力学系统，其轨迹跟踪控制重点在于，一是建立正确的动力学模型，二是选择合适的控制策略。对建模方法的研究，从拉格朗日提出Lagrange乘子法以来，多种求解方法相继问世，如旋量理论、凯恩方法、李群李代数方法等[1-2]。对于受约束系统来讲，利用拉格朗日乘子可以有效地进行约束力的计算，但实际中并非易为之事，特别是针对机器人这种复杂系统。

Udwadia等针对约束系统的动力学问题进行了长期研究，并取得了一定的研究成果，提出了Udwadia-Kalaba(U-K)方程[3-4]。该方法可以相对简单地建立约束下系统运动方程。因此，这种动力学建模的新方法越来越受到青睐，如文献[5]中对多体动力学研究、多自由度动力学研究都开始采用该方法。在该理论研究的基础上，Udwadia对于非线性机械系统的轨迹跟踪控制首先应用了伺服约束控制的方法。佐治亚理工学院Chen较系统地提出了机械系统伺服束控制的概念[6-7]，认为给定的力取决于约束力，利用伺服控制来实现约束力的设计问题。文献[8-10] 研究内容均在工业机器人动力学控制方面应用了该方法，文献[11-12]也应用了基于伺服约束的柔性机器人控制。由于机器人运动过程中存在的一些不确定性会影响其运动到预期的轨迹，为了实现精确的轨迹跟踪，许多控制策略已经被提出，文献[13-18]分别使用不同方法进行研究。主要是在PID参数的基础上进行迭代学习[13]，利用负反馈原理不断缩小误差。此外，模糊控制、神经网络控制、鲁棒控制等方法也被广泛使用[14-18]。其最终的目的都是抵消不确定性带来的误差。

针对协作机器人的轨迹跟踪控制问题，这里提出一种建立在U-K方程上的新的控制方法，将机器人给定的轨迹视为性能约束，通过系统约束来分析求解约束力，采用伺服控制方式来实现机器人的位置控制。在此过程中，理想状况下的约束问题应用了U-K方法，通过更少的计算量求解得出约束力。针对不确定性因素的影响，在上述的基础上提出来一种自适应鲁棒控制方法。

1 理想约束下的动力学系统

在拉格朗日力学的基础上。我们考虑这样的一个机器人动力学系统：

(1)

这是约束条件的一阶形式，并且一般情况下可能是非理想的，将其写成矩阵形式：

(2)

其中,A=[Ali]m×n,c=[c1,c2，...，cm]T

上式可以改写成二阶形式，并通过求取其关于t的微分。则得到二阶约束形式为：

写成矩阵形式为：

(3)

其中，b=[b1,b2，...，bm]T。式(3)即为被施加到系统上的约束的二阶形式。假定系统是处于理想情况下的，则可以做出如下假设：

假设1：对每个(q,t)∈Rn×R,σ∈∑,有M(q,σ,t)>0。

定义1：对给定的A和b，如果至少存在一个解，则约束称为一致的。

假设2：约束(3)是一致的。

综合考虑系统与约束力，在假设1和2的条件下，约束力可以表达为如下形式(即U-K方程)[4]：

(4)

该式遵守拉格朗日形式的达朗贝尔原理并使系统满足约束。在建模准确及不确定性已知的情况下，可由此给出输入控制力矩，在不确定性未知时，需要设计一种更实际的方法以使系统达到要求。

2 自适应鲁棒控制设计

由于外界环境等各种不确定性因素影响，机器人运动状态并非处于理想中。在设计控制τ时要考虑这些因素，把参数M,C和G作如下形式分解：

(5)

(6)

(7)

(8)

系统需要保持一定的稳定性，这里需要给出一个不确定性边界，在此继续作以下假设：

假设3：对每一个(q,t)∈Rn×R,A(q,t)都是满秩的，也就是说，A(q,t)AT(q,t)是可逆的。

假设4：在假设3的条件下，对于给定的P∈Rm×m,P>0，令：

(9)

则存在一个常值ρE>-1，使得所有的(q,t)∈Rn×R，都有：

(10)

所控制的机器人系统需要满足如下要求：①系统在理想状态下满足给定约束；②系统需要保证稳定性；③系统需要考虑不确定性带来的扰动。针对这几个方面的问题，提出如下控制策略：

(11)

令：

(12)

(13)

假设5：

(14)

(15)

令：

(16)

(17)

(18)

(19)

(20)

3 稳定性分析

控制理论中常借助李雅普诺夫方法直接判断稳定性，由于不确定性影响，该方法讨论的系统平衡点不容易找到。故在非线性系统中，在此基础上常用系统的一致有界性和一致最终有界性的概念来判断稳定性。

3.1 一致有界性与一致最终有界性

一致有界性：对于任何一个ξ>0，存在一个d(ξ)<∞，使得当δ(0)<ξ时，对所有的t≥t0，都有δ(t)

一致有界性与一致最终有界性如图1所示。

图1 一致有界性(左)与一致最终有界性(右)

一致最终有界性代表了一种比前者要求更严格的稳定性。其物理意义是，系统从初态出发，经一段时间，末态总能回到原点附近某个给定球域。

3.2 稳定性证明

选择李雅普诺夫函数如下：

(21)

求得其导数为：

(22)

(23)

由式(5)、式(6)、式(7)可知，

(24)

该式可以继续分为4部分计算：

(2)由式(13)可知；

(3)依据假设5(1)得到的式，可得：

(4)第4部分结合公式(16)得到：

μTPA(D+ΔD)p3=μTPADp3+μTPADEp3

根据式(8)、式(9)、式(10)，推出：

(25)

(26)

根据假设5的(2)得知：

(27)

将其代入式(25)、式(26)：

(28)

使用上面的自适应律(20)，可以得到：

(29)

所以可以得到，式(22)变为：

(30)

根据所证明结果,可得到其一致有界性[19-20]，

此外，一致最终有界性也可以得到：

4 仿真实例

上文使用的理论与推导的公式维度为n，适用于常见的多关节协作机器人。为阐述U-K方法解决机器人轨迹跟踪问题的基本步骤，这里对一个2R机器人进行仿真分析，便于读者理解与掌握。

通过分析当前常见的6轴协作机器人结构可以发现，机器人前3个关节确定其位置，后3个关节确定姿态，并且关节2和关节3的力臂最长、惯量变化范围最大、受重力影响最显著。

因此，将机座固定，不考虑腰部转动，末端几个确定姿态的关节视为负载，简化为一个2R机器人进行仿真分析，且考虑到旨在证明控制方法的有效性，也具有实际意义。其结构如图2所示。

图2 结构示意图

根据式(1)的数学模型，求得其主要参数如下：

G1=(m1+m2)gl1s1+m2gl2s12;G2=m2gl2s12;

选择下列函数以满足假设(5)：

使用以下参数进行仿真验证：

m1=m2=1,l1=1,lc1=lc2=0.5,g=9.8,P=6,ε=0.1,γ=12

初始位置q(1)=0.5，q(2)=0.5。

图3所示为两连杆的角度位置随时间变化的行踪，其中黑色部分为所选用的控制方法得到的结果，红色部分为给定的轨迹要求。可以看出，由于设置的初始条件并不满足约束条件，因此开始时时存在误差的，但随即很快逐渐趋于正常。

图3 约束连杆位置变化过程

图4表明了两连杆位置之和也即所给定的约束条件，可以看出，除了初始状态不满足设想状态，经过很快的时间达到并保持稳定。

并且通过使用普通PD鲁棒控制得到的约束误差与实验仿真结果进行对比，红色实线表示的是本文设计的方法的跟踪结果，蓝色虚线表示的是常规PD鲁棒控制得到的跟踪结果。图中小图为前5 s内的放大部分。

从仿真结果可以看出，由于初始状态的原因，开始是不满足精度要求的，但当前状态下，本方法的效果在约1 s内即达到约0.1 mm以内，相较之常规控制方法在4～5 s内才能达到标准而言，可以看出具有更快的收敛速度与稳定性。

图4 轨迹跟踪误差

图5和图6分别展示了两个关节所需控制力矩的变化，可以看出除初始略有波动外，达到稳定后所需力矩较为平稳，说明整个过程的稳定性很好。

图5 关节1所需控制力矩变化过程 图6 关节2所需控制力矩变化过程

5 结论

文章提出了一种基于U-K理论的自适应鲁棒控制器，来处理协作机器人的轨迹跟踪问题。针对一个受约束的协作机器人系统，将控制分为标称部分与不确定性部分。前者利用U-K方程求解得到理想状况下的主动作用力，对后者设计了自适应鲁棒控制器并验证了其稳定性。控制设计的过程中，通过将约束条件进行微分，可以同时处理完整约束与非完整约束。并且，除了假设不确定性边界的存在性以外不需要其他关于不确定性的信息。

仿真实例结果表明：①机器人连杆的位置按照给定约束运动，跟踪误差较小。②轨迹跟踪误收敛时间相较常规鲁棒控制小，表明该控制具有较快的响应速度。③控制输入的力矩波动变化平稳，表明了该系统具有很好的稳定性。因此，该方法有效地保证了协作机器人系统对于给定轨迹的跟踪性能。