一种基于递阶结构MPC的多自由度柔性关节机械臂系统的控制方法与流程

本发明属于工业控制及自动化技术领域，涉及机械臂的控制，特别涉及一种基于递阶结构mpc的多自由度柔性关节机械臂系统的控制方法。

背景技术：

相对于传统的刚性机械臂，柔性关节机械臂具有质量轻、体积小、能耗低等特点，其应用范围越来越广，涉及航天领域、医疗领域、工业装配等领域，这对机械臂的控制要求极高。而且实践证明，关节的柔性不仅影响关节的运行质量，更严重影响机械臂系统的运动特性。因此，柔性关节机械臂的建模以及控制方法的研究有非常重要的实际意义。

pid控制方法简单灵活、不依赖于系统模型，已被广泛地应用于工业机器人的控制系统中。但随着工业要求的不断提高，单一的pid控制器很难同时达到快速、精确和稳定的要求。机械臂的每一个关节都需要一个控制器，对于多关节机械臂，则需要多个控制器，不仅使系统动力学耦合问题复杂、难控制，而且成本较高。

技术实现要素：

为了克服上述现有技术的缺点，本发明的目的在于提供一种基于递阶结构mpc的多自由度柔性关节机械臂系统的控制方法，利用基于模型的闭环优化控制算法——预测控制(modelpredictivecontrol,mpc)算法，对多自由度机械臂模型进行改进，得到一种通用的模型，并设计一种递阶结构的控制器，将多输入多输出的预测控制器作为上层控制器，对关节角度进行优化，再将输出送入下层的pd控制器中进行执行，增强系统的稳定性；本发明实现了对被控对象施以实时滚动的最优控制，能很好地克服模型不精确带来的误差。其成本低廉、通用性强、灵活性好、结构简单，可以使机械臂实现精密动作，并且稳定性强。

为了实现上述目的，本发明采用的技术方案是：

一种基于递阶结构mpc的多自由度柔性关节机械臂系统的控制方法，包括如下步骤：

步骤一、假设连接在电机转子与连杆之间的弹簧弹性系数为柔性关节的刚度系数，即关节无减速装置，建立柔性关节机械臂的动力学模型；

步骤二、设计整体结构：首先在逆运动学中加入轨迹规划，构成笛卡尔控制器，其次将动力学部分与运动学部分相互联系，使得机械臂能够稳定运行笛卡尔控制器所设计的目标轨迹；

步骤三、控制器设计：在pd控制器的基础上，设计一种递阶结构的控制器，将上层预测控制器进行多变量控制，得到每一个关节运动pd控制器的设定值，再送入下层pd控制器进行实现；

步骤四、在所述整体结构下，基于所述递阶结构的控制器，实现多自由度柔性关节机械臂系统的控制。

柔性关节机械臂的动力学模型的公式如下：

式中，h(q)为机械臂的惯量矩阵，为关节空间的科氏力与向心力系数矩阵，g(q)为重力向量；

其中h1i、h2i、h3i分别为机械臂连杆1、连杆2、连杆3的惯量；c1i、c2i、c3i分别为关节空间的关节1、关节2、关节3的科氏力与向心力系数；g1、g2、g3分别为连杆1、连杆2、连杆3的重力向量；

具体地：

h11＝i1+a1cos²(q2)+a2cos²(q2+q3)+2a3cos(q2)cos(q2+q3)

h22＝i2+a1+a2+2a3cos(q3)

h23＝h32＝a2+a3cos(q3)

h33＝i3+a2

h12＝h21＝h13＝h31＝0

c21＝-c12

c31＝-c13

c33＝0

g1＝0

g2＝b1cos(q2)+b2cos(q2+q3)

g3＝b2cos(q2+q3)

a3＝m3r3l2

b1＝(m2r2+m3l2)g

b2＝m3r3g

mi表示连杆的质量，li表示连杆的长度；ri表示连杆质心的长度，ii表示连杆关于其转动轴的转动惯量。g表示重力加速度矢量，qi表示连杆的转动角度(转动关节的关节变量)，ai、bi为常数项，没有实际的含义。

j为转子惯量矩阵，k为关节扭转刚度矩阵，τm为电机驱动力矩向量，θ为电机转角向量，q为连杆转角向量；

其中j1、j2、j3分别为关节1、关节2、关节3的转子惯量；k1、k2、k3—分别为关节1、关节2、关节3的扭转刚度系数；j1-j3、k1-k3是给定值，可直接从零件的产品手册获得；τm1、τm2、τm3分别为关节1、关节2、关节3的电机驱动力矩；θ1、θ2、θ3分别为关节1、关节2、关节3的电机转角，由传感器测量得到；q1、q2、q3分别为连杆1、连杆2、连杆3的转角，由传感器测量得到。

所述整体结构设计过程如下：

1.将目标轨迹即已经规划的轨迹输入逆运动学中构成笛卡尔控制器，并解算目标关节角度，输出到pd控制器中；

2.pd控制器将关节角度转换成力矩驱动关节层；

3.关节层动作，由传感器测得实际关节角度并输出到正、逆运动学中；

4.实际关节角度输入到正运动学中，得到实际机械臂轨迹；

5.实际关节角度反馈至逆运动学中，通过解算得到下一步的目标关节角度；

6.返回步骤1，如此循环，直至机械臂执行完目标轨迹。

所述笛卡尔控制器解算目标关节角度，是将目标轨迹输入到逆运动学中，逆运动学即已知末端轨迹解算关节角度，需要解算时，直接调用程序；

所述pd控制器将关节角度转换成力矩驱动关节层，具体方法：式中的两项为pd控制的比例项和微分项，均为对角正定矩阵；

所述实际关节角度输入到正运动学中正运动学作用是已知关节角度得到末端轨迹，因此可得到实际机械臂轨迹，

所述实际关节角度反馈至逆运动学中，通过解算得到下一步的目标关节角度，因为逆向运动学中有雅可比矩阵，求解第二步的值需要反馈回来的上一步的值。

所述递阶结构的控制器为多输入多输出的mpc控制器，mpc通过把约束加到未来输入、输出上，将约束显式表示在二次规划或非线性规划问题中，当k≤1时，所搭建的柔性关节机械臂模型被用来预测未来输出；在第k步，在线求解一个涉及未来时域即预测时域np内预测信息的优化问题，来计算未来一段时域即控制时域nc内的控制输入；在第k+1步，该优化问题所涉及的时域向前推移一步，再次求解；后续时刻依次滚动进行。

所述步骤四中，控制算法如下：

1.通过逆运动解算目标轨迹得到目标关节角度；

2.将目标关节角度以矩阵形式输入上层mpc控制器中，得到优化后的关节角度；

3.将优化后的关节角度输入下层pd控制器中，驱动关节，并通过传感器测量实际关节角度；

4.将实际关节角度输入到正运动学，得到实际机械臂运动轨迹；

5.同时将实际关节角度反馈上层mpc控制器中，进行校正；

6.返回步骤1，如此循环，直至机械臂执行完目标轨迹。

所述柔性关节机械臂为三自由度时，优化关节角度如下：

st.i(k+m|k)∈[-180°，180°]0≤m≤nc-1

i(k+m|k)＝0，nc≤m≤np-1

其中，i＝[q1；q2；q3]，i(k+m|k)(m＝0，1，…，nc-1)为控制输入，为代价函数，o(·)为期望位置轨迹；y(k|k)为系统输出，求解上述优化问题，将求得的第一个控制输入i(k|k)即优化后的关节角度输入到下层pd控制器中。

与现有技术相比，本发明的有益效果是：

1、与其他多自由刚性关节机械臂和多自由柔性关节机械臂系统相比，本发明使用基于递阶结构的mpc控制方法，系统成本低廉，速度快，稳定性强。

2、本发明系统整体结构简单，操作方便，通用性强且灵活性好，可在多自由度模型的基础上拓展至任意多自由模型。

3、本发明中加入递阶结构控制器，最终可以完全消除关节柔性对系统影响。

附图说明

图1是本发明柔性关节模型示意图(无减速装置)。

图2是本发明递阶结构控制器结构示意图。

图3是本发明实施例三自由度机械臂自由运动的机械臂末端轨迹。

图4是本发明实施例三自由度机械臂第一维度关节角度变化曲线。

图5是本发明实施例三自由度机械臂第二维度关节角度变化曲线。

图6是本发明实施例三自由度机械臂第三维度关节角度变化曲线。

具体实施方式

下面结合附图和实施例详细说明本发明的实施方式。

本发明的基于递阶结构mpc的多自由度柔性关节机械臂系统的控制方法，包括以下步骤：

步骤一、建立柔性关节动力学模型。机械臂的关节主要由动力装置(电机)、传动装置(减速器)、轴系和传感器等组成，为方便建模，对关节进行简化，假设连接在电机转子与连杆之间的弹簧弹性系数为柔性关节的刚度系数，即关节无减速装置，该柔性的关节机械臂动力学模型通用性强，适用于多自由度。

柔性关节模型如图1所示，其中，关节力矩与刚度系数关系：

τi＝ki(θi-qi)(2.1)

其中，τi为电机端所出的力矩，ki为柔性关节的刚度系数，qi为连杆的转角。因此，利用拉格朗日方程并做适当简化后，建立柔性关节机械臂的动力学模型为：

式(2.2a)中，

分别为机械臂的惯量矩阵、关节空间的科氏力与向心力系数矩阵、重力向量，

式(2.2b)中，

分别为转子惯量矩阵、关节扭转刚度矩阵、电机驱动力矩向量、电机转角向量和关节输出转角向量。

步骤二、设计整体结构。首先在逆运动学中加入轨迹规划，构成笛卡尔控制器，其次将动力学部分与运动学部分相互联系，使得机械臂能够稳定的运行笛卡尔控制器所设计的目标轨迹。该结构将运动学、轨迹规划与动力学综合为一体，使系统更加便于控制，整体结构设计如下：

1.将目标轨迹输入逆运动学中构成笛卡尔控制器，并解算目标关节角度，输出到pd控制器中；

2.pd控制器将关节角度转换成力矩驱动关节层；

3.关节层动作，由传感器测得实际关节角度并输出到正、逆运动学中；

4.实际关节角度输入到正运动学中，得到实际机械臂轨迹；

5.上一步的实际关节角度反馈至逆运动学中，通过解算得到下一步的目标关节角度，如此循环，直至机械臂执行完目标轨迹。

步骤三、控制器设计

pd反馈控制方法简单且易于实现，不依赖于系统模型，已被广泛应用于机械臂的控制系统中，为了使柔性关节机械臂能够快速而平稳的运行目标轨迹，在pd控制器的基础上，设计一种递阶结构的控制器，，将上层预测控制器进行多变量控制，得到每一个关节运动pd控制器的设定值，再送入下层pd控制器进行实现，如图2所示。

其中，递阶结构控制器：

因为是三自由度机械臂系统，因此选用多输入多输出的mpc控制器。mpc通过把约束加到未来输入、输出上，将约束显式表示在二次规划或非线性规划问题中。具体地，当k≤1时，所搭建的柔性关节机械臂模型被用来预测未来输出；在第k步，在线求解一个涉及未来时域(预测时域np)内预测信息的优化问题，来计算未来一段时域(控制时域nc)内的控制输入；在第k+1步，该优化问题所涉及的时域向前推移一步，再次求解；后续时刻依次滚动进行。

采用mpc策略设计上层控制器，以三自由度为例，优化关节角度如下：

st.i(k+m|k)∈[-180°，180°]0≤m≤nc-1

i(k+m|k)＝0，nc≤m≤np-1

其中，i＝[q(1)；q(2)；q(3)]，i(k+m|k)(m＝0，1，…，nc-1)为控制输入，为代价函数，o(·)为期望位置轨迹；y(k|k)为系统输出。求解上述优化问题，将求得的第一个控制输入i(k|k)即优化后的关节角度输入到下层pd控制器中。

步骤四、在所述整体结构下，基于所述递阶结构的控制器，实现多自由度柔性关节机械臂系统的控制，具体算法：

1.通过逆运动解算目标轨迹得到目标关节角度；

2.将目标关节角度以矩阵形式输入上层mpc控制器中，得到优化后的关节角度；

3.将优化后的关节角度输入下层pd控制器中，驱动关节，并通过传感器测量实际关节角度；

4.将实际关节角度输入到正运动学，得到实际机械臂运动轨迹；

5.同时将实际关节角度反馈上层mpc控制器中，进行校正，如此循环，直至机械臂执行完目标轨迹。

6.返回步骤1，如此循环，直至机械臂执行完目标轨迹。

本发明三自由度机械臂的仿真结果参考图3、图4、图5和图6，根据图3，分析可得采用递阶结构的机械臂在自由运动中，从圆弧运动突然转换至直线运动时，能够快速的达到稳定，且整个运动过程中跟随性较好。

图4、图5、图6分别为第一维度、第二维度、第三维度在采用递阶结构控制器时关节角度的变化曲线，并与目标关节角度进行对比，由图可知，采用递阶结构控制器时，实际关节角度几乎与目标关节角度一致，且角度变化稳定。

本发明的结构、设计、算法在python、arduino、ros环境均可适用，且成本低，操作简单，处理速度快，可实现机械臂快速、稳定的工作且可完全消除关节柔性对系统的影响。