[发明专利]二自由度关节机器人轨迹的协调控制方法

摘要

本发明公开了一种二自由度关节机器人轨迹的协调控制方法，该控制方法将端口受控哈密顿与PD控制相结合，采用指数函数作为两者协调控制函数，通过调节两种控制方法的控制力度，使得PD控制在最初的作用力度较大，使系统具有良好的快速跟踪性能；而端口受控哈密顿控制在稳态时的作用力度较大，使系统具有较好的稳态性能。本发明方法不仅使整个机器人运动系统在动态响应性能方面获得改善，系统能够很快地进入稳态运行，缩短了系统的上升时间与调节时间，也使系统的控制精度得到提高。由于这种协调控制方案使每种控制方法的优点都在相应的时间段内得到最高效的利用，因而具有较好的应用价值。

主权项

1.二自由度关节机器人轨迹的协调控制方法，其特征在于，包括如下步骤：s1建立二自由度关节机器人的动力学模型在D‑H坐标系中，依据拉格朗日方法推导出二自由度关节机器人的动力学模型为：其中，τ＝[τ₁ τ₂]^T为输入向量，τ₁、τ₂分别表示关节1、关节2的控制力矩；q＝[q₁ q₂]^T表示关节角位移矢量，q₁、q₂分别表示关节1、关节2的角位移；表示关节角速度矢量，分别表示关节1、关节2的角速度；分别表示关节1、关节2的角加速度；为机器人惯性矩阵；为机器人哥氏力与向心力矩阵；G(q)＝[G₁(q) G₂(q)]^T＝[50.96cosq₁+25.48cos(q₁+q₂) 25.48cos(q₁+q₂)]^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)表示端口受控哈密顿控制系统的势能；表示端口受控哈密顿控制系统的广义动量矢量，p₁、p₂分别表示关节1、关节2的广义动量；选取端口受控哈密顿控制系统的状态向量为x＝[q p]^T＝[q₁ q₂ p₁ p₂]^T，将二自由度关节机器人的端口受控哈密顿控制系统模型写为：其中，为输入矩阵；τ_1‑PCH、τ_2‑PCH分别表示端口受控哈密顿控制系统对关节1、关节2输出的控制力矩；s2.1.3设计二自由度关节机器人的端口受控哈密顿控制器假设端口受控哈密顿控制系统期望的平衡点为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的期望广义动量；为将端口受控哈密顿控制系统渐近的稳定在期望的平衡点x_d附近，构造一个加入反馈控制后的闭环系统：其中，J_d(x)为期望的互联矩阵，且R_d(x)为期望的阻尼矩阵，且该闭环系统在期望的平衡点x_d处取极小值；选取期望的Hamiltonian函数为：其中，H_d(x_d)＝0；为期望惯性矩阵，a₁、a₂、a₃为设计参数；为比例增益，K_{P_PCH1}和K_{P_PCH2}分别为常数；配置满足：其中，为微分系数，K_{D_PCH1}、K_{D_PCH2}为设计参数；由于公式(2)与公式(5)均是对二自由度关节机器人的端口受控哈密顿控制系统的描述，将公式(2)与公式(5)中的消掉可得：进一步得到：解方程可得端口受控哈密顿控制器为：其中：s2.2设计PD控制器设计二自由度关节机器人的PD控制器为：其中，τ_PD＝[τ_1‑PD τ_2‑PD]^T为PD控制器对关节1、关节2的控制转矩；为比例增益矩阵；为微分增益矩阵；e＝[e₁ e₂]^T＝[q₁‑q_1d q₂‑q_2d]^T为位置误差矢量；为速度误差矢量；s3设计端口受控哈密顿控制系统与PD控制系统的协调控制器定义c_1‑PCH,c_2‑PCH分别为端口受控哈密顿控制系统的协调函数，c_1‑PD,c_2‑PD分别为PD控制系统的协调函数；则端口受控哈密顿控制系统与PD控制系统的协调控制函数能够设计为：其中，T_C为协调时间常数，c_1‑PCH∈[0,1]，c_2‑PCH∈[0,1]，c_1‑PD∈[0,1]，c_2‑PD∈[0,1]；二自由度关节机器人端口受控哈密顿控制系统与PD控制系统的协调控制器为：其中，τ₁、τ₂为协调控制器对关节1、关节2的转矩输出；s4基于端口受控哈密顿控制器与PD控制器的二自由度关节机器人运动控制系统的稳定性分析s4.1二自由度关节机器人的端口受控哈密顿控制系统稳定性分析由公式(6)所描述的期望端口受控哈密顿控制系统的能量函数可知H_d(x)＞0；对公式(6)求导，并将公式(5)代入可得：由于J_d(x)为反对称矩阵、R_d(x)为半正定矩阵，根据Lasalle定理，若包括在集合中的闭环系统最大不变集为{x_d}，则端口受控哈密顿控制系统在平衡点x_d处是渐进稳定的，其中，Rⁿ表示n维实数向量；s4.2二自由度关节机器人的PD控制系统稳定性分析依据PD控制系统的状态变量e＝[e₁ e₂]^T，将公式(11)代入到公式(1)中可得：由公式(15)可得为PD控制系统唯一静态平衡点；定义李雅普诺夫函数为：其中，U(q_d)表示端口受控哈密顿控制系统在期望平衡点处的势能；由于：公式(1)描述的机器人动力学模型，对于任意给定的足够小的正常数a，对角正定比例增益矩阵K_{P_PD}为可使下面二式同时成立：e^TG(q)+e^TK_{P_PD}e≥a||e||²         (19)将公式(17)和公式(18)代入公式(16)，可得：公式(1)描述的机器人动力学模型，惯性矩阵M(q)为对称、正定、有界，选择微分增益矩阵K_{D_PD}足够大，使得：K_{D_PD}‑2M(q)＞0        (21)K_{D_PD}‑M(q)‑εC_MI＞0         (22)其中，ε、C_M为正常数，I为单位矩阵；由公式(20)和公式(21)可得李雅普诺夫函数是正定的；对公式(16)求导，可得：由于将由公式(15)求得的代入公式(23)中可得：公式(1)描述的机器人动力学模型，为反对称矩阵，使得：公式(24)进一步写为：假设存在ε＞0，使得||e||≤ε，对于公式(1)描述的机器人动力学模型，存在下列关系：将公式(19)和公式(26)代入公式(25)可得：由公式(22)和公式(26)可得正定，半负定，由李雅普诺夫稳定性理论可知二自由度关节机器人的PD控制系统是渐进稳定的；s4.3整个协调控制系统稳定性分析二自由度关节机器人整个协调控制系统的李雅普诺夫控制函数可写为：V＝V_PCH+V_PD             (28)当时间t＝0时，c_1‑PCH(t)＝c_2‑PCH(t)＝0,c_1‑PD(t)＝c_2‑PD(t)＝1，只有PD控制系统作用于协调控制系统，V＝V_PD＞0，根据李雅普诺夫稳定性理论可知协调控制系统稳定；当时间0＜t＜∞时，c_1‑PCH,c_2‑PCH,c_1‑PD,c_2‑PD均为大于0小于1的常数，由于时间连续，属于共同控制；由于控制器各变量类型没有发生变化，结合端口受控哈密顿控制系统和PD控制系统稳定性分析可知，V正定，半负定，因此协调控制系统是渐进稳定的；当时间t→∞时，c_1‑PCH(t)＝c_2‑PCH(t)＝1,c_1‑PD(t)＝c_2‑PD(t)＝0，只有端口受控哈密顿控制系统作用于整个控制系统，V＝V_PCH＞0，协调控制系统稳定；由以上分析可知，整个协调控制系统是渐进稳定的。