[发明专利]二自由度SCARA机器人的位置跟踪控制方法

摘要

本发明公开了一种二自由度SCARA机器人的位置跟踪控制方法，该控制方法针对二自由度SCARA机器人使用单一控制方法难以实现快速、精准的位置跟踪控制的问题，设计了基于端口受控哈密顿与反步法的协调控制方案，其中，反步法控制在最初时刻起主要作用，以使系统具有良好的快速性；端口受控哈密顿控制在稳态时起主要作用，以使系统具有较好的稳态性能，本发明提出的协调控制方案能够使每种方法的优点在相应时间点得到充分利用，该方案在控制形式上的改变，使其具有较高的应用价值和实际意义。经仿真验证表明，本发明方法能够提高系统的动态性能和稳态性能，且抑制干扰能力强。

主权项

1.二自由度SCARA机器人的位置跟踪控制方法，其特征在于，包括如下步骤：s1建立二自由度SCARA机器人的动力学模型根据D‑H坐标法，推导出二自由度SCARA机器人的动力学模型为：其中，τ＝[τ₁ τ₂]^T为输入向量，τ₁、τ₂分别表示关节1、关节2的控制力矩；q＝[q₁ q₂]^T表示关节角位移矢量，q₁、q₂分别表示关节1、关节2的角位移；表示关节角速度矢量，分别表示关节1、关节2的角速度；分别表示关节1、关节2的角加速度；为机器人惯性矩阵；为机器人哥氏力与向心力矩阵；s2设计二自由度SCARA机器人的端口受控哈密顿控制器与反步法控制器s2.1设计端口受控哈密顿控制器s2.1.1能量耗散的端口受控哈密顿控制系统模型为：其中，x、τ_PCH和y分别表示端口受控哈密顿控制系统的状态向量、输入向量和输出向量；H(x)表示端口受控哈密顿控制系统的能量函数；R(x)为半正定对称矩阵，R(x)＝R^T(x)，它反映了端口受控哈密顿控制系统端口上的附加阻性结构；J(x)为反对称矩阵，J(x)＝‑J^T(x)，它反映了端口受控哈密顿控制系统内部的互联结构；g(x)反映了端口受控哈密顿控制系统的端口特性；s2.1.2建立二自由度SCARA机器人的端口受控哈密顿控制系统模型二自由度SCARA机器人的端口受控哈密顿控制系统的能量函数H(q,p)为：其中，表示端口受控哈密顿控制系统的动能，U(q)表示端口受控哈密顿控制系统的势能；表示端口受控哈密顿控制系统的广义动量矢量，p₁、p₂分别表示关节1、关节2的广义动量；选取端口受控哈密顿控制系统的状态向量为x＝[q p]^T＝[q₁ q₂ p₁ p₂]^T，将二自由度SCARA机器人的端口受控哈密顿控制系统模型写为：其中，为输入矩阵；τ_1‑PCH表示端口受控哈密顿控制系统对关节1输出的控制力矩，τ_2‑PCH表示端口受控哈密顿控制系统对关节2输出的控制力矩；s2.1.3设计二自由度SCARA机器人的端口受控哈密顿控制器确定二自由度SCARA机器人期望的平衡点为x_d＝[q_d p_d]^T；其中，q_d＝[q_1d q_2d]^T表示期望的关节角位移矢量，q_1d、q_2d分别表示关节1、关节2的期望角位移，分别表示关节1、关节2的期望角速度；p_d＝[p_1d p_2d]^T＝[0 0]^T表示期望的广义动量矢量，p_1d、p_2d分别表示关节1、关节2的期望广义动量；构造一个加入反馈控制后的闭环期望端口受控哈密顿控制系统的能量函数H_d(x)，使公式(2)所描述的系统渐进地稳定在期望的平衡点x_d附近，且闭环系统能够写为：选取闭环期望端口受控哈密顿控制系统的能量函数为：其中，H_d(x_d)＝0；为期望惯性矩阵，a₁、a₂、a₃为设计参数；为比例增益，K_P1和K_P2分别为常数；配置满足：其中，J_d(x)为期望的互联矩阵，且R_d(x)为期望的阻尼矩阵，且为微分系数，K_V1、K_V2为设计参数；由于公式(2)与公式(5)均是对二自由度SCARA机器人的端口受控哈密顿控制系统的描述，将公式(2)与公式(5)中的消掉可得：进一步得到：解方程可得端口受控哈密顿控制器为：其中：s2.1.4二自由度SCARA机器人的端口受控哈密顿控制系统稳定性分析由公式(6)所描述的期望端口受控哈密顿控制系统的能量函数可知H_d(x)＞0；对公式(6)求导，并将公式(5)代入可得：由于J_d(x)为反对称矩阵、R_d(x)为半正定矩阵，根据Lasalle定理，若包括在集合中的闭环系统最大不变集为{x_d}，则端口受控哈密顿控制系统在平衡点x_d处是渐进稳定的，其中，Rⁿ表示n维实数向量；s2.2设计反步法控制器s2.2.1设计二自由度SCARA机器人的反步法控制器定义变量x₁＝[q₁ q₂]^T，则二自由度SCARA机器人的动力学模型表示为：其中，τ_BS＝[τ_{1_BS} τ_{2_BS}]^T为反步法控制系统输出力矩，τ_{1_BS}为反步法控制系统为关节1提供的控制力矩，τ_{2_BS}为反步法控制系统为关节2提供的控制力矩；定义二自由度SCARA机器人的反步法控制系统输出误差变量为：e₁＝x₁‑x_1d＝[q₁‑q_1d q₂‑q_2d]^T               (13)其中，x_1d＝[q_1d q_2d]^T为期望的关节角位移矢量；对公式(13)两边求导，可得：为了确保x₁能有效跟踪期望信号x_1d，反步法控制系统的第一个子系统选取李雅普诺夫控制函数如下：对公式(15)求导得：选取虚拟控制函数：其中，为增益矩阵，k₁₁,k₁₂为设计参数；将公式(17)作为x₂代入公式(16)中，可得：定义虚拟控制误差变量为：其中，为期望的关节角速度矢量；反步法控制系统的第二个子系统选取李雅普诺夫函数为：为使二自由度SCARA机器人反步法控制系统稳定，必须设计τ_BS使负定，为此选取τ_BS为：其中，为增益矩阵，k₂₁，k₂₂为设计参数；进一步整理可得反步法控制器为：其中，k_b11＝‑M₁₁k₂₁k₁₁，k_b12＝‑M₁₂k₂₂k₁₂，k_s11＝(C₁₁‑M₁₁k₂₁‑M₁₁k₁₁)，k_s12＝(C₁₂‑M₁₂k₂₂‑M₁₂k₁₂)；k_b21＝‑M₂₁k₂₁k₁₁，k_s21＝(C₂₁‑M₂₁k₂₁‑M₂₁k₁₁)，k_s22＝(‑M₂₂k₂₂‑M₂₂k₁₂)；b₁＝M₁₁k₂₁k₁₁q_1d+M₁₂k₂₂k₁₂q_2d，b₂＝M₂₁k₂₁k₁₁q_1d+M₂₂k₂₂k₁₂q_2d；s2.2.2二自由度SCARA机器人的反步法控制系统稳定性分析对公式(19)两边求导，并将公式(12)、公式(21)代入可得：对公式(20)求导，并将公式(18)、公式(23)代入整理可得：由公式(20)可知V₂为正定、公式(24)可知为半负定，根据李雅普诺夫稳定性理论可知，二自由度SCARA机器人的反步法控制系统是渐进稳定的；s3设计端口受控哈密顿控制系统与反步法控制系统的协调控制器s3.1构建端口受控哈密顿控制系统与反步法控制系统的协调控制器定义c_1‑PCH，c_2‑PCH为端口受控哈密顿控制系统的协调函数，定义c_1‑BS,c_2‑BS为反步法控制系统的协调函数；则端口受控哈密顿控制系统与反步法控制系统的协调控制函数可设计为：其中，T_C为协调时间常数，c_1‑PCH∈[0,1]，c_2‑PCH∈[0,1]，c_1‑BS∈[0,1]，c_2‑BS∈[0,1]；二自由度SCARA机器人的端口受控哈密顿控制系统与反步法控制系统的协调控制器为：s3.2基于端口受控哈密顿控制与反步法控制的二自由度SCARA机器人位置跟踪控制系统的稳定性分析二自由度SCARA机器人整个协调控制系统的李雅普诺夫控制函数能够写为：V＝V_PCH+V₂                (27)当时间t＝0时，c_1‑PCH(t)＝c_2‑PCH(t)＝0、c_1‑BS(t)＝c_2‑BS(t)＝1，只有反步法控制系统作用于整个协调控制系统，V＝V₂＞0，根据李雅普诺夫稳定性理论可知整个协调控制系统稳定；当时间0＜t＜∞时，c_1‑PCH(t)、c_2‑PCH(t)、c_1‑BS(t)、c_2‑BS(t)均为大于0小于1的常数，属于共同控制，且随着时间的增加，反步法控制系统的作用力度逐渐减小，端口受控哈密顿控制系统的作用力度逐渐增加；由于两个控制系统的类型没有变化，结合端口受控哈密顿控制系统和反步法控制系统稳定性分析可知，V正定，半负定，因此整个协调控制系统是渐进稳定的；当时间t→∞时，c_1‑PCH(t)＝c_2‑PCH(t)＝1，c_1‑BS(t)＝c_2‑BS(t)＝0，只有端口受控哈密顿控制系统作用于整个协调控制系统，V＝V_PCH＞0，整个协调控制系统稳定；由以上分析可知，整个协调控制系统是渐进稳定的。