(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210703477.0 (22)申请日 2022.06.21 (71)申请人 江苏大学 地址 212013 江苏省镇江市京口区学府路 301号 (72)发明人 顾寄南　李兴家　季晨　王博　 张文浩　 (74)专利代理 机构 南京智造力知识产权代理有 限公司 32382 专利代理师 张明明 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种面向电子元器件柔顺装配的机械臂分 层多回路控制方法 (57)摘要 本发明提供一种面向电子元器件柔顺装配 的机械臂分层多回路控制方法， 将设计参考轨 迹、 阻抗力和上一个循环环境阻抗力输入电子元 器件装配场景阻抗特征知识库， 得到机械臂关节 的设计阻抗轨迹， 并与机械臂关节的位置信息求 差， 经MPC循环输出模型预测控制变量， 并与 IHOSMC循环输出求和， 得到辅助控制变量， 并经 逆动力学控制循环方程， 得到机械臂系统的控制 力矩， 进而得到关节转角变量、 末端执行器的输 出轨迹， 结合环境信息， 得到环境阻抗力； 当末端 执行器的输出轨迹等于设计阻抗轨迹、 环境阻抗 力等于设计阻抗力时， 机械臂系统稳定， 按照此 时的输出轨迹和环境阻抗力执行电子元器件的 装配。 本发明具有装配精度高、 响应速度快的特 点。 权利要求书2页 说明书7页 附图1页 CN 115157241 A 2022.10.11 CN 115157241 A 1.一种面向电子元器件柔 顺装配的机 械臂分层多回路控制方法， 其特 征在于： 将电子元器件装配任务所需的机械臂末端执行器参考轨迹、 设计阻抗力和上一个循环 的环境阻抗力输入电子元器件装配场景阻抗特征知识库， 得到机械臂末端 执行器的设计阻 抗轨迹； 机械臂关节的设计阻抗轨迹与机械臂关节的位置信 息求差， 传递给模型预测控制循环 单元， 输出模型 预测控制变量； 所述模型 预测控制变量与高阶滑模 控制循环输出信号 求和， 得到 辅助控制变量； 所述辅助控制变量经逆动力学控制循环方程， 得到机械臂系统的控制力矩， 进而得到 关节转角变量， 由所述关节转角变量得到末端执行器的输出轨迹， 进而 结合环境信息， 得到 环境阻抗力； 当末端执行器的输出轨迹等于所述设计阻抗轨迹、 环境阻抗力等于所述设计阻抗力 时， 多关节机械臂系统稳定， 机械臂按照此时的输出轨迹和环境阻抗力执行电子元器件的 装配。 2.根据权利要求1所述的机械臂分层多回路控制方法， 其特征在于， 所述电子元器件装 配场景阻抗特征知识库包括不同装配方式时机械臂装配电子元器件的过程的实验数据和 利用所述实验数据训练后的自适应神经模糊推理系统， 所述实验数据包括参考轨迹、 设计 阻抗力、 环境阻抗力和阻抗轨 迹。 3.根据权利要求1所述的机械臂分层多回路控制方法， 其特征在于， 所述模型预测控制 循环的对象为积分高阶滑模控制循环后离散的状态空间方程 并 设计如下代价 函数： 其中： 矩阵A 的离散形式 矩阵B的离散形式 T表示离散系统的采样 周期， xi(k)表示第i关节在离散系统的第k步时的关节转角信息， vi(k)表示第i关节在离散 系统的第k步时的辅助控制变量； 表示某一向量的2 ‑范数乘以权重因子Q， 表示某一向 量的2‑范数乘以权重因子R， 表示某一向量的2 ‑范数乘以权重因子P； vi(k+j)为第i个关 节在第k控制步时的控制输入量， ei(k+j)为第i个关节在第k控制步时的参考轨迹与真实轨 迹间的误差， j表示第j预测步， N为总的预测步长； xmin和xmax分别为机械臂关节转角设定的 最小值和最大值， vmin和vmax分别为机械臂关节 转角设定的最小驱动力矩 值和最大驱动力矩 值； i＝1， 2， 3， n， n 为机械臂的关节总个数。 4.根据权利要求3所述的机械臂分层多回路控制方法， 其特征在于， 所述模型预测控制 变量的离 散形式为uMPC(k)＝[v1(k),v2(k),…,vn(k)]T。 5.根据权利要求4所述的机械臂分层多回路控制方法， 其特征在于， 所述高阶滑模控制权　利　要　求　书 1/2 页 2 CN 115157241 A 2循环输出信号 为： 等效积分高阶滑模 控制输出量 为： 其中： 为大于辅助控制变量vi(t)上界的常数， sgn表示符号函数， 表示第i个 关节的积分滑模变量， 表示第i个关节的积分滑模变量的导数。 6.根据权利要求5所述的机械臂分层多回路控制方法， 其特征在于， 第i个关节的积分 滑模变量及其 导数满足： 其中： 为 的导数， 为 的导数， 表示第i关节的辅助控制变量 ηi(t) 对时间的一阶导数， 表示解耦后第i关节的角速度 对时间的一阶导数， 表示 解耦后第i关节的角速度 对时间的二阶导数， 表示积分滑模变量的积分项 对 时间的二阶导数， mi是正实数， 表示第i关节的辅助控制变量vi(t)对时间的一阶导数。 7.根据权利要求5所述的机械臂分层多回路控制方法， 其特征在于， 所述辅助控制变量 v(t)＝uMPC(t)+uIHOSMC(t)， 其中uMPC(t)模型预测控制变量的连续形式。 8.根据权利要求1所述的机械臂分层多回路控制方法， 其特征在于， 所述末端执行器的 输出轨迹与装配场景 的环境位置信息求差后， 与环境刚度相乘， 得到的乘积即为环境阻抗 力。 9.根据权利要求1所述的机械臂分层多回路控制方法， 其特征在于， 初始循环时， 所述 环境阻抗力为0， 所述高阶滑模 控制循环输出信号 为0， 所述关节的角速度为0 。 10.根据权利要求1所述的机械臂分层多回路控制方法， 其特征在于， 所述机械臂关节 的设计设计阻抗轨 迹由机械臂末端执 行器的设计阻抗轨 迹根据逆运动学 得到。权　利　要　求　书 2/2 页 3 CN 115157241 A 3