[发明专利]一种基于模糊控制和李雅普诺夫函数的平面欠驱动机械臂位置控制方法

权利要求书

1.一种基于模糊控制和李雅普诺夫函数的平面欠驱动机械臂位置控制方法，其特征在于，所述方法步骤包括：

(1)建立平面三自由度被动-主动-主动型(PAA型)欠驱动机械臂动力学模型，并分析其各关节角度、角速度之间的积分约束关系，具体为：

平面三自由度被动-主动-主动型(PAA型)欠驱动机械臂不受重力影响，且被动关节处可自由旋转，不具有制动力矩和摩擦力，将研究对象简化为一个刚体结构，根据拉格朗日方程，建立其动力学模型如下：

其中，M_ij∈R^3×3(i,j＝1,2,3)具体为，

a_k(k＝1,2,...,6)为系统的结构参数，q_i(i＝1,2,3)为机械臂的第i关节角，为机械臂的第i关节角加速度；

m_i为第i杆的质量(i＝1,2,3)，L_i为第i杆的长度(i＝1,2,3)，l_i为第i杆质心到前一关节的长度(i＝1,2,3)，J_i为第i杆的转动惯量(i＝1,2,3)；H_i∈R^3×1(i＝1,2,3)为哥氏力和离心力项，

τ＝(0,τ₂,τ₃)^T为关节力矩矢量，其中被动关节的输出力矩为0，为机械臂的第i关节角速度；

平面PAA型欠驱动机械臂是一阶非完整系统，平面PA型机械臂是完整系统；基于模型降阶的思想，将控制过程分为两个阶段，在第一阶段维持第三关节初始角度不变，控制第二关节到达目标角度；在第二阶段维持第二关节目标角度不变，控制第三关节到达目标角度；在式(1)中取出欠驱动所处行，并将式(4)代入可得：

利用分部积分，式(5)对时间t求定积分，可得：

在每个控制阶段维持角度不变，可近似认为关节角速度为零，因此，对式(6)两端再次求定积分，化简可得：

其中，q₁¹(0)表示第一阶段时被动杆初始角度，q₂(0)和q₃(0)分别表示第二杆和第三杆的初始角度，D₁,E₁,G₁,g₁为式(7)化简过程的中间项，

D₁＝(8a₃a₅-4a₂a₆-4a₁a₆-4a₄a₆)cosq₂(0)-a₁²-a₂²-4a₆²cos²q₂(0)-a₄²+4a₅²+4a₃²-2a₁a₂-2a₁a₄-2a₂a₄

G₁＝(a₁+a₂+a₄+2a₆cosq₂(0)-2a₅cosq₂(0)-2a₃)(cosq₂-1)+2a₅sinq₂(0)sinq₂

同理，在第二阶段的控制中维持第二关节角度不变，即可近似认为关节角速度为零，再次对式(6)两端求定积分可得到第二阶段关节角度约束；

(2)分析其可达空间，利用粒子群算法计算各驱动关节的目标角度，具体为：

以机械臂末端与目标位置的差值作为粒子群算法的适应度函数，计算出驱动关节的目标角度；

机械臂的工作空间是指机械臂末端执行器可以到达的空间位置的集合，将机械臂工作域记作A(p)，则机械臂各个关节角度与工作可达空间的映射可表示为：

A(p)＝{p(θ)|θ∈Q} (8)

其中Q为关节角的约束空间，以关节限位角作为机械臂的角度约束，即：

式中，和表示每个关节运动的下限和上限，θ_n表示第n个关节角度；此外，PA型机械臂主动、被动关节之间存在角度约束也应加入关节角约束空间；用蒙特卡洛方法计算多关节机械臂的工作空间，通过均匀分布赋以一定数量的符合关节变化要求的随机量，然后再对各关节变量进行组合，并利用机械臂的正向运动学递推计算机械臂末端执行器端点的坐标值，这些坐标值构成的集合即为机械臂末端的可达空间；

平面PAA型欠驱动机械臂末端位置笛卡尔坐标可以表示为：

其中(X,Y)为末端期望的笛卡尔坐标；选取机械臂末端坐标与预期位置差值作为评价函数，该评价函数可使粒子群不断向解空间内的最优区域移动，进而得到驱动杆的目标角度；

粒子群优化算法的步骤如下：

1)随机生成m个粒子的初始种群，初始化每个粒子的速度和位置，给定惯性权重ω、学习因子c1和c2、pbest、gbest；

2)计算每个粒子的适应度；

3)根据式(11)、(12)更新各粒子的飞行位置和速度；

v_i＝v_i+c₁*rand()*(pbest_i-x_i)+c₂*rand()*(gbest_i-x_i) (11)

x_i＝x_i+v_i (12)

4)若粒子的适应度优于原有个体极值pbest，则当前适应度设置为pbest，选择最优个体极值作为群体极值gbest；

5)判断算法是否达到终止条件，若是，则输出结果；否则进行从1到M的循环部分，其中M为最大迭代次数；

利用粒子群算法计算两个驱动关节的目标角度，为后续位置控制奠定基础；

(3)设计基于模糊控制和李雅普诺夫函数的分段位置控制律，第一阶段设计模糊控制器使第二关节到达目标角度，设计李雅普诺夫函数使第三关节保持初始状态；第二阶段设计模糊控制器使第三关节到达目标角度，设计李雅普诺夫函数使第二关节保持角度不变，实现位置控制，具体为：

首先将动力学模型改写为仿射非线性系统形式如下：

其中，F，G均为关于状态变量x的非线性函数，[G₁G₂ G₃]^T＝-M^-1(q)；按照李雅普诺夫函数的定义以及机械臂末端在目标点稳定的控制目标，所构造的函数保证驱动连杆的角度最终趋向于目标角度的同时，也要使驱动连杆的角速度最终收敛为零，因此构造的第三连杆李雅普诺夫函数如下：

其中，V₃(x)为一个半正定函数，对上式进行求导，并将系统状态方程式(12)代入可得：

其中a₂₂为G矩阵中第二行第二列的项，根据上式可求得控制律：

τ₃＝(-x₅+x₅(0)-F₃-μx₅-a₂₂τ₂)/a₂₂ (16)

其中μ为大于0的常数，用于调节收敛速度；x₅(0)为第三关节角的初始设定值；可以看到，第三关节的输出力矩随着第二关节的输出力矩而改变，无论第二关节角度如何变化，第三关节都稳定在设定的初始角度；接下来设计第二连杆的模糊控制律，取关节角度误差及误差变化率作为模糊系统输入，第二关节的力矩作为输出，采用重心法进行反模糊化，当第二关节角度到达目标位置，且角速度为0时，控制策略进入第二阶段；与第一阶段类似，分别构造第二关节的的李雅普诺夫函数，保持第二关节的初始角度不变，设计模糊控制器实现第三关节的控制目标；当驱动杆角度分别稳定在各自目标角度时，根据角度之间的约束关系，连带实现了欠驱动连杆的角度控制，最终实现PAA型机械臂的位置控制。