[发明专利]一种基于递阶结构MPC的多自由度柔性关节机械臂系统的控制方法

权利要求书

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

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

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

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

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

2.根据权利要求1所述基于递阶结构MPC的多自由度柔性关节机械臂系统的控制方法，其特征在于，所述柔性关节机械臂的动力学模型的公式如下：

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

其中h_1i、h_2i、h_3i分别为机械臂连杆1、连杆2、连杆3的惯量；c_1i、c_2i、c_3i分别为关节空间的关节1、关节2、关节3的科氏力与向心力系数；g₁、g₂、g₃分别为连杆1、连杆2、连杆3的重力向量；

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

其中j₁、j₂、j₃分别为关节1、关节2、关节3的转子惯量；k₁、k₂、k₃-分别为关节1、关节2、关节3的扭转刚度系数；j₁-j₃、k₁-k₃是给定值，可直接从零件的产品手册获得；τ_m1、τ_m2、τ_m3分别为关节1、关节2、关节3的电机驱动力矩；θ1、θ₂、θ₃分别为关节1、关节2、关节3的电机转角，由传感器测量得到；q₁、q₂、q₃分别为连杆1、连杆2、连杆3的转角，由传感器测量得到。

3.根据权利要求1所述基于递阶结构MPC的多自由度柔性关节机械臂系统的控制方法，其特征在于，所述整体结构设计过程如下：

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

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

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

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

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

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

4.根据权利要求1所述基于递阶结构MPC的多自由度柔性关节机械臂系统的控制方法，其特征在于，所述递阶结构的控制器为多输入多输出的MPC控制器，MPC通过把约束加到未来输入、输出上，将约束显式表示在二次规划或非线性规划问题中，当k≤1时，所搭建的柔性关节机械臂模型被用来预测未来输出；在第k步，在线求解一个涉及未来时域即预测时域N_P内预测信息的优化问题，来计算未来一段时域即控制时域N_C内的控制输入；在第k+1步，该优化问题所涉及的时域向前推移一步，再次求解；后续时刻依次滚动进行。

5.根据权利要求1所述基于递阶结构MPC的多自由度柔性关节机械臂系统的控制方法，其特征在于，所述步骤四中，控制算法如下：

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

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

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

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

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

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