技术领域
[0001] 本
发明属于机器人控制技术领域，特别涉及一种基于端口受控哈密顿与反步法协调控制的二自由度
SCARA机器人的
位置跟踪控制方法。
背景技术
[0002] SCARA机器人，即选择
顺应性装配机器手臂，在装配、
焊接、搬运等行业得到了广泛的应用，与此同时对SCARA机器人的动作速率和重复
定位精度提出了越来越高的要求。
[0003] 由于机械手是一种具有高度非线性的系统，传统的单一控制对
信号的处理有限，机器人位置控制难以同时具有良好的快速性和
稳定性。
[0004] 滑模控制的滑动模态与控制对象的参数变化和外界的干扰无关，因而滑模控制具有实时性好、鲁棒性好等的优点，但却使系统存在抖振现象；模糊控制无需建立精确的数学模型，但是其稳态精度不高；自适应控制能够通过修正自己的特性以适应外界扰动和对象变化带来的动态特性的变化，但是自适应控制理论尚不完整、参数设置难度较大以及应用场合有限；神经网络非线性拟合能
力强、准确度高，但是控制
算法复杂、网络结构和参数确定困难。
[0005] 反步法通过对每一个子系统设计Lyapunov函数和中间虚拟控制量，“后退”到整个系统并通过积分环节
串联从而形成整个系统控制，能够保证整个系统的稳定。近年来，随着非线性的发展，端口受控哈密顿(PCH，Port-Controlled Hamiltonian)因其在稳态性能方面性能优良，且控制系统设计和稳定性分析简单，因而得到高度关注。
[0006] 然而，反步法虽然能够提高系统的响应速度，但是稳态性能有待提高；而端口受控哈密顿控制虽然能够使系统具有良好的稳态性能，但是动态响应速度有待提高。综上，二自由度SCARA机器人系统使用单一控制方法难以实现快速、精准的位置跟踪控制的问题。
发明内容
[0007] 本发明的目的在于提出一种基于端口受控哈密顿与反步法协调控制的二自由度SCARA机器人的位置跟踪控制方法，以提高系统的动态性能和稳态性能，且抑制干扰能力强。
[0008] 为了实现上述目的，本发明采用如下技术方案：
[0009] 二自由度SCARA机器人的位置跟踪控制方法，包括如下步骤：
[0010] s1建立二自由度SCARA机器人的动力学模型
[0011] 根据D-H坐标法，推导出二自由度SCARA机器人的动力学模型为：
[0012]
[0013] 其中，τ＝[τ1 τ2]T为输入向量，τ1、τ2分别表示关节1、关节2的控制力矩；
[0014] q＝[q1 q2]T表示关节
角位移矢量，q1、q2分别表示关节1、关节2的角位移；
[0015] 表示关节
角速度矢量， 分别表示关节1、关节2的角速度；
[0017]为机
器人惯性矩阵；
[0018] 为机器人哥氏力与向心力矩阵；
[0019] s2设计二自由度SCARA机器人的端口受控哈密顿
控制器与反步法控制器
[0020] s2.1设计端口受控哈密顿控制器
[0021] s2.1.1
能量耗散的端口受控哈密顿控制系统模型为：
[0022]
[0023] 其中，x、τPCH和y分别表示端口受控哈密顿控制系统的状态向量、输入向量和输出向量；H(x)表示端口受控哈密顿控制系统的能量函数；R(x)为半正定对称矩阵，R(x)＝RT(x)，它反映了端口受控哈密顿控制系统端口上的附加阻性结构；J(x)为反对称矩阵，J(x)＝-JT(x)，它反映了端口受控哈密顿控制系统内部的互联结构；g(x)反映了端口受控哈密顿控制系统的端口特性；
[0024] s2.1.2建立二自由度SCARA机器人的端口受控哈密顿控制系统模型
[0025] 二自由度SCARA机器人的端口受控哈密顿控制系统的能量函数H(q,p)为：
[0026]
[0027] 其中， 表示端口受控哈密顿控制系统的
动能，U(q)表示端口受控哈密顿控制系统的
势能； 表示端口受控哈密顿控制系统的广义动量矢
量，p1、p2分别表示关节1、关节2的广义动量；
[0028] 选取端口受控哈密顿控制系统的状态向量为x＝[q p]T＝[q1 q2 p1 p2]T，将二自由度SCARA机器人的端口受控哈密顿控制系统模型写为：
[0029]
[0030] 其中，为输入矩阵；
[0031] τ1-PCH、τ2-PCH分别表示端口受控哈密顿控制系统对关节1、关节2输出的控制力矩；
[0032] s2.1.3设计二自由度SCARA机器人的端口受控哈密顿控制器
[0033] 确定二自由度SCARA机器人期望的平衡点为xd＝[qd pd]T；
[0034] 其中，qd＝[q1d q2d]T表示期望的关节角位移矢量，q1d、q2d分别表示关节1、关节2的期望角位移， 分别表示关节1、关节2的期望角速度；pd＝[p1d p2d]T＝[0 0]T表示期望的广义动量矢量，p1d、p2d分别表示关节1、关节2的期望广义动量；
[0035] 构造一个加入反馈控制后的闭环期望端口受控哈密顿控制系统的能量函数Hd(x)，使公式(2)所描述的系统渐进地稳定在期望的平衡点xd附近，且
闭环系统可写为：
[0036]
[0037] 选取闭环期望端口受控哈密顿控制系统的能量函数为：
[0038]
[0039] 其中，Hd(xd)＝0；
[0040] 为期望惯性矩阵，a1、a2、a3为设计参数；
[0041] 为比例增益，KP1和KP2分别为常数；
[0042] 配置满足：
[0043]
[0044] 其中，Jd(x)为期望的互联矩阵，且 Rd(x)为期望的阻尼矩阵，且为微分系数，KV1、KV2为设计参数；
[0045] 由于公式(2)与公式(5)均是对二自由度SCARA机器人的端口受控哈密顿控制系统的描述，将公式(2)与公式(5)中的 消掉可得：
[0046]
[0047] 进一步得到：
[0048]
[0049] 解方程可得端口受控哈密顿控制器为：
[0050]
[0051] 其中：
[0052]
[0053]
[0054]
[0055]
[0056] s2.1.4二自由度SCARA机器人的端口受控哈密顿控制系统稳定性分析
[0057] 由公式(6)所描述的期望端口受控哈密顿控制系统的能量函数可知Hd(x)＞0；
[0058] 对公式(6)求导，并将公式(5)代入可得：
[0059]
[0060] 由于Jd(x)为反对称矩阵、Rd(x)为半正定矩阵，根据Lasalle定理，若包括在集合中的闭环系统最大不变集为{xd}，则端口受控哈密顿控制系统在平衡点xd处是渐进稳定的，其中，Rn表示n维实数向量；
[0061] s2.2设计反步法控制器
[0062] s2.2.1设计二自由度SCARA机器人的反步法控制器
[0063] 定义变量x1＝[q1 q2]T， 则二自由度SCARA机器人的动力学模型表示为：
[0064]
[0065] 其中，τBS＝[τ1_BS τ2_BS]T为反步法控制系统输出力矩，τ1_BS、τ2_BS分别为反步法控制系统为关节1、关节2提供的控制力矩；
[0066] 定义二自由度SCARA机器人的反步法控制系统输出误差变量为：
[0067] e1＝x1-x1d＝[q1-q1d q2-q2d]T (13)
[0068] 其中，x1d＝[q1d q2d]T为期望的关节角位移矢量；
[0069] 对公式(13)两边求导，可得：
[0070]
[0071] 为了确保x1能有效跟踪期望信号x1d，反步法控制系统的第一个子系统选取李雅普诺夫控制函数如下：
[0072]
[0073] 对公式(15)求导得：
[0074]
[0075] 选取虚拟控制函数：
[0076]
[0077] 其中， 为增益矩阵，k11，k12为设计参数；
[0078] 将公式(17)作为x2代入公式(16)中，可得：
[0079]
[0080] 定义虚拟控制误差变量为：
[0081]
[0082] 其中， 为期望的关节角速度矢量；
[0083] 反步法控制系统的第二个子系统选取李雅普诺夫函数为：
[0084]
[0085] 为使二自由度SCARA机器人反步法控制系统稳定，必须设计τBS使 负定，为此选取τBS为：
[0086]
[0087] 其中， 为增益矩阵，k21，k22为设计参数；
[0089]
[0090] 其中，
[0091] kb11＝-M11k21k11，kb12＝-M12k22k12，ks11＝(C11-M11k21-M11k11)，ks12＝(C12-M12k22-M12k12)；
[0092] kb21＝-M21k21k11， ks21＝(C21-M21k21-M21k11)，ks22＝(-M22k22-M22k12)；
[0093] b1＝M11k21k11q1d+M12k22k12q2d，b2＝M21k21k11q1d+M22k22k12q2d；
[0094] s2.2.2二自由度SCARA机器人的反步法控制系统稳定性分析
[0095] 对公式(19)两边求导，并将公式(12)、公式(21)代入可得：
[0096]
[0097] 对公式(20)求导，并将公式(18)、公式(23)代入整理可得：
[0098]
[0099] 由公式(20)可知V2为正定、公式(24)可知 为半负定，根据李雅普诺夫稳定性理论可知，二自由度SCARA机器人的反步法控制系统是渐进稳定的；
[0100] s3设计端口受控哈密顿控制系统与反步法控制系统的协调控制器
[0101] s3.1构建端口受控哈密顿控制系统与反步法控制系统的协调控制器
[0102] 定义c1-PCH，c2-PCH为端口受控哈密顿控制系统的协调函数，定义c1-BS,c2-BS为反步法控制系统的协调函数；
[0103] 则端口受控哈密顿控制系统与反步法控制系统的协调控制函数可设计为：
[0104]
[0105] 其中，TC为协调时间常数，c1-PCH∈[0,1]，c2-PCH∈[0,1]，c1-BS∈[0,1]，c2-BS∈[0,1]；
[0106] 二自由度SCARA机器人的端口受控哈密顿控制系统与反步法控制系统的协调控制器为：
[0107]
[0108] s3.2基于端口受控哈密顿控制与反步法控制的二自由度SCARA机器人位置跟踪控制系统的稳定性分析
[0109] 二自由度SCARA机器人整个协调控制系统的李雅普诺夫控制函数可写为：
[0110] V＝VPCH+V2 (27)
[0111] 当时间t＝0时，c1-PCH(t)＝c2-PCH(t)＝0、c1-BS(t)＝c2-BS(t)＝1，只有反步法控制系统作用于整个协调控制系统，V＝V2＞0， 根据李雅普诺夫稳定性理论可知整个协调控制系统稳定；
[0112] 当时间0＜t＜∞时，c1-PCH(t)、c2-PCH(t)、c1-BS(t)、c2-BS(t)均为大于0小于1的常数，属于共同控制，且随着时间的增加，反步法控制系统的作用力度逐渐减小，端口受控哈密顿控制系统的作用力度逐渐增加；由于两个控制系统的类型没有变化，结合端口受控哈密顿控制系统和反步法控制系统稳定性分析可知，V正定，半负定，因此整个协调控制系统是渐进稳定的；
[0113] 当时间t→∞时，c1-PCH(t)＝c2-PCH(t)＝1，c1-BS(t)＝c2-BS(t)＝0，只有端口受控哈密顿控制系统作用于整个协调控制系统，V＝VPCH＞0， 整个协调控制系统稳定；
[0114] 由以上分析可知，整个协调控制系统是渐进稳定的。
[0115] 本发明具有如下优点：
[0116] 本发明针对二自由度SCARA机器人系统使用单一控制方法难以实现快速、精准的位置跟踪控制的问题，设计了基于端口受控哈密顿(PCH)与反步法的协调控制方案。其中，反步法能够提高系统的响应速度，但是稳态性能有待提高；而端口受控哈密顿控制能够使系统具有良好的稳态性能，但是动态响应速度有待提高。本发明采用指数函数作为协调控制函数，以适应SCARA机器人的位置控制，该方案通过协调上述两种控制方法的力度，当在机器人位置瞬态变化时，反步法起主要作用；而在稳态时，端口受控哈密顿起主要作用，从而使系统不仅具有良好的响应速度和良好的稳态性能，而且抑制干扰的能力提高，该协调控制方案能够使两种控制方法的优点在相应时间点得到最有效的利用，应用价值高。
附图说明
[0117] 图1为本发明中二自由度SCARA机器人的动力学模型示意图；
[0118] 图2为本发明中基于端口受控哈密顿与反步法控制的协调控制系统的
框图；
[0119] 图3为本发明中关节1在协调时间常数不同时的轨迹曲线图；
[0120] 图4为本发明中关节2在协调时间常数不同时的轨迹曲线图；
[0121] 图5为本发明中关节1在不同控制方法时的轨迹曲线图；
[0122] 图6为本发明中关节2在不同控制方法时的轨迹曲线图；
[0123] 图7为本发明中关节1在不同控制方法时干扰对轨迹曲线的影响曲线图；
[0124] 图8为本发明中关节2在不同控制方法时干扰对轨迹曲线的影响曲线图。
具体实施方式
[0125] 下面结合附图以及具体实施方式对本发明作进一步详细说明：
[0126] 二自由度SCARA机器人的位置跟踪控制方法，包括如下步骤：
[0127] s1建立二自由度SCARA机器人的动力学模型，如图1所示：
[0128] 根据D-H坐标法，推导出二自由度SCARA机器人的动力学模型为：
[0129]
[0130] 其中，τ＝[τ1τ2]T为输入向量，τ1、τ2分别表示关节1、关节2的控制力矩；
[0131] q＝[q1q2]T表示关节角位移矢量，q1、q2分别表示关节1、关节2的角位移；
[0132] 表示关节角速度矢量， 分别表示关节1、关节2的角速度；
[0134]为机
器人惯性矩阵；
[0135] 为机器人哥氏力与向心力矩阵。
[0136] s2设计二自由度SCARA机器人的端口受控哈密顿控制器与反步法控制器
[0137] s2.1设计端口受控哈密顿控制器
[0138] s2.1.1能量耗散的端口受控哈密顿控制系统模型为：
[0139]
[0140] 其中，x、τPCH和y分别表示端口受控哈密顿控制系统的状态向量、输入向量和输出T向量；H(x)表示端口受控哈密顿控制系统的能量函数；R(x)为半正定对称矩阵，R(x)＝R(x)，它反映了端口受控哈密顿控制系统端口上的附加阻性结构；J(x)为反对称矩阵，J(x)＝-JT(x)，它反映了端口受控哈密顿控制系统内部的互联结构；g(x)反映了端口受控哈密顿控制系统的端口特性。
[0141] s2.1.2建立二自由度SCARA机器人的端口受控哈密顿控制系统模型
[0142] 二自由度SCARA机器人的端口受控哈密顿控制系统的能量函数H(q,p)为：
[0143]
[0144] 其中， 表示端口受控哈密顿控制系统的动能，U(q)表示端口受控哈密顿控制系统的势能； 表示端口受控哈密顿控制系统的广义动量矢
量，p1、p2分别表示关节1、关节2的广义动量。
[0145] 选取端口受控哈密顿控制系统的状态向量为x＝[q p]T＝[q1 q2 p1 p2]T，将二自由度SCARA机器人的端口受控哈密顿控制系统模型写为：
[0146]
[0147] 其中，为输入矩阵。
[0148] τ1-PCH、τ2-PCH分别表示端口受控哈密顿控制系统对关节1、关节2输出的控制力矩。
[0149] s2.1.3设计二自由度SCARA机器人的端口受控哈密顿控制器
[0150] 确定二自由度SCARA机器人期望的平衡点为xd＝[qd pd]T；
[0151] 其中，qd＝[q1d q2d]T表示期望的关节角位移矢量，q1d、q2d分别表示关节1、关节2的期望角位移， 分别表示关节1、关节2的期望角速度；pd＝[p1d p2d]T＝[0 0]T表示期望的广义动量矢量，p1d、p2d分别表示关节1、关节2的期望广义动量。
[0152] 构造一个加入反馈控制后的闭环期望端口受控哈密顿控制系统的能量函数Hd(x)，使公式(2)所描述的系统渐进地稳定在期望的平衡点xd附近，且闭环系统可写为：
[0153]
[0154] 选取闭环期望端口受控哈密顿控制系统的能量函数为：
[0155]
[0156] 其中，Hd(xd)＝0；
[0157] 为期望惯性矩阵，a1、a2、a3为设计参数。
[0158] 为比例增益，KP1和KP2分别为常数；
[0159] 配置满足：
[0160]
[0161] 其中，Jd(x)为期望的互联矩阵，且 Rd(x)为期望的阻尼矩阵，且为微分系数，KV1、KV2为设计参数。
[0162] 由于公式(2)与公式(5)均是对二自由度SCARA机器人的端口受控哈密顿控制系统的描述，将公式(2)与公式(5)中的 消掉可得：
[0163]
[0164] 进一步得到：
[0165]
[0166] 解方程可得端口受控哈密顿控制器为：
[0167]
[0168] 其中：
[0169]
[0170]
[0171]
[0172]
[0173] s2.1.4二自由度SCARA机器人的端口受控哈密顿控制系统稳定性分析
[0174] 由公式(6)所描述的期望端口受控哈密顿控制系统的能量函数可知Hd(x)＞0；
[0175] 对公式(6)求导，并将公式(5)代入可得：
[0176]
[0177] 由于Jd(x)为反对称矩阵、Rd(x)为半正定矩阵，根据Lasalle定理，若包括在集合[0178] 中的闭环系统最大不变集为{xd}，则端口受控哈密顿控制系统在平衡点xd处是渐进稳定的，其中，Rn表示n维实数向量。
[0179] s2.2设计反步法控制器
[0180] s2.2.1设计二自由度SCARA机器人的反步法控制器
[0181] 定义变量x1＝[q1 q2]T， 则二自由度SCARA机器人的动力学模型表示为：
[0182]
[0183] 其中，τBS＝[τ1_BS τ2_BS]T为反步法控制系统输出力矩，τ1_BS、τ2_BS分别为反步法控制系统为关节1、关节2提供的控制力矩。
[0184] 定义二自由度SCARA机器人的反步法控制系统输出误差变量为：
[0185] e1＝x1-x1d＝[q1-q1d q2-q2d]T (13)
[0186] 其中，x1d＝[q1d q2d]T为期望的关节角位移矢量；
[0187] 对公式(13)两边求导，可得：
[0188]
[0189] 为了确保x1能有效跟踪期望信号x1d，反步法控制系统的第一个子系统选取李雅普诺夫控制函数如下：
[0190]
[0191] 对公式(15)求导得：
[0192]
[0193] 选取虚拟控制函数：
[0194]
[0195] 其中， 为增益矩阵，k11,k12为设计参数。
[0196] 将公式(17)作为x2代入公式(16)中，可得：
[0197]
[0198] 定义虚拟控制误差变量为：
[0199]
[0200] 其中， 为期望的关节角速度矢量。
[0201] 反步法控制系统的第二个子系统选取李雅普诺夫函数为：
[0202]
[0203] 为使二自由度SCARA机器人反步法控制系统稳定，必须设计τBS使 负定，为此选取τBS为：
[0204]
[0205] 其中， 为增益矩阵，k21，k22为设计参数。
[0206] 进一步整理可得反步法控制器为：
[0207]
[0208] 其中，
[0209] kb11＝-M11k21k11，kb12＝-M12k22k12，ks11＝(C11-M11k21-M11k11)，ks12＝(C12-M12k22-M12k12)；
[0210] kb21＝-M21k21k11， ks21＝(C21-M21k21-M21k11)，ks22＝(-M22k22-M22k12)；
[0211] b1＝M11k21k11q1d+M12k22k12q2d，b2＝M21k21k11q1d+M22k22k12q2d。
[0212] s2.2.2二自由度SCARA机器人的反步法控制系统稳定性分析
[0213] 对公式(19)两边求导，并将公式(12)、公式(21)代入可得：
[0214]
[0215] 对公式(20)求导，并将公式(18)、公式(23)代入整理可得：
[0216]
[0217] 由公式(20)可知V2为正定、公式(24)可知 为半负定，根据李雅普诺夫稳定性理论可知，二自由度SCARA机器人的反步法控制系统是渐进稳定的。
[0218] s3设计端口受控哈密顿控制系统与反步法控制系统的协调控制器
[0219] s3.1构建端口受控哈密顿控制系统与反步法控制系统的协调控制器，如图2所示：
[0220] 定义c1-PCH，c2-PCH为端口受控哈密顿控制系统的协调函数，定义c1-BS,c2-BS为反步法控制系统的协调函数；
[0221] 则端口受控哈密顿控制系统与反步法控制系统的协调控制函数可设计为：
[0222]
[0223] 其中，TC为协调时间常数，c1-PCH∈[0,1]，c2-PCH∈[0,1]，c1-BS∈[0,1]，c2-BS∈[0,1]。
[0224] 二自由度SCARA机器人的端口受控哈密顿控制系统与反步法控制系统的协调控制器为：
[0225]
[0226] s3.2基于端口受控哈密顿控制与反步法控制的二自由度SCARA机器人位置跟踪控制系统的稳定性分析
[0227] 二自由度SCARA机器人整个协调控制系统的李雅普诺夫控制函数可写为：
[0228] V＝VPCH+V2 (27)
[0229] 当时间t＝0时，c1-PCH(t)＝c2-PCH(t)＝0、c1-BS(t)＝c2-BS(t)＝1，只有反步法控制系统作用于整个协调控制系统，V＝V2＞0， 根据李雅普诺夫稳定性理论可知整个协调控制系统稳定；
[0230] 当时间0＜t＜∞时，c1-PCH(t)、c2-PCH(t)、c1-BS(t)、c2-BS(t)均为大于0小于1的常数，属于共同控制，且随着时间的增加，反步法控制系统的作用力度逐渐减小，端口受控哈密顿控制系统的作用力度逐渐增加；由于两个控制系统的类型没有变化，结合端口受控哈密顿控制系统和反步法控制系统稳定性分析可知，V正定，半负定，因此整个协调控制系统是渐进稳定的；
[0231] 当时间t→∞时，c1-PCH(t)＝c2-PCH(t)＝1，c1-BS(t)＝c2-BS(t)＝0，只有端口受控哈密顿控制系统作用于整个协调控制系统，V＝VPCH＞0， 整个协调控制系统稳定；
[0232] 由以上分析可知，整个协调控制系统是渐进稳定的。
[0233] 由于端口受控哈密顿控制和反步法控制具有互补性，本发明设计的协调控制系统能够将两种控制方法的优点在相应的时间点得到充分利用，使二自由度SCARA机器人位置跟踪控制系统同时具有良好的快速性、稳定性，且系统抵抗外界干扰能力提高。
[0234] 本发明在Matlab/Simulink环境下针对端口受控哈密顿与反步法的二自由度SCARA机器人位置跟踪控制方法进行仿真，以验证该协调控制方法对轨迹跟踪的控制性能。
[0235] 仿真所用参数如下：
[0236] 端口受控哈密顿控制系统中KP1＝200000，KP2＝20000，KV1＝10000，KV2＝1a1＝0.05，a2＝0.0001，a3＝0.0001；反步法控制系统中k11＝k12＝1000，k21＝k22＝200000。
[0237] 给定关节1和关节2的期望信号均为单位阶跃信号。
[0238] 图3和图4分别是关节1和关节2协调时间不同时协调控制的轨迹跟踪曲线，由图3和图4可知，协调时间常数TC分别为0.05、0.3、0.6，为使关节1和关节2同时具有较快的响应速度，较好控制效果，因此在协调控制仿真实验中取TC＝0.3。
[0239] 图5和图6为采用不同的控制方法时的轨迹跟踪曲线，由图5和图6可知，反步法的跟踪速度快，但是存在一定的稳态误差；端口受控哈密顿控制的稳态精度高，稳态性能好，但是轨迹跟踪速度慢；协调控制跟踪速度快且稳态精度高。
[0240] 图7和图8为在t＝0.6s施加扰动后的轨迹跟踪曲线，由图7和图8可知，存在干扰时，反步法控制
波形变化小，系统抑制干扰的能力强；端口受控哈密顿控制的波形变化较大，抑制干扰的能力较弱；协调控制波形渐进趋近于稳定，且抑制干扰的能力提高。
[0241] 经比较可知，协调控制能够有效的结合反步法控制和端口受控哈密顿控制的优点，系统的动态性能和稳态性能良好，且抑制干扰的能力较强，满足设计的要求。
[0242] 当然，以上说明仅仅为本发明的较佳
实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本
说明书的教导下，所做出的所有等同替代、明显
变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。
