CN108519740A - 一种全状态约束机械臂轨迹跟踪的协同控制方法 - Google Patents

Abstract

本发明涉及一种全状态约束机械臂轨迹跟踪的协同控制方法，属于控制工程技术领域。该方法将基于ABLF反推控制和基于Hamilton能量理论控制相结合，构建协同控制器：建立n个关节约束的机械臂系统动力学方程，对之进行坐标变换，得到该系统状态空间方程；设计基于ABLF的反推控制器，以提升系统初始响应速度，同时避免约束被破坏；设计基于Hamilton能量理论的控制器，以提高系统响应后期的跟踪稳定性；设计协同控制器，实现对全状态约束机械臂系统快速稳定控制。本发明在保证全状态约束条件不被破坏的前提下，能提升系统的动态和稳态性能，确保机械臂系统响应速度快、定位精度高，可有效提高机械臂在机器人系统中的工作性能。

Description

一种全状态约束机械臂轨迹跟踪的协同控制方法

技术领域

本发明涉及一种控制方法，尤其是一种全状态约束机械臂轨迹跟踪的协同控制方法，属于控制工程技术领域。

背景技术

机器人所用机械臂是一个非线性系统，其状态如位置、速度等受到各种约束条件的限制。这些实际约束一旦破坏，会严重降低系统性能，甚至危害系统。因此在实际系统中考虑约束是十分必要的。目前，非线性系统的约束控制问题已得到了广泛关注，解决这类问题的方法主要有基于障碍李雅普诺夫函数(BLF)的反推控制法和基于Hamilton(汉密尔顿)能量理论的控制法。

基于BLF的反推控制法可以通过设计每个子系统的Lyapunov(李雅普诺夫)函数和镇定函数，最终回馈到整个系统以保证系统的稳定性，同时具有快速的动态响应。目前，基于BLF的反推控制法已经应用在约束机器人系统中，有的利用BLF解决了不确定状态约束机器人系统的跟踪问题，这对进一步研究具有指导意义。

端口汉密尔顿(PCH)系统是一类重要的非线性系统，其Hamilton函数代表了系统的总能量，在稳定性分析中可作为一个良好的候选Lyapunov函数，因此，基于Hamilton能量理论的控制法在机器人控制领域得到了广泛关注。

然而，对于机器人系统，基于BLF的反推控制法虽然考虑到了实际约束对系统的影响，但存在跟踪精度低、稳态性能差等问题；而单纯的Hamilton控制法没有充分考虑由机械臂本身带来的位置和速度的实际约束，系统的动态性能仍需进一步完善。

发明内容

本发明的主要目的在于：针对现有技术的不足和空白，本发明将基于非对称障碍李雅普诺夫函数(ABLF)反推控制和基于Hamilton能量理论控制相结合，提供一种全状态约束机械臂轨迹跟踪的协同控制方法，发挥它们各自优势，在保证全状态约束条件不被违反的前提下，可以提升系统的动态和稳态性能，确保机械臂系统不仅具有快速的跟踪速度，而且具有较小的稳态误差。

为了达到以上目的，针对n个关节约束的机械臂，本发明一种全状态约束机械臂轨迹跟踪的协同控制方法，包括以下步骤：

步骤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)，可得所述全状态约束的机械臂系统状态空间方程为：

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

其约束条件为：

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

式中， 表示的广义逆矩阵，它满足z₂≠[0,0,…,0]^T，k_2i为正常数，i＝1,2,…n；

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

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

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

并且p的导数满足：

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

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

4-3，将式(15)代入式(1)，并由式(13)、(14)可得机械臂系统式(1)的PCH系统描述：

式中，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)

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

其导数为：

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

本发明的有益效果是：1)本发明所提基于非对称障碍李雅普诺夫函数(ABLF)控制方法可以提升系统初始响应速度，同时又可避免非对称全状态约束被破坏。2)所提基于Hamilton能量理论的控制方法可以提高系统响应后期的稳态性能。3)本发明充分利用上述两种方法各自的优势，控制算法简单，同时能确保系统具有良好的动态性能和较小的稳态误差。

附图说明

图1为本发明实施例两关节机械臂示意图。

图2为基于ABLF反推控制下两关节机械臂的角位置q(t)的轨迹跟踪仿真曲线。

图3为基于ABLF反推控制下两关节机械臂的角速度的仿真曲线。

图4为基于ABLF反推控制下两关节机械臂的输入力矩u_b(t)的仿真曲线。

图5为基于Hamilton能量理论控制下两关节机械臂的角位置q(t)的轨迹跟踪仿真曲线。

图6为基于Hamilton能量理论控制下两关节机械臂的角速度的仿真曲线。

图7为基于Hamilton能量理论控制下两关节机械臂的输入力矩u_h(t)的仿真曲线。

图8为本发明协同控制下两关节机械臂的角位置q(t)的轨迹跟踪仿真曲线。

图9为本发明协同控制下两关节机械臂的角速度的仿真曲线。

图10为本发明协同控制下两关节机械臂的输入力矩u_s(t)的仿真曲线。

图中标号：1-第一连杆，2-第二连杆，3-第一关节，4-第二关节。

具体实施方式

下面结合附图，对本发明作进一步详细说明。

以一个关节约束数n＝2的机械臂为例，如图1所示，该机械臂包括两个连杆：第一连杆1、第二连杆2，和两个约束关节：第一关节3、第二关节4。第一连杆1的质量和长度分别为m₁、l₁，第二连杆2的质量和长度分别为m₂、l₂；第一关节3到第一连杆1重心的距离为l_c1，第二关节4到第二连杆2重心的距离为l_c2；第一连杆1、第二连杆2的转动惯量分别为J₁、J₂；q₁为第一连杆1的角位置，q₂为第二连杆2的角位置。规定逆时针方向为机械臂的正转方向。

本发明一种全状态约束机械臂轨迹跟踪的协同控制方法，为了实现上述机械臂平稳运行，具体包括以下步骤：

步骤1，建立该机械臂的动力学方程：

式中，为角位置向量；为角速度向量，分别是第一连杆1、第二连杆2的角速度向量；和分别表示惯性矩阵、科氏力矩阵、重力向量；为控制力矩，为控制输入变量，且有：

其中， G₁＝(m₁l_c2+m₂l₁)g cos q₁+m₂l_c2g cos(q₁+q₂)，G₂＝m₂l_c2g cos(q₁+q₂)，g为重力加速度，g＝9.80665m/s²。

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

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

步骤2，令x₁＝q，u_b＝τ，代入式(21)，可得该机械臂的状态空间方程为：

式中，

其约束条件为：

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

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

3-2，对于z₁子系统，构造ABLF为：

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

定义：

则V₁的导数为：

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

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

将式(26)代入式(25)，得到：

3-3，对于z₂子系统，构造ABLF为：

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

定义：

则V₂的导数为：

其中，z₂的导数为：

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

式中， 表示的广义逆矩阵，它满足z₂≠[0,0,…,0]^T，k_2i为正常数，i＝1,2；

将式(30)、(31)代入式(29)，得：

步骤4，设计基于Hamilton能量理论的控制器，以提高跟踪响应后期的稳定性；控制器的设计步骤为：

4-1，对于式(21)，选择一个Hamilton函数：

其中，p＝[p₁,p₂]^T，

则有：

并且p的导数满足：

4-2，选定预反馈律：

其中，K_D＝diag{k_d1,k_d2}，为Hamilton输入。

4-3，将式(35)代入式(21)，并由式(33)、(34)可得该两关节机械臂式(21)的PCH系统结构：

式中，

4-4，定义期望平衡点状态误差规定期望的能量函数其中，

且有H_d(0)＝0，则系统式(36)的Hamilton控制律u_h为：

式中，J_a为注入的互联矩阵，R_a为注入的阻尼矩阵。

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

令

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

则协同控制律u_s为：

式中，

u_si＝c_bi(t)u_bi+c_hi(t)u_hi，i＝1,2 (40)

式(39)、式(40)就构成了全状态约束机械臂轨迹跟踪的协同控制器。合理选择协调函数c_b1(t)、c_b2(t)，使得在系统响应初期，借助反推控制以提升收敛速度，同时保证满则非对称约束条件式(22)。随着时间的推移，逐渐增加基于Hamilton能量理论控制的强度以降低稳态误差，同时避免超调振荡的发生。这样就同步提高了动态和稳态性能，受控系统机械臂趋于稳定，且在响应后期约束也不会被破坏。由此可见，基于Hamilton能量理论控制在稳态响应中起主导作用。

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

其导数为：

下面用一个优选实施例对本发明做进一步的说明。

图1所示的机械臂系统，其参数如下：m₁＝2kg，m₂＝0.85kg，l₁＝0.35m，l₂＝0.31m，J₁＝6.125×10^-2kgm²，J₂＝2.042×10^-2kgm²。

基于以上系统参数，系统仿真条件为：

该机械臂的初始条件为：q₁(0)＝1，q₂(0)＝0，需要跟踪的目标轨迹为：q_d＝[q_d1,q_d2]^T＝[sin(3t),2cos(3t)]^T，其中t∈(0,6s)。

对于t∈(0,6s)，q(t)及均需满足约束条件式(22)。式(22)中，k_v1＝[k_v11,k_v12]^T＝[1.21,2.21]^T，k_u1＝[k_u11,k_u12]^T＝[-1.17,-2.17]^T，k_v2＝[k_v21,k_v22]^T＝[3.2,6.2]^T，k_u2＝[k_u21,k_u22]^T＝[-3.18,-6.18]^T。

按上述仿真条件，对系统进行仿真，以此验证系统的轨迹跟踪能力。

1)基于ABLF的反推控制

镇定函数式(26)和反推控制器式(31)中的参数分别取为：k₁₁＝1.2，k₁₂＝1.1，k₂₁＝2.4，k₂₂＝1.8。仿真结果如图2、图3、图4所示。

图2为基于ABLF反推控制下该机械臂的角位置q(t)的轨迹跟踪仿真曲线，图中，虚线曲线表示期望目标角位置，实线曲线表示实际角位置。

图3为基于ABLF反推控制下该机械臂第一连杆1的角速度的仿真曲线和第二连杆2的角速度的仿真曲线。

图4所示为基于ABLF反推控制下该机械臂第一连杆1的输入力矩u_b1的仿真曲线(虚线曲线)和第二连杆2的输入力矩u_b2的仿真曲线(实线曲线)。

从图2、图3可以看出，在基于ABLF反推控制下，角位置q(t)大致能跟踪期望轨迹，且有较快的收敛速度，尤其是在系统响应初期；跟踪响应在稳态时存在偏差，但满足约束条件式(22)。

2)基于Hamilton能量理论的控制

式(35)中的K_D取值为：K_D＝diag{k_d1,k_d2}＝diag{2,2}，基于Hamilton能量理论控制中规定期望的能量函数其中的K_p取值为：K_p＝diag{k_p1,k_p2}＝diag{1.5,1.8}。仿真结果如图5、图6、图7所示。

图5为基于Hamilton能量理论控制下该机械臂的角位置q(t)的轨迹跟踪仿真曲线，图中，虚线曲线表示期望目标角位置，实线曲线表示实际角位置。图6为基于Hamilton能量理论控制下该机械臂第一连杆1的角速度的仿真曲线和第二连杆2的角速度的仿真曲线。

图7所示为基于Hamilton能量理论控制下该机械臂第一连杆1的输入力矩u_h1的仿真曲线(虚线曲线)和第二连杆2的输入力矩u_h2的仿真曲线(实线曲线)。

从图5、图6可以看出，在基于Hamilton能量理论控制下，角位置q(t)可以无静差跟踪期望轨迹，但q(t)和明显不满足其对应的约束条件。由此可见，基于Hamilton能量理论的控制具有良好的稳态性能，但比反推控制的响应速度慢。为此需要设计基于ABLF控制和基于Hamilton能量理论控制的协同控制器。

3)协同控制

将式(38)中的时间常数T_s取为1，仿真结果如图8、图9、图10所示。

图8为协同控制下该机械臂的角位置q(t)的轨迹跟踪仿真曲线，图中，虚线曲线表示期望目标角位置，实线曲线表示实际角位置。

图9为协同控制下该机械臂第一连杆1的角速度的仿真曲线和第二连杆2的角速度的仿真曲线。

图10为协同控制下该机械臂第一连杆1的输入力矩u_h1的仿真曲线(虚线曲线)和第二连杆2的输入力矩u_h2的仿真曲线(实线曲线)。

从图8可以看出，在协同控制下，角位置q(t)能快速跟踪期望轨迹q_d(t)，且稳态误差很小。从图8、图9可以看出，q(t)和都满足约束条件式(22)。

上述结果表明，本发明的协同控制方法能有效结合基于ABLF的反推控制和基于Hamilton能量理论的控制这两种控制方法的优势，响应速度快、定位精度高，具有理想的跟踪性能和较好的控制柔性。

Claims (1)

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)，可得所述全状态约束的机械臂系统状态空间方程为：

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

其约束条件为：

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

其中，i＝1,2,...,n；k_a、k_b均为正的常向量，且有 -k_a、k_b分别是z₁的上界、下界；

定义：

则V₁的导数为：

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

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

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

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

其中，i＝1,2,...,n；k_c、k_d均为正的常向量，且有 -k_c、k_d分别是z₂的上界、下界；

定义：

则V₂的导数为：

其中，z₂的导数为：

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

式中， 表示的广义逆矩阵，它满足z₂≠[0,0,…,0]^T，k_2i为正常数，i＝1,2,…n；

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

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

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

并且p的导数满足：

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

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

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

式中，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)

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

其导数为：

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