肖 帆 李公法 章晓峰 陶 波 蒋国璋 李 光
(1.武汉科技大学冶金装备及其控制教育部重点实验室，武汉 430081; 2.武汉科技大学精密制造研究院，武汉 430081;3.湖南工业大学机械工程学院，株洲 412007)
机器人可以代替人类从事危险和重复性的工作，其已被广泛应用于工业、农业、服务等行业中。机器人的任务也从搬运、码垛等拓展到了3D打印、弧焊、喷涂等对轨迹跟踪精度要求更高的任务中。在完成这些任务时，通常采用逆运动学[1]将机器人的笛卡尔轨迹跟踪问题转换为关节轨迹跟踪控制问题。然而，由于机器人的动力学高度非线性的本质及关节摩擦力等因素，使得机器人的轨迹跟踪控制问题变得困难。
机器人关节轨迹跟踪控制可分为基于模型控制和无模型控制两种。计算力矩控制[2]是一种基于模型控制的策略，其前馈路径能很好地抑制由于系统动态特性而引起的跟踪滞后，其反馈路径可以用来抵消不确定性引起的扰动。然而，由于结构和非结构的不确定性，使得精确的机器人动力学模型难以被确定，从而限制了计算力矩控制的跟踪性能。因此，机器人动力学模型通常被分为模型部分和不确定部分。后者由其他控制方法处理，如自适应控制[3]、鲁棒控制[4-5]、滑模控制[6-8]、智能控制[9-11]等。
比例-微分(PD)或比例-积分-微分(PID)[12-13]是一种典型的无模型控制算法，由于其控制规律简单，被广泛应用于机器的控制器中。具有重力补偿的PD控制算法被用于定点跟踪任务中时，能够使机器人的关节位置误差全局渐近稳定[14-15]。然而, 重力项在实际应用中会因载荷未知或时变而变化，不能被完全补偿。因此，PD控制方案存在稳态误差。在PID控制中引入积分项是消除PD控制引起的静态误差的一种方法。在执行定点跟踪任务时，因不需要考虑动态因素的影响，PID可以达到很好的效果。但是在轨迹跟踪中，PID控制方案下的跟踪误差会受到机器人非线性动态行为的影响。文献[16]认为PID不适合处理高度非线性的系统，因为控制律的设计仅仅是基于局部参数。因此，模糊逻辑[17-19]、自适应[20]、神经网络[21-22]等算法被用于在线调节PID的参数。虽然这些方法通过自动调整参数可以提高PID算法的跟踪精度，但额外的计算量阻碍了它们在实时处理控制中的应用。一些学者[23-26]基于文献[27]，采用饱和PID控制算法使机器人系统稳定。大部分机器人轨迹跟踪控制的方法在整个控制过程中参考轨迹基本与期望轨迹保持一致。实际上，可以通过修正参考轨迹来提高机器人的轨迹跟踪精度。
修正参考轨迹这种方法或思路，已经被一些学者研究并成功应用[28-34]，但是建立在传统迭代学习控制的基础上。该方法需要对期望轨迹重复跟踪，然后再修正参考轨迹，耗费大量时间成本。
本文提出一种基于参考轨迹实时修正的机器人轨迹控制方法。该方法将轨迹跟踪过程中已被跟踪点的误差进行累加，然后实时前馈补偿至参考轨迹上即将被跟踪的点。通过实时修正参考轨迹来实现机器人各个关节的实际运动符合期望轨迹。只需控制器中的算法保证速度误差稳定，即可保证轨迹跟踪误差收敛。通过仿真和实验验证所提方法的有效性。
1.1 控制框图
在机器人关节控制结构中加入了前馈补偿，如图1所示。在图中，qd表示期望的关节位置向量。它既可以表示为一个定点，也可以表示为一段连续轨迹。q表示机器人各关节当前的位置向量。qd和q之间的误差e称为跟踪误差。Kc是一个对角矩阵，其维度与机器人自由度一致，称为补偿增益。符号∑表示累加器，它将轨迹上已被跟踪点的Kce累加起来。累加器的输出加到qd上，得到新的关节命令或修正后的参考轨迹qc。然后控制器根据指令误差δq对机器人关节运动进行调整，δq是qc与q之间的误差。当Kc=0时，qc=qd，δq=e。
图1 基于参考轨迹实时修正的机器人控制结构
1.2 跟踪误差收敛性分析
假设每个关节的轨迹被离散为无限个点，相邻点之间的采样时间间隔为T。根据图1可以得到第1个期望点与其当前关节位置之间的误差为
(1)
以及第1次补偿后的新关节指令和指令误差
(2)
(3)
将式(1)、(2)代入式(3)可以得到第1个点的跟踪误差与指令误差间的关系
(4)
第1个点补偿后，在控制器作用后，机器人各个关节到达q(2)位置。类似之前，可以得到
(5)
(6)
(7)
将式(5)、(6)代入式(7)，得
(8)
以此类推，分别得到
(9)
(10)
接下来，令式(10)减去式(9)，整理后可得
(11)
其中
En+1=δq(n+1)-δq(n)
式中I——单位矩阵
En+1——指令误差的差分
为方便描述，利用无下标的E表示各次补偿中得到的指令误差的差分。
将式(11)迭代展开得到
(12)
式中e1——初始状态下关节实际位置与期望轨迹之间的误差
从式(12)可以看出，只要Kc是正定矩阵且指令误差的差分E等于零，跟踪误差e就会收敛，不受e1的影响，并且Kc中的元素值越大，收敛速度越快。
为了令式(12)具有物理意义，式(12)两边同时除以采样时间T，可得
(13)
式(13)中E/T表示由指令误差差分计算得到的指令速度误差。由于指令速度误差受控制器中算法的影响，因此，式(12)、(13)的一个收敛条件为控制器算法能保证指令速度误差稳定。
综上所述，本文提出的方法能够使跟踪误差收敛的前提是控制器中算法能够保证速度误差稳定且Kc正定。其中，Kc中元素的值越大，收敛速度越快。
1.3 PD控制律证明
对于机器人，其动力学描述为
(14)
M(q)——惯性矩阵，∈Rm
g(q)——重力矩项，∈Rm
τd——外部干扰，∈Rm
u——作用于各个关节的总控制输入，∈Rm
为了便于描述，式(14)的左边用f替代。
图1中控制器可采用PD控制律，其表达式为
(15)
式中KP、KD——比例和微分增益，都为正定矩阵
PD控制律稳定性的证明可参照文献[36-37]。文献[36]从理论上证明了在增益KD足够大的情况下，PD控制律可使机器人任意精度地跟踪给定期望轨迹。然而，实际中机器人每个驱动器的转矩有限，因此KD不可能被设置得非常大。文献[37]证明了PD控制一致有界，这表明可以通过调整参数使轨迹跟踪达到期望的精度。
将式(15)代入式(14)，并考虑Lyapunov函数V=δqTKPδq/2，得
(16)
控制器的采样时间越小或者期望轨迹的速度越小，振荡邻域范围会越小。这种情况下，相邻采样点之间的误差变化十分接近，相当于提高了δq调节的分辨率。因此，在控制过程中，KPδq+f的残差也会随之减小，系统也会更接近稳定点。因为摩擦力等不是连续变化的，所以实际跟踪中误差可能会出现突变。
在实际控制中，PD控制律通常采用离散形式，并且它的速度误差由位置误差差分计算得到。因此，式(15)可改写为
(17)
由1.2节和1.3节可知，增大KP和Kc都能有效地减小最终的跟踪误差。从已有的PD控制律的参数调节经验可知，过大的KD会放大噪声，从而引起系统振荡并导致机器人的关节抖动。因此，所提算法主要调节KP和Kc。这两个参数的调节方案如下：
方案1：如果系统受摩擦力等不连续力的影响大，则主要增加KP，可有效地降低在它们不连续区域的跟踪误差。
方案2：如果系统受摩擦力等不连续力的影响小，则主要增加Kc，可有效地缩小二者的调节范围。
在调节KP和Kc的过程中，如果跟踪误差出现了发散性的振荡，则适当降低KD，再增加KP或Kc。其中KP和KD的初始值可根据已有经验设置，Kc的初始值可以设置为一个接近于零的值。
3.1 期望轨迹
轨迹1用于参数调节和测试，轨迹2用于验证所提方法对不同轨迹的适应性以及与PID的对比，可得
qdi(t)=
轨迹2能激发机器人在慢速和快速状态下的动力学特性[35]。
3.2 仿真
以一款只含旋转关节的2自由度平面机器人为例。惯性矩阵表达式为
M=[mij]2×2
科氏力和向心力矩阵表达式为
C=[cij]2×2
重力矩表达式为
G=[G1G2]T
其中G1=(m1lc1+m2l1)gcosq1+m2lc2gcos(q1+q2)
G2=m2lc2gcos(q1+q2)
关节黏度/静摩擦力矩表达式为
Fr=[Fr1Fr2]T
函数sign()表示只取输入的正负符号。
干扰向量表达式为
τd=[τd1τd2]T
其中τd1=0.3sintτd2=0.1(1-e-t)
机器人参数分别为m1=10 kg,m2=5 kg,l1=1 m,l2=0.5 m,lc1=0.5 m,lc2=0.25 m,Ic1=0.83 kg·m2,Ic2=0.3 kg·m2,g=9.81 m/s2。所有算法采样时间T设为0.005 s。
3.2.1轨迹1跟踪结果
所提算法的3个参数按照第2节中的方案人工试错调节得到，分别为KP=diag{80,10 690}，KD=diag{30,30}，Kc=diag{21,1}。第1个关节参数根据方案1调节得到，第2个关节根据方案2调节得到。
图2为期望轨迹qd、关节实际轨迹q和实时修正后的参考轨迹qc。由图2可以看出，修正后参考轨迹与其他两条有很大的区别，而另外2条重合。表明本文所提出的方法在机器人轨迹跟踪控制问题上可行。图3为关节实际轨迹与期望轨迹之间的误差，两个关节误差分别在[-0.000 3,0.003 4]rad和[-0.000 2,0.000 45]rad之间，表明所提方法有效，跟踪精度优。但是跟踪误差存在周期性的尖峰，且在初始阶段尖峰值最大。周期性的尖峰是速度过零时引起摩擦力突变造成。初始阶段高尖峰值则还有重力矩作用的原因。因为初始阶段，所提方法作用效果是一个由弱到强的过程，因此它需要一段时间来抵消掉重力矩的影响。图4为指令误差差分，可以看出它与跟踪误差除了幅度以外，整体的变化趋势十分接近。
图2 关节轨迹跟踪曲线
图3 关节轨迹的跟踪误差
图4 关节指令误差差分
3.2.2轨迹2跟踪结果
由于P(I)D主要通过增加比例项KP和积分项KI来提高跟踪精度，因此，上一节设置的KD保持不变，也可更直观地比较本文方法与P(I)D的性能。通过人工调节，P(I)D的另外两个参数被确定为KP=diag{62 000, 41 800},KI=diag{62 000, 60 000}。
图5为本文方法与P(I)D跟踪轨迹2得到的跟踪误差。在图5中，本文方法在前0.2 s出现了较大的尖峰，随后会变周期性地出现较小的尖峰，但整体比较稳定。单纯的PD控制律在前1.2 s存在剧烈的振荡，而PID的振荡持续了约2 s。为了更好地对比性能，利用2 s后得到的跟踪误差绝对平均值来衡量跟踪精度，结果见表1。由表1可知，本文方法对两个关节的跟踪精度比PD分别提高93.14%和98.38%；
比PID分别提高79.39%和95.35%。
表1 3个算法跟踪误差绝对平均值
图5 本文方法和P(I)D跟踪轨迹2的误差对比
图6为3个算法计算得到的控制输入力矩。本文方法在关节1前0.2 s内存在一个突变，需要提供比P(I)D更大的控制输入。随后三者的控制输入相差不大。P(I)D的KP较大，可以很好地抑制重力矩和摩擦力的作用，但是也会使它们在控制初始阶段出现振荡。
图6 本文方法和P(I)D跟踪轨迹2的控制输入
综上所述，本文方法的收敛速度、稳定性和精度均较P(I)D更好，但是初始阶段在克服重力矩影响时，需要提供更大的控制力矩。
3.2.3重力矩补偿测试
根据提供的仿真模型，可将2个关节都在0 rad时计算得到的重力矩看作真实值，它们分别为110.36、12.26 N·m。实际操作中，很难准确估算出这两个值。因此，取它们的80%作为估算值补偿在式(17)右侧。图7、8分别为重力矩补偿前后所提方法的跟踪误差和控制输入。由图7、8可看出，所提的这个补偿方案可行且效果很好。初始阶段的控制输入显著下降，跟踪精度也得到了提高。
图7 本文方法重力矩补偿前后的跟踪误差
图8 本文方法重力矩补偿前后的控制输入
3.3 实验
机器人初始构型如图9所示，其各个关节由分辨率为0.001 5 rad/p的Dynamixel XM430-W350型舵机驱动。该款机器人总质量仅0.7 kg，各关节受重力矩影响小。此外，其各关节受摩擦力的影响较小。因此，该机器人实验能更好地反映所提方法的有效性。本文控制算法均在Arduino中编写，然后被上传至OpenCR1.0板以控制机器人的关节运动。所有算法在该控制板上的采样时间T设为0.005 ms。
图9 实物机器人
图10 3个算法在实物机器人上跟踪轨迹1的跟踪误差绝对值
由于各个关节的实际初始位置不处于真正的机械零位，因此对它们在0.08 s后的绝对平均值和最大绝对值进行比较，结果见表2。
表2 3个算法跟踪轨迹1跟踪误差的绝对平均值和最大绝对值
图11 3个算法在实物机器人上跟踪轨迹2的跟踪误差绝对值
表3 3个算法跟踪轨迹2得到的跟踪误差的绝对平均值和最大绝对值
由图10、11可知，所提方法只需很小的参数便可达到比P(I)D更小的跟踪误差。轨迹2比轨迹1的速度慢，结果表明所提方法跟踪速度越慢的期望轨迹，跟踪误差越小。
(1)将轨迹跟踪过程中已被跟踪点的误差累加起来作为前馈补偿至即将被跟踪的点上的方法，对机器人的轨迹跟踪控制问题可行。
(2)在控制器中的算法使速度误差稳定的前提下，所提方法可以令轨迹跟踪误差收敛。
(3)所提方法可以起到自动调节PD控制律中参数的作用。与PID一样有3个参数需要整定，但是收敛速度更快，跟踪误差更小。
(4)由于控制器采用PD算法，所提方法的跟踪效果也会受其影响：机器人期望轨迹的速度越小，跟踪误差会越小；
反之，跟踪误差也会增大。
(5)将初始位置的重力矩作为常数补偿，可以有效地减小所提方法初始阶段的误差和控制输入。
猜你喜欢力矩指令轨迹轨迹读友·少年文学(清雅版)(2020年4期)2020-08-24轨迹读友·少年文学(清雅版)(2020年3期)2020-07-24ARINC661显控指令快速验证方法测控技术(2018年5期)2018-12-09轨迹现代装饰(2018年5期)2018-05-26进化的轨迹（一）——进化，无尽的适应中国三峡(2017年2期)2017-06-09发动机阻力矩计算和起动机介绍山东青年(2016年12期)2017-03-02杀毒软件中指令虚拟机的脆弱性分析电信科学(2016年10期)2016-11-23小型力矩电机波动力矩的测量光学精密工程(2016年6期)2016-11-07弹性负载力矩下舵偏转角度的测量方法航天制造技术(2016年6期)2016-05-09基于D-最优化理论的陀螺仪力矩反馈测试法探测与控制学报(2015年4期)2015-12-15