一种基于模糊控制和李雅普诺夫函数的平面欠驱动机械臂位置控制方法与流程

本发明属于自动化控制
技术领域：
，涉及模型降阶、模糊控制法和李雅普诺夫直接控制法，具体是一种基于模糊控制和李雅普诺夫函数的平面欠驱动机械臂分段位置控制方法。
背景技术：
：随着航空航天技术和机器人控制技术的发展，特别是针对空间机械臂难以维修和水下机器人结构设计紧凑的实际情况，欠驱动机械系统的研究受到了全世界的广泛关注。由于欠驱动机械系统的外部控制输入个数少于自由度个数，全驱动机械臂当中的成熟的控制方法，如全状态反馈线性化方法，pid控制法，基于无源的自适应控制法等均不能直接推广到欠驱动机械臂当中。因此，欠驱动机械臂的控制问题是该领域内的公认难题。欠驱动机械臂可按照结构划分为水平平面运动和竖直平面运动两种。竖直平面内运动的欠驱动机械臂只有最高和最低两个平衡位置。但是，水平欠驱动机械臂不受重力影响，有无数个平衡位置。且第一关节为被动关节的水平欠驱动机械臂为非完整系统，这使得其位置控制更加困难。利用模型降阶的思想，将三自由度欠驱动机械臂降阶为两个二自由度机械臂。现有控制方法中，根据系统的状态方程，仅通过构造李雅普诺夫函数，推导出控制律。这种方法能够设计稳定的控制律，但难以找到最优的李雅普诺夫函数，所以控制的快速性不佳。基于单一滑模控制法实现平面paa型机械臂位置控制，将机械臂分为几个子系统，分别为每个子系统设计滑模面和切换函数，再构造总滑模面，使整个系统稳定。但该方法存在抖振问题，稳态性能不佳。控制过程的动态特性还亟待优化。技术实现要素：有鉴于此，鉴于现有控制方法难以兼顾快速性和准确性的问题，本发明提出一种基于模糊控制和李雅普诺夫函数的平面欠驱动机械臂分段位置控制方法，包括：建立平面三自由度被动-主动-主动型(paa型)欠驱动机械臂动力学模型，并分析其各关节角度、角速度之间的积分约束关系；分析其可达空间，利用粒子群算法计算各驱动关节的目标角度；设计基于模糊控制和李雅普诺夫函数的分段位置控制律，第一阶段设计模糊控制器使第二关节到达目标角度，设计李雅普诺夫函数使第三关节保持初始状态；第二阶段设计模糊控制器使第三关节到达目标角度，设计李雅普诺夫函数使第二关节保持角度不变，实现位置控制。上述方法中，建立平面三自由度被动-主动-主动型(paa型)欠驱动机械臂动力学模型，并分析其各关节角度、角速度之间的积分约束关系，包括：平面三自由度被动-主动-主动型(paa型)欠驱动机械臂不受重力影响，因此不用考虑势能，且被动关节处可自由旋转，不具有制动力矩和摩擦力，将研究对象简化为一个刚体结构，根据拉格朗日方程，建立其动力学模型如下：其中，mij∈r3×3(i,j＝1,2,3)具体为，ak(k＝1,2,...,6)为系统的结构参数，qi(i＝1,2,3)为机械臂的第i关节角，为机械臂的第i关节角加速度；mi为第i杆的质量(i＝1,2,3)，li为第i杆的长度(i＝1,2,3)，li为第i杆质心到前一关节的长度(i＝1,2,3)，ji为第i杆的转动惯量(i＝1,2,3)；hi∈r3×1(i＝1,2,3)为哥氏力和离心力项，τ＝(0,τ2,τ3)t为关节力矩矢量，其中被动关节的输出力矩为0，为机械臂的第i关节角速度；平面paa型欠驱动机械臂是一阶非完整系统，平面pa型机械臂是完整系统；基于模型降阶的思想，将控制过程分为两个阶段，在第一阶段维持第三关节初始角度不变，控制第二关节到达目标角度；在第二阶段维持第二关节目标角度不变，控制第三关节到达目标角度；在式(1)中取出欠驱动所处行，并将式(4)代入可得：利用分部积分，式(5)对时间t求定积分，可得：在每个控制阶段维持角度不变，可近似认为关节角速度为零，因此，对式(6)两端再次求定积分，化简可得：其中，q11(0)表示第一阶段时被动杆初始角度，q2(0)和q3(0)分别表示第二杆和第三杆的初始角度，d1,e1,g1,g1为式(7)化简过程的中间项，d1＝(8a3a5-4a2a6-4a1a6-4a4a6)cosq2(0)-a12-a22-4a62cos2q2(0)-a42+4a52+4a32-2a1a2-2a1a4-2a2a4g1＝(a1+a2+a4+2a6cosq2(0)-2a5cosq2(0)-2a3)(cosq2-1)+2a5sinq2(0)sinq2同理，在第二阶段的控制中维持第二关节角度不变，即可近似认为关节角速度为零，再次对式(6)两端求定积分可得到第二阶段关节角度约束。上述方法中，分析其可达空间，利用粒子群算法计算各驱动关节的目标角度，包括：机械臂的工作空间是指机械臂末端执行器可以到达的空间位置的集合，将机械臂工作域记作a(p)，则机械臂各个关节角度与工作可达空间的映射可表示为：a(p)＝{p(θ)|θ∈q}(8)其中q为关节角的约束空间，以关节限位角作为机械臂角度约束，即：式中，和表示关节运动的下限和上限，θn表示第n个关节角度。此外，pa型机械臂主动、被动关节之间存在角度约束也应加入关节角约束空间；用蒙特卡洛方法计算多关节机械臂的工作空间的实质是对关节变量通过均匀分布赋以一定数量的符合关节变化要求的随机量，然后再对各关节变量进行组合，并利用机械臂的正向运动学递推计算机械臂末端执行器端点的坐标值，这些坐标值构成的集合即为机械臂末端的可达空间；平面paa型欠驱动机械臂末端位置笛卡尔坐标可以表示为：其中(x,y)为末端期望的笛卡尔坐标；选取机械臂末端坐标与预期位置差值作为评价函数，该评价函数可使粒子群不断向解空间内的最优区域移动，进而得到驱动杆的目标角度；粒子群优化算法的步骤如下：1)随机生成m个粒子的初始种群，初始化每个粒子的速度和位置，给定惯性权重ω、学习因子c1和c2、pbest、gbest；2)计算每个粒子的适应度；3)根据式(11)、(12)更新各粒子的飞行位置和速度；vi＝vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbesti-xi)(11)xi＝xi+vi(12)4)若粒子的适应度优于原有个体极值pbest，则当前适应度设置为pbest，选择最优个体极值作为群体极值gbest；5)判断算法是否达到终止条件。若是，则输出结果；否则进行从1到m的循环部分，其中m为最大迭代次数；利用粒子群算法计算两个驱动关节的目标角度，为后续位置控制奠定基础。上述方法中，设计基于模糊控制和李雅普诺夫函数的分段位置控制律，第一阶段设计模糊控制器使第二关节到达目标角度，设计李雅普诺夫函数使第三关节保持初始状态；第二阶段设计模糊控制器使第三关节到达目标角度，设计李雅普诺夫函数使第二关节保持角度不变，实现位置控制，包括：首先将动力学模型改写为仿射非线性系统形式如下：其中，f，g均为关于状态变量x的非线性函数，[g1g2g3]t＝-m-1(q)；按照李雅普诺夫函数的定义以及机械臂末端在目标点稳定的控制目标，所构造的函数保证驱动连杆的角度最终趋向于目标角度的同时，也要使驱动连杆的角速度最终收敛为零，因此构造的第三连杆李雅普诺夫函数如下：其中，v3(x)为一个半正定函数，对上式进行求导，并将系统状态方程式(12)代入可得：其中a22为g矩阵中第二行第二列的项，根据上式可求得控制律：τ3＝(-x5+x5(0)-f3-μx5-aa22τ2)/aa22(16)其中μ为大于0的常数，用于调节收敛速度；x5(0)为第三关节角的初始设定值。可以看到，第三关节的输出力矩随着第二关节的输出力矩而改变，无论第二关节角度如何变化，第三关节都稳定在设定的初始角度。接下来设计第二连杆的模糊控制律，取关节角度误差及误差变化率作为模糊系统输入，第二关节的力矩作为输出，采用重心法进行反模糊化，当第二关节角度到达目标位置，且角速度为0时，控制策略进入第二阶段；与第一阶段类似，分别构造第二关节的的李雅普诺夫函数，保持第二关节的初始角度不变，设计模糊控制器实现第三关节的控制目标；当驱动杆角度分别稳定在各自目标角度时，根据角度之间的约束关系，连带实现了欠驱动连杆的角度控制，最终实现paa型机械臂的位置控制；【附图说明】为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。图1表示本发明实例控制方法框图；图2表示机械臂可达空间；图3表示机械臂末端位置与各关节角的关系；图4表示关节角度隶属度函数；图5表示关节角速度隶属度函数；图6表示位置控制变化过程；图7表示第一阶段控制曲线；图8表示第二阶段控制曲线。【具体实施方式】为了更好地理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。应当明确，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。一种基于模糊控制和李雅普诺夫函数的平面欠驱动机械臂分段位置控制方法，具体包括步骤如下：1.建立平面三自由度被动-主动-主动型(paa型)欠驱动机械臂动力学模型，并分析其各关节角度、角速度之间的积分约束关系；2.分析其可达空间，利用粒子群算法计算各驱动关节的目标角度；3.设计基于模糊控制和李雅普诺夫函数的分段位置控制律，第一阶段设计模糊控制器使第二关节到达目标角度，设计李雅普诺夫函数使第三关节保持初始状态；第二阶段设计模糊控制器使第三关节到达目标角度，设计李雅普诺夫函数使第二关节保持角度不变，实现位置控制。本发明实施例给出基于模糊和李雅普诺夫函数的平面欠驱动机械臂分段位置控制方法，请参考图1，其为本发明实施例给出基于模糊和李雅普诺夫函数的平面欠驱动机械臂分段位置控制方法的控制过程框图，该方法包括以下步骤：步骤101，建立平面三自由度被动-主动-主动型(paa型)欠驱动机械臂动力学模型，根据模型降阶将控制过程分解为两个控制阶段，并分析各关节角度、角速度之间的积分约束关系；具体的，建立平面三自由度被动-主动-主动型(paa型)欠驱动机械臂动力学模型如下：其中，mij∈r3×3(i,j＝1,2,3)具体为，ak(k＝1,2,...,6)为系统的结构参数，qi(i＝1,2,3)为机械臂的第i关节角，为机械臂的第i关节角加速度；mi为第i杆的质量(i＝1,2,3)，li为第i杆的长度(i＝1,2,3)，li为第i杆质心到前一关节的长度(i＝1,2,3)，ji为第i杆的转动惯量(i＝1,2,3)；hi∈r3×1(i＝1,2,3)为哥氏力和离心力项，τ＝(0,τ2,τ3)t为关节力矩矢量，其中被动关节的输出力矩为0，为机械臂的第i关节角速度；力学中的完整系统是指一个系统的所有约束条件均能直接通过积分方式变成不含坐标微商的约束，完整系统的运动被限制在一个光滑超曲面上，实际上，大多数机械系统均可看作是非完整系统，其中包括速度不可积的一阶非完整系统以及加速度不可积的二阶非完整系统，可积条件具体定义如下：定义1：第一可积条件：势能项所产生的力矩为常数；欠驱动关节变量不出现在惯性矩阵当中；定义2：第二可积条件：第一可积条件成立；惯性矩阵中的欠驱动项零空间分布是对合的；根据上述定义可知，平面paa型欠驱动机械臂是一阶非完整系统，平面pa型机械臂是完整系统；基于模型降阶的思想，将控制过程分为两个阶段，在第一阶段维持第三关节初始角度不变，控制第二关节到达目标角度；在第二阶段维持第二关节目标角度不变，控制第三关节到达目标角度；在式(1)中取出欠驱动所处行，并将式(4)代入可得：利用分部积分，式(5)对时间t求定积分，可得：在每个控制阶段维持角度不变，可近似认为关节角速度为零，因此，对式(6)两端再次求定积分，化简可得：其中，q11(0)表示第一阶段时被动杆初始角度，q2(0)和q3(0)分别表示第二杆和第三杆的初始角度，d1，e1，g1，g1为式(7)化简过程的中间项，d1＝(8a3a5-4a2a6-4a1a6-4a4a6)cosq2(0)-a12-a22-4a62cos2q2(0)-a42+4a52+4a32-2a1a2-2a1a4-2a2a4g1＝(a1+a2+a4+2a6cosq2(0)-2a5cosq2(0)-2a3)(cosq2-1)+2a5sinq2(0)sinq2同理，在第二阶段的控制中维持第二关节角度不变，即可近似认为关节角速度为零，再次对式(6)两端求定积分可得到第二阶段关节角度约束。步骤102，基于欠驱动机械臂的角度积分约束关系，分析其可达空间，以机械臂末端与目标位置的差值作为粒子群算法的适应度函数，计算出驱动关节的目标角度；具体的，机械臂的工作空间是指机械臂末端执行器可以到达的空间位置的集合，如图2所示，将机械臂工作域记作a(p)，则机械臂各个关节角度与工作可达空间的映射可表示为：a(p)＝{p(θ)|θ∈q}(8)其中q为关节角的约束空间，以关节限位角作为机械臂的角度约束，即：式中，和表示关节运动的下限和上限，θn表示第n个关节角度。此外，pa型机械臂主动、被动关节之间存在角度约束也应加入关节角约束空间；用蒙特卡洛方法计算多关节机械臂的工作空间的实质是对关节变量通过均匀分布赋以一定数量的符合关节变化要求的随机量，然后再对各关节变量进行组合，并利用机械臂的正向运动学递推计算机械臂末端执行器端点的坐标值，这些坐标值构成的集合即为机械臂末端的可达空间；根据运动学，如图3所示，平面paa型欠驱动机械臂末端位置笛卡尔坐标可以表示为：其中(x,y)为末端期望的笛卡尔坐标；根据系统末端点目标坐标求取驱动杆的角度，本质上就是系统的逆运动学求解；数值方法从本质上来讲是猜测和迭代，直到错误足够小，或者直到认为放弃；选取机械臂末端坐标与预期位置差值作为评价函数，该评价函数可使粒子群不断向解空间内的最优区域移动，进而得到驱动杆的目标角度；粒子群优化算法的步骤如下：1)随机生成m个粒子的初始种群，初始化每个粒子的速度和位置，给定惯性权重ω、学习因子c1和c2、pbest、gbest；2)计算每个粒子的适应度；3)根据式(11)、(12)更新各粒子的飞行位置和速度；vi＝vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbesti-xi)(11)xi＝xi+vi(12)4)若粒子的适应度优于原有个体极值pbest，则当前适应度设置为pbest，选择最优个体极值作为群体极值gbest；5)判断算法是否达到终止条件。若是，则输出结果；否则进行从1到m的循环部分，其中m为最大迭代次数；利用粒子群算法计算两个驱动关节的目标角度，为后续位置控制奠定基础。步骤103，提出一种基于模糊和李雅普诺夫函数的平面三自由度欠驱动机械臂分段位置控制方法，在第一阶段，设计模糊控制器使得第二关节到达目标角度，并且通过李雅普诺夫函数求得第三关节控制律使其保持初始状态；在第二阶段，设计模糊控制器使得第三关节到达目标角度，并通过李雅普诺夫函数求得第二关节控制律使其保持目标角度，包括：首先将动力学模型改写为仿射非线性系统形式如下：其中，f，g均为关于状态变量x的非线性函数，[g1g2g3]t＝-m-1(q)；按照李雅普诺夫函数的定义以及机械臂末端在目标点稳定的控制目标，所构造的函数保证驱动连杆的角度最终趋向于目标角度的同时，也要使驱动连杆的角速度最终收敛为零，因此构造的第三连杆李雅普诺夫函数如下：其中，v3(x)为一个半正定函数，对上式进行求导，并将系统状态方程式(12)代入可得：其中aa22为g矩阵中第二行第二列的项，根据上式可求得控制律：τ3＝(-x5+x5(0)-f3-μx5-aa22τ2)/aa22(16)其中μ为大于0的常数，用于调节收敛速度；x5(0)为第三关节角的初始设定值。可以看到，第三关节的输出力矩随着第二关节的输出力矩而改变，无论第二关节角度如何变化，第三关节都稳定在设定的初始角度。接下来设计第二连杆的模糊控制律，取关节角度误差及误差变化率作为模糊系统输入，第二关节的力矩作为输出，关节角度及关节角速度隶属度函数如图4、图5所示，采用重心法进行反模糊化，当第二关节角度到达目标位置，且角速度为0时，控制策略进入第二阶段；与第一阶段类似，分别构造第二关节的的李雅普诺夫函数，保持第二关节的初始角度不变，设计模糊控制器实现第三关节的控制目标；当驱动杆角度分别稳定在各自目标角度时，根据角度之间的约束关系，连带实现了欠驱动连杆的角度控制，最终实现paa型机械臂的位置控制；所述方法在稳态结果相差不大的情况下，该控制方法在超调量和稳态时间上有明显的下降，能够较好的对控制要素进行优化，使机械臂获得更好的动态性能。具体实施中，机械臂动力学参数及粒子群算法参数如下表所示：a1a2a3a4a5a6l1l2l31.578672.0161.5360.333330.40.60.81.21.0q1(0)q2(0)q3(0)npgen_maxc1c20001530021.8模糊控制器中模糊规则表为：e/denbnmzpmpbnbpbpmpmpsznmpmpmpsznszpmpsznsnmpmpsznsnmnbpbznsnmnmnb如图6所示，机械臂初始构形qinit＝[000]；以基座位置作为坐标原点，选取目标位置为(1,-0.5),通过粒子群算法解得相应的驱动关节角度为[3.0117,-1.7322]，单位为弧度。两阶段的控制曲线及主动关节力矩输出曲线如图7、图8所示，在可达空间的范围内，对于任意初始构型，所提控制方法能够有效完成位置控制，且末端位置符合误差范围。该方法在有限时间内实现了给定目标的位置控制，并提升了机械臂末端位置、超调量、稳态时间三方面的性能；具体为，在稳态结果相差不大的情况下，该控制方法在超调量和稳态时间上有明显的下降，能够较好的对控制要素进行优化，使机械臂获得更好的动态性能。以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。当前第1页12