二自由度关节机器人轨迹的协调控制方法与流程

本发明属于机器人控制技术领域，特别涉及一种基于端口受控哈密顿与pd控制的二自由度关节机器人轨迹的协调控制方法。

背景技术：

关节机器人，也称关节手臂机器人或关节机械手臂，是当今工业领域中最常见的工业机器人的形态之一，适合用于诸多工业领域的机械自动化作业，比如自动装配、喷漆、搬运、焊接等工作。在对其进行轨迹跟踪系统进行控制时，传统的转矩控制和pd控制等对位置误差信号的处理过于简单，大多数情况下对于系统的建模误差以及干扰信号的扰动其动态响应性能和误差抑制都表现不佳。滑模控制在机器人控制系统中曾被大量使用，优点是实时性好，但也会使系统存在较大的输出抖振现象。模糊控制虽然不需要建立精确的数学模型，但是其控制精度不高，系统存在较大的稳态误差，其稳定性问题还有待解决。神经网络控制算法比较复杂，对于机器人的数学建模以及控制器的求取和实现都增加了相当大的难度。自适应控制响应迅速，但是其参数设置难度较大。近年来，端口受控哈密顿控制越来越受到国内外人们的关注，国内的学者如山东大学的王玉振教授和青岛大学的于海生教授都对其进行了深入且全面的研究。端口受控哈密顿控制的设计较为简单，因其是在能量注入的基础上进行的控制推导，该方法简化了系统的稳定性分析过程，端口受控哈密顿控制虽然在控制精度以及稳定性方面表现良好，但是其动态响应速度较慢，实时性还有待提高。综上，二自由度关节机器人运动系统单独使用一种控制方法难以实现快速高效、高精度的问题。

技术实现要素：

本发明的目的在于提出一种基于端口受控哈密顿与pd控制的二自由度关节机器人轨迹的协调控制方法，以保证二自由度关节机器人运动系统的动态性能与稳态性能优良。

为了实现上述目的，本发明采用如下技术方案：

二自由度关节机器人轨迹的协调控制方法，包括如下步骤：

s1建立二自由度关节机器人的动力学模型

在d-h坐标系中，依据拉格朗日方法推导出二自由度关节机器人的动力学模型为：

其中，τ＝[τ1τ2]^t为输入向量，τ1、τ2分别表示关节1、关节2的控制力矩；

q＝[q1q2]^t表示关节角位移矢量，q1、q2分别表示关节1、关节2的角位移；

表示关节角速度矢量，分别表示关节1、关节2的角速度；

分别表示关节1、关节2的角加速度；

为机器人惯性矩阵；

为机器人哥氏力与向心力矩阵；

g(q)＝[g1(q)g2(q)]^t＝[50.96cosq1+25.48cos(q1+q2)25.48cos(q1+q2)]^t为机器人重力项矩阵；

s2设计二自由度关节机器人的端口受控哈密顿控制器与pd控制器

s2.1设计端口受控哈密顿控制器

s2.1.1能量耗散概念的端口受控哈密顿控制系统模型为：

其中，x、τpch和y分别表示端口受控哈密顿控制系统的状态向量、输入向量和输出向量；r(x)为半正定对称矩阵，r(x)＝r^t(x)≥0，它反映了端口受控哈密顿控制系统端口上的附加阻性结构；j(x)为反对称矩阵，j(x)＝-j^t(x)，它反映了端口受控哈密顿控制系统内部的互联结构；h(x)为hamiltonian函数，它反映了端口受控哈密顿控制系统总的能量存储；g(x)反映了端口受控哈密顿控制系统的端口特性；

s2.1.2建立二自由度关节机器人的端口受控哈密顿控制系统模型

二自由度关节机器人的端口受控哈密顿控制系统的能量函数h(q,p)为：

其中，表示端口受控哈密顿控制系统的动能，u(q)表示端口受控哈密顿控制系统的势能；表示端口受控哈密顿控制系统的广义动量矢量，p1、p2分别表示关节1、关节2的广义动量；

选取端口受控哈密顿控制系统的状态向量为x＝[qp]^t＝[q1q2p1p2]^t，将二自由度关节机器人的端口受控哈密顿控制系统模型写为：

其中，为输入矩阵；

τ1-pch、τ2-pch分别表示端口受控哈密顿控制系统对关节1、关节2输出的控制力矩；

s2.1.3设计二自由度关节机器人的端口受控哈密顿控制器

假设端口受控哈密顿控制系统期望的平衡点为xd＝[qdpd]^t；

其中，qd＝[q1dq2d]^t表示期望的关节角位移矢量，q1d、q2d分别表示关节1、关节2的期望角位移，分别表示关节1、关节2的期望角速度；pd＝[p1dp2d]^t＝[00]^t表示期望的广义动量矢量，p1d、p2d分别表示关节1、关节2的期望广义动量；

为将端口受控哈密顿控制系统渐近的稳定在期望的平衡点xd附近，构造一个加入反馈控制后的闭环系统：

其中，jd(x)为期望的互联矩阵，且rd(x)为期望的阻尼矩阵，且

该闭环系统在期望的平衡点xd处取极小值；

选取期望的hamiltonian函数为：

其中，hd(xd)＝0；

为期望惯性矩阵，a1、a2、a3为设计参数；

为比例增益，kp_pch1和kp_pch2分别为常数；

配置满足：

其中，为微分系数，kd_pch1、kd_pch2为设计参数；

由于公式(2)与公式(5)均是对二自由度关节机器人的端口受控哈密顿控制系统的描述，将公式(2)与公式(5)中的消掉可得：

进一步得到：

解方程可得端口受控哈密顿控制器为：

其中：

s2.2设计pd控制器

设计二自由度关节机器人的pd控制器为：

其中，τpd＝[τ1-pdτ2-pd]^t为pd控制器对关节1、关节2的控制转矩；

为比例增益矩阵；

为微分增益矩阵；

e＝[e1e2]^t＝[q1-q1dq2-q2d]^t为位置误差矢量；

为速度误差矢量；

s3设计端口受控哈密顿控制系统与pd控制系统的协调控制器

定义c1-pch,c2-pch分别为端口受控哈密顿控制系统的协调函数，c1-pd,c2-pd分别为pd控制系统的协调函数；

则端口受控哈密顿控制系统与pd控制系统的协调控制函数可设计为：

其中，tc为协调时间常数，c1-pch∈[0,1]，c2-pch∈[0,1]，c1-pd∈[0,1]，c2-pd∈[0,1]；

二自由度关节机器人端口受控哈密顿控制系统与pd控制系统的协调控制器为：

其中，τ1、τ2为协调控制器对关节1、关节2的转矩输出；

s4基于端口受控哈密顿控制器与pd控制器的二自由度关节机器人运动控制系统的稳定性分析

s4.1二自由度关节机器人的端口受控哈密顿控制系统稳定性分析

由公式(6)所描述的期望端口受控哈密顿控制系统的能量函数可知hd(x)＞0；

对公式(6)求导，并将公式(5)代入可得：

由于jd(x)为反对称矩阵、rd(x)为半正定矩阵，根据lasalle定理，若包括在集合中的闭环系统最大不变集为{xd}，则端口受控哈密顿控制系统在平衡点xd处是渐进稳定的，其中，rⁿ表示n维实数向量；

s4.2二自由度关节机器人的pd控制系统稳定性分析

依据pd控制系统的状态变量e＝[e1e2]^t，将公式(11)代入到公式(1)中可得：

由公式(15)可得为pd控制系统唯一静态平衡点；

定义李雅普诺夫函数为：

其中，u(qd)表示端口受控哈密顿控制系统在期望平衡点处的势能；

由于：

公式(1)描述的机器人动力学模型，对于任意给定的足够小的正常数a，对角正定比例增益矩阵kp_pd为可使下面二式同时成立：

e^tg(q)+e^tkp_pde≥a||e||²(19)

将公式(17)和公式(18)代入公式(16)，可得：

公式(1)描述的机器人动力学模型，惯性矩阵m(q)为对称、正定、有界，选择微分增益矩阵kd_pd足够大，使得：

kd_pd-2m(q)＞0(21)

kd_pd-m(q)-εcmi＞0(22)

其中，ε、cm为正常数，i为单位矩阵；

由公式(20)和公式(21)可得李雅普诺夫函数是正定的；

对公式(16)求导，可得：

由于将由公式(15)求得的代入公式(23)中可得：

公式(1)描述的机器人动力学模型，为反对称矩阵，使得：

公式(24)进一步写为：

假设存在ε＞0，使得||e||≤ε，对于公式(1)描述的机器人动力学模型，存在下列关系：

将公式(19)和公式(26)代入公式(25)可得：

由公式(22)和公式(26)可得

正定，半负定，由李雅普诺夫稳定性理论可知二自由度关节机器人的pd控制系统是渐进稳定的；

s4.3整个协调控制系统稳定性分析

二自由度关节机器人整个协调控制系统的李雅普诺夫控制函数可写为：

v＝vpch+vpd(28)

当时间t＝0时，c1-pch(t)＝c2-pch(t)＝0,c1-pd(t)＝c2-pd(t)＝1，只有pd控制系统作用于协调控制系统，v＝vpd＞0，根据李雅普诺夫稳定性理论可知协调控制系统稳定；

当时间0＜t＜∞时，c1-pch,c2-pch,c1-pd,c2-pd均为大于0小于1的常数，由于时间连续，属于共同控制；由于控制器各变量类型没有发生变化，结合端口受控哈密顿控制系统和pd控制系统稳定性分析可知，v正定，半负定，因此协调控制系统是渐进稳定的；

当时间t→∞时，c1-pch(t)＝c2-pch(t)＝1,c1-pd(t)＝c2-pd(t)＝0，只有端口受控哈密顿控制系统作用于整个控制系统，v＝vpch＞0，协调控制系统稳定；

由以上分析可知，整个协调控制系统是渐进稳定的。

本发明具有如下优点：

本发明针对二自由度关节机器人运动系统单独使用一种控制方法难以实现快速高效、高精度的问题，设计了基于端口受控哈密顿(pch)与pd控制的协调控制方案，端口受控哈密顿控制用于确保系统的稳定性问题，pd控制用于提高系统响应的快速性问题，采用指数函数作为协调函数以实现协调控制策略，从而适应二自由度机器人的误差扰动。本发明既实现了位置信号的快速跟踪，又使机器人的输出信号控制在较高的误差精度范围内。经仿真验证表明，当机器人的机械系统存在建模误差时，采用端口受控哈密顿与pd算法协调控制的二自由度关节机器人运动系统动态性能与稳态性能优良，且能够快速消除误差。

附图说明

图1为本发明中二自由度关节机器人的三维模型图；

图2为本发明中二自由度关节机器人的连杆坐标系统图；

图3为本发明中基于端口受控哈密顿与pd控制的协调控制系统框图；

图4为本发明中关节1在协调时间常数不同时的轨迹曲线图；

图5为本发明中关节2在协调时间常数不同时的轨迹曲线图；

图6为本发明中关节1在不同控制方法时的轨迹曲线图；

图7为本发明中关节2在不同控制方法时的轨迹曲线图；

图8为本发明中关节1不同控制方法时的轨迹误差曲线图；

图9为本发明中关节2不同控制方法时的轨迹误差曲线图。

具体实施方式

下面结合附图以及具体实施方式对本发明作进一步详细说明：

二自由度关节机器人轨迹的协调控制方法，包括如下步骤：

s1建立二自由度关节机器人的动力学模型，如图1和图2所示：

在d-h坐标系中，依据拉格朗日方法推导出二自由度关节机器人的动力学模型为：

其中，τ＝[τ1τ2]^t为输入向量，τ1、τ2分别表示关节1、关节2的控制力矩；

q＝[q1q2]^t表示关节角位移矢量，q1、q2分别表示关节1、关节2的角位移；

表示关节角速度矢量，分别表示关节1、关节2的角速度；

分别表示关节1、关节2的角加速度；

为机器人惯性矩阵；

为机器人哥氏力与向心力矩阵；

g(q)＝[g1(q)g2(q)]^t＝[50.96cosq1+25.48cos(q1+q2)25.48cos(q1+q2)]^t为机器人重力项矩阵；

s2设计二自由度关节机器人的端口受控哈密顿控制器与pd控制器

s2.1设计端口受控哈密顿控制器

s2.1.1能量耗散概念的端口受控哈密顿控制系统模型为：

其中，x、τpch和y分别表示端口受控哈密顿控制系统的状态向量、输入向量和输出向量；r(x)为半正定对称矩阵，r(x)＝r^t(x)≥0，它反映了端口受控哈密顿控制系统端口上的附加阻性结构；j(x)为反对称矩阵，j(x)＝-j^t(x)，它反映了端口受控哈密顿控制系统内部的互联结构；h(x)为hamiltonian函数，它反映了端口受控哈密顿控制系统总的能量存储；g(x)反映了端口受控哈密顿控制系统的端口特性；

s2.1.2建立二自由度关节机器人的端口受控哈密顿控制系统模型

二自由度关节机器人的端口受控哈密顿控制系统的能量函数h(q,p)为：

其中，表示端口受控哈密顿控制系统的动能，u(q)表示端口受控哈密顿控制系统的势能；表示端口受控哈密顿控制系统的广义动量矢量，p1、p2分别表示关节1、关节2的广义动量；

选取端口受控哈密顿控制系统的状态向量为x＝[qp]^t＝[q1q2p1p2]^t，将二自由度关节机器人的端口受控哈密顿控制系统模型写为：

其中，为输入矩阵；

τ1-pch、τ2-pch分别表示端口受控哈密顿控制系统对关节1、关节2输出的控制力矩；

s2.1.3设计二自由度关节机器人的端口受控哈密顿控制器

假设端口受控哈密顿控制系统期望的平衡点为xd＝[qdpd]^t；

其中，qd＝[q1dq2d]^t表示期望的关节角位移矢量，q1d、q2d分别表示关节1、关节2的期望角位移，分别表示关节1、关节2的期望角速度；pd＝[p1dp2d]^t＝[00]^t表示期望的广义动量矢量，p1d、p2d分别表示关节1、关节2的期望广义动量；

为将端口受控哈密顿控制系统渐近的稳定在期望的平衡点xd附近，构造一个加入反馈控制后的闭环系统：

其中，jd(x)为期望的互联矩阵，且rd(x)为期望的阻尼矩阵，且

该闭环系统在期望的平衡点xd处取极小值；

选取期望的hamiltonian函数为：

其中，hd(xd)＝0；

为期望惯性矩阵，a1、a2、a3为设计参数；

为比例增益，kp_pch1和kp_pch2分别为常数；

配置满足：

其中，为微分系数，kd_pch1、kd_pch2为设计参数；

由于公式(2)与公式(5)均是对二自由度关节机器人的端口受控哈密顿控制系统的描述，将公式(2)与公式(5)中的消掉可得：

进一步得到：

解方程可得端口受控哈密顿控制器为：

其中：

s2.2设计pd控制器

设计二自由度关节机器人的pd控制器为：

其中，τpd＝[τ1-pdτ2-pd]^t为pd控制器对关节1、关节2的控制转矩；

为比例增益矩阵；

为微分增益矩阵；

e＝[e1e2]^t＝[q1-q1dq2-q2d]^t为位置误差矢量；

为速度误差矢量；

s3设计端口受控哈密顿控制系统与pd控制系统的协调控制器，如图3所示：

定义c1-pch,c2-pch分别为端口受控哈密顿控制系统的协调函数，c1-pd,c2-pd分别为pd控制系统的协调函数；

则端口受控哈密顿控制系统与pd控制系统的协调控制函数可设计为：

其中，tc为协调时间常数，c1-pch∈[0,1]，c2-pch∈[0,1]，c1-pd∈[0,1]，c2-pd∈[0,1]；

二自由度关节机器人端口受控哈密顿控制系统与pd控制系统的协调控制器为：

其中，τ1、τ2为协调控制器对关节1、关节2的转矩输出；

s4基于端口受控哈密顿控制器与pd控制器的二自由度关节机器人运动控制系统的稳定性分析

s4.1二自由度关节机器人的端口受控哈密顿控制系统稳定性分析

由公式(6)所描述的期望端口受控哈密顿控制系统的能量函数可知hd(x)＞0；

对公式(6)求导，并将公式(5)代入可得：

由于jd(x)为反对称矩阵、rd(x)为半正定矩阵，根据lasalle定理，若包括在集合中的闭环系统最大不变集为{xd}，则端口受控哈密顿控制系统在平衡点xd处是渐进稳定的，其中，rⁿ表示n维实数向量；

s4.2二自由度关节机器人的pd控制系统稳定性分析

依据pd控制系统的状态变量e＝[e1e2]^t，将公式(11)代入到公式(1)中可得：

由公式(15)可得为pd控制系统唯一静态平衡点；

定义李雅普诺夫函数为：

其中，u(qd)表示端口受控哈密顿控制系统在期望平衡点处的势能；

由于：

公式(1)描述的机器人动力学模型，对于任意给定的足够小的正常数a，对角正定比例增益矩阵kp_pd为可使下面二式同时成立：

e^tg(q)+e^tkp_pde≥a||e||²(19)

将公式(17)和公式(18)代入公式(16)，可得：

公式(1)描述的机器人动力学模型，惯性矩阵m(q)为对称、正定、有界，选择微分增益矩阵kd_pd足够大，使得：

kd_pd-2m(q)＞0(21)

kd_pd-m(q)-εcmi＞0(22)

其中，ε、cm为正常数，i为单位矩阵；

由公式(20)和公式(21)可得李雅普诺夫函数是正定的；

对公式(16)求导，可得：

由于将由公式(15)求得的代入公式(23)中可得：

公式(1)描述的机器人动力学模型，为反对称矩阵，使得：

公式(24)进一步写为：

假设存在ε＞0，使得||e||≤ε，对于公式(1)描述的机器人动力学模型，存在下列关系：

将公式(19)和公式(26)代入公式(25)可得：

由公式(22)和公式(26)可得

正定，半负定，由李雅普诺夫稳定性理论可知二自由度关节机器人的pd控制系统是渐进稳定的；

s4.3整个协调控制系统稳定性分析

二自由度关节机器人整个协调控制系统的李雅普诺夫控制函数可写为：

v＝vpch+vpd(28)

当时间t＝0时，c1-pch(t)＝c2-pch(t)＝0,c1-pd(t)＝c2-pd(t)＝1，只有pd控制系统作用于协调控制系统，v＝vpd＞0，根据李雅普诺夫稳定性理论可知协调控制系统稳定；

当时间0＜t＜∞时，c1-pch,c2-pch,c1-pd,c2-pd均为大于0小于1的常数，由于时间连续，属于共同控制；由于控制器各变量类型没有发生变化，结合端口受控哈密顿控制系统和pd控制系统稳定性分析可知，v正定，半负定，因此协调控制系统是渐进稳定的；

当时间t→∞时，c1-pch(t)＝c2-pch(t)＝1,c1-pd(t)＝c2-pd(t)＝0，只有端口受控哈密顿控制系统作用于整个控制系统，v＝vpch＞0，协调控制系统稳定；

由以上分析可知，整个协调控制系统是渐进稳定的。

针对端口受控哈密顿与pd控制的二自由度关节机器人运动系统，采用matlab/simulink进行仿真，以验证该方法对机器人轨迹跟踪的控制性能。

仿真所用参数如下：端口受控哈密顿控制系统中：kp_pch1＝2000，kp_pch2＝2000，kd_pch1＝2000，kd_pch2＝2000，a1＝0.05，a2＝0.0001，a3＝0.0001；pd控制系统中：kp_pd1＝280，kp_pd2＝280，kd_pd1＝280，kd_pd2＝280。

给定关节1和关节2的期望位置信号分别为单位阶跃信号。

由图4和图5可知，协调时间常数分别为tc1＝0.1，tc2＝0.2，tc3＝0.6，当tc1＝0.1时的响应速度最快，并且控制精度最高，控制效果最好，因此在协调控制仿真实验中取tc1＝0.1。

由图6和图7可知，pd控制的响应快速性良好，但是出现了明显的超调量，存在一定的稳态误差；端口受控哈密顿控制轨迹跟踪效果良好，控制精度较好，但是与pd控制相比较，其响应速度有待提高。

对比图8和图9中的四条轨迹曲线可知，协调控制系统在符合判断条件时能及时的投入到系统的协调控制中，保证了系统的动态性能。由图8和图9可知，与端口受控哈密顿和pd控制相比较，协调控制系统的控制精度为10^-3，控制精度有明显提高。

经比较可知，该协调控制方法有效的结合了pd控制和端口受控哈密顿控制的优点，系统既能快速的跟踪给定轨迹信号，又有很好的稳态性能，满足设计的要求。

当然，以上说明仅仅为本发明的较佳实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本说明书的教导下，所做出的所有等同替代、明显变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。