[发明专利]一种全状态约束机械臂轨迹跟踪的协同控制方法

权利要求书

1.一种全状态约束机械臂轨迹跟踪的协同控制方法，其特征在于：包括以下步骤：

步骤1，建立全状态约束的机械臂系统动力学方程：

式中，q为角位置向量，且n为所述机械臂的关节数；是角速度向量，且和分别表示惯性矩阵、科氏力矩阵、重力向量；为控制力矩，为控制输入变量，记为u_b，且有

对于任意的t≥0，q及均需满足约束条件：

其中，k_uj、k_vj为常向量，且有k_uj＝[k_uj1，k_uj2，…，k_ujn]^T，k_vj＝[k_vj1，k_vj2,…，k_vjn]^T，j＝1，2；

步骤2，令x₁＝q，u_b＝τ，代入式(1)，可得所述全状态约束的机械臂系统状态空间方程为：

令b＝M^-1(x₁)(u_b-C(x₁，x₂)x₂-G(x₁))，则上式可写为：

其约束条件为：

步骤3，设计基于非对称障碍李雅普诺夫函数的反推控制器，以提升初始时刻系统的响应速度，同时避免非对称全状态约束被破坏；对于式(3)，控制器的设计步骤为：

3-1，令z₁＝x₁-q_d＝[z₁₁，z₁₂,...,z_1n]^T，z₂＝x₂-α＝[z₂₁，z₂₂，...，z_2n]^T，其中q_d为给定目标轨迹，且q_d＝[q_d1，q_d2，...，q_dn]^T，α为镇定函数，且α＝[α₁，α₂，...，α_n]^T；

3-2，对于z₁子系统，构造非对称障碍李雅普诺夫函数为：

其中，k_a、k_b均为正的常向量，且有-k_a、k_b分别是z₁的上界、下界；

定义：

则V₁的导数为：

其中，镇定函数α设计为：

式中，k_1i为正常数，i＝1，2，…，n；

将式(6)代入式(5)，得到：

3-3，对于z₂子系统，构造非对称障碍李雅普诺夫函数为：

其中，k_c、k_d均为正的常向量，且有-k_c、k_d分别是z₂的上界、下界；

定义：

则V₂的导数为：

其中，z₂的导数为：

当z₂≠[0，0，…，0]^T时，选定反推控制律u_b为：

式中，表示的广义逆矩阵，它满足k_2i为正常数，i＝1，2，…n；

将式(10)、(11)代入式(9)，得：

步骤4，将所述机械臂系统式(1)转化为端口汉密尔顿结构，并通过互联与阻尼分配及能量整形原则，设计基于Hamilton能量理论的控制器，以提高系统响应后期的跟踪稳定性；控制器的设计步骤为：

4-1，对于式(1)，考虑一个Hamilton函数H(q，p)＝K(q，p)+P(q)，其中表示广义动量，是系统的动能，为系统的虚拟势能，则有：

并且p的导数满足：

4-2，预置反馈控制律为：

其中，为常正定矩阵，为Hamilton控制输入；

4-3，将式(15)代入式(1)，并由式(13)、(14)可得所述机械臂系统式(1)的端口汉密尔顿系统描述：

式中，H(X)为Hamilton函数，是列满秩的，I_n为单位矩阵；

4-4，定义期望平衡点为状态误差，规定一个期望的Hamilton能量函数且H_d(0)＝0，则选定Hamilton控制律u_h为：

式中，J_a为互联矩阵，R_a为阻尼矩阵；

步骤5，设计协同控制器，具体方法为：

令

式中，c_bi(t)∈(0,1]，c_hi(t)∈[0,1)，i＝1,2,…,n分别表示反推控制器式(11)和Hamilton控制器式(17)对应的协调函数，T_s是时间常数；

则协同控制律u_s为：

式中，

u_si＝c_bi(t)u_bi+c_hi(t)u_hi，i＝1,2,…,n (20)

在0t∞时，整个系统的李雅普诺夫函数为：

其导数为：

式(19)、式(20)就构成了所述全状态约束机械臂轨迹跟踪的协同控制器。